Decomposition and Cross-Product-Based Method for Computing the Dynamic Equation of Robots

Abstract

This paper aims to demonstrate a clear relationship between Lagrange equations and Newton-Euler equations regarding computational methods for robot dynamics, from which we derive a systematic method for using either symbolic or on-line numerical computations. Based on the decomposition approach and cross-product operation, a computing method for robot dynamics can be easily developed. The advantages of this computing framework are that: it can be used for both symbolic and on-line numeric computation purposes, and it can also be applied to biped systems, as well as some simple closed-chain robot systems.

Keywords

Newton-Euler equation Lagrange formulation robotic dynamic equation symbolic computation biped robot

1. Introduction

When looking at robotics research, dynamic modelling is basically a form of artwork. The complexity of a dynamic model concerns the degrees of freedom (DOFs) of a machine or a robot and the dynamic model of a low DOFs robot can be easily developed, but dynamic models for a high DOFs robot are difficult to derive by hand. The drawbacks to creating the models by hand are: difficulty of maintenance, complexity of proofreading the dynamic model and inflexibility. The biped robot is the target of a complex dynamic system study [1] that also reveals a demand for a computer-aided systematic derivation tools for robotics.

Using an auto-deriving tool to obtain a dynamic model of a robot is very economical. Tošic [2] proposed the benefits of a symbolic simulation of an engineering system, while Cetinkunt [3] proposed a symbolic modelling of the dynamics of robotic manipulators on the numerical tool REDUCE. Since the coding of REDUCE is based on C and Fortran, modifying the system model by simply changing the symbol is rather difficult. Vibet demonstrated the symbolic tool FORM [4], which automatically generates the dynamic equations of the manipulator. The MACSYMA tool was then introduced to mechanical engineering education [5], providing the steps for determining a system dynamics model, but it does not support online simulations that integrate both system models and control rules.

Numerous contributions in both algorithms and their computational efficiency have been made in the field of robot dynamics [6 –9]. The need for an online computing [6] method is motivated by simulation studies of the dynamics of a robotic system with large DOFs. While computation efficiency continues to be crucial for the simulation and control of increasingly complex mechanisms operating at higher speeds, other aspects of the dynamics problem are also vital. Algorithms should be formulated under a simple framework to enable ease of development, modification, extension and understanding.

This paper aims to demonstrate a clear relationship between Lagrange equations and Newton-Euler equations regarding the computational methods for robot dynamics, from which we derive a systematic method using either symbolic or on-line numerical computations. First, this study reviews the Lagrange formulation, as well as its symbolic generation method, which is suitable for studying a low-DOF robotic system. Second, based on the decomposition approach and cross-product operation, a computing framework can be easily developed to calculate the inertia matrix, the Coriolis and centrifugal matrix, and the gravity force vector of robot dynamics equations. The advantages of this computing method are that: it can be used for both symbolic and the on-line numeric computation purposes, and it also can be applied to a closed-chain robot system. We present several robotic system examples to demonstrate the versatility of the proposed method.

2. Decomposition method

Assume that a robot system has a generalized coordinate with n DOFs and let q = [q₁ q₂ ṫ q_n]^T. The equation of robot motion [9] can be written as

D (q) \overset{..}{q} + C (q, \dot{q}) \dot{q} + G (q) = u,

(1)

where the inertia matrix D is an n×n positive-definite symmetric matrix, C is an n×n Coriolis and centrifugal matrix, and G is an n×1 gravity force vector. The control input u = u₁ u₂ ṫ u_n]^T is the vector of joint input forces or torques, and u_i ≡ 0, if the i-th joint is not actuated. Significantly, C is not unique, and S = Ḋ − 2C is a skew-symmetric matrix caused by the null energy property.

2.1 Lagrange formulation approach

The total kinetic energy K of a robot consists of the rotational kinetic energy K_ωi and the translational kinetic energy K_vi of each link. The total potential energy P consists of the potential energy P_i of each link; and therefore,

K = \sum_{i = 1}^{n} K_{ω ​ i} + \sum_{i = 1}^{n} K_{v i},

(2)

and

P = \sum_{i = 1}^{n} P_{i} .

(3)

The kinetic energies K_ωi and K_vi, and potential energy P_i of each link can be calculated as stated below. Let m_i be the mass of link i and I_i be the inertia of link i expressed in the body-attached frame. Let R_i be the rotational matrix, ω_i be the angular velocity and p_ci be the CoM (Centre of Mass) of link i. The angular velocity of link i can thus be expressed as

ω_{i} = J_{ω ​ i} ​ \dot{q},

(4)

where $J_{ω i} = \frac{\partial ω_{i}}{\partial \dot{q}}$ is the angular Jacobian matrix of link i, and the rotational kinetic energy of link i can be obtained through

K_{ω ​ i} = \frac{1}{2} ω_{i}^{T} (R_{i} I_{i} R_{i}^{T}) ​ ω_{i} = \frac{1}{2} {\dot{q}}^{T} (J_{ω ​ i}^{T} R_{i} I_{i} R_{i}^{T} J_{ω ​ i}) ​ \dot{q} .

(5)

The potential energy of link i can be obtained by

P_{i} = m_{i} g ​ p_{c i}^{T} e_{3},

(6)

where g is the acceleration of gravity and e₃ = [0 0 1]^T. The velocity of the CoM of link i can be computed by

{\dot{p}}_{c i} = J_{v i} \dot{q},

(7)

where $J_{v i} = \frac{\partial p_{c i}}{\partial q}$ is the linear Jacobian matrix of link i, and the translational energy of link i is via

K_{v i} = \frac{1}{2} m_{i} {\dot{p}}_{c i}^{T} {\dot{p}}_{c i} = \frac{1}{2} {\dot{q}}^{T} (m_{i} J_{v ​ i}^{T} J_{v ​ i}) ​ \dot{q}

(8)

From the Lagrange formulation, we have

\begin{array}{c} D_{ω ​ i} = \frac{\partial}{\partial \dot{q}} \frac{\partial K_{ω ​ i}}{\partial \dot{q}} \\ C_{ω ​ i} = \frac{\partial}{\partial q} \frac{\partial K_{ω ​ i}}{\partial \dot{q}} - \frac{1}{2} \frac{\partial}{\partial q} (D_{ω ​ i} \dot{q}) \end{array}

and

\begin{array}{c} D_{v i} = \frac{\partial}{\partial \dot{q}} \frac{\partial K_{v i}}{\partial \dot{q}} \\ C_{v i} = \frac{\partial}{\partial q} \frac{\partial K_{v i}}{\partial \dot{q}} - \frac{1}{2} \frac{\partial}{\partial q} (D_{v i} \dot{q}) \\ G_{i} = \frac{\partial P_{i}}{\partial q} . \end{array}

The D, C and G matrices of the robot's equations of motion are then

D = \sum_{i = 1}^{n} D_{ω ​ i} + \sum_{i = 1}^{n} D_{v i}

(9)

C = \sum_{i = 1}^{n} C_{ω ​ i} + \sum_{i = 1}^{n} C_{v i}

(10)

and

G = \sum_{i = 1}^{n} G_{i} .

(11)

2.2 Newton-Euler formulation approach

The decomposition of the robot's equations of motion can also be conducted using the Newton-Euler approach. The angular acceleration of link i is expressed as

{\dot{ω}}_{i} = J_{ω ​ i} ​ \overset{..}{q} + {\dot{J}}_{ω ​ i} ​ ​ \dot{q},

(12)

where ${\dot{J}}_{ω i} = \frac{\partial}{\partial q} (J_{ω i} \dot{q})$ is the differentiation of the angular Jacobian matrix J_ωi. The Euler equation of link i is

R_{i} I_{i} R_{i}^{T} {\dot{ω}}_{i} + ω_{i} \times (R_{i} I_{i} R_{i}^{T}) ​ ω_{i} = ​ τ_{i},

(13)

and hence,

R_{i} I_{i} R_{i}^{T} (J_{ω ​ i} ​ \overset{..}{q} + {\dot{J}}_{ω ​ i} ​ \dot{q}) + ω_{i} \times (R_{i} I_{i} R_{i}^{T}) ​ ω_{i} = ​ τ_{i} .

Since $J_{ω i}^{T} τ_{i} = u$ , we now have the equation

\begin{array}{l} J_{ω ​ i}^{T} R_{i} I_{i} R_{i}^{T} J_{ω ​ i} ​ \overset{..}{q} + J_{ω ​ i}^{T} R_{i} I_{i} R_{i}^{T} {\dot{J}}_{ω ​ i} ​ \dot{q} + \\ + J_{ω ​ i}^{T} ω_{i} \times (R_{i} I_{i} R_{i}^{T}) J_{ω ​ i} \dot{q} = J_{ω ​ i}^{T} ​ τ_{i} = u' \end{array}

which is the same as

D_{ω ​ i} \overset{..}{q} + C_{ω ​ i} ​ \dot{q} = u .

Therefore, D_ωi and C_ωi can be obtained respectively using:

D_{ω ​ i} = J_{ω ​ i}^{T} (R_{i} I_{i} R_{i}^{T}) J_{ω ​ i}

(14)

and

C_{ω ​ i} = J_{ω ​ i}^{T} (R_{i} I_{i} R_{i}^{T}) {\dot{J}}_{ω ​ i} + J_{ω ​ i}^{T} [ω_{i} \times] ​ (R_{i} I_{i} R_{i}^{T}) J_{ω ​ i},

(15)

where $[\begin{matrix} x \\ y \\ z \end{matrix}] \times \equiv [\begin{matrix} 0 & - z & y \\ z & 0 & - x \\ - y & x & 0 \end{matrix}]$ is the skew-symmetric matrix representation of the cross-product operation.

The acceleration of CoM of link i is

{\overset{..}{p}}_{c i} = J_{v i} \overset{..}{q} + {\dot{J}}_{v i} ​ \dot{q},

(16)

where ${\dot{J}}_{v i} = \frac{\partial}{\partial q} (J_{v i} \dot{q})$ is the differentiation of the linear Jacobian matrix J_vi. The Newton equation of the CoM of link i is

m_{i} {\overset{..}{p}}_{c i} + m_{i} g ​ e_{3} = f_{i}

(17)

and hence,

m_{i} (J_{v i} \overset{..}{q} + {\dot{J}}_{v i} ​ \dot{q}) + m_{i} g e_{3} = f_{i} .

Since $J_{v i}^{T} f_{i} = u$ , we formulate the equation

m_{i} J_{v i}^{T} J_{v i} \overset{..}{q} + m_{i} J_{v i}^{T} {\dot{J}}_{v i} ​ \dot{q} + m_{i} g J_{v i}^{T} e_{3} = J_{v i}^{T} f_{i} = u,

which is also the same as

D_{v i} \overset{..}{q} + C_{v i} ​ \dot{q} + G_{i} = u .

Therefore, the D_vi, C_vi and G_i matrices can be obtained by

D_{v i} = m_{i} J_{v i}^{T} J_{v ​ i}

(18)

C_{v i} = m_{i} J_{v i}^{T} {\dot{J}}_{v ​ i}

(19)

and

G_{i} = \frac{\partial P_{i}}{\partial q} .

(20)

2.3 Extended coordinate system

The Cartesian coordinates of the base point, [x₀ y₀ z₀]^T, are frequently added to form an extended set of generalized coordinates for the robot systems. Let q_e = [x₀ y₀ z₀ q₁ ṫ q_n]^T be the extended coordinate system and next define the total mass $M = \sum_{i = 1}^{n} m_{i}$ , Jacobian matrix $J_{C o M} = \frac{1}{M} \sum_{i = 1}^{n} m_{i} J_{v i}$ , and its derivative ${\dot{J}}_{C o M} = \frac{1}{M} \sum_{i = 1}^{n} m_{i} {\dot{J}}_{v i}$ . The robot's equations of motion in this extended coordinate system can thus be expressed as

D_{e} (q_{e}) {\overset{..}{q}}_{e} + C_{e} (q_{e}, {\dot{q}}_{e}) {\dot{q}}_{e} + G_{e} (q_{e}) = B_{e} u,

(21)

where

\begin{array}{l} D_{e} = [\begin{matrix} M ​ I_{3 \times 3} & M ​ J_{C o M}^{​} \\ M ​ J_{C o M}^{T} & D \end{matrix}] \\ C_{e} = [\begin{matrix} 0_{3 \times 3} & M {\dot{J}}_{C o M} \\ 0_{n \times 3} & C \end{matrix}] \\ G_{e} = [\begin{matrix} M e_{3} \\ G \end{matrix}] \end{array}

and

B_{e} = [\begin{matrix} 0_{3 \times n} \\ I_{n \times n} \end{matrix}] .

3. Procedures for computing the dynamic equations of a serial-link robot

Let m_i be the mass of link i, I_i be the inertia of link i expressed in the body-attached frame and then q = [q₁ q₂ ṫ q_n]^T be the joint coordinate vector, where n is the number of joints. For simplicity, it is assumed that the relative motion of link i with respect to link (i-1) is either the rotational (revolute joint) or the translational (prismatic joint) motion with one of the basic axes in the local body frame from the set {± x-axis, ± y-axis, and ± z-axis} and is denoted by symbol ρ_i, where ρ_i ∈ {+Rx, −Rx, +Ry, −Ry, +Rz, −Rz, +Tx, −Tx, +Ty, −Ty, −Tz, −Tz}.

Define three basic rotational matrices: R_x(q_i), R_y(q_i), and R_z(q_i), as shown below

\begin{array}{l} R_{x} (q_{i}) = [\begin{matrix} 1 & 0 & 0 \\ 0 & \cos q_{i} & - \sin q_{i} \\ 0 & \sin q_{i} & \cos q_{i} \end{matrix}] \\ R_{y} (q_{i}) = [\begin{matrix} \cos q_{i} & 0 & \sin q_{i} \\ 0 & 1 & 0 \\ - \sin q_{i} & 0 & \cos q_{i} \end{matrix}] \end{array}

and

R_{z} (q_{i}) = [\begin{matrix} \cos q_{i} & - \sin q_{i} & 0 \\ \sin q_{i} & \cos q_{i} & 0 \\ 0 & 0 & 1 \end{matrix}] .

The procedures for computing the dynamic equation of a serial-link robot are thus as follows:

Step 1: Computing the rotational matrix of link i, R_i, i = 1, 2, ṫ, n

Let R₀ = I_3×3, and

R_{i} = {\begin{matrix} R_{i - 1} R_{x} (\pm q_{i}) & ρ_{i} = \pm R x \\ R_{i - 1} R_{y} (\pm q_{i}) & ρ_{i} = \pm R y \\ R_{i - 1} R_{z} (\pm q_{i}) & ρ_{i} = \pm R z \\ R_{i - 1} & prismatic joint \end{matrix}, i = 1, 2, \dots, n .

(22)

Let a_i and t_i be the vectors of the corresponding rotational and translational axes of link i with respect to link (i-1), and then

a_{i} = {\begin{matrix} \pm R_{i - 1} (:, 1) & ρ_{i} = \pm R x \\ \pm R_{i - 1} (:, 2) & ρ_{i} = \pm R y \\ \pm R_{i - 1} (:, 3) & ρ_{i} = \pm R z \\ 0_{3 \times 1} & prismatic joint \end{matrix}

(23)

and

t_{i} = {\begin{matrix} \pm R_{i - 1} (:, 1) & ρ_{i} = \pm T x \\ \pm R_{i - 1} (:, 2) & ρ_{i} = \pm T y \\ \pm R_{i - 1} (:, 3) & ρ_{i} = \pm T z \\ 0_{3 \times 1} & revolute joint \end{matrix}, i = 1, 2, \dots, n,

(24)

where R(:, k), k = 1,2,3 represents the k-th column of the matrix R.

Step 2: Computing the angular Jacobian matrix of link i, ω_i = J _ωiq̇, i = 1,2, ṫ, n

Let ω₀ = 0_3×1, and

ω_{i} = ω_{i - 1} + a_{i} {\dot{q}}_{i}, i = 1, 2, \dots, n .

(25)

The angular Jacobian matrix of link i, J_ωi, is

\begin{array}{l} J_{ω i} (:, j) = {\begin{array}{l} a_{j} & j \leq i \\ 0_{3 x 1} & j > i \end{array} & ​ \\ ​ & , i, j = 1, 2, ..., n . \end{array}

(26)

where J_ωi (:, j) represents the j-th column of matrix J_ωi and the total rotational kinetic energy K_ω is obtained from

\begin{array}{l} K_{ω} = \sum_{i = 1}^{n} K_{ω ​ i} = \sum_{i = 1}^{n} \frac{1}{2} ω_{i}^{T} R_{i} I_{i} R_{i}^{T} ​ ω_{i} \\ = \frac{1}{2} {\dot{q}}^{T} (\sum_{i = 1}^{n} J_{ω ​ i}^{T} R_{i} I_{i} R_{i}^{T} J_{ω ​ i}) ​ \dot{q} . \end{array}

(27)

Matrix D_ωi is formed by

D_{ω ​ i} = J_{ω ​ i}^{T} R_{i} I_{i} R_{i}^{T} J_{ω ​ i}, i = 1, 2, \dots, n .

Step 3: Differentiation of the angular Jacobian matrices The angular acceleration of link i is obtained as

{\dot{ω}}_{i} = J_{ω ​ i ​} \overset{..}{q} + {\dot{J}}_{ω ​ i} \dot{q} i = 1, 2, \dots, n,

where ${\dot{J}}_{ω i} = \frac{\partial}{\partial q} (J_{ω i} \dot{q}) = \sum_{j = 1}^{n} \frac{\partial J_{ω i}}{\partial q_{j}} {\dot{q}}_{j}$ is found to be

{\dot{J}}_{ω ​ i} (:, j) = {\begin{matrix} \sum_{k = 1}^{j - 1} a_{k} \times a_{j} ​ {\dot{q}}_{k} & j \leq i \\ 0_{3 \times 1} & j > i \end{matrix}, i, j = 1, 2, \dots, n .

(28)

Matrix C_ωi is set up by

C_{ω ​ i} = J_{ω ​ i}^{T} R_{i} I_{i} R_{i}^{T} ​ {\dot{J}}_{ω ​ i} + J_{ω ​ i}^{T} [ω_{i} \times] ​ R_{i} I_{i} R_{i}^{T} J_{ω ​ i},

Step 4: Computing the linear Jacobian matrix of the CoM of link i, ṗ_ci = J_viq̇, i = 1,2, ṫ, n

First, compute the joint position and CoM position of link i, p_i and p_ci, i = 1, 2, ṫ, n. The linear Jacobian matrix of the CoM of link i, $J_{v i} = \frac{\partial p_{c i}}{\partial q}$ , is then found as

J_{v i} (:, j) = {\begin{matrix} {\begin{matrix} a_{j} \times (p_{c i} - p_{j}) & revolute joint \\ t_{j} & prismatic joint \end{matrix} & j \leq i \\ 0_{3 \times 1} & j > i \end{matrix} \begin{array}{l} , i, j = 1, 2, \dots, n . \end{array}

(29)

The total translational kinetic energy K_v and the total potential energy P are obtained as

\begin{array}{l} K_{v} = \sum_{i = 1}^{n} K_{v i} = \frac{1}{2} \sum_{i = 1}^{n} m_{i} {\dot{p}}_{c i}^{T} {\dot{p}}_{c i} \\ = \frac{1}{2} {\dot{q}}^{T} (\sum_{i = 1}^{n} m_{i} J_{v i}^{T} J_{v i}) \dot{q} \end{array}

(30)

and

P = \sum_{i = 1}^{n} P_{i} = \sum_{i = 1}^{n} m_{i} g ​ p_{c i}^{T} e_{3} .

(31)

The matrices D_vi and G_i are set up as

D_{v i} = m_{i} J_{v i}^{T} J_{v i},

and

G_{i} = m_{i} g ​ J_{v i}^{T} e_{3}, i = 1, 2, \dots, n .

Step 5: Differentiation of the linear Jacobian matrices The linear acceleration of the CoM of link i is obtained as

{\overset{..}{p}}_{c i} = J_{v i} \overset{..}{q} + \frac{\partial}{\partial q} (J_{v i} \dot{q}) ​ \dot{q} = J_{v i} \overset{..}{q} + {\dot{J}}_{v i} \dot{q},

where ${\dot{J}}_{v i} = \frac{\partial}{\partial q} (J_{v i} \dot{q}) = \sum_{j = 1}^{n} \frac{\partial J_{v i}}{\partial q_{j}} {\dot{q}}_{j}$ is found to be

{\dot{J}}_{v i} (:, j) = {\begin{matrix} {\begin{matrix} \sum_{k = 1}^{j - 1} a_{k} \times J_{v i} (:, j) {\dot{q}}_{k} + \sum_{k = j}^{i} a_{j} \times J_{v i} (:, k) {\dot{q}}_{k} & revolute joint \\ \sum_{k = 1}^{j - 1} a_{k} \times J_{v i} (:, j) {\dot{q}}_{k} & prismatic joint \end{matrix} & j \leq i \\ 0_{3 \times 1} & j > i \end{matrix}, i, j = 1, 2, \dots, n

(32)

The matrix C_vi is equal to

C_{v i} = m_{i} J_{v i}^{T} {\dot{J}}_{v i}, i = 1, 2, \dots, n .

Step 6: Putting it all together

The D, C, and G matrices of the robot's equations of motion are

D = \sum_{i = 1}^{n} D_{ω ​ i} + \sum_{i = 1}^{n} D_{v i} = \sum_{i = 1}^{n} J_{ω ​ i}^{T} R_{i} I_{i} R_{i}^{T} J_{ω ​ i} + \sum_{i = 1}^{n} m_{i} J_{v i}^{T} J_{v i}

(33)

\begin{array}{l} C = \sum_{i = 1}^{n} C_{ω ​ i} + \sum_{i = 1}^{n} C_{v i}, \\ = \sum_{i = 1}^{n} (J_{ω ​ i}^{T} R_{i} I_{i} R_{i}^{T} ​ {\dot{J}}_{ω ​ i} + J_{ω ​ i}^{T} [ω_{i} \times] ​ R_{i} I_{i} R_{i}^{T} J_{ω ​ i}) \\ + \sum_{i = 1}^{n} m_{i} J_{v i}^{T} {\dot{J}}_{v i} \end{array}

(34)

and

G = \sum_{i = 1}^{n} G_{i} = \sum_{i = 1}^{n} m_{i} g ​ J_{v i}^{T} e_{3} .

(35)

4. Illustrated examples

4.1 Example 1: A 3 DOFs spherical robot

Figure 1 shows an example of a simple 3-DOF spherical robot system with both revolute and prismatic joints, and generalized coordinate q = [q₁ q₂ r]^T, and is presented to verify the proposed computing method. The system parameters of the spherical robot are shown in Table 1 and the control input vector u = [u₁ u₂ u₃]^T. For a link with two joints, a virtual link with zero mass and/or zero inertia can be inserted for easy programming. The proposed method and the Lagrange method are shown to obtain the same equations of motion as follows

Table 1.

System parameters of the 3-DOF spherical robot system.

I	Joint ρ_i	Link mass m_i	Link length r_i	Link CoM d_i	Link inertia I_i
1	+Rx	0	[0 0 0]^T	[0 0 0]^T	zero inertia
2	+Ry	0	[0 0 0]^T	[0 0 0]^T	diag{I_x, I_y, I_z}
3	+Tz	M	[0 0 r]^T	[0 0 r]^T	zero inertia

Figure 1.

Example of a 3-DOF spherical robot system.

D (q) \overset{..}{q} + C (q, \dot{q}) \dot{q} + G (q) = u,

where

\begin{array}{l} D = [\begin{matrix} I_{z} + c_{2}^{2} (I_{x} - I_{z} + M r^{2}) & 0 & 0 \\ 0 & I_{y} + M ​ r^{2} & 0 \\ 0 & 0 & M \end{matrix}] \\ C = [\begin{matrix} c_{2} (M ​ r ​ c_{2} \dot{r} - (M r^{2} + I_{x} - I_{z}) s_{2} {\dot{q}}_{2}) \\ (M r^{2} + I_{x} - I_{z}) c_{2} s_{2} {\dot{q}}_{1} \\ - M ​ r ​ c_{2}^{2} {\dot{q}}_{1} \end{matrix} \\ \begin{matrix} - (M r^{2} + I_{x} - I_{z}) c_{2} s_{2} {\dot{q}}_{1} & M ​ r ​ c_{2}^{2} {\dot{q}}_{1} \\ M ​ r ​ \dot{r} & M ​ r ​ {\dot{q}}_{2} ​ \\ - M ​ r ​ {\dot{q}}_{2} & 0 \end{matrix}] \end{array}

and

G = [\begin{matrix} - M ​ g ​ r s_{1} c_{2} \\ - M ​ g ​ r c_{1} s_{2} \\ M ​ g ​ c_{1} c_{2} \end{matrix}],

where c_i = cos(q_i), s_i = sin(q_i), and i = 1,2.

4.2 Example 2: A 3-DOF robot system

Figure 2 presents an example of a 3-DOF robotic pendulum system with the generalized coordinate q = [q₁ q₂ q₃]^T. Table 2 lists the system parameters of this example and the control input vector u = [u₁ u₂ u₃]^T. The proposed method and the traditional Lagrange method are shown to obtain the same equations of motion,

Table 2.

System parameters of a 3-DOF robotic pendulum system.

I	Joint ρ_i	Link mass m_i	Link length r_i	Link CoM d_i	Link inertia I_i
1	+Rx	0	[0 0 0]^T	[0 0 0]^T	zero inertia
2	+Ry	M ₁	[0 0 L₁]^T	[0 0 L₁/2]^T	diag{I_1x,I_1y,I_1z}
3	+Rx	M ₂	[0 0 L₂]^T	[0 0 L₂/2]^T	diag{I_2x, I_2y,I_2z}

Figure 2.

Example of a 3-DOF robotic pendulum system.

D (q) \overset{..}{q} + C (q, \dot{q}) \dot{q} + G (q) = u .

4.3 Example 3: A 10-DOF biped robot

This example considers a 10-DOF fully actuated 3D biped with feet, as shown in Figure 3. The 3D biped robot consists of 7 links: a torso and two legs and two feet. Each leg has a knee and an equal length for the shin and thigh. The 10 actuators are installed as follows: two at the hip, one at the knee and two at the ankle. Table 3 lists the system parameters of this example.

Table 3.

System parameters of a biped robot.

I	Joint ρ_i	Link mass m_i	Link length r_i	Link CoM d_i	Link inertia I_i
0	base	M ₀	[0 0 0]^T	[0 0 0]^T	zero inertia
1	+Rx	0	[–a 0 L₀]^T	[0 0 0]^T	zero inertia
2	+Ry	M ₁	[0 0 0]^T	[0 0 L₁/2]^T	zero inertia
3	+Ry	M ₂	[0 0 L₁]^T	[0 0 L₂/2]^T	zero inertia
4	+Ry	0	[0 0 L₂]^T	[0 0 0]^T	zero inertia
5	+Rx	M ₃	[0 0 0]^T	[0 −W L₃/2]^T	zero inertia
6	−Rx	0	[0 −W 0]^T	[0 0 0]^T	zero inertia
7	+Ry	M ₂	[0 0 0]^T	[0 0 −L₂/2]^T	zero inertia
8	+Ry	M ₁	[0 0 −L₂]^T	[0 0 −L₁/2]^T	zero inertia
9	+Ry	0	[0 0 −L₁]^T	[0 0 0]^T	zero inertia
10	−Rx	M ₀	[0 0 0]^T	[0 0 −L₀/2]^T	zero inertia

Figure 3.

Biped coordinate system when the left foot is the stance foot.

Let n = 10, the generalized coordinate q = [q₁ q₂ ṫ q₁₀]^T, and the control input vector u = [u₁ ṫ u₁₀]^T. In the single-support phase, the biped's dynamic equation can be obtained as

D (q) \overset{..}{q} + C (q, \dot{q}) \dot{q} + G (q) = u,

where

\begin{array}{l} D = \sum_{i = 1}^{n} m_{i} J_{v i}^{T} J_{v i} \\ C = \sum_{i = 1}^{n} m_{i} J_{v i}^{T} {\dot{J}}_{v i} \end{array}

and

G = \sum_{i = 1}^{n} m_{i} g ​ J_{v i}^{T} e_{3} .

Aside from the equations of motion, significant amounts of important data for the analysis of the biped's walking ability are also computed with minimal additional effort. For example, the biped ground reaction force F = [F_x F_y F_z]^T and the zero moment point (ZMP) during the single-support phase [x_zmp y_zmp]^T can be set up as:

[\begin{matrix} F_{x} \\ F_{y} \\ F_{z} \end{matrix}] = [\begin{matrix} \sum_{i = 0}^{n} m_{i} {\overset{..}{x}}_{i} \\ \sum_{i = 0}^{n} m_{i} {\overset{..}{y}}_{i} \\ \sum_{i = 0}^{n} m_{i} (g + {\overset{..}{z}}_{i}) \end{matrix}]

and

[\begin{matrix} x_{z m p} \\ y_{z m p} \end{matrix}] = \frac{1}{\sum_{i = 0}^{n} m_{i} (g + {\overset{..}{z}}_{i})} [\begin{matrix} \sum_{i = 0}^{n} m_{i} x_{i} (g + {\overset{..}{z}}_{i}) - \sum_{i = 0}^{n} m_{i} z_{i} {\overset{..}{x}}_{i} - \sum_{i = 0}^{n} {(τ_{i})}_{y} \\ \sum_{i = 0}^{n} m_{i} y_{i} (g + {\overset{..}{z}}_{i}) - \sum_{i = 0}^{n} m_{i} z_{i} {\overset{..}{y}}_{i} + \sum_{i = 0}^{n} {(τ_{i})}_{x} \end{matrix}]

where g is the gravity acceleration, p_ci = [x_i, y_i, z_i]^T is the centre of mass of the i-th link, and τ_i = [(τ_i)_x, (τ_i)_y, (τ_i)_z]^T is the moment of the i-th link. For authentication, the 10-DOF biped robot online simulation work has been implemented on MATLAB. After symbolic calculation, we transfer the dynamic equations into C-Mex files to improve the efficiency of the biped robot walking simulation.

4.4 Example 4: A simple 2-DOF closed-chain robot

We can also apply the proposed method to calculate the equations of motion for simple closed-chain robotic systems. Figure 4 shows an example of a 2-DOF four-bar linkage robot system, where Link 1 and Link 4 are parallel links and their absolute angle θ₁ is controlled by the input torque u₁. Similarly, Link 2 and Link 3 are parallel links and their absolute angle θ₂ is controlled by the input torque u₂. The four-bar robot system can be treated as 2 serial-chain robot systems. The first serial-chain robot consists of Link 1 and Link 2 with generalized coordinates

Figure 4.

An example of a 2-DOF four-bar linkage robot system.

q = [\begin{matrix} q_{1} \\ q_{2} \end{matrix}] = [\begin{matrix} θ_{1} \\ θ_{2} - θ_{1} \end{matrix}] = A_{1} [\begin{matrix} θ_{1} \\ θ_{2} \end{matrix}],

where

A_{1} = [\begin{matrix} 1 & 0 \\ - 1 & 1 \end{matrix}] .

Similarly, the second serial-chain robot consists of Link 4 and Link 3 with generalized coordinates

q = [\begin{matrix} q_{1} \\ q_{2} \end{matrix}] = [\begin{matrix} θ_{2} \\ θ_{1} - θ_{2} \end{matrix}] = A_{2} [\begin{matrix} θ_{1} \\ θ_{2} \end{matrix}],

where

A_{2} = [\begin{matrix} 0 & 1 \\ 1 & - 1 \end{matrix}] .

For the first robot system, the system matrices D₁, C₁ and G₁ can be calculated using the above method. For the second robot system, the matrices D₂, C₂ and G₂ can be similarly obtained. The equations of motion for the four-bar linkage can thus be determined as:

\begin{array}{l} (A_{1}^{T} D_{1} A_{1} + A_{2}^{T} D_{2} A_{2}) ​ \overset{..}{θ} + \\ + (A_{1}^{T} C_{1} A_{1} + A_{2}^{T} C_{2} A_{2}) ​ \dot{θ} + (A_{1}^{T} G_{1} + A_{2}^{T} G_{2}) = u, \end{array}

and hence

\begin{array}{l} D = A_{1}^{T} D_{1} A_{1} + A_{2}^{T} D_{2} A_{2} = \\ [\begin{matrix} \frac{1}{2} (m_{1} + 2 m_{2}) L_{1}^{2} & \frac{1}{2} (m_{1} + m_{2}) L_{1} L_{2} \cos (θ_{1} - θ_{2}) \\ \frac{1}{2} (m_{1} + m_{2}) L_{1} L_{2} \cos (θ_{1} - θ_{2}) & \frac{1}{2} (m_{2} + 2 m_{1}) L_{2}^{2} \end{matrix}] \\ C = A_{1}^{T} C_{1} A_{1} + A_{2}^{T} C_{2} A_{2} = \\ [\begin{matrix} 0 & \frac{1}{2} (m_{1} + m_{2}) L_{1} L_{2} \sin (θ_{1} - θ_{2}) {\dot{θ}}_{2} \\ \frac{- 1}{2} (m_{1} + m_{2}) L_{1} L_{2} \sin (θ_{1} - θ_{2}) {\dot{θ}}_{1} & 0 \end{matrix}] \end{array}

and

G = A_{1}^{T} G_{1} + A_{2}^{T} G_{2} = [\begin{matrix} g (m_{1} + m_{2}) L_{1} \cos θ_{1} \\ g (m_{1} + m_{2}) L_{2} \cos θ_{2} \end{matrix}] .

5. Conclusion

This paper has developed a decomposition and cross-product-based method to compute robot dynamics, offering several examples that demonstrate the versatility of the proposed method. The focus of this paper is first to establish a clear relationship between Lagrange equations and Newton-Euler equations, and second to develop an extremely intuitive and systematic method to compute the inertia matrix, the Coriolis and centrifugal matrix, and the gravity force vector of robot dynamics equations.

Footnotes

6. Acknowledgements

The work of C.L. Shih is supported by the Taiwan National Science Council (NSC) under Grant NSC 100-2221-E-011-020.

References

Hurmuzlua

Genotb

Brogliatoc

(2004) Modeling, Stability and Control of Biped Robots—a General Framework. Automatica 40: 1647–1664.

Tošic

D.V

Lutovac

M.D

(2008) Symbolic Simulation of Engineering Systems. 4th European Conference on Circuits and Systems for Communications. pp. 59–68.

Cetinkunt

Ittoop

(1992) Computer-automated Symbolic Modeling of Dynamics of Robotic Manipulators with Flexible Links. IEEE Trans. on Robotics and Automation. 8: 94–105.

Vibet

(1995) Symbolic Modeling of Robot Kinematics and Dynamics. Robotics and Autonomous Systems. 14: 301–314.

Vira

Tunstel

(1992) Use of Symbolic Computation in Robotics Education. IEEE Trans. on Education. 35: 18–30.

Luh

Walker

Paul

(1980) On-line Computational Scheme for Mechanical Manipulators. ASME Journal of Dynamic Systems, Measurement, and Control. 102: 69–76.

Hollerbach

J.M

(1980) A Recursive Lagrangian Formulation of Manipulator Dynamics and a Comparative Study of Dynamics Formulation Complex. IEEE Trans. on Systems, Man, and Cybernetics. 11: 730–736.

Featherstone

Orin

(2000) Robot Dynamics: Equations and Algorithms. IEEE Inter. Conf. on Robotics and Automation. pp. 826–834.

Spong

Hutchinson

Vidyasagar

(2006) Robot Modeling and Control. John Wiley & Sons, Inc.