水面航行器的多传感器融合避障和航向自恢复系统及方法

本发明公开了一种水面航行器的多传感器融合避障和航向自恢复系统及方法，其通过毫米波雷达和主动声纳探测器实时采集水面航行器周围环境的多传感器数据；采用多层滤波以及基于空间坐标系转换和时间坐标系对准的时空对齐算法，构建环境感知模型；根据环境感知模型对障碍物运动状态进行判断，并在判定障碍物运动状态的相对速度向后时，计算避障航向角度信息，并基于控制算法解算实际舵角信息；利用水面航行器操舵控制装置或动力装置根据实际舵角信息对航向调整；在避障完成后根据存储的预设航向信息，利用航向控制模块自动恢复至原航线。本发明解决了水面航行器在复杂环境中感知精度不足、避障后航线偏离以及多传感器数据异步融合困难的问题。