六轴机械臂运动学正、逆解及轨迹规划带源码

（一）初识机械臂

1.1 六轴机械臂技术栈

六轴机械臂需要掌握的技术栈有：运动学正解、运动学逆解、轨迹规划等；其中轨迹规划通常分为：关节空间规划和笛卡尔空间规划。考虑到后面会用STM32F4芯片当作六轴机械臂的主控制芯片，且关节空间规划有着计算简单，需要CPU算力小等特点，所以本文主要讨论关节空间规划。关节空间规划分为：五次多项式轨迹规划和三次多项式轨迹规划。本文主要讨论五次多项式轨迹规划，别问为什么，问就是五次多项式效果更好(加速度和速度的变化曲线更加的平滑)。

1.2 机械臂相关概念

何为运动学正解？何为运动学逆解？轨迹规划又到底是什么呢？

运动学正解：已知各关节的运动参数，求末端执行器的相对参考坐标系的位姿。通俗讲：已知各个关节的角度值，求末端执行器相对于基坐标的坐标值；比如，六轴机械臂中第一关节到第六关节的电机分别转动的角度值为0°，90°，0°，45°，30°，60°，此时六轴机械臂末端执行器相对于基坐标的三维坐标就是我们需要求的值(也就是运动学正解需要求的值)。
运动学逆解：根据已给定的满足工作要求的末端执行器相对参考坐标系的位置和姿态，求各关节的运动参数。通俗讲：已知末端执行器相对于基坐标的坐标值，求各个关节的角度值；比如，六轴机械臂中末端执行器相对于基坐标的坐标值为(100,-20,80)。此时机械臂中第一关节到第六关节的电机所转动的角度值就是我们需要求的值(也就是运动学逆解需要求的值)。
轨迹规划：末端执行器按照我们计划的路线，从一个坐标点移动到另一个坐标点的过程。如果进行直线路径规划，那么末端执行器移动路线为一条直线。也可以进行圆弧路径规划，末端执行器移动路线就为一条圆弧曲线。注意的是进行圆弧路径规划需要三个坐标点才能确定一条圆弧。也还可以进行关节空间规划或者笛卡尔空间规划。
关节空间规划：已知六轴机械臂六个关节的起始角度(可以通过运动学逆解求出或者是末端执行器起始位姿角度值)和终止角度(可以通过运动学逆解求出)，通过五次多项式或者三次多项式规划机械臂末端的空间运动轨迹。优点是，不需要复杂的计算，对处理器性能需求不高。
笛卡尔空间规划：已知末端执行器的起始点和终止点的坐标及位姿，利用运动学逆解求出各个关节的转动角度值。优点是，可以对末端执行器进行精准的控制。缺点是，需要进行复杂的计算，对处理器性能需求较高。

1.3 标准型DH和改进型DH的区别

机械臂建立坐标系有标准型DH参数法和改进型DH参数法建立坐标系。
改进型(MDH)和标准型(SDH)对比图
标准型(SDH)

改进型(MDH)

SDH和MDH区别：
①固连坐标系的位置不同
SDH是以连杆的下一个关节作为固连坐标系(如上SDH图所示)，而MDH是以连杆的上一个关节作为固连坐标系(如上MDH图所示);
②x轴方向的确定方法不同
SDH中X轴相互垂直于当前Z轴和前一个Z轴；MDH中X