清华大学测试技术课程论文集锦-四旋翼控制-1

基于单目视觉的微型四旋翼飞行器悬停校正方文斌，刘幂，王雨辰（清华大学精仪系，北京100084）摘要：由于低成本的惯性测量单元(Inertialmeasurementunit,IMU)存在较大漂移，四旋翼飞行器难以稳定地悬停在固定区域，基于此，提出了一种基于单目视觉的四旋翼飞行器悬停校正方法。将无线摄像头安装在四旋翼飞行器底部，利用图像信息检测四旋翼飞行器相对地面的水平、垂直移动速度，最后通过扩展卡尔曼滤波方法融合IMU传感器和单目视觉测量信息解算出微型飞行器的位姿，实现悬停校正。试验结果表明：该方法能有效提高四旋翼飞行器的悬停稳定性，从而保证飞行器能够执行战场侦察、校正射击、干扰敌人等多种军事任务。关键词：微型飞行器；摄像头；悬停校正；位姿估计中图分类号：MonocularVisionBasedHoveringCorrectionofMicroAirVehiclesFANGWen-bin，LIUMi，WANGYu-chen(DepartmentofPrecisionInstrumentsandMechanologyTsinghuaUniversity,Beijing100084)Abstract:Themeasurementsofonboardinertialmeasurementunit(IMU)sensorssuchasgyrosandaccelerometersarecorruptedbylargeaccumulatederrors,andGPSsignalisunavailableinsuchsituation.Therefore,amonocularvisionbasedindoorMAVmotionestimationmethodispresented.Firstly,thecameraispresentedatthebottomofMAV,whichcollectsimagesforthemeasurementofhorizontalspeedandverticalspeed.Atlast,themeasurementsofIMUsensorsandvisionmodulearefusedwithextendedKalmanfiltertodothehoveringcorrection.TheexperimentshowsthatthemethodcanreliablyestimatetheindoormotionofMAVinreal-time,whichensurestocarryoutmultiplemilitarytaskssuchasbattlefieldreconnaissance,shooting,ortointerruptfoes.Keywords:microairvehicle;camera;hoveringcorrection;motionestimation0前言姿态估计无人机微型化后,飞行空间扩大到城市甚至建筑物内。由于遮挡、噪声干扰等原因,室内环境中的GPS信号十分不稳定,无法得到准确的定位信息,同时微型惯性单元(Inertialmeasurementunit,IMU)传感器长时间使用往往存在较大的积累误差,无法独立准确地提供位姿信息。微型飞行器(Microairv-ehicles,MAV)实用化的关键技术是自主飞行,姿态和位置的检测作为位姿稳定控制的前提在MAV自主飞行中起关键作用[1]。视觉系统具有独立性、准确性、可靠性以及信息完整性等优势,利用视觉系统对飞行器的位姿进行估计是近年来发展起来的一种先进的位姿估计方法。而MAV尺寸小、重量轻、耗能少,对负载的要求非常严格机载摄像机作为MAV必备任务载荷,使用其估计位姿不增加额外的重量,同图像中包含的环境信息也能很好地辅助MAV室内导航飞行。室外环境中的利视觉估计MAV位姿的方法主要是从图像中提取地平线,通过对直线信息分析得到飞行器的姿态[2]。这种方法要求飞行器获取的图像中必须有天空和地面,并且两部分的特征必须十分明显,算法受环境制约,同时该方法只能估计飞行器的俯仰角和滚转角。Celik提出了一种利用结构化环境中直线信息估计飞行器位姿的单目视觉方法,用于实现MAV室内环境中的导航[3]。该方法要求机载摄像机能够稳定捕捉墙壁与地面的交线,而当室内环境较复杂时,该方法就无法辅助飞行器导航。Achtelik利用四旋翼MAV上安装的双目摄像机和激光扫描雷达进行位姿估计和室内环境感知[4],然而双目摄像机和激光扫描雷达的重量和功耗严重制约了飞行器的航时和航程。为了满足MAV在非结构化室内环境中导航的需求和负载限制,本文提出了一种基于单目视觉的MAV悬停校正方法。首先引入基于MJPG-streamer的特征匹配用于稳定跟踪图像特征点,再根据五点算法获得帧间摄像机的运动参数和特征点的位置信息;结合室内环境中的平面关系,将特征点位置参数的维度由三维降低为二维,减少优化迭代过程中的参数数量;将降维后的初始解代入局部优化方法求得最大似然估计,提高位姿估计的精度。其次,通过卡尔曼滤波方法融合IMU传感器测量数据和单目视觉估计信息求解出MAV的位置和姿态。1硬件结构主控芯片采用了意法公司的STM32系列的STM32-F103C8T6单片机。惯性导航模块采用MPU9250，用来采集9轴信息，包括3轴加速度，3轴角速度以及3轴地磁信号。本项目中只采用了3轴加速度和3轴角速度信息，通过SPI接口传输到STM32单片机中。无线通讯模块采用了基于Openwrt的迷你无线路由器。路由器作为一套Linux系统，负责完成图像采集，并将图像发回给上位机（PC机）。电机控制直接采用了单片机控制输出的PWM波占空比的方式。系统硬件框架如图1所示。图1系统硬件框图2基于卡尔曼滤波的飞行器姿态解算机体的空间姿态可以看作是飞行器依次绕航向轴、俯仰轴、横滚轴作基本旋转后的复合结果。为了表示这个旋转过程，引入姿态矩阵C：coscossinsinsincossinsinsinsincossincoscoscossinsincoscossinsinsinsincoscossincoscosC其中为俯仰角，为航向角，为横滚角。当利用加速度计测量机体姿态时有，机体坐系下的加速分量为xyzaaaa，地理坐标系下的加速度分量为00Tgag。结合矩阵C有下式。00xyzaaCag（2-1）将式2-1代入C中可得：sinsincoscoscosyxzagagag（2-2）则俯仰角和横滚角为：11tansinxzyaaag（2-3）在实际运行过程中，电机振动对加速度计产生的噪声必须被消除，否则真实的测量值将淹没在噪声中，导致角度计算失效。可以通过软件和硬件结果改造处理振动。本文使用了滑步平均法来预处理加速度计的数据。在姿态解算时，单纯采用加速度计计算角度速度慢，无法满足姿态实时结算的要求，而单纯使用陀螺仪计算角度虽然满足了速度要求，又存在积分误差等问题。本文采用卡尔曼滤波进行数据融合。对于一个一般离散线性系统，它的状态方程和观测方程为：,1|1+,11+,11XkkkXkkBkkukkkwk（2-4）ZkCkXkYkvk（2-5）其中Xk为状态向量，Zk为观测向量，1uk为控制向量，1wk为随机干扰向量，Yk为测量系统的常值误差，vk为量测噪声向量，,1kk为状态转移矩阵，,1kk为误差系数矩阵，Ck为量测系数矩阵，,1Bkk为控制系数矩阵。假设噪声w，v是均值为零的白噪声序列，则满足0,,0,kkwNQvNR。其对应的卡尔曼滤波迭代公式如下：|1,11|1,1+,11,1TTPkkkkPkkkkkkQkkk（2-6）|1,11|1+,11+,11XkkkkXkkBkkukkkwk（2-7）||1|1+XkkXkkZkCkXkkKkYkvk（2-8）||1TTPkkIKkCkPkkIKkCkKkRkKk（2-9）本文分别对MAV的三个轴建立了状态方程和观测方程如下。1kkw（2-10）1aZkkv（2-11）其中为角度，为陀螺仪测得的角速度，w为状态噪声，a为加速度计测出的角度，v为量测噪声。3位置测量虽然依靠机载的IMU传感器可以提供飞行器的姿态信息，经过一定积分可以得到位置信息，但是考虑到IMU传感器精度较低，会产生累积误差，导致位置信息仅在短时间内有效，所以需要其他方式来提供位置信息。本文采用单目视觉配合地面标志物，通过MJPG-streamer实现基于IP地址的网络图传，进而基于AprilTags算法识别地面标志物位置，解算出MAV的坐标，进入位置负反馈。地面标志物如图2所示。图2地面标志物使用LabVIEW编写了上位机程序，对采集到的标志物进行二值化、腐蚀膨胀的图像处理过程，最终得到标志物在摄像头视野中的位置和大小，从而可以根据标定结果计算出MAV相对于标志物的位置，解算出MAV的坐标。根据计算出的MAV坐标，计算出MAV的运动指令，该指令通过蓝牙的方式传送给MAV，从而实现位置的负反馈。4控制器设计在获得飞行器的姿态与位置信息后，设计了PID串级控制器来控制MVC的姿态和位置。离散化PID为：01kdpjiTTukkckcjckckTT（4-1）由于姿态与位置有较强的直接耦合关系，所以以精确的姿态控制一定程度上就可以保证位置得到控制。所以这里位置控制采用了单级PID，来对横纵位移分别进行闭环控制。这样就可以稳定准确的控制飞行器的位置。完整的飞行器串级PID控制框图如图3所示。图3系统控制框图5实验结果为了验证算法的有效性，依据上文中的理论制作了原理样机如图4所示。预计测试垂直起降和定点悬停功能。设定俯仰角和横滚角为0°，通过蓝牙发送角度信息到上位机，绘制图线，分别观测无扰动和有扰动对角度的影响，从而验证MAV的悬停稳定性。由于图传部分尚且存在一些问题，因此实验部分还未完成。该部分从略。图4MAV实物图6结论由于室内环境中无法获得GPS信号,MAV仅靠机载的IMU传感器不能获得精确的位姿估计,必须依靠其他传感器提供额外的位姿信息。考虑到MAV的载荷限制,本文对基于单目视觉室内MAV的位姿估计方法进行了研究。基于MJPG-streamer特征点匹配算法，提高了MAV位置测量精度。通过融合IMU传感器和单目视觉模块测量的位姿信息,消除了IMU传感器的累积误差,获得有效可靠的飞行器位姿信息，很好地实现了飞行器的定点稳定信息。而且飞行器仍有很大的有效载荷，可以作为其他研究的开发平台。参考文献[1]王晓宇,闫继宏,秦勇,赵杰.基于扩展卡尔曼滤波的两轮机器人姿态估计[J].哈尔滨工业大学学报,2007,39(12):1921-1924.[2]赵鹤,王喆垚.基于UKF的MEMS传感器姿态测量系统[J].传感技术学报,2011,24(5):642-646.[3]陈小龙,唐强,车军.基于视觉的室内四旋翼定位于目标跟踪研究[A].Proceedingsofthe10thWorldCongressonIntelligentControlandAutomation[C].Beijing,China,2012:4747-4753.[4]CooperBills,JoyceChen,AshutoshSaxena.AutonomousMAVFligh