基于拉普拉斯人工势场的无人机避障控制*

顾育津，宋孝成，刘晓培，陆疌

(1 中国科学院上海微系统与信息技术研究所, 上海 200050； 2 中国科学院大学, 北京 100049； 3 上海科技大学, 上海 201210)

近些年来，无人机的自主飞行问题备受研究者的关注。无人机想要完成这一任务，关键是运动规划(motion planning)[1]。解决这一问题多数思路是预先规划生成合理的避障路径，再令无人机沿着路径行至目标点[2]。典型的路径规划算法包括如A*、D*、Dijkstra的最短路径算法[3]，基于快速随机搜索树(rapidly-exploring random tree，RRT)的一系列随机搜索算法，如DDRRT[4]，RRT*[5]以及人工势场法(artificial potential field)。人工势场法由Khatib于1986年提出[6]，其基本思想是建立一种目标位置处的引力场和障碍物位置处的斥力场共同作用下的人工势场，在进行路径规划时沿着负梯度的方向进行路径搜索。然而传统人工势场法最大的缺陷在于势场中可能存在局部最小值点。为解决这一问题，文献[7]提出一种基于搜索的方法，但需要较高的计算量；文献[8]针对局部最小点设置附加的虚拟势场，文献[9]针对特定场景制定特殊的导航策略，但都不够简洁直观。

研究表明基于拉普拉斯方程的调和势场函数(harmonic potential field) 不存在局部最小值[10]。本文基于传感器探测生成的全局地图建立调和势场，工作步骤如下：

1)采取边界元法(boundary element method，BEM)在欧式空间抽象出地图中的边界与障碍物，这种方法只需要利用地图中障碍物的边界单元，存储要求低并且可以快速求解出势场中任一点的梯度方向。

2)设计线性二次高斯(linear quadratic Gaussian，LQG)控制器进行顶层的运动控制，在获得相应的参考速度方向后，控制器跟踪调整无人机姿态使自身速度方向趋向参考方向，最终逐渐飞向目标点。这种基于速度场的控制器，不需要机械地沿着预先规划好的路径行走，在应对传感器误差、环境影响上更具鲁棒性。

3)在仿真平台Airsim 进行试验。实验结果显示面对数量较多且分布复杂的障碍物，本文提出的算法可以有效引导无人机进行避障控制并最终到达目标点。

本文设计的算法脱胎于传统人工势场方法的思想，着力寻找解决传统方法固有缺陷的途径，在实际应用中更具备实用性与可靠性。拉普拉斯人工势场可建立在由传感器扫描重建的地图上(如点云)，不同于传统人工势场，这种方式对于障碍物的边界与位置描述更为精准，有利于保障无人机在复杂环境中的路径规划与飞行安全，也更适合拓展到实时探测与规划的层面。对比RRT、A*、遗传算法等搜索算法，拉普拉斯人工势场从本质上避免了陷入“局部最优解”的烦恼，保证收敛到全局最优解。边界元法往往应用在力学等传统工程问题上，本文设计的算法将边界元法与地图模型特性良好结合，在求解导航问题时无须设置调试众多参数，在应用实现上更为简便可靠。

1 基于拉普拉斯方程的人工势场

1.1 人工势场的构建

人工势场法的基本思想是在无人机或者机器人的可行空间上建立合适的势函数，使其沿着函数负梯度方向运动，直至最终到达目标点。传统的人工势场法分别对障碍物跟目标点建立引力斥力函数从而进行代数叠加得到某一点合力函数。这种方法往往无法应对实际应用场景中各种复杂形态的障碍物，简单的几何替代会损失一定程度的规划精度；当地图中障碍物的数量增加、相对位置更为复杂后，这样产生的势场也容易变得局部畸形，比如可能存在的局部最小点将无人机困住无法逃离，某些点梯度方向的骤变导致最终的飞行路径不够光滑，加大了姿态控制的难度。

那么称φ在Ω上是调和的，其中Δ为拉普拉斯算子。调和函数具有许多良好的性质，由强最大最小定律[11]可知，φ在Ω上没有局部最小点，且可推导出如下的结论：

命题1表明在有界区域上的调和函数值被边界值所限定，且全局最大最小值只能出现在边界上。

由定义可知，调和函数是拉普拉斯方程的解，因而可以通过求解该偏微分方程构建调和势场。当合理设置边界条件时，相应的调和势场函数将唯一确定[12]。常见的拉普拉斯方程边界条件有两种——Dirichlet与Neumann。前者是给势场边界设置固定的势场值，后者则是设置边界处的外法线方向导数。在二维空间下，上述初始边界条件下的拉普拉斯方程可表示为

(1)

(2)

(3)

在可行空间上覆盖的调和函数没有局部极小点，二阶可微的性质令导出的梯度场仍是光滑的，非常适合后续的导航控制。将构建可行空间上的调和函数转化成式(1)～式(3)组成的边界值问题(boundary value problem)便是简单直观的方式。

Γo=∂Ω∪∂,

目标点的包围边界为目标边界Γg:

基于上述定义可构建针对无人机运动规划的拉普拉斯方程，在阻碍边界与目标边界处分别设置Dirichlet条件，式(1)～式(3) 转化为

(4)

φ(x)=M,∀x∈Γo，

(5)

φ(x)=m,∀x∈Γg.

(6)

式中：m与M均为实数，且M>m。

1.2 梯度场的求解

H(x,y)φ(y)]dS(y),

(7)

其中：

(8)

(9)

(10)

如图1,S表示有界区域的边界曲线，y是其上一点，r表示x与y两点间的欧氏距离。G(x,y)为二维空间下的格林函数，n为外法线方向向量。区域内任一点的梯度也可直接导出，即 ∀x∈Ω,i=1,2，

图1 边界积分方程Fig.1 Boundary integration equation

(11)

结合式(7)～式(11)，一旦求解出边界上的势场值与外法线方向导数，就可以得到集合区域内任一点的势场值与梯度值。

由于边界积分方程没有解析解，采取边界元法进行离散数值求解。本文将边界S分割成N个常量单元，每个边界单元为直线段，表示为ΔSi,i=1,…,N。每个单元中心上有一节点pi,i=1,…,N，单元上的势场值φi与 外法线方向导数qi等于节点上的值，从而将边界积分方程转化为分段线积分

(12)

其中

(13)

(14)

本文采用文献[13]中提到的解析解求解式(13)、式(14)中的线积分。特别地，由于可达区域为多连通集，在总构型边界∂Ω上的元素其线积分为逆时针方向，反之障碍物边界与目标边界上的线积分方向为顺时针。令向量Φ=(φ1,…,φN)，q=(q1,…,qN),并构造矩阵H,G∈n×n使其第i行，第j列的元素分别为Gij与Hij，式(12)转化为

(15)

本文中拉普拉斯方程的初始条件均为Dirichlet条件，无须进行元素交换，式(15)即为形如Ax=b的标准线性方程组。一旦求解出方程的解，便可利用边界元素上所有的φ与q求解可达区域中任意一点的负梯度值，并作为随后运动控制的参考速度方向。

通过以上研究，本文认为当前正处于转型期的中国不应只将经济增长、收入增加作为国家追求的终极目标，而应该在经济取得一定成就后，逐渐将视角转移到增强国民实际幸福感这一问题上。而提高国民幸福感除了采取措施提高收入外，改善医疗、教育、就业等民生问题显得十分重要。另外，由于个体之间对于幸福的感受存在异质性，政策的制定和实施应该做到有差别且具针对性，应该更多关注弱势群体和幸福感低的群体，提升他们的幸福度，以维护社会的稳定和经济的可持续性发展。

2 基于梯度场的姿态控制

实际飞行过程中，无人机的建模不是单纯的质点，需要面对空气阻力、自身传感器误差等诸多问题。本节设计的顶层控制算法将连接路径规划与底层的姿态控制，当无人机到达某一位置时，上一节构建的人工势场能够实时导出其下一时刻的参考速度方向，控制算法将对应调整姿态使其逐渐趋向所设计的运动状态。

惯性坐标系采用NED 坐标系，即x轴指向东，y轴指向北，z轴指向地面，令机身坐标系基底方向为 [x′y′z′],相关符号的详细含义参见表1。

表1 建立模型所用变量Table 1 Variables used in dynamic model

旋转矩阵R描述输入分量u到无人机姿态的映射，同时将姿态控制分解为对u的控制。考虑无人机飞行时的运动学方程，它被描述成如下的线性系统：

(16)

(17)

其中Q为二阶半正定矩阵，Qe与R为二阶正定矩阵。

图2 LQG伺服控制Fig.2 Linear quadratic Gaussian servo controller

其中Kf为卡尔曼系数。求解对应的 Riccati 方程

K∈2×4为对应增广状态方程下LQR调节器的反馈系数。在忽略系统扰动与观测噪声的情况下，增广系统表示为

相应的目标函数为

3 实验仿真与分析

本文采用 Airsim[16]作为实验仿真平台。Airsim 是微软开发的一款开源自动驾驶仿真平台，基于虚幻引擎打造，能最大限度地模拟现实环境中的物理表现。Airsim 为无人机模型提供基于PX4的底层控制，将上节提到的旋转矩阵R转换为四元数即可直接控制无人机的运动。

本文的Airsim仿真程序基于 C++ 编写，CPU为Xeon E5-2650，主频2.3 GHz。为检验算法的有效性跟可靠性，建立多种不同类型的全局地图。地图同样采用NED坐标系，无人机的高度限定在5 m。实验1中，底边长为20 m的立方柱紧密排列，构成一个类似走廊的地图环境。地图的总区域为一个140 m×110 m的截面，目标点位于(40 m,60 m)，以边长为1的正方形包围构成目标边界。设阻碍边界Γo初值为100，目标边界Γg初值为0，使用BEM算法求解对应的拉普拉斯方程，可求得如图3的人工势场。图3(a)、3(b)分别展示人工势场的等高线势场与流线图。

图4展示Airsim中的实际仿真结果。无人机的质量设定为1 kg，起点位于(-20 m,-15 m)，LQG控制器中的相应参数飞行速度大小‖v‖维持在3 m/s。当程序获取到地图全局信息后，通过BEM求解建立人工势场。求解过程一旦完成，无人机可实时计算出下一时刻的参考速度方向，并在控制器的引导下调整姿态从而逐步达到目标点。仿真得到的无人机实际轨迹与图3中的流线图几乎一致。可以看到，运动轨迹的光滑性体现了拉普拉斯势场无限可微的优势，这一特点也更好地适应对无人机的动力控制；另一方面路径选择也达到了路径长度最优与碰撞概率最小的某种平衡。

图4 实验1：Airsim仿真图Fig.4 Experiment 1: Airsim simulation

实验2中，地图中障碍物的数量增多，且分布更为凌乱。图5(a)～5(c)分别展示不同起点下无人机的飞行轨迹。结果显示，无论起点在何处，无人机都可以自行选择合适的飞行方向达到目标点，且不需要再进行重复的规划计算。传统的路径规划与导航控制算法大都采取预先根据地图规划并生成路径，再利用控制器控制无人机进行路径跟踪的理念。在实际应用中，一旦遭遇强风、信号干扰等意外，无人机的底层控制器超出稳态，其位置发生偏移，路径跟踪器将逐渐引导无人机回到之前的路径或进行重新规划。本文提出的算法在应对这种情况时有明显的优势，图5(d)展示强烈外界干扰后，无人机自行调整的飞行结果。在Airsim中模拟无人机遭遇瞬间大风后发生短暂失控的情形，其中折线段表示无人机的位置瞬间偏离，图5(d)中的无人机起点与图5(c)中一致，但由于遭遇意外干扰，在岔路口处路径选择与图5(c)并不一致，从而使得整个飞机轨迹都截然不同。这种调整并不需要额外的程序设计与线程来应对，节约了计算资源，实时性高，非常适合实际场景的应用。

图5 实验2：Airsim仿真图Fig.5 Experiment 2: Airsim simulation

仿真实验结果表明本文设计的算法非常适合迁移到实际飞行的应用中。实验2中的全局地图抽象得到的476个边界元，采用单线程求解时间仅需2.01 s。这一步骤可由无人机自带的处理器进行计算，也可线下计算通信传输，通信负担仅为每个边界元的位置坐标与H,G两个n×n矩阵。模拟飞行中，将无人机的底层控制频率设为100 Hz，在每个控制周期内，无人机根据自身位置实时计算出当前的参考速度，平均耗时仅在3 ms左右。图6展示整个飞行过程中无人机的飞行速度变化，其中实线为参考速度值，虚线为实际值。 即便当参考速度发生突变时，控制器也能及时响应，迅速改变调整无人机的速度达到理想状态。

图6 速度方向的追踪控制Fig.6 Velocity direction tracking control

4 结论

本文提出基于拉普拉斯方程的人工势场，

1)根据全局地图抽象出可行区域边界，合理设置初始边界条件，采用边界元法求解相应方程。求解得出的调和势场相较传统人工势场可以适应各种形态复杂的障碍物，内部不存在局部极小点，二阶可微使得导出的梯度场连续光滑，有利于无人机等移动机器人的控制；

2)基于人工势场导出的速度场，设计关于无人机姿态的控制器。控制器使无人机合理调整姿态，不断追踪更新中的参考速度，直至最终达到目标点；

3) 基于Airsim的拟真环境的仿真实验，验证了算法的有效性与鲁棒性。在应对外界环境的意外干扰下，算法保证了无人机路径与运动状态的合理选择，具有较好的实时性与较广泛的实际应用前景。鉴于本文提出的人工势场依旧构建在二维空间，对于三维空间下的应用扩展将是后续研究的方向。