基于UWB 定位的室内无人机编队飞行规划

张 勃 肖 雄 刘朝熙 胡向阳 黄瑾瑜 李 琪

（中国民航大学航空工程学院，天津300300）

随着无人机技术应用的日益广泛，诸多生产活动中，关于多机协同作业的无人机编队飞行的需求日渐增多，因此研究民航业相关领域中该类技术的应用具有良好的发展前景。编队化飞行的核心在于精确定位，虽然目前基于GPS、北斗导航系统等技术的编队飞行能满足室外场合作业，但面对有室内需求的飞行任务却无法获得高效稳定的定位数据。为解决室内编队飞行需求，本文采用基于UWB 定位技术进行室内无人机编队飞行算法研究，借由能搭载UWB 模块的四旋翼无人机模拟搭建了室内场合下的编队化飞行系统。

1 UWB 定位技术原理分析

1.1 UWB 的测距原理

UWB 定位技术通过极窄的高速脉冲接收传输数据，传输带宽高，适合局域网络通信或短途导航。其测距原理是双向飞行时间方法,即每个模块会发出一个独立的信号,通过信号的飞行时间来对模块间的距离进行计算。

比如存在两个模块A 和B, 首先由A 发出一条信号并记录发射时的时间TA1,记模块B 收到信号下时间为TB1,此时B 在时间TB2 发出一个响应信号, 记模块A 收到响应信号时间为TA2。即可计算出飞行时间得出距离。

记距离为S，光速为C，飞行时间为T

即可得出两模块之间的距离

1.2 基于UWB 的TDOA 定位方法

基于TDOA 方法的定位主要是利用基站接收到信号之间的时间差来确定标签位置，标签发出的信号到两基站的时间差为常数说明了标签所处的位置一定在这以这两点为焦点的双曲线上，故确定双曲线的交点，即确定好标签位置，因此TDOA定位算法又被称为双曲线算法。拥有四个基站则会有四条双曲线，四条双曲线的交点即标签的位置。

如图1 所示：UWB 定位基站的坐标分别为A（X1，Y1，Z1）、B（X2，Y2，Z2）、C（X3，Y3，Z3）、D（X4，Y4，Z4），基站A、B、C、D 在安装部署时位置固定且坐标已知，所求定位标签的坐标为（X，Y，Z）。求解坐标（X，Y）的方程组如下公式所示：

图1

1.2.1 其中四个基站A、B、C、D 的距离分别为r1，r2，r3，r4，接收到信号的时间分别为t1，t2，t3，t4：

1.2.2 因标签发出的信号到每两个基站间的距离差为常数,故可得到一条双曲线，所以有几个基站即可得到几条双曲线,而双曲线的交点即为所需的标签位置。

TDOA 不需要标签与基站之间进行重复的通信, 只需要标签发送一次信号,因而工作效率更为高效。

2 室内无人机编队系统架构与关键技术

2.1 编队系统架构

定位系统由四个定位基站以及一个定位标签组成，四个定位基站上装有UWB 定位通信模块，定位标签可以通过TODA算法解算出相对的空间坐标，从而得知标签的相对位置。飞行平台为四旋翼无人机，无人机上搭载APM2.6 飞控，通过单片机将接收标签返回的数据并给无人机的飞控传递相应的信号，无人机的飞控接收到信号后通过调整4 个电机的转速来控制无人机的飞行姿态。标签返回的数据可以通过Nassistant 程序来接收显示无人机的飞行轨迹（即引导标签的运行轨迹）。

2.2 领航-跟随法

跟随-领航法是目前比较成熟的一种无人机编队方法，通过对无人机进行编队，将对无人机编队时对多个无人机控制转换成对单个无人机运动的情况进行研究。该方法将无人机编队看作一个刚性体，领航飞行器作为固定参照目标，其余无人机跟随其保持队形。其优势在于简少了对多无人机系统的控制，简化了研究对象的复杂度，通过改变领航者与跟随者之间距离和角度可达到改变飞行队形的目的，其局限性主要体现在：编队系统对领航无人机的依赖性过强从而导致容错率低，抗干扰能力弱并且引导标签不能得到跟随机的实际坐标。

2.3 领航-跟随控制逻辑

基于领航跟随法原理，选取引导标签充当虚拟领航者，通过Nassistant 可视化程序可观察标签坐标和航向角大小，应用地面站设定跟随无人机的期望距离和相对角度，各个跟随者的坐标可以准确定位。利用飞行器上搭载的UWB 模块可实时接收无人机间相对距离以及飞行角度，因此只需根据多次飞行实验即可以确定在目标期望下两机相对角度与实际距离下误差的允许范围，从而达到编队控制的目的。图2 为在Nassistant 可视化程序下模拟出的引导标签及其跟随机的移动轨迹，其中蓝线为引导标签轨迹，红线为跟随者轨迹。

图2 Nassistant 可视化程序

2.4 具体实验步骤

步骤一：通过改写飞控APM2.6 开源程序设计搭载引导标签的飞行器飞行路线，基于Nassistant 可视化程序选择合适的跟随飞行器与引导标签进行信息交流，设置该跟随无人机为跟随者A。

步骤二：跟随飞行器基于UWB 模块传输定位开始跟随领航标签，并与各个跟随者之间进行信号交流，获得彼此定位坐标，飞行参数，各跟随无人机从不同初始位置出发，基于跟随者A 进行统一编队飞行规划。

步骤三：按照引导标签传输的飞行路径，分配给各个跟随机不同位置信息，在跟随领航标签运动的同时保证编队的稳定，一旦跟随者A 与引导标签间距离符合允许误差的安全半径，通过地面站保证各个跟随机进入减速状态，保证编队之间的相对飞行速度。

步骤四：当引航标签与跟随飞行器达到飞控中预定距离时，编队飞行系统内开始保持相对静止状态，各个飞行器间距离保持一定，速度相同。

在编队飞行规划中，基于mission planer 地面站多次观察引导标签与跟随飞行器的安全距离，当两者相对距离过大或过小时，跟随飞行器基于可视安全距离进行多次距离调整，实现无人机的编队飞行控制。（图3）

图3

3 系统软硬件设计与实现

3.1 无人机的结构与控制

应用“十”型无人机，中央方块部分为无人机电源装置及飞控装置，4 个顶点分别对应无人机的4 个电机，应用单片机可以实现接收处理UWB 标签返回的数据并且可以通过相应坐标数据再对无人机飞控传输相应指令，使无人机能够按照既定规划路线飞行。（图4）

图4

3.2 UWB 定位标签与单片机串口通信实现方式

3.2.1 UWB 定位标签与单片机的信号接收

实现UWB 定位标签与单片机间的有效通信，首先要对两串口进行相关设置。由于标签发射的位置信号的波特率固定，只需将信号采样模式发送给计算机即可实现串口参数设置。单片机主要通过发射端和接收端处理信号，因此要保持它的波特率处于对应状态。

3.2.2 UWB 标签与单片机间传输数据的优化

对传统通讯协议进行修善以保证单片机和标签间的通讯的稳定性和有效性。基于原有位置信息传输定位程序进行二次加工优化，使单片机在完成初始化、通过中断或扫描接受发射数据信息等过程中，不需要人工电脑和遥控器的操作，仅仅通过各种命令帧格式即可实现整个编队之间的通讯交流。

3.2.3 在UWB 定位下的Nassistant 程序参数检测

运用基于UWB 技术的Nassistant 程序检测无人机在室内空间中的坐标参数，可以很大程度上提高精度、较小误差。

（1）无人飞行器按照“十”字形行驶，其两个电机通过上升、下降方式进行角度调整，另外两个按照对滚转方式操控调整。

（2）通过mission planner 地面站提前设置好飞行器的I、D值为0，在无人机进行上下行驶操作过程中逐渐加大P 的数值。

（3）为保证无人机飞行器飞行平稳顺利，在无人机各参数比例调整测试中要适时调整，确认P 值，符合测试要求。

（4）在无人机飞行器飞行测试期间，要避免变速误差、防止等幅振荡等，因而要合理设置飞行器参数D，保证测试飞行的平稳，通过与Nassistant 程序上UWB 模块等位置参数进行合理规划，参照具体规划要求进行适当调整，保证飞行质量、完成飞行任务。

4 结论

卫星定位导航下的室内信号及精度都不是十分理想，很难达到预期定位要求。本文通过研究基于UWB 技术下室内无人机定位，并且通过单片机与定位标签的通讯，使无人机能够按照既定航线进行编队飞行。这对以后无人机在室内精确定位并编队飞行具有一定借鉴意义。