一种基于环境感知的水下蛇形机器人路径规划系统

本发明涉及水下机器人自主导航，具体为一种基于环境感知的水下蛇形机器人路径规划系统。

背景技术：

1、现有水下机器人路径规划技术主要沿四条路线发展：基于静态环境地图的经典方法假设环境静止，无法应对动态障碍物；基于传感器实时数据的反应式避障方法仅对当前环境做出响应，缺乏预见性且易陷入局部极小；基于模型预测控制的滚动时域规划方法虽具备一定前瞻能力，但预测时域长度固定，无法根据环境复杂度动态调节，且环境预测仅局限于障碍物位置外推，未系统性纳入洋流演化与风险传播等多种因素；考虑不确定性的鲁棒规划方法虽提升了安全裕度，但将预测误差、洋流波动与定位误差孤立处理，缺乏统一的时空耦合安全度量框架，导致路径过于保守、能耗偏高。

2、综上，现有技术普遍存在三大共性缺陷：一是缺乏对障碍物运动、洋流演变与风险区域扩展的联合预见性决策能力；二是可通行区域评估与安全裕度建模相脱节，对多源不确定性的综合考量不足，路径安全保障机制不完善；三是滚动时域规划窗口固定僵化，缺乏对环境复杂度的自适应调节能力。上述缺陷相互关联、彼此制约，单一环节的局部改进难以根本性解决问题，亟需构建一个将环境感知、演化预测、可通行性评估、安全域构建与自适应滚动规划深度耦合的系统性解决方案，以满足复杂动态水下环境中蛇形机器人自主导航与安全避障的实际需求。

技术实现思路

1、针对现有技术的不足，本发明提供了一种基于环境感知的水下蛇形机器人路径规划系统，具备预见性强，安全裕度充分，时域自适应，兼顾能耗与平滑性，实现复杂水下环境自主导航与安全避障等优点，解决了上述技术问题。

2、为实现上述目的，本发明提供如下技术方案：包括环境感知模块、环境演化预测模块、动态可通行域生成模块、动态安全域构建模块和自适应滚动域规划模块；

3、所述环境感知模块，用于通过多源传感器获取水下蛇形机器人周围环境的实时信息，对各传感器数据进行时间同步与坐标统一，提取环境中的障碍物信息并估计局部洋流速度、流向及环境扰动强度，构建当前时刻的环境状态模型；

4、所述环境演化预测模块，用于根据所述环境状态模型，对动态障碍物未来时刻的位置进行预测，结合声呐观测结果建立局部流场模型并预测未来时刻洋流方向及流速变化趋势，构建环境风险传播模型并对风险区域未来扩展趋势进行预测，形成未来环境状态；

5、所述动态可通行域生成模块，用于根据所述未来环境状态，将环境空间划分为多个离散网格单元，对每个网格单元计算可通行性指标，生成动态可通行域；

6、所述动态安全域构建模块用于在所述动态可通行域的基础上，在每个可通行网格周围引入安全缓冲区，根据障碍物预测误差、洋流波动程度及机器人定位误差动态调整缓冲范围，并将不同预测时刻下的安全域进行时序叠加，构建四维动态安全域；

7、所述自适应滚动域规划模块用于根据环境状态计算环境复杂度指标，根据环境复杂度动态调整预测时域长度，在所述四维动态安全域约束下求解最优路径，并通过滚动优化机制不断更新未来规划路径。

8、优选的，所述环境感知模块搭载的传感器包括前视声呐、多波束声呐、惯性测量单元、多普勒测速仪及深度传感器；所述前视声呐用于探测机器人前方障碍物的位置与轮廓信息，所述多波束声呐用于获取周围环境的三维点云数据，所述多普勒测速仪用于测量机器人相对于海底或水体的运动速度，所述惯性测量单元用于获取姿态角与角速度信息，所述深度传感器用于实时获取机器人所在水深。

9、优选的，所述环境演化预测模块中，针对不同类型的动态障碍物采用不同的运动模型进行预测，所述动态障碍物包括鱼群、漂浮物及其他自主水下航行器。

10、优选的，所述环境演化预测模块将流场因素作为环境约束条件引入预测模型。

11、优选的，所述可通行性指标的表达式如下：

12、；

13、其中，表示网格单元在时刻的综合可通行性指标值；表示归一化障碍物距离因子；表示洋流方向契合度因子；表示归一化风险水平因子；表示感知可信度因子；、、和表示权重系数；表示环境离散网格单元在二维水平平面中的行索引；表示环境离散网格单元在二维水平平面中的列索引；表示当前规划时刻。

14、优选的，所述动态可通行域的表达式如下：

15、；

16、其中，表示时刻的动态可通行域集合；表示环境离散网格单元的平面空间索引；表示网格单元在时刻的综合可通行性指标值；表示预设的可通行阈值。

17、优选的，所述可通行域的安全缓冲半径的表达式如下：

18、；

19、其中，表示在时刻网格单元周围需要预留的安全空间半径；表示障碍物预测位置的不确定性标准差；表示洋流波动程度；表示机器人定位误差标准差；、和为权重系数；表示环境离散网格单元在二维水平平面中的行索引；表示环境离散网格单元在二维水平平面中的列索引；表示当前规划时刻。

20、优选的，所述环境复杂度指标的表达式如下：

21、；

22、其中，表示归一化动态障碍物数量因子；表示归一化障碍物速度方差因子；表示归一化流场变化率因子；表示归一化风险区域占比因子；、、和表示权重系数。

23、优选的，所述根据环境复杂度动态调整预测时域长度，具体为：当环境复杂度指标高于预设高阈值时，采用较短预测窗口，当环境复杂度指标低于预设阈值时采用较长预测窗口。

24、优选的，所述自适应滚动域规划模块的综合优化目标函数为：

25、；

26、其中，表示综合优化目标函数值；为安全性代价；为能耗代价；为路径平滑性代价；表示当前时刻；为动态调整后的预测时域长度；、、分别为权重系数。

27、与现有技术相比，本发明提供了一种基于环境感知的水下蛇形机器人路径规划系统，具备以下有益效果：

28、1、本发明通过自适应滚动时域规划策略，通过环境复杂度指标对动态障碍物密度、运动混乱程度、流场变化剧烈度及风险区域占比进行综合量化评估，据此动态调整预测时域长度。在环境复杂时采用短时域高频重规划以增强响应敏捷性，在环境平稳时采用长时域低频重规划以降低计算开销，实现了规划精度与计算效率的自适应最优匹配，有效节约了水下机器人有限的计算资源。

1.一种基于环境感知的水下蛇形机器人路径规划系统，其特征在于：包括环境感知模块、环境演化预测模块、动态可通行域生成模块、动态安全域构建模块和自适应滚动域规划模块；

2.根据权利要求1所述的一种基于环境感知的水下蛇形机器人路径规划系统，其特征在于：所述环境感知模块搭载的传感器包括前视声呐、多波束声呐、惯性测量单元、多普勒测速仪及深度传感器；所述前视声呐用于探测机器人前方障碍物的位置与轮廓信息，所述多波束声呐用于获取周围环境的三维点云数据，所述多普勒测速仪用于测量机器人相对于海底或水体的运动速度，所述惯性测量单元用于获取姿态角与角速度信息，所述深度传感器用于实时获取机器人所在水深。

3.根据权利要求1所述的一种基于环境感知的水下蛇形机器人路径规划系统，其特征在于：所述环境演化预测模块中，针对不同类型的动态障碍物采用不同的运动模型进行预测，所述动态障碍物包括鱼群、漂浮物及其他自主水下航行器。

4.根据权利要求1所述的一种基于环境感知的水下蛇形机器人路径规划系统，其特征在于：所述环境演化预测模块将流场因素作为环境约束条件引入预测模型。

5.根据权利要求1所述的一种基于环境感知的水下蛇形机器人路径规划系统，其特征在于：所述可通行性指标的表达式如下：

6.根据权利要求1所述的一种基于环境感知的水下蛇形机器人路径规划系统，其特征在于：所述动态可通行域的表达式如下：

7.根据权利要求1所述的一种基于环境感知的水下蛇形机器人路径规划系统，其特征在于：所述可通行域的安全缓冲半径的表达式如下：

8.根据权利要求1所述的一种基于环境感知的水下蛇形机器人路径规划系统，其特征在于：所述环境复杂度指标的表达式如下：

9.根据权利要求1所述的一种基于环境感知的水下蛇形机器人路径规划系统，其特征在于：所述根据环境复杂度动态调整预测时域长度，具体为：当环境复杂度指标高于预设高阈值时，采用较短预测窗口，当环境复杂度指标低于预设阈值时采用较长预测窗口。

10.根据权利要求1所述的一种基于环境感知的水下蛇形机器人路径规划系统，其特征在于：所述自适应滚动域规划模块的综合优化目标函数为：