Angular momentum based balance controller for an under-actuated planar robot

Abstract

In this paper, a new control algorithm based on angular momentum is presented for balancing an under-actuated planar robot. The controller is able to stabilize the robot in any unstable balanced configuration in which the robot is controllable, and also it is able to follow a class of arbitrary trajectories without losing balance. Simulation results show the good performance of the controller in balancing and trajectory tracking motions of the robot. The simulations also show that the proposed controller is robust to significant imperfections in the system, such as errors in the controller’s dynamic model of the robot and imperfections in the sensors and actuators. The new controller is compared with three existing balance controllers and is shown to equal or outperform them.

Appendices

Appendix I

This appendix provides the analytical expression of $\dot{\eta }=h(\eta )$ and its linearized equation about $\eta =0$. To express $\dot{\eta } = (\dot{q}_1, \dot{q}_2, \ddot{q}_1, \ddot{q}_2)$ as a function of $\eta$, it is required to compute $\ddot{q}_1$ and $\ddot{q}_2$ in terms of q and $\dot{q}$. Thus, we solve the motion Equation in (1) and (2) for $\ddot{q}_1$ and $\ddot{q}_2$ and obtain

$$\begin{aligned} \ddot{q}_1= & {} \frac{1}{d} \Big ( c_2c_3\sin (q_2)(\dot{q}_1+\dot{q}_2)^2 + c_3^2 \sin (q_2)\cos (q_2)\dot{q}_1^2 \nonumber \\&-\;c_2c_4g\cos (q_1) + c_3c_5g\cos (q_2)\cos (q_1+q_2) \nonumber \\&-\;\tau (c_2+c_3\cos (q_2)) \Big ) \, , \end{aligned}$$

(28)

$$\begin{aligned} \ddot{q}_2= & {} \frac{1}{d} \Big ( -c_3\sin (q_2)(c_2+c_3\cos (q_2))(\dot{q}_1+\dot{q}_2)^2 \nonumber \\&-\;c_3\sin (q_2)(c_1+c_3\cos (q_2))\dot{q}_1^2 \nonumber \\&+\;c_4g\cos (q_1)(c_2+c_3\cos (q_2))\nonumber \\&-\;c_5g\cos (q_1+q_2)(c_1+c_3\cos (q_2))\nonumber \\&+\; \tau (c_1+c_2+2c_3\cos (q_2)) \Big ) \, , \end{aligned}$$

(29)

where $d = c_1c_2-c_3^2\cos (q_2)^2$ is the determinant of the joint-space inertia matrix. Then, we replace $\tau$ from (12) into the above equations and obtain

$$\begin{aligned} \ddot{q}_1= & {} \frac{1}{d} \Big ( c_2c_3\sin (q_2)(\dot{q}_1+\dot{q}_2)^2 + c_3^2 \sin (q_2)\cos (q_2)\dot{q}_1^2 \nonumber \\&-\;c_2c_4g\cos (q_1) - c_2c_5g\cos (q_1+q_2)\nonumber \\&-\;(c_2+c_3\cos (q_2)) \Big ( k_s(q_2^d-q_2) \nonumber \\&+\;k_p((c_1\,{+}\,c_2\,{+}\,2c_3\cos (q_2))\dot{q}_1 \,{+}\, (c_2\,{+}\,c_3\cos (q_2))\dot{q}_2) \nonumber \\&+\;k_{dd}g(c_4\sin (q_1)\dot{q}_1 + c_5\sin (q_1+q_2) (\dot{q}_1+\dot{q}_2)) \nonumber \\&-\; k_dg(c_4\cos (q_1)+c_5\cos (q_1+q_2)) \Big ) \Big ) , \end{aligned}$$

(30)

and

$$\begin{aligned} \ddot{q}_2= & {} \frac{1}{d} \Big ( -c_3\sin (q_2)(c_2+c_3\cos (q_2))(\dot{q}_1+\dot{q}_2)^2 \nonumber \\&-\;c_3\sin (q_2)(c_1+c_3\cos (q_2))\dot{q}_1^2 \nonumber \\&+\;g(c_2+c_3\cos (q_2))(c_4\cos (q_1)+c_5\cos (q_1+q_2)) \nonumber \\&+\;(c_1+c_2+2c_3\cos (q_2)) \Big ( k_s(q_2^d-q_2) \nonumber \\&+\;k_p((c_1\,{+}\,c_2\,{+}\,2c_3\cos (q_2))\dot{q}_1 \,{+}\, (c_2\,{+}\,c_3\cos (q_2))\dot{q}_2) \nonumber \\&+\;k_{dd}g(c_4\sin (q_1)\dot{q}_1 + c_5\sin (q_1+q_2) (\dot{q}_1+\dot{q}_2)) \nonumber \\&-\;k_dg(c_4\cos (q_1)+c_5\cos (q_1+q_2)) \Big ) \Big ) \, . \end{aligned}$$

(31)

To calculate matrix A, we linearize (30) and (31) about the equilibrium point $\eta =0$. Note that, in the equilibrium point, we have $X=0$ and therefore,

$$\begin{aligned} c_4 \cos \Big (q_1^d \Big ) + c_5 \cos \Big (q_1^d+q_2^d \Big ) = 0 \, . \end{aligned}$$

(32)

Hence, $A=\frac{\partial {h}}{\partial {\eta }}|_{\eta =0}$ will be

$$\begin{aligned} A = \begin{bmatrix} 0&0&1&0\\&&\\ 0&0&0&1\\&&\\ \frac{\partial {\ddot{q}_1}}{\partial {q_1}}|_{\eta =0}&\frac{\partial {\ddot{q}_1}}{\partial {q_2}}|_{\eta =0}&\frac{\partial {\ddot{q}_1}}{\partial {\dot{q}_1}}|_{\eta =0}&\frac{\partial {\ddot{q}_1}}{\partial {\dot{q}_2}}|_{\eta =0} \\&&\\ \frac{\partial {\ddot{q}_2}}{\partial {q_1}}|_{\eta =0}&\frac{\partial {\ddot{q}_2}}{\partial {q_2}}|_{\eta =0}&\frac{\partial {\ddot{q}_2}}{\partial {\dot{q}_1}}|_{\eta =0}&\frac{\partial {\ddot{q}_2}}{\partial {\dot{q}_2}}|_{\eta =0} \\ \end{bmatrix} \, , \end{aligned}$$

(33)

where

$$\begin{aligned} \frac{\partial {\ddot{q}_1}}{\partial {q_1}}|_{\eta =0}= & {} \frac{\gamma _2}{a} \Big ( k_d \Big (c_2+c_3\cos \Big (q_2^d \Big )\Big ) - c_2 \Big )\\ \frac{\partial {\ddot{q}_1}}{\partial {q_2}}|_{\eta =0}= & {} \frac{1}{a} \Big ( -k_dg \Big (c_2+c_3\cos \Big (q_2^d \Big ) \Big ) c_5\sin \Big (q_1^d+q_2^d \Big ) \\&+\;\!k_s\Big (c_2\!+\!c_3\cos \Big (q_2^d \Big )\Big ) \!+\! c_2c_5g\!\sin \Big (q_1^d\!+\!q_2^d \Big ) \Big ) \\ \frac{\partial {\ddot{q}_1}}{\partial {\dot{q}_1}}|_{\eta =0}= & {} \frac{1}{a} \Big (k_{dd} \gamma _2 - k_p \gamma _1 \Big ) \Big (c_2+c_3\cos (q_2^d)\Big ) \\ \frac{\partial {\ddot{q}_1}}{\partial {\dot{q}_2}}|_{\eta =0}= & {} -\frac{1}{a} \Big ( k_p \Big (c_2+c_3\cos \Big (q_2^d \Big )\Big )^2 \\&+\;k_{dd}g \Big (c_2+c_3\cos \Big (q_2^d \Big )\Big )c_5\sin \Big (q_1^d+q_2^d \Big ) \Big ) \\ \frac{\partial {\ddot{q}_2}}{\partial {q_1}}|_{\eta =0}= & {} \frac{\gamma _2}{a} \Big (c_2 + c_3\cos \Big (q_2^d \Big ) - k_d\gamma _1 \Big ) \\ \frac{\partial {\ddot{q}_2}}{\partial {q_2}}|_{\eta =0}= & {} \frac{1}{a} \Big ( (k_d g c_5\sin \Big (q_1^d+q_2^d \Big ) - k_s \Big ) \gamma _1 \\&- \Big (c_2+c_3\cos \Big (q_2^d \Big ) \Big )c_5g\sin \Big (q_1^d+q_2^d \Big ) \Big ) \\ \frac{\partial {\ddot{q}_2}}{\partial {\dot{q}_1}}|_{\eta =0}= & {} \frac{\gamma _1}{a} \Big (k_p \gamma _1 - k_{dd} \gamma _2 \Big ) \\ \frac{\partial {\ddot{q}_2}}{\partial {\dot{q}_2}}|_{\eta =0}= & {} \frac{\gamma _1}{a} \Big ( k_p (c_2+c_3\cos \Big (q_2^d \Big ) \Big )\\&+\;k_{dd}gc_5\sin \Big (q_1^d+q_2^d \Big ) \Big ) \end{aligned}$$

Appendix II

This appendix proves the statement that at any desired unstable balanced configuration of the 2D balancer robot other than a neutral balanced configuration, $K=0$ if and only if the velocity gain ($G_V$) is zero. Neutral balanced configurations are excluded because $G_V$ as defined in Featherstone (2012) is not defined at such configurations.

The balance condition in (10) can be written in the form

$$\begin{aligned} r \cos (\phi ) \cos \Big (q_1^d \Big ) - r \sin (\phi ) \sin \Big (q_1^d \Big ) = 0, \end{aligned}$$

(34)

where

$$\begin{aligned} r \cos (\phi ) = c_4 + c_5 \cos \Big (q_2^d \Big ) \end{aligned}$$

and

$$\begin{aligned} r \sin (\phi ) = c_5 \sin \Big (q_2^d \Big ) \,, \end{aligned}$$

which implies

$$\begin{aligned} r = \Big ( c_4^2 + c_5^2 + 2 c_4 c_5 \cos \Big (q_2^d \Big ) \Big )^{1/2} \,. \end{aligned}$$

Note that $r=0$ only if $c_4=c_5$ and $\cos (q_2^d)=-1$, which is the condition for neutral balance, so we can exclude this possibility from consideration. Given that $r\ne 0$, the balance condition in (34) simplifies to

$$\begin{aligned} \cos \Big (q_1^d+\phi \Big ) = 0 \,. \end{aligned}$$

This equation has two solutions, but the correct solution for an unstable balanced configuration is $q_1^d=\pi /2-\phi$. From this solution we get

$$\begin{aligned} \sin \Big (q_1^d \Big ) = \cos (\phi ) = \Big (c_4 + c_5 \cos \Big (q_2^d \Big ) \Big ) / r \end{aligned}$$

(35)

and

$$\begin{aligned} \sin \Big (q_1^d+q_2^d \Big )= & {} \cos (\phi ) \cos \Big (q_2^d \Big ) + \sin (\phi ) \sin \Big (q_2^d \Big ) \nonumber \\= & {} \Big (c_5 + c_4 \cos \Big (q_2^d \Big ) \Big ) / r \,. \end{aligned}$$

(36)

Substituting (35) and (36) into the expression for K in (14), and simplifying the result, gives

$$\begin{aligned} r K = c_2 c_4^2 - c_1 c_5^2 + \Big ( c_3 \Big (c_4^2-c_5^2\Big ) + \Big (c_2-c_1\Big ) c_4 c_5\Big ) \cos \Big (q_2^d\Big ) \,. \end{aligned}$$

(37)

According to Eq. 5 in Featherstone (2012), the velocity gain for a 2D balancer is

$$\begin{aligned} G_V(q_2) = \frac{c_y^2 + \frac{m_2\,c_x\,c_{2x}}{m_1+m_2}}{c_x^2+c_y^2} - \frac{H_{12}}{H_{11}} \, , \end{aligned}$$

(38)

where

$$\begin{aligned} H_{11}= & {} c_1+c_2+2c_3\cos (q_2) \, ,\\ H_{12}= & {} c_2+c_3\cos (q_2) \, ,\\ c_x= & {} (c_4+c_5\cos (q_2))/c \, ,\\ c_y= & {} c_5 \sin (q_2)/c \,,\\ c_{2x}= & {} l_{c2} \cos (q_2) \quad \text{ and } \quad c_{2y} = l_{c2} \sin (q_2) \, . \end{aligned}$$

So $G_V$ at a desired configuration can be written as

$$\begin{aligned} G_V(q_2^d)= & {} \frac{c_5^2+c_4c_5\cos \Big (q_2^d \Big )}{c_4^2+c_5^2+2c_4c_5\cos \Big (q_2^d \Big )}\nonumber \\&- \frac{c_2+c_3\cos \Big (q_2^d \Big )}{c_1+c_2+2c_3\cos \Big (q_2^d \Big )} \, . \end{aligned}$$

(39)

Note that the first denominator is nonzero because it is equal to $r^2$, and the second denominator is nonzero because it is the first element of the robot’s joint-space inertia matrix, which is a positive-definite matrix. If we multiply both sides of (39) by both denominators then we get, after some simplification,

$$\begin{aligned} r^2 H_{11} G_V(q_2^d)= & {} c_1 c_5^2 - c_2 c_4^2 \nonumber \\&+ \cos \Big (q_2^d \Big ) \Big ( c_3 \Big (c_5^2-c_4^2 \Big ) \nonumber \\&+ \Big (c_1-c_2 \Big ) c_4 c_5 \Big ) \,. \end{aligned}$$

(40)

On comparing (40) with (37) it can be seen that $K=-rH_{11}G_V$, which proves that $K=0$ if and only if $G_V=0$.