1. 项目概述
在工业自动化和机器人领域,轨迹规划与控制一直是核心课题。这个MATLAB项目将带您从零开始,完整实现一个小车的运动轨迹规划与控制系统。不同于教科书上的理论推导,我们将聚焦于如何将数学公式转化为实际可运行的代码,并解决工程实现中的各种实际问题。
我从事运动控制算法开发已有8年时间,发现很多初学者在学习轨迹规划时容易陷入两个极端:要么过于关注理论推导而不会编程实现,要么直接调用现成函数却不理解底层原理。这个项目正是为了填补这个空白——我们将从最基本的运动学模型出发,逐步构建完整的控制体系,最终实现小车对复杂轨迹的精确跟踪。
2. 系统建模与参数设定
2.1 小车运动学模型
我们采用两轮差速驱动小车作为研究对象,这是移动机器人中最常见的结构之一。其运动学模型可以用以下方程描述:
code复制ẋ = v * cosθ
ẏ = v * sinθ
θ̇ = ω
其中(x,y)是小车中心位置,θ是朝向角,v是线速度,ω是角速度。对于差速驱动,v和ω与左右轮速度(vL, vR)的关系为:
code复制v = (vR + vL)/2
ω = (vR - vL)/L
L是小车两轮间的轴距。在MATLAB中,我们首先需要定义这些基本参数:
matlab复制% 小车物理参数
L = 0.5; % 轴距(m)
wheel_radius = 0.1; % 轮子半径(m)
max_motor_rpm = 100; % 电机最大转速
注意:实际项目中这些参数需要实测获得。轴距测量误差超过2mm就会明显影响控制精度。
2.2 轨迹规划基础
常见的轨迹规划方法有三种:
- 多项式插值:计算简单但可能产生不合理的加速度
- 样条曲线:平滑性好但计算量较大
- 最优控制:性能最优但实现复杂
我们选择三次样条作为折中方案。MATLAB的spline函数可以方便地生成平滑轨迹:
matlab复制% 生成途经点
waypoints = [0 0; 1 2; 3 3; 5 1];
% 参数化路径
t = linspace(0,1,size(waypoints,1));
% 生成样条
spline_x = spline(t, waypoints(:,1)');
spline_y = spline(t, waypoints(:,2)');
3. 控制算法实现
3.1 PID控制器设计
轨迹跟踪需要两个层级的控制:
- 位置级PID:确保小车到达目标点
- 速度级PID:调节电机转速
位置级PID的核心代码如下:
matlab复制function [v, omega] = position_controller(x_err, y_err, theta_err, prev_err)
% PID参数
Kp = [0.5 0.5 0.3];
Ki = [0.01 0.01 0];
Kd = [0.05 0.05 0.02];
persistent integral;
if isempty(integral)
integral = [0 0 0];
end
% PID计算
integral = integral + [x_err y_err theta_err];
derivative = [x_err y_err theta_err] - prev_err;
v = Kp(1)*x_err + Ki(1)*integral(1) + Kd(1)*derivative(1);
omega = Kp(3)*theta_err + Ki(3)*integral(3) + Kd(3)*derivative(3);
end
实操心得:PID参数整定建议先用Ziegler-Nichols方法初步确定,再微调。实际调试时先调P,再调D,最后加I。
3.2 纯追踪算法改进
基础纯追踪算法(Look-ahead)存在曲率突变问题。我们改进为自适应前视距离算法:
matlab复制function [delta, lookahead_dist] = pure_pursuit(current_pose, path, min_Ld, max_Ld)
% 计算当前速度下的自适应前视距离
current_speed = norm(current_pose(3:4));
lookahead_dist = min_Ld + (max_Ld-min_Ld)*current_speed/max_speed;
% 寻找路径上最近点
[~, idx] = min(vecnorm(path(:,1:2) - current_pose(1:2), 2, 2));
% 计算目标点
target_point = find_target(path, idx, lookahead_dist);
% 计算转向角
alpha = atan2(target_point(2)-current_pose(2), target_point(1)-current_pose(1)) - current_pose(5);
delta = atan2(2*L*sin(alpha), lookahead_dist);
end
4. 仿真环境搭建
4.1 MATLAB Simulink模型
建议搭建如下图所示的仿真系统:
code复制[轨迹生成] → [控制器] → [小车模型] → [反馈]
关键配置要点:
- 使用Fixed-step solver,步长设为0.01s
- 启用Zero-crossing detection
- 小车模型需加入电机响应延迟(约50ms)
4.2 可视化调试技巧
创建实时动画可以大幅提高调试效率:
matlab复制h_anim = figure;
axis equal; grid on;
xlim([-1 6]); ylim([-1 4]);
h_car = rectangle('Position',[0 0 0.5 0.3],'Curvature',[0.3 0.3]);
h_path = plot(waypoints(:,1),waypoints(:,2),'b--');
while sim_running
% 更新小车位置
set(h_car,'Position',[x-0.25 y-0.15 0.5 0.3],'Rotation',theta*180/pi);
% 绘制实际轨迹
set(h_path,'XData',[get(h_path,'XData') x],...
'YData',[get(h_path,'YData') y]);
drawnow;
end
5. 实际部署注意事项
5.1 离散化处理
理论算法需要针对数字控制器进行离散化。以PID为例:
matlab复制% 连续域PID: C(s) = Kp + Ki/s + Kd*s
% 离散化(采用Tustin变换):
function [u, integral] = discrete_pid(e, e_prev, integral, Kp, Ki, Kd, Ts)
integral = integral + e*Ts;
derivative = (e - e_prev)/Ts;
u = Kp*e + Ki*integral + Kd*derivative;
end
采样时间Ts的选择经验:
- 位置环:10-50ms
- 速度环:1-5ms
- 电流环:0.1-1ms
5.2 抗饱和处理
实际电机有转速限制,必须加入抗饱和逻辑:
matlab复制% 在PID输出后添加限制
v_left = max(min(v_left, max_speed), -max_speed);
v_right = max(min(v_right, max_speed), -max_speed);
% 积分抗饱和
if (v_left >= max_speed && e>0) || (v_left <= -max_speed && e<0)
integral = integral - Ki*e*Ts;
end
6. 性能优化技巧
6.1 轨迹重参数化
原始样条可能不均匀,导致速度突变。建议采用弧长参数化:
matlab复制% 计算累积弧长
s = zeros(size(path,1),1);
for i = 2:length(s)
s(i) = s(i-1) + norm(path(i,1:2)-path(i-1,1:2));
end
% 均匀重采样
new_s = linspace(0,s(end),100);
new_path = interp1(s, path, new_s, 'spline');
6.2 前瞻控制
提前预判轨迹曲率变化,提前减速:
matlab复制% 计算前方曲率
lookahead_points = 10; % 前瞻点数
kappa = zeros(size(path,1)-lookahead_points,1);
for i = 1:length(kappa)
dx = path(i+lookahead_points,1) - path(i,1);
ddx = path(i+lookahead_points,2) - 2*path(i+round(lookahead_points/2),2) + path(i,2);
kappa(i) = abs(ddx) / (1 + dx^2)^1.5;
end
% 根据曲率限制速度
max_speed = min(max_speed, sqrt(max_lat_acc ./ kappa));
7. 常见问题排查
7.1 轨迹振荡问题
症状:小车在路径附近来回摆动
可能原因:
- 微分增益过大 → 降低Kd
- 采样时间过长 → 减小控制周期
- 执行机构延迟 → 加入延迟补偿
7.2 稳态误差问题
症状:始终无法准确到达目标点
解决方案:
- 检查积分项是否被限幅
- 增大Ki但要防止积分饱和
- 确认编码器分辨率是否足够
7.3 急转弯失控问题
症状:在急弯处偏离路径
优化方法:
- 加入前馈控制:基于曲率预先增加转向
- 动态调整前视距离
- 限制最大转向角速度
8. 进阶扩展方向
8.1 加入障碍物避障
将RRT或A*算法与轨迹规划结合:
matlab复制% 简单避障示例
function new_path = avoid_obstacle(path, obstacle)
% 寻找最近碰撞点
collision_idx = find(vecnorm(path(:,1:2)-obstacle(1:2),2,2) < obstacle(3), 1);
% 生成避障点
avoid_point = obstacle(1:2) + obstacle(3)*1.2*(path(collision_idx,1:2)-obstacle(1:2))/norm(path(collision_idx,1:2)-obstacle(1:2));
% 重新规划
new_path = [path(1:collision_idx-1,:);
avoid_point;
path(collision_idx+1:end,:)];
end
8.2 多车协同控制
扩展为多车系统时需考虑:
- 通信延迟补偿
- 优先级管理
- 冲突检测与消解
实现一个简单的车队保持算法:
matlab复制function [v, omega] = fleet_control(leader_pose, follower_pose, desired_offset)
% 计算相对位置
rel_pos = leader_pose(1:2) - follower_pose(1:2);
rel_angle = atan2(rel_pos(2), rel_pos(1));
% 距离和角度误差
dist_err = norm(rel_pos) - desired_offset;
angle_err = rel_angle - follower_pose(3);
% 生成控制量
v = leader_pose(4)*cos(angle_err) + Kp_dist*dist_err;
omega = leader_pose(5) + Kp_angle*angle_err;
end
在实际项目中,我发现最影响精度的往往不是算法本身,而是对系统特性的准确建模。建议花至少30%的时间在参数辨识和模型验证上。比如通过阶跃响应测试电机的时间常数,通过圆周运动测试轮子滑移率等。这些基础工作做好后,控制算法的调试会事半功倍。