一种多时间尺度融合的视觉辅助惯性定姿算法

一种多时间尺度融合的视觉辅助惯性定姿算法

0惯性系统中的融合算法在微机械系统中，基于传感器的柔性测量单元（imu）被广泛应用于动态跟踪技术，尤其是卫星信号不能穿过建筑物的环境。针对视觉传感器和惯性传感器的信息融合问题，已经出现了众多的融合算法。比如：使用次优H1次最优估计值为姿态角多时间尺度融合算法的具体实现方法如图1所示。首先，对视觉传感器采集的视频进行分帧，每隔a帧（a为自然数）提取一次图片，所提取的相邻两图之间的时间间隔为t其次，每隔b帧（b>a,b为自然数）提取图片，相邻两图之间的时间间隔为t若第二次最优估计值符合定姿的需求，则以第二次最优估计值为姿态角；否则，每隔c帧（c>b,c为自然数）解算得到的数值与第二次最优估计值进行融合，融合后的角度即为姿态角。多时间尺度形成一个逐级反馈调度机制，视觉信息不断更新IMU信息，利用递推的数值方法估计状态变量，不断修正角度误差，直到姿态角精度满足长时间定姿定位的需求。2提取本质矩阵用单目相机采集视频，将视频分帧，根据相邻图片之间的差异，可以估计载体的角度变化量。首先，从单幅图像中提取特征点，提取特征点采用具有保持不变性且处理效率高效的Surf算法由相机成像原理的针孔相机模型得：式中：pp根据归一化后的匹配点位置，有：式中，E为本质矩阵。本质矩阵E包含了相机运动的平移矢量信息t和旋转矩阵R:本质矩阵E为3×3矩阵，由矩阵运算关系和尺度等价性可知，只需5对匹配点的归一化像素位置即可求解出本质矩阵。如果图片中匹配点点数大于5，充分利用这些点，采用随机采样一致性（RandomSampleConsensus,RANSAC）算法，避免错误数据对整体结果的影响。由奇异值分解本质矩阵得到旋转矩阵R，将其转化为比较直观的欧拉角。令绕Z,X,Y轴旋转的角度分别为航向角ψ、俯仰角θ、横滚角γ，根据旋转矩阵，求解姿态角。由视觉估计载体姿态，利用连续的图像序列进行特征匹配得到匹配点，根据匹配点的坐标估计图像之间的角度变化量，进而得出导航坐标系下的角度变化量。3误差估计和噪声修正卡尔曼滤波采用均方误差最小准则式中：Δψ，确定了状态方程和量测方程之后，卡尔曼滤波的过程如下：预测状态：预测协方差矩阵：计算卡尔曼增益矩阵（权重）：更新状态（k时刻的最优解）：更新协方差矩阵（k时刻的状态变量的估计误差均方值最小）：利用视觉相邻图片之间解算结果无累积误差且漂移率小的优势修正陀螺仪的累积误差和较大的漂移率，但引入了观测噪声。根据多尺度融合算法的思路，选择视觉传感器的长时间尺度数据作为观测量，卡尔曼滤波融合后的最优估计作为状态量，根据式（8）～式（12）进行下一个过程的卡尔曼滤波融合，不断减小观测噪声和累积误差，直到精度满足定姿定位的需求。4视觉姿态精度实验采用HI219姿态传感器进行IMU数据的采集，频率为100Hz。采用单目视觉模块进行视觉数据的采集，帧速率30Hz，图像尺寸为1280×720。将视觉传感器和惯性传感器进行安装，由于相机坐标系和IMU坐标系在空间位置上存在安装误差，将它们的坐标系统移到物体坐标系中，以物体坐标系作为中间介质，完成相机传感器坐标系到惯性传感器坐标系的转换。本次实验的转换结果用T来表示：利用Matlab的CameraCalibrator工具箱标定相机的内参矩阵。拍摄不同方向、不同距离、规格为9×12的棋盘格16张，得到相机的内参矩阵K:结果显示，内参矩阵的平均重投影误差是0.07像素，满足求解视觉姿态精度的需求。标定了硬件设备的信息之后，进行数据采集。将固定后的器件安装在转台上，由于单目相机的工作范围有限，载体突然移动一个很大的角度，可能会产生果冻效应。实验中，采取低速的方式，使得特征点能够提取并成功匹配。为了验证结果的有效性和可行性，将载体放置在任意姿态，绕转台的航向轴逆时针缓慢旋转约4min，整个过程中完整地转了5圈，每圈之间稍有停顿。对视频信息进行分帧，每隔2帧选取一幅图片，即视觉传感器的帧速率为10Hz，相邻图片进行图像处理，解算得出地理坐标系下的角度变化量，与IMU解算的姿态角相同时间间隔（0.1s）的角度变化量进行第一次短时间尺度滤波融合，得到第一次最优估计，如图2所示。整个过程中，因为航向角的波动范围较大（（-π，π]），在图中的偏差不是很明显；俯仰角和横滚角的始末偏差和运动过程中的偏差比较明显。进一步，每隔5帧提取一张图片，即周期为0.2s，将其转移到地理坐标系下的角度变化量与第一次卡尔曼滤波后的最优估计进行长时间尺度的数据融合。两次滤波之后，姿态角精度有了提高，如图3所示。单时间尺度和多时间尺度的姿态角数据结果如表1和表2所示。该实验结果表明，相比于单时间尺度融合算法，多时间尺度融合算法在始末误差方面，航向角误差降低了42.29%，俯仰角误差降低了49.19%，横滚角误差降低了72.23%；在整个过程中，航向角误差降低了26.88%，俯仰角误差降低了23.16%，横滚角误差降低了63.21%，并且航向角、俯仰角和横滚角的精度可以保持在2°之内。利用多时间尺度融合的视觉辅助惯性定姿算法，各个角度的始末误差可以控制在1°以内，整个过程中最大偏差保持在2°以内，提高了姿态角的精度，满足在室内环境下长时间定姿定位的需求。5惯性定姿算法本文针对惯性导航中陀螺仪解算具有角度发散和累积误差的问题，提出一种多时间尺度融合的视觉辅助惯性定姿算法。短时间尺度消除IMU