基于矢量地图的无人机三维航线校正仿真

刘 霞，马 跃，周 倩

(1. 天津中德应用技术大学，天津 300350；2. 天津中医药大学，天津 300193)

1 引言

无人机的航线校正是在一定的限制条件下，建立能够实现无人机自动驾驶与执行任务的最佳飞行路径。无人机在执行任务时容易受到气流等环境因素影响，导致航线偏离，所以航线校正是当前研究的核心问题。

目前，用于航线校正的主要方法包括遗传算法[1]、人工势场法[2]和双目测距模块[3]等。遗传算法存在计算复杂，不利于快速收敛从而易导致产生局部最优；人工势场法通常用在二维的航迹搜索与校正，不利于对三维行线的校正；双目测距模块会提前设定无人机飞行方位，在行驶过程中倾角容易变大，当出现阻碍时会产生间歇性的目标丢失，导致其存在准确性较低的缺陷。

对此，本文引入适量地图，提出了一种新的航线校正方法，矢量地图主要作用于无人机运行轨迹显示设备当中，它能够展现出地理方位、设定运行范围，利于标记航迹信息，通常情况下，地图的使用策略都能够较好的运用在矢量地图上。该方法首先利用LPA算法[4]和矢量地图[5]的显示数据部署三维航线，并且利用优化后的估价函数有效搜索最优路径，结合了N.Nillson研究的一种对于航线搜索与部署传统式A*算法，能够快速寻找到最优路径。通过仿真实验验证该方法的时效性和准确性。

2 矢量地图的加载

地图仅显示地理方位不能满足实际运行要求，因此增添了城市、山脉、铁路、河流等显示信息，从而使地图显示变得更加完善。矢量地图能够根据数字高层地图收集阻碍飞行轨迹的信息，经过处理后构成阻碍运行模型，最后会展现在矢量地图上。缩放矢量地图的同时不会改变真实性，这也成为它的显著特性[7]。矢量地图可以通过构建空间坐标来描述地理方位，并且在实际运用过程中，技术人员能够通过无人机运行情况，适当的关闭与当前所需要无关的信息。无人机矢量地图加载过程分为以下几个阶段：

1)启动BIGEMAP加载软件[8]。

2)把鼠标移动到加载软件的左上方，单击页面中带有“选取地图”的标志，接下来点击地图下载渠道。

3)单击屏幕上的“行政范围选取”，接下来在列框中点击中国。

4)双击下载软件后，会弹出一个“下载对话框”，点击“行政区域”，然后会跳到矢量地图下载界面，同时选取矢量地图的储存方式“Shape File”，选择地理坐标，用鼠标单击“选择”，选取相应的坐标投影，但经纬度坐标是固定的WGS84。“运行名称”以及“保存方式”可以自行编辑。

(5)对加载区域级别进行选取，若选取“市”区域级别，那么此时矢量地图上的“基础数据”会根据所选择的区域进行编制，编制完成后单击“确定”，从而获得市级行政区域的矢量地图。

综上所述，就能够加载所需要的矢量地图，若想要下载市级或乡镇行政区域，要在第(4)阶段选取相应的行政阶级，但不会变更保留方式与方位坐标。

3 无人机三维航线校正

3.1 基于LPA算法的飞行轨迹部署

经典的A*算法通过启发式方法很好的采用了地图展现出来的信息，(作用在下面黄色部分)有效的减少了节点转换次数。该算法利用估价函数[9]f(n)预算现有节点n与终点间的距离，同时确定了搜寻方位。代价函数如式(1)所示

f(n)=g(n)+h(n)

(1)

其中，g(n)表示初始点位距离目标点位n的实际代价，h(n)表示启发(发生)函数。根据A*算法能够推导出许多启发式搜索方法[10]，但绝大部分都是应用在二维航线部署上，有极少部分使用在三维航线中，基于此，本文提出了LPA算法，该算法由A与SWSP-FP构成，主要核心就是利用前搜索树中的数据，来应对权值发生变化时产生重复部署的情况。针对无人机的飞行要求，把LPA算法转换到三维路径部署当中，然后利用矢量地图的数据信息，很好的避免了地势的阻碍，从而缩短了优化搜索时间，提高了航线校正效率。

经典的LPA算法通常被广泛用在二维航线中，但结合本文的题目会把该算法转换到三维航线中，保持和A*方法当中的非负相同启发值h(n)，由此可知三角不等式如下所示

(2)

其中，succ(n)表示n的下一个元素。LPA*算法的估价值分别为g(n)与rhs(n)，前者与A*用到的含义相同，后者表达式如式(3)所示

(3)

其中，rhs(n)表示n的父节点，c(s′，n)表示两节点n与s之间的最短距离。当g(s′)=rhs(n)时，说明n节点达成局部一致性。

LPA算法不仅对受阻碍的节点实施部署，并且要对所有部署后的飞行节点进行局部一致性核实，对于非局部一致性的节点会将其转化为初始点，因此当进行最后一次部署飞行轨迹时采用的仍是最佳路径。且通过rhs(n)对权值转变的应急能力来更换增加的节点。若矢量地图重新刷新时，则需检查节点的局部是否存在一致性，若不是应采用启发函数来增加节点，节点为一致性那么不需要进行改变，由此可知当权值发生变换时，不必对全局节点进行更改，从而提高了航线部署和校正效率，算法过程如下所示：

步骤1：首先让估价函数满足f(n)=g(n)+h(n)，h(n)选择此时位置与终点位置的最短路径，由给定限制条件可知，要合理选择参变量S0、α、β、Smax、Hmin、P、与S。更新U表，每个节点都要满足n∈Svertex，制定g(n)=rhs(n)=∞，rhs(Sstart)=0，将Sstart储存在U表里，从而仅有Sstart不存在一致性，然后不断运转以下步骤。

步骤2：不断推进步骤2到步骤8的运行，让rhs(Sstart)=g(Sstart)，也就是搜索到目标节点，然后从结束计算的地方重新向上推算，最终回到初始点，从而能够获得从初始点距离终点的极小代价飞行轨迹。如果U表内无储存内容，那么代表运行失败。

步骤3：在U表中选择含有极小f函数的节点，然后把它转换到Q表中。

步骤4：如果BestVode是最后搜索到的最优节点，那么对其求解。

步骤5：如果BestVode非最优节点，那么求出g(BestVode)与rhs(BestVode)，如果前者大于后者，那么直接执行步骤6，如果前者小于后者，那么跳转到步骤7。

步骤6：让g(BestVode)=rhs(BestVode)，扩大目标点位的范围。横截面的面积相当于转弯夹角的双倍，同时根据运行得到目标节点处的位置，采用横截面投影作为中心线。纵截面的面积相当于最长升降值的双倍，并垂直于横截面。扩大范围处的半径长短取决于极小步长S0。BestVode形成P×M个后继节点Successor，最后将所有Successor节点u按照步骤8运行。

步骤7：g(BestVode)=∞，将所有Successor节点u按照步骤8运行。

步骤8：构建u重新回到BestVode的方法：若u不能够满足Sstart，那么让rhs(u)=mins′∈pred(u)(g(s′)+c(s′，u))；若u∈U，则把此节点叫做OLD，通过对比新旧航迹的代价，来决定OLD节点的父节点；若g(u)≠rhs(u)，那么求出u的f大小，看能否实现极大距离限制与运行高度限制；若达成D(x)+SL(x)≤Smax与矢量中所有点距地面长度h≥Hmin，那么把u储存在U表中。

步骤9：观察是否有新阻碍产生。如果产生新的阻碍，使用ModifyCost()参数来调整阻碍大小，并用Update-Vertex()参数来检验局部非一致性节点，如果g(u)≠rhs(u)，那么对遭到阻碍的节点采用insert(u)参数，储存到U表中。

3.2 基于偏离部署航线的校正算法

无人机在完成任务时，可能会受到天气等因素的影响，导致航线偏离。因此，在已部署好的航线基础上提出新的三维航线校正方法。在进行航线校正前，对无人机进行准确定位是必要条件，定位的主要核心就是通过路面监控站监测无人机所在方位，接下来把方位数据利用监控站的通讯设备传输给无人机，无人机会采用方位数据与参变量，通过卡尔曼滤波方法估价无人机所在方位。

设该算法在tk时间的无人机三维空间坐标为(X(tk)，Y(tk)，Z(tk))，则可推导出以下公式

(4)

路面监控站的方位坐标用(Xi，Yi，0)来表示，在飞行过程中，若监控站可以监测出无人机此时的对应方向夹角与高度夹角，而且给出监控站的高度，则能够估价出无人机的空间方位。所以，如果监控站的高度等于零，那么可以忽略不计无人机所在高度夹角，由此可以判定无人机的飞行高度不会根据时间而发生改变。φ(tk)表示无人机的中心到监控站之间的直线与X轴夹角的角度，角度信息是经过监控站监测后传输到无人机中，由此可以推导公式为

(5)

式中，εk描述了监控站监测无人机方位中产生的所有偏差，满足平均值等于零，差值是σ2的高斯分布。代表根据时间发生改变的元素，但它只在无人机不在监测区域内同时接收到其它监控站信息时产生变化。若无人机仅接收此时监控站的信息，那么Ii(tk)=1。

以上公式能够根据坐标信息与参变量准确估价出无人机所在方位，成为航线校正的一种主要保障。

根据3.1的航线部署公式可以推算出，当实际航线受到干扰时可以表示为

(6)

(7)

本文校正算法在基于无人机偏离部署航线上，引入了一种校正因子θj(tk)，用于校正无人机的三维航线，其表达式为

(8)

(9)

航线经过校正后，根据式(9)可以达到与部署航线之间的最小方差值。

(10)

(11)

(12)

从而得到：

(13)

通过综上所述内容，能够有效校正当无人机偏离部署航线时的飞行角度。

4 仿真结果与实验分析

4.1 实验参数设置

为了检验本文算法的有效性，该实验环境采用编程规格为VisualC++6.0，矢量地图尺寸为128×128，同时加入了阻碍信息。实验使用的无人机为自行组装的六旋翼无人机，GPS选用M8N，相机采用经校验的SonyILCE-6000，水平视场角为36°，垂直视场角为27°。实验主要技术参数如表1所示。

表1 实验主要技术参数

使用无人机对某地拍摄了共 74幅图像。为说明本文方法可行，特选取了九幅正摄图像，验证本文方法。拍摄图像如图1所示。

图1 航线拍摄图像

4.2 时效性分析

在实验环境与参变量相同的情况下，根据无人机的GPS 与姿态信息，软件处理后给出了相机位置并计算得到稀疏点云，如图2所示。航线上拍摄的图像相对位置与所给的航线方案一致。

图2 稀疏点云

采用SAS算法与本文算法对比整个场景建模时间，结果如表2所示。能够看出本文算法在航线校正中所用时间相对较短，而且受到的阻碍节点也较少，从而提高了航线部署和校正效率。

表2 LPA和SAS算法的时效性对比分析

4.3 准确性分析

图3 无人机定位航线示意图

(X(t1)，Y(t1))=(0，0)

(14)

(15)

由上图可以看出，此时无人机路径为平滑可飞，处于最优状态。图4对无人机定位航线校正算法进行了准确性检验。在监控站监测范围内，无人机能够按照部署航线飞行，但不在监控范围内时，无人机开始慢慢偏离部署航线，校正开始失效，从而可以看出本文校正方法的准确性。

图4 无人机航线校正准确性

综上所述，基于矢量地图方法具有较高的时效性和准确性，满足了实际应用需求。主要原因在于引入适量地图加载，利用LPA航线部署方法分布三维航线点，并通过检验局部一致性来防止重新对全局进行搜索，减少了干扰因素的阻碍与搜索空间范围，从而提高了校正实时性与准确性。

5 结论

无人机在飞行过程中容易受到环境等因素影响，导致偏离航线，所以本文首先提出了算法，它能够根据前期搜索树上的数据提高后期的搜索效率，循环使用信息以免发生重复部署，同时克服一部分节点受到阻碍的影响，增强了后续的航线校正效率。卡尔曼滤波算法计算出无人机当前位置，最后通过路面监控站传输的

方位数据计算出无人机偏离角度，并提出校正因子对飞行航线进行有效校正，实验结果证明了该方法的可行性。