满足转弯半径限制的二维航迹规划

Published at ACTA ELECTRONICA SINICA
2000　Vol.28　No.3
满足直飞限制二维航迹规划方法研究

Research on 2D Route Planning Approach Constrainted for Straight-Flight

涂吉林 丁明跃 周成平

（华中理工大学图象识别与人工智能研究所，

国家教委信息处理与智能控制开放实验室，

武汉 430074）

【提要】 本文首先对飞行器航迹规划过程中带直飞限制的二维航迹规划问题进行了探讨。为了解决这一问题，提出将二维航迹简化为由直线段与圆弧段组成的规则航迹，并给出航迹相切点的定义。在此基础上，给出了一种满足直飞限制的二维航迹规划方法。实验证明，该方法所规划出的航迹不仅很好地满足转弯半径限制，而且能够保证飞行器直飞通过相切点，从而为飞行器进行匹配导航、航空摄影等应用提供了便利。

关键词: 转弯半径，相切点TPs，航迹规划

Abstract: Straight-flight constrained 2D route planning for unmanned air vehicle is conducted in this paper. To meet the request of straight-flight constraint, 2D flight route can be simplified as canonical route that consists of line and arc segment, and the definition of turning points (TPs) is given. Based on this definition, a new method for straight-flight constrained route planning is presented. Experiments shows that the obtained optimal route not only meets the request of minimal radius constraint, but also keeps air vehicle fly straight passing through turning points, facilitating matching navigation and flight photographing for air vehicle.

Keywords: Turning Radius, Turning Points (TPs), Route Planning

一、 引言

飞行器航迹规划的目的就是为飞行器寻找一条从起点到终点的优化飞行航迹。在规划时，需要综合考虑飞行器机动性能、飞行区域等对航迹所施加的约束，如飞行器的转弯半径、地形高程、航空交通状况等等。尤其当飞行器执行特定任务时，还要考虑飞行任务对航迹的约束，例如当飞行器需要执行航拍任务时，为了使所拍摄的航片失真度最小，飞行器在经过匹配区、航测区时必须保持平稳直飞状态，我们称这种对航迹的限制为直飞限制。

在航迹规划中，目前较常用的一种规划算法是A*算法。这种算法以搜索代价函数为优化目标，以OPEN表和CLOSE表为数据结构，在满足可纳性前提下对状态空间进行全局最优搜索 [1]。

采用A*算法进行规划首先需要选择规划节点。许多规划方法选择航迹上的特殊点作为规划节点[2]，所得到的航迹往往由通过这些点的折线组成，如图1所示，其中Pi表示飞行器所经过的特定区域。

图1不满足直飞限制的折线航迹

显然，如果选择有直飞限制的特殊点为规划节点，所得到的航迹可能不能满足直飞限制。因此，如何规划满足直飞限制的二维航迹就成为飞行器航迹规划应用中一个亟须解决的问题。

本文以Dubins理论[3]为基础，提出了相切点(Tangential Points缩写为TPs)的概念，进而对基于A*算法的二维航迹规划方法进行改进，使其能够保证飞行器直飞通过所有相切点，从而满足航拍、匹配导航等实际应用的需要。

二、带直飞限制的二维航迹

2.1规则航迹与相切点

定义1： 规则航迹为以直线段和圆弧段为基元组成的连续一阶可导的二维曲线航迹[3] [4]。

假定飞行器以匀速V作平面运动，对直线段航迹，从起始状态（x, y,φ）经过时间t后的状态为

(1)

记直线段航迹为s。

对以匀速V、半径R逆时针转弯的圆弧航迹，从起始状态（x, y,φ）经过时间t后的状态为

(2)

记逆时针转弯航迹段为l。

同理，沿顺时针转弯圆弧航迹段经过时间t后的状态为

(3)

记右转弯航迹段为r。

根据(1)~(3)，我们可以唯一表示规则航迹在任意时刻的状态。例如，一段从初始状态（0，0，0）先后作时间t的直线运动、时间u的顺时针转弯运动（转弯半径为R）、时间w的直线运动后的状态为：

(4)

定义2：相切点是规则航迹上的航迹基元之间的切点，记为TPs（Tangential Points）。

根据定义，可以认为相切点是无限短的连接航迹基元的直线段。因此如果假定航迹为规则航迹，把有直飞限制要求的特殊点当作相切点，航迹的直飞限制问题就迎刃而解了。我们称这种特殊点为直飞相切点，记为STPs（Straight-flight Tangential Points）。

相切点TPs具有以下性质：

性质1：若E={V,W}，有TPsV。其中E为N×N的二维规划网格区域，V为可飞区集合，W为禁飞区集合。

性质2：若对航迹w(t)存在t0，使w(t0)TPs，则有φ(t0-)=φ(t0+)；

直飞相切点STPs具有以下性质：

性质3：任意STPs点P0、P1之间的规则航迹w(t)至多包括一个圆弧段航迹，其转弯弧度不大于π，其中t0≤t≤t1，w(t0)=P0, w(t1)=P1。

如图2所示，在STPs点之间的规则航迹由t-u-w组成，其中u为圆弧航迹段，t、w为直线段航迹。

图2 两个STPs点之间的规则航迹

推论1：STPs点之间的航迹长度是圆弧段航迹半径的递减函数。

推论2：任意两个STPs点之间的最短规则航迹由至多一段直线段和至多一段圆弧段组成。记直线段与圆弧段之间的切点为辅助相切点FTPs，每两个STPs点之间的最短航迹上至多存在一个FTPs。

推论1、2的证明见附录。

三、 满足直飞限制的

二维航迹规划方法

从功能上，A*算法可以分为两大模块：核心算法与应用接口。核心算法与具体应用对象无关 [1]，本文不复赘述；应用接口主要包括当前节点的扩展和扩展节点的评价两部分。以下主要就这两部分对满足直飞限制的二维航迹规划方法展开讨论。

3.1 规划节点

在航迹规划过程中，规划区域通常被均匀量化为N×N大小的二维网格图[5]，其中每个网格被赋以不同属性值，代表航迹经过该网格点的代价，这种图又称为飞行代价图。

我们把分布在飞行代价图上需直飞通过的特殊点标记为直接相切点STPs并以STPs作为我们的规划节点.规划节点表示包括STPs点坐标与该点的航迹方向。

3.2 当前节点的扩展

当A*算法规划到节点N时，需要根据STPs的性质以及航迹的最小转弯半径限制，从规划图上寻找合适的后继STPs作为扩展节点。

合理的扩展方法应保留可能的优化规则航迹，同时排除所有的非优航迹。

图3 扩展节点区

如图3所示， Ni为当前规划节点， rmin为最小转弯半径，D为扩展区域圆的最大半径。将阴影区内的STPs的航迹进入方向以Δθ为单位进行量化，每个状态都被扩展为扩展节点。

3.3 扩展节点的评价

对每个扩展节点，我们需要计算它们的航迹代价，这包括以下步骤：

1）最佳规则航迹的计算

假设：当前规划节点

扩展节点

以航迹最短为优化目标，我们给出以下计算当前规划节点n和扩展节点之间的最佳规则航迹的方法。

a)将当前规划节点和扩展节点旋转平移变换到计算空间，得到

;

b)当=0，最短航迹为连接TP1与TP2的直线。

当<0，最短航迹中存在顺时针转弯圆弧段，转弯半径为Rs=min(,)

如果Rs=，FTPs为。最佳航迹由连接与FTPs，转弯半径为Rs的圆弧段和连接FTPs与的直线段组成。

如果时，辅助TPs为，最佳航迹由连接与FTPs的直线段和连接FTPs与，转弯半径为Rs的圆弧段组成。

同理可以计算当>0时的最佳航迹。如图4所示。

图4 计算规则航迹

c)将得到的最佳航迹反变换到规划空间。

d)去除Rs小于飞行器最小转弯半径的航迹。

我们将STPs点看作一段以TPs点为中心的线段，如图5所示。以TPs进入点和离开点作为航迹的左右相切点，可以保证航迹直飞过TPs点。

图5 直飞通过的STPs点

2）当前航迹代价计算

如果以航迹最短为最优化目标，可以得到航迹代价表达式为

(5)

在其中，C为航迹代价，L为航迹长度。

3） 预测代价计算

在航迹规划中，预测代价h(n)一般取从当前节点n到达目标点的欧氏直线距离的比例函数

(6)

其中为当前节点n的坐标，为目标点的坐标，为规划区域的平均代价。

当前航迹代价与预测代价之和即为每个扩展节点的代价。

四、实验结果

规划实验是在25km×25km的区域内进行。该区域被量化为256×256大小的网格图。飞行器的最小转弯半径为3km，飞行起点网格坐标为(220,219)，终点为(76, 46)，飞行任务要求航迹最短，同时需要进行匹配导航。

本实验在一台主频为300MHz的PC机上进行，首先我们采用无直飞限制的航迹规划算法规划出一条不满足直飞限制的二维航迹，如图6-(a)所示，该航迹长度为23.7km，其规划时间为54秒。

用本文方法得到的航迹如图6-(b)所示，航迹长度为24.6km，规划时间为65秒。图7-(b)中深色背景区域为自由飞行区，浅色区域为禁飞区，均匀分布的浅色小点为STPs点，白色小点为已被扩展的节点，航迹上的黑色圆点为航迹的直飞相切点STPs，而浅色圆点为辅助相切点FTPs；以箭头指示的白点为发射点，而圆圈的的中心点为目标点。

(a) 无直飞限制 (b)有直飞限制

图7有/无直飞限制的二维航迹结果

图7中的白色扇形区域为A*航迹规划时节点扩展时的扩展区域。

图8 放大的航迹

图8为放大的一段航迹，可以看到，我们的算法在最优性和规划速度上略逊于无直飞限制的规划方法，但保证了飞行器直飞通过TPs点，这一点对飞行器执行匹配导航以及航拍等任务是非常重要。

五、结论

本文通过将飞行器航迹近似为由直线与圆弧组成的规则航迹，以规则航迹基元之间的相切点为规划节点，提出了一种带直飞限制的二维航迹规划方法，满足了为执行航拍等任务的飞行器规划航迹的需求。实验证明，我们的方法取得了成功。

参 考 文 献

[1] 傅京孙，蔡自兴，徐光佑，<<人工智能及其应用>>，p44-55，清华大学出版社，1987。

[2] Wang Bin, “Human-Machine Interactive Route Planning Method for Large Searching Space”, Journal of Data Acquisition and Processing, Vol 12, No4, Dec. 1997

[3] L.E.Dubins, “On Curves of minimal length with a constraint on average curvature and with prescribed initial and terminal positions and tangents”, Amer.J.Math., 79(1957),497-516.

[4] J.A. Reeds, L.A.Shepp, “Optimal Paths For A Car That Goes Both Forwards And Backwards”, Pacific Journal of Mathematics, Vol 145, No 2,1990.

[5] Mitchell, J.S.B. and Papadimitriou, C.H., “Planning shortest paths”, proceedings SIAM Conference on Geometric Modeling and Robotics, Albany, NY(1985).

【附录】航迹切点之推论1、2的证明

证明：不失一般性，我们假设TP1=w(t0)=(0,0), φ(t0)=0; TP2=w(t1)= (x1,y1), φ(t1)=φ1，假设从TP1到TP2之间存在一次顺时针转弯，由(4)式得：

(I)

分别求出

(II)

航迹段TP1-TP2的长度为

L=V(t+u+w)=L(V,R,x1 ,y1 ,φ1) (III)

由u≥0可得≤0，我们取，当(x1 ,y1 ,φ1)已知，V为常数，有，可知

(IV)

因为航迹在每两个直接相切点上的转弯弧度满足，所以航迹长度随转弯段航迹半径变大而变短。推论1得证。

由(III)可知，t≥0可得；w≥0可得。因此从TP1到TP2的最短规则航迹的转弯段半径Rs=min(,)

航迹的最短长度为V(t+u)或V(u+w)。

进一步，当Rs=，有t=0，u=u(Rs)，w=w(Rs)，FTPs为。

当时，有w=0，u=u(Rs)，t=t(Rs), 可得FTPs为。

推论2证毕。

本文来源：https://www.2haoxitong.net/k/doc/a0740153640e52ea551810a6f524ccbff121cad3.html