微型四旋翼(也称四旋翼飞行器)是一种高智能的自主飞行器[1],由于其在军事和民用领域具有较为广阔的应用前景,近年来,众多科技爱好者和学者致力于微型四旋翼装置、导航和控制方法的研究。随着地面移动机器人在未知环境中自主导航技术的发展[2],越来越多的研究者开始关注于空中机器人在室内环境下的自主飞行问题[3-4]。对于室外作业的飞行器而言,常采用惯性传感器与GPS数据相融合的方法实现飞行器状态的稳定与控制[5-6]。由于在室内环境下无线信号屏蔽、干扰等的影响较为严重,基于GPS信号、无线定位信号等常用定位信号无法有效获取,研究室内环境下的自主导航成为无人机导航技术新的挑战[7],需要解决以下几个问题:1)自主性,即无人机必须自主完成自身相对位置信息的获取;2)感知性,即无人机依靠自带传感器,通过数据融合和处理实现对周围环境的感知;3)鲁棒性,由于温度、气流等扰动会对传感器数据造成干扰,导航系统需要克服这些扰动的影响;4)导航系统的输出延迟,计算及通信均存在延时,导航系统必须考虑到送种延迟[8]。
以四旋翼飞行器平台为研究对象,在对其定位控制进行详细分析基础上,对其飞行控制部分进行分析与设计,采用多传感器实现飞行姿态的获取,完成飞行器姿态与位置控制,使飞行器能够悬停在空间中的一个指定的点,并向期望的位置运动,实现无人机室内环境下的自主悬停。
1 飞行控制系统总体方案微型四旋翼的飞行控制系统主要由以下几个部分组成:传感器信号采集、姿态解算、姿态控制器、水平位置控制器、高度位置控制器。系统总体设计框图如图1所示。图1中,由陀螺仪和加速度计构成惯性测量单元,陀螺仪提供角速率信息,加速度计提供加速度信息,根据测量数据进行姿态解算得到飞机当前的姿态。光流传感器测量飞机速度,标签通过基站定位,测量飞机位置,根据速度和位置信息进行水平位置控制。超声波传感器与气压计测量高度信息,并对数据融合后的高度信息进行高度控制[9]。
250碳纤机架作为微型四旋翼机架,银燕2204无刷电机和12 A银燕无刷电调作为动力装置和驱动装置,桨叶为5030型尼龙材料,11.1 V 1 500 mAh锂电池作为飞行器供电装置[10]。微型四旋翼如图2所示。
四旋翼飞控系统硬件包括基于STM32主控芯片、PWM输出(控制电机信号输出)、PWM输入捕获(PWM遥控信号输入)、惯性测量单元、气压计、电压模块和预留USART接口等,如图3所示。
采用超宽带(ultra wideband,UWB)定位与光流技术进行组合定位[11-12]:STM32F103C8T6作为DWM1000主控芯片,DWM1000作为载体进行开发[13]。其中,4个DWM1000作为基站(anchor),固定在10 m×10 m房间4个角;1个作为标签(tag),固定于飞行器上。通过与基站的通信,实时解算飞机距离各个基站的位置,通过串口将数据传输给飞控系统。标签通过测量到4个基站的距离便可唯一确定自己的空间位置。
选用3DR公司Px4Flow作为光流传感器,其中摄像头光学传感器用于采集地面图像,声呐超声波用于低空距离测量,板载角速度传感器测量角速度,通过多个传感器信息融合,能够得到飞行器在低空飞行的水平方向速度大小。
3 微型四旋翼运行参数解算 3.1 姿态解算 3.1.1 姿态描述和数据标定考虑到飞行器的姿态解算需要频繁组合旋转和用旋转变换向量,因此选用四元素表示载体姿态模型[13]。
设三轴陀螺仪测得的角速率信息为
$\omega = {\left[ {\begin{array}{*{20}{c}}{{\omega _x}} & {{\omega _y}} & {{\omega _z}}\end{array}} \right]^{\rm{T}}}$ | (1) |
式中ωx、ωy、ωz分别表示x、y、z方向的角速率。
根据四元素微分方程,可得四元素更新方程为
$\left[\!\!\!\!\! {\begin{array}{*{20}{c}}{\begin{array}{*{20}{c}}{ {{\hat{q}_0}}}\\{ {{\hat{q}_1}}}\end{array}}\\{ {{\hat{q}_2}}}\\{ {{\hat{q}_3}}}\end{array}} \!\!\!\!\!\right] = \left[ \!\!\!\!\!{\begin{array}{*{20}{c}}{\begin{array}{*{20}{c}}{{q_0}}\\{{q_1}}\end{array}}\\{{q_2}}\\{{q_3}}\end{array}} \!\!\!\!\!\right] + \left[ \!\!\!\!\!{\begin{array}{*{20}{c}}{\begin{array}{*{20}{c}}0 \\{{\omega _x}}\end{array}}\\{{\omega _y}}\\{{\omega _z}}\end{array}\begin{array}{*{20}{c}}{\begin{array}{*{20}{c}}\!\!\!\!\!{ - {\omega _x}}\\0\end{array}}\\{ - {\omega _z}}\\{{\omega _y}}\end{array}\begin{array}{*{20}{c}}{\begin{array}{*{20}{c}}\!\!\!\!\!{ - {\omega _y}}\\{{\omega _z}}\end{array}}\\0\\{ - {\omega _x}}\end{array}\begin{array}{*{20}{c}}{\begin{array}{*{20}{c}}\!\!\!\!\!{ - {\omega _z}}\\{ - {\omega _y}}\end{array}}\\{{\omega _x}}\\0\end{array}} \!\!\!\!\!\right]\left[ \!\!\!\!\!{\begin{array}{*{20}{c}}{\begin{array}{*{20}{c}}{{q_0}}\\{{q_1}}\end{array}}\\{{q_2}}\\{{q_3}}\end{array}} \!\!\!\!\!\right]\frac{{\Delta t}}{2}$ | (2) |
式中:q0、q1、q2、q3为实数,Δt为陀螺角速率采样时间。
利用陀螺仪和加速度计静止时测量的3个坐标轴角速率输出应该为0这一特性校正零偏,标定前后角速度数据如图4所示。采集陀螺仪和加速度计输出的3个轴角速率n次,然后相加取平均即可得各个轴的零偏,可以完成加速度数据标定。
数据融合采用IMU_Updata算法,并对其进行改进,提高系统姿态估计更新实时性,同时将加速度计与陀螺仪信息进行融合,保证姿态估计高精度。
因为有加速度计补偿,横滚与俯仰角在静态下精度为0.2°,偏航角静态情况下在短时间内(小于5 min)每分钟漂移角度小于1.5°。图5为动态性能测试,通过摆动X轴改变俯仰角度的变化来测试其跟随性能的好坏[14]。
因为摆动过程中未保证使X、Z轴角度保持不变,所以图5中可以看到X、Z轴对应角度有微小变化。
3.2 位置解算通过测量2个物体(标签Tag与基站Anchor)之间的飞行时间差(time of flight,TOF),然后乘以电磁波的速度c得到它们之间距离实现解算,如式(3)所示。
$d = cT = c\frac{{{t_2} - {t_1} - {t_{{\rm{reply}}}}}}{2}$ | (3) |
式中:c为电磁波在空气中的传输速度,c=3×108 m/s;T为飞行时间差;treply为Anchor发送无线电电报与接收到无线电电报之间的时间差。
在室内搭载的4个基站(简写为A),坐标摆放位置如图6,坐标轴单位为m,图中基站坐标分别为A0(0, 0, 1)、A1(0, 10, 1)、A2(10, 10, 1)、A3(10, 0, 1)。
测量得到基站A0、A1、A2、A3的距离分别为s0、s1、s2、s3。可解算得标签Tag位置。
$\left\{ {\begin{aligned}& {X = \displaystyle\frac{{\left( {{s_1}^2 - {s_2}^2 + 100} \right)}}{{20}}}\\& {Y = \displaystyle\frac{{\left( {{{\rm{s}}_0}^2 - {s_1}^2 + 100} \right)}}{{20}}}\\& {Z = {s_0}^2 - {X^2} - {Y^2} + 1}\end{aligned}} \right.$ | (4) |
对采集的距离值进行离散卡尔曼滤波,即进行线性最小方差估计,得到卡尔曼滤波前后静态测试Tag到Anchor距离对比如图7。
由图7可以看到,经过卡尔曼滤后的数据更加平滑,实现对实际距离值的最优估计,更加有利于飞控控制。
3.3 速度解算将光流传感器固定在飞机正下方,与载体坐标系的OXY平面平行,测量飞行器X轴与Y轴坐标系的速度。通过使用3DR官方提供的QGroundControl地面站观看光流数据如图8、9所示。
图8为静止时测得的,可以看到噪声大小小于0.09 m/s;图9为保持X轴方向速度为0,反复移动Px4flow的Y轴,从图中可以看出Y轴速度变化的趋势,X轴的数据接近于0。
4 微型四旋翼室内悬停仿真研究 4.1 姿态控制仿真研究四旋翼实现室内悬停,需要对其进行姿态控制,在理想条件下,当四旋翼的横滚角与俯仰角始终为零,速度、加速度也为零时,四旋翼能够稳定的悬停在空间中一点。用MATLAB对四旋翼进行姿态控制仿真,图10为四旋翼姿态控制器仿真位姿三维波形。
图10(b)可以看出,在第1秒时刻给一个绕Y轴方向的期望角度,飞行器的姿态横滚角θ约在0.7 s的时间内调整到了期望横滚角度。同理,给入期望俯仰角与偏航角可以观察出相似现象。可以看到整个过程中飞行器的位置是一条随机的曲线轨迹,这是因为没有引入位置控制环的原因所在。因此,通过设计姿态控制器,可以实现四旋翼飞行器的平稳飞行与前后左右和垂直运动控制。
用MATLAB对四旋翼进行仿真,图11为四旋翼位置控制器仿真位姿波形图。
图11表明,当给定不同方向的期望角度时,对应的飞行器向一个方向飞行,从速度波形曲线中可以看到,速度基本不变,从位置波形曲线中可以看到,飞行器做匀速直线运动。因此通过位置控制器可以实现飞行器室内悬停时的位置控制。
为验证自主悬停控制方法的效果,使用微型四旋翼进行室内自主悬停实验。通过手动遥控使机器人到目标物体,飞行高度约3 m。设定悬停点为当前位置,悬停高度为3 m,然后启动自主悬停模式。记录开启自主悬停模式后2 min内的机器人位置误差如图12所示。
图12表明,当启动自主悬停模式后,从水平位置误差曲线看,x轴和y轴方向误差都控制在2 cm以内;从高度误差曲线看,高度方向误差始终控制在正负1 cm以内,四旋翼位置基本保持不变,可以实现室内自主悬停。
5 结论针对传统室外定位装置不能满足微型四旋翼室内定位的需求,设计了一种基于室内基站、光流传感器等多传感信息的定位与悬停控制方法,并仿真验证了方法的可行性及有效性。
1) 采用分布式多传感器构建环境信息检测系统,实时获取四旋翼飞行器姿态和相对位置信息,提高了悬停动态稳定性。
2) 通过设计姿态、位置和速度控制器能够实现对飞行器的动态控制。
3) 飞行器能够在室内空间中的任何一点悬停,并仅在小范围内波动。
4) 该方法提高了四旋翼室内悬停定位的准确性,为飞行器室内平稳飞行和更多功能的开发提供了基础。
[1] | MCMICHAEL J M, FRANCIS M S. Micro air vehicle-toward a new dimension in flight[J]. Unmanned system, 1997, 15(3): 10-17. (0) |
[2] | 姜成平. 一种四旋翼无人机控制系统的设计与实现研究[D]. 哈尔滨: 哈尔滨工业大学, 2014: 13-19. (0) |
[3] | LINDSEY Q, MELLINGER D, KUMAR V. Construction of cubicstructure with quadrotor teams[J]. Autonomous robots, 2012, 33(3): 323-336. (0) |
[4] | BILLS C, CHEN J, SAXENA A. Autonomous MAV flight in indoor environments using single image perspective cues[C]// 2011 IEEE Int Conf on Robotics and Automation. Shanghai, China, 2011: 5776-5783. (0) |
[5] | QI J T, SONG D L, DAI L, et al. The new evolution for SIA rotorcraft UAV project[J]. J of robotics, 2010(1): 1-9. (0) |
[6] | POZNYAK A S. Advanced mathematical tools for automatic control engineers, 2[J]. International journal of production economics, 2009, 98(2): 114-128. (0) |
[7] | 王立, 熊蓉, 禇健, 等. 基于模糊评价的未知环境地图构建探测规划[J]. 浙江大学学报: 工学版, 2010, 44(2): 253-258. (0) |
[8] | 郑伟. 基于视觉的微小型四旋翼飞行机器人位姿估计与导航研究[D]. 长沙: 中国科学技术大学, 2014: 21-26 (0) |
[9] | 曹娟娟, 房建成, 盛蔚, 等. 低成本多传感器组合导航系统在小型无人机自主飞行中的研究与应用[J]. 航空学报, 2009, 30(10): 1923-1929. (0) |
[10] | 黄坡, 马艳, 杨万扣. 基于Mahony滤波器和PID控制器的四旋翼飞行器姿态控制[J]. 电脑知识与技术, 2014(7): 1611-1617. (0) |
[11] | MANKIBI M E. Hybrid ventilation control design and management[J]. ASHRAE transactions(Part 2), 2009, 115: 3-9. (0) |
[12] | 胡禹超. 基于多传感器的四旋翼无人机室内自主导航研究[D]. 沈阳: 东北大学, 2014: 46-50 (0) |
[13] | 张洪涛. 四旋翼微型飞行器位姿及控制策略的研究[D]. 哈尔滨: 哈尔滨工业大学, 2014: 37-50 (0) |
[14] | 陈卓. 微型无人机视觉惯性导航[D]. 哈尔滨: 哈尔滨工业大学, 2015: 11-13 (0) |
[15] | GUERRERO-CASTELLANOS J F, MARCHAND N, HABLY A, et al. Bounded attitude control of rigid bodies: real-time experimentation to a quadrotor mini-helicopter[J]. Control engineering practice, 2011, 19(8): 790-797. (0) |