2. 航天神舟飞行器有限公司, 天津 300301
2. Aerospace Shenzhou Aerial Vehicle Ltd., Tianjin 300301, China
1 引言
无人机的定位问题主要是指利用自身传感器来确定无人机在飞行环境中相对于惯性坐标系的位置和姿态信息. 准确的位姿估计是实现无人机避障、 轨迹规划及目标跟踪等复杂飞行任务的前提和基础. 广泛使用的定位方法主要是基于全球卫星定位系统(GPS)的定位方法[1],然而在一些特殊环境中,例如楼群之间、 大型建筑物内部等,GPS信号很弱,传统定位方法较难应用. 而基于视觉的定位方法仅使用机载摄像头作为外部传感器,具有体积小、 重量轻、 价格低、 精度高等优势,这也使得视觉导航逐渐成为四旋翼无人机自主控制研究的主要趋势.
光流是指图像灰度模式的表面运动,可以看作是带有灰度的像素点在图像平面运动产生的瞬时速度场,它是利用图像序列中像素在时间域上的变化及相邻帧之间的相关性来找到上一帧与当前帧之间的对应关系,从而可计算出相邻帧之间物体的运动信息[2].
清华大学和韩国首尔大学的研究人员分别采用ADNS-2610[3]和ADNS-3080[4]光流传感器获取无人机在空间内的水平速度信息,实现无人机的定点悬停功能. 瑞士苏黎世联邦理工学院使用PX4FLOW光流传感器实现四旋翼无人机的悬停和矩形轨迹跟踪[5]. 但是由于光流法计算的是相邻两帧特征点之间的相对运动,而不是绝对运动,不可避免地面临位置漂移问题,不适合长时间的导航[6].
常用的视觉同步定位与建图(simultaneous localization and mapping,SLAM)主要依靠并行跟踪与建图(parallel tracking and mapping,PTAM)来实现,PTAM是由Klein和Murray于2007年提出来的[7]. 其基本原理是通过跟踪地图中的关键帧及关键帧中的特征点来进行位置和姿态估计,并且建立的地图可以随定位过程而逐步扩充.
近几年,很多高校和研究机构都在进行基于视觉SLAM的四旋翼无人机自主控制系统的研究. 瑞士苏黎世联邦理工学院率先将PTAM算法应用于四旋翼无人机控制上,并实现了基于视觉SLAM的无人机自主飞行控制[8, 9]. 德国慕尼黑工业大学应用视觉SLAM算法实现了四旋翼无人机的自主定位与导航[6],但视觉SLAM算法运行在地面站上,尚未实现机载运算,控制系统存在较大的时间延迟. 瑞典林雪平大学将基于视觉SLAM的状态估计用于非线性自适应控制研究和简单着陆控制的轨迹规划研究[10],仿真结果表明,其系统能够实现四旋翼无人机的自主着陆,但尚未进行实验证明. 美国加州大学河滨分校的研究人员中也使用了视觉SLAM作为位置信息来源,实现无人机的自主定位功能,但该算法运行在地面站上,尚未实现机载控制[11].
上述研究方案虽取得了一定的成果,但是大多数研究机构均采用基于地面站架构的控制方法,在实时性、 有效性和通用性等方面还存在不足. 在这种结构中,机载摄像头采集到的图像信息需经Wifi无线网络传送到地面站处理,处理后产生的控制命令再经Wifi发给无人机的飞行控制器,控制过程存在较大的时间延迟,系统的实时性较差. 此外,采用无线数据传输还限制了无人机的飞行范围,增加了其对传输线路的依赖性,也容易受到数据传输过程中其它信号的干扰. 为解决数据传输延迟,瑞士苏黎世联邦理工学院的研究人员采用有线信号传输方式,但这种方法严重限制了无人机的飞行范围,且不够安全,系统有效性较差[12]. 此外,视觉SLAM算法对地图依赖性较强,在地图信息短暂丢失的情况下,可以使用加速度计来更新位置信息. 但当地图信息存在较长时间丢失的情况下,该方法将会存在较大控制误差,甚至会导致无人机失去控制[13].
考虑到上述研究中存在的不足,本文设计了一种基于嵌入式架构的四旋翼无人机视觉控制系统. 首先,对传统的视觉SLAM算法进行了改进,增加特征点提取数量和优化关键帧存储,降低了对于环境特征信息的依赖. 由于视觉SLAM算法对地图信息的依赖性较强,本文引入光流传感器作为辅助位置信息测量单元,当视觉SLAM算法出现特征匹配地图信息丢失时,程序自动切换为利用光流信息进行辅助控制,提高了无人机飞行控制的安全性. 但是考虑到光流传感器积分过程中的累计误差,当地图信息恢复时,再自动切换回采用视觉SLAM位置信息进行四旋翼无人机的控制. 然后,采用扩展卡尔曼滤波(EKF)融合视觉SLAM或光流传感器提供的位置信息和惯性测量单元(inertial measurement unit,IMU)提供的3维加速度,从而得到精确的位置信息,并提高数据输出频率. 最后,采用上述算法得到的水平位置和速度信息作为反馈量,设计了PID和一种非线性位置控制器,实现四旋翼无人机的自主飞行控制. 上述运算均运行在嵌入式机载计算机上,保证了控制系统的实时性.
与国内外其它基于视觉的四旋翼无人机控制系统相比,本文的创新点列举如下: 第一,本文提出了一种互补的视觉控制算法,使用视觉SLAM算法获取无人机的位置信息并用于自主飞行控制,而当视觉SLAM地图信息丢失情况下采用光流法进行四旋翼无人机的自主飞行控制,提高了无人机飞行控制的安全性,并成功地克服了视觉SLAM图像丢失问题和光流法存在的位置漂移问题,更适用于四旋翼无人机长时间的自主飞行控制. 第二,本文提出了一种非线性控制策略,并首次将其应用于四旋翼无人机的控制实验中,与常用的PID飞行控制方法相比,本文提出的非线性控制算法较好地提高了无人机的控制精度,增加了控制系统的鲁棒性. 第三,本文采用嵌入式机载控制系统,地面站只负责远程监控,避免了图像及控制命令在无线传输过程中引起的时间延迟和信号干扰,保证了闭环系统的实时性,进而提高了无人机的飞行控制精度. 第四,本文提出了一种通用的无人机视觉控制结构,对其它摄像头和无人机平台具有较好的通用性.
2 基于视觉的四旋翼无人机控制设计本文提出了一种改进的视觉SLAM位置估计算法,同时使用光流传感器来获取无人机的速度和位置信息,用于视觉SLAM丢失情况下无人机的自主控制,实现四旋翼无人机的自主飞行控制.
2.1 控制系统中坐标系定义在基于视觉的四旋翼无人机控制系统中使用单目摄像头、 IMU、 光流传感器作为主要传感器. 在整个视觉控制系统中涉及到的主要坐标系如图 1所示.
1) 惯性坐标系{I},是四旋翼无人机自主定位和控制所采用的坐标系,并以正西方向为X轴正方向,正北方向为Y轴正方向,竖直向上为Z轴正方向.
2) 机体坐标系{B},该坐标系的原点为四旋翼无人机的几何中心点,并以机头方向为X轴正方向,机身右侧为Y轴正方向,垂直向下为Z轴正方向.
3) 摄像头坐标系{C},与机体坐标系{B}方向相同.
4) 光流传感器坐标系{O},与机体坐标系{B}方向相同.
5) 视觉SLAM坐标系{V},视觉SLAM算法数据输出采用的坐标系,与惯性坐标系{I}方向相同.
2.2 改进的视觉算法
为提高无人机控制效果,本文对传统视觉算法做了以下的改进: 第一,由于普通地面纹理较差,不能提供足够的特征点,本文通过计算亚像素级精度的角点,从而增加了特征点提取数量,同时缩短了关键帧添加距离,优化了关键帧存储,这样能够获取更多的环境信息,提高了控制算法的鲁棒性. 第二,视觉SLAM算法虽精度较高,但其对于环境中地图信息依赖性较强,在结构化的环境中容易造成地图信息的丢失. 为保证在视觉SLAM丢失情况下无人机的自主控制,本文引入光流传感器作为辅助位置信息测量单元,通过控制程序检测视觉SLAM算法特征点匹配比例: 当匹配比例低于30%时,自动切换为光流法控制; 而当匹配比例高于30%时,再次切换为视觉SLAM控制.
2.3 传感器数据融合由于四旋翼无人机具有非线性、 强耦合、 欠驱动和静不稳定等特点,故要求控制算法具有较高的实时性. 而视觉SLAM的计算量较大,受机载计算机硬件条件的限制,本文中使用的视觉SLAM算法运行频率约为20 Hz,较难满足四旋翼无人机对于实时性的要求. 因此,采用EKF融合视觉SLAM和IMU数据,提高系统运算频率至IMU数据输出频率,本文中使用的IMU数据输出频率为100 Hz,得到了较为精确的位置信息[6].
根据EKF模型,系统在k时刻的非线性过程方程和测量方程分别为
其中,xk=[pxpypzvxvyvz]T是系统k时刻的状态向量,[pxpypz]T为惯性坐标系下四旋翼无人机的3维位置向量,[vxvyvz]T为惯性坐标系下四旋翼无人机的3维速度向量,uk=[axayaz]T是系统k时刻的输入向量,zk=[pxpypz]T是系统k时刻的观测向量. ωk-1和υk为k-1时刻过程激励噪声和k时刻观测噪声. 假设过程噪声和观测噪声是相互独立的且均为零均值白噪声向量,即ωk~N(0,Q),υk~N(0,R).
在滤波值附近,对式(1)做泰勒级数展开,忽略2阶以上的高阶项,原系统就近似成为一个线性系统,如式(2)所示:
其中,
式(3)、 (4)中的δt由系统的采样时间确定,在本文中,由于IMU数据输出频率为100 Hz,故选取δt=0.01 s.
2.4 飞行控制算法设计本文采用内外环的控制结构[14]. 外环根据位置误差计算得出内环姿态的期望角度,内环跟踪期望角度控制无人机的姿态稳定性. 本文主要介绍外环(位置)控制算法.
本节采用上述提出的算法获取的实时位置和速度信息,利用PID控制器进行水平方向控制,再将该控制量发送给飞行控制器,高度方向则采用气压计获得无人机的高度信息,采用PID控制器产生控制量,实现四旋翼无人机自主控制. 其PID控制器设计如下:
其中,kp、 ki、 kd分别为比例、 微分、 积分系数,xd、 yd、 zd为四旋翼无人机参考轨迹,x、 y为视觉SLAM算法得到的四旋翼无人机的水平位置,z为气压计测得的无人机的高度,vx、 vy为视觉SLAM算法得到的四旋翼无人机的水平速度. 为避免传统PID控制中因微分运算引起的高频干扰,本文在水平方向使用速度的比例控制代替位置的微分运算.
由于在室外飞行环境中四旋翼无人机受到较强不确定因素的干扰,为提高控制系统的鲁棒性,本节中采用了一种非线性控制方法——RISE[15],这也是该方法首次应用于四旋翼无人机的控制实验中. 但考虑到系统设计的简洁性,本节只在水平方向使用RISE控制方法. 采用视觉SLAM或光流传感器获取的四旋翼无人机水平位置信息,并将该信息通过RISE控制器产生控制信号发送给四旋翼无人机. 其水平位置误差如下所示:
其中,xd、 yd为四旋翼无人机水平方向参考轨迹,x、 y为视觉SLAM算法或光流法得到的四旋翼无人机的水平位置. 为简化控制器的设计,定义如下虚拟误差信号:
其中,αx和αy为正增益系数. 水平方向控制器设计如下:
其中,ksx、 ksy、 βx和βy为正增益系数,sgn(·)为标准的符号函数. 高度方向的控制仍采用上述PID控制方法.
3 软硬件系统设计
3.1 飞行平台
本文所搭建的实验平台包括四旋翼无人机本体、地面站、遥控器等. 四旋翼无人机本体如图 3所示,搭载了嵌入式计算机(该计算机内嵌Intel Core i3双核处理器,主频1.8 GHz)、 机载摄像头(分辨率为640×480)、 PX4FLOW光流传感器、 飞行控制器(含IMU和气压计模块等). 地面站包括一台装有Linux操作系统的笔记本计算机,用于机载程序的启动及远程监控.
3.2 系统软件设计
在四旋翼无人机的自主控制过程中,为提高响应速度,本文采用了多线程编程,成功避免了某项任务长时间占用CPU时间,提高了控制系统的实时性.
该实验平台采用的软件架构如图 4所示,由视觉SLAM模块(VSLAM)、 光流传感器模块(OFS)、 IMU模块(IMU)、 EKF模块(EKF)和控制模块(Control)组成. 每个模块对应一种设备或任务,并在独立的子线程中完成,避免了时间延迟. 在控制过程中,各个子线程的创建与销毁和子线程间的数据交换由MAIN主线程完成. 视觉SLAM模块主要用于视觉SLAM的算法的运行,从而得到无人机在视觉坐标系下的3维位置信息和姿态信息. 光流传感器模块主要用于无人机水平速度信息提取和水平位置信息计算. IMU模块主要是用于机体坐标系下的三轴加速度、 三轴角速度信息的提取.
EKF模块主要是融合视觉算法得到位置信息和IMU得到的加速度信息,用系统上一时刻的状态来预测当前状态,得到融合后的位置和速度信息. 控制模块主要是检测视觉SLAM算法特征点匹配比例. 若匹配比例高于30%,则采用EKF模块融合视觉SLAM算法得到的无人机位置信息和IMU得到的加速度信息; 若匹配比例低于30%,则采用EKF模块融合光流传感器得到的位置信息和IMU得到的加速度信息,并将融合后的数据通过飞行控制算法对飞行器进行控制,实现四旋翼无人机的自主飞行控制.
4 飞行实验结果及分析为验证基于视觉的四旋翼无人机的定位与控制效果,本文对上述实验平台进行了多组飞行控制实验,考虑到无人机的尺寸及室内环境的限制,无人机的飞行实验均在室外进行,实验过程存在约3 m/s的风扰且该过程中并未使用任何GPS信号.
4.1 视觉SLAM实验结果由于室外环境中无法获取准确的参考位置信息,为了验证本文提出的改进视觉SLAM算法的精度,本节进行了室内验证实验,实验平台如图 5所示. 由于实际飞行过程中采用的无人机尺寸较大,实验过程中,将机载摄像头和嵌入式计算机搭载在本研究组自主开发的地面移动机器人MrCollie上,将摄像头朝上放置以获取天花板的特征信息. 实验过程中使用高架摄像头作为全局摄像头采集MrCollie顶部的识别板来控制移动机器人的运动,采用机载摄像头获取的环境信息再经由本文提出的改进视觉SLAM算法来获取自身的位置信息. 实验过程中通过控制MrCollie沿x轴方向运动,并将工作站获取的MrCollie运动轨迹作为位置输出的真实值(其位置控制精度为0.005 m),如图 6中虚线所示,而采用视觉SLAM算法获得的测量轨迹如图 6中实线所示. 图 7为真实轨迹与测量轨迹的位置误差曲线,可以看到视觉SLAM算法的x方向位置误差大部分位于±0.05 m以内,y方向位置误差大部分位于±0.02 m以内,且其位置输出不会随时间漂移,证明了本文提出的改进视觉SLAM算法具有较高的控制精度.
4.2 PID飞行控制结果
控制目标是实现四旋翼无人机在指定的目标位置的全自主悬停功能. 其中式(6)中的PID控制参数主要是根据实际飞行过程中实验效果进行调节,其具体参数如表 1所示. 实际飞行过程中设定目标位置为xd=0.38 m,yd=-0.22 m,zd=4.74 m. 稳定悬停状态下无人机的飞行数据曲线如图 8、 9所示. 其中,图 8为四旋翼无人机位置变化
曲线,图 9为目标位置与实际位置之间的水平误差曲线,从图中可以看出,其水平位置误差大多位于±0.2 m以内,达到了较好的悬停效果.
4.3 RISE飞行控制结果为验证本文提出的非线性位置控制算法,本节采用RISE控制器进行上述室外悬停飞行实验. 其中式(10)中RISE非线性位置控制器主要可以近似分为PID部分和符号函数积分部分. 在实际飞行实验中,PID部分参数根据经验进行调节,直至无人机达到稳定飞行状态. 在此基础之上,根据外界环境中风等不确定因素的干扰以及实验效果来调节符号函数积分部分参数,使得四旋翼无人机的悬停飞行误差最小. 其具体参数如表 2所示. 实际飞行过程中设定目标位置为xd=0.75 m,yd=-0.14 m,zd=1.23 m,期望姿态角为 d=0°,θd=0°,φd=358°. 稳定悬停状态下无人机的飞行数据曲线如图 10~12所示. 其中,图 10为无人机的位置变化曲线; 图 11为目标位置与实际位置之间的水平位置误差曲线,从图中可以看出,其水平位置误差大多位于±0.15 m以内,控制效果明显优于PID控制器;图 12为无人机姿态角曲线,可以看到滚转角和俯仰角晃动范围大都在±2°以内,偏航角控制误差大都在±1°以内,整个实验过程中四旋翼无人机的姿态稳定性良好. 实验过程中最大风速约为3 m/s,故无人机存在一定的姿态角度倾斜,这也证明了本文设计的控制系统具有较好的抗干扰能力.
4.4 视觉SLAM地图丢失情况下飞行控制结果
为验证本文提出的控制策略在视觉SLAM匹配的地图信息丢失时的控制效果,本节做了相应的实验. 在实验过程中,无人机的y方向为飞行员手动飞出视觉SLAM已匹配 区域x方向为无人机自动控制. 实验结果如图 13、 14所示,图 13为视觉SLAM丢失情况下x方向位置曲线,图 14为y方向位置曲线,其中蓝色曲线表示视觉SLAM测得的无人机水平位置,绿色曲线表示光流传感器得到的无人机位置,红色曲线表示用于控制的位置曲线. 在0~23.85 s时间段内采用视觉SLAM算法获取无人机的位置信息进行控制. 在23.85 s~34.05 s时间段内,视觉SLAM地图信息丢失,其测得的无人机位置信息不再更新,在图中表现为一段直线,控制系统自动切换为光流传感器控制. 34.05 s以后,视觉SLAM信号恢复,测得的位置信息继续更新,为避免光流传感器长时间积分累积误差造成的位置信息漂移,此时系统自动切换为视觉SLAM控制,实现了无人机在地图丢失情况下的自主控制. 从图 13中可以看出,在整个实验过程中x方向的位置控制误差约为±0.1 m,证明了本文提出的控制策略的有效性.
5 结论
本文设计了基于视觉的无人机的自主定位与控制方法,并搭建了基于嵌入式架构的四旋翼无人机飞行实验平台,进行了较为精确的室外飞行实验. 首先对传统的视觉SLAM算法进行了改进,并在室内环境中进行了视觉SLAM精度验证实验,同时引入光流传感器作为辅助位置信息测量单元,当视觉SLAM算法出现特征匹配地图信息丢失时,利用光流信息进行辅助控制,提高了无人机飞行控制的安全性. 再采用EKF融合位置信息和IMU数据,提高输出信号的精度和频率. 最后结合PID和RISE非线性控制器设计,实现四旋翼无人机的自主控制. 本文提出的控制方法优势在于采用嵌入式机载计算机运行所有算法,减少了图像和控制信号传输过程中的干扰和延时,并实现了在视觉SLAM地图丢失情况下的自主控制,提高了无人机自主飞行控制的安全性,且本文提出的控制策略对于风等不确定因素的干扰具有较强的鲁棒性. 下一步研究工作为优化视觉SLAM算法,在不影响状态估计精度的前提下减小程序计算量,提高数据输出频率,以实现在微小型嵌入式系统的应用.
[1] | David C, Rita C, Carlos S. A nonlinear quadrotor trajectory tracking controller with disturbance rejection[J]. Control Engineering Practice, 2014, 26(1): 1-10. |
[2] | 魏伟波, 潘振宽, 李媛媛, 等. 小位移光流计算的中值公式[J]. 仪器仪表学报, 2011, 32(10): 2256-2260. Wei W B, Pan Z K, Li Y Y, et al. Median formula for optic flow computation of small displacement[J]. Chinese Journal of Scientific Instrument, 2011, 32(10): 2256-2260. . |
[3] | Lim H, Lee H, Kim H J. Onboard flight control of a micro quadrotor using single strapdown optical flow sensor[C]//2012 IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, NJ, USA: IEEE, 2012: 495-500. |
[4] | Bai Y, Liu H, Shi Z, et al. Robust control of quadrotor unmanned air vehicles[C]//31st Chinese Control Conference. Piscataway, NJ, USA: IEEE, 2012: 4462-4467. |
[5] | Meier L, Tanskanen P, Fraundorfer F, et al. Pixhawk: A system for autonomous flight using onboard computer vision[C]//2011 IEEE International Conference on Robotics and Automation (ICRA). Piscataway, NJ, USA: IEEE, 2011: 2992-2997. |
[6] | Engel J, Sturm J, Cremers D. Camera-based navigation of a low-cost quadrocopter[C]//2012 IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, NJ, USA: IEEE, 2012: 2815-2821. |
[7] | Klein G, Murray D. Parallel tracking and mapping for small AR workspaces[C]//6th IEEE and ACM International Symposium on Mixed and Augmented Reality. Piscataway, NJ, USA: IEEE, 2007: 225-234. |
[8] | Weiss S, Scaramuzza D, Siegwart D. Monocular-SLAM-based navigation for autonomous micro helicopters in GPS-denied environments[J]. Journal of Field Robotics, 2011, 28(6): 854-874. |
[9] | Weiss S, Achtelik M W, Lynen S, et al. Monocular vision for long-term micro aerial vehicle state estimation: A compendium[J]. Journal of Field Robotics, 2013, 30(5): 803-830. |
[10] | Olofsson J. Towards autonomous landing of a quadrotor using monocular SLAM techniques[D]. Linkoping, Sweden: Linkoping University, 2012. |
[11] | Ghadiok V, Goldin J, Ren W. On the design and development of attitude stabilization, vision-based navigation, and aerial gripping for a low-cost quadrotor[J]. Autonomous Robots, 2012, 30(1/2): 41-68. |
[12] | Blosch M, Weiss S, Scaramuzza D, et al. Vision based MAV navigation in unknown and unstructured environments[C]//2010 IEEE International Conference on Robotics and Automation. Piscataway, NJ, USA: IEEE, 2010: 21-28. |
[13] | Grabe V, Bulthoff H, Giordano P R. On-board velocity estimation and closed-loop control of a quadrotor UAV based on optical flow[C]//2012 IEEE International Conference on Robotics and Automation (ICRA). Piscataway, NJ, USA: IEEE, 2012: 491-497. |
[14] | 张垚, 鲜斌, 殷强, 等. 基于ARM处理器的四旋翼无人机自主控制系统研究[J]. 中国科学技术大学学报, 2012, 42(9): 753-760. Zhang Y, Xian B, Yin Q, et al. Autonomous control system for the quadrotor unmanned aerial vehicle based on ARM processors[J]. Journal of University of Science and Technology of China, 2012, 42(9): 753-760. . |
[15] | Xian B, Dawson D M, de Queiroz M S, et al. A continuous asymptotic tracking control strategy for uncertain nonlinear systems[J]. IEEE Transactions on Automatic Control, 2004, 49(7): 1206-1211. |