APF与CBF融合的机器人路径规划算法实现

1. 项目概述：APF与CBF融合的机器人路径规划

在移动机器人导航领域，路径规划算法需要同时满足两个核心需求：高效的目标导向性和绝对的安全避障能力。传统人工势场法(APF)通过虚拟力场引导机器人运动，但存在局部极小值和动态避障效果不佳的固有缺陷。而控制障碍函数(CBF)作为现代安全控制理论的重要工具，能够将安全约束转化为数学形式保证系统状态始终处于安全集内。

本课程设计通过Matlab实现两者的创新性融合：利用APF生成全局引导方向作为期望输入，通过CBF构建二次规划(QP)问题求解满足所有安全约束的最优控制指令。这种混合架构既保留了APF的路径规划能力，又通过CBF的数学保证避免了碰撞风险。实测表明，在包含5个圆形障碍物的10m×10m环境中，机器人能以1.5m/s的最大速度安全抵达目标点，平均路径偏差小于0.3m。

2. 核心算法原理与实现

2.1 人工势场法(APF)建模

APF的核心思想是将目标点建模为引力源，障碍物建模为斥力源。引力势场函数设计为：

code复制U_att = 0.5 * K_att * ||p - p_goal||²

其中K_att为引力增益系数，p和p_goal分别表示机器人当前位置和目标位置。对应的引力计算函数为：

matlab复制function F_att = calc_attractive_force(pos, goal, K_att)
    vec_to_goal = goal - pos;
    F_att = K_att * vec_to_goal;
end

斥力势场采用分段函数设计，当机器人与障碍物有效距离d_eff（实际距离减去双方半径）小于影响阈值ρ_0时激活：

code复制U_rep = 0.5 * K_rep * (1/d_eff - 1/ρ_0)²

对应的斥力计算需注意方向处理：

matlab复制if dist_eff <= rho_0 && dist_eff > 0
    term1 = (1/dist_eff - 1/rho_0);
    term2 = 1 / (dist_eff^2);
    F_i = K_rep * term1 * term2 * (dist_vec / dist);
end

2.2 控制障碍函数(