第5章 动态未知环境下的避障策略

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

Local Risk Obstacle Avoidance Algorithm of USV

---

博主 的硕士毕业论文

第5章 动态未知环境下的避障策略

　　水面无人艇在真实复杂海域执行任务期间，由于硬件（如AIS、航海雷达等）条件的限制，部分障碍物的运动信息难免出现不可预测的情况。在无人艇行进的途中，必须对这些不可预见的事件（如障碍物）敏捷的做出实时反应，顺利避过这些不可预见的事件，使其能够快速、安全的到达终点或子目标点。在机器人领域，主动回避障碍物是机器人的一项基本要求，对于USV也是其能否实现自主航行的关键。因此，对一些没有先验知识的障碍物，水面无人艇需要根据视觉传感器或避障声呐等障碍识别系统实时探测到的信息进行避障。本文利用模型预测控制及滚动优化原理，结合滚动窗口方法，解决动态未知环境下的实时避障问题。

5.1 滚动窗口动态路径规划方法

5.1.1 滚动窗口动态路径规划方法概述

　　国内学者席裕庚等$^{[58-60]}$提出了一种基于滚动窗口的思路，用于移动机器人的路径规划问题。该方法采用预测控制中的滚动优化原理，针对不确定环境下的动态路径规划问题，利用机器人本身得到的局部环境实时信息，以滚动的方式在线规划，从而实现优化与反馈的结合。
　　而无人艇的未知局部危险避障，就是在障碍物信息不可预测的情况下，USV只能依据实时探测到的障碍物信息进行避碰。解决此类未知环境下的路径规划问题时，一般也是借鉴业已成熟的预测控制理论及其中的滚动优化原理，即采取不断循环进行的局部优化来代替只进行一次的全局优化结果，并在每次局部优化时，充分利用当时得到的最新的局部环境信息。

图5-1 滚动窗口路径规划原理图
　　预测控制理论提出来是在上世纪70年代，是一种最优控制算法，它可以根据优化过程中的已有信息来判断未来某个状态的输入和输出，非常适合控制那些难以建立高精度数学模型的复杂的生产过程。预测控制理论的核心主要有三个：预测模型、滚动优化、反馈校正。在基于滚动窗口的USV动态路径规划方法中，分别对应为场景预测、滚动窗口优化和反馈初始化三个步骤（如图5-1）：
　　（1）场景预测：描述无人艇运动环境的预测模型，包括出发点坐标、终点坐标、无人艇运动情况、环境中已知的动态和静态障碍物。只要给定了无人艇从起点到终点的运动路径，就可以根据这个模型展示无人艇的运动过程，并判断其能否避开障碍物；
　　（2）滚动窗口优化：优化是基于周期的驱动，每向前滚动一步，就定义以USV的当前位置为中心的区域作为优化窗口。在一个周期内，以当前点作为起点，确定了该窗口区域的局部目标后，依据窗口内已知信息提供的场景预测，进行规划，求出合适的局部路径，无人艇就沿着此路径移动，直到下一个窗口，进入下个周期，如此往复，直到求出全局的最优路径；
　　（3）反馈初始化：在滚动规划求取局部最优路径时，无人艇要通过本身的传感器系统获取周围环境的实时信息，初始化窗口所定义的区域内的环境障碍物。这个过程既是对区域内已知障碍物信息的调整，又是对不确定障碍物的预测，同时，如果程序设计得当，还能对未知的动态障碍物的运动趋势进行预测，从而，实现了局部规划时最新信息的获取。

5.1.2 滚动窗口动态路径规划流程

　　基于滚动窗口的无人艇动态路径规划算法是依靠无人艇实时探测得到的局部环境信息，以向前滚动的方式进行的实时在线规划。在规划过程中的每一个窗口中，无人艇以以启发式的方法，根据探测得到的局部障碍信息生成优化子目标，在当前的滚动窗口内，再进行局部规划。然后以滚动式的方式移到下一个窗口，不断的获取新的环境信息，从而实现优化与反馈的动态结合。
　　图5-2即是基于滚动窗口的动态路径规划流程。在该程序中，产生局部子目标${P_{win}}(t)$的方法就是在每次滚动窗口的环境信息更新后，利用启发式函数$\min f(P) = g(P) + h(P)$来确定子目标，其中P为窗口上的点，$g(P)$为无人艇从当前位置行驶到P点的代价，而$g(P)$值的确定则取决于P的位置和当前环境信息， $h(P)$为P点到终点的代价，其值的确定则采用P点到终点的距离来估计。之所以这样来选择子目标，就是为了在全局优化的要求和局部有限信息中找到平衡，使函数值最小的窗口边界点P作为子目标${P_{win}}(t)$，即：

$$\min f(P) = g(P) + h(P)　　　　　　s.t. P \in \partial Win({P_R}(t)) \cap FD$$

　　为了简化计算，降低对优化的要求，并着重研究具有可行性的算法，仅以P是否属于可行域值来确定 $g(P)$，即：
　　$g(P) = \left\{ \matrix{ 0, (P \in FD) \cr + \infty , (P \in NFD) \cr} \right.$　　　（5-1）
　　其中：FD：无障碍自由空间；
　　　 　 NFD：障碍空间。

图5-2 滚动窗口路径规划算法流程图
　　子目标的选择问题就简化为以下优化问题：
　　$\min J = \min d{\rm{(}}{{\rm{P}}_{{\rm{win}}}}{\rm{(t),}}{{\rm{P}}_{{\rm{goal}}}}{\rm{)}}$　　　$s.t. P \in \partial Win({P_R}(t)) \cap FD$　　（5-2）
　　简化为求到终点距离最小的P为${P_{win}}(t)$，若求得的${P_{win}}(t)$值不唯一时，可任选其一。
　　此外，对于滚动窗口内的局部规划方法，采用严格趋近可行路径的算法，关键步骤说明如下：
　　Step 1：朝终点笔直走，直至以下任一情况发生：
　　　　　　ⅰ若已到达终点，结束规划；
　　　　　　ⅱ到达障碍物膨胀模型边界，转Step 2。
　　Step2：沿能使到终点距离减小的障碍物一侧边界行走，直至以下任一情况发生：
　　　　　　ⅰ若已到达终点，结束规划；
　　　　　　ⅱ若朝终点行走并且不会碰到障碍物，转Step 1。

5.1.3 滚动窗口动态路径规划方法的不足

　　滚动窗口路径规划方法实现了优化结果和反馈信息、全局环境信息和实时探测的局部信息之间的有效结合，从而解决未知环境下的避障问题。
　　然而滚动窗口动态路径规划方法在解决现实问题时，仍存在一些问题：
　　（1） 滚动窗口动态路径规划是一种实时在线的路径规划方法，其不进行离线的路径规划，只通过每一步长的窗口子目标点来确定其前进方向；而子目标点只是通过避让窗口内静态、动态障碍物，并到目标点最短距离来确定的，该方法未考虑到全局的优化目标。若将该方法应用在复杂的广阔环境中，得到的全局优化结果会很不理想。
　　（2） 滚动窗口动态路径规划方法虽然考虑了全局优化信息，即在启发式函数 中 为P点到终点的代价。但仅仅依靠该函数对规划指导，无法全面的考察全局规划因素，当遇到某些障碍物陷阱时，会出现振荡等陷入陷阱的问题，使得机器人无法正常到达终点。
　　（3） 该方法是基于严格的环境假设，当现实环境不能满足这些假设要求时，该算法的可达性得不到保证。
　　（4） 对于安全性而言，该方法是建立在对未知障碍物运动信息准确预测的基础上，无人艇在向障碍物靠近时，一旦障碍物不再按照预测轨迹运动时，无人艇极有可能与障碍物发生碰撞，安全性得不到保证。

5.2 基于滚动窗口的未知环境下的避障策略

　　本文将借鉴滚动窗口动态路径规划方法，将其应用到水面无人艇的未知环境下的避障中。结合水面无人艇的自身特征，以及基于PSO的已知静态路径规划方法和基于PSO并融合海事规则的已知动态路径规划方法，提出一种基于滚动窗口的未知环境下的避障策略。该避障策略包括：环境信息建模、障碍物运动轨迹预测、滚动窗口实时探测、信息反馈、局部路径规划和控制执行，下面分别对该避障策略的步骤进行详细阐述。

5.2.1 环境信息建模

5.2.1.1 水面无人艇观测窗口

　　假设水面无人艇视觉传感器或避障声呐等障碍识别系统探测范围为R（即为滚动窗口半径），则无人艇在点P(t)处的观测窗口区域定义为$Win(P(t)) = \left\{ {P|P \in W,d(P,P(t)) \le R} \right\}$。无人艇观测窗口的角度则与无人艇障碍识别系统的型号和配置有关，一般该角度形成的观测窗口成一扇形区域，如图5-3所示。

图5-3 水面无人艇观测窗口建模示意图
　　设无人艇运动速度为V，控制周期为T，则在一个周期内的航行步长可以表示为$\varepsilon = T \times \Delta V$（$\Delta V$为无人艇在该周期内的平均速度）。对于滚动窗口半径R，需要符合条件：$0 < \varepsilon < R$，即滚动窗口半径值要大于无人艇的每个周期内行进步长。滚动窗口的子目标点为Pwin(t)。

5.2.1.2 障碍物膨胀模型的建立

　　对于水面运动信息不可预测的障碍物，其在航行过程中存在诸多不确定性因素，因此需要对其运动信息进行进一步的预测。本文采用建立膨胀模型的方法。
　　同静态障碍物处理方法一样，对于长宽比较大的动态未知障碍，为了将障碍物进行更贴合实际的模拟，保留更多的可行路径，所以将此类动态未知障碍定义为具有方向性的矩形障碍物DOU$^{ri}$；而对于长宽比较小的动态未知障碍物定义为障碍圆DOU$^{ci}$；并将动态未知具有方向性的矩形障碍物和障碍圆定义为DOU$^{i}$。
　　虽然在经历下一运动周期后，动态未知障碍物的具体位置不可知，但可预测其在经历一个周期T后可能到达的区域。对于第i个动态未知障碍物，定义其在t时刻，一个控制周期T后的预测运动区域为DOU$^{i}$(t+T)，该区域即为此障碍物DOU$^{i}$在t时刻的膨胀模型，动态未知障碍物膨胀模型建立示意图如图5-4所示。

图5-4 动态未知障碍物膨胀模型建立示意图
　　对于长宽比较小的动态未知障碍圆DOU$^{ci}$，设t时刻在极坐标系下障碍圆的圆心为$\left( {{\rho _i}(t),{\theta _i}(t)} \right)$，半径为r$^{i}$。将障碍圆圆心沿该时刻探测运动方向移动Δr=Vi×T，其中Vi为该时刻探测到的第i个障碍物运动速度，得到膨胀模型的圆心$\left( {{\rho _i}^\prime (t),{\theta _i}^\prime (t)} \right)$，膨胀模型圆的半径为${r_i}^\prime = {r_i} + {{{V_i} \times T} \over 2}$。则动态未知障碍圆在极坐标系下形成的曲线可表示为：
　　${\rho ^2} - 2\rho {\rho _i}(t)\left( {cos(\theta - {\theta _i}(t)} \right) + {\rho _i}{(t)^2} = {({r_i}^\prime )^2}$　　　（5-3）
　　对于长宽比较大的动态未知矩形障碍物DOU$^{ri}$，其膨胀模型可表示为：
　　位于运动方向前方的两个角点，均分别在矩形障碍物局部坐标系下，沿运动方向和运动方向垂直的方向移动Δd=Vi×T，其中V$^{i}$为该时刻探测到的第i个障碍物运动速度；再经过局部坐标系下的坐标向全局坐标系转换，便可得前方两角点在极坐标系下的坐标值。
　　位于运动方向后方的两个角点，均分别在矩形障碍物局部坐标系下，沿运动方向垂直的方向移动Δd=Vi×T，其中V$^{i}$为该时刻探测到的第i个障碍物运动速度；再经过局部坐标系向全局坐标系转换可得后方两角点在极坐标系下的坐标值。
使用该障碍物膨胀建模方法，无人艇的可行区域可表示为：
　　$FD = \left( {\mathop \cap \limits_{i \in SOnum} {{\left( {SO_i^o} \right)}^c}} \right) \cap \left( {\mathop \cap \limits_{j \in DOnum} {{\left( {DO_j^o(t + \Delta t)} \right)}^c}} \right) \cap \left( {\mathop \cap \limits_{k \in DOUnum} {{\left( {DO_k^o(t + \Delta t)} \right)}^c}} \right)$　　　（5-4）
　　其中SO$_{num}$为静态障碍物数目，DO$_{num}$为已知动态障碍物数目，DOU$_{num}$为动态未知障碍物数目。
　　即同时要满足规避所有静态障碍物，及在该时刻下的已知动态障碍物，和经过实时探测并经过膨胀建模的未知动态障碍物。
　　无人艇窗口范围内的可行区域可表示为：
　　$WFD(t + \Delta t) = Win\left( {{P_R}(t)} \right) \cap \left( {\mathop \cap \limits_{i \in SOnum} {{\left( {SO_i^o} \right)}^c}} \right) \cap \left( {\mathop \cap \limits_{j \in DOnum} {{\left( {DO_j^o(t + \Delta t)} \right)}^c}} \right) \cap \left( {\mathop \cap \limits_{k \in DOUnum} {{\left( {DO_k^o(t + \Delta t)} \right)}^c}} \right)$　　　（5-5）
　　即在满足无人艇可行区域的基础上，还要满足滚动窗口的可行区域。

5.2.2 窗口子目标点的确定

　　水面无人艇在航行过程中，首先要在无人艇感知区域内进行全局已知静态障碍物和已知动态障碍物的路径规划，再根据已知障碍物在最优路径上确定一系列子目标点，然后无人艇沿着该条最优路径前行，并实时探测外界环境进入实时避障模块。在行进途中，若探测到有动态未知障碍物，便通过滚动窗口避障策略进行实时避障解算，完成此段周期内的避障且窗口内没有动态未知障碍物时，无人艇就可沿下一子目标点的方向前行，从而实现全局路径规划和局部避障的结合。下面将讨论如何选取子目标点。
　　选取指定子目标点的根本目的便是通过子目标点存储全局优化信息，从而保证在进行实时滚动窗口避障策略时，不仅能够敏捷的进行避障，同时还能充分考虑到全局优化路径，以避免陷入障碍陷阱。从上面可以看出，子目标点的制定，是全局规划和局部实时避障结合的基础。
　　制定子目标点是在完成已知障碍路径规划后进行的，即在全局最优路径上选取，并将整条优化路径分为若干个子路径段。各个子目标点之间的最小距离值为无人艇滚动窗口半径，最大值则为起始点与目标点之间的最优全局路径。对于无人艇的二维工作空间而言，子目标点的选取包括两个方面：
　　① 通过基于PSO的路径优化方法，求解已知静态障碍物得到的全局静态最优路径与基准维半径的交点；
　　② 通过基于PSO并融入海事规则，求解已知动态障碍物得到的全局动态最优路径与附加维半径的交点。

图5-5 窗口子目标点制定示意图
　　如图5-5所示，图中“Start”和“Goal”代表无人艇的起点和终点，图中包括三个已知静态障碍物（“SOC1”和“SOC2”代表已知静态圆形障碍物，“SOR1”代表已知静态矩形障碍物），两个动态已知障碍物（“DOC1”代表动态已知圆形障碍物，“DOR1”代表动态已知矩形障碍物）。图中最优全局路径线为，在求解已知静态障碍物得到的全局静态最优路径（Path1 ~ Path4即为静态全局最优路径节点）基础上，求解已知动态障碍物得到的全局动态最优路径（P_AD0 ~ P_AD1即为动态全局最优路径节点）。对于在该情况下，窗口子目标点即取Path1 ~ Path4和P_AD0 ~ P_AD1六个坐标点。

5.2.3 基于滚动窗口的未知环境下的避障方法

　　窗口子目标点确定后，需要确定一种避障方法，使得能对障碍物进行高效稳定的规划局部避障路线。滚动窗口路径规划方法首先是朝着目标点运动，直至接近障碍物后，便沿着趋近目标的边缘前进。可以看出使用此种方式无疑会对无人艇对外感知的传感器要求很高，需要精准的对障碍物运动信息进行预测；然而即使传感器可以准确地预测障碍物运动信息，当无人艇运动到障碍物边缘时，如果障碍物不再按照传感器预测的轨迹运动，很有可能会发生碰撞。
　　针对无人艇要采用此种方法进行避障，需要考虑的一个问题是，无人艇不能像陆上机器人那样可以快速的转向或改变速度。无人艇要改变其运动状态，需要一定的反应时间，并且还需要符合无人艇的最小回转直径的要求。如果仅仅按照滚动窗口法考虑下一个控制周期（T0）的运动轨迹，当遇到障碍物时，采用滚动窗口法得出的避障策略，对无人艇而言要求极高，很有可能使得避障策略实现不了，最终导致发生碰撞。所以说，考虑到无人艇运动性能，还需要考虑T0之后的障碍物运动区域。
　　自此，可以得出考虑到无人艇运动特性，并基于滚动窗口的未知环境避障方法基本过程：无人艇在时刻t以航速VR向目标点航行，视觉传感器或避障声呐等障碍识别系统实时探测环境信息，并对障碍物预测运动信息，然后进行膨胀处理；再通过预测的运动信息和膨胀模型，预测障碍物和无人艇在下一周期（T）及再下一周期（2T）的位置；进而通过预测的位置判断无人艇处在的危险状态，来指导无人艇在下一周期的运动状态。
　　通过预测的位置判断无人艇处在的会遇状态可包括以下四种状态：
　　（1） 危险状态：若无人艇按照本时刻的速度和方向前进，在下一周期T0内会进入障碍物预测膨胀区域内，则可能会发生与障碍物碰撞，则该情况即为无人艇在T0时间内处于危险状态，如图5-6(1) 所示（图中实线障碍物代表（T+T0）时刻，虚线障碍物代表（T+2T0）时刻）。此种情况必须马上采取避障措施，进行紧急转向或者减速避让。
　　（2） 准危险状态：若无人艇按照本时刻的速度和方向前进，在下一周期T0内不会进入障碍物预测膨胀区域内；但在T0后的下一个周期（T+2T0）内会进入障碍物（T+2T0）时刻内的障碍物预测膨胀区域（如图5-6(2)中虚线圆圈所示）。此种情况说明，无人艇在按原运动状态向前航行T0后的下一周期内，会与障碍物发生碰撞。因此，虽然在T0时间内判断为安全状态，但如果不进行改变就继续前进，就会出现危险状态。所以，对此种情况需要按照（T+2T0）时间段的预测膨胀区域，利用滚动窗口的避障算法求解避障路径。
　　（3） 潜在危险状态：当无人艇滚动窗口中存在动态未知障碍物，且此障碍物朝着无人艇运动时，若无人艇继续沿着当前运动状态前进，在下一个周期（T+T0）及下两个周期（T+2T0）内，均不会航行到障碍物相应的预测膨胀区域中，此种具有潜在碰撞可能性的状态称之为潜在危险状态，如图5-6(3)。可见此种状态下继续前进，在下两个预测周期内不会出现碰撞的情况；且无人艇还有一个控制周期T的时间去改变运动状态而避开障碍物，再加上障碍物运动的不确定性因素（第三个预测周期内不能有效保证预测的障碍物运动信息准确），所以如果在T时刻就采取避障策略，不能确保避障行为是有效的。
　　（4） 安全状态：当无人艇滚动窗口中没有动态未知障碍物，或者存在动态障碍物但预测其运动方向是远离无人艇的，说明若无人艇继续沿着当前运动状态前进不会发生碰撞。此种状态即称之为安全状态，如图5-6(4)。

图5-6 滚动窗口会遇状态示意图

5.2.4 算法描述

　　无人艇在航行过程中不断的进行探测周围环境内的障碍信息，当遇到环境先验知识未知的动态障碍物时，马上进入滚动窗口避障策略模块进行解算，求解避障方式。此时，无人艇首要任务便是进行快速避障，随着无人艇向前航行，滚动窗口的信息也是实时在线更新的，每个周期内的局部最优解也只是暂时的。
　　过多的考虑并花费时间去求解滚动窗口内的最优路径实际意义并不大，同时也有可能因为花费时间求解最优，导致避障最佳时间有所延误，甚至使得避障策略不可行。所以避障策略尽可能简单、可靠、处理时间短。下面将给出本文无人艇的基于滚动窗口避障算法基本步骤：
　　Step1：无人艇沿全局最优路径行进，判断是否到达终点。若到达终点，则结束避障。若还未到达终点，观测窗口持续探测环境信息。假设在T时刻观测窗口Win(T)中探测到动态未知障碍物，根据传感器信息进行运动轨迹预测，并将障碍物进行膨胀处理。
　　　Step2：判断无人艇处于会遇状态中的哪一种，并采取下面的几个步骤：
　　（1） 判断无人艇是否处于安全状态。若滚动窗口中动态未知障碍物运动方向远离无人艇，则其正处于安全状态，进入Step4；若非处于安全状态，即动态未知障碍物驶向无人艇，则又分为下面的三种情况；
　　（2） 若经过预测，无人艇在下一周期后，其航行位置位于未知障碍物的预测运动轨迹中，此时处在危险状态，马上采取避障措施，产生最大转向角，即以无人艇当时速度所对应的最小回转直径转向，进行紧急避障，进入Step3。
　　（3） 若无人艇正处于准危险状态，则在这个周期内就采用避障措施，进入Step3。
　　（4） 若无人艇正处于潜在危险状态，则进入Step4。
　　Step3：通过基于滚动窗口避障算法，判断无人艇调整方向进行避障（障碍物膨胀区域的切线方向，取符合条件且离当前航向偏角最小的那个），此处还要进行判断新产生的路径是否与已知障碍物碰撞，是否满足无人艇最小回转直径的限制，若符合限制条件的要求进入Step4。
　　Step4：无人艇前进一个周期，然后进入Step1。
　　滚动窗口避障算法是通过无人艇周期性的探测、解算、行动这些行为来达到避障的目的，基于滚动窗口的未知环境下的避障策略流程图如图5-7所示。

图5-7 基于滚动窗口的未知环境下的避障策略流程图
　　为了避免无人艇因为障碍物预测危险区域稍有变化，就导致无人艇频繁的调整航行状态（在实际过程中，无人艇是无法实现的），给出以下方法解决此问题：对航行状态设置限制阈值δ，若无人艇通过基于滚动窗口避障算法解算出需要调整的航行状态小于此阈值δ，就不进行调整航行状态。航行状态限制阈值δ包括两个变量：航向角和航行速度，可表示为δ=（δν,δα）。航行状态限制条件可表示为：
　　$\left\{ {\matrix{ {\Delta \alpha = 0\matrix{ {} & {} & {(\left| {\Delta \alpha } \right| < {\delta _\alpha })} \cr } } \cr {\Delta \nu = 0\matrix{ {} & {} & {(\left| {\Delta \nu } \right| < {\delta _\nu })} \cr } } \cr } } \right.$　　（5-6）

5.3 仿真实验及结果分析

　　本节将针对基于滚动窗口的未知环境下的避障策略在Visual C++平台上进行仿真分析。分别对滚动窗口观测到单个未知圆形动态障碍物、单个未知矩形动态障碍物和多个未知障碍物这三种环境下进行仿真实验。
　　在本节仿真实验中，均假定水面无人艇的全局最优路径为正东方，“Start”为起始点，“Goal”为终止点，运行速度为40Kn，滚动窗口半径为1200m，控制周期为2s。红色双点划线的半圆表示观测窗口第一次探测到障碍物时所处的位置。实心蓝色图形为该障碍物进入无人艇观测窗口时所处的位置，向外扩展的蓝色虚线表示在该障碍物进入观测窗口时刻下一周期的预测膨胀模型。黄色实线表示该障碍物在无人艇观测窗口中消失的那一刻所处的位置，即此时刻无人艇已完全逃离该障碍物的预测膨胀区域。蓝色箭头指向表示此时刻预测到的障碍物运动方向。图中连接起始点和终止点的红色虚线即代表无人艇走过的避障路线。在以下避障路径节点的表格中，列出来的极角均是相对于上一个路径节点的极角，即以上一路径节点为圆点，平行于X轴并与其指向相同的方向为极轴的局部极坐标系中该点的极角。

图5-8 单个未知动态障碍物结果图
　　情景一：无人艇在沿全局最优解行进途中只探测到了单个未知圆形动态障碍物（相关参数见表5-1）的情景，如图5-8(1)所示。当无人艇运行到P点位置(极径：245.73m，极角：0.00rad)时探测到未知圆形动态障碍物UOC，此刻开始进行基于滚动窗口的避障。经过判断此时刻无人艇处在潜在危险状态，在下一个周期及下两周期内不会发生碰撞，如表5-2所列前两周期极角均不变，所以继续行进。在第3周期，无人艇处于准危险状态，避障模块做出解算，算出沿UOC膨胀区域的切线方向，找出两个切线方向改变量较小的那个角度，以此角度调整航向。行进到第4周期结束时，UOC就已完全消失在无人艇的滚动窗口中，无人艇也完成了对UOC的避障。在第5周期开始转向，从第6周期开始无人艇便沿着到达终点的方向前进。

表 5-1 情景一：未知圆形动态障碍物UOC参数

序号	极径(m)	极角(rad)	直径(m)	方向角(°)	速度(Kn)
1	600	0.00	120	190	30

表 5-2 情景一：避障路径节点

序号	极角(rad)	序号	极角(rad)	序号	极角(rad)
1	0.000	3	0.409	5	0.060
2	0.000	4	0.409	6	6.228

　　情景二：无人艇在沿全局最优解行进途中只探测到了单个未知矩形动态障碍物（相关参数见表5-1）的情景，如图5-8(2)所示。当无人艇航行到P点位置(极径：164.62m，极角：0.00rad)时探测到未知矩形动态障碍物UOR，此刻开始进行基于滚动窗口的避障。后面的避障情况同情景一，可参照上面一段文字，在此就不再赘述了。

表 5-3 情景二：未知矩形动态障碍物UOR参数

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

表 5-4 情景二：避障路径节点

序号	极角(rad)	序号	极角(rad)	序号	极角(rad)
1	0.000	3	0.673	5	0.324
2	0.000	4	0.673	6	6.208

图5-9 多个未知动态障碍物结果图

　　情景三：如图5-9所示，该情景为无人艇在沿全局最优解行进途中探测到了多个障碍物，包括3个未知圆形障碍物UOC和2个未知矩形障碍物UOR，相关障碍物参数见表5-5和表5-6所示。
　　当无人艇航行到P点位置(极径：164.62m，极角：0.00rad)时，滚动窗口先探测到UOC1，此时无人艇处于潜在危险状态，继续向前航行。之后的几个周期内分别探测到了UOC2、UOC3，如表5-7所示从第1周期至第5周期无人艇均未出现危险状况和准危险状况，沿着全局最优路径前行，极角为0.0rad。
从第6周期开始出现危险状况，通过滚动窗口算法解算避障策略，第6周期结束后针对UOC1、UOC2、UOC3的避障结束。第7周期开始转向，从第8周期开始无人艇便沿着到达终点的方向前进。
之后从第8周期到第15周期均未出现危险状况和准危险状况。
　　从第16周期开始对探测到的UOR1进行避障，经过3个周期到第18周期结束时，完成了对UOR1的避障。第19周期开始转向，从第20周期开始无人艇便沿着到达终点的方向前进。
　　之后的5个周期内均未出现危险状况和准危险状况。
　　从第25周期开始对探测到的UOC4进行避障，经过2个周期到第26周期结束时，完成了对UOC4的避障。第27周期开始转向，从第28周期开始无人艇便沿着到达终点的方向前进。
　　之后的11个周期内均未出现危险状况和准危险状况。
　　从第39周期开始对探测到的UOR2进行避障，经过1个周期，完成了对UOC4的避障。从第28周期开始无人艇便沿着到达终点的方向前进。之后的周期内均未出现危险状况和准危险状况。

表 5-5 情景三：未知圆形动态障碍物UOC参数

序号	极径(m)	极角(rad)	直径(m)	方向角(°)	速度(Kn)
1	512.140	0.418	120	220	15
2	519.892	5.954	80	200	20
3	600.000	0.000	120	190	30
4	1320.00	0.000	120	170	15

表 5-6 情景三：未知矩形动态障碍物UOR参数

序号	中心点极径(m)	中心点极角(rad)	半长(m)	半宽(m)	方向角(°)	速度(Kn)
1	1004.988	0.100	120	32	220	30
2	1721.046	0.035	80	24	130	20

表 5-7 情景三：避障路径节点

序号	极角(rad)	序号	极角(rad)	序号	极角(rad)	序号	极角(rad)
1	0.000	11	6.269	21	6.23	31	6.195
2	0.000	12	6.269	22	6.23	32	6.195
3	0.000	13	6.269	23	6.23	33	6.195
4	0.000	14	6.269	24	6.23	34	6.195
5	0.000	15	6.269	25	0.224	35	6.195
6	0.651	16	0.422	26	0.657	36	6.195
7	0.302	17	0.422	27	0.307	37	6.195
8	6.269	18	0.422	28	6.195	38	6.195
9	6.269	19	0.073	29	6.195	39	5.846
10	6.269	20	6.23	30	6.195	40	6.195

　　为了验证本文所设计的基于滚动窗口的未知环境下的避障策略，设立了以上三种情况。仿真实验结果及分析表明，该避障方法有效可行，且运行速度快，可以很好的满足在线实时避障的要求，使得水面无人艇具备了对真实复杂、具有多种不确定性因素环境的自主适应性。

博主 的硕士毕业论文

---

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