第6章 USV局部危险避障系统及综合仿真实验分析

水面无人艇局部危险避障算法研究

Local Risk Obstacle Avoidance Algorithm of USV

---

博主 的硕士毕业论文

第6章 USV局部危险避障系统及综合仿真实验分析

　　以上三章分别就三层水面无人艇局部危险避障算法进行了深入研究，并分别在仿真实验平台上进行了验证和讨论。本章将综合考虑三层避障算法，并将三层避障算法合理地结合起来，建立能同时考虑已知静态环境下的全局优化目标（路径短、能耗低等），又能根据传感器信息实时规避未知障碍物的USV局部危险避障系统。之后简要的介绍自行开发的水面无人艇局部危险避障仿真实验平台；然后将在仿真实验平台上建立包含三种障碍物的真实环境，并对USV局部危险避障系统进行综合仿真实验分析。

6.1 USV局部危险避障系统

　　本文提出的USV局部危险避障系统（见图6-1所示）基本思路为：以已知静态路径规划算法规划出来的局部静态最优路径为基础和依据向前航行，并不断的感知外界环境，若遇到新的障碍物，则以期望的航向和航速为优化性能指标，进行实时局部危险避障，实现边行进边规避障碍物的实时避障。
　　在给定水面无人艇航行任务后，首先由离线的全局路径规划系统指定局部危险避障的子目标段。在无人艇航向过程中，每行进到子目标段的起点，便进入USV局部危险避障系统，并主动获取已知静态障碍物信息；同时进入“基于PSO的已知静态路径规划模块”，由该模块实时解算局部静态最优路径，所以该模块强调的是在较短的时间内得到子目标段内的路径最优化问题，对应的是本论文第三章的研究内容。当该模块求得局部最优路径之后，便已经根据当前环境信息确定了该子目标段的若干子目标点。此时，无人艇便可以朝着每个子目标点行进。
　　无人艇在朝向子目标点行进途中，便一直利用自身携带的传感器作为环境反馈装置，实时探测环境信息，对威胁到无人艇行进的障碍物进行快速合理的规避，所以强调的是迅速可靠的产生可行路径。若在子目标点处，无人艇探测到运动状态可知或可预测的障碍物，便进入“基于PSO并融合海事规则的已知动态路径规划模块”，由该模块实时解算出满足海事规则的，最优航速和航向改变量，该模块对应的是本论文第四章的研究内容。在前两个模块的基础上，若无人艇实时探测到运动状态不可知或不可预测的障碍物，此时，无人艇首要任务便是进行快速避障，进入“基于滚动窗口的未知局部危险避障模块”，根据滚动窗口内障碍物所处的危险状态，实时规划出下一控制周期的运动状态，该模块对应的是本论文第五章的研究内容。

图6-1 水面无人艇局部危险避障系统

6.2 水面无人艇局部危险避障算法仿真实验平台

　　为了可视化验证本文提出的三层水面无人艇局部危险避障算法的可行性，开发了此水面无人艇局部危险避障算法仿真实验平台，主界面如图6-2所示。

图6-2 水面无人艇局部危险避障算法仿真实验平台主界面

　　该水面无人艇局部危险避障算法仿真实验平台，是在Microsoft Visual C++平台上进行开发的，并利用微软基础类库MFC实现C++的图形化界面。在用户界面上，采用了当今广泛流行使用的Ribbon界面进行设计。
　　Ribbon界面最早应用于 Office 2007 中，在后来的Windows系统中，Ribbon界面已广泛的应用到了系统应用以及各种第三方应用中。与传统的菜单式用户界面相比较而言，Ribbon界面具有较多的优势：所有功能有组织的集中存放，无需查找级联菜单和工具栏；更好的组织命令；可以图示显示命令；更加适合触摸屏操作等。
　　所以，在进行用户界面设计时，抛弃了传统的菜单式界面，转而采用Ribbon界面进行设计。
　　本文开发的仿真实验平台，包括上方的Ribbon功能区（涵盖本仿真实验平台所有功能），中间的主显示绘图区域，绘图区域角落的避障信息显示，以及下方可实时显示绘图区域鼠标指向坐标的状态栏。下面分别对平台各区域做简要的说明。

6.2.1 Ribbon功能区

　　Ribbon功能区涵盖了本仿真实验平台所有功能，根据三层避障算法的特点分为三个类别（即选项卡）：已知局部危险避障，未知局部危险避障和设置。
　　（1）“已知局部危险避障”类别包含了四个面板（见图6-3）。

图6-3 仿真实验平台Ribbon界面(1)
　　“初始化设置”面板为设置无人艇的起始点和初始速度，对话框见图6-4。

图6-4 初始化对话框
　　“基于PSO的已知静态避障策略”面板用来进行第一层避障算法的计算。包括静态圆形、静态矩形障碍物的设置，对话框见图6-5(1)(2)；粒子群优化算法参数设置，对话框见图6-5(3)；以及只进行粒子群优化算法的“计算路径”按钮，只进行路径优化“优化路径”按钮，和“计算并优化”按钮。

图6-5 静态已知环境建模及参数设置对话框
　　“基于PSO的已知动态避障策略”面板用来进行第二层避障算法的计算。包括动态圆形、动态矩形障碍物的设置，对话框见图6-6(1)(2)；粒子群优化算法参数设置，对话框见图6-6(3)；进行粒子群优化算法的“计算”按钮；以及可设置显示避障结束点处动态障碍物位置的“动态显示”按钮。

图6-6 动态已知环境建模及参数设置对话框
　　“数据”面板中的“数据输出”按钮，用来对环境信息和计算信息进行输出，保存计算数据以供分析。
　　（2）“未知局部危险避障”类别包含了两个面板（见图6-7）。

图6-7 仿真实验平台Ribbon界面(2)
　　“环境参数设置-未知”面板用来进行未知圆形、未知矩形障碍物的设置，对话框见图6-8(1)(2)。“基于滚动窗口法的避障策略”面板用来进行第三层避障算法的计算，包括无人艇未知局部危险避障参数设置，对话框见图6-8(3)；进行滚动窗口算法计算的“计算”按钮。

图6-8 动态已知环境建模及参数设置对话框
　　（3）“设置”类别包含了三个面板（见图6-9）。
　　“窗口”面板可进行窗口大小的设置，对话框见图6-10；“视图”面板可设置状态栏、标题栏的显示与否；“显示设置”面板可设置主显示绘图区域中视图信息、始末点、路径节点、各个障碍物的显示方式。

图6-9 仿真实验平台Ribbon界面(3)

图6-10 改变窗口大小对话框

6.2.2 其它区域说明

　　（1）中间的主显示绘图区域是用来可视化显示算法避障效果的，包括绘制障碍环境、避障路径、及显示相关参数。
　　（2）绘图区域角落为避障信息显示区域。
　　右上方的“Scale：1:4”为视图显示的比例尺。
　　右下方区域中，“DynamicPSO”代表第二层已知动态路径规划方法中的粒子群优化算法，下方的“Generations”代表迭代次数，“Particle_num”代表粒子数，“WeightV”代表航速改变权值，“WeightDa”代表航向角改变权值。而“StaticPSO”代表第一层已知静态路径规划方法中的粒子群优化算法，下方的“Generations”代表迭代次数，“Particle_num”代表粒子数，“SetV_numMAX”代表调整速度最大次数，“SetInitialV”代表初始化速度值（为2π/ SetInitialV）。
　　左下方区域中，“Test_Num”代表自打开程序进行过计算的次数；“ThisPSO_Num”代表在同一组粒子群参数下进行的计算次数；“StaticPSO_BestIndex”代表已知静态路径规划算法最佳代数；“UsedTime(S)”代表计算第一层已知静态路径规划算法的耗时；“UsedTime(D)”代表计算第二层已知动态路径规划算法的耗时；“BestFitness_CalculatePSO”代表计算第一层已知静态路径规划算法不进行路径优化的最优路径长度；“BestFitness_OptimizePath”代表第一层已知静态路径规划算法并进行路径平滑优化处理的最优路径长度；“BestFitness_DynamicPSO”代表经过前两层路径规划算法得到的最优路径长度。
　　（3）下方状态栏处可实时显示绘图区域鼠标指向坐标的区域，可方便进行判断，及添加障碍物。

6.3 综合仿真实验及结果分析

　　下面将结合前文提出的三层水面无人艇局部危险避障算法，建立较为真实的障碍环境，同时使用三层避障算法进行避障仿真分析。
　　水面无人艇在沿全局优化路径行进的途中，不断的在全局优化路径中选取子路径起点和子路径终点，设置子路径段，供局部危险避障算法计算。假设无人艇运行到某海域，由全局路径优化算法指定了本段局部危险避障算法的起点“Start”和终点“Goal”。至此，无人艇便进入了局部危险避障。下面将分步进行描述本段子路径段的局部危险避障。
　　首先给出无人艇以及各个算法的初始化设置。子路径段起点“Start”和终点“Goal”之间的距离为4400m。设置无人艇航速为40Kn。在第一层基于PSO的已知静态路径规划中，粒子群优化算法的惯性权重ω采用线型递减策略，且 ωmax=0.95，ωmin=0.2；c1=c2=2；迭代次数100，粒子数50。在第二层基于PSO的已知动态路径规划中，粒子群优化算法的惯性权重ω采用线型递减策略，且ωmax=0.95，ωmin=0.2；c1=c2=2；m1=1，m2=70；设置粒子群迭代次数50，粒子数50。在第三层基于滚动窗口的未知环境下的避障中，滚动窗口半径为800m，控制周期为2s。
　　（1） 基于PSO的已知静态路径规划
　　在该段路径中存在四个已知静态圆形障碍物（SOC1、SOC2、SOC3、SOC4），和两个已知静态矩形障碍物（SOR1、SOR2），相关参数见表6-1和表6-2，每个粒子共八个维度。经过第一层基于PSO的已知静态路径规划算法的解算，得到最优路径为4491.78m，计算耗时为1.466s，如图6-11所示，规划的路径节点见表6-3所列。

图6-11 基于PSO的已知静态路径规划

表 6-1 已知静态圆形障碍物SOC参数

序号 极径(m) 极角(rad) 直径(m) 1 1000.00 0.000 400 2 2000.00 0.000 800 3 2630.59 6.131 200 4 3492.85 0.231 400

表 6-2 已知静态矩形障碍物SOR参数

序号 中心点极径(m) 中心点极角(rad) 半长(m) 半宽(m) 方向角(°) 1 2340.940 5.934 112 20 140 2 3820.995 6.178 120 24 200

表 6-3 已知静态路径规划路径节点

路径点 极径(m) 极角(rad) 路径点 极径(m) 极角(rad) 1 1000.00 0.226 5 2630.59 0.125 2 2000.00 0.226 6 3492.85 0.048 3 2229.95 0.182 7 3714.15 0.034 4 2455.24 0.148 8 3930.80 0.022

　　（2） 起点“Start”和“Path1”之间的避障

图6-12 “Start”和“Path1”之间的避障

图6-13 “Path1”和 “Path2”之间的避障

　　如图6-12所示，无人艇在起点“Start”处首先探测到了已知动态矩形障碍物DOR1（上海外高桥18,000箱级集装箱船，相关参数见表6-4），并与无人艇呈252.03°相遇，即夹角在195°和315°之间，为海事规则中的交叉相遇情形，需要从DOR1的后方通过，即无人艇左转向通行。通过第二层基于PSO的已知动态路径规划算法解算，耗时0.094s，得到的避障方法为向左舷转向13.01°，航速不变行进，相关参数见表6-5。无人艇运行到避障结束点后，就已成功完成了对障碍物DOR1的规避，此时便转向朝向本段的子目标点Path1行进。
　　
表 6-4 已知动态矩形障碍物DOR1参数

序号 中心点极径(m) 中心点极角(rad) 半长(m) 半宽(m) 方向角(°) 速度(Kn) 1 581.377684 0.464 200 28 265 20

表 6-5 已知动态环境避障路径节点及改变量

路径点 航向(°)改变量 航速(Kn)改变量 极径(m) 极角(rad) 航速(Kn) 解算时间(s) 0 13.01 0.00 539.77 0.453 40 0.094

　　无人艇从起点“Start”开始行进时，便不断的进行未知障碍物的探测。当无人艇运行到(极径：539.78m，极角：0.453rad)位置时探测到未知动态矩形障碍物UOR1（相关参数见表6-6），此刻开始进行基于滚动窗口的避障。经过判断此时刻无人艇处在潜在危险状态，在下一个周期内不会发生碰撞，如表6-7所列第一周期极角不变，所以继续行进。在第2周期，无人艇处于准危险状态，避障模块做出解算，调整航向。行进到第3周期结束时，UOR1就已完全消失在无人艇的滚动窗口中，无人艇也完成了对UOR1的避障。在第4周期开始转向，从第5周期开始无人艇便沿着到达子目标点“Path1”的方向前进。

表 6-6 未知动态矩形障碍物UOR1参数

序号 中心点极径(m) 中心点极角(rad) 半长(m) 半宽(m) 方向角(°) 速度(Kn) 1 600 0.00 120 32 180 30

表 6-7 “Start”和“Path1”之间的未知动态避障路径节点

序号 极角(rad) 序号 极角(rad) 序号 极角(rad) 1 0.453 3 0.981 5 0.318 2 0.981 4 0.632 —— ——

　　（3） “Path1”和“Path2”之间的避障
　　无人艇在第一路径点“Path1”处探测到了已知动态矩形障碍物DOR2（上海外高桥18,000箱级集装箱船，相关参数见表6-8），已知动态圆形障碍物DOC1和DOC2（相关参数见表6-9）。经第二层基于PSO的已知动态路径规划算法判断DOC1和DOR2均不会与无人艇相碰，不予进行避障考虑。而DOC2与无人艇呈182.03°相遇，即夹角在165°和195°之间，为海事规则中的正面相遇情形，需要从DOC2的左侧通行，即无人艇左转向通行。通过第二层基于PSO的已知动态路径规划算法解算，耗时0.125s，得到的避障方法为向左舷转向13.01°，航速不变行进。无人艇运行到避障结束点后，就已成功完成了对障碍物DOC2的规避，此时便转向朝向本段的子目标点Path2行进，如图6-1,3所示。

表 6-8 已知动态矩形障碍物DOR2参数

序号 中心点极径(m) 中心点极角(rad) 半长(m) 半宽(m) 方向角(°) 速度(Kn) 1 1360.00 5.793 200 28 320 17

表 6-9 已知圆形动态障碍物DOC1、DOC2参数

序号 极径(m) 极角(rad) 直径(m) 方向角(°) 速度(Kn) 1 1501.47 0.729 200 60 10 2 1620.49 0.224 120 195 5

表 6-10 已知动态环境避障路径节点及改变量

路径点 航向(°)改变量 航速(Kn)改变量 极径(m) 极角(rad) 航速(Kn) 解算时间(s) 0 9.66 0.00 1457.18 0.280 40 0.125

　　（4） “Path5”和“Path6”之间的避障

图6-14 各路径点之间的避障
　　“Path5”和“Path6”之间的避障如图6-14(1)所示。无人艇在“Path5”处探测到了已知动态矩形障碍物DOR3，通过基于PSO的已知动态路径规划算法解算出“P_AD5”动态路径点。向前运动的途中先后探测到了UOC1和UOC2，并通过基于滚动窗口的避障算法解算出实时规划路径，如途中虚线。相关障碍物参数和路径节点就不再赘述。
　　（5） “Path6”和“Path7”之间的避障
　　“Path6”和“Path7”之间的避障如图6-14(2)所示。无人艇在“Path6”处探测到了已知动态矩形障碍物DOC3，通过基于PSO的已知动态路径规划算法解算处“P_AD6”动态路径点。
　　（6） “Path8”和目标点“Goal”之间的避障
　　“Path8”和目标点“Goal”之间的避障如图6-14(3)所示。无人艇在从“Path8”驶向目标点“Goal”途中，探测到了UOC3，并通过基于滚动窗口的避障算法解算出实时规划路径，如途中虚线。

图6-15 最终避障示意图

　　为了验证本论文提出的水面无人艇三层局部危险避障算法，在仿真平台上建立了包含三种障碍物的综合避障场景。从上述仿真结果和分析中可以得出，在综合避障场景中，本论文提出的避障算法较好的完成了各项避障任务；且该避障算法有效可行、运行速度快，可以很好的满足在线实时避障的要求，使得水面无人艇具备了对真实复杂、具有多种不确定性因素环境的自主适应性。

博主 的硕士毕业论文

---

===========文档信息============
版权声明：非商用自由转载-保持署名-注明出处
署名(BY) ：dkjkls（dkj卡洛斯）
文章出处：http://blog.csdn.net/dkjkls