无人驾驶的规划与控制（四）——反馈控制

文章目录

• 1 自行车模型
• 2 PID反馈控制
• • 2.1 横向控制
  • 2.2 纵向控制

1 自行车模型

在自行车模型中，我们假设车辆姿态处于一个二维的平面坐标系内，车辆的姿态可以由位移（position）和车身夹角（heading）完全描述，并且前后轮由一个刚性（rigid）轴连接，前轮可转动，后轮只能直行。

自行车模型的一个重要特征是：如果车辆不向前移动，就不能横向位移，称为非完整性约束（nonholonomic constraint）。

符号	含义
e ‾ x , e ‾ y \overline{e}_{x},\overline{e}_{y} ex​,ey​	x和y方向的单位向量
p r p_{r} pr​	position rear tyre 后轮所在位置的向量
p ˙ r \dot{p}_{r} p˙​r​	后轮地面接触点瞬时速度向量
p f p_{f} pf​	position front tyre 前轮所在位置的向量
p ˙ f \dot{p}_{f} p˙​f​	前轮地面接触点瞬时速度向量
θ \theta θ	车身的朝向角，即后轮朝向角 P ˙ r \dot{P}_r P˙r​和 e ‾ x \overline{e}_x ex​的夹角
δ \delta δ	方向盘转角，前轮朝向和车身朝向的夹角
l l l	刚性轴长度

这样后轮的x方向速度分量 x ˙ r \dot{x}_r x˙r​，y方向速度分量 y ˙ r \dot{y}_r y˙​r​，和朝向角可以表示为：

x ˙ r = v r cos ⁡ ( θ ) \dot{x}_{r}=v_{r} \cos (\theta) x˙r​=vr​cos(θ)

y ˙ r = v r sin ⁡ ( θ ) \dot{y}_{r}=v_{r} \sin (\theta) y˙​r​=vr​sin(θ)

θ = v r tan ⁡ ( δ ) / l \theta=v_{r} \tan (\delta) / l θ=vr​tan(δ)/l

同理，前轮的x方向速度分量 x ˙ f \dot{x}_f x˙f​，y方向速度分量 y ˙ f \dot{y}_f y˙​f​，和朝向角可以表示为：

x ˙ f = v f cos ⁡ ( θ + δ ) \dot{x}_{f}=v_{f} \cos (\theta+\delta) x˙f​=vf​cos(θ+δ)

y ˙ f = v f sin ⁡ ( θ + δ ) \dot{y}_{f}=v_{f} \sin (\theta+\delta) y˙​f​=vf​sin(θ+δ)

θ = v f sin ⁡ ( δ ) / l \theta=v_{f} \sin (\delta) / l θ=vf​sin(δ)/l

前后轮的切向速度满足： v r = v f cos ⁡ ( δ ) v_{r}=v_{f} \cos (\delta) vr​=vf​cos(δ)

在这样的车辆模型下，反馈控制需要解决的问题之一，就是寻找满足车辆动态姿态限制的方向盘转角 δ ∈ [ δ m i n , δ m a x ] \delta\in[\delta_{min},\delta_{max}] δ∈[δmin​,δmax​]和前向速度 v r ∈ [ v m i n , v m a x ] v_r\in[v_{min},v_{max}] vr​∈[vmin​,vmax​]。

2 PID反馈控制

无人车反馈控制模块的任务，是控制车辆尽可能遵循上游动作规划（轨迹和速度）出的时空轨迹。

这里使用两个PID控制器分别控制方向盘转角 δ \delta δ以及前进速度 v s v_s vs​。

2.1 横向控制

横向控制主要跟踪的变量误差主要是轨迹的横向误差，和角度误差。

在n采样时刻，控制方向盘转角的PID控制器如下：

δ n = K 1 θ e + K 2 l e / V s + K 3 i e + K 4 ∑ i = 1 n l e Δ t \delta_{n}=K_{1} \theta_{e}+K_{2} l_{e} / V_{s}+K_{3} i_{e}+K_{4} \sum_{i=1}^{n} l_{e} \Delta t δn​=K1​θe​+K2​le​/Vs​+K3​ie​+K4​i=1∑n​le​Δt

符号	说明
θ e \theta_e θe​	当前车朝向，和基准轨迹点（Reference Point）之间的跟踪角度误差
l e l_e le​	横向位置和基准点轨迹之间的距离误差
V s V_s Vs​	车辆在纵向方向的速度

2.2 纵向控制

纵向控制主要跟踪的速度误差，其中速度误差是轨迹曲率和基准轨迹点曲率的函数：

V e = − f ( k  Vehicle  , k  Reference  ) V_e=-f\left(k_{\text { Vehicle }}, k_{\text { Reference }}\right) Ve​=−f(k Vehicle ​,k Reference ​)

因此前进速度的PID控制器可以写成：

U V = K P V e + K I ∑ V e Δ t + K D Δ V e / Δ t U_{V}=K_{P} V_{e}+K_{I} \sum V_{e} \Delta t+K_{D} \Delta V_{e} / \Delta t UV​=KP​Ve​+KI​∑Ve​Δt+KD​ΔVe​/Δt

符号	说明
k  Vehicle  k_{\text { Vehicle }} k Vehicle ​	车辆当前的轨迹点曲率
k  Reference  k_{\text { Reference }} k Reference ​	基准轨迹点的基准点曲率

参考文章

《第一本无人驾驶技术书》刘少山

在Typora中输入公式

Typora开启行内公式

常用数学符号的 LaTeX 表示方法

一份不太简短的LATEX2ε介绍【精华】