0 引言
以多旋翼无人机为主的小型空中机器人技术取得了长足发展,广泛应用于军事侦察、武装打击、人员搜救,场景重建等领域。上述应用场景中,无人机主要依靠工作人员遥控操作,只有少部分可以根据电脑终端设定的轨迹进行小范围、低复杂度的飞行。小型多旋翼无人机的定位是实现其在复杂环境下自主移动的基本问题,在军事、民用等多种领域下有着广阔的应用前景。对于实现无人机的智能化与自主化而言,最重要的前提是无人机能够利用机载传感器完成对环境的感知和自身位姿的精确估计。常见机载传感器有GPS、北斗、惯性测量单元、视觉传感器和激光传感器等。GPS、北斗等卫星导航定位系统具有全天候工作、覆盖范围广等特点,将其与惯性测量单元融合,可以获得较高的位姿估计精度,在军用和民用无人机上有较多应用,但当无人机在城市或丛林等遮挡严重的环境中时,卫星导航系统的定位数据不可靠,甚至无法使用[1]。文献[2]利用激光雷达产生的点云数据实现无人机在缺乏GPS信号情况下进行自身定位,但激光雷达体积大、且价格昂贵,不适用于小型旋翼无人机。文献[3]使用单目相机估计无人机位姿并基于Apriltag2进行融合以获取深度信息,该方法可以弥补单目相机无法获取环境尺度的缺陷,尺度信息依赖于Apriltag2,在室内场景下有较好效果,但不适用于室外环境。文献[4]提出了使用RTK技术辅助定位的方案,在室外可以获得较高的定位精度,但无人机搭载的相机仅用于拍摄与后期数据处理,没有介绍在RTK信号覆盖不到场景下的解决方案。文献[5]则进一步将RTK定位数据融合到基于视觉的定位方案中,通过扩展卡尔曼滤波算法将不同信息进行有效融合,有利于解决由于累积误差造成的漂移问题,但该方式需要事先在环境中架设RTK基站,不具有普适性。文献[6]使用视觉与高度传感器进行无人机在室内环境下的定位,但只适用于低速运动情况,且在室外光线变化大时效果不理想。基于优化的视觉惯性里程计(visual-inertial-odometry,VIO)采用视觉和惯性测量单元紧耦合的方式来估计本体运动状态[7],充分结合了2种传感器的特性,在GPS信号缺失的场景下具有定位精度高、成本低等优点,因此该方式逐渐成为当前主流发展趋势,受到了普遍关注。
针对小型多旋翼无人机在无GNSS信号的复杂场景下对自身位姿的估计问题,在基于优化的视觉惯性里程计基础上,提出改进的里程计并构建一套无人机系统进行验证。通过改进后的RANSAC算法剔除外点,设计飞控通信接口与控制模块并搭建了gazebo仿真环境。实验验证,改进后算法具有良好的定位精度与实时性,对无人机在复杂环境下的自主运动具有实际工程意义。
1 视觉惯性里程计
1.1 视觉SLAM数学模型
经典的视觉SLAM数学模型如下:
(1)
式(1)表示运动方程和观测方程,xk与xk-1分别表示k,k-1时刻的位姿,uk表示k时刻的运动输入量,wk表示对应的噪声。zk, j表示k时刻,在xk位姿下对yj路标的观测值,vk, j表示k时刻的观测噪声。在视觉惯性里程计中, f(xk-1,uk)由惯性测量单元的数学模型确定,h(yj,xk)由相机投影模型确定。惯性测量单元通过预积分模型构造残差项,而视觉部分通过重投影误差构造残差项,最后利用非线性优化算法求解代价函数即可得到融合后的最优状态量。
1.2 对极约束与基础矩阵
相机分别从视角1与视角2观察三维空间中同一点P,在P点未知的情况下,求解视角1到视角2相机的运动,可以通过对极几何约束解决,如图1所示。
图1 对极几何
Fig.1 Epipolar geometry
对极约束关系可表示为公式(2):
(2)
式中: p1与p2为真实物理世界中某点P分别在视角1与视角2下的投影, F为3×3的基础矩阵。p1与p2使用齐次坐标,在相差一个常系数情况下式(2)仍然成立,因此F矩阵的自由度为8,一般使用8点法[8]计算。假设p1=[x1,y1,1]T, p2=[x2,y2,1]T,代入式(2),得到:
(3)
将基础矩阵F各元素展开为一个向量处理,可得到8点法求解基础矩阵的一般式。
文献[9]认为,原始图像坐标点的齐次值各分量的数量级相差太大,导致8点法求解基础矩阵的结果不稳定,提出一种预处理方式,先对像素坐标进行归一化处理,再使用8点法求解基础矩阵,最后通过基础矩阵分解得到相机运动的旋转矩阵和平移向量。
在已知三维空间点和像素点的情况下,可以通过n点透视法(Perspective-n-Point,PnP)求解相机位姿。无论是通过对极约束还是PnP方法求解相机位姿,求解精度与特征点的匹配精度都有很大关系,因此,对前端算法进行改进,通过剔除错误匹配点,达到提高里程计精度与鲁棒性的目的。
2 改进RANSAC算法剔除外点
2.1 RANSAC算法
根据前端光流追踪或特征匹配算法,不同视角下两幅图像的匹配点通常远远大于8对,为充分利用更多的点计算基础矩阵,可以采用最小二乘算法。但在实践中考虑匹配存在误差,由于噪声的存在,匹配点对之间并不能严格遵循式(2),最小二乘算法中错误的匹配数据可能会导致结果偏离真实值。随机采样一致性算法(random sample consensus,RANSAC)是一种从包含错误值的数据中迭代估计模型参数的算法[10]。它具有异常检测的特性,在视觉惯性里程计中,将错误匹配点称为外点,正确的匹配点称为内点,借助该算法可以实现2个作用:在缺失3D点时计算基础矩阵,从而进一步分解出相机的运动姿态;向里程计后端发送匹配点对之前,通过对极约束计算基础矩阵的方式,尽可能剔除外点,为后端优化创造更好的条件。
传统的RANSAC算法包含以下几个步骤:
1) 将所有匹配点集S中的特征点进行归一化处理。
2) 在匹配点集S中随机采样抽取8对点,采用8点法计算基础矩阵F。
3) 在基础矩阵F下,计算所有匹配点的重投影误差e,并与预设阈值T进行比较,当e≤T时,记该点为内点,否则将该点标记为外点,通过内点占总匹配点的比例计算该模型得分score。
4)重复步骤2)、3),直到达到最大迭代次数或满足内点比例超过预设值,获取得分最高的结果作为基础矩阵F,并从匹配点集S中剔除该估计下的所有外点。
2.2 改进的RANSAC算法
2.2.1 带优先级的样本点采集策略
实际环境中由于存在动态目标的干扰,如果某次采样的匹配点位于动态目标区域,基础矩阵的估计可能会失效,尽管RANSAC算法能够在一定程度上避免这种情况,但传统RANSAC算法的样本点采集完全随机,如果多次采样都存在样本点位于动态目标区域的情况,基础矩阵估计的精度和鲁棒性会降低[11]。
因此本文中提出一种带优先级的样本点采集策略:首先通过目标检测算法对潜在的动态目标进行检测,获取目标二维包围框集合B,假设b为检测结果集合B中某一目标包围框,其满足:
b=[cx,cy,h,w]T
(4)
式中: cx、cy表示包围框b的中心点像素坐标,h和w分别表示包围框b的高和宽。其次,构造特征容器与哈希表,特征容器的元素保存特征点的二维像素坐标,哈希表的键码k保存特征点在容器中的索引,对应的值记为mp[k],它的值由特征点坐标值与集合B中各包围框的关系确定,计算公式如下:
(5)
(6)
式(5)中,m为大于0的正整数,px与py表示键码k所对应特征点的像素坐标,改进后带优先级的样本点采集策略为:在一次迭代中,对于某特征点p,先在哈希表中查询其对应值是否为0,如果为0,则将其作为一个样本点,如果是大于0的正整数,则将其值减1,重新选择样本点。通过优先级的设置,在采集样本点时将优先采集潜在的动态目标区域之外的特征点,从而在一定程度上提高基础矩阵的估计精度。本文中并没有将潜在的动态目标包围框内部的特征点直接剔除,因为仅依靠目标检测算法获取的包围框内的目标并不一定处于运动状态,如果目标所占图像比例较大且处于静止状态,直接剔除无疑会浪费较多的有效特征点,因此,在不借助其他更加复杂算法如目标跟踪算法的前提下,通过对预设值m的自减,既提高了包围框外部特征点优先被采集的概率,又不会完全剔除该部分特征点,一定程度提高了算法的鲁棒性。
2.2.2 并行化计算
RANSAC算法虽然能够将匹配点划分为内点与外点,但如果迭代次数过少,无法保证得到的基础矩阵精度与外点的剔除效果,迭代次数过多,又会影响系统的实时性。针对该问题的一般做法是先设置一个最大迭代次数,当某次迭代计算得到一个得分最高的F矩阵时,更新迭代次数,如果达到最大迭代次数或当前得分最高的解对应的内点比例超过设定值,则终止算法,具有一定通用性。内点比例的设定是一个经验值,虽然可以将迭代次数限制在较低水平,在一定程度上保证较高的内点比例,但由于迭代次数的快速下降,最终可能导致所估计的基础矩阵F不是全局最优解。文献[12]通过加入预检验过程,过滤掉偏差较大的模型参数,一定程度上提高了算法的效率,但计算过程过于复杂。文献[13]通过对样本点集合预先设置,使初始样本集合中内点比例增加,从而减少迭代次数。
针对该问题,采用多线程技术,对基础矩阵F估计进行并行化计算的改进,达到保证计算效率的同时,提高内点比例的效果。具体地,首先获取系统当前可用进程数N,开辟n个线程执行改进后的RANSAC算法,其中n≤N。所有线程各自维护一个该线程的最优基础矩阵。最后,从所有线程中寻找得分最高的基础矩阵,并将该基础矩阵对应的所有外点从匹配点集合中剔除。
3 无人机系统软硬件设计
3.1 硬件系统搭建
系统硬件组成如图2所示,板载计算机使用Intel NUC11TNKi5,主控芯片为11代酷睿i5系列,具有四核八线程,同时具有8GB DDR4主存。飞控使用Holybro Pixhawk4 mini,其主控为ARM Cortex M7,并内置了BMI055惯性测量元件,输出频率可达200 Hz。视觉传感器使用Intel d455实感相机,分辨率为640×480,帧率设置为30FPS,RGB彩色相机水平视角86°±3°,垂直视角57°±3°。
图2 系统硬件组成
Fig.2 System hardware composition
3.2 软件总体设计
VINS-Fusion[14]是目前先进的基于优化的多传感器融合里程计通用框架,将本文改进后的RANSAC算法应用于该系统并作为无人机位姿来源。软件系统基于ubuntu和ROS框架实现,主要包括部分:改进的视觉惯性里程计、飞控通信接口和控制器,软件系统工作框图如图3所示。
图3 软件系统工作框图
Fig.3 Software system block diagram
3.3 飞控通信接口设计
硬件平台所使用的Holybro Pixhawk4 mini飞控采用mavlink通信协议,在ROS框架下,需要借助mavros功能包与板载计算机进行通信,通过mavros进行飞控状态查询、控制量输出等操作变得异常复杂,为了提高调试与开发的便捷性,设计了专门的通信接口,对mavros话题进行了封装。
无人机状态量state包含:连接状态connected、加解锁状态arm、模式mode、系统启动时间start_time、当前位置pos、当前速度vel、当前姿态q、欧拉角rpy、三轴角速度angular_v和三轴加速度linear_a。其中当前姿态q与欧拉角rpy分别满足式(7)与式(8):
q=(w,x,y,z)
(7)
(8)
本方案利用不同mavros消息将无人机控制器封装成接口形式,直接调用相应接口即可实现不同控制方式,控制系统采用状态模式设计,控制系统状态转移图如图4所示。
图4 控制系统状态转移图
Fig.4 Control system state transition diagram
3.4 仿真系统
为高效便捷的验证算法有效性,选取gazebo物理仿真平台作为本方案的仿真软件,该平台能够提供高保真度的物理模拟。将双目相机模型搭载于四旋翼无人机,该相机具有90°水平视角,使用开源协议PX4作为飞控软件,其内部的惯性元件数据及无人机搭载的相机图像,可作为视觉惯性里程计的输入数据,仿真框架示意图如图5所示。
图5 仿真框架示意图
Fig.5 Schematic diagram of the simulation framework
为模拟现实世界的复杂环境,通过树木、车辆等模型构建了仿真环境如图6所示。
图6 仿真环境
Fig.6 Simulation environment
4 实验验证与结果分析
4.1 改进RANSAC算法实验
为验证改进RANSAC算法在实际场景中的效果,本文进行如下实验,其中所涉及硬件使用图2所示平台。如图7为南京理工大学校园内采集的6组实验图像,其中每组图片由两幅相机在不同视角下对同一场景拍摄的图像组成。实验时直接使用无人机搭载的d455相机拍摄,每幅图像的分辨率为640×480。特征点使用opencv库的ORB[15]提取器进行提取,描述子分别使用256位的BRIEF描述子[16]和512位的BAD描述子[17],6组图像使用传统RANSAC与改进后算法进行内点比例的比较,置信度设置为0.99,其他参数均一致,结果如图8、图9所示。为验证改进后算法的时间效率,对6组图像在256位的BRIEF描述子下2种算法分别进行100次测试,并计算平均值,得到结果如图10所示。
图7 实验图像
Fig.7 Experimental image
图8 采用BRIEF描述子的内点比例
Fig.8 Inlier scale using the BRIEF descriptor
图9 采用BAD描述子的内点比例
Fig.9 Inlier scale using the BAD descriptor
图10 运行时间对比
Fig.10 Running time comparison
分析实验结果可知,改进的RANSAC算法在运行时间方面与传统RANSAC算法基本一致,满足无人机应用中实时性的要求。算法理论上可以获取更多组解,从而提高基础矩阵的估计精度,从实验上看,改进后的算法在不同特征点数目上的内点比例均高于传统算法,因此能够更好剔除外点。
4.2 数据集里程计测试
本文使用EuRoC MAV真实场景的视觉惯性数据集[18]评估改进后的算法,数据集使用一个小型无人机采集,其中包括双目图像,频率为200 Hz同步的惯性测量数据,真实状态使用vicon运动捕捉系统和Leica MS50全站仪提供。在本实验中,使用MH_01和MH_02两个数据集评估算法对VINS-Fusion的改进效果,通过evo[19]评估工具对程序运行结果进行分析,轨迹误差图如图11、图12所示。与未改进的VINS-Fusion在绝对误差估计下得到的均方根误差(root-mean-square-error,RMSE)进行对比如表1所示。
图11 MH_01轨迹误差
Fig.11 MH_01 trajectory error
图12 MH_02轨迹误差
Fig.12 MH_02 trajectory error
表1 RMSE轨迹估计误差参数
Table 1 RMSE trajectory estimation error parameters
数据序列VINS-Fusion/m改进后VINS-Fusion/mMH_010.2430.240MH_020.1890.170
最后,使用无人机进行飞行实验如图13所示,实验时Intel d455相机双目图像帧率设置为30 FPS,满足实时性要求。
图13 飞行试验
Fig.13 Flight experiment
5 结论
1) 改进的RANSAC算法,在计算基础矩阵时平均内点比例提高1.8%。
2) 将改进的算法嵌入VINS-Fusion,在MH_01序列上的实验,绝对轨迹误差的均方根误差降低了0.001 m,在MH_02序列上的实验,绝对轨迹误差的均方根误差降低了0.019 m。
3) 基于Linux和ROS系统编写无人机通信接口,简化了无人机飞控与板载计算机的通信方式,实现上层规划模块、控制模块和底层飞控的解耦。
4) 算法最终部署到所搭建的无人机平台,达到了30 FPS的实时效果,满足无人机对实时性的要求。
[1] YANG B,YANG E.A survey on radio frequency based precise localisation technology for UAV in GPS-denied environment[J].Journal of Intelligent and Robotic Systems,2021,103(3):1-30.
[2] MILIJAS R,MARKOVIC L,IVANOVIC A,et al.A comparison of lidar-based slam systems for control of unmanned aerial vehicles[C]//2021International Conference on Unmanned Aircraft Systems(ICUAS).IEEE,2021:1148-1154.
[3] 安晨亮,高喜天,冀东.基于Apriltag算法的在线恢复深度的无人机单目SLAM系统[J].自动化与仪表,2021,36(5):16-21,36.
AN Chenliang,GAO Xitian,JI Dong.An uav monocular SLAM system with online recovery depth based on Apriltag algorithm[J].Automation &Instrumentation,2021,36(5):16-21,36.
[4] 漆志军,邹海峰,于群松,等.基于无人机RTK辅助定位技术的研究与应用[J].公路,2020,65(12):201-203.
QI Zhijun,ZOU Haifeng,YU Qunsong,et al.Research and application of RTK aided positioning technology based on UAV[J].Highway,2020,65(12):201-203.
[5] 陈玉权,翟学锋,宋煜,等.无人机高精度定位和视觉自动跟踪融合技术[J].组合机床与自动化加工技术,2021(9):103-106.
CHEN Yuquan,ZHAI Xuefeng,SONG Yu,et al.UAV highprecision positioning and visual automatic tracking fusion technology[J].Modular Machine Tool &Automatic Manufacturing Technique,2021(9):103-106.
[6] TRUJILLO J C,MUNGUIA R,URZUA S,et al.Monocular visual SLAM based on a cooperative UAV-target system[J].Sensors,2020,20(12):3531-3537.
[7] MA S,BAI X,WANG Y,et al.Robust stereo visual-inertial odometry using nonlinear optimization[J].Sensors,2019,19(17):3747.
[8] 吴宇豪,曹雪峰,安籽鹏.自适应阈值的基础矩阵估计算法[J].测绘科学,2019,44(11):22-27,34.
WU Yuhao,CAO Xuefeng,AN Zipeng.Robust estimation method of fundamental matrix with a self-adaptive threshold[J].Science of Surveying and Mapping,2019,44(11):22-27,34.
[9] HARTLEY R I.In defense of the eight-point algorithm[J].IEEE Transactions on Pattern Analysis and Machine Intelligence,1997,19(6):580-593.
[10]LI H,QIN J,XIANG X,et al.An efficient image matching algorithm based on adaptive threshold and RANSAC[J].IEEE Access,2018(6):66963-66971.
[11]ZHANG Z.Determing the epipolar geometry and its uncertainty:A Review[J].International Journal of Computer Vision,1996,27(2):71-76.
[12]化春键,潘瑞,陈莹.基于改进ORB-RANSAC的双目测距方法[J].激光与光电子学进展,2021,58(22):366-373.
HUA Chunjian,PAN Rui,Chen Ying.Binocular ranging method based on improved ORB-RANSAC[J].Laser &Opto-electronics Progress,2021,58(22):366-373.
[13]赵谦,童申鑫,贺顺,等.改进的SURF-RANSAC图像匹配算法[J].计算机工程与设计,2021,42(10):2902-2909.
ZHAO Qian,TONG Shenxin,HE Shun,et al.Improved SURF-RANSAC image matching algorithm[J].Computer Engineering and Design,2021,42(10):2902-2909.
[14]QIN T,PAN J,CAO S,et al.A general optimization-based framework for local odometry estimation with multiple sensors[J].Highway,2018,19(1):36-38.
[15]RUBLEE E,RABSUD V,KONOLIGE K,et al.ORB:An efficient alternative to SIFT or SURF[C]//2011International Conference on Computer Vision.Barcelona:IEEE,2011:2564-2571.
[16]CALONDER M,LEPETIT V,STRECHA C,et al.Brief:Binary robust independent elementary features[C]// 2010 European Conference on Computer Vision.Heraklion:Springer,2010:778-792.
[17]SU
REZ I,BUENAPOSADA J M,BAUMELA L.Revisiting binary local image description for resource limited devices[J].IEEE Robotics and Automation Letters,2021,6(4):8317-8324.
[18]BURRI M,NIKOLIC J,GOHL P,et al.The EuRoC micro aerial vehicle datasets[J].The International Journal of Robotics Research,2016,35(10):1157-1163.
[19]GRUPP M.EVO:Python package for the evaluation of odometry and slam[J].Highway,2017(5):54-62.