自动驾驶：控制算法概述
常见控制算法
PID算法
PID（Proportional-Integral-Derivative）控制是一种经典的反馈控制算法，通常用于稳定性和响应速度要求不是特别高的控制系统。PID控制基于三个主要组成部分：比例项（P），积分项（I），和微分项（D）。比例项根据当前误差来产生控制输出，积分项处理历史误差的总和，微分项处理误差变化的速度。PID控制通常通过多次试验、调整参数来实现，通过对PID参数的调节进而改善系统的性能，如稳定性、超调和振荡等。
PID控制算法的控制输出计算如下：
u ( t ) = K p ⋅ e ( t ) + K i ⋅ ∫ 0 t e ( τ ) d τ + K d ⋅ d e ( t ) d t u(t) = K_p\cdot e(t) + K_i \cdot \int_0^t e(\tau)d\tau + K_d \cdot \frac{de(t)}{dt} u(t)=Kp⋅e(t)+Ki⋅∫0te(τ)dτ+Kd⋅dtde(t)
其中， u ( t ) u(t) u(t)是控制输出， K p K_p Kp、 K i K_i Ki和 K d K_d Kd分别是比例、积分和微分系数， e ( t ) e(t) e(t)是当前误差， d e ( t ) d t \frac{de(t)}{dt} dtde(t)是误差的变化率。
- 比例项（P - Proportional）：
比例项与当前误差成正比，用来调整控制输出。当前误差是期望值与实际值之间的差值。
比例项的作用是减小误差，使系统更快地接近期望值。
比例系数（通常表示为 K p K_p Kp）用来控制比例项的影响，过大的 K p K_p Kp可能引起超调，而过小的 K p K_p Kp可能导致响应过慢。 - 积分项（I - Integral）：
积分项用来处理历史误差的总和。它消除系统的稳态误差，确保系统最终稳定在期望值附近。
积分项的作用是减小稳态误差，特别是在系统存在系统饱和或其他非线性问题时。
积分系数（通常表示为 K i K_i Ki）控制积分项的影响，过大的 K i K_i Ki可能导致系统不稳定，而过小的 K i K_i Ki可能使响应过慢。 - 微分项（D - Derivative）：
微分项用来处理误差变化的速度，预测未来误差趋势。它有助于减小振荡和改善系统的响应速度。
微分项的作用是减小系统的振荡，尤其在系统响应速度需要控制的情况下。
微分系数（通常表示为 K d K_d Kd）控制微分项的影响，过大的 K d K_d Kd可能引起噪声敏感性，而过小的 K d K_d Kd可能无法改善振荡问题。
PID控制对于线性系统和某些非线性系统非常有效，但不适用于所有类型的系统，而且它需要多次试验不断调參才能满足系统性能要求。对于一些复杂系统，更高级控制策略可能更为合适。
LQR算法
LQR（Linear Quadratic Regulator）控制算法是一种用于线性系统的最优控制方法，旨在寻找一种最优的控制策略，以最小化一个线性二次性能指标。LQR控制输入是系统状态的线性组合以及反馈信号，输出控制信号。它可以用于稳态和动态控制以及应用于多变量系统，通过权重矩阵调整性能指标，以满足特定需求。LQR的设计需要系统的状态空间模型和性能指标，通常使用状态反馈来调整系统行为，并可以提供最优的控制输入，使系统稳定并满足性能要求。
- 系统模型
LQR控制的第一步是建立线性时间不变（LTI）系统的状态空间模型。这个模型通常由状态方程和输出方程组成。
状态方程描述了系统状态（状态向量）如何随时间演变，通常用如下形式表示：
x ˙ ( t ) = A x ( t ) + B u ( t ) \dot{x}(t) = Ax(t) + Bu(t) x˙(t)=Ax(t)+Bu(t)
输出方程描述了系统状态和控制输入之间的关系，通常用如下形式表示：
y ( t ) = C x ( t ) + D u ( t ) y(t) = Cx(t) + Du(t) y(t)=Cx(t)+Du(t)
其中， x ( t ) x(t) x(t)是状态向量， x ˙ ( t ) \dot{x}(t) x˙(t)是状态向量的导数， u ( t ) u(t) u(t)是控制输入， y ( t ) y(t) y(t)是系统的输出， A A A、 B B B、 C C C和 D D D是系统的矩阵参数。 - 性能指标
LQR控制的目标是最小化一个线性二次性能指标，通常称为LQR性能指标或成本函数。
LQR性能指标的一般形式如下：
J = ∫ 0 ∞ ( x T Q x + u T R u ) d t J = \int_0^\infty \left( x^T Q x + u^T R u \right) dt J=∫0∞(xTQx+uTRu)dt
其中， Q Q Q是状态权重矩阵， R R R是控制输入权重矩阵。这些权重矩阵可以用来调整性能指标的权重，以满足特定的性能要求。 - 最有控制策略
LQR控制通过求解Riccati方程来找到最优的控制策略。Riccati方程的一般形式如下：
A T P + P A − ( P B ) R − 1 ( B T P ) + Q = 0 A^T P + PA - (PB)R^{-1}(B^T P) + Q = 0 ATP+PA−(PB)R−1(BTP)+Q=0
这里， P P P是状态反馈矩阵，包含了最优控制策略的信息。 R R R和 Q Q Q是性能指标中的权重矩阵， A A A和 B B B是系统的状态方程参数。
一旦求解了Riccati方程，最优的状态反馈矩阵 P P P就可以用来计算最优的控制输入，通常如下：
u ( t ) = − R − 1 B T P x ( t ) u(t) = -R^{-1}B^T P x(t) u(t)=−R−1BTPx(t) - 闭环控制
一旦找到了最优的状态反馈矩阵PP，可以将其应用于系统，以实现闭环控制。
最优的控制输入可以通过 u ( t ) = − R − 1 B T P x ( t ) u(t) = -R^{-1}B^T P x(t) u(t)=−R−1BTPx(t)计算，并用于调整系统状态，使系统的性能指标最小化。
然而，LQR控制也有一些限制，主要是它要求系统是线性的，且系统参数必须是已知的。对于非线性系统或具有参数不确定性的系统，可能需要使用其他控制策略，如模型预测控制（MPC）。
MPC算法
MPC（Model Predictive Control）控制算法是一种先进的模型预测控制策略，通常用于复杂、非线性、时变和多变量系统。MPC的核心思想是在每个时刻基于系统数学模型对未来的系统行为进行预测，并通过优化来选择最优的控制输入序列，以满足性能和约束要求。MPC允许系统在考虑约束条件下实时调整控制策略，以满足性能和操作限制，它处理复杂系统和需要考虑多种约束的情况下非常有效。
- 系统模型：
MPC的核心是系统的动态模型，这个模型通常以状态空间形式表示，包括状态方程和输出方程。
状态方程描述了系统状态如何随时间演变，通常表示为：
x ( k + 1 ) = f ( x ( k ) , u ( k ) ) x(k+1) = f(x(k), u(k)) x(k+1)=f(x(k),u(k))
输出方程描述了系统状态和控制输入之间的关系，通常表示为：
y ( k ) = g ( x ( k ) ) y(k) = g(x(k)) y(k)=g(x(k))
在MPC中，系统模型通常是非线性的，但也可以是线性的。 - 性能指标：
MPC的目标是通过调整控制输入来最小化一个性能指标，通常是成本函数，以满足性能要求。
一般性能指标的形式如下：
J = ∑ k = 0 N − 1 ( ℓ ( x ( k ) , u ( k ) ) + m ( x ( N ) ) ) J = \sum_{k=0}^{N-1} \left( \ell(x(k), u(k)) + m(x(N)) \right) J=k=0∑N−1(ℓ(x(k),u(k))+m(x(N)))
其中， N N N是控制预测的时域长度， ℓ ( ⋅ ) \ell(\cdot) ℓ(⋅)表示每个时刻的瞬时成本， m ( ⋅ ) m(\cdot) m(⋅)表示终端成本。 - 控制预测：
在每个时刻，MPC对未来的系统行为进行预测，计算一系列控制输入，通常称为控制输入序列。
这些预测是通过模拟系统状态演变，基于当前状态和控制输入，利用系统模型来完成的。
预测时域通常涵盖多个时刻，以确保系统在未来时间内满足性能指标。 - 优化问题：
MPC使用一个优化算法来选择最优的控制输入序列，以最小化性能指标。
优化问题的目标是在满足约束条件的情况下，最小化性能指标。
优化问题通常是非线性的，并可以包括状态、控制输入和约束的非线性约束条件。 - 控制策略：
一旦找到最优的控制输入序列，只应用第一个控制输入，然后在下一个时刻重新计算。
这使得MPC成为一种适用于非线性系统和多变量系统的灵活控制策略，可以在实时中重新优化控制输入。 - 约束条件：
MPC可以处理各种约束条件，包括状态、控制输入和输出的约束。
这些约束可用于确保系统在操作限制内运行，以避免不稳定或不安全的操作。
然而，MPC控制算法需要较强的计算能力，因为它涉及到在线优化问题的求解。而且它对系统模型的准确性要求较高，可能需要对控制器的参数进行精细调整以实现所需的性能。
自动驾驶控制算法
自动驾驶控制往往是横纵向解耦控制，横向控制器一般采用LQR算法，而纵向控制器一般采用PID算法，下面将详细介绍横纵向控制。
横向控制
横向控制的输入车辆位置（定位信息）、车身参数信息（底盘信息）和规划轨迹，它主要通过改变方向盘扭矩或转角等进而控制车辆的前轮转角，使车辆按照规划的路径行驶。如下图所示，横向控制主要由前馈开环控制器和反馈闭环控制器构成。
纵向控制
纵向控制的输入为规划的位置、速度和加速度，它主要为速度控制，通过控制刹车和油门等实现对车速的控制，如下图所示，纵向控制主要由两个闭环控制和一个开环控制组成。
