1. 轮式移动机器人轨迹跟踪的核心挑战
轮式移动机器人的轨迹跟踪问题本质上是一个非线性系统的控制问题。在实际工程中，我们常常遇到几个典型痛点：首先是系统存在未建模动态和外部扰动，比如地面摩擦系数变化、负载突变等；其次是运动学和动力学耦合带来的控制复杂性；再者是实时性要求高，算法必须在有限计算资源下稳定运行。
我去年参与的一个AGV项目就深刻体会到了这些难点。当机器人以1.5m/s速度运行时，传统PID控制在小曲率路径上会产生最大12cm的跟踪误差，这完全不能满足工业场景要求。后来我们采用了双闭环控制架构，结合自抗扰思想，最终将误差控制在3cm以内。
2. 系统建模与双闭环控制架构
2.1 运动学模型建立
对于差速驱动的轮式机器人，其运动学模型可以表示为：
matlab复制% 机器人位姿状态方程
function dx = kinematic_model(t, x, u)
v = u(1); % 线速度
w = u(2); % 角速度
dx = [v*cos(x(3));
v*sin(x(3));
w];
end
这个模型描述了机器人位姿(x,y,θ)随时间的变化关系。但要注意，实际应用中需要考虑轮径误差、轮距变化等参数不确定性，我们在代码中通常会做成可配置参数：
matlab复制% 实际工程中的鲁棒参数处理
wheel_diameter = 0.15; % 标称轮径
wheel_base = 0.35; % 标称轮距
param_variation = 0.1; % 允许10%参数变化
2.2 动力学模型补偿
单纯依靠运动学控制会导致动态性能不足，特别是在高速或大曲率运动时。完整的动力学模型应考虑：
- 电机电磁特性
- 传动系统阻尼
- 质量分布影响
在MATLAB中我们通常用状态空间表示：
matlab复制A = [0 1 0;
0 -b/J K/J;
0 -K/L -R/L];
B = [0 0;
0 1/J;
1/L 0];
其中J是转动惯量，b是阻尼系数，K是电机常数，R是电阻，L是电感。
2.3 双闭环控制结构设计
我们的控制架构采用内外环分工：
- 外环（运动学层）：基于期望轨迹生成速度指令
- 内环（动力学层）：精确跟踪速度指令
mermaid复制graph TD
A[期望轨迹] --> B(轨迹规划)
B --> C{外环控制器}
C --> D[速度指令]
D --> E{内环控制器}
E --> F[电机电压]
F --> G[实际轨迹]
G --> C
在MATLAB实现时，两个环路的采样周期需要特别注意：
matlab复制outer_loop_time = 0.05; % 外环50ms
inner_loop_time = 0.01; % 内环10ms
3. 自抗扰控制(ADRC)的核心实现
3.1 扩张状态观测器设计
ADRC的核心在于将模型不确定性和外部扰动视为"总和扰动"，通过ESO进行实时估计。在MATLAB中实现三阶ESO：
matlab复制function [z, dz] = eso(f, y, u, z, h, beta)
e = z(1) - y;
dz = zeros(3,1);
dz(1) = z(2) - beta(1)*e;
dz(2) = z(3) - beta(2)*fal(e,0.5,h) + b*u;
dz(3) = -beta(3)*fal(e,0.25,h);
z = z + dz*h; % 欧拉积分
end
function f = fal(e,alpha,delta)
if abs(e)>delta
f = abs(e)^alpha*sign(e);
else
f = e/(delta^(1-alpha));
end
end
参数整定经验：
- β1=100, β2=300, β3=1000 是较好的起始点
- 观测器带宽应比控制器带宽高3-5倍
- 对于2kg的机器人，delta取0.01效果较好
3.2 非线性状态误差反馈
采用非线性组合代替传统PID的线性组合：
matlab复制function u = nlsef(e1, e2, alpha, delta)
u = fal(e1,alpha,delta) + fal(e2,alpha,delta);
end
实际调试中发现，α=0.75时能在快速性和超调间取得较好平衡。
4. MATLAB实现的关键技巧
4.1 实时性优化方案
- 代码向量化：避免循环，使用矩阵运算
matlab复制% 不好的写法
for i = 1:100
y(i) = sin(x(i));
end
% 优化写法
y = sin(x);
- 预分配内存：
matlab复制results = zeros(1,1000); % 预先分配
- 使用S函数：对于实时控制，建议封装成S-function
4.2 参数调试可视化工具
开发了交互式调试界面：
matlab复制figure;
subplot(2,1,1);
h1 = plot(t, ref, 'r--', t, actual, 'b-');
legend('期望','实际');
subplot(2,1,2);
h2 = stem(t, error);
通过GUI滑块实时调整参数：
matlab复制beta1_slider = uicontrol('Style', 'slider',...
'Min',50,'Max',200,'Value',100,...
'Callback', @update_beta1);
4.3 工程实践中的问题排查
- 抖动问题：
- 现象：小车出现高频小幅振荡
- 排查：检查ESO带宽是否过高
- 解决：降低β参数，增加delta值
- 稳态误差：
- 现象：最终存在恒定偏差
- 排查：检查观测器是否收敛
- 解决：增加ESO阶数或调整非线性函数
- 响应迟缓：
- 现象：转向响应慢
- 排查：内外环采样周期设置
- 解决：调整外环周期至30ms
5. 完整实现案例
5.1 主控制流程
matlab复制function main()
% 初始化
T = 10; h = 0.01;
N = T/h;
% 参考轨迹生成
ref = @(t) [sin(t); cos(t); t];
% 控制器实例
ctrl = ADRC_Controller();
% 主循环
for k = 1:N
t = k*h;
y = robot.get_pose(); % 获取实际位姿
% 外环控制
[v_ref, w_ref] = ctrl.outer_loop(ref(t), y);
% 内环控制
[vl, vr] = ctrl.inner_loop(v_ref, w_ref);
% 执行
robot.set_wheel_speed(vl, vr);
% 数据记录
log_data(t, y, ref(t));
end
end
5.2 性能对比测试
在3种不同轨迹下的表现：
| 轨迹类型 | 最大误差(m) | 稳态误差(m) | 计算耗时(ms) |
|---|---|---|---|
| 直线 | 0.012 | 0.005 | 2.1 |
| 圆周 | 0.025 | 0.008 | 2.3 |
| 8字形 | 0.038 | 0.015 | 2.7 |
测试环境：
- 处理器：i5-8250U
- MATLAB版本：R2021b
- 机器人质量：2.5kg
- 最大速度：1.8m/s
6. 进阶优化方向
- 参数自适应机制：
matlab复制function adjust_params(error)
persistent last_error;
if isempty(last_error)
last_error = error;
end
delta = error - last_error;
if norm(delta) > threshold
% 触发参数调整
beta = beta * (1 + sign(delta)*0.05);
end
last_error = error;
end
- 机器学习结合：
- 使用LSTM预测扰动变化
- 强化学习优化非线性参数
- 硬件在环测试：
matlab复制% 连接Speedgoat实时目标机
tg = slrt;
load(tg, 'controller_model');
start(tg);
在实际项目中，我们发现将ADRC与模糊逻辑结合能进一步提升在非结构化环境中的性能。特别是在地面材质突变时，传统方法会产生约15%的性能波动，而混合方法能将波动控制在5%以内。