多无人机协同障碍规避控制方法

张立鹏，茹常剑， 周欢(.解放军空军西安飞行学院 理训系，陕西 西安，70306；.空军工程大学 航空航天学院，陕西 西安，70038)



多无人机协同障碍规避控制方法

张立鹏1，茹常剑2， 周欢2
(1.解放军空军西安飞行学院 理训系，陕西 西安，710306；2.空军工程大学 航空航天学院，陕西 西安，710038)

摘要：为有效地解决不确定动态环境下多无人机协同障碍规避问题，提出一种联合扩展卡尔曼滤波和模型预测控制的控制器设计方法。首先构建分布式无人机协同障碍规避体系架构、无人机的运动模型及其通信拓扑。采用扩展卡尔曼滤波(EKF)预测动态障碍物的轨迹，并设计一种信息补偿规则。然后，基于模型预测控制(MPC)方法，设计障碍规避控制器。仿真结果表明：EKF方法能够准确地预测动态障碍物的轨迹；无人机之间通过协作，可以有效地降低预测误差。

关键词：无人机协同；障碍规避；扩展卡尔曼滤波；模型预测控制

无人机被誉为“尖兵之翼”，能够执行全空域侦察监视、精确打击以及战场救援等多种任务[1]。这些任务要求无人机在较低的高度飞行，这就使其易于和静态或动态障碍物(如UAV或有人机)发生碰撞，从而导致任务失败[2−4]。针对该问题，有人机驾驶员可通过机载TCAS系统较好地实现障碍规避，保证安全飞行。然而，无人机主要依赖地面控制站的飞行员操控，由于飞行员缺乏良好的环境感知能力，若操作不当，极易发生安全问题，因此，设计有效的自主障碍规避算法就显得十分必要。为了使无人机快速地规避障碍，需要设计一种简单有效的障碍规避方法。目前，国内外相关学者已提出势场法[5−7]、几何法[8−9]、网格法和遗传算法[10]等多种障碍规避方法，但这些算法不能很好地处理无人机的动力学约束。为此，一些学者将模型预测控制(MPC)方法引入到无人机防碰撞控制问题中。该方法的基本思想是滚动优化[11−12]。文献[13]提出一种分层的 MPC 策略用于无人机编队避障和防碰撞控制。文献[14]设计一种简单的 NMPC 算法用于碰撞规避问题。文献[15]开发一种基于 NMPC 算法的编队飞行避障技术，并通过仿真验证了该方法的有效性。但上述研究中，关于 MPC 的策略设计较简单，且障碍指标较单一[16−18]。为此，本文作者提出一种联合扩展卡尔曼滤波和模型预测控制的控制器设计方法。在构建无人机协同障碍规避控制体系架构的基础上，对无人机和动态障碍物进行建模，采用扩展卡尔曼滤波(EKF)方法预测障碍的状态与轨迹。基于模型预测控制(MPC)方法设计无人机障碍规避控制器，最后通过仿真实验来验证所提方法的可行性。

1　问题描述与建模

1.1分布式协同障碍规避的体系架构

无人机协同障碍规避问题的本质是指多架无人机协同感知−规避，具体是指各无人机使用机载光电/红外照相机或雷达等传感器，探测正在逼近的各种障碍物，并通过机间的信息交互、相互协作进行合理的规避，以确保不发生碰撞。为此，设计分布式协同控制体系架构，如图1所示。

每架 UAV 的障碍控制器采用分层结构的设计理念，由动态障碍物分析层、协同规划层和规避行为决策层构成。

动态障碍物分析层根据全局环境的认知信息，判断并预测本机所探测区域上动态障碍物的状态和轨迹；协同规划层根据动态障碍物分析层输出的结果以及全局无人机的分布状态进行无人机协同的宏观规划；规避行为决策层的作用是为无人机完成微观、具体的决策，利用滚动优化思想为无人机求解出下一时刻的飞行方向和速度。

图1　分布式协同障碍规避控制体系架构Fig.1　Control architecture of distributedCooperative obstacle avoidance

1.2无人机模型

UAV的控制系统通常采用双回路结构。内回路控制姿态变化，而外回路控制位置变化。每架 UAV 都装备有自动驾驶仪，可以为飞机提供滚转、俯仰和偏航的控制输入并且具有速度和高度保持功能。由于UAV的高度信息可由高度传感器直接获得，为简化运动模型，假定固定翼 UAV 以恒定高度飞行，则在惯性参考系中，其运动学模型如下[19]：

其中：(xi, yi) ，vi和 ωi分别为第i架无人机的位置、速度和角速度；θi∈(−π,π]，为第 i 架无人机速度矢量与Y轴正方向的夹角。

假定UAV自动驾驶仪的控制律如下：

对模型(1)进行离散化，设采样周期为 T，离散后的无人机运动方程为

为描述简单，将模型(3)及约束条件简写为

1.3通信模型

假设无人机只接收处于其通讯范围内的友机所检测到有关障碍的信息。

2 障碍轨迹预测

无人机能够检测并分类受控的移动目标以及测量其位置。由于障碍物是移动的，无人机的测量值可能会出现误差，故最好能够提前对目标的轨迹进行预测，以保证无人机及时地进行相应的规避策略。这里采用扩展卡尔曼滤波(EKF)算法预测动态障碍物的轨迹。

2.1状态估计

首先，假设所检测到的动态障碍物是一个受控系统，无人机所测得关于该障碍物的相关信息为x (k)=[s(k)θ(k)z(k)]T。其 中：s(k)，θ(k)和 z(k)y分别表示动态障碍物的速度、方位和高度。当其高度对应于 z 轴的位置时，通过坐标变换可以将其速度和方位转化为xy平面的位置 x(k)=[x(k)y(k)]T，即满足：

将其转化为状态空间表示形式，可得

EKF 要求在每个采样时刻将式(12)进行线性化。为此，在 工作点处进行一阶泰勒近似展开，并代入式(12)，可得

其中：

无人机通过机载传感器来测量动态障碍物在 xyz轴的位置，其量测方程为

考虑1个无人机编队，在每个采样时刻分成由通信范围内成员组成的小编队。若第j架UAV离开子编队i，则它的量测方程为

其中：yij(k)，Hij和 vij(k)分别为编队i中第j架UAV的量测向量、观测矩阵和传感器噪声向量。若 n 个相互通信的 UAV(编队 i 的成员)检测到同样的目标，则式(9)可扩展为

式(10)等价于

2.2轨迹预测

对于每个预测步长τ，要得到最优的障碍轨迹，就必须使其位置预测误差的协方差最小，即满足

其中：x (k+τ)和 x(k+τ|k)分别为障碍位置及其位置的预测值。x(k+τ)可以通过下式求得：

选择 x(k+τ|k)，使得式(7)对于每个τ∈{1,L,hp}，都能够产生最优的轨迹预测值，即

2.3信息补偿

在理想情况下，在协同障碍规避过程中，无人机应能实时地获得位于其通信范围内所有无人机发送的信息。但在通信受干扰、数据丢包等情况下，无人机难以获得准确信息，各无人机所测有关障碍的信息就会出现偏差[20]。此时，为最大限度地保证信息的一致性，应对无人机本地获得的信息进行补偿。

记与UAV i具有通信连接关系的UAV j的集合为j∈ Ni，假设无人机i与j之间的通信时延为常量 ∆t，则无人机i在k时刻接收来自无人机j有关障碍的信息为 yj(k−∆t)。为保证UAV i与UAV j之间的信息一致，设计如下补偿法则：

3 障碍规避控制器设计

图2单机障碍规避控制器Fig.2Obstacle avoidanceController for single UAV

定义单架UAV障碍规避的目标函数为

该准则的主要目的是使 UAV 自动驾驶仪在 hc处设定点的增量最小，其数学表达式为其中：Λ为1个3hc×3hc的对角矩阵，对设定点的增量向量进行加权。

其中：ξo为常数；dmin为 UAV 与障碍之间的最小距离。

2)目标准则。在无人机追踪动态目标的过程中，应尽量减少油耗，因此，所采取的目标跟踪准则要保证使沿着 hp所预测的 UAV 位置和最优目标位置之间距离的2-范数最小。该准则的数学表达式为

式中：ξt(τ)为权衡常数。

与此同时，为了提高多无人机协作完成任务的效率，各架无人机到达目标的时间差应该尽可能地小，最好能够作到同时到达，因此，可得

架无人机的预测位置。则 Jt(k)可以表示为

式中：a和b均为权衡常数。

其次，建立各种约束条件。

1)必须限制UAV在hc处设定点的增量及设定点以保证UAV的安全操作。

2)选择最小的 UAV 速度以保证正常飞行，而最大的UAV速度必须考虑飞机的物理限制，因此，

3)无人机的机间防碰撞约束为

式中：Lmin表示2架飞机的最小安全距离。

对于每架无人机，其预测控制轨迹都是通过求解滚动优化得到。该问题的具体数学描述为

模型预测控制(MPC)方法的本质是通过重复地求解1个最优控制问题来获得控制行为。基于本地状态和邻近无人机的信息，每架 UAV 计算自身的控制输入。求解模型(19)可得预测控制轨迹ui(k)=[ui(k|k),ui(k+1|k),L,ui( k+N−1|k)]，将其第1项作为k时刻的指令输出，即

将ui(k)施加于无人机控制系统，得 到相应状态反馈，并将其作为下一预测时域的初始状态 xi(k+1)，进而求解出 ui(k+1)。如此往复，即可得到连续的决策指令 ui(k),ui(k+1),L，驱动无人机障碍规避过程持续进行。

4　仿真与分析

为了验证本文所提算法的有效性，搭建基于MATLAB R2008的仿真环境，所有的计算在1台CPU为因特尔酷睿 i5、主频为 2.8 GHz，操作系统为Windows的电脑上执行。

实验场景中分布3个动态障碍和1个目标点，3架无人机的任务是尽可能地以最小代价到达同一目标点，在任务过程中需要规避一系列未知的动态障碍物。无人机、障碍的初始信息及约束如表1所示。

表1　仿真初始信息及约束Table1　Initial information andConstraints for simulation

4.1状态估计与轨迹预测

这里给出2个仿真场景。场景1验证单机采用EKF算法进行状态估计与轨迹预测的有效性；场景2验证多机协作测量对于轨迹预测的影响。

4.1.1仿真场景1：单机状态估计与轨迹预测

状态估计仿真结果如图3所示。在 50～70 s 内，轨迹预测如图4所示。

从图 3和图4 可以看出：基于扩展卡尔曼滤波的状态估计与轨迹预测误差较小，无人机可以较好地实现对障碍物的状态估计和轨迹预测。

4.1.2仿真场景2：多机协同状态估计与轨迹预测

将双机协同与单机条件下的状态估计与轨迹预测结果进行对比。双机与单机的标准差之比的变化如图5所示。

从图5可以看出：无人机编队通过协同融合算法，可以有效降低预测误差，较好地提高轨迹预测的精度，以更好地认知环境。

对比上述 2个仿真场景可以得出：虽然单架无人机采用EKF方法可以有效地预测障碍轨迹，但是若采用多无人机协作来预测障碍轨迹，则预测精度更高。

4.2多机规避动态障碍

这里给出2个仿真场景，主要验证 UAVs 在任务过程中协作或无协作地规避障碍的性能。

图3　单机状态估计Fig.3　State estimation for single UAV

图4单机对探测障碍的轨迹预测Fig.4Trajectory of detected obstacle predicted by single UAV

4.2.1测试场景1：非合作型多机障碍规避

图6所示为非合作型多架 UAV 障碍规避的动态过程。UAV1和UAV2分别检测到动态障碍物1和2。从在图6(a)可以看出这2架UAV能够采取恰当的动作分别规避障碍1和障碍2。然而，由于UAV2和UAV3之间没有协作，所以，UAV3检测即将到来的障碍较晚，进而被迫执行猛烈的机动以规避障碍物，如图6(b)和图6(c)所示。因此，UAV3会晚于UAV2到达目标点，如图 6(d)所示。3 架 UAV 完成任务(即尽可能地同时到达同一目标)的总时间为240 s。

图5　双机协同与单机的位置预测误差的标准差之比Fig.5　Ratio of standard deviation of position prediction error betweenCooperative UAV and single UAV

图6　非合作型多机规避障碍过程Fig.6　Processes of multi-UAVs obstacle avoidance withoutCooperation

4.2.2测试场景2：合作型多机障碍规避

图7所示为多架UAV协同障碍规避的动态过程。从图 7(a)可知：UAV1和 UAV3分别检测到动态障碍1和动态障碍3，并成功地规避了障碍，此时UAV3将检测到的障碍信息传递给 UAV2。从图 7(b)可知 UAV2成功地规避了障碍2。从图7(c)可知：UAV3成功规避掉障碍3后将信息传递给 UAV2，UAV1接收到 UAV2和 UAV3所测的有关障碍2的信息，成功地规避掉障碍 2。从图7(d)可知：3 架 UAV 经过协同成功地规避掉3个动态障碍物到达目标点。经计算，UAVs 之间的合作使任务时间大约降低15%。

对比上述2个测试场景可以得出：1)若无人机之间没有协作，则必须采取猛烈的机动才能规避障碍物，并且任务执行时间较长。2)若无人机之间通过相互通信实现协作，则此时检测到障碍物的时间会明显缩短，而且不需要激烈的动作来规避障碍，这明显地缩短了任务执行时间。

图7　多无人机协同规避障碍过程Fig.7　Processes of multi-UAVs obstacle avoidance withCooperation

5　结论

1)EKF方法能够实现对动态障碍物的轨迹预测，且误差较小。

2)装载障碍规避控制器的无人机通过相互通信实现协作，可以有效地降低预测误差，缩短任务的完成时间。

3)下一步工作是在半物理仿真系统中测试所提出的算法，并应用到本课题组的小型无人机上进行实物验证，从而验证所提算法的实际应用效果。

参考文献：

[1]ZEITLIN A D.Issues and tradeoffs in sense and avoid for unmanned aircraft[C]//Proceedings of the 4th Annual SystemsConference.Piscataway,NJ: IEEE,2010: 61−65.

[2]DEGARMO M,NELSON G M.Prospective unmanned aerial vehicle operations in the future national airspace system[C]//AIAA 4th Aviation Technology,Integration and Operations(ATIO)Forum.Chicago,IL: AIAA,2004: 20-23.

[3]MUJUMDAR A,PADHI R.Evolving philosophies on autonomous obstacle/collision avoidance of unmanned aerial vehicles[J].Journal of AerospaceComputing,Information,andCommunication,2011,8(2):17−41.

[4]XAVIER P,LUIS D,JORGE R.Requirement,issues,andChallenges for sense and avoidance in unmanned aircraft system[J].Journal of Aircraft,2012,49(3): 677−687.

[5]LEE L.Decentralized motion planning within an artificial potential framework(APF)forCooperative payload transport by multi-robotCollectives[M].Buffalo,New York,2004: 32−33.

[6]CHOU F Y,YANGC Y,YANG J S.Support vector machine based artificial potential filed for autonomous guided vehicle[C]//Proceeding of the 4th International Symposium on Precision Mechanical Measurements.Bellingham,USA,2008: 71304J.1−71304J.6.

[7]PAUL T,KROGSTAD T R,GRAVDAHL J T.Modeling of UAV formation flight using 3D potential field[J].Simulation Modeling Practice and Theory,2008,16(9):1453−1462.

[8]LEE J.Design of UAV formation flightController based on formation geometryCenterConcept[D].Seoul: Seoul University.School of Mechanical and Aerospace Engineering,2009: 95−96.

[9]ANUSHA M,RADHAKANT P.ReactiveCollision avoidance using geometric and differential geometric guidance[J].Journal of Guidance,Control,and Dynamics,2011,34(1): 303−310.

[10]GARDINER B,AHMAD W,COOPER T,et al.Collision avoidance techniques for unmanned aerial vehicles technical report[R].Auburn,AL: Auburn University,2011.

[11]IZADI HA.GORDON B W,ZHANG Youming.Rule-basedCooperativeCollision avoidance using decentralized model predictiveControl[C]//AIAA Infotech @ Aerpsapce.St Louis,Missouri,USA,2011:1−14.

[12]WESSELOWSKI K,FIRERRO R.A dual model predictiveController for robot formations[C]//42th IEEEConference on Decision andControl.Maui,Hawaii,USA,2003: 3615−3620.

[13]BEMPORAD A,ROCCHIC.Decentralized hybrid model predictiveControl of a formation of unmanned aerial vehicles[C]//Proc18th IFAC WorldCongress.Milano,Italy,2011:11900−11906.

[14]GUERREIRO B J N,SILVESTREC,CUNHA R.Terrain avoidance nonlinear model predictiveControl for autonomous rotorcraft[J].Journal of Intelligent & Robotic Systems,2012,68(9): 69−85.

[15]SHIN J,KIM H J.Nonlinear model predictive formation flight[J].IEEE Transactions on System,Man,andCybernetics-Part A: Systems and Humans,2009,39(5):1116−1125.

[16]XIE F,FIERRO R.Stabilization of nonholonomic robot formations: A first-stateContractive model predictiveControl approach[J].Journal ofComputing and Information Technology,2007,17(1): 37−50.

[17]BOIVIN E,DESBIENS A.Collision avoidance usingCooperative predictiveControl[C]//Proceedings of the16th MediterraneanConference onControl and Automation.Ajaccio,France,2008: 682−688.

[18]PRÉVOSTC G,THÉRIAULT O,DESBIENS A,et al.Receding horizon model-based predictiveControl for dynamic target tracking: AComparative study[C]//Guidance,Navigation,andControlConference.Chicago,Illinois: AIAA,2009:1−9.

[19]KANG Y,HEDRICK J.Design of on linear model predictiveController for a small fixed-wing unmanned aerial vehicle[C]//AIAA Guidance,Navigation,andControlConference and Exhibit.Keystone,Colorado,2006: 21−24.

[20]INNOCENTI M,POLLONI L.Management ofCommunication failures in formation flight[J].Journal of AerospaceComputing,Information,andCommunication,2004,1(1):19−35.

(编辑 陈灿华)

Control method for multi-UAVsCooperative obstacle avoidance

ZHANG Lipeng1, RUChangjian2, ZHOU Huan2
(1.Department of Theory and Training,Air Force Xi’an FlightCollege,Xi’an 710306,China; 2.College of Aeronautics and Aerospace Engineering,Air Force Engineering University,Xi’an 710038,China)

Abstract:For solving the problem of multi-UAVs(multi-unmanned aerial vehicles)Cooperative obstacle avoidance in dynamic environment,a method forController design inCombination with the extended Kalman filter(EKF)and model predictiveControl(MPC)was proposed.Firstly,distributed architecture for UAVCooperative obstacle avoidance,the motion model of UAV and theCommunication topology were established,respectively.Then the EKF algorithm was used to predict the trajectory of dynamic obstacle,and an informationCompensation rule was designed.Afterwards,based on the model predictiveControl(MPC)method,theController for UAV obstacle avoidance was designed.The results show that the proposed EKF methodCan predict the trajectory of dynamic obstacleCorrectly,and that theCooperation between the UAVsCan reduce the predictive errors effectively.

Key words:UAVsCooperation;obstacle avoidance;extended Kalman filter;model predictiveControl

中图分类号：V279；TP273

文献标志码：A

文章编号：1672−7207(2016)01−0114−09

DOI:10.11817/j.issn.1672-7207.2016.01.017

收稿日期：2015−01−10；修回日期：2015−03−08

基金项目(Foundation item)：国家自然科学基金资助项目(61105012)；中国航空科学基金资助项目(20135896027)(Project(61105012)supported by the National Natural Science Foundation ofChina; Project(20135896027)supported by theChina Aviation Science Foundation)

通信作者：张立鹏，助教，从事无人机群协同控制问题研究；E-mail: zhang121860@163.com