P06 控制律

Target

建立水平运动模型
研究其可控性

假设

横向控制与纵向控制解耦

通过方向控制进行轨迹跟踪。
车辆的速度通过横向控制在可接受的约束（最大速度）内“承受”。

使用自行车运动学模型：

低速情况下，偏航动力学和侧滑忽略不计。
Ackermann假设。
参考点是后轴的中心。
后轴的两轮和前轴的两轮被替换为每个车轴中心的一个虚拟轮。

$x$ : 后轴中心点 R 的横坐标
$y$ : 后轴中心点 R 的纵坐标
$\Psi$：车辆相对于 x 轴的方向
$b$：轴距
$a$ ：一轴的两轮之间的距离（车道宽度，未表示）
$v$ ：车辆速度
$\delta$：车轮的转向角
$\kappa$ : 车辆轨迹的曲率
$P$ : 瞬时转动中心

输入:
- $v$ 和 $\delta$
输出:
$x, y, \Psi$ 和 $\kappa$

建模

动力学模型

$(x) :v_x=v * \cos (\Psi)$
$(y) : v_y=v * \sin (\Psi)$
角速度 $\dot{\Psi}=\frac{v}{R}$
曲率半径：$R=\frac{1}{\kappa}=\frac{b}{\tan (\delta)}$
曲率的微分：$\dot{\kappa}=\frac{d}{d t}\left[\frac{\tan (\delta)}{b}\right]=\frac{\dot{\delta}}{b * \cos ^2(\delta)}=\sigma$

设状态向量vecteur d’état$X=[x, y, \Psi, \kappa]$，建立动力学模型：

$\dot{X}=F(X) * v+G(X) * \sigma$

即：

$\frac{d}{d t}\left[\begin{array}{l}x \\y \\\Psi \\\kappa\end{array}\right]=\left[\begin{array}{c}\cos (\Psi) \\\sin (\Psi) \\\kappa \\0\end{array}\right] v+\left[\begin{array}{l}0 \\0 \\0 \\1\end{array}\right] \sigma$

线性化

线性化假设

由于模型非线性，需要在平衡点$X_0=\left[x_0, y_0, \Psi_0\right] / \dot{x}_0=0, \dot{y}_0=0, \dot{\Psi}_0=0, \kappa_0=0$附近进行线性化。

为此，增加假设：

车辆速度为常数，等于 $v_{\max }$ （带符号，取决于车辆行驶方向）。
平行泊车：车辆方向 $\Psi$ 的变化较小，因此可以将其平均值固定在平衡状态 $\bar{\Psi}=20^{\circ}$ 。
在实际停车过程中，给定参考轨迹仅为 $\left[x{r e f}, y{r e f}, \Psi_{r e f}\right]$ ，简化为状态 $[x, y, \Psi]$ 。

$\frac{d}{d t}\left[\begin{array}{l} x \\ y \\ \Psi \end{array}\right]=\left[\begin{array}{c} \cos (\Psi) \\ \sin (\Psi) \\ \kappa \end{array}\right] v \quad \text { 其中 } \quad \kappa=\frac{\tan (\delta)}{b}$

记$\widetilde{U}=\tan (\delta)$，则有：

$\frac{d}{d t}\left[\begin{array}{l}x \\y \\\Psi\end{array}\right]=\left[\begin{array}{c}\cos (\Psi) \\\sin (\Psi) \\\frac{\widetilde U}{b}\end{array}\right] v=\left[\begin{array}{l}H_1(x, y, \Psi, \widetilde{U}) \\H_2(x, y, \Psi, \widetilde{U}) \\H_3(x, y, \Psi, \widetilde{U})\end{array}\right]=H(x, y, \Psi, \widetilde{U})$

线性化

计算雅可比矩阵：

$\begin{aligned}A= \left[\begin{array}{lll} \frac{\partial H_1}{\partial x} & \frac{\partial H_1}{\partial y} & \frac{\partial H_1}{\partial \Psi} \\ \frac{\partial H_2}{\partial x} & \frac{\partial H_2}{\partial y} & \frac{\partial H_2}{\partial \Psi} \\ \frac{\partial H_3}{\partial x} & \frac{\partial H_3}{\partial y} & \frac{\partial H_3}{\partial \Psi} \end{array}\right],B = \left[\begin{array}{l} \frac{\partial H_1}{\partial \widetilde{U}} \\ \frac{\partial H_2}{\partial \widetilde{U}} \\ \frac{\partial H_3}{\partial \widetilde{U}} \end{array}\right]\end{aligned}$

得线性化之后的系统：

$\Delta \dot{X}=\left(\begin{array}{ccc}0 & 0 & -v \cdot \sin (\bar{\Psi}) \\0 & 0 & v \cdot \cos (\bar{\Psi}) \\0 & 0 & 0\end{array}\right) \Delta X+\left(\begin{array}{c}0 \\0 \\\frac{v}{b}\end{array}\right) \Delta \widetilde{U}$

可控性 commandabilité

计算可控性矩阵：

$C =\left[\begin{array}{l} B & A B & A^2 B \end{array}\right]=\left[\begin{array}{ccc}0 & \frac{-\left(\mathbf{v}^2 \cdot \sin (\bar{\Psi})\right)}{b} & 0 \\0 & \frac{\left(\mathbf{v}^2 \cdot \cos (\bar{\Psi})\right)}{b} & 0 \\\frac{\mathbf{v}}{\mathbf{b}} & 0 & 0\end{array}\right]$

其行列式为0，因此不可控。