P06 控制律
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，因此不可控。
