Sage Journals: Discover world-class research

Abstract

Dual-arm cooperating manipulators subject to a certain constraint brought about by the desired trajectory and geometric constraint show high nonlinearity and coupling in their dynamic characteristic. Therefore, it is hard to build dynamical equation with traditional Lagrange equation. The Udwadia–Kalaba equation presents a new idea of dynamic modeling of multi-body systems. However, the dynamic modeling of the unconstrained systems still depends on the traditional Lagrange equation and is quite tedious for dual-arm cooperating manipulators. A generalized dynamical equation of multi-link planar manipulators is thus presented and proven to make modeling conveniently. The constraint relationship is established from a new perspective, and the dynamical equation of dual-arm cooperating manipulator subject to the desired trajectory is acquired with the Udwadia–Kalaba equation. The simple approach overcomes the disadvantage of obtaining dynamical equation from traditional Lagrange equation by Lagrange multiplier. The simulation results of varying law of the joint angles and the motion path of the bar prove that the dynamical equation established by this method conforms to reality.

Keywords

Constraint dual-arm cooperating manipulators dynamic modeling Udwadia–Kalaba equation

Introduction

In the field of industrial robotics, the coordinated and collaborative operation of multi-robot performing more complicated and burdensome task instead of human is an irreversible trend. The dual-arm cooperating manipulator (DACM) is a typical example in the multi-robot field. For industrial manipulators, the accurate kinematic and dynamical model is the basis for completing all kinds of tasks. Compared with mature kinematic modeling, the dynamical modeling, especially the manipulator subject to a certain constraint brought about by a desired trajectory, shows high nonlinearity and coupling in its dynamic characteristic. Therefore, it is an important basis for realizing high-quality motion control to establish mathematical model that can describe the dynamic characteristics of this system.

At present, most scholars pay attention to the control algorithm of DACM. Only a few focus on the dynamical modeling problem. When two robotic manipulators hold an object, they create a closed kinematic chain with redundant actuation. This closed-chain system poses the problem of how to reflect the constraint relationship in the dynamic equation. The traditional Lagrange multiplier method was utilized to deal with dynamical modeling of DACM.¹ The object and manipulator dynamics were established, respectively, and the two dynamics equations were combined by the acting force between the object and the end-effectors.² The disadvantage of the Lagrange multiplier method is that it relies on special approaches to determine the multipliers. Finding multipliers to obtain the explicit equations of motion for systems that have large numbers of degrees of freedom and a series of non-integrable constraints is often difficult. Therefore, eliminating the Lagrange multiplier is considered. A general computational scheme of the inverse dynamics of the closed-link mechanisms which is based on d’Alembert’s principle but without computing the Lagrange multiplier was proposed.³ The scheme is computationally efficient compared to the conventional method, while the choice of the cut joints of the closed kinematic loops depends on the redundancy of the actuators.⁴ Kane⁵ proposed a general method to obtain the dynamical system’s equation of motion that originates from Gibbs and Appell’s quasi-coordinates. The equation of motion for the unconstrained planar dual-arm space robot was derived with Kane’s equation.⁶ By inserting the constraint equations in terms of accelerations into the equations of motion for the unconstrained system, the reduced-order equations for the constrained system, which contain only independent motion variables, were obtained. Kane’s equation applies to holonomic and nonholonomic constraints and does not involve the Lagrange multiplier. However, Kane’s method has its disadvantage. It requires a suitable choice of problem-specific generalized speeds that relies largely on experience. Thus, partial velocities largely depend on a specific system. In addition, Kane’s equation is not a general closed-form dynamic equation.

Gibbs–Appell equations require a felicitous choice of problem-specific quasi-coordinates in dealing with systems with a large number of degrees of freedom and many non-integrable constraints. Compared to the Lagrange multiplier method, there is no need to determine the multiplier which is often very difficult to obtain for systems having a large number of degrees of freedom and many non-integrable constraints. Also, if using the formulations offered by Gibbs–Appell equations and Kane’s equation, proper quasi-coordinates will be required, but there is no such problem using the Udwadia–Kalaba approach.

It is well known that it is impossible to create a new fundamental principle for the theory of motion and equilibrium of discrete dynamical systems. In 1996, Firdaus Udwadia of the University of Southern California came up with a new multi-body dynamical modeling method called the Udwadia–Kalaba equation. This method can establish dynamical equations subject to holonomic and nonholonomic constraints in a relatively simple way. With this dynamical equation, the analytical expression of constraint force without Lagrange multiplier can be obtained; this is a significance breakthrough in the field of analytical mechanics.^7–16 However, to date, this method has not been given much attention, and its main application is limited to dynamical modeling and control research of satellite systems. It has seldom been utilized in the field of robotics. A parallel robot dynamical model and a machine fish dynamical model were established through this method.^17,18 A simple industrial robot subject to constraint dynamical model was established based on the Udwadia–Kalaba approach.¹⁹ However, the generalized dynamical equation of multi-link manipulators was not established. So it is difficult to obtain the unconstraint dynamical equation of DACM.

The modeling process of the Udwadia–Kalaba equation involves three steps. First, in terms of the generalized coordinates, the unconstrained dynamical system whose equations of motion can be expressed with the traditional Lagrange method is considered. Second, the constraint equations are formed. Third, the additional generalized forces of constraint are imposed on the system. Specifically, the force resulting from the presence of constraints is added to the unconstrained system’s force.

The rest of this article is organized as follows. First, the Udwadia–Kalaba equation is briefly introduced. Second, the general equations of motion of n-link planar manipulators are presented and proven. They can be utilized to establish the dynamical equation of the unconstrained system conveniently. Third, the geometric constraint relationship is obtained from a new perspective. The motion path of the gripped object and the geometric constraint relationship are regarded as external constraint of DACM, and the dynamical equation is obtained based on the Udwadia–Kalaba equation. Finally, the numerical simulations are performed to demonstrate the efficacy of this method.

Udwadia–Kalaba equation

The general form of the dynamical equation of an industrial manipulator without constraint can be expressed as

M (q) \cdot \overset{\cdot\cdot}{q} + C (q, \overset{\cdot}{q}) \cdot \overset{\cdot}{q} + G (q) = τ

(1)

in which $q$ is the generalized coordinate n-vector. $M (q)$ is the $n \times n$ mass matrix. $C (q, \overset{\cdot}{q}) \cdot \overset{\cdot}{q}$ is the $n \times 1$ centrifugal force and coriolis force vector. $G (q)$ is the gravitational vector of $n \times 1$ which is related to morpheme $q$ of the manipulator. $τ$ is the joint torque vector of $n \times 1$ .

The unconstrained system is subjected to the $p$ constraints provided by

φ_{j} (q, \overset{\cdot}{q}, t) = 0, j = 1, 2, \dots, p

(2)

These $p$ constraints include both geometric and trajectory requirements in this article. Differentiating equation (2) with respect to time twice yields the following constraint equation

A (q, \overset{\cdot}{q}, t) \overset{\cdot\cdot}{q} = b (q, \overset{\cdot}{q}, t)

(3)

in which $A (q, \overset{\cdot}{q}, t)$ is a $p \times n$ matrix and $b (q, \overset{\cdot}{q}, t)$ is a $p \times 1$ vector. $t$ is a time variable. The dynamical equation of the constrained system is obtained as

M (q) \cdot \overset{\cdot\cdot}{q} = Q (q, \overset{\cdot}{q}, t) + Q_{c} (q, \overset{\cdot}{q}, t)

(4)

in which $Q = τ - C (q, \overset{\cdot}{q}) \cdot \overset{\cdot}{q} - G (q)$ . $Q_{c}$ is the control force n-vector that arises to ensure that the control requirements of the form of equation (3) are satisfied. Using the Udwadia–Kalaba equation,⁹ constraint force $Q_{c}$ can be explicitly obtained as

Q_{c} (q, \overset{\cdot}{q}, t) = M (q)^{\frac{1}{2}} B^{+} (q, t) [b (q, \overset{\cdot}{q}, t) - A (q, \overset{\cdot}{q}, t) M^{- 1} (q) Q (q, \overset{\cdot}{q}, t)]

(5)

in which $B (q, t) = A (q, \overset{\cdot}{q}, t) M^{- 1 / 2} (q)$ , and the superscript “+” represents the Moore–Penrose generalized inverse.

The dynamical equation of the constraint to the multi-body mechanical system is shown as the following general form

M (q) \cdot \overset{\cdot\cdot}{q} = Q (q, \overset{\cdot}{q}, t) + M (q)^{\frac{1}{2}} B^{+} (q, t) [b (q, \overset{\cdot}{q}, t) - A (q, \overset{\cdot}{q}, t) M^{- 1} (q) Q (q, \overset{\cdot}{q}, t)]

(6)

Dynamic modeling of n-link planar manipulators

Review of partial derivative of a matrix with respect to a vector

Several basic concepts of matrix calculus should be reviewed to understand the following proof.

Theorem 1

When matrix $A = A (x) \in R^{m \times p}$ is a function of vector $x \in R^{n}$ , and $x (t)$ is a function of the time, the following rule is obtained¹⁹

\frac{d A (x)}{dt} = \frac{\partial A (x)}{\partial x} (I_{n} \otimes \overset{\cdot}{x})

(7)

Theorem 2

When matrix $A \in R^{m \times p}$ and $B \in R^{p \times s}$ are function of vector $x \in R^{n}$ , and $x (t)$ is a function of the time, the following rule is obtained²⁰

\frac{d (A (x) B (x))}{d x} = \frac{\partial A (x)}{\partial x} (B (x) \otimes I_{n}) + A (x) \frac{\partial B (x)}{\partial x}

(8)

in which $I_{n}$ is the $n \times n$ identity matrix.

General equations of n-link planar manipulators

As shown in Figure 1, it is a schematic of n-link planar manipulators. The system generalized coordinate is $q = [\begin{matrix} q_{1} & q_{2} & \dots & q_{n} \end{matrix}]^{T}$ . The lengths of each link are assumed to be $l_{i}$ , the qualities of each link are $m_{i}$ , the torques of each joint are $τ_{i}$ , and the rotational inertias of each link are $I_{i}$ , in which $i = 1, 2, \dots, n$ . $XOY$ is the global coordinate system.

Figure 1.

Sketch map of n-link planar manipulator.

Result

The dynamical equation of n-link planar manipulators can be obtained as

M (q) \cdot \overset{\cdot\cdot}{q} + \tilde{C} (q, \overset{\cdot}{q}) \cdot {\overset{\cdot}{q}}^{2} + G (q) = τ

(9)

in which the elements, $m_{i, j}$ , of the matrix $M (q)$ are provided as

m_{ii} = J_{i} + l_{i}^{2} (\frac{1}{4} m_{i} + \sum_{k = i + 1}^{n} m_{k}) 1 \leq i \leq n

(10)

m_{i j} = l_{i} l_{j} [(\frac{1}{2} m_{j} + \sum_{k = j + 1}^{n} m_{j}) \cos (q_{i} - q_{j})] 1 \leq i < j \leq n

(11)

The elements, ${\tilde{c}}_{i, j}$ , of the matrix $\tilde{C} (q, \overset{\cdot}{q})$ are provided as

{\tilde{c}}_{ii} = 0 1 \leq i \leq n

(12)

{\tilde{c}}_{i j} = l_{i} l_{j} [(\frac{1}{2} m_{j} + \sum_{k = j + 1}^{n} m_{j}) \sin (q_{i} - q_{j})] 1 \leq i < j \leq n

(13)

The elements, $g_{i}$ , of the matrix $G (q)$ are provided as

g_{i}^{n} = g l_{i} [(\frac{1}{2} m_{i} + \sum_{k = i + 1}^{n} m_{i}) \sin q_{i}] 1 \leq i \leq n

(14)

Proof

The equation of motion for the unconstrained multi-body systems using Lagrange method is given as²¹

τ = \frac{d}{dt} {(\frac{\partial T}{\partial \overset{\cdot}{q}})}^{T} - {(\frac{\partial T}{\partial q})}^{T} + {(\frac{\partial E}{\partial q})}^{T}

(15)

The kinetic energy $T$ of the system can be expressed as

T = \frac{1}{2} {\overset{\cdot}{q}}^{T} M (q) \overset{\cdot}{q}

(16)

The $n \times n$ matrix $M (q)$ has the following form²⁰

M (q) = \sum_{i = 1}^{n} (m_{i} J_{L_{i}}^{T} J_{L_{i}} + J_{R_{i}}^{T} I_{i} J_{R_{i}})

(17)

in which $J_{L_{i}}$ and $J_{R_{i}}$ are the translational and rotational Jacobian matrices of the link $i$ , respectively.

The coordinates $(x_{n}, y_{n})$ of the n-link planar manipulators of the nth link can be expressed as

\begin{matrix} x_{n} = \sum_{j = 1}^{n - 1} l_{j} \cos (q_{j}) + \frac{1}{2} l_{n} \cos (q_{n}) \\ y_{n} = \sum_{j = 1}^{n} l_{j} \sin (q_{j}) + \frac{1}{2} l_{n} \sin (q_{n}) \end{matrix}

(18)

Differentiating equation (18) with respect to time once yields

\begin{matrix} {\overset{\cdot}{x}}_{n} = - \sum_{j = 1}^{n - 1} l_{j} \sin (q_{j}) {\overset{\cdot}{q}}_{j} - \frac{1}{2} l_{n} \sin (q_{n}) {\overset{\cdot}{q}}_{n} \\ {\overset{\cdot}{y}}_{n} = \sum_{j = 1}^{n} l_{j} \cos (q_{j}) {\overset{\cdot}{q}}_{j} + \frac{1}{2} l_{n} \cos (q_{n}) {\overset{\cdot}{q}}_{n} \end{matrix}

(19)

Thus, $J_{L_{i}}$ can be obtained as

J_{L_{1}} = {[\begin{matrix} - \frac{1}{2} l_{1} \sin (q_{1}) & 0 & \dots & 0 \\ \frac{1}{2} l_{1} \cos (q_{1}) & 0 & \dots & 0 \\ 0 & 0 & \dots & 0 \\ ⋮ & ⋮ & ⋮ & ⋮ \\ 0 & 0 & \dots & 0 \end{matrix}]}_{n \times n}, J_{L_{2}} = {[\begin{matrix} - l_{1} \sin (q_{1}) & - \frac{1}{2} l_{2} \sin (q_{2}) & 0 & \dots & 0 \\ l_{1} \cos (q_{1}) & \frac{1}{2} l_{2} \cos (q_{2}) & 0 & \dots & 0 \\ 0 & 0 & 0 & \dots & 0 \\ ⋮ & ⋮ & ⋮ & ⋮ & ⋮ \\ 0 & 0 & 0 & \dots & 0 \end{matrix}]}_{n \times n}, \dots, J_{L_{n}} = {[\begin{matrix} - l_{1} \sin (q_{1}) & - l_{2} \sin (q_{2}) & \dots & - \frac{l_{n}}{2} \sin (q_{n}) \\ l_{1} \cos (q_{1}) & l_{2} \cos (q_{2}) & \dots & \frac{l_{n}}{2} \cos (q_{n}) \\ 0 & 0 & \dots & 0 \\ ⋮ & ⋮ & ⋮ & ⋮ \\ 0 & 0 & \dots & 0 \end{matrix}]}_{n \times n}

(20)

The angular velocity of links may be expressed as

ω_{1} = {[\begin{matrix} 0 \\ 0 \\ ⋮ \\ {\overset{\cdot}{q}}_{1} \end{matrix}]}_{n \times 1}, ω_{2} = {[\begin{matrix} 0 \\ 0 \\ ⋮ \\ {\overset{\cdot}{q}}_{2} \end{matrix}]}_{n \times 1}, \dots, ω_{n} = {[\begin{matrix} 0 \\ 0 \\ ⋮ \\ {\overset{\cdot}{q}}_{n} \end{matrix}]}_{n \times 1}

(21)

Thus, $J_{R_{i}}$ can be expressed as

J_{R_{1}} = {[\begin{matrix} 0 & 0 & \dots & 0 \\ 0 & 0 & \dots & 0 \\ ⋮ & 0 & \dots & 0 \\ 0 & ⋮ & \dots & ⋮ \\ 1 & 0 & \dots & 0 \end{matrix}]}_{n \times n}, J_{R_{2}} = {[\begin{matrix} 0 & 0 & \dots & \dots & 0 \\ 0 & 0 & \dots & \dots & 0 \\ ⋮ & ⋮ & ⋮ & ⋮ & ⋮ \\ 0 & 0 & 0 & \dots & 0 \\ 0 & 1 & 0 & \dots & 0 \end{matrix}]}_{n \times n}, \dots, J_{R_{n}} = {[\begin{matrix} 0 & \dots & \dots & 0 \\ ⋮ & ⋮ & ⋮ & ⋮ \\ ⋮ & ⋮ & ⋮ & 0 \\ 0 & \dots & 0 & 1 \end{matrix}]}_{n \times n}

(22)

Substituting equations (20) and (22) into equation (17) yields equations (10) and (11).

Using equations (7) and (8), the partial derivative of the kinetic energy $T$ with respect to vector $\overset{\cdot}{q}$ is obtained as

\begin{matrix} \frac{\partial T}{\partial \dot{q}} = \frac{1}{2} \frac{\partial}{\partial \dot{q}} [{\dot{q}}^{T} M (q) \dot{q}] \\ = \frac{1}{2} {\frac{\partial {\dot{q}}^{T}}{\partial \dot{q}} [(M (q) \dot{q}) \otimes I_{n}] + {\dot{q}}^{T} \frac{\partial}{\partial \dot{q}} (M (q) \dot{q})} \end{matrix}

(23)

in which

\begin{matrix} \frac{\partial {\overset{\cdot}{q}}^{T}}{\partial \overset{\cdot}{q}} [(M (q) \overset{\cdot}{q}) \otimes I_{n}] = [\begin{matrix} e_{1}^{T} & e_{2}^{T} & \dots & e_{n}^{T} \end{matrix}] [\begin{matrix} {(M (q) \overset{\cdot}{q})}_{1} I_{n} \\ {(M (q) \overset{\cdot}{q})}_{2} I_{n} \\ ⋮ \\ {(M (q) \overset{\cdot}{q})}_{n} I_{n} \end{matrix}] \\ = {[M (q) \overset{\cdot}{q}]}^{T} \\ = {\overset{\cdot}{q}}^{T} M (q) \end{matrix}

(24)

{\dot{q}}^{T} \frac{\partial}{\partial \dot{q}} (M (q) \dot{q}) = {\dot{q}}^{T} [\frac{\partial M (q)}{\partial \dot{q}} (\dot{q} \otimes I_{n}) + M (q) \frac{\partial \dot{q}}{\partial \dot{q}}] = {\dot{q}}^{T} M (q)

(25)

Substituting equations (24) and (25) into equation (23) yields

\frac{\partial T}{\partial \overset{\cdot}{q}} = {\overset{\cdot}{q}}^{T} M (q)

(26)

\frac{d}{dt} {(\frac{\partial T}{\partial \overset{\cdot}{q}})}^{T} = M (q) \overset{\cdot\cdot}{q} + \overset{\cdot}{M} (q) \overset{\cdot}{q}

(27)

The partial derivative of the kinetic energy $T$ with respect to vector $q$ is obtained as

\begin{matrix} \frac{\partial T}{\partial q} = \frac{1}{2} \frac{\partial}{\partial q} [{\overset{\cdot}{q}}^{T} M (q) \overset{\cdot}{q}] \\ = \frac{1}{2} {\frac{\partial {\overset{\cdot}{q}}^{T}}{\partial q} [(M (q) \overset{\cdot}{q}) \otimes I_{n}] + {\overset{\cdot}{q}}^{T} \frac{\partial}{\partial q} (M (q) \overset{\cdot}{q})} \\ = \frac{1}{2} {\overset{\cdot}{q}}^{T} \frac{\partial}{\partial q} (M (q) \overset{\cdot}{q}) \\ = \frac{1}{2} {\overset{\cdot}{q}}^{T} [\frac{\partial M (q)}{\partial q} (\overset{\cdot}{q} \otimes I_{n}) + M (q) \frac{\partial \overset{\cdot}{q}}{\partial q}] \\ = \frac{1}{2} {\overset{\cdot}{q}}^{T} \frac{\partial M (q)}{\partial q} (\overset{\cdot}{q} \otimes I_{n}) \end{matrix}

(28)

The potential energy $E$ can be expressed as

\begin{matrix} E (q) = \frac{1}{2} m_{1} g l_{1} \sin (q_{1}) \\ + [l_{1} \sin (q_{1}) + \frac{1}{2} l_{2} \sin (q_{2})] m_{2} g + \dots \\ + [l_{1} \sin (q_{1}) + l_{2} \sin (q_{2}) + \dots + \frac{1}{2} l_{n} \sin (q_{n})] m_{n} g \end{matrix}

(29)

The partial derivative of the potential energy $E$ with respect to vector $q$ is obtained as

G (q) = \frac{\partial E (q)}{\partial q} = [\begin{matrix} (\frac{1}{2} m_{1} + m_{2} + \dots + m_{n}) g l_{1} \cos (q_{1}) \\ (\frac{1}{2} m_{2} + \dots + m_{n}) g l_{2} \cos (q_{2}) \\ ⋮ \\ \frac{1}{2} m_{n} g l_{n} \cos (q_{n}) \end{matrix}]

(30)

Substituting equations (27) and (28) into equation (15) yields

\begin{matrix} τ = \frac{d}{dt} {(\frac{\partial T}{\partial \overset{\cdot}{q}})}^{T} - {(\frac{\partial T}{\partial q})}^{T} + \frac{\partial E (q)}{\partial q} \\ = M (q) \overset{\cdot\cdot}{q} + \overset{\cdot}{M} (q) \overset{\cdot}{q} - \frac{1}{2} {[{\overset{\cdot}{q}}^{T} \frac{\partial M (q)}{\partial q} (\overset{\cdot}{q} \otimes I_{n})]}^{T} + \frac{\partial E (q)}{\partial q} \end{matrix}

(31)

Thus, the following equation can be obtained as

\begin{matrix} C (q, \overset{\cdot}{q}) \overset{\cdot}{q} = \overset{\cdot}{M} (q) \overset{\cdot}{q} - \frac{1}{2} {[{\overset{\cdot}{q}}^{T} \frac{\partial M (q)}{\partial q} (\overset{\cdot}{q} \otimes I_{n})]}^{T} \\ = {\frac{\partial M (q)}{\partial q} (I_{n} \otimes \overset{\cdot}{q}) - \frac{1}{2} {[\frac{\partial M (q)}{\partial q} (\overset{\cdot}{q} \otimes I_{n})]}^{T}} \overset{\cdot}{q} \end{matrix}

(32)

in which

\frac{\partial M (q)}{\partial q} = [\begin{matrix} 0 \dots 0 & \frac{\partial m_{ij}}{\partial q_{i}} \\ ⋱ \\ \frac{\partial m_{ij}}{\partial q_{j}} & 0 \dots 0 \end{matrix}]

(33)

I_{n} \otimes \overset{\cdot}{q} = [\begin{matrix} I_{n} {\overset{\cdot}{q}}_{1} \\ I_{n} {\overset{\cdot}{q}}_{2} \\ ⋱ \\ I_{n} {\overset{\cdot}{q}}_{n} \end{matrix}]

(34)

\overset{\cdot}{q} \otimes I_{n} = [\begin{matrix} I_{n} {\overset{\cdot}{q}}_{1} \\ I_{n} {\overset{\cdot}{q}}_{2} \\ ⋮ \\ I_{n} {\overset{\cdot}{q}}_{n} \end{matrix}]

(35)

in which

\begin{matrix} \frac{\partial m_{ij}}{\partial q_{i}} = - l_{i} l_{j} [(\frac{1}{2} m_{j} + \sum_{k = j + 1}^{n} m_{j}) \sin (q_{i} - q_{j})] \\ \frac{\partial m_{ij}}{\partial q_{j}} = l_{i} l_{j} [(\frac{1}{2} m_{j} + \sum_{k = j + 1}^{n} m_{j}) \sin (q_{i} - q_{j})] \end{matrix}

Substituting equations (33)–(35) into equation (32) yields

C (q, \overset{\cdot}{q}) \overset{\cdot}{q} = \tilde{C} (q, \overset{\cdot}{q}) {\overset{\cdot}{q}}^{2}

(36)

Dynamic modeling and simulation of DACM

Dynamic modeling on manipulator without constraint

As shown in Figure 2, it is a schematic of two three-link robot manipulators carrying a bar. $X O_{1} Y$ is the global coordinate system. $xAy$ is the local coordinate system of the end-effector of the left robot manipulator. $O_{1} O_{2} = L$ is the distance between the two base frames of the robot manipulators. $AB$ is a bar. The bar is assumed to be rigidly held by the two end-effectors, so the two end-effectors and the bar are regarded as one link, as shown in Figure 3. Therefore, the system can be regarded as a closed-loop five-link mechanism. The system generalized coordinate is assumed to be $θ = [\begin{matrix} q_{1} & q_{2} & q_{3} & q_{4} & q_{5} \end{matrix}]^{T}$ .

Figure 2.

Sketch map of DACM carrying a bar.

Figure 3.

Sketch map of the relationship between the bar and the end-effectors.

As shown in Figure 2, the lengths of each link are assumed to be $l_{1}, l_{2}, l_{3}, l_{4}, l_{5}$ ; the masses of each link are $m_{1}, m_{2}, m_{3}, m_{4}, m_{5}$ ; and the rotational inertias of each link are $I_{1}, I_{2}, I_{3}, I_{4}, I_{5}$ . The dynamical equation of the unconstrained system can be acquired with the Lagrange equation as follows

M (θ) \cdot \overset{\cdot\cdot}{θ} + \tilde{C} (θ) \cdot {\overset{\cdot}{θ}}^{2} + G (θ) = τ

(37)

in which $M (θ)$ is the $5 \times 5$ inertial matrix. $\tilde{C} (θ) \cdot {\overset{\cdot}{θ}}^{2}$ is the $5 \times 1$ centrifugal force and coriolis force vector. $G (θ)$ is the gravitational vector of $5 \times 1$ which is related to morpheme $θ$ of the manipulator. $τ$ is the joint torque vector of $5 \times 1$ . Using equations (10)–(14), the elements of the matrices $M (θ)$ , $\tilde{C} (θ)$ , and $G (θ)$ can be obtained.

Then, the following dynamic equation is obtained, which is of the form

M \overset{\cdot\cdot}{θ} = Q

(38)

Given that the constraints have not been considered thus far, $τ = [\begin{matrix} 0 & 0 & 0 & 0 & 0 \end{matrix}]^{T}$ . The following equation is obtained, which is of the form

Q = - \tilde{C} (θ) {\overset{\cdot}{θ}}^{2} - G (θ)

(39)

Dynamics modeling subject to constraint

A generalized constraint relationship is proposed, namely, the left end-effector always points to the right end-effector. That is, the vector $\vec{AB}$ is always parallel to the x-axis of the coordinate system of the left end-effector. Specifically

\hat{x} \times \vec{AB} = 0

(40)

in which $\hat{x} = [\begin{matrix} 1 & 0 & 0 \end{matrix}]^{T}$ . $\vec{AB} = ξ = [\begin{matrix} X_{B} - X_{A} Y_{B} - Y_{A} Z_{B} - Z_{A} \end{matrix}]^{T}$ . $(\begin{matrix} X_{A} & Y_{A} \end{matrix})$ and $(\begin{matrix} X_{B} & Y_{B} \end{matrix})$ are the endpoint coordinates of the bar in the global coordinate system. The Euler angle of the left end-effector is assumed to be $(α, β, γ)$ . × denotes the cross product between the vectors. Let $R$ be the transformation matrix that maps the overall frame into the local frame, which is of the form²¹

\begin{matrix} R = [\begin{matrix} R_{1} \\ R_{2} \\ R_{3} \end{matrix}] = [\begin{matrix} R_{11} & R_{12} & R_{13} \\ R_{21} & R_{22} & R_{23} \\ R_{31} & R_{32} & R_{33} \end{matrix}] \\ = [\begin{matrix} \cos α \cos β & \sin α \cos β & - \sin β \\ \cos α \sin β \sin γ - \sin α \cos γ & \sin α \sin β \sin γ + \cos α \cos γ & \cos β \sin γ \\ \cos α \sin β \cos γ + \sin α \sin γ & \sin α \sin β \cos γ - \cos α \sin γ & \cos β \cos γ \end{matrix}] \end{matrix}

(41)

Then equation (40) is rewritten as

[\begin{matrix} i & j & k \\ 1 & 0 & 0 \\ R_{1} ξ & R_{2} ξ & R_{3} ξ \end{matrix}] = 0 i - R_{3} ξ j + R_{2} ξ k = [\begin{matrix} 0 \\ 0 \\ 0 \end{matrix}]

(42)

Hence

R_{21} (X_{B} - X_{A}) + R_{22} (Y_{B} - Y_{A}) + R_{23} (Z_{B} - Z_{A}) = 0

(43)

Because of $Z_{A} = Z_{B}$ , $γ = 0$ , and the two end-effectors and the bar are regarded as one link, so $α = q_{3}$ . Substituting $R$ into equation (43) yields

\frac{Y_{B} - Y_{A}}{X_{B} - X_{A}} = - \frac{R_{21}}{R_{22}} = \tan α = \tan q_{3}

(44)

in which

\begin{array}{l} Y_{B} = - (l_{4} \sin q_{4} + l_{5} \sin q_{5}), X_{B} = L - l_{4} \cos q_{4} - l_{5} \cos q_{5} \\ Y_{A} = l_{1} \sin q_{1} + l_{2} \sin q_{2}, X_{A} = l_{1} \cos q_{1} + l_{2} \cos q_{2} \end{array}

(45)

Substituting equation (45) into equation (44) yields

\frac{- (l_{4} \sin q_{4} + l_{5} \sin q_{5}) - l_{1} \sin q_{1} - l_{2} \sin q_{2}}{L - l_{4} \cos q_{4} - l_{5} \cos q_{5} - l_{1} \cos q_{1} - l_{2} \cos q_{2}} = \frac{l_{3} \sin q_{3}}{l_{3} \cos q_{3}}

(46)

According to the corresponding result of the numerator and denominator, we can get

l_{1} \cos q_{1} + l_{2} \cos q_{2} + l_{3} \cos q_{3} + l_{4} \cos q_{4} + l_{5} \cos q_{5} = L

(47)

l_{1} \sin q_{1} + l_{2} \sin q_{2} + l_{3} \sin q_{3} = - (l_{4} \sin q_{4} + l_{5} \sin q_{5})

(48)

Differentiating equation (47), equation (48) with respect to time twice results in

A_{1} \overset{\cdot\cdot}{θ} = b_{1}

(49)

in which

A_{1} = [\begin{matrix} - l_{1} \sin q_{1} & - l_{2} \sin q_{2} & - l_{3} \sin q_{3} & - l_{4} \sin q_{4} & - l_{5} \sin q_{5} \\ l_{1} \cos q_{1} & l_{2} \cos q_{2} & l_{3} \cos q_{3} & l_{4} \cos q_{4} & l_{5} \cos q_{5} \end{matrix}]

b_{1} = [\begin{array}{l} l_{1} \cos q_{1} {\dot{q}}_{1}^{2} + l_{2} \cos q_{2} {\dot{q}}_{2}^{2} + l_{3} \cos q_{3} {\dot{q}}_{3}^{2} + l_{4} \cos q_{4} {\dot{q}}_{4}^{2} + l_{5} \cos q_{5} {\dot{q}}_{5}^{2} \\ l_{1} \sin q_{1} {\dot{q}}_{1}^{2} + l_{2} \sin q_{2} {\dot{q}}_{2}^{2} + l_{3} \sin q_{3} {\dot{q}}_{3}^{2} + l_{4} \sin q_{4} {\dot{q}}_{4}^{2} + l_{5} \sin q_{5} {\dot{q}}_{5}^{2} \end{array}]

The trajectory constraint of the bar is summarized as

\begin{matrix} X_{3} = 1.5 \\ Y_{3} = 0.5 (1 - \cos t) \end{matrix}

(50)

in which $(\begin{matrix} X_{3} & Y_{3} \end{matrix})$ is the center-of-mass coordinate of the bar in the global coordinate system.

As shown in Figure 2, the following equation is also obtained

\begin{matrix} X_{3} = l_{1} \cos q_{1} + l_{2} \cos q_{2} + \frac{1}{2} l_{3} \cos q_{3} \\ Y_{3} = l_{1} \sin q_{1} + l_{2} \sin q_{2} + \frac{1}{2} l_{3} \sin q_{3} \end{matrix}

(51)

Differentiating equation (51) with respect to time twice

\begin{matrix} {\overset{\cdot\cdot}{X}}_{3} = - l_{1} \sin q_{1} {\overset{\cdot\cdot}{q}}_{1} - l_{2} \sin q_{2} {\overset{\cdot\cdot}{q}}_{2} - \frac{1}{2} l_{3} \\ \sin q_{3} {\overset{\cdot\cdot}{q}}_{3} - l_{1} \cos q_{1} {\overset{\cdot}{q}}_{1}^{2} - l_{2} \cos q_{2} {\overset{\cdot}{q}}_{2}^{2} - \frac{1}{2} l_{3} \cos q_{3} {\overset{\cdot}{q}}_{3}^{2} \\ {\overset{\cdot\cdot}{Y}}_{3} = l_{1} \cos q_{1} {\overset{\cdot\cdot}{q}}_{1} + l_{2} \cos q_{2} {\overset{\cdot\cdot}{q}}_{2} + \frac{1}{2} l_{3} \cos q_{3} {\overset{\cdot\cdot}{q}}_{3} - l_{1} \\ \sin q_{1} {\overset{\cdot}{q}}_{1}^{2} - l_{2} \sin q_{2} {\overset{\cdot}{q}}_{2}^{2} - \frac{1}{2} l_{3} \sin q_{3} {\overset{\cdot}{q}}_{3}^{2} \end{matrix}

(52)

Differentiating equation (49) with respect to time twice

\begin{matrix} {\overset{\cdot\cdot}{X}}_{3} = 0 \\ {\overset{\cdot\cdot}{Y}}_{3} = 0.5 \cos t \end{matrix}

(53)

According to equations (52) and (53), the following constraint equation can be obtained as

A_{2} \overset{\cdot\cdot}{θ} = b_{2}

(54)

in which

A_{2} = [\begin{matrix} - l_{1} \sin q_{1} & - l_{2} \sin q_{2} & - \frac{l_{3}}{2} \sin q_{3} & 0 & 0 \\ l_{1} \cos q_{1} & l_{2} \cos q_{2} & \frac{l_{3}}{2} \cos q_{3} & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 \end{matrix}]

b_{2} = [\begin{array}{l} l_{1} \cos q_{1} {\dot{q}}_{1}^{2} + l_{2} \cos q_{2} {\dot{q}}_{2}^{2} + \frac{1}{2} l_{3} \cos q_{3} {\dot{q}}_{3}^{2} \\ l_{1} \sin q_{1} {\dot{q}}_{1}^{2} + l_{2} \sin q_{2} {\dot{q}}_{2}^{2} + \frac{1}{2} l_{3} \sin q_{3} {\dot{q}}_{3}^{2} + 0.5 \sin t \end{array}]

The following constraint equations are obtained, which are of the form of equation (3)

[\begin{matrix} A_{1} \\ A_{2} \end{matrix}] \overset{\cdot\cdot}{θ} = [\begin{matrix} b_{1} \\ b_{2} \end{matrix}]

(55)

Then, the additional torques for the system to satisfy the given trajectory are explicitly determined by

Q_{C} = M^{\frac{1}{2}} {(A M^{- \frac{1}{2}})}^{+} (b - A M^{- 1} Q)

(56)

in which $A = [\begin{matrix} A_{1} & A_{2} \end{matrix}]^{T}$ and $b = [\begin{matrix} b_{1} & b_{2} \end{matrix}]^{T}$ .

Finally, dynamic equation of the DACM is obtained as

M \overset{\cdot\cdot}{θ} = Q + Q_{C}

(57)

Result and simulation analysis

It is assumed that the initial configuration are $q_{1} = q_{5} = 60^{\circ}$ , $q_{3} = 0$ , $q_{2} = q_{4} = - 60^{\circ}$ , ${\overset{\cdot}{q}}_{1} = {\overset{\cdot}{q}}_{2} = {\overset{\cdot}{q}}_{3} = {\overset{\cdot}{q}}_{4} = {\overset{\cdot}{q}}_{5} = 0 rad / s$ , $m_{i} = 1 kg$ , $l_{i} = 1 m$ , and $I_{i} = 0.33 kg m^{2}$ ( $i = 1, 2, 3, 4, 5$ ). The simulations are performed on a 2.9 GHz Intel Core2 Quad PC computer. The ordinary differential equation (ODE) solver ODE45 in MATLAB 2011a is used to numerically integrate the system, and the error tolerance is set to $10^{- 12}$ . The simulation time is 20 s. The initial configuration schematic is shown in Figure 4.

Figure 4.

Sketch map of the initial configuration of DACM carrying a bar.

The theoretical curve of the joint angles can be obtained by solving equations (47), (48), (50), and (51). It can be regarded as the reference curve for the following study. Figure 5 presents the joint angles at each joint of the robotic system, in which $q_{i th}$ represents the theoretical value, $q_{i - num}$ presents the numerical integration value. It can be seen that the dash line approximates the solid line, and also the obvious error is excited. $q_{3} \equiv 0$ means that the third link always remains horizontal. The first and fifth links move from the initial configuration to 90° and then return to the initial position. The second and fourth links move from the initial configuration to the horizontal position and then return to the initial position. The first, second, fourth, and fifth link’s movements are periodic, which leads to the periodic movement of the bar from the lowest position to the highest one and then back to the lowest position. Figures 6 and 7 depict the numerical results of whether the geometric constraints in the X- and Y-directions are followed. Figures 8 and 9 show the numerical results of whether the motion trajectories of the bar in the X- and Y-directions are followed. The major drift is observed in the X-direction. Figure 10 presents displacement errors for the geometric constraint and trajectory constraint described by equations (47) and (51)₁. The largest displacement error corresponds to about $8 \times 10^{- 3}$ and $18 \times 10^{- 3} m$ . This is very small when compared with $X_{3} = 1.5 m$ and $L = 3 m$ . Therefore, the errors are so small that the constraint relationship in the X-direction is being maintained well as desired.

Figure 5.

Sketch map of the joint angles.

Figure 6.

Comparison of the geometric constraint in the X-direction.

Figure 7.

Comparison of the geometric constraint in the Y-direction.

Figure 8.

Comparison of the trajectory constraint in the X-direction.

Figure 9.

Comparison of the trajectory constraint in the Y-direction.

Figure 10.

Displacement errors in the X-direction.

The DACM have six joints and every joint has one actuator. However, the system dynamic equation is obtained using five generalized coordinates and the degree of freedom of DACM is 3. That is to say, the servo control of the coordinated robot is actually a redundantly actuated problem. The additional torques $Q_{c}$ are provided by the adjacent actuators, namely, $Q_{c_{1}} = τ_{1} + φ τ_{2}$ , $Q_{c_{2}} = τ_{2} + η τ_{3}$ , $Q_{c_{3}} = τ_{3} + ψ τ_{4}$ , $Q_{c_{4}} = - τ_{4} + λ τ_{5}$ , and $Q_{c_{5}} = - τ_{5} + μ τ_{6}$ , which are of the form of matrix

[\begin{matrix} Q_{c 1} \\ Q_{c 2} \\ Q_{c 3} \\ Q_{c 4} \\ Q_{c 5} \end{matrix}] = [\begin{matrix} 1 & φ & 0 & 0 & 0 & 0 \\ 0 & 1 & η & 0 & 0 & 0 \\ 0 & 0 & 1 & ψ & 0 & 0 \\ 0 & 0 & 0 & - 1 & λ & 0 \\ 0 & 0 & 0 & 0 & - 1 & μ \end{matrix}] [\begin{matrix} τ_{1} \\ τ_{2} \\ τ_{3} \\ τ_{4} \\ τ_{5} \\ τ_{6} \end{matrix}]

(58)

Then, the joint torques can be obtained as

[\begin{matrix} τ_{1} \\ τ_{2} \\ τ_{3} \\ τ_{4} \\ τ_{5} \\ τ_{6} \end{matrix}] = {[\begin{matrix} 1 & φ & 0 & 0 & 0 & 0 \\ 0 & 1 & η & 0 & 0 & 0 \\ 0 & 0 & 1 & ψ & 0 & 0 \\ 0 & 0 & 0 & - 1 & λ & 0 \\ 0 & 0 & 0 & 0 & - 1 & μ \end{matrix}]}^{+} [\begin{matrix} Q_{c 1} \\ Q_{c 2} \\ Q_{c 3} \\ Q_{c 4} \\ Q_{c 5} \end{matrix}]

(59)

in which the superscript + represents the Moore–Penrose generalized inverse. $φ$ , $η$ , $ψ$ , $λ$ , and $μ$ are the actuator constants. $φ = η = - 1$ , $ψ = 1$ , and $λ = μ = 1$ . Figure 11 depicts the additional torques of every link and their magnitude for satisfying the trajectory requirement of the bar. Figure 12 represents the constraint torques to be required at each joint for its movement.

Figure 11.

Sketch map of the additional torque.

Figure 12.

Sketch map of the joint torque.

Conclusion and future work

With the aim of dynamic modeling of DACM, this study establishes the dynamic equation based on the Udwadia–Kalaba equation. The research process results in several conclusions as follows:

The general dynamic equations of n-link planar manipulators are presented and proven to establish the dynamic equations of the unconstrained DACM.

The nature of the constraint is revealed by a generalized constraint relationship, and the analytical formula is obtained. The geometric constraint of the DACM and the trajectory of the bar are regarded as the external constraint of the system, and the constraint relationship of the system is integrated into the dynamic equation dexterously based on the idea of the Udwadia–Kalaba equation. The simple app-roach overcomes the disadvantage of obtaining dynamical equation from traditional Lagrange equation by Lagrange multiplier effectively.

The numerical simulation results prove that the dynamical equation established by this method conforms to reality largely. However, the simulation is executed on the condition which the initial condition satisfies the constrained equation, then the next research direction is the dynamics modeling of DACM which the initial condition does not satisfy the constrained equation and the error reduction of the dynamic modeling.

Footnotes

Academic Editor: Antonio Mendes Lopes

Declaration of conflicting interests

The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Funding

The author(s) received no financial support for the research, authorship, and/or publication of this article.

References

Korayem

Nekoo

Esfeden

RA.

Dynamic load-carrying capacity of multi-arm cooperating wheeled mobile robots via optimal load distribution method. Arab J Sci Eng 2014; 39: 6421–6433.

Sheng

Shi

. The novel approaches for computing the dynamic load-carrying capacity of multiple cooperating robotic manipulators. Mech Mach Theory 1998; 34: 637–643.

Nakamura

Ghodoussi

Dynamics computation of closed-link robot mechanisms with nonredundant and redundant actuators. IEEE T Robotic Autom 1989; 5: 294–302.

Wang

LCT

Ming

. Dynamic load-carrying capacity and inverse dynamics of multiple cooperating robotic manipulators. IEEE T Robotic Autom 1994; 10: 71–77.

Kane

TR.

Dynamics of non-holonomic systems. J Appl Mech 1961; 28: 574–578.

Jia

SJ.

Dynamics and adaptive control of a dual-arm space robot with closed-loop constraints and uncertain inertial parameters. Acta Mech Sinica 2014; 1: 112–124.

Udwadia

Kalaba

RE.

A new perspective on constrained motion. P Roy Soc Lond A Mat 1992; 439: 407–410.

Udwadia

Schutte

AD.

Equations of motion for general constrained systems in Lagrangian mechanics. Acta Mech 2010; 213: 111–129.

Udwadia

Kalaba

RE.

Explicit equations of motion for mechanical systems with nonideal constraints. J Appl Mech 2001; 68: 462–467.

10.

Udwadia

Kalaba

RE.

On constrained motion. Appl Math Comput 2005; 164: 313–320.

11.

Udwadia

Wanichanon

Control of uncertain nonlinear multi-body mechanical systems. J Appl Mech 2013; 81: 1–11.

12.

Pennestri

Valentini

Falco

DD.

An application of the Udwadia–Kalaba dynamic formulation to flexible multi-body systems. J Franklin 2010; 347: 173–194.

13.

Schutte

Udwadia

FE.

New approach to the modeling of complex multi-body dynamical systems. J Appl Mech 2010; 78: 856–875.

14.

Cho

New approach to satellite formation-keeping: exact solution to the full nonlinear problem. J Aerospace Eng 2009; 22: 445–455.

15.

Zhen

Zhao

Huang

. On Kepler’s law: application of the Udwadia–Kalaba theory. Sci Sin: Phys Mech Astron 2014; 44: 24–31.

16.

Zhen

Huang

Zhao

. Why can a free-falling cat always manage to land safely on its feet?Nonlinear Dyn 2015; 79: 2237–2250.

17.

Huang

Chen

Zhong

ZH.

Udwadia–Kalaba approach for parallel manipulator dynamics. J Dyn Syst: T ASME 2013; 135: 1012–1030.

18.

Zhao

Zhen

Chen

YH.

Dynamic modeling and simulation of multi-body systems using the Udwadia–Kalaba theory. Chin J Mech Eng 2013; 26: 839–850.

19.

Liu

Simple method to the dynamic modeling of industrial robot subject to constraint. Adv Mech Eng 2016; 8: 1–9.

20.

Khang

NV.

Consistent definition of partial derivatives of matrix functions in dynamics of mechanical systems. Mech Mach Theory 2010; 45: 981–988.

21.

Xiong

YL.

Fundamentals of robot techniques. 15th ed.Wuhan, China: Huazhong, 1996, p.25.

Dynamic modeling of dual-arm cooperating manipulators based on Udwadia–Kalaba equation

Abstract

Keywords

Introduction

Udwadia–Kalaba equation

Dynamic modeling of n-link planar manipulators

Review of partial derivative of a matrix with respect to a vector

Theorem 1

Theorem 2

General equations of n-link planar manipulators

Result

Proof

Dynamic modeling and simulation of DACM

Dynamic modeling on manipulator without constraint

Dynamics modeling subject to constraint

Result and simulation analysis

Conclusion and future work

Footnotes

Declaration of conflicting interests

Funding

References