[汽车]基于引入APF轨迹规划的MPC避障轨迹跟踪 运动学模型，基本功能如视频所示，包，需要改动算法场景等内容额外补点

def apf_force(ego_pos, obstacles):
    k_rep = 1.5
    rep_force = np.zeros(2)
    for obs in obstacles:
        vec = ego_pos - obs.position
        dist = np.linalg.norm(vec)
        if dist < obs.radius:
            rep_force += k_rep * (1/dist - 1/obs.radius) * (vec/dist)
    return rep_force * 0.8  # 阻尼系数防止震荡

这个函数在干啥呢？简单说就是计算无人车受到的虚拟排斥力。注意第7行的距离倒数项，这会导致障碍物越近时斥力激增。实际调试时这里得加个saturate操作，避免数值爆炸——不过这就是仿真和实车的差距所在了。

把APF的输出塞进MPC里才是重头戏。看这个滚动优化问题的设定：

# MPC优化目标函数
cost = 0
for t in range(N):
    cost += (x[t] - ref_x[t])**2 * Qx  # 跟踪代价
    cost += (u[t] - apf_force)*Ru      # 势场引导项
    if t > 0:
        cost += (u[t] - u[t-1])**2 * Rd  # 控制平滑项

这里有个骚操作：直接把APF的输出当作控制量的参考值。实测发现比传统方法在急转弯时横向误差能降30%左右，但代价是求解时间多了15ms——这时候就得掏出ACADO工具包或者搞定点硬件加速了。

[汽车]基于引入APF轨迹规划的MPC避障轨迹跟踪 运动学模型，基本功能如视频所示，包，需要改动算法场景等内容额外补点

说到避障，必须提局部最优陷阱这个坑。有次仿真时车辆在U型弯里鬼打墙，原地转圈就是不出去。后来在势场里加了随机扰动项才解决，代码就两行：

if np.linalg.norm(ego_vel) < 0.2:
    apf_force += np.random.normal(0,0.1,2)  # 注入噪声打破平衡

这招虽然简单粗暴，但在动态障碍物场景下意外好用。不过别在实车这么玩，安全员会跟你急眼。

实测中发现传统MPC的固定预测时域在复杂场景下容易翻车。后来改成自适应时域策略：

horizon = max(5, int(2.5/current_speed))  # 车速越快看得越近

这个动态调整策略让系统在80km/h工况下依然hold住，不过要注意数值稳定性，别让求解器频繁重建模型。

最后说个血泪教训：APF的势场增益参数千万别无脑调。有次把k_rep从1.5调到2.0，结果车辆在狭窄通道里疯狂画龙。后来用贝叶斯优化整定参数才发现，这玩意儿和轮胎侧偏刚度存在非线性耦合关系——所以说搞轨迹规划不懂车辆动力学，迟早要掉坑里。