1. 人工势场法路径规划原理剖析
人工势场法(Artificial Potential Field)的核心思想是将无人车所处的环境建模为一个虚拟的势场。这个势场由两部分组成:目标点产生的引力场和障碍物产生的斥力场。引力场使无人车向目标点运动,斥力场则阻止无人车与障碍物碰撞。
在MATLAB中实现时,我们首先需要建立势场函数。引力势场通常采用二次函数表示:
matlab复制function U_att = attractive_potential(x, y, goal, k_att)
distance = norm([x y] - goal);
U_att = 0.5 * k_att * distance^2;
end
其中k_att是引力增益系数,控制引力场的强度。
斥力势场则更为复杂,需要考虑障碍物的影响范围:
matlab复制function U_rep = repulsive_potential(x, y, obstacles, k_rep, rho_0)
U_rep = 0;
for i = 1:size(obstacles,1)
dist = norm([x y] - obstacles(i,:));
if dist <= rho_0
U_rep = U_rep + 0.5 * k_rep * (1/dist - 1/rho_0)^2;
end
end
end
rho_0表示障碍物的最大影响距离,k_rep是斥力增益系数。
关键参数选择经验:k_att通常取1-5,k_rep取10-50,rho_0根据障碍物大小设置为2-5倍车体半径。这些参数需要根据具体场景调试。
2. MATLAB实现框架搭建
2.1 环境建模与初始化
首先需要构建仿真环境,包括:
- 设置起点和终点坐标
- 定义障碍物位置和大小
- 确定无人车参数(最大速度、转向角等)
matlab复制% 环境参数设置
start_pos = [0 0]; % 起点
goal_pos = [10 10]; % 终点
obstacles = [3 3; 5 5; 7 7]; % 障碍物位置
% 无人车参数
max_speed = 0.5; % 最大速度
dt = 0.1; % 时间步长
2.2 势场计算与合力求解
总势场是引力势场和斥力势场的叠加:
matlab复制function F = total_force(pos, goal, obstacles, k_att, k_rep, rho_0)
% 计算引力
F_att = k_att * (goal - pos);
% 计算斥力
F_rep = [0 0];
for i = 1:size(obstacles,1)
dist_vec = pos - obstacles(i,:);
dist = norm(dist_vec);
if dist <= rho_0
F_rep = F_rep + k_rep*(1/dist-1/rho_0)*(1/dist^3)*dist_vec;
end
end
F = F_att + F_rep; % 合力
end
2.3 路径迭代与更新
基于计算得到的合力,更新无人车位置:
matlab复制path = start_pos; % 初始化路径
current_pos = start_pos;
while norm(current_pos - goal_pos) > 0.5 % 未到达目标
% 计算合力
F = total_force(current_pos, goal_pos, obstacles, k_att, k_rep, rho_0);
% 限制最大速度
desired_vel = min(norm(F), max_speed) * (F/norm(F));
% 更新位置
new_pos = current_pos + desired_vel * dt;
path = [path; new_pos];
current_pos = new_pos;
end
3. 典型问题与优化方案
3.1 局部极小值问题
人工势场法最著名的缺陷是可能陷入局部极小值点。当引力和斥力平衡时,无人车会停止运动。解决方法包括:
- 随机扰动法:检测到停滞时施加随机力
matlab复制if norm(desired_vel) < 0.01 % 检测停滞
F = F + 0.1*randn(1,2); % 添加随机扰动
end
- 虚拟目标点法:在局部极小点附近设置临时目标
3.2 动态障碍物处理
对于移动障碍物,需要引入速度因素:
matlab复制function F_rep_dynamic = dynamic_repulsion(pos, vel, obstacle, obstacle_vel)
relative_vel = vel - obstacle_vel;
dist = norm(pos - obstacle);
if dist < safe_distance
F_rep_dynamic = repulsion_gain * relative_vel / dist^2;
else
F_rep_dynamic = [0 0];
end
end
3.3 参数调优技巧
- 引力系数k_att:过大导致路径震荡,过小则收敛慢
- 斥力系数k_rep:过大可能产生排斥震荡,过小则避障不及时
- 影响距离rho_0:应根据传感器实际探测范围设置
建议采用自适应参数:
matlab复制k_att = 2 * (1 - exp(-0.1*norm(current_pos - goal_pos))); % 距离目标越近,引力越小
4. 完整MATLAB实现示例
下面给出一个完整的实现代码框架:
matlab复制%% 初始化
clear; clc;
start_pos = [0 0];
goal_pos = [20 20];
obstacles = [5 5; 8 12; 15 15; 12 5];
k_att = 1.0;
k_rep = 30;
rho_0 = 3.0;
max_speed = 0.8;
dt = 0.1;
%% 路径规划主循环
path = start_pos;
current_pos = start_pos;
figure; hold on;
plot(goal_pos(1), goal_pos(2), 'g*', 'MarkerSize', 10);
plot(obstacles(:,1), obstacles(:,2), 'ro', 'MarkerSize', 8);
while norm(current_pos - goal_pos) > 0.5
% 计算合力
F = total_force(current_pos, goal_pos, obstacles, k_att, k_rep, rho_0);
% 速度限制
desired_vel = min(norm(F), max_speed) * (F/norm(F));
% 随机扰动(防局部极小)
if norm(desired_vel) < 0.05
F = F + 0.2*randn(1,2);
desired_vel = min(norm(F), max_speed) * (F/norm(F));
end
% 更新位置
new_pos = current_pos + desired_vel * dt;
path = [path; new_pos];
current_pos = new_pos;
% 实时显示
plot(path(:,1), path(:,2), 'b-');
plot(current_pos(1), current_pos(2), 'bo');
drawnow;
pause(0.05);
end
%% 势场可视化
[X,Y] = meshgrid(0:0.5:20);
U = zeros(size(X));
for i = 1:size(X,1)
for j = 1:size(Y,2)
U(i,j) = attractive_potential(X(i,j),Y(i,j),goal_pos,k_att) + ...
repulsive_potential(X(i,j),Y(i,j),obstacles,k_rep,rho_0);
end
end
figure;
surf(X,Y,U);
xlabel('X'); ylabel('Y'); zlabel('Potential');
5. 进阶优化方向
- 混合算法:结合A*或RRT等全局规划算法
- 势场改进:使用谐波势场避免局部极小
- 机器学习:用强化学习优化势场参数
- 多车协同:考虑多智能体间的相互作用势
在实际无人车系统中,人工势场法常作为局部规划器,与全局规划器配合使用。MATLAB的Robotics System Toolbox提供了更专业的路径规划工具,但理解基础原理对于算法调优至关重要。
