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

article/2025/10/24 10:06:43

拉格朗日动力学

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

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

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

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

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

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

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

本文最佳食用方式！

先快速浏览下公式部分都有什么东西，以确保后续要用时你能找到

直接去例题部分，跟着推导一边，需要用到啥公式再去公式部分查找，再详细看具体的公式解释

然后去看代码实现，你手推一遍之后就会很容易看懂公式，争取能自己写出来不同题目的代码

想进一步了解的，请看相关资料部分，老卢卡主页的Robotics里面的course material部分有PPT和历次的考试题

题是真的难的一匹。。。~~期中八成要挂了，跪地吐血~~

~~果然挂了，吐血~~

动力学总公式

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

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

M(q) 表示广义机器人的惯性矩阵，对称的，正定的， $\forall q \Rightarrow$ 总是可逆的,求法请继续往下看
$\ddot{q}=\begin{bmatrix} \ddot{q}_{c1} \\ ... \\ \ddot{q}_{cn} \end{bmatrix}$ 表示连杆质心C的角加速度， $q=\begin{bmatrix} q_{c1} \\ ... \\ q_{cn} \end{bmatrix}$ 表示角度, $c$ 表示连杆质心C
$\dot{q})$ 表示 Coriolis力的系数
$g (q)$ 表示重力项

动能部分

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

$T_{i} = \frac{1}{2}m_{i} v_{i}^{T}v_{i}+\frac{1}{2} \omega_{i}^{T}I_{ci}\omega_{i} \tag{1, König定理}$

各部分参数详细解析：

$v$ 表示连杆质心 $C_{i}$ 的线速度
$\omega$ 表示连杆质心 $C_{i}$ 的角速度
$I_{ci} = \begin{bmatrix} I_{ci,xx}& & \\ & I_{ci,yy}& \\ & & I_{ci,zz} \end{bmatrix}$

势能部分

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

$U = - m_{i} g_{0} d_{i}$

$m_{i}$ 表示连杆质量
$g_{0}$ 表示重力加速度，一般为 $9.81m/s^{2}$
$d_{i}$ 表示初始连接点到连杆质心C的 $d_{ci}$ 在 $g_{0}$ 方向的分量,一般 $d_{i} = d_{ci}cos(\theta)$

M(q)部分

整个机器人的总动能为：

$T_{1}+ ... +T_{n} \tag{2}$

另有：

$\frac{1}{2} \dot{q}^{T} M(q) \dot{q} \tag{3}$

$\dot{q}= \begin{bmatrix} \dot{q}_{c1} \\ ... \\ \dot{q}_{cn} \end{bmatrix}$ 表示角速度

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

$c(q,\dot{q})$ 部分

$c_{k}(q,\dot{q})=\dot{q}^{T}C_{k}(q)\dot{q}$

$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}$

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

g(q)部分

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

PR机械臂例题

在这里插入图片描述

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

$P_{c1}=\begin{pmatrix} q_{1}-d_{c1} \\ 0 \\ 0 \end{pmatrix}$

$P_{c1}=\begin{pmatrix} q_{1}+d_{c2}cos(q_{2}) \\ d_{c2}sin(q_{2}) \\ 0 \end{pmatrix}$

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

$\dot{P_{c1}}=\begin{pmatrix} \dot{q_{1}} \\ 0 \\ 0 \end{pmatrix}$

$\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}$

$\omega_{1}=\begin{pmatrix}0 \\ 0 \\ 0 \end{pmatrix}$

$\omega_{2}=\begin{pmatrix}0 \\ 0 \\ \dot{q_{2}} \end{pmatrix}$

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

$T_{1} = \frac{1}{2} m_{1} \dot{q_{1}}^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$

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

在这里插入图片描述

第五步，将M 带入式（4）求得 $C_{k}(q)$ 继而求得 $\dot{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];
%% 求T1pc1 = [q1-d1; 0;0];
vc1=diff(pc1,q1)*qv1+diff(pc1,q2)*qv2;
T1 = 1/2*m1*(vc1'*vc1);%% 求T2pc2 = [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;%% 求总TT = simplify(T1 + T2)%% 求MM(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)%% 求cM1=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]