平面2R机器人(二连杆)运动学与动力学建模+附仿真模型

Mr_Winter
475 阅读3分钟

一起养成写作习惯！这是我参与「掘金日新计划 · 4 月更文挑战」的第15天，点击查看活动详情。

1 平面2R机器人概述

如图所示为本文的研究本体——平面2R机器人。对参数进行如下定义：机器人广义坐标θ1\theta _1为连杆1与yy轴负半轴的夹角，逆时针为正；θ2\theta _2为连杆2与连杆1延长线的夹角，逆时针为正；连杆1、2的关节力矩分别定义为τ1\tau_1、τ2\tau_2。设连杆质量均匀分布，长度都为ll，质量都为MM，末端执行器笛卡尔坐标为P(x,y)P(x,y)。

在这里插入图片描述

2 运动学建模

2.1 正运动学模型

指数积方法的核心原理是基于运动旋量的==指数积公式==： TAT(θ)=e[V1]θ1e[V2]θ2⋯e[Vn]θnTAT(0)_{\boldsymbol{T}}^{\boldsymbol{A}}\boldsymbol{T}\left( \boldsymbol{\theta } \right) =e^{\left[ \boldsymbol{V}_1 \right] \theta _1}e^{\left[ \boldsymbol{V}_2 \right] \theta _2}\cdots {e^{\left[ \boldsymbol{V}_n \right] \theta _n}}_{\boldsymbol{T}}^{\boldsymbol{A}}\boldsymbol{T}\left( 0 \right)

可以证明，连杆变换之积(DH法)等价于指数积。对平面2R机器人，确定各个关节轴线的线矢量如图所示，不必建立连杆坐标系。

在这里插入图片描述

各轴线方向如下： ω1=ω2=[001],[ω1]=[ω2]=[0−10100000]\omega _1=\omega _2=\left[ \begin{array}{c} 0\\ 0\\ 1\\\end{array} \right] ,\mathrm{ }\left[ \omega _1 \right] =\left[ \omega _2 \right] =\left[ \begin{matrix} 0& -1& 0\\ 1& 0& 0\\ 0& 0& 0\\\end{matrix} \right]

取各个运动轴上的一点：

r1=[000],r2=[0−l0]r_1=\left[ \begin{array}{c} 0\\ 0\\ 0\\\end{array} \right] ,\mathrm{ }r_2=\left[ \begin{array}{c} 0\\ -l\\ 0\\\end{array} \right]

可得各个运动旋量的指数形式

e[V1]θ1=[cos⁡θ1−sin⁡θ100sin⁡θ1cos⁡θ10000100001],e[V2]θ2=[cos⁡θ2−sin⁡θ20−lsin⁡θ2sin⁡θ2cos⁡θ20−l(1−cos⁡θ2)00100001]e^{\left[ \boldsymbol{V}_1 \right] \theta _1}=\left[ \begin{matrix} \cos \theta _1& -\sin \theta _1& 0& 0\\ \sin \theta _1& \cos \theta _1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\\\end{matrix} \right] , e^{\left[ \boldsymbol{V}_2 \right] \theta _2}=\left[ \begin{matrix} \cos \theta _2& -\sin \theta _2& 0& -l\sin \theta _2\\ \sin \theta _2& \cos \theta _2& 0& -l\left( 1-\cos \theta _2 \right)\\ 0& 0& 1& 0\\ 0& 0& 0& 1\\\end{matrix} \right]

关节变量θ=[θ1θ2]\boldsymbol{\theta }=\left[ \begin{matrix} \theta _1& \theta _2\\\end{matrix} \right]为0时即为初始位姿：

TBT(0)=[1000010−2l00100001]_{T}^{B}\boldsymbol{T}\left( 0 \right) =\left[ \begin{matrix} 1& 0& 0& 0\\ 0& 1& 0& -2l\\ 0& 0& 1& 0\\ 0& 0& 0& 1\\\end{matrix} \right]

所以由_{T}^{B}\boldsymbol{T}\left( \boldsymbol{\theta } \right) =e^{\left[ \boldsymbol{V}_1 \right] \theta _1}e^{\left[ \boldsymbol{V}_2 \right] \theta _2}\!\:_{T}^{S}\boldsymbol{T}\left( 0 \right)即得

TBT(θ)=[cos⁡(θ1+θ2)−sin⁡(θ1+θ2)0lsin⁡(θ1+θ2)+lsin⁡θ1sin⁡(θ1+θ2)cos⁡(θ1+θ2)0−lcos⁡(θ1+θ2)−lcos⁡θ100100001]_{T}^{B}\boldsymbol{T}\left( \boldsymbol{\theta } \right) =\left[ \begin{matrix} \cos \left( \theta _1+\theta _2 \right)& -\sin \left( \theta _1+\theta _2 \right)& 0& l\sin \left( \theta _1+\theta _2 \right) +l\sin \theta _1\\ \sin \left( \theta _1+\theta _2 \right)& \cos \left( \theta _1+\theta _2 \right)& 0& -l\cos \left( \theta _1+\theta _2 \right) -l\cos \theta _1\\ 0& 0& 1& 0\\ 0& 0& 0& 1\\\end{matrix} \right]

可以验证，基于运动旋量法的机器人正运动学模型，与基于D-H法的正运动学模型相同，可交叉验证正运动学模型的理论正确性。

2.2 逆运动学模型

以图所示的象限为例，以几何方法进行机器人运动学反解。 在这里插入图片描述

对于位形一，在ΔOAB\varDelta OAB中运用余弦定理可得2l2cos⁡(π−θ2)=l2+l2−L22l^2\cos \left( \pi -\theta _2 \right) =l^2+l^2-L^2，其中L2=x2+y2L^2=x^2+y^2，显然末端执行器坐标需要满足0<x2+y2⩽2l0<\sqrt{x^2+y^2}\leqslant 2l

解得：

θ2=arccos⁡(x2+y22l2−1)\theta _2=\mathrm{arc}\cos \left( \frac{x^2+y^2}{2l^2}-1 \right)

有两个解，位形一对应正解，位形二对应负解。 同样在ΔOAB\varDelta OAB中运用余弦定理可得

ψ=arccos⁡(x2+y22l)\psi =\mathrm{arc}\cos \left( \frac{\sqrt{x^2+y^2}}{2l} \right)

为方便起见，这里总是取ψ\psi的正解。再根据

β=∣arctan⁡(yx)∣\beta =\left| \mathrm{arc}\tan \left( \frac{y}{x} \right) \right|

可得位形一下的关节参数θ1=π2−(β+ψ)\theta _1=\frac{\pi}{2}-\left( \beta +\psi \right)。当机械臂运动到其他象限时同理，求得平面2R机器人运动学反解为:

{θ1=π2−(β±ψ),θ2>0时取+θ1=π2+(β±ψ),θ2>0时取−θ1=3π2−(β±ψ),θ2>0时取+θ1=3π2+(β±ψ),θ2>0时取−\begin{cases} \theta _1=\frac{\pi}{2}-\left( \beta \pm \psi \right) , \theta _2>0\text{时取}+\\ \theta _1=\frac{\pi}{2}+\left( \beta \pm \psi \right) , \theta _2>0\text{时取}-\\ \theta _1=\frac{3\pi}{2}-\left( \beta \pm \psi \right) , \theta _2>0\text{时取}+\\ \theta _1=\frac{3\pi}{2}+\left( \beta \pm \psi \right) , \theta _2>0\text{时取}-\\\end{cases}

2.3 机器人运动学仿真

基于Matlab Robotics工具箱进行平面2R机器人的运动学仿真验证。验证过程如下：分别选取位于四个象限的关节参数

[θ1θ2]=[35°55°],[θ1θ2]=[95°25°],[θ1θ2]=[230°20°],[θ1θ2]=[300°70°]\left[ \begin{array}{c} \theta _1\\ \theta _2\\\end{array} \right] =\left[ \begin{array}{c} 35\degree\\ 55\degree\\\end{array} \right] , \left[ \begin{array}{c} \theta _1\\ \theta _2\\\end{array} \right] =\left[ \begin{array}{c} 95\degree\\ 25\degree\\\end{array} \right] , \left[ \begin{array}{c} \theta _1\\ \theta _2\\\end{array} \right] =\left[ \begin{array}{c} 230\degree\\ 20\degree\\\end{array} \right] , \left[ \begin{array}{c} \theta _1\\ \theta _2\\\end{array} \right] =\left[ \begin{array}{c} 300\degree\\ 70\degree\\\end{array} \right]

进行正运动的验证，而后基于四组末端执行器的坐标进行运动学反解，得到该坐标下的另一个位形对应的关节参数，将该参数代入Matlab机器人模型，验证此时末端执行器的坐标是否与位形一下的重合。将四组参数分别代入上式，得到

[xy]=[1.57−0.82],[xy]=[1.860.59],[xy]=[−1.710.99],[xy]=[−0.69−1.48]\left[ \begin{array}{c} x\\ y\\\end{array} \right] =\left[ \begin{array}{c} 1.57\\ -0.82\\\end{array} \right] , \left[ \begin{array}{c} x\\ y\\\end{array} \right] =\left[ \begin{array}{c} 1.86\\ 0.59\\\end{array} \right] , \left[ \begin{array}{c} x\\ y\\\end{array} \right] =\left[ \begin{array}{c} -1.71\\ 0.99\\\end{array} \right] , \left[ \begin{array}{c} x\\ y\\\end{array} \right] =\left[ \begin{array}{c} -0.69\\ -1.48\\\end{array} \right]

将这四组笛卡尔坐标值代入式，得到另一位形的关节参数值

[θ1θ2]=[90°−55°],[θ1θ2]=[120°−25°],[θ1θ2]=[249°−20°],[θ1θ2]=[10°−70°]\left[ \begin{array}{c} \theta _1\\ \theta _2\\\end{array} \right] =\left[ \begin{array}{c} 90\degree\\ -55\degree\\\end{array} \right] , \left[ \begin{array}{c} \theta _1\\ \theta _2\\\end{array} \right] =\left[ \begin{array}{c} 120\degree\\ -25\degree\\\end{array} \right] , \left[ \begin{array}{c} \theta _1\\ \theta _2\\\end{array} \right] =\left[ \begin{array}{c} 249\degree\\ -20\degree\\\end{array} \right] , \left[ \begin{array}{c} \theta _1\\ \theta _2\\\end{array} \right] =\left[ \begin{array}{c} 10\degree\\ -70\degree\\\end{array} \right]

在这里插入图片描述

3 动力学建模

3.1 计算动能

取定连杆上一质量微元dm\mathrm{d}m，其位置坐标为(x,y)(x,y)，采用极坐标表示即为

连杆I{x=rsin⁡θ1y=−rcos⁡θ1  连杆II{x=lsin⁡θ1+rsin⁡(θ1+θ2)y=−lcos⁡θ1−rcos⁡(θ1+θ2)\text{连杆I}\begin{cases} x=r\sin \theta _1\\ y=-r\cos \theta _1\\\end{cases}\,\, \text{连杆II}\begin{cases} x=l\sin \theta _1+r\sin \left( \theta _1+\theta _2 \right)\\ y=-l\cos \theta _1-r\cos \left( \theta _1+\theta _2 \right)\\\end{cases}

对时间求导，得到

连杆I{x˙=rθ˙1cos⁡θ1y˙=rθ˙1sin⁡θ1  连杆II{x˙=lθ˙1cos⁡θ1+r(θ˙1+θ˙2)cos⁡(θ1+θ2)y˙=lθ˙1sin⁡θ1+r(θ˙1+θ˙2)sin⁡(θ1+θ2)\text{连杆I}\begin{cases} \dot{x}=r\dot{\theta}_1\cos \theta _1\\ \dot{y}=r\dot{\theta}_1\sin \theta _1\\\end{cases}\,\, \text{连杆II}\begin{cases} \dot{x}=l\dot{\theta}_1\cos \theta _1+r\left( \dot{\theta}_1+\dot{\theta}_2 \right) \cos \left( \theta _1+\theta _2 \right)\\ \dot{y}=l\dot{\theta}_1\sin \theta _1+r\left( \dot{\theta}_1+\dot{\theta}_2 \right) \sin \left( \theta _1+\theta _2 \right)\\\end{cases}

所以质量微元速度为

{连杆I：v12=x˙2+y˙2=r2θ˙12连杆II：v22=x˙2+y˙2=l2θ˙12+r2(θ˙1+θ˙2)2+2lrθ˙1(θ˙1+θ˙2)cos⁡θ2\begin{cases} \text{连杆I：}v_{1}^{2}=\dot{x}^2+\dot{y}^2=r^2\dot{\theta}_{1}^{2}\\ \text{连杆II：}v_{2}^{2}=\dot{x}^2+\dot{y}^2=l^2\dot{\theta}_{1}^{2}+r^2\left( \dot{\theta}_1+\dot{\theta}_2 \right) ^2+2lr\dot{\theta}_1\left( \dot{\theta}_1+\dot{\theta}_2 \right) \cos \theta _2\\\end{cases}

因此机器人动能为

Ek=12∫mdm⋅v2=12ρ[∫0lr2θ˙12dr+∫0ll2θ˙12+r2(θ˙1+θ˙2)2+2lrθ˙1(θ˙1+θ˙2)cos⁡θ2dr]=12ml2[(53+cos⁡θ2)θ˙12+(23+cos⁡θ2)θ˙1θ˙2+13θ˙22]E_k=\frac{1}{2}\int_m{\mathrm{d}m\cdot v^2}\\=\frac{1}{2}\rho \left[ \int_0^l{r^2\dot{\theta}_{1}^{2}\mathrm{d}r}+\int_0^l{l^2\dot{\theta}_{1}^{2}+r^2\left( \dot{\theta}_1+\dot{\theta}_2 \right) ^2+2lr\dot{\theta}_1\left( \dot{\theta}_1+\dot{\theta}_2 \right) \cos \theta _2\mathrm{d}r} \right] \\=\frac{1}{2}ml^2\left[ \left( \frac{5}{3}+\cos \theta _2 \right) \dot{\theta}_{1}^{2}+\left( \frac{2}{3}+\cos \theta _2 \right) \dot{\theta}_1\dot{\theta}_2+\frac{1}{3}\dot{\theta}_{2}^{2} \right]

3.2 势能计算与动力学方程

以基坐标系xx轴为零势能面，则系统总势能为

Ep=−mgl[32cos⁡θ1+12cos⁡(θ1+θ2)]E_p=-mgl\left[ \frac{3}{2}\cos \theta _1+\frac{1}{2}\cos \left( \theta _1+\theta _2 \right) \right]

对于任何机械系统．拉格朗日函数定义为系统总动能和势能之差，即

L=Ek−Ep=12ml2[(53+cos⁡θ2)θ˙12+(23+cos⁡θ2)θ˙1θ˙2+13θ˙22]+mgl[32cos⁡θ1+12cos⁡(θ1+θ2)]L=E_k-E_p\\=\frac{1}{2}ml^2\left[ \left( \frac{5}{3}+\cos \theta _2 \right) \dot{\theta}_{1}^{2}+\left( \frac{2}{3}+\cos \theta _2 \right) \dot{\theta}_1\dot{\theta}_2+\frac{1}{3}\dot{\theta}_{2}^{2} \right] +mgl\left[ \frac{3}{2}\cos \theta _1+\frac{1}{2}\cos \left( \theta _1+\theta _2 \right) \right]

基于拉格朗日函数可得

{∂L∂θ1=−mgl[32sin⁡θ1+12sin⁡(θ1+θ2)]∂L∂θ˙1=12ml2[(103+2cos⁡θ2)θ˙1+(23+cos⁡θ2)θ˙2]ddt(∂L∂θ˙1)=12ml2[(103+2cos⁡θ2)θ¨1−2θ˙1θ˙2sin⁡θ2+(23+cos⁡θ2)θ¨2−θ˙22sin⁡θ2]\begin{cases} \frac{\partial L}{\partial \theta _1}=-mgl\left[ \frac{3}{2}\sin \theta _1+\frac{1}{2}\sin \left( \theta _1+\theta _2 \right) \right]\\ \frac{\partial L}{\partial \dot{\theta}_1}=\frac{1}{2}ml^2\left[ \left( \frac{10}{3}+2\cos \theta _2 \right) \dot{\theta}_1+\left( \frac{2}{3}+\cos \theta _2 \right) \dot{\theta}_2 \right]\\ \frac{\mathrm{d}}{\mathrm{d}t}\left( \frac{\partial L}{\partial \dot{\theta}_1} \right) =\frac{1}{2}ml^2\left[ \left( \frac{10}{3}+2\cos \theta _2 \right) \ddot{\theta}_1-2\dot{\theta}_1\dot{\theta}_2\sin \theta _2+\left( \frac{2}{3}+\cos \theta _2 \right) \ddot{\theta}_2-\dot{\theta}_{2}^{2}\sin \theta _2 \right]\\\end{cases}
{∂L∂θ2=−12mglsin⁡(θ1+θ2)−12ml2(θ˙12+θ˙1θ˙2)sin⁡θ2∂L∂θ˙2=12ml2[(23+cos⁡θ2)θ˙1+23θ˙2]ddt(∂L∂θ˙2)=12ml2[(23+cos⁡θ2)θ¨1−θ˙1θ˙2sin⁡θ2+23θ¨2]\begin{cases} \frac{\partial L}{\partial \theta _2}=-\frac{1}{2}mgl\sin \left( \theta _1+\theta _2 \right) -\frac{1}{2}ml^2\left( \dot{\theta}_{1}^{2}+\dot{\theta}_1\dot{\theta}_2 \right) \sin \theta _2\\ \frac{\partial L}{\partial \dot{\theta}_2}=\frac{1}{2}ml^2\left[ \left( \frac{2}{3}+\cos \theta _2 \right) \dot{\theta}_1+\frac{2}{3}\dot{\theta}_2 \right]\\ \frac{\mathrm{d}}{\mathrm{d}t}\left( \frac{\partial L}{\partial \dot{\theta}_2} \right) =\frac{1}{2}ml^2\left[ \left( \frac{2}{3}+\cos \theta _2 \right) \ddot{\theta}_1-\dot{\theta}_1\dot{\theta}_2\sin \theta _2+\frac{2}{3}\ddot{\theta}_2 \right]\\\end{cases}

根据拉格朗日方程

{τ1=ddt(∂L∂θ˙1)−∂L∂θ1τ2=ddt(∂L∂θ˙2)−∂L∂θ2\begin{cases} \tau _1=\frac{\mathrm{d}}{\mathrm{d}t}\left( \frac{\partial L}{\partial \dot{\theta}_1} \right) -\frac{\partial L}{\partial \theta _1}\\ \tau _2=\frac{\mathrm{d}}{\mathrm{d}t}\left( \frac{\partial L}{\partial \dot{\theta}_2} \right) -\frac{\partial L}{\partial \theta _2}\\\end{cases}

求得机器人动力学模型为

τ=D1(θ)θ¨⏟惯性力项+D2(θ)θ˙2⏟向心力项+D3(θ)θ˙ij⏟科氏力项+D4(θ)⏟重力项⇔[τ1τ2]=D1(θ)[θ¨1θ¨2]+D2(θ)[θ˙12θ˙22]+D3(θ)[θ˙1θ˙2θ˙1θ˙2]+D4(θ)\boldsymbol{\tau }=\underset{\text{惯性力项}}{\underbrace{\boldsymbol{D}_1\left( \boldsymbol{\theta } \right) \boldsymbol{\ddot{\theta}}}}+\underset{\text{向心力项}}{\underbrace{\boldsymbol{D}_2\left( \boldsymbol{\theta } \right) \boldsymbol{\dot{\theta}}^2}}+\underset{\text{科氏力项}}{\underbrace{\boldsymbol{D}_3\left( \boldsymbol{\theta } \right) \boldsymbol{\dot{\theta}}_{ij}}}+\underset{\text{重力项}}{\underbrace{\boldsymbol{D}_4\left( \boldsymbol{\theta } \right) }}\\\Leftrightarrow \left[ \begin{array}{c} \tau _1\\ \tau _2\\\end{array} \right] =\boldsymbol{D}_1\left( \boldsymbol{\theta } \right) \left[ \begin{array}{c} \ddot{\theta}_1\\ \ddot{\theta}_2\\\end{array} \right] +\boldsymbol{D}_2\left( \boldsymbol{\theta } \right) \left[ \begin{array}{c} \dot{\theta}_{1}^{2}\\ \dot{\theta}_{2}^{2}\\\end{array} \right] +\boldsymbol{D}_3\left( \boldsymbol{\theta } \right) \left[ \begin{array}{c} \dot{\theta}_1\dot{\theta}_2\\ \dot{\theta}_1\dot{\theta}_2\\\end{array} \right] +\boldsymbol{D}_4\left( \boldsymbol{\theta } \right)

其中

{D1(θ)=ml2[53+cos⁡θ213+12cos⁡θ213+12cos⁡θ213]D2(θ)=ml2[0−12sin⁡θ212sin⁡θ20]D3(θ)=ml2[−sin⁡θ2000]D4(θ)=mgl[32sin⁡θ1+12sin⁡(θ1+θ2)12sin⁡(θ1+θ2)]\begin{cases} \boldsymbol{D}_1\left( \boldsymbol{\theta } \right) =ml^2\left[ \begin{matrix} \frac{5}{3}+\cos \theta _2& \frac{1}{3}+\frac{1}{2}\cos \theta _2\\ \frac{1}{3}+\frac{1}{2}\cos \theta _2& \frac{1}{3}\\\end{matrix} \right]\\ \boldsymbol{D}_2\left( \boldsymbol{\theta } \right) =ml^2\left[ \begin{matrix} 0& -\frac{1}{2}\sin \theta _2\\ \frac{1}{2}\sin \theta _2& 0\\\end{matrix} \right]\\ \boldsymbol{D}_3\left( \boldsymbol{\theta } \right) =ml^2\left[ \begin{matrix} -\sin \theta _2& 0\\ 0& 0\\\end{matrix} \right]\\ \boldsymbol{D}_4\left( \boldsymbol{\theta } \right) =mgl\left[ \begin{array}{c} \frac{3}{2}\sin \theta _1+\frac{1}{2}\sin \left( \theta _1+\theta _2 \right)\\ \frac{1}{2}\sin \left( \theta _1+\theta _2 \right)\\\end{array} \right]\\\end{cases}

3.3 动力学仿真

初始状态为[θ1θ2]=[180°0°]\left[ \begin{array}{c} \theta _1\\ \theta _2\\\end{array} \right] =\left[ \begin{array}{c} 180\degree\\ 0\degree\\\end{array} \right]、[θ˙1θ˙2]=[00]\left[ \begin{array}{c} \dot{\theta}_1\\ \dot{\theta}_2\\\end{array} \right] =\left[ \begin{array}{c} 0\\ 0\\\end{array} \right]，输入力矩为[τ1τ2]=[20.5]\left[ \begin{array}{c} \tau _1\\ \tau _2\\\end{array} \right] =\left[ \begin{array}{c} 2\\ 0.5\\\end{array} \right] 该初始条件下，平面2R机器人的角度、角速度、角加速度、能量、雅克比矩阵行列式数值波形图以及机器人末端执行器运动轨迹图如图所示

在这里插入图片描述

更多内容欢迎关注我的AI频道"AI技术社"

avatar
文章
阅读
粉丝