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

1 平面2R机器人概述

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

图1

2 运动学建模

2.1 正运动学模型

指数积方法的核心原理是基于运动旋量的指数积公式：
$${}_{\mathbf{T}}^{\mathbf{A}}\mathbf{T}\left(\mathbf{\theta }\right)=e^{\left[{\mathbf{V}}_1\right]{\theta }_1}{e}^{\left[{\mathbf{V}}_2\right]{\theta }_2}\cdots {e^{\left[{\mathbf{V}}_n\right]{\theta }_n}}_{\mathbf{T}}^{\mathbf{A}}\mathbf{T}\left(0\right)$$

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

图2

各轴线方向如下：
$${\omega }_1={\omega }_2=\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right],\left[{\omega }_1\right]=\left[{\omega }_2\right]=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 0\end{array}\right]$$

取各个运动轴上的一点：

$$r_1=\left[\begin{array}{c}0\\ 0\\ 0\end{array}\right],r_2=\left[\begin{array}{c}0\\ -l\\ 0\end{array}\right]$$

可得各个运动旋量的指数形式
$$e^{\left[{\mathbf{V}}_1\right]{\theta }_1}=\left[\begin{array}{cccc}\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{array}\right],e^{\left[{\mathbf{V}}_2\right]{\theta }_2}=\left[\begin{array}{cccc}\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{array}\right]$$

关节变量$\mathbf{\theta }=\left[\begin{array}{cc}{\theta }_1& {\theta }_2\end{array}\right]$为0时即为初始位姿：

$${}_T^B\mathbf{T}\left(0\right)=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& -2l\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

所以由${}_T^B\mathbf{T}\left(\mathbf{\theta }\right)=e^{\left[{\mathbf{V}}_1\right]{\theta }_1}{e}^{\left[{\mathbf{V}}_2\right]{\theta }_2}{}_T^S\mathbf{T}\left(0\right)$即得

$${}_T^B\mathbf{T}\left(\mathbf{\theta }\right)=\left[\begin{array}{cccc}\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{array}\right]$$

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

2.2 逆运动学模型

以图3所示的象限为例，以几何方法进行机器人运动学反解。

图3

对于位形一，在$\Delta OAB$中运用余弦定理可得$2l^2\cos \left(\pi -{\theta }_2\right)=l^2+l^2-L^2$，其中$L^2=x^2+y^2$，显然末端执行器坐标需要满足$0<\sqrt{x^2+y^2}⩽2l$

解得：
$${\theta }_2=\mathrm{a}\mathrm{r}\mathrm{c}\cos \left(\frac{x^2+y^2}{2l^2}-1\right)$$

有两个解，位形一对应正解，位形二对应负解。
同样在$\Delta OAB$中运用余弦定理可得
$$\psi =\mathrm{a}\mathrm{r}\mathrm{c}\cos \left(\frac{\sqrt{x^2+y^2}}{2l}\right)$$

为方便起见，这里总是取$\psi$的正解。再根据
$$\beta =\left|\mathrm{a}\mathrm{r}\mathrm{c}\tan \left(\frac{y}{x}\right)\right|$$

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

$$\{\begin{array}{l}{\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{array}$$

2.3 机器人运动学仿真

基于Matlab Robotics工具箱进行平面2R机器人的运动学仿真验证。验证过程如下：分别选取位于四个象限的关节参数
$$\left[\begin{array}{c}{\theta }_1\\ {\theta }_2\end{array}\right]=\left[\begin{array}{c}35°\\ 55°\end{array}\right],\left[\begin{array}{c}{\theta }_1\\ {\theta }_2\end{array}\right]=\left[\begin{array}{c}95°\\ 25°\end{array}\right],\left[\begin{array}{c}{\theta }_1\\ {\theta }_2\end{array}\right]=\left[\begin{array}{c}230°\\ 20°\end{array}\right],\left[\begin{array}{c}{\theta }_1\\ {\theta }_2\end{array}\right]=\left[\begin{array}{c}300°\\ 70°\end{array}\right]$$

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

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

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

图4

3 动力学建模

3.1 计算动能

取定连杆上一质量微元$\mathrm{d}m$，其位置坐标为$(x,y)$，采用极坐标表示即为
$$\text{连杆I}\{\begin{array}{l}x=r\sin {\theta }_1\\ y=-r\cos {\theta }_1\end{array}\text{连杆II}\{\begin{array}{l}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{array}$$

对时间求导，得到
$$\text{连杆I}\{\begin{array}{l}\dot{x}=r{\dot{\theta }}_1\cos {\theta }_1\\ \dot{y}=r{\dot{\theta }}_1\sin {\theta }_1\end{array}\text{连杆II}\{\begin{array}{l}\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{array}$$

所以质量微元速度为

$$\{\begin{array}{l}\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{array}$$

因此机器人动能为
$$\begin{gathered} 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}m{l}^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] \end{gathered}$$

3.2 势能计算与动力学方程

以基坐标系$x$轴为零势能面，则系统总势能为
$$E_p=-mgl\left[\frac{3}{2}\cos {\theta }_1+\frac{1}{2}\cos \left({\theta }_1+{\theta }_2\right)\right]$$

对于任何机械系统．拉格朗日函数定义为系统总动能和势能之差，即
$$\begin{gathered} L=E_k-E_p \\ =\frac{1}{2}m{l}^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] \end{gathered}$$

基于拉格朗日函数可得

$$\{\begin{array}{l}\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}m{l}^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}m{l}^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{array}$$

$$\{\begin{array}{l}\frac{\partial L}{\partial {\theta }_2}=-\frac{1}{2}mgl\sin \left({\theta }_1+{\theta }_2\right)-\frac{1}{2}m{l}^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}m{l}^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}m{l}^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{array}$$

根据拉格朗日方程
$$\{\begin{array}{l}{\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{array}$$

求得机器人动力学模型为

$$\begin{gathered} \mathbf{\tau }=\underset{\text{惯性力项}}{\underbrace{{\mathbf{D}}_1\left(\mathbf{\theta }\right)\ddot{\mathbf{\theta }}}}+\underset{\text{向心力项}}{\underbrace{{\mathbf{D}}_2\left(\mathbf{\theta }\right){\dot{\mathbf{\theta }}}^2}}+\underset{\text{科氏力项}}{\underbrace{{\mathbf{D}}_3\left(\mathbf{\theta }\right){\dot{\mathbf{\theta }}}_{ij}}}+\underset{\text{重力项}}{\underbrace{{\mathbf{D}}_4\left(\mathbf{\theta }\right)}} \\ \iff \left[\begin{array}{c}{\tau }_1\\ {\tau }_2\end{array}\right]={\mathbf{D}}_1\left(\mathbf{\theta }\right)\left[\begin{array}{c}{\ddot{\theta }}_1\\ {\ddot{\theta }}_2\end{array}\right]+{\mathbf{D}}_2\left(\mathbf{\theta }\right)\left[\begin{array}{c}{\dot{\theta }}_1^2\\ {\dot{\theta }}_2^2\end{array}\right]+{\mathbf{D}}_3\left(\mathbf{\theta }\right)\left[\begin{array}{c}{\dot{\theta }}_1{\dot{\theta }}_2\\ {\dot{\theta }}_1{\dot{\theta }}_2\end{array}\right]+{\mathbf{D}}_4\left(\mathbf{\theta }\right) \end{gathered}$$

其中
$$\{\begin{array}{l}{\mathbf{D}}_1\left(\mathbf{\theta }\right)=m{l}^2\left[\begin{array}{cc}\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{array}\right]\\ {\mathbf{D}}_2\left(\mathbf{\theta }\right)=m{l}^2\left[\begin{array}{cc}0& -\frac{1}{2}\sin {\theta }_2\\ \frac{1}{2}\sin {\theta }_2& 0\end{array}\right]\\ {\mathbf{D}}_3\left(\mathbf{\theta }\right)=m{l}^2\left[\begin{array}{cc}-\sin {\theta }_2& 0\\ 0& 0\end{array}\right]\\ {\mathbf{D}}_4\left(\mathbf{\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{array}$$

3.3 动力学仿真

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

本文的完整工程代码请通过下方名片联系我获取

---

🔥 更多精彩专栏：

• 《ROS从入门到精通》
• 《机器人原理与技术》
• 《机器学习强基计划》
• 《计算机视觉教程》
• …

👇源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系👇