运动平台跟踪系统姿态四元数模型参数插值法

吕 舒，张涯辉，包启亮，陈洪斌

（1.中国科学院光电技术研究所，四川 成都610209；2.中国科学院大学，北京100049）

0 引 言

运动平台下，平台的平动或转动均会给跟踪系统造成干扰［1］。普遍的做法便是测量平台姿态，对跟踪系统进行补偿［2］，以此来隔离扰动。

相比较传统的欧拉角法，四元数法在描述姿态的应用方面有计算量小、无奇异性以及可以直接作为捷联系统的控制量等优点［3－6］，现已经广泛应用于姿态控制与空间变换领域，成为描述姿态的有效工具。

工程应用中，为了匹配控制系统的频率，需要对描述平台姿态的四元数序列进行插值。主要方法有四元数线性插值、四元数角插值和四元数球矢插值［7］。四元数线性插值的结果序列夹角不规律［8］，会给控制系统引入震荡；而四元数角插值的结果序列夹角虽然均等，但是插值区间内转轴固定，相邻插值区间旋转轴有较大跳变，应用受限；球矢插值使用最新的数据节点，实时性强，而且充分考虑旋转的球面性，插值的结果序列不仅夹角均等，转轴也渐变。球矢插值一般用于平稳旋转的姿态描述，因此在平台运转平稳时描述姿态较为准确。但若平台的机动性较强，球矢插值由于其平稳的特点，会产生失真，从而影响光电跟踪系统的整体精度。针对这一应用，本文提出一种四元数球矢插值的改进方法称为模型参数球矢插值法，其相邻插值结果四元数之间夹角误差大大减小；并通过仿真实验对比分析给出相关结论。

1 姿态四元素

运动平台下的光电跟踪系统不仅需要同地基光电跟踪系统一样对目标进行快速稳定跟踪，还要克服运动平台带来的干扰。克服运动平台干扰的一个有效方法即测量运动平台的姿态，加入控制系统进行补偿。

而在实际光电跟踪系统中，传感器采样率较低，有些传感器甚至会带有延迟，为了配合控制系统平稳控制，需要对表示姿态的四元数进行插值和外推如图1所示，其中点为基点，叉为插值点。

图1 插值外推点

姿态四元数序列的物理意义在于表示平台的旋转。设有姿态四元数：q＝q0＋q1i＋q2j＋q3k，表示向量由r到r′的旋转。将其规范化

则四元数q可以写为

称为四元数的球面表示法。四元数球面表示法的物理意义如图2所示，其中α表示转角，珒ξ表示转轴。

2 插值策略

旋转由转轴和转角组成。因此对姿态四元数序列的插值策略也主要考虑这两个方面。最直观的策略为均等策略，即插值结果将插值基均分。此策略在运动平台较为平稳运行时有较好的插值性能，而且操作简单。但是如果运动平台机动性较强，均等策略此时便会稍显劣势。

图2 定点旋转的四元数表示

针对机动性较强的运动平台的姿态描述，应该采用更加能体现机动性的策略。可根据已知转角趋势，预测插值点的转角，再依据预测趋势对姿态四元数进行插值。预测夹角趋势策略的好处在于利用量测信息，可以及时体现运动平台的姿态变化，而此策略的劣势在于未考虑旋转轴的变化。然而旋转轴的变化规律描述复杂，不适合加入插值策略中。

3 线性插值

四元数线性插值，是将传统的插值推广到四元数领域的应用，设已知一对四元数qa、qb，四元数的线性插值公式

式中：t——插值参数。

值得注意的是，由式（3）得到的四元数q 是非规范化的，无法直接应用于姿态描述。因此插值后须对其进行规范化处理［7］。于是，式（3）完整形式为

式（4）中含有求平方根运算，增加了计算负担和程序中控制的复杂性［8，9］。此外，线性插值法并未考虑到四元数所代表的物理意义。当以时间点为参数t等步长带入时，得到的四元数序列其相邻点之间的变化并不相同［8］，其变化规律与插值端点相关，四元数这一工具的优势并未充分发挥。

4 球矢插值

四元数在姿态控制的应用中表示的是一种旋转，而旋转具有球面性。考虑到这一点，球面矢量插值的方法被引入。对四元数序列进行插值。球矢插值公式

式中：ω——四元数qa、qb的夹角，其计算公式为

式中：qa·qb——向量点乘。

球面矢量插值法，充分考虑了四元数代表旋转的物理意义，因此当插值参数t等步长变化时，插值结果的夹角也是均等的。如图3所示，在每个插值区间内插值结果的夹角是均等的，表现为图中阶梯型曲线中呈现出水平线段，而插值区间之间呈现突跳。

图3 球矢插值结果序列夹角

5 模型参数球矢插值

在球矢插值中，参数t是作为时间等步长带入，所得插值结果的夹角也均等，如图3所示。此种方法应用于虚拟现实和计算机视觉等领域时，对平滑已知起点与终点的运动起到了良好的效果［10］。然而将四元数用于动平台的实时姿态描述时，往往并不希望其均匀变化，而是期待其根据平台姿态真实的变化规律而变化。由于球矢插值利用最新两个基点进行插值，能较真实地反应姿态变化；而且球矢插值利用了四元数所代表的旋转的物理意义，所以仍可以利用球矢插值的形式，但是要通过对其中的步长参数t建立模型，控制插值结果尽可能接近姿态变化规律。

5.1 参数模型

在球矢插值中结果四元数与初始四元数的夹角

式中：ω——起始四元数与终点四元数之间的夹角。

在球矢插值中，当式（5）中的t遍历0到1时，所得结果四元数从起始四元数到终点四元数之间变化。因此，四元数的夹角代表两四元数之间的抽象距离。

图4 四元数夹角序列

式中：n＝f／F。

由于插值频率较大，在插值周期内，四元数变化较小，因此近似可得

定义状态四元数序列珗qm与初始状态的距离

结合式（7）、式（10）和式（11）可得，对应时刻ti的插值参数

5.2 模型参数球矢插值法迭代流程

（1）计算测量姿态四元数夹角

（2）计算测量姿态四元数与初始状态的距离

（4）计算插值姿态四元数夹角

（5）由式（13）计算插值参数t

（6）将参数t带入式（5）求得插值结果。采样Ti＋1时刻的姿态四元数（i＋1），返回（1）。

6 仿真结果及分析

仿真如下进行，让单位向量 ［1 0 0］ 按某种规律旋转，模拟运动平台的姿态变化。考察按照插值结果四元数旋转后的向量与目标向量（后称goal）的误差来比较两种插值方法的优劣。仿真中，F＝10 Hz，f＝50 Hz。给出测量的姿态角序列（偏航角（yaw），俯仰角（pitch）以及横滚角（roll））如图5所示。

图5 姿态角序列

分别使用球矢插值和模型参数插值的算法对姿态四元数序列进行插值。单位向量按照两组插值结果旋转，对比结果。

从图6中，整体看来球矢插值的结果序列夹角为阶梯型，并不能准确反映夹角的变化规律，相反，本文中提出的模型参数球矢插值法所得结果的夹角与目标基本一致。

图6 球矢插值与模型参数插值结果夹角对比

将两种方法的旋转向量目标序列对比，表1分别列出了旋转向量位置（position）、速度（velocity）以及结果夹角（angel）的误差均方差。可以看出，本文提出的模型参数法（mod－arg）减小了球矢插值方法的误差，特别是夹角（angel）误差显著减小。

表1 两种插值方法误差均方差比对

7 结束语

本文所提出的模型参数球矢插值不涉及归一化，可以根据应用需求对参数建模，来适应多种情况下的应用，所得插值结果的误差在可接受范围内，同时大大减小相邻四元数夹角的误差。今后的工作可以为夹角最小二乘拟合过程加入适当的权值，以提高精度，同时可以考虑将旋转轴的变化规律加入插值策略中，以进一步降低插值误差和扩大模型参数法的适用范围。

［1］ZHANG Xianku，WANG Fengying，YANG Yansheng，et al.Robust self－stabilization control of the shipborne TV tracker［J］.Journal of Test and Measurement Technology，2006，20（8）：213－222 （in Chinese）.［张显库，王凤英，杨盐生，等.舰载电视跟踪仪鲁棒自稳定控制 ［J］.测试技术学报，2006，20 （8）：213－222.］

［2］ZHANG Haiyang，SU Xiuqin，HAO Wei，et al.Study on forecast tracking technique of photoelectric transit on ship ［J］.ACTA Photonica Sinica，2008，37 （9）：1878－1882 （in Chinese）.［张海洋，苏秀琴，郝伟，等.舰载光电经纬仪预测跟踪技术研究 ［J］.光子学报，2008，37 （9）：1878－1882.］

［3］QIAO Xiangwei，ZHOU Weidong，JI Yuren.Study on aerial vehicle attitude estimation based on quaternion particle filter algorithm ［J］.ACTA ArmamentarII，2012，33 （9）：1070－1075 （in Chinese）. ［乔相伟，周卫东，吉宇人.基于四元数粒子滤波的飞行器姿态估计算法研究 ［J］.兵工学报，2012，33 （9）：1070－1075.］

［4］XIA Xiwang，DU Han，LIU Hanbing.Discussion on the transformation between attitude quaternion and Euler angles in large－angle maneuver scenario ［J］.Missiles and Space Vehicles，2012，41 （5）：47－53 （in Chinese）.［夏喜旺，杜涵，刘汉兵.关于大角度范围内四元数与欧拉角转换的思考 ［J］.导弹与航天运载技术，2012，41 （5）：47－53.］

［5］ZHANG Ronghui，JIA Hongguang，CHEN Tao，et al.Attitude solution for strapdown inertial navigation system based on quaternion algorithm ［J］.Optics and Precision Engineering，2008，16 （10）：1963－1970 （in Chinese）. ［张荣辉，贾宏光，陈涛，等.基于四元数法的捷联式惯性导航系统的姿态解算［J］.光学精密工程，2008，16 （10）：1963－1970.］

［6］QI Xin，PENG Qinsu，ZHAO Changshan，et al.A new method based on attitude quaternion matching for transfer alignment［J］.Journal of Astronautics，2011，32 （1）：81－87 （in Chinese）.［齐鑫，彭勤素，赵长山，等.一种新的基于姿态四元数匹配的传递对准方法 ［J］.宇航学报，2011，32 （1）：81－87.］

［7］Ahn Jinsu，Chung Wonjee，Jung Changdoo.Realization of orientation interpolation of 6－axis articulated robot using quaternion ［J］.J Cent South Univ，2012，19 （12）：3407－3414.

［8］LIU Aidong，HUANG Bin，LU Zhongwu.Research on method of coordinate rotation and interpo－lation based on quaterni－on ［J］.Computer and Mod－ernization，2012 （2）：44－47 （in Chinese）.［刘爱东，黄彬，卢中武.基于四元数的坐标旋转与插值方法研究 ［J］.计算机与现代化，2012 （2）：44－47.］

［9］MA Yanhong，HU Jun.On attitude quaternion ［J］.Aerospace Control and Application，2008，34 （3）：55－60 （in Chinese）.［马艳红，胡军.姿态四元数相关问题 ［J］.空间控制技术与应用，2008，34 （3）：55－60.］

［10］XIE Rong，LU Haiyan.Simulation research of ship motion gestures based on quaternion ［J］.Shipbuilding of China，2011，52 （4）：147－152 （in Chinese）.［谢荣，鲁海燕.基于四元数的船舶运动姿态仿真研究 ［J］.中国造船，2011，52（4）：147－152.］

［11］XIAO Wei，LIANG Jiuzhen，CHEN Weiqi.3Dobject rotation and motion interpolation based on quaternion ［J］.Journal of System Simulation，2012，24 （3）：624－627 （in Chinese）.［肖伟，梁久祯，陈玮琪.基于四元数的3D 物体旋转及运动插值 ［J］.系统仿真学报，2012，24 （3）：624－627.］