摘要
一.介绍
二.搜索空间
2.1路线规划空间表示
2.2UAV的物理限制
三.算法设计
3.1轨迹点规划法
3.2评价函数设计
3.在线路由规划算法实现
四.移动目标模型
4.1Singer模型
4.2“Current”统计模型
4.交互式多模型(IMM)方法
五.跟踪模拟移动目标
5.静止目标航路规划模拟
5.2机动目标跟踪模拟
六.结论
摘要
无人机(UAV)实时目标跟踪在未知环境是未知环境中的航路规划问题。针对这一问题，本文提出了基于改进的稀疏性a *搜索(SAS)算法三维(3)D)航路规划方法。该方法改进了评价函数。去除SAS构建任务环境中的威胁信息作为评估函数的一部分，引导无人机远离威胁区域。该方法规划的轨迹考虑了实时环境信息的更新，可以实现实时轨迹规划。在此基础上，对辛格模型、当前统计模型和交互式多模型进行了比较(IMM)算法的滤波效果最终选择当前的统计模型作为移动目标跟踪模型。
关键词：航路规划，目标跟踪，SAS算法，A*算法
一.介绍
无人机路线规划是无人机任务规划的核心技术之一，是确保无人机安全高效执行任务的有效手段。然而，在复杂的空间环境中，随着信息和规划约束的增加，如何设计一个简单、快速的三维路线规划算法是无人机实时三维路线规划的关键。当您提前获得完整的环境信息时，您通常可以一次规划从起点到终点的最佳轨迹。
目前的航路规划算法主要包括Voronoi图，遗传算法，A*算法等。Voronoi图形方法是一种几何方法，属于确定性搜索方法。利用Voronoi图将复杂空间条件下的规划问题转化为简单加权图的搜索问题。但一般只适用于二维平面的航路规划，不考虑无人机的物理特性和约束，也不考虑威胁源的性能参数。遗传算法是一种概率搜索算法，不需要固定的模型鲁棒性强。但是，如果约束条件和障碍类型较多，则很难编码算法。此外，该算法的计算成本也很高，搜索时间长，容易陷入局部最优解。
A*由于算法简单，易于在工程[1]中实现，算法在世界上得到了广泛的应用。但在动态范围内，特别是在未知环境中，其速度并不理想。1994年，Anthony Stentz提出动态a *算法，称为D*算法[2]。但由于无人机飞行速度高，其运行时间不能满足无人机实时航迹规划的要求，也不能用于在线航迹规划。2005年，Sven Koening等人。
在D*提出了算法的基础D*Lite算法[3]易于实现，但不能满足实时在线路由规划的要求。j。
Szcaerba提出了改进A*算法，称为稀疏A*算法(Sparse A* Search)，以下简称SAS算法[4]。SAS算法根据无人机的物理约束规划飞行路径，离散无人机的下一个飞行方向，减少计算所需的计算量。最后，飞行路径点更符合实际情况。
3D路径规划所需的计算时间和信息存储量远远大于2D路径规划[9]。此外，还需要算法实时处理一些突发威胁。因此，有必要提高三维路径规划算法的速度。
本文设计了一个基础SAS算法的三维路径规划算法。无人机从起飞点开始，规划每次飞行距离的部分前进路线。对评价函数进行了改进，以满足实时要求。评估函数由威胁成本和启发成本组成。遇到突发威胁时，只需将威胁信息添加到威胁信息中，即可重新规划威胁信息，减少计算量和存储空间需求。因此，本文得到了改进SAS算法是一种实时路径规划算法，可应用于几乎所有类型的路径规划，包括但不限于海、陆、空等领域。最后给出了模拟结果。
本文其余部分总结如下：第二部分描述了搜索空间的规划和建设，第三节提供了改进SAS算法的三维路径规划和设计，第四部分在第五节提供三个目标跟踪模型和模拟，以确定哪些模型可以获得更好的目标轨迹，第五节显示3 d第六节总结讨论了模拟目标跟踪的过程。
二.搜索空间
路线规划首先要考虑地形信息。为确保无人机在飞行过程中不与地形碰撞，需要掌握无人机周围详细的地形环境。
2.1路线规划空间表示
数字高程模型(DEM)是常用的数字地形模型之一。DEM是两个自变量X和Y不同的连续函数(X, Y)坐标对应不同的高度z。通常用规则网格单元组成的矩阵表示。
每个网格单元对应一个高程值。在计算机中，它被实现为一个二维数组。在一般的科学研究中，数字地图是通过模拟现实世界的地形来随机生成的。
要解决路线规划问题，需要根据已知地形信息构建数字高程图，并增加威胁源。山地地形采用函数法模拟：
其中()h k是第k峰值的高度，0 ()x kx坐标是第k峰值，0 ()y ky坐标是第k峰值，()ix k是沿x轴的第k峰值斜率值，()iy k是第k个峰值沿y轴的斜率值。
随机表面形状采用菱形平法获得。选择两个模型中较高的值作为地形高度。
威胁模型包括禁飞区、异常天气、雷达和火炮模型。为了方便研究，本文建立了它们的数学模型。威胁被简化为具有特定半径的圆柱体。无人机离圆柱体越近，威胁就越大。由于本文采用数字标高图，为了区分地形威胁，威胁模型不适合使用与地形相同的方法，因此使用圆柱形来表示这些区域。
2.2UAV的物理限制
在规划航道时，应考虑无人机本身的物理限制，否则无人机将无法按照生成的轨道飞行。本文主要包括以下几个方面。
（1）最大偏转角：最大偏转角取决于特定无人机的物理性能，限制在小于或等于预先确定的最大角度范围内转动。
(2)最大爬升/俯冲角度:最大爬升/俯冲角度由车辆的机动性决定，限制了垂直平面上履带的最大升降角度。
(3)最小航迹长度:在开始改变飞行姿势之前，限制无人机直接飞行的最短距离。减少导航误差无人机在长距离飞行中一般不希望频繁绕行和转弯。
(4)最小飞行高度:如果飞行高度过低，会增加无人机与地面碰撞的概率。
因此，应根据无人机的机动性能来设置最小飞行高度。
三.算法设计
传统的A*算法通常将规划空间划分为正方形，并提前计算每个正方形的评价函数值。这种形式虽然有利于在计算机上进行计算，但没有考虑到无人机自身的约束。因此，本文采用改进的SAS算法来规划无人机的下一个航迹，并根据无人机的约束条件对无人机的前进方向进行编码。在计算其评价函数时，只需计算所涉及的轨迹点，节省了大量存储空间。但即便如此，该方法在三维空间规划时间较长，难以实现在线路线规划。因此，为保证无人机在遭遇突发威胁时能够快速响应，并在当前已知信息的前提下保持规划的最优性，本文采用实时改进的SAS算法对无人机进行实时在线规划航路。同时，为了更好地跟踪目标并消除传感器噪声的影响，对SAS算法启发式函数的高度代价进行了改进。这避免了无人机在飞行过程中由于目标高度噪声的存在而持续进行俯仰机动，从而避免了飞行轨迹剧烈晃动，使在线规划的飞行轨迹更符合实际情况。
3.1轨迹点规划法
对飞行方向进行离散化，以满足无人机在空间中转弯的要求。无人机在三维平面上的轨迹点展开图如图1所示。
采用无人机空间位置和方向图表示无人机航迹点。让无人机的跟踪点信息 ( ), , , , (), x y zψθ,x y z表示无人机的坐标位置在三维空间中,ψ表示无人机的偏航角θ表示无人机的螺旋角,设置跟踪点信息在时间t () 0 0 0 0 0 0,,,, x y zψθ,取1 tΔ=,然后从几何关系,跟踪点的信息可以获得在时刻0 t t +Δ:
我们需要从扩展的轨迹点中选择一系列轨迹点，形成最优轨迹点序列。如果无人机在每个航迹点位置有M个可供选择的方向，规划N步会产生NM不同的路线可供选择。有不同的路线可供选择，我们所要做的就是在这些路线中选择最好的路线。本文从算法的实现和实时性的要求出发，对SAS算法进行了改进。
3.2评价函数设计
A*启发式搜索算法需要定义一个评价函数:
其中()g k为消费成本，()h k为估计成本，gω和hω分别为消费成本和估计成本的权重。因为我们想要达到的目的是实时规划，所以我们需要做一些改变。
考虑到障碍物和威胁的存在，()g k表示当前轨迹点的威胁代价函数，主要用于避开路线上的障碍物和威胁，()h k是启发式函数，主要用于选择最优方向。在本文中，我们使用平面距离.但考虑到传感器检测到的目标位置可能与实际位置存在误差，即传感器本身具有测量噪声，导致无人机在飞行过程中不断进行俯仰机动，俯仰机动会增加导航误差，使无人机承受较大的过载。为了使无人机在跟踪目标时尽量减少俯仰方向的机动，本文改进了SAS算法的启发式函数，使无人机能够根据实际跟踪目标更好地跟踪，估计代价设为:
其中为目标当前时刻的空间位置，()， i i ix y z为无人机当前时刻的空间位置，h为无人机与待维护目标之间的高度，，目标kz为目标在坐标系中k时刻的高度值，，0goalz为目标在坐标系中初始时刻的高度值，σ为传感器噪声的标准差。Aω、bω分别表示平面距离开销和垂直高度开销的权重。通过改变它们的大小，可以确定无人机在遇到前方障碍物时是选择向上飞行还是水平飞行。
消耗的代价，即威胁代价函数是利用在选定方向飞行的直线上遇到的障碍物与无人机之间的距离来计算的。无人机威胁规避区域示意图如图2所示。
计算方法为:
式中，dρ为无人机所在位置与障碍物在当前飞行方向上的距离。Dω为距离威胁系数。minR为最小转弯半径。
3.3在线路由规划算法实现步骤
以无人机起始位置信息和目标位置信息为输入，一系列航迹点序列为输出，假设无人机在每个位置有M个可选择的标题，规划N步，则扩展航迹点总数为在线路由规划的具体步骤如下:
Step1:以无人机当前位置信息和方向信息为初始化信息，初始化开放列表和关闭列表为空，以无人机当前位置和方向()，，，，x y z ψθ为起始点信息，加入到开放列表中。
Step2:设置每个轨迹点为[]m n，其中(1)m m n≤≤表示扩展的第m步，表示第m步的第n个扩展节点。规划步骤的数量m + 1。步骤数对应的可行扩展节点由式(2)计算。
Step3:存储所有扩展节点的父节点，由无人机自身的物理约束限制无人机扩展航迹点。
Step4:如果扩展轨迹点不在开放列表或关闭列表中，计算扩展轨迹点的位置和方向并存储，将可行的扩展轨迹点添加到开放列表中，然后计算这些扩展轨迹点的威胁代价()g k和估计代价()h k，并计算评估函数()f k的值。否则请执行步骤5。
Step5:在打开列表中找到()f k最小的节点，并将其作为当前节点。如果规划的步骤数达到预设的规划步骤N，则停止循环，执行下一步操作。否则，请执行步骤2。
步骤6:从打开列表中移除当前节点，并将其添加到关闭列表中。
Step7:输出计划步数指定的()f k最小的节点，输出该节点的位置信息和方向信息作为无人机的下一个跟踪点。
步骤8:无人机将按照规划的航迹点飞行。如果模拟时间达到设定的时间，则退出，否则进入Step1;如果出现威胁，则更新威胁并转移到Step1以继续。
四.机动目标模型
本文采用三维恒速协同转弯模型(CT模型)和三维CV模型[5]来模拟目标的运动。在实际无人机目标跟踪中，由于目标可机动且目标模型未知，因此需要使用卡尔曼滤波。对于目标的数学模型，在实际应用中，它在跟踪目标时很少是准确的。在已知目标的数学模型的情况下，机动目标的状态估计是一个问题。本文采用singer模型、当前的“统计”模型和IMM算法对机动目标状态进行估计。
4.1Singer模型
目前应用最广泛的机动目标模型是时间相关模型及其修正序列。
这些模型的共同特点是将目标的机动看作是与时间相关的彩色噪声序列的结果，而不是统计独立的白噪声。R Singer在1969年[6]首次提出了时间相关模型，因此也被称为Singer模型。
4.2“Current”统计模型
在对机动目标进行估计时，只需要考虑机动加速度的“当前”概率密度，即目标机动的当前可能性。在周洪仁于1983年提出的当前“统计”模型[7]中，当目标以一定加速度机动时，下一时刻的加速度值是有限的，只能在“当前”加速度的附近。因此，在描述机动加速度的概率密度时，完全不需要考虑机动加速度取值的所有可能性。
4.3交互式多模型(IMM)方法
1984 - 1989年，Blom和Bar-Shalom在广义伪贝叶斯算法[8]的基础上，提出了一种具有马尔可夫转移概率的交互式多模型(IMM)算法。该算法在多模型算法的基础上考虑多模型之间的相互作用，推导出目标的状态估计。
交互式多模型算法由多个滤波器(每个滤波器对应一个模型)、一个模型概率估计器、一个交互动作(在滤波器输入处)和一个估计混合器(在滤波器输出处)组成，多个模型通过交互跟踪目标的机动运动。本文采用具有CV、CA和CSCT模型的IMM算法作为目标跟踪模型。
五.机动目标跟踪仿真
5.1静止目标航路规划仿真
验证了基于改进SAS算法的无人机三维实时航路规划的可行性和有效性。
进行了仿真实验。无人机航路规划空间22.5 km×22.5 km×1 km，采用North East Down坐标系。无人机起始点坐标为(301米，301米，-500米)，起始方向为偏航角0，俯仰角0，而目标点坐标为(7650 m, 20.8 km， -50 m)。计划N=2步，UAV的每个位置有一个可选择的方向M=35,其中有7个偏航方向，5个俯仰方向，最大偏航速率 ψ㼿 =0.2 rad/s,UAV速度V =45 m/s, gω =0.8, hω =0.2, minR V t ψ= ⋅ Δ 㼿 =225 m.为简化仿真，假设在仿真实验中无人机能够到达预期航迹点，以上述规划的航迹点位置作为无人机当前位置，假设无人机最终在目标H=500 m处悬停。
首先利用数字高程图生成地形图，然后利用本文提出的规划步骤的改进SAS算法在数字高程图中进行无人机飞行规划。图2为无人机在复杂地形区域执行任务时的轨迹规划。轨迹在图2中用一条蓝色实线表示。
从图2可以看出，当0.2aω =时，无人机的航路规划更倾向于爬越障碍物，而不是横向绕过障碍物。当0.5aω =时，当遇到障碍物时，无人机的航迹规划更倾向于上升而不是绕过障碍物。
仿真结果表明，该路线规划方法可行有效，规划时间短。
5.2机动目标跟踪仿真
在PC机(windows7, 2.80 GHz) MATLAB R2018b环境下进行仿真。假设无人机在有突然禁飞区的地形环境中跟踪一次性敏感目标。障碍是地形障碍和禁飞区。禁飞区中心坐标为(7500 m, 8750 m， -600 m)，出现在100 s，期望无人机能在水平面上对目标上方600m高度和距目标800 m高度的目标进行跟踪。
无人机参数:无人机航速45米/秒，最大偏航率0.2 rad/s，东北下坐标系起始坐标为(1500 m, 1500 m， -300 m)，俯仰角和偏航角均为0 rad。
机动目标参数:机动目标初始位置为(3250m, 5000m， -100m)，速度方向向东，大小为30m/s。假设目标进行三维协同转弯机动，为保证目标不与地形障碍物碰撞，目标的运动路线设计如表1所示:
表1。机动目标状态移动表
产生的目标移动路径如下图所示。
假设传感器在某一方向上的测量噪声为均值为0，标准差为15m的高斯白噪声，则得到的目标实际测量轨迹如下:
利用辛格模型、“电流”统计模型和IMM方法对运动目标进行卡尔曼滤波得到的滤波值和真实值的RMSE如下所示，其中假设传感器的采样时间为0.2s。
从图5可以看出，当前“统计”模型下的卡尔曼滤波滤波效果最好。本文将利用当前的统计模型来获得滤波值进行目标跟踪，得到的目标运动轨迹如下:
采用本文的实时航线规划方法，取0.5gω =和0.5hω =，仿真结果如下图所示:
从图中可以看出，在该参数的设置下，无人机与目标之间的距离保持在500m左右，保证了无人机可以持续跟踪目标。地面目标进入禁飞区后，无人机为保证不飞进禁飞区，沿禁飞区边缘飞行，与目标有较大的水平距离，但离开禁飞区后仍能跟踪目标。另外，从图9可以清楚地看出，采用本文改进的SAS算法后，在机动目标噪声较大、航迹平滑的情况下，无人机不会进行太大的俯仰机动，更符合实际情况。
六.结论
提出了一种改进的SAS地面目标跟踪算法。该算法对启发式函数进行改进，提高了无人机在跟踪目标过程中寻找航迹的速度，并在评估函数中对生成的航迹点进行平滑处理。最后，改进算法保证了在地面目标变化不大的情况下，无人机在一定高度保持不变，减少了无人机不必要的俯仰机动，提高了算法的实用性，对无人机实时跟踪目标具有一定的参考意义。