apollo LQR代码解读

### Apollo LQR源码分析与解释

#### 背景介绍
百度Apollo自动驾驶平台中的LQR（线性二次调节器）主要用于实现车辆的横向控制。该方法通过最小化给定性能指标来优化系统的响应特性，从而使得实际行驶轨迹尽可能贴近期望路径。

#### 整体架构概述
在Apollo框架下，LQR控制器作为Path Following模块的一部分被集成到纵向和横向上联合工作的整体控制系统之中。对于特定工况下的汽车运动建模采用简化假设：小角度转向以及近似匀速前进条件下建立状态空间表达式[^1]。

#### 数学模型构建
为了应用LQR理论设计控制器，需要先定义描述系统动态行为的状态方程：

\[ \dot{x}(t)=Ax(t)+Bu(t) \]

其中\( A\in R^{n\times n} \)， \( B\in R^{n\times m}\ )分别是系数矩阵；\( x(t)\in R^n\)表示由位置偏差、方向误差及其导数组成的状态向量；而\( u(t)\in R^m\)则代表作用于系统的输入变量即前轮转角指令。当涉及到离散时间域内的仿真时，则需进一步完成连续形式至离散化的转换过程[^2]。

// Discretization of continuous-time state-space model using zero-order hold (ZOH)
MatrixXd Ad = MatrixXd::Identity(n, n) + dt * Ac;
MatrixXd Bd = dt * Bc;

这里展示了如何利用零阶保持法(ZOH)来进行从连续型到离散类型的变换操作，在此过程中引入了采样周期`dt`这一重要参数。

#### 控制律推导
基于上述所获得的状态转移关系，可以构造代价函数并求得最优反馈增益K:

\[ J=\int_{0}^\infty(x^TQx+u^TRu)dt \]

最终得到如下所示的形式用于在线调整执行机构的动作幅度以达到预期效果：

\[ u=-Kx=-(R+B^TPB)^{-1}B^TPA*x \]

此处P满足代数黎卡提方程式(ALRE):

\[ PA+A^TP-PBR^{-1}B^TP+Q=0 \]

具体实现上会调用数值解算库解决ALRE问题进而获取所需权重因子集合[K]。

from scipy.linalg import solve_continuous_are as care

# Solve Algebraic Riccati Equation to get optimal gain matrix K
P = care(A, B, Q, R)
K = np.dot(np.dot(scipy.linalg.inv(R), B.T), P)

#### 输入输出处理
除了核心算法部分外，还需要注意数据预处理环节。这包括但不限于读取来自感知层的目标参考轨迹点集{xi,yi},提取局部片段形成多项式拟合曲线以便后续微分运算；同时也要考虑传感器噪声过滤和平滑过渡等问题确保估计精度可靠稳定。