1、引用格式：徐丹旸，宋潇 动基座视觉双惯性姿态测量数据融合方法研究 电光与控制，（）：,（）：动基座视觉双惯性姿态测量数据融合方法研究徐丹旸，宋 潇（河南科技大学，河南 洛阳）摘 要：由于视觉传感器与惯性传感器的高度互补性，视觉和惯性融合是一种常见的姿态测量手段。随着机载、车载等仪器设备的迅速发展，传统静止基座下的姿态测量已无法满足一些实际应用场景的需求。由于受到基座运动信息的干扰，很难对动基座上的目标物进行准确、快速的姿态测量。为了发挥视觉和惯性融合的优势，并实现动态基座下的目标物相对姿态测量，提出了一种基于单目视觉与双陀螺仪的姿态测量系统。分析了测量系统中坐标系的建立以及坐标系之间相对关系，
2、提出了坐标系归一化方案，并利用容积卡尔曼滤波对视觉和惯性结果进行融合，实现了高精度、大范围、快速、稳定的姿态测量。在搭建的视觉和惯性融合姿态测量系统平台上进行了实验验证，结果证明该系统坐标系归一方案与融合算法的有效性。实验结果表明，所搭建系统具有较高的准确度，融合后的俯仰角和方位角的均方根误差（）均不超过 。关键词：动基座姿态测量；视觉和惯性融合系统；容积卡尔曼滤波；坐标系归一化中图分类号：文献标志码：,（,）：,：；收稿日期：修回日期：作者简介：徐丹旸（），女，湖南长沙人，硕士，讲师。通讯作者：宋 潇（），女，河南洛阳人，硕士，讲师。引言基于视觉和惯性数据融合的姿态确定方法已成功应用于无人地
3、面车辆（）、头部运动跟踪、姿态跟踪等领域，其设计均考虑了单传感器的特点。视第 卷 第 期 年 月 电 光 与 控 制 觉传感器没有累积误差，测量稳定性高，但存在视线和环境光干扰问题；惯性传感器具有重量轻、适合快速运动跟踪的特点，但由于存在严重的漂移问题而缺乏长期稳定性。因此，通常采用视觉和惯性传感器融合技术来克服单一传感器固有的缺点。目前的视觉 惯性组合系统多为相机与单惯性传感器的组合。当目标物在一个运动的基座上做相对运动时，其相对于运动基座的运动与基座本身的运动耦合在一起，无法通过单一陀螺仪有效地分离开来，所以，单惯性传感器无法满足动基座上相对姿态测量的要求。本文介绍并搭建了基于视觉和双陀螺
4、仪的姿态测量系统。在该系统中，惯性测量由两个陀螺仪共同完成：其中一个陀螺仪与运动基座固连，作为辅陀螺仪，用以测量运动基座的角速度；另外一个陀螺仪与目标物固连，作为主陀螺仪，用以测量运动基座和主陀螺仪的耦合角速度。通过对两个陀螺仪的角速度进行差分计算，可以从主陀螺仪数据中剔除运动基座的干扰角速度。在融合视觉测量与惯性测量之前，各个器件的输出系统统一变换到参考系下进行，涉及多个坐标系的转换。为此，本文给出了系统中坐标系归一化方案。卡尔曼滤波是传感器融合的常用方法，但其仅适用于线性系统，而在视觉与惯性融合中，系统方程式是非线性的。为了解决非线性问题，文献提出了一种基于采样点的卡尔曼滤波，即容积卡尔曼
5、滤波。利用一组确定性采样点来捕获系统的相关统计参量，从而避免对非线性系统方程的线性化，提高了滤波算法的稳定性。针对所搭建系统，本文设计了一种容积卡尔曼滤波，有效地实现了对姿态角的估计。本文搭建了基于视觉 双陀螺仪传感器的动基座姿态测量系统，建立了测量系统的坐标系，并提出全局坐标系归一化方案，实现了不同坐标系下传感器数据的统一。利用容积卡尔曼滤波对视觉和惯性结果进行融合，实现了高精度、大范围快速、稳定的姿态测量，最后将该算法在实验平台上进行测试，验证其有效性。视觉和惯性融合姿态测量原理图 所示为基于视觉 双陀螺仪传感器的运动基座姿态测量系统。转台模拟被测目标物，其上固定连接有主陀螺仪和视觉靶标，
6、当转台转动时，主陀螺仪和视觉靶标做同步跟随运动。相机拍摄立体靶标图像，利用姿态解算算法，通过图像中特征点坐标完成视觉姿态解算。当基座运动时，主陀螺仪输出的角速度中包括转台转动角速度和基座运动角速度。而辅陀螺仪仅感知到基座的运动并输出相应的角速度。对主陀螺仪和辅陀螺仪输出角速度进行差分计算，便可将运动基座的运动信息从主陀螺仪输出角速度中剔除，得到转台相对于运动基座的转动角速度。惯性和视觉测量在不同的坐标系中进行，必须将不同的测量统一到相同的参考坐标系下。图 姿态测量系统 ）定义目标坐标系为 系，表示为。目标坐标系与转台固连，随着转台的转动而做同步转动。）定义相机坐标系为 系，表示为。原点 为相机
7、镜头的光学中心，轴与镜头光轴方向一致。）定义视觉靶标坐标系为 系，表示为。本文中，靶标坐标系即为对靶标 点坐标测量的双目测量系统的坐标系。）定义主陀螺仪坐标系为 系，表示为。）定义辅陀螺仪坐标系为 系，表示为。）定义参考坐标系为 系，表示为。令参考坐标系与转台位于零位时的转台坐标系重合。与目标坐标系不同的是，参考坐标系与运动基座固连，不随转台的转动而运动。在测量中，靶标坐标系、主陀螺仪坐标系和目标物坐标系之间的关系是固定不变的。在转台和摇摆台运动中，由于相机和辅陀螺仪是固定连接的，所以相机坐标系 系和辅陀螺仪坐标系 系之间的转换关系也是固定的，分别利用旋转矩阵，来表示 系到 系、系到 系与 系
8、到 系的变换关系，需要在测量开始之前对这 个旋转矩阵进行标定。辅陀螺仪坐标系到主陀螺仪坐标系的旋转矩阵可以通过以下的差分方程计算得到（）（）（）式中，为 系相对于 系的角速度。在 系中，和 分别为主陀螺仪和辅陀螺仪输出角速度。斜对称矩阵 定义为第 卷电 光 与 控 制徐丹旸等：动基座视觉双惯性姿态测量数据融合方法研究 （）（）（）（）（）（）（）由式（）、式（）可以看到，在进行测量之前，必须对各个坐标系进行归一化，即标定系统中固定不变的旋转矩阵，和。全局坐标系归一化 标定 标定中，主陀螺仪和视觉靶标相对位置关系保持不变。在适合的距离，正对于视觉靶标安装一个高分辨率相机。在标定过程中，相机保持静
9、止，主陀螺仪和靶标一起做多个自由度的运动。在这个过程中，相机不断拍摄靶标图像，标定主陀螺仪输出角速度。图 所示为靶标系与主陀螺仪系标定。图 靶标系与主陀螺仪系标定 图 中，假设两相邻图像时刻，对应的靶标坐标系到高分辨率相机的坐标系旋转矩阵分别为，和，。由于惯性输出频率高于视觉输出频率，所以在时刻 之间，会有多组惯性输出。利用方向余弦矩阵的迭代公式，对陀螺仪角速度积分，可以求得时刻 的主陀螺仪坐标系的旋转变换为，。假设在任意小的时间区间，）中，陀螺仪角速度 为常值，则方向余弦矩阵 的迭代式为（）（）（）式中：表示单位矩阵；，为采样间隔。旋转矩阵，和，之间存在如下关系，（，），（）用，和，（，），
10、分别表示主陀螺仪和视觉靶标的相对旋转增量，式（）可简化为，（）用四元数，和，来表示式（）中的旋转矩阵，和，得到，（）由四元数相乘的性质，可得，（），（）其中：；（）；（）为四元数 的标量部分；为四元数 的矢量部分；，分别表示两种计算方式。结合式（）、式（）和式（），可得（，），（）假设共有 帧有效图像，可求解为：（）式中：矩阵 为 ，；（）（，），是为了防止视觉误差导致计算不准确而引入的权重矩阵，权重系数可以由估计误差来调整，若误差越大，则该相邻两帧图像的数据权重越小（，）（）式中，为调整因子。通过不断地根据估计误差调整 的值，迭代计算，能不断被优化，直到两次迭代中的值变化小于阈值。标定 转台
11、上固定安装主陀螺仪，分别绕其两个轴进行单轴匀速运动。设转台围绕其俯仰轴 匀速转动时，旋转角速度在 为，主陀螺仪输出即坐标系下的角速度为。设转台围绕其方位轴 匀速转动时，旋转角速度在 为，主陀螺仪输出即 坐标系下的角速度为。，与 的关系为 （）（）由式（）可以构造矩阵方程 （）式中：，为已知转台运动角速度；，分别为在 第 期两个转轴匀速转动时陀螺仪输出的均值。可解得 为 。（）标定 辅陀螺仪坐标系到相机坐标系的标定，与主陀螺仪坐标系到靶标坐标系的标定类似。标定过程中，辅陀螺仪和相机位置关系始终保持不变。在适合的距离正对相机固定放置一个高精度靶标。在标定过程中，靶标始终保持静止，辅陀螺仪和相机一起
12、做多自由度运动。整个过程中，相机不断拍摄靶标图像，辅陀螺仪输出角速度值。假设两相邻图像时刻，对应的相机坐标系到高精度靶标坐标系的旋转矩阵为，和，。时刻 的辅陀螺仪坐标系的旋转变换为，。旋转矩阵，和，之间存在如下关系，（，），。（）旋转矩阵最优正交化由于旋转矩阵具有正交性，所以在标定和测量中，旋转矩阵需要进行最优正交化。假设存在旋转矩阵，则对其最优化的迭代算法为（）（）（）（）（）（）（）式中：为迭代次数；为 的初始估计值。当条件（）（）成立时，完成迭代，把满足式（）的（）作为 的标定结果。式中：与 分别代表两次相邻迭代；为设定的最优化阈值。基于容积卡尔曼滤波的数据融合在本系统中，状态向量 除了
13、包括待估计的 个姿态角，还包括对 个陀螺仪的零偏。相对于欧拉角，四元数具有不存在万向锁的优点，故状态中的角度可以用四元数 来替代表示。由于四元数中参数冗余，用矢量部分 来代替四元数进行求解。取状态向量，是 维向量。具体滤波步骤如下。）计算容积点。（），（）式中：为容积点个数；，为状态向量维数；为 时刻的后验状态误差方阵；表示对 维单位向量 ，的元素改变元素符号和进行全排列产生的点集的第 列；包含四元数矢量部分、主陀螺仪漂移部分与辅陀螺仪漂移部分 部分，即 ，。（）四元数和陀螺仪漂移部分容积点为，（）式中，。四元数 和 ，是相应四元数矢量对应的完整四元数，算式为 ，（），（，），。（）计算经过状
14、态方程传递后的容积点，（），（）其中：，为利用估计零偏补偿后的陀螺仪角速度，（，）。）计算 时刻的一步状态预测值。因四元数求均值不能直接相加，可求得 ，（，）（）式中：（，），（，）（），（），（），（，）（），。（）计算状态误差协方差矩阵 （）（）式中，为预先设定的系统过程噪声。）计算更新后的状态容积点 （）（），（）容积点为第 卷电 光 与 控 制徐丹旸等：动基座视觉双惯性姿态测量数据融合方法研究，()（），（），（）其中，。）预测测量值为，（，）（）式中，（，），（，）。，（），。（）计算信息，（）。（）计算卡尔曼滤波增益为 （）（）式中：，（，）（），（，）（），（，）（，）（）（），
15、（），（）其中，为预先设定的观测噪声。）计算 时刻的状态估计值（）（），（）状态中零偏和四元数部分更新为，（），（）。（）状态误差协方差矩阵更新为 。（）实验测试及结果分析动基座视觉双惯性姿态测量系统装置如图 所示。相机型号为 ，中的陀螺仪作为系统中的陀螺仪。相机和陀螺仪参数见表。图 测量系统实验平台 表 陀螺仪和相机参数 仪器参 数数值陀螺仪零偏不稳定性（）角度随机游走（）相 机水平 垂直像素尺寸 分辨率 像素 为了验证测量的精度，利用一个高精度二维转台作为目标运动物体，主陀螺仪、立体靶标通过螺丝安装在转台上，转台固定在摇摆台上。相机和辅陀螺仪通过光学支撑棒固定在摇摆台上。当转台位于零位时，
16、靶标正对相机。实验中使用的三维立体靶标内嵌 个非共面红外 点，波长为 。通过对特征点的几何特征等的筛选，可以排除背景中的干扰，实现对特征点的定位提取。为了验证本系统滤波算法的有效性，利用搭建的系统进行角度测量实验，该系统中转台用以模拟头部运动姿态。对于实际的头部运动，俯仰运动的范围相对较小，而方位运动的范围相对较大。所以在实验中，转台从零位开始，俯仰角以 为间隔，从 运动到。在每个间隔中，方位轴先从 运动到，再从 运动到，以 为间隔往返运动。转台实际转动角度和视觉测量结果见图。图 转台实际转角和视觉测量 由于本系统中的转台仅可以绕两个轴进行转动，所以仅研究俯仰角度和方位角度的估计效果。可以发现，第 期视觉测量的范围有限，当转动角度大于一定程度时，相机就无法拍摄到 个特征点，视觉信息缺失。在实际应用中，视线的模糊、环境的干扰等也会导致视觉中断。除此之外，视觉测量的频率较低。由于图像处理、传感器数据读取等原因，视觉测量的频率一般远低于惯性测量。仅依靠视觉测量，无法满足实时测量的需求。转台转角的惯性测量结果见图。图 转台转角的惯性测量 从图 可知，仅利用陀螺仪去估计角度是不可行的，会在较短时