Exemple 1 车辆的横向控制建模与侧方位通车
Exemple 1 车辆的横向控制建模与侧方位通车
假设
横向控制与纵向控制解耦
- 通过方向控制进行轨迹跟踪。
- 车辆的速度通过横向控制在可接受的约束（最大速度）内“承受”。
使用自行车运动学模型：
- 低速情况下，偏航动力学和侧滑忽略不计。
- 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，因此不可控。
控制率 loi de commande
我们希望应用\(LQ\)控制：
\[ J=\int_0^{\infty}\left(\Delta X(t)^T Q \Delta X(t)+\Delta \widetilde{U}(t)^T R \Delta \widetilde{U}\right) d t \]
但是因为\((A,B)\)不可控，所以\(LQ\)不可应用于我们当前的系统。
简化系统
为了应用\(LQ\)，需要将系统简化为唯一可控变量。存在可逆矩阵\(T\)，使得\(\Delta X=T * \Delta \tilde{X}\)，此时演化方程转化为：
\[ \Delta \dot{\tilde{X}}=T^{-1} \cdot A \cdot T \Delta \tilde{X}+T^{-1} B \Delta \widetilde{U} \]
为了方便计算，设\(a 13=-v \cdot \sin (\bar{\Psi}), a 23=v \cdot \cos (\bar{\Psi}) \text { et } b 3=\frac{v}{b}\)。
选择：
\[ \boldsymbol{T}^{-1}=\left(\begin{array}{ccc}\frac{a 23}{a 13 * b 3} & \frac{-1}{b 3} & \frac{1}{b 3} \\\frac{1}{a 13 * b 3} & 0 & 0 \\\frac{-a 23}{a 13} & 1 & 0\end{array}\right) \]
得到系统转化为：
\[ \Delta \dot{\tilde{X}}=\left(\begin{array}{c}\Delta \dot{\widetilde{x_1}} \\\Delta \dot{\widetilde{x_2}} \\\Delta \dot{\widetilde{x_3}}\end{array}\right)=\left(\begin{array}{ccc}0 & 0 & 0 \\1 & 0 & \frac{1}{b 3} \\0 & 0 & 0\end{array}\right) \Delta \tilde{X}+\left(\begin{array}{l}1 \\0 \\0\end{array}\right) \Delta \widetilde{U} \]
从而将系统简化为：
\[ \delta \dot{\widetilde{X_{com}}} = \left(\begin{array}{ll} 0 & 0 \\ 1 & 0 \end{array}\right)\delta\widetilde{X_{com}}+\binom{1}{0}\delta\widetilde{U_{ c o m}} \]
应用\(LQ\)控制
取\(Q = I_2,R = I_1\)，Riccati方程：
\[ \begin{aligned}A^{\top}P+P A-P B R^{-1} B^{\top} P+Q=0\Leftrightarrow\\\left[\begin{array}{cc} -P 1_1^2+P 1_2+P 2_1+1 & P 2_2-P 1_1 P 1_2 \\ P 2_2-P 1_1 P 2_1 & 1-P 1_2 P 2_1 \end{array}\right] = 0\end{aligned} \]
唯一的正定解为：
\[ P = \left[\begin{array}{cc}\sqrt 3 & 1 \\1 & \sqrt 3\end{array}\right] \]
而\(L = R^{-1} B^T P = [\sqrt 3, 1]\)。
因此有：
\[ \Delta \widetilde{U}=-\left(\begin{array}{ll}L & 0\end{array}\right) \Delta \tilde{X}=-L_{t o t} T^{-1} \Delta X \]
闭环控制系统
\[ \begin{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} \\=&\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)L_{\text {tot }} T^{-1} \Delta X \end{aligned} \]
