适用于地磁辅助导航航迹规划的粒子群优化方法技术

本发明专利技术公开一种适用于地磁辅助导航航迹规划的粒子群优化方法，包括如下步骤：步骤1，考虑水下环境约束条件在内，建立水下航行器路径规划评判模型；步骤2，根据模型中时间、函数关系、约束、目标条件和变量，将路径规划问题转化为航迹优化问题；步骤3，采用Dijkstra算法+粒子群优化算法的混合算法进行航迹规划，将粗选航迹作为最优解的一个初始估计输入，并对粒子群优化算法编码，将时间信息看作搜索空间中的“粒子”，改善算法的收敛性能。此种优化方法可提高地磁导航航迹规划的精确度和可靠性。

Particle swarm optimization method for trajectory planning of geomagnetic aided navigation

The invention discloses a method for geomagnetic navigation path planning particle swarm optimization method, which comprises the following steps: Step 1, considering the environmental constraints of the underwater vehicle, path planning evaluation model established under water; step 2, according to the model in time, function relation, constraint, conditions and variables, path planning the trajectory optimization problem is transformed into a problem; step 3, the hybrid algorithm Dijkstra algorithm and particle swarm optimization algorithm for route planning, the roughing track as an initial solution estimation of the input, and the particle swarm optimization algorithm for encoding, and time information in the search space as \particles\, to improve the convergence performance of the algorithm. This optimization method can improve the accuracy and reliability of geomagnetic navigation route planning.

【技术实现步骤摘要】
适用于地磁辅助导航航迹规划的粒子群优化方法
本专利技术属于水下地磁导航
，特别涉及一种适用于地磁辅助导航航迹规划的粒子群优化方法。
技术介绍
水下自主航行器(AUV)是人类探索和开发海洋资源的重要工具。作为航行器任务规划的核心之一，航迹规划的目的就是从航行任务出发，利用算法为航行器规划一条符合航行任务、机动性能、威胁规避等各类边界控制条件的最短路径。在复杂的海洋现象下，在水下进行高精度自主导航问题是水下航行器发展的难题。高精度自主导航和定位是AUV的关键技术。一方面，AUV需要在深海长时间航行，获取有效资源信息，完成作业任务；另一方面，AUV需要根据水下环境信息，避开障碍物和敌方检测，自主规划行驶航迹。惯性导航由于其体积小、成本低、精度高、不依赖外界信息、不向外界辐射能量、抗干扰能力极强、隐蔽性好等优点成为水下航行器最常用的导航方式。但是，惯性导航系统的定位误差随时间积累发散，长航时无法保持高精度的固有缺陷尚难以克服。因此，惯性导航常常需要辅助导航系统进行误差修正。惯导/地磁组合导航系统具有可靠、全天候、中高精度、连续导航等优点，作为较为理想的潜用导航系统，地磁图适配性分析以及航迹规划问题是其中的两项关键技术。在水下地磁导航中，一方面，最优航迹的确定与地磁适配区分布密切相关；另一方面，为了获得准确度高的匹配位置，也需要对水下航行器的航迹进行最优规划。航迹规划是一个综合性很强的跨领域研究课题，也就是非确定性多项式的复杂问题，无法用已知的多项式时间问题来求取最优解，往往采用优化算法来获取问题解。航迹规划可以采用的算法有多种，按照规划范围，可以分为全局性规划算法和局部寻优算法；按照规划的计算方法，可以分为最优式算法、启发式算法和随机性算法。算法各有优缺点，不同地磁图特征对不同规划算法又有不同程度的影响，融合多种算法技术，适合于地磁导航特征的混合航迹规划算法是水下航行器航迹规划研究的方向之一。
技术实现思路
本专利技术的目的，在于针对惯导误差随时间积累，无法长时间保持导航精度的问题，提供一种适用于地磁辅助导航航迹规划的粒子群优化方法，其可提高地磁导航航迹规划的精确度和可靠性。一种适用于地磁辅助导航航迹规划的粒子群优化方法，包括如下步骤：步骤1，考虑水下环境约束条件在内，建立水下航行器路径规划评判模型；步骤2，根据模型中时间、函数关系、约束、目标条件和变量，将路径规划问题转化为航迹优化问题；步骤3，采用Dijkstra算法+粒子群优化算法的混合算法进行航迹规划，将粗选航迹作为最优解的一个初始估计输入，并对粒子群优化算法编码，将时间信息看作搜索空间中的“粒子”，改善算法的收敛性能。上述步骤1中，水下环境约束条件包括地磁特征约束、最大转向角约束和障碍物约束，其中，地磁特征约束表达式为：式中，Jintensity为满足安全航行的地磁强度代价函数，fsafe为最小安全航行深度，fmission为水下航行器航行时的离海底深度；Jslope＝1-P(i,j)式中，k、是S函数参数，f(i,j)是关于网格点位置(i,j)的地磁强度函数，μ为特征阈值，Jslope为坡度跟随代价函数；最大转向角约束表达式为：式中，θmax为航行器的最大转角，θ为航行器的当前转向角，Jangle指航行器向左或者向右转到极限位置与不发生偏转时中心线所形成的角度；障碍物约束表达式为：式中，j为威胁源的个数，r＝[x,y]T是位置向量，Q为监听源的威胁程度，ε和H分别为平均值和方差,Hj表示j取值为0～n-1中的某一数值时对应的μj表示各个威胁源的二维坐标；航迹规划评价函数表达式为：minfit(xi,yi)＝ω1Jintensity(xi,yi)+ω2Jslope(xi,yi)+ω3Jangle(xi,yi)+ω4Jmon(xi,yi)s.t.0＜ω1＜10＜ω2＜10＜ω3＜10＜ω4＜1ω1+ω2+ω3+ω4＝1式中，fit(xi,yi)为航行器进行路径规划的航迹评价函数，ω1、ω2、ω3、ω4分别为各约束条件的代价函数的加权因子。上述步骤2中，对模型中不同的代价函数进行归一化处理，采取如下变换方法：J’intensity，J’slope，J'angle，J'mon分别为归一化以后的地磁强度代价函数，坡度跟随代价函数，转向角代价函数，障碍物代价函数；max(Jintensity)，max(Jslope)，max(Jangle)，max(Jmon)分别为所有节点的地磁强度代价函数，坡度跟随代价函数，转向角代价函数，障碍物代价函数的最大值；min(Jintensity)，min(Jslope)，min(Jangle)，min(Jmon)分别为所有节点的地磁强度代价函数，坡度跟随代价函数，转向角代价函数，障碍物代价函数的最大值；归一化处理后的航迹评价函数为：fit(xi,yi)＝ω1J’intensity(xi,yi)+ω2J’slope(xi,yi)+ω3J'angle(xi,yi)+ω4J'mon(xi,yi)(14)式中，fit(xi,yi)为航行器进行路径规划的航迹评价函数，ω1、ω2、ω3、ω4分别为各约束条件的代价函数的加权因子。上述步骤3包括如下具体步骤：步骤31，初始化粒子群的位置信息xi＝(xi1xi2…xiM)和速度信息vi＝(vi1vi2…viM)；步骤32，计算每个粒子的当前适应度函数值fit(xi',yi')；步骤33，输出当前最优解Gid；步骤34，判断是否达到最大迭代次数，是则算法结束，否则返回步骤32。上述步骤31的具体内容是：A.获取地图信息，确定起始节点和目标节点，利用Dijkstra算法得出粗选航迹；B.设置加速因子c1，c2，惯性权重ω，最大迭代次数k，适应度函数fit，种群规模M；C.初始化粒子群的位置信息xi＝(xi1xi2…xiM)和速度信息vi＝(vi1vi2…viM)。上述步骤32的具体内容是：A.根据下式更新粒子当前的位置信息和速度信息：其中，Pid为单个粒子经历过的最好位置；在D维搜索空间，群体规模为m，则Pid是当前第i(i＝1,2,…,m)个粒子所获得的最好的适应度函数值；Gid为群体中所有粒子找到的最佳位置；粒子速度向量满足约束条件——动态系统Lipschitz条件：式中，ω表示惯性权重，表示第i个粒子d维分量在第t次迭代中的速度和位置，c1、c2表示加速因子，r1、r2为[0,1]上均匀分布的随机数；B.根据每个粒子的位置信息和速度信息更新当前局部最优值Pid和全局最优值Gid，与历史最优值比较，如果比历史最优值小，则更新Pid和Gid，记录全局最优的粒子的位置信息。采用上述方案后，本专利技术建立水下航行器建立水下航行器路径规划评判模型，并且对惯性权重和编码方式进行了改进，采用Dijkstra算法+粒子群优化算法的混合算法进行航迹规划，对粗选航迹进行优化与对比，说明采用混合算法进行航迹规划的有效性。比起标准的粒子群优化算法，组合规划算法具有更优的全局搜索能力。附图说明图1是本专利技术的原理图；图2是Dijkstra算法+粒子群优化算法的混合粒子群优化方法解决航迹规划问题的流程图。具体实施方式以下将结合附图，对本专利技术的技术方案及有益效果进行详细说明。如图1所示，本专利技术提供一种适用于地磁辅助导航本文档来自技高网...

【技术保护点】
一种适用于地磁辅助导航航迹规划的粒子群优化方法，其特征在于包括如下步骤：步骤1，考虑水下环境约束条件在内，建立水下航行器路径规划评判模型；步骤2，根据模型中时间、函数关系、约束、目标条件和变量，将路径规划问题转化为航迹优化问题；步骤3，采用Dijkstra算法+粒子群优化算法的混合算法进行航迹规划，将粗选航迹作为最优解的一个初始估计输入，并对粒子群优化算法编码，将时间信息看作搜索空间中的“粒子”，改善算法的收敛性能。

【技术特征摘要】
1.一种适用于地磁辅助导航航迹规划的粒子群优化方法，其特征在于包括如下步骤：步骤1，考虑水下环境约束条件在内，建立水下航行器路径规划评判模型；步骤2，根据模型中时间、函数关系、约束、目标条件和变量，将路径规划问题转化为航迹优化问题；步骤3，采用Dijkstra算法+粒子群优化算法的混合算法进行航迹规划，将粗选航迹作为最优解的一个初始估计输入，并对粒子群优化算法编码，将时间信息看作搜索空间中的“粒子”，改善算法的收敛性能。2.如权利要求1所述的适用于地磁辅助导航航迹规划的粒子群优化方法，其特征在于：所述步骤1中，水下环境约束条件包括地磁特征约束、最大转向角约束和障碍物约束，其中，地磁特征约束表达式为：式中，Jintensity为满足安全航行的地磁强度代价函数，fsafe为最小安全航行深度，fmission为水下航行器航行时的离海底深度；Jslope＝1-P(i,j)式中，k、是S函数参数，f(i,j)是关于网格点位置(i,j)的地磁强度函数，μ为特征阈值，Jslope为坡度跟随代价函数；最大转向角约束表达式为：式中，θmax为航行器的最大转角，θ为航行器的当前转向角，Jangle指航行器向左或者向右转到极限位置与不发生偏转时中心线所形成的角度；障碍物约束表达式为：式中，j为威胁源的个数，r＝[x,y]T是位置向量，Q为监听源的威胁程度，ε和H分别为平均值和方差,Hj表示j取值为0～n-1中的某一数值时对应的μj表示各个威胁源的二维坐标；航迹规划评价函数表达式为：minfit(xi,yi)＝ω1Jintensity(xi,yi)+ω2Jslope(xi,yi)+ω3Jangle(xi,yi)+ω4Jmon(xi,yi)s.t.0＜ω1＜10＜ω2＜10＜ω3＜10＜ω4＜1ω1+ω2+ω3+ω4＝1式中，fit(xi,yi)为航行器进行路径规划的航迹评价函数，ω1、ω2、ω3、ω4分别为各约束条件的代价函数的加权因子。3.如权利要求1所述的适用于地磁辅助导航航迹规划的粒子群优化方法，其特征在于：所述步骤2中，对模型中不同的代价函数进行归一化处理，采取如下变换方法：J’intensity，J’slope，J'angle，J'mon分别为归一化以后的地磁强度代价函数，坡度跟随代价函数，转向角代价函数，障碍物代价函数；max(Jintensity)，max(Jslope)，max(Jangle)，max(Jmon)分...

【专利技术属性】
技术研发人员：王立辉，马明珠，余乐，
申请(专利权)人：东南大学，
类型：发明
国别省市：江苏,32