平面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{惯性力项}}{{\mathbf{D}}_1\left(\mathbf{\theta }\right)\ddot{\mathbf{\theta }}}+\underset{\text{向心力项}}{{\mathbf{D}}_2\left(\mathbf{\theta }\right){\dot{\mathbf{\theta }}}^2}+\underset{\text{科氏力项}}{{\mathbf{D}}_3\left(\mathbf{\theta }\right){\dot{\mathbf{\theta }}}_{ij}}+\underset{\text{重力项}}{{\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机器人的角度、角速度、角加速度、能量、雅克比矩阵行列式数值波形图以及机器人末端执行器运动轨迹图如图所示