双引擎船自动驾驶仅用GPS呈正弦轨迹，如何搭配传感器和滤波实现直行

你遇到的300米航程内正弦波轨迹，核心是纯GPS导航定位刷新率低、航向数据易受干扰、控制回路滞后于扰动导致的反复过冲修正，按以下方案调整即可解决：

• 工业级MEMS IMU：优先选6轴（加速度计+陀螺仪）型号如ICM20602，预算充足可直接选9轴带地磁的成品模块，用来输出最高200Hz的实时角速度、加速度数据，填补GPS（默认1Hz）更新间隙的姿态、航向推算空白，安装时需远离电机、电调等强电磁干扰源。
• 双引擎转速采集模块：给两套动力系统分别加装转速传感器，或者直接读取电调的转速反馈信号，实时感知双引擎推力差，不需要等GPS检测到航向偏移再做修正，大幅降低控制滞后。
• 可选配件：若作业水域水流、风力扰动较强，可加装流速/风向传感器，用来做扰动前馈控制，进一步降低环境因素带来的轨迹偏移。

• 入门级：互补滤波
  代码实现难度低，STM32算力完全够用，核心是把GPS、地磁输出的低刷新率绝对位置、航向数据，和IMU输出的高刷新率相对姿态数据做加权融合，就能得到100Hz以上的稳定实时状态量，解决纯GPS更新太慢导致的控制滞后问题，90%的场景下用这个就能消除正弦波振荡。
• 进阶级：扩展卡尔曼滤波（EKF）
  如果互补滤波的精度达不到要求，可直接移植开源无人船/无人机的EKF代码，把IMU、GPS、地磁、转速传感器的所有数据都输入滤波框架，得到精度更高、抗干扰能力更强的位置、航向、速度、角速度状态估计。
• 控制优化：给航向控制PID加死区
  不要一检测到航向偏差就调整动力输出，设置1~2度的偏差死区，偏差小于阈值时不做修正，避免传感器噪声带来的频繁动力调整，从根源上减少振荡。

• 参照SE100的配置说明，将GPS的刷新率从默认1Hz调整为5~10Hz，不需要加硬件就能大幅提升控制响应速度。
• 先给内置的HMC5983地磁传感器做硬铁、软铁校准，校准后航向误差可降到1度以内，减少因航向数据不准带来的无效修正。

内容的提问来源于stack exchange，提问作者PYJTER