作者：王继红, 吴伯彪, 张亚超, 赵明冬

作者单位：郑州科技学院电气工程学院，河南 郑州 450064

关键词：单目视觉;惯性导航;位姿估计;尺度估计;扩展卡尔曼滤波

摘要：

针对无人机在卫星信号失锁时仅利用纯惯导无法精确获取位姿信息的问题，提出通过扩展卡尔曼滤波对单目视觉和惯导进行信息融合的算法，并设计一个包含单目视觉、IMU及超声波传感器的组合系统。首先，利用超声波传感器通过最小二乘法完成尺度估计值的获取；其次建立IMU的系统方程进行惯导信息的解算，并完成误差状态方程的求解；最后，通过扩展卡尔曼滤波实现单目视觉与惯导的信息融合。利用该算法对无人机在200 m动态飞行的信息进行解算，实验结果表明，位置误差的均方根在0.995 m以下，水平姿态角的均方根误差在1.915°以下，偏航角的均方根误差达到了2.235°。不但提高了位姿的估计精度，而且解决了纯视觉输出频率低的问题，满足了无人机对高动态特性的需求，可解决无人机在卫星信号失锁时无法精确定位的问题。

Pose estimation of small UAV based on vision and INS information fusion
WANG Jihong, WU Bobiao, ZHANG Yachao, ZHAO Mingdong
School of Electrical Engineering, Zhengzhou University of Science and Technology, Zhengzhou 450064, China
Abstract: In order to solve the problem that the position and attitude information of UAV can not be accurately obtained only by using pure inertial navigation when the satellite signal is out of lock, an information fusion algorithm based on extended Kalman filter for monocular vision and inertial navigation is proposed, and an integrated system including monocular vision, IMU and ultrasonic sensor is designed. Firstly, the ultrasonic sensor is used to obtain the scale estimation by the least square method. Secondly, the IMU system equation is established to calculate the ins information, and the error state equation is solved. Finally, the information fusion of monocular vision and INS is realized by extended Kalman filter. The experimental results show that the root mean square error of position error is less than 0.995 m, and the root mean square error of horizontal attitude angle is 1.915°. The root mean square error of yaw angle is 2.235°. It not only improves the accuracy of pose estimation, but also solves the problem of low frequency of pure vision output, meets the needs of UAV for high dynamic characteristics, and solves the problem that UAV can not accurately locate when the satellite signal is out of lock.
Keywords: monocular vision;inertial navigation;pose estimation;scale estimation;extended Kalman filter (EKF)
2021, 47(11):134-140,152  收稿日期: 2021-05-07;收到修改稿日期: 2021-06-13
基金项目: 2018年度河南省科技厅科技攻关课题（182102210137）
作者简介: 王继红（1983-），女，河南洛阳市人，副教授，硕士，研究方向为智能检测控制
参考文献
[1] DURRANT-WHYTE H, BAILEY T. Simultaneous localisation and mapping: part I[J]. IEEE Robotics & Automation Magazine, 2006, 13(2): 99-110
[2] KLEIN G, MURRAY D. Parallel tracking and mapping for small AR workspaces[C]//Proceedings of the 2007 IEEE and ACM International Symposium on Mixed and Augmented Reality. Washington, DC: IEEE Computer Society. 2007: 1-10.
[3] MUR-ARTAL R, MONTIEL J M M, TARDOS J D. ORB-SLAM: A versatile and accurate monocular SLAM system[J]. IEEE Transactions on Robotics, 2015, 31(5): 1147-1163
[4] FORSTER C, PIZZOLI M, SCARAMUZZA D. SVO: Fast semi-di-rect monocular visual odometry[C]//Proceedings of the 2014 IEEE International Conference on Robotics and Automation. New York: IEEE. 2014: 15-22.
[5] ENGEL J, SCHOPS T, CREMERS D. LSD-SLAM: Large-scale direct monocular SLAM[C]//Proceedings of the 2014 European Conference on Computer Vision. Cham: Springer International Pubfishing, 2014: 834-849.
[6] 陆艺, 沈添秀, 郭小娟, 等. 线结构光视觉传感器机器人手眼关系标定[J]. 中国测试, 2019, 45(10): 6-9
[7] 胡丙华, 张虎龙, 张杰. 低空摄影测量飞机惯导姿态精度评估技术[J]. 中国测试, 2019, 45(5): 145-150
[8] RIGATOS G G. Extended Kalman and particle filtering for sensor fusion in motion control of mobile robots[J]. Mathematics and Computers in Simalation, 2010, 81(3): 590-607
[9] 陈雷, 张晓明, 檀杰, 等. 基于卡尔曼滤波的旋转弹药弹体磁场校正方法[J]. 中国测试, 2019, 45(2): 48-53
[10] WEISS S, ACHTELIK M W, LYNEN S, et a1. Monocular vision for long·term micro aerial vehicle state estimation: A compendium[J]. Journal of Field Robotics, 2013, 30(5): 803-831
[11] 周绍磊, 吴修振, 刘刚, 等. 一种单目视觉ORB_SLAM/INS组合导航方法[J]. 中国惯性技术学报, 2016, 24(5): 633-637
[12] MOURIKIS A I. ROUMELIOTIS S I. A multi-state constraint Kalman filter for vision-aided inertial navigation[C]// Proceedings of the 2007 IEEE International Conference on Robotics and Automation. New York: IEEE. 2007: 3565-3572.
[13] SHELLEY M A. Monocular visual inertial odometry on a mobile device[D]. Munchen: Technischen Universitat Munchen, 2014: 5-29.
[14] 胡小平. 自主导航技术[M]. 北京: 国防工业出版社, 2016: 9-104.