5-DOF机械臂课程设计（一）之正逆运动学本文已参与「新人创作礼」活动，一起开启掘金创作之路。基本要求开发上位机

本文已参与「新人创作礼」活动，一起开启掘金创作之路。

基本要求

开发上位机程序，要求有良好的界面，能提供关节空间下和笛卡尔坐标下表示的目标位姿点和中间若干经过点的输入功能，进行轨迹规划，并控制机械臂沿轨迹运动，最终完成目标抓取的任务。

连杆长度：2个相邻关节轴线之间的距离

连杆扭角：2个相邻关节轴线之间的角度

连杆偏距：2个关节坐标系的X轴之间的距离

关节角度：关节变量计算时需要加初始角度偏移

根据《机器人学导论》的内容，建立坐标系的规则如下：

坐标系Z轴确定规则：

如果关节是旋转的，Z轴按右手定则大拇指指向为正方向。如果关节是滑动的，Z轴沿直线运动方向的正方向。连杆长度D为关节变量。
X轴确定规则： 1）情况1：两关节Z轴既不平行也不相交取两Z轴公垂线方向作为X轴方向。 2）情况2：两关节Z轴平行此时，两Z轴之间有无数条公垂线，可挑选与前一关节的公垂线的一条公垂线 3）情况3：两关节Z轴相交取两条Z轴的叉积方向作为X轴（叉积：向量积）
Y轴确定原则：右手定则

最终建立的机械臂坐标系如下图：

根据上述坐标系的建立，可以得到DH参数表如下：

i	$\alpha_{i-1}$	$a_{i-1}$	$d_i$	$\theta_i$
1	0	0	80	$\theta_1$
2	$\pi/2$	10	0	$\theta_2+\pi/2$
3	0	105	0	$\theta_3$
4	0	96	-11	$\theta_4$
5	$\pi/2$	0	60	$\theta_5+\pi/2$
6	0	0	95	0

根据书本中公式， ${ }_{i}^{i-1} T$ 的一般表达式如公式(1)所示：

{ }_{i}^{i-1} T=\left[\begin{array}{cccc} c \theta_{i} & -s \theta_{i} & 0 & a_{i-1} \\ s \theta_{i} c \alpha_{i-1} & s \theta_{i} c \alpha_{i-1} & -s \alpha_{i-1} & -s \alpha_{i-1} d_{i} \\ s \theta_{i} s \alpha_{i-1} & c \theta_{i} s \alpha_{i-1} & c \alpha_{i-1} & c \alpha_{i-1} d_{i} \\ 0 & 0 & 0 & 1 \end{array}\right]

可以计算得到工具坐标系相对于固定坐标系的位置和姿态，如下列所示

{ }_{6}^{0} T={ }_{1}^{0} T \cdot{ }_{2}^{1} T \cdot{ }_{3}^{2} T \cdot{ }_{4}^{3} T \cdot{ }_{5}^{4} T \cdot{ }_{6}^{5} T

将工具坐标系相对于固定坐标系的位姿齐次矩阵, 转化成笛卡尔坐标形式, 姿态采用绕定轴 X-Y-Z 旋转 (RPY 角) 的形式描述, 其转化过程可描述如下:

{ }_{B}^{A} T=\left[\begin{array}{cc} { }_{B}^{A} R & { }_{B}^{A} P \\ 0 & 0 & 0 \end{array}\right]

其中,

{ }_{B}^{A} R=\left[\begin{array}{lll} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{array}\right]{ }_{B}^{A} P=\left[\begin{array}{l} x \\ y \\ z \end{array}\right]

其转化为 $R P Y$ 角对应的公式为:

\left\{\begin{array}{c} \theta=-a \sin \left(r_{31}\right) \\ \psi=\operatorname{atan} 2\left(r_{32} / \cos (\theta), r_{33} / \cos (\theta)\right) \\ \phi=\operatorname{atan} 2\left(r_{21} / \cos (\theta), r_{11} / \cos (\theta)\right) \end{array}\right.

对应的笛卡尔坐标为:

\text { Carte }=[x, y, z, \theta, \psi, \phi]

已知工具坐标系末端相对于固定坐标系的位置和姿态, 通过几何法对机械臂的各关节角度进行求解。首先可以直接确定第一个关节和第五个关节的角度值, 第一个关节的角度可以通过 $\mathrm{x}$ 和 $\mathrm{y}$ 直接确定, 第四个关节的角度值等于 $\phi$ , 即

\begin{gathered} \theta_{1}=\operatorname{atan} 2(y, x) \\ \theta_{5}=\phi \end{gathered}

由于该机械辟只有五个自由度, 最后一个关节为空制夹爪的末端, 所以首先要对工具坐标系末端相对于固定坐标系的位置进行处理, 分别减去夹爪在 $x 、 y 、 z$ 方向上的分量，如下所示：

对于剩下的三个关节，求解其角度值可以看作是已知第三连杆的位姿求解平面三连杆中三个关节的角度值，其中，第三连杆末端的位置为 $x_1, y_1)$ ,倾斜角度为 $\psi$ 。三连杆分别对应的角度为 $\theta_1',\theta_2',\theta_3'$ ,

对于三连杆所在的平面，有如下几何关系：

\begin{gathered} x_{1}=l_{1} \cdot \cos \theta_{1}{ }^{\prime}+l_{2} \cdot \cos \left(\theta_{1}{ }^{\prime}+\theta_{2}{ }^{\prime}\right)+l_{3} \cdot \cos \left(\theta_{1}{ }^{\prime}+\theta_{2}{ }^{\prime}+\theta_{3}{ }^{\prime}\right) \\ y_{1}=l_{1} \cdot \sin \theta_{1}{ }^{\prime}+l_{2} \cdot \sin \left(\theta_{1}{ }^{\prime}+\theta_{2}{ }^{\prime}\right)+l_{3} \cdot \sin \left(\theta_{1}{ }^{\prime}+\theta_{2}{ }^{\prime}+\theta_{3}{ }^{\prime}\right) \\ \text { 令 } m=\mathrm{l}_{1} \cdot \cos \psi-x_{1}, n=\mathrm{l}_{2} \cdot \sin \psi-y_{1}, \text { 对于 } \theta_{1} \text { 有 } \\ \qquad \begin{array}{c} \theta_{1}=\left(l_{1} \cdot \cos \theta_{1}{ }^{\prime}+m\right)^{2}+\left(l_{1} \cdot \sin \theta_{1}{ }^{\prime}+n\right)^{2} \\ \left(\theta_{1}{ }^{\prime}+\theta_{2}{ }^{\prime}\right)=\left(l_{1} \cdot \cos \left(\theta_{1}{ }^{\prime}+\theta_{2}{ }^{\prime}\right)+m\right)^{2}+\left(l_{1} \cdot \sin \left(\theta_{1}{ }^{\prime}+\theta_{2}{ }^{\prime}\right)+n\right)^{2} \\ \text { 进而有, } \\ m \cdot \cos \theta_{1}{ }^{\prime}+n \cdot \sin \theta_{1}{ }^{\prime}=k_{1} \\ k_{1}=\frac{l_{2}^{2}-l_{1}^{2}-m^{2}-n^{2}}{2 l_{1}} \\ m \cdot \cos \left(\theta_{1}{ }^{\prime}+\theta_{2}{ }^{\prime}\right)+n \cdot \sin \left(\theta_{1}{ }^{\prime}+\theta_{2}{ }^{\prime}\right)=k_{2} \\ k_{1}=\frac{l_{1}^{2}-l_{2}^{2}-m^{2}-n^{2}}{2 l_{2}} \end{array} \end{gathered}

\begin{aligned} &\text { 令 } b_{1}=-2 \cdot n \cdot k_{1}, c_{1}=k_{1}^{2}-m^{2}, \Delta_{1}=\mathrm{b}_{1}^{2}-4 \cdot a \cdot c_{1}, \text { 当 } \Delta_{1}<0 \text { 时, 则表示出现 } \\ &\text { 非实数解的情况, 此时机械臂对于输入的笛卡尔坐标工作空间不可达。同理, 令 } b_{2}= \\ &-2 \cdot n \cdot k_{2}, c_{2}=k_{2}^{2}-m^{2}, \Delta_{2}=\mathrm{b}_{2}^{2}-4 \cdot a \cdot c_{2}, \text { 当 } \Delta_{2}<0 \text { 时, 则表示工作空间不可 } \\ &\text { 达。 } \\ \end{aligned}

根据书本公式[1]，可以解得

\begin{aligned} &\left.\theta_{1}^{\prime}=\operatorname{atan} 2(n, m) \pm \operatorname{atan} 2\left(a-k_{1}^{2}\right), k_{1}\right) \\ &\left.\theta_{1}{ }^{\prime}+\theta_{2},=\operatorname{atan} 2(n, m) \pm \operatorname{atan} 2\left(a-k_{2}{ }^{2}\right), k_{2}\right) \end{aligned}

最终，根据 $\theta_1'+\theta_2'+\theta_3=\psi$ 可以求解出平面三连杆中各个关节的倾斜角度。

平面三连杆在求解时会出现多解的情况，经过思考分析，对多解情况提出了一种可行的方案，如下图所示

上式的 $\theta_1'$ 有两组解，由于机械臂的臂长 $l_1$ 和 $l_2$ 的长度不同，所以会有：

\theta_{11}+\theta_{12}>\theta_{21}+\theta_{22}

此处 $\theta_{22}<0$ , 并且 $\theta_{21}>\theta_{11}$ , 由此关系可以对两组解进行区分。则可以求出机械臂第 2、3、4 关节的角度

\begin{gathered} \theta_{2}=\theta_{1}^{\prime}-90 \\ \theta_{3}=\theta_{2}^{\prime} \\ \theta_{4}=\theta_{3}^{\prime} \end{gathered}

最终可得机械臂逆运动学的解

\text { JVar }=\left[\theta_{1}, \theta_{2}, \theta_{3}, \theta_{4}, \theta_{5}\right]

方式二：关注公众号技术开发小圈，在菜单栏中点击项目分享，即可获得百度网盘链接。