基于椭圆聚类-碰撞锥推演的无人艇海洋动态避障控制算法
【技术领域】
[0001] 本发明涉及一种基于椭圆聚类-碰撞锥推演的无人艇海洋动态避障方法，本发明 所涉及到的无人艇避障方法不仅适用于静态避障，尤其适用并主要运用于动态环境下的避 障。
【背景技术】
[0002] 在海洋强国战略中，海图是军事和民用海洋安全航行不可或缺的基本保障，但是 我国海图序列不完整，原因是目前海测船的吃水深，无法靠近岛礁岸线等复杂海域进行海 图测绘;另外，海洋环境监测船也由于吃水深的问题，很难到海岸线、海岛、油气平台附近进 行环境监测。而无人艇具有吃水浅、自主航行、自主避障、任务载荷配置灵活的特点，是解决 近海岛礁海域海图序列不完整问题、复杂海域环境监测问题的有效科技手段，同时也是提 升我国海洋军事装备能力，维护我国海洋权益、处理南海等热点海域局势的重要海洋装备。
[0003] 随着世界各国对水面无人艇的应用研究越来越广泛，对水面无人艇智能化水平的 要求也在不断提高，要求水面无人艇在运动过程中具有与周围环境交互的能力，即要求水 面无人艇具有从周围环境中收集有用信息，构造符号化的环境模型，并利用模型来规划并 执行上层任务的能力，其中路径规划问题就是核心问题之一。水面无人艇的路径规划是指 在静动态障碍物并存的海洋环境中，寻找一条从给定起点到终点的满足一定评价标准的运 动路径，使水面无人艇在航行过程中能安全可靠地避开所有障碍物。
[0004] 路径规划的分类方法很多，按层次关系可分为全局路径规划和局部路径规划；按 照对环境的了解程度，分为环境已知，环境部分已知，环境未知三种规划；同时还有基于功 能、行为的规划等等。全局路径规划和局部路径规划是建立在机器人对其环境了解程度的 基础上。水面无人艇的全局规划着眼于规划出一条从起始点到目标点的无碰撞路径，综合 考虑上层模块的任务要求和水面无人艇本身的运动性能，确立某种最优性能函数，通过一 定的搜索方式，就能在水面无人艇的运动空间中找到相应的优化路径。水面无人艇的局部 规划立足于实时性，根据传感器信息确定水面无人艇在地图中的当前位置以及周围局部范 围内障碍物的分布情况，规划下一时刻的运动，从而避开动态障碍物。
[0005] 全局路径规划的方法主要有:几何法，栅格法、构形空间法、可视图法、拓扑法和概 率路径图法。局部路径规划的主要方法有:人工势场法、模糊逻辑算法、神经网络方法。很多 适用于全局路径规划的方法经过改进都可以用于局部路径规划，而适用于局部路径规划的 方法也适用于全局路径规划。
[0006] 每一种路径规划方法都具有各自的优点，但总的看来，以上方法都或多或少地存 在着一些问题:如栅格法的规范性和一致性较好，易于实现计算机的建模、存储、处理、更新 于分析，适合于A星算法或Dijkstra算法搜索路径。但对于栅格法及其演化而来的单元树 法，前者在复杂的环境下易引起组合爆炸，后者需要计算单元间的邻接关系，增加了计算 量。分层单元分解法是上述两种环境建模的糅合，可谓是取其所长，避其所短;Khatib提出 的人工势场法，是一种虚拟力法。势场法搜索路径具有启发信息，势场法无需大量的预计 算，就可以自动生成较光滑的路径，但人工势场法把所有信息压缩为单个合力，容易丢弃有 关障碍物分布的有价值信息，故具有陷入局部最小而不能使机器人到达最终目标位置的缺 陷；模糊控制算法将模糊控制本身所具有的鲁棒性与基于生理学上的"感知，动作"行为结 合起来，适用于未知时变环境下的路径规划，实时性较好，但对于必要因素：障碍物的速度 大小和方向，在实际应用中很难得到;基于行为的神经网络法，虽然在真实世界里表现出较 高的鲁棒性、实时性，但当任务和环境变得复杂时，要完全依靠程序员的手工编程实现其基 本行为的设计和组织工作，就变得非常繁重，甚至是不可能的。
[0007] 随着智能计算方法的迅速发展，将智能算法应用到多目标决策问题一一路径规划 中成为一种趋势。智能算法主要包括进化算法，粒子群优化算法和蚁群算法。
【发明内容】
[0008] 本发明针对已有技术存在的不足公开了一种基于椭圆聚类-碰撞锥推演的无人艇 海洋动态避障控制方法。在海洋环境下，无人艇所面对的动态障碍主要是船只，船只的长轴 与短轴比例悬殊，因此本发明对动态船只障碍物进行椭圆聚类;在此基础上，根据无人艇自 身航向、速度和聚类椭圆体线速度、公转速度和自转速度计算无人艇和障碍物船只的碰撞 的运动角度，迭代所有可能碰撞的角度生成碰撞锥，由此计算两者如按照当前运动状态是 否会发生碰撞并进一步采取相应措施。
[0009] 为达到上述目的，本发明采用下述技术方案：
[0010] -种基于碰撞锥推演的无人艇海洋动态避障控制方法，其特征在于操作步骤如 下：
[0011] 第1步在软件中对动态船只障碍物按照其尺寸形态进行聚类成动态椭圆障碍物， 并且米集其运动状态；
[0012] 第2步规划系统初始参数，包括目标距离、目标之间的方位角、质点速度一无人艇 速度、椭圆速度一动态障碍物速度、椭圆长轴、椭圆短轴、质点速度方向一无人艇速度方向、 椭圆速度方向一动态障碍物速度方向；
[0013] 目标距离:无人艇质点与动态船只障碍物中心的距离；
[0014] 目标之间的方位角:动态船只障碍物中心点相对于无人艇质点的方位角；
[0015] 质点速度:无人艇当前的运动速度大小；
[0016] 椭圆速度:动态船只障碍物当前的运动速度大小；
[0017]椭圆长、短轴:动态船只障碍物聚类为椭圆的长短轴参数；
[0018] 质点速度方向:无人艇的运动速度方向；
[0019] 椭圆速度方向：动态船只障碍物的运动速度方向。
[0020] 第3步根据椭圆的相关参数确定椭圆的两个焦点位置一焦点C1和焦点C2,并且计 算两个焦点与无人艇质点之间的方位角；
[0021] 第4步根据椭圆两个焦点与无人艇质点之间的位置关系分别计算出两个焦点与无 人艇质点之间的相对距离；
[0022] 第5步根据椭圆和无人艇质点的运动状态以及位置关系分别计算出无人艇质点与 椭圆中心、焦点C1和焦点C2之间的相对距离变化速度和相对方位角的变化速度；
[0023] 第6步通过以上得出的无人艇质点和椭圆动态障碍物之间的相对参数，利用推演 的点和椭圆之间的碰撞锥计算方法对两者进行碰撞检测，并计算出碰撞锥区间。
[0024] 椭圆边界上的点具有以下特性:边界上的点到达两个焦点的距离之和为椭圆长轴 的两倍。根据椭圆的这个特性，我们可以利用这一点计算无人艇质点和椭圆障碍物在运动 过程中无人艇质点与两个焦点之间距离和的最小值。如果该距离和的最小值大于该椭圆障 碍物的长轴的两倍，则说明无人艇质点在任何时刻都不会与椭圆障碍物相遇而碰撞，相反， 如果该最小值小于该椭圆障碍物的长轴的两倍，则说明无人艇质点会在此刻运动到椭圆内 部，即无人艇会与动态障碍物发生碰撞。
[0025] 第7步通过第6步计算的碰撞结果，依据所得出的碰撞锥区间，无人艇采取相应的 措施：
[0026] 如检测到无人艇和动态障碍物不会发生碰撞，则可不采取措施；
[0027] 如检测到无人艇和动态障碍物会发生碰撞，则可采取以下措施：
[0028] (1)改变无人艇的速度方向，不改变速度大小；依据所计算出的碰撞锥区间，无人 艇的速度方向只要取在该区间之外就可完成避障行为；
[0029] (2)改变无人艇的速度大小，不改变速度方向；无人艇的速度增大或减小可进行有 效的避障;改变速度大小后进行碰撞检测，如检测到不会发生碰撞则说明采取了有效的避 障措施；
[0030] (3)即改变无人艇的速度方向，也改变速度大小;这种措施能更加有效迅速的进行 有效的避障行为。
[0031] 第8步按照动态障碍物的运动状态重复第2步到第7步；当无人艇所搭载的传感器 检测到动态船只障碍物的运动状态发生改变，则应当重新进行碰撞计算。
[0032] 本发明相对于现有的其他无人艇海洋避障方法，具有如下显而易见的突出实质性 特点和显著技术进步：
[0033] (1)本发明公开的无人艇避障方法可以预测无人艇对于动态障碍物的避障行为。
[0034] (2)本发明公开的无人艇避障方法用椭圆对动态障碍物进行聚类，更加适用于无 人