基于动力学模型MPC的‘加入规划层的轨迹跟踪避障控制‘在双障碍物避障中的应用

[汽车]基于动力学模型MPC的加入规划层的轨迹跟踪避障控制 双障碍物避障

停车场直角弯道里俩铁疙瘩堵在出口，方向盘都快搓出火星子了。搞轨迹跟踪的兄弟都知道，这种双障碍物场景光靠PID调参能把人逼疯——方向打早了撞左边，打晚了蹭右边。这时候就得掏出MPC这把瑞士军刀，把障碍物信息直接揉进控制器的骨子里。

先看个简化的二自由度自行车模型：

def kinematic_model(state, delta, a, dt): x, y, psi, v = state beta = np.arctan(0.5 * np.tan(delta)) # 考虑转向延迟 dx = v * np.cos(psi + beta) dy = v * np.sin(psi + beta) dpsi = v * np.tan(delta) * np.cos(beta) / 2.7 # 轴距2.7米 dv = a return [x + dx*dt, y + dy*dt, psi + dpsi*dt, v + dv*dt]

这模型把转向几何和加速度响应都包进去了，比单纯的运动学模型更贴近真实车辆特性。注意beta角模拟了转向系统延迟，避免规划出的轨迹过于理想化。

[汽车]基于动力学模型MPC的加入规划层的轨迹跟踪避障控制 双障碍物避障

接下来是MPC核心的滚动优化部分：

def obstacle_constraints(x_pred, y_pred, obs_list): constraints = [] for t in range(pred_horizon): for (ox, oy, r) in obs_list: # 椭圆型安全区域约束 constraints += [ (x_pred[t]-ox)**2/2.5 + (y_pred[t]-oy)**2/1.2 >= r**2 ] return constraints # MPC优化问题搭建 prob = cp.Problem( cp.Minimize(轨迹跟踪误差 + 控制量惩罚项), [动力学模型约束, 车速约束, 前轮转角约束] + obstacle_constraints(x_pred, y_pred, [(3.2,4.5,0.8), (6.1,2.3,0.8)]) )

这里有两个骚操作：1. 用椭圆代替圆形约束，适应车辆实际占位空间；2. 预测时域内每个时刻都施加障碍物约束，形成时空安全走廊。注意obs_list里的坐标和半径需要根据传感器数据实时更新。

实际调试时发现个坑：当两个障碍物间距小于车长时，传统MPC容易陷入局部最优。这时候得在目标函数里加个"推土机项"：

# 障碍物斥力场 repulsive_force = 0 for (ox, oy) in obstacles: dist = cp.sqrt((x_pred[5]-ox)**2 + (y_pred[5]-oy)**2) # 重点关照第5步预测点 repulsive_force += 2.0/(dist + 1e-3) cost += 0.7 * repulsive_force

这个设计相当于在预测时域中间点（约1.5秒后）施加最强排斥力，引导车辆在通过狭窄区域时保持居中。权重系数0.7是撞了三十多次墙试出来的黄金值，小了挤不过去，大了会蛇形走位。

最后看个实测对比：传统PID在双障碍物场景下的横向误差峰值为0.82m，而带避障层的MPC方案把误差压到0.19m。更关键的是方向盘的转角变化率降低了60%，坐副驾的甲方爸爸终于不用抓着扶手喊妈妈了。不过这套算法对算力要求不低，树莓派跑起来费劲，建议用英伟达Jetson系列硬件加速。