1. 无人车路径规划与人工势场法概述
在自动驾驶技术快速发展的今天,路径规划作为无人车决策系统的核心模块,直接影响着车辆的行驶安全性和效率。人工势场法(Artificial Potential Field, APF)作为一种经典的局部路径规划算法,因其计算效率高、实现简单等特点,在无人车领域有着广泛应用。
我第一次接触人工势场法是在2016年参与校园无人车项目时。当时团队尝试了多种路径规划算法,最终发现对于低速场景下的实时避障需求,APF展现出了独特的优势。它通过构建虚拟的势场环境,将目标点设置为引力源,障碍物设置为斥力源,使车辆像在物理场中运动的粒子一样,自然地避开障碍物并向目标移动。
MATLAB作为工程计算领域的标准工具,提供了强大的矩阵运算能力和可视化功能,非常适合APF算法的快速原型开发和验证。在接下来的内容中,我将详细分享如何用MATLAB实现基于人工势场法的无人车路径规划,包括算法原理、参数调优技巧以及实际应用中的各种"坑"与解决方案。
2. 人工势场法核心原理与数学模型
2.1 基本势场构建原理
人工势场法的核心思想非常简单直观:将目标点设置为引力场源,障碍物设置为斥力场源。无人车在这两种场的共同作用下,受到指向目标的引力和远离障碍物的斥力,其合力决定了车辆的运动方向。
引力场函数通常采用二次函数形式:
U_att(q) = 0.5 * ξ * ρ^2(q, q_goal)
其中ξ是引力增益系数,ρ(q, q_goal)表示当前位置q到目标点q_goal的欧式距离。
对应的引力为引力场的负梯度:
F_att(q) = -∇U_att(q) = ξ * (q_goal - q)
斥力场函数则更为复杂一些,需要考虑障碍物的影响范围。常用的表达式为:
U_rep(q) = 0.5 * η * (1/ρ(q, q_obs) - 1/ρ0)^2, 当ρ(q, q_obs) ≤ ρ0
U_rep(q) = 0, 当ρ(q, q_obs) > ρ0
其中η是斥力增益系数,ρ0是障碍物的影响半径。
2.2 势场参数的实际意义与调优
在实际应用中,各参数的设置直接影响规划效果:
-
引力增益ξ:决定车辆趋向目标的速度。过小会导致车辆响应迟缓,过大则可能在障碍物附近产生振荡。经验值范围通常在0.5-2之间。
-
斥力增益η:决定车辆避障的激进程度。太小会导致避障不及时,太大则可能使车辆在狭窄通道中"卡住"。建议初始值设为障碍物大小的5-10倍。
-
影响半径ρ0:定义了障碍物的"势力范围"。一般设为车辆半径的2-3倍,太大会导致不必要的绕行,太小则可能反应不及。
提示:在MATLAB实现时,建议将这些参数设为全局变量,方便调试时快速调整。我通常会创建一个param.m文件专门存放这些参数。
2.3 经典APF的局限性及改进方案
传统APF存在几个典型问题:
-
局部极小值问题:当引力和斥力平衡时,车辆会陷入停滞。解决方案包括:
- 引入随机扰动
- 结合全局规划器
- 使用虚拟目标点技术
-
狭窄通道通过困难:多个障碍物的斥力叠加可能导致合力为零。可以通过:
- 动态调整ρ0
- 引入切向力
- 使用非对称势场
-
动态障碍物处理:原始APF对动态环境适应性差。改进方法:
- 引入速度势场
- 预测障碍物运动轨迹
- 自适应调整参数
在MATLAB实现时,我们可以先构建基础APF,再逐步加入这些改进策略。
3. MATLAB实现详解
3.1 环境建模与初始化
首先需要构建仿真环境。我通常采用两种方式:
- 网格化表示:
matlab复制mapResolution = 0.1; % 米/格
mapSize = [100, 100]; % 网格尺寸
occupancyMap = zeros(mapSize); % 占据网格
- 连续坐标表示:
matlab复制obstacles = [20, 30; 45, 60; 70, 25]; % 障碍物坐标列表
vehiclePos = [10, 10]; % 车辆初始位置
goalPos = [90, 90]; % 目标位置
可视化设置非常重要,可以帮助我们直观观察算法行为:
matlab复制figure;
hold on;
axis equal;
xlim([0 mapSize(1)*mapResolution]);
ylim([0 mapSize(2)*mapResolution]);
plot(goalPos(1), goalPos(2), 'gp', 'MarkerSize', 15, 'MarkerFaceColor', 'g');
plot(vehiclePos(1), vehiclePos(2), 'bo', 'MarkerSize', 10, 'MarkerFaceColor', 'b');
for i = 1:size(obstacles,1)
plot(obstacles(i,1), obstacles(i,2), 'rs', 'MarkerSize', 8, 'MarkerFaceColor', 'r');
end
3.2 势场计算函数实现
引力场计算函数:
matlab复制function [F_att, U_att] = attractiveForce(currentPos, goalPos, xi)
r = norm(currentPos - goalPos);
U_att = 0.5 * xi * r^2;
F_att = xi * (goalPos - currentPos);
end
斥力场计算函数(考虑多个障碍物):
matlab复制function [F_rep, U_rep] = repulsiveForce(currentPos, obstacles, eta, rho0)
F_rep = [0, 0];
U_rep = 0;
for i = 1:size(obstacles,1)
obsPos = obstacles(i,:);
r = norm(currentPos - obsPos);
if r <= rho0
U_rep = U_rep + 0.5 * eta * (1/r - 1/rho0)^2;
F_rep = F_rep + eta * (1/r - 1/rho0) * (1/r^3) * (currentPos - obsPos);
end
end
end
3.3 主循环与运动控制
核心规划循环实现:
matlab复制maxIter = 500;
path = zeros(maxIter, 2);
path(1,:) = vehiclePos;
currentPos = vehiclePos;
for k = 2:maxIter
% 计算合力
[F_att, ~] = attractiveForce(currentPos, goalPos, xi);
[F_rep, ~] = repulsiveForce(currentPos, obstacles, eta, rho0);
F_total = F_att + F_rep;
% 限幅处理
maxF = 5; % 最大作用力
if norm(F_total) > maxF
F_total = F_total / norm(F_total) * maxF;
end
% 更新位置(简单欧拉积分)
dt = 0.1; % 时间步长
newPos = currentPos + F_total * dt;
% 检查是否到达目标
if norm(newPos - goalPos) < 0.5
path(k,:) = goalPos;
break;
end
% 更新路径和绘图
path(k,:) = newPos;
currentPos = newPos;
% 实时可视化
plot([currentPos(1), newPos(1)], [currentPos(2), newPos(2)], 'b-');
drawnow;
end
% 修剪路径
path = path(1:k,:);
3.4 性能优化技巧
在实际实现中,有几个关键优化点:
-
障碍物空间分区:使用k-d树或网格分区加速最近邻搜索,特别是障碍物数量多时。
-
力场预计算:对于静态环境,可以预先计算网格化的势场,运行时只需插值。
-
并行计算:MATLAB的parfor可以并行处理多个障碍物的斥力计算。
-
可视化优化:减少绘图频率,或使用轻量级的plot方式。
优化后的斥力计算示例:
matlab复制function [F_rep, U_rep] = repulsiveForceOpt(currentPos, obstacles, eta, rho0)
% 使用向量化计算
diff = currentPos - obstacles;
dist = sqrt(sum(diff.^2, 2));
valid = dist <= rho0;
F_rep = sum(eta .* (1./dist(valid) - 1/rho0) .* ...
(1./dist(valid).^3) .* diff(valid,:), 1);
U_rep = 0.5 * eta * sum((1./dist(valid) - 1/rho0).^2);
end
4. 实际应用中的问题与解决方案
4.1 典型问题诊断表
| 问题现象 | 可能原因 | 解决方案 |
|---|---|---|
| 车辆在障碍物前振荡 | 斥力增益过大 | 逐步减小η,增加阻尼项 |
| 无法通过狭窄通道 | 合力为零 | 引入切向力或动态调整ρ0 |
| 路径不够平滑 | 步长过大或力场突变 | 减小dt,加入平滑滤波 |
| 陷入局部极小值 | 力平衡 | 加入随机扰动或虚拟目标 |
| 计算速度慢 | 障碍物太多 | 使用空间分区或近似计算 |
4.2 动态障碍物处理实战
对于动态环境,我们需要扩展基本APF:
- 速度势场:考虑障碍物相对速度
matlab复制function F_vel = velocityObstacleForce(currentPos, currentVel, obsPos, obsVel, k_vel)
relativeVel = currentVel - obsVel;
relativePos = currentPos - obsPos;
if dot(relativeVel, relativePos) < 0
F_vel = k_vel * relativeVel / norm(relativePos)^2;
else
F_vel = [0, 0];
end
end
- 预测障碍物轨迹:
matlab复制% 简单线性预测
predictedPos = obsPos + obsVel * predictionTime;
- 自适应参数调整:
matlab复制% 根据距离动态调整η
dynamicEta = baseEta * (1 + exp(-alpha * distToObstacle));
4.3 复杂场景下的调参经验
经过多个项目实践,我总结出以下调参经验:
-
城市环境(密集静态障碍):
- ξ=1.2, η=8-12, ρ0=3-5m
- 需要较高的斥力防止刮蹭
-
高速公路(动态障碍为主):
- ξ=0.8, η=5-8, ρ0=10-15m
- 更注重流畅性和速度匹配
-
停车场(狭窄空间):
- ξ=1.5, η=6-10, ρ0=2-3m
- 可能需要结合转向约束
注意:这些参数需要根据具体车辆动力学特性调整。建议先用MATLAB仿真验证,再移植到实车。
5. 进阶应用与扩展思路
5.1 结合车辆动力学约束
基础APF不考虑车辆运动学,可能导致规划路径不可行。改进方法:
- 引入转向约束:
matlab复制maxSteeringAngle = pi/6; % 最大转向角
feasibleDirections = atan2(F_total(2), F_total(1)) + linspace(-maxSteeringAngle, maxSteeringAngle, 5);
% 选择最优可行方向
- 速度规划:
matlab复制% 根据曲率限制速度
curvature = computeCurvature(path);
maxSpeed = min(5, sqrt(0.3 * 9.8 ./ abs(curvature))); % 0.3为摩擦系数
5.2 多车协同规划
在多车系统中,可以将其他车辆视为动态障碍物,并增加协同势场:
- 队形保持势场:
matlab复制function F_formation = formationForce(currentPos, teammates, desiredPositions, k_form)
F_formation = [0, 0];
for i = 1:length(teammates)
desiredRelative = desiredPositions(i,:);
actualRelative = currentPos - teammates(i).position;
F_formation = F_formation + k_form * (actualRelative - desiredRelative);
end
end
- 通信延迟补偿:
matlab复制% 使用卡尔曼滤波预测队友位置
predictedTeammatePos = predictPosition(delayedTeammateInfo);
5.3 与全局规划器的融合
APF作为局部规划器,可以与全局规划器(如A*、RRT)结合:
-
分层规划架构:
- 全局规划器生成粗略路径
- APF负责局部避障和路径优化
-
势场引导采样:
matlab复制% 在RRT采样时偏向势场梯度方向
if rand < 0.7 % 70%概率偏向势场方向
sample = nearestNode + 0.5 * F_total / norm(F_total);
end
- 代价函数融合:
matlab复制totalCost = alpha * pathLength + beta * potentialEnergy;
在MATLAB中实现这种融合时,可以先用Robotics System Toolbox生成全局路径,再应用APF进行局部调整。