动基座视觉双惯性姿态测量数据融合方法研究

徐丹旸， 宋 潇

(河南科技大学，河南 洛阳 471000)

0 引言

基于视觉和惯性数据融合的姿态确定方法已成功应用于无人地面车辆(UGV)、头部运动跟踪、姿态跟踪等领域[1-2]，其设计均考虑了单传感器的特点。视觉传感器没有累积误差，测量稳定性高，但存在视线和环境光干扰问题[3]；惯性传感器具有重量轻、适合快速运动跟踪的特点，但由于存在严重的漂移问题而缺乏长期稳定性[4]。因此，通常采用视觉和惯性传感器融合技术来克服单一传感器固有的缺点。

目前的视觉/惯性组合系统多为相机与单惯性传感器的组合[5-8]。当目标物在一个运动的基座上做相对运动时，其相对于运动基座的运动与基座本身的运动耦合在一起，无法通过单一陀螺仪有效地分离开来，所以，单惯性传感器无法满足动基座上相对姿态测量的要求[9]。本文介绍并搭建了基于视觉和双陀螺仪的姿态测量系统。在该系统中，惯性测量由两个陀螺仪共同完成：其中一个陀螺仪与运动基座固连，作为辅陀螺仪，用以测量运动基座的角速度；另外一个陀螺仪与目标物固连，作为主陀螺仪，用以测量运动基座和主陀螺仪的耦合角速度。通过对两个陀螺仪的角速度进行差分计算，可以从主陀螺仪数据中剔除运动基座的干扰角速度。在融合视觉测量与惯性测量之前，各个器件的输出系统统一变换到参考系下进行，涉及多个坐标系的转换。为此，本文给出了系统中坐标系归一化方案。

卡尔曼滤波是传感器融合的常用方法[10-13]，但其仅适用于线性系统，而在视觉与惯性融合中，系统方程式是非线性的。为了解决非线性问题，文献[14]提出了一种基于采样点的卡尔曼滤波，即容积卡尔曼滤波。利用一组确定性采样点来捕获系统的相关统计参量，从而避免对非线性系统方程的线性化，提高了滤波算法的稳定性。针对所搭建系统，本文设计了一种容积卡尔曼滤波，有效地实现了对姿态角的估计。

本文搭建了基于视觉/双陀螺仪传感器的动基座姿态测量系统，建立了测量系统的坐标系，并提出全局坐标系归一化方案，实现了不同坐标系下传感器数据的统一。利用容积卡尔曼滤波对视觉和惯性结果进行融合，实现了高精度、大范围快速、稳定的姿态测量，最后将该算法在实验平台上进行测试，验证其有效性。

1 视觉和惯性融合姿态测量原理

图1所示为基于视觉/双陀螺仪传感器的运动基座姿态测量系统。

转台模拟被测目标物，其上固定连接有主陀螺仪和视觉靶标，当转台转动时，主陀螺仪和视觉靶标做同步跟随运动。相机拍摄立体靶标图像，利用姿态解算算法，通过图像中特征点坐标完成视觉姿态解算。当基座运动时，主陀螺仪输出的角速度中包括转台转动角速度和基座运动角速度。而辅陀螺仪仅感知到基座的运动并输出相应的角速度。对主陀螺仪和辅陀螺仪输出角速度进行差分计算，便可将运动基座的运动信息从主陀螺仪输出角速度中剔除，得到转台相对于运动基座的转动角速度。惯性和视觉测量在不同的坐标系中进行，必须将不同的测量统一到相同的参考坐标系下。

图1 姿态测量系统Fig.1 Attitude measurement system

1) 定义目标坐标系为b系，表示为ObXbYbZb。目标坐标系与转台固连，随着转台的转动而做同步转动。

2) 定义相机坐标系为c系，表示为OcXcYcZc。原点Oc为相机镜头的光学中心，OcZc轴与镜头光轴方向一致。

3) 定义视觉靶标坐标系为t系，表示为OtXtYtZt。本文中,靶标坐标系即为对靶标LED点坐标测量的双目测量系统的坐标系。

4) 定义主陀螺仪坐标系为m系，表示为OmXmYmZm。

5) 定义辅陀螺仪坐标系为s系，表示为OsXsYsZs。

6) 定义参考坐标系为r系，表示为OrXrYrZr。令参考坐标系与转台位于零位时的转台坐标系重合。与目标坐标系不同的是，参考坐标系与运动基座固连，不随转台的转动而运动。

(1)

(2)

2 全局坐标系归一化

2.1 标定

标定中，主陀螺仪和视觉靶标相对位置关系保持不变。在适合的距离，正对于视觉靶标安装一个高分辨率相机。在标定过程中，相机保持静止，主陀螺仪和靶标一起做多个自由度的运动。在这个过程中，相机不断拍摄靶标图像，标定主陀螺仪输出角速度[15]。图2所示为靶标系与主陀螺仪系标定。

图2 靶标系与主陀螺仪系标定Fig.2 Calibration of target coordinate and master gyro coordinate

(3)

(4)

(5)

(6)

由四元数相乘的性质，可得

(7)

(8)

其中：

(9)

(10)

qw为四元数q的标量部分;qv为四元数q的矢量部分;R,L分别表示两种计算方式。结合式(6)、式(7)和式(8)，可得

(11)

(12)

式中:矩阵A为

(13)

W=diag(w1,w2,…,wK),是为了防止视觉误差导致计算不准确而引入的权重矩阵,权重系数可以由估计误差来调整，若误差越大，则该相邻两帧图像的数据权重越小

(14)

2.2 标定

(15)

由式(15)可以构造矩阵方程

(16)

(17)

2.3 标定

辅陀螺仪坐标系到相机坐标系的标定，与主陀螺仪坐标系到靶标坐标系的标定类似。标定过程中，辅陀螺仪和相机位置关系始终保持不变。在适合的距离正对相机固定放置一个高精度靶标。在标定过程中，靶标始终保持静止，辅陀螺仪和相机一起做多自由度运动。整个过程中，相机不断拍摄靶标图像，辅陀螺仪输出角速度值。

(18)

2.4 旋转矩阵最优正交化

由于旋转矩阵具有正交性，所以在标定和测量中，旋转矩阵需要进行最优正交化。假设存在旋转矩阵C，则对其最优化的迭代算法为

(19)

(20)

成立时，完成迭代，把满足式(20)的C(p+1)作为C的标定结果。式中:fp与fp+1分别代表两次相邻迭代；ε为设定的最优化阈值。

3 基于容积卡尔曼滤波的数据融合

在本系统中，状态向量xk除了包括待估计的3个姿态角，还包括对2个陀螺仪的零偏。相对于欧拉角，四元数具有不存在万向锁的优点，故状态中的角度可以用四元数q来替代表示。由于四元数中参数冗余，用矢量部分qv来代替四元数进行求解。取状态向量xk=[qvk,bmk,bsk]T，是9×1维向量。具体滤波步骤如下。

1)计算容积点。

(21)

(22)

四元数和陀螺仪漂移部分容积点为

(23)

(24)

(25)

2) 计算经过状态方程传递后的容积点

(26)

(27)

3) 计算k+1时刻的一步状态预测值。因四元数求均值不能直接相加，可求得

(28)

式中：

(29)

(30)

(31)

(32)

(33)

4) 计算状态误差协方差矩阵

(34)

式中，Qk为预先设定的系统过程噪声。

5) 计算更新后的状态容积点

(35)

(36)

容积点为

(37)

(38)

(39)

其中，i=1,2,…,n。

6) 预测测量值为

(40)

(41)

(42)

7) 计算信息

(43)

8) 计算卡尔曼滤波增益为

(44)

式中:

(45)

(46)

(47)

(48)

(49)

其中，Rk为预先设定的观测噪声。

9) 计算k+1时刻的状态估计值

(50)

(51)

状态中零偏和四元数部分更新为

(52)

(53)

(54)

10) 状态误差协方差矩阵更新为

(55)

4 实验测试及结果分析

动基座视觉双惯性姿态测量系统装置如图3所示。相机型号为Basler acA2040-120 μm， Sensonor STIM 300 MEMS IMU中的陀螺仪作为系统中的陀螺仪。相机和陀螺仪参数见表1。

图3 测量系统实验平台Fig.3 Experimental platform of measurement system

表1 陀螺仪和相机参数

为了验证测量的精度，利用一个高精度二维转台作为目标运动物体，主陀螺仪、立体靶标通过螺丝安装在转台上，转台固定在摇摆台上。相机和辅陀螺仪通过光学支撑棒固定在摇摆台上。当转台位于零位时，靶标正对相机。实验中使用的三维立体靶标内嵌4个非共面红外LED点，波长为940 nm。通过对特征点的几何特征等的筛选，可以排除背景中的干扰，实现对特征点的定位提取。

为了验证本系统滤波算法的有效性，利用搭建的系统进行角度测量实验，该系统中转台用以模拟头部运动姿态。对于实际的头部运动，俯仰运动的范围相对较小，而方位运动的范围相对较大。所以在实验中，转台从零位开始，俯仰角以15°为间隔，从-60°运动到60°。在每个间隔中，方位轴先从-100°运动到100°，再从100°运动到-100°，以20°为间隔往返运动。

转台实际转动角度和视觉测量结果见图4。

图4 转台实际转角和视觉测量Fig.4 Rotation angle and the vision measurement

由于本系统中的转台仅可以绕两个轴进行转动，所以仅研究俯仰角度和方位角度的估计效果。可以发现，视觉测量的范围有限，当转动角度大于一定程度时，相机就无法拍摄到4个特征点，视觉信息缺失。在实际应用中，视线的模糊、环境的干扰等也会导致视觉中断。除此之外，视觉测量的频率较低。由于图像处理、传感器数据读取等原因，视觉测量的频率一般远低于惯性测量。仅依靠视觉测量，无法满足实时测量的需求。

转台转角的惯性测量结果见图5。

图5 转台转角的惯性测量Fig.5 Inertial measurement of rotation angle

从图5可知，仅利用陀螺仪去估计角度是不可行的，会在较短时间内产生无法接受的误差。陀螺仪输出的角速度中包含着随机噪声和变化缓慢的零偏等误差项。这些误差项随着时间不断累积，会导致测量误差越来越大，最后结果发散。所以，必须借助额外的信息对角偏移以及零偏进行估计和补偿。但是惯性测量具有不受视场等限制，测量速度快等优点。

由上述分析可知，视觉和惯性是高度互补的两种测量手段。将视觉测量和惯性测量进行数据融合，不仅能解决视场限制的问题，还能提高测量稳定性和测量速度。本系统中，视觉输出的频率为50 Hz，陀螺仪输出频率为1000 Hz，将二者通过容积卡尔曼滤波进行多频率数据融合，可以达到与陀螺仪输出相同的测量速度。

角度测量的融合结果和融合误差如图6所示。

图6 融合结果和融合误差Fig.6 Fusion results and errors

两台陀螺仪的零偏初值均设为零，在测量初始时，误差存在一个峰值，这是由于在滤波对于零偏的估计还没有收敛的情况下，视觉的突然消失导致了陀螺仪角速度中的零偏无法被充分补偿，产生较快的误差累积。但几十秒后，零偏估计逐渐接近真值，即使在视觉不存在的时间段中，基于纯惯性的角度估计误差增加也比较缓慢。这说明，算法能够有效地利用视觉对两台陀螺仪的零偏进行较为准确的估计。整体来看，测量的绝对误差在绝大多数时间内低于0.1°，精度较高。表2中列出了测量角度的均方根误差。

表2 单轴运动不同算法的姿态角均方根误差

由于初始时刻陀螺仪零偏不准导致的误差，使得整体融合的精度下降，但是即使在这种情况下，融合结果的均方根误差仍小于0.1°，而在零偏估计收敛后，融合的精度得到了大幅度的提高。由于实验中视觉的测量范围较小，且存在很多纯惯性测量的区间，所以方位角的融合误差大于纯视觉误差，但俯仰角在零偏稳定后的精度高于视觉精度。这个结果证明了融合视觉和惯性不仅能扩大测量范围、提高测量速度和稳定性，还可以在一定程度上提高测量精度。

5 结论

为解决运动基座上的姿态测量问题，本文研究了一种基于视觉和双陀螺仪的姿态测量系统，并对测量系统中的坐标系进行了定义，提出了坐标系全局归一化的方法，并利用容积卡尔曼滤波对视觉和惯性测量数据进行融合，实现了高精度、大范围、快速、稳定的姿态测量。实验证明，数据融合能够实现视觉与惯性两种传感器的优势互补，并有效地对惯性角度误差累积和两台陀螺仪零偏进行校正补偿，在俯仰角范围为±60°、方位角范围为±100°的大范围动基座测量实验中，两个姿态角融合结果的均方根误差均不超过0.1°。