一种跟踪移动目标的路径规划方法

一种跟踪移动目标的路径规划方法
One Path Planning Method for Tracking the Moving Target
李建,钱海力(中国电子科技集团公司第二十八研究所,江苏南京210007)
Li Jian,Qian Hai-li(The28th research institute of china electronics technology group corporation,
Jiangsu Nanjing210007)
摘要：在静态目标路径规划问题中，可以直接以路径长度衡量规划效果，而对于移动目标跟踪问题，"缩短完成任务所需的时间"通常比"缩短完成任务所需的路径长度"更重要。

该文提出一种双向Dijkstra搜索算法，将移动目标跟踪问题转化为双向搜索的时间/空间相交问题进行求解。

理论分析证明，在目标运动情报已知下，新算法能够给出使跟踪任务完成时间最短的跟踪方路径和运动方案。

通过大样本测试集仿真实验，对新算法的性能进行了验证，并针对典型应用场景进行了算例说明。

关键词：路径规划；移动目标跟踪；任务时间最短；双向搜索
中图分类号：TP391文献标识码：A文章编号：1003-0107(2020)12-0007-06
Abstract:In the path planning for static target,the path length can be directly used as the evaluation index of the planning effect.But for the moving target tracking problems,shortening the task time is often more important than shortening the length of the task path.In this paper,a bidirectional Dijkstra search algorithm is proposed to transform the moving target tracking problem into the time/space intersection problem of bidirectional search. Theoretical analysis shows that the new algorithm can provide the tracking path and motion scheme which guarantee the shortest task time,when the target motion information is known.The performance of the new algorithm is verified by a large sample test set simulation experiment,and examples are given for typical application scenarios.
Key words:path planning;moving target tracking;shortest task time;bidirectional search
CLC number:TP391Document code:A Article ID：1003-0107(2020)12-0007-06
0引言
路径规划是任务规划领域的一个重要的基本问题[1-5]。

目前常见的路径规划方法大多以缩短完成任务所需的路径长度作为规划目标，而实际作战指挥过程中，"缩短完成任务所需的时间"往往比"缩短完成任务所需的路径长度"更重要。

一般地，如果目标始终静止，则任务时间最短问题与任务路径长度最短问题等价，其规划方法相同；但对于运动目标跟踪问题，除了路径轨迹之外，还必须考虑跟踪过程的运动能力和运动策略，任务时间最短问题的复杂度大大提高，图1所示为示例说明。

图1为使用无人机向运动中的地面车辆运送物资，无人机飞行路径最短方案中，无人机在X点送达物资，地面车辆继续进至X点时接收物资；任务时间最短方案中，无人机在Y点与地面车辆会合并交接物资。

显然，现有以路径长度为指标的规划方法难以描述和处理此类问题。

作者简介院李建(1989-)，男，工程师，硕士，研究方向为指挥决策技术。

电子质量2020年第12期(总第405期)
图1路径最短/时间最快运送物资任务示例
现有针对移动目标的路径规划方法主要是动态多次重规划[6-9]。

文献[10]在快速随机搜索树算法的基础上，采用滚动时间帧的思想，周期性地采集环境信息与目标运动状况，从而不断更新跟踪方案。

文献[11]采用CPD 算法构建当前地图环境的压缩数据库，通过A*算法和快速数据库查询来缩短目标运动时的重规划时间。

文献[12]采用增量式技术，当目标发生移动而必须重规划跟踪路径时，重复利用上次规划搜索树，从而加快运动目标规划动态规划速度。

此类方法虽然能够通过迭代过程实现对目标的渐进收敛跟踪，但是并不能充分利用已知的目标运动情报，因此即使在目标运动信息完备的场景下，也不能保证获取时间最短方案，且只能给出跟踪路径轨迹信息而不能给出跟踪过程具体的运动策略。

为此，本文构建了基于时间最短的动目标跟踪问题模型，提出了一种基于双向Dijkstra搜索的移动目标最短时间跟踪方法，将移动目标跟踪问题转化双向搜索的时间/空间相交问题。

新方法能够基于目标运动信息实现针对运动目标的全局跟踪规划，并保证给出任务完成时间最短的跟踪方案，相比于现有方法，能够更好地解决运动目标最短时间跟踪问题。

1运动目标最短时间跟踪问题
假设目标运动信息已知(例如图1中的运送物资任务)，在栅格化环境中，目标运动可表述为：
Target(t)=(x target(t),y target(t))(1)
其中，x target(t)和y target(t)分别为目标在t时刻所处的栅格坐标，Target(0)=(x target(0),y target(0))为目标运动起始点。

反之，t=Target-1(x,y)表示(当坐标点x,y位于目标预期运动轨迹上时)目标抵达位坐标点x,y的时刻。

假设我方跟踪单元高度不变，跟踪单元速度上限为V max则我方跟踪方案可描述为π={P0,…,P i,…,P N}，P i={v i, t i,x i,y i}为代表单个路径点的元组，P i.x,P i.y为第i个路径点的栅格坐标，P i.t为跟踪单元在该路径点的滞留时间，P i.v≤V max为跟踪单元在路径段P i→P i+1上的速度，N为方案点总数，x N，y N为预期的目标汇合点。

我方跟踪单元抵达预期汇合点的时间time(P N)为：
time(P N)=
N
i=1
∑(dis(P i-1,P i)/P i-1,.v+P i-1.t)(2)
其中，dis(P i-1,P i)为路径点P i-1与P i之间的距离，当地图栅格边长为L时，dis(P i-1,P i)计算方法为：
dis(P i-1,P i)=L×(P i.x-P i-1.x)2+(P i.y-P i-1.y)2
√(3)考虑到跟踪单元提前抵达任务交汇点然后等待目标的情况，则方案π能够完成跟踪任务的完成条件为：存在时刻t满足：
(1)t≥time(π.P N)
(2)x target(t)==π.x N且y target(t)==π.y N
即有效的跟踪方案不仅需要在空间轨迹上与目标相交，而且应当满足时间上的相遇条件。

其中π.P N，π. x N，π.y N表示方案π的路径终点及其横纵坐标。

对于任意有效方案π，满足上述时间/空间相交条件的最小时间t(跟踪任务完成时间)称为方案π的基准时间，记为T(π)：
T(π)=max{time(π.P N),Target-1(π.x N，π.y N)}(4)
其中Target-1(π.x N，π.y N)为目标抵达汇合点π.P N 的时间。

综上，移动目标最短时间跟踪规划问题即为最小基准时间T(π)及其对应方案π的求解问题。

2双向Dijkstra移动目标跟踪算法
2.1基本思路
移动目标最短时间跟踪模型存在时间/空间耦合约束，其规划过程需要同时考虑跟踪路径轨迹和运动策略，且通常时间最短跟踪方案并非唯一。

当保持跟踪路径轨迹不变时，根据跟踪单元的不同运动速度策略时，可以形成一系列不同的方案，其集合称为一个簇。

将不满足小节1跟踪条件的方案的基准时间记为正无穷+∞。

根据公式(4)，容易理解，对于同一簇内的两个方案π1和π2，由于与目标的相遇点相同，则有：time(π2.P N)≤time(π1.P N)T(π2)≤T(π1)(5)
其中P N为π1和π2所属方案簇公共路径的终点。

因此，在相同跟踪路径上，当采用不同运动策略时，如果跟踪单元抵达路径终点的时间减少，则对应方案的基准时间始终单调非增。

因此，虽然时间最短跟踪方案具有非唯一性，但同一簇方案中，抵达路径终点最快的方案(称
为保守方案)始终非劣于其它任意方案。

>=
因此，本文提出一种基于双向Dijkstra搜索的移动目标最短时间跟踪方法，其基本思路是将移动目标跟踪问题转化双向搜索的时间/空间相交问题。

其中，目标运动信息转化为目标端路径延伸方案，而跟踪端采用Dijkstra搜索规则，并在各个航路点均采用最速保守运动策略。

采用松弛化的时间相交规则来调节两个搜索活动之间的扩展顺序和扩展速度，在双向搜索的交接面进行跟踪方案有效性判定，并给出最优方案的筛选规则。

2.2数据结构
采用一个与地图行列数相同的矩阵代表地图环境，栅格边长为L，矩阵的每一个元素对应一个栅格路径点。

三维环境中，则忽略跟踪方和目标方的高度变化，并将双方路径轨迹投影至同一平面。

定义1一个栅格点p可以表示为一个元组{x，y，type f，time f，mode f，parent f，type b，time b，mode b，parent b，}。

其中，x、y分别是栅格点p所在列数和行数；type f∈{true, false}表示栅格点p对于跟踪方的是否可通行；time f表示前向搜索(跟踪方)抵达栅格点p的时间，mode f表示栅格点p在前向搜索中的状态，包括"未访问"、"等待扩展"和"已扩展"三种情况；parent f为指向栅格点p在前向搜索中上一节点(称为父代节点)的指针；type b∈{true, false}表示栅格点p对于目标方的是否可通行；time b表示后向搜索(目标方)抵达栅格点p的时间，mode b表示栅格点p在后向搜索中的状态；parent b为指向栅格点p在后向搜索中父代节点的指针。

需要注意的是，对于同一栅格，type f和type b未必一致。

2.3算法流程
基于双向Dijkstra搜索的移动目标最短时间跟踪方法的具体步骤如下：
Step1：地图初始化
加载地图，根据可通行性确定地图上所有栅格的type f和type b；初始化地图上所有栅格的mode f，mode b为"未访问"；初始化地图上所有栅格的parent f和parent b 为无效空指针NULL；初始化地图上所有栅格的time f和time b为-1(表示无效)。

构建前向搜索待扩展队列Openlist f，初始为空；
构建后向搜索待扩展队列Openlist b，初始为空；
对目标轨迹情报进行离散化，记目标运动序列为{(G0, T G0),(G1,T G1),(G2,T G2),…}，其中G i为目标轨迹经过的第i 个路径栅格点，T Gi为目标抵达G i的时间。

Step2：搜索启动
将跟踪单元起始位置栅格P0放入前向搜索待扩展队列Openlist f，且：
S.time f=0
S.mode f=待扩展
S.parent f=NULL
⎧
⎩
⏐⏐
⏐⏐
⏐⎨
⏐⏐
⏐⏐
⏐
(6)
将目标起始位置栅格G0放入后向搜索待扩展队列Openlist b，且：
G0.time b=0
G0.mode b=待扩展
G0.parent b=NULL
⎧
⎩
⏐⏐
⏐⏐
⏐⎨
⏐⏐
⏐⏐
⏐
(7)
令i=1；
Step3：时间窗口更新
令T max=T Gi；
Step4：搜索方向选择
记前向搜索待扩展队列Openlist f中time f最小的栅格点为CBN。

如果T max<CBN.time f，则跳转至Step6终止条件判断；否则，将CBN从Openlist f移出，令CBN.mode f=已扩展，继续Step5前向搜索。

Step5：前向搜索
采用保守跟踪策略时，跟踪单元从当前路径点CBN 抵达下一(相邻)路径点p child的耗时为：
ΔT(CBN,p child)=dis(CBN,p child)/V max
其中dis(CBN,p child)表示路径点CBN与路径点p child 之间的距离。

在八向图中，如果p child位于CBN同一行或者列，则dis(CBN,p child)=L；如果p child位于CBN的斜对角方向，则dis(CBN,p child)=1.414×L。

对于CBN的任一可通行相邻栅格点p child j(即p child j. type f=true)，如果满足以下两条件之一：
1)p child j.mode b=待扩展；
2)p child j.mode b≠待扩展&&CBN.time f+ΔT(CBN,p child j) <p child j.time f
则对栅格点p child j进行更新：
p child
j
.time f=CBN.time f+ΔT(CBN,p child
j
)
p child
j
.mode f=待扩展
p child
j
.parent f=CBN
⎧
⎩
⏐⏐
⏐⏐
⏐⏐
⏐⎨
⏐⏐
⏐⏐
⏐⏐
⏐
(8)
将p j放入前向搜索待扩展队列Openlist f，并对Openlist f中元素栅格按time f从小到大排序。

然后跳转至Step4选择搜索方向。

Step6：终止条件判断
1)如果目标轨迹点G i满足G i.mode f=已扩展，则说明前向搜素(跟踪单元)比后向搜索(目标)更早到达G i，搜索结束。

电子质量
2020年第12期(总第405期)
此时，在前向搜索中回溯G i 父代栅格，即可获得跟踪路径；后向搜索待扩展队列Openlist b 从队首至队尾，依次为对应的目标运动轨迹。

假设跟踪方案记为π={P 0,…,P i ,…,P N }，其中N 为跟踪路径点总数，则采用保守策略，对于除P N 外的任意路径点P i ：
P i .v=V max P i .t =0{(9)
对于跟踪终点P N ：
P N .v=0
{P N .t =T Gi -G i .time f
{
(10)
方案π的基准时间T (π)：T (π)=T max 亦即T (π)=T Gi
(11)
2)如果目标轨迹点G i .mode f ≠已扩展，则跟踪任务的时空交汇条件尚未满足，跳转至Step 7进行后向搜索。

Step 7：后向扩展
对于后向搜索待扩展队列Openlist b 当前队尾元素G i -1，令其mode f =已扩展；
将栅格G i 放入Openlist b 尾部：
G i .time b =T Gi
G i .mode b =待扩展G i .parent b =G i -1
⎧⎩
⏐
⏐⏐⏐⏐⎨⏐⏐⏐⏐⏐(12)
令i=i+1，然后跳转至Step 3。

3最优性证明
记算法获得结果方案为π={P 0,…,P i ,…,P N }。

假设π不是最优方案π'={P 0,…,P i ',…,P N ''}，则必然存在方案满足时空汇合条件，
且T (π')<T (π)。

情况1：方案π'与方案π属于同一方案簇(即路径轨迹相同，此时P N =P N '')。

由于方案π是该方案簇中的最保守运动策略方案，满足time (π.P N )≤time (π'.P N )此时必然有：
T (π)≤T (π')
(13)
这与假设矛盾，
情节1不成立。

情况2：方案π'与方案π不属于同一方案簇(即路径轨迹不同，此时P N ≠P N '')。

根据跟踪汇合空间条件，P N 和P N ''均位于目标轨迹，假设P N 为G m ，P N ''为G n 。

根据跟踪汇合空间条件：T (π)=T Gm T (π')=T Gn
{
(14)
根据假设有T (π')<T (π)，而T Gi 随下标单调递增，因此必然有n<m 。

由于算法直到第m 次迭代才结束，故第n 次迭代
必然未满足终止条件，亦即i=n 的Step 6中，P N ''尚未进行前向扩展(P N ''.mode f ≠已扩展)。

只要S 与G 0不重合，则前向搜索至少需要执行一步，因此S (亦即P 0)必然已经经历前向扩展。

当算法运行至i=n 的Step 6时，{P 0,…,P 1',…,P N ''}中必然存在P k '(0≤k<N')满足：
P k '.mode f =已扩展&&P k+1'.mode f ≠已扩展进一步，P k '已扩展而P k+1'未扩展之前，P k+1'栅格必然位于前向待扩展队列Openlist f 中，且由于k +1≤N '，必然有：
P k+1'.time f ≤P N ''.time f ≤T Gn =T max
(15)
根据定义，CBN 是当前Openlist f 中time f 最小栅格，故此时：
CBN.time f ≤P k+1'.time f ≤T max
(16)
但是根据算法规则，"CBN.time f ≤T max "与"算法处
于Step 6"是矛盾的，因此情况2也不成立。

综上，
方案π'的存在性假设不成立，不存在满足时空交汇条件且基准时间比方案π更短的方案，故方案π为运动目标最短时间跟踪问题的最优方案。

4仿真试验
为验证本文双向Dijkstra 跟踪方法的效果，在大样本地图集进行性能测试，并给出了典型应用场景算例。

实验硬件为CPU 型号AMD R7，主频2.9GHz。

4.1性能测试
大样本性能测试选取加拿大阿尔伯塔大学Moving AI 实验室公开的BG2测试集和MAZE 测试集[13-15]。

图2所示为大样本性能测试地图集示例。

BG2测试集取自同名电脑游戏(Baldur's Gate II)实景，
共包含75张地图和93160算例，地图大小均为512×512(单位为栅格)，图2(a)给出示例。

Mazes 测试集是人工设置的迷宫场景，常用于AI 寻路算法测试，共包含60张迷宫地图和
586370算例，迷宫大小为512×512，走廊宽度为1,2,4,8,16或32，图2(b)给出示例。

(a)BG2
(b)MAZE
图2大样本性能测试地图集示例
需要注意的是，Moving AI原始测试集只针对静态场景，因此本文实验中，为每个测试算例的目标单元随机选取八向之一作为运动方向。

目标水平或者竖直移动一个栅格单元所需时间记为一个时间单元，斜向移动一个栅格单元需1.414个时间单元。

实验中假设障碍物(图2中黑色)仅针对跟踪单元，即目标单元可在任意栅格运行。

为保障跟踪任务可行，跟踪单元速度应当大于目标单元速度，实验中双方速度之比取3.0。

性能测试实验结果如表1所示。

表1中列出了实验中，新算法能够解决的有效算例数，平均每解决一个算例所需的规划耗时(计量单位为ms)和方案基准时间(即任务耗时，计量单位为时间单元)。

容易理解，任务耗时实际上等于任务完成前的目标移动距离。

表1可以看出，实验中"路径长度/任务耗时"远小于跟踪方/目标方速度比(即3.0)，说明跟踪方并不需要始终追逐目标，而是提前抵达汇合点然后停留等待任务目标，实现了本文设计的最短跟踪策略，实验结果符合预期。

以跟踪方和目标运动距离之和衡量问题规模，对于路径之和不超过500.0的规划问题，仿真实验的规划时间控制在20ms以下，具有较好的实时性。

表1性能测试实验结果4.2典型应用场景说明
为进一步说明本文双向Dijkstra跟踪方法的效果，以无人机向地面运动车辆运送物资为例，选取典型算例进行说明。

此时任务完成条件为无人机抵达目标车辆正上方，无人机与目标车辆速度比取3.0。

此外选取了现有的动态重规划策略A*算法进行对比。

该算法中，跟踪单元始终沿上次规划(A*算法)路径运动，而每当目标位置(所在栅格)变化，则立刻以跟踪单元和目标单元当前位置为起始点/终点重新规划路径。

图3所示为跟踪目标车辆示例。

图3跟踪目标车辆示例
图3中，黑色线条为目标车辆轨迹。

无人机采用现有动态规划A*方法时轨迹为蓝线，从T=0时刻，无人机开始跟踪目标；T=4时刻，目标车辆进入山中隧道，无人机无法进入隧道，只能原地等待；T=5时刻，目标车辆驶出隧道，无人机航线折回，重新开始跟踪，跟踪过程不断根据目标实时位置调整路径，最终追上目标位置为X，时间为8。

而采用本文方法(红色轨迹)，无人机在T=3.5时刻抵达Y点后开始等待；T=5时刻，目标抵达Y点，无人机完成任务。

5结论
针对信息支持的运动目标跟踪问题，本文构建了基于时间最短的动目标跟踪问题模型，提出了一种基于双向Dijkstra搜索的移动目标最短时间跟踪方法，将移动目标跟踪问题转化双向搜索的时间/空间相交问题。

理论分析证明，该方法能够保证给出任务完成时间最短的跟踪方案。

大样本性能测试和典型应用场景算例说明，新方法能够通过对目标运动信息的充分利用实现对运动目标的全局跟踪方案规划，相比于现有方法，规划耗时大大减少，规划成功率提高，
跟踪任务完成效果更好。

电子质量2020年第12期(总第405期)
参考文献：
[1]刘文兵,王艺栋.多无人机协同搜索多目标的路径规划问题研究[J].电光与控制,2019,26(3):35-38,73.
[2]王光磊,魏巍,丁轶.运输机航路规划算法[J].指挥信息系统与技术,2016,7(2):43-46.
[3]刘蓉,杨帆,张衡.基于改进混沌蚁群算法的无人机航路规划[J].指挥信息系统与技术,2018,9(6):41-48. [4]Bulitko V V,Bjornsson Y,Lawrence R.Case-based Sub-goaling in Real-Time Heuristic Search for Video Game Pathfinding[J].Journal of Artificial Intelligence Research, 2010,(39):269-300.
[5]Stentz A.The Focussed D*Algorithm for Real-Time Rep-lanning[C].International joint conference on artificial inte-lligence,1995,(2):1652-1659.
[6]Hart P E,Nilsson N J,Raphael B.A Formal Basis for the Heuristic Determination of Minimum Cost Paths[J].IEEE Transactions on Systems Science and Cybernetics,1968, 4(2):100-107.
[7]黄宇峰.遗传算法在AUV动态规避中的应用研究[D].哈尔滨:哈尔滨工程大学,2008.
[8]庄肖波,齐亮.高速无人艇动目标避碰规划方法研究[J].舰船电子工程,2008,28(12):95-97,117.
[9]严浙平,黄宇峰,李锋.遗传算法在AUV局部路径规划
中的应用研究[J].应用科技,2009,36(2):46-51.
[10]樊晓平,李双艳,瞿志华.机器人对多运动障碍物环境中方向可变运动目标的跟踪[J].控制理论与应用,2006, 23(3):347-350,361.
[11]Botea A,Baier J A,Harabor D,et al.Moving Target Search with Compressed Path Databases[C].International Confe-rence on Automated Planning and Scheduling,2013:288-292.
[12]Sun X,Uras T,Koenig S,et al.Incremental ARA*:an in-cremental anytime search algorithm for moving-target search[C].International conference on automated planning and scheduling,2012:243-251.
[13]Sturtevant N R.The Grid-Based Path-Planning Compe-tition[J].AI Magazine,2014,35(3):66-69.
[14]Sturtevant N R.Benchmarks for Grid-Based Pathfinding [J].IEEE Transactions on Computational Intelligence and AI in Games,2012,4(2):144-148.
[15]Sturtevant N R.The Grid-Based Path Planning Compe-tition:2014Entries and Results[C].The Eighth Interna-tional Symposium on Combinatorial Search;Ein Gedi,Israel, 2015:241-251.
..。