1. 项目背景与核心思路
车辆编队控制在智能交通和无人驾驶领域有着广泛的应用前景。领航追随法（Leader-Follower）作为经典的编队控制策略，通过指定领航车辆和追随车辆的相对位置关系，实现整个车队的协同运动。但在实际应用中，单纯依靠领航追随法难以应对复杂环境中的障碍物避让问题。
我在实际项目中发现，将人工势场法（Artificial Potential Field）与领航追随法结合，可以在保持编队形态的同时实现动态避障。这种混合策略既保留了领航追随法结构简单的优势，又通过人工势场法增强了系统的环境适应能力。
2. 系统建模与控制器设计
2.1 车辆运动学模型
对于轮式移动机器人或智能车辆，通常采用以下简化运动学模型：
code复制ẋ = v·cosθ
ẏ = v·sinθ
θ̇ = ω
其中(x,y)表示车辆位置，θ为航向角，v为线速度，ω为角速度。在MATLAB中，我们可以将这个模型封装为一个函数：
matlab复制function [dx, dy, dtheta] = vehicle_model(v, omega, theta)
dx = v * cos(theta);
dy = v * sin(theta);
dtheta = omega;
end
2.2 领航追随法实现
领航追随法的核心是保持追随车辆与领航车辆之间的期望相对位置。设领航车位置为(xₗ,yₗ)，追随车位置为(xᵢ,yᵢ)，期望的相对位置为(ρᵢ,φᵢ)，则控制目标可表示为：
code复制xᵢ = xₗ - ρᵢ·cos(φᵢ + θₗ)
yᵢ = yₗ - ρᵢ·sin(φᵢ + θₗ)
在MATLAB中实现这个控制律：
matlab复制function [v, omega] = leader_follower_control(leader_pose, follower_pose, desired_rho, desired_phi)
% leader_pose: [x_l, y_l, theta_l]
% follower_pose: [x_f, y_f, theta_f]
error_x = leader_pose(1) - follower_pose(1) - desired_rho*cos(desired_phi + leader_pose(3));
error_y = leader_pose(2) - follower_pose(2) - desired_rho*sin(desired_phi + leader_pose(3));
Kp = 0.5; % 比例增益
v = Kp * sqrt(error_x^2 + error_y^2);
desired_theta = atan2(error_y, error_x);
omega = 1.2 * (desired_theta - follower_pose(3)); % 航向控制
end
注意：实际应用中需要根据车辆动力学特性调整控制增益，避免响应过快导致震荡。
3. 人工势场法避障设计
3.1 势场函数构建
人工势场法通过构建虚拟势场实现避障。障碍物产生斥力势场，目标点产生引力势场。对于单个障碍物，斥力势函数可表示为：
code复制U_rep(q) = { ½η(1/d(q) - 1/d₀)², if d(q) ≤ d₀
{ 0, if d(q) > d₀
其中η为斥力增益系数，d(q)为车辆到障碍物的距离，d₀为势场影响范围。
MATLAB实现示例：
matlab复制function F_rep = repulsive_force(vehicle_pos, obstacle_pos, eta, d0)
d = norm(vehicle_pos - obstacle_pos);
if d <= d0
F_rep = eta*(1/d - 1/d0)*(1/d^2)*((vehicle_pos - obstacle_pos)/d);
else
F_rep = [0; 0];
end
end
3.2 势场与编队控制的融合
将人工势场产生的斥力作为额外控制输入，与领航追随法的控制输出叠加：
matlab复制function [v_total, omega_total] = combined_control(leader_pose, follower_pose, obstacles)
% 领航追随法基础控制
[v_lf, omega_lf] = leader_follower_control(leader_pose, follower_pose, 2.0, pi/4);
% 人工势场避障
F_rep_total = [0; 0];
eta = 0.8; d0 = 3.0;
for i = 1:size(obstacles,1)
F_rep = repulsive_force(follower_pose(1:2), obstacles(i,:), eta, d0);
F_rep_total = F_rep_total + F_rep;
end
% 力向量转换为速度指令
v_rep = 0.3 * norm(F_rep_total);
omega_rep = 0.5 * atan2(F_rep_total(2), F_rep_total(1));
% 控制量合成
v_total = v_lf + v_rep;
omega_total = omega_lf + omega_rep;
end
4. MATLAB仿真实现
4.1 仿真环境搭建
创建一个包含领航车、追随车和障碍物的仿真场景：
matlab复制% 初始化
leader_traj = [linspace(0,20,100)' linspace(0,0,100)' linspace(0,0,100)'];
obstacles = [5 -1; 8 1.5; 12 -0.8]; % 障碍物位置
% 追随车初始状态
follower_pose = [leader_traj(1,1)-2, leader_traj(1,2)-2, 0];
% 仿真参数
dt = 0.1; % 时间步长
poses = zeros(length(leader_traj), 3); % 存储追随车轨迹
4.2 主仿真循环
matlab复制for k = 1:length(leader_traj)
% 获取当前领航车状态
leader_pose = leader_traj(k,:);
% 组合控制
[v, omega] = combined_control(leader_pose, follower_pose, obstacles);
% 更新追随车状态
[dx, dy, dtheta] = vehicle_model(v, omega, follower_pose(3));
follower_pose = follower_pose + [dx, dy, dtheta]*dt;
poses(k,:) = follower_pose;
% 可视化
plot_scene(leader_pose, follower_pose, obstacles);
pause(0.05);
end
4.3 可视化函数
matlab复制function plot_scene(leader, follower, obstacles)
clf; hold on; axis equal;
% 绘制领航车
plot(leader(1), leader(2), 'ro', 'MarkerSize', 10, 'LineWidth', 2);
quiver(leader(1), leader(2), 0.5*cos(leader(3)), 0.5*sin(leader(3)), 'r');
% 绘制追随车
plot(follower(1), follower(2), 'bo', 'MarkerSize', 10, 'LineWidth', 2);
quiver(follower(1), follower(2), 0.5*cos(follower(3)), 0.5*sin(follower(3)), 'b');
% 绘制障碍物
plot(obstacles(:,1), obstacles(:,2), 'ks', 'MarkerSize', 12, 'LineWidth', 2);
xlim([-2 22]); ylim([-3 3]);
xlabel('X位置'); ylabel('Y位置');
title('车辆编队控制与避障仿真');
grid on;
end
5. 关键参数调优与问题排查
5.1 参数影响分析
-
领航追随法参数：
- 期望距离ρ：影响编队紧凑程度，过小可能导致碰撞风险
- 期望角度φ：决定编队形态（直线、三角等）
- 控制增益Kp：影响响应速度，过大易引发震荡
-
人工势场参数：
- 斥力增益η：决定避障强度，过大可能导致路径抖动
- 影响范围d₀：决定障碍物检测范围，过大可能过早偏离路径
5.2 常见问题与解决方案
| 问题现象 | 可能原因 | 解决方案 |
|---|---|---|
| 编队形态不稳定 | 控制增益过大 | 逐步减小Kp直到系统稳定 |
| 避障时震荡 | 斥力场过强 | 降低η或增大d₀ |
| 无法避开障碍物 | d₀设置过小 | 适当增大d₀或增加η |
| 追随车偏离过大 | 领航车速度过快 | 限制领航车最大速度 |
5.3 实际调试技巧
-
分步调试法：
- 先单独测试领航追随法，确保基础编队功能正常
- 然后加入单个障碍物测试避障效果
- 最后测试多障碍物场景
-
可视化调试：
matlab复制% 在控制函数中添加势场可视化 quiver(follower_pose(1), follower_pose(2), F_rep_total(1), F_rep_total(2), 'g'); -
参数自动调优：
可以编写简单的参数搜索算法，寻找最优参数组合：matlab复制function best_params = parameter_tuning() param_ranges = struct('Kp', 0.1:0.1:1.0, 'eta', 0.5:0.1:1.5, 'd0', 2:0.5:4); % ...实现参数搜索和性能评估... end
6. 扩展与改进方向
6.1 多车编队实现
对于多追随车系统，可以构建如下控制结构：
matlab复制% 初始化多车状态
num_followers = 3;
follower_poses = repmat([leader_traj(1,1)-2, leader_traj(1,2)-2, 0], num_followers, 1);
desired_rhos = [2.0, 2.2, 1.8]; % 各车期望距离
desired_phis = [pi/4, -pi/4, 0]; % 各车期望角度
for k = 1:length(leader_traj)
for i = 1:num_followers
[v, omega] = combined_control(leader_traj(k,:), follower_poses(i,:), obstacles);
% 更新各车状态...
end
end
6.2 动态障碍物处理
对于移动障碍物，需要引入速度项到斥力场计算中：
matlab复制function F_rep = dynamic_repulsion(vehicle_pos, obstacle_pos, obstacle_vel, eta, d0)
relative_vel = [0;0]; % 假设车辆当前静止
d = norm(vehicle_pos - obstacle_pos);
if d <= d0
F_rep = eta*(1/d - 1/d0)*(1/d^2)*((vehicle_pos - obstacle_pos)/d) + ...
0.2*relative_vel/d;
else
F_rep = [0; 0];
end
end
6.3 通信延迟补偿
在实际多车系统中，通信延迟会影响控制性能。可以设计预测补偿算法：
matlab复制function predicted_pose = predict_pose(current_pose, history, delay_steps)
% 基于历史数据预测delay_steps后的状态
if size(history,1) < 3
predicted_pose = current_pose;
else
% 简单线性预测
vel = (history(end,:) - history(end-1,:)) / dt;
predicted_pose = current_pose + vel * delay_steps * dt;
end
end
在实测中，我发现当领航车速度超过1.5m/s时，传统的领航追随法会出现明显的跟随滞后。通过引入速度前馈补偿，可以显著改善这一情况：
matlab复制% 在leader_follower_control函数中添加前馈项
v_ff = 0.7 * norm([leader_pose(1)-prev_leader(1), leader_pose(2)-prev_leader(2)])/dt;
v = Kp * sqrt(error_x^2 + error_y^2) + v_ff;
对于更复杂的场景，可以考虑将人工势场法改进为基于强化学习的自适应避障策略，这是我下一步计划尝试的方向。在实际部署中，还需要考虑传感器噪声、定位误差等实际问题，可以通过扩展卡尔曼滤波等方法增强系统鲁棒性。