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

来源:用户上传      作者:

　　摘  要： 针对基于微机电系统的惯性传感器中，陀螺仪在姿态解算过程中存在误差累积的问题，提出一种多时间尺度融合的视觉辅助惯性定姿算法。利用视觉传感器在不同时间尺度的角度变化量，不断修正并更新惯性器件的角度累积误差，对姿态角进行多次校正，进而输出较高精度的姿态角。实验结果表明，采用多时间尺度融合的视觉辅助惯性定姿算法，相对于单时间尺度的融合算法，航向角误差降低了27%，俯仰角误差降低了23%，横滚角误差降低了63%，角度误差能够保持在2°以内，满足定姿定位的需求。
　　关键词： 姿态解算; 多时间尺度融合; 惯性测量单元; 数据采集; 姿态角校正; 角度误差更新
　　中图分类号： TN820.2+2?34; TP212.9              文献标识码： A                    文章编号： 1004?373X（2020）12?0014?04
　　Abstract： In allusion to the error accumulation in the attitude resolving of the gyroscope in the inertial sensor based on micro?electro mechanical system， a visual aided inertial attitude determination algorithm based on multi?time scale fusion is proposed. By means of the angle variation of the vision sensor at different time scales， the angle cumulative error of the inertial device are corrected and updated continuously， and the attitude angle is corrected for many times to output the attitude angle with higher precision. The experimental results show that， in comparison with the single?time scale fusion algorithm， the visual aided inertial attitude determination algorithm based on the multi?time scale fusion reduces the course angle error by 27%， the pitch angle error by 23%， and the roll angle error by 63%. The angle error can be able to keep within 2° to meet the needs of attitude?determination and positioning.
　　Keywords： attitude resolving; multi?time scale fusion; inertial measurement unit; data acquisition; attitude angle correction; angle error renewal
　　0  引  言
　　基于微机电系统传感器的惯性测量单元（Inertial Measurement Unit，IMU）在惯性定位技术中得到了广泛应用，尤其是卫星信号不能穿透建筑物的室内环境[1]。由于IMU中的陀螺仪存在较高的漂移误差，而且捷联惯导系统在解算过程中采用积分的方法，导致内部误差和外部误差不断累积，并且缺乏自我纠正的手段，远远达不到高精度室内定位的需求。视觉传感器误差不随时间累积，成本低，但比较依赖光照条件和可视环境[2]。将视觉传感器和惯性传感器的信息进行优势互补：求解载体的姿态采用以惯性传感器为主，视觉传感器为辅的数据融合方式[3]。在无视觉输出信息或视觉信息不充足的情况下，IMU能够持续提供载体的姿态变化;在视觉信息充足的情况下，视觉信息抑制IMU的姿态发散，校正惯性器件的参数，保证载体的姿态精度。
　　针对视觉传感器和惯性传感器的信息融合问题，已经出现了众多的融合算法。比如：使用次优[H∞]滤波迭代融合算法，融合IMU和单目相机的测量结果，用融合结果和惯性结果的差值修正并更新漂移误差曲线，精度达到了理想效果，但计算繁琐[4];将IMU与视觉进行扩展卡尔曼滤波，并与其他算法结合的融合算法，但在融合过程中消除部分IMU累积误差的同时，视觉的误差也会被融进一部分，过滤时条件苛刻，具有一定的局限性[5?7]。基于以上问题，本文提出一种多时间尺度融合的惯性定姿算法，多时间尺度的短时间尺度实时性强，长时间尺度约束短时间尺度，减小观测噪声的累积，长时间尺度和短时间尺度相互影响，利用视觉的不同时间尺度的角度变化量不断地去修正并更新IMU的解算姿态，对角度信息进行多次校正，从而消除IMU解算的累积误差，得到精度较高的姿态角。
　　1  多时间尺度融合算法
　　多时间尺度融合算法的具体实现方法如图1所示。
　　首先，对视觉传感器采集的视频进行分帧，每隔a帧（a为自然数）提取一次图片，所提取的相邻两图之间的时间间隔为t1。此时，视觉信息得到的是载体坐标系下以t1为周期的角度变化量，根据姿态矩阵，将其转换到地理坐标系下作为融合值。视觉传感器的帧速率小于惯性传感器的频率，惯性器件利用四阶龙格?库塔法更新四元数[8]，解算的每t1时间内数据不止一个，因此惯性数据具有一定的累积误差。惯性器件解算的每t1时间内姿态角的变化量作为融合值，进行第一次数据融合，即短时间尺度融合，保证结果的实时性，输出第一次最优估计值。 　　其次，每隔b帧（b>a，b为自然数）提取图片，相邻两图之间的时间间隔为t2，显然，t2>t1。以t2为周期的视觉角度变化量和第一次最优估计值中每t2时间的角度变化量作为融合值，进行第二次数据融合，即长时间尺度融合，约束短时间尺度，减小短时间尺度累积的观测噪声，输出第二次最优估计值。
　　若第二次最优估计值符合定姿的需求，则以第二次最优估计值为姿态角;否则，每隔c帧 （c>b，c为自然数）解算得到的数值与第二次最优估计值进行融合，融合后的角度即为姿态角。
　　多时间尺度形成一个逐级反馈调度机制，视觉信息不断更新IMU信息，利用递推的数值方法估计状态变量，不断修正角度误差，直到姿态角精度满足长时间定姿定位的需求。
　　2  视觉姿态解算
　　用单目相机采集视频，将视频分帧，根据相邻图片之间的差异，可以估计载体的角度变化量。
　　首先，从单幅图像中提取特征点，提取特征点采用具有保持不变性且处理效率高效的Surf算法[9];其次，匹配相邻图像的特征点，利用穷举法对图片中的每一个特征点与其相邻图片中的所有特征点测量描述子的距离，方差最小的点为匹配点;最后，根据匹配的像素点求解相关参数，问题的实质为对极几何约束[10]。
　　利用视觉相邻图片之间解算结果无累积误差且漂移率小的优势修正陀螺仪的累积误差和较大的漂移率，但引入了观测噪声。根据多尺度融合算法的思路，选择视觉传感器的长时间尺度数据作为观测量，卡尔曼滤波融合后的最优估计作为状态量，根据式（8）～式（12）进行下一个过程的卡尔曼滤波融合，不断减小观测噪声和累积误差，直到精度满足定姿定位的需求。
　　4  实验结果与分析
　　采用HI219姿态传感器进行IMU数据的采集，频率为100 Hz。采用单目视觉模块进行视觉数据的采集，帧速率30 Hz，图像尺寸为1 280×720。将视觉传感器和惯性传感器进行安装，由于相机坐标系和IMU坐标系在空间位置上存在安装误差，将它们的坐标系统移到物体坐标系中，以物体坐标系作为中间介质，完成相机传感器坐标系到惯性传感器坐标系的转换。本次实验的转换结果用T来表示：
　　标定了硬件设备的信息之后，进行数据采集。将固定后的器件安装在转台上，由于单目相机的工作范围有限，载体突然移动一个很大的角度，可能会产生果冻效应。实验中，采取低速的方式，使得特征点能够提取并成功匹配。
　　为了验证结果的有效性和可行性，将載体放置在任意姿态，绕转台的航向轴逆时针缓慢旋转约4 min，整个过程中完整地转了5圈，每圈之间稍有停顿。
　　对视频信息进行分帧，每隔2帧选取一幅图片，即视觉传感器的帧速率为10 Hz，相邻图片进行图像处理，解算得出地理坐标系下的角度变化量，与IMU解算的姿态角相同时间间隔（0.1 s）的角度变化量进行第一次短时间尺度滤波融合，得到第一次最优估计，如图2所示。整个过程中，因为航向角的波动范围较大（（-π，π]），在图中的偏差不是很明显;俯仰角和横滚角的始末偏差和运动过程中的偏差比较明显。
　　进一步，每隔5帧提取一张图片，即周期为0.2 s，将其转移到地理坐标系下的角度变化量与第一次卡尔曼滤波后的最优估计进行长时间尺度的数据融合。两次滤波之后，姿态角精度有了提高，如图3所示。
　　单时间尺度和多时间尺度的姿态角数据结果如表1和表2所示。
　　该实验结果表明，相比于单时间尺度融合算法，多时间尺度融合算法在始末误差方面，航向角误差降低了42.29%，俯仰角误差降低了49.19%，横滚角误差降低了72.23%;在整个过程中，航向角误差降低了26.88%，俯仰角误差降低了23.16%，横滚角误差降低了63.21%，并且航向角、俯仰角和横滚角的精度可以保持在2°之内。利用多时间尺度融合的视觉辅助惯性定姿算法，各个角度的始末误差可以控制在1°以内，整个过程中最大偏差保持在2°以内，提高了姿态角的精度，满足在室内环境下长时间定姿定位的需求。
　　5  结  语
　　本文针对惯性导航中陀螺仪解算具有角度发散和累积误差的问题，提出一种多时间尺度融合的视觉辅助惯性定姿算法。短时间尺度消除IMU的累积误差并保持结果的实时性，长时间尺度约束短时间尺度并减小累积观测噪声，长、短时间尺度相互影响，相互作用。通过实验结果可以看出，此算法能够有效地抑制陀螺仪的发散速度和姿态角的累积误差，精准地校正了姿态角，实现了姿态角高精度的稳定输出。
　　参考文献
　　[1] 王奇，黎海涛.基于机器学习与惯性导航的室内定位技术研究[J].电子测量技术，2016，39（8）：138?143.
　　[2] SHABAYEK A E R， DEMONCEAUX C， MOREL O， et al. Vision based UAV attitude estimation： progress and insights [J]. Journal of intelligent & robotic systems， 2012， 65（1/4）： 295?308.
　　[3] 史俊，杨功流，陈雅洁，等.视觉辅助惯性定位定姿技术研究[J].航空计算技术，2016，46（1）：119?122.
　　[4] 孙长库，徐怀远，张宝尚，等.基于IMU与单目视觉融合的姿态测量方法[J].天津大学学报（自然科学与工程技术版），2017，50（3）：313?320.
　　[5] ALATISE M B， HANCKE G P. Pose estimation of a mobile robot based on fusion of IMU data and vision data using an extended Kalman filter [J]. Sensors， 2017， 17（10）： 2164.
　　[6] OH H， YOU S C， AN J， et al. Pose estimation of surgical instrument using sensor data fusion with optical tracker and IMU based on Kalman filter [J]. MATEC web of conferences， 2015， 32： 04008.
　　[7] 夏凌楠，张波，王营冠，等.基于惯性传感器和视觉里程计的机器人定位[J].仪器仪表学报，2013，34（1）：166?172.
　　[8] 冀亮，钱正洪，白茹.基于四元数的四轴无人机姿态的估计和控制[J].现代电子技术，2015，38（11）：112?116.
　　[9] JINDAL A， AGGARWAL N， GUPTA S. An obstacle detection method for visually impaired persons by ground plane removal using speeded?up robust features and gray level co?occurrence matrix [J]. Pattern recognition and image analysis， 2018， 28（2）： 288?300.
　　[10] 詹煜欣，董文永.基于对极几何约束的动态背景下运动目标检测[J].计算机应用研究，2018，35（11）：268?271.
　　[11] 杨承凯，曾军，黄华.多传感器融合中的卡尔曼滤波探讨[J].现代电子技术，2009，32（14）：159?161.
转载注明来源:https://www.xzbu.com/8/view-15248553.htm