参考文献：

1. 布鲁诺·西西里安诺等[意] 《机器人学：建模、规划与控制》

2. 四元数小总结 - 孤独の巡礼 - 博客园 (cnblogs.com)

3. 基于单位四元数的姿态插补（Matlab） - 知乎 (zhihu.com)

4. 基于四元数的工业机器人姿态规划与插补算法的研究 - 豆丁网Docin

本文总结了用于机器人姿态规划的常见三种姿态规划方法，包括：欧拉角、角轴以及四元数。三种方法各有优劣，根据使用场景自由选择。

欧拉角

通过为欧拉角欧拉角ϕ= （φ，θ，ψ） 指定时间律来描述指向。通常使用三次多项式或混合抛物线线性分段时间律是方便的。基于这种方法，时变坐标系的角速度将具有连续量。

例子：

已知条件：初始和目标欧拉角已知，初始速度、加速度为0，最终速度、加速度为0。插值时间为tf。使用五次多项式时间律可以满足条件，位置、速度、加速度均连续。

有插值公式：

但是欧拉角姿态规划的方法存在万向节锁死的问题。

角轴

对于定义在操作空间的误差变量（位置和方向）进行处理，其表达式由下式给出：

其中， 表示末端执行器的期望值， 表示末端执行器的计算值。

对于涉及到方向误差的部分，其表达式取决于末端执行器方向的详细表示，即，欧拉角，角和轴，单位四元数。

对于初始旋转矩阵 和目标旋转矩阵 ，有：

所以有，

对于旋转变换 ，可以表示为绕空间中一固定轴的旋转矩阵。计算轴的单位向量 和 ，可以求得初始旋转矩阵绕该轴的角速度和角加速度。

其中，轴的单位向量 ：

为 指定一个时间律，当 ，且有 。我们使用五次多项式插值为 指定时间律，其中， 为总的姿态运动时间，由人为指定。

为常量，由此得到的速度和加速度为：

最后，为了表征末端执行器关于基坐标系的指向轨迹，需要进行如下变化：

由此我们可以获取机械臂的姿态关节的运动速度，再通过雅可比逆解到各个关节。

但是以上基于角轴表达式的姿态规划存在奇异问题，当   = 0或   = Π时，单位向量r是奇异的。我们对  = 0这种场景进行了特殊处理。当  = 0时，默认姿态无转动，强制规划各姿态关节速度为0。  = Π时尚未做处理，后续考虑进行补充优化。

四元数

1. 四元数基础公式

 2. 四元数球面线性插值

 2.1 四元数球面线性插值的基本概念：

2.2 四元数球面线性插值的基本公式

我们采用第二种构造方式进行姿态规划！