[发明专利]面向多导航站接力导航的运动体路径规划方法

摘要

本发明公开了一种面向多导航站接力导航的运动体路径规划方法，该方法是在考虑基站有效导航范围约束、导航交接约束和运动体曲率约束的前提下，为运动体规划出一条能够各种满足各种约束条件且快速从起点抵达终点的水平二维空间路径；首先，判断满足导航交接约束的难易程度，然后，当约束较易满足时，采用基于Dubins路径的路径规划方法，当约束较难满足时，采用基于交接区域内最长路径模式的Dubins路径规划方法；本发明提供的面向多基站接力导航的运动体路径规划方法打破了单个基站导航范围小的局限，有效延伸了运动体的可控移动范围，可以解决海、陆、空不同情形多约束条件下运动体从起点到终点的路径规划问题。

主权项

一种面向多导航站接力导航的运动体路径规划方法，其特征在于，包括如下步骤：步骤0、输入路径规划信息，具体包括：运动体起点和终点位置信息、导航站导航范围约束信息、导航站导航交接约束信息和运动体曲率约束信息；所述导航站导航范围约束信息是指导航站中心的坐标及有效作用范围，即运动体从起点到终点的整条路径须全部位于导航圆内；所述导航圆表示导航站有效作用范围，半径为r；所述导航站导航交接约束是指确保导航交接成功而设定的运动体在交接区域内的最短路径长度D；所述导航交接是指多个导航站之间移交运动体的导航权；所述交接区域是指两个导航圆之间的重叠区域；所述运动体曲率约束信息是指运动体最小转弯半径rtc，路点之间采用限定曲率的Dubins路径连接；步骤1、判断导航交接约束的难易程度，并用难易系数γj表示难易程度；γj为第j个交接区域内导航交接约束Dj与导航交接区域最长Dubins路径长度lmax的比值；若γj＜0.7，采用基于Dubins路径的路径规划方法，即采用步骤2至5的方法获得最终路径；若γj≥0.7，采用基于交接区域内最长路径模式的Dubins路径规划方法，即采用步骤6获得最终路径；步骤2、随机生成初始路径种群；一条路径由起点位置、终点位置和中间路点的位置及朝向共同确定；所述中间路点是指除起点和终点之外的路点；其中，中间路点与中间路点之间采用Dubins的“CLC”路径连接；将中间路点设置在交接区域边界弧上，随机生成多条路径组成初始路径种群，其中，中间路点的位置在所属边界弧上随机生成，中间路点的朝向范围由其位置所决定，具体朝向在其朝向范围内随机生成；步骤3、评价路径；评价指标包括路径总长度和约束违反程度；所述路径总长度是指按照导航顺序依次连接起点、中间路点、终点所形成的全部路段长度之和；所述路段是指两个相邻路点之间的路径；所述约束违反程度是指导航交接约束违反程度，当运动体在交接区域内的路径长度大于或等于约束值时，约束违反程度为0，否则，约束违反程度为两者的绝对差；步骤4、记录初始种群的最优路径；若初始种群中存在约束违反程度为0的路径，则在约束违反程度为0的路径中选取路径总长度最短的那条路径作为初始种群的最优路径，若初始种群所有路径的约束违反程度均大于0，则选取约束违反程度最小的那条路径作为初始种群的最优路径；步骤5、采用差分进化算法优化路径；首先通过变异、交叉操作产生新路径，其中，变异和交叉操作仅作用于路点的位置，路点的朝向范围随其位置的改变而改变，朝向在其范围内随机生成；然后在新路径和旧路径中进行选择，留下较优的路径；选择操作的原则为：当两条路径的约束违反程度相等时，留下路径总长度较短的那条路径，当两条路径的约束违反程度不相等时，留下约束违反程度较小的那条路径；将选择操作留下的路径与之前记录的种群最优路径进行比较，比较方法与选择操作的原则相同，将两者中较优的一条作为当前种群的最优路径；当种群进化代数达到给定的最大进化代数时，终止循环过程，输出当前种群的最优路径，完成路径规划；步骤6、采用基于交接区域内最长路径模式的Dubins路径规划，具体包括如下步骤：S401、计算各个交接区域的宽度rd，以及交接区域宽度rd与导航圆半径r的比值所述交接区域的宽度是指交接区域入弧中点与出弧中点之间的距离；S402、计算运动体最小转弯半径rtc与导航圆半径r的比值S403、根据与通过查下表确定运动体在各个交接区域内的路径模式：所述交接区域内的路径模式包括7种，分别定义如下：模式1：双顶点相切S型；该模式下，运动体在交接区域的入点和出点分别是交接区域的两个顶点，运动体在入点的朝向与本交界区域对应的第一个导航圆相切，运动体在出点的朝向与本交界区域对应的第二个导航圆相切；运动体在交接区域内的最长路径为Dubins的CLC路径，形如“S”，其中第一个C表示入点辅助圆的圆弧段，第二个C表示出点辅助圆的圆弧段，L表示与两条圆弧相切的直线段；所述入点辅助圆是指以入点为切点，以rtc为半径作的两个辅助圆，称之为左转弯圆和右转弯圆；所述出点辅助圆是指以出点为切点，以rtc为半径作的两个辅助圆，称之为左转弯圆和右转弯圆；运动体从上顶点进入交接区域，从下顶点离开交接区域，或者从下顶点进入交接区域，从上顶点离开交接区域；模式2：中心相切S型；该模式下，所述交接区域的中心为交接区域两顶点连线与交接区域所属的两个导航圆圆心连线的交点；运动体在交接区域内的最长路径为Dubins的CLC路径，形如“S”，其中第一个C表示入点辅助圆的圆弧段，第二个C表示出点辅助圆的圆弧段，L表示与两条圆弧相切的直线段；入点辅助圆与出点辅助圆相切于交接区域的中心，直线段的长度为0；运动体从上半段入弧进入交接区域，从下半段出弧离开交接区域，或者从下半段入弧进入交接区域，从上半段出弧离开交接区域；模式3：单顶点相切S型；该模式下，运动体在交接区域的入点和出点为一个位于交接区域的顶点，若入点位于交接区域的顶点，则其朝向与第一个导航圆相切，若出点位于交接区域的顶点，则其朝向与第二个导航圆相切；运动体在交接区域内的最长路径为Dubins的CLC路径，形如“S”，其中第一个C表示入点辅助圆的圆弧段，第二个C表示出点辅助圆的圆弧段，L表示与两条圆弧相切的直线段；运动体从上半段入弧进入交接区域，从下半段出弧离开交接区域，或者从下半段入弧进入交接区域，从上半段出弧离开交接区域；模式4：双顶点相切C型；该模式下，运动体在交接区域的入点和出点分别是交接区域的两个顶点，运动体在入点和出点的朝向要么都与第一个导航圆相切，要么都与第二个导航圆相切；运动体在交接区域内的最长路径为Dubins的CLC路径，形如“C”，其中第一个C表示入点辅助圆的圆弧段，第二个C表示出点辅助圆的圆弧段，L表示与两条圆弧相切的直线段；运动体从上顶点进入交接区域，从下顶点离开交接区域，或者从下顶点进入交接区域，从上顶点离开交接区域；模式5：勺型；该模式下，运动体在交接区域的入点和出点只有一个位于交接区域的顶点，运动体在交接区域内的最长路径为Dubins的CLC路径，形如“勺子”，其中第一个C表示入点辅助圆的圆弧段，第二个C表示出点辅助圆的圆弧段，L表示与两条圆弧相切的直线段；运动体从上半段入弧进入交接区域，从下半段出弧离开交接区域，或者从下半段入弧进入交接区域，从上半段出弧离开交接区域；模式6：单顶点相切C型；该模式下，运动体在交接区域的入点和出点只有一个位于交接区域的顶点，若入点位于交接区域的顶点，则入点朝向与第二个导航圆相切，若出点位于交接区域的顶点，则出点朝向与第一个导航圆相切；运动体在交接区域内的最长路径为Dubins的CLC路径，形如“C”，其中第一个C表示入点辅助圆的圆弧段，第二个C表示出点辅助圆的圆弧段，L表示与两条圆弧相切的直线段；运动体从上半段入弧进入交接区域，从下半段出弧离开交接区域，或者从下半段入弧进入交接区域，从上半段出弧离开交接区域；模式7：循环转圈圆型；运动体在交接区域内循环转圈移动；S404、由运动体所在非交接区域的前、后两个相邻交接区域内的路径模式确定运动体在非交接区域内的路径模式；S405、对中间路点位置进行编码，具体为：中间路点包括入点和出点两种，入点为位于入弧上的中间路点，入弧为运动体进入交接区域的边界弧；出点为位于出弧上的中间路点，所述出弧为运动体离开交接区域的边界弧；入点位置的表示方式如下：将交接区域的后导航圆的圆心作为极点，水平方向向右为极轴，逆时针为正方向，建立局部极坐标系，入点的位置用极角表示；所述交接区域的后导航圆是指运动体即将进入的导航圆；出点位置的表示方法如下：将交接区域的前导航圆的圆心作为极点，水平方向向右为极轴，逆时针为正方向，建立局部极坐标系，出点的位置用极角θ表示；所述交接区域的前导航圆是指运动体即将离开的导航圆；由此，一条路径所有中间路点的位置由入点极角和出点极角联合表示为θ＝[θ1,θ2,...,θn]，θi表示第i个中间路点的位置，i为奇数时，表示为入点的极角，i为偶数时，表示为出点的极角；限定θi的取值范围为[θi,min,θi,max]，其中θi,min和θi,max为交接区域顶点所对应的极角；所述交接区域顶点为入弧与出弧的交点；采取相对编码的方式，先将中间路点位置的解范围映射到[0,1]区间，即将θ＝[θ1,θ2,...,θn]，θi∈[θi,min,θi,max]转化为x＝[x1,x2,...,xn]，xi∈[0,1]；S406、随机生成初始路径种群；当运动体在交接区域内的路径模式为模式1时，中间路点位置种群初始化方法如下：对于第j个导航交接区域，随机生成NP×γj个中间路点位置向量(x2j‑1,x2j)，入点位置x2j‑1和出点位置x2j满足：x2j‑1∈[0,1‑γj]&x2j∈[0,1‑γj]或x2j‑1∈[γj,1]&x2j∈[γj,1]，以及随机生成NP‑NP×γj个不满足以上条件的中间路点位置向量；所述NP表示种群规模；当运动体在交接区域内的路径模式为模式2时，设模式2中最长Dubins路径的入点位置为σ1或σ2，同样出点位置为σ2或σ1；中间路点位置种群初始化方法如下：对于第j个导航交接区域，随机生成NP×γj个中间路点位置向量，入点位置x2j‑1和出点位置x2j满足：x2j‑1∈[σ1‑σ1(1‑γj),σ1+(0.5‑σ1)(1‑γj)]且x2j∈[σ1‑σ1(1‑γj),σ1+(0.5‑σ1)(1‑γj)]或者x2j‑1∈[σ2‑(σ2‑0.5)(1‑γj),σ2+(1‑σ2)(1‑γj)]且x2j∈[σ2‑(σ2‑0.5)(1‑γj),σ2+(1‑σ2)(1‑γj)]，以及随机生成NP‑NP×γj个不满足以上条件的中间路点位置向量；当运动体在交接区域内的路径模式为模式3～模式6时，中间路点位置种群初始化方法如下：对于第j个导航交接区域，随机生成NP×γj个路点位置向量(x2j‑1,x2j)，入点位置x2j‑1和出点位置x2j满足：x2j‑1∈[0,1‑γj]&x2j∈[0,0.5]或x2j‑1∈[γj,1]&x2j∈[0.5,1]，以及随机生成NP‑NP×γj个不满足以上条件的路点位置向量；当运动体在交接区域内的路径模式为模式7时，中间路点位置种群初始化方法如下：以交接区域对应的两个导航圆的圆心作半径为r‑rtc的同心圆，这两个同心圆的公共区域即为循环转圈圆的圆心位置范围；对于第j个导航交接区域，在循环转圈圆圆心位置范围内随机生成NP个位置向量(x2j‑1,x2j)，其中(x2j‑1,x2j)表示循环转圈圆相对编码圆心坐标；S407、评价路径；评价指标包括路径总长度和约束违反程度；所述路径总长度是指按照导航顺序依次连接起点、中间路点、终点所形成的全部路段长度之和；所述路段是指两个相邻路点之间的路径；所述约束违反程度是指导航交接约束违反程度，当运动体在交接区域内的路径长度大于或等于约束值时，约束违反程度为0，否则，约束违反程度为两者的绝对差；其中，当交接区域内的路径模式为模式7，运动体的路径存在多条时，计算每条路径的路径总长度和约束违反程度，并选择其中最优的路径作为运动体的路径；S408、记录初始种群的最优路径；若初始种群中存在约束违反程度为0的路径，则在约束违反程度为0的路径中选取路径总长度最短的那条路径作为初始种群的最优路径，若初始种群所有路径的约束违反程度均大于0，则选取约束违反程度最小的那条路径作为初始种群的最优路径；S409、采用步骤5的方法对S406的初始路径种群进行路径优化，最后输出当前种群的最优路径，完成路径规划。