基于模糊控制的避障仿真实现MATLAB/Simulink方案
一、模糊控制避障核心原理
1.1 系统架构
传感器输入 → 模糊化 → 模糊推理 → 解模糊 → 控制输出 → 机器人运动
(距离信息) (隶属函数) (规则库) (清晰值) (速度/转向) (避障轨迹)
1.2 模糊控制器设计优势
| 特点 | 传统控制 | 模糊控制 | 避障优势 |
|---|---|---|---|
| 模型要求 | 精确数学模型 | 无需精确模型 | 适应环境不确定性 |
| 规则表达 | 数学方程 | "如果-那么"语言规则 | 更符合人类驾驶经验 |
| 鲁棒性 | 对参数敏感 | 强鲁棒性 | 适应传感器噪声 |
| 实时性 | 计算复杂 | 推理速度快 | 满足实时避障需求 |
二、完整MATLAB仿真代码
%% 基于模糊控制的机器人避障仿真系统
clear; close all; clc;
%% 1. 参数初始化
% 仿真环境参数
map_size = [0, 100, 0, 100]; % 地图边界 [xmin, xmax, ymin, ymax]
obstacles = [30, 50, 8; % 障碍物1 [x, y, radius]
60, 30, 10;
40, 70, 6;
70, 60, 7;
20, 20, 5];
goal = [90, 90]; % 目标点
start = [10, 10]; % 起始点
% 机器人参数
robot_pos = start; % 初始位置
robot_radius = 3; % 机器人半径
robot_theta = atan2(goal(2)-start(2), goal(1)-start(1)); % 初始朝向
max_speed = 2.0; % 最大速度 (m/s)
max_steer = pi/4; % 最大转向角 (rad)
safety_distance = 5; % 安全距离
% 传感器参数
num_sensors = 8; % 传感器数量
sensor_range = 20; % 传感器探测范围 (m)
sensor_angles = linspace(-pi, pi, num_sensors+1); % 传感器角度
sensor_angles = sensor_angles(1:end-1);
% 仿真参数
dt = 0.1; % 时间步长 (s)
sim_time = 100; % 总仿真时间 (s)
steps = ceil(sim_time/dt);
%% 2. 创建模糊逻辑控制器
fis = mamfis('Name', 'Obstacle_Avoidance');
% 2.1 输入变量定义
% 输入1: 前方障碍物距离 (0-20m)
fis = addInput(fis, [0, sensor_range], 'Name', 'front_distance');
fis = addMF(fis, 'front_distance', 'zmf', [3, 8], 'Name', 'Very_Near');
fis = addMF(fis, 'front_distance', 'trimf', [5, 10, 15], 'Name', 'Near');
fis = addMF(fis, 'front_distance', 'trimf', [12, 16, 20], 'Name', 'Far');
fis = addMF(fis, 'front_distance', 'smf', [18, 20], 'Name', 'Very_Far');
% 输入2: 左侧障碍物距离 (0-20m)
fis = addInput(fis, [0, sensor_range], 'Name', 'left_distance');
fis = addMF(fis, 'left_distance', 'zmf', [5, 10], 'Name', 'Near');
fis = addMF(fis, 'left_distance', 'trimf', [8, 12, 16], 'Name', 'Medium');
fis = addMF(fis, 'left_distance', 'smf', [14, 20], 'Name', 'Far');
% 输入3: 右侧障碍物距离 (0-20m)
fis = addInput(fis, [0, sensor_range], 'Name', 'right_distance');
fis = addMF(fis, 'right_distance', 'zmf', [5, 10], 'Name', 'Near');
fis = addMF(fis, 'right_distance', 'trimf', [8, 12, 16], 'Name', 'Medium');
fis = addMF(fis, 'right_distance', 'smf', [14, 20], 'Name', 'Far');
% 输入4: 目标方向偏差 (-pi到pi rad)
fis = addInput(fis, [-pi, pi], 'Name', 'goal_error');
fis = addMF(fis, 'goal_error', 'trapmf', [-pi, -pi, -pi/2, -pi/6], 'Name', 'Left_High');
fis = addMF(fis, 'goal_error', 'trimf', [-pi/3, -pi/12, 0], 'Name', 'Left_Low');
fis = addMF(fis, 'goal_error', 'trimf', [-pi/12, 0, pi/12], 'Name', 'Straight');
fis = addMF(fis, 'goal_error', 'trimf', [0, pi/12, pi/3], 'Name', 'Right_Low');
fis = addMF(fis, 'goal_error', 'trapmf', [pi/6, pi/2, pi, pi], 'Name', 'Right_High');
% 2.2 输出变量定义
% 输出1: 转向角 (-pi/4到pi/4 rad)
fis = addOutput(fis, [-max_steer, max_steer], 'Name', 'steering_angle');
fis = addMF(fis, 'steering_angle', 'trimf', [-max_steer, -max_steer/2, 0], 'Name', 'Turn_Left_High');
fis = addMF(fis, 'steering_angle', 'trimf', [-max_steer/2, -max_steer/4, 0], 'Name', 'Turn_Left_Low');
fis = addMF(fis, 'steering_angle', 'trimf', [-max_steer/8, 0, max_steer/8], 'Name', 'Go_Straight');
fis = addMF(fis, 'steering_angle', 'trimf', [0, max_steer/4, max_steer/2], 'Name', 'Turn_Right_Low');
fis = addMF(fis, 'steering_angle', 'trimf', [0, max_steer/2, max_steer], 'Name', 'Turn_Right_High');
% 输出2: 速度 (0到最大速度 m/s)
fis = addOutput(fis, [0, max_speed], 'Name', 'speed');
fis = addMF(fis, 'speed', 'zmf', [0.2, 0.5], 'Name', 'Very_Slow');
fis = addMF(fis, 'speed', 'trimf', [0.3, 0.6, 0.9], 'Name', 'Slow');
fis = addMF(fis, 'speed', 'trimf', [0.6, 0.9, 1.2], 'Name', 'Medium');
fis = addMF(fis, 'speed', 'smf', [1.0, max_speed], 'Name', 'Fast');
% 2.3 模糊规则库（基于经验的避障规则）
rules = [
"如果 front_distance 是 Very_Near 那么 steering_angle 是 Turn_Left_High 且 speed 是 Very_Slow", 1;
"如果 front_distance 是 Near 且 left_distance 是 Near 那么 steering_angle 是 Turn_Right_High 且 speed 是 Slow", 1;
"如果 front_distance 是 Near 且 right_distance 是 Near 那么 steering_angle 是 Turn_Left_High 且 speed 是 Slow", 1;
"如果 front_distance 是 Far 且 goal_error 是 Left_High 那么 steering_angle 是 Turn_Left_Low 且 speed 是 Medium", 1;
"如果 front_distance 是 Far 且 goal_error 是 Right_High 那么 steering_angle 是 Turn_Right_Low 且 speed 是 Medium", 1;
"如果 front_distance 是 Very_Far 且 goal_error 是 Straight 那么 steering_angle 是 Go_Straight 且 speed 是 Fast", 1;
"如果 left_distance 是 Near 且 right_distance 是 Far 那么 steering_angle 是 Turn_Right_Low 且 speed 是 Medium", 0.8;
"如果 right_distance 是 Near 且 left_distance 是 Far 那么 steering_angle 是 Turn_Left_Low 且 speed 是 Medium", 0.8;
"如果 front_distance 是 Near 且 left_distance 是 Far 且 right_distance 是 Far 那么 steering_angle 是 Go_Straight 且 speed 是 Slow", 0.9;
"如果 front_distance 是 Very_Far 且 goal_error 是 Left_Low 那么 steering_angle 是 Turn_Left_Low 且 speed 是 Fast", 0.7;
"如果 front_distance 是 Very_Far 且 goal_error 是 Right_Low 那么 steering_angle 是 Turn_Right_Low 且 speed 是 Fast", 0.7;
];
fis = addRule(fis, rules);
% 2.4 可视化模糊系统
figure('Position', [100, 100, 1200, 800]);
subplot(2,3,1); plotmf(fis, 'input', 1); title('前方距离隶属函数');
subplot(2,3,2); plotmf(fis, 'input', 2); title('左侧距离隶属函数');
subplot(2,3,3); plotmf(fis, 'input', 3); title('右侧距离隶属函数');
subplot(2,3,4); plotmf(fis, 'input', 4); title('目标偏差隶属函数');
subplot(2,3,5); plotmf(fis, 'output', 1); title('转向角隶属函数');
subplot(2,3,6); plotmf(fis, 'output', 2); title('速度隶属函数');
%% 3. 仿真主循环
% 初始化记录变量
trajectory = zeros(steps, 2);
trajectory(1,:) = robot_pos;
sensor_readings = zeros(steps, num_sensors);
controls = zeros(steps, 2); % [转向角, 速度]
dist_to_goal = zeros(steps, 1);
dist_to_goal(1) = norm(robot_pos - goal);
collision_flag = false;
fprintf('开始模糊控制避障仿真...\n');
fprintf('目标点: (%.1f, %.1f)\n', goal(1), goal(2));
% 创建实时可视化
figure('Position', [100, 100, 1000, 800]);
h_ax = axes;
hold on;
% 绘制静态元素
rectangle('Position', [map_size(1), map_size(3), ...
map_size(2)-map_size(1), map_size(4)-map_size(3)], ...
'EdgeColor', 'k', 'LineWidth', 2);
% 绘制障碍物
for i = 1:size(obstacles, 1)
viscircles(obstacles(i,1:2), obstacles(i,3), ...
'Color', [0.7, 0.2, 0.2], 'LineWidth', 1.5);
% 添加安全边界
viscircles(obstacles(i,1:2), obstacles(i,3) + safety_distance, ...
'Color', [1, 0.8, 0.8], 'LineWidth', 0.5, 'LineStyle', '--');
end
% 绘制起始点和目标点
plot(start(1), start(2), 'go', 'MarkerSize', 10, 'MarkerFaceColor', 'g');
text(start(1)+2, start(2), '起点', 'FontSize', 10, 'FontWeight', 'bold');
plot(goal(1), goal(2), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r');
text(goal(1)+2, goal(2), '目标', 'FontSize', 10, 'FontWeight', 'bold');
% 初始化动态元素
h_robot = rectangle('Position', [robot_pos(1)-robot_radius, robot_pos(2)-robot_radius, ...
2*robot_radius, 2*robot_radius], ...
'Curvature', [1,1], 'FaceColor', [0.2, 0.5, 0.8], ...
'EdgeColor', 'b', 'LineWidth', 2);
h_direction = quiver(robot_pos(1), robot_pos(2), ...
cos(robot_theta), sin(robot_theta), ...
robot_radius*2, 'Color', 'k', 'LineWidth', 2, ...
'MaxHeadSize', 2);
h_trajectory = plot(robot_pos(1), robot_pos(2), 'b-', 'LineWidth', 1.5);
h_sensors = gobjects(num_sensors, 1);
for i = 1:num_sensors
h_sensors(i) = plot([robot_pos(1), robot_pos(1)], ...
[robot_pos(2), robot_pos(2)], ...
'c-', 'LineWidth', 0.5);
end
axis equal; grid on; box on;
xlim([map_size(1)-5, map_size(2)+5]);
ylim([map_size(3)-5, map_size(4)+5]);
title('模糊控制避障仿真');
xlabel('X坐标 (m)'); ylabel('Y坐标 (m)');
%% 主仿真循环
for step = 2:steps
% 3.1 传感器模拟（计算到最近障碍物的距离）
sensor_dist = sensor_range * ones(1, num_sensors);
for sensor_idx = 1:num_sensors
sensor_angle = robot_theta + sensor_angles(sensor_idx);
sensor_end = robot_pos + sensor_range * [cos(sensor_angle), sin(sensor_angle)];
% 检查与每个障碍物的交点
for obs_idx = 1:size(obstacles, 1)
obs_pos = obstacles(obs_idx, 1:2);
obs_radius = obstacles(obs_idx, 3);
% 计算线段与圆的交点
[intersect, points] = line_circle_intersection(robot_pos, sensor_end, obs_pos, obs_radius);
if intersect
dist = norm(points(1,:) - robot_pos);
if dist < sensor_dist(sensor_idx)
sensor_dist(sensor_idx) = dist;
end
end
end
end
sensor_readings(step,:) = sensor_dist;
% 3.2 提取关键距离信息（用于模糊控制器输入）
front_idx = find(abs(sensor_angles) == min(abs(sensor_angles)));
left_idx = find(sensor_angles < 0);
right_idx = find(sensor_angles > 0);
front_dist = min(sensor_dist(front_idx));
left_dist = min(sensor_dist(left_idx));
right_dist = min(sensor_dist(right_idx));
% 3.3 计算目标方向偏差
goal_vector = goal - robot_pos;
goal_angle = atan2(goal_vector(2), goal_vector(1));
angle_error = wrapToPi(goal_angle - robot_theta); % 归一化到[-pi, pi]
% 3.4 模糊推理
fis_input = [front_dist, left_dist, right_dist, angle_error];
fis_output = evalfis(fis, fis_input);
steering_angle = fis_output(1);
speed = fis_output(2);
% 限制输出范围
steering_angle = max(min(steering_angle, max_steer), -max_steer);
speed = max(min(speed, max_speed), 0);
controls(step,:) = [steering_angle, speed];
% 3.5 更新机器人状态（简单运动学模型）
robot_theta = robot_theta + steering_angle * dt;
robot_theta = wrapToPi(robot_theta); % 保持角度在[-pi, pi]
robot_pos = robot_pos + speed * dt * [cos(robot_theta), sin(robot_theta)];
% 检查边界
robot_pos(1) = max(min(robot_pos(1), map_size(2)-1), map_size(1)+1);
robot_pos(2) = max(min(robot_pos(2), map_size(4)-1), map_size(3)+1);
trajectory(step,:) = robot_pos;
dist_to_goal(step) = norm(robot_pos - goal);
% 3.6 碰撞检测
collision = false;
for obs_idx = 1:size(obstacles, 1)
if norm(robot_pos - obstacles(obs_idx,1:2)) < (obstacles(obs_idx,3) + robot_radius)
collision = true;
collision_flag = true;
fprintf('步骤 %d: 发生碰撞！\n', step);
break;
end
end
if collision
speed = 0; % 停止
end
% 3.7 更新可视化
set(h_robot, 'Position', [robot_pos(1)-robot_radius, robot_pos(2)-robot_radius, ...
2*robot_radius, 2*robot_radius]);
set(h_direction, 'XData', robot_pos(1), 'YData', robot_pos(2), ...
'UData', cos(robot_theta)*robot_radius*2, ...
'VData', sin(robot_theta)*robot_radius*2);
set(h_trajectory, 'XData', trajectory(1:step,1), ...
'YData', trajectory(1:step,2));
% 更新传感器线
for sensor_idx = 1:num_sensors
sensor_angle = robot_theta + sensor_angles(sensor_idx);
sensor_end = robot_pos + sensor_dist(sensor_idx) * [cos(sensor_angle), sin(sensor_angle)];
if sensor_dist(sensor_idx) < sensor_range
line_color = [1, 0, 0]; % 红色表示检测到障碍物
else
line_color = [0, 1, 1]; % 青色表示无障碍物
end
set(h_sensors(sensor_idx), ...
'XData', [robot_pos(1), sensor_end(1)], ...
'YData', [robot_pos(2), sensor_end(2)], ...
'Color', line_color);
end
% 3.8 检查是否到达目标
if dist_to_goal(step) < 3
fprintf('步骤 %d: 成功到达目标！\n', step);
break;
end
% 3.9 检查超时
if step > steps-10
fprintf('仿真超时，未到达目标\n');
break;
end
% 暂停以便观察
pause(0.01);
% 刷新图形
drawnow;
end
%% 4. 性能分析与可视化
fprintf('\n仿真完成！\n');
fprintf('最终位置: (%.2f, %.2f)\n', robot_pos(1), robot_pos(2));
fprintf('距离目标: %.2f m\n', dist_to_goal(step));
fprintf('是否碰撞: %s\n', string(collision_flag));
% 4.1 创建性能分析图
figure('Position', [100, 100, 1400, 600]);
% 轨迹分析
subplot(2,3,1);
plot(trajectory(1:step,1), trajectory(1:step,2), 'b-', 'LineWidth', 2);
hold on;
for i = 1:size(obstacles, 1)
rectangle('Position', [obstacles(i,1)-obstacles(i,3), obstacles(i,2)-obstacles(i,3), ...
2*obstacles(i,3), 2*obstacles(i,3)], 'Curvature', [1,1], ...
'EdgeColor', 'r', 'FaceColor', [1,0.8,0.8]);
end
plot(start(1), start(2), 'go', 'MarkerSize', 10, 'MarkerFaceColor', 'g');
plot(goal(1), goal(2), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r');
plot(robot_pos(1), robot_pos(2), 'bo', 'MarkerSize', 8, 'MarkerFaceColor', 'b');
title('机器人运动轨迹');
xlabel('X (m)'); ylabel('Y (m)');
axis equal; grid on; legend('轨迹', '障碍物', '起点', '目标', '终点');
% 传感器读数
subplot(2,3,2);
sensor_time = (1:step) * dt;
for i = 1:min(4, num_sensors)
plot(sensor_time, sensor_readings(1:step,i), 'LineWidth', 1.5);
hold on;
end
title('传感器距离读数');
xlabel('时间 (s)'); ylabel('距离 (m)');
grid on; legend('传感器1', '传感器2', '传感器3', '传感器4');
ylim([0, sensor_range]);
% 控制输出
subplot(2,3,3);
yyaxis left;
plot(sensor_time, controls(1:step,1)*180/pi, 'b-', 'LineWidth', 2);
ylabel('转向角 (°)');
ylim([-max_steer*180/pi, max_steer*180/pi]);
yyaxis right;
plot(sensor_time, controls(1:step,2), 'r-', 'LineWidth', 2);
ylabel('速度 (m/s)');
ylim([0, max_speed]);
title('控制输出');
xlabel('时间 (s)'); grid on;
legend('转向角', '速度');
% 距离目标变化
subplot(2,3,4);
plot(sensor_time, dist_to_goal(1:step), 'm-', 'LineWidth', 2);
title('到目标的距离');
xlabel('时间 (s)'); ylabel('距离 (m)');
grid on;
% 模糊规则激活度
subplot(2,3,5);
rule_firing = zeros(step, size(rules,1));
for t = 1:step
fis_input = [sensor_readings(t,front_idx(1)), ...
min(sensor_readings(t,left_idx)), ...
min(sensor_readings(t,right_idx)), ...
angle_error];
[~, ~, rule_output] = evalfis(fis, fis_input);
rule_firing(t,:) = rule_output;
end
imagesc(sensor_time, 1:size(rules,1), rule_firing');
colorbar; colormap(jet);
title('模糊规则激活强度');
xlabel('时间 (s)'); ylabel('规则编号');
set(gca, 'YDir', 'normal');
% 3D控制曲面
subplot(2,3,6);
[x_grid, y_grid] = meshgrid(linspace(0, sensor_range, 20), ...
linspace(-max_steer, max_steer, 20));
z_grid = zeros(size(x_grid));
for i = 1:numel(x_grid)
% 固定其他输入，查看front_distance和goal_error对steering的影响
test_input = [x_grid(i), sensor_range/2, sensor_range/2, y_grid(i)];
output = evalfis(fis, test_input);
z_grid(i) = output(1); % steering_angle
end
surf(x_grid, y_grid, z_grid);
title('控制规则曲面 (转向角)');
xlabel('前方距离 (m)'); ylabel('目标偏差 (rad)'); zlabel('转向角 (rad)');
grid on; view(45, 30);
%% 5. 对比实验：模糊控制 vs PID控制
fprintf('\n\n对比实验：模糊控制 vs PID控制\n');
% PID控制器参数
Kp = 0.5; Ki = 0.01; Kd = 0.1;
pid_integral = 0;
pid_last_error = 0;
% 重新初始化进行对比
robot_pos_pid = start;
robot_theta_pid = atan2(goal(2)-start(2), goal(1)-start(1));
trajectory_pid = zeros(steps, 2);
trajectory_pid(1,:) = robot_pos_pid;
for step_pid = 2:steps
% 传感器测量（使用相同的传感器模型）
sensor_dist_pid = sensor_range * ones(1, num_sensors);
for sensor_idx = 1:num_sensors
sensor_angle = robot_theta_pid + sensor_angles(sensor_idx);
sensor_end = robot_pos_pid + sensor_range * [cos(sensor_angle), sin(sensor_angle)];
for obs_idx = 1:size(obstacles, 1)
[intersect, points] = line_circle_intersection(robot_pos_pid, sensor_end, ...
obstacles(obs_idx,1:2), obstacles(obs_idx,3));
if intersect
dist = norm(points(1,:) - robot_pos_pid);
if dist < sensor_dist_pid(sensor_idx)
sensor_dist_pid(sensor_idx) = dist;
end
end
end
end
% PID避障：简单启发式规则
front_dist_pid = min(sensor_dist_pid(front_idx));
% 如果前方有障碍物，转向
if front_dist_pid < safety_distance
% 检查左右哪边更安全
left_dist_pid = min(sensor_dist_pid(left_idx));
right_dist_pid = min(sensor_dist_pid(right_idx));
if left_dist_pid > right_dist_pid
steering_pid = max_steer/2; % 向左转
else
steering_pid = -max_steer/2; % 向右转
end
speed_pid = max_speed * 0.3; % 减速
else
% 朝向目标
goal_vector = goal - robot_pos_pid;
goal_angle = atan2(goal_vector(2), goal_vector(1));
angle_error = wrapToPi(goal_angle - robot_theta_pid);
% PID控制
pid_integral = pid_integral + angle_error * dt;
pid_derivative = (angle_error - pid_last_error) / dt;
steering_pid = Kp * angle_error + Ki * pid_integral + Kd * pid_derivative;
pid_last_error = angle_error;
steering_pid = max(min(steering_pid, max_steer), -max_steer);
speed_pid = max_speed * 0.8;
end
% 更新状态
robot_theta_pid = robot_theta_pid + steering_pid * dt;
robot_theta_pid = wrapToPi(robot_theta_pid);
robot_pos_pid = robot_pos_pid + speed_pid * dt * [cos(robot_theta_pid), sin(robot_theta_pid)];
% 边界检查
robot_pos_pid(1) = max(min(robot_pos_pid(1), map_size(2)-1), map_size(1)+1);
robot_pos_pid(2) = max(min(robot_pos_pid(2), map_size(4)-1), map_size(3)+1);
trajectory_pid(step_pid,:) = robot_pos_pid;
if norm(robot_pos_pid - goal) < 3
break;
end
end
% 对比可视化
figure('Position', [100, 100, 1200, 500]);
subplot(1,2,1);
plot(trajectory(1:step,1), trajectory(1:step,2), 'b-', 'LineWidth', 2);
hold on;
plot(trajectory_pid(1:step_pid,1), trajectory_pid(1:step_pid,2), 'r--', 'LineWidth', 2);
for i = 1:size(obstacles, 1)
rectangle('Position', [obstacles(i,1)-obstacles(i,3), obstacles(i,2)-obstacles(i,3), ...
2*obstacles(i,3), 2*obstacles(i,3)], 'Curvature', [1,1], ...
'EdgeColor', 'k', 'FaceColor', [0.9,0.9,0.9]);
end
plot(start(1), start(2), 'go', 'MarkerSize', 10, 'MarkerFaceColor', 'g');
plot(goal(1), goal(2), 'mo', 'MarkerSize', 10, 'MarkerFaceColor', 'm');
legend('模糊控制', 'PID控制', '障碍物', '起点', '目标');
title('控制方法对比');
xlabel('X (m)'); ylabel('Y (m)'); axis equal; grid on;
subplot(1,2,2);
performance_metrics = {
'路径长度', norm(diff(trajectory(1:step,:)), 'fro'), norm(diff(trajectory_pid(1:step_pid,:)), 'fro');
'平均速度', mean(controls(1:step,2)), mean(speed_pid);
'转向波动', std(controls(1:step,1)), std(steering_pid);
'到达时间', step*dt, step_pid*dt;
'最小障碍距离', min(min(sensor_readings(1:step,:))), min(min(sensor_dist_pid));
};
bar_data = [cell2mat(performance_metrics(:,2)), cell2mat(performance_metrics(:,3))];
h_bar = bar(bar_data);
set(gca, 'XTickLabel', performance_metrics(:,1), 'XTickLabelRotation', 45);
ylabel('数值');
title('性能指标对比');
legend('模糊控制', 'PID控制');
grid on;
%% 辅助函数定义
function [intersect, points] = line_circle_intersection(p1, p2, center, radius)
% 计算线段与圆的交点
intersect = false;
points = [];
% 线段向量
d = p2 - p1;
f = p1 - center;
% 二次方程系数
a = dot(d, d);
b = 2 * dot(f, d);
c = dot(f, f) - radius^2;
discriminant = b^2 - 4*a*c;
if discriminant >= 0
discriminant = sqrt(discriminant);
t1 = (-b - discriminant) / (2*a);
t2 = (-b + discriminant) / (2*a);
% 检查交点是否在线段上
if t1 >= 0 && t1 <= 1
intersect = true;
points = [points; p1 + t1*d];
end
if t2 >= 0 && t2 <= 1 && abs(t1-t2) > 1e-6
intersect = true;
points = [points; p1 + t2*d];
end
end
end
function angle = wrapToPi(angle)
% 将角度包装到[-pi, pi]范围
angle = mod(angle + pi, 2*pi) - pi;
end
三、Simulink模糊控制避障系统
对于更复杂的系统，可以使用Simulink实现：
%% Simulink模糊避障系统构建指南
% 1. 创建Simulink模型组件
% - 机器人动力学模型子系统
% - 传感器模拟子系统
% - 模糊逻辑控制器(FLC)模块
% - 环境与障碍物模块
% - 可视化显示模块
% 2. 关键Simulink模块配置
% a) 模糊逻辑控制器模块：
% - 使用Fuzzy Logic Controller模块
% - 载入上面创建的fis对象
% - 采样时间设置为dt
% b) 传感器子系统：
% - 使用MATLAB Function模块实现距离计算
% - 输出多个距离信号到模糊控制器
% c) 机器人动力学：
% - 使用积分器模块实现位置更新
% - 考虑非完整约束（如差速驱动模型）
% 3. 仿真配置
% - 使用可变步长求解器ode45
% - 最大步长设置为0.1
% - 启用数据记录用于分析
%% 差速驱动机器人模型示例代码
function [v_left, v_right] = differential_drive_controller(steering, speed, robot_params)
% 差速驱动控制
% 输入: steering - 期望转向角
% speed - 期望速度
% robot_params - 机器人参数（轮距等）
wheel_base = robot_params.wheel_base;
max_wheel_speed = robot_params.max_wheel_speed;
% 差速驱动运动学
linear_vel = speed;
angular_vel = (2 * linear_vel * tan(steering)) / wheel_base;
% 计算左右轮速度
v_left = linear_vel - (angular_vel * wheel_base / 2);
v_right = linear_vel + (angular_vel * wheel_base / 2);
% 限制轮速
v_left = max(min(v_left, max_wheel_speed), -max_wheel_speed);
v_right = max(min(v_right, max_wheel_speed), -max_wheel_speed);
end
四、算法优化与进阶
4.1 自适应模糊控制
% 自适应调整模糊规则权重
function updated_fis = adapt_fuzzy_rules(fis, performance_error, learning_rate)
% 根据性能误差调整规则权重
rules = fis.Rules;
for i = 1:length(rules)
% 如果这条规则在上次决策中激活度高但效果差，降低权重
if rules(i).FiringStrength > 0.5 && performance_error > 0.1
rules(i).Weight = max(0.1, rules(i).Weight - learning_rate * performance_error);
elseif rules(i).FiringStrength > 0.5 && performance_error < -0.05
rules(i).Weight = min(1.0, rules(i).Weight - learning_rate * performance_error);
end
end
fis.Rules = rules;
updated_fis = fis;
end
4.2 与UKF融合
% UKF状态估计 + 模糊控制决策
function [control_output, estimated_state] = ukf_fuzzy_fusion(sensor_data, fis, prev_state)
% UKF用于状态估计
[estimated_state, state_covariance] = ukf_state_estimation(prev_state, sensor_data);
% 从估计状态提取模糊控制器输入
front_dist_est = estimated_state(1);
left_dist_est = estimated_state(2);
right_dist_est = estimated_state(3);
goal_error_est = estimated_state(4);
% 考虑估计不确定性调整模糊输入
uncertainty_factor = diag(state_covariance);
adjusted_inputs = [
front_dist_est * (1 + 0.1*uncertainty_factor(1)),
left_dist_est * (1 + 0.1*uncertainty_factor(2)),
right_dist_est * (1 + 0.1*uncertainty_factor(3)),
goal_error_est
];
% 模糊推理
control_output = evalfis(fis, adjusted_inputs);
end
参考代码 基于模糊控制的避障仿真 www.youwenfan.com/contentcno/96418.html
五、部署建议
-
计算优化：
- 使用查表法替代实时模糊推理
- 简化隶属函数数量（5-7个为宜）
- 考虑使用ANFIS（自适应神经模糊系统）自动优化规则
-
传感器融合：
- 结合激光雷达、摄像头、超声波等多传感器
- 使用卡尔曼滤波融合传感器数据
- 考虑传感器故障容错机制
-
实时性保证：
- 在嵌入式平台（如ROS+STM32）上部署
- 使用固定点运算替代浮点
- 设定最大推理时间限制
浙公网安备 33010602011771号