机械臂笔记（六）机械臂的动力学模型（牛顿-欧拉方程）

通过欧拉-拉格朗日方程在步骤上可以方便地对机器人进行动力学建模，但是最后得到的公式会比较复杂，难以计算。最终工程上还是会用牛顿-欧拉法对机器人进行动力学建模，更利于控制算法的设计。

一般来说，我们（忽略地球的自转和公转等）选择机械臂底座的中心作为世界坐标系（惯性系）；如果底座也在动，我们需要选择地面上的一个固定点作为坐标系的原点。
这个坐标系不加速、不转动，或者相对真正的宇宙惯性系只做匀速直线运动

推导过程

我们假设上图蓝色的物体是一个刚体，我们选择刚体上的某一点，在此点上定义了一个与该刚体固连的坐标系B，即本体系。假设该刚体是均匀的，是由许多微元组成。最终，我们希望得到外力与刚体相对惯性系的速度与加速度的关系式。

符号定义

比较容易搞混的概念就是不同坐标系下的变换，所以符号相对于哪个坐标系非常重要。

符号	表示坐标系	说明
$T$	$S$	代表刚体的动能，必须相对于惯性系，不然坐标系选择会影响数值
$P_b$	$B$	刚体相对于惯性系的动量，用本体系的坐标表示
${\mathrm{\Pi }}_b$	$B$	刚体相对于惯性系的角动量，用本体系的坐标表示
$U_b$	$B$	刚体相对于惯性系的速度，用本体系的坐标表示
${\mathrm{\Omega }}_b$	$B$	刚体相对于惯性系的角速度，用本体系的坐标表示
$U_s$	$S$	刚体相对于惯性系的速度，用惯性系的坐标表示
${\mathrm{\Omega }}_s$	$S$	刚体相对于惯性系的角速度，用惯性系的坐标表示
$V_{i_b}$	$B$	刚体微元$i$ 相对于惯性系的速度，用本体系的坐标表示
$\overrightarrow{p_b}$	$B$	向量$p$ 在本体系下的表示
$\overrightarrow{p_s}$	$S$	向量$p$ 在惯性系下的表示
$\overrightarrow{r_{c_b}}$	$B$	向量$r_c$ 在本体系下的表示
$\overrightarrow{r_{i_b}}$	$B$	向量$r_i$ 在本体系下的表示
$m$	-	表示刚体的质量
$I_b$	-	刚体的惯量矩阵，相对于本体系
$F_{ex{t}_b}$	B	刚体所受的合外力，在本体系下的表示
$M_{ex{t}_b}$	B	刚体所受的合外力矩，在本体系下的表示

其中有一些定义公式，可能对以后的推导有帮助：

• 质心

$$\begin{array}{}\text{(0.1)}& r_{c_b}=\frac{1}{m}\int r_{i_b}dm\end{array}$$

• 标准向量恒等式（对称的）

$$\begin{array}{}\text{(0.2)}& \begin{array}{lll}(a\times b)\cdot (a\times b)& =& a^T(||b|{|}^{2}I-bbT)a\\ & =& b^T(||a|{|}^{2}I-aaT)b\end{array}\end{array}$$

• 惯量矩阵

$$\begin{array}{}\text{(0.3)}& I_b=\int (||r_{i_b}||I-r_{i_b}{r}_{i_b}^T)dm\end{array}$$

• 对于任意向量$a$，其在惯性系与本体系之间的导数关系为欧拉动力学方程所示：

$$\begin{array}{}\text{(0.4)}& (\frac{da}{dt}{)}_s=(\frac{da}{dt}{)}_b+{\mathrm{\Omega }}_b\times a\end{array}$$

• 向量微分

$$\begin{array}{}\text{(0.5)}& \frac{d}{dt}(a\times b)=a^{\prime }\times b+a\times b^{\prime }\end{array}$$

计算刚体动能

刚体的动能等于所有微元的动能之和：

$$\begin{array}{}\text{(1)}& T=\int \frac{1}{2}{V}_{i_b}^{2}dm\end{array}$$

由于刚体上任意微元的运动都可以分解为参考本体系的平动和转动，因此微元的速度可以表示为线速度和转动诱导速度之和：

$$\begin{array}{}\text{(2)}& V_{i_b}=U_b+{\mathrm{\Omega }}_b\times r_{i_b}\end{array}$$

联立式$(0.1),(0.2),(0.3),(1),(2)$，可得：

$$\begin{array}{}\text{(3)}& \begin{array}{lll}T& =& \int \frac{1}{2}{V}_{i_b}^{2}dm\\ & =& \int \frac{1}{2}(U_b+{\mathrm{\Omega }}_b\times r_{i_b})\cdot (U_b+{\mathrm{\Omega }}_b\times r_{i_b})dm\\ & =& \int \frac{1}{2}{U}_b^{2}dm+\int U_b\cdot ({\mathrm{\Omega }}_b\times r_{i_b})dm+\int \frac{1}{2}({\mathrm{\Omega }}_b\times r_{i_b})\cdot ({\mathrm{\Omega }}_b\times r_{i_b})dm\\ & =& \frac{1}{2}m{U}_b^T{U}_b+U_b\cdot {\mathrm{\Omega }}_b\times \int r_{i_b}dm+\frac{1}{2}{\mathrm{\Omega }}_b^T[\int (||r_{i_b}|{|}^{2}I-r_{i_b}{r}_{i_b}^T)dm]{\mathrm{\Omega }}_b\\ & =& \frac{1}{2}m{U}_b^T{U}_b+m{U}_b\cdot {\mathrm{\Omega }}_b\times r_{c_b}+\frac{1}{2}{\mathrm{\Omega }}_b^T{I}_b{\mathrm{\Omega }}_b\end{array}\end{array}$$

计算刚体动量

动能对速度求导，即可得到动量：

$$\begin{array}{}\text{(4)}& \begin{array}{lll}{P}_b& =& \frac{\partial T}{\partial U_b}\\ & =& m{U}_b+m{\mathrm{\Omega }}_b\times r_{c_b}\end{array}\end{array}$$

动能对角速度求导，即可得到角动量：

$$\begin{array}{}\text{(5)}& \begin{array}{lll}{\mathrm{\Pi }}_b& =& \frac{\partial T}{\partial {\mathrm{\Omega }}_b}\\ & =& m{r}_{c_b}\times U_b+I_b{\mathrm{\Omega }}_b\end{array}\end{array}$$

计算动能/动量与合外力的关系

在惯性系下，角动量与力矩的关系如下：

$$\begin{array}{}\text{(6)}& \frac{d}{dt}{\mathrm{\Pi }}_s=M_s\end{array}$$

但是到本体系里面，结合式$(0.4)$，它与合外力矩的关系是：

$$\begin{array}{}\text{(7)}& \begin{array}{lll}\frac{d}{dt}{\mathrm{\Pi }}_s& =& \frac{d}{dt}{\mathrm{\Pi }}_b+{\mathrm{\Omega }}_b\times {\mathrm{\Pi }}_b\\ & \Downarrow & \\ \frac{d}{dt}{\mathrm{\Pi }}_b& =& \frac{d}{dt}{\mathrm{\Pi }}_s-{\mathrm{\Omega }}_b\times {\mathrm{\Pi }}_b\\ & \Downarrow & {}_{(\mathrm{复}\mathrm{杂}\mathrm{的}\mathrm{恒}\mathrm{等}\mathrm{变}\mathrm{换})}\\ M_{ex{t}_b}& =& M_{ex{t}_b}-U_b\times P_b\end{array}\end{array}$$

在惯性系下，结合$(4)$ 式，对动量和角动量对时间求导：

$$\begin{array}{}\text{(8)}& \begin{array}{lll}{F}_{ex{t}_b}& =& \frac{d^s{P}_b}{dt}\\ & =& m\frac{d^s{U}_b}{dt}+\frac{d^s{\mathrm{\Omega }}_b\times r_{c_b}}{dt}\\ & =& m{U}_b^{\prime }+m{\mathrm{\Omega }}_b\times U_b+m{\mathrm{\Omega }}_b^{\prime }\times r_{c_b}+m{\mathrm{\Omega }}_b\times ({\mathrm{\Omega }}_b\times r_{c_b})\end{array}\end{array}$$

同样，对于公式$(7)$ 有：

$$\begin{array}{}\text{(9)}& \begin{array}{lll}{M}_{ex{t}_b}& =& \frac{d^s{\mathrm{\Pi }}_b}{dt}+U_b\times P_b\\ & =& m\frac{d^s(r_{c_b}\times Ub)}{dt}+\frac{d^s{I}_b{\mathrm{\Omega }}_b}{dt}+m{U}_b\times ({\mathrm{\Omega }}_b\times r_{c_b})\\ & =& m(r_{c_b}\times U_b^{\prime }+{\mathrm{\Omega }}_b\times (r_{c_b}\times U_b))+I_b{\mathrm{\Omega }}_b^{\prime }+{\mathrm{\Omega }}_b\times (I_b{\mathrm{\Omega }}_b)+m{U}_b\times ({\mathrm{\Omega }}_b\times r_{c_b})\\ & =& m{r}_{c_b}\times U_b^{\prime }+I_b{\mathrm{\Omega }}_b^{\prime }+{\mathrm{\Omega }}_b\times (I_b{\mathrm{\Omega }}_b)+m{r}_{c_b}\times ({\mathrm{\Omega }}_b\times U_b)\end{array}\end{array}$$

整理后得到矩阵的形式：

$$\begin{array}{}\text{(10)}& \left[\begin{array}{c}{F}_{ex{t}_b}\\ M_{ex{t}_b}\end{array}\right]=\left[\begin{array}{cc}m{I}_{3\times 3}& -m\overrightarrow{r_{c_b}}\\ m\overrightarrow{r_{c_b}}& I_b\end{array}\right]\left[\begin{array}{c}{U}_b^{\prime }\\ {\mathrm{\Omega }}_b^{\prime }\end{array}\right]+\left[\begin{array}{c}m{\mathrm{\Omega }}_b\times U_b+m{\mathrm{\Omega }}_b\times ({\mathrm{\Omega }}_b\times r_{c_b})\\ m{r}_{c_b}\times ({\mathrm{\Omega }}_b\times U_b)+{\mathrm{\Omega }}_b\times (I_b{\mathrm{\Omega }}_b)\end{array}\right]\end{array}$$

简写成如下形式：

$$\begin{array}{}\text{(11)}& F_{ex{t}_j}=M_j{V}_j^{\prime }+{\beta }_j\end{array}$$

其中：

• $j$ 表示第j 个连杆；
• $V_j=\left[\begin{array}{c}{U}_j\\ {\mathrm{\Omega }}_j\end{array}\right]$ 表示连杆j 相对于惯性系的速度和角速度组成的六维向量，在本体系下表示。
• ${}^j{H}_{j+1}$ 表示一个六维变换矩阵，可以将j+1 系下的力变化到j 系下
• $F_{ex{t}_j}$ 表示连杆收到的外力，外力矩在j 系下的表示。

建模示例--平面2R 机械臂

如果不想看上面公式，可以简单捋一下，下面的步骤。

机构与参数定义

• 关节变量：$\overrightarrow{q}=\left[\begin{array}{c}{q}_1\\ q_2\end{array}\right]$，对应的角速度${\overrightarrow{q}}^{\prime }$，角加速度${\overrightarrow{q}}^{″}$
• 连杆参数：
  • 长度： $l_1,l_2$
  • 质心到前一关节的距离： $r_1,r_2$
  • 质量： $m_1,m_2$
  • 转动惯量：$I_1,I_2$
• 重力： $\overrightarrow{g}=\left[\begin{array}{c}0\\ -g\\ 0\end{array}\right]$
• 旋转矩阵：$R_i=R_z(q_i)=\left[\begin{array}{ccc}cos{q}_i& -sin{q}_i& 0\\ sin{q}_i& cos{q}_i& 0\\ 0& 0& 1\end{array}\right]$，表示坐标系i 相对于坐标系i−1 的方向
• 矩阵$R_i^T=R_i^{-1}$，表示把一个在i−1 系下表达的向量，转换为在 i 系下的表达

正向递推（速度与加速度）

因为底座（惯性系）固定在地面，有：

$$\begin{array}{}\text{(12)}& \begin{array}{lllll}& & {\overrightarrow{\omega }}_0& =& \left[\begin{array}{c}0\\ 0\\ 0\end{array}\right]\\ & & {\overrightarrow{\alpha }}_0& =& \left[\begin{array}{c}0\\ 0\\ 0\end{array}\right]\\ {\overrightarrow{a}}_0& =& -\overrightarrow{g}& =& \left[\begin{array}{c}0\\ g\\ 0\end{array}\right]\end{array}\end{array}$$

因为每个质点都会受到向下的重力加速度$\overrightarrow{g}$，如果另惯性系有一个向上的虚拟加速度$-\overrightarrow{g}$，那么在后悔递推中各连杆的质心加速度哦都会自动包含重力效应。

对于连杆i 的角速度和角加速度有：

$$\begin{array}{}\text{(13)}& \begin{array}{lll}{\overrightarrow{\omega }}_i& =& R_i^T{\overrightarrow{\omega }}_{i-1}+q_i^{\prime }\overrightarrow{z}\\ {\overrightarrow{\alpha }}_i& =& R_i^T{\overrightarrow{\alpha }}_{i-1}+q_i^{″}\overrightarrow{z}\end{array}\end{array}$$

其中$\overrightarrow{z}=[0,0,1{]}^T$ 表示$z$ 轴的单位向量。$q_i^{\prime }\overrightarrow{z}$可以把标量转化为角速度（角加速度）的向量。

关节原点的加速度：

$$\begin{array}{}\text{(14)}& {\overrightarrow{a}}_i=R_i^T(\underset{\mathrm{平}\mathrm{移}\mathrm{继}\mathrm{承}\mathrm{项}(\mathrm{前}\mathrm{杆}\mathrm{带}\mathrm{动})}{\underbrace{{\overrightarrow{a}}_{i-1}}}+\underset{\mathrm{切}\mathrm{向}\mathrm{加}\mathrm{速}\mathrm{度}（\mathrm{前}\mathrm{杆}\mathrm{旋}\mathrm{转}\mathrm{加}\mathrm{速}）}{\underbrace{{\overrightarrow{\alpha }}_{i-1}\times {\overrightarrow{p}}_{i-1,i}}}+\underset{\mathrm{向}\mathrm{心}\mathrm{加}\mathrm{速}\mathrm{度}（\mathrm{前}\mathrm{杆}\mathrm{匀}\mathrm{速}\mathrm{旋}\mathrm{转}）}{\underbrace{{\overrightarrow{\omega }}_{i-1}\times ({\overrightarrow{\omega }}_{i-1}\times {\overrightarrow{p}}_{i-1,i})}})\end{array}$$

其中：

$$\begin{gathered} \begin{array}{}\text{(15)}& {\overrightarrow{P}}_{0,1}=[0,0,0{]}^T \\ {\overrightarrow{P}}_{2,1}=[l_1,0,0{]}^T\end{array} \end{gathered}$$

很自然可以联想到这是关节到上一关节的拓扑参数。

这里可以看到一个关键点：$R_i^T$

连杆的质心加速度：

$$\begin{array}{}\text{(16)}& {\overrightarrow{a_c}}_i=\underset{\mathrm{关}\mathrm{节}\mathrm{点}\mathrm{加}\mathrm{速}\mathrm{度}}{\underbrace{{\overrightarrow{a}}_i}}+\underset{\mathrm{切}\mathrm{向}\mathrm{加}\mathrm{速}\mathrm{度}}{\underbrace{{\overrightarrow{\alpha }}_i\times {\overrightarrow{r}}_i}}+\underset{\mathrm{向}\mathrm{心}\mathrm{加}\mathrm{速}\mathrm{度}}{\underbrace{{\overrightarrow{\omega }}_i\times ({\overrightarrow{\omega }}_i\times {\overrightarrow{r}}_i)}}\end{array}$$

其中${\overrightarrow{r}}_i=[r_i,0,0{]}^T$，表示质心相对于本体坐标系的位置。

反向递推（力与力矩）

假设末端无外力，则从末端到基座的力与力矩为：

$$\begin{gathered} \begin{array}{}\text{(17)}& {\overrightarrow{f}}_3=[0,0,0{]}^T \\ {\overrightarrow{n}}_3=[0,0,0{]}^T\end{array} \end{gathered}$$

本关节惯性力与惯性矩：

$$\begin{gathered} \begin{array}{}\text{(18)}& {\overrightarrow{F}}_i=m_i{\overrightarrow{a}}_i \\ {\overrightarrow{N}}_i=I_i{\overrightarrow{\alpha }}_i+\overrightarrow{\omega }\times (I_i{\overrightarrow{\omega }}_i)\end{array} \end{gathered}$$

递推到关节：

$$\begin{gathered} \begin{array}{}\text{(19)}& {\overrightarrow{f}}_i=R_{i+1}{\overrightarrow{f}}_{i+1}+{\overrightarrow{F}}_i \\ {\overrightarrow{n}}_i=\underset{\text{外力矩}}{\underbrace{{\overrightarrow{N}}_i}}+\underset{\text{后杆传来的力矩}}{\underbrace{R_{i+1}{\overrightarrow{n}}_{i+1}}}+\underset{\text{惯性力力矩}}{\underbrace{{\overrightarrow{r}}_i\times {\overrightarrow{F}}_i}}+\underset{\text{后杆传力产生的力矩}}{\underbrace{{\overrightarrow{p}}_{i,i+1}\times (R_{i+1}{\overrightarrow{f}}_{i+1})}}\end{array} \end{gathered}$$

关节驱动力矩

对于转动关节：

$$\begin{array}{}\text{(20)}& {\tau }_i={\overrightarrow{n}}_i\cdot \overrightarrow{z}\end{array}$$

整理后可以得到标准形式：

$$\mathit{\tau }=\underset{\text{惯性项}}{\underbrace{\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}}}}+\underset{\text{科氏力和离心力项}}{\underbrace{\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}}}+\underset{\text{重力项}}{\underbrace{\mathbf{G}(\mathbf{q})}}$$

显示结果

为简化表示令$c_i=cos{q}_i,s_i=sin{q}_i$:

惯性矩阵

$$\begin{array}{lllll}& & M_{11}& =& I_1+I_2+m_1{r}_1^2+m_2(l_1^2+r_2^2+2l_1{r}_2{c}_2)\\ M_{12}& =& M_{21}& =& I_2+m_2(r_2^2+l_1{r}_2{c}_2)\\ & & M_{22}& =& I_2+m_2{r}_2^2\end{array}$$

科氏/离心项

$$\begin{array}{lll}{C}_1& =& -m_2{l}_1{r}_2{s}_2(2q_1^{\prime }{q}_2^{\prime }+q_2^{\prime 2})\\ C_2& =& m_2{l}_1{r}_2{s}_2{q}_1^{\prime 2}\end{array}$$

重力项

$$\begin{array}{lll}{G}_1& =& (m_1{r}_1+m_2{l}_1)gcos(q_1)+m_2{r}_{2}gcos(q_1+q_2)\\ G_2& =& m_2{r}_{2}gcos(q_1+q_2)\end{array}$$

注意的点

上面步骤只适用于旋转关节，如果想添加平动环节（旋转矩阵是单位矩阵），还需要作出如下改动：

加速度递推

$${\mathit{\omega }}_i=\underset{\text{继承的角速度}}{\underbrace{R_i^T{\mathit{\omega }}_{i-1}}}+\underset{\text{本关节贡献}}{\underbrace{\{\begin{array}{ll}{\dot{q}}_i\mathbf{z},& \text{R 关节}\\ 0,& \text{P 关节}\end{array}}}$$

角加速度递推

$${\mathit{\alpha }}_i=\underset{\text{继承的角加速度}}{\underbrace{R_i^T{\mathit{\alpha }}_{i-1}}}+\underset{\text{本关节贡献}}{\underbrace{\{\begin{array}{ll}{\ddot{q}}_i\mathbf{z},& \text{R 关节}\\ 0,& \text{P 关节}\end{array}}}$$

线速度递推

$${\mathbf{a}}_i=R_i^T(\underset{\text{平移继承}}{\underbrace{{\mathbf{a}}_{i-1}}}+\underset{\text{切向加速度}}{\underbrace{{\mathit{\alpha }}_{i-1}\times {\mathbf{p}}_{i-1,i}}}+\underset{\text{向心加速度}}{\underbrace{{\mathit{\omega }}_{i-1}\times ({\mathit{\omega }}_{i-1}\times {\mathbf{p}}_{i-1,i})}})+\underset{\text{本关节贡献}}{\underbrace{\{\begin{array}{ll}0,& \text{R 关节}\\ {\ddot{q}}_i\mathbf{z}+2{\mathit{\omega }}_i\times ({\dot{q}}_i\mathbf{z}),& \text{P 关节}\end{array}}}$$

总结

根据上面的建模步骤，可以看到，即使公式推导过程难以理解，但是实际建模却非常有规律，如果用编程实现的话可以看到函数是可以递归调用的。
运动学和动力学可以通过以下方式进行有机的结合：

期望末端轨迹 x_d(t)
        ↓  逆运动学 / 雅可比
期望关节轨迹 q_d(t), ẋ_d(t), q̈_d(t)
        ↓  反馈控制律
误差 e = q_d - q
        ↓
期望关节加速度 q̈_cmd
        ↓  逆动力学（前馈）
期望力矩 τ_cmd
        ↓
电机驱动器 → 实际机器人 → 传感器反馈

感谢ChatGPT/Gemini/Calude!

算是一点疑惑或者抱怨

本人虽学习过一段经典控制理论，但是还没毕业基本都还给老师了。作为一个程序员，是否要耗费大量精力去了解和学习理论推导过程？

• 学：会陷入无尽的公式推导，数学基础薄弱；
• 不学：没有一个可靠的输入，去网上也很难找到一个容易扩展的工具或案例
  是我的方向错了吧

AI 回复说：

如果你真把牛顿–欧拉、拉格朗日、完整控制理论全补完，你得到的能力，90% 在真实项目里用不上。 而你现在最缺的，其实是：
一个可跑通的控制框架
一套自证正确的测试方法
一种不会把你拖进数学深渊的学习节奏

参考资料

1. 机器人动力学建模之牛顿欧拉法推导
2. 关于地转偏向力（科氏力）。在旋转的坐标系中，物体脱手后相对于惯性系是匀速直线运动，但是相对于旋转系则是曲线运动，像是被某个力改变了运动轨迹一般。