Contingency Speed Planning

简单实现。

1. Control Model

• 状态变量：$x = [s, v, a]^T$;
• 控制变量：$u = [j]$;
• 运动学模型：$\dot{x} = A x + B u$;

$$ A = \left[\begin{matrix} 0 & 1 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & 0 \\ \end{matrix}\right] , B = \left[\begin{matrix} 0 \\ 0 \\ 1 \\ \end{matrix}\right] $$

• 优化函数：$J = J_{share} + P_n J_{n} + P_c J_{c}$ $$ J_{share} = J_{n} = J_{c} = w_{j} j^2 + w_{s} (s-s_{ref})^2 + w_{v} (v-v_{ref})^2 $$ 将匀速行驶作为参考位置和速度。

• 约束条件：

  • 起点约束；
  • 运动学约束；
  • 碰撞约束；

2. Config

config.m中为参数设置。

• 优化问题权重；
• 状态变量和控制变量限制；
• 事件概率；
• 规划时域设置；
• 主车初始状态设置；
• CutIn车辆设置；

3. Planner

contingency planning优化求解；

4. PlotResults

画图。

5. 运行

main.m

• cut in概率为0：

• cut in概率为0.1：

• cut in概率为0.7：