自动驾驶轨迹优化实战iLQR算法在避障与车道保持中的应用当一辆自动驾驶汽车在城市道路上穿行时它需要实时处理复杂的道路环境——突然出现的行人、变道的车辆、弯曲的车道线。传统控制方法往往难以应对这种高维非线性优化问题而迭代线性二次调节器iLQR算法凭借其高效的数值优化特性正在成为解决这类问题的利器。本文将深入探讨iLQR在自动驾驶中的工程实现细节特别是如何处理各种形式的避障约束和车道保持要求。1. iLQR算法核心原理与实现iLQR算法的本质是通过迭代线性化和二次近似将复杂的非线性优化问题转化为一系列更容易求解的线性二次型问题。与标准的LQR不同iLQR不需要系统本身就是线性的也不要求代价函数必须是二次型——它通过局部线性化和二次近似来处理非线性系统。1.1 算法框架与数学表达考虑一个标准的离散时间系统x_{k1} f(x_k, u_k)其中x_k表示系统状态u_k表示控制输入。iLQR的目标是最小化如下代价函数J ∑[L_k(x_k,u_k)] L_N(x_N)算法的核心在于反向传播和前向传播两个阶段反向传播从终点开始反向计算价值函数的一阶和二阶导数前向传播利用计算得到的控制策略正向模拟系统行为# iLQR算法伪代码示例 def ilqr_solve(): # 初始化轨迹和控制序列 x_traj, u_traj initialize_trajectory() for i in range(max_iter): # 反向传播计算增益 K, d backward_pass(x_traj, u_traj) # 前向传播应用控制 x_new, u_new forward_pass(x_traj, u_traj, K, d) # 检查收敛条件 if check_convergence(x_traj, x_new): break x_traj, u_traj x_new, u_new return x_traj, u_traj1.2 关键改进正则化与线搜索原始iLQR算法可能面临数值不稳定的问题特别是当Hessian矩阵接近奇异时。工程实践中常用的改进措施包括正则化通过添加单位矩阵的倍数来保证Hessian可逆δu -(P_uu ρI)^(-1)(P_u P_uxδx)线搜索通过回溯法确定合适的步长保证代价函数单调下降提示实际应用中正则化参数ρ通常选择在1.5到2.0之间线搜索的收缩因子γ常取0.5。2. 避障约束的工程化建模方法自动驾驶中的避障约束需要平衡精确性和计算效率。不同形状的障碍物表示方法会直接影响优化问题的复杂度和求解难度。2.1 障碍物表示方法对比表示方法适用场景计算复杂度凸性保证点质量模型简单场景低是圆形表示低速场景中是椭圆表示结构化道路中是多边形表示复杂形状高取决于形状凸包表示非凸障碍很高是2.2 椭圆障碍物建模实践对于车辆这类有明显运动方向的障碍物椭圆表示法能够更好地反映其运动特性。一个典型的椭圆约束可以表示为1 - (x-x_o)^T RPR^T (x-x_o) 0其中Pdiag(1/a²,1/b²)R是旋转矩阵。% MATLAB示例椭圆障碍物约束计算 function g ellipse_constraint(x, xo, a, b, theta) R [cos(theta) -sin(theta); sin(theta) cos(theta)]; P diag([1/a^2, 1/b^2]); delta x(1:2) - xo; g 1 - delta * R * P * R * delta; end2.3 多边形障碍物的凸近似处理当使用多边形表示障碍物时可以通过凸分解或凸松弛技术来处理非凸约束。一种实用的方法是构建局部凸可行区域计算车辆轮廓点到障碍物的最小距离在参考点处线性化距离约束构建局部凸可行半空间注意这种方法需要保证初始解在可行区域内或者使用松弛技术处理不可行初始解。3. 车道保持约束的工程实现车道保持是自动驾驶的基本要求但将车辆约束在弯曲的车道线内会引入复杂的非凸约束。工程中常用的方法包括3.1 车道线表示方法分段线性近似将车道线离散为多段直线每段使用线性不等式约束实现简单但需要密集采样多项式拟合用3-5阶多项式拟合车道线连续光滑但难以处理复杂形状可能产生非凸可行域3.2 实用技巧动态参考系转换对于大曲率道路可以在Frenet坐标系下表述约束沿参考线建立曲线坐标系横向位置约束表示为d_min(s) ≤ d ≤ d_max(s)将约束转换回笛卡尔坐标系进行优化# Frenet到笛卡尔坐标转换示例 def frenet_to_cartesian(s, d, ref_path): x_ref, y_ref, theta_ref ref_path.query(s) x x_ref - d * np.sin(theta_ref) y y_ref d * np.cos(theta_ref) return x, y4. 约束处理的数值方法比较将约束融入iLQR框架主要有三种方法各有优缺点4.1 障碍函数法对数障碍函数形式b(g) - (1/t) * log(-g)特点保证严格可行需要可行初始解参数t需要谨慎调节4.2 增广拉格朗日法将约束转化为代价项J_aug J λc (μ/2)cc迭代更新乘子和惩罚系数λ ← λ μc μ ← φμ (φ1)4.3 数值性能对比方法初始解要求等式约束不等式约束数值稳定性障碍函数严格可行不支持支持边界处较差罚函数无要求支持支持权重大会病态增广拉格朗日宽松支持支持较好在实际项目中我们通常会根据问题特点组合使用这些方法。例如对硬安全约束使用障碍函数对软约束使用增广拉格朗日法。5. 工程实践中的调优经验经过多个自动驾驶项目的实践验证以下技巧能显著提升iLQR在轨迹优化中的表现5.1 代价函数设计要点控制量代价平滑加速度和转向指令L_u u R u跟踪代价平衡全局参考轨迹和局部优化L_x (x-x_ref) Q (x-x_ref)终端代价确保轨迹终点状态稳定L_N (x_N-x_goal) Q_N (x_N-x_goal)5.2 实时性优化技巧热启动使用上一帧的解初始化当前优化并行计算利用GPU加速矩阵运算稀疏性利用Hessian矩阵通常有特殊结构代码优化使用Eigen等线性代数库5.3 典型问题排查指南问题现象可能原因解决方案不收敛正则化不足增加ρ值震荡步长过大加强线搜索违反约束障碍函数参数不当调整t值数值溢出条件数过大尺度归一化在一次城市道路测试中我们发现车辆在急弯处偶尔会偏离车道。分析发现是多项式拟合的车道约束在曲率突变点不够精确改为分段线性表示后问题得到解决同时保持了实时性能。6. 前沿发展与挑战虽然iLQR在自动驾驶轨迹优化中表现出色但仍面临一些挑战非凸优化复杂场景下的全局最优性难以保证不确定性对感知预测误差的鲁棒性实时性超大规模问题的求解效率交互性多智能体间的博弈考虑最新的研究趋势包括结合深度学习学习更好的代价函数使用并行计算加速大规模问题求解开发考虑不确定性的随机iLQR变种在项目实践中我们经常需要在算法性能和工程实现复杂度之间做出权衡。iLQR提供了一个很好的平衡点——既有足够的表达能力处理复杂约束又能满足实时计算要求。随着计算硬件的进步和算法的持续优化这类基于优化的方法将在自动驾驶中发挥更大作用。