Feedback Control of Inverted Pendulums

Abstract

Modeling and control of a humanoid robot as a simple inverted pendulum is a common approach. On the other hand, a real walking robot has a floating rigid body dynamics under the unidirectional force constraints. This discrepancy can cause many “unexpected phenomenons” in experiments of real walking robots. In this chapter, we examine this problem by using a simple model and show a practical technique to solve it.

Appendix: Derivation of the General Equation of Motion for Planer Mechanism

In this appendix, we explain the derivation of an equation of motion of the form of (5). As a specific example, we take the model of biped illustrated in Fig. 13; however, we can apply the same method for a general planer link mechanism [4].

Fig. 13

A model of biped

1.1 Kinematics

An instantaneous pose of the biped of Fig. 13 can be represented by its hip position (x, z) and the link inclinations θ := [θ₁, θ₂, …, θ₅]^T as shown in the left. Let us use these variables to represent the center of mass (CoM) positions of the links The horizontal components are given as

$$\displaystyle \begin{aligned} \left[ \begin{array}{c} x_1 \\ x_2 \\ x_3 \\ x_4 \\ x_5 \end{array} \right] = \left[ \begin{array}{ccccc} a_1 & 0 & 0 & 0 & 0 \\ 0 & a_2-r_2 & 0 & 0 & 0 \\ 0 & -r_2 & a_3-r_3 & 0 & 0 \\ 0 & 0 & 0 & a_4-r_4 & 0 \\ 0 & 0 & 0 & -r_4 & a_5-r_5 \end{array} \right] \left[ \begin{array}{c} \sin \theta_1 \\ \sin \theta_2 \\ \sin \theta_3 \\ \sin \theta_4 \\ \sin \theta_5 \end{array} \right] + \left[ \begin{array}{c} 1 \\ 1 \\ 1 \\ 1 \\ 1 \end{array} \right] x \end{aligned} $$

(29)

where x_i is the horizontal position of the CoM of the i-th link (i = 1, 2, 3, 4, 5). The parameter r_i is the length of the i-th link and a_i is the relative CoM position in the link as illustrated in the right of Fig. 13. Let us rewrite this equation into the following simpler form:

$$\displaystyle \begin{aligned} \boldsymbol{x} = \boldsymbol{\varPsi} \boldsymbol{s} + \boldsymbol{1} x \end{aligned} $$

(30)

We introduce the following new vectors and matrix:

$$\displaystyle \begin{aligned} \begin{array}{rcl} \boldsymbol{x} &\displaystyle :=&\displaystyle [x_1 \ x_2 \ x_3 \ x_4 \ x_5]^T, \\ \boldsymbol{\varPsi} &\displaystyle :=&\displaystyle \left[ \begin{array}{ccccc} a_1 &\displaystyle 0 &\displaystyle 0 &\displaystyle 0 &\displaystyle 0 \\ 0 &\displaystyle a_2-r_2 &\displaystyle 0 &\displaystyle 0 &\displaystyle 0 \\ 0 &\displaystyle -r_2 &\displaystyle a_3-r_3 &\displaystyle 0 &\displaystyle 0 \\ 0 &\displaystyle 0 &\displaystyle 0 &\displaystyle a_4-r_4 &\displaystyle 0 \\ 0 &\displaystyle 0 &\displaystyle 0 &\displaystyle -r_4 &\displaystyle a_5-r_5 \end{array} \right] \\ \boldsymbol{s} &\displaystyle :=&\displaystyle [\sin \theta_1 \ \sin \theta_2 \ \sin \theta_3 \ \sin \theta_4 \ \sin \theta_5]^T \\ \boldsymbol{1} &\displaystyle :=&\displaystyle [1 \ 1 \ 1 \ 1 \ 1]^T \end{array} \end{aligned} $$

The matrix Ψ represents the structure of the mechanism.

In the same way, we can represent the vector of vertical CoM position z.

$$\displaystyle \begin{aligned} \boldsymbol{z} = \boldsymbol{\varPsi} \boldsymbol{c} + \boldsymbol{1} z \end{aligned} $$

(31)

where

$$\displaystyle \begin{aligned} \begin{array}{rcl} \boldsymbol{z} &\displaystyle :=&\displaystyle [z_1 \ z_2 \ z_3 \ z_4 \ z_5]^T, \\ \boldsymbol{c} &\displaystyle :=&\displaystyle [\cos \theta_1 \ \cos \theta_2 \ \cos \theta_3 \ \cos \theta_4 \ \cos \theta_5]^T \end{array} \end{aligned} $$

1.2 CoM Speed

We can obtain the horizontal CoM speed from the time derivative of the equation (30):

$$\displaystyle \begin{aligned} \dot{\boldsymbol{x}} = \boldsymbol{\varPsi} \dot{\boldsymbol{s}} + \boldsymbol{1} \dot x \end{aligned} $$

(32)

The time derivative of the vector of sin functions can be expressed as

$$\displaystyle \begin{aligned} \dot{\boldsymbol{s}} = \left[ \begin{array}{c} \dot \theta_1 \cos \theta_1 \\ \dot \theta_2 \cos \theta_2 \\ \vdots \\ \dot \theta_5 \cos \theta_5 \end{array} \right] = \left[ \begin{array}{cccc} \cos \theta_1 & 0 & \hdots & 0 \\ 0 & \cos \theta_2 & \hdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \hdots & \cos \theta_5 \end{array} \right] \left[ \begin{array}{c} \dot \theta_1 \\ \dot \theta_2 \\ \vdots \\ \dot \theta_5 \end{array} \right] =: \boldsymbol{C} \dot{\boldsymbol{\theta}} \end{aligned} $$

(33)

where we defined

$$\displaystyle \begin{aligned} \boldsymbol{C} := \left[ \begin{array}{cccc} \cos \theta_1 & 0 & \hdots & 0 \\ 0 & \cos \theta_2 & \hdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \hdots & \cos \theta_5 \end{array} \right], \quad\dot{\boldsymbol{\theta}} := \left[ \begin{array}{c} \dot\theta_1 \\ \dot\theta_2 \\ \vdots \\ \dot\theta_5 \end{array} \right]. \end{aligned} $$

(34)

By substituting (33) into (32) it is obtained

$$\displaystyle \begin{aligned} \dot{\boldsymbol{x}} = \boldsymbol{\varPsi} \boldsymbol{C} \dot{\boldsymbol{\theta}} + \boldsymbol{1} \dot x. \end{aligned} $$

(35)

For the vertical CoM speeds, it can be obtained the similar equation from the time derivative of the equation (31):

$$\displaystyle \begin{aligned} \dot{\boldsymbol{z}} = - \boldsymbol{\varPsi} \boldsymbol{S} \dot{\boldsymbol{\theta}} + \boldsymbol{1} \dot z \end{aligned} $$

(36)

where the newly introduced matrix is given as

$$\displaystyle \begin{aligned} \boldsymbol{S} := \left[ \begin{array}{cccc} \sin \theta_1 & 0 & \hdots & 0 \\ 0 & \sin \theta_2 & \hdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \hdots & \sin \theta_5 \end{array} \right]. \end{aligned} $$

(37)

1.3 Derivation of Equation of Motion

The total kinetic energy T of the robot can be calculated by using the CoM speeds defined by (35) and (36) as

$$\displaystyle \begin{aligned} T = \frac{1}{2} (\dot{\boldsymbol{x}}^T \boldsymbol{M} \dot{\boldsymbol{x}} + \dot{\boldsymbol{z}}^T \boldsymbol{M} \dot{\boldsymbol{z}} + \dot{\boldsymbol{\theta}}^T \boldsymbol{I} \dot{\boldsymbol{\theta}}), \end{aligned} $$

(38)

where the matrices M and I are defined as

$$\displaystyle \begin{aligned} \boldsymbol{M} := \left[ \begin{array}{cccc} m_1 & 0 & 0 & 0 \\ 0 & m_2 & 0 & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \hdots & m_5 \end{array} \right] , \quad\boldsymbol{I} := \left[ \begin{array}{cccc} I_1 & 0 & 0 & 0 \\ 0 & I_2 & 0 & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \hdots & I_5 \end{array} \right], \end{aligned} $$

(39)

where I₁…I₅ are the moment of inertia of the corresponding links.

The total potential energy of the robot is defined as

$$\displaystyle \begin{aligned} U = g \boldsymbol{m}^T \boldsymbol{z}, \end{aligned} $$

(40)

where g is the gravitational acceleration constant and the law vector m^T is defined as

$$\displaystyle \begin{aligned} \boldsymbol{m}^T := [ m_1 \; m_2 \; m_3 \; m_4 \; m_5 ].\end{aligned} $$

(41)

The equation of motion can be derived from the total kinetic energy T and the total potential energy U by using Lagrange’s method. The equations of motion for the translational components are given as

$$\displaystyle \begin{aligned} \begin{array}{rcl} \frac{d}{dt} \left( \frac{\partial T}{\partial \dot{x}} \right) - \frac{\partial T}{\partial x} + \frac{\partial U}{\partial x} = f_{x} \end{array} \end{aligned} $$

(42)

$$\displaystyle \begin{aligned} \begin{array}{rcl} \frac{d}{dt} \left( \frac{\partial T}{\partial \dot{z}} \right) - \frac{\partial T}{\partial z} + \frac{\partial U}{\partial z} = f_{z} \end{array} \end{aligned} $$

(43)

where (f_x, f_z) are the generalized force which is the external force acting at the point (x, z).

The equation of motion for the rotational components is given as

$$\displaystyle \begin{aligned} \frac{d}{dt} \left( \frac{\partial T}{\partial \dot{\boldsymbol{\theta}}} \right) - \frac{\partial T}{\partial \boldsymbol{\theta}} + \frac{\partial U}{\partial \boldsymbol{\theta}} = \boldsymbol{\tau}, \end{aligned} $$

(44)

where τ is the generalized torque which is the torque acting on the joints from the absolute frame.

By substituting (38), (40) into (42), (43), and (44), the final equation of motion is obtained as

(45)

where the following variables are newly defined :

$$\displaystyle \begin{aligned} \begin{array}{rcl} \boldsymbol{X} &\displaystyle :=&\displaystyle \boldsymbol{\varPsi} \boldsymbol{S} \end{array} \end{aligned} $$

(46)

$$\displaystyle \begin{aligned} \begin{array}{rcl} \boldsymbol{Z} &\displaystyle :=&\displaystyle \boldsymbol{\varPsi} \boldsymbol{C} \end{array} \end{aligned} $$

(47)

$$\displaystyle \begin{aligned} \begin{array}{rcl} \boldsymbol{A} &\displaystyle :=&\displaystyle \boldsymbol{X}^T \boldsymbol{M} \boldsymbol{X} + \boldsymbol{Z}^T \boldsymbol{M} \boldsymbol{Z} + \boldsymbol{I} \end{array} \end{aligned} $$

(48)

$$\displaystyle \begin{aligned} \begin{array}{rcl} \boldsymbol{B} &\displaystyle :=&\displaystyle \boldsymbol{X}^T \boldsymbol{M} \boldsymbol{Z} - \boldsymbol{Z}^T \boldsymbol{M} \boldsymbol{X} \end{array} \end{aligned} $$

(49)

$$\displaystyle \begin{aligned} \begin{array}{rcl} \dot{\boldsymbol{\theta}}^2 &\displaystyle :=&\displaystyle [\dot\theta_1^2 \; \dot\theta_2^2 \; \hdots \; \dot\theta_5^2]^2 \end{array} \end{aligned} $$

(50)