摘  要：针对复杂海底环境中的潜器路径规划问题，提出了一种采用量子粒子群算法的潜器路径规划方法。该方法首先对从海图中提取的水深数据，基于自然邻点插值和随机中点位移插值得到密集规格水深数据，然后由此数据建立海底三维模型，确定一个路径安全性检测方案及避碰原则，将海流大小方向对潜器航行的影响和路径点的转弯角度对航行的影响转化为相应的路径长度。最后将总长度作为适应度函数，利用量子粒子群算法迭代来求取最优路径。仿真结果得到了一条安全、简洁的路径，验证了该方法的有效性和可行性。

关键词：量子粒子群算法；海图；潜器导航；路径规划

Autonomous underwater vehicles path planning by using QPSO

Abstract: To the submersible path planning problem in complex undersea environment, the submersible path planning method is put forward ,which is based on quantum-behaved particle swarm optimization.First, the bathymetric data whice is extracted from the  chart is dealed with the natural neighbor interpolation and the random midpoint displacement interpolation to get the intensive specifications depth data, then we can  establish the undersea 3D  model and determine a path  security testing program and the prevention of collisions principle,  the  influences of the ocean currents size and direction to submersible navigation and the turning angle of the path points to navigation are transformed into the corresponding path length.The total length is used as the fitness function. Last the optima l path is obtained of iteration of quantum-behaved pa rticle swarm optimization(QPSO). The safe and simple path is got from the result of simulation. Effectiveness and feasib ility of the method is verified. 

Keywords: quantum-behaved particle swarm optimization; chart ;  submersible navigation; path planning

水下潜器路径规划是潜器智能化航行研究的一个重要课题。智能算法作为一种新型优化工具在此领域得到了广泛的应用[1-3]。孙俊等受量子物理理论启发提出了量子粒子群算法[4-6]，该算法作为一种改进的粒子群算法具有更强的寻优能力和更快的收敛速度。量子粒子群算法在进化过程中没有速度分量，这使得粒子进化方程形式变得更加简单，而且参数更少更容易控制，作为一种优化工具已在众多领域得到应用。本文利用量子粒子群算法对潜器路径进行寻优。首先基于海图水深数据建立海底模型，然后综合考虑影响航行的各个因素得到合适的适应度  函数，并设计算法迭代步骤，最后进行仿真实验。

1  基于海图数据建立海底模型

为了更好地模拟真实海底环境，从电子海图中提取水深点数据，并对数据进行加工处理，最终依据海底标准网格点的水深数值建立海底三维模型。

1.1  水深数据的提取 选取某海域电子海图作为研究对象，确定潜器路径规划区域的经纬度范围，遍历与此区域有交集的所有海图的水深图层，读取并储存此区域内所有水深点的数据，由此得到潜器路径规划区域内各水深点的坐标及水深数值。 由于从海图中得到的水深数据为无规律的离散数据点，为了便于计算和存储，需要对原始数据进行规格插值，利用文献[7-8]中介绍的基于自然邻点插值方法，将得到的无规律离散数据点转化为等间距标准网格数据。

1.2  对规格数据进行加密处理

由于海图中的水深数据点比较稀疏，因而基于自然邻点Laplace插值得到的标准网格数据点仍然比较稀疏，仍不能满足精确导航的要求，故需要对 1.1中得到的数据进行加密处理，利用改进的随机中点位移法插值[9]对数据点进行加密处理。由于文献[9]中介绍的菱形- 方形细分法在进行插值时，只有方形中心点插值考虑了四周点对它的影响，而对于边中心点插值只考虑其所在的边的端点值对其的影响，这使得处理后的数据只在单一方向（横向或纵向）上比较连续，而在另一方向上容易形成断层，具有一定局限性。对标准网格数据的加密处理时，基于文献[9]的算法进行了改进，对得到的规格数据按如下步骤处理。 1 ）将1.1 得到的规格网格数据记为水深数据集1 ，按式（1 ）计算所有规格数据网格中心点位置的水深数值，将得到的这些位置点数据加入到水深数据1 中得到水深数据 2 。 04/ D H H H H Hd c b a m+ + + + = ） （    （1 ） 式中： d c b aH H H H ， ， ， 为插值点周围4 个点的水深值，0D 为随机偏移量。 2 ）水深数据集 1 中非边界边的中点均为水深数据集2 中一个正方形的中心点，即此边的两端点与该点相邻的2 个网格中心点构成正方形。此时该类点也可以由式（1 ）计算得到其水深数据值。 3 ）对于边界上点，由于它仅有 3 个方向存在相邻水深数值点，因而其只能看作长方形一条边的中点，将计算公式改为式（2 ）进行计算。 *03/ D H H H Hc b a m+ + + = ） （        （2） 通过改进，除边界上的点外其他所有的插值点在进行插值时均考虑了4 个方向数据点数值对其的影响，考虑了点在各个方向的连续性，使得地形曲面更加连续平滑。 进行一轮这样的插值就能将原来的nn´ 数据加密为(2 1) (2 1) nn-´ -数据。进行多轮这样的插值，直到达到满意的数据点密度。利用 MATLAB将得到的数据绘成海底三维地形图，如图1 所示。  图1 海底三维模型 Fig.1 Seabed of three-dimensional model

2  模型处理

2.1  海底模型处理

为了方便模型的处理，在此模型中将潜器看作一个质点。但为了保证潜器安全航行，考虑到潜器的实际大小及模型水深数据的精度，对所有海底水深进行减少10 米处理，得到一个新的海底模型，因此潜器可以贴着新的海底安全航行，即在规划中只要潜器路径点深度不大于该点新海底水深数据就视为安全。

2.2  潜器航行过程中海流的影响 潜器在航行过程中不可避免地受到海流的影响，在航海上，一般将海流分成潮流、定海流、风生流等[10]。由于定海流具有流向、流速固定性以及其他海流的不确定性，因此本文在进行静态路径规划时仅考虑定海流对潜器航行的影响。 定海流对潜器航行的影响与其流速v 及航行方向与海流流向的夹角j 均有关。顺海流方向航行有利于减少航行能量消耗，逆海流方向航行则需要更大的能耗，并且流向与航向所在直线夹角越大则产生的力矩越大，越不利于潜器控制。为了更好地在潜器路径规划中体现海流情况对路径规划的影响，根据海洋运载工具动力学规则波阻力分析，定义一个简化的海流影响效益路径表示函数，将海流对潜器航行的影响转化为相应路径长度的增减。其函数表达式为 ) sin cos ( j j c b l v a ci i- × × = .     （3） 式中：a 表示海流-路径转化系数，b 表示海流在潜器航行方向分量的影响系数，c 表示海流在潜器航向垂直方向分量的影响系数。本文中参数a 取0.15，b取0.9，c 取1.3。

2.3  潜器航行过程中角度限制及影响 考虑到潜器的灵活性，规划路径中路径点拐角不能大于潜器允许的最大转弯角度[1]，设第3 期  邹梅魁，等：采用量子粒子群算法的潜器路径规划  ·3·    111 (, ,) iiiiiii x xyyzz --- =- - - l 为第i 路径的向量，若潜器允许的最大转弯角为maxq ，则路径点拐角应满足式（4 ）： T1max1cos( )||| |iiiiq++£×llll， ) ,... 2, 1 ( n i =   （4 ） 此外考虑到潜器在转弯过程中，需要提供额外的动力且水平方向与竖直方向的转弯动力不一样，角度越大所耗能量越大。为了评价航行轨迹曲度对优化路径的影响，根据潜器动力学分析，定义一个路径曲度惩罚函数，将每次转弯能耗转化为相应的路径长度，曲度惩罚路径表示函数为： itan( ) tan( )ii i wAla qb J =×× × +× （）    （5）                式中：A 为角度-路径转换系数，a 为水平方向转弯成本系数，iq 为水平转弯角，b 为竖直方向转弯成本系数，iJ 为竖直转弯角。本文中参数 A 取0.2，a取0.2，b 取2 。

2.4  规划路径安全性要求 为了保证潜器安全航行，潜器路径规划中的每一段路径线段必须避免与海底面相碰撞。设111 (, ,) iiiiiii x xyyzz --- =- - - l 为第i 段路径线段。[]} , s Z {s1 i ix x S-Î Î = ，当x 取值 S xsÎ 时，由直线上点的坐标公式计算可得路径中对应点的坐标） （s s sz y x , , 。若路径线段不与海底曲面相交，必须使路径中的每一点位置深度小于该点的海底水深。由于在海底模型中仅知道标准网格上点的水深，因而只能将路径线段上点的坐标与相邻的标准网格点水深作比较。设 ) (1 s sy fix y = ， 1 ) (2+ =s sy fix y ，其中) (sy fix 为sy 的整数部分。若对任意 S s Î 均满足) , (1s s sy y x x h z = = < ，且 ) , (2s s sy y x x h z = = < ，则该路径线段与海底无交点。

2.5  适应度函数 根据上述分析，由于要综合考虑路径长度、海流效益、路径点曲度三者来规划一条最优路径。因此定义一个综合路径长度f ，其值为粒子的总路径长度、总曲度惩罚路径表示函数值及总海流效益路径表示函数值三者之和。由于在式(3)和式(5)中，海流影响效益和路径点曲度惩罚已转化为相应的路径长度值，因而总路径长度即为 å å å-= = =+ + =10 1 1niiniiniiw c l f .          （6 ） 式中：212121) ( ) ( ) (- - -- + - + - =i i i i i i iZ Z Y Y X X l 为第i 段路径长度，ic 为第i 段路径海流效益路径转换值， iw为路径点i 的转弯成本路径转换值。

3  导航算法及步骤

3.1  量子粒子群算法 在PSO 算法中，粒子的运动状态由位置和速度描述，随着时间的演化，粒子的运动轨迹是既定的；同时粒子的速度受到一定的限制，使得粒子的搜索空间是一个有限的并逐渐减小的区域，不能覆盖整个可行解空间，从而PSO 算法不能保证全局收敛[11-12]。针对PSO 算法的这个缺点，孙俊等受量子物理理论启发提出了一种改进的粒子群优化算法—量子粒子群算法（quantum-behaved particle swarm optimization, QPSO）。其粒子进化方程为： (1) () ()ln[1/()] ij ij j ij ijtp t t ut a += ± - × XCX   （7 ） () () [1 ()] ()ij j ij j jptt tt ff =×+- × PG     （8 ）11() ()NjijittN == å CP             （9 ） 式中：,1 , 2 ,[, ,, ],1,2,,. iiiiN tXX Xi M ==LL （） X 为第i个粒子t 时刻的位置，N 为搜索空间为数，M 为粒子群体个数。 ) 1, 0( ~) (Ut jj ， ) 1, 0( ~ ) ( U t uij ， ()jt C 为粒子的平均最好位置，()it P 为粒子i 个体最优解， () t G为群体最优解，a 为收缩- 扩张系数（CE coefficient ），它是算法除群体规模和迭代次数以外的惟一控制参数，对参数a 控制可以采用固定取值和线性减少的方式[13]，在此本文参照文献[13] 中方法，取a 线性递减为 min max min max max=+(-)(-)/ TtT a aaa´ 。其中取mina =0.5 ，maxa =1，maxT 为最大迭代次数，t 为当前迭代次数。

3.2  导航算法步骤 1 ）确定路径起点 ) , , (0 0 0 0Z Y X P 和目标点) , , (n n n nZ Y X P 。对海底模型环境进行分割处理，连接线段 nP P0 并对其进行n 等分，并在每个等分点处作垂直X 轴的平面并记为 ) 1 ,..., 2, 1 ( - = Õ n PP 。共选取N 个粒子进行T 次迭代计算来求取最优路径。对任意粒子i 而言，其位置为在各平面上均取一点所组·4·  智   能   系   统   学   报   第8 卷  成的点集{}） （ ） （1 1 1 1 1 1, , , , , ,- - - n n nz y x z y x L 。粒子i 的初始位置定义为在任意平面P 上均随机选取一个初始位置点 ) 0(iPX ，设为 (,,) X YZ 。由于该平面上所有点的X 值相同，则相当于随机定义一个二元数(, ) YZ 。对该位置点高度值Z 进行检验，判断Z 值是否大于(,) X Y 点处的水深值，若不满足则舍去，并对该点位置继续进行随机选择，直到所有粒子在每个平面上的初始位置都大于所在点的水深。此时粒子迭代次数为 1 t = 。 2 ）对任意粒子，连接该粒子在相邻平面上的位置点即构成了该粒子搜索的路径线段，检查此路径线段是否满足2.4节中的路径安全性要求，若不满足安全条件则将路径中与海底相交部分的线段端点进行变异处理：将线段端点坐标中Z 值各增加一个单位长度。再次对其进行安全性判断，若仍不满足安全条件则继续进行端点位置变异处理，直到所有路径线段均满足路径安全性要求。 3 ）计算每个粒子i 搜索路径中各线段的长度j il, ，各线段与海流夹角 j i ,j 各路径点的转弯角 j i ,q ，并按公式求得粒子i 的路径长度il ，转弯影响值iw ，海流影响值 ic ，从而求得各粒子的适应度值[()]if t X 。 4 ）由式（9 ）计算粒子群的平均最好位置。将每个粒子i 的当前位置 ()it X 的适应度值与前一次迭代 (1) it - P 的适应度值比较（ (0) (0)ii= PX），如果[()] [(1)] ii ftft <- XP则置 () ()iitt= PX；否则，() ( 1)iitt=- PP 。将 ()it P 的适应度值与全局最好位置(1) t - G 的适应度值作比较，若 [()] [( 1)] ift ft <- PG 则置() ()itt= GP；否则，() ( 1) tt=- GG 。 5 ）对粒子 i 的每一维，由式（8 ）计算得到一个随机点的位置，由式（7）计算更新各个粒子的位置。 6 ）判断迭代次数 t 是否大于T ，是则结束搜索，否则令 1 tt=+ 并返回2 ）对粒子继续进行算法迭代。

4  仿真实验及结果分析

4.1  仿真实验 基于上述算法，设潜器出发点为A (0,0,-60) 目标点为B (100,100,-70), 航行区域有东偏北25°速度为0.12 m/s 的水平定海流。将线段AB十等分，然后再每个等分点上作X 轴的垂平面，共9 个平面，即每个粒子均为一个二维数组X [2][9] ，数组第 1 维表示9 个路径节点的 Y 坐标，第2 维表示9 个路径节点的Z 坐标。使用量子粒子群算法进行寻优，取粒子种群数为40，迭代次数为 1 000。算法最佳个体适应度变化如图2 所示。  图2 最佳个体适应度值 Fig.2 Best individual fitness value 实验结果得到的路径点位置为（10，18.3 ，-60），（20，37.3 ，-61.1），（30 ，51.6,，62.2 ），（40，60.5 ，-63.4），（50，64.5 ，-64.7），（60 ，67.8 ，-65 ），（70，69.3 ，-61.3），（80，73.5 ，-61.7），（90 ，86.7 ，-65.8）。潜器运行总路径长度为136.47 km。实验仿真如图3和4 所示。  图3 仿真效果正视图 Fig.3 Simulation effect diagram  图4 仿真效果俯视图 Fig.4 Simulation effect overlook diagram

4.2  实验结果分析 由于实验所应用的量子粒子群算法采用仅有位移的模型，模型较粒子群算法更加简单，更适合高维情况下的寻优，并且量子粒子群算法的控制参数第3 期  邹梅魁，等：采用量子粒子群算法的潜器路径规划  ·5·    更少更容易控制。 利用参考文献[13-14]中的方法，利用蚁群算法对水下潜器进行三维空间路径规划，其仿真效果如图5 所示。  图5 蚁群算法效果 Fig.5 Effect diagram of the ant colony algorithm 比较2 种方法可知：本文的模拟地形图更加接近实际地形，地形的划分更加精细，路径规划精确度更高；由于文献中的方法水下潜器路径为贴着海底航行，路径曲率较大，对潜器的灵活性能要求比较高，安全性能也比较差。因而量子粒子群算法能得一条从起点到终点更加安全的安全、简洁的规划路径。 在本文的实验环境中，考虑了定海流及路径曲度对潜器规划路径总长度的影响。如果在试验中不考虑这些因素的影响，由于海底环境的特殊性，其规划范围可看成一个长方体空间，该空间的长宽为km 级，而高度为m 级。若只考虑单纯总路径长度，其结果将为沿着起点与终点的连线，向上避开海底障碍而得到的一条最短路径，在不考虑海流及路径曲度影响时，其仿真结果如图6 所示。  图6 仿真效果 Fig.6 Simulation effect diagram 5  结束语 文章通过对从海图中提取的水深数据进行处理建立更加真实的海底三维模型，利用量子粒子群算法进行水下潜器在海底三维模型中的路径寻优，仿真结果表明量子粒子群算法能较好地实现水下潜器全局路径规划。本文仅就已知海底环境进行全局路径规划，对于复杂的充满动态障碍物的海洋环境下实时的潜器导航还有待进一步研究。

    参考文献： [1]  于飞，唐小勇，潘洪悦．改进粒子群算法在三维水下导航规划中的应用[J]．北京理工大学学报, 2010, 30(9): 1059-1063. YU Fei, TANG Xiaoyong, PAN Hongyue. The application of an improved PSO to the submersible path-planning[J]. Transactions of Beijing Institute of Technology, 2010, 30(9): 1059-1063. [2]  唐小勇，于飞，潘洪悦．改进粒子群算法的潜器导航规划[J]．智能系统学报, 2010, 5(5): 443-447. TANG Xiaoyong, YU Fei, P AN Hongyue. Submersible path-planning based on an improved PSO[J]. CAAI Transactions on Intelligent Systems, 2010, 5(5): 443-447. [3]  赵百轶，张利军，贾鹤鸣．基于四叉树和改进蚁群算法的全局路径规划[J]．应用科技, 2011, 38(10): 23-28. ZHAO Baiyi, ZHANG Lijun,  JIA Heming. Global path planning based on quadtree and improved ant colony optimization algorithm[J]. Applied Science and Technology, 2011, 38(10): 23-28. [4]  SUN Jun, XU Wenbo, FE NG Bin. A global search strategy of quantum-behaved  particle swarm optimization[C]//Proceedings of the IEEE Conference on Cybernetics and Intelligent Systems. Singapore, 2004: 111-116.  [5]  SUN Jun, FANG Wei, WU Xiaojun, et al. Quantum-behaved particle swarm optimization: analysis of the individual particle's behavi or and parameter selection[J]. Evolutionary Computation, 2012, 20(3): 349-393. [6]  CHAI Zhilei, SUN Jun, CAI Rui, et al. Implementing quantum-behaved particle sw arm optimization algorithm in FPGA for embedded real-time applications[C]//Proceedings of the 2009 Fourth Internatio nal Computer Sciences and Convergence Information Technology. Washington, DC, USA: IEEE Computer Society, 2009: 886-890. [7]  申浩，田峰敏，赵玉新．基于VoronoiCells 插值的三维海底地形图生成方法[J]．系统仿真学报, 2006, 18(2): 444-446. SHEN Hao, TIAN Fengmin, ZHAO Yuxin. Method for generating 3D digital map based upon VoronoiCells[J]. Journal of System Simulation, 2006, 18(2): 444-446. [8]  姜辉．水下潜器路径规划仿真平台的设计与实现[D] ．哈尔滨：哈尔滨工程大学, 2009. JIANG Hui. Design and implem entation of the simulation platform for underwater vehicle path planning[D]. Harbin: Harbin Engineering University, 2009. ·6·  智   能   系   统   学   报   第8 卷  [9]  梁俊，王琦，刘坤良，等．基于随机中点位移法的三维地形模拟[J]．计算机仿真, 2005, 22(1): 213-215. LIANG Jun, WANG Qi, LIU Kunliang, et al. 3D terrain simulation based on the method of random mid-point displacement[J]. Computer Simulation, 2005, 22(1): 213-215. [10]   毛宇峰，庞永杰．改进粒子群算法在水下机器人路径规划中的应用[J]．计算机应用, 2010, 30(3): 789-792. MAO Yufeng, PANG Yongjie. Application of improved particle swam optimi zation in path planning of underwater vehicles[J]. Journal of Computer Applications, 2010, 30(3): 789-792. [11]   VAN DEN BERGH F, ENG ELBRECHT A P. A new locally convergent particle swarm optimizer[C]//IEEE International Conference on Systems, Man and Cybernetics. Hammamet, Tunisia, 2002, 3: 94-99. [12]   SUN Jun, FANG Wei, WU Xiaojun, et al. Quantum-behaved particle swarm optimization: analysis of the individual particle’s behavi or and parameter selection[J]. Evolutionary Computation, 2012, 20(3): 349-393. [13]   WARREN C W. A technique for autonomous underwater vehicle route planning[J].  IEEE Journal of Oceanic Engineering, 1990, 15(3): 199-204. [14]   VASUDEVAN C, GANESAN L. Case -based path planning for autonomous underwater  vehicles[J]. Autonomous Robots, 1996, 3(2): 79-89.