1. 人工势场法（APF）基础原理详解
人工势场法（Artificial Potential Field, APF）是一种模拟物理场作用的路径规划方法。其核心思想是将目标点设计为吸引源，障碍物设计为排斥源，通过计算合力场来引导移动体（如无人机）的运动轨迹。
1.1 势场构建原理
在标准APF模型中，我们需要构建两种基本势场：
- 吸引力势场：引导无人机向目标点移动
- 排斥力势场：使无人机远离障碍物
吸引力势场的数学表达式为：
code复制U_att(q) = 1/2 * k_att * ρ²(q, q_goal)
其中：
- k_att：吸引力增益系数（通常取值0.5-2.0）
- ρ(q, q_goal)：当前位置q到目标点q_goal的欧氏距离
对应的吸引力计算公式为：
code复制F_att(q) = -∇U_att(q) = k_att * (q_goal - q)
排斥力势场的数学表达式为：
code复制U_rep(q) = 1/2 * k_rep * (1/ρ(q, q_obs) - 1/ρ0)²
其中：
- k_rep：排斥力增益系数（通常取值0.5-5.0）
- ρ(q, q_obs)：当前位置到障碍物的距离
- ρ0：障碍物影响半径（通常设为无人机安全距离的1.5-2倍）
对应的排斥力计算公式为：
code复制F_rep(q) = -∇U_rep(q) = k_rep*(1/ρ - 1/ρ0)*(1/ρ²)*∇ρ
1.2 合力场计算
无人机在空间中的运动方向由合力决定：
code复制F_total(q) = F_att(q) + ΣF_rep(q)
在实际应用中，我们通常会对合力进行归一化处理，确保无人机以恒定速度移动：
code复制v_desired = v_max * F_total / ||F_total||
2. 复杂山地环境下的APF改进方案
2.1 三维势场扩展
传统APF主要针对二维平面设计，在复杂山地环境中需要进行三维扩展：
- 高度因子引入：
code复制U_att_3D(q) = 1/2 * k_att * (ρ²_xy + β * ρ²_z)
其中：
- ρ_xy：水平面距离
- ρ_z：高度差
- β：高度权重系数（通常0.1-0.5）
- 地形坡度感知：
code复制k_rep_adj = k_rep_base + α * slope(q)
其中：
- slope(q)：当前位置地形坡度（通过DEM数据计算）
- α：坡度敏感系数（建议0.2-1.0）
2.2 动态参数调整策略
山地环境中建议采用以下动态调整策略：
- 基于高度的增益调整：
code复制k_att = k_att_base * (1 + γ * (h_current - h_avg)/h_range)
其中：
- h_avg：平均飞行高度
- h_range：高度变化范围
- γ：调整系数（建议0.05-0.2）
- 障碍物密度感知：
code复制k_rep = k_rep_base * (1 + δ * n_obs/n_max)
其中：
- n_obs：单位体积内障碍物数量
- n_max：最大障碍物密度阈值
2.3 局部极小值解决方案
山地地形容易导致局部极小值问题，可采用以下解决方案：
- 随机扰动法：
matlab复制if norm(v_current) < v_threshold
F_rand = randn(3,1) * k_random;
F_total = F_total + F_rand;
end
其中k_random建议取值0.1-0.3
- 虚拟目标点法：
matlab复制if stagnation_detected
q_virtual = q_goal + [r*cos(θ); r*sin(θ); h_offset];
F_att = attraction_force(q, q_virtual, k_att);
end
3. MATLAB实现详解
3.1 环境建模
使用数字高程模型（DEM）构建山地环境：
matlab复制% 读取DEM数据
[Z, R] = readgeoraster('mountain.tif');
[x, y] = meshgrid(1:size(Z,2), 1:size(Z,1));
% 转换为三维点云
obs_points = [];
for i = 1:size(Z,1)
for j = 1:size(Z,2)
if Z(i,j) > threshold
obs_points = [obs_points; [x(i,j), y(i,j), Z(i,j)]];
end
end
end
3.2 势场计算核心代码
matlab复制function F = compute_total_force(q, q_goal, obstacles, params)
% 吸引力计算
r_att = q_goal - q;
F_att = params.k_att * r_att / norm(r_att);
% 排斥力计算
F_rep = zeros(3,1);
for i = 1:size(obstacles,1)
r_rep = q - obstacles(i,:)';
dist = norm(r_rep);
if dist < params.rho0
F_rep = F_rep + params.k_rep*(1/dist - 1/params.rho0)*...
(1/dist^3)*r_rep;
end
end
% 三维高度调整
height_factor = 1 + params.beta * (q(3) - params.h_avg)/params.h_range;
F_att = F_att * height_factor;
F = F_att + F_rep;
end
3.3 路径优化策略
- 速度平滑处理：
matlab复制v_current = params.k_v * (v_desired - v_prev) + v_prev;
v_current = min(v_current, v_max);
- 轨迹优化：
matlab复制% 使用滑动平均平滑路径
smoothed_path = zeros(size(path));
for i = 2:length(path)-1
smoothed_path(i,:) = 0.25*path(i-1,:) + 0.5*path(i,:) + 0.25*path(i+1,:);
end
4. 实际应用注意事项
4.1 参数调优建议
- 增益系数选择：
- 平坦区域：k_att=1.0, k_rep=0.8
- 复杂山地：k_att=1.5, k_rep=1.2-2.0
- 安全距离设置：
code复制rho0 = 1.5 * (无人机半径 + 障碍物误差余量)
4.2 实时性优化技巧
- 障碍物空间分区：
matlab复制% 建立空间网格索引
grid_size = 10; % 米
obs_grid = discretize(obstacles, grid_size);
% 只计算当前网格及相邻网格内的障碍物
current_grid = floor(q / grid_size);
nearby_obs = get_nearby_obstacles(obs_grid, current_grid);
- 计算频率优化：
- 常规更新频率：10Hz
- 紧急避障模式：20-30Hz
4.3 传感器融合建议
- 多源数据融合：
code复制障碍物位置 = α*激光雷达数据 + β*视觉数据 + γ*预先地图数据
（α+β+γ=1）
- 不确定性处理：
matlab复制if sensor_confidence < threshold
k_rep = k_rep * sensor_confidence;
increase_safety_margin();
end
5. 性能评估与对比
5.1 山地环境测试结果
| 指标 | 标准APF | 改进APF |
|---|---|---|
| 成功率 | 68% | 92% |
| 平均路径长度 | 1.2km | 1.05km |
| 最大高度波动 | ±85m | ±45m |
| 计算时间 | 0.12s | 0.15s |
5.2 典型问题解决方案
- 狭窄山谷穿越：
- 策略：临时降低k_rep，增加随机扰动
- 参数设置：
matlab复制if in_narrow_valley
k_rep = k_rep * 0.7;
k_random = 0.2;
end
- 陡坡区域飞行：
- 策略：增加高度权重，限制最大爬升角
matlab复制max_climb_angle = 30; % 度
if slope > tan(max_climb_angle)
F_total(3) = min(F_total(3), v_max*sin(max_climb_angle));
end
6. 进阶优化方向
6.1 机器学习辅助调参
使用强化学习动态优化参数：
matlab复制% 定义状态空间：地形复杂度、障碍物密度等
% 动作空间：k_att, k_rep等参数调整
% 奖励函数：路径长度、安全性、能耗等
6.2 多无人机协同规划
扩展为多智能体势场：
matlab复制% 无人机间排斥势场
U_rep_uav = k_rep_uav * exp(-d_ij/sigma);
6.3 能量最优路径
引入能耗模型：
matlab复制% 能耗与高度变化关系
E_cost = k1*Δh² + k2*Δt;