A kind of Lagrange dynamic simplified modeling method for multi-DOF robot 1

Abstract

To solve the problem between large computation and complex process of dynamic modeling on multi-DOF(degree of freedom) robot, the paper introduces Lagrange equation into the problem so that only external force needs to be considered and unknown binding force is no longer to be considered, which is convenient for the design of control system and dynamic simulation. Also, the paper introduces Q matrix into traditional Lagrange dynamic equation of robot and linearizes the dynamic equation under reference frame. Finally, the paper proposes a Lagrange equation simplified calculation method for dynamic modeling on multi-DOF robots and gives the calculation equations. The paper takes five DOF Upper limb rehabilitation robot as example and model the robot using the new method. The simulation result shows that the established mathematical model is accurate and the method is fast, efficient and can be used as a new method for a class of multi-DOF robots modeling.

Keywords

Multi-DOF robot Lagrange dynamic equation simplified calculation linearization transition

1 Introduction

At present, the system dynamic model of the multi-DOF (degree of freedom) robot is solved by using the main research methods such as Newton-Euler method, Lagrange equation method, Kane equation method and Variational method etc. These research methods rely on the computer numerical analysis to solve the complex multi-DOF problems of the machinery system dynamics. However, each method has its own advantages and disadvantages because of its different calculation principles.

Newton-Euler method: The method is quick in the calculation speed and it can satisfy the needs of the speed and the sampling frequency in the servo system. In addition, it is also convenient for the real-time control. However, given the constraining force between the adjacent bars in the equation, the additional calculation will be made to remove the constraining force, so the structure becomes more complex [5, 6].

Lagrange equation method: The second type Lagrange equation is suitable for the complete system for that it solves the complex system dynamic equation in the simplest form and that it is of the explicit structure. The Lagrange equation is mainly used for calculating kinetic energy, potential energy and generalized force. The most important part is how to create the coordinate system and consider the system constraint in the Lagrange equation.

Kane equation method: This calculation method uses many codes for equations instead of giving the general dynamic equation suitable for any multi-rigid-body system. Therefore, it is rather difficult for proofreading and the selection of the generalized speed also requires certain experience and skills— such are the disadvantages of this method. However, this method is praised for it only requires calculating the dot product and the cross product of vectors without involving the kinetic function and the derivation [9, 10].

Variational method: According to the optimization theory, this method solves the accelerate speed at that time according to the location and speed of the mechanical arm every moment without setting up the kinetic equations. Meanwhile, the kinetic analysis can be conducted along with the system optimization. However, compared with other methods, it involves more calculation in the same condition [11, 12].

Given that the multi-DOF robot is large in the number of rigid bodies and that connection states are complex, the robot should be convenient for programming to analyze its kinetic performances and the physical significance of the intermediate output parameter should be definite. Research has shown that the recursive algorithm of the variational method is easier in programming but involves more calculation than other equations, while Lagrange equation and Newton-Euler are definite in the physical significance of the intermediate parameters with variational method second to them and Kane equation the worst. Given the factors above, the paper finally uses Lagrange equation method for it only involves the external force without considering the unknown constraining force. Thus, it is convenient for the control system design and the dynamic simulation of the robot before it can be used for the dynamic modeling of the robot. Based on this, the kind of Lagrange dynamic modeling method will be popularized for the multi-degree-freedom robot.

2 Lagrange dynamic equation

Lagrange mechanics method is based on energy item against system variables and time differential. The definition of Lagrange function is: $L = K - P$

Where, L is Lagrange function, K is system kinetic energy, and P is system potential energy.

The definition of Lagrange equation is: $τ_{i} = \frac{d}{dt} [\frac{\partial L}{\partial {\dot{q}}_{i}}] - \frac{\partial L}{\partial q_{i}}, i = 1, 2, \dots, n$

Where, L is Lagrange function, τ_i is generalized force or torque of the system, q_i is system variable, and ${\dot{q}}_{i}$ is first-order derivative of system variable. Lagrange equation can be used to establish the kinetic equation without containing constraining force. In addition, it can be also used to solve the active force working on the system in the given system.

Thus, the generalized form of Lagrange kinetic equation for the robot is: $\begin{matrix} τ_{i} = \sum_{j = 1}^{n} D_{ij} {\ddot{q}}_{j} + \sum_{j = 1}^{n} \sum_{k = 1}^{n} D_{ijk} {\dot{q}}_{j} {\dot{q}}_{k} \\ + D_{i} (i = 1, 2, 3 \dots, n) \end{matrix}$ (1) $\begin{matrix} In equation D_{ij} \\ = \sum_{m = max (i, j)}^{n} Trace [\frac{\partial T_{m}}{\partial q_{i}} J_{m} {(\frac{\partial T_{m}}{\partial q_{j}})}^{T}] \end{matrix}$ (2) $D_{ijk} = \sum_{m = max (i, j, k)}^{n} Trace [\frac{\partial T_{m}}{\partial q_{i}} J_{m} {(\frac{\partial^{2} T_{m}}{\partial q_{j} q_{k}})}^{T}]$ (3) $D_{i} = \sum_{m = i}^{n} - M_{m} [g^{T}, 0] \frac{\partial T_{m}}{\partial q_{i}} {[{\bar{r}}_{m}^{T}, 1]}^{T}$ (4)

Where $q, \dot{q}, \ddot{q} \in R^{n}$ are displacement, speed, and accelerated speed of the joint;

τ ∈ Rⁿ is required controlling force or torque, i.e. control input;

M_i is mass of bar i;

${\bar{r}}_{m}$ is position vector of bar centroid in the local coordinate system;

J_i is the 4 × 4 pseudo-inertia force matrix of bar i in the local coordinate system; $J_{i} = [\begin{matrix} {\bar{I}}_{i} + M_{i} {\bar{r}}_{i} {\bar{r}}_{i}^{T} & M_{i} {\bar{r}}_{i} \\ M_{i} {\bar{r}}_{i}^{T} & M_{i} \end{matrix}]$ (5)

I_i is the 3 × 3 inertia force matrix of bar i’s centroid in the local coordinate system;

${\bar{r}}_{i}$ is position vector of any point on bar i in the local coordinate system;

T_i is the 4 × 4 homogeneous transfer matrix of bar i from the local coordinate system to the fixed reference coordinate system;

g is acceleration vector of gravity in the reference coordinate system;

Moreover, D_ij = D_ji, D_ijk = D_ikj, D_ijk = - D_kji (k, i ≥ j), D_iji = 0 (i ≥ j).

3 Simplified calculation of Lagrange kinetic equation

In Equation (1), Lagrange equation altogether requires [(128/3) n⁴ + (512/3) n³ + (844/3) n² + (76/3) n] multiplications and [ (98/3) n⁴ + (781/6) n³ + (637/3) n² + (107/6) n] additive operations. It requires a lot of calculation and is difficult for the multi-degree-freedom robot to do calculation and programming.

Lagrange kinetic equation of the robot which introduces Q matrix includes:

Rotational joint takes $\begin{matrix} Q_{i} & = & [\begin{matrix} 0 & - 1 & 0 & 0 \\ 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \end{matrix}] = [\begin{matrix} Q_{i}^{11} & Q_{i}^{12} \\ 0 & 0 \end{matrix}], \\ Q_{i}^{11} & = & [\begin{matrix} 0 & - 1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 0 \end{matrix}], Q_{i}^{12} = [\begin{matrix} 0 & 0 & 0 \end{matrix}]; \end{matrix}$

Gliding joint takes $\begin{matrix} Q_{i} & = & [\begin{matrix} 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 \end{matrix}] = [\begin{matrix} Q_{i}^{11} & Q_{i}^{12} \\ 0 & 0 \end{matrix}], \\ Q_{i}^{11} & = & [\begin{matrix} 0 & 0 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \end{matrix}], Q_{i}^{12} = [\begin{matrix} 0 & 0 & 1 \end{matrix}] . \end{matrix}$

Then $\frac{\partial A_{i}}{\partial q_{i}} = {QA}_{i}$

So $\frac{\partial T_{m}}{\partial q_{i}} = U_{mi} = T_{i - 1} {QT}_{i - 1}^{- 1} T_{m}, (i \leq m)$

Suppose $Δ = T_{i - 1} {QT}_{i - 1}^{- 1}$ $\begin{matrix} Δ & = & T_{i - 1} {QT}_{i - 1}^{- 1} \\ = & [\begin{matrix} R_{i - 1} & P_{i - 1} \\ 0 & 1 \end{matrix}] [\begin{matrix} Q_{i}^{11} & Q_{i}^{12} \\ 0 & 0 \end{matrix}] [\begin{matrix} R_{i - 1}^{T} & - R_{i - 1}^{T} P_{i - 1} \\ 0 & 1 \end{matrix}] \\ = & [\begin{matrix} R_{i - 1} Q_{i}^{11} R_{i - 1}^{T} & - R_{i - 1} Q_{i}^{11} R_{i - 1}^{T} P_{i - 1} + R_{i - 1} Q_{i}^{12} \\ 0 & 0 \end{matrix}] \\ = & [\begin{matrix} {\tilde{δ}}_{i} & λ_{i} \\ 0 & 0 \end{matrix}] = [\begin{matrix} 0 & - δ_{iz} & δ_{iy} & λ_{ix} \\ δ_{iz} & 0 & - δ_{ix} & λ_{iy} \\ - δ_{iy} & δ_{ix} & 0 & λ_{iz} \\ 0 & 0 & 0 & 0 \end{matrix}] \end{matrix}$ (6)

Where ${\tilde{δ}}_{i} = [\begin{matrix} 0 & - δ_{iz} & δ_{iy} \\ δ_{iz} & 0 & - δ_{ix} \\ - δ_{iy} & δ_{ix} & 0 \end{matrix}], δ_{i} = [\begin{matrix} δ_{ix} \\ δ_{iy} \\ δ_{iz} \end{matrix}], λ_{i} = [\begin{matrix} λ_{ix} \\ λ_{iy} \\ λ_{iz} \end{matrix}]$ (7) $\begin{matrix} δ_{i} & = & {[\begin{matrix} δ_{ix} & δ_{iy} & δ_{iz} \end{matrix}]}^{T} \\ = & {\begin{matrix} a_{i - 1}, & Rotational joint \\ 0, & Gliding joint \end{matrix} \\ λ_{i} & = & {[\begin{matrix} λ_{ix} & λ_{iy} & λ_{iz} \end{matrix}]}^{T} \\ = & {\begin{matrix} P_{i - 1} \times a_{i - 1}, & Rotational joint \\ a_{i - 1}, & Gliding joint \end{matrix} \end{matrix}$

Here a_i-1 is vector of the third row for R_i-1 matrix. So $\frac{\partial T_{m}}{\partial q_{i}} = U_{mi} = {\begin{matrix} Δ_{i} T_{m}, i \leq m \\ 0, i > m \end{matrix}$ (8) $\frac{\partial^{2} T_{m}}{\partial q_{j} q_{k}} = U_{mjk} = {\begin{matrix} Δ_{j} Δ_{k} T_{m}, m \geq k \geq j \\ Δ_{k} Δ_{j} T_{m}, m \geq j \geq k \\ 0, m < j, m < k \end{matrix}$ (9)

Substitute Equation (8) in Equation (2), the following can be obtained:

$\begin{matrix} D_{ij} & = & \sum_{m = j}^{n} Trace (Δ_{i} T_{m} J_{m} T_{m}^{T} Δ_{j}^{T}) \\ = & Trace [Δ_{i} (\sum_{m = j}^{n} T_{m} J_{m} T_{m}^{T}) Δ_{j}^{T}] \end{matrix}$ (10)

Where $\begin{matrix} \sum_{m = j}^{n} T_{m} J_{m} T_{m}^{T} \\ = \sum_{m = j}^{n} [\begin{matrix} R_{m} & P_{m} \\ 0 & 1 \end{matrix}] [\begin{matrix} {\bar{I}}_{m} + M_{m} {\bar{r}}_{m} {\bar{r}}_{m}^{T} M_{m} \bar{r} \\ M_{m} {\bar{r}}_{m}^{T} M_{m} \end{matrix}] [\begin{matrix} R_{m}^{T} & 0 \\ P_{m}^{T} & 1 \end{matrix}] \\ = \sum_{m = j}^{n} [\begin{matrix} R_{m} {\bar{I}}_{m} R_{m}^{T} + M_{m} & M_{m} (R_{m} \bar{r} + P_{m}) \\ (R_{m} \bar{r} + P_{m}) \\ {(R_{m} \bar{r} + P_{m})}^{T} \\ {[M_{m} (R_{m} \bar{r} + P_{m})]}^{T} & M_{m} \end{matrix}] \\ = [\begin{matrix} U_{j} & V_{j} \\ V_{j}^{T} & X_{j} \end{matrix}] \end{matrix}$

In the equation $\begin{matrix} U_{j} & = & \sum_{m = j}^{n} [R_{m} {\bar{I}}_{m} R_{m}^{T} + M_{m} (R_{m} \bar{r} + P_{m}) \\ {(R_{m} \bar{r} + P_{m})}^{T}] \\ V_{j} & = & \sum_{m = j}^{n} M_{m} (R_{m} \bar{r} + P_{m}) \\ X_{j} & = & \sum_{m = j}^{n} M_{m} \end{matrix}$

In the equation listed above, U_jV_jX_j can be written in the recursive forms and they are respectively written as: $\begin{matrix} U_{j} & = & U_{j + 1} + R_{j} {\bar{I}}_{j} R_{j}^{T} \\ + M_{j} (R_{j} \bar{r} + P_{j}) {(R_{j} \bar{r} + P_{j})}^{T} \end{matrix}$ (12) $V_{j} = V_{j + 1} + M_{j} (R_{j} \bar{r} + P_{j})$ (13) $X_{j} = X_{j + 1} + M_{j}$ (14)

Then Equation (10) can be written as

$\begin{matrix} D_{ij} & = & Trace {[\begin{matrix} {\tilde{δ}}_{i} & λ_{i} \\ 0 & 0 \end{matrix}] [\begin{matrix} U_{j} & V_{j} \\ V_{j}^{T} & X_{j} \end{matrix}] [\begin{matrix} {\tilde{δ}}_{j}^{T} & 0 \\ λ_{j}^{T} & 0 \end{matrix}]} \\ = & Trace [{\tilde{δ}}_{i} (U_{j} {\tilde{δ}}_{j}^{T} + V_{j} λ_{j}^{T}) \\ + λ_{i} (V_{j}^{T} {\tilde{δ}}_{j}^{T} + X_{j} λ_{j}^{T})] \end{matrix}$ (15)

Similarly, substitute Equation (9) in Equation (3), the following can be obtained:

$\begin{matrix} D_{ijk} & = & \sum_{m = j}^{n} Trace (Δ_{i} T_{m} J_{m} T_{m}^{T} Δ_{k}^{T} Δ_{j}^{T}) \\ = & Trace [Δ_{i} (\sum_{m = j}^{n} T_{m} J_{m} T_{m}^{T}) Δ_{k}^{T} Δ_{j}^{T}] \end{matrix}$ (16)

Similarly, Equation (12) can be written as $\begin{matrix} D_{ijk} & = & Trace {[\begin{matrix} {\tilde{δ}}_{i} & λ_{i} \\ 0 & 0 \end{matrix}] [\begin{matrix} U_{k} & V_{k} \\ V_{k}^{T} & X_{k} \end{matrix}] [\begin{matrix} {\tilde{δ}}_{k}^{T} & 0 \\ λ_{k}^{T} & 0 \end{matrix}] [\begin{matrix} {\tilde{δ}}_{j}^{T} & 0 \\ λ_{j}^{T} & 0 \end{matrix}]} \\ = & Trace {[{\tilde{δ}}_{i} (U_{k} {\tilde{δ}}_{k}^{T} + V_{k} λ_{k}^{T}) \\ + λ_{i} (V_{k}^{T} {\tilde{δ}}_{k}^{T} + X_{k} λ_{k}^{T})]} {\tilde{δ}}_{j}^{T}, (m \geq j \geq k) \end{matrix}$ (17)

Or $\begin{matrix} D_{ijk} & = & Trace {[\begin{matrix} {\tilde{δ}}_{i} & λ_{i} \\ 0 & 0 \end{matrix}] [\begin{matrix} U_{k} & V_{k} \\ V_{k}^{T} & X_{k} \end{matrix}] [\begin{matrix} {\tilde{δ}}_{j}^{T} & 0 \\ λ_{j}^{T} & 0 \end{matrix}] [\begin{matrix} {\tilde{δ}}_{k}^{T} & 0 \\ λ_{k}^{T} & 0 \end{matrix}]} \\ = & Trace {[{\tilde{δ}}_{i} (U_{j} {\tilde{δ}}_{j}^{T} + V_{j} λ_{j}^{T}) \\ + λ_{i} (V_{j}^{T} {\tilde{δ}}_{j}^{T} + X_{j} λ_{j}^{T})]} {\tilde{δ}}_{k}^{T}, (m \geq k \geq j) . \end{matrix}$ (18)

Similarly, the following can be obtained:

$\begin{matrix} D_{i} & = & \sum_{m = i}^{n} - M_{m} [g^{T}, 0] \frac{\partial T_{m}}{\partial q_{i}} {[{\bar{r}}_{m}^{T}, 1]}^{T} \\ = & \sum_{m = i}^{n} - [M_{m} (g^{T}, 0) Δ_{i} T_{m} {\bar{r}}_{m}^{T}, 1] \\ = & - (g^{T}, 0) Δ_{i} \sum_{m = i}^{n} M_{m} T_{m} {[{\bar{r}}_{m}^{T}, 1]}^{T} \end{matrix}$ (19)

Where $\sum_{m = i}^{n} M_{m} T_{m} {[{\bar{r}}_{m}^{T}, 1]}^{T} = {[V_{i}^{T}, X_{i}]}^{T}$ (20)

So Equation (19) can be written as:

$\begin{matrix} D_{i} & = & - (g^{T}, 0) [\begin{matrix} {\tilde{δ}}_{i} & λ_{i} \\ 0 & 0 \end{matrix}] [\begin{matrix} V_{i} \\ X_{i} \end{matrix}] \\ = & - g^{T} ({\tilde{δ}}_{i} V_{i} + X_{i} λ_{i}) \end{matrix}$ (21)

If ${\begin{matrix} φ_{i} = U_{i} {\tilde{δ}}_{i}^{T} + V_{i} λ_{i}^{T} \\ ψ_{i} = V_{i}^{T} {\tilde{δ}}_{i}^{T} + X_{i} λ_{i}^{T} \end{matrix}$ (22)

Substitute Equation (22) in Equations (15, 17, 18, 21), then $D_{ij} = Trace [{\tilde{δ}}_{i} φ_{j} + λ_{i} ψ_{j}]$ (23) ${\begin{matrix} D_{ijk} = Trace ({\tilde{δ}}_{i} φ_{k} + λ_{i} ψ_{k}) {\tilde{δ}}_{j}^{T}, (m \geq j \geq k) \\ D_{ijk} = Trace ({\tilde{δ}}_{i} φ_{j} + λ_{i} ψ_{j}) {\tilde{δ}}_{k}^{T}, (m \geq k \geq j) \end{matrix}$ (24) $D_{i} = - g^{T} ψ_{i}^{T}$ (25)

Then each item in Equation (1) can be calculated through Equations (23–25). In the calculation method, [(9/8) n⁴ + (11/6) n³ + (371/24) n² + (451/4) n - 81] multiplicationsand [(5/4) n⁴ + (25/12) n³ + (49/3) n² + (195/2) n - 71] additive operations make the largest amount of calculation. When n is 5, this method involves 1801 multiplications and 1566 additive operations. It can be seen that: this method has the advantage of small calculation amount and can greatly improve the calculation efficiency for the multi-degree-freedom robot.

4 Kinetic modeling of 5-DOF rehabilitation robot

The 5-DOF rehabilitation robot, which is as shown in Fig. 1, acts on hemiplegic patients suffering from the motor function damage. Its mode of motion follows the nerve-muscle repair rules [13] and conducts the rehabilitation therapy on patients gradually, stably and effectively. In this way, the single joint motion and the compound motion of multiple joints can be realized from large joints to small joints within a wide range, truly representing and simulating the motion training of patients in their daily lives [14].

The five degrees of freedom in the 5-DOF upper-limb rehabilitation robot are respectively shoulder extension/retraction (joint 1), bending/stretching (joint 2), elbow bending/stretching (joint 3), wrist bending/stretching (joint 5) and medial rotation/lateral rotation (joint 4) and they can simulate the human arm movement to the maximum. Every degree of freedom is an independent motor-driven joint as shown in Fig. 1. The upper arm and forearm of the rehabilitation robot can be adjustable in the position so that the right-and-left dressing can be realized to meet the specific requirements of patients. Moreover, the design also optimizes the structure of the mechanical arm and makes patients feel more comfortable while participating in the training. Meanwhile, the design becomes more suitable for dressing.

The mechanical arms of the 5-DOF upper-limb rehabilitation robot have five rotational joints. If the connecting rod is uniform in texture between joints, the two degrees of freedom (joint 4— wrist rotation and joint 5— wrist pitching) are mainly used to adjust the hand poses at the end in the mechanical arm. And they will not have a great influence on the static torque in the motion process of other joints. Moreover, the mass of bar 4 (m₄) can be known from the structure parameter of the mechanical arm, and the mass of bar 5 (m₅) is far less than that of bar 1 (m₁), bar 2 (m₂), and bar 3(m₃). Therefore, the influence of the mass distribution can be ignored in calculating these two items. Then, it is considered that all the mass concentrates on the end of joint 3. Next, the position of the hand terminal point is fixed at the centre of the moving axis and it combines with the forearm as an integral whole before making calculations, namely m_III = m₃ + m₄ + m₅.

The upper-limb rehabilitation robot is simplified into the spatial 3-DOF connecting rod structure which comprises shoulder bending/stretching joint, shoulder extension/retraction joint and elbow bending/stretching joint. In the structure, the motor plane of shoulder bending/stretching joint is perpendicular to that of shoulder extension/retraction joint. Meanwhile, the shoulder joint and the elbow joint make the two-linking rod movement featuring bending and stretching on the same plane, which reestablishes the structure modeling and the coordinate system as shown in Fig. 2.

Where, l_I is length of bar I between shoulder joint extension/retraction and shoulder joint bending/stretching, l_{c
_I} is length centroid of bar I, m_I is the mass of bar I, l_II is length of bar II between shoulder joint bending/stretching and elbow joint bending/ stretching, l_{c
_II} is length centroid of bar II, m_II is the mass of bar II, l_III is length from elbow joint bending/stretching to the terminal bar III, l_{c
_III} is length centroid of bar III, and m_III is the mass of bar III.

The bar transfer matrix is $\begin{matrix} ^{0} T_{3} & = & A_{1} A_{2} A_{3} \\ = & [\begin{matrix} C_{1} C_{2} C_{3} - C_{1} S_{2} S_{3} & - C_{1} C_{2} S_{3} - C_{1} S_{2} C_{3} & S_{1} & a_{1} C_{1} C_{2} \\ S_{1} C_{2} C_{3} - S_{1} S_{2} S_{3} & - S_{1} C_{2} S_{3} - S_{1} S_{2} C_{3} & - C_{1} & a_{1} S_{1} C_{2} \\ S_{2} C_{3} + C_{2} S_{3} & - S_{2} S_{3} + C_{2} C_{3} & 0 & a_{1} S_{2} \\ 0 & 0 & 0 & 1 \end{matrix}] \end{matrix}$

The following can be calculated through Equations (15, 16, 19, 22, 23–25). $\begin{matrix} D_{11} & = & m_{I} l_{c_{I}}^{2} + m_{II} l_{I}^{2} + m_{I I I} l_{I}^{2} + \frac{1}{3} m_{I} l_{I}^{2} \\ D_{12} & = & m_{II} l_{I} l_{c_{II}} S_{1} sin S_{2} + m_{I I I} l_{I} l_{II} S_{1} S_{2} \\ + m_{I I I} l_{I} l_{c_{III}} S_{1} S_{23} \\ D_{13} & = & m_{I I I} l_{I} l_{c_{III}} S_{1} S_{23} \\ D_{122} & = & m_{II} l_{I} l_{c_{II}} S_{1} C_{2} + m_{I I I} l_{I} l_{II} S_{1} C_{2} \\ + m_{I I I} l_{I} l_{c_{III}} S_{1} C_{23} \\ D_{133} & = & m_{I I I} l_{I} l_{c_{III}} S_{1} C_{23} \\ D_{123} & = & 2 m_{I I I} l_{I} l_{c_{III}} S_{1} C_{23} \\ D_{1} & = & m_{I} {gd}_{1} S_{1} + m_{II} {gl}_{I} S_{1} + m_{I I I} {gl}_{II} S_{1} \\ D_{21} & = & m_{II} l_{I} l_{c_{II}} S_{1} S_{2} + m_{I I I} l_{I} l_{II} S_{1} S_{2} \\ + m_{I I I} l_{I} l_{c_{III}} S_{1} S_{23} \\ D_{22} & = & m_{II} l_{c_{II}}^{2} + m_{I I I} l_{II}^{2} + m_{I I I} l_{c_{III}}^{2} \\ + 2 m_{I I I} l_{II} l_{c_{III}} C_{3} + \frac{1}{3} m_{II} l_{II}^{2} + \frac{1}{3} m_{III} l_{III}^{2} \\ D_{23} & = & m_{I I I} l_{c_{III}}^{2} + m_{I I I} l_{II} l_{c_{III}} C_{3} + \frac{1}{3} m_{III} l_{III}^{2} \\ D_{211} & = & m_{II} l_{I} l_{c_{II}} C_{1} S_{2} + m_{I I I} l_{I} l_{II} C_{1} S_{2} \\ + m_{I I I} l_{I} l_{c_{III}} C_{1} S_{23} \\ D_{233} & = & - m_{I I I} l_{II} l_{c_{III}} S_{3} \\ D_{223} & = & - 2 m_{I I I} l_{I} l_{c_{III}} S_{3} \\ D_{2} & = & m_{II} {gl}_{c_{II}} S_{2} + m_{I I I} {gl}_{II} S_{2} + m_{I I I} {gl}_{c_{III}} S_{23} \\ D_{31} & = & m_{III} l_{I} l_{c_{III}} S_{1} S_{23} \\ D_{32} & = & m_{I I I} l_{c_{III}}^{2} + m_{I I I} l_{II} l_{c_{III}} C_{3} + \frac{1}{3} m_{III} l_{III}^{2} \\ D_{33} & = & m_{I I I} l_{c_{III}}^{2} + \frac{1}{3} m_{III} l_{III}^{2} \\ D_{311} & = & m_{I I I} l_{I} l_{c_{III}} C_{1} S_{23} \\ D_{322} & = & m_{I I I} l_{II} l_{c_{III}} S_{3} \\ D_{3} & = & m_{I I I} {gl}_{c_{III}} S_{23} \end{matrix}$

According to Equation (1), the following can be obtained: $\begin{matrix} τ_{I} & = & (m_{I} l_{c_{I}}^{2} + m_{II} l_{I}^{2} + m_{I I I} l_{I}^{2} + \frac{1}{3} m_{I} l_{I}^{2}) {\ddot{q}}_{1} \\ + (m_{II} l_{I} l_{c_{II}} S_{1} S_{2} + m_{I I I} l_{I} l_{II} S_{1} S_{2} \\ + m_{I I I} l_{I} l_{c_{III}} S_{1} S_{23}) {\ddot{q}}_{2} + (m_{I I I} l_{I} l_{c_{III}} S_{1} S_{23}) {\ddot{q}}_{3} \\ + (m_{II} l_{I} l_{c_{II}} S_{1} C_{2} + m_{I I I} l_{I} l_{II} S_{1} C_{2} \\ + m_{I I I} l_{I} l_{c_{III}} S_{1} C_{23}) {\dot{q}}_{2}^{2} \\ + (m_{I I I} l_{I} l_{c_{III}} S_{1} C_{23}) {\dot{q}}_{3}^{2} \\ + (2 m_{I I I} l_{I} l_{c_{III}} S_{1} C_{23}) {\dot{q}}_{2} {\dot{q}}_{3} + m_{I} {gl}_{c_{I}} S_{1} \\ + m_{II} {gl}_{I} S_{1} + m_{I I I} {gl}_{II} S_{1} \end{matrix}$

$\begin{matrix} τ_{II} & = & (m_{II} l_{I} l_{c_{II}} S_{1} S_{2} + m_{I I I} l_{I} l_{II} S_{1} S_{2} \\ + m_{I I I} l_{I} l_{c_{III}} S_{1} S_{23}) {\ddot{q}}_{1} \\ + (m_{II} l_{c_{II}}^{2} + m_{I I I} l_{II}^{2} + m_{I I I} l_{c_{III}}^{2} \\ + 2 m_{I I I} l_{II} l_{c_{III}} C_{3} + \frac{1}{3} m_{II} l_{II}^{2} + \frac{1}{3} m_{III} l_{III}^{2}) {\ddot{q}}_{2} \\ + (m_{I I I} l_{c_{III}}^{2} + m_{I I I} l_{II} l_{c_{III}} C_{3} + \frac{1}{3} m_{III} l_{III}^{2}) {\ddot{q}}_{3} \\ + (m_{II} l_{I} l_{c_{II}} C_{1} S_{2} + m_{I I I} l_{I} l_{II} C_{1} S_{2} \\ + m_{I I I} l_{I} l_{c_{III}} C_{1} S_{23}) {\dot{q}}_{1}^{2} + (- m_{I I I} l_{II} l_{c_{III}} S_{3}) {\dot{q}}_{3}^{2} \\ + (- 2 m_{I I I} l_{I} l_{c_{III}} S_{3}) {\dot{q}}_{2} {\dot{q}}_{3} + m_{II} {gl}_{c_{II}} S_{2} \\ + m_{I I I} {gl}_{II} S_{2} + m_{I I I} {gl}_{c_{III}} S_{23} \end{matrix}$ $\begin{matrix} τ_{III} & = & (m_{III} l_{I} l_{c_{III}} S_{1} S_{23}) {\ddot{q}}_{1} \\ + (m_{I I I} l_{c_{III}}^{2} + m_{I I I} l_{II} l_{c_{III}} C_{3} + \frac{1}{3} m_{III} l_{III}^{2}) {\ddot{q}}_{2} \\ + (m_{I I I} l_{c_{III}}^{2} + \frac{1}{3} m_{III} l_{III}^{2}) {\ddot{q}}_{3} \\ + (m_{I I I} l_{I} l_{c_{III}} C_{1} S_{23}) {\dot{q}}_{1}^{2} + (m_{I I I} l_{II} l_{c_{III}} S_{3}) {\dot{q}}_{2}^{2} \\ + m_{I I I} {gl}_{c_{III}} S_{23} \end{matrix}$

The parameters of each item for the 5-DOF upper limb rehabilitation robot are shown in Tables 1 and 2.

Substitute that in the equation and get the torque equations of each joint for the 5-DOF upper-limb rehabilitation robot $\begin{matrix} τ_{I} & = & 0.08 {\ddot{q}}_{1} + (1.01 S_{1} S_{2}) {\ddot{q}}_{2} + 0.04 S_{1} S_{23} ({\ddot{q}}_{2} + {\ddot{q}}_{3}) \\ + (0.12 S_{1} C_{2}) {\dot{q}}_{2}^{2} + 0.04 S_{1} C_{23} ({\dot{q}}_{2}^{2} + {\dot{q}}_{3}^{2}) \\ + (0.07 S_{1} C_{23}) {\dot{q}}_{2} {\dot{q}}_{3} + 13.7 S_{1} \end{matrix}$ $\begin{matrix} τ_{II} & = & (0.03 S_{1} S_{2} + 0.04 S_{1} S_{23}) {\ddot{q}}_{1} \\ + (0.38 + 0.2 C_{3}) {\ddot{q}}_{2} + (0.1 + 0.1 C_{3}) {\ddot{q}}_{3} \\ + (0.12 C_{1} S_{2} + 0.04 C_{1} S_{23}) {\dot{q}}_{1}^{2} - (0.1 S_{3}) {\dot{q}}_{3}^{2} \\ - (0.07 S_{3}) {\dot{q}}_{2} {\dot{q}}_{3} + 11.37 S_{2} + 3.43 S_{23} \end{matrix}$ $\begin{matrix} τ_{III} & = & (0.04 S_{1} S_{23}) {\ddot{q}}_{1} + (0.1 + 0.1 C_{3}) {\ddot{q}}_{2} \\ + 0.1 {\ddot{q}}_{3} + (0.04 C_{1} S_{23}) {\dot{q}}_{1}^{2} \\ + (0.1 S_{3}) {\dot{q}}_{2}^{2} + 3.43 S_{23} \end{matrix}$

5 Simulation

The 3D entity model of the 5-DOF upper-limb rehabilitation robot is established by using SolidWorks, as shown in Fig. 3.

The simulation model can easily complete the visual motion simulation analysis and obtain the tip trajectory of the robot and the real-time changes of driving joints to realize the simulation of the dynamic modeling. The control system simulation block diagram is established based on the3D entity model of 5-DOF upper-limb rehabilitation robot, as shown in Fig. 4.

The state equation described in Equation (10) is written into S-function in MATLAB. Then it is compared with the simulation results of the SimMechanics model in the environment of Simulink when the simulation time is 3 s. As shown in Fig. 5, Figure A is the comparison of zero-input and zero-state response curve, and Figure B is zero-state response curve when τ = ^T is input.

6 Conclusion

Clearly, it can be seen in the Simulation results that only minor difference exists between the simulation results of the mathematical model and those of SimMechanics. Thus, the established mathematical model is relatively accurate. Meanwhile, it is found that the input is non-linear to the output and that the input of τ₁ influences all the inputs. Moreover, the coupling is also found between system input and system output. In conclusion, it can be seen from the simulation results that: this paper establishes the mathematical model of the upper-limb rehabilitation system control member and reflects the relationship between the system input torque and the motion based on the pseudo-inertia matrix and Lagrange equation. Briefly, this system is a non-linear coupling system of multi-variables.

Footnotes

Acknowledgments

This work is supported by “Fundamental Research Funds for the Central Universities” (N150804001), 2015 Liaoning province Doctoral Fund (201501142) and National Natural Science Foundation of China (61503070).

References

Hongqin

, Kinematics, Kinetic Researches and Applications about Robots Based on Screw Theory [D]. Nanjing University of Aeronautics and Astronautics, 2007.

Dasgupta

and Mruthyunjaya

T.S.

, A Newton-Euler equationtion for the inverse dynamics of the Stewart Platform manipulator[J], Mechanism and Machine Theory33(8) (1998), 1135–1152.

Zhifu

, Xianguo

and Chen

, Kinetic study of 3-DOF parallel structure based on lagrange equation[J], Journal of Beijing University of Aeronautics and Astronautics33(8) (1998), 1135–1152.

Guowei

and Weigang

, Kane methods to solve kinetic problems of parallel robots [J], Journal of System Simulation, 200416(7), 1386–1391.

Jin

, Song

, Gang

, et al., Kinetic modeling and motion control of two-wheeled self-balancing robots[J], Journal of Xi’an Technological University34(10) (2014), 851–855.

Hongge

and Wei

, Rope features and motion trajectory planning of 6-DOF IRPMs parallel wire-driven manipulator in cranes[J], Mechanical Transmission38(11) (2014), 67–71.

Lifu

, Haiyan

and Qingyong

, Research on rigid-elastic coupling dynamics using Lagrange equation[J], Harbin Gongcheng Daxue Xuebao/Journal of Harbin Engineering University36(4) (2015), 456–460.

Jianhua

, Xiang

, Weihai

and Yan

, Dynamic analysis of cable-driven humanoid arm based on Lagrange’s equation[C], Advances in Reconfigurable Mechanisms and Robots I (2012), 619–628.

, Meng

, Wang

, et al., Realize key technologies for automated docking system of sea replenishment using 6-Rus parallel structure[J], Robots33(1) (2011), 28–34.

10.

, Meng

, Wang

, et al., Kinematics of parallel-series position and pose adjustment mechanism for automated docking system of replenishment at sea[C], IEEE International Conference on Information and Automation, USA, IEEE, 2010, pp. 2273–2276.

11.

Wei

, Bingbing

, Guoyan

, et al., Iterative algorithm approximating the minimum norm fixed point and variational inequality solution of strict pseudo-contractive mapping [J], Journal of Jiaxing College26(6) (2014), P58–P63.

12.

Bofu

, Kinetics and Algorithm of Robot Manipulation[M]. Beijing: China Machine Press, 1983, pp. 63–79, 183–208.

13.

Tengmao

, Study on Auxiliary Control of 5-DOF Upper-limb Rehabilitation Robot[D], Harbin Institute of Technology, Harbin, 2007.

14.

Xing

, Jianhui

and Xiaoke

, Dynamic modeling and simulation of 5-DOF upper-limb rehabilitation robot[J], Control Engineering5 (2012), 823–826+831.