【Matlab 六自由度机器人】机器人动力学之推导拉格朗日方程（附MATLAB机器人动力学拉格朗日方程推导代码）_六自由度机器人动力学方程

近期更新

【汇总】

$\boxed{【Matlab六自由度机器人】系列文章汇总}$

【主线】

$运动学$

1. 定义标准型及改进型D-H参数，建立机器人模型。
2. 运动学正解
3. 基于蒙特卡罗方法(Monte Carlo Method)构建机器人工作空间

$动力学$
（待补充）

【补充说明】

1. 关于灵活工作空间与可达工作空间的理解
2. 关于改进型D-H参数(modified Denavit-Hartenberg)的详细建立步骤
3. 关于旋转的参数化（欧拉角、姿态角、四元数）的相关问题
4. 关于双变量函数atan2(x,y)的解释
5. 关于机器人运动学反解的有关问题

前言

本篇对机器人动力学进行一个概述。
之前谈到的运动学方程仅描述了机器人的运动过程，没有考虑到产生运动的力和扭矩，而动力学方程能描述力和运动之间的关系，因此我们在此引入动力学的概念。

本人在读研期间仅在机器人运动学的基础上完成论文的撰写，有些遗憾未能将机器人动力学应用到文章之中，在此写下机器人动力学的概述以及学习过程中遇到的问题和解决思路。

---

以下是本篇文章正文内容

分析机器人操作的动态数学模型，主要采用下列两种理论

1. 动力学基本理论，包括牛顿欧拉方程。
2. 拉格朗日力学，特别是二阶拉格朗日方程。

第一个方法：牛顿—欧拉方程即力的动态平衡法。当用此法时，需从运动学出发求得加速度，并消去各内作用力。对于较复杂的系统，此种分析方法十分复杂与麻烦。
第二个方法：拉格朗日方程即拉格朗日功能平衡法，也称为欧拉—拉格朗日方程，它只需要速度而不必求内作用力。因此，这是一种直截了当和简便的方法。

在本篇文章中主要采用拉格朗日方程来分析和求解机械手的动力学问题。

正文

一、拉格朗日方程的推导

1. 单自由度系统

我们以图中所示的单自由度系统为例，来说明如何从牛顿第二定律推导出拉格朗日方程。图中的圆点为带有质量的一个粒子，下面称为质点。该质点受到重力$g$的效果和拉力$f$的效果。

质量为$m$的粒子受到限制，只能在垂直方向移动，这就构成了一个单自由度系统。 重力$mg$向下作用， 外力$f$则向上作用。

根据牛顿第二定律，这个系统中的质点的运动方程为：
$$ma=f-mg⟹m{\ddot{y}}_{}^{}=f-mg$$
左侧的$m{\ddot{y}}_{}^{}$也可以用以下方程推出
$$m{\ddot{y}}_{}^{}=\frac{\mathrm{d}}{\mathrm{d}t}\left(m\dot{y}\right)=\frac{\mathrm{d}}{\mathrm{d}t}\frac{\partial }{\partial \dot{y}}\left(\frac{1}{2}m{\dot{y}}^2\right)=\frac{\mathrm{d}}{\mathrm{d}t}\left(\frac{\partial \mathcal{K}}{\partial \dot{y}}\right)$$
注：$m\dot{y}可以写作\frac{\partial }{\partial \dot{y}}\left(\frac{1}{2}m{\dot{y}}^2\right)$，即$\frac{1}{2}m{\dot{y}}^2$对$y$进行求偏导。

其中，动能$\mathcal{K}=\frac{1}{2}m{\dot{y}}^2$

类似上面的方程，我们可以将重力表达为：
$$mg=\frac{\partial }{\partial y}\left(mgy\right)=\frac{\partial \mathcal{P}}{\partial y}$$
其中，重力势能$\mathcal{P}=mgy$

定义函数$\mathcal{L}$，它是系统的动能和势能之差，也称为系统的拉格朗日算子
$$\mathcal{L}=\mathcal{K}-\mathcal{P}=\frac{1}{2}m{\dot{y}}^2-mgy$$
$①\mathcal{L}对\dot{y}求偏导，可得到\frac{\partial \mathcal{L}}{\partial \dot{y}}=\frac{\partial \mathcal{K}}{\partial \dot{y}}$

$②\mathcal{L}对\dot{y}求偏导，可得到\frac{\partial \mathcal{L}}{\partial y}=-\frac{\partial \mathcal{P}}{\partial y}⟹\frac{\partial \mathcal{P}}{\partial y}=-\frac{\partial \mathcal{L}}{\partial y}$

那么对上式的$m{\ddot{y}}_{}^{}=f-mg$可以写作如下公式
$$\frac{\mathrm{d}}{\mathrm{d}t}\frac{\partial \mathcal{L}}{\partial \dot{y}}=f-\left(-\frac{\partial \mathcal{L}}{\partial y}\right)⟹f=\frac{\mathrm{d}}{\mathrm{d}t}\frac{\partial \mathcal{L}}{\partial \dot{y}}-\frac{\partial \mathcal{L}}{\partial y}$$
至此，单自由度系统的拉格朗日方程就推导出来了，方程如下：
$$f=\frac{\mathrm{d}}{\mathrm{d}t}\frac{\partial \mathcal{L}}{\partial \dot{y}}-\frac{\partial \mathcal{L}}{\partial y}$$

下面是建立拉格朗日函数的一般步骤
首先写出系统的动能和势能，并以广义坐标 $\left(q_1,\cdots ,q_n\right)$ 的形式来表示，其中 $n$ 是系统的自由度数目；然后，根据下述公式来计算 $n^{-}$自由度系统的运动方程
$$\frac{\mathrm{d}}{\mathrm{d}t}\frac{\partial \mathcal{L}}{\partial {\dot{q}}_k}-\frac{\partial \mathcal{L}}{\partial q_k}={\tau }_k,k=1,\cdots ,n$$
其中，${\tau }_k$ 是与广义坐标 $q_k$ 相关的广义力。
在上述单自由度系统的例子中，变量 $y$ 作为广义坐标。欧拉-拉格朗日方程不仅可以导出一组耦合的二阶常微分方程，它还提供了一种 等同于通过牛顿第二定律得到动力学方程的构造方法。然而，正如我们将要看到的那样，对于诸如多连杆机器人等复杂系统，使用拉格朗日方法更为有利。

下面介绍单连杆机械臂系统和双连杆机械臂系统的拉格朗日方程

2. 单连杆机械臂系统

如下图中所示的单连杆机器人，它包括一个刚性连杆，该连杆通过齿轮系连接到直流电机。令${\theta }_l$和${\theta }_m$分别表示连杆和电机轴的转动角度。那么${\theta }_m=r{\theta }_l$，其中$r:1$为齿轮变速比。如果连杆一端直接接在电机的旋转轴上，那么$r=1$。连杆转角和电机轴转角之间的代数关系表明该系统只有一个自由度，因此我们可以将${\theta }_l$或${\theta }_m$作为广义坐标。

单连杆机器人：电机输出轴通过齿轮系耦连到连杆的转动轴，齿轮系放大了电机扭矩并降低了电机转速。

在此选用${\theta }_{\ell }$作为广义坐标
系统的动能可以表示为${\theta }_{\ell }$的函数，其表示如下所示
$$\mathcal{K}=\frac{1}{2}{J}_m{\dot{\theta }}_m^2+\frac{1}{2}{J}_{\ell }{\dot{\theta }}_{\ell }^2=\frac{1}{2}{J}_m{r}^2{\dot{\theta }}_{\ell }^2+\frac{1}{2}{J}_{\ell }{\dot{\theta }}_{\ell }^2=\frac{1}{2}\left(r^2{J}_m+J_{\ell }\right){\dot{\theta }}_{\ell }^2$$
其中，$J_m,J_{\ell }$分别为电机和连杆的转动惯量。系统的势能如下所示
$$\mathcal{P}=Mg\ell \left(1-\cos {\theta }_{\ell }\right)$$
其中，$M$ 是连杆的总体质量，$\ell$ 是关节轴线与连杆质心之间的距离。定义 $J=r^2{J}_m+J_{\ell }$，拉格朗日算子$\mathcal{L}$如下
$$\mathcal{L}=\frac{1}{2}J{\dot{\theta }}_{\ell }^2-Mg\ell \left(1-\cos {\theta }_{\ell }\right)$$
将上述表达式代人到公式$\frac{\mathrm{d}}{\mathrm{d}t}\frac{\partial \mathcal{L}}{\partial {\dot{q}}_k}-\frac{\partial \mathcal{L}}{\partial q_k}={\tau }_k,$中，其中 $n=1$，广义坐标为 ${\theta }_{\ell }$，得到下列运动方程
$$J\ddot{\theta }{}_{\ell }+Mg\ell \sin {\theta }_{\ell }={\tau }_{\ell }$$
广义力 ${\tau }_{\ell }$ 表示那些无法从势函数推导出的外力和外力矩。对于这个例子，${\tau }_{\ell }$ 包括反映 到连杆上的电机输入力矩 $u=r{\tau }_m$，以及 (非保守) 阻尼力矩 $B_m{\dot{\theta }}_m$ 和 $B_{\ell }{\dot{\theta }}_1$ 。将电机阻尼反映到连杆上，得出
$${\tau }_{\ell }=u-B{\dot{\theta }}_{\ell }⟹u={\tau }_{\ell }+B{\dot{\theta }}_{\ell }$$
其中，$B=r{B}_m+B_{\ell }$ 。因此，该系统完整的动力学表达式为
$$u=J{\ddot{\theta }}_{\ell }+B{\dot{\theta }}_{\ell }+Mg\ell \sin {\theta }_{\ell }$$

3. 双连杆机械臂系统

下面推导双连杆机械臂系统的动能和位能，如下图。这种运动机构具有开式运动链，与复摆运动有许多相似之处。

图中
$T_1$ 和 $T_2$ 为转矩；

$m_1$ 和 $m_2$为连杆1和连杆2的质量，该连杆的质量以连杆末端的质点来进行表示；
$d_1$ 和 $d_2$分别为两连杆的长度；
${\theta }_1$ 和 ${\theta }_2$为广义坐标；
$g$为重力加速度。

先计算连杆1的动能$K_1$和动能$P_1$

K 1 = 1 2 m 1 v 1 2 v 1 = d 1 θ ˙ 1 P 1 = m 1 g h h 1 = − d 1 cos ⁡ θ 1

$$\begin{aligned} {K_1}&=\frac{1}{2} m_{1} v_{1}^{2} \\ v_{1}&=d_{1} \dot{\theta}_{1} \\ {P_1}&=m_1gh \\ h_1&=-d_{1} \cos \theta_{1} \end{aligned}$$ K1​v1​P1​h1​​=21​m1​v12​=d1​θ˙1​=m1​gh=−d1​cosθ1​​
推导得
$$\begin{array}{rl}{K}_1& =\frac{1}{2}{m}_1{d}_1^2{\dot{\theta }}_1^2\\ P_1& =-m_{1}g{d}_1\cos {\theta }_1\end{array}$$
再求连杆 2 的动能 $K_2$ 和位能 $P_2$ 。
$$\begin{array}{rl}{K}_2& =\frac{1}{2}{m}_2{v}_2^2\\ P_2& =mg{y}_2\end{array}$$式中
$$\begin{array}{rl}{v}_2^2& ={\dot{x}}_2^2+{\dot{y}}_2^2\\ x_2& =d_1\sin {\theta }_1+d_2\sin \left({\theta }_1+{\theta }_2\right)\\ y_2& =-d_1\cos {\theta }_1-d_2\cos \left({\theta }_1+{\theta }_2\right)\\ {\dot{x}}_2& =d_1\cos {\theta }_1{\dot{\theta }}_1+d_2\cos \left({\theta }_1+{\theta }_2\right)\left({\dot{\theta }}_1+{\dot{\theta }}_2\right)\\ {\dot{y}}_2& =d_1\sin {\theta }_1{\dot{\theta }}_1+d_2\sin \left({\theta }_1+{\theta }_2\right)\left({\dot{\theta }}_1+{\dot{\theta }}_2\right)\end{array}$$
于是可求得
$${\mathbf{v}}_2^2=d_1^2{\dot{\theta }}_1^2+d_2^2\left({\dot{\theta }}_1^2+2{\dot{\theta }}_1{\dot{\theta }}_2+{\dot{\theta }}_2^2\right)+2d_1{d}_2\cos {\theta }_2\left({\dot{\theta }}_1^2+{\dot{\theta }}_1{\dot{\theta }}_2\right)$$
以及
$$\begin{array}{cc}{K}_2& =\frac{1}{2}{m}_2{d}_1^2{\dot{\theta }}_1^2+\frac{1}{2}{m}_2{d}_2^2{\left({\dot{\theta }}_1+{\dot{\theta }}_2\right)}^2+m_2{d}_1{d}_2\cos {\theta }_2\left({\dot{\theta }}_1^2+{\dot{\theta }}_1{\dot{\theta }}_2\right)\\ P_2& =-m_{2}g{d}_1\cos {\theta }_1-m_{2}g{d}_2\cos \left({\theta }_1+{\theta }_2\right)\end{array}$$
这样, 二连杆机械手系统的总动能和总位能分别为
$$\begin{array}{rl}K& =K_1+K_2\\ & =\frac{1}{2}\left(m_1+m_2\right)d_1^2{\dot{\theta }}_1^2+\frac{1}{2}{m}_2{d}_2^2{\left({\dot{\theta }}_1+{\dot{\theta }}_2\right)}^2+m_2{d}_1{d}_2\cos {\theta }_2\left({\dot{\theta }}_1^2+{\dot{\theta }}_1{\dot{\theta }}_2\right)\\ P& =P_1+P_2\\ & =-\left(m_1+m_2\right)g{d}_1\cos {\theta }_1-m_{2}g{d}_2\cos \left({\theta }_1+{\theta }_2\right)\end{array}$$

二、MATLAB实例推导

1. 机器人模型的建立

1. 根据文章上述的双连杆机械臂对其进行建模，具体建模方法可参阅以下文章：

• 定义标准型及改进型D-H参数建立机器人模型（附MATLAB建模代码）
• 【Matlab 六自由度机器人】关于改进型D-H参数(modified Denavit-Hartenberg)的详细建立步骤

单杆机械臂系统代码如下：

%% 单杆机械臂系统
clc
clear
close all
warning off

%% MOD-DH参数
d1 = 0;
d2 = 100;
a1 = 0;
a2 = 0;
alpha1 = pi/2;
alpha2 = 0;
%       theta  d    a    alpha  offset(关节变量偏移量)
L(1)=Link([0   d1   a1   alpha1   0   ],'modified');
L(2)=Link([0   d2   a2   alpha2   0   ],'modified');
Single_Robot = SerialLink(L,'name','SingleRobot');
Single_Robot.teach()
%限制机器人的关节空间
theta1min = -180;        theta1max = 180;
theta2min = -180;        theta2max = 180;
L(1).qlim = [theta1min theta1max]*pi/180;
L(2).qlim = [theta2min theta2max]*pi/180;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

双连杆机械臂系统代码如下：

%% 双连杆机械臂系统
clc
clear
close all
warning off

%% MOD-DH参数
d1 = 0;
d2 = 0;
d3 = 0;
a1 = 0;
a2 = 100;
a3 = 100;
alpha1 = pi/2;
alpha2 = 0;
alpha3 = 0;
%       theta  d    a    alpha  offset(关节变量偏移量)
L(1)=Link([0   d1   a1   alpha1   0   ],'modified');
L(1).offset = -pi/2;
L(2)=Link([0   d2   a2   alpha2   0   ],'modified');
L(3)=Link([0   d3   a3   alpha3   0   ],'modified');
Double_Robot = SerialLink(L,'name','SingleRobot');
Double_Robot.display()
Double_Robot.teach()
%限制机器人的关节空间
theta1min = -180;        theta1max = 180;
theta2min = -180;        theta2max = 180;
theta3min = -180;        theta3max = 180;
L(1).qlim = [theta1min theta1max]*pi/180;
L(2).qlim = [theta2min theta2max]*pi/180;
L(3).qlim = [theta3min theta3max]*pi/180;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31

接下来对其进行运动学上的轨迹规划：

1. 对于单杆系统，其轨迹规划如下：

n = 1:100;
q0 = [0 0];
q1 = [30 30];

% 由q0移动到q1
[q,qd,qdd] = jtraj(q0,q1,n);

figure(2)
subplot(3,1,1)
plot(n,q)
subplot(3,1,2)
plot(n,qd)
subplot(3,1,3)
plot(n,qdd)
1
2
3
4
5
6
7
8
9
10
11
12
13
14

其效果如下图：

2. 对于双连杆系统

n = 1:100;
q0 = [0 0 0];
q1 = [30 60 90];

[q,qd,qdd] = jtraj(q0,q1,n);

figure(2)
subplot(3,1,1)
plot(n,q)
subplot(3,1,2)
plot(n,qd)
subplot(3,1,3)
plot(n,qdd)
1
2
3
4
5
6
7
8
9
10
11
12
13

其效果如下图：

往后会对关节空间轨迹规划$jtraj()函数$ 和 笛卡尔空间轨迹规划$ctraj()函数$ 进行单独篇章的撰写和探讨。

至此，运动学的前期准备已经完毕，下面进行机器人动力学 拉格朗日方程的推导。

2. 动力学代码

使用robot.dyn()函数查看建立的机器人的动力学参数

代码如下：

% 查看robot机器人所有的连杆的动力学参数
robot.dyn;
% 查看robot机器人第n根连杆的动力学参数
robot.dyn(n);

% 对Single_Robot进行动力学参数的设置

1
2
3
4
5
6
7

---

总结

本篇对机器人动力学进行一个概述。
之前谈到的运动学方程仅描述了机器人的运动过程，没有考虑到产生运动的力和扭矩，而动力学方程能描述力和运动之间的关系，因此我们在此引入动力学的概念。
第一章是机器人动力学之推导拉格朗日方程的内容，本文详细介绍了如何理解拉格朗日方程以及如何进行推导，介绍了如何求出机构的动能及位能。
第二章是拉格朗日方程代码的实现。

参考文献

1. 机器人学、机器视觉与控制：MATLAB算法基础
   
2. 机器人学
3. 机器人建模和控制
4. MATLAB机器人工具箱（四）动力学
5. MATLAB机器人工具箱【3】—— 动力学相关函数及用法