混合A*算法与MPC在自动驾驶路径规划中的应用

1. 混合A*运动规划算法概述

混合A算法是传统A算法在连续状态空间中的扩展版本，特别适合处理车辆这类非完整约束系统的运动规划问题。与离散化的A不同，混合A通过在状态空间中采样连续的运动基元（motion primitives）来构建搜索树，既保留了A*的启发式搜索特性，又能生成满足车辆运动学约束的平滑路径。

在实际自动驾驶系统中，混合A*通常作为全局路径规划器使用。它接收高清地图、起点终点坐标以及障碍物信息作为输入，输出一条符合车辆运动学特性的可行路径。这条路径虽然可行，但往往不够平滑，需要后续的路径优化处理。

注意：混合A*的启发式函数设计直接影响搜索效率。常见做法是结合Reeds-Shepp曲线（考虑车辆最小转弯半径的路径）和欧几里得距离，前者用于近距离精确引导，后者用于远距离快速估计。

1.1 算法核心流程解析

混合A*的执行流程可以分为以下几个关键步骤：

1. 节点扩展：从开放集中取出代价最低的节点，生成若干条符合车辆运动学的前向模拟轨迹（通常3-5条不同转向角的轨迹）
2. 碰撞检测：对每条生成轨迹进行碰撞检查，剔除与障碍物相交的无效路径
3. 启发式评估：计算剩余轨迹到终点的Reeds-Shepp距离和欧式距离的加权和
4. 节点处理：将有效节点加入开放集，并记录父节点指针用于回溯路径

python复制# 混合A*节点扩展示例代码
def expand_node(current_node):
    motion_primitives = generate_motion_primitives(current_node)
    for primitive in motion_primitives:
        new_node = simulate_kinematics(current_node, primitive)
        if not collision_check(new_node):
            new_node.h = reed_shepp_heuristic(new_node, goal)
            new_node.g = current_node.g + path_length(primitive)
            new_node.f = new_node.g + new_node.h
            open_set.add(new_node)

1.2 车辆运动学建模关键

混合A*的性能很大程度上取决于车辆运动学模型的准确性。常用的自行车模型包含以下参数：

车辆状态更新公式：

code复制x' = x + v * cos(θ) * dt
y' = y + v * sin(θ) * dt
θ' = θ + (v/L) * tan(δ) * dt

其中(x,y)为车辆后轴中心坐标，θ为航向角，δ为前轮转向角。

2. 路径规划与跟踪的整体架构

完整的运动规划系统通常采用分层架构，混合A*作为全局规划器生成粗略路径，再由局部规划器（如MPC）进行实时避障和轨迹优化，最后通过路径跟踪控制器（LQR/PID）执行精确跟踪。

2.1 系统工作流程

1. 全局路径规划层：混合A*生成初始路径
2. 轨迹优化层：对初始路径进行平滑处理（梯度下降/样条插值）
3. 局部规划层：MPC考虑动态障碍物生成安全轨迹
4. 控制层：LQR/PID计算转向和油门/刹车指令

实测经验：在复杂场景下，混合A*的规划时间可能达到100-300ms，因此实际系统中常采用异步规划策略，即车辆执行上一周期规划结果的同时进行下一周期的规划计算。

2.2 各模块接口设计

模块间的数据传递需要特别注意坐标系一致性：

3. 模型预测控制(MPC)实现细节

MPC通过在线求解有限时域的最优控制问题，能够显式处理系统约束（如转向角限制）和障碍物避碰约束。在路径跟踪中，MPC通常采用以下成本函数：

code复制min Σ(位置误差² + 航向误差² + 控制量变化率²) + 终端成本
s.t. 车辆动力学约束
     控制量上下限约束
     障碍物距离约束

3.1 MPC关键参数调优

python复制# MPC问题构建示例（使用cvxpy）
def build_mpc_problem():
    x = cvxpy.Variable((nx, T+1))  # 状态变量
    u = cvxpy.Variable((nu, T))    # 控制变量
    
    cost = 0
    constraints = []
    
    for t in range(T):
        cost += cvxpy.quad_form(x[:,t]-x_ref[:,t], Q)
        cost += cvxpy.quad_form(u[:,t], R)
        constraints += [x[:,t+1] == A@x[:,t] + B@u[:,t]]  # 动力学约束
        constraints += [cvxpy.abs(u[0,t]) <= max_steer]    # 转向角约束
    
    problem = cvxpy.Problem(cvxpy.Minimize(cost), constraints)
    return problem

3.2 实时性优化技巧

1. 热启动：使用上一周期的解作为当前优化的初始猜测
2. 稀疏求解器：利用QP问题的带状结构特性
3. 固定计算时间：设置最大迭代次数保证实时性
4. 降阶模型：在远距离段使用简化的车辆模型

4. LQR与PID控制器的对比实现

4.1 LQR控制器设计

线性二次调节器(LQR)通过求解Riccati方程得到最优状态反馈矩阵K。对于路径跟踪问题，通常将系统在参考轨迹上线性化：

code复制误差动力学模型：
e = [x-x_ref; y-y_ref; θ-θ_ref]
u = -K*e

LQR的优势在于它考虑了系统各状态变量间的耦合关系，特别适合高速情况下的稳定控制。但需要准确的车辆模型参数：

4.2 PID控制器调参

PID虽然简单，但在低速场景下仍然有效。对于转向控制，典型参数范围为：

实测技巧：先调纯P控制器，待车辆能基本跟踪路径后，再加入D项抑制振荡，最后加I项消除残余误差。夜间测试时适当降低增益20%，因为视觉定位误差通常更大。

4.3 混合控制策略

实际系统常采用分层控制策略：

• 高速段（>5m/s）：LQR为主
• 低速段：PID为主
• 过渡区域：加权混合输出

code复制u_final = α*u_lqr + (1-α)*u_pid
α = sigmoid(v - v_threshold)

5. 系统集成与实测问题排查

5.1 典型问题速查表

5.2 实车调试心得

1. 定位误差处理：在MPC成本函数中加入定位不确定性权重，对GPS信号弱区域加大航向误差惩罚
2. 湿滑路面适应：动态调整LQR的Q矩阵，降低横向加速度权重
3. 紧急制动策略：当跟踪误差超过阈值时，触发安全接管模式
4. 计算资源分配：优先保证感知和控制的CPU资源，规划模块可适当降频

在最后部署阶段，建议先在仿真环境中完成以下测试场景：

• 直角弯道跟踪（测试控制精度）
• 动态障碍物避让（测试MPC反应速度）
• 低附着路面（测试鲁棒性）
• 定位丢失场景（测试降级策略）

经过我们团队的实际验证，这套算法组合在园区物流车应用中实现了平均跟踪误差<0.15m（低速）和<0.3m（高速）的性能指标，计算耗时控制在50ms以内，满足实时性要求。