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

1 自行车模型

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

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

符号	含义
${\overline{e}}_x,{\overline{e}}_y$	x和y方向的单位向量
$p_r$	position rear tyre 后轮所在位置的向量
${\dot{p}}_r$	后轮地面接触点瞬时速度向量
$p_f$	position front tyre 前轮所在位置的向量
${\dot{p}}_f$	前轮地面接触点瞬时速度向量
$\theta$	车身的朝向角，即后轮朝向角${\dot{P}}_r$和${\overline{e}}_x$的夹角
$\delta$	方向盘转角，前轮朝向和车身朝向的夹角
$l$	刚性轴长度

这样后轮的x方向速度分量${\dot{x}}_r$，y方向速度分量${\dot{y}}_r$，和朝向角可以表示为：
$${\dot{x}}_r=v_r\cos (\theta )$$

$${\dot{y}}_r=v_r\sin (\theta )$$

$$\theta =v_r\tan (\delta )/l$$

同理，前轮的x方向速度分量${\dot{x}}_f$，y方向速度分量${\dot{y}}_f$，和朝向角可以表示为：
$${\dot{x}}_f=v_f\cos (\theta +\delta )$$

$${\dot{y}}_f=v_f\sin (\theta +\delta )$$

$$\theta =v_f\sin (\delta )/l$$

前后轮的切向速度满足：$v_r=v_f\cos (\delta )$

在这样的车辆模型下，反馈控制需要解决的问题之一，就是寻找满足车辆动态姿态限制的方向盘转角$\delta \in [{\delta }_{min},{\delta }_{max}]$和前向速度$v_r\in [v_{min},v_{max}]$。

2 PID反馈控制

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

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

2.1 横向控制

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

在n采样时刻，控制方向盘转角的PID控制器如下：
$${\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$$

符号	说明
${\theta }_e$	当前车朝向，和基准轨迹点（Reference Point）之间的跟踪角度误差
$l_e$	横向位置和基准点轨迹之间的距离误差
$V_s$	车辆在纵向方向的速度

2.2 纵向控制

纵向控制主要跟踪的速度误差，其中速度误差是轨迹曲率和基准轨迹点曲率的函数：
$$V_e=-f\left(k_{\text{ Vehicle }},k_{\text{ Reference }}\right)$$
因此前进速度的PID控制器可以写成：
$$U_V=K_P{V}_e+K_I\sum V_e\Delta t+K_D\Delta V_e/\Delta t$$

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

参考文章

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

在Typora中输入公式

Typora开启行内公式

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

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