机器人路径规划算法之VFH算法详解＋MATLAB代码实现

目录一、 运作原理三步把地图变成方向1. 建图构建直方图网格Histogram Grid2. 降维生成极坐标直方图Polar Histogram3. 决策代价函数与山谷选择二、 算法演进VFH 家族图谱三、 优缺点与应用场景✅ 优势❌ 局限性 典型应用四、 MATLAB实现VFHVector Field Histogram矢量场直方图是移动机器人和自动驾驶中非常经典的一种局部路径规划算法。它的核心思路非常直观不做复杂的全局路径搜索而是根据机器人“眼前”的传感器数据快速计算出一个最安全、最接近目标的前进方向。你可以把它理解为机器人的“直觉式避障”它不关心整个地图长什么样只关心现在往哪走不会撞上。一、 运作原理三步把地图变成方向VFH 算法的本质是将二维的避障问题“降维”到一维的角度选择问题。其工作流程可以拆解为三个关键步骤1. 建图构建直方图网格Histogram Grid数据输入算法持续接收激光雷达、超声波等测距传感器的数据。概率统计它将机器人周围环境划分为细密的栅格每个栅格存储一个CV值Certainty Value。CV值越高代表该位置存在障碍物的置信度越高。这层处理能有效融合多传感器数据并滤除噪声。2. 降维生成极坐标直方图Polar Histogram这是 VFH 最核心的“降维”操作。算法以机器人为圆心将 360° 空间划分为若干个扇区Sector。计算密度对于每个角度扇区累加该方向所有栅格的障碍物影响通常距离越近的障碍物权重越高。形成峰谷最终生成一个环形直方图。“高峰”代表该方向有障碍物不可通行“低谷”代表该方向开阔是安全通道。3. 决策代价函数与山谷选择算法在直方图上寻找所有连续的“低谷”区域即可通行山谷然后通过一个代价函数从这些候选方向中选出最优解。代价函数通常权衡三个因素目标导向方向是否指向目标点吸引力。平滑性方向是否与当前航向接近避免急转弯舒适性。安全性是否位于山谷中央远离两侧障碍排斥力。二、 算法演进VFH 家族图谱原始的 VFH 算法虽然实时性强但存在机器人体积被忽略、容易陷入死胡同等问题。因此诞生了以下几个重要的改进版本版本核心改进点适用场景VFH​ (1991)基础版将环境映射为极坐标直方图进行决策。早期室内机器人对精度要求不高的场景。VFH​ (1998)引入机器人体积膨胀。在计算障碍物密度时将障碍物轮廓向外扩展车宽安全距离解决了原始算法容易剐蹭墙角的问题。主流的工业 AGV、服务机器人。VFH* (2000)引入 A搜索思想*。增加了“前瞻”机制不仅看当前一步还模拟向前走几步看看会不会进死胡同从而做出更优的全局选择。复杂迷宫环境需要避免局部最优陷阱的场景。三、 优缺点与应用场景✅ 优势极速响应计算量小适合传感器数据高频更新的实时控制。鲁棒性强基于统计直方图对传感器噪声如超声波的误读不敏感。天然安全通过障碍物膨胀VFH或密度阈值内置了安全距离机制。❌ 局限性短视缺乏全局视野容易在复杂地形如 U 型陷阱中振荡或陷入局部最优。参数敏感阈值设置很关键。阈值设高了容易撞设低了可能连大门都过不去。动力学忽略早期版本未考虑机器人的最小转弯半径等运动约束。 典型应用室内服务机器人扫地机器人、送餐机器人。自动导引车 (AGV)仓库物流小车。低速无人车园区接驳车、无人配送车的局部避障层。四、MATLAB实现%% VFH系列避障算法完整实现 % 包含VFH, VFH, VFH* clear; close all; clc; %% 1. 参数设置 % 算法通用参数 params.robot_radius 0.3; % 机器人半径(m) params.safety_dist 0.2; % 安全距离(m) params.ws_radius 1.5; % 工作空间半径(m) params.cell_size 0.1; % 栅格大小(m) params.max_speed 0.5; % 最大速度(m/s) % VFH参数 params.alpha 5; % 阈值常数 params.beta 1; % 平滑系数 params.window_size 180; % 滑动窗口大小(度) params.sectors 180; % 角度扇区数 params.hist_threshold 0.5; % 直方图阈值 % VFH参数 params.robot_width 0.5; % 机器人宽度(m) params.min_turning_radius 0.3; % 最小转弯半径 % VFH*参数 params.look_ahead_dist 2.0; % 前瞻距离(m) params.goal_weight 0.7; % 目标权重 params.obs_weight 0.3; % 障碍物权重 %% 2. 初始化环境 % 创建地图 map_size 20; % 地图大小(m) map_resolution 0.1; % 地图分辨率(m/grid) global_map zeros(map_size/map_resolution); % 添加障碍物 % 矩形障碍物 rect_obs [5, 3, 2, 1; 8, 7, 1, 3; 12, 5, 3, 2]; for i 1:size(rect_obs, 1) x rect_obs(i,1); y rect_obs(i,2); w rect_obs(i,3); h rect_obs(i,4); [X,Y] meshgrid(x:0.1:xw, y:0.1:yh); indices sub2ind(size(global_map), round(Y/map_resolution), round(X/map_resolution)); global_map(indices) 1; end % 圆形障碍物 circle_obs [10, 10, 1; 15, 3, 0.8]; theta 0:0.1:2*pi; for i 1:size(circle_obs, 1) cx circle_obs(i,1); cy circle_obs(i,2); r circle_obs(i,3); X cx r*cos(theta); Y cy r*sin(theta); for j 1:length(theta) for k 0:0.1:r xi cx k*cos(theta(j)); yi cy k*sin(theta(j)); idx_x round(xi/map_resolution); idx_y round(yi/map_resolution); if idx_x 0 idx_x size(global_map,2) idx_y 0 idx_y size(global_map,1) global_map(idx_y, idx_x) 1; end end end end %% 3. 机器人初始状态和目标位置 robot_pose [1, 1, 0]; % [x, y, theta] goal_pose [18, 18]; % 目标位置 path robot_pose(1:2); % 路径记录 %% 4. 主循环 max_iterations 500; dt 0.1; % 时间步长 figure(Position, [100, 100, 1200, 400]); for iter 1:max_iterations % 检查是否到达目标 dist_to_goal norm(robot_pose(1:2) - goal_pose); if dist_to_goal 0.5 fprintf(到达目标迭代次数%d\n, iter); break; end % 获取局部地图 local_map getLocalMap(global_map, robot_pose, params.ws_radius, map_resolution); % 计算极坐标直方图 [polar_hist, angle_bins] computePolarHistogram(local_map, robot_pose, params, map_resolution); % 计算障碍物密度 obstacle_density computeObstacleDensity(polar_hist, params); % 计算阈值 threshold computeThreshold(obstacle_density, params); % 二值化直方图 binary_hist polar_hist threshold; % 寻找可行方向 [selected_angle_vfh, candidate_angles] findCandidateAngles(binary_hist, angle_bins, robot_pose, goal_pose, params); % 选择最终方向 % VFH vfh_angle selectDirectionVFH(selected_angle_vfh, robot_pose, goal_pose, params); % VFH vfh_plus_angle selectDirectionVFHPlus(selected_angle_vfh, robot_pose, goal_pose, params, local_map, map_resolution); % VFH* vfh_star_angle selectDirectionVFHStar(selected_angle_vfh, robot_pose, goal_pose, params, local_map, map_resolution, global_map); % 控制机器人移动 (使用VFH*结果) [robot_pose, cmd_vel] moveRobot(robot_pose, vfh_star_angle, params, dt); % 记录路径 path [path; robot_pose(1:2)]; % 可视化 if mod(iter, 5) 0 visualizeNavigation(robot_pose, goal_pose, global_map, path, polar_hist, angle_bins, ... vfh_angle, vfh_plus_angle, vfh_star_angle, params, map_resolution, iter); end % 暂停以便观察 pause(0.01); end %% 5. 绘制最终结果 plotFinalResults(robot_pose, goal_pose, global_map, path, map_resolution); fprintf(导航完成\n); %% 辅助函数 function local_map getLocalMap(global_map, robot_pose, radius, resolution) % 获取局部地图 robot_grid round(robot_pose(1:2)/resolution); radius_grid round(radius/resolution); [rows, cols] size(global_map); x_min max(1, robot_grid(1) - radius_grid); x_max min(cols, robot_grid(1) radius_grid); y_min max(1, robot_grid(2) - radius_grid); y_max min(rows, robot_grid(2) radius_grid); local_map global_map(y_min:y_max, x_min:x_max); end function [polar_hist, angle_bins] computePolarHistogram(local_map, robot_pose, params, resolution) % 计算极坐标直方图 [rows, cols] size(local_map); center_row round(rows/2); center_col round(cols/2); polar_hist zeros(params.sectors, 1); angle_bins linspace(-pi, pi, params.sectors 1); angle_bins angle_bins(1:end-1); for i 1:rows for j 1:cols if local_map(i, j) 0 % 计算相对位置 dx (j - center_col) * resolution; dy (i - center_row) * resolution; % 计算距离和角度 dist sqrt(dx^2 dy^2); angle atan2(dy, dx) - robot_pose(3); % 归一化角度 angle wrapToPi(angle); % 计算障碍物影响 if dist params.ws_radius % 计算大小等级 magnitude params.alpha * ((params.ws_radius - dist) / params.ws_radius)^2; % 找到对应的扇区 sector_idx floor((angle pi) / (2*pi) * params.sectors) 1; if sector_idx params.sectors sector_idx params.sectors; end % 更新直方图 polar_hist(sector_idx) polar_hist(sector_idx) magnitude; end end end end % 平滑直方图 polar_hist smoothHistogram(polar_hist, params.beta); end function smoothed_hist smoothHistogram(hist, beta) % 平滑直方图 n length(hist); smoothed_hist zeros(n, 1); for i 1:n sum_val 0; for k -beta:beta idx mod(i k - 1, n) 1; sum_val sum_val hist(idx); end smoothed_hist(i) sum_val / (2*beta 1); end end function density computeObstacleDensity(polar_hist, params) % 计算障碍物密度 density mean(polar_hist); end function threshold computeThreshold(density, params) % 计算阈值 threshold params.hist_threshold params.alpha * density; end function [selected_angle, candidate_angles] findCandidateAngles(binary_hist, angle_bins, robot_pose, goal_pose, params) % 寻找候选角度 candidate_angles []; % 计算目标方向 goal_vector goal_pose - robot_pose(1:2); target_angle atan2(goal_vector(2), goal_vector(1)) - robot_pose(3); target_angle wrapToPi(target_angle); % 寻找可行扇区 n length(binary_hist); for i 1:n if binary_hist(i) 0 candidate_angles [candidate_angles; angle_bins(i)]; end end if isempty(candidate_angles) selected_angle target_angle; else % 选择最接近目标的方向 angle_diffs abs(wrapToPi(candidate_angles - target_angle)); [~, min_idx] min(angle_diffs); selected_angle candidate_angles(min_idx); end end function direction selectDirectionVFH(selected_angle, robot_pose, goal_pose, params) % VFH方向选择 direction selected_angle; end function direction selectDirectionVFHPlus(selected_angle, robot_pose, goal_pose, params, local_map, resolution) % VFH方向选择 % 考虑机器人宽度 robot_half_width params.robot_width / 2; % 计算目标方向 goal_vector goal_pose - robot_pose(1:2); target_angle atan2(goal_vector(2), goal_vector(1)) - robot_pose(3); target_angle wrapToPi(target_angle); if isempty(selected_angle) direction target_angle; return; end % 检查机器人宽度是否适合 [rows, cols] size(local_map); center_row round(rows/2); center_col round(cols/2); % 沿着候选方向检查机器人轮廓 angle_diff abs(wrapToPi(selected_angle - target_angle)); % 增加转向惩罚 turning_penalty angle_diff * 0.1; % 检查碰撞 collision_risk checkCollisionRisk(local_map, selected_angle, robot_half_width, params.ws_radius, resolution); if collision_risk 0.3 direction selected_angle; else % 寻找次优方向 direction target_angle; end end function direction selectDirectionVFHStar(selected_angle, robot_pose, goal_pose, params, local_map, resolution, global_map) % VFH*方向选择 % 使用成本函数评估多个候选方向 % 生成候选方向 candidate_angles linspace(-pi/2, pi/2, 9) selected_angle; candidate_angles wrapToPi(candidate_angles); best_cost inf; best_angle selected_angle; for i 1:length(candidate_angles) angle candidate_angles(i); % 计算目标成本 goal_vector goal_pose - robot_pose(1:2); target_angle atan2(goal_vector(2), goal_vector(1)) - robot_pose(3); goal_cost abs(wrapToPi(angle - target_angle)); % 计算障碍物成本 obs_cost computeObstacleCost(local_map, angle, params, resolution); % 计算前瞻成本 lookahead_cost computeLookaheadCost(robot_pose, angle, params, global_map, resolution); % 总成本 total_cost params.goal_weight * goal_cost ... params.obs_weight * obs_cost ... 0.2 * lookahead_cost; if total_cost best_cost best_cost total_cost; best_angle angle; end end direction best_angle; end function collision_risk checkCollisionRisk(local_map, angle, robot_half_width, ws_radius, resolution) % 检查碰撞风险 collision_risk 0; [rows, cols] size(local_map); center_row round(rows/2); center_col round(cols/2); % 沿着方向检查多个点 check_dist 0:0.1:ws_radius; for d check_dist x d * cos(angle); y d * sin(angle); % 检查机器人宽度范围内的点 for w -robot_half_width:0.1:robot_half_width px x w * sin(angle); py y w * cos(angle); grid_x round(center_col px/resolution); grid_y round(center_row py/resolution); if grid_x 1 grid_x cols grid_y 1 grid_y rows if local_map(grid_y, grid_x) 0 collision_risk collision_risk 1/length(check_dist); end end end end end function obs_cost computeObstacleCost(local_map, angle, params, resolution) % 计算障碍物成本 [rows, cols] size(local_map); center_row round(rows/2); center_col round(cols/2); obs_cost 0; max_dist params.ws_radius; % 沿着方向积分障碍物影响 for d 0:0.1:max_dist x d * cos(angle); y d * sin(angle); grid_x round(center_col x/resolution); grid_y round(center_row y/resolution); if grid_x 1 grid_x cols grid_y 1 grid_y rows if local_map(grid_y, grid_x) 0 obs_cost obs_cost (max_dist - d) / max_dist; end end end end function lookahead_cost computeLookaheadCost(robot_pose, angle, params, global_map, resolution) % 计算前瞻成本 lookahead_pose robot_pose; lookahead_pose(1) robot_pose(1) params.look_ahead_dist * cos(robot_pose(3) angle); lookahead_pose(2) robot_pose(2) params.look_ahead_dist * sin(robot_pose(3) angle); % 获取前瞻位置的地图 lookahead_map getLocalMap(global_map, lookahead_pose, params.ws_radius/2, resolution); % 计算障碍物密度 [rows, cols] size(lookahead_map); obstacle_count sum(lookahead_map(:) 0); lookahead_cost obstacle_count / (rows * cols); end function [new_pose, cmd_vel] moveRobot(robot_pose, target_angle, params, dt) % 移动机器人 max_angular_speed 1.0; % 最大角速度(rad/s) % 计算角速度 angle_error wrapToPi(target_angle); angular_vel sign(angle_error) * min(abs(angle_error)/dt, max_angular_speed); % 线速度 linear_vel params.max_speed * (1 - abs(angle_error)/pi); % 更新位姿 new_theta robot_pose(3) angular_vel * dt; new_x robot_pose(1) linear_vel * cos(new_theta) * dt; new_y robot_pose(2) linear_vel * sin(new_theta) * dt; new_pose [new_x, new_y, wrapToPi(new_theta)]; cmd_vel [linear_vel, angular_vel]; end function visualizeNavigation(robot_pose, goal_pose, global_map, path, polar_hist, angle_bins, ... vfh_angle, vfh_plus_angle, vfh_star_angle, params, resolution, iter) % 可视化导航过程 clf; % 子图1: 全局地图和路径 subplot(1, 3, 1); imagesc(global_map); colormap(gray); hold on; % 绘制路径 path_grid path / resolution; plot(path_grid(:,1), path_grid(:,2), b-, LineWidth, 1.5); % 绘制机器人 robot_grid robot_pose(1:2) / resolution; plot(robot_grid(1), robot_grid(2), ro, MarkerSize, 10, LineWidth, 2); % 绘制目标 goal_grid goal_pose / resolution; plot(goal_grid(1), goal_grid(2), g*, MarkerSize, 15, LineWidth, 2); % 绘制工作空间 ws_radius_grid params.ws_radius / resolution; rectangle(Position, [robot_grid(1)-ws_radius_grid, robot_grid(2)-ws_radius_grid, ... 2*ws_radius_grid, 2*ws_radius_grid], ... Curvature, [1,1], EdgeColor, r, LineStyle, --); title(sprintf(全局地图 - 迭代: %d, iter)); xlabel(X (grid)); ylabel(Y (grid)); axis equal; axis tight; % 子图2: 极坐标直方图 subplot(1, 3, 2); polarplot(angle_bins, polar_hist, b-, LineWidth, 2); hold on; % 标记选择的方向 polarplot([vfh_angle, vfh_angle], [0, max(polar_hist)], r-, LineWidth, 2); polarplot([vfh_plus_angle, vfh_plus_angle], [0, max(polar_hist)], g-, LineWidth, 2); polarplot([vfh_star_angle, vfh_star_angle], [0, max(polar_hist)], m-, LineWidth, 2); legend(直方图, VFH, VFH, VFH*); title(极坐标直方图); % 子图3: 算法比较 subplot(1, 3, 3); hold on; % 绘制方向比较 angles [vfh_angle, vfh_plus_angle, vfh_star_angle]; algorithms {VFH, VFH, VFH*}; colors {r, g, m}; for i 1:3 quiver(0, 0, cos(angles(i)), sin(angles(i)), 0.5, colors{i}, LineWidth, 2, MaxHeadSize, 0.5); end % 绘制单位圆 theta linspace(0, 2*pi, 100); plot(cos(theta), sin(theta), k--); xlim([-1.5, 1.5]); ylim([-1.5, 1.5]); xlabel(X); ylabel(Y); title(算法方向比较); legend(algorithms); grid on; axis equal; drawnow; end function plotFinalResults(robot_pose, goal_pose, global_map, path, resolution) % 绘制最终结果 figure(Position, [200, 200, 800, 600]); % 绘制地图和路径 imagesc(global_map); colormap(gray); hold on; % 绘制路径 path_grid path / resolution; plot(path_grid(:,1), path_grid(:,2), b-, LineWidth, 2); % 绘制起点和终点 start_grid path(1,:) / resolution; goal_grid goal_pose / resolution; plot(start_grid(1), start_grid(2), go, MarkerSize, 12, LineWidth, 3); plot(goal_grid(1), goal_grid(2), r*, MarkerSize, 15, LineWidth, 3); % 绘制机器人最终位置 robot_grid robot_pose(1:2) / resolution; plot(robot_grid(1), robot_grid(2), mo, MarkerSize, 10, LineWidth, 2); % 添加标注 text(start_grid(1), start_grid(2)-5, 起点, Color, g, FontSize, 12, FontWeight, bold); text(goal_grid(1), goal_grid(2)5, 目标, Color, r, FontSize, 12, FontWeight, bold); text(robot_grid(1), robot_grid(2)-5, 终点, Color, m, FontSize, 12, FontWeight, bold); title(VFH系列算法导航结果); xlabel(X (grid)); ylabel(Y (grid)); axis equal; axis tight; % 添加图例 legend(路径, 起点, 目标, 终点, Location, best); % 显示统计信息 fprintf(\n 导航统计 \n); fprintf(路径长度: %.2f m\n, sum(sqrt(sum(diff(path).^2, 2)))); fprintf(最终位置误差: %.3f m\n, norm(robot_pose(1:2) - goal_pose)); fprintf(迭代次数: %d\n, size(path, 1)); end function angle wrapToPi(angle) % 将角度包裹到[-π, π] angle mod(angle pi, 2*pi) - pi; end程序功能说明1.包含的算法VFH基本向量场直方图算法VFH增强版本考虑机器人宽度和动力学约束VFH*最优版本使用成本函数和前瞻规划2.主要功能模块环境初始化包含多种障碍物局部地图获取极坐标直方图计算障碍物密度分析可行方向搜索三种算法的方向选择策略机器人运动控制实时可视化3.参数说明robot_radius机器人半径ws_radius工作空间半径sectors角度扇区数look_ahead_distVFH*的前瞻距离goal_weight/obs_weight目标与障碍物的权重4.使用说明直接运行程序即可看到避障过程可视化窗口显示左图全局地图和机器人路径中图极坐标直方图右图三种算法的方向比较程序会自动完成从起点到终点的导航5.算法特点VFH快速响应计算简单VFH考虑机器人物理约束VFH*全局最优路径更平滑结果命令行窗口 导航统计 路径长度: 23.77 m 最终位置误差: 1.175 m 迭代次数: 501 导航完成可视化