【机器人学】牛顿-欧拉动力学方程迭代形式

1 牛顿方程和欧拉方程

作者推导了牛顿欧拉动力学方程的迭代形式和矩阵，本文重点描述动力学方程的迭代形式，动力学方程的矩阵形式见博文。
假定机械臂的连杆均为刚体，若连杆质心的位置和惯性张量已知，那么它的质量分布特定将完全确定。要使连杆运动，必须对连杆进行加减速。连杆运动所需的力是关于连杆期望加速度及其质量分布的函数。牛顿方程以及描述旋转运动的欧拉方程描述了力、惯量和加速度之间的关系。

1.1 牛顿方程

假定刚体质心正以加速度 v c ˙ \dot{v_c} vc˙做加速运动。由牛顿方程可得作用在质心上的力 F F F引起刚体的加速度为（式中 m m m为刚体的总质量）： F = m v c ˙ \boldsymbol{F}=m\dot\boldsymbol{v_c} F=mvc˙

1.2 欧拉方程

假定以旋转刚体角速度和角加速度分别为 ω \boldsymbol{ω} ω、 ω ˙ \dot\boldsymbol{ω} ω˙，由欧拉方程可得作用在刚体上的力矩 N N N引起刚体的转动为（式中 I c I_c Ic是刚体在坐标系 c {c} c中的惯性张量。刚体的质心在坐标系 c {c} c的原点上）： N = I c ω ˙ + ω × I c ω \boldsymbol{N}=\boldsymbol{I_c}\dot\boldsymbol{ω}+\boldsymbol{ω}×\boldsymbol{I_c}\boldsymbol{ω} N=Icω˙+ω×Icω

2 牛顿-欧拉动力学方程

假设已知关节的位置、速度和加速度，结合机器人运动学和质量分布，可以计算出驱动关节运动所需的力矩。

2.1 向外迭代计算速度和加速度

为了计算作用在连杆上的惯性力，需要计算操作臂每个连杆在某一时刻的角速度、线加速度以及角加速度。可采用迭代法，首先计算连杆1，接着计算下一个连杆，已知向外迭代到连杆n。对于第 i + 1 i+1 i+1个关节的运动，其速度及加速度如下（具体推导过程见《串联机械臂连杆的速度及加速度推导》）
ω i + 1 i + 1 = { R i i + 1 ω i i + θ ˙ i + 1 z i + 1 i + 1 , if J i + 1 is revolute R i i + 1 ω i i , if J i + 1 is prismatic v i + 1 i + 1 = { R i i + 1 ( v i i + ω i i × r i + 1 i ) , if J i + 1 is revolute R i i + 1 ( v i i + ω i i × r i + 1 i ) + d ˙ i + 1 z i + 1 i + 1 , if J i + 1 is prismatic ω ˙ i + 1 i + 1 = { R i i + 1 ω ˙ i i + θ ¨ i + 1 z i + 1 i + 1 + R i i + 1 ω i i × θ ˙ i + 1 z i + 1 i + 1 , if J i + 1 is revolute R i i + 1 ω ˙ i i , if J i + 1 is prismatic v ˙ i + 1 i + 1 = { R i i + 1 ( v ˙ i i + ω ˙ i i × r i + 1 i + ω i i × ( ω i i × r i + 1 i ) ) , if J i + 1 is revolute R i i + 1 ( v ˙ i i + ω ˙ i i × r i + 1 i + ω i i × ( ω i i × r i + 1 i ) ) + d ¨ i + 1 z i + 1 i + 1 + 2 ω i + 1 i + 1 × d ˙ i + 1 z i + 1 i + 1 , if J i + 1 is prismatic \begin{aligned} \boldsymbol{ω}_{i+1}^{i+1} &= \begin{cases} \boldsymbol{R}_i^{i+1}\boldsymbol{ω}_{i}^{i}+\dot{θ}_{i+1}\boldsymbol{z}_{i+1}^{i+1}, & \text{if $J_{i+1}$ is revolute} \\ \boldsymbol{R}_i^{i+1}\boldsymbol{ω}_{i}^{i}, & \text{if $J_{i+1}$ is prismatic} \end{cases}\\ \boldsymbol{v}_{i+1}^{i+1} &= \begin{cases} \boldsymbol{R}_i^{i+1}(\boldsymbol{v}_i^i+\boldsymbol{ω}_{i}^i\times\boldsymbol{r}_{i+1}^i), & \text{if $J_{i+1}$ is revolute} \\ \boldsymbol{R}_i^{i+1}(\boldsymbol{v}_i^i+\boldsymbol{ω}_{i}^i\times\boldsymbol{r}_{i+1}^i)+\dot{d}_{i+1}\boldsymbol{z}_{i+1}^{i+1}, & \text{if $J_{i+1}$ is prismatic} \end{cases}\\ \dot\boldsymbol{ω}_{i+1}^{i+1} &= \begin{cases} \boldsymbol{R}_i^{i+1}\dot\boldsymbol{ω}_{i}^{i}+\ddot{θ}_{i+1}\boldsymbol{z}_{i+1}^{i+1}+\boldsymbol{R}_i^{i+1}\boldsymbol{ω}_{i}^i\times\dot{θ}_{i+1}\boldsymbol{z}_{i+1}^{i+1}, & \text{if $J_{i+1}$ is revolute} \\ \boldsymbol{R}_i^{i+1}\dot\boldsymbol{ω}_{i}^{i}, & \text{if $J_{i+1}$ is prismatic} \end{cases}\\ \dot\boldsymbol{v}_{i+1}^{i+1} &= \begin{cases} \boldsymbol{R}_i^{i+1}(\dot\boldsymbol{v}_{i}^{i}+\dot\boldsymbol{ω}_{i}^{i}\times\boldsymbol{r}_{i+1}^{i}+\boldsymbol{ω}_{i}^{i}\times(\boldsymbol{ω}_{i}^{i}\times\boldsymbol{r}_{i+1}^{i})), & \text{if $J_{i+1}$ is revolute} \\ \boldsymbol{R}_i^{i+1}(\dot\boldsymbol{v}_{i}^{i}+\dot\boldsymbol{ω}_{i}^{i}\times\boldsymbol{r}_{i+1}^{i}+\boldsymbol{ω}_{i}^{i}\times(\boldsymbol{ω}_{i}^{i}\times\boldsymbol{r}_{i+1}^{i}))+\ddot{d}_{i+1}\boldsymbol{z}_{i+1}^{i+1}+2\boldsymbol{ω}_{i+1}^{i+1}\times\dot{d}_{i+1}\boldsymbol{z}_{i+1}^{i+1}, & \text{if $J_{i+1}$ is prismatic} \end{cases} \end{aligned} ωi+1i+1vi+1i+1ω˙i+1i+1v˙i+1i+1={Rii+1ωii+θ˙i+1zi+1i+1,Rii+1ωii,if Ji+1 is revoluteif Ji+1 is prismatic={Rii+1(vii+ωii×ri+1i),Rii+1(vii+ωii×ri+1i)+d˙i+1zi+1i+1,if Ji+1 is revoluteif Ji+1 is prismatic={Rii+1ω˙ii+θ¨i+1zi+1i+1+Rii+1ωii×θ˙i+1zi+1i+1,Rii+1ω˙ii,if Ji+1 is revoluteif Ji+1 is prismatic={Rii+1(v˙ii+ω˙ii×ri+1i+ωii×(ωii×ri+1i)),Rii+1(v˙ii+ω˙ii×ri+1i+ωii×(ωii×ri+1i))+d¨i+1zi+1i+1+2ωi+1i+1×d˙i+1zi+1i+1,if Ji+1 is revoluteif Ji+1 is prismatic

假定坐标系 C i C_{i} Ci固连在连杆 i i i上，坐标系 C i C_{i} Ci原点位于连杆质心，且各坐标轴方位与原连杆坐标系 i i i方位相同。由于从连杆坐标系到连杆质心坐标系的线加速度转换与对应关节的运动形式无关，因此无论是旋转关节还是移动关节，连杆 i i i质心对应的线加速度均可表示为： v c ˙ i i = v ˙ i i + ω ˙ i i × r c i i + ω i i × ( ω i i × r c i i ) \dot\boldsymbol{v_c}_{i}^{i}=\dot\boldsymbol{v}_{i}^{i}+\dot\boldsymbol{ω}_{i}^{i}\times\boldsymbol{r_c}_{i}^{i}+\boldsymbol{ω}_{i}^{i}\times(\boldsymbol{ω}_{i}^{i}\times\boldsymbol{r_c}_{i}^{i}) vc˙ii=v˙ii+ω˙ii×rcii+ωii×(ωii×rcii)

2.2 向内迭代计算力和力矩

计算出每个连杆质心的线加速度和角加速度后，运动牛顿方程和欧拉方程就可以计算出作用在连杆质心上的惯性力和力矩，其中坐标系 C i C_i Ci的原点位于连杆质心，各坐标轴方位与原连杆坐标系 i i i方位相同。即： F i = m v c ˙ i \boldsymbol{F}_i=m\dot\boldsymbol{v_c}_{i} Fi=mvc˙i N i = I c i ω ˙ i + ω i × I c i ω i \boldsymbol{N}_i=\boldsymbol{I_c}_{i}\dot\boldsymbol{ω}_i+\boldsymbol{ω}_i×\boldsymbol{I_c}_{i}\boldsymbol{ω}_i Ni=Iciω˙i+ωi×Iciωi计算出每个连杆上的力和力矩后，需要计算关节力矩，它们是实际施加在连杆上的力和力矩。
每个连杆都受到相邻连杆的作用力和力矩以及附加的惯性力和力矩，如图典型连杆在无重力状态下的受力图所示，图中 f i \boldsymbol{f}_i fi表示连杆 i − 1 {i-1} i−1作用在连杆 i i i上的力， n i \boldsymbol{n}_i ni表示连杆 i − 1 {i-1} i−1作用在连杆 i i i上的力矩。

将所有作用在连杆上的力相加，可得到力的平衡方程： F i i = f i i − R i + 1 i f i + 1 i + 1 (2-1) \boldsymbol{F}_i^i=\boldsymbol{f}_{i}^{i}-\boldsymbol{R}_{i+1}^i\boldsymbol{f}_{i+1}^{i+1}\tag{2-1} Fii=fii−Ri+1ifi+1i+1(2-1)将所有作用在质心上的力矩相加，并且令它们的和为零，得到力矩平衡方程为： N i i = n i i − n i + 1 i + ( − p c i i ) × f i i − ( p i + 1 i − p c i i ) × f i + 1 i (2-2) \boldsymbol{N}_{i}^i=\boldsymbol{n}_i^i-\boldsymbol{n}_{i+1}^i+(-\boldsymbol{p_c}_{i}^i)×\boldsymbol{f}_i^i-(\boldsymbol{p}_{i+1}^i-\boldsymbol{p_c}_{i}^i)×\boldsymbol{f}_{i+1}^i\tag{2-2} Nii=nii−ni+1i+(−pcii)×fii−(pi+1i−pcii)×fi+1i(2-2)，利用力平衡方程(2-1)以及附加旋转矩阵的方法，式(2-2)可以改写成： N i i = n i i − R i + 1 i n i + 1 i + 1 − p c i i × F i i − p i + 1 i × R i + 1 i f i + 1 i + 1 (2-3) \boldsymbol{N}_{i}^i=\boldsymbol{n}_i^i-\boldsymbol{R}_{i+1}^i\boldsymbol{n}_{i+1}^{i+1}-\boldsymbol{p_c}_{i}^i×\boldsymbol{F}_i^i-\boldsymbol{p}_{i+1}^i×\boldsymbol{R}_{i+1}^i\boldsymbol{f}_{i+1}^{i+1}\tag{2-3} Nii=nii−Ri+1ini+1i+1−pcii×Fii−pi+1i×Ri+1ifi+1i+1(2-3)重新排列力和力矩方程，形成相邻连杆从高序号向低序号排列的迭代关系： f i i = R i + 1 i f i + 1 i + 1 + F i i (2-4) \boldsymbol{f}_i^i=\boldsymbol{R}_{i+1}^i\boldsymbol{f}_{i+1}^{i+1}+\boldsymbol{F}_{i}^{i}\tag{2-4} fii=Ri+1ifi+1i+1+Fii(2-4) n i i = N i i + R i + 1 i n i + 1 i + 1 + p c i i × F i i + p i + 1 i × R i + 1 i f i + 1 i + 1 (2-5) \boldsymbol{n}_{i}^i=\boldsymbol{N}_i^i+\boldsymbol{R}_{i+1}^i\boldsymbol{n}_{i+1}^{i+1}+\boldsymbol{p}_{c_i}^i×\boldsymbol{F}_i^i+\boldsymbol{p}_{i+1}^i×\boldsymbol{R}_{i+1}^i\boldsymbol{f}_{i+1}^{i+1}\tag{2-5} nii=Nii+Ri+1ini+1i+1+pcii×Fii+pi+1i×Ri+1ifi+1i+1(2-5)关节 i i i上广义力可通过将力 f i i \boldsymbol{f}_i^i fii投影到移动关节得到，或者是将力矩 n i i \boldsymbol{n}_i^i nii投影到旋转关节得到。因此关节 i i i上的中广义力表达如下：
τ i = { z i i T f i i p r i s m a t i c z i i T n i i r o t a t i o n (2-6) \boldsymbol{τ}_i=\left\{ \begin{array}{rcl} {\boldsymbol{z}_i^i}^T\boldsymbol{f}_i^i & & {prismatic }\\ {\boldsymbol{z}_i^i}^T\boldsymbol{n}_i^i & & {rotation} \end{array} \right.\tag{2-6} τi={ziiTfiiziiTniiprismaticrotation(2-6)对一个在自由空间中运动的机器人来说， f N + 1 N + 1 \boldsymbol{f}_{N+1}^{N+1} fN+1N+1和 n N + 1 N + 1 \boldsymbol{n}_{N+1}^{N+1} nN+1N+1等于0。如果机器人与环境接触， f N + 1 N + 1 \boldsymbol{f}_{N+1}^{N+1} fN+1N+1和 n N + 1 N + 1 \boldsymbol{n}_{N+1}^{N+1} nN+1N+1不等于0，力平衡方程就包含了这些接触力和力矩。

2.3 牛顿-欧拉迭代动力学算法

由关节运动计算关节力矩的完整算法由两部分组成。第一部分是对每个连杆应用牛顿-欧拉方程，从连杆1到连杆 n n n向外迭代计算连杆的速度和加速度。第二部分是从连杆 n n n到连杆1向内迭代计算连杆间的相互作用力和力矩以及关节驱动力矩。

对于转动关节而言，递推算法归纳如下：

向外递推： i : 0 → n − 1 i:0→n-1 i:0→n−1
ω i + 1 i + 1 = R i i + 1 ω i i + θ ˙ i + 1 z i + 1 i + 1 ω ˙ i + 1 i + 1 = R i i + 1 ω ˙ i i + θ ¨ i + 1 z i + 1 i + 1 + R i i + 1 ω i i × θ ˙ i + 1 z i + 1 i + 1 v ˙ i + 1 i + 1 = R i i + 1 ( v ˙ i i + ω ˙ i i × p i + 1 i + ω i i × ( ω i i × p i + 1 i ) ) v c ˙ i + 1 i + 1 = v ˙ i + 1 i + 1 + ω ˙ i + 1 i + 1 × p c i + 1 i + 1 + ω i + 1 i + 1 × ( ω i + 1 i + 1 × p c i + 1 i + 1 ) F i + 1 i + 1 = m i + 1 v c ˙ i + 1 i + 1 N i + 1 i + 1 = I c i + 1 i + 1 ω ˙ i + 1 i + 1 + ω i + 1 i + 1 × I c i + 1 i + 1 ω i + 1 i + 1 \begin{aligned} \boldsymbol{ω}_{i+1}^{i+1} &=\boldsymbol{R}_i^{i+1}\boldsymbol{ω}_{i}^{i}+\dot{θ}_{i+1}\boldsymbol{z}_{i+1}^{i+1}\\ \dot\boldsymbol{ω}_{i+1}^{i+1} &=\boldsymbol{R}_i^{i+1}\dot\boldsymbol{ω}_{i}^{i}+\ddot{θ}_{i+1}\boldsymbol{z}_{i+1}^{i+1}+\boldsymbol{R}_i^{i+1}\boldsymbol{ω}_{i}^i\times\dot{θ}_{i+1}\boldsymbol{z}_{i+1}^{i+1}\\ \dot\boldsymbol{v}_{i+1}^{i+1} &=\boldsymbol{R}_i^{i+1}(\dot\boldsymbol{v}_{i}^{i}+\dot\boldsymbol{ω}_{i}^{i}\times\boldsymbol{p}_{i+1}^{i}+\boldsymbol{ω}_{i}^{i}\times(\boldsymbol{ω}_{i}^{i}\times\boldsymbol{p}_{i+1}^{i}))\\ \dot\boldsymbol{v_c}_{i+1}^{i+1}&=\dot\boldsymbol{v}_{i+1}^{i+1}+\dot\boldsymbol{ω}_{i+1}^{i+1}\times\boldsymbol{p_c}_{i+1}^{i+1}+\boldsymbol{ω}_{i+1}^{i+1}\times(\boldsymbol{ω}_{i+1}^{i+1}\times\boldsymbol{p_c}_{i+1}^{i+1})\\ \boldsymbol{F}_{i+1}^{i+1}&=m_{i+1}\dot\boldsymbol{v_c}_{i+1}^{i+1}\\ \boldsymbol{N}_{i+1}^{i+1}&=\boldsymbol{I_c}_{i+1}^{i+1}\dot\boldsymbol{ω}_{i+1}^{i+1}+\boldsymbol{ω}_{i+1}^{i+1}×\boldsymbol{I_c}_{i+1}^{i+1}\boldsymbol{ω}_{i+1}^{i+1} \end{aligned} ωi+1i+1ω˙i+1i+1v˙i+1i+1vc˙i+1i+1Fi+1i+1Ni+1i+1=Rii+1ωii+θ˙i+1zi+1i+1=Rii+1ω˙ii+θ¨i+1zi+1i+1+Rii+1ωii×θ˙i+1zi+1i+1=Rii+1(v˙ii+ω˙ii×pi+1i+ωii×(ωii×pi+1i))=v˙i+1i+1+ω˙i+1i+1×pci+1i+1+ωi+1i+1×(ωi+1i+1×pci+1i+1)=mi+1vc˙i+1i+1=Ici+1i+1ω˙i+1i+1+ωi+1i+1×Ici+1i+1ωi+1i+1向内递推： i : n → 1 i:n→1 i:n→1
f i i = R i + 1 i f i + 1 i + 1 + F i i n i i = N i i + R i + 1 i n i + 1 i + 1 + p c i i × F i i + p i + 1 i × R i + 1 i f i + 1 i + 1 τ i = z i i T n i i \begin{aligned} \boldsymbol{f}_i^i&=\boldsymbol{R}_{i+1}^i\boldsymbol{f}_{i+1}^{i+1}+\boldsymbol{F}_{i}^{i}\\ \boldsymbol{n}_{i}^i&=\boldsymbol{N}_i^i+\boldsymbol{R}_{i+1}^i\boldsymbol{n}_{i+1}^{i+1}+\boldsymbol{p}_{c_i}^i×\boldsymbol{F}_i^i+\boldsymbol{p}_{i+1}^i×\boldsymbol{R}_{i+1}^i\boldsymbol{f}_{i+1}^{i+1}\\ \boldsymbol{τ}_i&={\boldsymbol{z}_i^i}^T\boldsymbol{n}_i^i \end{aligned} fiiniiτi=Ri+1ifi+1i+1+Fii=Nii+Ri+1ini+1i+1+pcii×Fii+pi+1i×Ri+1ifi+1i+1=ziiTnii

对于移动关节而言，递推算法归纳如下：

向外递推： i : 0 → n − 1 i:0→n-1 i:0→n−1
ω i + 1 i + 1 = R i i + 1 ω i i ω ˙ i + 1 i + 1 = R i i + 1 ω ˙ i i v ˙ i + 1 i + 1 = R i i + 1 ( v ˙ i i + ω ˙ i i × r i + 1 i + ω i i × ( ω i i × r i + 1 i ) ) + d ¨ i + 1 z i + 1 i + 1 + 2 ω i + 1 i + 1 × d ˙ i + 1 z i + 1 i + 1 v c ˙ i + 1 i + 1 = v ˙ i + 1 i + 1 + ω ˙ i + 1 i + 1 × p c i + 1 i + 1 + ω i + 1 i + 1 × ( ω i + 1 i + 1 × p c i + 1 i + 1 ) F i + 1 i + 1 = m i + 1 v c ˙ i + 1 i + 1 \begin{aligned} \boldsymbol{ω}_{i+1}^{i+1} &=\boldsymbol{R}_i^{i+1}\boldsymbol{ω}_{i}^{i}\\ \dot\boldsymbol{ω}_{i+1}^{i+1} &=\boldsymbol{R}_i^{i+1}\dot\boldsymbol{ω}_{i}^{i}\\ \dot\boldsymbol{v}_{i+1}^{i+1} &=\boldsymbol{R}_i^{i+1}(\dot\boldsymbol{v}_{i}^{i}+\dot\boldsymbol{ω}_{i}^{i}\times\boldsymbol{r}_{i+1}^{i}+\boldsymbol{ω}_{i}^{i}\times(\boldsymbol{ω}_{i}^{i}\times\boldsymbol{r}_{i+1}^{i}))+\ddot{d}_{i+1}\boldsymbol{z}_{i+1}^{i+1}+2\boldsymbol{ω}_{i+1}^{i+1}\times\dot{d}_{i+1}\boldsymbol{z}_{i+1}^{i+1}\\ \dot\boldsymbol{v_c}_{i+1}^{i+1}&=\dot\boldsymbol{v}_{i+1}^{i+1}+\dot\boldsymbol{ω}_{i+1}^{i+1}\times\boldsymbol{p_c}_{i+1}^{i+1}+\boldsymbol{ω}_{i+1}^{i+1}\times(\boldsymbol{ω}_{i+1}^{i+1}\times\boldsymbol{p_c}_{i+1}^{i+1})\\ \boldsymbol{F}_{i+1}^{i+1}&=m_{i+1}\dot\boldsymbol{v_c}_{i+1}^{i+1}\\ \end{aligned} ωi+1i+1ω˙i+1i+1v˙i+1i+1vc˙i+1i+1Fi+1i+1=Rii+1ωii=Rii+1ω˙ii=Rii+1(v˙ii+ω˙ii×ri+1i+ωii×(ωii×ri+1i))+d¨i+1zi+1i+1+2ωi+1i+1×d˙i+1zi+1i+1=v˙i+1i+1+ω˙i+1i+1×pci+1i+1+ωi+1i+1×(ωi+1i+1×pci+1i+1)=mi+1vc˙i+1i+1向内递推： i : n → 1 i:n→1 i:n→1
f i i = R i + 1 i f i + 1 i + 1 + F i i τ i = z i i T f i i \begin{aligned} \boldsymbol{f}_i^i&=\boldsymbol{R}_{i+1}^i\boldsymbol{f}_{i+1}^{i+1}+\boldsymbol{F}_{i}^{i}\\ \boldsymbol{τ}_i&={\boldsymbol{z}_i^i}^T\boldsymbol{f}_i^i \end{aligned} fiiτi=Ri+1ifi+1i+1+Fii=ziiTfii

2.4 考虑重力的动力学算法

上述的推导过程基于无重力环境。如果要考虑重力场的影响，令 v ˙ 0 0 = G \dot\boldsymbol{v}^0_0=\boldsymbol{G} v˙00=G就可以很简单地将作用在连杆上的重力因素包含到动力学方程中，其中 G \boldsymbol{G} G与重力矢量 g \boldsymbol{g} g大小相等，而方向相反。这等效于机器人正以1 g \boldsymbol{g} g的加速度在做向上加速运动。这个假想的向上的加速度与重力作用在连杆上的效果是相同的。因而不需要其他额外的计算就可以对重力影响进行计算。

2.5 牛顿欧拉动力学方程的迭代形式的代码实现

NewtonEulerDynamics_Iterate