--控制--

“控制”是最终的执行环节。它接收“规划”层输出的指令（例如：“以30km/h的速度，沿当前车道中心线行驶，并在5秒后开始轻柔地向右变道”），并将其转化为车辆底盘执行机构（如油门、刹车、方向盘）能够理解和执行的具体动作。

控制层输入输出

• ​输入​：

  • ​来自规划层​：
    • ​目标轨迹​：一系列包含位置、速度、加速度、方向、时间戳的路径点。
    • ​目标速度/加速度。
  • ​来自感知层和定位层​：
    • ​车辆自身状态​：通过CAN总线和IMU/GPS获取的实时数据，如当前车速、横摆角速度、加速度、方向盘转角、车轮转速等。
    • ​车辆定位信息​：车辆在当前环境中的精确位置和姿态。

• ​输出​：

  • ​油门开度​（或扭矩请求）
  • ​刹车压力​（或减速度请求）
  • ​方向盘转角​（或扭矩请求）
  • （对于传统车辆）档位控制指令

控制层核心任务

控制层的主要任务是将规划轨迹​（Trajectory）转化为控制命令​（Control Command）。这具体包括三个子任务：

1. ​纵向控制​：控制车辆的前后运动。即控制车速，通过协调油门（加速）​​ 和刹车（减速）​​ 来实现。​目标​是精准地跟随规划层给出的目标速度或目标加速度，并保持与前后车的安全距离。
2. ​横向控制​：控制车辆的左右运动。即控制行驶方向，通过转向系统​（方向盘）来实现。​目标​是精准地使车辆沿着规划层给出的期望路径（通常是一条光滑的曲线）行驶，保持车辆在车道中心，或在变道、转弯时平稳过渡。
3. ​纵向与横向的协同控制​：在复杂的驾驶场景中（如紧急避障、高速过弯），纵向和横向控制需要紧密配合，不能独立工作。例如，在急转弯时，控制系统需要先减速（纵向），再转向（横向），以确保车辆稳定性。

核心控制算法

1.PID控制(比例-积分-微分控制)

PID是比例（Proportional）、积分（Integral）、微分（Derivative）​​ 三者的缩写。它是一种基于误差（Error）​​ 反馈的控制律，其核心思想是根据“当前”、“过去”和“未来”的误差趋势来计算控制量，从而让被控对象（如汽车的速度、方向）精确、稳定地达到目标值。

想象一下你像开车定速巡航在70km/h：

1. ​当前误差（P）​​：你看到当前速度是60km/h，离70还差10。于是你本能地深踩油门。​误差越大，你踩油门的力度就越大。​​ 这就是比例（P）​​ 的作用。
2. ​累积误差（I）​​：你踩了油门，但速度卡在68km/h，始终差2km/h上不去。于是你随着时间的推移，慢慢地、不知不觉地再把油门踩深一点，直到误差完全消除。​这个“慢慢弥补微小误差”的过程，就是积分（I）​​ 的作用。
3. ​误差趋势（D）​​：当你深踩油门后，车速表指针快速地向70km/h逼近。如果你发现指针上升的速度太快，眼看就要冲过70了，你会提前松一点油门，让速度平稳地达到目标，而不是 overshoot（超调）然后来回振荡。​这个“根据变化趋势提前采取措施”的过程，就是微分（D）​​ 的作用。

PID控制器的输出 u(t) 由三部分组成：​u(t) = P + I + D = Kₚ * e(t) + Kᵢ * ∫e(τ)dτ + K_d * de(t)/dt

• e(t)​​ = ​误差​ = ​目标值（Setpoint）​​ - ​当前测量值（Process Variable）​​
  • 例如：目标速度70km/h，当前速度60km/h，则 e(t) = 10。
• ​Kₚ​：比例增益系数。​调大它，响应变快，但容易 overshoot 和振荡。​​
• ​Kᵢ​：积分增益系数。​调大它，能更快地消除静差，但可能使系统稳定性下降。​​
• ​K_d​：微分增益系数。​调大它，能抑制 overshoot，增加系统稳定性，但对外部噪声更敏感。

1.比例控制(P) - 处理“现在”

• 公式​： P = Kₚ * e(t)
• ​作用​：产生与当前误差 e(t) 成比例的控制作用。误差越大，控制力越强。
• ​缺点​：​存在稳态误差​。例如，由于风阻或上坡，汽车可能需要持续提供20%的油门才能维持70km/h。纯P控制器在速度达到70时误差为0，油门也会归0，导致速度又会掉下来，永远无法精确达到并维持目标。就像你无法用一根没有弹性的绳子拉着小车匀速前进一样。

2.积分控制(I) - 处理“过去”

• 公式​： I = Kᵢ * ∫e(τ)dτ （即误差随时间的累积和）
• ​作用​：​消除稳态误差。只要还有误差（即使很小），积分项就会不断累积增大，从而提供足够的控制力来完全消除误差。其核心作用是提供稳态控制力，以彻底消除稳态误差。举个例子，如果我们期望的速度是70km/h，当达到70km/h时e(t)=0，这时比例控制P=0，油门为0%，但是实际上还会存在各种阻力，这就会使小车速度再次减慢而不是稳定下来，积分I就可以让小车保持恒定的目标速度，假设你的车要保持70km/h的速度定速巡航需要恒定20%油门，速度从0到70km/h这个过程中比例控制P随着小车速度的提升不断减小，而积分项却在不停累加，可以理解为油门控制权逐渐从P转交给I，直到最后P=0，这时油门完全由积分项控制，输出20%维持小车定速巡航。​
• ​缺点​：​积分饱和​。如果积分项已经有了一个很大的累积值，那么即使速度已经达到甚至超过了期望值，它仍然会继续“推动”车辆加速，导致严重的超调（Overshoot）和振荡。我们以一个上坡场景为例，目标速度70km/h，由于坡度大，即使油门给到100%，车速只能维持在65 km/h，但是积分项 I 在每个控制周期都在累加这个正误差，变得非常巨大（比如累积到了 I = 30）。这个巨大的 I 值意味着“控制器认为需要持续提供极大的油门才能克服这个持续的误差”，当坡道结束，车辆驶入平路时，当速度达到70 km/h时，​误差 e(t) 第一次变为0，但是积分项 I 仍然保持着之前累积的巨大数值，因此，车辆会继续加速，冲过70 km/h的目标。巨大的正积分项 I 需要很长时间的负误差累加才能被“消化”掉。所以必须抗积分饱和，下面会介绍。

3.微分控制(D) - 预测“未来”

• ​公式​： D = K_d * de(t)/dt （即误差的变化率）
• ​作用​：​阻尼、抑制振荡。它根据误差变化的趋势（斜率）进行控制。误差变化越快，微分项产生的反方向控制力就越大，相当于一个“阻尼器”，使系统更平稳地接近目标，减少超调。
• ​缺点​：​对噪声敏感。传感器测量值中的高频噪声会使 de(t)/dt 计算值产生剧烈跳变（放大噪声），从而导致控制输出剧烈抖动。因此，在实际使用中通常需要对测量值进行滤波。

积分项I容易出现饱和，所以必须要抗积分饱和，其核心思想是：​在适当的时候，停止积分项的盲目累加，防止它变得过大。​最常用的方法是clamping（钳位法），这种方法主要通过设定输出限幅来明确控制输出的物理极限，例如油门输出 u(t) 的范围只能是 [0, 100]（0%到100%）。如果计算出的总输出 u(t) 已经达到上限（100%）​，​并且误差 e(t) 仍然为正​（即还需要更大的输出才能减少误差），那么就停止对积分项进行累加，同理，如果输出达到下限（0%），且误差为负，也停止积分累加。

由数学知识可知，在积分控制中和微分控制中要求函数e(t)是一个连续函数，在理论分析和连续时间系统中，e(t) 确实是一个连续函数。但在实际的数字控制（如自动驾驶、机器人）中，e(t) 是在离散时间点上采样得到的一个个确定的数值序列。比如所有的现代控制系统（包括自动驾驶汽车）都是由微处理器（CPU）实现的，CPU以固定的周期运行，这意味着它只能在离散的时间点上做计算。这样e(t) 变成了一个数值序列​，控制系统以固定的采样周期（Ts）​（例如，每10毫秒一次）工作，因此，e(t) 在计算机里实际上是一个数列：{e₀, e₁, e₂, ..., eₖ}，其中 k 是第 k 个采样时刻，那如何对这个对e(t)进行积分(即计算累积误差)和微分(即计算变化趋势)？

• 积分：计算机无法计算曲线下无限的面积，但它可以计算面积的近似值，最常用的方法是矩形法（累加和）​。即I ≈ Kᵢ * Ts * (e₀ + e₁ + e₂ + ... + eₖ)，也就是说，​积分项变成了所有历史误差值的累加和，再乘以采样周期 Ts。每次新的控制周期，只需要做一次加法：I_current = I_previous + (K_i * Ts * e_k)
• 微分：计算机无法计算瞬时的变化率，但它可以计算平均变化率。D ≈ K_d * (eₖ - eₖ₋₁) / Ts，也就是说，​微分项等于本次误差减去上一次误差，再除以采样时间。这个结果 (eₖ - eₖ₋₁) / Ts 就是最近一段时间内误差变化的平均速率，用它来近似瞬时导数。

2.LQR(线性二次调节器)

与PID的经验调参不同，LQR基于数学模型，通过求解优化问题来得到理论上最优的控制策略。LQR的核心思想是​成本函数最小化。找到一个最佳的控制策略，使得系统在从初始状态调整到目标状态的过程中，总的“成本”最低。​​这个“成本”是一个权衡，包含​状态成本和控制成本​，状态成本指系统状态（如位置、速度误差）偏离理想值的代价；​控制成本​指使用执行器（如油门、方向盘）的代价。LQR就是在寻找一个平衡点：既不能让车跑得太偏离轨迹（状态成本高），也不能让驾驶员感觉像在坐过山车（控制成本高）。它追求的是“恰到好处”的平滑控制。​

要使用LQR必须提供以下四样，它们共同定义了整个控制问题：这里先列出来，下面会进行具体解释

1. 系统动力学模型 (The Plant Model)​​： A 矩阵和 B 矩阵。

   • ​这是最重要的部分。它用数学方程描述了您的车辆（或任何被控对象）是如何运动的。即，​状态​ 和 ​控制输入​ 之间存在的物理关系。

2. ​状态代价矩阵 (State Cost Matrix) - Q​：

   • 这是一个对角矩阵，定义了您有多关心每个状态变量与目标值的偏差。
   • ​规则​：Q 矩阵中对角线上的值越大，表示您越希望对应的状态变量快速、精确地收敛到零。您通过它来告诉LQR：“这个状态对我来说非常重要，不能有偏差！”

3. ​控制代价矩阵 (Control Cost Matrix) - R：

   • 这也是一个对角矩阵，定义了有多“舍不得”使用您的控制输入（如方向盘、油门）。
   • ​规则​：R 矩阵中的值越大，表示越希望控制动作平滑、节能、温和。通过它来告诉LQR：“省着点用方向盘和油门，别太猛！”

4. ​最终目标 (The Target)​​：

   • 对于标准的调节器 (Regulator)​​ 问题，目标始终是将所有状态稳定在零点。
   • 这意味着，您需要将您的物理控制目标（如“保持在车道中心”）​转化为状态空间中的零值

我们以方向盘转向来理解一下LQR的作用和过程：首先LQR需要知道它的目标，对于标准的调节器 (Regulator)​​ 问题，目标始终是将所有状态稳定在零点，这个状态指的是什么呢？稳定在零点意味着什么？我们以车辆横向控制模型为例，我们希望车辆沿车道的中心线行驶(或者沿规划模块给的轨迹行驶)，但是我们如何知道车辆此时的状态是否满足目标或者说与目标差异是多少？为此我们定义状态变量x来说明当前车辆与目标的差异，如下所示，我们定义的状态变量就是：x = [e, ė, Δψ, Δψ̇ ]ᵀ

• ​横向误差 e​：车辆质心到车道中心线的垂直距离(或者是到规划轨迹中心线的垂直距离)。这是最重要的状态。e = 0 意味着车在中心。
• ​横向误差变化率 ė​：e 的变化速度，近似等于车辆横向速度。
• ​航向误差 Δψ​：车辆当前朝向与车道中心线切线方向的夹角(或者是与规划轨迹中心线切线方向的夹角)。Δψ = 0 意味着车头正对车道方向。
• ​航向误差变化率 Δψ̇*：Δψ 的变化速度，其实就是车辆的横摆角速度

如果我们希望车辆沿着规划轨迹行驶，那最理想的情况就是状态变量x = [e, ė, Δψ, Δψ̇ ]ᵀ ​等于 [0, 0, 0, 0]ᵀ，这也是LQR的目标，即上面说到的始终是将所有状态稳定在零点。为了实现这个目标，作为驾驶员我们的唯一输入就是方向盘，对于LQR实现车辆横向控制是同样的，通过控制输入u(u=[δ]，前轮转向角，这是我们唯一能直接控制的东西)来控制车辆实现目标。但是LQR是不知道输入u与输出的车辆状态是如何对应的，也就是不知道当前轮转动一个角度 δ 时，四个状态变量 (e, ė, Δψ, Δψ̇) 会如何变化。为此我们需要建立状态方程(线性模型)，其标准形式是ẋ(t) = A * x(t) + B * u(t)。状态方程源于状态空间表示法，其核心思想是：​一个系统的“未来”只取决于它的“现在”，而不依赖于它的“过去”而x(t)就是描述系统“现在”的最小信息集合。知道了 x(t)，你就完全知道了系统在 t 时刻的全部情况，状态方程ẋ= Ax + Bu描述的就是“现在”如何决定“未来”。它回答了：“在已知当前状态 x(t) 和当前输入 u(t) 的情况下，系统在下一个瞬间会如何变化？”

• x(t)——状态向量：一个列向量，维度n x 1(n 是状态变量的个数)。x = [e, ė, Δψ, Δψ̇]ᵀ，此x状态向量维度为4，n=4​
• u(t)——控制输入向量：一个列向量，维度m x 1(m 是控制输入的个数)，包含所有可以由控制器直接操纵的变量。在横向控制中u = [δ]ᵀ，此列向量为一维，m=1
• A——状态矩阵：一个 n × n 的常数矩阵。它描述了各个状态变量之间如何相互影响，即系统的内部动力学。A矩阵的物理意义是定义了在没有外部控制输入 (u = 0) 的情况下，系统状态会如何自行演化（例如，如何因为惯性、阻尼等自然规律而变化）。例如，当前的横向速度 ė 会如何影响横向加速度 dė/dt（这可能反映了车辆的阻尼特性）；当前的航向角 Δψ 会如何影响横向误差 e 的变化率 de/dt（这反映了几何关系）等等
• B——输入矩阵：一个 n × m 的常数矩阵。它描述了控制输入 u 如何有效地推动或改变每个状态变量。B 矩阵建立了控制指令到系统状态的桥梁，它量化了“打方向盘到底有多大的效果”。例如，B 矩阵中的某个元素明确指出了单位转向角 δ 会产生多大的横摆角加速度 d(Δψ̇)/dt。这个值取决于车辆参数，如轴距、轮胎侧偏刚度、车速等。
• ẋ(t)——状态向量的导数：ẋ = [de/dt, dė/dt, d(Δψ)/dt, d(Δψ̇)/dt]ᵀ  状态向量 x 对时间 t 的导数。它直接表示每个状态变量的变化率。在上面介绍时我们说到“状态方程ẋ= Ax + Bu描述的就是“现在”如何决定“未来””，ẋ(t)只是 t 时刻x(t)的瞬时变化率，它本身并不直接代表未来，但是​它是构建未来的最基本、最核心的“原材料”​，换句话说未来就是由这无数个连续的“瞬时变化”积分而成的。假设把系统在 t 时刻的状态 x(t) 想象成一辆车在 t 时刻的精确经纬度坐标，那么，状态方程计算出的 ẋ(t)就是此时此刻，给车辆下达的一个精确的“瞬时驾驶指令”​(请以 ​X米/秒​ 的速度向北偏东 ​Y度​ 的方向行驶)​，这个指令 ẋ(t)) 只在这一瞬间 (t) 有效。到了下一个瞬间 (t+dt)，我们会根据车辆新的位置，再计算一个新的指令。t时刻车辆位于 x(t)。我们根据状态方程计算出瞬时指令​ẋ(t)，在t+dt时刻车辆执行了这个指令，它移动了一小段距离，这时车辆的新位置就是 “新位置 ≈ 旧位置 + 速度 × 时间 ≈ x(t) + ẋ(t) * dt”，这个 x(t+dt) 就是未来。ẋ(t) 和未来的关系，就如同“迈出一步”和“走完全程”的关系，ẋ(t) 是 ​​“当前这一步要怎么迈”​​ 的指令，未来 (x(t+Δt)) 是 ​​“从起点开始，一步步按指令走下来的结果”​。状态方程 ẋ = Ax + Bu 的伟大之处在于，它提供了一个万能公式。在任何时间、任何地点​（任何状态 x）、任何操作​（任何输入 u）下，它都能立刻给出一个正确的“下一步指令”。

现在我们再来深入理解一下ẋ= Ax + Bu方程：这个等式之所以成立，不是因为它是一个假设，而是因为它是对物理世界因果规律的高度概括和数学描述，简单来说这个状态方程是牛顿第二定律的扩展形式，几乎所有经典物理系统的动力学都天然地遵循这个形式。下面举两个由牛顿第二定律推到状态方程的例子

• 以最简单的推滑块为例，状态x = [v](我们只关心滑块的速度v)，控制输入 u = [F](我们施加的力 F)，由牛顿第二定律决得F = m * a，我们知道加速度是速度的导数a = dv/dt，所以m * dv/dt = F 整理一下得dv/dt = (1/m) * F，现在，把它和状态方程 ẋ = Ax + Bu 对比，ẋ就是 dv/dt，A 是一个 1x1 的矩阵。这里没有只与速度本身有关的力（比如没有阻尼），所以 A = [0]，即Ax = 0 * v = 0，B 是一个 1x1 的矩阵，B = [1/m]，Bu = [1/m] * [F] = F/m，所以，​牛顿第二定律直接写出了状态方程​：dv/dt = 0 + (1/m)*F。
• 再看一个复杂点的例子：弹簧-质量-阻尼系统。滑块连接着一个弹簧和一个阻尼器。合力F_total = F_applied (你推的力) - k*x (弹簧力) - c*v (阻尼力)，由牛顿第二定律得m * dv/dt = F_applied - k*x - c*v，整理得dv/dt = -(k/m)*x - (c/m)*v + (1/m)*F_applied。现在我们有两个状态变量：位置 x 和速度 v，令状态向量 x = [x, v]ᵀ。上面的方程可以写成如下矩阵形式，即ẋ= Ax + Bu 的形式。​：

当我们写下 ẋ= Ax + Bu 时，我们是在做出一个非常重要的声明​系统状态的变化率 ẋ，来源于两个独立且可叠加的贡献：一项是系统“自力更生”的变化 (Ax)​​，这是由系统当前的内部状态​ x 自身驱动的变化。​另一项是外部“强加”的变化 (Bu)​​，这是由外部控制输入​ u 强加给系统的变化。我发现一个很好的例子来理解这个公式：小船航行，想象一艘小船在河里航行，它的速度矢量（即位置的变化率 ẋ）由两个独立的部分叠加而成，水流本身流速得影响和小船发动机的影响，河水本身有一个流速和方向，即使你关闭发动机不施加任何控制 (u=0)，小船也会被水流推着走，这个速度只取决于小船现在的位置​ x（因为不同位置的流速可能不同），矩阵 A 就描述了“位置”与“被水流推动的速度”之间的关系；操控发动机和舵会产生一个推力，这个推力的效果由 B 矩阵描述（比如发动机的功率和效率），你产生的推力 u，通过 B 的转换，成为了一个速度矢量 Bu。现在，小船在下一瞬间的瞬时速度 ()​​ 就是水流速度 (Ax) 和你发动机产生的速度 (Bu) 的矢量叠加！​

现在已经有了状态方程，相当于你拥有了一个在计算机里完全模拟真实车辆物理规律的数字化模型。你可以在这个模型上做各种测试。接下来你需要给控制器提供矩阵Q和矩阵R(用于成本函数的计算)，Q和R用来告诉LQR什么是“好”、什么是“坏”。

• Q矩阵——状态代价权重（“我们有多在乎偏离目标？”）：Q 是一个 n × n 的对角矩阵（非对角线元素通常为0），n 是状态变量个数。它定义了你有多希望每个状态变量稳定在零值。Q 矩阵中的每个对角元素值，是对应状态变量误差的惩罚权重，值越大表示你越不能容忍这个状态的偏差。控制器会不惜一切代价（甚至使用更猛烈的控制）​​ 来快速消除这个误差，值越小表示你不太关心这个状态是否有微小偏差。控制器愿意在这个状态上妥协，以换取其他好处（比如更平滑的控制），值为0表示完全不在乎这个状态。例如，对于x = [e, ė, Δψ, Δψ̇ ]ᵀ，Q = diag([100, 2, 50, 0.1])，Q的100对应e(横向误差)权重很高，这意味着“精准地保持在车道中心是最高优先级；Q的2对应ė(横向速度)权重低，这意味着车是否正在横向移动，我不太关心，只要最终位置是对的就行；Q的50对应Δψ （航向误差)权重高，这意味车头方向正也很重要。
• R矩阵——控制代价权重（“我们有多舍不得用力？”）：R 是一个 m × m 的对角矩阵，m 是控制输入个数。它定义了你使用控制输入的“成本”或“代价”。R 矩阵中的每个对角元素值，是对应控制输入活动的惩罚权重，值越大表示你越希望减少控制动作。控制器会变得非常“懒惰”和“温和”，会倾向于使用最小的、最平滑的控制输入，但这可能会导致状态误差收敛变慢；值越小表示你不在乎控制动作的幅度。控制器会变得非常“激进”，会猛烈地打方向盘来快速纠正误差，这可能导致车辆晃动，乘坐不适；值为0表示使用控制输入完全没有成本（理论上，现实中不可能）。比如R[10]是一个很大的值意味着省着点用方向盘，控制器会非常温和，转向动作极其平滑，但车辆对误差的反应会变慢，可能总是微微偏离中心。R[0.1]是一个很小的值，意味着放心大胆地去打方向盘吧，控制器会非常激进，快速消除误差，但可能让乘客感觉像在开赛车，左右晃动。

现在已经有了状态方程和矩阵Q、R，剩下的步骤非常直接，你只需要调用LQR求解器(比如python中直接使用以下代码片段)

# Python (using SciPy)
from scipy import linalg
K, S, E = linalg.lqr(A, B, Q, R)

之后，函数会返回几个值，我们唯一需要关心的是K：最优状态反馈增益矩阵。它是一个 m × n 的矩阵，在我们的例子中 (m=1个输入，n=4个状态)，K 是一个 1 × 4 的矩阵，形如：K = [k1, k2, k3, k4]。这个矩阵包含了所有关于如何最优控制系统的信息。有了K以后在每个控制周期只需要执行以下步骤：

1. ​获取状态​：从传感器（摄像头、IMU、GPS等）读取数据，并计算出当前的状态向量 x。

   • 例如：x_current = [e, ė , Δψ, Δψ̇ ]ᵀ = [0.15, 0.02, 1.2, -0.5]ᵀ

2. ​计算控制指令​：执行那个神奇的公式。
   u = -K * x_current

   • 这只是一个简单的矩阵乘法。
   • 在我们的例子中：δ = - [k1, k2, k3, k4] * [0.15, 0.02, 1.2, -0.5]ᵀ = - (k1 * 0.15 + k2 * 0.02 + k3 * 1.2 + k4*(-0.5))

3. ​执行指令​：将计算出的控制量 u（即转向角 δ）发送给车辆的线控转向执行机构。

4. ​循环​：等待下一个控制周期，回到步骤1。

K 矩阵中的每个元素 k1, k2, k3, k4 都有一个直观的物理意义：

• ​**k1​：​横向误差补偿增益**。它决定了系统对位置偏差 e 的敏感度。k1 越大，对于同样的偏差，方向盘打得越多。
• ​**k2​：​阻尼增益**。它根据横向速度 ė  提供阻尼效应。如果车正在快速偏离车道 (ė 很大)，k2 会产生一个更强的纠正力矩来抑制这个趋势，防止超调。
• ​**k3​：​航向误差补偿增益**。它决定了对方向偏差 Δψ 的修正力度。
• ​**k4​：​横摆阻尼增益**。它根据横摆角速度 Δψ̇  提供阻尼，让车辆的转向动作更平滑，防止“画龙”

总结整个过程步骤(以车辆横向控制为例)：

1. ​建模​：根据车辆动力学，推导出状态空间模型 ẋ = Ax + Bu，其中 x = [e, ė, Δψ, Δψ̇ ]ᵀ, u = [δ]。
2. ​设计​：定义价值观。选择 Q = diag([100, 2, 50, 0.1]) 表示更看重位置和航向精度，选择 R = [1] 表示对转向动作的消耗持中等态度。
3. ​计算​：在计算机上离线调用 lqr(A, B, Q, R) 函数，得到增益矩阵 K = [k1, k2, k3, k4]。在车辆出厂或控制器部署之前，工程师在开发环境中（比如用MATLAB或Python）​只需要调用一次 lqr(A, B, Q, R) 函数。这个函数内部会进行复杂的矩阵运算和解Riccati方程等“大量计算”。计算完成后，得到的结果——增益矩阵 K——是一个常数矩阵。这个矩阵被直接硬编码（Hardcode）或存储在自动驾驶系统的内存中。
4. ​部署​：将计算好的常数矩阵 K 烧录到车载控制器的内存中。
5. ​运行​：控制器在每个循环中执行：
   • ​感知​：通过传感器测量，计算得到当前 x。
   • ​决策​：计算 δ = -K * x。这就是全部的“决策”过程。
   • ​执行​：将转向指令 δ 发送给方向盘电机

PID与LQR的比较

调整参数时最直观的感受：

• 调PID参数​：像在调试一个黑盒机器。你转动 Kp, Ki, Kd 三个旋钮，听着机器的声音，看着它的输出曲线，凭经验和试错来找到一组“听起来不错”的设置。你不知道每个旋钮精确影响了机器内部的哪个物理环节，你只知道“这个旋钮往右拧，响应会变快，但也更容易抖”。
• ​调LQR参数​：像在为一位世界级的工程师制定设计规范。你通过 Q 和 R 告诉他：“这个项目（控制系统），我最看重的是精度（Q 要大），但预算有限，不能太费能源（R 也要大）。” 这位工程师会根据你给的规范和他掌握的完整蓝图（系统模型 A, B），计算出理论上绝对最优的设计方案（增益矩阵 K）。

LQR的四大核心优势：

1. 参数物理意义清晰，调试更直观：

   1.  ​PID​：三个参数 Kp, Ki, Kd 交织在一起，共同影响系统的性能（如超调、稳定时间）。调整一个参数会同时影响多个性能指标，耦合性很强，调试如同“猜谜”。

   2. ​LQR​：参数 Q 和 R 的物理意义完全解耦，极其直观。

      • ​Q​：直接对应状态变量。你想让哪个状态（如横向误差 e）收敛得快，就把 Q 矩阵中对应那个状态的对角线元素调大。它的效果是局部的、可预测的。
      • ​R​：直接对应控制输入。你希望哪个执行器（如方向盘 δ）动作平滑、省能量，就把 R 矩阵中对应的值调大
2. 适用于多输入多输出系统：
   1. ​PID​：本质是单输入单输出的。要控制一个四状态系统（如 [e, ė, Δψ, Δψ̇]），你需要设计四个独立的PID控制器​（一个用于 e，一个用于 Δψ...），这些控制器会互相干扰、打架，整定起来是噩梦。
   2. ​LQR​：天生就是为多变量系统设计的。它一次性处理所有状态 [e, ė, Δψ, Δψ̇] 和所有控制输入，​自动地计算出它们之间的最优协调关系。你得到的增益矩阵 K 已经完美地处理了所有耦合关系。

3. 基于模型，性能更优，有理论保证：​​​​​​​

   1. PID​：是反应式的。它只根据当前的误差、过去的误差累积和未来的误差趋势来反应。它不了解系统本身的特性（比如“打多大方向盘，车会转多快”）。
   2. ​LQR​：是预测式的。因为它拥有系统模型 A, B，它知道控制输入会产生什么后果。因此，LQR计算出的控制策略是前瞻性的。它能提前“看到”过于激进的控制会导致超调，从而主动地“收着点”控制
4. 自动处理复杂控制关系：

   1. PID：控制器输出的是 u = Kp*e + Ki*∫e + Kd*ė。它只用了误差 e 及其导数和积分。

   2. LQR：控制律是 u = -K * x = - (k1*e + k2*ė + k3*Δψ + k4*r)。它不仅包含了误差 e 和其变化率 ė，还自动地包含了航向误差 Δψ 和横摆角速度 r。这意味着LQR天然地知道：即使横向误差 e 还很小，但如果车头方向不对 (Δψ 很大)，也应该提前转向。即使横向误差 e 很小，但如果车辆正在快速旋转 (r 很大)，也应该施加反向转向来阻尼这个运动，防止 overshoot。

PID 通过 ​**Ki**​ 项来引入积分作用，以消除稳态误差。而在LRQ的Q矩阵中似乎没有直接对应积分项的权重，那是否会产生稳态误差呢？实际上是不会的，这是因为LQR的设计天然地倾向于产生一个高增益的控制器，当您通过 Q 矩阵强烈地惩罚某个状态误差（比如 e）时，LQR求解器会努力寻找一个策略，使得这个误差在稳态时必须为零，否则整个成本函数 J 会趋于无穷大（因为误差持续存在，其平方的积分会无限大），换句话说，LQR通过其优化过程，自动地创造出了积分控制的效果，而不需要显式地定义一个积分项。通过LQR求出的K其效果等价于一个精心设计的、包含了比例、积分、微分（甚至更多）作用的复杂控制器。k1 项并不简单等同于​ Kp，它是 Kp、Ki、Kd 以及其他状态补偿项混合后的最优解。

3.MPC(模型预测控制)

在文章规划最后已介绍过

--规划---CSDN博客