在做水下机器人控制的时候轨迹跟踪是个绕不开的基本功。给一个AUV或者ROV设定好期望路径比如海底管线巡检的直线段、定深下潜、螺旋搜索控制系统要实时把位置和速度拽回期望值。这里用的控制算法我选了增量式PID并且在纯仿真环境里做了完整的轨迹跟踪验证。这篇文章就把整套思路和操作过程摊开讲从模型简化、控制器设计、参数整定到问题排查都是我实际跑仿真时一步步验证过的东西适合刚接触水下机器人运动控制的同学也适合那些用位置式PID发现调不动、想换思路的同行。1. 为什么轨迹跟踪选择增量式PID1.1 轨迹跟踪要面对的真实约束水下机器人和地面移动机器人最大的区别在于动力学参数随水深深浅、航行速度、姿态角剧烈变化而且执行器螺旋桨、推进器存在明显的饱和与死区特性。轨迹跟踪如果只看几何路径不考虑时间约束那是路径规划的问题不是控制问题。跟踪一定意味着“某个时刻我要到某个位置”所以误差信号是横纵向位置误差、深度误差和航向误差的实时组合。这个场景下控制算法要解决的典型矛盾有三个稳态精度、动态超调、执行机构保护。水下环境里传感器噪声比地面上大得多DVL测速、超短基线定位、深度计采样都有各自的延迟和噪声导致误差信号本身质量就不高。增量式PID在增量层面做运算不直接对绝对误差的历史总和做处理抗数据跳变的能力比位置式PID更容易调出合适手感。1.2 位置式PID和增量式PID的本质差异先摆出两者公式。位置式PID离散化后写为[ u(k) K_p e(k) K_i \sum_{j0}^{k} e(j) T K_d \frac{e(k) - e(k-1)}{T} ]增量式PID则计算控制量的变化量[ \Delta u(k) K_p [e(k) - e(k-1)] K_i T e(k) \frac{K_d}{T} [e(k) - 2e(k-1) e(k-2)] ]实际控制量为 (u(k) u(k-1) \Delta u(k))。对比维度位置式PID增量式PID输出形式绝对控制量控制增量再累加积分项处理误差全程累加易积分饱和增量输出避免直接积分饱和切换手自动需要手动跟踪否则冲击大增量输出天然适合手动/自动无扰切换对误操作容错一次误输出直接驱动大动作只附加一个增量危害小很多计算量小略大但现代MCU毫无压力适用场景执行器可用绝对开度表达的场合带累加型执行器或需要平滑控制的场合水下机器人推进器基本都属于“给定推力/转速指令”的执行器把增量累积在控制器内部相当于给执行器加了一层软缓冲。我最初也试过位置式PID结果在下潜变深度的阶段积分项把推进器推到饱和深度过冲之后要花很长时间才拉回来尤其当期望深度变化比较大的时候特别明显。换增量式之后控制量变化被限幅整体动作平滑很多。2. 先把水下机器人模型简化到能仿真的程度2.1 模型不是越精细越好一谈到水下机器人动力学很多教材直接扔出六自由度方程带附加质量、科氏力、水动力阻尼、浮力恢复项看着很全但仿真阶段真要拿这套东西做控制器调试光是参数辨识就能耗掉一大半时间。我的做法是做两层模型第一层是三自由度水平面运动模型用来调PID参数和验证轨迹跟踪框架第二层才加入垂向自由度把深度和纵倾耦合放进来。水平面模型简化为[ \begin{cases} (m m_x)\dot{u} F_u - X_u u - X_{u|u|} u|u| \ (m m_y)\dot{v} F_v - Y_v v - Y_{v|v|} v|v| \ (I_z m_z)\dot{r} M_r - N_r r - N_{r|r|} r|r| \end{cases} ]这里的 (m_x, m_y, m_z) 是附加质量(X_u, Y_v, N_r) 是线性阻尼系数(X_{u|u|})这类是二次阻力项。实际仿真中二次阻力项在大速度下才明显低速跟踪场景可以先忽略用线性阻尼近似足够。注意附加质量的数值如果拿不准可以用经验公式估算。球体附加质量约为排开水质量的一半细长体沿轴向可以取0.05~0.2倍排水质量横向则接近排水质量。这个精度对控制器参数整定完全够用等真机联调再修正就行。2.2 仿真平台和采样周期的选择我用的仿真环境是某主流商业建模仿真工具搭框架同时用Python写离线版本做批量参数扫描。这里要给新手一个明确建议不要一头扎进复杂3D仿真环境先把连续模型在普通仿真环境里跑通看误差曲线、控制量曲线再考虑可视化和水动力细节。采样周期很关键。水下机器人的运动带宽低横摇纵摇响应慢水平面位置环的期望采样周期在0.1~0.2秒就够。注意这个不等于传感器物理采样频率而是控制器计算周期。我最终把仿真步长设为0.05秒控制器计算周期0.1秒相当于每个控制周期内做两次动力学积分减少离散化误差。2.3 控制器结构和期望轨迹的设定轨迹跟踪控制器我用了两级结构外层是运动学控制器把期望轨迹和当前位置的误差转化为期望速度内层是动力学控制器用增量式PID把期望速度落实到推进器指令。不过在做简化验证时也可以直接让PID面对位置误差输出推力跳过内层速度环。我用后者做了第一步验证简单直接也更接近很多入门教程的写法。期望轨迹我设了三种直线往返、圆形巡航、螺旋下潜。直线用于看稳态误差和收敛时间圆形用于看连续转向时的跟踪能力螺旋线则强制三个通道同时工作能暴露耦合问题。仿真结果中圆形轨迹的稳态误差在0.3米以内螺旋下潜的深度误差在0.15米以内说明参数整定方向是没错的。3. 增量式PID控制器的实现细节3.1 控制律的离散化写法实际编程中增量式PID不需要维护积分累计和只维护前两次误差即可[ \Delta u(k) K_p [e(k) - e(k-1)] K_i T e(k) \frac{K_d}{T} [e(k) - 2e(k-1) e(k-2)] ]对应的Python核心代码class IncrementalPID: def __init__(self, kp, ki, kd, dt): self.kp kp self.ki ki self.kd kd self.dt dt self.priority_err 0.0 self.prev_err 0.0 self.prev_prev_err 0.0 self.u_prev 0.0 def update(self, err): p self.kp * (err - self.prev_err) i self.ki * self.dt * err d self.kd / self.dt * (err - 2 * self.prev_err self.prev_prev_err) delta_u p i d u self.u_prev delta_u u max(-10.0, min(10.0, u)) # 控制量限幅 self.u_prev u self.prev_prev_err self.prev_err self.prev_err err return u注意这段代码里的dt必须传实际控制周期也就是0.1秒。微分项除以dt积分项乘以dt这个不能省。有同学把dt直接省略把Kd调大冒充微分效果结果轨迹一变化就高频抖动这就是单位不一致导致的。3.2 抗积分饱和和输出限幅的双保险很多人说增量式PID天然抗积分饱和这个说法只对了一半。增量式PID的输出在经过限幅后若被截断控制器内部累积的 (u_{prev}) 和实际执行器位置不一致当误差反向时输出会被“卡住”一段时间本质上还是积分饱和的变形。我的做法是双限幅单周期增量限幅(|\Delta u| \leq 2.0)防止微分项在误差突变时产生过大冲击控制量总幅限幅(|u| \leq 10.0)对应推进器的最大推力百分比。两个限幅同时生效实测效果是深度误差从5米阶跃变化时推进器指令不会一下顶到满量程而是以固定斜率上升超调量显著降低回到稳态时间反而缩短因为避免了执行器长时间深饱和后的“反向死区”。3.3 轨迹误差的坐标变换水下机器人的期望轨迹一般是绝对坐标大地坐标系下的但控制器输出的推力要作用在载体坐标系上。所以计算误差时要先把期望位置转换到机器人本体坐标系[ \begin{bmatrix} e_x \ e_y \end{bmatrix}\begin{bmatrix} \cos\psi \sin\psi \ -\sin\psi \cos\psi \end{bmatrix} \begin{bmatrix} x_d - x \ y_d - y \end{bmatrix} ]这个转换经常被人忽略。如果不做坐标变换直接拿大地坐标差算PID当机器人的航向持续变化时即使位置误差为零横向控制通道也可能出现虚假误差导致摇头。我早期仿真中圆形轨迹外圈出现的波浪形速度波动就是忘了做这个旋转后来加上变换才消除。4. 参数整定全流程实录4.1 经典整定法在仿真里的落地方式仿真环境下整定PID参数不需要像真机那样小心翼翼但也不能乱试。我采用了“先P再I后D”的思路第一步把K_i和K_d设为0K_p从0.5开始逐步翻倍观察位置误差曲线是否收敛。如果误差曲线出现等幅振荡说明K_p到了临界值第二步在临界K_p的0.6倍下加入K_i从0.02开始递增观察稳态误差是否向着0收敛。如果出现低频振荡说明K_i过大第三步保持前两个参数加入K_d从0.05起逐步增大观察曲线是否出现高频毛刺。毛刺一旦出现立即回退到上一个值。我最终整定的水平面参数是(K_p 3.2)(K_i 0.08)(K_d 1.5)垂直面参数为(K_p 2.8)(K_i 0.06)(K_d 1.2)。可以明显看到垂直面参数比水平面略低这是因为深度控制有浮力恢复力辅助增益不需要太高。4.2 跟踪误差与控制量同步分析只看误差曲线容易自欺欺人控制量曲线同样重要。我把系统记录的三条曲线放在一起看期望轨迹与实际轨迹、位置误差随时间变化、推进器指令随时间变化。增量式PID调到比较理想状态时有三个标志误差曲线快速收敛没有超过20%的过冲控制量指令在稳态阶段有小幅抖动但频率不高不像高频振荡圆形轨迹的向心加速度需要持续控制输出控制量不会降到零。如果控制量在稳态时完全静止那说明PID退化成纯比例控制后续遇到扰动一定会出现静态误差。适当保留微弱的积分波动是正常的它代表控制器在持续修正误差平均值的漂移。4.3 模型不确定性下的鲁棒性验证一套参数只对某个模型有效这在真机应用里是不够的。我做了一组拉偏测试把附加质量系数上下浮动30%线性阻尼系数上下浮动30%然后看同一组PID参数的跟踪效果。测试结果比较意外附加质量变化对跟踪影响较小圆形轨迹误差从0.3米变到0.35米阻尼系数变化影响较大尤其是二次阻尼项被放大时系统在不同速度条件下呈现相反的超调趋势。这个拉偏测试的结论是增量式PID参数不必追求最优追求“在模型偏差30%范围内依然稳定”更实在。如果要覆盖更宽的工况后续可以考虑模糊PID或者增益调度但在仿真验证这一步先把拉偏测试做扎实比换算法更有价值。5. 常见问题与排查技巧实录5.1 高频振荡误差曲线像锯齿这是我在调参时遇到最多的问题拿位置式PID调的时候尤其明显换成增量式后缓解不少。排查思路先看微分项是否太大。微分项对误差高频分量敏感如果定位传感器有噪声D过大会把噪声放大成推进器抖动。解决方法是先降K_d再看曲线是否平滑。再看采样周期是否过小或者仿真步长是否符合稳定性条件。连续系统离散化后采样周期太小会让控制器对噪声更敏感采样周期太大则延迟严重。我用的0.1秒控制周期在多数情况下是合理的。最后检查传感器模型是否有低通滤波。仿真里很多人忘记给传感器加高斯白噪声导致控制器在理想信号下表现好换到带噪声环境就废。务必在仿真中加入测量噪声哪怕噪声幅值只有真实情况的五分之一也能提前暴露问题。5.2 稳态误差差一点到不了零增量式PID因为输出是增量累积理论上有积分作用稳态误差应该能被消除。实际中出现稳态误差优先排查这几个因素控制器计算出的控制量被限幅了说明执行器推力不够要么增大限幅要么降低期望速度模型里有库仑摩擦死区小控制量被死区吃掉而无法产生推力增量此时PID输出的增量不断累加直到突破死区容易形成极限环。解决方法是加死区补偿或者在前向通道加一个小的常值偏置积分增益K_i太小收敛过程太慢仿真时间不够长导致误差还没落到零就停止了。延长仿真时间看后半段误差是否继续缓慢下降即可判断。5.3 微分冲击设定值突变时控制量瞬间顶到极限这个特别容易在阶梯式期望轨迹上出现。比如深度指令从5米直接跳到10米误差的一阶导瞬间变成一个很大的值微分项输出一个尖峰直接把推进器打到饱和。增量式PID相对好一点但也不是完全没有。我用的处理办法是两点对期望轨迹做“S型”限速而不是阶跃跳跃。具体做法是把期望轨迹生成器内部加一个斜坡限速使期望位姿的变化率有界对误差信号做低通滤波后再送入微分项。但注意滤波时间常数不能太大否则微分作用变成摆设。我用的是一阶低通时间常数0.1秒与控制周期匹配。5.4 仿真结果和真机差距大的原因最后说一个仿真圈很常见的问题仿真里调得好好的参数换到真机上完全不对。我在一开始做这套仿真时也踩了一次后来总结出几个原因执行器动态被忽略。仿真里推力瞬间产生真实推进器有响应延迟大约0.2到0.5秒。在仿真模型里加入一个一阶惯性环节模拟推进器动态比追求更精细的水动力模型更有效传感器延迟没有建模。定位系统输出有几十毫秒到两百毫秒的延迟控制器在这个延迟下稳定性会变差。在仿真里加一个信号延迟模块几乎一定能看到误差曲线变差采样周期不匹配。仿真里控制周期是0.1秒但实际控制板可能因为任务调度跑不稳实际周期忽长忽短。遇到这种情况要么把控制器改成时间戳驱动方式要么加一个定时器管脚来强制控制周期。6. 一点个人体会这套增量式PID轨迹跟踪仿真做下来我最深的感触是控制算法只是系统里很小的一部分真正影响结果的是那一大堆“周边配套”——坐标变换、限幅逻辑、采样周期、传感器噪声、执行器动态。增量式PID本身简单到本科课堂上就能推导完但把它用得顺手、不踩坑需要对水下机器人整个控制链路有完整的认识。最后分享一个扩展技巧在增量式PID框架之上加入一个前馈项——期望加速度乘以惯性参数直接叠加到输出增量上能明显提高圆形轨迹的跟踪精度。这个前馈公式很简单但效果立竿见影。做完再回头看你会发现增量式PID就像系统的一个舵前馈才是指挥方向的那只手。先舵后手循序渐进这套仿真就能成为后续做高级运动控制算法的基础平台。
