基于单位四元数的姿态插补算法

基于单位四元数的姿态插补算法

摘要

现代制造领域对工业机器人的需求越来越多，对工业机器人性能要求也越来越苛刻。机器人运动轨迹插补算法是决定机器人性能的核心技术之一，直接影响着机器人的运动精度、速度和平滑性等主要性能。在曲面、曲线加工，喷涂、弧焊等领域，机器人的轨迹插补，尤其是姿态插补对加工质量和加工效率起到决定性作用。目前对机器人的姿态描述主要使用欧拉法，此类方法存在奇异性以及角速度耦合等问题。单位四元数对姿态的描述更加自然，另外还有效避免了欧拉角旋转时奇异性的问题，且基于单位四元数的运动插补算法计算效率要比欧拉角和余弦矩阵高。目前，单位四元数已经在航天器姿态控，动画制作以及 CAD三维建模等多个领域有着广泛的应用。本文研究的算法等材料可以在gihub下载-链接

单位四元数空间与欧式空间的转化

构造姿态曲线的主要问题在于在单位四元数空间中的姿态曲线的构造问题。因为单位四元数存在于${\mathbf{S}}^3$ 空间中，然而欧式空间中的度量并不适用于${\mathbf{S}}^3$ 空间。同时在欧氏空间中位移的导数为速度，而${\mathbf{S}}^3$ 空间中四元数的导数不是角速度，而是角速度与四元数本身的乘积: $q^{\prime }=\omega q/2$. 下面研究将单位四元数曲线构造问题转换为在欧氏空间中单位矢量的球面曲线构造问题。

四元数的旋转变换表示空间定点旋转

定理: 设$q$和$R$为非标量四元数，如果令
$$q=|\mathbf{E}|(cos\frac{\alpha }{2}+\frac{\mathbf{E}}{|\mathbf{E}|}sin\frac{\alpha }{2})$$
则
$$R^{\prime }=q\ast R\ast q^{-1}$$
是四元数，其范数和标量与四元数$R$相同，其矢量部分Vect $R^{\prime }$为Vect $R$绕欧拉轴$\mathbf{E}$旋转$\alpha$角。因此，绕定点矢量旋转可以用四元数变换来表示。

注意q是单位四元数，可表示为
$$q=q_0+q_1\mathbf{i}+q_2\mathbf{j}+q_3\mathbf{k}=cos\frac{\alpha }{2}+\omega sin\frac{\alpha }{2}$$
其中$\omega$是单位矢量。

为什么四元数可以表示坐标系旋转变换？因为坐标系是由三个单位矢量构成的，上面定理发现，四元数可以对矢量进行旋转变换，对坐标系做旋转变换可以看成是对坐标系的三个单位矢量做旋转变换。

因为表示一个旋转变换可以用一个欧拉轴的单位矢量$\omega$和一个角度$\alpha$来描述，有的书称为等效轴表示法。而这里$\omega$是个定值，对该旋转变换进行插补时，只需对角度$\theta$进行插补即可，而四元数又是联系这两个参数和旋转变换矩阵表示的桥梁，可以把中间插补姿态转化为旋转矩阵。

四元数和旋转矩阵的关系如下：

已知旋转矩阵计算四元数
$$R=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$$
对应的等效轴参数为

（1）若(trR-1)/2=1(即为单位阵),那么$\alpha =0$, $\omega$未定义。

（2）若trR=-1,那么$\alpha =\pi$, $\omega$的解为以下可行的向量
$$\omega =\frac{1}{\sqrt{2(1+r_{33})}}\left[\begin{array}{c}{r}_{13}\\ r_{23}\\ 1+r_{33}\end{array}\right]$$
或者
$$\omega =\frac{1}{\sqrt{2(1+r_{22})}}\left[\begin{array}{c}{r}_{12}\\ 1+r_{22}\\ r_{32}\end{array}\right]$$
或者
$$\omega =\sqrt{2(1+r_{11})}$$