机器人操作空间轨迹规划 -- 姿态规划

这里我们主要讨论机器人操作空间的姿态规划方法。机器人末端执行器的指向，由（时变的）末端执行器坐标系相对基坐标系的旋转矩阵指定。旋转矩阵的3列表示末端执行器坐标系相对基坐标系的3个单位向量。

常见的姿态规划方式包括欧拉角和角轴两种。

I. 欧拉角

欧拉角是最为常见的也最简单的一种姿态规划方式，通过为欧拉角指定时间律来描述指向。末端执行器的欧拉角三元组由旋转、俯仰、偏摆组成：Φe =（φ, ϑ, ψ ）。通常，Φe 沿连接其初始值Φi 到其最终值Φf 的分段移动。通常选用三次多项式或混合抛物线线性分段时间律是方便的，通过这种方式，时变坐标系的角速度 $\omega e$ 将具有连续量。

示例：五次多项式时间律的位置、速度、加速度曲线，

II. 角和轴

给定两个在笛卡尔空间具有相同原点和不同指向的坐标系，总可以确定一个单位向量，使得可以通过第一个坐标系绕单位向量的轴，旋转1个合适的角度，得到第二个坐标系。

1. 基于角轴的姿态规划

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

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

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

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

所以有，

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

其中，轴的单位向量：

为指定一个时间律，当，且有。为常量，由此得到的速度和加速度为：

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

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

但是以上基于角轴表达式的姿态规划存在奇异问题，当θ= 0或θ= Π时，单位向量r是奇异的。

2. 基于四元数的姿态规划

单位四元数对姿态的描述更加自然，另外还有效避免了欧拉角旋转时奇异性的问题，且基于单位四元数的运动插补算法计算效率要比欧拉角和余弦矩阵高。

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

对于旋转矩阵R：

对于四元数Q：

Q = [q1, q2, q3, q4 ]

四元数转旋转矩阵：

旋转矩阵转四元数：

通过以上转换关系我们可以实现四元数与旋转矩阵的相互转换。

2.2 基于四元数的姿态规划

对于单位四元数Q = {η，ε }，有：

假定，则角度θ∈[-Π, Π]，相当于四元数可以描述所有的旋转。相比于角轴表达式，没有了奇异现象了。因此，我们采用单位四元数规划姿态。

首先将旋转矩阵的初始值，和目标值，转换成单位四元数，得到：

初始四元数：

目标四元数

对于旋转变换：

有四元数变换：

其中，S(·)为反对称算子。

因此, 对于末端执行器姿态关节速度有：

再对该速度雅可比逆解即可得到各关节速度。