智能车基于五次多项式的智能车横向避幢模型，首先根据工况计算出预碰撞时间，进而计算出最小转向距离，通过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控制才是重头戏。核心是这个滚动优化问题：

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的代价函数里增加曲率变化率的惩罚项。具体实现就是在状态变量里加入航向角二阶导的约束，这里面的微分关系处理起来还挺烧脑的。