CN107356254B - 适用于地磁辅助导航航迹规划的粒子群优化方法 - Google Patents

Abstract

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

Description

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

技术领域

本发明属于水下地磁导航技术领域，特别涉及一种适用于地磁辅助导航航迹 规划的粒子群优化方法。

背景技术

水下自主航行器(AUV)是人类探索和开发海洋资源的重要工具。作为航行 器任务规划的核心之一，航迹规划的目的就是从航行任务出发，利用算法为航行 器规划一条符合航行任务、机动性能、威胁规避等各类边界控制条件的最短路径。 在复杂的海洋现象下，在水下进行高精度自主导航问题是水下航行器发展的难 题。高精度自主导航和定位是AUV的关键技术。一方面，AUV需要在深海长时间 航行，获取有效资源信息，完成作业任务；另一方面，AUV需要根据水下环境信 息，避开障碍物和敌方检测，自主规划行驶航迹。惯性导航由于其体积小、成本 低、精度高、不依赖外界信息、不向外界辐射能量、抗干扰能力极强、隐蔽性好 等优点成为水下航行器最常用的导航方式。但是，惯性导航系统的定位误差随时 间积累发散，长航时无法保持高精度的固有缺陷尚难以克服。因此，惯性导航常 常需要辅助导航系统进行误差修正。

惯导/地磁组合导航系统具有可靠、全天候、中高精度、连续导航等优点， 作为较为理想的潜用导航系统，地磁图适配性分析以及航迹规划问题是其中的两 项关键技术。在水下地磁导航中，一方面，最优航迹的确定与地磁适配区分布密 切相关；另一方面，为了获得准确度高的匹配位置，也需要对水下航行器的航迹 进行最优规划。航迹规划是一个综合性很强的跨领域研究课题，也就是非确定性 多项式的复杂问题，无法用已知的多项式时间问题来求取最优解，往往采用优化 算法来获取问题解。航迹规划可以采用的算法有多种，按照规划范围，可以分为 全局性规划算法和局部寻优算法；按照规划的计算方法，可以分为最优式算法、 启发式算法和随机性算法。算法各有优缺点，不同地磁图特征对不同规划算法又 有不同程度的影响，融合多种算法技术，适合于地磁导航特征的混合航迹规划算法是水下航行器航迹规划研究的方向之一。

发明内容

本发明的目的，在于针对惯导误差随时间积累，无法长时间保持导航精度的 问题，提供一种适用于地磁辅助导航航迹规划的粒子群优化方法，其可提高地磁 导航航迹规划的精确度和可靠性。

一种适用于地磁辅助导航航迹规划的粒子群优化方法，包括如下步骤：

步骤1，考虑水下环境约束条件在内，建立水下航行器路径规划评判模型；

步骤2，根据模型中时间、函数关系、约束、目标条件和变量，将路径规划 问题转化为航迹优化问题；

步骤3，采用Dijkstra算法+粒子群优化算法的混合算法进行航迹规划，将 粗选航迹作为最优解的一个初始估计输入，并对粒子群优化算法编码，将时间信 息看作搜索空间中的“粒子”，改善算法的收敛性能。

上述步骤1中，水下环境约束条件包括地磁特征约束、最大转向角约束和障 碍物约束，

其中，地磁特征约束表达式为：

式中，J_intensity为满足安全航行的地磁强度代价函数，f_safe为最小安全航行深 度，f_mission为水下航行器航行时的离海底深度；

J_slope＝1-P(i,j)

式中，k、

是S函数参数，f(i,j)是关于网格点位置(i,j)的地磁强度函数， μ为特征阈值，J_slope为坡度跟随代价函数；

最大转向角约束表达式为：

式中，θ_max为航行器的最大转角，θ为航行器的当前转向角，J_angle指航行器 向左或者向右转到极限位置与不发生偏转时中心线所形成的角度；

障碍物约束表达式为：

式中，j为威胁源的个数，r＝[x,y]^T是位置向量，Q为监听源的威胁程度， ε和H分别为平均值和方差,H_j表示j取值为0～n-1中的某一数值时对应的

μ_j表示各个威胁源的二维坐标；

航迹规划评价函数表达式为：

min fit(x_i,y_i)＝ω₁J_intensity(x_i,y_i)+ω₂J_slope(x_i,y_i)+ω₃J_angle(x_i,y_i)+ω₄J_mon(x_i,y_i)

s.t.0＜ω₁＜1

0＜ω₂＜1

0＜ω₃＜1

0＜ω₄＜1

ω₁+ω₂+ω₃+ω₄＝1

式中，fit(x_i,y_i)为航行器进行路径规划的航迹评价函数，ω₁、ω₂、ω₃、ω₄分别为各约束条件的代价函数的加权因子。

上述步骤2中，对模型中不同的代价函数进行归一化处理，采取如下变换方 法：

J’_intensity，J’_slope，J'_angle，J'_mon分别为归一化以后的地磁强度代价函数，坡度跟随代价函数，转向角代价函数，障碍物代价函数；

max(J_intensity)，max(J_slope)，max(J_angle)，max(J_mon)分别为所有节点的地磁强 度代价函数，坡度跟随代价函数，转向角代价函数，障碍物代价函数的最大值；

min(J_intensity)，min(J_slope)，min(J_angle)，min(J_mon)分别为所有节点的地磁强 度代价函数，坡度跟随代价函数，转向角代价函数，障碍物代价函数的最大值；

归一化处理后的航迹评价函数为：

fit(x_i,y_i)＝ω₁J’_intensity(x_i,y_i)+ω₂J’_slope(x_i,y_i)+ω₃J'_angle(x_i,y_i)+ω₄J'_mon(x_i,y_i) (14)

式中，fit(x_i,y_i)为航行器进行路径规划的航迹评价函数，ω₁、ω₂、ω₃、ω₄分别为各约束条件的代价函数的加权因子。

上述步骤3包括如下具体步骤：

步骤31，初始化粒子群的位置信息x_i＝(x_i1 x_i2…x_iM)和速度信息 v_i＝(v_i1 v_i2…v_iM)；

步骤32，计算每个粒子的当前适应度函数值fit(x_i',y_i')；

步骤33，输出当前最优解G_id；

步骤34，判断是否达到最大迭代次数，是则算法结束，否则返回步骤32。

上述步骤31的具体内容是：

A.获取地图信息，确定起始节点和目标节点，利用Dijkstra算法得出粗选 航迹；

B.设置加速因子c₁，c₂，惯性权重ω，最大迭代次数k，适应度函数fit， 种群规模M；

C.初始化粒子群的位置信息x_i＝(x_i1 x_i2…x_iM)和速度信息 v_i＝(v_i1 v_i2…v_iM)。

上述步骤32的具体内容是：

A.根据下式更新粒子当前的位置信息和速度信息：

其中，P_id为单个粒子经历过的最好位置；在D维搜索空间，群体规模为m， 则P_id是当前第i(i＝1,2,…,m)个粒子所获得的最好的适应度函数值；G_id为群体中 所有粒子找到的最佳位置；

粒子速度向量满足约束条件——动态系统Lipschitz条件：

式中，ω表示惯性权重，

表示第i个粒子d维分量在第t次迭代中的 速度和位置，c₁、c₂表示加速因子，r₁、r₂为[0,1]上均匀分布的随机数；

B.根据每个粒子的位置信息和速度信息更新当前局部最优值P_id和全局最优 值G_id，与历史最优值比较，如果比历史最优值小，则更新P_id和G_id，记录全局 最优的粒子的位置信息。

采用上述方案后，本发明建立水下航行器建立水下航行器路径规划评判模 型，并且对惯性权重和编码方式进行了改进，采用Dijkstra算法+粒子群优化算 法的混合算法进行航迹规划，对粗选航迹进行优化与对比，说明采用混合算法进 行航迹规划的有效性。比起标准的粒子群优化算法，组合规划算法具有更优的全 局搜索能力。

附图说明

图1是本发明的原理图；

图2是Dijkstra算法+粒子群优化算法的混合粒子群优化方法解决航迹规划 问题的流程图。

具体实施方式

以下将结合附图，对本发明的技术方案及有益效果进行详细说明。

如图1所示，本发明提供一种适用于地磁辅助导航航迹规划的粒子群优化方 法，包括如下步骤：

步骤1，考虑水下环境约束条件在内，建立水下航行器路径规划评判模型；

水下航行器路径规划已被证明是一个包含3D运动学和动力学约束的不确定 多项式问题。综合考虑地磁图特征对适配性能的影响，对于单独的地磁图特征参 数，同样需要做出约束。水下环境约束条件包括地磁特征约束、最大转向角约束、 障碍物约束等。

其中，地磁特征约束表达式为：

式中，J_intensity为满足安全航行的地磁强度代价函数，f_safe为最小安全航行深 度，f_mission为水下航行器航行时的离海底深度。

J_slope＝1-P(i,j) (3)

式中，k、

是S函数参数，f(i,j)是关于网格点位置(i,j)的地磁强度函数， μ为特征阈值，J_slope为坡度跟随代价函数。

最大转向角约束表达式为：

式中，θ_max为航行器的最大转角，θ为航行器的当前转向角，J_angle指航行器 向左或者向右转到极限位置与不发生偏转时中心线所形成的角度,可通过相邻三 个航迹点(x_i-1,y_i-1)、(x_i,y_i)、(x_i+1,y_i+1)计算。

(x_i-1,y_i-1)、(x_i,y_i)两点间的距离由d(i-1,i)表示，航行器的当前转向角为θ， 可知：

障碍物约束表达式为：

式中，j为威胁源的个数，r＝[x,y]^T是位置向量，Q为监听源的威胁程度， ε和H分别为平均值和方差,H_j表示j取值为0～n-1中的某一数值时对应的

μ_j表示各个威胁源的二维坐标。

航迹规划评价函数表达式为：

min fit(x_i,y_i)＝ω₁J_intensity(x_i,y_i)+ω₂J_slope(x_i,y_i)+ω₃J_angle(x_i,y_i)+ω₄J_mon(x_i,y_i)

(9)#

s.t.0＜ω₁＜1

0＜ω₂＜1

0＜ω₃＜1

0＜ω₄＜1

ω₁+ω₂+ω₃+ω₄＝1

式中，fit(x_i,y_i)为航行器进行路径规划的航迹评价函数，ω₁、ω₂、ω₃、ω₄分别为各约束条件的代价函数的加权因子。

步骤2，根据模型中时间、函数关系、约束、目标条件和变量，将路径规划 问题转化为航迹优化问题。

在代价函数表示中，地磁强度、转向角和障碍物约束等，量值往往不是同一 个数量级的，甚至可能相差好几个数量级，必然导致数量级相差较远的代价函数 对加权因子的依赖关系不敏感。因此，有必要对不同的代价函数进行归一化处理。 路径规划要求对评价函数取最小值，采取如下变换方法。

J’_intensity，J’_slope，J'_angle，J'_mon分别为归一化以后的地磁强度代价函数，坡度跟随代价函数，转向角代价函数，障碍物代价函数；

max(J_intensity)，max(J_slope)，max(J_angle)，max(J_mon)分别为所有节点的地磁强 度代价函数，坡度跟随代价函数，转向角代价函数，障碍物代价函数的最大值；

min(J_intensity)，min(J_slope)，min(J_angle)，min(J_mon)分别为所有节点的地磁强 度代价函数，坡度跟随代价函数，转向角代价函数，障碍物代价函数的最大值。

归一化处理后的航迹评价函数为：

fit(x_i,y_i)＝ω₁J’_intensity(x_i,y_i)+ω₂J’_slope(x_i,y_i)+ω₃J'_angle(x_i,y_i)+ω₄J'_mon(x_i,y_i) (14)

不难看出，路径规划的评价模型等价于多目标约束优化问题。将多个目标函 数通过归一化处理以及赋予加权因子，每个目标的权重可以在搜索过程中动态变 化。

步骤3，在步骤1和步骤2的理论基础上，采用Dijkstra算法+粒子群优化 算法的混合算法进行航迹规划，配合图2所示，将粗选航迹作为最优解的一个初 始估计输入，并对粒子群优化算法编码，将时间信息看作搜索空间中的“粒子”， 改善算法的收敛性能。

分析如下：

①Dijkstra算法

将节点分为两部分，一部分是临时标记节点集合S，一部分是未标记节点集 合U。初始时S只有起始点，以后每求出一条最短路径，则将节点加入集合S。 步骤如下：

Step1：初始化节点列表NodeList，初始化所有节点的权值列表matrix，调 用Insert操作初始化优先级队列Heap，将源节点入队列，同时创建Heap与 NodeList对应节点的索引Index。

Step2：抽取优先级队列中最短距离节点Node[j]，取队列头结点V_j，此时 Node[j]为起始点V₁到节点V_j的最短路径终点。若节点V_j是目标节点时，释放队 列Heap中剩余节点及其索引关系，算法结束。

Step3：将从节点V_j出发的邻接节点V_k加入队列，选择V_k的原则是 dist[k]＝min{matrix_1k,dist[j]+matrix_jk}，并相应更新索引表Index。若节点V_k已 出队列，则不作处理。

Step4：重复Step2，直到找到目标节点V_D。

②粒子群优化算法

设第i个粒子在搜索空间的位置表示为x_i＝(x_i1,x_i2,…,x_iD)，对应的粒子位置 改变的速度v_i＝(v_i1,v_i2,…,v_iD)。在第t迭代中，各粒子的位置和速度由下面的式 子进行更新：

其中，P_id为单个粒子经历过的最好位置，称为“局部最优值”。在D维搜 索空间，群体规模为m，则P_id是当前第i(i＝1,2,…,m)个粒子所获得的最好的适 应度函数值；G_id为群体中所有粒子找到的最佳位置，称为“全局最优值”。

粒子速度向量满足约束条件——动态系统Lipschitz条件。

式中，ω表示惯性权重，

表示第i个粒子d维分量在第t次迭代中的 速度和位置，c₁、c₂表示加速因子，r₁、r₂为[0,1]上均匀分布的随机数。

③惯性权重的改进

设I_max为最大迭代次数，ω_max和ω_min分别为最大惯性权重值和最小惯性权重 值。则第i次迭代的惯性权重值ω_i的计算公式为：

对惯性权重ω的线性变化过程分为两个部分，引入参数ω_λ：

式中，I_λ为转折迭代次数。此分段权重函数可以由用户根据实际情况切换全 局搜索和局部搜索，有效提高粒子群优化算法的灵活性。

④编码方式的改进

保持起始节点和目标节点不变，根据约束条件对粗选航迹中间节点的位置做 出优化调整，得出由新的航迹点V_i'(x_i',y_i')组成的优化航迹 L'₀＝{S V₁' V₂'…E}。调整策略如下：

将时间信息t_i作为粒子，在取值范围为[0,1]内进行搜索，使航迹评价函数最 小。其中t_i∈[0,1]，反映了新航迹点是在粗选航迹点周围进行搜索，而不是在整 个地磁图平面进行搜索，提高了搜索效率。

基于以上分析，步骤3包括如下内容：

Step1：初始化。

A.获取地图信息，确定起始节点和目标节点，利用Dijkstra算法得出粗选 航迹。

B.设置加速因子c₁，c₂，惯性权重ω，最大迭代次数k，适应度函数fit， 种群规模M。

C.初始化粒子群的位置信息x_i＝(x_i1 x_i2…x_iM)和速度信息 v_i＝(v_i1 v_i2…v_iM)。

Step2：将粒子中的时间信息代入式(19)计算坐标值，计算每个粒子的当前 适应度函数值fit(x_i',y_i')。

A.根据式(15)和(16)更新粒子当前的位置信息和速度信息。

B.根据每个粒子的位置信息和速度信息更新当前局部最优值P_id和全局最优 值G_id，与历史最优值比较，如果比历史最优值小，则更新P_id和G_id，记录全局 最优的粒子的位置信息。

Step3：输出当前最优解G_id。

Step4：判断是否达到最大迭代次数，是则算法结束，否则返回Step2。

以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围， 凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本 发明保护范围之内。

Claims (5)

1.一种适用于地磁辅助导航航迹规划的粒子群优化方法，其特征在于包括如下步骤：

步骤1，考虑水下环境约束条件在内，建立水下航行器路径规划评判模型；

所述水下环境约束条件包括地磁特征约束、最大转向角约束和障碍物约束，

其中，地磁特征约束表达式为：

式中，J_intensity为满足安全航行的地磁强度代价函数，f_safe为最小安全航行深度，f_mission为水下航行器航行时的离海底深度；

J_slope＝1-P(i,j)

式中，k、

是函数参数，f(i,j)是关于网格点位置(i,j)的地磁强度函数，μ为特征阈值，J_slope为坡度跟随代价函数；

最大转向角约束表达式为：

式中，θ_max为航行器的最大转角，θ为航行器的当前转向角，J_angle指航行器向左或者向右转到极限位置与不发生偏转时中心线所形成的角度；

障碍物约束表达式为：

式中，j为威胁源的个数，r＝[x,y]^T是位置向量，H为方差，Q_j表示第j个威胁源的威胁程度，H_j表示j取值为0～n-1中的某一数值时对应的

μ_j表示各个威胁源的二维坐标；

航迹规划评价函数表达式为：

min fit(x_i,y_i)＝ω₁J_intensity(x_i,y_i)+ω₂J_slope(x_i,y_i)+ω₃J_angle(x_i,y_i)+ω₄J_mon(x_i,y_i)

s.t.0＜ω₁＜1

0＜ω₂＜1

0＜ω₃＜1

0＜ω₄＜1

ω₁+ω₂+ω₃+ω₄＝1

式中，fit(x_i,y_i)为航行器进行路径规划的航迹评价函数，ω₁、ω₂、ω₃、ω₄分别为各约束条件的代价函数的加权因子；

步骤2，根据模型中时间、函数关系、约束、目标条件和变量，将路径规划问题转化为航迹优化问题；

步骤3，采用Dijkstra算法+粒子群优化算法的混合算法进行航迹规划，将粗选航迹作为最优解的一个初始估计输入，并对粒子群优化算法编码，将时间信息看作搜索空间中的“粒子”，改善算法的收敛性能；

粒子群优化算法中的惯性权重改进为如下形式：

其中，ω_i为第i次迭代的惯性权重值，I_max为最大迭代次数，ω_max和ω_min分别为最大惯性权重值和最小惯性权重值，I_λ为转折迭代次数，ω_λ为参数。

2.如权利要求1所述的适用于地磁辅助导航航迹规划的粒子群优化方法，其特征在于：所述步骤2中，对模型中不同的代价函数进行归一化处理，采取如下变换方法：

J'_intensity，J'_slope，J'_angle，J'_mon分别为归一化以后的地磁强度代价函数，坡度跟随代价函数，转向角代价函数，障碍物代价函数；

max(J_intensity)，max(J_slope)，max(J_angle)，max(J_mon)分别为所有节点的地磁强度代价函数，坡度跟随代价函数，转向角代价函数，障碍物代价函数的最大值；

min(J_intensity)，min(J_slope)，min(J_angle)，min(J_mon)分别为所有节点的地磁强度代价函数，坡度跟随代价函数，转向角代价函数，障碍物代价函数的最小值；

归一化处理后的航迹评价函数为：

min fit(x_i,y_i)＝ω₁J'_intensity(x_i,y_i)+ω₂J'_slope(x_i,y_i)+ω₃J'_angle(x_i,y_i)+ω₄J'_mon(x_i,y_i)

式中，fit(x_i,y_i)为航行器进行路径规划的航迹评价函数，ω₁、ω₂、ω₃、ω₄分别为各约束条件的代价函数的加权因子。

3.如权利要求1所述的适用于地磁辅助导航航迹规划的粒子群优化方法，其特征在于：所述步骤3包括如下具体步骤：

步骤31，初始化粒子群的位置信息x_i＝(x_i1 x_i2 … x_iM)和速度信息v_i＝(v_i1 v_i2 …v_iM)；

步骤32，计算每个粒子的当前适应度函数值fit(x'_i,y'_i)；

步骤33，输出当前最优解G_id；

步骤34，判断是否达到最大迭代次数，是则算法结束，否则返回步骤32。

4.如权利要求3所述的适用于地磁辅助导航航迹规划的粒子群优化方法，其特征在于：所述步骤31的具体内容是：

A.获取地图信息，确定起始节点和目标节点，利用Dijkstra算法得出粗选航迹；

B.设置加速因子c₁，c₂，惯性权重ω，最大迭代次数k，适应度函数fit，种群规模M；

C.初始化粒子群的位置信息x_i＝(x_i1 x_i2 … x_iM)和速度信息v_i＝(v_i1 v_i2 … v_iM)。

5.如权利要求4所述的适用于地磁辅助导航航迹规划的粒子群优化方法，其特征在于：所述步骤32的具体内容是：

A.根据下式更新粒子当前的位置信息和速度信息：

其中，P_id为单个粒子经历过的最好位置；在D维搜索空间，群体规模为m，则P_id是当前第i个粒子所获得的最好的适应度函数值，其中，i＝1,2,…,m；G_id为群体中所有粒子找到的最佳位置；

粒子速度向量满足约束条件——动态系统Lipschitz条件：

式中，ω表示惯性权重，

表示第i个粒子d维分量在第t次迭代中的速度和位置，c₁、c₂表示加速因子，r₁、r₂为[0,1]上均匀分布的随机数；

B.根据每个粒子的位置信息和速度信息更新当前局部最优值P_id和全局最优值G_id，与历史最优值比较，如果比历史最优值小，则更新P_id和G_id，记录全局最优的粒子的位置信息。

Images