平面三自由度机器人动力学建模与仿真

机器臂建模
- 1、拉格朗日方程
- 2、动力学模型
控制器设计
机器臂轨迹规划
机器臂仿真
仿真结果

网上二自由度机器臂动力学分析有很多，三自由度比较少，碰巧本科课设需要完成相关项目，分享一些经验供参考。

机器臂建模

实际的三连杆机器臂的结构相对较复杂，很难进行精确地描述，因此，在本文中利用简化的数学模型进行讨论。简化条件如下：

假设机器臂是刚性结构，不考虑形变；
忽略各构件之间的摩擦力；
假设机器臂的连杆是密度均匀的刚体。

平面三连杆机器臂模型结构如图1所示， mim_imi 为第 iii 杆的质量， LiL_iLi 为 iii 杆的长度，LciL_{ci}Lci 为 iii 杆的质心到前一关节的长度，JiJ_iJi 为第 iii 杆的转动惯量。

对于平面三连杆机器臂而言， q1q_1q1 、 q2q_2q2 和 q3q_3q3 唯一地确定了其位置， q1q_1q1 、 q2q_2q2 和 q3q_3q3 的一阶导数和二阶导数确定了其运动状态，它们是动力学描述所需的基础变量。

1、拉格朗日方程

平面三连杆机器臂的拉格朗日算子为动能和势能之差，即：

L(q,q˙)=T(q,q˙)−U(q)L(q,\dot{q})=T(q,\dot{q})-U(q) L(q,q˙)=T(q,q˙)−U(q)
其中， T(q,q˙)T(q,\dot{q})T(q,q˙) 和 U(q)U(q)U(q) 分别是机器臂的动能和势能。

基于上面的拉格朗日算子，可得到平面三连杆机器臂的拉格朗日动力学方程为：
ddt∂∂q˙iL(q,q˙)−∂∂qiL(q,q˙)=τi\frac{d}{{dt}}\frac{\partial }{{\partial {{\dot q}_i}}}L(q,\dot q) - \frac{\partial }{{\partial {q_i}}}L(q,\dot q) = {\tau _i} dtd∂q˙i∂L(q,q˙)−∂qi∂L(q,q˙)=τi
其中， τi\tau _iτi 是作用在第 iii 杆的驱动力矩。进一步化简拉格朗日方程可以进一步表达为：
ddt∂∂q˙iT(q,q˙)−∂∂qiT(q,q˙)+∂∂qiU(q)=τi\frac{d}{{dt}}\frac{\partial }{{\partial {{\dot q}_i}}}T(q,\dot q) - \frac{\partial }{{\partial {q_i}}}T(q,\dot q) + \frac{\partial }{{\partial {q_i}}}U(q) = {\tau _i} dtd∂q˙i∂T(q,q˙)−∂qi∂T(q,q˙)+∂qi∂U(q)=τi

2、动力学模型

将拉格朗日方程写成动力学方程的表达方式：
M(q)q¨+C(q,q˙)+G(q)=τM(q)\ddot q + C(q,\dot q) + G(q) = \tau M(q)q¨+C(q,q˙)+G(q)=τ
其中， τ=[τ1τ2τ3]T\tau = { {\begin{bmatrix}{} {{\tau _1}}&{{\tau _2}}&{{\tau _3}} \end{bmatrix}} ^T}τ=[τ1τ2τ3]T 是控制力矩向量， q¨=[q¨1q¨2q¨3]T\ddot q = {\begin{bmatrix}{} {{\ddot q_1}}&{{\ddot q_2}}&{{\ddot q_3}} \end{bmatrix}^T}q¨=[q¨1q¨2q¨3]T 是系统的角加速度向量，M(q)∈R3×3M(q) \in R ^{3 \times 3}M(q)∈R3×3 是惯性矩阵，具有正定性和对称性， C(q,q˙)∈R3×1C(q,\dot q) \in R ^{3 \times 1}C(q,q˙)∈R3×1 是哥式力和离心力的结合向量，G(q)∈R3×1G(q) \in R ^{3 \times 1}G(q)∈R3×1 是重力矩阵。它们的具体形式如下：
M(q)=[M11M12M13M21M22M23M31M32M33]M(q) = {\begin{bmatrix}{} {{M_{11}}}&{{M_{12}}}&{{M_{13}}}\\ {{M_{21}}}&{{M_{22}}}&{{M_{23}}}\\ {{M_{31}}}&{{M_{32}}}&{{M_{33}}} \end{bmatrix}} M(q)=⎣⎡M11M21M31M12M22M32M13M23M33⎦⎤

C(q,q˙)=[C1C2C3]C(q,\dot q) = {\begin{bmatrix}{} {{C_1}}\\ {{C_2}}\\ {{C_3}} \end{bmatrix}} C(q,q˙)=⎣⎡C1C2C3⎦⎤

G(q)=[G1G2G3]G(q) = {\begin{bmatrix}{} {{G_1}}\\ {{G_2}}\\ {{G_3}} \end{bmatrix}} G(q)=⎣⎡G1G2G3⎦⎤

其中， M(q)M(q)M(q) 中的系数如下：
{M11=a1+a2+a4+2a3cos⁡q2+2a5cos⁡(q2+q3)+2a6cos⁡q3,M12=a2+a4+a3cos⁡q2+cos⁡(q2+q5)+2a6cos⁡q3,M13=a4+a5cos⁡(q2+q3)+a6cos⁡q3,M21=M12,M22=a2+a4+2a6cos⁡q3,M23=a4+a6cos⁡q3,M31=M13,M32=M23,M33=a4.\left\{ \begin{array}{l} {M_{11}} = {a_1} + {a_2} + {a_4} + 2{a_3}\cos {q_2} + 2{a_5}\cos ({q_2} + {q_3}) + 2{a_6}\cos {q_3},\\ {M_{12}} = {a_2} + {a_4} + {a_3}\cos {q_2} + \cos ({q_2} + {q_5}) + 2{a_6}\cos {q_3},\\ {M_{13}} = {a_4} + {a_5}\cos ({q_2} + {q_3}) + {a_6}\cos {q_3},\\ {M_{21}} = {M_{12}},\\ {M_{22}} = {a_2} + {a_4} + 2{a_6}\cos {q_3},\\ {M_{23}} = {a_4} + {a_6}\cos {q_3},\\ {M_{31}} = {M_{13}},\\ {M_{32}} = {M_{23}},\\ {M_{33}} = {a_4}. \end{array} \right. ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧M11=a1+a2+a4+2a3cosq2+2a5cos(q2+q3)+2a6cosq3,M12=a2+a4+a3cosq2+cos(q2+q5)+2a6cosq3,M13=a4+a5cos(q2+q3)+a6cosq3,M21=M12,M22=a2+a4+2a6cosq3,M23=a4+a6cosq3,M31=M13,M32=M23,M33=a4.
其中： ai(i=1,2,...,6){a_i}(i = 1,2,...,6)ai(i=1,2,...,6) 是系统的结构参数，有
{a1=m1Lc12+J1+(m2+m3)L12,a2=J2+m2Lc22+m3L22,a3=(m2Lc2+m3L2)L1,a4=J3+m3Lc32,a5=m3Lc3L1,a6=m3Lc3L2.\left\{ \begin{array}{l} {a_1} = {m_1}L_{c1}^2 + {J_1} + ({m_2} + {m_3})L_1^2,\\ {a_2} = {J_2} + {m_2}L_{c2}^2 + {m_3}L_2^2,\\ {a_3} = ({m_2}{L_{c2}} + {m_3}{L_2}){L_1},\\ {a_4} = {J_3} + {m_3}L_{c3}^2,\\ {a_5} = {m_3}{L_{c3}}{L_1},\\ {a_6} = {m_3}{L_{c3}}{L_2}. \end{array} \right. ⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧a1=m1Lc12+J1+(m2+m3)L12,a2=J2+m2Lc22+m3L22,a3=(m2Lc2+m3L2)L1,a4=J3+m3Lc32,a5=m3Lc3L1,a6=m3Lc3L2.
其中， C(q,q˙)C(q,\dot q)C(q,q˙) 中的系数如下：
{C1=−a3(2q˙1+q˙2)q˙2sin⁡q2−a5(2q˙1+q˙2+q˙3)(q˙2+q˙3)sin⁡(q2+q3)−a6(2q˙1+2q˙2+q˙3)q˙3sin⁡q3,C2=a3q˙12sin⁡q2+a5q˙12sin⁡(q2+q3)−a6(2q˙1+2q˙2+q˙3)q˙3sin⁡q3,C3=a5q˙12sin⁡(q2+q3)+a6(q˙1+q˙2)2sin⁡q3.\left\{ \begin{array}{l} {C_1} = - {a_3}(2{{\dot q}_1} + {{\dot q}_2}){{\dot q}_2}\sin {q_2} - {a_5}(2{{\dot q}_1} + {{\dot q}_2} + {{\dot q}_3})({{\dot q}_2} + {{\dot q}_3})\sin ({q_2} + {q_3}) - \\ \;\;\;\;\;\;\;\;\;{a_6}(2{{\dot q}_1} + 2{{\dot q}_2} + {{\dot q}_3}){{\dot q}_3}\sin {q_3},\\ {C_2} = {a_3}\dot q_1^2\sin {q_2} + {a_5}\dot q_1^2\sin ({q_2} + {q_3}) - {a_6}(2{{\dot q}_1} + 2{{\dot q}_2} + {{\dot q}_3}){{\dot q}_3}\sin {q_3},\\ {C_3} = {a_5}\dot q_1^2\sin ({q_2} + {q_3}) + {a_6}{({{\dot q}_1} + {{\dot q}_2})^2}\sin {q_3}. \end{array} \right. ⎩⎪⎪⎨⎪⎪⎧C1=−a3(2q˙1+q˙2)q˙2sinq2−a5(2q˙1+q˙2+q˙3)(q˙2+q˙3)sin(q2+q3)−a6(2q˙1+2q˙2+q˙3)q˙3sinq3,C2=a3q˙12sinq2+a5q˙12sin(q2+q3)−a6(2q˙1+2q˙2+q˙3)q˙3sinq3,C3=a5q˙12sin(q2+q3)+a6(q˙1+q˙2)2sinq3.
其中， G(q)G(q)G(q) 中的系数如下：
{G1=m3g[L1cos⁡q1+L2cos⁡(q1+q2)+Lc3cos⁡(q1+q2+q3)]+m2g[L1cos⁡q1+Lc2cos⁡(q1+q2)]+m1g[Lc1cos⁡q1],G2=m3g[L2cos⁡(q1+q2)+Lc3cos⁡(q1+q2+q3)]+m2g[Lc2cos⁡(q1+q2)],G3=m3g[Lc3cos⁡(q1+q2+q3)].\left\{ \begin{array}{l} {G_1} = {m_3}g[{L_1}\cos {q_1} + {L_2}\cos ({q_1} + {q_2}) + {L_{c3}}\cos ({q_1} + {q_2} + {q_3})] + \\ \;\;\;\;\;\;\;\;\;{m_2}g[{L_1}\cos {q_1} + {L_{c2}}\cos ({q_1} + {q_2})] + {m_1}g[{L_{c1}}\cos {q_1}],\\ {G_2} = {m_3}g[{L_2}\cos ({q_1} + {q_2}) + {L_{c3}}\cos ({q_1} + {q_2} + {q_3})] + \\ \;\;\;\;\;\;\;\;\;{m_2}g[{L_{c2}}\cos ({q_1} + {q_2})],\\ {G_3} = {m_3}g[{L_{c3}}\cos ({q_1} + {q_2} + {q_3})]. \end{array} \right. ⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧G1=m3g[L1cosq1+L2cos(q1+q2)+Lc3cos(q1+q2+q3)]+m2g[L1cosq1+Lc2cos(q1+q2)]+m1g[Lc1cosq1],G2=m3g[L2cos(q1+q2)+Lc3cos(q1+q2+q3)]+m2g[Lc2cos(q1+q2)],G3=m3g[Lc3cos(q1+q2+q3)].
至此，平面三连杆机器臂的动力学模型建立完成，动力学方程的形式体现出机器臂系统是一个具有强耦合和非线性的复杂系统，系统在运动过程中，各个关节相互影响。基于拉格朗日方程得到的机器臂动力学模型的建立起了机器臂关节驱动力矩与机器臂运动参数和机械参数之间的关系，为机器臂系统PD控制算法提供了基础。

控制器设计

对于自由运动机器人来说，其控制器设计可以按是否考虑机器人的动力学特性分两类：一类是完全不考虑机器人的动力学特性，只是按照机器人实际轨迹与期望轨迹间的偏差进行负反馈控制；另一类控制器设计方法被称为动态控制,这类方法是根据机器人动力学模型的性质设计出更精细的非线性控制律，所以又常称为以模型为基础的控制。

本文采用的是动态控制中的PD控制律，机器人的动力学方程为：
M(q)q¨+C(q,q˙)+G(q)=τM(q)\ddot q + C(q,\dot q) + G(q) = \tau M(q)q¨+C(q,q˙)+G(q)=τ
当给定连杆参数时，可以计算出 M(q)M(q)M(q) 、 C(q,q˙)C(q,\dot q)C(q,q˙) 、G(q)G(q)G(q) 的估计值 M^(q)\hat M(q)M^(q) 、 C^(q,q˙)\hat C(q,\dot q)C^(q,q˙) 、 G^(q)\hat G(q)G^(q) 。以 u=q¨u = \ddot qu=q¨ 作为新的输入量来考虑下列非线性反馈控制律：
τ=M^(q)u+C^(q,q˙)+G^(q)\tau = \hat M(q)u + \hat C(q,\dot q) + \hat G(q) τ=M^(q)u+C^(q,q˙)+G^(q)
如果估计值全为正值，目标轨迹的关节期望加速度为 q¨d{\ddot q_d}q¨d ，并取 u=q¨u = \ddot qu=q¨ ，于是可以完全实现目标轨迹的跟踪任务，这种考虑方法称为转矩计算法。但是，通常机器人的物理参数的估计值不可能完全正确，所以本文采用下图所示的PD反馈控制律：

这时，如果考虑用下式表示轨迹误差
e=qd−qe = {q_d} - q e=qd−q
可以得出
e¨+KVe˙+KPe=0\ddot e + {K_V}\dot e + {K_P}e = 0 e¨+KVe˙+KPe=0
对于该式，通过适当地设定 KpK_pKp 和 KvK_vKv 就有可能使轨迹误差收敛到0。

机器臂轨迹规划

关节空间轨迹规划的基本方法是通过机器臂逆运动学，将机器臂末端操作手中心点所要经过的空间位置转换成各个关节对应的关节角度。本文采用的方法是，利用逆运动学计算出各个空间位置所对应的关节角度，对每两个位置的关节角度之间进行多项式插值，然后各个关节按照所规划的角度变化曲线进行动。

基于三次多项式插值法的轨迹规划算法不能保证各个点之间速度曲线的平滑过渡，这样的速度和加速度曲线在机器臂应用过程中产生一些不利影响，为获得在各个点之间过渡平滑的速度曲线，继续增加初始和终止的加速度边界条件，此时需要采用五次多项式对运行轨迹进行规划：
q(t)=q0+b1(t−t0)+b2(t−t0)2+b3(t−t0)3+b4(t−t0)4+b5(t−t0)5\begin{array}{l} q(t) = {q_0} + {b_1}(t - {t_0}) + {b_2}{(t - {t_0})^2} + {b_3}{(t - {t_0})^3} + {b_4}{(t - {t_0})^4} + {b_5}{(t - {t_0})^5} \end{array} q(t)=q0+b1(t−t0)+b2(t−t0)2+b3(t−t0)3+b4(t−t0)4+b5(t−t0)5
根据条件有：
{q(t0)=q0q(t1)=q1q˙(t0)=v0q˙(t1)=v1q¨(t0)=a0q¨(t1)=a1\left\{ \begin{array}{l} q({t_0}) = {q_0}\;\;\;q({t_1}) = {q_1}\\ \dot q({t_0}) = {v_0}\;\;\;\dot q({t_1}) = {v_1}\\ \ddot q({t_0}) = {a_0}\;\;\;\ddot q({t_1}) = {a_1} \end{array} \right. ⎩⎨⎧q(t0)=q0q(t1)=q1q˙(t0)=v0q˙(t1)=v1q¨(t0)=a0q¨(t1)=a1
令h=q1−q0τ=t1−t0h = {q_1} - {q_0}\;\;\;\tau = {t_1} - {t_0}h=q1−q0τ=t1−t0 ，求的多项式系数为：
{b0=q0,b1=v0,b2=12a0,b3=20h−(8v1+12v0)τ−(3a0−a1)τ22τ3,b4=−30h+(14v1+16v0)τ+(3a0−2a1)τ22τ4,b5=12h−(6v1+6v0)τ+(−a0+a1)τ22τ5.\left\{ \begin{array}{l} {b_0} = {q_0},\\ {b_1} = {v_0},\\ {b_2} = \frac{1}{2}{a_0},\\ {b_3} = \frac{{20h - (8{v_1} + 12{v_0})\tau - (3{a_0} - {a_1}){\tau ^2}}}{{2{\tau ^3}}},\\ {b_4} = \frac{{ - 30h + (14{v_1} + 16{v_0})\tau + (3{a_0} - 2{a_1}){\tau ^2}}}{{2{\tau ^4}}},\\ {b_5} = \frac{{12h - (6{v_1} + 6{v_0})\tau + ( - {a_0} + {a_1}){\tau ^2}}}{{2{\tau ^5}}}. \end{array} \right. ⎩⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎧b0=q0,b1=v0,b2=21a0,b3=2τ320h−(8v1+12v0)τ−(3a0−a1)τ2,b4=2τ4−30h+(14v1+16v0)τ+(3a0−2a1)τ2,b5=2τ512h−(6v1+6v0)τ+(−a0+a1)τ2.

机器臂仿真

在 MATLAB 中建立三连杆系统模型。由于考虑机器人系统的重力模型引入了非线性项目，故采用 Simscape 建立仿真模型如下图所示。

建立起来的模型示意图如下（这个是robot toolbox工具箱生成，并非Simscape生成）：

以第一个连杆为例，建立第一个连杆的力矩估计，

对应方框图中红色框中的模型如下：

H1代码，这部分就是之前求得的动力学方程

function y = fcn(Gddq1,Gddq2,Gddq3,q2, q3, a1, a2, a3, a4, a5, a6)
h11 = a1+a2+a4+2*a3*cos(q2)+2*a5*cos(q2+q3)+2*a6*cos(q3);
h21 = a2+a4+a3*cos(q2)+a5*cos(q2+q3)+2*a6*cos(q3);
h31 = a4+a5*cos(q2+q3)+a6*cos(q3);
y = Gddq1*h11+Gddq2*h21+Gddq3*h31;

CG1代码

function y = fcn(q1, dq1, q2, dq2, q3, dq3, a1, a2, a3, a4, a5, a6, L1, Lc1, L2, Lc2, Lc3, m1, m2, m3)
C1 = -a3*(2*dq1+dq2)*dq2*sin(q2) - a5*(2*dq1+dq2+dq3)*(dq2+dq3)*sin(q2+q3) ...-a6*(2*dq1+2*dq2+dq3)*dq3*sin(q3);
G1 = m3*9.8*(L1*cos(q1)+L2*cos(q1+q2)+Lc3*cos(q1+q2+q3)) + m2*9.8*(L1*cos(q1)+Lc2*cos(q1+q2)) ...+m1*9.8*Lc1*cos(q1);
y = C1+G1;

将上面的封装，如下

第一个关节的全部仿真模型如下，其中左边为PD控制器，上面为使用 Simscape 搭建模拟机器臂

仿真结果

将三个关节都搭建起来（方法同第一个关节），大致如下：

示波器输出每个关节角度跟踪情况

加了一些场景布置，三层隔板，机器臂对一和三层进行访问（假装），仿真结果动态图

上图仿真的下载链接如下（需要安装robot toolbox，压缩包内已包括安装包和安装教程）：

https://download.csdn.net/download/qq_35231630/74358490

稍作改进就可以实现如下效果（这个就不放链接了）

未经本文允许不得转载。