[发明专利]一种水下路径规划方法和系统

说明书

本发明公开了一种水下路径规划方法和系统，属于路径规划技术领域。水下环境中水流的影响较大，在水下的障碍物大多不是静止的，而是随着水流运动的同时水下潜航器的运动也会受到水流的影响，本发明充分考虑这些因素，结合水下潜航器的运动学特性、水流的影响、水下潜航器的运行速度以及运行时间等，设计了针对水下环境的代价函数；在进行轨迹优化时，采用BSpline算法结合水流的速度和轨迹的平滑度，能够生成一条安全、动态可行的轨迹，更适用于运行于水下环境的路径规划；并且本发明采用视觉SLAM来进行水下建图，相比声纳更加低廉且更容易获得，成本更低。

技术领域

本发明属于路径规划技术领域，更具体地，涉及一种水下路径规划方法和系统。

背景技术

21世纪以来，随着陆地资源短缺、人口膨胀、环境恶化等问题日益严重，各个国家纷纷把目光投向了海洋，加快对海洋的开发与利用。水下潜航器作为开发海洋，探索海洋的重要工具，非常适合用来进行做水下搜索、水下识别和水下打捞作业。路径规划技术作为水下潜航器的核心部分，路径规划水平的高低标志着水下潜航器智能化水平的高低。因此研究水下潜航器的路径规划算法对于水下潜航器的发展是至关重要的。

目前水下路径规划算法发展十分迅猛，常用于水下的路径规划算法主要有A*算法、RRT算法和Hybrid A*算法等。A*算法是采用欧式几何距离作为代价函数，没有考虑水下潜航器的运动学特性，生成的轨迹往往不符合实际的运动情况。RRT算法在进行路径规划时，没有考虑到水下水流对水下潜航器运动的影响，在真实水下环境中会出现较大的偏差。Hybrid A*算法处理的是静态障碍物，它没有考虑到由于水流的影响，水下的障碍物并不是静止不动的，而是会随着水流一起运动的。因此，设需要设计一种适用于水下环境的路径规划算法。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种水下路径规划方法和系统，其目的在于，针对水下运行环境对现有路径规划和优化算法进行改进，为水下潜航器生成安全、动态可行的轨迹。

为实现上述目的，本发明提供了一种水下路径规划方法，包括：

S1.对水下潜航器进行定位并获得周围环境的点云图；

S2.利用定位设备的位姿信息和点云图构建稠密图；

S3.结合水下潜航器的运动学特性、水流对水下潜航器消耗能量的影响、水下潜航器的运行速度和水下潜航器的运行时间，利用Hybrid A*算法对水下潜航器进行路径规划；

S4.结合障碍物相对水下潜航器的方向和水流方向，利用Bspline算法对规划路径进行优化，得到最终的水下潜航器运行路径；其中，障碍物相对水下潜航器的方向根据稠密图得到。

进一步地，步骤S1具体为，利用视觉传感器对水下潜航器进行定位并获得周围环境的点云图，以降低成本。

进一步地，步骤S2具体为，利用视觉传感器的位姿信息和点云图构建ESDF图，以便对障碍物进行距离和梯度信息的查询，减少计算量。

进一步地，步骤S3具体包括：

01.预测水下潜航器下一个时刻到达节点：

利用代价函数f_total＝λ₁J(T)+λ₂W_c+λ₃T+λ₄F_v选取水下潜航器设定邻域内代价值最小的节点，作为水下潜航器下一个时刻到达节点；