做轮式移动机器人轨迹跟踪我最初在MATLAB里踩过不少坑明明参考轨迹画出来很简单机器人却动不动跑偏、抖动甚至发散。后来把运动学和动力学两层模型都用上构成双闭环结构又借鉴自抗扰控制ADRC里扩张状态观测器的思路把摩擦、负载变化、轮径差异这类说不清道不明的扰动在线估计掉问题才算真正解决。这篇文章把这套MATLAB程序的建模过程、控制律推导、代码实现和参数整定经验完整整理出来非常适合正在做课程设计、毕业设计或者想给AGV类项目做轨迹跟踪仿真预研的人参考。我会把为什么这么设计也讲清楚因为光抄代码不搞懂逻辑换个机器人参数基本调不出来。1. 双闭环和自抗扰先把控制结构定明白再写代码我见过很多刚接触轨迹跟踪的人第一版程序只有一个位姿外环根据当前位置和目标轨迹的偏差直接算出一个期望速度下发。这个结构放在理想运动学模型里确实能收敛因为参考轨迹给到位移位姿误差直接反映成速度指令。但一旦加入动力学约束、轮径差异、地面摩擦这些实际因素期望速度与实际速度之间就不是瞬时一致的关系了。轮子响应需要时间驱动力矩有限还可能受扰。于是位姿误差虽然被外环调小却始终在抖越接近期望越容易来回振荡。这时候就必须在内环补一个动力学控制器。1.1 单环控制缺了什么一句话概括运动学外环只能决定“机器人该以什么速度运动”不能决定“实际能不能产生这个速度”。单环把这两件事混为一谈等于默认执行器可以瞬间满足任意速度指令。但电机、减速器、轮胎摩擦、地面倾角这些因素都会让实际速度与期望速度之间出现一个动态过程的差价。这个差价如果不处理轨迹跟踪的误差就是周期性的甚至会发散。我在模型里加入摩擦项后原来的单环程序直接失效这也是我下定决心改双闭环的直接原因。1.2 双闭环结构的分工双闭环的核心思路是把轨迹跟踪拆成两层问题外环是运动学控制器输入当前位姿和参考位姿输出期望线速度 vc 和期望角速度 ωc。它只处理几何关系不关心力矩。内环是动力学控制器输入期望速度和实际速度输出左右轮的驱动力矩 τR、τL。它只解决物理执行问题不关心参考轨迹长什么样。内环把速度误差收敛好之后外环看到的被控对象就近似是“指令速度到实际速度一致”的理想模型运动学控制律才能真正发挥效果。打个比方外环是导航员告诉你该往哪走、走多快内环是驾驶员负责把油门和方向盘踩稳。导航员喊得再准驾驶员手抖脚抖车一样走偏。层级使用模型输入输出主要作用外环运动学位姿误差期望线速度 vc、期望角速度 ωc保证轨迹收敛内环动力学速度误差左右轮力矩 τR、τL保证速度跟踪1.3 为什么借鉴自抗扰而不是直接加积分器传统做法会在内环加PI控制器速度误差积分可以消掉稳态偏差但遇到时变扰动时消不干净还容易带来超调和相位滞后。自抗扰控制的思路更直接把“动力学模型中的未知动态 外部扰动”当成一个总扰动用扩张状态观测器把它当作一个状态变量在线估计出来然后实时补偿掉。补偿之后被控对象在控制器眼里就变成了近似理想的积分器剩下只需要简单的反馈控制。这个项目里未知参数主要存在质量 m、转动惯量 Iz、轮半径 r、轮距半宽 L以及地面摩擦和打滑。如果走参数辨识路线工程量大且辨识不准。而ESO不需要精确知道这些参数的每一个只需要把它们的综合影响看成一个总扰动观测出来再扣除。这也是我在标题里写“借鉴自抗扰控制”的原因——并没有完整使用整套ADRC框架而是把它的核心思想嵌入了动力学内环。2. 运动学与动力学模型搭建MATLAB可仿真的被控对象写控制程序之前先把被控对象模型在MATLAB里搭对。模型错了后面所有控制器都是白调。差速驱动机器人的标准做法是状态取车体位姿和车体速度控制量为左右驱动轮的力矩。2.1 符号约定我用的变量定义如下q [x, y, θ]其中 x、y 是车体在世界坐标系下的位置θ 是车体朝向角v车体前进方向线速度单位 m/sω车体绕垂直轴旋转角速度单位 rad/sτR、τL右侧、左侧驱动轮电机的驱动力矩单位 N·md_v、d_ω线速度通道、角速度通道的等效扰动加速度单位 m/s² 和 rad/s²2.2 运动学方程非完整约束下的差速驱动机器人运动学模型是ẋ v·cosθ ẏ v·sinθ θ̇ ω这个模型在几乎所有移动机器人教材里都有但有一个细节经常被忽略θ 是一个循环坐标仿真中如果不做角度限制累计几十秒后数值会越来越大三角函数的精度会受影响。所以后面代码里要定期对 θ 做 wrap 处理。2.3 动力学方程从力和力矩的角度推导左右轮驱动力可以写成左轮驱动力F_L τL / r右轮驱动力F_R τR / r沿车头方向的总推力是两个轮子驱动力之和F_sum F_L F_R (τL τR) / r于是线速度动力学为v̇ (τR τL) / (m·r) d_v绕垂直轴的总力矩取决于左右轮驱动力对质心的力臂差。左右轮距质心的距离都是 LL 是轮距的一半所以T_sum L·F_R - L·F_L L·(τR - τL) / r角速度动力学为ω̇ L·(τR - τL) / (Iz·r) d_ω这样差速驱动机器人的动力学就被拆成了两个几乎解耦的一阶系统。这非常符合自抗扰控制的使用场景每个通道都能写成“输入增益 × 控制量 总扰动”的标准形式。2.4 MATLAB函数实现参数用结构体保存方便批量和做参数扫描。仿真动力学建议写成独立的MATLAB函数主循环里调用即可后面换模型也方便。% wmr_dynamics.m % 差速轮式移动机器人动力学 % state [x; y; th; v; w] % control [tau_R; tau_L] % dist [d_v; d_w] function dstate wmr_dynamics(state, control, dist, prm) x state(1); y state(2); th state(3); v state(4); w state(5); tau_R control(1); tau_L control(2); d_v dist(1); d_w dist(2); % 线速度通道 v_dot (tau_R tau_L) / (prm.m * prm.r) d_v; % 角速度通道 w_dot prm.L * (tau_R - tau_L) / (prm.Iz * prm.r) d_w; dstate [v*cos(th); v*sin(th); w; v_dot; w_dot]; end模型中 d_v、d_w 可以认为是“集中扰动”仿真时既可以注入外部干扰也可以塞入参数偏差造成的等效加速度。实际上真实系统的扰动有很多来源但在模型里统一用这两个标量代替简洁有效。3. 外环运动学控制律从位姿误差到期望速度外环的目标是让机器人位姿收敛到参考轨迹。这里我用的是基于车体坐标误差的运动学反步控制律也是轨迹跟踪里最常用、最稳的一类方法。3.1 车体坐标下的误差变换全局坐标系下的位置误差不能直接当控制量用因为车体是朝向哪里运动取决于它的当前航向角。最合理的做法是把位置误差投影到车体坐标系下function [ex, ey, eth] tracking_error(p, ref) % p: [x; y; th]当前位姿 cx cos(p(3)); sx sin(p(3)); % 车体坐标系下ex沿车头方向ey沿车体横轴 ex (ref.x - p(1))*cx (ref.y - p(2))*sx; ey -(ref.x - p(1))*sx (ref.y - p(2))*cx; eth wrapToPi(ref.th - p(3)); % 航向角误差 end这里的 wrapToPi 来自 MATLAB 的 Mapping Toolbox如果没有装也可以用 atan2(sin(angle), cos(angle)) 替代。3.2 运动学反步控制律外环控制律选定为vc vr·cos(eth) k1·ex ωc ωr k2·ey k3·sin(eth)其中 vr、ωr 是参考轨迹的线速度和角速度。第一项 vr·cos(eth) 是在当前航向误差下把参考速度分解到车头方向的有效分量k1·ex 负责消掉沿车头方向的距离误差k2·ey 负责消掉横向距离误差k3·sin(eth) 负责纠正航向角偏差。写成代码function [vc, wc] kinematic_controller(p, ref, k) [ex, ey, eth] tracking_error(p, ref); % 参考速度信息从ref结构体读取 vc ref.v * cos(eth) k.k1 * ex; wc ref.w k.k2 * ey k.k3 * sin(eth); end增益 k1、k2、k3 的物理含义分别是位置误差、横向误差、航向误差的反馈强度。这三个增益不是越大越好因为外环输出会作为内环的期望速度指令如果外环增益过大指令速度突变剧烈内环根本追不上整个系统就会从轨迹跟踪变成“速度跟踪失稳”。一般 k1 取 0.5~2 之间k2 略大于 k1k3 稍小。3.3 给内环一个平滑的参考输入外环输出的 vc、ωc 是比例形式的瞬时输出内含很多反馈噪声。如果直接把它当成内环的参考速度内环求导时会放大这些高频分量。这个问题我在最初调试时吃了大亏内环用差分算期望加速度结果指令抖动特别厉害力矩输出像锯齿一样。解决办法是在外环与内环之间加一阶滤波器% 滤波器状态更新 v_ref_dot (vc - v_ref) / tau_f; w_ref_dot (wc - w_ref) / tau_f; % 更新参考速度 v_ref v_ref Ts * v_ref_dot; w_ref w_ref Ts * w_ref_dot;tau_f 取 0.02~0.1 秒左右。这样外环输出的参考速度不仅平滑还能同时获得干净的参考加速度信号 v_ref_dot、w_ref_dot给内环前馈使用。这个细节很多人会漏掉实际上对稳定性影响非常大。4. 内环动力学的自抗扰实现ESO在线估计扰动内环是整套程序的第二个核心。它接收外环传来的参考速度并输出左右轮力矩让实际速度尽可能精确地跟踪参考速度。4.1 为什么纯PD内环还不够如果只做PD内环公式是τR τL ∝ (v_ref - v)这能保证速度误差最终收敛但收敛的能力取决于扰动被抑制的程度。地面摩擦变化、负载变化这类低频扰动PD反馈需要靠误差本身来对抗最终会出现稳态误差或者周期性波动。所以我在内环引入一个ESO把扰动直接观测出来并前馈扣除让PD只需要处理一个小得多的残差。4.2 ESO的离散迭代式以内环线速度通道为例。把动力学方程改写成ADRC标准形式v̇ b0·u w(t)其中 b0 是名义输入增益u 是控制输入。在我们模型里实际输入是质量量纲的驱动力 u_m_v τR τL单位N·m其实除以r后才是力但代码里方便起见统一含义对应 b0 1/(m·r)。为了程序简洁我把输入归一化成“等效驱动力”这样 b0_v 1/m。如果你想严格按 N·m 单位来b0_v 1/(m·r) 即可结论一样。ESO 的离散递推用前向欧拉e_eso y - z1 z1 z1 Ts·(z2 b0·u β1·e_eso) z2 z2 Ts·(β2·e_eso)其中 y 是实际速度z1 估计速度z2 估计总扰动。β1、β2 按观测器带宽 ω0 配置β1 2·ω0β2 ω0²ω0 一般取内环控制器带宽的 3~5 倍。角速度通道完全同理。4.3 内环控制律与左右力矩合成内环的控制律写成u0_v v_ref_dot kp_v·(v_ref - v) u_acc_v u0_v - z2_v u_m_v m · u_acc_v上面 u0_v 是期望加速度z2_v 是观测到的总扰动态度二者相减后乘质量得到驱动力这等于把扰动在进入系统之前就扣掉了。角速度通道同理u0_w w_ref_dot kp_w·(w_ref - w) u_acc_w u0_w - z2_w u_m_w Iz · u_acc_w最后合成左右轮力矩。注意前面推导的关系τR τL u_m_v · r τR - τL u_m_w · r / L解得tau_R (u_m_v * prm.r u_m_w * prm.r / prm.L) / 2; tau_L (u_m_v * prm.r - u_m_w * prm.r / prm.L) / 2;完整的离散化代码片段如下% ---- 线速度通道 ESO ---- e_eso_v v - z1_v; z1_v z1_v Ts * (z2_v b0_v * u_m_v_prev 2*w0_eso * e_eso_v); z2_v z2_v Ts * (w0_eso^2 * e_eso_v); u0_v v_ref_dot kp_v * (v_ref - v); u_acc_v u0_v - z2_v; u_m_v prm.m * u_acc_v; % ---- 角速度通道 ESO ---- e_eso_w w - z1_w; z1_w z1_w Ts * (z2_w b0_w * u_m_w_prev 2*w0_eso * e_eso_w); z2_w z2_w Ts * (w0_eso^2 * e_eso_w); u0_w w_ref_dot kp_w * (w_ref - w); u_acc_w u0_w - z2_w; u_m_w prm.Iz * u_acc_w; % ---- 力矩合成与限幅 ---- tau_R (u_m_v * prm.r u_m_w * prm.r / prm.L) / 2; tau_L (u_m_v * prm.r - u_m_w * prm.r / prm.L) / 2; tau_R max(-tau_max, min(tau_max, tau_R)); tau_L max(-tau_max, min(tau_max, tau_L));注意 ESO 更新用的 u_m_v_prev 是上一采样周期已经发出的控制量这符合“先观测、后控制”的因果关系。不能把当前步还没算出来的控制量拿去更新观测器否则引入代数环仿真会出现一步延迟误差。4.4 ESO与内环控制器的配合逻辑整个内环控制器在单步里的正确顺序是用上一时刻控制量更新 ESO得到当前扰动估计 z2用当前参考速度和实际速度计算 PD 控制量 u0从 u0 中减去扰动估计 z2得到等效加速度换算成力矩限幅后输出给机器人模型这个顺序如果颠倒ESO 观测到的仍是旧控制量等于有一个采样周期的延迟。在采样时间较小、带宽较高时这个延迟会明显降低系统稳定性。这也是我后来反复对比后确定的稳定顺序。5. 仿真主程序、参考轨迹生成与参数整定模型和控制律都写好后剩下就是把这些模块放进主循环里跑起来并解决调参问题。5.1 主循环骨架主循环用固定步长 Ts每次迭代完成“参考轨迹计算→外环→滤波→内环→ESO→动力学更新→记录数据”这一整套动作。示例骨架% 参数初始化 prm.m 10; % kg prm.Iz 0.5; % kg·m^2 prm.r 0.1; % m prm.L 0.4; % m Ts 0.01; % s % 控制器与ESO参数 k.k1 1.2; k.k2 1.5; k.k3 0.35; kp_v 6; kp_w 12; w0_eso 20; tau_f 0.05; tau_max 5; % 初始状态 state [0; 0; 0; 0; 0]; % x, y, th, v, w v_ref 0; w_ref 0; z1_v 0; z2_v 0; z1_w 0; z2_w 0; u_m_v_prev 0; u_m_w_prev 0; % 仿真时长与数据记录 Tfinal 15; N round(Tfinal / Ts); log zeros(N, 10); for k 1:N t (k-1) * Ts; % 1. 参考轨迹以圆轨迹为例 R 1.0; omega_ref 0.5; xr R * cos(omega_ref * t); yr R * sin(omega_ref * t); xr_dot -R * omega_ref * sin(omega_ref * t); yr_dot R * omega_ref * cos(omega_ref * t); xr_dot2 -R * omega_ref^2 * cos(omega_ref * t); yr_dot2 -R * omega_ref^2 * sin(omega_ref * t); vr sqrt(xr_dot^2 yr_dot^2); thr wrapToPi(atan2(yr_dot, xr_dot)); wr (xr_dot * yr_dot2 - yr_dot * xr_dot2) / (xr_dot^2 yr_dot^2 1e-6); ref.x xr; ref.y yr; ref.th thr; ref.v vr; ref.w wr; % 2. 外环 p state(1:3); [vc, wc] kinematic_controller(p, ref, k); % 3. 参考速度滤波 v_ref_dot (vc - v_ref) / tau_f; w_ref_dot (wc - w_ref) / tau_f; v_ref v_ref Ts * v_ref_dot; w_ref w_ref Ts * w_ref_dot; % 4. 提取实际速度 v state(4); w state(5); % 5. 内环控制器ESO PD见上一节代码 % ... 省略重复部分 ... % 6. 仿真正动力学 d_v 0.2 * sin(2*t); % 线速度通道扰动 d_w 0.1 * cos(1.5*t); % 角速度通道扰动 dist [d_v; d_w]; dstate wmr_dynamics(state, [tau_R; tau_L], dist, prm); state state Ts * dstate; state(3) wrapToPi(state(3)); % 7. 记录数据 log(k, :) [t, state, v_ref, w_ref, z2_v, z2_w]; end5.2 参考轨迹生成圆轨迹相对简单。如果想验证更复杂的转向能力可以用8字形轨迹。参考轨迹的角速度不能直接对航向角做差分而是用速度与加速度的解析关系ωr (x_dot·y_ddot - y_dot·x_ddot) / (x_dot² y_dot²)这个式子的来源是航向角 θr atan2(y_dot, x_dot)对时间求导后经过代数化简就是这个形式。用这个公式的好处是数值稳定不会因为 wrap 产生跳变。轨迹参数示例圆轨迹x R·cos(ωt)y R·sin(ωt)8字轨迹x A·sin(ωt)y B·sin(2ωt)8字轨迹在交叉点附近线速度会减小对控制器的低速性能要求更高能暴露出很多圆轨迹看不出来的问题。5.3 参数初始值与整定顺序我建议的初始参数如下表整套参数可以稳定跑通圆轨迹参数初始值物理含义调大后的影响k11.2纵向误差反馈沿车头方向收敛更快过大则速度指令突变k21.5横向误差反馈横向纠偏更快过大容易振荡k30.35航向误差反馈转角度更积极过大易超调kp_v6线速度通道PD增益速度跟踪变快过大直接引发抖动kp_w12角速度通道PD增益角速度跟踪变快过大造成角速度噪声放大w0_eso20ESO带宽扰动估计快过大噪声放大tau_f0.05参考速度滤波时间常数越大指令越平滑但轨迹跟踪滞后整定顺序有讲究我建议按“内环→ESO→外环”的顺序来先把ESO关掉只保留PD内环把kp_v、kp_w调到速度跟踪稳定、不抖。这一步是在确认底层执行器没问题。打开ESO从较小的 ω0 开始逐步增大观察扰动估计 z2 是否平滑。看到一个数字很大的 z2 不要慌关键是看它是否稳定且有界。最后调外环 k1、k2、k3。用圆轨迹起步因为圆轨迹参数恒定问题容易定位稳定后再换8字轨迹测试转向能力。6. 扰动对比和踩坑记录实测中我改了什么才把它跑稳仿真程序搭好之后我拿它做了几组对比实验也在这个过程中找到了许多容易被忽视的问题。6.1 相同参数下开/关ESO的扰动差异我在第5节的主循环里注入了正弦型扰动d_v 0.5·sin(2t)d_w 0.3·cos(3t)同时把ESO分别开启和关闭对比圆轨迹的横向误差表现工况最大横向偏差稳态呈现关闭ESO约 0.08 m周期性波动明显开启ESO约 0.02 m波动被明显抑制这说明ESO把大部分时变扰动在进入速度回路之前就扣掉了内环PD不需要再靠误差去硬抗扰动。同样如果把负载变化仿真成质量在 10 kg 附近按正弦波动 ±20%ESO开启后速度跟踪误差仍然能维持在很小范围这是我最初没想到的。6.2 几个典型的坑角度环绕问题 仿真超过几十秒后如果不做 wrapToPiθ 累加得很大三角函数精度下降外环的航向误差计算也会出现 ±π 突跳。务必在每个采样步完成后对 θ 做 wrap。ESO带宽过高 w0_eso 调到 40 以上时ESO开始把测量噪声当成扰动z2 剧烈抖动控制量跟着抖。解决办法是记住一个原则ESO带宽只要高于实际扰动主频的3~5倍即可不需要无限大。内环与外环带宽比 内环必须比外环快一般内环闭环带宽取外环的5倍以上。如果内环响应慢外环的误差反馈和速度指令之间会形成大相位滞后系统表现就是“慢悠悠地振荡”。我遇到的情况是 k2 调大后系统反而发散最终靠提高内环 kp_w 解决的。控制量饱和 力矩限幅不能只限模型里的输出还要保证ESO的输入用的是实际发出的控制量。也就是如果控制器算出来的力矩被限幅了那么传给ESO的 u_m_v_prev 也应该是限幅后的值。否则ESO会继续以为控制器发出了很大的力导致观测器错误、整个系统发散。这个坑非常隐蔽我调试了很久才定位到。参考速度过零 8字轨迹在交叉点附近线速度接近零外环的 vc vr·cos(eth) k1·ex 可能变成负值机器人会试图倒车。如果项目要求只能前进需要在外环里加一个速度方向判定或者改用允许双向运动但带转向约束的控制方案。6.3 换个方式继续扩展这套程序还可以做几个方向的扩展把电机简化成一阶惯性环节加进动力学模型模仿真实电机响应延迟在ESO基础上加入参数自适应让 b0 的估计值随载荷变化移植到Simulink里把采样时间和控制时间分开更接近真实控制器的离散化形式把轨迹跟踪结果导出来与基于视觉的全局路径规划结合做完整的自主导航验证我最后想给一个最实际的建议如果你正在调这个程序遇到发散或抖动先不要急着怀疑算法先去看看控制顺序、限幅位置和角度环绕这三个地方是最容易出问题的。把基础框架跑稳之后再去折腾ESO带宽和增益整个系统会温顺很多。这套“运动学外环 动力学内环 ESO扰动补偿”的架构我用它跑过圆轨迹、8字轨迹和加扰实验稳定性都在可接受范围内希望你也能调出满意的曲线。 
长河君 · 2026/10/21 8:03:00 · 阅读 1.2k MATLAB实现移动机器人轨迹跟踪：双闭环与自抗扰控制的完整方案
做轮式移动机器人轨迹跟踪我最初在MATLAB里踩过不少坑明明参考轨迹画出来很简单机器人却动不动跑偏、抖动甚至发散。后来把运动学和动力学两层模型都用上构成双闭环结构又借鉴自抗扰控制ADRC里扩张状态观测器的思路把摩擦、负载变化、轮径差异这类说不清道不明的扰动在线估计掉问题才算真正解决。这篇文章把这套MATLAB程序的建模过程、控制律推导、代码实现和参数整定经验完整整理出来非常适合正在做课程设计、毕业设计或者想给AGV类项目做轨迹跟踪仿真预研的人参考。我会把为什么这么设计也讲清楚因为光抄代码不搞懂逻辑换个机器人参数基本调不出来。1. 双闭环和自抗扰先把控制结构定明白再写代码我见过很多刚接触轨迹跟踪的人第一版程序只有一个位姿外环根据当前位置和目标轨迹的偏差直接算出一个期望速度下发。这个结构放在理想运动学模型里确实能收敛因为参考轨迹给到位移位姿误差直接反映成速度指令。但一旦加入动力学约束、轮径差异、地面摩擦这些实际因素期望速度与实际速度之间就不是瞬时一致的关系了。轮子响应需要时间驱动力矩有限还可能受扰。于是位姿误差虽然被外环调小却始终在抖越接近期望越容易来回振荡。这时候就必须在内环补一个动力学控制器。1.1 单环控制缺了什么一句话概括运动学外环只能决定“机器人该以什么速度运动”不能决定“实际能不能产生这个速度”。单环把这两件事混为一谈等于默认执行器可以瞬间满足任意速度指令。但电机、减速器、轮胎摩擦、地面倾角这些因素都会让实际速度与期望速度之间出现一个动态过程的差价。这个差价如果不处理轨迹跟踪的误差就是周期性的甚至会发散。我在模型里加入摩擦项后原来的单环程序直接失效这也是我下定决心改双闭环的直接原因。1.2 双闭环结构的分工双闭环的核心思路是把轨迹跟踪拆成两层问题外环是运动学控制器输入当前位姿和参考位姿输出期望线速度 vc 和期望角速度 ωc。它只处理几何关系不关心力矩。内环是动力学控制器输入期望速度和实际速度输出左右轮的驱动力矩 τR、τL。它只解决物理执行问题不关心参考轨迹长什么样。内环把速度误差收敛好之后外环看到的被控对象就近似是“指令速度到实际速度一致”的理想模型运动学控制律才能真正发挥效果。打个比方外环是导航员告诉你该往哪走、走多快内环是驾驶员负责把油门和方向盘踩稳。导航员喊得再准驾驶员手抖脚抖车一样走偏。层级使用模型输入输出主要作用外环运动学位姿误差期望线速度 vc、期望角速度 ωc保证轨迹收敛内环动力学速度误差左右轮力矩 τR、τL保证速度跟踪1.3 为什么借鉴自抗扰而不是直接加积分器传统做法会在内环加PI控制器速度误差积分可以消掉稳态偏差但遇到时变扰动时消不干净还容易带来超调和相位滞后。自抗扰控制的思路更直接把“动力学模型中的未知动态 外部扰动”当成一个总扰动用扩张状态观测器把它当作一个状态变量在线估计出来然后实时补偿掉。补偿之后被控对象在控制器眼里就变成了近似理想的积分器剩下只需要简单的反馈控制。这个项目里未知参数主要存在质量 m、转动惯量 Iz、轮半径 r、轮距半宽 L以及地面摩擦和打滑。如果走参数辨识路线工程量大且辨识不准。而ESO不需要精确知道这些参数的每一个只需要把它们的综合影响看成一个总扰动观测出来再扣除。这也是我在标题里写“借鉴自抗扰控制”的原因——并没有完整使用整套ADRC框架而是把它的核心思想嵌入了动力学内环。2. 运动学与动力学模型搭建MATLAB可仿真的被控对象写控制程序之前先把被控对象模型在MATLAB里搭对。模型错了后面所有控制器都是白调。差速驱动机器人的标准做法是状态取车体位姿和车体速度控制量为左右驱动轮的力矩。2.1 符号约定我用的变量定义如下q [x, y, θ]其中 x、y 是车体在世界坐标系下的位置θ 是车体朝向角v车体前进方向线速度单位 m/sω车体绕垂直轴旋转角速度单位 rad/sτR、τL右侧、左侧驱动轮电机的驱动力矩单位 N·md_v、d_ω线速度通道、角速度通道的等效扰动加速度单位 m/s² 和 rad/s²2.2 运动学方程非完整约束下的差速驱动机器人运动学模型是ẋ v·cosθ ẏ v·sinθ θ̇ ω这个模型在几乎所有移动机器人教材里都有但有一个细节经常被忽略θ 是一个循环坐标仿真中如果不做角度限制累计几十秒后数值会越来越大三角函数的精度会受影响。所以后面代码里要定期对 θ 做 wrap 处理。2.3 动力学方程从力和力矩的角度推导左右轮驱动力可以写成左轮驱动力F_L τL / r右轮驱动力F_R τR / r沿车头方向的总推力是两个轮子驱动力之和F_sum F_L F_R (τL τR) / r于是线速度动力学为v̇ (τR τL) / (m·r) d_v绕垂直轴的总力矩取决于左右轮驱动力对质心的力臂差。左右轮距质心的距离都是 LL 是轮距的一半所以T_sum L·F_R - L·F_L L·(τR - τL) / r角速度动力学为ω̇ L·(τR - τL) / (Iz·r) d_ω这样差速驱动机器人的动力学就被拆成了两个几乎解耦的一阶系统。这非常符合自抗扰控制的使用场景每个通道都能写成“输入增益 × 控制量 总扰动”的标准形式。2.4 MATLAB函数实现参数用结构体保存方便批量和做参数扫描。仿真动力学建议写成独立的MATLAB函数主循环里调用即可后面换模型也方便。% wmr_dynamics.m % 差速轮式移动机器人动力学 % state [x; y; th; v; w] % control [tau_R; tau_L] % dist [d_v; d_w] function dstate wmr_dynamics(state, control, dist, prm) x state(1); y state(2); th state(3); v state(4); w state(5); tau_R control(1); tau_L control(2); d_v dist(1); d_w dist(2); % 线速度通道 v_dot (tau_R tau_L) / (prm.m * prm.r) d_v; % 角速度通道 w_dot prm.L * (tau_R - tau_L) / (prm.Iz * prm.r) d_w; dstate [v*cos(th); v*sin(th); w; v_dot; w_dot]; end模型中 d_v、d_w 可以认为是“集中扰动”仿真时既可以注入外部干扰也可以塞入参数偏差造成的等效加速度。实际上真实系统的扰动有很多来源但在模型里统一用这两个标量代替简洁有效。3. 外环运动学控制律从位姿误差到期望速度外环的目标是让机器人位姿收敛到参考轨迹。这里我用的是基于车体坐标误差的运动学反步控制律也是轨迹跟踪里最常用、最稳的一类方法。3.1 车体坐标下的误差变换全局坐标系下的位置误差不能直接当控制量用因为车体是朝向哪里运动取决于它的当前航向角。最合理的做法是把位置误差投影到车体坐标系下function [ex, ey, eth] tracking_error(p, ref) % p: [x; y; th]当前位姿 cx cos(p(3)); sx sin(p(3)); % 车体坐标系下ex沿车头方向ey沿车体横轴 ex (ref.x - p(1))*cx (ref.y - p(2))*sx; ey -(ref.x - p(1))*sx (ref.y - p(2))*cx; eth wrapToPi(ref.th - p(3)); % 航向角误差 end这里的 wrapToPi 来自 MATLAB 的 Mapping Toolbox如果没有装也可以用 atan2(sin(angle), cos(angle)) 替代。3.2 运动学反步控制律外环控制律选定为vc vr·cos(eth) k1·ex ωc ωr k2·ey k3·sin(eth)其中 vr、ωr 是参考轨迹的线速度和角速度。第一项 vr·cos(eth) 是在当前航向误差下把参考速度分解到车头方向的有效分量k1·ex 负责消掉沿车头方向的距离误差k2·ey 负责消掉横向距离误差k3·sin(eth) 负责纠正航向角偏差。写成代码function [vc, wc] kinematic_controller(p, ref, k) [ex, ey, eth] tracking_error(p, ref); % 参考速度信息从ref结构体读取 vc ref.v * cos(eth) k.k1 * ex; wc ref.w k.k2 * ey k.k3 * sin(eth); end增益 k1、k2、k3 的物理含义分别是位置误差、横向误差、航向误差的反馈强度。这三个增益不是越大越好因为外环输出会作为内环的期望速度指令如果外环增益过大指令速度突变剧烈内环根本追不上整个系统就会从轨迹跟踪变成“速度跟踪失稳”。一般 k1 取 0.5~2 之间k2 略大于 k1k3 稍小。3.3 给内环一个平滑的参考输入外环输出的 vc、ωc 是比例形式的瞬时输出内含很多反馈噪声。如果直接把它当成内环的参考速度内环求导时会放大这些高频分量。这个问题我在最初调试时吃了大亏内环用差分算期望加速度结果指令抖动特别厉害力矩输出像锯齿一样。解决办法是在外环与内环之间加一阶滤波器% 滤波器状态更新 v_ref_dot (vc - v_ref) / tau_f; w_ref_dot (wc - w_ref) / tau_f; % 更新参考速度 v_ref v_ref Ts * v_ref_dot; w_ref w_ref Ts * w_ref_dot;tau_f 取 0.02~0.1 秒左右。这样外环输出的参考速度不仅平滑还能同时获得干净的参考加速度信号 v_ref_dot、w_ref_dot给内环前馈使用。这个细节很多人会漏掉实际上对稳定性影响非常大。4. 内环动力学的自抗扰实现ESO在线估计扰动内环是整套程序的第二个核心。它接收外环传来的参考速度并输出左右轮力矩让实际速度尽可能精确地跟踪参考速度。4.1 为什么纯PD内环还不够如果只做PD内环公式是τR τL ∝ (v_ref - v)这能保证速度误差最终收敛但收敛的能力取决于扰动被抑制的程度。地面摩擦变化、负载变化这类低频扰动PD反馈需要靠误差本身来对抗最终会出现稳态误差或者周期性波动。所以我在内环引入一个ESO把扰动直接观测出来并前馈扣除让PD只需要处理一个小得多的残差。4.2 ESO的离散迭代式以内环线速度通道为例。把动力学方程改写成ADRC标准形式v̇ b0·u w(t)其中 b0 是名义输入增益u 是控制输入。在我们模型里实际输入是质量量纲的驱动力 u_m_v τR τL单位N·m其实除以r后才是力但代码里方便起见统一含义对应 b0 1/(m·r)。为了程序简洁我把输入归一化成“等效驱动力”这样 b0_v 1/m。如果你想严格按 N·m 单位来b0_v 1/(m·r) 即可结论一样。ESO 的离散递推用前向欧拉e_eso y - z1 z1 z1 Ts·(z2 b0·u β1·e_eso) z2 z2 Ts·(β2·e_eso)其中 y 是实际速度z1 估计速度z2 估计总扰动。β1、β2 按观测器带宽 ω0 配置β1 2·ω0β2 ω0²ω0 一般取内环控制器带宽的 3~5 倍。角速度通道完全同理。4.3 内环控制律与左右力矩合成内环的控制律写成u0_v v_ref_dot kp_v·(v_ref - v) u_acc_v u0_v - z2_v u_m_v m · u_acc_v上面 u0_v 是期望加速度z2_v 是观测到的总扰动态度二者相减后乘质量得到驱动力这等于把扰动在进入系统之前就扣掉了。角速度通道同理u0_w w_ref_dot kp_w·(w_ref - w) u_acc_w u0_w - z2_w u_m_w Iz · u_acc_w最后合成左右轮力矩。注意前面推导的关系τR τL u_m_v · r τR - τL u_m_w · r / L解得tau_R (u_m_v * prm.r u_m_w * prm.r / prm.L) / 2; tau_L (u_m_v * prm.r - u_m_w * prm.r / prm.L) / 2;完整的离散化代码片段如下% ---- 线速度通道 ESO ---- e_eso_v v - z1_v; z1_v z1_v Ts * (z2_v b0_v * u_m_v_prev 2*w0_eso * e_eso_v); z2_v z2_v Ts * (w0_eso^2 * e_eso_v); u0_v v_ref_dot kp_v * (v_ref - v); u_acc_v u0_v - z2_v; u_m_v prm.m * u_acc_v; % ---- 角速度通道 ESO ---- e_eso_w w - z1_w; z1_w z1_w Ts * (z2_w b0_w * u_m_w_prev 2*w0_eso * e_eso_w); z2_w z2_w Ts * (w0_eso^2 * e_eso_w); u0_w w_ref_dot kp_w * (w_ref - w); u_acc_w u0_w - z2_w; u_m_w prm.Iz * u_acc_w; % ---- 力矩合成与限幅 ---- tau_R (u_m_v * prm.r u_m_w * prm.r / prm.L) / 2; tau_L (u_m_v * prm.r - u_m_w * prm.r / prm.L) / 2; tau_R max(-tau_max, min(tau_max, tau_R)); tau_L max(-tau_max, min(tau_max, tau_L));注意 ESO 更新用的 u_m_v_prev 是上一采样周期已经发出的控制量这符合“先观测、后控制”的因果关系。不能把当前步还没算出来的控制量拿去更新观测器否则引入代数环仿真会出现一步延迟误差。4.4 ESO与内环控制器的配合逻辑整个内环控制器在单步里的正确顺序是用上一时刻控制量更新 ESO得到当前扰动估计 z2用当前参考速度和实际速度计算 PD 控制量 u0从 u0 中减去扰动估计 z2得到等效加速度换算成力矩限幅后输出给机器人模型这个顺序如果颠倒ESO 观测到的仍是旧控制量等于有一个采样周期的延迟。在采样时间较小、带宽较高时这个延迟会明显降低系统稳定性。这也是我后来反复对比后确定的稳定顺序。5. 仿真主程序、参考轨迹生成与参数整定模型和控制律都写好后剩下就是把这些模块放进主循环里跑起来并解决调参问题。5.1 主循环骨架主循环用固定步长 Ts每次迭代完成“参考轨迹计算→外环→滤波→内环→ESO→动力学更新→记录数据”这一整套动作。示例骨架% 参数初始化 prm.m 10; % kg prm.Iz 0.5; % kg·m^2 prm.r 0.1; % m prm.L 0.4; % m Ts 0.01; % s % 控制器与ESO参数 k.k1 1.2; k.k2 1.5; k.k3 0.35; kp_v 6; kp_w 12; w0_eso 20; tau_f 0.05; tau_max 5; % 初始状态 state [0; 0; 0; 0; 0]; % x, y, th, v, w v_ref 0; w_ref 0; z1_v 0; z2_v 0; z1_w 0; z2_w 0; u_m_v_prev 0; u_m_w_prev 0; % 仿真时长与数据记录 Tfinal 15; N round(Tfinal / Ts); log zeros(N, 10); for k 1:N t (k-1) * Ts; % 1. 参考轨迹以圆轨迹为例 R 1.0; omega_ref 0.5; xr R * cos(omega_ref * t); yr R * sin(omega_ref * t); xr_dot -R * omega_ref * sin(omega_ref * t); yr_dot R * omega_ref * cos(omega_ref * t); xr_dot2 -R * omega_ref^2 * cos(omega_ref * t); yr_dot2 -R * omega_ref^2 * sin(omega_ref * t); vr sqrt(xr_dot^2 yr_dot^2); thr wrapToPi(atan2(yr_dot, xr_dot)); wr (xr_dot * yr_dot2 - yr_dot * xr_dot2) / (xr_dot^2 yr_dot^2 1e-6); ref.x xr; ref.y yr; ref.th thr; ref.v vr; ref.w wr; % 2. 外环 p state(1:3); [vc, wc] kinematic_controller(p, ref, k); % 3. 参考速度滤波 v_ref_dot (vc - v_ref) / tau_f; w_ref_dot (wc - w_ref) / tau_f; v_ref v_ref Ts * v_ref_dot; w_ref w_ref Ts * w_ref_dot; % 4. 提取实际速度 v state(4); w state(5); % 5. 内环控制器ESO PD见上一节代码 % ... 省略重复部分 ... % 6. 仿真正动力学 d_v 0.2 * sin(2*t); % 线速度通道扰动 d_w 0.1 * cos(1.5*t); % 角速度通道扰动 dist [d_v; d_w]; dstate wmr_dynamics(state, [tau_R; tau_L], dist, prm); state state Ts * dstate; state(3) wrapToPi(state(3)); % 7. 记录数据 log(k, :) [t, state, v_ref, w_ref, z2_v, z2_w]; end5.2 参考轨迹生成圆轨迹相对简单。如果想验证更复杂的转向能力可以用8字形轨迹。参考轨迹的角速度不能直接对航向角做差分而是用速度与加速度的解析关系ωr (x_dot·y_ddot - y_dot·x_ddot) / (x_dot² y_dot²)这个式子的来源是航向角 θr atan2(y_dot, x_dot)对时间求导后经过代数化简就是这个形式。用这个公式的好处是数值稳定不会因为 wrap 产生跳变。轨迹参数示例圆轨迹x R·cos(ωt)y R·sin(ωt)8字轨迹x A·sin(ωt)y B·sin(2ωt)8字轨迹在交叉点附近线速度会减小对控制器的低速性能要求更高能暴露出很多圆轨迹看不出来的问题。5.3 参数初始值与整定顺序我建议的初始参数如下表整套参数可以稳定跑通圆轨迹参数初始值物理含义调大后的影响k11.2纵向误差反馈沿车头方向收敛更快过大则速度指令突变k21.5横向误差反馈横向纠偏更快过大容易振荡k30.35航向误差反馈转角度更积极过大易超调kp_v6线速度通道PD增益速度跟踪变快过大直接引发抖动kp_w12角速度通道PD增益角速度跟踪变快过大造成角速度噪声放大w0_eso20ESO带宽扰动估计快过大噪声放大tau_f0.05参考速度滤波时间常数越大指令越平滑但轨迹跟踪滞后整定顺序有讲究我建议按“内环→ESO→外环”的顺序来先把ESO关掉只保留PD内环把kp_v、kp_w调到速度跟踪稳定、不抖。这一步是在确认底层执行器没问题。打开ESO从较小的 ω0 开始逐步增大观察扰动估计 z2 是否平滑。看到一个数字很大的 z2 不要慌关键是看它是否稳定且有界。最后调外环 k1、k2、k3。用圆轨迹起步因为圆轨迹参数恒定问题容易定位稳定后再换8字轨迹测试转向能力。6. 扰动对比和踩坑记录实测中我改了什么才把它跑稳仿真程序搭好之后我拿它做了几组对比实验也在这个过程中找到了许多容易被忽视的问题。6.1 相同参数下开/关ESO的扰动差异我在第5节的主循环里注入了正弦型扰动d_v 0.5·sin(2t)d_w 0.3·cos(3t)同时把ESO分别开启和关闭对比圆轨迹的横向误差表现工况最大横向偏差稳态呈现关闭ESO约 0.08 m周期性波动明显开启ESO约 0.02 m波动被明显抑制这说明ESO把大部分时变扰动在进入速度回路之前就扣掉了内环PD不需要再靠误差去硬抗扰动。同样如果把负载变化仿真成质量在 10 kg 附近按正弦波动 ±20%ESO开启后速度跟踪误差仍然能维持在很小范围这是我最初没想到的。6.2 几个典型的坑角度环绕问题 仿真超过几十秒后如果不做 wrapToPiθ 累加得很大三角函数精度下降外环的航向误差计算也会出现 ±π 突跳。务必在每个采样步完成后对 θ 做 wrap。ESO带宽过高 w0_eso 调到 40 以上时ESO开始把测量噪声当成扰动z2 剧烈抖动控制量跟着抖。解决办法是记住一个原则ESO带宽只要高于实际扰动主频的3~5倍即可不需要无限大。内环与外环带宽比 内环必须比外环快一般内环闭环带宽取外环的5倍以上。如果内环响应慢外环的误差反馈和速度指令之间会形成大相位滞后系统表现就是“慢悠悠地振荡”。我遇到的情况是 k2 调大后系统反而发散最终靠提高内环 kp_w 解决的。控制量饱和 力矩限幅不能只限模型里的输出还要保证ESO的输入用的是实际发出的控制量。也就是如果控制器算出来的力矩被限幅了那么传给ESO的 u_m_v_prev 也应该是限幅后的值。否则ESO会继续以为控制器发出了很大的力导致观测器错误、整个系统发散。这个坑非常隐蔽我调试了很久才定位到。参考速度过零 8字轨迹在交叉点附近线速度接近零外环的 vc vr·cos(eth) k1·ex 可能变成负值机器人会试图倒车。如果项目要求只能前进需要在外环里加一个速度方向判定或者改用允许双向运动但带转向约束的控制方案。6.3 换个方式继续扩展这套程序还可以做几个方向的扩展把电机简化成一阶惯性环节加进动力学模型模仿真实电机响应延迟在ESO基础上加入参数自适应让 b0 的估计值随载荷变化移植到Simulink里把采样时间和控制时间分开更接近真实控制器的离散化形式把轨迹跟踪结果导出来与基于视觉的全局路径规划结合做完整的自主导航验证我最后想给一个最实际的建议如果你正在调这个程序遇到发散或抖动先不要急着怀疑算法先去看看控制顺序、限幅位置和角度环绕这三个地方是最容易出问题的。把基础框架跑稳之后再去折腾ESO带宽和增益整个系统会温顺很多。这套“运动学外环 动力学内环 ESO扰动补偿”的架构我用它跑过圆轨迹、8字轨迹和加扰实验稳定性都在可接受范围内希望你也能调出满意的曲线。