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

​ 机械臂简图

构型：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=\left[\begin{array}{c}{\theta }_1\\ {\theta }_2\\ {\theta }_3\end{array}\right]b=\left[\begin{array}{c}x\\ y\end{array}\right]\text{\hspace{1em}(2.3)}$$

$$

$$

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

因此，$A$的解的结构为：
$$\{\begin{array}{ll}\infty ，& M点在工作空间内\\ 1，& M点在边界\\ 0，& M点不在工作空间内\end{array}\text{\hspace{1em}(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$$