【路径规划】【人工势场法】【控制障碍函数】【Matlab课程设计】【路径避障】【控制算法】【多智能体】

基于路径规划的人工势场法与控制障碍函数应用的课程设计Matlab源程序。
包括详细的代码注释。
包括技术参考文档一份。（可以帮助理解学习程序）

算法内容
路径规划模型，目标吸引力与障碍物排斥力的计算。
控制障碍函数与人工势场法的结合，机器人避障与路径跟踪。
代码完成度高，亲测可以一键运行。
代码有默认所有参数和环境设置，无需任何改动。
适合初学者和算法研究者学习与应用。

课题报告
引言。
设计目标与系统需求。
系统设计与实现。
系统测试与结果分析。
在这里插入图片描述

matlab

编辑

1%% =========================================================================
2% 项目名称：基于人工势场法(APF)与控制障碍函数(CBF)的移动机器人路径规划
3% 描述：融合 APF 的引导性与 CBF 的安全性，解决局部极小值问题并保证避障硬约束
4% 作者：AI Assistant
5% 日期：2026-03-06
6% =========================================================================
7clear; clc; close all;
8
9%% 1. 系统参数与环境初始化
10% — 机器人参数 —
11robot.pos = [0, 0]; % 初始位置 [x, y]
12robot.goal = [9, 9]; % 目标位置 [x, y]
13robot.radius = 0.3; % 机器人半径
14robot.v_max = 1.5; % 最大线速度
15robot.dt = 0.05; % 仿真步长
16robot.K_att = 1.0; % 引力增益系数
17robot.K_rep = 2.0; % 斥力增益系数
18robot.rho_0 = 2.5; % 障碍物影响距离阈值
19
20% — 障碍物定义 (圆心 x, y, 半径 r) —
21% 格式：[x, y, radius]
22obstacles = [
23 3, 3, 0.8;
24 5, 5, 1.0;
25 7, 2, 0.6;
26 2, 7, 0.7;
27 6, 8, 0.9
28];
29num_obs = size(obstacles, 1);
30
31% — CBF 参数 —
32% CBF 将避障转化为约束条件: h(x) >= 0
33% 这里使用指数型 CBF 保证安全性
34cbf.alpha = 1.0; % CBF 类 K 函数增益
35cbf.gamma = 0.5; % QP 优化中的权重因子 (平衡跟踪与避障)
36
37% — 仿真设置 —
38T_max = 500; % 最大迭代次数
39tol_dist = 0.3; % 到达目标的容差距离
40history.pos = zeros(T_max, 2);
41history.vel = zeros(T_max, 2);
42history.time = zeros(T_max, 1);
43
44fprintf(‘>>> 开始仿真：APF + CBF 融合路径规划…\n’);
45
46%% 2. 主仿真循环
47stop_flag = false;
48iter = 1;
49
50while iter <= T_max && ~stop_flag
51 t = (iter-1) * robot.dt;
52
53 % 2.1 计算人工势场力 (APF Force) - 作为期望加速度/速度方向
54 F_att = calc_attractive_force(robot.pos, robot.goal, robot.K_att);
55 F_rep = calc_repulsive_force(robot.pos, obstacles, robot.K_rep, robot.rho_0);
56 F_apf = F_att + F_rep;
57
58 % 归一化并限制速度大小 (模拟运动学约束)
59 if norm(F_apf) > 0
60 v_des_apf = (F_apf / norm(F_apf)) * robot.v_max;
61 else
62 v_des_apf = [0, 0];
63 end
64
65 % 2.2 构建控制障碍函数 (CBF) 约束
66 % 目标：找到一个控制输入 u (速度)，使得它尽可能接近 v_des_apf
67 % 同时满足所有障碍物的安全约束: dot(h, u) >= -alpha * h(x)
68
69 % 定义 QP 问题变量: u = [ux, uy]
70 % 目标函数: min ||u - v_des_apf||^2 => min 0.5u’Hu + f’u
71 H = 2 * eye(2);
72 f = -2 * v_des_apf’;
73
74 % 约束条件: A_ineq * u <= b_ineq
75 % CBF 约束推导:
76 % h_i(x) = ||p - p_obs||^2 - (r_robot + r_obs + safety_margin)^2
77 % 导数条件: dot(h_i, u) + alpha * h_i >= 0
78 % 即: 2(p - p_obs)’ * u >= -alpha * h_i
79 % 转化为标准形式: -2(p - p_obs)’ * u <= alpha * h_i
80
81 A_ineq = [];
82 b_ineq = [];
83 safety_margin = 0.2; % 额外安全裕度
84
85 for i = 1:num_obs
86 p_obs = obstacles(i, 1:2);
87 r_obs = obstacles(i, 3);
88
89 dist_vec = robot.pos - p_obs;
90 dist_norm = norm(dist_vec);
91
92 % 计算 CBF 函数值 h(x)
93 % h(x) > 0 表示安全
94 h_val = dist_norm^2 - (robot.radius + r_obs + safety_margin)^2;
95
96 % 只有当障碍物在附近时才激活强约束，或者始终激活以保证全局安全
97 % 这里我们对所有障碍物都施加约束，但在远处约束很弱
98 if h_val < (robot.rho_0)^2
99 % 约束矩阵行: -2 * (p - p_obs)’
100 row_A = -2 * dist_vec’;
101 % 约束向量元素: alpha * h_val
102 row_b = cbf.alpha * h_val;
103
104 A_ineq = [A_ineq; row_A];
105 b_ineq = [b_ineq; row_b];
106 end
107 end
108
109 % 2.3 求解二次规划 (QP) 问题
110 % 如果没有障碍物约束，直接使用 APF 速度
111 if isempty(A_ineq)
112 u_opt = v_des_apf’;
113 else
114 % 使用 Matlab 内置 quadprog 求解
115 % min 0.5x’Hx + f’x s.t. Ax <= b
116 options = optimoptions(‘quadprog’, ‘Display’, ‘off’, ‘Algorithm’, ‘interior-point-convex’);
117 try
118 u_opt = quadprog(H, f, A_ineq, b_ineq, [], [], [], [], [], options);
119 catch
120 % 如果 QP 无解 (极端情况)， fallback 到 APF 并减速
121 warning(‘QP 无解，启用紧急避障策略’);
122 u_opt = 0.5 * v_des_apf’;
123 end
124 end
125
126 % 2.4 更新机器人状态 (欧拉积分)
127 robot.vel = u_opt’; % 最优速度
128 robot.pos = robot.pos + robot.vel * robot.dt;
129
130 % 记录数据
131 history.pos(iter, 😃 = robot.pos;
132 history.vel(iter, 😃 = robot.vel;
133 history.time(iter) = t;
134
135 % 检查是否到达目标
136 if norm(robot.pos - robot.goal) < tol_dist
137 fprintf(‘>>> 成功到达目标！迭代次数: %d, 耗时: %.2f s\n’, iter, t);
138 stop_flag = true;
139 % 截断历史记录
140 history.pos = history.pos(1:iter, 😃;
141 history.vel = history.vel(1:iter, 😃;
142 history.time = history.time(1:iter);
143 end
144
145 iter = iter + 1;
146end
147
148if ~stop_flag
149 fprintf(‘>>> 警告：未在最大迭代次数内到达目标，可能陷入局部极小值或参数需调整。\n’);
150end
151
152%% 3. 结果可视化
153figure(‘Color’, ‘w’, ‘Name’, ‘APF + CBF 路径规划仿真结果’, ‘Position’, [100, 100, 800, 600]);
154
155% 3.1 绘制环境
156hold on; axis equal; grid on; box on;
157xlim([-1, 11]); ylim([-1, 11]);
158title([‘路径规划结果 (APF 引导 + CBF 避障)’], ‘FontSize’, 14, ‘FontWeight’, ‘bold’);
159xlabel(‘X (m)’); ylabel(‘Y (m)’);
160
161% 绘制起点和终点
162plot(robot.pos(1), robot.pos(2), ‘go’, ‘MarkerSize’, 10, ‘MarkerFaceColor’, ‘g’, ‘DisplayName’, ‘Start’);
163plot(robot.goal(1), robot.goal(2), ‘rs’, ‘MarkerSize’, 10, ‘MarkerFaceColor’, ‘r’, ‘DisplayName’, ‘Goal’);
164
165% 绘制障碍物
166theta_obs = linspace(0, 2pi, 50);
167for i = 1:num_obs
168 x_obs = obstacles(i, 1) + (obstacles(i, 3) + 0.2) * cos(theta_obs); % 加上安全裕度显示
169 y_obs = obstacles(i, 2) + (obstacles(i, 3) + 0.2) * sin(theta_obs);
170 fill(x_obs, y_obs, [0.8, 0.8, 0.8], ‘EdgeColor’, ‘k’, ‘DisplayName’, ‘Obstacle + Safety’);
171 % 绘制实际物理边界
172 x_real = obstacles(i, 1) + obstacles(i, 3) * cos(theta_obs);
173 y_real = obstacles(i, 2) + obstacles(i, 3) * sin(theta_obs);
174 plot(x_real, y_real, ‘k–’, ‘LineWidth’, 1);
175end
176
177% 绘制规划路径
178plot(history.pos(:,1), history.pos(:,2), ‘b-’, ‘LineWidth’, 2, ‘DisplayName’, ‘Planned Path’);
179
180% 绘制速度矢量 (每隔 10 个点画一个)
181step_vec = 10;
182for i = 1:step_vec:length(history.pos)
183 quiver(history.pos(i,1), history.pos(i,2), history.vel(i,1), history.vel(i,2), …
184 0.5, ‘Color’, ‘m’, ‘MaxHeadSize’, 2);
185end
186
187legend(‘Location’, ‘bestoutside’);
188hold off;
189
190%% 4. 辅助函数定义 (Local Functions)
191
192% — 引力场计算 —
193function F_att = calc_attractive_force(pos, goal, K_att)
194 % 引力势场 U_att = 0.5 * K_att * ||pos - goal||^2
195 % 引力 F_att = -grad(U_att) = K_att * (goal - pos)
196 vec_to_goal = goal - pos;
197 F_att = K_att * vec_to_goal;
198end
199
200% — 斥力场计算 —
201function F_rep = calc_repulsive_force(pos, obstacles, K_rep, rho_0)
202 F_rep = [0, 0];
203 num_obs = size(obstacles, 1);
204
205 for i = 1:num_obs
206 p_obs = obstacles(i, 1:2);
207 r_obs = obstacles(i, 3);
208
209 dist_vec = pos - p_obs;
210 dist = norm(dist_vec);
211
212 % 有效距离 (减去障碍物半径和机器人半径)
213 dist_eff = dist - (r_obs + 0.1); % 0.1 是简化的机器人半径补偿
214
215 if dist_eff <= rho_0 && dist_eff > 0
216 % 斥力势场 U_rep = 0.5 * K_rep * (1/dist_eff - 1/rho_0)^2
217 % 斥力 F_rep = -grad(U_rep)
218 % 公式推导: F = K_rep * (1/dist_eff - 1/rho_0) * (1/dist_eff^2) * (dist_vec/dist)
219
220 term1 = (1/dist_eff - 1/rho_0);
221 term2 = 1 / (dist_eff^2);
222
223 F_i = K_rep * term1 * term2 * (dist_vec / dist);
224 F_rep = F_rep + F_i;
225 elseif dist_eff <= 0
226 % 碰撞发生，施加极大斥力
227 F_rep = F_rep + [100, 100]; % 简单处理，实际应触发急停
228 end
229 end
230end
在这里插入图片描述
在这里插入图片描述