简单实现。
$$ 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} + Pn J{n} + Pc J{c}$ $$ J{share} = J{n} = J{c} = w{j} j^2 + w{s} (s-s{ref})^2 + w{v} (v-v{ref})^2 $$ 将匀速行驶作为参考位置和速度。
约束条件:
config.m中为参数设置。
contingency planning优化求解;
画图。
main.m