【姿态规划】基于四元数的工业机器人姿态及位移规划

  写了一周的研究方案终于告一段落了，抽点时间来更新下博客，这次更新内容是工业机器人姿态规划，闲暇时间帮别人搞的，别说我不务正业 ：）

0. 任务目标

  实现六关节机器人任务空间轨迹规划（含位移规划和姿态规划）。

1. 思路分析

  工业机器人单纯的直线路径规划之前已经做过了，当时是对直线路径进行速度规划，保证运动过程中速度平滑，最后对插值点求反解得到六个关节的关节角曲线即可。
  这次的客户需求是想在位移规划的基础上再做姿态规划，也算是自己之前一直想做的领域，所以就尝试了一下，整个流程也不算很复杂，运用四元数进行规划即可。
  单位四元数对姿态的描述更加自然，另外还有效避免了欧拉角旋转时奇异性的问题，且基于单位四元数的运动插补算法计算效率要比欧拉角和余弦矩阵高。目前，单位四元数已经在航天器姿态控制等多个领域有着广泛的应用。

  基于四元数的姿态规划需要考虑的流程为：

• 确定姿态变化范围
• 旋转矩阵转四元数轴角形式，对四元数的角进行角速度规划
• 四元数转旋转矩阵
• 逆运动学反算关节角

2. 确定姿态变化范围

  我这里的初始姿态为Rs = [1 0 0;0 -1 0;0 0 -1]，令姿态变化为绕自己的z轴旋转90度，即目标姿态为Re = Rs * rotz(90)，姿态变化范围则有deltaR = Re * inv(Rs)。

3. 旋转矩阵与四元数的互相转换

  将姿态变化的旋转矩阵转换为四元数轴角形式，利用四元数进行角速度平滑规划。转换关系可参考这篇文章【机器人】四元数与旋转矩阵的转换关系

4. 四元数角速度规划

  对于位置规划而言，速度规划的方式有很多，五次多项式七次多项式那些，看你个人选择；同理，对于四元数姿态规划来说，角速度的规划方式也有很多，为了统一且减少不必要的工作量，这里统一采用七段式S型曲线来对速度和角速度进行规划。（借用一下海桑的代码）

对姿态角速度进行规划之后，再将四元数反转换为旋转矩阵形式。

5. 模型搭建

  此次模型是基于ABB工业机器人的虚拟样机来实现的，应用SimMechanics方法模块搭建三维模型如下图所示。对了，中间搭建的时候还遇到个问题，同样是SolidWorks外部导进来的文件，之前的 File Solid Unit Type 可以设置成 from file，而这次只能设置为 custom，而 custom 类型不支持自定义选择坐标系 Frame ，因此还必须要在 SolidWorks 里面量坐标系的三维关系才能建立 Simmechanics 里面的坐标系，有点不太方便，这点暂时还没找到原因。

6. 仿真验证

  将多关节机器人初始位置设在零位q=[0,0,0,0,0,0]rad，此时末端执行器初始位姿矩阵为$$T_s=\left[\begin{array}{cccc}1& 0& 0& 0.7353\\ 0& -1& 0& 0.0004\\ 0& 0& -1& 0.4840\\ 0& 0& 0& 1\end{array}\right]$$设末端执行器位姿变化设为X移动-0.2m，Y移动-0.2m，Z移动-0.2m，姿态绕自己的z轴旋转正90度，即目标位姿矩阵有$$T_d=\left[\begin{array}{cccc}0& -1& 0& 0.5353\\ -1& 0& 0& -0.1996\\ 0& 0& -1& 0.2840\\ 0& 0& 0& 1\end{array}\right]$$通过以上的方法来进行位移和姿态规划，其中六个关节的角速度、角加速度边界均设置为0，姿态始末角速度和角加速度也均设置为0，此外，设机械臂运行时间10s，插值点数100个，每个插值点间的运行时间为0.1s。

关节角规划：
规划出来的各关节角度变化曲线：

各关节变化趋势满足七段式规划要求，其中第四个关节角的纵坐标单位为10的-16次方，基本认为是0，即第四关节角度没有变化。

仿真结果：

  可以看出，机械臂在运动过程中发生了位置以及姿态的变换，末端XYZ坐标位移了[-0.2 -0.2 -0.2] ，姿态变换也绕自身z轴旋转了90度（末端执行器z轴朝下，所以是顺时针旋转90度），运动过程中位移速度和姿态角速度变化平滑，达到了速度和角速度规划的目标，完成了姿态规划的任务。

总结与展望

  基于以上结论，成功的实现了工业机器人的姿态规划，且将之前的位移规划进行了融合，达成了工业机器人的任务空间的位置和姿态同时规划的目标，通过仿真验证了方法的可行性，为之后的轨迹跟踪奠定了基础。

  改进之处有：

• 此为二姿态间的插补规划，如果面对更复杂的情况，需要考虑多姿态间的插补方法；
• 轨迹规划方式可以多涉猎一点，除了本文的七段式S型曲线，还有什么五次多项式、五次B样条曲线、正正弦加速度规划等，这些方法也可以多掌握一些。