简介运动控制是无人系统自主作业的核心环节PID控制作为经典且高效的反馈控制策略在工程实践中占据主导地位。在移动机器人领域轨迹跟踪与制导律设计决定了系统能否准确执行任务。水面无人艇USV作为典型的水上无人平台其运动建模精度直接影响控制器设计——通过三自由度模型与Nomoto模型对动力学进行近似结合LOS制导律将跟踪误差转化为期望航向再交由串级PID实现底层控制。这一技术链路不仅适用于USV也可推广至AUV及各类移动机器人。本文从系统建模、制导律设计到实船调参系统梳理了水面无人艇运动控制项目的完整技术路线与应用场景为相关开发人员提供工程参考。 前阵子把一个水面无人艇的运动控制项目完整跑通了从最开始的运动学建模、动力学参数辨识到后面的LOS制导、串级PID轨迹跟踪再到实船调试时踩过的各种坑前后折腾了两个月。这个项目最核心的一条链路就是水面无人艇的数学模型建到什么精度轨迹跟踪的控制律怎么设计以及PID参数在实际工程里到底怎么调才不飘。今天把整条技术路线和实操过程完整拆开讲一遍如果你正在做USV、AUV或者任何移动机器人的运动控制这篇内容应该能帮你省下不少试错时间。这个项目要做的事简单说就是让一艘1.6米长、排水量40公斤左右的无人艇在GPS导航下沿着预设的航迹点自动巡航横向偏差控制在1米以内。整个控制系统的核心由三块组成系统建模、轨迹跟踪、PID控制。三者不是孤立的模型是控制器设计的基础轨迹跟踪是控制目标PID是最底层的执行手段。适合人群很明确船舶自动化、机器人控制方向的学生以及想快速把理论控制算法落地到真实硬件上的工程师。1. 整体设计思路为什么从“建模”开始而不是直接调PID1.1 这个项目到底在做什么很多刚接触无人艇的人第一反应是“不就是加个GPS然后调个PID让它走直线吗”真做起来完全不是这么回事。水面无人艇的运动控制本质上是一个典型的分层控制问题导航层负责路径规划和任务理解制导层负责把“沿着这条路径走”翻译成“该朝哪个方向开”控制层才轮到PID去具体执行航向和速度的调节。整个项目的逻辑链路是这样的GPS和IMU采集位置、姿态、速度信息经过状态估计之后送入制导模块制导模块根据当前船位与目标航迹之间偏差计算出期望航向这个期望航向再传给底层PID控制器PID算出差速或舵角指令最终作用到左右两个推进器上。值得强调的是建模在最前面的地位。控制器的品质上限是由模型精度决定的这句话在真实工程里体会特别深。一开始我图省事跳过建模直接按经验调PID结果在湖面试航时船一直在目标航线两侧来回横摆怎么调参数都压不住。后来老老实实把模型建出来才发现问题出在转艏动力学的时间常数远大于我预估的值PID参数一直按错量级在调自然调不出来。1.2 为什么PID在今天依然是无人艇控制的主力这年头一提控制算法很多人张嘴就是LQR、MPC、滑模、自适应但实际上商用无人艇和大量科研平台最底层的控制环用的依然还是PID。原因很直接PID不依赖精确模型调参直观算力开销几乎可以忽略而且鲁棒性在绝大多数工况下都够用。这个项目里我也对比过LQR轨迹跟踪方案。LQR的优势在于可以显式处理多变量耦合能直接对状态向量做全状态反馈从控制理论的角度看确实更“高级”。但落地的时候问题就来了LQR需要比较准确的状态空间模型而且加权矩阵Q、R的整定不像PID那样有清晰的物理语义调起来全凭感觉。实际测试下来在模型参数存在明显误差的情况下LQR的跟踪性能反而不如一个调好的PID稳定。当然PID不是万能的这个在后面会详细说。但在水面无人艇这个场景下PID作为底层执行控制器是性价比最高的选择上层可以用更高级的制导律来弥补PID在非线性跟踪上的不足。1.3 控制架构与信息流向整个控制系统采用串级结构这个设计是整个项目最关键的决策之一。串级控制的好处是把不同时间尺度的控制任务分开处理外环是慢回路处理位置和速度跟踪内环是快回路处理航向和航向率。信息流向是GPS输出位置误差制导律将位置误差转换为期望航向角期望航向角与当前航向角的偏差送入航向PID外环外环输出期望转艏率转艏率偏差送入内环内环输出差速或舵角指令给推进器。这样的分层让每一级控制器的任务都变得非常单纯也方便分步调试。我用的控制结构是位置环加航向环的典型组合位置环实际上是一个比例控制器把横向跟踪误差映射为期望航向的修正量本质上就是后面要讲的LOS制导律。航向环则是标准的PID结构。这种架构在工程上非常成熟调试的时候也是先内环后外环一环一环确认无误后再整体联动排查问题非常方便。2. 系统建模把物理世界变成控制器能读懂的数学2.1 从六自由度到三自由度简化模型的边界水面无人艇在实际水面上运动是六个自由度纵荡、横荡、垂荡、横摇、纵摇和艏摇。如果全部建模物理量之间的耦合关系会让模型复杂到难以用于控制器设计。好在无人艇绝大部分时间在静水或缓流水域航行垂荡、横摇、纵摇这三者要么幅度很小要么对水平面内的运动影响有限可以安全地忽略。所以项目里采用了经典的三自由度模型只保留纵荡、横荡和艏摇。这是一个非常关键的工程取舍必须清楚它的边界条件当风浪等级超过3级、船体出现明显横摇时这个简化模型的误差会急剧增大航向控制精度会肉眼可见地下降。这种情况下就需要考虑横摇和纵摇耦合的完整模型了。从方法论角度多说一句这种“先限定场景、再选择模型复杂度”的思路在很多工程领域都是一样的。比如水文领域做HEC-HMS水文建模也是先把复杂的水文过程抽象成降雨、产流、汇流这几个核心环节的数学关系再在特定流域场景下反复率定参数而不是一上来就追求全物理过程模拟。建模的本质都是“在可接受的精度下做最合理的简化”。2.2 运动学模型坐标变换是轨迹跟踪的命根子运动学模型描述的是位置和姿态随时间的变化关系不涉及力的分析。先定义两个坐标系大地坐标系NED系北东地系固定在岸基参考点上船体坐标系固定在船体上。这两个坐标系之间的变换关系是轨迹跟踪的数学基础很多跟踪误差算不准的问题都出在这一步。三自由度模型的运动学方程写成矩阵形式η̇ R(ψ)ν其中η [x, y, ψ]ᵀ是大地坐标系下的位置和艏向角ν [u, v, r]ᵀ是船体坐标系下的纵荡速度、横荡速度和转艏角速度R(ψ)是旋转矩阵R(ψ) [cosψ -sinψ 0; sinψ cosψ 0; 0 0 1]展开就是三个标量方程ẋ u·cosψ - v·sinψ ẏ u·sinψ v·cosψ ψ̇ r第二个式子特别值得停下来看船的实际运动方向是纵荡速度和横荡速度的矢量叠加而不是沿船头方向。这就是为什么在侧风或者水流干扰下船会呈现“漂移”状态——船头指的方向和实际运动方向并不一致。轨迹跟踪中计算跟踪误差时如果不考虑这个横漂就会在转弯时出现明显的过冲问题。2.3 动力学模型惯性、科氏力、阻尼和推进力动力学模型描述的是力与运动之间的关系是三自由度模型中最核心的方程。完整形式是Mν̇ C(ν)ν D(ν)ν τM矩阵包含刚体惯性矩阵和附加质量矩阵的叠加。刚体惯性是船本身的物理质量特性附加质量则来自船体加速运动时带动周围水体的效应——可以理解为船在水中运动时相当于带着一团水一起动这团水的等效质量甚至能占到船体自身质量的20%到30%。C矩阵是科氏力和向心力矩阵来源于船体转动和线速度之间的耦合。D矩阵是阻尼矩阵包含线性阻尼和二次阻尼项。τ是推进器产生的控制力和力矩。对于水面无人艇阻尼矩阵在对角方向上的占优特性比较明显工程上可以近似为对角形式。纵荡方向的阻尼主要是兴波阻力横荡方向主要是横向阻力艏摇方向主要是转艏阻尼力矩。这些参数我通过水上自由衰减实验做了辨识具体方法后面会展开。2.4 从模型到控制器工程上常用的模型化简完整的非线性三自由度模型直接用于PID设计太过复杂。工程上最常用的做法是进一步化简。第一个化简是假定纵荡速度恒定即船以恒定的巡航速度前进这在很多USV任务场景中是合理的预设。这样一来横荡方向的动力学退化为一个以转艏角速度为自变量的方程纵荡和横荡之间的耦合被一定程度上解耦。第二个化简是采用一阶Nomoto模型近似描述航向响应特性r/δ K/(1Ts)这里 δ 是舵角或差速指令K是静态增益转艏速度对控制输入的稳态响应T是时间常数反映从指令到响应到达63.2%所需的时间。Nomoto模型虽然极其简单但在航向控制器的设计上非常实用它直接告诉了你两个关键参数系统控制增益有多大、响应有多快。我在湖面试航时通过阶跃响应实验测得的K约为0.8T约为2.5秒这两个参数直接指导了PID初值的估算。3. 轨迹跟踪控制设计任务分解与制导律3.1 轨迹跟踪任务的本质轨迹跟踪问题可以这样描述给定一条参数化路径或者一条随时间变化的轨迹控制无人艇实际航行轨迹与该目标轨迹之间的偏差收敛到零。在实际任务中水面无人艇通常是沿预设航迹点巡航作业所以路径跟踪比轨迹跟踪更常见——它只要求船沿几何路径航行不强制时间同步。但不管是路径跟踪还是轨迹跟踪控制任务都可以拆成两级制导级负责根据当前位置与期望路径的关系生成期望航向控制级负责驱动船实际跟踪这个航向。这个解耦思路非常关键它让高层的制导律可以用较复杂的算法而底层依然用简单可靠的PID。3.2 LOS制导律把跟踪问题转换成航向跟踪我采用的是视线制导律Line of SightLOS。LOS的思路非常直观在期望路径上向前看一个距离Δ取一个前视点然后把船当前位置到前视点的连线方向作为期望航向。船往这个方向开就会自然地向路径收敛。设横向跟踪误差为 ye则期望航向角为ψ_d ψ_p atan2(-ye, Δ)这里 ψ_p 是期望路径的切线方向Δ 是前视距离。这个公式的逻辑非常美当横向误差为正船在路径右侧时期望航向会向左偏船就会回到路径上当横向误差为零时期望航向就是切线方向船沿路径直线行驶。横向误差的动力学方程为ẏe U·sin(ψ - ψ_p)其中 U 是合速度。可以看出如果直接令 ψ ψ_d代入后。当 Δ 为正数时ye 会指数收敛到零收敛速度由 Δ 决定。Δ 越大收敛越平缓对路径的切入越柔和Δ 越小收敛越快但容易造成航向超调和振荡。实际操作中我根据船速动态调整Δ船速1.5m/s时设Δ为8到10米转弯半径相对较大的场景会适当加大到12米以上。如果Δ固定不死会出现在湖面小半径转弯时严重切弯的问题。3.3 PID控制与LQR的取舍在项目推进过程中我把PID和LQR轨迹跟踪方案都实际验证了一遍两种算法各自的适用场景很明显。PID控制的优点前面已经提过模型依赖弱、实现简单、调参直观。缺点是处理多变量耦合问题比较吃力而且固定PID参数在工况变化较大的场景下适应性有限。LQR轨迹跟踪需要把非线性模型在平衡点线性化得到状态空间方程后用线性二次型最优控制方法设计反馈增益。LQR的优点是理论上保证了最优性并且在仿真条件下跟踪精度很高。但问题在于线性化模型只在平衡点附近有效一旦偏离太大LQR的性能会明显下降而且模型参数不准确时反馈增益的鲁棒性较差。我最终的方案是用LOS制导加串级PID作为主力控制方案这个组合在湖面试航中横向偏差实测稳定在0.6米以内。LQR方案作为性能对比基准留在了半物理仿真环境中用于验证不同工况下的表现差异。如果你想做更深入的对比验证强烈建议在同一套仿真环境中设置相同的扰动和噪声条件否则对比结果没有意义。4. PID控制器的实现与调参实战4.1 串级PID结构设计底层PID控制器要解决的核心问题是航向跟踪我在实际项目中采用了串级PID结构外环航向PID接收期望航向角输出期望转艏率内环转艏率PID接收期望转艏率输出差速指令。这个结构比单环PID多了一个内环对抑制转艏率的突变有显著改善。串级结构的原理不复杂但工程意义重大。水面上的扰动风、浪、流对转艏率的影响比对航向角的影响更直接、更快速内环对这个快变量的及时纠正让外环压力大大降低。结构示意图上可以这样理解外环是比较慢的闭环带宽约0.2Hz内环是快闭环带宽可以做到1Hz以上。两层闭环在频域上明显分离调参时互不干扰这是串级控制的精髓。单环PID虽然也能用但是面对湖面阵风或船后浪的突发扰动时航向角偏差已经很明显才开始修正动态响应慢半拍。串级PID的抗扰能力明显要好一个档次。4.2 从理论公式到嵌入式的代码实现我在嵌入式平台上实现的是位置式PID采样周期50ms直接对误差信号做比例、积分、微分运算。离散化之后的核心代码段如下float pid_calculate(pid_t *pid, float ref, float feedback) { float err ref - feedback; float output; // 角度误差归一化到 [-PI, PI]处理航向角跳变 while (err PI) err - 2.0f * PI; while (err -PI) err 2.0f * PI; pid-err_last pid-err_prev; pid-err_prev err; // 比例项 float P_out pid-Kp * err; // 积分项带抗积分饱和处理 pid-integral err * pid-dt; pid-integral constrain(pid-integral, -pid-lim_it, pid-lim_it); float I_out pid-Ki * pid-integral; // 微分项带一阶低通滤波 float derivative (err - pid-err_prev) / pid-dt; pid-deriv_lpf pid-lpf_alpha * (derivative - pid-deriv_lpf); float D_out pid-Kd * pid-deriv_lpf; output P_out I_out D_out; output constrain(output, -pid-lim_out, pid-lim_out); return output; }这里有几个关键点几乎是所有入门者都会踩的坑逐一说清楚。航向角误差归一化是必须处理的。航向角是周期量179度和-179度在数值上差了358度但实际的角偏差只是2度。如果不做归一化控制器会在航向角跨越0度边界时输出一个巨大的错误控制量船会突然打满舵场面很惊险。这个坑我在仿真时没遇到实船第一次跑到这个位置时整艘船猛转了一圈数据回看才定位到问题。积分限幅和输出限幅都是为了防饱和。积分饱和是PID控制里最常见的工程问题——船在转向过程中受水流阻碍达不到期望速度积分项会持续累积一旦障碍消除输出量已经远超执行器上限导致严重超调。输出限幅则是保护推进器和舵机的单片机的PWM输出范围是有限制的控制量突然过大对执行器的冲击也很大。微分项低通滤波单独说一下水面实测中GPS航向数据本身就有噪声IMU数据虽然好一些但也有高频抖动。微分项对噪声极其敏感不滤波的话高频分量会被放大几十倍控制量会像锯齿一样抖动。我用的截止频率是5Hz的一阶低通滤波器实测下来效果很好。4.3 裸机环境下的PID实时性问题项目的主控用的是一颗STM32F407我在裸机环境下跑控制循环没有上RTOS。在这种“裸机PID控制电机”的模式下实时性完全靠定时器中断保证。我用了定时器8产生50ms的更新中断在中断服务函数里完成数据采集、PID计算和PWM输出更新。这里有个非常重要的细节控制循环的执行时间抖动必须严格控制。即使中断优先级一致、中断服务函数执行时间基本恒定一旦有其他中断或串口任务抢占了执行时间PID计算的节拍就会产生抖动导致微分项计算不准。所以我把IMU读取、GPS数据解析、PID计算和PWM输出全部放在同一个高优先级中断里主循环只负责状态上报和指令接收实测下来时间抖动控制在1毫秒以内。如果你打算在一个线程里跑PID同时又用另一个线程处理日志存储和网络通讯一定要确认定时任务的调度确定性。很多PID在仿真里效果很好、实船就飘原因不是算法错了而是控制周期不稳定导致微分项的dt不准确。4.4 调参流程先内环后外环先P后I再D调参是整个项目里最考验耐心、也最依赖经验的部分。我的调参顺序严格遵循“先内环后外环、先P后I再D”的原则每一环节都有明确的测试方法和判据。内环转艏率PID的调参方法是给一个固定的转艏率阶跃指令观察实际转艏率的响应。先把Ki和Kd清零只留比例项Kp。从小到大慢慢加大Kp观察响应曲线直到出现等幅振荡记下此时的临界增益Ku再取Ku的45%到60%作为实际Kp。然后加一点点Ki消除静差Kd先不加确认内环无振荡后再逐一微调。外环航向PID的调参方法类似给一个期望航向的阶跃指令观察航向角响应。同样先P后I再D找到合适的临界增益后回退。需要注意的是内环和外环的带宽要有明显差异外环的响应速度比内环慢3到5倍是最合理的否则两个闭环会互相干扰系统容易出现低频振荡。我的最终参数只做一个参考内环 Kp1.8Ki0.15Kd0.4外环 Kp2.2Ki0.05Kd0.6。这只是针对我这艘艇三体船型的参数船型变了、推进器动力变了参数肯定要重新调。但整个调参方法是可以直接复用的尤其是“临界增益回退法”在真实系统上非常可靠。5. 实船调试的常见问题与排查实录5.1 积分饱和转向时突然“抽风”的元凶最早的水面测试中船在规定航线上出现了非常诡异的运动模式转大弯时一切正常但恢复直行后船会先大幅度向反方向偏然后才慢慢回正整个过程中控制量饱和时间持续了好几秒。回看控制量日志发现积分项已经累积到了限幅值的两三倍但因为没有限幅输出的控制量早就超出了推进器的实际执行上限。解决方法是积分分离与限幅并用。一方面给积分项设置独立的限幅值我设置了输出限幅的60%左右另一方面当误差超过某阈值时停止积分累积误差进入一定范围后再恢复积分。这个阈值一般取期望航向误差5度以内。用这两个方法后“抽风”现象彻底消失。5.2 GPS航向噪声与微分项放大GPS输出的航向角在静止状态下有±0.5度左右的噪声航向角速度的计算值噪声会更大直接使用的话微分项输出会高频抖动。第一次实船测试舵机有明显啸叫轴承也有异常磨损风险。排查时先怀疑是PWM频率问题后来把传感器原始遥测数据拉出来分析才发现噪声是源头。解决思路有两条一是传感器硬件上用IMU融合的航向角替代GPS原始航向角IMU的航向角在静止时噪声可以压到±0.1度以内二是在PID微分项前端加低通滤波切断高频分量的放大通路。两者结合之后舵机的声音立刻安静了很多。5.3 波浪扰动下的航向抖动湖面情况不可能一直平静稍有风浪时船的航向就会出现周期性抖动。最先出现的情况是航向在期望值附近以大约0.3Hz的频率来回摆动幅度在±3度左右PID参数越调越激进反而振荡越厉害。后来意识到这是典型的波浪低频扰动问题。解决方案是适当降低PID外环的增益让控制系统不过度响应波浪这类高频扰动同时适当增加内环的阻尼使整体的高频响应被压住。还有一个非常有效的辅助手段是增加一个前馈补偿当GPS检测到船的横向漂移速度时提前给一个航向修正量等偏差出来了再纠正效果会好很多。5.4 模型参数误差太大时怎么办实测中船模参数是从理论公式估算的然后通过自由衰减实验做了修正。但水面环境复杂同一个水域的流速分布也不均匀模型参数很难做到全工况精确。这时PID的鲁棒性优势就体现出来了——即使模型有20%到30%的误差一个调好的PID仍然能维持可接受的跟踪性能。如果误差再大就要考虑在线辨识和参数自适应了。我在这个项目里做了一个简单的水速在线估计器把水流干扰当作一个低频扰动估计出来并补偿到控制量中。这个策略对轨迹跟踪精度的提升非常明显横向偏差从0.9米改善到了0.5米左右。5.5 问题排查速查表现象可能原因排查方法解决方案航向角跨0度时船猛打舵角度误差未归一化查看控制量日志是否出现突变误差归一化到[-PI, PI]转向后恢复直行时反向偏转积分饱和查看积分项是否超限积分限幅积分分离舵机高频抖动有啸叫微分项放大噪声检查传感器噪声频谱低通滤波用IMU航向风浪中航向周期摆动波浪扰动被过度响应观察扰动频率与系统带宽降低增益加前馈补偿控制量打满但动作缓慢执行器响应延迟测量实际PWM到动作延迟加入延迟补偿环节转弯时严重切弯LOS前视距离过小观察误差与转弯半径关系动态调整前视距离6. 一点个人体会整个项目做下来我最大的感触是水面无人艇的控制并没有想象中那么玄乎但也没有任何一步可以跳过。把系统建模做扎实比反复调PID参数管用得多把制导律设计清楚比盲目堆控制算法管用得多。PID参数调不出来的时候优先去查模型和时间常数而不是怀疑代码写错了。如果你正准备开始类似的项目我的建议是先花一周时间把模型建清楚、把Nomoto模型参数测出来再花一周时间把仿真环境搭好最后再上船实测。前面慢就是后面快这个顺序不要反。后续如果想继续提升跟踪精度可以在制导层引入自适应前馈或者在控制层做模糊PID切换底层的串级PID框架完全可以平滑扩展不需要推翻重来。本文还有配套的精品资源点击获取 水面无人艇运动控制：从系统建模到串级PID调参实战
简介运动控制是无人系统自主作业的核心环节PID控制作为经典且高效的反馈控制策略在工程实践中占据主导地位。在移动机器人领域轨迹跟踪与制导律设计决定了系统能否准确执行任务。水面无人艇USV作为典型的水上无人平台其运动建模精度直接影响控制器设计——通过三自由度模型与Nomoto模型对动力学进行近似结合LOS制导律将跟踪误差转化为期望航向再交由串级PID实现底层控制。这一技术链路不仅适用于USV也可推广至AUV及各类移动机器人。本文从系统建模、制导律设计到实船调参系统梳理了水面无人艇运动控制项目的完整技术路线与应用场景为相关开发人员提供工程参考。 前阵子把一个水面无人艇的运动控制项目完整跑通了从最开始的运动学建模、动力学参数辨识到后面的LOS制导、串级PID轨迹跟踪再到实船调试时踩过的各种坑前后折腾了两个月。这个项目最核心的一条链路就是水面无人艇的数学模型建到什么精度轨迹跟踪的控制律怎么设计以及PID参数在实际工程里到底怎么调才不飘。今天把整条技术路线和实操过程完整拆开讲一遍如果你正在做USV、AUV或者任何移动机器人的运动控制这篇内容应该能帮你省下不少试错时间。这个项目要做的事简单说就是让一艘1.6米长、排水量40公斤左右的无人艇在GPS导航下沿着预设的航迹点自动巡航横向偏差控制在1米以内。整个控制系统的核心由三块组成系统建模、轨迹跟踪、PID控制。三者不是孤立的模型是控制器设计的基础轨迹跟踪是控制目标PID是最底层的执行手段。适合人群很明确船舶自动化、机器人控制方向的学生以及想快速把理论控制算法落地到真实硬件上的工程师。1. 整体设计思路为什么从“建模”开始而不是直接调PID1.1 这个项目到底在做什么很多刚接触无人艇的人第一反应是“不就是加个GPS然后调个PID让它走直线吗”真做起来完全不是这么回事。水面无人艇的运动控制本质上是一个典型的分层控制问题导航层负责路径规划和任务理解制导层负责把“沿着这条路径走”翻译成“该朝哪个方向开”控制层才轮到PID去具体执行航向和速度的调节。整个项目的逻辑链路是这样的GPS和IMU采集位置、姿态、速度信息经过状态估计之后送入制导模块制导模块根据当前船位与目标航迹之间偏差计算出期望航向这个期望航向再传给底层PID控制器PID算出差速或舵角指令最终作用到左右两个推进器上。值得强调的是建模在最前面的地位。控制器的品质上限是由模型精度决定的这句话在真实工程里体会特别深。一开始我图省事跳过建模直接按经验调PID结果在湖面试航时船一直在目标航线两侧来回横摆怎么调参数都压不住。后来老老实实把模型建出来才发现问题出在转艏动力学的时间常数远大于我预估的值PID参数一直按错量级在调自然调不出来。1.2 为什么PID在今天依然是无人艇控制的主力这年头一提控制算法很多人张嘴就是LQR、MPC、滑模、自适应但实际上商用无人艇和大量科研平台最底层的控制环用的依然还是PID。原因很直接PID不依赖精确模型调参直观算力开销几乎可以忽略而且鲁棒性在绝大多数工况下都够用。这个项目里我也对比过LQR轨迹跟踪方案。LQR的优势在于可以显式处理多变量耦合能直接对状态向量做全状态反馈从控制理论的角度看确实更“高级”。但落地的时候问题就来了LQR需要比较准确的状态空间模型而且加权矩阵Q、R的整定不像PID那样有清晰的物理语义调起来全凭感觉。实际测试下来在模型参数存在明显误差的情况下LQR的跟踪性能反而不如一个调好的PID稳定。当然PID不是万能的这个在后面会详细说。但在水面无人艇这个场景下PID作为底层执行控制器是性价比最高的选择上层可以用更高级的制导律来弥补PID在非线性跟踪上的不足。1.3 控制架构与信息流向整个控制系统采用串级结构这个设计是整个项目最关键的决策之一。串级控制的好处是把不同时间尺度的控制任务分开处理外环是慢回路处理位置和速度跟踪内环是快回路处理航向和航向率。信息流向是GPS输出位置误差制导律将位置误差转换为期望航向角期望航向角与当前航向角的偏差送入航向PID外环外环输出期望转艏率转艏率偏差送入内环内环输出差速或舵角指令给推进器。这样的分层让每一级控制器的任务都变得非常单纯也方便分步调试。我用的控制结构是位置环加航向环的典型组合位置环实际上是一个比例控制器把横向跟踪误差映射为期望航向的修正量本质上就是后面要讲的LOS制导律。航向环则是标准的PID结构。这种架构在工程上非常成熟调试的时候也是先内环后外环一环一环确认无误后再整体联动排查问题非常方便。2. 系统建模把物理世界变成控制器能读懂的数学2.1 从六自由度到三自由度简化模型的边界水面无人艇在实际水面上运动是六个自由度纵荡、横荡、垂荡、横摇、纵摇和艏摇。如果全部建模物理量之间的耦合关系会让模型复杂到难以用于控制器设计。好在无人艇绝大部分时间在静水或缓流水域航行垂荡、横摇、纵摇这三者要么幅度很小要么对水平面内的运动影响有限可以安全地忽略。所以项目里采用了经典的三自由度模型只保留纵荡、横荡和艏摇。这是一个非常关键的工程取舍必须清楚它的边界条件当风浪等级超过3级、船体出现明显横摇时这个简化模型的误差会急剧增大航向控制精度会肉眼可见地下降。这种情况下就需要考虑横摇和纵摇耦合的完整模型了。从方法论角度多说一句这种“先限定场景、再选择模型复杂度”的思路在很多工程领域都是一样的。比如水文领域做HEC-HMS水文建模也是先把复杂的水文过程抽象成降雨、产流、汇流这几个核心环节的数学关系再在特定流域场景下反复率定参数而不是一上来就追求全物理过程模拟。建模的本质都是“在可接受的精度下做最合理的简化”。2.2 运动学模型坐标变换是轨迹跟踪的命根子运动学模型描述的是位置和姿态随时间的变化关系不涉及力的分析。先定义两个坐标系大地坐标系NED系北东地系固定在岸基参考点上船体坐标系固定在船体上。这两个坐标系之间的变换关系是轨迹跟踪的数学基础很多跟踪误差算不准的问题都出在这一步。三自由度模型的运动学方程写成矩阵形式η̇ R(ψ)ν其中η [x, y, ψ]ᵀ是大地坐标系下的位置和艏向角ν [u, v, r]ᵀ是船体坐标系下的纵荡速度、横荡速度和转艏角速度R(ψ)是旋转矩阵R(ψ) [cosψ -sinψ 0; sinψ cosψ 0; 0 0 1]展开就是三个标量方程ẋ u·cosψ - v·sinψ ẏ u·sinψ v·cosψ ψ̇ r第二个式子特别值得停下来看船的实际运动方向是纵荡速度和横荡速度的矢量叠加而不是沿船头方向。这就是为什么在侧风或者水流干扰下船会呈现“漂移”状态——船头指的方向和实际运动方向并不一致。轨迹跟踪中计算跟踪误差时如果不考虑这个横漂就会在转弯时出现明显的过冲问题。2.3 动力学模型惯性、科氏力、阻尼和推进力动力学模型描述的是力与运动之间的关系是三自由度模型中最核心的方程。完整形式是Mν̇ C(ν)ν D(ν)ν τM矩阵包含刚体惯性矩阵和附加质量矩阵的叠加。刚体惯性是船本身的物理质量特性附加质量则来自船体加速运动时带动周围水体的效应——可以理解为船在水中运动时相当于带着一团水一起动这团水的等效质量甚至能占到船体自身质量的20%到30%。C矩阵是科氏力和向心力矩阵来源于船体转动和线速度之间的耦合。D矩阵是阻尼矩阵包含线性阻尼和二次阻尼项。τ是推进器产生的控制力和力矩。对于水面无人艇阻尼矩阵在对角方向上的占优特性比较明显工程上可以近似为对角形式。纵荡方向的阻尼主要是兴波阻力横荡方向主要是横向阻力艏摇方向主要是转艏阻尼力矩。这些参数我通过水上自由衰减实验做了辨识具体方法后面会展开。2.4 从模型到控制器工程上常用的模型化简完整的非线性三自由度模型直接用于PID设计太过复杂。工程上最常用的做法是进一步化简。第一个化简是假定纵荡速度恒定即船以恒定的巡航速度前进这在很多USV任务场景中是合理的预设。这样一来横荡方向的动力学退化为一个以转艏角速度为自变量的方程纵荡和横荡之间的耦合被一定程度上解耦。第二个化简是采用一阶Nomoto模型近似描述航向响应特性r/δ K/(1Ts)这里 δ 是舵角或差速指令K是静态增益转艏速度对控制输入的稳态响应T是时间常数反映从指令到响应到达63.2%所需的时间。Nomoto模型虽然极其简单但在航向控制器的设计上非常实用它直接告诉了你两个关键参数系统控制增益有多大、响应有多快。我在湖面试航时通过阶跃响应实验测得的K约为0.8T约为2.5秒这两个参数直接指导了PID初值的估算。3. 轨迹跟踪控制设计任务分解与制导律3.1 轨迹跟踪任务的本质轨迹跟踪问题可以这样描述给定一条参数化路径或者一条随时间变化的轨迹控制无人艇实际航行轨迹与该目标轨迹之间的偏差收敛到零。在实际任务中水面无人艇通常是沿预设航迹点巡航作业所以路径跟踪比轨迹跟踪更常见——它只要求船沿几何路径航行不强制时间同步。但不管是路径跟踪还是轨迹跟踪控制任务都可以拆成两级制导级负责根据当前位置与期望路径的关系生成期望航向控制级负责驱动船实际跟踪这个航向。这个解耦思路非常关键它让高层的制导律可以用较复杂的算法而底层依然用简单可靠的PID。3.2 LOS制导律把跟踪问题转换成航向跟踪我采用的是视线制导律Line of SightLOS。LOS的思路非常直观在期望路径上向前看一个距离Δ取一个前视点然后把船当前位置到前视点的连线方向作为期望航向。船往这个方向开就会自然地向路径收敛。设横向跟踪误差为 ye则期望航向角为ψ_d ψ_p atan2(-ye, Δ)这里 ψ_p 是期望路径的切线方向Δ 是前视距离。这个公式的逻辑非常美当横向误差为正船在路径右侧时期望航向会向左偏船就会回到路径上当横向误差为零时期望航向就是切线方向船沿路径直线行驶。横向误差的动力学方程为ẏe U·sin(ψ - ψ_p)其中 U 是合速度。可以看出如果直接令 ψ ψ_d代入后。当 Δ 为正数时ye 会指数收敛到零收敛速度由 Δ 决定。Δ 越大收敛越平缓对路径的切入越柔和Δ 越小收敛越快但容易造成航向超调和振荡。实际操作中我根据船速动态调整Δ船速1.5m/s时设Δ为8到10米转弯半径相对较大的场景会适当加大到12米以上。如果Δ固定不死会出现在湖面小半径转弯时严重切弯的问题。3.3 PID控制与LQR的取舍在项目推进过程中我把PID和LQR轨迹跟踪方案都实际验证了一遍两种算法各自的适用场景很明显。PID控制的优点前面已经提过模型依赖弱、实现简单、调参直观。缺点是处理多变量耦合问题比较吃力而且固定PID参数在工况变化较大的场景下适应性有限。LQR轨迹跟踪需要把非线性模型在平衡点线性化得到状态空间方程后用线性二次型最优控制方法设计反馈增益。LQR的优点是理论上保证了最优性并且在仿真条件下跟踪精度很高。但问题在于线性化模型只在平衡点附近有效一旦偏离太大LQR的性能会明显下降而且模型参数不准确时反馈增益的鲁棒性较差。我最终的方案是用LOS制导加串级PID作为主力控制方案这个组合在湖面试航中横向偏差实测稳定在0.6米以内。LQR方案作为性能对比基准留在了半物理仿真环境中用于验证不同工况下的表现差异。如果你想做更深入的对比验证强烈建议在同一套仿真环境中设置相同的扰动和噪声条件否则对比结果没有意义。4. PID控制器的实现与调参实战4.1 串级PID结构设计底层PID控制器要解决的核心问题是航向跟踪我在实际项目中采用了串级PID结构外环航向PID接收期望航向角输出期望转艏率内环转艏率PID接收期望转艏率输出差速指令。这个结构比单环PID多了一个内环对抑制转艏率的突变有显著改善。串级结构的原理不复杂但工程意义重大。水面上的扰动风、浪、流对转艏率的影响比对航向角的影响更直接、更快速内环对这个快变量的及时纠正让外环压力大大降低。结构示意图上可以这样理解外环是比较慢的闭环带宽约0.2Hz内环是快闭环带宽可以做到1Hz以上。两层闭环在频域上明显分离调参时互不干扰这是串级控制的精髓。单环PID虽然也能用但是面对湖面阵风或船后浪的突发扰动时航向角偏差已经很明显才开始修正动态响应慢半拍。串级PID的抗扰能力明显要好一个档次。4.2 从理论公式到嵌入式的代码实现我在嵌入式平台上实现的是位置式PID采样周期50ms直接对误差信号做比例、积分、微分运算。离散化之后的核心代码段如下float pid_calculate(pid_t *pid, float ref, float feedback) { float err ref - feedback; float output; // 角度误差归一化到 [-PI, PI]处理航向角跳变 while (err PI) err - 2.0f * PI; while (err -PI) err 2.0f * PI; pid-err_last pid-err_prev; pid-err_prev err; // 比例项 float P_out pid-Kp * err; // 积分项带抗积分饱和处理 pid-integral err * pid-dt; pid-integral constrain(pid-integral, -pid-lim_it, pid-lim_it); float I_out pid-Ki * pid-integral; // 微分项带一阶低通滤波 float derivative (err - pid-err_prev) / pid-dt; pid-deriv_lpf pid-lpf_alpha * (derivative - pid-deriv_lpf); float D_out pid-Kd * pid-deriv_lpf; output P_out I_out D_out; output constrain(output, -pid-lim_out, pid-lim_out); return output; }这里有几个关键点几乎是所有入门者都会踩的坑逐一说清楚。航向角误差归一化是必须处理的。航向角是周期量179度和-179度在数值上差了358度但实际的角偏差只是2度。如果不做归一化控制器会在航向角跨越0度边界时输出一个巨大的错误控制量船会突然打满舵场面很惊险。这个坑我在仿真时没遇到实船第一次跑到这个位置时整艘船猛转了一圈数据回看才定位到问题。积分限幅和输出限幅都是为了防饱和。积分饱和是PID控制里最常见的工程问题——船在转向过程中受水流阻碍达不到期望速度积分项会持续累积一旦障碍消除输出量已经远超执行器上限导致严重超调。输出限幅则是保护推进器和舵机的单片机的PWM输出范围是有限制的控制量突然过大对执行器的冲击也很大。微分项低通滤波单独说一下水面实测中GPS航向数据本身就有噪声IMU数据虽然好一些但也有高频抖动。微分项对噪声极其敏感不滤波的话高频分量会被放大几十倍控制量会像锯齿一样抖动。我用的截止频率是5Hz的一阶低通滤波器实测下来效果很好。4.3 裸机环境下的PID实时性问题项目的主控用的是一颗STM32F407我在裸机环境下跑控制循环没有上RTOS。在这种“裸机PID控制电机”的模式下实时性完全靠定时器中断保证。我用了定时器8产生50ms的更新中断在中断服务函数里完成数据采集、PID计算和PWM输出更新。这里有个非常重要的细节控制循环的执行时间抖动必须严格控制。即使中断优先级一致、中断服务函数执行时间基本恒定一旦有其他中断或串口任务抢占了执行时间PID计算的节拍就会产生抖动导致微分项计算不准。所以我把IMU读取、GPS数据解析、PID计算和PWM输出全部放在同一个高优先级中断里主循环只负责状态上报和指令接收实测下来时间抖动控制在1毫秒以内。如果你打算在一个线程里跑PID同时又用另一个线程处理日志存储和网络通讯一定要确认定时任务的调度确定性。很多PID在仿真里效果很好、实船就飘原因不是算法错了而是控制周期不稳定导致微分项的dt不准确。4.4 调参流程先内环后外环先P后I再D调参是整个项目里最考验耐心、也最依赖经验的部分。我的调参顺序严格遵循“先内环后外环、先P后I再D”的原则每一环节都有明确的测试方法和判据。内环转艏率PID的调参方法是给一个固定的转艏率阶跃指令观察实际转艏率的响应。先把Ki和Kd清零只留比例项Kp。从小到大慢慢加大Kp观察响应曲线直到出现等幅振荡记下此时的临界增益Ku再取Ku的45%到60%作为实际Kp。然后加一点点Ki消除静差Kd先不加确认内环无振荡后再逐一微调。外环航向PID的调参方法类似给一个期望航向的阶跃指令观察航向角响应。同样先P后I再D找到合适的临界增益后回退。需要注意的是内环和外环的带宽要有明显差异外环的响应速度比内环慢3到5倍是最合理的否则两个闭环会互相干扰系统容易出现低频振荡。我的最终参数只做一个参考内环 Kp1.8Ki0.15Kd0.4外环 Kp2.2Ki0.05Kd0.6。这只是针对我这艘艇三体船型的参数船型变了、推进器动力变了参数肯定要重新调。但整个调参方法是可以直接复用的尤其是“临界增益回退法”在真实系统上非常可靠。5. 实船调试的常见问题与排查实录5.1 积分饱和转向时突然“抽风”的元凶最早的水面测试中船在规定航线上出现了非常诡异的运动模式转大弯时一切正常但恢复直行后船会先大幅度向反方向偏然后才慢慢回正整个过程中控制量饱和时间持续了好几秒。回看控制量日志发现积分项已经累积到了限幅值的两三倍但因为没有限幅输出的控制量早就超出了推进器的实际执行上限。解决方法是积分分离与限幅并用。一方面给积分项设置独立的限幅值我设置了输出限幅的60%左右另一方面当误差超过某阈值时停止积分累积误差进入一定范围后再恢复积分。这个阈值一般取期望航向误差5度以内。用这两个方法后“抽风”现象彻底消失。5.2 GPS航向噪声与微分项放大GPS输出的航向角在静止状态下有±0.5度左右的噪声航向角速度的计算值噪声会更大直接使用的话微分项输出会高频抖动。第一次实船测试舵机有明显啸叫轴承也有异常磨损风险。排查时先怀疑是PWM频率问题后来把传感器原始遥测数据拉出来分析才发现噪声是源头。解决思路有两条一是传感器硬件上用IMU融合的航向角替代GPS原始航向角IMU的航向角在静止时噪声可以压到±0.1度以内二是在PID微分项前端加低通滤波切断高频分量的放大通路。两者结合之后舵机的声音立刻安静了很多。5.3 波浪扰动下的航向抖动湖面情况不可能一直平静稍有风浪时船的航向就会出现周期性抖动。最先出现的情况是航向在期望值附近以大约0.3Hz的频率来回摆动幅度在±3度左右PID参数越调越激进反而振荡越厉害。后来意识到这是典型的波浪低频扰动问题。解决方案是适当降低PID外环的增益让控制系统不过度响应波浪这类高频扰动同时适当增加内环的阻尼使整体的高频响应被压住。还有一个非常有效的辅助手段是增加一个前馈补偿当GPS检测到船的横向漂移速度时提前给一个航向修正量等偏差出来了再纠正效果会好很多。5.4 模型参数误差太大时怎么办实测中船模参数是从理论公式估算的然后通过自由衰减实验做了修正。但水面环境复杂同一个水域的流速分布也不均匀模型参数很难做到全工况精确。这时PID的鲁棒性优势就体现出来了——即使模型有20%到30%的误差一个调好的PID仍然能维持可接受的跟踪性能。如果误差再大就要考虑在线辨识和参数自适应了。我在这个项目里做了一个简单的水速在线估计器把水流干扰当作一个低频扰动估计出来并补偿到控制量中。这个策略对轨迹跟踪精度的提升非常明显横向偏差从0.9米改善到了0.5米左右。5.5 问题排查速查表现象可能原因排查方法解决方案航向角跨0度时船猛打舵角度误差未归一化查看控制量日志是否出现突变误差归一化到[-PI, PI]转向后恢复直行时反向偏转积分饱和查看积分项是否超限积分限幅积分分离舵机高频抖动有啸叫微分项放大噪声检查传感器噪声频谱低通滤波用IMU航向风浪中航向周期摆动波浪扰动被过度响应观察扰动频率与系统带宽降低增益加前馈补偿控制量打满但动作缓慢执行器响应延迟测量实际PWM到动作延迟加入延迟补偿环节转弯时严重切弯LOS前视距离过小观察误差与转弯半径关系动态调整前视距离6. 一点个人体会整个项目做下来我最大的感触是水面无人艇的控制并没有想象中那么玄乎但也没有任何一步可以跳过。把系统建模做扎实比反复调PID参数管用得多把制导律设计清楚比盲目堆控制算法管用得多。PID参数调不出来的时候优先去查模型和时间常数而不是怀疑代码写错了。如果你正准备开始类似的项目我的建议是先花一周时间把模型建清楚、把Nomoto模型参数测出来再花一周时间把仿真环境搭好最后再上船实测。前面慢就是后面快这个顺序不要反。后续如果想继续提升跟踪精度可以在制导层引入自适应前馈或者在控制层做模糊PID切换底层的串级PID框架完全可以平滑扩展不需要推翻重来。本文还有配套的精品资源点击获取