﻿ 重于水型AUV模糊路径规划与优化
 舰船科学技术  2017, Vol. 39 Issue (12): 76-80 PDF

Fuzzy path planning and optimization of a heavier-than-water AUV
YU Jiong-min, WU Chao, ZHAO Min, GE Tong
School of Naval Architecture, Ocean and Civil Engineering, Shanghai Jiaotong University, Shanghai 200240, China
Abstract: In order to achieve the goal of local path planning of a Heavier-Than-Water AUV in unknown underwater environment. A matlab kinematic model of the AUV was built based on its equation of motion and lift characteristics. A fuzzy controller was designed that enables AUV to plan path in real time. The controller takes obstacles' distance and orientation obtained from the sonar as input to control the AUV. At last, genetic algorithm was used to optimize the membership function of the fuzzy controller for improving energy utilization. And simulation tests showed that AUV consumed less energy during obstacle avoidance stage after optimization, which proved that the path planning method has practical value.
Key words: Heavier-Than-Water AUV     fuzzy path planning     genetic algorithm     obstacle avoidance
0 引　言

1 模型 1.1 重于水型AUV模型

 图 1 “飞鱼”Ⅱ型AUV Fig. 1 Flying-fish Ⅱ AUV

 图 2 坐标系 Fig. 2 Coordinate system

 ${\mathit{\boldsymbol{M}}}\dot v + {\mathit{\boldsymbol{C}}}(v)v + {\mathit{\boldsymbol{D}}}(v)v + g(\eta ) = \tau\text{，}$ (1)

η为大地坐标系下AUV的位置和欧拉角信息；ν为体坐标系中的（角）速度信息；τ为输入的力（矩）。其中，ην之间存在转换关系 $\dot \eta = {\mathit{\boldsymbol{J}}}(\eta )v$ ${\mathit{\boldsymbol{J}}}(\eta )$ 为大地坐标系与体坐标系之间的映射矩阵，表示为式（2），具体形式参考文献[9]。

 ${\mathit{\boldsymbol{J}}}(\eta ) = \left[ {\begin{array}{*{20}{c}}{{J_l}(\varphi ,\theta ,\psi )}&{{0_{3 \times 3}}}\\{{0_{3 \times 3}}}&{{J_a}(\varphi ,\theta ,\psi )}\end{array}} \right] \text{。}$ (2)

“飞鱼”Ⅱ型AUV在水中重力 P 大于浮力 B，且方向总是铅锤的，故在大地坐标系中分量为 $[0,0,P - B]$ 。根据大地坐标系 ${{\{ E\} }}$ 到体坐标系 ${\rm{\{ }}B\}$ 的转换矩阵 ${\mathit{\boldsymbol{T}}}$ （具体参考文献[10]），将其转换到体坐标系中，表示为静力项 $g(\eta )$ ，见式（3）。其中，浮心坐标为 $({x_C},{y_C},{z_C})$ ，重心坐标为 ${x_G} = {y_G} = {z_G} = 0$ （已省略）。

 $g(\eta ) = \left[ {\begin{array}{*{20}{c}}{ - (P - B)\sin \theta }\\{(P - B)\cos \theta \sin \varphi }\\{\left( {P - B} \right)\cos \theta \cos \varphi }\\{ - {y_C}B\cos \theta \cos \varphi + {z_C}B\cos \theta \sin \varphi }\\{{z_C}B\sin \theta + {x_C}B\cos \theta \cos \varphi }\\{ - {x_C}B\cos \theta \sin \varphi - {y_C}B\sin \theta }\end{array}} \right] \text{。}$ (3)

“飞鱼”Ⅱ型的机翼布置于AUV重心正上方，在运动中将产生升力，在体坐标系中的分量表示为 $[0,0,L]$ ，再将其加入到式（1）的输入项τ中。升力L由式（4）给出。其中，升力系数CL估算公式由式（5）给出， ${C_{L,0}}$ 是攻角为0时机翼的升力系数。 ${C_{L,\alpha }}$ 为不考虑机翼和艇体干扰的升力系数随攻角的变化率。根据机翼面积 $S{}_W$ 和潜水器的主尺度l，可将其转换为考虑相互干扰的升力系数变化率 ${\hat C_{L,\alpha }}$

 $L = \frac{1}{2}\rho {v^2}S{}_W{C_L} \text{，}$ (4)
 $\left\{ \begin{array}{l}{C_L} = {{\hat C}_{L,\alpha }}(\alpha - {\alpha _0}) + {C_{L,0}} \text{，} \\{{\hat C}_{L,\alpha }} = \frac{{{S_w}}}{{{l^2}}}{C_{L,\alpha }} \text{。} \end{array} \right.$ (5)

 $\dot v = - {{\mathit{\boldsymbol{M}}}^{ - 1}}\tilde {\mathit{\boldsymbol{D}}}(v)v + {{\mathit{\boldsymbol{M}}}^{ - 1}}(\tau - g(\eta )) \text{。}$ (6)
1.2 声呐模型

 图 3 避障声呐 Fig. 3 Obstacle avoidance sonar
2 模糊控制器计与优化

AUV在水下的低速运动可以解耦为水平面和垂直面2个平面空间的运动，本文设计的模糊控制器实现的是水平面内的局部路径规划，AUV的深度由单独的深度控制器控制，拟采用S面控制器[12]。模糊控制器的输出为垂直舵的舵角和螺旋桨转速。模糊控制器的主要结构包括：模糊化模块、知识库、模糊推理模块和清晰化模块4部分，如图4中虚线框部分。图4为遗传算法优化的模糊路径规划系统。

 图 4 模糊路劲规划系统 Fig. 4 Fuzzy path planning system

 图 5 模糊隶属度函数 Fig. 5 Membership function
2.1 模糊语言变量定义

1）声呐获取的3个方向的障碍物距离信息 ${d_i}(i = 1,2,3)$ ，模糊变量定义为：左方距离（DisLeft），前方距离（DisFront），右方距离（DisRight），各包含{Near, Far}2个模糊集合。

2）目标方向（Direction）表示目标位置和体坐标原点连线与AUV首向的夹角 ${\psi _d}$ ，如图2所示，包含{Left, Middle, Right}3个集合。

3）AUV当前的“速度（Speed）”包含{Slow，Fast}2个集合。

“舵角（Rudder）”和螺旋桨的“转速（RPM）”，为了实现对输出量较为精细地控制，舵角和转速分别划分为5个模糊集合，分别为{BRight, SRight, Middle, SLeft, BRight}和{NB, NS, ZZ, PS, PB}。

2.2 隶属度函数设计

AUV具有转向半径较大和欠驱动等特点。所以，需要根据其特点专门设计隶属度函数和模糊规则库。设计如下：

1）语言变量“距离”的隶属度函数采用不对称的设计，形状为梯形，论域为[–1，1]，如图5（a）所示，整体偏向右侧，即AUV更加倾向于将障碍物距离判断为Near，保证其与障碍物之间留有足够的安全距离。但是这样做也可能导致规划路径变长、能耗增加，因此后面将对隶属度函数进行优化。

2）目标方向隶属度函数为对称设计，障碍物在首向左侧为负，右侧为正，如图5（b）所示。

3）速度隶属度函数如图5（c）所示。

4）输出量舵角（Rudder）和转速（RPM）的隶属度函数形状一致，皆如图5（d）所示。

2.3 模糊控制规则库

2.4 遗传算法优化

 图 6 遗传算法流程图 Fig. 6 Genetic algorithm flowchart

1）初始种群生成。首先产生包含30个个体的“种群”，个体的基因选择为隶属度函数图5中标注拐点处的横坐标值，表示为向量 $M = [{M_1}\text{,}{M_2}\text{,}{M_3}\text{,}{M_4}\text{,}{M_5}\text{,}$ ${M_6}\text{,}{M_7}\text{,}{M_8}\text{,}{M_9}\text{,}{M_{10}}\text{,}{M_{11}}]$ 。基因M为实数向量，代表一个个体，对应着一种模糊控制器。初始种群的生成是在设定的定义域内随机产生实数向量M

2）仿真测试。调用每个个体设置模糊控制器，控制AUV进行路径规划，并返回AUV的运行结果。

3）计算适应度。在单个个体控制AUV进行避障路劲规划后，选取推进器在运动期间的总旋转圈数（简称总转数）作为种群中每个个体的适应度评价指标，见式（7）。并线性地给每个个体分配适应度，见式（8）。总转数越低，适应度越大，即个体基因有更大概率遗传到下一代；反之就越小，更容易被淘汰（碰到障碍物的个体直接被淘汰）。

 ${\bar n_i} = \int_{{t_0} = 0}^t {n(t){\rm d}t} \text{，} (i = 1,2,...,30) \text{，}$ (7)
 $fitness(i) = ranking({\bar n_1},{\bar n_2},...,{\bar n_{30}}) \text{。}$ (8)

4）根据每个个体的总转数，计算种群的标准差 $stdev({\bar n_i})$ ，以此判断种群的基因多样性，当标准差小于一定值时表明优化已收敛到最优解，停止种群的进化；若不满足则继续执行。种群的最大进化代数设置为30代。

5）选择。根据每个个体的适应度，运用轮盘赌选择法对个体进行选择。

6）最后进行复制、变异和交叉操作，产生新种群。基因交叉的概率设置为80%，变异概率为8%。然后重复步骤 2 ~ 步骤 5。

3 结果与分析

 图 7 各代总耗能 Fig. 7 Total energy consumption of each generation

 图 8 转速对比 Fig. 8 Comparison of rpm

 图 9 路径对比 Fig. 9 Comparison of route

 图 10 复杂环境转速对比 Fig. 10 Comparison of rpm in complex environment

 图 11 复杂环境路径对比 Fig. 11 Comparison of route in complex environment
4 结　语

本文建立了重于水型AUV的动力学模型，为其设计了模糊控制器，并使用遗传算法对控制器进行优化。优化后的模糊控制器使重于水型AUV可以在未知水域中更加节能地完成路径规划任务，对于提高能量利用效率具有重要意义。模糊控制器输入输出量严格对应实际潜水器，可通过离散化的方式将控制器翻译为C语言，并移植到水下嵌入式平台上，对于实现重于水型AUV在水下环境中的局部路劲规划具有重要的应用价值。

 [1] KHATIB O. Real time obstacle avoidance for manipulators and mobile robots[J]. International Journal of Robotics and Research, 1986, 5 (1): 90–98. DOI: 10.1177/027836498600500106 [2] LI Ying, HONG Sheng, HUANG Jun, et al. The new environment model building of penetration mission based on the artificial potencial field approach[C]. Proc of 2012 IEEE International Muti-Disciplinay Conference on Congnitive Methods in Situation Awareness and Decision Support, 2012: 121–124. [3] 陈卫东, 朱奇光. 基于模糊算法的移动机器人路径规划[J]. 电子学报, 2011 (04): 971–974+980. [4] 付宜利, 靳保, 王树国, 等. 未知环境下基于行为的机器人模糊路径规划方法[J]. 机械工程学报, 2006 (05): 120–125. DOI: 10.3321/j.issn:0577-6686.2006.05.023 [5] 孙兵, 朱大奇, 杨元元. 基于粒子群优化的自治水下机器人模糊路径规划[J]. 高技术通讯, 2013, 12 : 1284–1291. DOI: 10.3772/j.issn.1002-0470.2013.12.011 [6] 杨敬辉, 洪炳镕, 朴松昊. 基于遗传模糊算法的机器人局部避障规划[J]. 哈尔滨工业大学学报, 2004, 07 : 946–948. DOI: 10.3321/j.issn:0367-6234.2004.07.033 [7] 颜翚, 葛彤, 王彪. 一种新型重于水的自治式潜水器及其先进性分析[J]. 上海交通大学学报, 2012, 08 : 1173–1177. [8] M, et al. Modelling and simulation of a robust energy effcient AUV controller[J]. Mathematics and Computers in Simulation, 2016, 121 : 34–47. DOI: 10.1016/j.matcom.2015.08.021 [9] FOSSEN. guidance and control of ocean vehicles[M]. Norway, 1994. [10] 施生达. 潜艇操纵性[M]. 北京: 国防工业出版社, 1995. [11] 毛宇峰, 庞永杰, 李晔. 速度矢量坐标系下水下机器人动态避障方法[J]. 哈尔滨工程大学学报, 2010, 02 : 159–164. DOI: 10.3969/j.issn.1006-7043.2010.02.005 [12] 刘建成, 于华男, 徐玉如, 等. 水下机器人改进的S面控制方法[J]. 哈尔滨工程大学报, 2002, 23 (1): 33–36. [13] 孙增圻. 智能控制理论与技术[M]. 北京: 清华大学出版社, 1996.