粒子群算法在惯性/地磁组合导航航迹规划中的应用

2018-03-06王立辉孙德胜马明珠

中国惯性技术学报 2018年6期

乔楠，王立辉，孙德胜，马明珠，余乐

（1. 东南大学微惯性仪表与先进导航技术教育部重点实验室，南京 210096；2. 北京航天自动控制研究所，北京 100854；3. 宇航智能控制技术国家级重点实验室，北京 100854）

惯性/地磁组合导航利用惯性导航提供的位置信息，结合磁力仪测得的位置磁场信息，在地磁图上实现地磁特征匹配，惯性导航系统的解算轨迹误差可由匹配后的轨迹修正，从而实现匹配定位和导航。惯性/地磁组合导航应用中涉及的航迹规划方法是基于地磁基准库数据，利用航迹规划算法为航行器规划一条符合航行任务、机动性能、威胁规避等各类边界控制条件的，有利于开展组合导航应用的最短路径，是一个非确定性多项式的复杂问题[1]，无法用已知的多项式时间问题来求取最优解。

相对于启发式算法和随机型算法[2]，最优化算法[3]以航迹代价模型为适应度函数，根据粒子的进化和种群的演练，获得更好的优化性能，而最优化算法往往存在收敛速度慢、易陷入局部最优的缺点。

本文以提高航行器航迹规划效率和精度为目的，综合考虑地磁导航匹配区分布及各项环境约束因素，探讨一种采用粒子群算法的高精度的航迹规划方法，利用最短路径算法在地磁特征约束下进行航行器离线规划，将粗选航迹点作为粒子群优化算法的初始输入，采用粒子群优化算法改进编码方式，将时间信息作为搜索空间中的粒子，改善算法的收敛性能和全局寻优能力。

1 建立航迹代价评估函数

航行器航迹规划的目的是设计一条符合航行任务、机动性能、威胁规避等各类边界控制条件的最短路径，涉及到地磁特征代价函数Jterrain、坡度跟随代价函数Jslope、转向角代价函数Jangle、障碍物代价函数Jmon、航行长度约束Jlength等[2]，而各代价函数之间的量化关系很难确定。为了降低计算复杂度，采用简化的航迹代价函数计算公式[4]。赋予各代价函数不同权值，航迹评价函数模型为：

其中，fit(xi,yi)为航行器进行路径规划的航迹评价函数，ω1、ω2、ω3、ω4、ω5分别为各约束条件的代价函数的加权因子，各因子量值区间为(0,1)。地磁特征代价函数Jterrain，坡度跟随代价函数Jslope，转向角代价函数Jangle，障碍物代价函数Jmon，航行长度约束Jlength均经过归一化处理。

地磁特征代价函数Jterrain为满足安全航行的叠加地磁强度代价函数，用于防止航行器与海底发生撞击，设置有安全航行深度区间。地磁坡度越大，水下航行器的跟随越困难，当坡度比航行器的最大纵倾角还要大时，航行器的地磁跟随将受到限制，通常选择坡度跟随代价函数Jslope作为量化的威胁因素[5]。为避免急剧转弯引起的碰撞风险，最大转向角限制了航行器运行的轨迹，航行器的最大转角必须满足转向角代价函数Jangle。航行器在行进过程中，需要采用障碍物代价函数Jmon对障碍物进行有效规避。航行长度约束Jlength体现了从起点到目标点算法规划的航行路径长度和目标路径长度的约束关系。

2 最短路径算法实现航迹的粗规划

在满足航迹代价评估函数约束的前提下，最短路径算法按路径长度的次序递增，迭代产生最短路径，生成以固定的起点为树结构的根，根到各个节点的路径即为根到该节点的最短路径[6]。

将水下地磁信息网络分割为子网，建立各子网拓扑结构关系，体现了地磁空间网络数据的空间属性特征。地理网络信息为节点与弧段构成的拓扑结构点群。网络节点间不存在负权值，由最短路径生成树的过程中对是否加入新的节点取决于新节点到根的距离以及节点间的邻接关系，任两点间的权值就是该路径上所有边的权值总和。最短路径算法按不递减次序依次计算出长度最小的路径，然后是长度次小的最短路径，最后是长度最大的最短路径。最短路径算法将节点分为两部分[7]，一部分是临时标记节点集合S，一部分是未标记节点集合U。初始时S只有起始点，以后每求出一条最短路径，则将节点加入集合S。

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

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

步骤 3：将从节点Vj出发的邻接节点Vk入队列，选择Vk的原则是：

disk[k]=min{matrix1k,dist[j]+matrixjk}，

并相应更新索引表Index。若节点Vk已出队列，则不作处理。

步骤 4：重复步骤2，直到获得目标节点VD。

最短路径算法的运算效率与航迹代价数值、邻接矩阵、适配区分布、节点数目等因素有关，耗时较大，无法达到水下航行器实时性的要求。最短路径算法适用于在局部海域范围内，对水下地磁导航路径信息进行推算，实现粗选航迹的规划，所求得的粗航迹作为最优解的一个初始估计，用于后续的航迹路径优化，逐步优化有限迭代序列，提高全局规划能力。

3 粒子群算法实现航迹优化

粒子群优化算法采用群体智能的随机搜索方法，可以弥补最短路径算法的不足[8]，运算过程具有并行性、收敛速度快等优势，可以与最短路径算法优势互补，实现路径优化求解算法。

粒子群优化算法中所有的粒子都有一个被优化函数决定的适应值，且粒子在搜索空间由速度向量来决定运动的速度和方向。粒子间的信息传递和相互作用，引导粒子群向目标解移动，通过迭代求解空间的目标解。粒子群优化算法根据其特有的记忆功能可以动态跟踪当前的搜索情况并做出相应的调整[9]，每一次迭代过程，每个个体都能够存储当前位置，并找到最佳位置，且通过跟踪两个极值来更新。一个是单个粒子经历过的最好位置，称作“局部最优值”，记为Pid，在D维搜索空间，群体规模为m，则Pid是当前第i(i=1,2,,m)个粒子所获得的最好的适应度函数值；另一个是群体中所有粒子找到的最佳位置，称为“全局最优值”，d=1,2,,D，记为Gid。

第i个粒子在搜索空间的位置表示为xi=(xi1,xi2,,xiD)，对应的粒子位置改变的速度vi=(vi1,vi2,,viD)。在第t迭代中，各粒子的位置和速度由式(2)和式(3)进行更新：

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

其中，ω为惯性权重，能够动态调整粒子运动速度，使粒子趋于局部收敛；vt、xt为第i粒子d维分量在idid第t次迭代中的速度和位置；c1、c2为加速因子，取非负值，控制每个粒子向位置Pid和Gid运动的速度快慢；r1、r2为随机数，服从区间[0,1]上的均匀分布；vid∈[ -vmax,vmax]，常数vmax是速度最大值，可由用户根据实际情况设定；Vmax为速度向量的最大常数。

3.1 惯性权重的改进

惯性权重ω作为线性变化的系数引入粒子群优化算法，惯性权重系数能够平衡全局搜索和局部搜索，合适的惯性权重值能够以最少的迭代次数搜索到最优解。一个搜索性能优秀的粒子群优化算法，在运行初期应当具有较大的惯性权重值，从而保证算法的全局搜索能力。随着迭代次数的递增，惯性权重减小，以较小的权重值来提高算法的局部搜索能力。

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

或

对初始化过程的惯性权重进行约束，有利于算法快速定位至全局最优解[8,10]，在少数次迭代后，快速减小权值使算法进入局部搜索阶段，对惯性权重ω的线性变化过程分为两个部分，其数学描述为：其中，Iλ为转折迭代次数。此分段权重函数可以根据实际情况切换全局搜索和局部搜索，有效提高粒子群优化算法的灵活性。

3.2 编码方式的改进

由粗选航迹点组成的折线，即粗选航迹L0={S,V1,V1,,E}，其中，S为起始节点，E为目标节点，Vi为折线的端点，各端点分别对应S（x0,y0）,V1（x1,y1）,E（xD,yD）。保持起始节点和目标节点不变，根据约束条件对粗选航迹中间节点的位置优化调整，得出由新的航迹点Vi′（xi′,yi′）组成的优化航迹L0′={S,V1′,V2′,,E}。调整策略为：

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

4 仿真验证与分析

利用Matlab程序构建某海域的地磁强度特征图如图1所示，平均地磁强度为1831.4016 nT，最小地磁强度为1344 nT，最大地磁强度为2329 nT，忽略测量误差。地磁强度为经度范围66°N～66.0357°N，纬度范围78°E～78.0523°E，以栅格的形式存储，网格数为44×44，网格间距0.00083°，约为90 m。

图2为该海域地磁图适配区分布，阴影部分代表不可匹配区，空白部分代表适配区。确立航迹规划的任务为以S点为起始节点，E点为终止节点，规划出一条最优航迹，利用Dijkstra算法规划出航迹如图2所示。

图1 地磁强度三维视图Fig.1 Three-dimensional view of geomagnetic intensity

由于Dijkstra算法的遍历所有节点，其时间复杂度为O(n2)，在节点数目很大时，耗时较大，无法达到水下航行器实时性的要求。在所选进行航迹规划的海域范围不是很大时，Dijkstra算法适用于运行前对水下地磁数据库的地磁信息进行分析，完成粗选航迹规划。

图2 适配区分布及粗选航迹Fig.2 Distribution of adaptation area and primary selection of trajectory

将粗选航迹作为粒子群优化算法的粗选航迹，获得粒子群优化算法航迹，如图3所示。粒子群的参数设置如表1所示。

绘制标准粒子群算法和混合粒子群算法下的适应度对比图如图4所示。标准粒子群算法的初始适应度函数值比混合算法的适应度函数值更大，这是由于标准粒子群算法的搜索空间是整个平面，初始函数值更大。随着迭代次数的增加，两种算法都达到全局最优值，标准粒子群算法适应度达到49.211，混合算法的适应度达到43.304，混合算法的适应度函数值更加优化，所需代价更小。混合算法在保证全局最优的同时也保证了局部最优，可以有效规避非适配区。

表1 粒子群算法参数设置Tab.1 Parameter settings of particle swarm optimization algorithm

图3 航迹规划对比结果Fig.3 Comparison results of track planning

图4 适应度对比图Fig.4 Comparison of fitness

由图3直接对规划的航迹进行分析可得：从起始节点S到目标节点E，由混合算法求得的航迹明显比由 Dijkstra算法求得的粗选航迹更加优化。相对于由Dijkstra算法求得的航迹，混合算法优化了转弯处的节点，使得所求得的航迹长度更短。标准粒子群算法的搜索空间是整个平面，初始函数值更大，其初始适应度函数值比粒子群优化算法的适应度函数值更大。随着迭代次数的增加，粒子群优化算法的适应度函数值更加优化，所需代价更小，在保证全局最优的同时也保证了局部最优，可以有效规避非适配区。