【多类型运动副组合机械臂运动学解算与Matlab仿真】抬升机构+连杆机构+球窝关节机构（上）_运动副 rr

​ 机械臂简图

构型：PRRR+RRR，即 抬升+三连杆+球关节型，可将其解耦为：抬升三连杆+球关节。
（ 该文章主要讲解机械臂运动学解算方法，Matlab仿真完成后将会在后续进行更新… ）

若对以下机械臂解算过程存在疑问可以学习我发布的上一篇文章：【一篇文章教你搞懂-机器人运动学基础知识】

1.正运动学

​ 将球关节视为末端执行器，并简化成一个点。

俯视图：

由上图可知，末端执行器M点的坐标分别为：
$$x=l_1+a_{1}cos{\theta }_1+a_{2}cos({\theta }_1+{\theta }_2)+a_{3}cos({\theta }_1+{\theta }_2+{\theta }_3)\text{\hspace{1em}(1.1)}$$

$$y=a_{1}sin{\theta }_1+a_{2}sin({\theta }_1+{\theta }_2)+a_{3}sin({\theta }_1+{\theta }_2+{\theta }_3)\text{\hspace{1em}(1.2)}$$

$$z=d_1^{\ast }+d_2+d_3\text{\hspace{1em}(1.3)}$$

其中：${\theta }_1、{\theta }_2、{\theta }_3、d^{\ast }$为变量，$L_1、a_1、a_2、a_3、d_2、d_3$为常量。

2.逆运动学

​ 由$M(x,y,z)$解出关节所需要达到的角度。

2.1 求解$d_1^{\ast }$

​ 由于$z$只和$d_1^{\ast }+d_2+d_3$有关，而$d_2,d_3$为常量，那么容易解得：
$$d_1^{\ast }=M_z-d_2-d_3\text{\hspace{1em}(2.1)}$$

2.2 求解${\theta }_3$

​ $x,y$由三连杆结构给出,

求上图中各关节角度$\theta$值，相当于求解：
$$Ax=b\text{\hspace{1em}(2.2)}$$
其中
x = [ θ 1 θ 2 θ 3 ] b = [ x y ] (2.3) x=\left[

$$\begin{matrix} \theta_1 \\ \theta_2 \\ \theta_3 \end{matrix}$$ \right] \qquad b=\left[ $$\begin{matrix} x\\ y \end{matrix}$$ \right] \tag{2.3} x= ​θ1​θ2​θ3​​ ​b=[xy​](2.3)

$$

$$

方程的解$A$明显行满秩列不满秩，因此方程有无穷多解。

因此，$A$的解的结构为：
{ ∞ ， M 点在工作空间内 1 ， M 点在边界 0 ， M 点不在工作空间内 (2.4)

$$\begin{cases} ∞ ，&M点在工作空间内 \\ 1 ，&M点在边界 \\ 0 ，&M点不在工作空间内 \end{cases}$$ \tag{2.4} ⎩ ⎨ ⎧​∞，1，0，​M点在工作空间内M点在边界M点不在工作空间内​(2.4)
但，如果给出了第三根杆的姿态，则方程组便退化为：两个未知数，两条方程式；方程就有解了。幸运的是，第三根杆的姿态可由自定义控制器获取，设获取的姿态角为$\beta$,

由上图可知，第三根杆W点的坐标分别为：
$$W_x=M_x-a_{3}cos\beta \text{\hspace{1em}(2.5)}$$

$$W_y=M_y-a_{3}sin\beta \text{\hspace{1em}(2.6)}$$

其中：
$${\theta }_3=\beta -({\theta }_1+{\theta }_2)\text{\hspace{1em}(2.7)}$$
因此，只需再求出${\theta }_1$与${\theta }_2$，便能完成对机械臂除了球关节的逆运动学，而求${\theta }_1$和${\theta }_2$，无非就是求平面双连杆的逆运动学，这个还是比较简单的。

2.3 求解${\theta }_1、{\theta }_2$

​ 求解${\theta }_1$、${\theta }_2$，即求解平面双连杆的逆运动学，

声明：坐标系平移了$L_1$单位，将原点与连杆原点进行了对齐

因此，
$$P_x=W_x-L_1\text{\hspace{1em}(2.8)}$$

$$P_y=W_y\text{\hspace{1em}(2.9)}$$

余弦定理有：
$$cos{\theta }_2=\frac{P_x^2+P_y^2-a_1^2-a_2^2}{2a_1{a}_2}=D\text{\hspace{1em}(2.10)}$$
则：
$$sin{\theta }_2=\pm \sqrt{1-D^2}\text{\hspace{1em}(2.11)}$$
那么可以得出${\theta }_2$的值:
$${\theta }_2=arctan\frac{\pm \sqrt{1-D^2}}{D}\text{\hspace{1em}(2.12)}$$
取正负是因为有双解，分别对应上肘位与下肘位，如下图：

从图中可以看出：
$${\theta }_1=\alpha -\varphi \text{\hspace{1em}(2.13)}$$
容易得出：
$$\varphi =ta{n}^{-1}(\frac{a_{2}sin{\theta }_2}{a_1+a_{2}cos{\theta }_2})\text{\hspace{1em}(2.14)}$$

$$\alpha =ta{n}^{-1}(\frac{P_y}{P_x})\text{\hspace{1em}(2.15)}$$

最终求得${\theta }_1$的值：
$${\theta }_1=ta{n}^{-1}(\frac{P_y}{P_x})-ta{n}^{-1}(\frac{a_{2}sin{\theta }_2}{a_1+a_{2}cos{\theta }_2})\text{\hspace{1em}(2.16)}$$

3.球型手腕

3.1 引言

​ 自定义控制器会添加一个IMU，将其放置在操作手的手心处采集姿态，从而对腕关节电机进行姿态角控制，这个操作和控制云台差不多，因此就不需要解算。

​ 但考虑到后续加入自动兑矿的功能，此处也提供一下解算，完成逆运动学的最后一环。

3.2 解算

球关节被放到了整个机械臂的最后面，因此，姿态会受前面关节的影响。

设机械臂末端姿态为：
R = [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ] (3.1) R=\left[

$$\begin{matrix} r_{11} &r_{12} &r_{13} \\ r_{21} &r_{22} &r_{23}\\ r_{31} &r_{32} &r_{33} \end{matrix}$$ \right]\tag{3.1} R= ​r11​r21​r31​​r12​r22​r32​​r13​r23​r33​​ ​(3.1)
则有如下关系：
$$R_7^4=(R_4^0{)}^{T}R\text{\hspace{1em}(3.2)}$$
其中：$R_7^4$表示球关节末端相对于三连杆末端的姿态，$R_4^0$表示三连杆末端相对于世界坐标系*（基坐标系）*的姿态。

因为关节1是一个抬升机构，对姿态没有产生影响，因此，

三连杆姿态：

R 2 1 = [ c 1 − s 1 0 s 1 c 1 0 0 0 1 ] R 3 2 = [ c 2 − s 2 0 s 2 c 2 0 0 0 1 ] R 4 3 = [ c 3 − s 3 0 s 3 c 3 0 0 0 1 ] (3.3) R_2^1=\left[

$$\begin{matrix} c_1 &-s_1 &0\\ s_1 &c_1 &0\\ 0 &0 &1 \end{matrix}$$ \right]\qquad R_3^2=\left[ $$\begin{matrix} c_2 &-s_2 &0\\ s_2 &c_2 &0\\ 0 &0 &1 \end{matrix}$$ \right]\qquad R_4^3=\left[ $$\begin{matrix} c_3 &-s_3 &0\\ s_3 &c_3 &0\\ 0 &0 &1 \end{matrix}$$ \right]\tag{3.3} R21​= ​c1​s1​0​−s1​c1​0​001​ ​R32​= ​c2​s2​0​−s2​c2​0​001​ ​R43​= ​c3​s3​0​−s3​c3​0​001​ ​(3.3)
其中，$R_2^1$为杆一，$R_3^2$为杆二，$R_4^3$为杆三，

那么：
R 4 0 = R 1 0 R 2 1 R 3 2 R 4 3 = [ c 123 − s 123 0 s 123 c 123 0 0 0 1 ] (3.4) R_4^0=R_1^0R_2^1R_3^2R_4^3=\left[

$$\begin{matrix} c_{123} &-s_{123} &0\\ s_{123} &c_{123} &0\\ 0 &0 &1 \end{matrix}$$ \right]\tag{3.4} R40​=R10​R21​R32​R43​= ​c123​s123​0​−s123​c123​0​001​ ​(3.4)
为了简洁：$c_1$为$cos{\theta }_1$，$s_1$为$sin{\theta }_1$，$s_{123}$为$sin({\theta }_1+{\theta }_2+{\theta }_3)$，其他同理。

球型手腕的姿态由$R_x,R_y,R_z$相乘得到，即绕$x$轴、绕$y$轴、绕$z$轴旋转综合起来的结果，这也符合我们的直观印象，

因此：
R 7 4 = [ c 5 c 6 c 7 − s 5 s 7 − c 5 c 6 s 7 − s 5 c 7 c 5 s 6 s 5 c 6 c 7 + c 5 s 7 − s 5 c 6 s 7 + c 5 c 7 s 5 s 6 − s 6 c 7 s 6 s 7 c 6 ] (3.5) R_7^4=\left[

$$\begin{matrix} c_5c_6c_7-s_5s_7 &-c_5c_6s_7-s_5c_7 &c_5s_6\\ s_5c_6c_7+c_5s_7 &-s_5c_6s_7+c_5c_7 &s_5s_6\\ -s_6c_7 &s_6s_7 &c_6 \end{matrix}$$ \right]\tag{3.5} R74​= ​c5​c6​c7​−s5​s7​s5​c6​c7​+c5​s7​−s6​c7​​−c5​c6​s7​−s5​c7​−s5​c6​s7​+c5​c7​s6​s7​​c5​s6​s5​s6​c6​​ ​(3.5)

( R 4 0 ) T R = [ c 123 r 11 + s 123 r 21 c 123 r 12 + s 123 r 22 c 123 r 13 + s 123 r 23 c 123 r 21 − s 123 r 11 c 123 r 22 − s 123 r 12 c 123 r 23 − s 123 r 13 r 31 r 32 r 33 ] (3.6) (R_4^0)^TR=\left[

$$\begin{matrix} c_{123}r_{11}+s_{123}r_{21} &c_{123}r_{12}+s_{123}r_{22} &c_{123}r_{13}+s_{123}r_{23}\\ c_{123}r_{21}-s_{123}r_{11} &c_{123}r_{22}-s_{123}r_{12} &c_{123}r_{23}-s_{123}r_{13}\\ r_{31} &r_{32} &r_{33} \end{matrix}$$ \right]\tag{3.6} (R40​)TR= ​c123​r11​+s123​r21​c123​r21​−s123​r11​r31​​c123​r12​+s123​r22​c123​r22​−s123​r12​r32​​c123​r13​+s123​r23​c123​r23​−s123​r13​r33​​ ​(3.6)

由式(3.2)，有：
$$c_5{s}_6=c_{123}{r}_{13}+s_{123}{r}_{23}\text{\hspace{1em}(3.7)}$$

$$s_5{s}_6=c_{123}{r}_{23}-s_{123}{r}_{13}\text{\hspace{1em}(3.8)}$$

$$c_6=r_{33}\text{\hspace{1em}(3.9)}$$

①如果$c_5{s}_6$与$s_5{s}_6$不同时为$0$，则可求解${\theta }_6$：
$${\theta }_6=ta{n}^{-1}(\frac{\pm \sqrt{1-r_{33}^2}}{r_{33}})\text{\hspace{1em}(3.10)}$$
因此${\theta }_6$有双解。

②如果$c_5{s}_6$与$s_5{s}_6$同时为$0$，则$R_7^4$变化为：
R 7 4 = [ c 5 c 7 − s 5 s 7 − c 5 s 7 − s 5 c 7 0 s 5 c 7 + c 5 s 7 − s 5 s 7 + c 5 c 7 0 0 0 1 ] = [ c 5 + 7 − s 5 + 7 0 s 5 + 7 c 5 + 7 0 0 0 1 ] (3.11) R_7^4=\left[

$$\begin{matrix} c_5c_7-s_5s_7 &-c_5s_7-s_5c_7 &0\\ s_5c_7+c_5s_7 &-s_5s_7+c_5c_7 &0\\ 0 &0 &1 \end{matrix}$$ \right]=\left[ $$\begin{matrix} c_{5+7} &-s_{5+7} &0\\ s_{5+7} &c_{5+7} &0\\ 0 &0 &1 \end{matrix}$$ \right]\tag{3.11} R74​= ​c5​c7​−s5​s7​s5​c7​+c5​s7​0​−c5​s7​−s5​c7​−s5​s7​+c5​c7​0​001​ ​= ​c5+7​s5+7​0​−s5+7​c5+7​0​001​ ​(3.11)

因此，只能解出${\theta }_5,{\theta }_7$之和的值，而无法解出${\theta }_5$与${\theta }_7$的特定值，而${\theta }_5$与${\theta }_7$的组合有无穷多种，如果实际中遇到，则任意赋个值，解出另外一个。

只要求出${\theta }_6$，另外两个角度就呼之欲出了

①如果我们选择了正的${\theta }_6$，那么$s_6>0$，

则：
$${\theta }_5=ta{n}^{-1}(\frac{c_{123}{r}_{23}+s_{123}{r}_{13}}{c_{123}{r}_{13}+s_{123}{r}_{23}})\text{\hspace{1em}(3.12)}$$

$${\theta }_7=ta{n}^{-1}(\frac{r_{32}}{-r_{31}})\text{\hspace{1em}(3.13)}$$

②如果我们选择了负的${\theta }_6$，那么$s_6<0$，
$${\theta }_5=ta{n}^{-1}(\frac{c_{123}{r}_{23}-s_{123}{r}_{13}}{-c_{123}{r}_{13}-s_{123}{r}_{23}})\text{\hspace{1em}(3.14)}$$

$${\theta }_7=ta{n}^{-1}(\frac{-r_{32}}{r_{31}})\text{\hspace{1em}(3.15)}$$

至此，机械臂的正逆运动学已经求解完毕。

4.雅可比矩阵

三连杆：
$$x=a_{1}cos{\theta }_1+a_{2}cos({\theta }_1+{\theta }_2)+a_{3}cos({\theta }_1+{\theta }_2+{\theta }_3)\text{\hspace{1em}(4.1)}$$

$$y=a_{1}sin{\theta }_1+a_{2}sin({\theta }_1+{\theta }_2)+a_{3}sin({\theta }_1+{\theta }_2+{\theta }_3)\text{\hspace{1em}(4.2)}$$

对式(4.1)和式(4.2)两边同时求导，并定义 X ˙ = [ x ˙ y ˙ ] \dot{X} = \left[

$$\begin{matrix} \dot{x} \\ \dot{y} \end{matrix}$$ \right] X˙=[x˙y˙​​]，$\dot{q}=\left[\begin{array}{c}\dot{{\theta }_1}\\ \dot{{\theta }_2}\\ \dot{{\theta }_3}\end{array}\right]$，有
$$\dot{X}=Ja\dot{q}\text{\hspace{1em}(4.3)}$$
$J_a$便是机械臂的雅可比矩阵
$$J_a=\left[\begin{array}{ccc}-a_1{s}_1-a_2{s}_{12}-a_3{s}_{123}& -a_2{s}_{12}-a_3{s}_{123}& -a_3{s}_{123}\\ a_1{c}_1+a_2{c}_{12}+a_3{c}_{123}& a_2{c}_{12}+a_3{c}_{123}& a_3{c}_{123}\end{array}\right]\text{\hspace{1em}(4.4)}$$
定义
$$F_t=\left[\begin{array}{c}F\\ T\end{array}\right]\tau =\left[\begin{array}{c}{\tau }_1\\ {\tau }_2\\ {\tau }_3\end{array}\right]\text{\hspace{1em}(4.5)}$$
$F$为沿三连杆末端与起点连线方向(即图中的虚线)上的推力，$T_p$为此连线切线方向上的力，${\tau }_1、{\tau }_2、{\tau }_3$，分别三连杆三个关节电机的扭矩

根据虚功原理
$$\tau =J_a^T{F}_t\text{\hspace{1em}(4.6)}$$
完成虚拟力与关节力矩的映射

电子版可能会有错漏，可以和手写稿对着一起看

若对以上机械臂解算过程存在疑问可以学习我发布的上一篇文章：【一篇文章教你搞懂-机器人运动学基础知识】

如果错漏，多请指教…