1. 轨迹优化器在自动驾驶规划系统中的作用
轨迹优化器是自动驾驶规划系统的核心组件之一,它负责将前端路径搜索模块生成的粗略轨迹转化为平滑、安全且符合车辆动力学约束的最终轨迹。在DFTPAV系统中,traj_optimizer.cpp实现了基于MINCO(Minimum Control)的多段五次多项式轨迹优化算法。
1.1 前端与后端的协同工作
典型的自动驾驶规划系统采用分层架构:
- 前端(如KinoAstar):负责在复杂环境中快速搜索出一条可达的粗略路径,考虑基本的避障和全局路径约束
- 后端(如本优化器):对前端结果进行精细化处理,确保轨迹满足:
- 连续性(位置、速度、加速度的平滑过渡)
- 动力学可行性(不超过车辆物理极限)
- 舒适性(加速度、加加速度适度)
- 安全性(静态/动态障碍物避让)
1.2 MINCO轨迹表示的优势
MINCO(Minimum Control)轨迹使用分段五次多项式表示,具有以下特点:
- 每段轨迹由五次多项式描述,需要6个系数
- 段与段之间通过位置、速度、加速度连续约束连接
- 天然满足平滑性要求,只需优化控制点位置和时间分配
- 计算效率高,适合实时系统
数学形式上,每段轨迹可以表示为:
code复制σ(t) = c₅t⁵ + c₄t⁴ + c₃t³ + c₂t² + c₁t + c₀
其中系数c通过边界条件和优化目标共同确定。
需要模型API调用? 免费领10W Token,多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. 优化问题建模与求解
2.1 优化变量定义
从代码中可以看到,优化变量主要包括:
cpp复制std::vector<Eigen::MatrixXd> initInnerPts; // 路径控制点
Eigen::VectorXd initTs; // 时间分配
std::vector<std::vector<Eigen::MatrixXd>> hPoly_container; // 障碍物约束
std::vector<int> singuls; // 运动方向(前进/后退)
2.2 目标函数构成
优化目标由多个代价项加权组成:
- 平滑性代价(Jerk Cost):
cpp复制smoo_cost = jerkOpt_container[trajid].getTrajJerkCost();
最小化jerk(加加速度)使乘坐更舒适
- 时间代价:
cpp复制costT = RT.sum() * wei_time_;
鼓励更短的行程时间
- 障碍物惩罚:
cpp复制costs(0) += omg * step * wei_obs_ * violaPosPena;
保证轨迹与障碍物保持安全距离
- 动力学约束惩罚:
cpp复制// 速度约束
if (violaVel > 0.0) {
positiveSmoothedL1(violaVel, violaVelPena, violaVelPenaD);
costs(2) += omg * step * wei_feas_ * violaVelPena;
}
// 加速度约束
if (violaAcc > 0.0) {
positiveSmoothedL1(violaAcc, violaAccPena, violaAccPenaD);
costs(2) += omg * step * wei_feas_ * violaAccPena;
}
// 曲率约束
if(violaCurL > 0.0){
positiveSmoothedL1(violaCurL, violaCurPenaL, violaCurPenaDL);
costs(2) += omg * step * wei_feas_ * 10.0 * violaCurPenaL;
}
2.3 约束处理技巧
- 平滑L1惩罚函数:
cpp复制positiveSmoothedL1(violaPos, violaPosPena, violaPosPenaD);
相比硬约束,使用平滑惩罚函数:
- 保持问题可微性
- 允许暂时违反约束(数值稳定性更好)
- 通过权重调节约束严格程度
- 虚拟时间映射:
cpp复制void VirtualT2RealT(const EIGENVEC &VT, Eigen::VectorXd &RT)
将无约束的虚拟时间变量映射到实际时间,确保时间分配始终为正。
- 方向相关约束:
cpp复制if(singul_ > 0){
max_vel = max_forward_vel;
max_acc = max_forward_acc;
} else {
max_vel = max_backward_vel;
max_acc = max_backward_acc;
}
前进和后退允许不同的最大速度/加速度。
3. 实现细节解析
3.1 L-BFGS优化器配置
代码中使用L-BFGS算法进行优化,关键参数配置:
cpp复制lbfgs::lbfgs_parameter_t lbfgs_params;
lbfgs_params.mem_size = memsize; // 128
lbfgs_params.past = past; // 3
lbfgs_params.g_epsilon = 1.0e-16;
lbfgs_params.min_step = 1.0e-32;
lbfgs_params.delta = delta;
lbfgs_params.max_iterations = 12000;
L-BFGS适合此类问题因为:
- 只需要计算目标函数和梯度
- 内存效率高(不存储完整的Hessian矩阵)
- 对光滑问题收敛速度快
3.2 梯度计算细节
梯度计算是优化效率的关键,代码中实现了精确的解析梯度:
- 位置约束梯度:
cpp复制gradViolaPc = beta0 * outerNormal.transpose() +
beta1 * outerNormal.transpose() * (singul_ * temp_l_Bl * z_h0 - ego_R * le * dsigma.transpose() * vel2_reci);
- 速度约束梯度:
cpp复制gradViolaVc = 2.0 * beta1 * dsigma.transpose();
- 加速度约束梯度:
cpp复制gradViolaAc = 2.0 * beta1 * (z_h4 * ddsigma.transpose() - z_h4 * z_h4 * dsigma.transpose()) +
2.0 * beta2 * z_h4 * dsigma.transpose();
3.3 动态障碍物处理
对于动态障碍物,代码中通过预测轨迹与时变安全区域处理:
cpp复制if(surround_trajs_!=NULL){
costs(1) += dynamicObsGradCostP(omg,step,t + step * j,beta0,beta1,alpha,i,K,sigma,dsigma,ddsigma,ego_R,help_R,trajid,trajtime);
}
关键点:
- 考虑障碍物未来位置分布
- 建立随时间变化的安全区域
- 在对应时间点施加避障约束
4. 工程实践与调优经验
4.1 参数调优建议
- 权重选择:
cpp复制wei_obs_ = 1.0; // 障碍物权重
wei_feas_ = 1.0; // 动力学约束权重
wei_time_ = 1.0; // 时间权重
实际部署时需要根据场景调整:
- 城市道路:提高障碍物权重
- 高速公路:提高速度/舒适性权重
- 泊车场景:降低时间权重
- 约束阈值设置:
cpp复制max_forward_vel = 5.0; // 最大前进速度(m/s)
max_backward_vel = 2.0; // 最大后退速度(m/s)
max_forward_acc = 2.0; // 最大前进加速度(m/s²)
max_backward_acc = 1.0; // 最大后退加速度(m/s²)
max_cur = 0.3; // 最大曲率(1/m)
应根据具体车辆动力学参数调整。
4.2 常见问题排查
- 优化失败:
cpp复制if(final_cost>=50000.0){
ROS_ERROR("optimization fails! cost is too high!");
flag_success = false;
}
可能原因:
- 初始猜测太差(前端路径不可行)
- 约束冲突(如时间分配过紧)
- 数值不稳定(尝试调整平滑系数)
- 轨迹震荡:
- 检查jerk权重是否足够
- 确认是否满足加速度连续性约束
- 增加轨迹分段数提高灵活性
- 计算超时:
- 减少最大迭代次数
- 降低轨迹分辨率
- 使用更简单的障碍物表示
4.3 性能优化技巧
- 热启动:
- 使用上一次优化的结果作为初始猜测
- 对相似场景建立参数模板
- 并行计算:
- 多段轨迹的代价计算可并行化
- 利用SIMD指令加速矩阵运算
- 近似计算:
- 在远距离使用低分辨率检测
- 对非关键障碍物使用简化表示
5. 扩展与改进方向
5.1 高阶动力学模型
当前系统主要考虑:
- 速度/加速度约束
- 曲率约束
- 基本避障
可扩展更精细的车辆模型:
- 轮胎摩擦圆约束
- 横摆角速度限制
- 动力系统响应延迟
5.2 不确定环境处理
现有假设:
- 静态障碍物位置确定
- 动态障碍物预测轨迹准确
改进方向:
- 概率障碍物表示
- 鲁棒优化考虑预测误差
- 应急轨迹生成
5.3 学习增强方法
结合学习技术:
- 预测其他交通参与者意图
- 学习型代价函数设计
- 优化参数自动调节
在实际应用中,轨迹优化器的性能直接影响自动驾驶系统的舒适性和安全性。通过合理设计优化目标、精心调参并结合领域知识,可以实现在复杂场景下的高质量轨迹生成。
