机械臂动力学——动力学建模

一、动力学基础概念

基本动力学模型
τ＝D(q)q¨+C(q,q˙)+G(q)\tau　＝　D(q)\ddot{q}+C(q,\dot{q})+G(q) τ　＝　D(q)q¨+C(q,q˙)+G(q)

τ\tauτ为关节力矩，D(q)D(q)D(q)为机械臂的n×nn \times nn×n质量矩阵，C(q,q˙)C(q,\dot{q})C(q,q˙)为n×1n \times 1n×1离心力和科氏力矢量，G(q)G(q)G(q)为n×1n \times 1n×1的重力矢量

建模方法

牛顿-欧拉法
拉格朗日法

连杆质量，连杆质心位置矢量，连杆质心惯性矩阵（通过动力学参数识别获得）

二、牛顿-欧拉法

运动外推：向外迭代计算连杆的角速度、角加速度和线加速度
力外推：计算作用在连杆质心上的惯性力和力矩
力矩内推：向内迭代计算关节力矩

2.1 运动向外迭代

2.1.1 刚体线速度和角速度

线速度

坐标系{A}为固定，坐标系{B}固连在刚体上。

AVQ^{A}V_QAVQ为Q点在坐标系{A}的速度；BVQ^{B}V_QBVQ为Q点在坐标系{B}的速度；BAR^{A}_{B}RBAR为坐标系{B}相对于坐标系{A}的变换矩阵；AVBORG^{A}V_{BORG}AVBORG为位置矢量的导数。
AVQ=AVBORG+BARBVQ^{A}V_Q = ^{A}V_{BORG} + ^{A}_{B}R\ ^{B}V_Q AVQ=AVBORG+BAR BVQ

该方程适用于两个坐标系相对方位保持不变。

角速度

坐标系原点重合，线速度为零。

BQ^{B}QBQ为矢量，确定了坐标系{B}中一个固定点的位置；AΩB^{A}\Omega_BAΩB为BQ^{B}QBQ相对于坐标系{A}的角速度；AVQ^{A}V_QAVQ为Q点在坐标系{A}的速度；BVQ^{B}V_{Q}BVQ为Q点在坐标系{B}的速度；BAR^{A}_{B}RBAR为坐标系{B}相对于坐标系{A}的变换矩阵。
AVQ=BARBVQ+AΩB×BARBQ^{A}V_Q =^{A}_{B}R \ ^{B}V_{Q} + ^{A}\Omega_B \times ^{A}_{B}R\ ^{B}Q AVQ=BAR BVQ+AΩB×BAR BQ

线速度+角速度

从坐标系{A}观测坐标系{B}中固定速度矢量的普遍公式
AVQ=AVBORG+BARBVQ+AΩB×BARBQ^{A}V_Q =^{A}V_{BORG} + ^{A}_{B}R \ ^{B}V_{Q} + ^{A}\Omega_B \times ^{A}_{B}R\ ^{B} Q AVQ=AVBORG+BAR BVQ+AΩB×BAR BQ

2.1.2 连杆速度

连杆i+1的速度为连杆i的速度加上附加到关节i+1上的速度分量。

注意：线速度相对于一点，角速度相对于一个物体，因此，"连杆的速度“指连杆坐标原点的线速度和连杆的角速度

角速度

连杆i+1的角速度等于连杆i的角速度加上一个由于关节i+1上的角速度引起的分量。
iwi+1=iwi+i+1iRθ˙i+1i+1Z^i+1θ˙i+1i+1Z^i+1=i+1[00θ˙i+1]^{i}w_{i+1} = ^{i}w_{i}+\ _{i+1}^{i}R \dot{\theta}_{i+1}\ ^{i+1}\hat{Z}_{i+1}\\ \dot{\theta}_{i+1}\ ^{i+1}\hat{Z}_{i+1} = ^{i+1}\begin{bmatrix} 0\\0\\\dot{\theta}_{i+1} \end{bmatrix} iwi+1=iwi+ i+1iRθ˙i+1 i+1Z^i+1θ˙i+1 i+1Z^i+1=i+1⎣⎡00θ˙i+1⎦⎤

两边同时左乘ii+1R_{i}^{i+1}Rii+1R可获得连杆i+1的角速度相对于坐标系{i+1}表达式：
i+1wi+1=ii+1Riwi+θ˙i+1i+1Z^i+1(1-1)^{i+1}w_{i+1} = _{i}^{i+1}R\ ^{i}w_{i}+ \dot{\theta}_{i+1}\ ^{i+1}\hat{Z}_{i+1} \tag{1-1} i+1wi+1=ii+1R iwi+θ˙i+1 i+1Z^i+1(1-1)

对θ\thetaθ求导
i+1w˙i+1=ii+1Riw˙i+ii+1Riwi×θ˙i+1i+1Z^i+1+θ¨i+1i+1Z^i+1(1-2)^{i+1}\dot{w}_{i+1} = _{i}^{i+1}R\ ^{i}\dot{w}_{i}+ _{i}^{i+1}R\ ^{i}w_{i}\times \dot{\theta}_{i+1}\ ^{i+1}\hat{Z}_{i+1} +\ddot{\theta}_{i+1}\ ^{i+1}\hat{Z}_{i+1} \tag{1-2} i+1w˙i+1=ii+1R iw˙i+ii+1R iwi×θ˙i+1 i+1Z^i+1+θ¨i+1 i+1Z^i+1(1-2)

线速度

坐标系{i+1}原点的线速度等于坐标系{i}原点的线速度加上一个由于连杆i的角速度引起的新的分量。（Pi+1P_{i+1}Pi+1为位置矢量）

从坐标原点指向质点所在位置的矢量称为位置矢量，简称位矢
直角坐标系：r=xi^+yj^+zk^r = x \hat{i} + y \hat{j} +z \hat{k}r=xi^+yj^+zk^

ivi+1=ivi+iwi×iPi+1^{i}v_{i+1} = ^{i}v_{i}+\ ^{i}w_i \times ^{i}P_{i+1} ivi+1=ivi+ iwi×iPi+1

两边同时左乘ii+1R_{i}^{i+1}Rii+1R：
i+1vi+1=ii+1R(ivi+iwi×iPi+1)^{i+1}v_{i+1} = _{i}^{i+1}R(^{i}v_{i}+\ ^{i}w_i \times ^{i}P_{i+1}) i+1vi+1=ii+1R(ivi+ iwi×iPi+1)

对位置矢量PPP求导为
i+1v˙i+1=ii+1R(iw˙i×iPi+1+iwi×(iwi×iPi+1)+iv˙i)(1-3,1-4)^{i+1}\dot{v}_{i+1} = _{i}^{i+1}R(^{i}\dot{w}_i \times ^{i}P_{i+1}+ ^{i}w_i \times (^{i}w_i \times ^{i}P_{i+1})+^{i}\dot{v}_{i}) \tag{1-3,1-4} i+1v˙i+1=ii+1R(iw˙i×iPi+1+iwi×(iwi×iPi+1)+iv˙i)(1-3,1-4)

2.2 力向外迭代

2.2.1 牛顿方程（Newton）

用于描述刚体的平动。

质点的牛顿方程
mr¨=Fm \ddot{r} = F mr¨=F
mmm为质点质量，rrr为矢径，r¨\ddot{r}r¨为加速度，FFF为质点的合力。

平动刚体的牛顿方程

刚体平动为刚体上每一点速度一致。
mrp¨=Fm \ddot{r_p} = F mrp¨=F
rp¨\ddot{r_p}rp¨为刚体上任一点加速度。

一般运动刚体的牛顿方程

刚体平动为刚体上各点速度不相同。
mrc¨=F(1-5)m \ddot{r_c} = F \tag{1-5} mrc¨=F(1-5)

rc¨\ddot{r_c}rc¨为刚体质心的加速度。

2.2.2 欧拉方程（Euler方程）

绕定点转动刚体的Euler方程
Mo=Iow˙+w×Iow(1-6)M_o = I_o \dot{w} + w \times I_o w　\tag{1-6} Mo=Iow˙+w×Iow　(1-6)

www为刚体固连坐标系的角速度，IoI_oIo为常值矩阵，表示刚体在与刚体固连坐标系中对O点的惯性张量矩。

2.3 力和力矩向内迭代

上图为典型连杆在无重力状态下受力图，根据该图建立力平衡方程和力矩平衡方程。

力平衡方程

iFi=ifi−i+1iRi+1fi+1^{i}F_i = ^{i}f_i - ^{i}_{i+1}R^{i+1}f_{i+1} iFi=ifi−i+1iRi+1fi+1

fif_ifi为连杆i-1作用在连杆i上的力

力矩平衡方程

iNi=ini−ini+1+(−iPCi)×ifi−(iPi+1−iPCi)×ifi+1^{i}N_i = ^{i}n_i - ^{i}n_{i+1} + (-^{i}P_{C_i}) \times ^{i}f_i - (^{i}P_{i+1}-^{i}P_{C_i}) \times ^{i}f_{i+1} iNi=ini−ini+1+(−iPCi)×ifi−(iPi+1−iPCi)×ifi+1

nin_ini为连杆i-1作用在连杆i上的力矩

ini+1=i+1iRi+1ni+1iFi=ifi−i+1iRi+1fi+1=iFi=ifi−ifi+1^{i}n_{i+1} =^{i}_{i+1}R\ ^{i+1}n_{i+1}\\ ^{i}F_i = ^{i}f_i - ^{i}_{i+1}R^{i+1}f_{i+1} =^{i}F_i = ^{i}f_i - ^{i}f_{i+1} ini+1=i+1iR i+1ni+1iFi=ifi−i+1iRi+1fi+1=iFi=ifi−ifi+1

根据力平衡方程和附加旋转矩阵可化简该公式

iNi=ini−i+1iRi+1ni+1+(−iPCi)×ifi−(−iPCi)×ifi+1−iPi+1×ifi+1=ini−i+1iRi+1ni+1−iPCi×iFi−iPi+1×i+1iRi+1fi+1\begin{aligned} ^{i}N_i &= ^{i}n_i -^{i}_{i+1}R\ ^{i+1}n_{i+1} +(-^{i}P_{C_i}) \times ^{i}f_i - (-^{i}P_{C_i}) \times ^{i}f_{i+1} -^{i}P_{i+1} \times ^{i}f_{i+1}\\ &= ^{i}n_i -^{i}_{i+1}R\ ^{i+1}n_{i+1} -^{i}P_{C_i} \times ^{i}F_i - ^{i}P_{i+1} \times ^{i}_{i+1}R\ ^{i+1}f_{i+1} \end{aligned} iNi=ini−i+1iR i+1ni+1+(−iPCi)×ifi−(−iPCi)×ifi+1−iPi+1×ifi+1=ini−i+1iR i+1ni+1−iPCi×iFi−iPi+1×i+1iR i+1fi+1

迭代方程

按照连杆序号高到低排序获得迭代关系为
ifi=i+1iRi+1fi+1+iFi(2-1)^{i}f_i = ^{i}_{i+1}R^{i+1}f_{i+1} + ^{i}F_i \tag{2-1} ifi=i+1iRi+1fi+1+iFi(2-1)

ini=iNi+i+1iRi+1ni+1+iPCi×iFi+iPi+1×i+1iRi+1fi+1(2-2)^{i}n_i = ^{i}N_i + ^{i}_{i+1}R\ ^{i+1}n_{i+1}+ ^{i}P_{C_i} \times ^{i}F_i + ^{i}P_{i+1} \times ^{i}_{i+1}R\ ^{i+1}f_{i+1} \tag{2-2} ini=iNi+i+1iR i+1ni+1+iPCi×iFi+iPi+1×i+1iR i+1fi+1(2-2)

2.4 建立动力学方程

2.5 两连杆动力学方程

假设连杆质量集中在连杆末端，质量分别为m1m_1m1和m2m_2m2。

连杆质心位置矢量

1Pc1=l1x1^=[l100]2Pc2=l2x2^=[l200]^{1}P_{c_1} = l_1 \hat{x_1} = \begin{bmatrix}l_1\\0\\0\end{bmatrix}\\^{2}P_{c_2} = l_2 \hat{x_2}= \begin{bmatrix}l_2\\0\\0\end{bmatrix} 1Pc1=l1x1^=⎣⎡l100⎦⎤2Pc2=l2x2^=⎣⎡l200⎦⎤

因质量集中，因此连杆质心惯性张量为零矩阵
C1I1=0C2I2=0^{C_1}I_{1} = 0 \\ ^{C_2}I_{2} = 0 C1I1=0C2I2=0

末端执行器上无作用力

f3=0n3=0f_3 =0\\ n_3 =0 f3=0n3=0

机器人基座为固定

w0=[000]w0˙=[000]w_0 =\begin{bmatrix} 0\\0\\0 \end{bmatrix}\\ \dot{w_0} =\begin{bmatrix} 0\\0\\0 \end{bmatrix} w0=⎣⎡000⎦⎤w0˙=⎣⎡000⎦⎤

包括重力因素（在牛顿－欧拉法下，只需要底座包含重力因素，其余连杆可以不考虑重力的影响）

0v0˙=gY0^=[0g0]^{0}\dot{v_0} = g \hat{Y_0} = \begin{bmatrix} 0\\g\\0 \end{bmatrix} 0v0˙=gY0^=⎣⎡0g0⎦⎤

连杆坐标系之间相对转动
i+1iR=[ci+1−si+10si+1ci+10001],ii+1R=[ci+1si+10−si+1ci+10001]_{i+1}^{i}R = \begin{bmatrix} c_{i+1} & -s_{i+1} &0 \\ s_{i+1} & c_{i+1} & 0\\ 0 & 0 & 1 \end{bmatrix} ,\\ _{i}^{i+1}R = \begin{bmatrix} c_{i+1} & s_{i+1} &0 \\ -s_{i+1} & c_{i+1} & 0\\ 0 & 0 & 1 \end{bmatrix} i+1iR=⎣⎡ci+1si+10−si+1ci+10001⎦⎤,ii+1R=⎣⎡ci+1−si+10si+1ci+10001⎦⎤

向外迭代（i:0→1i:0 \rightarrow 1i:0→1）

连杆1
连杆2

向内迭代（i:2→1i:2 \rightarrow 1i:2→1）
连杆2
连杆1

力矩（取ini^{i}n_{i}ini中的Z^\hat{Z}Z^方向分量，平行与关节的方向）

状态空间方程
τ＝M(Θ)Θ¨+C(Θ,Θ˙)+G(Θ)τ=[τ1τ2]\tau　＝　M(\Theta )\ddot{\Theta }+C(\Theta ,\dot{\Theta })+G(\Theta)\\\tau =\begin{bmatrix}\tau _1\\\tau _2\end{bmatrix}\\ τ　＝　M(Θ)Θ¨+C(Θ,Θ˙)+G(Θ)τ=[τ1τ2]
Θ\ThetaΘ为关节角度矩阵
质量矩阵M(Θ)M(\Theta )M(Θ)

速度项V(Θ,Θ˙)V(\Theta,\dot{\Theta} )V(Θ,Θ˙)包含与关节速度相关项

重力项G(Θ)G(\Theta )G(Θ)包含了与重力加速度g有关项

Matlab建模

% 动力学建模
syms l1 l2 m1 m2 g;
syms q1 q2 dq1 dq2 ddq1 ddq2;%% 参数初始化
R{1}=[cos(q1) -sin(q1) 0;sin(q1) cos(q1) 0;0 0 1];
R{2}=[cos(q2) -sin(q2) 0;sin(q2) cos(q2) 0;0 0 1];
R{3}=[1 0 0;0 1 0;0 0 1];
% 坐标系原点位移，用P{1}表示坐标系1与坐标系0原点位置关系，用P{2}表示坐标系2与坐标系1原点位置关系。
P = cell(1,3);
P{1}=[0;0;0];P{2}=[l1;0;0];P{3}=[l2;0;0];
% 每个连杆质心的位置矢量
Pc = cell(1,3);
Pc{1}=[0;0;0];Pc{2}=[l1;0;0];Pc{3}=[l2;0;0];
% 连杆质量
m = cell(1,3);
m{2}=m1;
m{3}=m2;
% 惯性张量
I = cell(1,3);
I{2}=[0;0;0];
I{3}=[0;0;0];% 连杆间角速度和角加速度
w = cell(1,3);dw = cell(1,3);
w{1}=[0;0;0];dw{1}=[0;0;0];% 机器人底座不旋转
% 连杆坐标原点和质心加速度
dv = cell(1,3);dvc = cell(1,3);
dv{1}=[0;g;0];% 重力因素% 关节速度和加速度
dq = cell(1,3); ddq = cell(1,3);
dq{2}=[0;0;dq1];dq{3}=[0;0;dq2];
ddq{2}=[0;0;ddq1];ddq{3}=[0;0;ddq2];% 末端执行器没有力
f = cell(1,4);n = cell(1,4);
f{4}=[0;0;0];
n{4}=[0;0;0];%% 建立运动学方程
% 外推
for i=1:2 %matlab下标从1开始
w{i+1}=R{i}.'*w{i}+dq{i+1};
dw{i+1}=R{i}.'*dw{i}+cross(R{i}.'*w{i},dq{i+1})+ddq{i+1};dv{i+1}=R{i}.'*(cross(dw{i},P{i})+cross(w{i},cross(w{i},P{i}))+dv{i});
dvc{i+1}=cross(dw{i+1},Pc{i+1})+cross(w{i+1},cross(w{i+1},Pc{i+1}))+dv{i+1};F{i+1}=m{i+1}*dvc{i+1};
N{i+1}=[0;0;0];%假设质量集中，每个连杆惯性张量为0
end% 内推
for i=3:-1:2f{i}=R{i}*f{i+1}+F{i};n{i}=N{i}+R{i}*n{i+1}+cross(Pc{i},F{i})+cross(P{i},R{i}*f{i+1});
end% 力矩
tau = cell(1,2);
tau{1} = n{2}(3);
tau{2} = n{3}(3);
celldisp(tau)

三、拉格朗日法

拉格朗日法为基于能量的动力学方法。

3.1 动能

第i根连杆的动能kik_iki为连杆质心线速度产生动能和连杆角速度产生动能之和
ki=12vCiTmivCi+12iwiTCiIiiwik_i = \frac{1}{2} v^{T}_{C_i} m_i v_{C_i} + \frac{1}{2} \ ^{i}w_{i}^{T} \ ^{C_i}I_{i} \ ^{i}w_i ki=21vCiTmivCi+21 iwiT CiIi iwi

整个机械臂动能为各个连杆动能之和
k=∑i+1nkik = \sum_{i+1}^{n} k_i k=i+1∑nki

vcv_cvc和iwi^{i}w_iiwi为Θ\ThetaΘ（关节位置）和Θ˙\dot{\Theta}Θ˙（关节速度）的函数，因此机械臂动能可描述为关节位置和速度的标量函数，M(Θ)M(\Theta)M(Θ)为机械臂n×nn \times nn×n的质量矩阵
k(Θ,Θ˙)=12Θ˙TM(Θ)Θ˙k(\Theta,\dot{\Theta}) = \frac{1}{2} \dot{\Theta}^T M(\Theta) \dot{\Theta} k(Θ,Θ˙)=21Θ˙TM(Θ)Θ˙

3.2 势能

第i根连杆的势能uiu_iui为
ui=−mi0gT0PCi+urefiu_i = - m_i \ ^{0}g^{T} \ ^{0}P_{C_i}+u_{refi} ui=−mi 0gT 0PCi+urefi

0g^{0}g0g为3×13 \times 13×1重力矢量，0PCi^{0}P_{C_i}0PCi为第i根连杆质心矢量3×13 \times 13×1，urefiu_{refi}urefi是使uiu_iui的最小值为零的常数。

urefiu_{refi}urefi的引入用于表示连杆的势能的参考点，但实际上在动力学方程中，仅计算势能相对于Θ\ThetaΘ的偏导数∂u∂Θ\frac{\partial u}{\partial \Theta}∂Θ∂u，因此该常数值可为任意值，即势能可相对于任意一个参考零点。

3.3 构建动力学方程

拉格朗日函数定义为系统的动能减去势能
L(Θ,Θ˙)=k(Θ,Θ˙)−u(Θ)\mathcal L(\Theta,\dot{\Theta})=k(\Theta,\dot{\Theta})-u(\Theta) L(Θ,Θ˙)=k(Θ,Θ˙)−u(Θ)

操作臂运动方程为
ddt∂L∂Θ˙−∂L∂Θ=τ\frac{d}{dt} \frac{\partial \mathcal L}{\partial \dot{\Theta}}- \frac{\partial \mathcal L}{\partial \Theta}=\tau dtd∂Θ˙∂L−∂Θ∂L=τ

τ\tauτ为n×1n \times 1n×1力矩矢量，运动方程代入拉格朗日函数为
ddt∂k∂Θ˙−∂k∂Θ+∂u∂Θ=τ\frac{d}{dt} \frac{\partial k}{\partial \dot{\Theta}}- \frac{\partial k}{\partial \Theta}+\frac{\partial u}{\partial \Theta}=\tau dtd∂Θ˙∂k−∂Θ∂k+∂Θ∂u=τ

3.4 两连杆动力学方程

2R机械臂在处在xy平面内，重力沿-y方向。

连杆质心线速度

连杆1

[x1y1]=[L1cos⁡θ1L1sin⁡θ1][x˙1y˙1]=[−L1sin⁡θ1L1cos⁡θ1]θ˙1\left[\begin{matrix} x_1\\ y_1 \end{matrix}\right]= \left[\begin{matrix} L_1 \cos\theta_1\\ L_1 \sin\theta_1 \end{matrix}\right]\\ \left[\begin{matrix} \dot x_1\\ \dot y_1 \end{matrix}\right]= \left[\begin{matrix} -L_1 \sin\theta_1\\ L_1 \cos\theta_1 \end{matrix}\right]\dot \theta_1 [x1y1]=[L1cosθ1L1sinθ1][x˙1y˙1]=[−L1sinθ1L1cosθ1]θ˙1

连杆2

[x2y2]=[L1cos⁡θ1+L2cos⁡(θ1+θ2)y1sin⁡θ1+L2sin⁡(θ1+θ2)][x˙2y˙2]=[−L1sin⁡θ1−L2sin⁡(θ1+θ2)−L2sin⁡(θ1+θ2)L1cos⁡θ1+L2cos⁡(θ1+θ2)L2cos⁡(θ1+θ2)][θ˙1θ˙2]\left[\begin{matrix} x_2\\ y_2 \end{matrix}\right]= \left[\begin{matrix} L_1 \cos\theta_1+L_2 \cos(\theta_1+\theta_2)\\ y_1 \sin\theta_1+L_2 \sin(\theta_1+\theta_2) \end{matrix}\right]\\ \left[\begin{matrix} \dot x_2\\ \dot y_2 \end{matrix}\right]= \left[\begin{matrix} -L_1 \sin \theta_1-L_2 \sin(\theta_1+\theta_2)& -L_2 \sin(\theta_1+\theta_2)\\ L_1 \cos \theta_1+L_2 \cos(\theta_1+\theta_2) & L_2 \cos(\theta_1+\theta_2) \end{matrix}\right] \left[\begin{matrix} \dot\theta_1\\ \dot\theta_2 \end{matrix}\right] [x2y2]=[L1cosθ1+L2cos(θ1+θ2)y1sinθ1+L2sin(θ1+θ2)][x˙2y˙2]=[−L1sinθ1−L2sin(θ1+θ2)L1cosθ1+L2cos(θ1+θ2)−L2sin(θ1+θ2)L2cos(θ1+θ2)][θ˙1θ˙2]

连杆角速度

因质量集中，连杆质心惯性张量为零矩阵
C1I1=0C2I2=0^{C_1}I_{1} = 0 \\ ^{C_2}I_{2} = 0 C1I1=0C2I2=0
因此连杆角速度产生动能为0，即
ki=12vCiTmivCik_i = \frac{1}{2} v^{T}_{C_i} m_i v_{C_i} ki=21vCiTmivCi
连杆动能

K1=12m1(x˙12+y˙12)=12m1L12θ˙12K2=12m2(x˙22+y˙22)=12m2((L12+2L1L2cos⁡θ2+L22)θ˙1+2(L22+L1L2cos⁡θ2)θ˙1θ˙2+L22θ˙22)\begin{aligned} \mathcal K_1 &=\frac{1}{2}m_1(\dot x_1^2+\dot y_1^2)=\frac{1}{2}m_1L_1^2\dot\theta_1^2\\ \mathcal K_2 &=\frac{1}{2}m_2(\dot x_2^2+\dot y_2^2)\\ &=\frac{1}{2}m_2((L_1^2+2L_1L_2 \cos\theta_2+L_2^2)\dot\theta_1+2(L_2^2+L_1L_2 \cos\theta_2)\dot\theta_1\dot\theta_2+L_2^2\dot\theta_2^2) \end{aligned} K1K2=21m1(x˙12+y˙12)=21m1L12θ˙12=21m2(x˙22+y˙22)=21m2((L12+2L1L2cosθ2+L22)θ˙1+2(L22+L1L2cosθ2)θ˙1θ˙2+L22θ˙22)

连杆势能
u1=m1gy1=m1gL1sinθ1u2=m2gy2=m2g(L1sinθ1+L2sin(θ1+θ2))\begin{aligned} u_1&=m_1gy_1=m_1gL_1sin\theta_1\\ u_2&=m_2gy_2=m_2g(L_1sin\theta_1+L_2sin(\theta_1+\theta_2)) \end{aligned} u1u2=m1gy1=m1gL1sinθ1=m2gy2=m2g(L1sinθ1+L2sin(θ1+θ2))

拉格朗日方程
L=(K1+K2)−(u1+u2)=(12m1L12θ˙12+12m2((L12+2L1L2cos⁡θ2+L22)θ˙1+2(L22+L1L2cos⁡θ2)θ˙1θ˙2+L22θ˙22))−(m1gL1sinθ1+m2g(L1sinθ1+L2sin(θ1+θ2)))\begin{aligned} \mathcal L =& (\mathcal K_1 + \mathcal K_2) - (u_1 +u_2)\\ =& (\frac{1}{2}m_1L_1^2\dot\theta_1^2 + \frac{1}{2}m_2((L_1^2+2L_1L_2 \cos\theta_2+L_2^2)\dot\theta_1+2(L_2^2+L_1L_2 \cos\theta_2)\dot\theta_1\dot\theta_2+L_2^2\dot\theta_2^2)) \\& -(m_1gL_1sin\theta_1+m_2g(L_1sin\theta_1+L_2sin(\theta_1+\theta_2))) \end{aligned} L==(K1+K2)−(u1+u2)(21m1L12θ˙12+21m2((L12+2L1L2cosθ2+L22)θ˙1+2(L22+L1L2cosθ2)θ˙1θ˙2+L22θ˙22))−(m1gL1sinθ1+m2g(L1sinθ1+L2sin(θ1+θ2)))

使用运动方程求力矩
τi=ddt∂L∂θ˙i−∂Lθi,i=1,2\tau_i=\frac{d}{dt}\frac{\partial \mathcal L}{\partial \dot\theta_i}-\frac{\partial \mathcal L}{\theta_i}, i=1,2 τi=dtd∂θ˙i∂L−θi∂L,i=1,2

τ1=(m1L12+m2(L12+2L1L2cos⁡θ2+L22))θ¨1+m2(L1L2cos⁡θ2+L22)θ¨2−m2L1L2sin⁡θ2(2θ˙1θ˙2+θ˙22)+(m1+m2)L1gcos⁡θ1+m2gL2cos⁡(θ1+θ2)τ2=m2(L1L2cos⁡θ2+L22)θ¨1+m2L22θ¨2+m2L1L2θ˙12sin⁡θ2+m2gL2cos⁡(θ1+θ2)\begin{aligned} \tau_1 =& (m_1L_1^2+m_2(L_1^2+2L_1L_2 \cos\theta_2+L_2^2))\ddot\theta_1+\\ & m_2(L_1L_2 \cos\theta_2+L_2^2)\ddot\theta_2-m_2L_1L_2 \sin\theta_2(2\dot\theta_1\dot\theta_2+\dot\theta_2^2)+\\ & (m_1+m_2)L_1g \cos\theta_1+m_2gL_2 \cos(\theta_1+\theta_2)\\ \tau_2 = & m_2(L_1L_2 \cos\theta_2+L_2^2)\ddot\theta_1 + m_2L_2^2 \ddot\theta_2 + m_2 L_1 L_2 \dot \theta_1^2 \sin \theta_2 \\ & +m_2gL_2 \cos(\theta_1+\theta_2) \end{aligned} τ1=τ2=(m1L12+m2(L12+2L1L2cosθ2+L22))θ¨1+m2(L1L2cosθ2+L22)θ¨2−m2L1L2sinθ2(2θ˙1θ˙2+θ˙22)+(m1+m2)L1gcosθ1+m2gL2cos(θ1+θ2)m2(L1L2cosθ2+L22)θ¨1+m2L22θ¨2+m2L1L2θ˙12sinθ2+m2gL2cos(θ1+θ2)

状态空间方程
M(θ)=[m1L12+m2(L12+2L1L2cos⁡θ2+L22)m2(L1L2cos⁡θ2+L22)m2(L1L2cos⁡θ2+L22)m2L22]c(θ,θ˙)=[−m2(L1L2sin⁡θ2(2θ˙1θ˙2+θ˙22)m2L1L2θ˙12sin⁡θ2]g(θ)=[(m1+m2)L1gcos⁡θ1+m2gL2cos⁡(θ1+θ2)m2gL2cos⁡(θ1+θ2)]\begin{aligned} M(\theta)&=\left [ \begin{matrix} m_1L_1^2+m_2(L_1^2+2L_1L_2 \cos\theta_2+L_2^2) &m_2(L_1L_2 \cos\theta_2+L_2^2)\\ m_2(L_1L_2 \cos\theta_2+L_2^2) &m_2L_2^2\\ \end{matrix}\right]\\ c(\theta,\dot\theta)&=\left [ \begin{matrix} -m_2(L_1L_2 \sin\theta_2(2\dot\theta_1\dot\theta_2 +\dot\theta_2^2)\\ m_2L_1L_2\dot\theta_1^2 \sin\theta_2\\ \end{matrix}\right]\\ g(\theta)&=\left [ \begin{matrix} (m_1+m_2)L_1g \cos\theta_1+m_2 g L_2 \cos(\theta_1+\theta_2)\\ m_2 g L_2 \cos(\theta_1+\theta_2)\\ \end{matrix}\right] \end{aligned} M(θ)c(θ,θ˙)g(θ)=[m1L12+m2(L12+2L1L2cosθ2+L22)m2(L1L2cosθ2+L22)m2(L1L2cosθ2+L22)m2L22]=[−m2(L1L2sinθ2(2θ˙1θ˙2+θ˙22)m2L1L2θ˙12sinθ2]=[(m1+m2)L1gcosθ1+m2gL2cos(θ1+θ2)m2gL2cos(θ1+θ2)]

matlab建模

% 质量集中，无惯性矩阵，连杆长度与质心位置重合
syms m1 m2 l1 l2 g
syms q1 q2 q1d q2d q1dd q2dd
syms x1(t) x1d x1dd x2(t) x2d x2ddx1d=diff(x1,t); x2d=diff(x2,t);
x1dd=diff(x1,t,t); x2dd=diff(x2,t,t);%质量矩阵
M1=diag([m1,m1]);
M2=diag([m2,m2]);% 速度
V1=[[l1*cos(x1(t)) 0];[l1*sin(x1(t)) 0]]*[x1d;x2d];
V2=[[-l1*sin(x1(t))-l2*sin(x1(t)+x2(t)) -l2*sin(x1(t)+x2(t))];[l1*cos(x1(t))+l2*cos(x1(t)+x2(t)) l2*cos(x1(t)+x2(t))]]*[x1d;x2d];% 动能
K1=simplify((1/2)*V1.'*M1*V1);
K2=simplify((1/2)*V2.'*M2*V2);
K =K1+K2;% 势能
u1=m1*g*l1*sin(q1);
u2=m2*g*(l1*sin(q1)+l2*sin(q1+q2));
u =u1+u2;% 拉格朗日方程
L=K-u;
L=subs(L,{x1,x2,x1d,x2d,x1dd,x2dd},{q1,q2,q1d,q2d,q1dd,q2dd});% 利用运动方程计算力矩方程
dLdqd=[diff(L,q1d); diff(L,q2d)];
dLdqd =subs(dLdqd, {q1,q2,q1d,q2d,q1dd,q2dd}, {x1,x2,x1d,x2d,x1dd,x2dd});
ddLdqddt=diff(dLdqd,t);
ddLdqddt= subs(ddLdqddt,{x1,x2,x1d,x2d,x1dd,x2dd},{q1,q2,q1d,q2d,q1dd,q2dd});
dLdq=[diff(L,q1); diff(L,q2)];% 输出结果
syms f
f=simplify(ddLdqddt-dLdq)

四、算法对比

通过牛顿欧拉方程导出的逆动力算法较拉格朗日方程推导计算量要少，但实际上两者差异在于表示连杆角速度的方式不一致，牛顿欧拉方程采用三维矢量，拉格朗日方程采用3×33 \times 33×3旋转矩阵导数，因此多余因素造成计算量增大，若在计算动能时采用三维角速度矢量描述，则拉格朗日方程可推导处与牛顿欧拉方程相同的形式的逆动力方程。

在实际应用中，多轴机械臂连杆结合计算机建模更适合选用牛顿-欧拉法，手动推算下拉格朗日法更加简单直观，但推导多轴机械臂较为困难。

参考

MATLAB机器人运动学与动力学

MATLAB 中的机械臂算法——动力学

三关节机械臂动力学模型问题？

【机器人学】机器人开源项目KDL源码学习：（4）机械臂逆动力学的牛顿欧拉算法

matlab递推牛顿-欧拉法解机械臂动力学方程

《机器人动力学与控制》第九章——动力学 9.1初探欧拉拉格朗日方程法

机器人动力学–拉格朗日公式

《机器人动力学与控制》

《机器人技术基础》

《机器人学导论》

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