自动驾驶的类人决策与控制

### 自动驾驶的类人决策与控制 #### 1. 自动驾驶决策框架 为解决无信号环岛处自动驾驶车辆（AVs）的驾驶冲突，采用博弈论方法设计了类人决策框架。定义了三种驾驶风格：激进型、保守型和正常型。 决策框架包含以下关键部分： - **决策制定**：综合考虑驾驶安全、乘坐舒适性和行驶效率构建决策收益函数。 - **运动预测**：采用基于车辆运动学模型的运动预测模块，提升决策算法性能。 - **运动规划与控制**：根据决策结果进行运动规划和控制。 决策框架流程如下： ```mermaid graph LR A[决策制定] --> B[收益函数] B --> C[安全] B --> D[舒适] B --> E[效率] A --> F[多约束] A --> G[运动规划] G --> H[轨迹] G --> I[速度] A --> J[运动控制] K[运动预测] --> A L[车辆运动学模型] --> K M[博弈论方法] --> A N[周围车辆] --> A O[主车辆] --> A P[自动驾驶] --> A P --> Q[激进型] P --> R[正常型] P --> S[保守型] T[联网驾驶] --> A ``` 不同驾驶风格的加权系数如下表所示： | 驾驶风格 | 安全系数 \(k_i^s\) | 舒适系数 \(k_i^c\) | 效率系数 \(k_i^e\) | | ---- | ---- | ---- | ---- | | 激进型 | 0.4 | 0.5 | 0.1 | | 正常型 | 0.3 | 0.3 | 0.4 | | 保守型 | 0.1 | 0.4 | 0.5 | #### 2. 用于决策的自动驾驶车辆运动预测 采用简化的车辆运动学模型进行运动预测，具体步骤如下： 1. **将车辆运动学模型表示为时变线性系统**： \(\dot{x}(t) = A_tx(t) + B_tu(t)\) 其中，时变系数矩阵 \(A_t\) 和 \(B_t\) 由下式推导得出： \(A_t = \frac{\partial f}{\partial x}\big|_{x_t,u_t}\)，\(B_t = \frac{\partial f}{\partial u}\big|_{x_t,u_t}\) 2. **将时变线性系统表示为离散模型**： \(\begin{cases}x(k + 1) = A_kx(k) + B_ku(k)\\u(k) = u(k - 1) + \Delta u(k)\end{cases}\) 其中，状态向量 \(x(k) = [v_x(k), \phi(k), X(k), Y(k)]^T\)，系数矩阵 \(A_k = e^{A_t\Delta T}\)，\(B_k = \int_{0}^{\Delta T}e^{A_t\tau}B_td\tau\)，\(\Delta T\) 为采样时间，控制向量 \(u(k) = [a_x(k), \delta_f(k)]^T\)，控制增量向量 \(\Delta u(k) = [\Delta a_x(k), \Delta \delta_f(k)]^T\)。 3. **提出新的状态向量**： \(\xi(k) = [x(k), u(k - 1)]^T\) 4. **重写离散模型**： \(\begin{cases}\xi(k + 1) = \hat{A}_k\xi(k) + \hat{B}_k\Delta u(k)\\y(k) = \hat{C}_k\xi(k)\end{cases}\) 其中，\(\hat{A}_k = \begin{bmatrix}A_k & B_k\\0_{2\times4} & I_2\end{bmatrix}\)，\(\hat{B}_k = \begin{bmatrix}B_k\\I_2\end{bmatrix}\)，\(\hat{C}_k = \begin{bmatrix}I_4 & 0_{4\times2}\end{bmatrix}\)。 5. **定义预测时域 \(N_p\) 和控制时域 \(N_c\)（\(N_p > N_c\)）**： 在时间步 \(k\)，已知状态向量 \(\xi(k)\)、控制向量 \(\Delta u(k)\) 和系数矩阵 \(\hat{A}_{p,k}\)、\(\hat{B}_{p,k}\)、\(\hat{C}_{p,k}\) 时，可推导预测状态向量： \(\begin{cases}\xi(p + 1|k) = \hat{A}_{p,k}\xi(p|k) + \hat{B}_{p,k}\Delta u(p|k)\\y(p|k) = \hat{C}_{p,k}\xi(p|k)\end{cases}\) 假设 \(\hat{A}_{p,k} = \hat{A}_k\)，\(\hat{B}_{p,k} = \hat{B}_k\)，\(\hat{C}_{p,k} = \hat{C}_k\)，则预测运动状态可表示为： \(\xi(k + 1|k) = \hat{A}_k\xi(k|k) + \hat{B}_k\Delta u(k|k)\) \(\xi(k + 2|k) = \hat{A}_k^2\xi(k|k) + \hat{A}_k\hat{B}_k\Delta u(k|k) + \hat{B}_k\Delta u(k + 1|k)\) \(\cdots\) \(\xi(k + N_c|k) = \hat{A}_k^{N_c}\xi(k|k) + \hat{A}_k^{N_c - 1}\hat{B}_k\Delta u(k|k) + \cdots + \hat{B}_k\Delta u(k + N_c - 1|k)\) \(\cdots\) \(\xi(k + N_p|k) = \hat{A}_k^{N_p}\xi(k|k) + \hat{A}_k^{N_p - 1}\hat{B}_k\Delta u(k|k) + \cdots + \hat{A}_k^{N_p - N_c}\hat{B}_k\Delta u(k + N_c - 1|k)\) 6. **计算输出向量序列**： \(\Upsilon(k) = [y^T(k + 1|k), y^T(k + 2|k), \cdots, y^T(k + N_p|k)]^T\) 根据上述预测运动状态，\(\Upsilon(k)\) 可由下式推导得出： \(\Upsilon(k) = \overline{C}\xi(k|k) + \overline{D}\Delta u(k)\) 其中，\(\Delta u(k) = [\Delta u^T(k|k), \Delta u^T(k + 1|k), \cdots, \Delta u^T(k + N_c - 1|k)]^T\)，\(\overline{C} = [(\hat{C}_k\hat{A}_k)^T, (\hat{C}_k\hat{A}_k^2)^T, \cdots, (\hat{C}_k\hat{A}_k^{N_p})^T]^T\)，\(\overline{D} = \begin{bmatrix}\hat{C}_k\hat{B}_k & 0 & \cdots & 0\\\hat{C}_k\hat{A}_k\hat{B}_k & \hat{C}_k\hat{B}_k & \cdots & 0\\\vdots & \vdots & \ddots & \vdots\\\hat{C}_k\hat{A}_k^{N_p - 1}\hat{B}_k & \hat{C}_k\hat{A}_k^{N_p - 2}\hat{B}_k & \cdots & \hat{C}_k\hat{A}_k^{N_p - N_c}\hat{B}_k\end{bmatrix}\)。 通过以上步骤完成基于模型的自动驾驶车辆运动预测，预测的运动状态用于决策算法设计，提高决策安全性。 #### 3. 基于博弈论方法的决策算法设计 采用 Stackelberg 博弈方法解决无信号环岛处自动驾驶车辆的驾驶冲突，实现类人决策。具体步骤如下： 1. **构建决策收益函数**：综合考虑驾驶安全、乘坐舒适性和行驶效率。 2. **考虑不同阶段的决策行为**：进入、通过和驶出阶段具有不同的决策行为。 3. **考虑不同对手情况**：先考虑单对手情况，再推广到多对手情况。 4. **设置决策约束**：包括安全约束、舒适性约束、速度约束和控制约束。 5. **结合模型预测控制（MPC）进行优化决策**。 决策算法流程如下： ```mermaid graph LR A[构建收益函数] --> B[考虑单对手] B --> C[进入阶段决策] B --> D[通过和驶出阶段决策] A --> E[考虑多对手] F[设置约束] --> G[安全约束] F --> ```