Modern Robotics单个刚体的动力学

Modern Robotics单个刚体的动力学

基本公式

考虑刚体由质量为$m_i$的一系列质点$i$组成，总质量为$\text{m}={\sum }_i{\text{m}}_i$。物体坐标系原点建立在刚体质心处$r_i=(x_i,y_i,z_i)$，则有${\sum }_i{\text{m}}_i{r}_i=0$.

设一个刚体的运动旋量为${\mathcal{V}}_b=({\omega }_b,v_b)$,质点${\text{m}}_i$的位置坐标为$p_i(t)$, 在惯性坐标系{b}的最初位置为$r_i$。注意这些物理量都是在物体坐标系中描述，那么有
$$\begin{gathered} {\dot{p}}_i=v_b+{\omega }_b\times p_i \\ {\ddot{p}}_i={\dot{v}}_b+\frac{d}{dt}{\omega }_b\times p_i+{\omega }_b\times \frac{d}{dt}{p}_i \\ ={\dot{v}}_b+{\dot{\omega }}_b\times p_i+{\omega }_b\times (v_b+{\omega }_b\times p_i) \end{gathered}$$
把$r_i$代替$p_i$，并使用反对称矩阵表示叉乘，那么有
$${\ddot{p}}_i={\dot{v}}_b+[{\dot{\omega }}_b]r_i+[{\omega }_b]v_b+[{\omega }_b{]}^2{r}_i$$
根据牛顿第二定律，对于一个质点有$f_i=m_i{\ddot{p}}_i$,作用在质点$m_i$的力为
$$f_i={\text{m}}_i({\dot{v}}_b+[{\dot{\omega }}_b]r_i+[{\omega }_b]v_b+[{\omega }_b{]}^2{r}_i)$$
这意味着力矩为
$$m_i=[r_i]f_i.$$
作用在刚体的总力和总力矩用力旋量${\mathcal{F}}_b$表示为
$${\mathcal{F}}_b=\left[\begin{array}{c}{m}_b\\ f_b\end{array}\right]=\left[\begin{array}{c}{\sum }_i{m}_i\\ {\sum }_i{f}_i\end{array}\right]$$
下面化简$f_b,m_b$,注意到${\sum }_i{\text{m}}_i{r}_i=0$(因此，${\sum }_i{\text{m}}_i[r_i]=0$), 对于$a,b\in {\mathbb{R}}^3,[a]=-[a{]}^T,[a]b=-[b]a,[a][b]=([b][a]{)}^T$.这些关系在下面化简中会用到。

平动动力学
$$\begin{gathered} f_b=\sum _i{\text{m}}_i({\dot{v}}_b+[{\dot{\omega }}_b]r_i+[{\omega }_b]v_b)+[{\omega }_b{]}^2{r}_i \\ =\sum _i{\text{m}}_i({\dot{v}}_b+[{\omega }_b]v_b)-\sum _i{\text{m}}_i[r_i]{\dot{\omega }}_b+\sum _i{\text{m}}_i[r_i][{\omega }_b]{\omega }_b \\ =\text{m}({\dot{v}}_b+[{\omega }_b]v_b).\text{\hspace{1em}(8.22)} \end{gathered}$$
转动动力学
$$\begin{gathered} m_b=\sum _i{\text{m}}_i[r_i]({\dot{v}}_b+[{\dot{\omega }}_b]r_i+[{\omega }_b]v_b+[{\omega }_b{]}^T{r}_i) \\ =\sum _i{\text{m}}_i[r_i]{\dot{v}}_b+\sum _i{\text{m}}_i[r_i][{\omega }_b]v_b+\sum _i{\text{m}}_i[r_i]([{\dot{\omega }}_b]r_i+[{\omega }_b{]}^T{r}_i) \\ =\sum _i{\text{m}}_i(-[r_i{]}^2{\dot{\omega }}_b-[r_i{]}^T[{\omega }_b{]}^T[r_i]{\omega }_b) \\ =\left(-\sum _i{\text{m}}_i[r_i{]}^2\right){\dot{\omega }}_b+[{\omega }_b]\left(-\sum _i{\text{m}}_i[r_i{]}^2\right) \\ ={\mathcal{I}}_b{\dot{\omega }}_b+[{\omega }_b]{\mathcal{I}}_b{\omega }_b \end{gathered}$$

即
$$m_b={\mathcal{I}}_b{\dot{\omega }}_b+[{\omega }_b]{\mathcal{I}}_b{\omega }_b\text{\hspace{1em}(8.23)}$$
这里${\mathcal{I}}_b=-{\sum }_i{\text{m}}_i[r_i{]}^2\in {\mathbb{R}}^{3\times 3}$ 是旋转惯量矩阵。方程（8.23）称为旋转刚体的欧拉方程。

写出$I_b$的每个元素，有
$$\begin{gathered} {\mathcal{I}}_b=\left[\begin{array}{ccc}\sum {\text{m}}_i(y_i^2+z_i^2)& -\sum {\text{m}}_i{x}_i{y}_i& -\sum {\text{m}}_i{x}_i{z}_i\\ -\sum {\text{m}}_i{x}_i{y}_i& \sum {\text{m}}_i(x_i^2+z_i^2)& -\sum {\text{m}}_i{y}_i{z}_i\\ -\sum {\text{m}}_i{x}_i{z}_i& -\sum {\text{m}}_i{y}_i{z}_i& \sum {\text{m}}_i(x_i^2+y_i^2)\end{array}\right] \\ =\left[\begin{array}{ccc}{\mathcal{I}}_{xx}& {\mathcal{I}}_{xy}& {\mathcal{I}}_{xz}\\ {\mathcal{I}}_{xy}& {\mathcal{I}}_{yy}& {\mathcal{I}}_{yz}\\ {\mathcal{I}}_{xz}& {\mathcal{I}}_{yz}& {\mathcal{I}}_{zz}\end{array}\right] \end{gathered}$$
上面求和形式可以写成积分形式，物体体积为$\mathcal{B}$, 体积微元$dV$, 质点${\text{m}}_i$以密度函数替换$\rho (x,y,z)$,
$$\{\begin{array}{l}{\mathcal{I}}_{xx}={\int }_{\mathcal{B}}(y^2+z^2)\rho (x,y,z)dV\\ {\mathcal{I}}_{yy}={\int }_{\mathcal{B}}(x^2+z^2)\rho (x,y,z)dV\\ {\mathcal{I}}_{zz}={\int }_{\mathcal{B}}(x^2+y^2)\rho (x,y,z)dV\\ {\mathcal{I}}_{zz}={\int }_{\mathcal{B}}(x^2+y^2)\rho (x,y,z)dV\\ {\mathcal{I}}_{xy}={\int }_{\mathcal{B}}xy\rho (x,y,z)dV\\ {\mathcal{I}}_{xz}={\int }_{\mathcal{B}}xz\rho (x,y,z)dV\\ {\mathcal{I}}_{yz}={\int }_{\mathcal{B}}yz\rho (x,y,z)dV\end{array}\text{\hspace{1em}(8.24)}$$
给定一个惯量矩阵${\mathcal{I}}_b$, 惯量主轴由${\mathcal{I}}_b$的特征向量和特征值给定。设特征向量为$v_1,v_2,v_3$,特征值为${\lambda }_1,{\lambda }_2,{\lambda }_3$. 那么惯量主轴的方向为特征向量$v_1,v_2,v_3$的方向，在这些轴的转动惯量值为主惯量矩，分别是${\lambda }_1,{\lambda }_2,{\lambda }_3>0$ .

假设在坐标系{b}中，一个惯量矩阵表示为${\mathcal{I}}_b$, 坐标系{c}可由旋转矩阵$R_{bc}$描述，那么该惯量矩阵在坐标系{c}中表示为${\mathcal{I}}_c$。 由于旋转刚体的动能与选择的坐标系无关，因此有
$$\begin{gathered} \frac{1}{2}{\omega }_c^T{\mathcal{I}}_c{\omega }_c=\frac{1}{2}{\omega }_b^T{\mathcal{I}}_b{\omega }_b \\ =\frac{1}{2}{\omega }_c^T(R_{bc}^T{\mathcal{I}}_b{R}_{bc}){\omega }_c. \end{gathered}$$
也就是说
$${\mathcal{I}}_c=R_{bc}^T{I}_b{R}_{bc}.\text{\hspace{1em}(8.26)}$$
如果{b}的坐标轴与惯量主轴不平行，我们可以在经过旋转变换$R_{bc}$的坐标系{c}中表示该惯量矩阵, 从而可以把惯量矩阵对角化。$R_{bc}$ 的列就是${\mathcal{I}}_b$的向量。

有时惯量矩阵不在质心的坐标系表示更方便，例如在关节处的坐标系表示。那么可以通过斯坦纳定理（平行轴定理）转换。

定理：惯量矩阵${\mathcal{I}}_q$定义在坐标系{q}中，各轴与坐标系{b}平行，q为坐标系{b}中坐标为$q=(q_x,q_y,q_z)$的一点，定义在质心{b}的坐标系的惯量矩阵为${\mathcal{I}}_b$,那么
$${\mathcal{I}}_q={\mathcal{I}}_b+\text{m}(q^{T}qI-q{q}^T),\text{\hspace{1em}(8.27)}$$
这里$I$是$3\times 3$的单位矩阵，$\text{m}$为刚体的质量。

力旋量

上面的式(8.22)和式(8.23)可以写为
$$\left[\begin{array}{c}{m}_b\\ f_b\end{array}\right]=\left[\begin{array}{cc}{\mathcal{I}}_b& 0\\ 0& \text{m}I\end{array}\right]\left[\begin{array}{c}{\dot{\omega }}_b\\ {\dot{v}}_b\end{array}\right]+\left[\begin{array}{cc}[{\omega }_b]& 0\\ 0& [{\omega }_b]\end{array}\right]\left[\begin{array}{cc}{\mathcal{I}}_b& 0\\ 0& \text{m}I\end{array}\right]\left[\begin{array}{c}{\omega }_b\\ v_b\end{array}\right]\text{\hspace{1em}(8.29)}$$

这里$I$是$3\times 3$的单位阵。另外$[v]v=v\times v=0,[v{]}^T=-[v]$，那么式（8.29）可以写为如下等效形式
$$\begin{gathered} \left[\begin{array}{c}{m}_b\\ f_b\end{array}\right]=\left[\begin{array}{cc}{\mathcal{I}}_b& 0\\ 0& \text{m}I\end{array}\right]\left[\begin{array}{c}{\dot{\omega }}_b\\ {\dot{v}}_b\end{array}\right]+\left[\begin{array}{cc}[{\omega }_b]& [v_b]\\ 0& [{\omega }_b]\end{array}\right]\left[\begin{array}{cc}{\mathcal{I}}_b& 0\\ 0& \text{m}I\end{array}\right]\left[\begin{array}{c}{\omega }_b\\ v_b\end{array}\right] \\ =\left[\begin{array}{cc}{\mathcal{I}}_b& 0\\ 0& \text{m}I\end{array}\right]\left[\begin{array}{c}{\dot{\omega }}_b\\ {\dot{v}}_b\end{array}\right]-{\left[\begin{array}{cc}[{\omega }_b]& 0\\ [v_b]& [{\omega }_b]\end{array}\right]}^T\left[\begin{array}{cc}{\mathcal{I}}_b& 0\\ 0& \text{m}I\end{array}\right]\left[\begin{array}{c}{\omega }_b\\ v_b\end{array}\right]\text{\hspace{1em}(8.30)} \end{gathered}$$
这样方程中的每一项可以用一个6维的空间量表示：

(1)向量$({\omega }_b,v_b)，(m_b,f_b)$分别用物体旋量${\mathcal{V}}_b$和力旋量${\mathcal{F}}_b$表示,
$${\mathcal{V}}_b=\left[\begin{array}{c}{\omega }_b\\ v_b\end{array}\right],{\mathcal{F}}_b=\left[\begin{array}{c}{m}_b\\ f_b\end{array}\right]\text{\hspace{1em}(8.31)}$$
(2)空间惯量矩阵${\mathcal{G}}_b\in {\mathbb{R}}^{6\times 6}$定义为
$${\mathcal{G}}_b=\left[\begin{array}{cc}{\mathcal{I}}_b& 0\\ 0& \text{m}I\end{array}\right]\text{\hspace{1em}(8.32)}$$
这时候动能用空间惯量矩阵可以表示为
$$E=\frac{1}{2}{\omega }_b^T{\mathcal{I}}_b{\omega }_b+\frac{1}{2}\text{m}{v}_b^T{v}_b=\frac{1}{2}{\mathcal{V}}_b^T{\mathcal{G}}_b{\mathcal{V}}_b.\text{\hspace{1em}(8.33)}$$
(3)空间动量${\mathcal{P}}_b\in {\mathbb{R}}^{6\times 6}$定义为
$${\mathcal{P}}_b=\left[\begin{array}{c}{\mathcal{I}}_b\\ \text{m}{v}_b\end{array}\right]=\left[\begin{array}{cc}{\mathcal{I}}_b& 0\\ 0& \text{m}I\end{array}\right]\left[\begin{array}{c}{\omega }_b\\ v_b\end{array}\right]={\mathcal{G}}_b{\mathcal{V}}_b.\text{\hspace{1em}(8.34)}$$

观察发现式（8.30）右边第二项是${\mathcal{P}}_b$左乘下面的矩阵构成

$$-{\left[\begin{array}{cc}[{\omega }_b]& 0\\ [v_b]& [{\omega }_b]\end{array}\right]}^T.\text{\hspace{1em}(8.35)}$$

注意，两个矢量的叉乘可以用反对称矩阵表示

$$[{\omega }_1\times {\omega }_2]=[{\omega }_1][{\omega }_2]-[{\omega }_2][{\omega }_1].\text{\hspace{1em}(8.36)}$$

式（8.35）中的矩阵可以看成是6维旋量叉乘运算的一般化。给定两个旋量${\mathcal{V}}_1=({\omega }_1,v_1),{\mathcal{V}}_2=({\omega }_2,v_2)$, 进行

$$\begin{gathered} [{\mathcal{V}}_1][{\mathcal{V}}_2]-[{\mathcal{V}}_2][{\mathcal{V}}_1]=\left[\begin{array}{cc}[{\omega }_1]& v_1\\ 0& 0\end{array}\right]\left[\begin{array}{cc}[{\omega }_2]& v_2\\ 0& 0\end{array}\right]-\left[\begin{array}{cc}[{\omega }_2]& v_2\\ 0& 0\end{array}\right]\left[\begin{array}{cc}[{\omega }_1]& v_1\\ 0& 0\end{array}\right] \\ =\left[\begin{array}{cc}[{\omega }_1][{\omega }_2]-[{\omega }_2][{\omega }_1]& [{\omega }_1]v_2-[{\omega }_2]v_1\\ 0& 0\end{array}\right] \\ =\left[\begin{array}{cc}[{\omega }^{\prime }]& [v^{\prime }]\\ 0& 0\end{array}\right], \end{gathered}$$
还可以用向量写成更简洁的形式
$$\left[\begin{array}{c}{\omega }^{\prime }\\ v^{\prime }\end{array}\right]=\left[\begin{array}{cc}[{\omega }_1]& 0\\ [v_1]& [{\omega }_1]\end{array}\right]\left[\begin{array}{c}{\omega }_2\\ v_2\end{array}\right]$$
一般地两个旋量的叉乘称为李括号.

定义：给定两个旋量${\mathcal{V}}_1=({\omega }_1,v_1),{\mathcal{V}}_2=({\omega }_2,v_2)$, ${\mathcal{V}}_1$和${\mathcal{V}}_2$的李括号写成$[{\text{ad}}_{{\mathcal{V}}_1}]{\mathcal{V}}_2$ 或者${\text{ad}}_{{\mathcal{V}}_1}({\mathcal{V}}_2)$, 定义为
$$\left[\begin{array}{cc}[{\omega }_1]& 0\\ [v_1]& [{\omega }_1]\end{array}\right]\left[\begin{array}{c}{\omega }_2\\ v_2\end{array}\right]=[{\text{ad}}_{{\mathcal{V}}_1}]{\mathcal{V}}_1={\text{ad}}_{{\mathcal{V}}_1}({\mathcal{V}}_2)\in {\mathbb{R}}^6,\text{\hspace{1em}(8.37)}$$
其中
$$[{\text{ad}}_{\mathcal{V}}]=\left[\begin{array}{cc}[\omega ]& 0\\ [v]& [\omega ]\end{array}\right]\in {\mathbb{R}}^{6\times 6}$$
定义：给一个运动旋量$\mathcal{V}=(\omega ,v)$和力旋量$\mathcal{F}=(m,f)$, 定义映射
$${\text{ad}}_{\mathcal{V}}^T(\mathcal{F})=[{\text{ad}}_{\mathcal{V}}{]}^T(\mathcal{F})={\left[\begin{array}{cc}[\omega ]& 0\\ [v]& [\omega ]\end{array}\right]}^T\left[\begin{array}{c}m\\ f\end{array}\right]=\left[\begin{array}{c}-[\omega ]m-[v]f\\ -[\omega ]f\end{array}\right].\text{\hspace{1em}(8.39)}$$
使用上述符号和定义，单个刚体的动力学方程可以写成
$$\begin{gathered} {\mathcal{F}}_b={\mathcal{G}}_b\dot{\mathcal{V}}-{\text{ad}}_{{\mathcal{V}}_b}^T({\mathcal{P}}_b) \\ ={\mathcal{G}}_b\dot{\mathcal{V}}-[{\text{ad}}_{{\mathcal{V}}_b}{]}^T{\mathcal{G}}_b{\mathcal{V}}_b\text{\hspace{1em}(8.40)} \end{gathered}$$

在其他坐标系的动力学

式（8.40）依赖于使用的质心坐标系{b}。在另一个坐标系{a}中的动力学方程，可以通过动能不依赖于坐标系的性质推导出来
$$\begin{gathered} \frac{1}{2}={\mathcal{V}}_a^{\mathcal{T}}{\mathcal{G}}_a{\mathcal{V}}_a=\frac{1}{2}{\mathcal{V}}_b^{\mathcal{T}}{\mathcal{G}}_b{\mathcal{V}}_b \\ =\frac{1}{2}([A{d}_{T_{ba}}]{\mathcal{V}}_a{)}^T{\mathcal{G}}_b[A{d}_{T_{ba}}]{\mathcal{V}}_a \end{gathered}$$
其中Ad是伴随表示. 也就是说{a}坐标系的空间惯量矩阵${\mathcal{G}}_a$与${\mathcal{G}}_b$的关系可以用下式表示
$${\mathcal{G}}_a=[A{d}_{T_{ba}}{]}^T{\mathcal{G}}_b[A{d}_{T_{ba}}].\text{\hspace{1em}(8.42)}$$
使用空间惯量矩阵${\mathcal{G}}_a$,在{b}坐标系的对应的运动方程(8.40)在{a}坐标系可等效地写成
$${\mathcal{F}}_a={\mathcal{G}}_a{\dot{\mathcal{V}}}_a-[{\text{ad}}_{{\mathcal{V}}_a}{]}^T{\mathcal{G}}_a{\mathcal{V}}_a,\text{\hspace{1em}(8.43)}$$
其中${\mathcal{F}}_a,{\mathcal{V}}_a$是在坐标系{a}中描述的力旋量和运动旋量.

参考文献

[1] Kenvin M. Lynch , Frank C. Park, Modern Robotics Mechanics,Planning , and Control. May 3, 2017