企业官网建设流程全解析

做轮式移动机器人轨迹跟踪，我最初在MATLAB里踩过不少坑：明明参考轨迹画出来很简单，机器人却动不动跑偏、抖动甚至发散。后来把运动学和动力学两层模型都用上，构成双闭环结构，又借鉴自抗扰控制（ADRC）里扩张状态观测器的思路，把摩擦、负载变化、轮径差异这类说不清道不明的扰动在线估计掉，问题才算真正解决。这篇文章把这套MATLAB程序的建模过程、控制律推导、代码实现和参数整定经验完整整理出来，非常适合正在做课程设计、毕业设计，或者想给AGV类项目做轨迹跟踪仿真预研的人参考。我会把为什么这么设计也讲清楚，因为光抄代码不搞懂逻辑，换个机器人参数基本调不出来。

1. 双闭环和自抗扰：先把控制结构定明白再写代码

我见过很多刚接触轨迹跟踪的人，第一版程序只有一个位姿外环：根据当前位置和目标轨迹的偏差，直接算出一个期望速度下发。这个结构放在理想运动学模型里确实能收敛，因为参考轨迹给到位移，位姿误差直接反映成速度指令。但一旦加入动力学约束、轮径差异、地面摩擦这些实际因素，期望速度与实际速度之间就不是瞬时一致的关系了。轮子响应需要时间，驱动力矩有限，还可能受扰。于是位姿误差虽然被外环调小，却始终在抖，越接近期望越容易来回振荡。这时候就必须在内环补一个动力学控制器。

1.1 单环控制缺了什么

一句话概括：运动学外环只能决定“机器人该以什么速度运动”，不能决定“实际能不能产生这个速度”。单环把这两件事混为一谈，等于默认执行器可以瞬间满足任意速度指令。但电机、减速器、轮胎摩擦、地面倾角这些因素，都会让实际速度与期望速度之间出现一个动态过程的差价。这个差价如果不处理，轨迹跟踪的误差就是周期性的，甚至会发散。我在模型里加入摩擦项后，原来的单环程序直接失效，这也是我下定决心改双闭环的直接原因。

1.2 双闭环结构的分工

双闭环的核心思路，是把轨迹跟踪拆成两层问题：

• 外环是运动学控制器：输入当前位姿和参考位姿，输出期望线速度 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·m
• d_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

绕垂直轴的总力矩，取决于左右轮驱动力对质心的力臂差。左右轮距质心的距离都是 L（L 是轮距的一半），所以：

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 略大于 k1，k3 稍小。

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与内环控制器的配合逻辑

整个内环控制器在单步里的正确顺序是：

1. 用上一时刻控制量更新 ESO，得到当前扰动估计 z2
2. 用当前参考速度和实际速度计算 PD 控制量 u0
3. 从 u0 中减去扰动估计 z2，得到等效加速度
4. 换算成力矩，限幅后输出给机器人模型

这个顺序如果颠倒，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]; end

5.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 参数初始值与整定顺序

我建议的初始参数如下表，整套参数可以稳定跑通圆轨迹：

整定顺序有讲究，我建议按“内环→ESO→外环”的顺序来：

1. 先把ESO关掉，只保留PD内环，把kp_v、kp_w调到速度跟踪稳定、不抖。这一步是在确认底层执行器没问题。
2. 打开ESO，从较小的 ω0 开始，逐步增大，观察扰动估计 z2 是否平滑。看到一个数字很大的 z2 不要慌，关键是看它是否稳定且有界。
3. 最后调外环 k1、k2、k3。用圆轨迹起步，因为圆轨迹参数恒定，问题容易定位；稳定后再换8字轨迹测试转向能力。

6. 扰动对比和踩坑记录：实测中我改了什么才把它跑稳

仿真程序搭好之后，我拿它做了几组对比实验，也在这个过程中找到了许多容易被忽视的问题。

6.1 相同参数下开/关ESO的扰动差异

我在第5节的主循环里注入了正弦型扰动：d_v = 0.5·sin(2t)，d_w = 0.3·cos(3t)，同时把ESO分别开启和关闭，对比圆轨迹的横向误差表现：

这说明ESO把大部分时变扰动在进入速度回路之前就扣掉了，内环PD不需要再靠误差去硬抗扰动。同样，如果把负载变化仿真成质量在 10 kg 附近按正弦波动 ±20%，ESO开启后速度跟踪误差仍然能维持在很小范围，这是我最初没想到的。

6.2 几个典型的坑

1. 角度环绕问题 仿真超过几十秒后，如果不做 wrapToPi，θ 累加得很大，三角函数精度下降，外环的航向误差计算也会出现 ±π 突跳。务必在每个采样步完成后对 θ 做 wrap。

2. ESO带宽过高 w0_eso 调到 40 以上时，ESO开始把测量噪声当成扰动，z2 剧烈抖动，控制量跟着抖。解决办法是记住一个原则：ESO带宽只要高于实际扰动主频的3~5倍即可，不需要无限大。

3. 内环与外环带宽比 内环必须比外环快，一般内环闭环带宽取外环的5倍以上。如果内环响应慢，外环的误差反馈和速度指令之间会形成大相位滞后，系统表现就是“慢悠悠地振荡”。我遇到的情况是 k2 调大后系统反而发散，最终靠提高内环 kp_w 解决的。

4. 控制量饱和 力矩限幅不能只限模型里的输出，还要保证ESO的输入用的是实际发出的控制量。也就是，如果控制器算出来的力矩被限幅了，那么传给ESO的 u_m_v_prev 也应该是限幅后的值。否则ESO会继续以为控制器发出了很大的力，导致观测器错误、整个系统发散。这个坑非常隐蔽，我调试了很久才定位到。

5. 参考速度过零 8字轨迹在交叉点附近线速度接近零，外环的 vc = vr·cos(eth) + k1·ex 可能变成负值，机器人会试图倒车。如果项目要求只能前进，需要在外环里加一个速度方向判定，或者改用允许双向运动但带转向约束的控制方案。

6.3 换个方式继续扩展

这套程序还可以做几个方向的扩展：

• 把电机简化成一阶惯性环节加进动力学模型，模仿真实电机响应延迟
• 在ESO基础上加入参数自适应，让 b0 的估计值随载荷变化
• 移植到Simulink里，把采样时间和控制时间分开，更接近真实控制器的离散化形式
• 把轨迹跟踪结果导出来，与基于视觉的全局路径规划结合，做完整的自主导航验证

我最后想给一个最实际的建议：如果你正在调这个程序，遇到发散或抖动，先不要急着怀疑算法，先去看看控制顺序、限幅位置和角度环绕，这三个地方是最容易出问题的。把基础框架跑稳之后，再去折腾ESO带宽和增益，整个系统会温顺很多。这套“运动学外环 + 动力学内环 + ESO扰动补偿”的架构，我用它跑过圆轨迹、8字轨迹和加扰实验，稳定性都在可接受范围内，希望你也能调出满意的曲线。