智能车基于五次多项式的智能车横向避幢模型，首先根据工况计算出预碰撞时间，进而计算出最小转向距离，通过MPC预测控制算法来对规划路径进行跟踪控制。
先说说预碰撞时间（TTC）的计算，这相当于系统的预警雷达。直接上代码更直观：
def calculate_ttc(ego_speed, obstacle_speed, distance): relative_speed = ego_speed - obstacle_speed return distance / relative_speed if relative_speed !=0 else float('inf')注意这里要处理相对速度为0的边界情况，否则程序会直接炸给你看。得到TTC后，结合车辆动力学特性算最小转向距离。这里有个经验公式：
def min_avoidance_distance(speed, friction=0.8): return (speed**2) / (2 * friction * 9.8) * 1.2 # 留20%安全余量这里的安全系数不能照搬教科书，得根据实际路况动态调整，雨天和晴天参数差得不是一星半点。
五次多项式的精髓在于生成顺滑的避障路径。看这个参数生成函数：
def quintic_poly_coeffs(start, end, T): a0 = start[0] a1 = start[1] a2 = start[2]/2.0 A = np.array([ [T**3, T**4, T**5], [3*T**2, 4*T**3, 5*T**4], [6*T, 12*T**2, 20*T**3] ]) b = np.array([ end[0] - (a0 + a1*T + a2*T**2), end[1] - (a1 + 2*a2*T), end[2] - 2*a2 ]) x = np.linalg.solve(A, b) return [a0, a1, a2, x[0], x[1], x[2]]这矩阵方程看着吓人，其实就是把起终点位置、速度、加速度都匹配上。注意数值稳定性问题，当T趋近于0时得做特殊处理。
智能车基于五次多项式的智能车横向避幢模型，首先根据工况计算出预碰撞时间，进而计算出最小转向距离，通过MPC预测控制算法来对规划路径进行跟踪控制。
MPC控制才是重头戏。核心是这个滚动优化问题：
import cvxpy as cp def mpc_controller(x0, ref_path): N = 10 # 预测时域 dt = 0.1 Q = np.diag([10, 1, 5, 2]) # 状态权重 R = np.diag([0.5, 0.2]) # 控制权重 # 定义优化变量 x = cp.Variable((4, N+1)) u = cp.Variable((2, N)) cost = 0 constraints = [] for t in range(N): cost += cp.quad_form(x[:,t]-ref_path[:,t], Q) cost += cp.quad_form(u[:,t], R) # 车辆动力学约束 constraints += [ x[:,t+1] == dynamics_model(x[:,t], u[:,t], dt) ] # 初始状态约束 constraints += [x[:,0] == x0] prob = cp.Problem(cp.Minimize(cost), constraints) prob.solve(solver=cp.ECOS) return u[:,0].value这里有几个实战技巧：预测时域不是越长越好，得在计算资源和控制效果间找平衡。权重参数Q和R需要实车标定，有时候还得搞自适应调整策略。
调试时发现个反直觉的现象：有时候增加路径跟踪的权重反而导致控制抖动。后来发现是动力学模型里的轮胎侧偏刚度参数不准，模型和实际车辆特性不匹配导致的。所以再好的算法也得建立在准确的车辆模型基础上。
最后说个避坑经验：五次多项式生成的路径虽然连续平滑，但急弯场景下可能出现曲率突变。这时候需要在前端路径规划时加入曲率约束，或者在MPC的代价函数里增加曲率变化率的惩罚项。具体实现就是在状态变量里加入航向角二阶导的约束，这里面的微分关系处理起来还挺烧脑的。
