基于模糊控制的避障导航算法
基于模糊控制的避障导航算法的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:3
for 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_obstacles
valid = false;
while ~valid
pos = [randi([10,90]), randi([10,90])];
% 确保不靠近起点和目标点
if norm(pos - [5,5]) > 15 && norm(pos - goal) > 15
obstacles(i,:) = pos;
plot(pos(1), pos(2), 'ro', 'MarkerSize', 10, 'LineWidth', 1.5);
valid = true;
end
end
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_threshold
disp('成功到达目标点!');
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;
end
drawnow;
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:3
min_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_dist
min_dist = dist_to_obs;
end
end
dists(i) = min(min_dist, robot.sensor_range);
end
end
function 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:3
end_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
end
function collision = check_collision(robot, obstacles)
collision = false;
safety_dist = 2.5;
for i = 1:size(obstacles,1)
if norm(robot.pos - obstacles(i,:)) < safety_dist
collision = true;
return;
end
end
end
算法说明
- 模糊系统设计：
- 输入变量：三个方向的障碍物距离（左、中、右）
- 输出变量：转向角度（-90°到90°）
- 隶属函数：
- 距离：Near(0-2m), Medium(1-4m), Far(3-5m)
- 转向：HardLeft(-60°), Left(-30°), Straight(0°), Right(30°), HardRight(60°)
- 27条模糊规则，基于"近处障碍物方向决定转向方向"原则
- 机器人模型：
- 位置、方向、速度和传感器配置
- 三个虚拟传感器（左30°、正前、右30°）
- 传感器范围：15米
- 避障逻辑：
- 优先避开最近障碍物
- 当所有方向无障碍时直行
- 根据障碍物位置分布选择最佳转向
- 仿真特性：
- 随机生成障碍物环境
- 实时显示传感器数据和轨迹
- 碰撞检测和成功到达目标检测
参考仿真代码 youwenfan.com/contentcnb/77886.html
仿真结果
运行代码后，你将看到：
- 蓝色圆形机器人从(5,5)出发
- 红色目标点位于(95,95)
- 红色障碍物随机分布
- 三条虚线表示传感器检测范围：
- 红色：左侧传感器
- 绿色：前方传感器
- 蓝色：右侧传感器
- 蓝色轨迹线显示机器人路径
机器人行为：
- 在障碍物附近转向避开
- 无障碍区域直行向目标
- 复杂环境自主导航
- 到达目标或碰撞时停止
调整建议
-
优化模糊规则：
- 修改
ruleList矩阵调整避障行为 - 增加目标方向引导规则
- 修改
-
参数调整：
robot.speed = 2.0; % 增加速度 robot.sensor_range = 20; % 增大传感器范围 -
添加新功能：
- 动态障碍物
- 路径优化算法
- 速度模糊控制
此实现展示了模糊控制在不确定环境中的鲁棒性，适用于移动机器人、无人机等自主导航系统。
浙公网安备 33010602011771号