机器人拉格朗日动力学应用公式详解

2023-12-10 21:46:27

拉格朗日动力学

动力学总公式
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=21viTvi+21ωiTIciω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,xxIci,yyIci,zz⎦⎤

势能部分

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

U = − m i g 0 d i U = - m_{i} g_{0} d_{i} U=−mig0di

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=dcicos(θ)

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=21q˙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−dc100⎠⎞

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+dc2cos(q2)dc2sin(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˙−dc2sin(q2)q2˙dc2cos(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=21m1q1˙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=21m2(q1˙2+dc22q2˙2−2dc2sin(q2)q1˙q2˙)+21Ic2,zzq2˙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]

码农公寓