拉格朗日动力学

• 动力学总公式
• • 动能部分
  • 势能部分
  • M(q)部分
  • c ( q , q ˙ ) c(q,\dot{q}) c(q,q˙​)部分
  • g(q)部分
• PR机械臂例题
• Matlab代码实现
• 相关资料

---

请务必先看此处！！！！！

本文没有理论分析，是直接开干！！！

公式部分详细介绍了公式及各参数及其由来

例题部分详细写了公式各部分的详细计算和使用

代码部分则是实现了例题部分的Matlab代码

相关资料部分写了主要的参考来源

---

动力学总公式

中间的推导过程略过，感兴趣得朋友可以自行去查看资料

M ( q ) q ¨ + c ( q , q ˙ ) + g ( q ) = u (动力学总公式) M(q) \ddot{q} +c(q, \dot{q})+g(q)=u \tag{动力学总公式} M(q)q¨​+c(q,q˙​)+g(q)=u(动力学总公式)

• M(q) 表示广义机器人的惯性矩阵，对称的，正定的， ∀ q ⇒ \forall q \Rightarrow ∀q⇒总是可逆的,求法请继续往下看

• q ¨ = [ q ¨ c 1 . . . q ¨ c n ] \ddot{q}=\begin{bmatrix} \ddot{q}_{c1} \\ ... \\ \ddot{q}_{cn} \end{bmatrix} q¨​=⎣⎡​q¨​c1​...q¨​cn​​⎦⎤​ 表示连杆质心C的角加速度， q = [ q c 1 . . . q c n ] q=\begin{bmatrix} q_{c1} \\ ... \\ q_{cn} \end{bmatrix} q=⎣⎡​qc1​...qcn​​⎦⎤​表示角度, c c c 表示连杆质心C

• c ( q , q ˙ ) c(q, \dot{q}) c(q,q˙​) 表示 Coriolis力的系数

• g ( q ) g(q) g(q) 表示重力项

动能部分

第 i i i 个单个连杆的总动能公式：

T i = 1 2 v i T v i + 1 2 ω i T I c i ω i ( 1, K o ¨ nig定理 ) T_{i} = \frac{1}{2} v_{i}^{T}v_{i}+\frac{1}{2} \omega_{i}^{T}I_{ci}\omega_{i} \tag{1, König定理} Ti​=21​viT​vi​+21​ωiT​Ici​ωi​(1, Ko¨nig定理)

各部分参数详细解析：

• v v v 表示连杆质心 C i C_{i} Ci​ 的线速度

• ω \omega ω 表示连杆质心 C i C_{i} Ci​的角速度

• I c i = [ I c i , x x I c i , y y I c i , z z ] I_{ci} = \begin{bmatrix} I_{ci,xx}& & \\ & I_{ci,yy}& \\ & & I_{ci,zz} \end{bmatrix} Ici​=⎣⎡​Ici,xx​​Ici,yy​​Ici,zz​​⎦⎤​

势能部分

第 i i i 个单个连杆的总势能公式：

U = − m i g 0 d i U = - m_{i} g_{0} d_{i} U=−mi​g0​di​

• m i m_{i} mi​表示连杆质量

• g 0 g_{0} g0​表示重力加速度，一般为 9.81 m / s 2 9.81m/s^{2} 9.81m/s2

• d i d_{i} di​表示初始连接点到连杆质心C的 d c i d_{ci} dci​ 在 g 0 g_{0} g0​ 方向的分量,一般 d i = d c i c o s ( θ ) d_{i} = d_{ci}cos(\theta) di​=dci​cos(θ)

M(q)部分

整个机器人的总动能为：

T = T 1 + . . . + T n (2) T = T_{1}+ ... +T_{n} \tag{2} T=T1​+...+Tn​(2)

另有：

T = 1 2 q ˙ T M ( q ) q ˙ (3) T = \frac{1}{2} \dot{q}^{T} M(q) \dot{q} \tag{3} T=21​q˙​TM(q)q˙​(3)

• q ˙ = [ q ˙ c 1 . . . q ˙ c n ] \dot{q}= \begin{bmatrix} \dot{q}_{c1} \\ ... \\ \dot{q}_{cn} \end{bmatrix} q˙​=⎣⎡​q˙​c1​...q˙​cn​​⎦⎤​ 表示角速度

让 ( 2 ) ( 3 ) (2)(3) (2)(3) 相等，只有M(q)为未知数，则可求得M(q)

c ( q , q ˙ ) c(q,\dot{q}) c(q,q˙​)部分

c k ( q , q ˙ ) = q ˙ T C k ( q ) q ˙ c_{k}(q,\dot{q})=\dot{q}^{T}C_{k}(q)\dot{q} ck​(q,q˙​)=q˙​TCk​(q)q˙​

C k ( q ) = 1 2 ( ∂ M k ∂ q + ( ∂ M k ∂ q ) T − ∂ M ∂ q k ) (4) C_{k}(q)=\frac{1}{2}(\frac{\partial M_{k}}{\partial q} + (\frac{\partial M_{k}}{\partial q})^{T}- \frac{\partial M}{\partial q_{k}}) \tag{4} Ck​(q)=21​(∂q∂Mk​​+(∂q∂Mk​​)T−∂qk​∂M​)(4)

这里的 M k M_{k} Mk​ 的详细解释请参见下方的例题

g(q)部分

g ( q ) = ∂ U ∂ q (5) g(q)=\frac{\partial U}{\partial q} \tag{5} g(q)=∂q∂U​(5)

PR机械臂例题

• 第一步，先仔细看题目(废话)，然后写出每个连杆质心的位置坐标 P，比如该题为：

P c 1 = ( q 1 − d c 1 0 0 ) P_{c1}=\begin{pmatrix} q_{1}-d_{c1} \\ 0 \\ 0 \end{pmatrix} Pc1​=⎝⎛​q1​−dc1​00​⎠⎞​

P c 1 = ( q 1 + d c 2 c o s ( q 2 ) d c 2 s i n ( q 2 ) 0 ) P_{c1}=\begin{pmatrix} q_{1}+d_{c2}cos(q_{2}) \\ d_{c2}sin(q_{2}) \\ 0 \end{pmatrix} Pc1​=⎝⎛​q1​+dc2​cos(q2​)dc2​sin(q2​)0​⎠⎞​

• 第二步，对 P 的时间 t 求导（其实这里的 q i = q i ( t ) q_{i}=q_{i}(t) qi​=qi​(t)，只是简写为q）得其线速度, 写出其角速度(同理，对q的时间求导)， ω 1 = 0 \omega_{1}=0 ω1​=0，因为是P类连杆，压根没角速度：

P c 1 ˙ = ( q 1 ˙ 0 0 ) \dot{P_{c1}}=\begin{pmatrix} \dot{q_{1}} \\ 0 \\ 0 \end{pmatrix} Pc1​˙​=⎝⎛​q1​˙​00​⎠⎞​

P c 2 ˙ = ( q 1 ˙ − d c 2 s i n ( q 2 ) q 2 ˙ d c 2 c o s ( q 2 ) q 2 ˙ 0 ) \dot{P_{c2}}=\begin{pmatrix} \dot{q_{1}}-d_{c2}sin(q_{2})\dot{q_{2}} \\ d_{c2}cos(q_{2})\dot{q_{2}} \\ 0 \end{pmatrix} Pc2​˙​=⎝⎛​q1​˙​−dc2​sin(q2​)q2​˙​dc2​cos(q2​)q2​˙​0​⎠⎞​

ω 1 = ( 0 0 0 ) \omega_{1}=\begin{pmatrix}0 \\ 0 \\ 0 \end{pmatrix} ω1​=⎝⎛​000​⎠⎞​

ω 2 = ( 0 0 q 2 ˙ ) \omega_{2}=\begin{pmatrix}0 \\ 0 \\ \dot{q_{2}} \end{pmatrix} ω2​=⎝⎛​00q2​˙​​⎠⎞​

• 第三步，将上述结果带入式（1），求得动能：

T 1 = 1 2 m 1 q 1 ˙ 2 T_{1} = \frac{1}{2} m_{1} \dot{q_{1}}^2 T1​=21​m1​q1​˙​2

T 2 = 1 2 m 2 ( q 1 ˙ 2 + d c 2 2 q 2 ˙ 2 − 2 d c 2 s i n ( q 2 ) q 1 ˙ q 2 ˙ ) + 1 2 I c 2 , z z q 2 ˙ 2 T_{2}=\frac{1}{2} m_{2} (\dot{q_{1}}^2+d_{c2}^{2}\dot{q_{2}}^2-2d_{c2}sin(q_{2}) \dot{q_{1}} \dot{q_{2}})+\frac{1}{2}I_{c2,zz}\dot{q_{2}}^2 T2​=21​m2​(q1​˙​2+dc22​q2​˙​2−2dc2​sin(q2​)q1​˙​q2​˙​)+21​Ic2,zz​q2​˙​2

• 第四步，将T带入式（3），求得M(q)：

• 第五步，将M 带入式（4）求得 C k ( q ) C_{k}(q) Ck​(q) 继而求得 c ( q , q ˙ ) c(q, \dot{q}) c(q,q˙​)：

• 第六步，按说该求势能部分的，但是这个机械臂是平面的，没有势能，所以 g(q)=0， 所以这一步就是将上边求得的式子带入动力学总公式：

Matlab代码实现

clear all
close all
clc

%% 定义各种要用的符号

syms m1 m2 m3 m4 real     %各连杆质量
syms d1 d2 d3 d4 real     %各质心长度
syms L1 L2 L3 L4 L h real %各连杆长度
syms I1xx I1yy I1zz real  
syms I2xx I2yy I2zz real 
syms I3xx I3yy I3zz real  
syms I4xx I4yy I4zz real  
syms q1 q2 q3 q4 real     %角
syms qv1 qv2 qv3 qv4 real %角速度
syms qa1 qa2 qa3 qa4 real %角加速度
syms u1 u2 u3 u4 g0 real  


%% 定义下要用的东西

q = [q1; q2];
qv = [qv1; qv2];
qa = [qa1; qa2];
%% 求T1

pc1 = [q1-d1; 0;0];
vc1=diff(pc1,q1)*qv1+diff(pc1,q2)*qv2;
T1 = 1/2*m1*(vc1'*vc1);


%% 求T2

pc2 = [q1+d2*cos(q2); d2*sin(q2);0];
vc2=diff(pc2,q1)*qv1+diff(pc2,q2)*qv2;
T2a = 1/2*m2*(vc2'*vc2);

om2 = [0; 0; qv2];
T2b = 1/2*om2'*diag([I2xx I2yy I2zz])*om2;

T2=T2a+T2b;

%% 求总T

T = simplify(T1 + T2)

%% 求M

M(1,1)=diff(T,qv1,2);
M(2,2)=diff(T,qv2,2);
TempB12=diff(T,qv1);
M(1,2)=diff(TempB12,qv2);
M(2,1)=M(1,2);
M=simplify(M)

%% 求c

M1=M(:,1);
C1=(1/2)*(jacobian(M1,q)+jacobian(M1,q)'-diff(M,q1))
M2=M(:,2);
C2=(1/2)*(jacobian(M2,q)+jacobian(M2,q)'-diff(M,q2))

c1=qv'*C1*qv;
c2=qv'*C2*qv;
c=[c1;c2]

%% 最终结果

M*qa+c == [u1; u2]

相关资料

理论来源于本人机器人学课程的教授——机器人领域老牛：Prof. Alessandro De Luca

代码来源于这个老哥

标签：q1,拉格朗,q2,机器人,syms,详解,pmatrix,real,dot
来源： https://blog.csdn.net/potato_uncle/article/details/115775186