一种水下清洗机器人船体表面定位与跟踪方法

本发明涉及一种水下清洗机器人定位与跟踪方法，定位包括通过扩展卡尔曼滤波器将水下清洗机器人上多传感器数据融合，经一阶低通滤波算法过滤传感器测量噪声后获得水下清洗机器人在船体表面的坐标。跟踪方法包括基于定位方法结合迭代线性二次调节器ILQR，实现水下清洗机器人的路径跟踪。本发明创新性提出编码器+IMU+深度计的多传感器融合方案，解决水下清洗机器人贴壁状态下的定位难题。双平面差异化处理，从而更加贴合机器人在船壁或船底的工作状态，使定位更加准确。提出几何距离约束的目标点更新机制，实时调整参考轨迹的最近邻点，解决传统方法因更新速率失配导致的跟踪滞后问题。