基于模糊控制的避障导航算法

基于模糊控制的避障导航算法的MATLAB图形化仿真。该方案使用模糊逻辑系统处理传感器数据，控制机器人避开障碍物并导航至目标点。

完整MATLAB代码

%% 基于模糊控制的避障导航仿真
clear; clc; close all;% ========== 模糊逻辑系统设计 ==========
fis = newfis('ObstacleAvoidance');% 添加输入变量（三个方向的障碍物距离）
fis = addvar(fis, 'input', 'Left_Dist', [0 5]);
fis = addvar(fis, 'input', 'Center_Dist', [0 5]);
fis = addvar(fis, 'input', 'Right_Dist', [0 5]);% 添加输出变量（转向角度）
fis = addvar(fis, 'output', 'Steering_Angle', [-90 90]);% 定义输入隶属函数
dist_mf = {'Near', 'trapmf', [0 0 1 2]; ...'Medium', 'trapmf', [1 2 3 4]; ...'Far', 'trapmf', [3 4 5 5]};for i = 1:3for j = 1:size(dist_mf, 1)fis = addmf(fis, 'input', i, dist_mf{j,1}, dist_mf{j,2}, dist_mf{j,3});end
end% 定义输出隶属函数
steer_mf = {'HardLeft', 'gaussmf', [15 -60]; ...'Left', 'gaussmf', [15 -30]; ...'Straight', 'gaussmf', [15 0]; ...'Right', 'gaussmf', [15 30]; ...'HardRight', 'gaussmf', [15 60]};for i = 1:size(steer_mf, 1)fis = addmf(fis, 'output', 1, steer_mf{i,1}, steer_mf{i,2}, steer_mf{i,3});
end% 定义模糊规则
ruleList = [1 1 1 5 1 1;   % 所有方向近 -> 急右转1 1 2 5 1 1;   % 左前中 -> 急右转1 1 3 5 1 1;   % 左远 -> 急右转1 2 1 4 1 1;   % 前中右近 -> 右转1 2 2 4 1 1;   1 2 3 4 1 1;   1 3 1 4 1 1;   1 3 2 4 1 1;   1 3 3 3 1 1;   % 左远前中右远 -> 直行2 1 1 2 1 1;   % 左中右近 -> 左转2 1 2 2 1 1;2 1 3 2 1 1;2 2 1 3 1 1;   % 前中 -> 直行2 2 2 3 1 1;2 2 3 3 1 1;2 3 1 3 1 1;2 3 2 3 1 1;2 3 3 3 1 1;   % 所有方向远 -> 直行3 1 1 1 1 1;   % 右近 -> 急左转3 1 2 1 1 1;3 1 3 1 1 1;3 2 1 1 1 1;3 2 2 1 1 1;3 2 3 1 1 1;3 3 1 2 1 1;   % 右中 -> 左转3 3 2 2 1 1;3 3 3 3 1 1;   % 无障碍 -> 直行];fis = addrule(fis, ruleList);% ========== 仿真环境设置 ==========
% 创建图形窗口
figure('Name', '模糊控制避障导航仿真', 'NumberTitle', 'off');
axis([0 100 0 100]);
grid on; hold on;
title('基于模糊控制的机器人避障导航');
xlabel('X坐标 (m)'); ylabel('Y坐标 (m)');% 设置目标点
goal = [95, 95];
plot(goal(1), goal(2), 'rp', 'MarkerSize', 15, 'LineWidth', 2);
text(goal(1)+2, goal(2), '目标点');% 随机生成障碍物
num_obstacles = 15;
obstacles = zeros(num_obstacles, 2);
for i = 1:num_obstaclesvalid = false;while ~validpos = [randi([10,90]), randi([10,90])];% 确保不靠近起点和目标点if norm(pos - [5,5]) > 15 && norm(pos - goal) > 15obstacles(i,:) = pos;plot(pos(1), pos(2), 'ro', 'MarkerSize', 10, 'LineWidth', 1.5);valid = true;endend
end% ========== 机器人初始化 ==========
robot.pos = [5, 5];      % 初始位置
robot.angle = 45;        % 初始角度(度)
robot.speed = 1.5;       % 移动速度
robot.size = 3;          % 机器人尺寸
robot.sensor_range = 15; % 传感器范围
robot.sensor_angle = 30; % 传感器角度偏移% 绘制机器人
robot_plot = plot(robot.pos(1), robot.pos(2), 'bo', 'MarkerSize', robot.size*2, 'LineWidth', 2);
direction_plot = quiver(robot.pos(1), robot.pos(2), ...cosd(robot.angle), sind(robot.angle), ...'b', 'LineWidth', 2, 'MaxHeadSize', 2);
sensor_plots = gobjects(1,3);% 轨迹记录
trajectory = robot.pos;
traj_plot = plot(trajectory(:,1), trajectory(:,2), 'b-');% ========== 主循环 ==========
max_steps = 500;
step = 0;
goal_threshold = 3;while step < max_steps% 检测传感器距离sensor_dists = get_sensor_data(robot, obstacles);% 模糊推理steering_angle = evalfis(fis, sensor_dists);% 更新机器人方向robot.angle = robot.angle + steering_angle;% 更新位置new_x = robot.pos(1) + robot.speed * cosd(robot.angle);new_y = robot.pos(2) + robot.speed * sind(robot.angle);robot.pos = [new_x, new_y];% 保存轨迹trajectory = [trajectory; robot.pos];% 检查是否到达目标if norm(robot.pos - goal) < goal_thresholddisp('成功到达目标点!');break;end% 更新图形set(robot_plot, 'XData', robot.pos(1), 'YData', robot.pos(2));set(direction_plot, 'XData', robot.pos(1), 'YData', robot.pos(2), ...'UData', robot.size*cosd(robot.angle), 'VData', robot.size*sind(robot.angle));set(traj_plot, 'XData', trajectory(:,1), 'YData', trajectory(:,2));% 更新传感器显示delete(sensor_plots);sensor_plots = plot_sensors(robot, sensor_dists);% 检查碰撞if check_collision(robot, obstacles)disp('发生碰撞!');plot(robot.pos(1), robot.pos(2), 'rx', 'MarkerSize', 15, 'LineWidth', 2);break;enddrawnow;step = step + 1;pause(0.05);
end% ========== 辅助函数 ==========
function dists = get_sensor_data(robot, obstacles)angles = [robot.angle - robot.sensor_angle, ...robot.angle, ...robot.angle + robot.sensor_angle];dists = zeros(1,3);for i = 1:3min_dist = robot.sensor_range;sensor_dir = [cosd(angles(i)), sind(angles(i))];for j = 1:size(obstacles,1)% 障碍物方向向量obs_dir = obstacles(j,:) - robot.pos;dist_to_obs = norm(obs_dir);% 计算夹角cos_theta = dot(sensor_dir, obs_dir) / (norm(sensor_dir) * dist_to_obs);if cos_theta > cosd(15) && dist_to_obs < min_distmin_dist = dist_to_obs;endenddists(i) = min(min_dist, robot.sensor_range);end
endfunction h = plot_sensors(robot, dists)angles = [robot.angle - robot.sensor_angle, ...robot.angle, ...robot.angle + robot.sensor_angle];h = gobjects(1,3);colors = ['r', 'g', 'b'];for i = 1:3end_x = robot.pos(1) + dists(i) * cosd(angles(i));end_y = robot.pos(2) + dists(i) * sind(angles(i));h(i) = plot([robot.pos(1), end_x], [robot.pos(2), end_y], ...colors(i), 'LineStyle', '--');end
endfunction collision = check_collision(robot, obstacles)collision = false;safety_dist = 2.5;for i = 1:size(obstacles,1)if norm(robot.pos - obstacles(i,:)) < safety_distcollision = true;return;endend
end

算法说明

1. 模糊系统设计：
   • 输入变量：三个方向的障碍物距离（左、中、右）
   • 输出变量：转向角度（-90°到90°）
   • 隶属函数：
     • 距离：Near(0-2m), Medium(1-4m), Far(3-5m)
     • 转向：HardLeft(-60°), Left(-30°), Straight(0°), Right(30°), HardRight(60°)
   • 27条模糊规则，基于"近处障碍物方向决定转向方向"原则
2. 机器人模型：
   • 位置、方向、速度和传感器配置
   • 三个虚拟传感器（左30°、正前、右30°）
   • 传感器范围：15米
3. 避障逻辑：
   • 优先避开最近障碍物
   • 当所有方向无障碍时直行
   • 根据障碍物位置分布选择最佳转向
4. 仿真特性：
   • 随机生成障碍物环境
   • 实时显示传感器数据和轨迹
   • 碰撞检测和成功到达目标检测

参考仿真代码 youwenfan.com/contentcnb/77886.html

仿真结果

运行代码后，你将看到：

1. 蓝色圆形机器人从(5,5)出发
2. 红色目标点位于(95,95)
3. 红色障碍物随机分布
4. 三条虚线表示传感器检测范围：
   • 红色：左侧传感器
   • 绿色：前方传感器
   • 蓝色：右侧传感器
5. 蓝色轨迹线显示机器人路径

机器人行为：

• 在障碍物附近转向避开
• 无障碍区域直行向目标
• 复杂环境自主导航
• 到达目标或碰撞时停止

调整建议

1. 优化模糊规则：

   • 修改ruleList矩阵调整避障行为
   • 增加目标方向引导规则

2. 参数调整：

   robot.speed = 2.0;  % 增加速度
   robot.sensor_range = 20; % 增大传感器范围
   

3. 添加新功能：

   • 动态障碍物
   • 路径优化算法
   • 速度模糊控制

此实现展示了模糊控制在不确定环境中的鲁棒性，适用于移动机器人、无人机等自主导航系统。