Contingency Speed Planning
简单实现。
1. Control Model
- 状态变量： x = [ s , v , a ] T x = [s, v, a]^T x=[s,v,a]T;
- 控制变量： u = [ j ] u = [j] u=[j];
- 运动学模型： x ˙ = A x + B u \dot{x} = A x + B u x˙=Ax+Bu;
A = [ 0 1 0 0 0 1 0 0 0 ] , B = [ 0 0 1 ] 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] A= 000100010 ,B= 001
-
优化函数： J = J s h a r e + P n J n + P c J c J = J_{share} + P_n J_{n} + P_c J_{c} J=Jshare+PnJn+PcJc
J s h a r e = J n = J c = w j j 2 + w s ( s − s r e f ) 2 + w v ( v − v r e f ) 2 J_{share} = J_{n} = J_{c} = w_{j} j^2 + w_{s} (s-s_{ref})^2 + w_{v} (v-v_{ref})^2 Jshare=Jn=Jc=wjj2+ws(s−sref)2+wv(v−vref)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：
扫一扫
1580
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
