Robot dynamics concerns about why
a robot moves. Dynamics equation establishes a relationship between the
forces/torques and their acceleration/velocities/positions.
Lagrange method provides a systematic way to derive dynamics
equation of a mechanical system. Suppose a \(n\)-DOF mechanical
system is described by a set of
independent variables \(q_{i}, i=1, \ldots, n\), named generalized
coordinates, the Lagrangian of the system is defined as
\[\mathcal{L}=\mathcal{T}-\mathcal{U}\]
where \(\mathcal{T}\) and \(\mathcal{U}\) are the kinetic energy and
potential energy of the system, respectively. The Lagrangian dynamics equation
is
where \(\xi_{i}\) is the generalized force associated with \(q_{i}\) (\(\xi_i\) is dual to \(q_i\) in the sense that
\(\xi_i*q_i\) generate power). In compact form, the dynamics equation can be
written as
For an open-chain robot arm, the generalized
coordinates are joint variables \(\boldsymbol{q}\). The generalized forces
are the net torque/force at each joint.
As shown in Fig. 63, the kinetic energy of Link \(i\) is given by
(29)#\[
\mathcal{T}_{\ell_{i}}=\frac{1}{2} \int_{V_{\ell_{i}}} \dot{\boldsymbol{p}}_{i}^{* T} \dot{\boldsymbol{p}}_{i}^{*} \rho d V\]
where \(\rho\) is the density of the infinitesmal particle of volume
\(d V\); \(\dot{\boldsymbol{p}}_{i}^{*}\) is the linear velocity vector of the infinitesmal particle; and \(V_{\ell_{i}}\) is the whole volume of Link \(i\). Here,
\(\boldsymbol{p}_{i}^{*}\) is expressed in the base frame.
The
position \(\boldsymbol{p}_{l_{i}}\) of center of mass (COM) of the link \(i\) is given by
\[\boldsymbol{p}_{\ell_{i}}=\frac{1}{m_{\ell_{i}}} \int_{V_{\ell_{i}}} \boldsymbol{p}_{i}^{*} \rho d V\]
where \(m_{\ell_{i}}\) is the mass of link \(i\).
We also define the relative position of infinitesmal particle
where \(\dot{\boldsymbol{p}}_{\ell_{i}}\) is the linear velocity of COM and \(\boldsymbol{\omega}_{i}\) is the angular velocity of
the link.
By substituting (30) into
(29), it leads to that the total kinetic energy \( \mathcal{T}_{\ell_{i}}\) of link \(i\) will include the following three terms
Translational term
(31)#\[\frac{1}{2} \int_{V_{\ell_{i}}} \dot{\boldsymbol{p}}_{\ell_{i}}^{T} \dot{\boldsymbol{p}}_{\ell_{i}} \rho d V=\frac{1}{2} m_{\ell_{i}} \dot{\boldsymbol{p}}_{\ell_{i}}^{T} \dot{\boldsymbol{p}}_{\ell_{i}}\]
Cross term
(32)#\[2\left(\frac{1}{2} \int_{V_{\ell_{i}}} \dot{\boldsymbol{p}}_{\ell_{i}}^{T} \boldsymbol{S}\left(\boldsymbol{\omega}_{i}\right) \boldsymbol{r}_{i} \rho d V\right)=2\left(\frac{1}{2} \dot{\boldsymbol{p}}_{\ell_{i}}^{T} \boldsymbol{S}\left(\boldsymbol{\omega}_{i}\right) \int_{V_{\ell_{i}}}\left(\boldsymbol{p}_{i}^{*}-\boldsymbol{p}_{\ell_{i}}\right) \rho d V\right)=0\]
Rotational term
(33)#\[\frac{1}{2} \int_{V_{\ell_{i}}} \boldsymbol{r}_{i}^{T} \boldsymbol{S}^{T}\left(\boldsymbol{\omega}_{i}\right) \boldsymbol{S}\left(\boldsymbol{\omega}_{i}\right) \boldsymbol{r}_{i} \rho d V=\frac{1}{2} \boldsymbol{\omega}_{i}^{T}\left(\int_{V_{\ell_{i}}} \boldsymbol{S}^{T}\left(\boldsymbol{r}_{i}\right) \boldsymbol{S}\left(\boldsymbol{r}_{i}\right) \rho d V\right) \boldsymbol{\omega}_{i}=\frac{1}{2} \boldsymbol{\omega}_{i}^{T} \boldsymbol{I}_{\ell_{i}} \boldsymbol{\omega}_{i}\]
\[\begin{split}\begin{aligned}
\boldsymbol{I}_{\ell_{i}} & =\left[\begin{array}{ccc}
\int\left(r_{i y}^{2}+r_{i z}^{2}\right) \rho d V & -\int r_{i x} r_{i y} \rho d V & -\int r_{i x} r_{i z} \rho d V \\
-\int r_{i x} r_{i y} & \int\left(r_{i x}^{2}+r_{i z}^{2}\right) \rho d V & -\int r_{i y} r_{i z} \rho d V \\
-\int r_{i x} r_{i z} \rho d V & -\int r_{i y} r_{i z} \rho d V & \int\left(r_{i x}^{2}+r_{i y}^{2}\right) \rho d V
\end{array}\right]
\end{aligned}\end{split}\]
is the inertia tensor relative to the COM of Link \(i\). Since \(\boldsymbol{r}_i\) is expressed in the base frame, this inertia tensor is thus also
expressed in the base frame, thus is configuration-dependent.
Let \(\boldsymbol{r}_i^i\) be expressed in the frame of link \(i\)
\[\begin{split}\begin{gathered}
\boldsymbol{J}_{P j}^{\left(\ell_{i}\right)}= \begin{cases}\boldsymbol{z}_{j-1} & \text { for a prismatic joint } \\
\boldsymbol{z}_{j-1} \times\left(\boldsymbol{p}_{\ell_{i}}-\boldsymbol{p}_{j-1}\right) &
\text { for a revolute joint }\end{cases} \qquad
\boldsymbol{\jmath}_{O j}^{\left(\ell_{i}\right)}= \begin{cases}\mathbf{0} & \text { for a prismatic joint } \\
\boldsymbol{z}_{j-1} & \text { for a revolute joint. }\end{cases}
\end{gathered}\end{split}\]
where \(\boldsymbol{p}_{j-1}\) is the position of the origin of Frame
\(j-1\) and \(\boldsymbol{z}_{j-1}\) is the unit vector of axis \(z\) of Frame
\(j-1\).
It follows that the kinetic energy of Link \(i\) in (34) can be written as
\[\mathcal{U}_{\ell_{i}}=-\int_{V_{\ell_{i}}} \boldsymbol{g}^{T} \boldsymbol{p}_{i}^{*} \rho d V=-m_{\ell_{i}} \boldsymbol{g}^{T} \boldsymbol{p}_{\ell_{i}}\]
where \(\boldsymbol{g}\) is the gravity acceleration vector in the
base frame (e.g., \(\boldsymbol{g}=\)\(\left[\begin{array}{lll}0 & 0 & -g\end{array}\right]^{T}\) if \(z\) is the
vertical axis), and \(\boldsymbol{p}_{\ell_{i}}\) the center of mass of
Link \(i\).
Having computed the total kinetic energy \(\mathcal{T}(\boldsymbol{q}, \dot{\boldsymbol{q}})\) in (36) and potential energy \(\mathcal{U}(\boldsymbol{q})\) in (38) of the robot arm,
the Lagrangian of the system is
\(\sum_{j=1}^{n} b_{i j}(\boldsymbol{q}) \ddot{q}_{j}\): the diagonal coefficient \(b_{i i}\) represents the
moment of inertia at Joint \(i\) axis, and off-diagonal coefficient
\(b_{i j}\) accounts for the coupling effect of acceleration of Joint \(i\) on
Joint \(j\).
\(\sum_{j=1}^{n} \sum_{k=1}^{n} h_{i j k}(\boldsymbol{q}) \dot{q}_{k} \dot{q}_{j}\): the term
\(h_{i j j} \dot{q}_{j}^{2}\) is the centrifugal effect to
Joint \(i\) by Joint \(j\). The term
\(h_{i j k} \dot{q}_{j} \dot{q}_{k}\) represents the Coriolis effect
on Joint \(i\) by Joints \(j\) and \(k\).
\(g_{i}(\boldsymbol{q})\): the term \(g_{i}\) represents
the moment generated at Joint \(i\) axis by the gravity.
There is another commonly-used form of dynamics equation derived from (40), which will be introduced below. The quadratic velocity term in (40) has
where \(\boldsymbol{F}_{v}\) is the \((n \times n)\) diagonal matrix of
viscous friction coefficients; \(\boldsymbol{F}_{s}\) is an \((n \times n)\)
diagonal matrix and \(\operatorname{sgn}(\dot{\boldsymbol{q}})\) is
the \((n \times 1)\) vector whose components are given by the sign
functions of the single joint velocities; and \(\boldsymbol{h}_{e}\)
denotes the vector of force and moment exerted by the end-effector on
the environment.
Property: Skew-symmetry of \(\dot{\boldsymbol{B}}-2\boldsymbol{C}\)#
The idea: the total time derivative of kinetic energy of a robot arm equals the power generated by all the forces/torques
including the gravity.
which holds for any \(\dot{\boldsymbol{q}}\). It means that
\(\boldsymbol{B}(\boldsymbol{q})\dot{\boldsymbol{q}}-2\boldsymbol{C}(\boldsymbol{q}, \dot{\boldsymbol{q}})\)
is a skew-symmetric matrix.
The vector of generalized coordinates is
\(\boldsymbol{q}=\left[\begin{array}{ll}\vartheta_{1} & \vartheta_{2}\end{array}\right]^{T}\).
Let \(\ell_{1}, \ell_{2}\) be the distances of the centres of mass of the
two links from the respective joint axes. Also let
\(m_{\ell_{1}}, m_{\ell_{2}}\) be the masses of the two links, and let \(I_{\ell_{1}}, I_{\ell_{2}}\) be
the moments of inertia relative to the Centers of mass of the two links,
respectively.
With the chosen coordinate frames, by applying (35), the Jacobians for the COM of each
link are
As for the
gravitational terms, here the gravity accelration vector is
\(\boldsymbol{g}=\left[\begin{array}{lll}0 & -g & 0\end{array}\right]^{T}\). By applying (42)
\[\begin{split}\begin{aligned}
& g_{1}=\left(m_{\ell_{1}} \ell_{1}+m_{\ell_{2}} a_{1}\right) g c_{1}+m_{\ell_{2}} \ell_{2} g c_{12} \\
& g_{2}=m_{\ell_{2}} \ell_{2} g c_{12} .
\end{aligned}\end{split}\]
Given only motor toruqes \(\tau_{1}\) and \(\tau_{2}\) applied to the
each joint, the
dynamics equation is
