基于时间约束的无人机避障研究

杨秀霞，刘小伟，张毅

(海军航空工程学院控制工程系，山东烟台264001)

0 引言

目前，无人机在军事及民用领域已得到越来越广泛的应用。如何使无人机能够安全导引到目标并顺利避开动态障碍物是众多学者研究的重点课题。

最早对这一问题开始研究的是 Khatib［1］，Erdmann等［2］。后来，学者们把在静态环境下得到成功应用的路径规划及避障方法推广到动态环境中。如在文献［3］中提到了确定及随机的路径地图方法;在文献［4］中考虑机器人及障碍物的速度，并加入到势能函数的构造中，得到适用于动态环境的势场法;在文献［5］中通过动态窗口方法进行路径规划及避障。这些方法在静态环境下得到了较好的结果，但是在动态环境下，尤其是应用于无人机高速运动这一特殊情况下，避障效果并不理想。其原因主要是在动态环境下，碰撞判断及避障情况下对速度的控制与静态环境相比，存在较大的区别。

为解决动态环境下的碰撞判断问题，文献［6-7］提出了基于运动学方程的碰撞判断方法。文献［6］定义了碰撞锥的概念，依据相对速度角度与障碍圆的切线来确定碰撞。在文献［7］中基于视线角速度方程，对直接碰撞和间接碰撞进行了比较。

同时，考虑到动态环境下障碍物速度变化的问题，学者们提出了速度障碍的概念。例如文献［8］在考虑运动障碍物速度的基础上进行碰撞锥的构建，在此基础上，通过使机器人的速度转移到碰撞锥之外达到避障目的，但是对障碍物的速度要求必须是线性的。为解决障碍物速度为不定值的避障问题，文献［9］通过对某一时刻，在考虑无人机及障碍物相对位置情况下，对可能的碰撞速度进行时间积分，得到非线性速度障碍集进行避障。同时考虑到多无人机之间的避碰问题，文献［10］提出了互惠速度障碍的概念，即两个即将相碰的无人机之间互相主动避碰，达到避碰目的。

但上面所有的方法均未考虑避障过程中避障完成时间的问题。现实情况下，只有避障完成时间小于无人机到达避障点时间，才能确保无人机成功避障。因此，基于对避障完成时间的考虑，本文提出了滑模变结构有限时间收敛制导律的避障算法，使无人机相对障碍物的速度方向在有限时间内转移到碰撞锥之外达到避障目的。本文首先在极坐标系下建立无人机与障碍物的相对运动学方程，通过引入滑模变结构有限时间收敛制导律进行避障算法设计。最后对有限收敛时间进行分析，得到了满足要求的参数范围，可以使无人机的收敛时间小于到达避障点时间，确保避障完成。最后利用Matlab对设计的避障算法进行了仿真验证。仿真结果表明，在选择合适的参数下，无人机相对障碍物速度方向收敛到避障切线方向的时间小于无人机到达避障点的时间，验证了算法的有效性。

1 几何关系及运动学方程

1.1 问题描述

当无人机与障碍物在惯性空间运动时，建立无人机与障碍物的几何关系示意图，如图1所示。无人机的速度大小为vuav，方向为ψuav;障碍物的速度大小为vobs，方向为ψobs。无人机相对障碍物的速度大小为vrel，方向为ψrel，其中vrel=vuav－ vobs，－ vobs为障碍物速度相反速度。由速度障碍原理，通过扩大障碍圆半径为RP，则把无人机简化为质点U，基于质点U及障碍圆切线BU和AU，建立碰撞区域UAB。若vrel位于UAB内，则存在碰撞危险，因此需要调整无人机的速度大小及方向，使vrel调整到UAB之外。调整时，一般考虑vrel角度变化最小即避障完成时间短、偏离原目标轨迹短原则。结合图1，选取避障切线为BU，即避障点为B，其中避障切线BU的长度为rBU，方向为 θBU。

本文提出的避障算法是基于滑模变结构有限时间收敛制导律，使无人机能够在有限时间内到达避障点B，且相对速度方向ψrel与切线BU方向θBU相等，确保避障完成。

图1 无人机与障碍物几何关系示意图Fig.1 Schematic diagram for geometric relation of UAV and obstacle

1.2 建立运动学方程

由于假设障碍圆是刚性圆，因此B点的速度大小及航向角与圆心一致。结合图1，在极坐标系下可得无人机U与B点之间的距离rBU及视线角θBU的变化率为:

为研究方便，建立无人机与障碍物的相对运动方程，此时无人机以相对速度vrel飞向避障点B:

式(3)给出了无人机与避障点B之间的距离变化关系，式(4)给出了无人机与避障点B之间的视线角的变化方程。

对式(4)求导，并将式(3)代入，整理可得:

式中:urel为相对速度vrel的加速度在视线法向上的分量。

选取无人机在发现障碍物开始避障的初始时刻为t=t0，初始状态为rBU(t0)和，初始视线角为θBU(t0)，在t时刻的状态为rBU(t)和，令x，则式(5)可写为:

2 滑模变结构有限时间收敛避障算法设计

无人机在避障过程中控制系统容易受到参数变化和外界环境干扰，因此制导律的设计必须考虑系统的鲁棒性。滑模变结构控制对参数摄动和外界干扰具有不变性，使系统具有良好的抗干扰能力，因此基于滑模变结构控制理论设计有限时间收敛避障算法是有实际意义的。下面将基于滑模控制理论设计有限时间收敛制导律，通过选择合适的参数，使避障完成时间小于到达避障点时间，完成避障。

2.1 制导律设计

由初始时刻x1(t0)=0，x2(t0)≠0设计非奇异滑模面为:

式中:β＞0，p和q(p＞q)为正奇数。

设计非奇异滑模控制器为:

式中:1＜p/q＜2，η＞0。

则在控制器式(10)的作用下，系统将克服外界干扰，在有限时间内到达滑模面式(9)，并且状态量x1和x2沿滑模面运动，将在有限时间内收敛到零。

2.2 稳定性分析

对于滑模到达阶段，选取Lyapunov函数:

对式(11)求导，可得:

结合式(8)和式(9)可得:

把式(10)代入式(13)得:

因此可得:

由于1＜p/q＜2，则0＜p/q－1＜1，又由于β ＞0，p和q(p＞q)为正奇数，则0时)。因此可得:

可见，控制器满足Lyapunov稳定条件，系统状态在有限时间内收敛到滑模面。

2.3 有限收敛时间分析

基于滑模变结构控制律，系统运动分为滑模到达和在滑模上运动到平衡点两个阶段。因此，收敛时间分为相应的两部分。针对滑模到达阶段，设s(t0)≠0到s(t)=0的时间为tr。当t=t0+tr时，s(t0+tr)=0。由得:

当 s≥0时，有:

对式(17)两边积分得:

则:

即:

同理，当s≤0时，有:

下一步计算状态变量沿滑模面运动到平衡点的时间ts。考虑到系统初始条件的奇异性，设计滑模面为:

当状态变量在滑模上运动时，满足s=0，即:

同时式(21)等价于

则:

因此总的制导系统的收敛时间可表示为:

综上可知，所设计的制导律能够使制导系统状态在有限时间内收敛。由式(20)及式(23)可以看出，系统状态到达滑模面的时间与选取的参数η成反比，即η越大，系统状态收敛越快，收敛时间就越短;当系统状态到达滑模面后，收敛到原点的时间与选取参数p，q及β有关。因此可以通过调节参数η，p，q及β来控制系统的收敛速度，使总的制导系统的收敛时间t小于到达避障点时间trea=rBU(t0)/vrel，完成避障。

3 无人机航向角控制

由上述避障算法设计可求出控制urel。假设相对速度大小不变，即，则由式(6)可知:

进而可知:

假设障碍物为匀速运动，则arel全部由无人机提供，因此可得无人机速度大小及方向角变化率为:

由式(27)及式(28)就可求出无人机的运动轨迹。

4 仿真结果及分析

为验证上述避障算法的有效性，对避障算法进行仿真。

假设障碍物为匀速运动，vobs=43 m/s，ψobs=0 rad，初始坐标为 xobs0= －10 km，yobs0=10 km，无人机初始速度大小为vuav0=43 m/s，初始方向角为ψobs0=(π/2)rad，初始坐标为xuav0=0 km，yuav0=0 km，假设无人机和障碍物圆半径RP=250 m。

通过计算，可求得无人机到达避障点B的时间为:

因此，通过选择合适的参数η，p，q及β，可使无人机在到达避障点前相对速度角度收敛到期望角度。

通过计算，选择η=0.002，p=8，q=7，β=0.17，仿真结果见图2～图7。

图2 视线角变化示意图Fig.2 Schematic diagram for variation of the angle of sight

图3 视线角速率变化示意图Fig.3 Schematic diagram for variation of the angular rate of sight

图4 相对速度角度变化示意图Fig.4 Schematic diagram for variation of the angle of relative velocity

图5 无人机速度变化示意图Fig.5 Schematic diagram for velocity variation of UAV

图6 无人机速度方向角变化示意图Fig.6 Schematic diagram for variation of velocity and orientation angle

图7 无人机避障过程示意图Fig.7 Schematic diagram for UAV to avoid the obstacles

由图2可知，当无人机观察避障点B的视线角稳定时，t=20 s，小于trea=22 s，且稳定视线角与初始时刻发现避障点B的切线角相等，保证无人机能够顺利避障。由图3可知，在t=20 s时，视线角速率，视线稳定，再次证明避障完成。由图4可知，当t=20 s时，相对速度角度ψrel=θBU=2.717 6 rad，即相对速度角度稳定时，相对速度角度到达避障方向，避障完成。由图5和图6可知，无人机实际速度大小及方向角变化不剧烈，即过载不是很大，保证了过载要求。由图7可知，障碍物圆与无人机圆相遇时两圆相切，也就保证了避障完成。

5 结束语

本文基于无人机与障碍物之间的几何关系，得到无人机与障碍物在极坐标系下的运动方程，在考虑避障完成时间的基础上，通过引入滑模变结构有限时间收敛导引律，设计避障算法，选择合适的参数，可使避障完成时间小于无人机到达障碍物的时间，保证了避障完成。仿真结果证明了该算法的有效性。

本算法的实现仅需要知道障碍物及无人机的位置及速度这4个参数，因此，算法操作起来比较方便。下一步的工作是将该算法扩展到三维空间，并对障碍物是变加速的情况进行研究。

［1］ Khatib O.Real time collision avoidance for manipulators and mobile robots［J］.The International Journal of Robotics Research，1986，5(1):90-98.

［2］ Erdmann M，Lozano-Perez T.On multiple moving objects［J］.Algorithmica，1987，2(1):477-521.

［3］ Fujimura K，Samet H.Roadmap-based motion planning in dynamic environments［J］.IEEE Trans.Robot，2005，21(5):885-897.

［4］ Ge S，Cui Y.Dynamic motion planning for mobile robots using potential field method［J］.Auton Robots，2002，13(3):207-233.

［5］ Seder M，Petrovic I.Dynamic window based approach to mobile robot motion control in the presence of moving obstacles［C］//Conf.Robot.Italy:Autom，Roma，2007:1986-1992.

［6］ Chakravarthy A，Ghose D.Obstacle avoidance in dynamic environment:a collision cone approach［J］.IEEE Transactions Systems，Man Cybernetics:Systems，Humans，1998，28(5):562-574.

［7］ Belkhouche F，Belkhouche B.Kinematics based characterization of the collision course［J］.Robot Autom，2008，23(2):127-136.

［8］ 朱齐丹，钟训昱，张智.基于速度变化空间的移动机器人动态避碰规划［J］.机器人，2009，31(6):539-547.

［9］ Large F，Sckhavat S，Shiller Z，et al.Using non-linear velocity obstacles to plan motions in a dynamic environment［C］//ICARCV Control，Automation，Robot and Vision Conference.France:IEEE，2002，734-739.

［10］ Van den Berg J，Lin M，Manocha D.Reciprocal velocity obstacles for real-time multi-agent navigation［C］//ICRA Robotics and Automation Conference.CA:IEEE，2008，1928-1935.