标签：q1 拉格朗 q2 机器人 syms 详解 pmatrix real dot

拉格朗日动力学

• 动力学总公式
• • 动能部分
  • 势能部分
  • 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

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。