基于超声波传感器的四旋翼飞行器室内定位与控制

蓝波，吴昊，郭清才，郭朋，刘兴东，曾飞

（北京石油化工学院信息工程学院，北京102617）

近年来，四旋翼飞行器的应用与研究广泛受到关注，随着嵌入式处理器、微传感器技术和控制理论的发展和成熟，四旋翼飞行器逐步向高效、多功能化方向发展，广泛应用于军事、民用、以及科学研究等多个领域[1]。与此同时，四旋翼飞行器在定位与控制方面也得到了很大发展，目前定位技术主要采用GPS、人工视觉、光流法和超声波等实现。GPS在四旋翼导航与定位方面应用已经相对成熟，但其主要用于室外，在室内应用时存在信号采集不稳定，定位不精确的情况[2-3]。人工视觉的摄像头对室内光线的要求较高，当光线偏暗或明暗变化较快时，存在采不到数据或误采集情况；另一方面，摄像头采集的数据量巨大，运算处理时间较长，实时性得不到可靠保障[4-5]。近年来，使用光流法进行飞行导航定位和避障也成为小型飞行器研究领域的一个热点问题，但光流法算法复杂，对处理器运算速度要求较高，定位精度还不高［6-7］。超声波技术具有不受室内光线影响、数据处理简单、测距精度高、成本低廉、方便应用等优点，在机器人测距与定位方面得到广泛应用。本文提出一种将超声波传感器应用于四旋翼飞行器进行室内定位与运动控制的方案[8-9]。

1 系统架构

设计采用PIXHawk作为飞行控制芯片，6个AVR单片机ATmega 328P作为协处理控制芯片。安装在四旋翼飞行器不同位置的超声波传感器负责接收来自各个方向障碍物的反射波，并传送给AVR单片机，通过计算发射与反射之间的时间差得到飞行器与室内障碍物之间的距离。负责高度测量的AVR单片机通过IIC的方式与飞行控制芯片通信，负责前、后、左、右4个水平方向距离测量的AVR单片机先通过IIC通讯方式将数据先发送到主AVR单片机，主AVR单片机再通过串口通讯的方式将数据发送到飞行控制芯片中。飞行控制芯片将五组数据整合处理后得到四旋翼飞行器在房间内的方位，并输出四路PWM波信号到电调，进而控制无刷电机，实现对四旋翼飞行器飞行姿态的控制。整个系统的架构如图1所示。

图1 系统整体架构

2 硬件系统实现

2.1 超声波传感器

方案选用US-100超声波传感器，如图2所示。该传感器探测距离为2～450 cm，精度为0.3 cm±1%，可以实现对距离的精确测量，确保室内定位的精确度。同时，US-100自带温度补偿功能，可以对测距结果进行校正，提高了测距精度。

图2 US-100超声波传感器

US-100超声波测距模块测距分为两种模式测距分为两种模式，一种为电平触发测距模式，另一种为串口触发测距模式，本设计采用电平触发模式。该模式只需要在Trig/TX管脚输入一个10US以上的高电平，系统便可发出8个40 kHz的超声波脉冲，然后检测回波信号。当检测到回波信号后，模块还要进行温度值的测量，然后根据当前温度对测距结果进行校正，将校正后的结果通过Echo/RX管脚输出。在此模式下，模块将距离值转化为340 μm/s时的时间值的2倍，通过Echo端输出一高电平，可根据此高电平的持续时间来计算距离值。即距离值为（高电平时间*340 μm/s）/2[10-13]。电平触发模式时序如图3所示。

图3 US-100电平触发时序

2.2 飞行控制芯片

飞行控制芯片PIXHawk是一个开源的高端自动驾驶仪硬件，采用多余度供电系统，可实现不间断供电，保障了四旋翼飞行器飞行时的稳定性以及安全性。另一方面，PIXHawk5个串口可以实现与5个协处理控制芯片单独通信，加快了数据传输速度。PIXHawk的MCU采用32位STM32F427，STM32F427拥有Cortex-M4核心同时带有浮点运算器，处理速度快，对飞行的实时性控制提供了保障[14]。飞行控制芯片如图4所示。

图4 飞行控制芯片PIXHawk

2.3 协处理控制芯片

AVR ATmega 328P作为协处理控制芯片，用来处理来自不同方位超声波传感器接收到的数据，处理后通过串口以及IIC的通讯方式发送到飞行控制芯片上。采用协处理控制芯片进行预处理超声波传感器接收到的数据大大节省了主控制芯片处理数据的负担，提高了四旋翼飞行控制的实时性[15-16]。

水平方向共有前后左右4个AVR单片机以及与其对应的US-100超声波传感器，二者通过直插接口的方式相连接。水平方向AVR单片机对US-100超声波传感器反馈回的数据进行处理，然后以IIC方式发送至主AVR单片机中。水平方向AVR单片机和超声波传感器数据通信电路如图5所示。

图5 水平方向AVR数据通信电路

主AVR单片机由IIC方式接收到水平四个方向的数据后进行数据整合，然后经过串口通讯的方式发送给飞行控制芯片，飞行控制芯片通过计算得出偏差，发出相对应的四路PWM波到四个电调，经信号放大后，电调产生相应的电流驱动无刷电机，对四旋翼飞行器水平位置进行调整，进而实现飞行器避障飞行以及定点等稳定的飞行状态[17-18]。主AVR数据通信电路如图6和图7所示。

为减轻主AVR单片机处理数据的复杂程度，四旋翼飞行器的高度测量由高度AVR单片机单独负责。负责高度测量的超声波传感器US-100返回数据后直接传送给高度AVR单片机，由其对数据进行整合计算后通过IIC方式传送到飞行控制芯片，根据偏差产生相应PWM波对四旋翼飞行器的高度进行调整。高度AVR数据通信及IIC接口电路如图8所示。

图6 主AVR数据通信网络

图7 主AVR数据通信接口

3 软件系统实现

协处理控制芯片程序主要实现对各个方向障碍物距离的测量并将数据发送至飞控芯片。AVR单片机程序流程图如图9所示。

飞控程序用于对飞行姿态的控制，同时利用协处理控制芯片提供的距离数据进行偏差调整，使四旋翼飞行器达到预定房间内位置，实现飞行器室内定位悬停与定距离飞行。飞行控制芯片程序流程图如图10所示。

图8 高度AVR数据通信电路

图9 协处理控制芯片程序流程图

图10 飞控程序流程图

协处理控制芯片中写入超声波测距程序以及与飞行控制芯片串口通信的程序。因四旋翼飞行器对实时性要求较高，所以将串口通信波特率设置为115200以提高数据的传送速率。飞行控制芯片中的程序主要负责飞行姿态的调整实现室内定位，首先设置期望四旋翼飞行器飞到的高度：

#define Altitude_Expectation_cm 70

设置4个方向超声波传感器期望的悬停定位距离：

uint16_t Rng_Expectation_cm[SONARNUMBER]={200，200，100，100}；

设定变量用于记录四旋翼在调整飞行姿态定位时超声波小于特定值的次数，当记录次数达到特定值时触发中断，四旋翼飞行器提前做出减速动作，避免由于惯性带来的碰撞：

uint16_t Count_In100=0；//超声波探测距离小于2 m的次数

uint16_t Count_In50=0；//超声波探测距离小于1 m的次数

如果左侧超声波探测距离小于期望值距离或探测距离小于设定的安全距离，就向右飞，控制系数越大，四旋翼飞行器飞行速度越快：

if（Rng_Target_cm[S_LEFT]＜Rng_Expectation_cm[S_LEFT]&&（Rng_Target_cm[S_LEFT]＞ 0））

{RollControl+=（Rng_Expectation_cm[S_LEFT]-Rng_Target_cm[S_LEFT]）*5；}

如果右侧超声波探测距离小于期望值距离或探测距离小于设定的安全距离，就向左飞，控制系数越大，四旋翼飞行器飞行速度越快：

if （Rng_Target_cm[S_RIGHT] ＜Rng_Expecta⁃tion_cm[S_RIGHT]&&（Rng_Target_cm[S_RIGHT]＞0））

{RollControl-=（Rng_Expectation_cm[S_RIGHT]-Rng_Target_cm[S_RIGHT]）*5；}

同理，前后两个方向也按照以上算法对四旋翼位置进行调整。

4 结论

在试验过程中，四旋翼飞行器悬停定位误差前后左右四个方向不超过2 cm，高度误差不超过1 cm。试验数据结果表明，基于超声波传感器的四旋翼飞行器室内定位与控制方案，有效地解决了飞行器在室内的悬停定位问题，可以用于室内飞行避障。在室外环境时，可以通过与GPS等技术的结合，为四旋翼飞行器提供更加精确的悬停定位。

参考文献：

[1]岳基隆，张庆杰，朱华勇.微小型四旋翼无人机研究进展及关键技术浅析[J].电光与控制，2010（10）：46-52.

[2]黄艳虎，陈家裕，陈倍昌，等.基于GPS的四旋翼飞行器定位系统的设计[J].电脑知识与技术，2016（8）：205-206.

[3]He R，Prentice S，Roy N.Planning in information space for a quadrotor helicopter in a GPS-denied environment[C]//Robotics and Automation，2008.ICRA 2008.IEEE International Conference on.IEEE，2008：1814-1820.

[4]陈小龙，唐强，车军，等.基于人工视觉的四旋翼飞行器室内定位与控制[J].兵工自动化，2012（5）：61-64.

[5]曹美会，鲜斌，张旭，等.基于视觉的四旋翼无人机自主定位与控制系统[J].信息与控制，2015（2）：190-196，202.

[6]宋宇，翁新武，郭昕刚.基于光流和惯性导航的小型无人机定位方法[J].传感器与微系统，2015（1）：13-16.

[7]Zingg Simon，Scaramuzza Davide，Weiss Stephan，et al.MAV navigation through indoor corridors us⁃ing optical flow[C]//IEEE International Conference on Robotics and Automation，Anchorage，USA，2010：3361-3368.

[8]杨淑媛.旋翼无人机室内自主避障飞行研究[J].山西电子技术，2016（6）：86-88.

[9]韩军，常瑞丽.智能移动机器人超声波测距定位系统的研究[J].机械设计与制造，2010（1）：164-166.

[10]程华彬，张颖超.基于超声波传感器的移动机器人定位研究[J].传感器与微系统，2013（4）：58-60，64.

[11]方登建，戴宇进，赵红超，康宇航.四旋翼无人飞行器的室内定位、导航与建图[J].海军航空工程学院学报，2014（4）：345-350.

[12]Tsai C C.A localization system of a mobile robot by fusing dead-reckoning and ultrasonic measure⁃ments[J].IEEE Transactions on Instrumentation and Measurement，1998，47（5）：1399-1404.

[13]Ohya I，Kosaka A，Kak A.Vision-based naviga⁃tion by a mobile robot with obstacle avoidance us⁃ing single-camera vision and ultrasonic sensing[J].IEEE Transactions on Robotics and Automation，1998，14（6）：969-978.

[14]Meier L，Tanskanen P，Fraundorfer F，et al.The PIXHAWK open-source computer vision frame⁃work for MAVs[J].The International Archives of the Photogrammetry，Remote Sensing and Spatial Information Sciences，2011，38（1）：C22.

[15]Wani S A，Chaudhari R P.Ethernet enabled digital I/O control in embedded systems[C]//Computing，Electronics and Electrical Technologies（ICCEET），2012 International Conference on.IEEE，2012：634-637.

[16]崔陆军，尚会超.基于AVR单片机的无线测温系统[J].仪表技术与传感器，2012（9）：57-59.

[17]黄克亚.基于虚拟仿真和ISP下载的AVR单片机实验模式研究[J].实验技术与管理，2013（8）：81-85.

[18]赵道新，李炳辰.基于PCF8591的I2C总线多路温度测量系统[J].制造业自动化，2013（5）：36-37，55.