Manipulator Dynamics

Abstract

This book chapter is about fundamentals of manipulator dynamics and their applications. Two approaches of manipulator dynamics, namely, recursive Newton-Euler approach and the Lagrange equations, are introduced and discussed. Examples are included to demonstrate their application in manipulator dynamics simulations and analysis. This book chapter can provide basic understanding on manipulator dynamics, which is applicable to manipulators, including serial and parallel manipulators.

These keywords were added by machine and not by the authors. This process is experimental and the keywords may be updated as the learning algorithm improves.

Introduction

Manipulator dynamics concerns with the actuation forces required to generate motion of manipulators. The manipulator dynamics is expressed in terms of equations of motion, which describe the relationships between the actuation, reaction forces, and the accelerations and motion trajectories of manipulators. In robotics, dynamics is a fundamental for the manipulator design, analysis, and control.

Two types of problems are involved in the dynamics study, namely, the inverse and forward dynamics. The inverse dynamics calculates the required actuating forces at joints for a prescribed trajectories or known kinematic information (position, velocity, and acceleration). The forward dynamics, on the other hand, determines the joints’ and end-effector accelerations with applied actuating forces at joints.

The equations of motion can be established with several approaches. The most used approaches include recursive Newton-Euler approach and the Lagrange equations, which can be found in literatures (Murray et al. 1994; Angeles 1997; Featherstone and Orin 2000). In this chapter, the dynamics equation formulation with the above two approaches is introduced. The formulations are confined within the rigid-body dynamics. Readers with interests in flexible body manipulators are directed to literatures (Shabana 2013; Low 1987; Dwivedy and Eberhard 2006). Two examples are included to demonstrate their applications, one applicable to a serial manipulator and the other to a parallel robot.

Recursive Newton-Euler Method

The motion of a rigid body can be described by the Newton and Euler’s equations, the former for the translation and the latter for the rotation. Basically, the equations of motion of a manipulator can be established for each body of the manipulator on the basis of free-body diagrams. Alternatively, the equations of motion can be formulated through the recursive Newton-Euler method for serial manipulators, with which the free-body diagrams can be omitted. The formulation presented is based on the method introduced by Luh et al. (1980). This formulation is well suited for inverse dynamics problems, i.e., to find the driving torques with known kinematics information.

The recursive Newton-Euler method consists of two steps. The first step is an outward recursion, from the base to the end link, to calculate velocity and acceleration. The second step is an inward recursion, from the end link to the base, to calculate the forces and torques at joints. In the following formulation, the recursive method is applied to a manipulator with n rigid links. Coordinate systems are established by following Denavit-Hartenberg’s convention (D-H convention) (Denavit and Hartenberg 1955).

Outward Recursion to Calculate Velocities and Accelerations

In the inverse dynamics problems, the positions, velocities, and accelerations (${\theta}_i,{\dot{\theta}}_i,{\ddot{\theta}}_i$ for a revolute joint and d _i ,${\dot{d}}_i$,${\ddot{d}}_i$ for a prismatic joint) of the joints are known. The outward recursion propa gates velocities, and accelerations of each link from the base to the end link.

Revolute joints

Referring to Fig. 1, the angular velocity of a revolute joint, propagated from link i−1 to link i, is given by

Fig. 1

The illustration of the symbols

$$ {\boldsymbol{\omega}}_i={\boldsymbol{\omega}}_{i-1}+{\overset{.}{\theta}}_i{\mathbf{z}}_i $$

(1)

or in local components

$$ {\mathbf{R}}_i{\boldsymbol{\omega}}_i^{\prime }={\mathbf{R}}_{i-1}{\boldsymbol{\omega}}_i^{\prime }+\overset{.}{\theta }{\mathbf{R}}_i{\mathbf{z}}_i^{\prime } $$

(2)

where R _i stands for the orientation matrix of link i and ω ^′ _i is the angular velocity of link i with respect to the ith frame (local coordinate system). Premultiplying both sides of Eq. 2 with R ^T _i and simplifying yield

$$ {\boldsymbol{\omega}}_i^{\prime }={\mathbf{R}}_{i-1}^i{\boldsymbol{\omega}}_i^{\prime }+{\overset{.}{\theta}}_i{\mathbf{z}}_i^{\prime } $$

(3)

where R ⁱ _i−1 is the rotation matrix relating link i−1 and link i. z′ _i is the unit vector [0,0,1]^T parallel to the z _i axis. Similarly, the propagation of linear velocity from link i−1 to link i can be derived as

$$ {\mathbf{v}}_i^{\prime }={\mathbf{R}}_{i-1}^i\left({\mathbf{v}}_{i-1}^{\prime }+{\boldsymbol{\omega}}_{i-1}^{\prime}\times {\mathbf{p}}_i^{i-1}\right) $$

(4)

where p ⁱ⁻¹ _i denotes the position vector of the frame of link i in the (i−1)th frame.

The angular acceleration is calculated through

$$ {\dot{\boldsymbol{\omega}}}_i^{\prime }={\mathbf{R}}_{i-1}^i{\dot{\boldsymbol{\omega}}}_{i-1}^{\prime }+{\ddot{\theta}}_i{\mathbf{z}}_i^{\prime }+{\mathbf{R}}_{i-1}^i{\boldsymbol{\omega}}_{i-1}^{\prime}\times {\dot{\theta}}_i{\mathbf{z}}_i^{\prime } $$

(5)

The linear acceleration with respect to the link-attached coordinate system is given by

$$ {\dot{\mathbf{v}}}_i^{\prime }={\mathbf{R}}_{i-1}^i\left({\dot{\mathbf{v}}}_{i-1}^{\prime }+{\dot{\boldsymbol{\omega}}}_{i-1}^{\prime}\times {\mathbf{p}}_i^{i-1}+{\boldsymbol{\omega}}_{i-1}^{\prime}\times \left({\boldsymbol{\omega}}_{i-1}^{\prime}\times {\mathbf{p}}_i^{i-1}\right)\right) $$

(6)

where ${\dot{\mathbf{v}}}_i^{\prime }$ is the linear acceleration of link i in the ith frame.

The linear acceleration of the center of mass of link i is further calculated from the velocity and acceleration of the link

$$ {\dot{\mathbf{v}}}_{Ci}^{\prime }={\dot{\mathbf{v}}}_i^{\prime }+{\dot{\boldsymbol{\omega}}}_i^{\prime}\times {\mathbf{p}}_{Ci}^{\prime }+{\boldsymbol{\omega}}_i^{\prime}\times \left({\boldsymbol{\omega}}_i^{\prime}\times {\mathbf{p}}_{Ci}^{\prime}\right) $$

(7)

where ${\mathbf{p}}_{C_i}^{\prime }$ denotes the position vector of the center of mass of link i in its own frame.

Prismatic Joints

If the joints in question are prismatic, the angular velocities are identical for link i and link i−1, which means ω _i  = ω _i−1. Expressing with local components yields

$$ {\mathbf{R}}_i{\boldsymbol{\omega}}_i^{\prime }={\mathbf{R}}_{i-1}{\boldsymbol{\omega}}_{i-1}^{\prime } $$

(8)

that is,

$$ {\boldsymbol{\omega}}_i^{\prime }={\mathbf{R}}_{i-1}^i{\boldsymbol{\omega}}_{i-1}^{\prime } $$

(9)

where matrix R ⁱ _i−1 is independent to rotations.

Likewise, the local angular acceleration of link i is

$$ {\dot{\boldsymbol{\omega}}}_i^{\prime }={\mathbf{R}}_{i-1}^i{\dot{\boldsymbol{\omega}}}_{i-1}^{\prime } $$

(10)

Furthermore, the local linear velocity and acceleration become

$$ {\mathbf{v}}_i^{\prime }={\mathbf{R}}_{i-1}^i\left({\mathbf{v}}_{i-1}^{\prime }+{\boldsymbol{\omega}}_{i-1}^{\prime}\times {\mathbf{p}}_i^{i-1}\right)+{\dot{d}}_i{\mathbf{z}}_i^{\prime } $$

(11)

$$ {\dot{\mathbf{v}}}_i^{\prime }={\mathbf{R}}_{i-1}^i\left({\dot{\mathbf{v}}}_{i-1}^{\prime }+{\dot{\boldsymbol{\omega}}}_{i-1}^{\prime}\times {\mathbf{p}}_i^{i-1}+{\boldsymbol{\omega}}_{i-1}^{\prime}\times \left({\boldsymbol{\omega}}_{i-1}^{\prime}\times {\mathbf{p}}_i^{i-1}\right)\right)+2{\boldsymbol{\omega}}_i^{\prime}\times {\dot{d}}_i{\mathbf{z}}_i^{\prime }+{\ddot{d}}_i{\mathbf{z}}_i^{\prime } $$

(12)

where d _i is the displacement from axis x _i−1 to x _i along the z _i axis.

Inward Recursion to Calculate Forces and Torques

With the velocities and accelerations found, the inertial force and torque applied to link i at its center of mass can be calculated by applying the Newton-Euler equations :

$$ {\mathbf{f}}_{C_i}^{\prime }={m}_i{\dot{\mathbf{v}}}_{C_i}^{\prime } $$

(13)

$$ {\mathbf{n}}_{C_i}^{\prime }={\mathbf{I}}_i{\dot{\boldsymbol{\omega}}}_i^{\prime }+{\boldsymbol{\omega}}_i^{\prime}\times {\mathbf{I}}_i{\boldsymbol{\omega}}_i^{\prime } $$

(14)

where m _i is the mass of the link and I _i denotes the inertia tensor of link i about its center of mass, calculated in a frame identical to the link’s coordinate system except the origin. In the equations above, all items are expressed in the local coordinate systems.

The forces and torques acting on the links are calculated through the outward recursion. The joint torques that generate the forces and torques will be calculated through the inward recursion.

Referring to Fig. 1, the inward recursion propagates the joint forces and torques from the end link to the base as

$$ {\mathbf{f}}_i^{\prime }={\mathbf{f}}_{C_i}^{\prime }+{\mathbf{R}}_{i+1}^i{\mathbf{f}}_{i+1}^{\prime } $$

(15)

$$ {\mathbf{n}}_i^{\prime }={\mathbf{n}}_{C_i}^{\prime }+{\mathbf{R}}_{i+1}^i{\mathbf{n}}_{i+1}^{\prime }+{\mathbf{p}}_{C_i}^{\prime}\times {\mathbf{f}}_{C_i}^{\prime }+{\mathbf{p}}_{i+1}^i\times {\mathbf{R}}_{i+1}^i{\mathbf{f}}_{i+1}^{\prime } $$

(16)

where f _i ′ and n _i ′ are the force and torque exerted on link i with respect to the ith frame.

The actuating torque on a revolute joint can be calculated by

$$ {\tau}_i={\mathbf{n}}_i^{\prime T}{\mathbf{z}}_i^{\prime } $$

(17)

For prismatic joints, the driving force is

$$ {f}_i={\mathbf{f}}_i^{\prime T}{\mathbf{z}}_i^{\prime } $$

(18)

Forward Dynamics Problem

In the forward dynamics problem, the joint accelerations are to be calculated with known joint torques. One method called articulated-body algorithm (ABA) (Featherstone 1983) is applicable to this type of problem with recursive approach. More dynamics modeling methods were reviewed and discussed in (Featherstone and Orin 2000).

Lagrange’s Equation

The equations of motion for a manipulator can also be formulated with Lagrangian dynamics method. Two types of Lagrange’s equations can be formulated, one with dependent coordinates (1st kind) and the other with independent coordinates (2nd kind). This section starts with the Lagrange’s equation of the 1st kind. Hereafter, the vector q represents a set of n unknown dependent coordinates, m is the total number of independent kinematically constraint equations, and f = n − m is the number of dynamic degrees of freedom (de Jalón and Bayo 1994). The constraint conditions for the entire system are written in the following general form:

$$ \boldsymbol{\Phi} \left(\mathbf{q}\right)=0 $$

(19)

Differentiating Eq. 19 with respect to time leads to

$$ \frac{\mathrm{d}\boldsymbol{\Phi } \left(\mathbf{q}\right)}{\mathrm{d}t}=\frac{\partial \boldsymbol{\Phi}}{\partial \mathbf{q}}\frac{\mathrm{d}\mathbf{q}}{\mathrm{d}t}={\boldsymbol{\Phi}}_{\mathbf{q}}\dot{\mathbf{q}}=0 $$

(20)

where matrix Φ _q is the Jacobian matrix of the constraint Eq. 19.

Let L = T − V be the Lagrangian of the system, where T = T(q, $\dot{\mathbf{q}}$) and V = V(q) are the kinetic and potential energies, respectively, and Q _ex is the vector of generalized external forces acting along the dependent coordinates q. Considering a manipulator as a constrained multibody system, the Lagrange’s equation can be derived by generalizing the Hamilton’s principle by means of Lagrange multiplier technique(de Jalón and Bayo 1994). Skipping details of derivation, the equations of motion become

$$ \frac{\mathrm{d}}{\mathrm{d}t}\left(\frac{\partial L}{\partial \dot{\mathbf{q}}}\right)-\frac{\partial L}{\partial \mathbf{q}}+{\boldsymbol{\Phi}}_{\mathbf{q}}^T\boldsymbol{\uplambda} ={\mathbf{Q}}_{ex} $$

(21)

which is the Lagrange’s equation of the 1st kind. In the equation, λ is a m-dimensional vector of Lagrange multipliers. Item Φ ^T _q λ represents the generalized forces due to reaction forces at joints.

With independent coordinates, the constraint Jacobian vanishes, and the term with Lagrange multipliers is dropped out. The Lagrange’s equation of the 2nd kind is thus obtained:

$$ \frac{\mathrm{d}}{\mathrm{d}t}\left(\frac{\partial L}{\partial \dot{\mathbf{q}}}\right)-\frac{\partial L}{\partial \mathbf{q}}={\mathbf{Q}}_{ex} $$

(22)

The Lagrange’s equation of the 1st kind stands for a system of n equations for n dependent co-ordinates and m unknown Lagrange multipliers. To solve the equations, additional acceleration equations from the kinematic constraints are required, as presented below.

The kinetic energy of a robotic system can be written as follows:

$$ T=\frac{1}{2}{\dot{\mathbf{q}}}^T\mathbf{M}\left(\mathbf{q}\right)\dot{\mathbf{q}} $$

(23)

where M(q) = M is the mass matrix of the system. The first and second terms in Eq. 21 can be expressed as below:

$$ \frac{\mathrm{d}}{\mathrm{d}t}\left(\frac{\partial L}{\partial \dot{\mathbf{q}}}\right)=\mathbf{M}\ddot{\mathbf{q}}+\dot{\mathbf{M}}\dot{\mathbf{q}};\kern0.0em \frac{\partial L}{\partial \mathbf{q}}={L}_{\mathbf{q}}={T}_{\mathbf{q}}-{V}_{\mathbf{q}} $$

(24)

For the general case in which the kinetic energy depends on q, Eq. 21 becomes

$$ \mathbf{M}\ddot{\mathbf{q}}+{\boldsymbol{\Phi}}_{\mathbf{q}}^T\boldsymbol{\uplambda} ={\mathbf{Q}}_{ex}+{L}_{\mathbf{q}}-\dot{\mathbf{M}}\dot{\mathbf{q}} $$

(25)

On the other hand, differentiating the constraint Eq. 19 twice with respect to time yields

$$ {\boldsymbol{\Phi}}_{\mathbf{q}}\ddot{\mathbf{q}}+{\dot{\boldsymbol{\Phi}}}_{\mathbf{q}}\dot{\mathbf{q}}=\mathbf{0} $$

(26)

Let $\mathrm{c}=-{\dot{\boldsymbol{\Phi}}}_{\mathbf{q}}\dot{\mathbf{q}}$ and n = Q _ex  + L _q −$\dot{\mathbf{M}}\dot{\mathbf{q}}$ . Combining Eqs. 25 and 26 leads to:

$$ \left[\begin{array}{cc}\hfill \mathbf{M}\hfill & \hfill {\boldsymbol{\Phi}}_{\mathbf{q}}^T\hfill \\ {}\hfill {\boldsymbol{\Phi}}_{\mathbf{q}}\hfill & \hfill \mathbf{0}\hfill \end{array}\right]\left[\begin{array}{l}\ddot{\mathbf{q}}\\ {}\boldsymbol{\uplambda} \end{array}\right]=\left[\begin{array}{l}\mathbf{n}\\ {}\mathbf{c}\end{array}\right] $$

(27)

Equation 27 can solve simultaneously the accelerations and the Lagrange multipliers.

Examples

Two examples are included to apply the equations of motion to robotic manipulators. In the first example, a lightweight robotic arm consisting of five revolute joints developed at Aalborg University (AAU), Denmark, is considered. In the second example, the dynamics of a spherical parallel manipulator is presented.

Example I: A 5-dof Lightweight Robotic Arm

The lightweight robotic arm is demonstrated in Fig. 2a. Following the D-H convention, Cartesian coordinate systems are attached to each link of the manipulator, as shown in Fig. 2b. The D-H parameters of the manipulator are listed in Table 1.

Fig. 2

AAU 5-dof robotic arm: (a) a prototype and (b) coordinate systems

Table 1 D-H parameters of the 5-dof robotic arm

Dynamics Modeling

The kinematics of the robotic arm can be found in (Zhou et al. 2011). This chapter includes here only the dynamics formulation.

Jacobian matrix The joint angular velocity can be calculated with the Jacobian matrix

$$ \dot{\boldsymbol{\theta}}={\mathbf{J}}^{-1}{\mathbf{v}}_{ef} $$

(28)

where $\overset{\cdotp }{\boldsymbol{\theta}}={\left[{\dot{\theta}}_1,{\dot{\theta}}_2\dots, {\dot{\theta}}_n\right]}^T$ denotes an n-dimensional (n denotes the number of dof) vector of the joint angular velocities, J is the Jacobian of the robotic arm, and v _ef the velocity of the end-effector.

For a revolute joint, the Jacobian matrix can be calculated by (Tsai 1999)

$$ \mathbf{J}=\left[{\mathbf{j}}_1,{\mathbf{j}}_2,\dots, {\mathbf{j}}_n\right],\kern0.36em {\mathbf{j}}_i=\left[\begin{array}{l}{\mathbf{z}}_{i-1}\times {\mathbf{p}}_{i-1}\\ {}\kern0.72em {\mathbf{z}}_{i-1}\end{array}\right] $$

(29)

where z _i−1 and p _i−1 are given by

$$ {\mathbf{z}}_{i-1}={\mathbf{R}}_{i-1}^i\Big[\begin{array}{ccc}\hfill 0\hfill & \hfill 0\hfill & \hfill 1\Big]{}^{\mathrm{T}},\kern0.1em {\mathbf{p}}_{i-1}={\mathbf{R}}_{i-1}^i\hfill \end{array}{\mathbf{q}}_{i-1}+{\mathbf{p}}_i $$

(30)

where q _i−1 = [a _i cos θ _i , a _i sin θ _i , d _i ]^T. The local coordinates of the end-effector are defined as p _n  = [0, 0, 0]^T. When the desired end-effector velocity v _ef is given, the joint angular velocity can be solved by Eq. 28.

The joint angular acceleration $\ddot{\boldsymbol{\theta}}$ can be calculated through time derivative of the velocity $\dot{\boldsymbol{\theta}}$.

$$ \ddot{\boldsymbol{\theta}}={\dot{\mathbf{J}}}^{-1}{\mathbf{v}}_{e\;f}+{\mathbf{J}}^{-1}{\dot{\mathbf{v}}}_{e\;f} $$

(31)

Simulation Results

The dynamics of the 5-dof robotic arm was simulated with a program implemented in MATLAB. The trajectory of the end-effector in the global coordinate system is defined as x _ef (t) = 5 + 400 (1 − cos(t)), y _ef (t) = −990 + 800(1 − cos(t/2)), and z _ef (t) = 280 + 250(cos(t/2) − 1), all with unit of mm, which leads to

$$ {\dot{\mathbf{x}}}_{e\;f}=\left[\begin{array}{l}400 \sin (t)\\ {}400 \sin \left(t/2\right)\\ {}-125 \sin \left(t/2\right)\end{array}\right] $$

The Euler angles for the end-effector are given as ψ = [0, cos(t/180), 0]^T, following ZXZ convention. The rotation implies the end-effector remains horizontal during the prescribed motion. For the prescribed motion, the rotation matrix R and angular velocity ω can be readily found. The velocities of the end-effector are ${\mathbf{v}}_{ef}={\left[{\dot{\mathbf{x}}}_{ef}^T,{\boldsymbol{\omega}}^T\right]}^T$, from which the joint angular velocities and accelerations are calculated.

To implement the Newton-Euler method to solve the inverse dynamics problem, the equations in section “Recursive Newton-Euler Method” are summarized in a recursive manner:

Outward recursion: i : 1 → 5

$$ {\boldsymbol{\omega}}_i^{\prime }={\mathbf{R}}_{i-1}^i{\boldsymbol{\omega}}_{i-1}^{\prime }+{\dot{\theta}}_i{\mathbf{z}}_i^{\prime } $$

(32a)

$$ {\boldsymbol{\omega}}_i^{\prime }={\mathbf{R}}_{i-1}^i{\boldsymbol{\omega}}_{i-1}^{\prime }+{\ddot{\theta}}_i{\mathbf{z}}_i^{\prime }+{\mathbf{R}}_{i-1}^i{\boldsymbol{\omega}}_{i-1}^{\prime}\times {\dot{\theta}}_i{\mathbf{z}}_i^{\prime } $$

(32b)

$$ {\dot{\mathbf{v}}}_i^{\prime }={\mathbf{R}}_{i-1}^i\left({\dot{\mathbf{v}}}_{i-1}^i+{\dot{\boldsymbol{\omega}}}_{i-1}^{\prime}\times {\mathbf{p}}_i^{i-1}+{\boldsymbol{\omega}}_{i-1}^{\prime}\times \left({\boldsymbol{\omega}}_{i-1}^{\prime}\times {\mathbf{p}}_i^{i-1}\right)\right) $$

(32c)

$$ {\dot{\mathbf{v}}}_{C_i}^{\prime }={\dot{\mathbf{v}}}_{i-1}^{\prime }+{\dot{\boldsymbol{\omega}}}_{i-1}^{\prime}\times {\mathbf{p}}_{C_i}^{\prime }+{\boldsymbol{\omega}}_{i-1}^{\prime}\times \left({\boldsymbol{\omega}}_{i-1}^{\prime}\times {\mathbf{p}}_{C_i}^{\prime}\right) $$

(32d)

$$ {\mathbf{f}}_{Ci}^{\prime }={m}_i{\dot{\mathbf{v}}}_{C_i}^{\prime } $$

(32e)

$$ {\mathbf{n}}_{Ci}^{\prime }={\mathbf{I}}_i{\dot{\boldsymbol{\omega}}}_i^{\prime }+{\boldsymbol{\omega}}_i^{\prime}\times {\mathbf{I}}_i{\boldsymbol{\omega}}_i^{\prime } $$

(32f)

Inward recursion: i : 5 → 1

$$ {\mathbf{f}}_i^{\prime }={\mathbf{f}}_{C_i}^{\prime }+{\mathbf{R}}_{i+1}^i{\mathbf{f}}_{i+1}^{\prime } $$

(32g)

$$ {\mathbf{n}}_i^{\prime }={\mathbf{n}}_{C_i}^{\prime }+{\mathbf{R}}_{i+1}^i{\mathbf{n}}_{i+1}^{\prime }+{\mathbf{p}}_{C_i}^{\prime}\times {\mathbf{f}}_{C_i}^{\prime }+{\mathbf{p}}_{i+1}^{\prime}\times {\mathbf{R}}_{i+1}^i{\mathbf{f}}_{i+1}^{\prime } $$

(32h)

$$ {\tau}_i={\mathbf{n}}_i^{\prime T}{\mathbf{z}}_i^{\prime } $$

(32i)

The joint angular velocities and accelerations are solved through Eq. 28, with results displayed in Fig. 3. Let the payload at the end-effector of the robot be 5 kg, which implies f ₆ = 5g N, n ₆ = 0, where g is the vector of gravity acceleration. Through the modeling of inverse dynamics of Eqs. 32g–32i, the joint torques are solved, as shown in Fig. 4.

Fig. 3

Angular velocities and accelerations of the robotic arm joints

Fig. 4

Joint torques of the robotic arm

Example II: A Spherical Parallel Manipulator

In this example, the Lagrange’s equation is illustrated with a spherical parallel manipulator (SPM) (Gosselin and Angeles 1989). The SPMs are of closed-kinematic chain. The Lagrange’s equation of the 1st kind is adopted for the dynamics modeling of this manipulator.

A general SPM is shown in Fig. 5a with the parameterized ith leg in Fig. 5b. The ith limb consists of three revolute joints, whose axes are parallel to the unit vectors u _i , v _i , and w _i . All three limbs have identical architectures, defined by angles α ₁ and α ₂. Moreover, β and γ define the geometry of two triangular pyramids on the base and the mobile platforms, respectively. The origin O of the base coordinate system xyz is located at point O. The z axis is normal to the bottom surface of the base pyramid and points upward, while the y axis is located in the plane made by the z axis and u ₁.

Fig. 5

A general SPM and the coordinate system, where only one leg is shown for clarity

Kinematic Modeling

Under the prescribed coordinate system, unit vector u _i is derived as

$$ {\mathbf{u}}_i={\left[\begin{array}{ccc}\hfill - \sin {\eta}_i \sin \gamma \hfill & \hfill \cos {\eta}_i \sin \gamma \hfill & \hfill - \cos \gamma \hfill \end{array}\right]}^T $$

(33)

where η _i  = 2(i − 1)π/3, i = 1, 2, 3.

The unit vector v _i of the axis of the intermediate revolute joint in the ith leg is expressed as:

$$ {\mathbf{v}}_i=\left[\begin{array}{c}\hfill -\mathrm{s}{\eta}_i\mathrm{s}\gamma \mathrm{c}{\alpha}_1+\left(\mathrm{c}{\eta}_is{\theta}_i-\mathrm{s}{\eta}_i\mathrm{c}\gamma c{\theta}_i\right)\mathrm{s}{\alpha}_1\hfill \\ {}\hfill \mathrm{c}{\eta}_i\mathrm{s}\gamma \mathrm{c}{\alpha}_1+\left(\mathrm{s}{\eta}_i\mathrm{s}{\theta}_i+\mathrm{c}{\eta}_{\mathrm{i}}\mathrm{c}\gamma \mathrm{c}{\theta}_{\mathrm{i}}\right)\mathrm{s}{\alpha}_1\hfill \\ {}\hfill -\mathrm{c}\gamma \mathrm{c}{\alpha}_1+\mathrm{s}\gamma \mathrm{c}{\theta}_i\mathrm{s}{\alpha}_1\hfill \end{array}\right] $$

(34)

where s(⋅) = sin(⋅) and c(⋅) = cos(⋅).

The unit vector w _i of the top revolute joint in the ith leg is a function of the orientation of the mobile platform (MP), namely,

$$ {\mathbf{w}}_i=\Big[\begin{array}{ccc}\hfill {w}_{ix}\hfill & \hfill {w}_{iy}\hfill & \hfill {w}_{iz}\Big]{}^T=\mathbf{Q}{\mathbf{w}}_i^{*}\hfill \end{array} $$

(35)

where Q is the rotation matrix that carries the MP from its reference orientation to the current one and w ^* _i is the unit vector of the axis of the top revolute joint in the ith leg when the mobile platform is in its reference orientation, which is given as

$$ {\mathbf{w}}_i^{*}=\Big[\begin{array}{ccc}\hfill - \sin {\eta}_i \sin \beta \hfill & \hfill \cos {\eta}_i \sin \beta \hfill & \hfill \cos \beta \Big]{}^T\hfill \end{array} $$

(36)

Kinematic Jacobian Matrix

Let ω = [ω _x , ω _y , ω _z ]^T denote the angular velocity of the mobile platform; the velocity equation via the ith leg can be stated as

$$ \boldsymbol{\omega} ={\dot{\theta}}_i{\mathbf{u}}_i+{\dot{\psi}}_i{\mathbf{v}}_i+{\dot{\xi}}_i{\mathbf{w}}_i $$

(37)

To eliminate ${\dot{\psi}}_i$ and ${\dot{\xi}}_i$, dot-multiplying Eq. 37 on both sides with v _i  × w _i yields

$$ \left({\mathbf{v}}_i\times {\mathbf{w}}_i\right)\cdot \boldsymbol{\omega} =\left({\mathbf{u}}_i\times {\mathbf{v}}_i\right)\cdot {\mathbf{w}}_i{\dot{\theta}}_i $$

(38)

from which the SPM velocity equation is obtained as

$$ \mathbf{A}\boldsymbol{\omega } =\mathbf{B}\dot{\boldsymbol{\theta}} $$

(39)

with

$$ \mathbf{A}=\Big[\begin{array}{ccc}\hfill {\mathbf{v}}_1\times {\mathbf{w}}_1\hfill & \hfill {\mathbf{v}}_2\times {\mathbf{w}}_2\hfill & \hfill {\mathbf{v}}_3\times {\mathbf{w}}_3\Big]{}^T\hfill \end{array} $$

(40a)

$$ \mathbf{B}=\mathrm{diag}\;\Big[\begin{array}{ccc}\hfill \left({\mathbf{u}}_1\times {\mathbf{v}}_1\right)\cdot {\mathbf{w}}_1\hfill & \hfill \left({\mathbf{u}}_2\times {\mathbf{v}}_2\right)\cdot {\mathbf{w}}_2\hfill & \hfill \left({\mathbf{u}}_3\times {\mathbf{v}}_3\right)\cdot {\mathbf{w}}_3\Big]\hfill \end{array} $$

(40b)

where $\overset{\cdotp }{\boldsymbol{\theta}}={\left[{\dot{\theta}}_1,{\dot{\theta}}_2,{\dot{\theta}}_3\right]}^T$. Matrices A and B are the forward and inverse Jacobian matrices of the manipulator, respectively. The kinematic Jacobian matrix J of the manipulator can be expressed as follows as long as matrix A is not singular:

$$ \mathbf{J}={\mathbf{B}}^{-1}\mathbf{A}={\left[\begin{array}{ccc}\hfill {\mathbf{j}}_1\hfill & \hfill {\mathbf{j}}_2\hfill & \hfill {\mathbf{j}}_3\hfill \end{array}\right]}^T;\kern0.2em {\mathbf{j}}_i=\frac{{\mathbf{v}}_i\times {\mathbf{w}}_i}{{\mathbf{u}}_i\times {\mathbf{v}}_i\cdot {\mathbf{w}}_i} $$

(41)

Equation 38 is thus rewritten for the single joint velocity as

$$ {\dot{\theta}}_i={\mathbf{j}}_i^T\boldsymbol{\omega} $$

(42)

Velocity and Acceleration Analysis

The motions of the link and mobile platform are shown in Fig. 6. The angle rates $\overset{\cdotp }{\phi }={\left[\dot{\phi},\dot{\theta},\dot{\sigma}\right]}^T$ and the angular velocity ω are linearly dependent, namely, $\boldsymbol{\omega} =\boldsymbol{\Psi}\;\dot{\phi}$. Differentiating the equation with respect to time yields

Fig. 6

The movement of the mobile platform and links

$$ \dot{\boldsymbol{\omega}}=\boldsymbol{\uppsi} \ddot{\phi} + \dot{\boldsymbol{\uppsi}}\dot{\phi} $$

(43)

Matrix Ψ is dependent on the rotations. For example, a rotation with the Euler convention ZY Z of Q = R _z (ϕ)R _y (θ)R _z (σ−ϕ), matrix Ψ is

$$ \boldsymbol{\uppsi} =\left[\begin{array}{ccc}\hfill -\mathrm{s}\theta \mathrm{c}\phi \hfill & \hfill -\mathrm{s}\phi \hfill & \hfill \mathrm{s}\theta \mathrm{c}\phi \hfill \\ {}\hfill -\mathrm{s}\theta \mathrm{s}\phi \hfill & \hfill \mathrm{c}\phi \hfill & \hfill \mathrm{s}\theta \mathrm{s}\phi \hfill \\ {}\hfill 1-\mathrm{c}\theta \hfill & \hfill 0\hfill & \hfill \mathrm{c}\phi \hfill \end{array}\right] $$

(44)

The velocity $\dot{\psi}$ of the intermediate joint of ith leg is found by making use of Eq. 37 to eliminate ${\dot{\theta}}_i$ and ${\dot{\xi}}_i$. Dot-multiplying Eq. 37 on both sides with u _i  × w _i leads to

$$ \left({\mathbf{u}}_i\times {\mathbf{w}}_i\right)\cdot \boldsymbol{\omega} =\dot{\psi}\left({\mathbf{u}}_i\times {\mathbf{w}}_i\right)\cdot {\mathbf{v}}_i\kern0.3em \mathrm{or}\kern0.3em \dot{\psi}=\frac{{\left({\mathbf{u}}_i\times {\mathbf{w}}_i\right)}^T}{\left({\mathbf{u}}_i\times {\mathbf{w}}_i\right)\cdot {\mathbf{v}}_i}\boldsymbol{\omega} \equiv {\mathrm{j}}_{\psi i}^T\boldsymbol{\omega} $$

(45)

The angular velocity of the distal link in the ith leg in the reference frame xyz is found as ${\rho}_i={\dot{\theta}}_i\;{\mathbf{u}}_i+{\dot{\psi}}_i\;{\mathbf{v}}_i$. Let ρ _li denote the corresponding angular velocity in the local frame x _i y _i z _i , which means

$$ {\boldsymbol{\rho}}_i=\left[\begin{array}{ccc}\hfill {\mathbf{e}}_{ix}\hfill & \hfill {\mathbf{e}}_{iy}\hfill & \hfill {\mathbf{e}}_{iz}\hfill \end{array}\right]{\boldsymbol{\rho}}_{li}\kern1em \mathrm{or}\kern1em {\boldsymbol{\rho}}_i={\mathbf{E}}_i{\boldsymbol{\rho}}_{li} $$

(46)

with

$$ {\mathbf{e}}_{ix}=\frac{{\mathbf{v}}_i+{\mathbf{w}}_i}{\left|\left|{\mathbf{v}}_i+{\mathbf{w}}_i\right|\right|},{\mathbf{e}}_{iy}=\frac{{\mathbf{v}}_i+{\mathbf{w}}_i}{\left|\left|{\mathbf{v}}_i+{\mathbf{w}}_i\right|\right|},{\mathbf{e}}_{iz}=\frac{{\mathbf{v}}_i+{\mathbf{w}}_i}{\left|\left|{\mathbf{v}}_i+{\mathbf{w}}_i\right|\right|} $$

(47)

which yields

$$ {\boldsymbol{\rho}}_{li}={\mathbf{E}}_i^T{\boldsymbol{\rho}}_i $$

(48)

Dynamic Modeling

The Lagrange’s equation for the SPM is expressed as

$$ \frac{\mathrm{d}}{\mathrm{d}t}\left(\frac{\partial L}{\partial \dot{\mathbf{q}}}\right)-\frac{\partial L}{\partial \mathbf{q}}+{\mathbf{C}}_q^T\boldsymbol{\uplambda} ={\mathbf{Q}}_{ex} $$

(49)

where q = [θ ₁, θ ₂, θ ₃, ϕ, θ, σ]^T, Q _ex  = [τ ^T, 0]^T ∈ ℝ⁶ is the vector of external forces, and vector τ = [τ ₁, τ ₂, τ ₃]^T characterizes the actuator torques. Moreover, λ = [λ₁, λ₂, λ₃]^T is a vector of Lagrange multipliers. Matrix C _q is the system’s constraint Jacobian, which can be found from the velocity Eq. 39, namely,

$$ \mathbf{B}\dot{\boldsymbol{\theta}}-\mathbf{A}\boldsymbol{\omega } =\Big[\begin{array}{cc}\hfill \mathbf{B}\hfill & \hfill -\mathbf{A}\boldsymbol{\uppsi } \left]\kern0.24em \right[\begin{array}{cc}\hfill {\dot{\boldsymbol{\theta}}}^T\hfill & \hfill \kern0.3em {\dot{\boldsymbol{\phi}}}^T\Big]{}^T=\mathbf{0}\hfill \end{array}\hfill \end{array} $$

(50)

that is,

$$ {\mathbf{C}}_q\dot{\mathbf{q}}=\mathbf{0} $$

(51)

with C _q  = [B−AΨ].

Lagrangian of the Mobile Platform

The local frame x _p y _p z _p of the MP is established with the origin located at point P, i.e., the center of mass of the MP of position vector p. Henceforth, the Lagrangian of the mobile platform is obtained as

$$ {L}_p={T}_p-{V}_p=\frac{1}{2}{\boldsymbol{\omega}}^{\prime T}{\mathbf{I}}_p\boldsymbol{\omega}^{\prime }-{m}_p{\boldsymbol{g}}^T\mathbf{p} $$

(52)

where I _p denotes the local inertia tensor of the mobile platform and ω′ its local angular velocity. Moreover, g = [0, 0, 9.81]^T.

Lagrangian of a Single Leg

The Lagrangian of the ith leg is derived as

$$ {L}_i={T}_i-{V}_i=\frac{1}{2}{I}_{l1}{\dot{\theta}}_i^2+\frac{1}{2}{\boldsymbol{\rho}}_{li}^T{\mathbf{I}}_{l2}{\boldsymbol{\rho}}_{li}-{m}_{l1}{\mathbf{g}}^T{\mathbf{h}}_i-{m}_{l2}{\mathbf{g}}^T{\mathbf{k}}_i\kern1em i=1,2,3 $$

(53)

where I _l1 is the proximal link’s mass moment of inertia about u _i and I _l2 is the distal link’s local inertia tensor about point O. Moreover, h _i and k _i indicate the position vectors for the centers of the mass of the proximal and distal links, respectively.

Substituting the Lagrangian L _p and L _i , i = 1, 2, 3, into Eq. 49, the inertia matrix can be derived as

$$ \mathbf{M}\left(\mathbf{q}\right)=\left[\begin{array}{cc}\hfill {\mathbf{M}}_1\hfill & \hfill {\mathbf{0}}_3\hfill \\ {}\hfill {\mathbf{0}}_3\hfill & \hfill {\mathbf{M}}_2\hfill \end{array}\right] $$

(54)

with

$$ {\mathbf{M}}_1=\mathrm{diag}\;\Big[\begin{array}{ccc}\hfill {I}_{l1}\hfill & \hfill {I}_{l1}\hfill & \hfill {I}_{l1}\Big]\hfill \end{array} $$

(55a)

$$ {\mathbf{M}}_2={\boldsymbol{\Psi}}^T\Big[\mathbf{Q}{\mathbf{I}}_p{\mathbf{Q}}^T+{\displaystyle {\sum}_{i=1}^3\left({\mathbf{j}}_i{\mathbf{u}}_i^T+{\mathbf{j}}_{\psi i}{\mathbf{v}}_i^T\right)\mathbf{E}{\mathbf{I}}_{l2}{\mathbf{E}}^T\kern0.24em \left({\mathbf{u}}_i{\mathbf{j}}_i^T+{\mathbf{v}}_i{\mathbf{j}}_{\psi i}^T\right)\Big]\;\boldsymbol{\Psi}} $$

(55b)

and the other terms also can be derived. Moreover, differentiating Eq. 51 with respect to time yields

$$ {\mathbf{C}}_q\ddot{\mathbf{q}}=-{\dot{\mathbf{C}}}_q\dot{\mathbf{q}} $$

(56)

with

$$ {\mathbf{C}}_q\ddot{\mathbf{q}}={\mathbf{C}}_q\Big[\begin{array}{cc}\hfill {\ddot{\boldsymbol{\theta}}}^T\hfill & \hfill {\ddot{\boldsymbol{\phi}}}^T\Big]{}^T\hfill \end{array};\kern1em {\dot{\mathbf{C}}}_q\dot{\mathbf{q}}=\left[\dot{\mathbf{B}}\kern-0.2em -\kern-0.2em \dot{\mathbf{A}}\boldsymbol{\Psi} \kern-0.2em -\kern-0.2em \mathbf{A}\dot{\boldsymbol{\Psi}}\right]\;\dot{\mathbf{q}} $$

(57)

where

$$ \dot{\mathbf{A}}=\mathbf{\Big[}\begin{array}{ccc}\hfill {\mathbf{j}}_{A1}\hfill & \hfill {\mathbf{j}}_{A2}\hfill & \hfill {\mathbf{j}}_{A 3}\Big]{}^T\hfill \end{array} $$

(58a)

$$ \dot{\mathbf{B}}=\mathrm{diag}\;\Big[\begin{array}{ccc}\hfill {j}_{B1}\hfill & \hfill {j}_{B2}\hfill & \hfill {j}_{B3}\Big]\hfill \end{array} $$

(58b)

and

$$ {\mathbf{j}}_{Ai}={\dot{\theta}}_i\left({\mathbf{u}}_i\times {\mathbf{v}}_i\right)\times {\mathbf{w}}_i+{\mathbf{v}}_i\times \left(\boldsymbol{\omega} \times {\mathbf{w}}_i\right) $$

(59a)

$$ {j}_{Bi}={\dot{\theta}}_i\left[{\mathbf{u}}_i\times \left({\mathbf{u}}_i\times {\mathbf{v}}_i\right)\right]\cdot {\mathbf{w}}_i+\left({\mathbf{u}}_i\times {\mathbf{v}}_i\right)\cdot \left(\boldsymbol{\omega} \times {\mathbf{w}}_i\right) $$

(59b)

Combining Eqs. 49 and 56 leads to

$$ \left[\begin{array}{cc}\hfill \mathbf{M}\left(\mathbf{q}\right)\hfill & \hfill {\mathbf{C}}_q^T\hfill \\ {}\hfill {\mathbf{C}}_q\hfill & \hfill \mathbf{0}\hfill \end{array}\right]\left[\begin{array}{l}\ddot{\mathbf{q}}\\ {}\boldsymbol{\uplambda} \end{array}\right]=\left[\begin{array}{c}\hfill \boldsymbol{\tau} -\dot{\mathbf{M}}\left(\mathbf{q}\right)\dot{\mathbf{q}}+{T}_{\mathbf{q}}-{V}_{\mathbf{q}}\hfill \\ {}\hfill -{\dot{\mathbf{C}}}_q\dot{\mathbf{q}}\hfill \end{array}\right] $$

(60)

With an external moment vector m, the actuator torques are expressed as

$$ {\boldsymbol{\tau}}_a=\boldsymbol{\tau} -{\mathbf{J}}^{-T}\mathbf{m} $$

(61)

Simulation Results

The previously developed dynamic model is applied to a co-axial SPM (Bai 2010), whose properties are given in Tables 2 and 3, where inertia tensors I _p and I _l2 contain only diagonal entries. Simulations were conducted for motion of constant angular accelerations at actuating joints, their initial conditions being specified in Table 2. The results of the actuating torques are shown in Fig. 7. The results agree well with the simulation results obtained from CAE software, namely, the MSC Adams.

Table 2 Parameters of the SPM and the initial simulation condition

Table 3 Mass and inertia properties of the SPM

Fig. 7

The simulation of inverse dynamics: actuator torques

Summary

This book chapter introduces the fundamental of manipulator dynamics, with focus on the recursive Newton-Euler approach and the Lagrange equations, which are approaches mostly common used in robot dynamics analysis and control. Both inverse and forward dynamic problems are discussed. Applications to manipulators are demonstrated.

In solving the dynamics equation, numerics problems may arise. One is computational efficiency. The recursive approach presented in this chapter is quite efficient, while the Lagrange’s equations, as a closed-form equation, implies high computational cost in finding solutions, which uses numerical methods, for example, Newton–Raphson method. Other problems include the singular Jacobian and the instability of the solution, etc., among others. The Jacobian matrix can become singular when a singular configuration is reached or redundant constraints are included, which may lead to a crash of simulation or a large error. Instability takes place in numerical integration of the equation of motion, when the roundoff errors increase with time, which can lead to the constraint equations not satisfied. A method to improve the stability is the Baumgarte stabilization method, which is introduced in (de Jalón and Bayo 1994).

The approaches presented in this chapter are closely relevant to multibody dynamics, which was extensively studied in the last century. With the advances of robotics in the twenty-first century, manipulator dynamics has significantly been expanded, not limited to multibody dynamics, but get involved with other subsets of the dynamics. A new development among others is the continuum manipulators, where continuum mechanics is the major theory applicable (Walker 2013). The new development of manipulator dynamics, which is not discussed in this chapter, presents new opportunity and challenges for robotics.