A New Simple Model of Balancing in the Plane

Abstract

This paper presents a new model of the dynamics of balancing in the plane, in which the essential parameters of the robot’s balancing behaviour are reduced to just two numbers, both of which are simple functions of basic physical properties of the robot mechanism. A third number describes the effect of other movements on the robot’s balance. Given this model, a high-performance balance controller can be constructed as a simple four-term control law with gains that are trivial functions of the two model parameters and a single value chosen by the user that determines the overall speed of balancing. The model is first developed for a double pendulum, and then extended to a general planar mechanism. Simulation results are presented showing the controller’s performance at following commanded motion trajectories while simultaneously maintaining the robot’s balance.

Appendix

This appendix proves the result that $p_i=\varvec{s}_i^\mathrm {T}\varvec{h}_{\nu (i)}$, where $p_i$ and $\varvec{s}_i$ are the momentum variable and axis vector of joint i, and $\varvec{h}_{\nu (i)}$ is the total momentum of the subtree or self-contained subsystem consisting of body i and its descendants. A self-contained subsystem, in this context, is defined to be a subsystem in which every kinematic loop that involves at least one body in the subsystem is entirely contained within the subsystem. In general, $\varvec{s}_i$ and $\varvec{h}_{\nu (i)}$ will be spatial vectors. However, if the whole system is planar then they may instead be planar vectors.

Consider a kinematic tree consisting of N bodies and joints numbered from 1 to N according to a regular numbering scheme. Without loss of generality, we assume that every joint has only a single degree of freedom (DoF), which means that every multi-DoF joint has already been replaced by a kinematically equivalent chain of single-DoF joints connected by massless bodies, and that these extra bodies and joints are already included in N.

Let $\varvec{p}$ and $\dot{\varvec{q}}$ denote the joint-space momentum and velocity vectors of the tree, or the spanning tree if there are kinematic loops. By definition, the two are related by the equation

$$\begin{aligned} \varvec{p}= \varvec{H}\dot{\varvec{q}}\,, \end{aligned}$$

(44)

where $\varvec{H}$ is the joint-space inertia matrix. This implies that

$$\begin{aligned} p_i = \sum _{j=1}^N H_{ij} \dot{q}_j \,. \end{aligned}$$

(45)

The definition of $\varvec{H}$ for a general kinematic tree with single-DoF joints is

$$\begin{aligned} H_{ij} = \left\{ \begin{array}{cc} \varvec{s}_i^\mathrm {T}\varvec{I}_{\nu (i)} \varvec{s}_j &{} \text { if } i \in \nu (j) \\ \varvec{s}_i^\mathrm {T}\varvec{I}_{\nu (j)} \varvec{s}_j &{} \text { if } j \in \nu (i) \\ 0 &{} \text { otherwise} \end{array} \right. \end{aligned}$$

(46)

where $\varvec{s}_i$ is the joint axis vector (i.e., joint motion subspace vector) of joint i, $\varvec{I}_{i}$ is the inertia of body i (spatial or planar as appropriate), $\nu (i)$ is the set of all bodies in the subtree beginning at body i, and $\varvec{I}_{\nu (i)}=\sum _{j\in \nu (i)}\varvec{I}_{j}$.

Let $\bar{\kappa }(i)$ be the set of all bodies on the path between body i and the base (body 0), excluding both body i and the base, and let $\kappa (i)=\bar{\kappa }(i)\cup \{i\}$ be the same set including body i. If we use the terms ‘ancestor’ and ‘descendant’ in an inclusive sense, meaning that body i is both an ancestor and a descendant of itself, and use the term ‘proper ancestor’ in an exclusive sense, then the sets $\nu (i)$, $\kappa (i)$ and $\bar{\kappa }(i)$ can be seen to be the sets of descendants, ancestors and proper ancestors, respectively, of body i. $\kappa (i)$ is also the set of joints on the path between body i and the base.

We now rewrite Eq. 46 as follows:

$$\begin{aligned} H_{ij} = \left\{ \begin{array}{cc} \varvec{s}_i^\mathrm {T}\varvec{I}_{\nu (i)} \varvec{s}_j &{} \text { if } j \in \bar{\kappa }(i) \\ \varvec{s}_i^\mathrm {T}\varvec{I}_{\nu (j)} \varvec{s}_j &{} \text { if } j \in \nu (i) \\ 0 &{} \text { otherwise} \end{array} \right. \end{aligned}$$

(47)

which makes it clear that $H_{ij}$ is nonzero only if $j\in \bar{\kappa }(i)$ or $j\in \nu (i)$. Substituting Eq. 47 into Eq. 45 gives

$$\begin{aligned} p_i= & {} \varvec{s}_i^\mathrm {T}\big ( \sum _{j\in \bar{\kappa }(i)} \varvec{I}_{\nu (i)} \varvec{s}_j \dot{q}_j + \sum _{j\in \nu (i)} \varvec{I}_{\nu (j)} \varvec{s}_j \dot{q}_j \big ) \nonumber \\= & {} \varvec{s}_i^\mathrm {T}\big ( \sum _{j\in \bar{\kappa }(i)} \sum _{k\in \nu (i)} \varvec{I}_{k} \varvec{s}_j \dot{q}_j + \sum _{j\in \nu (i)} \sum _{k\in \nu (j)} \varvec{I}_{k} \varvec{s}_j \dot{q}_j \big ) \nonumber \\= & {} \varvec{s}_i^\mathrm {T}\big ( \sum _{k\in \nu (i)} \sum _{j\in \bar{\kappa }(i)} \varvec{I}_{k} \varvec{s}_j \dot{q}_j + \sum _{k\in \nu (i)} \sum _{j\in \nu (i)\cap \kappa (k)} \varvec{I}_{k} \varvec{s}_j \dot{q}_j \big ) \nonumber \\= & {} \varvec{s}_i^\mathrm {T}\sum _{k\in \nu (i)} \varvec{I}_{k} \sum _{j\in \kappa (k)} \varvec{s}_j \dot{q}_j \,. \end{aligned}$$

(48)

The step from the second to the third line works as follows: $\sum _{j\in \nu (i)}\sum _{k\in \nu (j)}$ is the sum over all j, k pairs where j is a descendant of i and k is a descendant of j, whereas $\sum _{k\in \nu (i)}\sum _{j\in \nu (i)\cap \kappa (k)}$ is the sum over all j, k pairs where k is a descendant of i, and j is both a descendant of i and an ancestor of k; but these two sets of pairs are the same. The step from the third to the fourth line exploits the fact that $\nu (i)\cap \kappa (k)$ is the set of all ancestors of body k from i onwards, whereas $\bar{\kappa }(i)$ is the set of all ancestors of body k prior to i, so the union of the two sets is $\kappa (k)$.

Now let $\varvec{v}_k$ be the velocity of body k, let $\varvec{h}_k=\varvec{I}_{k}\varvec{v}_k$ be the momentum of body k, and let $\varvec{h}_{\nu (i)}=\sum _{k\in \nu (i)}\varvec{h}_k$ be the total momentum of the subtree beginning at body i. The velocity of any body in a rigid-body system is the sum of the joint velocities along any one path between that body and the base, so $\varvec{v}_k=\sum _{j\in \kappa (k)}\varvec{s}_j\dot{q}_j$. We can now further simplify Eq. 48 as follows:

$$\begin{aligned} p_i \,=\, \varvec{s}_i^\mathrm {T}\!\! \sum _{k\in \nu (i)} \!\! \varvec{I}_{k} \varvec{v}_k \,=\, \varvec{s}_i^\mathrm {T}\!\! \sum _{k\in \nu (i)} \!\! \varvec{h}_k \,=\, \varvec{s}_i^\mathrm {T}\varvec{h}_{\nu (i)} \,, \end{aligned}$$

(49)

which establishes the desired result for the case of a kinematic tree. If the system contains kinematic loops then we find that Eq. 49 no longer holds for all joints, but does still hold for any joint that is not involved in any kinematic loop. This is equivalent to the condition stated at the beginning that the subsystem consisting of the bodies in $\nu (i)$ be self-contained.