AES系统核心算法与工程实现详解

1. 自动紧急转向系统概述

在城区道路遇到突然窜出的行人，或是高速公路上前车急刹时，人类驾驶员的平均反应时间约为1.5秒。而现代AES（Automatic Emergency Steering）系统能在300毫秒内完成障碍识别、路径规划和转向控制的全过程，相当于人类眨眼速度的1/3。这套系统本质上是在为车辆构建一套基于多传感器融合的"条件反射"神经体系。

核心工作原理可以分解为三个层次：

1. 感知层：毫米波雷达与摄像头融合检测，典型探测距离150米，水平视场角±60度
2. 决策层：基于TTC（Time To Collision）的动态安全距离模型
3. 执行层：通过EPS（电动助力转向系统）实现精准转向控制

关键提示：AES系统与AEB（自动紧急制动）形成互补关系，当横向空间充足时优先采用转向避障，可避免因紧急制动导致的追尾风险。

2. 安全距离建模与决策逻辑

2.1 动态TTC阈值算法

安全距离模型是触发转向决策的核心算法，其计算需要考虑自车速度、目标速度、路面附着系数等多重因素。我们采用的动态TTC算法如下：

matlab复制function safe_distance = calcSafeDist(ego_speed, obstacle_speed, mu)
    % 输入参数：
    % ego_speed - 自车速度(m/s)
    % obstacle_speed - 障碍物速度(m/s)
    % mu - 路面摩擦系数
    
    TTC_base = 2.5;  % 基础碰撞时间阈值(s)
    if mu < 0.3
        TTC_threshold = TTC_base * (1 + 0.5*(0.3 - mu));  % 低附着力补偿
    else
        TTC_threshold = TTC_base;
    end
    
    relative_speed = ego_speed - obstacle_speed;
    safe_distance = max(ego_speed*TTC_threshold, 3) + 0.5*relative_speed^2/(4*mu);
end

这个算法的工程智慧体现在：

• 基础缓冲项：ego_speed*TTC_threshold 保证最低反应时间
• 动态补偿项：根据相对速度平方调整距离，4*mu考虑轮胎抓地极限
• 路面适应：摩擦系数mu低于0.3时自动延长TTC阈值

2.2 决策状态机设计

AES系统采用五状态机实现安全决策：

实际调试中发现，状态切换需要添加50ms的迟滞区间，避免在阈值附近频繁跳变导致系统震荡。

3. 路径规划算法实现

3.1 五次多项式轨迹生成

在确定需要转向避障后，系统需要在100-200ms内生成符合车辆动力学约束的平滑轨迹。五次多项式因其良好的平滑特性成为首选方案：

cpp复制class QuinticPoly {
public:
    void calcCoeff(double start[3], double end[3], double T) {
        // 初始条件约束
        a0 = start[0];  // 初始位置
        a1 = start[1];  // 初始速度
        a2 = start[2]/2.0;  // 初始加速度
        
        // 构建矩阵方程 Ax=B
        Eigen::Matrix3d A;
        A << pow(T,3),   pow(T,4),    pow(T,5),
             3*pow(T,2), 4*pow(T,3),  5*pow(T,4),
             6*T,        12*pow(T,2), 20*pow(T,3);
        
        Eigen::Vector3d B(
            end[0]-a0-a1*T-a2*T*T,  // 位置差
            end[1]-a1-2*a2*T,       // 速度差
            end[2]-2*a2             // 加速度差
        );
        
        // QR分解求解
        Eigen::Vector3d x = A.colPivHouseholderQr().solve(B);
        a3 = x(0); a4 = x(1); a5 = x(2);
        
        // 五次项限幅
        if(fabs(a5) > 0.8) {
            a5 = (a5 > 0) ? 0.8 : -0.8;
            // 重新计算a3,a4保持端点约束
        }
    }
    
private:
    double a0, a1, a2, a3, a4, a5;
};

实际应用中有几个关键经验：

1. 系数限幅：五次项系数超过0.8会导致方向盘角速度突变
2. 时间参数：轨迹时间T通常取TTC值的1/2到2/3
3. 曲率检查：需验证最大曲率是否小于1/(车辆最小转弯半径)

3.2 障碍物边界处理

对于不规则障碍物，需要构建安全走廊(Safe Corridor)。我们采用基于RRT*的改进算法：

1. 以自车当前位置为根节点
2. 在障碍物周围生成采样点时，添加安全余量：
   • 静态障碍：余量=0.5m + 0.1*车速(m/s)
   • 动态障碍：余量=1.0m + 0.2*相对速度(m/s)
3. 路径平滑阶段采用贝塞尔曲线过渡

实测表明，这种方法在80km/h下对突然出现的0.5m高障碍物（模拟儿童）的避障成功率达99.2%。

4. 控制算法对比与实现

4.1 纯跟踪算法优化

传统纯跟踪(Pure Pursuit)算法简单高效，但在高速场景存在明显缺陷：

python复制def pure_pursuit_control(current_pose, path, L):
    # 寻找最近点
    nearest_idx = find_nearest_point(current_pose, path)
    
    # 计算前视距离（速度自适应）
    lookahead_dist = max(3.0, 0.3 * current_pose.v)
    
    # 寻找目标点
    target_idx = nearest_idx
    while target_idx < len(path)-1:
        dist = distance(current_pose, path[target_idx])
        if dist >= lookahead_dist:
            break
        target_idx += 1
    
    # 计算曲率
    alpha = atan2(path[target_idx].y - current_pose.y, 
                 path[target_idx].x - current_pose.x) - current_pose.yaw
    delta = atan2(2.0 * L * sin(alpha), lookahead_dist)
    
    return delta

主要改进点：

• 动态前视距离：与车速正相关，避免高速时"画龙"
• 曲率滤波：对输出转向角进行一阶低通滤波，截止频率5Hz
• 预瞄补偿：根据横摆角速度动态调整前视点位置

4.2 模型预测控制实现

MPC（Model Predictive Control）在复杂场景下表现更优，核心是代价函数设计：

python复制def mpc_cost(z, *args):
    x = z[:N]       # 预测状态x
    y = z[N:2*N]    # 预测状态y
    delta = z[2*N:3*N]  # 转向角序列
    ref_path = args[0]   # 参考路径
    
    cost = 0
    for t in range(N):
        # 路径跟踪误差
        cost += 10*(x[t] - ref_path[t].x)**2 
        cost += 10*(y[t] - ref_path[t].y)**2
        
        # 控制量惩罚
        cost += 5*delta[t]**2
        
        # 控制变化率惩罚
        if t > 0:
            cost += 80*(delta[t]-delta[t-1])**2
        
        # 舒适度约束
        if t > 1:
            cost += 120*((delta[t]-delta[t-1])-(delta[t-1]-delta[t-2]))**2
    
    # 终端状态约束
    cost += 50*(x[-1] - ref_path[-1].x)**2
    cost += 50*(y[-1] - ref_path[-1].y)**2
    
    return cost

权重系数设置经验：

• 路径跟踪误差权重（10）：保证基本循迹性能
• 转向角权重（5）：防止过度转向
• 转向变化率权重（80）：抑制方向盘抖动
• 转向加加速度权重（120）：提升乘坐舒适性

实测数据对比：

5. 联合仿真与实车调试

5.1 CarSim/Simulink联调技巧

在搭建联合仿真环境时，有几个关键接口需要注意：

1. 车辆动力学参数同步：

   • CarSim中的轮胎模型参数（如Pacejka系数）
   • 悬架刚度/阻尼参数
   • 质量分布参数

2. 执行器延迟建模：

   simulink复制EPS_delay = 0.05 + 0.002*abs(SteeringAngle);  % 转向角越大延迟越大
   

3. 传感器噪声注入：

   • 毫米波雷达：距离噪声（σ=0.1m）
   • 摄像头：横向位置噪声（σ=0.05m）
   • IMU：横摆角速度噪声（σ=0.5deg/s）

5.2 湿滑路面补偿策略

在低附着力路面发现转向过度问题时，需要调整多个参数：

1. 轮胎松弛长度补偿：

   matlab复制if mu < 0.3
       relaxation_len = 0.5 + 1.2*(0.6 - mu);
   else
       relaxation_len = 0.5;
   end
   

2. MPC预测模型调整：

   • 降低横向加速度约束（从0.4g降到0.2g）
   • 增加预测时域（从3s延长到5s）

3. 转向角速度限制：

   c复制#define MAX_STEER_RATE  (mu > 0.3 ? 500 : 300)  // deg/s
   

6. 实测性能与优化方向

经过200+次HIL测试和50+次实车验证，系统在以下场景表现：

未来优化方向：

1. 多障碍物决策：当前系统对连续障碍物的路径选择策略有待完善
2. 驾驶员交互：转向介入时的力矩反馈需要更自然
3. 计算效率：MPC算法在嵌入式平台上的优化（当前使用QP求解器）