机器人动力学建模之牛顿欧拉法推导

机器人动力学建模之牛顿欧拉法推导

最近在研究多连杆的机器人建模，发现许多使用牛顿欧拉法的建模方式，都直接采用了如下的一条公式：
$$F_{extj}=M_j\dot{V_j}+{\beta }_j\text{\hspace{1em}(0)}$$
其中：
$$V_j=\left[\begin{array}{c}{U}_j\\ {\Omega }_j\end{array}\right],这是连杆j相对于惯性系的速度和角速度组成的六维向量（表示在j系下）。$$
$${}^j{H}_{j+1},代表的就是一个6维的变换矩阵，将j+1系下的力变换到j系下。$$
$$F_{extj},连杆受到的外力、外力矩在j系下的表示。$$
$$M_j=\left[\begin{array}{cc}m{I}_{3\times 3}& -m\widehat{r_j}\\ m\widehat{r_j}& I_j\end{array}\right]$$
$${\beta }_j=\left[\begin{array}{c}m{\Omega }_j\times U_j+m{\Omega }_j\times ({\Omega }_j\times r_j)\\ m{r}_j\times ({\Omega }_j\times U_j)+{\Omega }_j\times I_j{\Omega }_j\end{array}\right]$$
$$\hat{x}=\left[\begin{array}{ccc}0& -x_3& x_2\\ x_3& 0& -x_1\\ -x_2& x_1& 0\end{array}\right],\hat{x}p=x\times p$$

这个公式的形式很漂亮，用起来也比较方便。但是，如果从牛顿-欧拉方程来看，很难直接推出这个公式。看了很多文献，也没有找到推导过程，于是索性自己推导了一番。

推导过程

1 问题描述

图中蓝色的物体表示一个刚体，我们选择刚体上的某一点，定义了一个与刚体固连的坐标系B，即本体系。
我们假设刚体可被视作为很多微元的组合，并且假设刚体是均匀的。
最终，我们希望得到外力与刚体相对惯性系的速度和加速度的关系式。

2 符号说明

$T$：代表刚体的动能
$P_b$：代表刚体的动量，在{B}系下的表示
${\Pi }_b$：代表刚体的角动量，在{B}系下的表示
$U_b$：代表刚体相对于惯性系的速度，在{B}系下的表示
$U_s$：代表刚体相对于惯性系的速度，在{S}系下的表示
$V_{ib}$：代表刚体微元 i 相对于惯性系的速度，在{B}系下的表示
${\Omega }_b$：代表刚体相对于惯性系的角速度，在{B}系下的表示
${\Omega }_s$：代表刚体相对于惯性系的角速度，在{S}系下的表示
$p_b$：代表p向量在{B}系下的表示
$p_s$：代表p向量在{S}系下的表示
$r_{cb}$：代表$r_c$向量在{B}系下的表示
$r_{ib}$：代表$r_i$向量在{B}系下的表示
$m$：刚体质量
$I_b$：刚体的惯量矩阵，相对于{B}系的
$F_{extb}$：刚体所受合外力在{B}系的表示
$M_{extb}$：刚体所受合外力矩在{B}系的表示

3 基本原理

第一步，计算刚体的动能。

动能是个标量，在任何坐标系下表示都相等，但这个动能必须是相对惯性系而言的。
也就是说，速度和角速度必须是相对与惯性系的。

第二步，动能对速度、角速度求导，得到动量、角动量。

速度可以是表示在{B}系下的，也可以是{S}系下的。如果是相对于表示在{B}系下的速度求导，那么求出来的动量也是表示在{B}系下的。反之亦然。

第三步，动量和角动量对时间求导，得到外力。

这一步的求导需要格外注意。我们需要在惯性系下对时间求导，这样出来的才会是外力。牛顿定律只在惯性系下有效。
如何在惯性系下求导，参考博文[关于机器人运动学与动力学建模的几点领悟](https://blog.csdn.net/handsome_for_kill/article/details/96473701)

链接：关于机器人运动学与动力学建模的几点领悟

4 好戏开始

4.1 计算刚体动能

刚体的动能会等于所有刚体微元的动能之和。
即
$$T=\int \frac{1}{2}{V}_{ib}^{2}dm\text{\hspace{1em}(1)}$$

我们知道，$V_{ib}$可以被表示为：
$$V_{ib}=U_b+{\Omega }_b\times r_{ib}\text{\hspace{1em}(2)}$$

所以，我们把公式(2)代入公式(1)，可以得到：
$$\begin{array}{rl}T& =\int \frac{1}{2}{V}_{ib}^{2}dm\\ & =\int \frac{1}{2}(U_b+{\Omega }_b\times r_{ib})\cdot (U_b+{\Omega }_b\times r_{ib})dm\\ & =\int \frac{1}{2}(U_b\cdot U_b+2U_b\cdot {\Omega }_b\times r_{ib}+{\Omega }_b\times r_{ib}\cdot {\Omega }_b\times r_{ib})dm\\ & =\int \frac{1}{2}{U}_b^{2}dv+\int U_b\cdot {\Omega }_b\times r_{ib}dm+\int \frac{1}{2}{\Omega }_b\times r_{ib}\cdot {\Omega }_b\times r_{ib}dm\\ & =\frac{1}{2}m{U}_b^2+U_b\cdot {\Omega }_b\times \int r_{ib}dm+\frac{1}{2}{\Omega }_b\int r_{ib}\times {\Omega }_b\times r_{ib}dm\\ & =\frac{1}{2}m{U}_b^2+m{U}_b\cdot {\Omega }_b\times r_{cb}+\frac{1}{2}{\Omega }_b{I}_b{\Omega }_b\\ & =\frac{1}{2}m{U}_b^2+m{U}_b\cdot {\Omega }_b\times r_{cb}+\frac{1}{2}{I}_b{\Omega }_b^2\end{array}\text{\hspace{1em}(3)}$$

在这部分的推导中，用到了很多公式：
$$\begin{array}{rl}& {\Omega }_b\times r_{ib}=-r_{ib}\times {\Omega }_b\\ & U_b\cdot {\Omega }_b\times r_{ib}=U_b\times {\Omega }_b\cdot r_{ib}\\ & m=\int 1dm\\ & r_{cb}=\frac{\int r_{ib}dm}{\int 1dm}\\ & \int r_{ib}\times {\Omega }_b\times r_{ib}dm=\int \left[\begin{array}{ccc}{r}_{iby}^2+r_{ibz}^2& -r_{iby}{r}_{ibx}& -r_{ibz}{r}_{ibx}\\ -r_{ibx}{r}_{iby}& r_{ibz}^2+r_{ibx}^2& -r_{ibz}{r}_{iby}\\ -r_{ibx}{r}_{ibz}& -r_{iby}{r}_{ibz}& r_{ibx}^2+r_{iby}^2\end{array}\right]dm\cdot {\Omega }_b=I_b{\Omega }_b\end{array}$$

最终，我们得到了动能的表达式：

$$T=\frac{1}{2}m{U}_b^2+m{U}_b\cdot {\Omega }_b\times r_{cb}+\frac{1}{2}{I}_b{\Omega }_b^2\text{\hspace{1em}(4)}$$

4.2 计算刚体动量

动能对速度求导，得到动量：
$$P_b=\frac{\partial T}{\partial U_b}=m{U}_b+m{\Omega }_b\times r_{cb}\text{\hspace{1em}(5)}$$
动能对角速度求导，得到角动量：
$${\Pi }_b=\frac{\partial T}{\partial {\Omega }_b}=m{r}_{cb}\times U_b+I_b{\Omega }_b\text{\hspace{1em}(6)}$$

4.3 计算与合外力的关系

首先，我们要分析，上一步算出来的刚体动量和动量矩是什么？

动量的定义没有歧义，但是动量矩有。

我们知道，根据动量矩定理，只有刚体对固定点(惯性系原点S)的动量矩对时间求导等于刚体上的合外力对该固定点的合外力矩。即：

$$\frac{\mathrm{d}}{\mathrm{d}\mathrm{t}}{\Pi }_s=M_s$$

而我们上面计算的是对动点b的绝对动量矩，它和$M_{extb}$是什么关系呢？

这篇文章中就不详细推导了，这部分推导可以参见文章：机器人动力学建模之刚体动力学基础学习

这里直接给出结论：

$$\frac{\mathrm{d}}{\mathrm{d}\mathrm{t}}{\Pi }_b=M_{extb}-U_b\times P_b\text{\hspace{1em}(7)}$$

接下来，进入正题：
注意，之前提到，这里需要在惯性系下，将动量和角动量对时间求导：

1、动量对时间求导，得到合外力：
$$\begin{array}{rl}{F}_{extb}=\frac{d^s{P}_b}{dt}& =m\frac{d^s{U}_b}{dt}+m\frac{d^s({\Omega }_b\times r_{cb})}{dt}\\ & =m(\dot{U_b}+{\Omega }_b\times U_b)+m{\dot{\Omega }}_b\times r_{cb}+m{\Omega }_b\times ({\Omega }_b\times r_{cb})\\ & =m\dot{U_b}+m{\Omega }_b\times U_b+m{\dot{\Omega }}_b\times r_{cb}+m{\Omega }_b\times ({\Omega }_b\times r_{cb})\end{array}\text{\hspace{1em}(8)}$$

2、根据公式(7)，角动量对时间求导，得到合外力矩：
$$\begin{array}{rl}{M}_{extb}=\frac{d^s{\Pi }_b}{dt}+U_b\times P_b& =m\frac{d^s(r_{cb}\times U_b)}{dt}+\frac{d^s(I_b{\Omega }_b)}{dt}+m{U}_b\times ({\Omega }_b\times r_{cb})\\ & =m(r_{cb}\times {\dot{U}}_b+{\Omega }_b\times (r_{cb}\times U_b))+I_b{\dot{\Omega }}_b+{\Omega }_b\times (I_b{\Omega }_b)+m{U}_b\times ({\Omega }_b\times r_{cb})\\ & =m{r}_{cb}\times \dot{U_b}+I_b{\dot{\Omega }}_b+{\Omega }_b\times (I_b{\Omega }_b)+m[{\Omega }_b\times (r_{cb}\times U_b)+U_b\times ({\Omega }_b\times r_{cb})]\\ & =m{r}_{cb}\times \dot{U_b}+I_b{\dot{\Omega }}_b+{\Omega }_b\times (I_b{\Omega }_b)+m{r}_{cb}\times ({\Omega }_b\times U_b)\end{array}\text{\hspace{1em}(9)}$$

整理一下公式(7)和公式(8)，得到：
$$\begin{array}{rl}& F_{extb}=m\dot{U_b}+m{\Omega }_b\times U_b+m{\dot{\Omega }}_b\times r_{cb}+m{\Omega }_b\times ({\Omega }_b\times r_{cb})\\ & M_{extb}=m{r}_{cb}\times \dot{U_b}+I_b{\dot{\Omega }}_b+{\Omega }_b\times (I_b{\Omega }_b)+m{r}_{cb}\times ({\Omega }_b\times U_b)\end{array}\text{\hspace{1em}(10)}$$

整理成矩阵形式：
$$\left[\begin{array}{c}{F}_{extb}\\ M_{extb}\end{array}\right]=\left[\begin{array}{cc}m{I}_{3\times 3}& -m{\hat{r}}_{cb}\\ m{\hat{r}}_{cb}& I_b\end{array}\right]\left[\begin{array}{c}\dot{U_b}\\ {\dot{\Omega }}_b\end{array}\right]+\left[\begin{array}{c}m{\Omega }_b\times U_b+m{\Omega }_b\times ({\Omega }_b\times r_{cb})\\ m{r}_{cb}\times ({\Omega }_b\times U_b)+{\Omega }_b\times (I_b{\Omega }_b)\end{array}\right]\text{\hspace{1em}(11)}$$
可以看到，这个式子和公式(0)是完全一摸一样的，推到这里我们就得到了答案。

值得注意，这里的推导用到了下面的求导方法，其中$r_{cb}$是常数向量。
$${\Omega }_b\times r_{cb}=[{\Omega }_{bx},{\Omega }_{by},{\Omega }_{bz}]\times [r_x,r_y,r_z]=[{\Omega }_{by}{r}_z-{\Omega }_{bz}{r}_y,{\Omega }_{bz}{r}_x-{\Omega }_{bx}{r}_z,{\Omega }_{bx}{r}_y-{\Omega }_{by}{r}_x]$$
$$\begin{array}{rl}\frac{d^s({\Omega }_b\times r_{cb})}{dt}& =\frac{d^s}{dt}[({\Omega }_{by}{r}_z-{\Omega }_{bz}{r}_y)\cdot i_b+({\Omega }_{bz}{r}_x-{\Omega }_{bx}{r}_z)\cdot j_b+({\Omega }_{bx}{r}_y-{\Omega }_{by}{r}_x)\cdot k_b]\\ & =\frac{d^s({\Omega }_{by}{r}_z-{\Omega }_{bz}{r}_y)}{dt}\cdot i_b+\frac{d^s({\Omega }_{bz}{r}_x-{\Omega }_{bx}{r}_z)}{dt}\cdot j_b+\frac{d^s({\Omega }_{bx}{r}_y-{\Omega }_{by}{r}_x)}{dt}\cdot k_b\\ & +({\Omega }_{by}{r}_z-{\Omega }_{bz}{r}_y)\cdot \frac{d^s{i}_b}{dt}+({\Omega }_{bz}{r}_x-{\Omega }_{bx}{r}_z)\cdot \frac{d^s{j}_b}{dt}+({\Omega }_{bx}{r}_y-{\Omega }_{by}{r}_x)\cdot \frac{d^s{k}_b}{dt}\\ & =({\dot{\Omega }}_{by}{r}_z-{\dot{\Omega }}_{bz}{r}_y)\cdot i_b+({\dot{\Omega }}_{bz}{r}_x-{\dot{\Omega }}_{bx}{r}_z)\cdot j_b+({\dot{\Omega }}_{bx}{r}_y-{\dot{\Omega }}_{by}{r}_x)\cdot k_b\\ & +{\Omega }_b\times ({\Omega }_{by}{r}_z-{\Omega }_{bz}{r}_y)\cdot i_b+{\Omega }_b\times ({\Omega }_{bz}{r}_x-{\Omega }_{bx}{r}_z)\cdot j_b+{\Omega }_b\times ({\Omega }_{bx}{r}_y-{\Omega }_{by}{r}_x)\cdot k_b\\ & ={\dot{\Omega }}_b\times r_{cb}+{\Omega }_b\times ({\Omega }_b\times r_{cb})\end{array}$$
同理，我们有：
$$\frac{d^s(r_{cb}\times U_b)}{dt}=r_{cb}\times {\dot{U}}_b+{\Omega }_b\times (r_{cb}\times U_b)$$
此外，还有：
$${\Omega }_b\times (r_{cb}\times U_b)+U_b\times ({\Omega }_b\times r_{cb})=r_{cb}\times ({\Omega }_b\times U_b)$$

另外，由于对于{b}系而言，$I_b$是常数矩阵，所以
$$\begin{array}{rl}\frac{d^s(I_d{\Omega }_b)}{dt}& =\frac{d^s}{dt}\left(\left[\begin{array}{ccc}{I}_{xx}& I_{xy}& I_{xz}\\ I_{yx}& I_{yy}& I_{yz}\\ I_{zx}& I_{zy}& I_{zz}\end{array}\right]\left[\begin{array}{c}{\Omega }_{bx}\\ {\Omega }_{by}\\ {\Omega }_{bz}\end{array}\right]\right)\\ & =\frac{d^s}{dt}\left(\begin{array}{c}{I}_{xx}{\Omega }_{bx}+I_{xy}{\Omega }_{by}+I_{xz}{\Omega }_{bz}\\ I_{yx}{\Omega }_{bx}+I_{yy}{\Omega }_{by}+I_{yz}{\Omega }_{bz}\\ I_{zx}{\Omega }_{bx}+I_{zy}{\Omega }_{by}+I_{zz}{\Omega }_{bz}\end{array}\right)\\ & =\left(\begin{array}{c}\frac{d^s}{dt}(I_{xx}{\Omega }_{bx}\cdot i_b+I_{xy}{\Omega }_{by}\cdot j_b+I_{xz}{\Omega }_{bz}\cdot k_b)\\ \frac{d^s}{dt}(I_{yx}{\Omega }_{bx}\cdot i_b+I_{yy}{\Omega }_{by}\cdot j_b+I_{yz}{\Omega }_{bz}\cdot k_b))\\ \frac{d^s}{dt}(I_{zx}{\Omega }_{bx}\cdot i_b+I_{zy}{\Omega }_{by}\cdot j_b+I_{zz}{\Omega }_{bz}\cdot k_b))\end{array}\right)\\ & =\left(\begin{array}{c}{I}_{xx}{\dot{\Omega }}_{bx}\cdot i_b+I_{xy}{\dot{\Omega }}_{by}\cdot j_b+I_{xz}{\dot{\Omega }}_{bz}\cdot k_b+{\Omega }_b\times (I_{xx}{\Omega }_{bx}\cdot i_b+I_{xy}{\Omega }_{by}\cdot j_b+I_{xz}{\Omega }_{bz}\cdot k_b)\\ I_{yx}{\dot{\Omega }}_{bx}\cdot i_b+I_{yy}{\dot{\Omega }}_{by}\cdot j_b+I_{yz}{\dot{\Omega }}_{bz}\cdot k_b+{\Omega }_b\times (I_{yx}{\Omega }_{bx}\cdot i_b+I_{yy}{\Omega }_{by}\cdot j_b+I_{yz}{\Omega }_{bz}\cdot k_b)\\ I_{zx}{\dot{\Omega }}_{bx}\cdot i_b+I_{zy}{\dot{\Omega }}_{by}\cdot j_b+I_{zz}{\dot{\Omega }}_{bz}\cdot k_b+{\Omega }_b\times (I_{zx}{\Omega }_{bx}\cdot i_b+I_{zy}{\Omega }_{by}\cdot j_b+I_{zz}{\Omega }_{bz}\cdot k_b)\end{array}\right)\\ & =I_b{\dot{\Omega }}_b+{\Omega }_b\times (I_b{\Omega }_b)\end{array}$$

小结

最近学习建模耽误了很多时间，其实这么老的方法，直接把论文里的公式拿来用就OK了，但是我就是控制不住自己，想要把来龙去脉搞清楚。查了很多资料都没有查到，最后只好自己动手。

当然这里也借鉴了很多地方的资料，参考如下：

惯性矩、惯性积、转动惯量、惯性张量
关于机器人运动学与动力学建模的几点领悟
牛顿—欧拉方程
UNDERWATER GLIDERS: DYNAMICS, CONTROL AND DESIGN

最后，希望本文能够对大家有所帮助！