CN106643733A - 面向多导航站接力导航的运动体路径规划方法 - Google Patents

Abstract

本发明公开了一种面向多导航站接力导航的运动体路径规划方法，该方法是在考虑基站有效导航范围约束、导航交接约束和运动体曲率约束的前提下，为运动体规划出一条能够各种满足各种约束条件且快速从起点抵达终点的水平二维空间路径。本发明提出了路点角度编码方式，通过角度代替坐标来表达路点的位置，既压缩了搜索空间，又便于路径的表达和约束的处理；本发明采用部分编码方案，仅对路点的位置进行编码优化，朝向在其范围内随机生成，显著减小了计算代价，有利于路径规划效率的提高。本发明提供的面向多基站接力导航的运动体路径规划方法打破了单个基站导航范围小的局限，有效延伸了运动体的可控移动范围，可以解决海、陆、空不同情形多约束条件下运动体从起点到终点的路径规划问题。

Description

面向多导航站接力导航的运动体路径规划方法

技术领域

本发明属于运动体路径规划研究领域，具体涉及一种面向多导航站接力导航的运动体路径规划方法，用于实现海、陆、空不同情形多约束条件下运动体从起点到终点的路径规划。

背景技术

运动体的多导航站接力导航是通过散布于不同空间位置且导航作用范围能够覆盖较大空间的多个导航站依次为运动体接续导航的联合导航方法。这种导航方式不仅可靠性强，而且可以有效延伸运动体的可控飞行范围。面向多导航站接力导航的运动体路径规划问题是实现这种具有前沿性的先进导航方式所需解决的一个关键问题，需要考虑三个基本约束条件，即导航范围约束、导航交接约束、运动体机动能力约束。其中，运动体机动能力约束用运动体曲率约束表示，导航范围约束和导航交接约束使得本发明中的路径规划问题有别于一般的路径规划问题，也是本发明中路径规划问题的主要难点。

运动体有多种导航方式，包括自身导航和外部导航，外部导航包括卫星导航、导航站导航等。其中，运动体自身导航方式精度有限且易受干扰，卫星导航方式易遭破坏且造价成本高，而导航站导航方式可靠性强，且可通过多站联合接力导航的方式为运动体执行远程任务提供路径导引。

发明内容

为了有效延伸运动体的可控移动范围，提高导航的可靠性，本发明提供了一种面向多导航站接力导航的运动体路径规划方法。该方法是在考虑导航站导航范围约束、导航交接约束和运动体曲率约束的前提下，为运动体规划出一条能够各种满足各种约束条件且快速从起点抵达终点的水平二维空间路径。

实现本发明的技术方案如下：

一种面向多导航站接力导航的运动体路径规划方法，包括如下步骤：

步骤1、输入路径规划信息，具体包括：运动体起点和终点位置信息、导航站导航范围约束信息、导航站导航交接约束信息和运动体曲率约束信息；所述导航站导航范围约束信息是指导航站中心的坐标及有效作用范围，即运动体从起点到终点的整条路径须全部位于导航圆内；所述导航圆表示导航站有效作用范围；所述导航站导航交接约束信息是指确保导航交接成功而设定的运动体在交接区域内的最短路径长度D；所述导航交接是指多个导航站之间移交运动体的导航权；所述交接区域是指两个导航圆之间的重叠区域；所述运动体曲率约束信息是指运动体最小转弯半径r_tc；

步骤2、随机生成初始路径种群；一条路径由起点位置、终点位置和中间路点的位置及朝向共同确定；所述中间路点是指除起点和终点之外的路点；

其中，中间路点与中间路点之间采用Dubins的“CLC”路径连接；

将中间路点设置在交接区域边界弧上，随机生成多条路径组成初始路径种群，其中，中间路点的位置在所属边界弧上随机生成，中间路点的朝向范围由其位置所决定，具体朝向在其朝向范围内随机生成；

步骤3、评价路径:评价指标包括路径总长度和约束违反程度；所述路径总长度是指按照导航顺序依次连接起点、中间路点、终点所形成的全部路段长度之和；所述路段是指两个相邻路点之间的路径；所述约束违反程度是指导航交接约束违反程度，当运动体在交接区域内的路径长度大于或等于约束值时，约束违反程度为0，否则，约束违反程度为两者的绝对差；

步骤4、记录初始种群的最优路径；若初始种群中存在约束违反程度为0的路径，则在约束违反程度为0的路径中选取路径总长度最短的那条路径作为初始种群的最优路径，若初始种群所有路径的约束违反程度均大于0，则选取约束违反程度最小的那条路径作为初始种群的最优路径；

步骤5、采用差分进化算法优化路径；首先通过变异、交叉操作产生新路径，其中，变异和交叉操作仅作用于路点的位置，路点的朝向范围随其位置的改变而改变，朝向在其范围内随机生成；

然后在新路径和旧路径中进行选择，留下较优的路径；选择操作的原则为：当两条路径的约束违反程度相等时，留下路径总长度较短的那条路径，当两条路径的约束违反程度不相等时，留下约束违反程度较小的那条路径；将选择操作留下的路径与之前记录的种群最优路径进行比较，比较方法与选择操作的原则相同，将两者中较优的一条作为当前种群的最优路径；基于当前路径种群，继续进行变异、交叉和选择操作并得到新的路径种群和最优路径，直到种群进化代数达到给定的最大进化代数时，终止循环过程，输出当前种群的最优路径，完成路径规划。

较佳的，所述步骤2中中间路点位置的具体表示方式为：

定义入点为位于入弧上的中间路点，所述入弧为运动体进入交接区域的边界弧；定义出点为位于出弧上的中间路点，所述出弧为运动体离开交接区域的边界弧；入点位置的表示方式如下：将交接区域的后导航圆的圆心作为极点，水平方向向右为极轴，逆时针为正方向，建立局部极坐标系，入点的位置用极角表示；所述交接区域的后导航圆是指运动体即将进入的导航圆；

出点位置的表示方法如下：将交接区域的前导航圆的圆心作为极点，水平方向向右为极轴，逆时针为正方向，建立局部极坐标系，出点的位置用极角θ表示；所述交接区域的前导航圆是指运动体即将离开的导航圆；

由此，一条路径所有中间路点的位置由入点极角和出点极角联合表示为θ＝[θ₁,θ₂,...,θ_n]，θ_i表示第i个中间路点的位置，i为奇数时，表示为入点的极角，i为偶数时，表示为出点的极角；限定θ_i的取值范围为[θ_i,min,θ_i,max]，其中θ_i,min和θ_i,max为交接区域顶点所对应的极角；所述交接区域顶点为入弧与出弧的交点；采取相对编码的方式，先将中间路点位置的解范围映射到[0，1]区间，即将

θ＝[θ₁，θ₂，...，θ_n]，θ_i∈[θ_i，min，θ_i，max]转化为x＝[x₁，x₂，...，x_n]，x_i∈[0，1]。

较佳的，所述步骤2中中间路点朝向范围的具体计算方法为：

过入点作交接区域的后导航圆的切线，切线将以入点为中心的360°角度范围分割成两半，取靠近终点的180°角度范围作为入点的朝向范围；过出点作交接区域的前导航圆的切线，切线将以出点为中心的360°角度范围分割成两半，取靠近终点的180°角度范围作为出点的朝向范围。

本发明的有益效果是：

第一、本发明提供了一种面向多导航站接力导航的运动体路径规划方法，采用多导航站接力导航这种具有前沿性的先进导航方式，打破了单个导航站导航范围小的局限，有效延伸了运动体的可控移动范围；

第二、本发明采用角度编码的方式，通过角度(一个变量)代替坐标(两个变量)来表示路点的位置，既压缩了解空间，又便于路径表示和约束处理；

第三、本发明采用部分编码方案，仅对路点的位置进行编码优化，朝向在其范围内随机生成；这种方案具有两个主要优点：一是便于差分进化算法的操作，降低了操作的维数，二是如果朝向也进行优化，因为路点的位置决定路点的朝向范围，当路点的位置确定后，还需要大量的迭代次数去优化路点的朝向，从而导致整体路径规划的迭代次数为位置迭代次数与朝向迭代次数的乘积，计算量过大，时间成本过高，而采用这种部分编码方案显著减小了计算代价，有利于提高路径规划的效率；

第四、本发明采用外部引导与运动体自身导航相结合的方式，精度高，可靠性强，即使在导航站范围缩小的情况下，仍有可能为运动体规划出一条可行的路径；

第五、本发明中的路径规划方法适用范围广，导航站可以是天基、地基、机载、舰载等，运动体可以是无人机、无人车、无人水下航行器等，可用于实现海、陆、空不同情形多约束条件下运动体从起点到终点的路径规划。

附图说明

图1为面向多导航站接力导航的运动体路径规划流程图；

图2为导航交接约束示意图；

图3为运动体曲率约束示意图；

图4为入点位置表示方法示意图；

图5为出点位置表示方法示意图；

图6为路点朝向范围示意图；

图7为仿真实验一结果图；

图8为仿真实验二结果图。

具体实施方式

下面结合附图和实施例对本发明作详细说明。

本发明提供了一种面向多导航站接力导航的运动体路径规划方法。该方法是在考虑导航站导航范围约束、导航交接约束和运动体曲率约束的前提下，为运动体规划出一条能够各种满足各种约束条件且快速从起点抵达终点的水平二维空间路径。本发明中路径规划为连续域约束优化问题，一些具有较强全局搜索能力的智能优化算法适用于解决该问题，本发明以差分进化算法作为面向多导航站接力导航的运动体路径规划的优化求解算法；

如图1所示，所述面向多导航站接力导航的运动体路径规划方法具体步骤如下：

步骤1、输入路径规划信息，具体包括：运动体起点和终点位置信息、导航站导航范围约束信息、导航站导航交接约束信息和运动体曲率约束信息；所述运动体起点和终点位置信息是指起点和终点的二维坐标；所述导航站导航范围约束信息是指导航站中心的坐标及有效作用范围，运动体从起点到终点的整条路径须全部位于导航圆内；所述导航圆表示导航站有效作用范围，半径为r；所述导航站导航交接约束信息是指确保导航交接成功而设定的运动体在交接区域内的最短路径长度D；所述导航交接是指多个导航站之间移交运动体的导航权；所述交接区域是指两个导航圆之间的重叠区域；所述运动体曲率约束信息是指运动体最小转弯半径r_tc，路点之间采用限定曲率的Dubins路径连接；

如图2所示，运动体的起点位于第一个导航站的导航圆内，终点位于第三个导航站的导航圆内，因此，运动体需由三个导航站进行接力导航；运动体的路径由从起点到终点的多个路点并依此连接各路点形成的路段1～路段5构成，其中路段2和路段4为运动体在交接区域内的路径；l_2j(j＝1,2)表示第j个交接区域内运动体路径长度，若l_2j≥D(j＝1,2)且交接区域内运动体的路径不超出交接区域范围，则该路径满足导航交接约束；

如图3所示，A和B表示路点，箭头表示运动体朝向，所述运动体曲率约束是指运动体采用限定曲率的Dubins路径实现其水平二维空间内朝向和位置的变化，即路段由直线段和半径为运动体最小转弯半径r_tc的圆上的弧组成；

步骤2、随机生成初始路径种群；

定义入点为位于入弧上的中间路点，所述入弧为运动体进入交接区域的边界弧；定义出点为位于出弧上的中间路点，所述出弧为运动体离开交接区域的边界弧；入点位置表示方法如图4所示，将交接区域的后导航圆的圆心作为极点，水平方向向右为极轴，逆时针为正方向，建立局部极坐标系，入点的位置用极角θ和极径ρ表示，由于交接区域的后导航圆的半径已知，且入点位于交接区域后导航圆的圆周上，因此，入点的位置可用θ一个变量来表示；出点位置表示方法如图5所示，将交接区域的前导航圆的圆心作为极点，水平方向向右为极轴，逆时针为正方向，建立局部极坐标系，出点的位置用极角θ和极径ρ表示，由于交接区域的前导航圆的半径已知，且出点位于交接区域的前导航圆的圆周上，因此，出点的位置用θ一个变量来表示；由此，一条路径所有中间路点的位置表示为θ＝[θ₁,θ₂,...,θ_n]，限定θ_i的取值范围为[θ_i,min,θ_i,max]，其中θ_i,min和θ_i,max为交接区域顶点所对应的极角；所述交接区域顶点为入弧与出弧的交点；本发明采取相对编码的方式，将中间路点位置的解范围映射到[0,1]区间，即将

θ＝[θ₁,θ₂,...,θ_n](θ_i∈[θ_i,min,θ_i,max])转化为x＝[x₁,x₂,...,x_n](x_i∈[0,1])；

一条路径由起点位置、终点位置和中间路点的位置及朝向共同确定；

中间路点与中间路点之间采用Dubins路径连接；所述Dubins路径是指对于在同一平面上具有朝向的任意两点，在限定曲率的情况下，采用几何方法计算出的由一点到达另一点且具有朝向的最短路径；本发明主要采用Dubins路径的“CLC”路径，第一个C表示Dubins路径起点辅助圆上的圆弧，第二个C表示Dubins路径终点辅助圆上的圆弧，L表示两条圆弧的切线；所述Dubins路径起点辅助圆是指以Dubins路径起点为切点、以r_tc为半径作的两个辅助圆，称之为Dubins路径起点左转弯圆和右转弯圆；所述Dubins路径终点辅助圆是指以Dubins路径终点为切点、以r_tc为半径作的两个辅助圆，称之为Dubins路径终点左转弯圆和右转弯圆；所述Dubins路径的“CLC”路径共有4种情形，分别为“Dubins路径起点左转弯圆圆弧-切线段-Dubins路径终点左转弯圆圆弧”、“Dubins路径起点左转弯圆圆弧-切线段-Dubins路径终点右转弯圆圆弧”、“Dubins路径起点右转弯圆圆弧-切线段-Dubins路径终点左转弯圆圆弧”和“Dubins路径起点右转弯圆圆弧-切线段-Dubins路径终点右转弯圆圆弧”，Dubins路径算法在这4种情形中选择长度最短的那条路径作为最终的Dubins路径；

起点与中间路点之间采用首端朝向松弛的Dubins路径连接；所述首端朝向松弛是指路径首端朝向可以是任意方向；所述首端朝向松弛的Dubins路径共有2种情形，分别为“切线-Dubins路径终点左转弯圆圆弧”和“切线-Dubins路径终点右转弯圆圆弧”；首端朝向松弛的Dubins路径算法在这2种情形中选择长度最短的那条路径作为最终的Dubins路径；

中间路点与终点之间采用终端朝向松弛的Dubins路径连接；所述终端朝向松弛是指路径终端朝向可以是任意方向；所述终端朝向松弛的Dubins路径共有2种情形，分别为“Dubins路径起点左转弯圆圆弧-切线”和“Dubins路径起点右转弯圆圆弧-切线”；终端朝向松弛的Dubins路径算法在这2种情形中选择长度最短的那条路径作为最终的Dubins路径；

将中间路点设置在交接区域边界弧上，随机生成多条路径组成初始路径种群，其中，中间路点的位置在所属边界弧上随机生成，中间路点的朝向范围由其位置所决定，具体朝向在其朝向范围内随机生成；

中间路点的朝向范围如图6所示，过入点作交接区域的后导航圆的切线，切线将以入点为中心的360°角度范围分割成两半，取靠近终点的180°角度范围作为入点的朝向范围；过出点作交接区域的前导航圆的切线，切线将以出点为中心的360°角度范围分割成两半，取靠近终点的180°角度范围作为出点的朝向范围；

步骤3、评价路径；评价指标包括路径总长度和约束违反程度；所述路径总长度是指按照导航顺序依次连接起点、中间路点、终点所形成的全部路段长度之和；所述路段是指两个相邻路点之间的路径；所述约束违反程度是指导航交接约束违反程度，当运动体在交接区域内的路径长度大于或等于约束值时，约束违反程度为0，否则，约束违反程度为两者的绝对差；

步骤4、记录初始种群的最优路径；若初始种群中存在约束违反程度为0的路径，则在约束违反程度为0的路径中选取路径总长度最短的那条路径作为初始种群的最优路径，若初始种群所有路径的约束违反程度均大于0，则选取约束违反程度最小的那条路径作为初始种群的最优路径；

步骤5、采用差分进化算法优化路径；首先随机生成初始路径种群并评价，记录当前种群的最优路径；然后进入到算法的主循环，反复执行进化操作，直至达到算法终止条件，最后输出当前种群的最优路径作为路径规划问题的较优解；一次完整的循环包括：变异、交叉、选择和更新当前种群的最优路径；首先通过变异、交叉操作产生新路径，其中，变异和交叉操作仅作用于路点的位置，路点的朝向范围随其位置的改变而改变，朝向在其范围内随机生成；然后在新路径和旧路径中进行选择，留下较优的路径；选择操作的原则为：当两条路径的约束违反程度相等时，留下路径总长度较短的那条路径，当两条路径的约束违反程度不相等时，留下约束违反程度较小的那条路径；将选择操作留下的路径与之前记录的种群最优路径进行比较，比较方法与选择操作的原则相同，将两者中较优的一条作为当前种群的最优路径；当种群进化代数达到给定的最大进化代数时，终止循环过程，输出当前种群的最优路径，完成路径规划。

下面结合仿真实验结果对多导航站接力导航下运动体的路径规划方法进行说明；

如图7所示，导航站中心坐标为(0,0)、(1,0)、(2,1)，导航圆的半径为r＝1，运动体起点坐标为(-1,0)，终点坐标为(3,1)，运动体最小转弯半径为r_tc＝0.2，导航交接约束为D＝[0.2,0.1]，差分进化算法种群规模为NP＝50，最大进化代数为Gen＝600；由于导航交接约束较小，理论最优路径为从起点到终点的一条直线段，路径总长度为4.1231；由仿真实验结果可知，算法得到的路径几乎为一条直线段，路径总长度为4.1251，相对误差为0.0485％，误差较小，十分逼近理论最优路径；

如图8所示，对于导航交接约束值较大的情况，导航站中心坐标设为(0,0)、(1.5,0)、(3,0)，导航圆的半径为r＝1，运动体起点坐标为(-1,0)，终点坐标为(3,0)，运动体最小转弯半径为r_tc＝0.3，导航交接约束为D＝[0.8689,0.8689]，差分进化算法种群规模为NP＝50，最大进化代数为Gen＝1000，得到的路径规划结果满足所有约束条件，且运动体在交接区域内的路径，即路段2(0.8959)与路段4(0.8803)的长度十分接近导航交接约束值，相对误差小于3.1％，总路径长度较小，得到了较优的路径规划结果；

以上公开的实施例在以本发明技术方案为前提下进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于所述的实施例。通过以上所述可知，本发明中的许多内容可作修改和替换，本实施例固定了某些取值只是为了更好地说明本发明的原理和应用，从而更易理解和运用。凡在本发明的技术方案的基础上所做的局部改动、等同替换、改进等均应包含在本发明的保护范围之内。

Claims (3)

1.一种面向多导航站接力导航的运动体路径规划方法，其特征在于，包括如下步骤：

步骤1、输入路径规划信息，具体包括：运动体起点和终点位置信息、导航站导航范围约束信息、导航站导航交接约束信息和运动体曲率约束信息；所述导航站导航范围约束信息是指导航站中心的坐标及有效作用范围，即运动体从起点到终点的整条路径须全部位于导航圆内；所述导航圆表示导航站有效作用范围；所述导航站导航交接约束信息是指确保导航交接成功而设定的运动体在交接区域内的最短路径长度D；所述导航交接是指多个导航站之间移交运动体的导航权；所述交接区域是指两个导航圆之间的重叠区域；所述运动体曲率约束信息是指运动体最小转弯半径r_tc；

步骤2、随机生成初始路径种群；一条路径由起点位置、终点位置和中间路点的位置及朝向共同确定；所述中间路点是指除起点和终点之外的路点；

其中，中间路点与中间路点之间采用Dubins的“CLC”路径连接；

将中间路点设置在交接区域边界弧上，随机生成多条路径组成初始路径种群，其中，中间路点的位置在所属边界弧上随机生成，中间路点的朝向范围由其位置所决定，具体朝向在其朝向范围内随机生成；

步骤3、评价路径:评价指标包括路径总长度和约束违反程度；所述路径总长度是指按照导航顺序依次连接起点、中间路点、终点所形成的全部路段长度之和；所述路段是指两个相邻路点之间的路径；所述约束违反程度是指导航交接约束违反程度，当运动体在交接区域内的路径长度大于或等于约束值时，约束违反程度为0，否则，约束违反程度为两者的绝对差；

步骤4、记录初始种群的最优路径；若初始种群中存在约束违反程度为0的路径，则在约束违反程度为0的路径中选取路径总长度最短的那条路径作为初始种群的最优路径，若初始种群所有路径的约束违反程度均大于0，则选取约束违反程度最小的那条路径作为初始种群的最优路径；

步骤5、采用差分进化算法优化路径；首先通过变异、交叉操作产生新路径，其中，变异和交叉操作仅作用于路点的位置，路点的朝向范围随其位置的改变而改变，朝向在其范围内随机生成；

然后在新路径和旧路径中进行选择，留下较优的路径；选择操作的原则为：当两条路径的约束违反程度相等时，留下路径总长度较短的那条路径，当两条路径的约束违反程度不相等时，留下约束违反程度较小的那条路径；将选择操作留下的路径与之前记录的种群最优路径进行比较，比较方法与选择操作的原则相同，将两者中较优的一条作为当前种群的最优路径；基于当前路径种群，继续进行变异、交叉和选择操作并得到新的路径种群和最优路径，直到种群进化代数达到给定的最大进化代数时，终止循环过程，输出当前种群的最优路径，完成路径规划。

2.如权利要求1所述的一种面向多导航站接力导航的运动体路径规划方法，其特征是，所述步骤2中中间路点位置的具体表示方式为：

定义入点为位于入弧上的中间路点，所述入弧为运动体进入交接区域的边界弧；定义出点为位于出弧上的中间路点，所述出弧为运动体离开交接区域的边界弧；入点位置的表示方式如下：将交接区域的后导航圆的圆心作为极点，水平方向向右为极轴，逆时针为正方向，建立局部极坐标系，入点的位置用极角表示；所述交接区域的后导航圆是指运动体即将进入的导航圆；

出点位置的表示方法如下：将交接区域的前导航圆的圆心作为极点，水平方向向右为极轴，逆时针为正方向，建立局部极坐标系，出点的位置用极角θ表示；所述交接区域的前导航圆是指运动体即将离开的导航圆；

由此，一条路径所有中间路点的位置由入点极角和出点极角联合表示为θ＝[θ₁,θ₂,...,θ_n]，θ_i表示第i个中间路点的位置，i为奇数时，表示为入点的极角，i为偶数时，表示为出点的极角；限定θ_i的取值范围为[θ_i,min,θ_i,max]，其中θ_i,min和θ_i,max为交接区域顶点所对应的极角；所述交接区域顶点为入弧与出弧的交点；采取相对编码的方式，先将中间路点位置的解范围映射到[0，1]区间，即将θ＝[θ₁,θ₂,...,θ_n]，θ_i∈[θ_i,min,θ_i,max]转化为x＝[x₁,x₂,...,x_n]，x_i∈[0,1]。

3.如权利要求1或2所述的一种面向多导航站接力导航的运动体路径规划方法，其特征是，所述步骤2中中间路点朝向范围的具体计算方法为：

过入点作交接区域的后导航圆的切线，切线将以入点为中心的360°角度范围分割成两半，取靠近终点的180°角度范围作为入点的朝向范围；过出点作交接区域的前导航圆的切线，切线将以出点为中心的360°角度范围分割成两半，取靠近终点的180°角度范围作为出点的朝向范围。