MATLAB路径规划算法

关注
MATLAB路径规划算法www.shan-machinery.com

首先声明算法基于Optimal trajectories for time-critical street scenarios using discretized terminal manifolds,也有部分图片出自该文章。开始介绍,该算法的推导过程使用了大量的最优控制的理论,这里不会介绍,只会给出结论。

首先该算法的坐标系定义为Frenet坐标系,该坐标使用s,d来表示车辆的位置,s代表道路中心线的曲线长度,是一种自然坐标系,d代表车辆到中心线的距离。MATLAB中队Frenet的坐标系支持不算太好,需要自己写代码实现,然而MMA却可以支持 。MMA中的FrenetSerretSystem函数用法示例如下:

FrenetSerretSystem[{Cos[t], Sin[t]}, t]得到{{1}, {{-Sin[t], Cos[t]}, {-Cos[t], -Sin[t]}}}

{Cos[t], Sin[t]}是曲线参数方程,t为参数,1是曲率{-Sin[t], Cos[t]}是切向量,{-Cos[t], -Sin[t]}是法向量。下面是一个示例,能给出如下图像。

viv = {#, #^2/5} &;basis = Last[FrenetSerretSystem[viv[t], t]] // Simplify;{tangent, normal} = Map[Arrow[{viv[t], viv[t] + #}] &, basis];Manipulate[ Show[ParametricPlot[viv[s], {s, 0, 5}, PlotStyle -> Gray], Graphics[{Blue, tangent, Red, normal}], PlotRange -> {{0, 6}, {0, 6}}] // Evaluate, {t, 0, 5}]

下面的讨论基于如下状态空间, \xi ^ { \mathrm { T } } = \left[ \xi _ { 1 } , \xi _ { 2 } , \xi _ { 3 } \right] 各元素分别代表位置,速度和加速度, u ( t ) = \dddot\xi _ { 1 } ( t ) 表示控制量。 \dot { \xi } = \left[ \begin{array} { c c c } { 0 } & { 1 } & { 0 } \\ { 0 } & { 0 } & { 1 } \\ { 0 } & { 0 } & { 0 } \end{array} \right] \xi + \left[ \begin{array} { l } { 0 } \\ { 0 } \\ { 1 } \end{array} \right] u = : f ( \xi , u )

定义代价函数如下:

J _ { \xi } : = \int _ { 0 } ^ { \tau } f _ { 0 } ( u ) \mathrm { d } t + ( h ( \xi ( t ) , t ) ) _ { \tau } , \quad f _ { 0 } ( u ) : = \frac { 1 } { 2 } u ^ { 2 } ( t )

其中 ( h ( \xi ( t ) , t ) ) _ { \tau } 代表终点代价,比如到达终所用时间等。在该代价函数下,最优路径如下:

\left[ \begin{array} { c } { \xi _ { 1 } } \\ { \xi _ { 2 } } \\ { \xi _ { 3 } } \\ { \psi _ { 1 } } \\ { \psi _ { 2 } } \\ { \psi _ { 3 } } \end{array} \right] = \left[ \begin{array} { c c c c c c } { 1 } & { t } & { t ^ { 2 } } & { t ^ { 3 } } & { t ^ { 4 } } & { t ^ { 5 } } \\ { 0 } & { 1 } & { 2 t } & { 3 t ^ { 2 } } & { 4 t ^ { 3 } } & { 5 t ^ { 4 } } \\ { 0 } & { 0 } & { 2 } & { 6 t } & { 12 t ^ { 2 } } & { 20 t ^ { 3 } } \\ { 0 } & { 0 } & { 0 } & { 6 } & { 24 t } & { 60 t ^ { 2 } } \\ { 0 } & { 0 } & { 0 } & { 0 } & { - 24 } & { - 120 t } \\ { 0 } & { 0 } & { 0 } & { 0 } & { 0 } & { 120 } \end{array} \right] \left[ \begin{array} { c } { c _ { 0 } } \\ { c _ { 1 } } \\ { c _ { 2 } } \\ { c _ { 3 } } \\ { c _ { 4 } } \\ { c _ { 5 } } \end{array} \right]

接下就是如何确定 c_0c_5 各系数,变形上式。

\xi ( t ) = M _ { 1 } ( t ) c _ { 012 } + M _ { 2 } ( t ) c _ { 345 },\quad c_{xyz}=\left[ \begin{array} { c } c_x& c_y & c_z \end{array} \right] ^T

其中:

M _ { 1 } ( t ) = \left[ \begin{array} { c c c } { 1 } & { t } & { t ^ { 2 } } \\ { 0 } & { 1 } & { 2 t } \\ { 0 } & { 0 } & { 2 } \end{array} \right] , \quad M _ { 2 } ( t ) = \left[ \begin{array} { c c c } { t ^ { 3 } } & { t ^ { 4 } } & { t ^ { 5 } } \\ { 3 t ^ { 2 } } & { 4 t ^ { 3 } } & { 5 t ^ { 4 } } \\ { 6 t } & { 12 t ^ { 2 } } & { 20 t ^ { 3 } } \end{array} \right]

先求解 c_{012} :

\xi _ { 0 } = M _ { 1 } ( 0 ) c _ { 012 } ,\quad c _ { 012 } = M _ { 1 } ( 0 ) ^ { - 1 } \xi _ { 0 } = q _ { 012 } \left( \xi _ { 1 } ( 0 ) , \xi _ { 2 } ( 0 ) , \xi _ { 3 } ( 0 ) \right)

再求解 c_{345} :

\xi _ { \tau } = M _ { 1 } ( \tau ) c _ { 012 } + M _ { 2 } ( \tau ) c _ { 345 } ,\quad c _ { 345 } = M _ { 2 } ( \tau ) ^ { - 1 } \left[ \xi _ { \tau } - M _ { 1 } ( \tau ) c _ { 012 } \right] = q _ { 345 } \left( \xi _ { 1 } ( 0 ) , \xi _ { 2 } ( 0 ) , \xi _ { 3 } ( 0 ) \right)

如果只需约束终点的速度,那么 c_{012} 依然可以按照上述方法计算,但是在终点时仅需满足下式,基于此即可求 c_{34}

\left[ \begin{array} { c } { \xi _ { 2 } ( \tau ) } \\ { \xi _ { 3 } ( \tau ) } \end{array} \right] = \left[ \begin{array} { c c c c c } { 0 } & { 1 } & { 2 t } & { 3 t ^ { 2 } } & { 4 t ^ { 3 } } \\ { 0 } & { 0 } & { 2 } & { 6 t } & { 12 t ^ { 2 } } \end{array} \right] \left[ \begin{array} { c } { c _ { 0 } } \\ { c _ { 1 } } \\ { c _ { 2 } } \\ { c _ { 3 } } \\ { c _ { 4 } } \end{array} \right]=\left[ \begin{array} { c c c } { 0 } & { 1 } & { 2 t } \\ { 0 } & { 0 } & { 2 } \end{array} \right] \left[ \begin{array} { c } { c _ { 0 } } \\ { c _ { 1 } } \\ { c _ { 2 } } \end{array} \right] + \left[ \begin{array} { c c } { 3 t ^ { 2 } } & { 4 t ^ { 3 } } \\ { 6 t } & { 12 t ^ { 2 } } \end{array} \right] \left[ \begin{array} { c } { c _ { 3 } } \\ { c _ { 4 } } \end{array} \right]

c_{34}= q _ { 34 } \left( \xi _ { 2 } ( \tau ) , \xi _ { 3 } ( \tau ) \right)

总结表格如下:

其中 \delta,\sigma,\nu 代表到终点的误差,为什么要设置误差,那是因为如果一次在指定时间内达到终点可能需要更多的控制量,所以综合考量,可能为了更小的花费一次规划不能到达终点。详细内容可以参考论文,这里只是简单介绍,重点放在如何编程上。

首先我们看一下怎么从Frenet坐标系转换到直角坐标系,t_c,n_c 是切向量和法向量。:

\boldsymbol { R } [ \boldsymbol { x } - \boldsymbol { r } ] = [ d , 0 ] ^ { \mathrm { T } },\boldsymbol { R } : = \left[ \boldsymbol { t } _ { c } , \boldsymbol { n } _ { c } \right]

那怎么求 \boldsymbol { r } 就成为一个必须要解决的问题,如果我们知道中心线的曲线方程和 \boldsymbol { r } 所在的曲线长度s,那么是可以求出\boldsymbol { r } 的。

function x = s2x(f,s)syms tdf = diff(f);st = int(sqrt(df^2+1),0,t)-s;st = matlabFunction(st);options = optimoptions('fsolve');options = optimoptions(options,'Display', 'off');options = optimoptions(options,'Algorithm', 'levenberg-marquardt');x =fsolve(st,5,options);end

然后我们求出切向量和法向量。

function theta = tangent(f,x)df = diff(f);df = matlabFunction(df);theta = atan(df(x));endfunction [t,n] = basis(f,x)tang = tangent(f,x);t = [cos(tang),sin(tang)]';n = [cos(tang+pi/2),sin(tang+pi/2)]';end

上面f是曲线方程,s是所在点的曲线弧长。st是弧长方程,通过求解弧长方程就可以求解对应的自变量x。接下来可以求解速度,可有下式,其中 \kappa _ { c } 是道路中心的曲率:

v = \sqrt { \left[ 1 - \kappa _ { c } d \right] ^ { 2 } \dot { s } ^ { 2 } + \dot { d } ^ { 2 } }

function [kappa,dkappa] = curvature(f,x)df = diff(f);ddf = diff(df);kappa = abs(ddf)/(1+df^2)^(3/2);dkappa = diff(kappa);f = matlabFunction(kappa);df = matlabFunction(dkappa);kappa = f(x);dkappa = df(x);end

由于MATLAB中没有直接求曲率的方法,手写一段代码如上,顺带求出了 \frac{d\kappa_c}{dx}

\begin{array} { r l } { \dot { d } } & { = \frac { \mathrm { d } } { \mathrm { d } t } d = \frac { \mathrm { d } s } { \mathrm { d } t } \frac { \mathrm { d } } { \mathrm { d } s } d = \dot { s } d ^ { \prime } } \\ { \ddot { d } } & { = \frac { \mathrm { d } } { \mathrm { d } t } \dot { s } d ^ { \prime } = \ddot { s } d ^ { \prime } + \dot { s } \frac { \mathrm { d } s } { \mathrm { d } t } \frac { \mathrm { d } } { \mathrm { d } s } d ^ { \prime } = \ddot { s } d ^ { \prime } + \dot { s } ^ { 2 } d ^ { \prime \prime } } \end{array}

由上式我们需要求出 d',d'' ,其中 ( ) ^ { \prime } : = ( \partial / \partial s ) 。然后我们可从下式求出 \Delta \theta : = \theta - \theta _ { c } ,由此可以求出 \theta

d ^ { \prime } = \frac { \mathrm { d } } { \mathrm { d } s } d = \frac { \mathrm { d } t } { \mathrm { d } s } \frac { \mathrm { d } } { \mathrm { d } t } d = \frac { \dot { d } } { \dot { s } } = \left[ 1 - \kappa _ { c } d \right] \tan \Delta \theta

求出 \theta 后我们就可以进一步求出 \kappa

d ^ { \prime \prime } = - \left[ \kappa _ { c } ^ { \prime } d + \kappa _ { c } d ^ { \prime } \right] \tan \Delta \theta + \frac { 1 - \kappa _ { c } d } { \cos ^ { 2 } \Delta \theta } \left[ \kappa \frac { 1 - \kappa _ { c } d } { \cos \Delta \theta } - \kappa _ { c } \right]

最后我们可以求出a。

\begin{array} { l } { a : = \dot { v } = \ddot { s } \frac { 1 - \kappa _ { c } d } { \cos \Delta \theta } + \dot { s } \frac { \mathrm { d } s } { \mathrm { d } t } \frac { \mathrm { d } } { \mathrm { d } s } \frac { 1 - \kappa _ { c } d } { \cos \Delta \theta } = \ddot { s } \frac { 1 - \kappa _ { c } d } { \cos \Delta \theta } + } \\ { \frac { \dot { S } ^ { 2 } } { \cos \Delta \theta } \left[ \left[ 1 - \kappa _ { c } d \right] \tan \Delta \theta \left[ \kappa \frac { 1 - \kappa _ { c } d } { \cos \Delta \theta } - \kappa _ { c } \right] - \left[ \kappa _ { c } ^ { \prime } d + \kappa _ { c } d ^ { \prime } \right] \right] } \end{array}

function [x1,x2,theta,kappa,v,a] = Frenet2Global(f,s,ds,dds,d,dd,ddd)% get x based on sx = s2x(f,s);fh = matlabFunction(f);y = fh(x);% get normal vector[~,n] = basis(f,x);x1 = x+d*n(1);x2 = y+d*n(2);[kappac,dkappac] = curvature(f,x);t1 = 1-kappac*d;v = sqrt(t1^2*ds^2+dd^2);pd = dd/ds;ppd = (ddd-dds*pd)/ds^2;dtheta = atan(pd/t1);theta = dtheta+tangent(f,x);t2 = t1/cos(dtheta);pkappac = dkappac/ds;t3 = pkappac*d+kappac*pd;t = ppd+t3*tan(dtheta);t = t/t2/cos(dtheta)+kappac;kappa = t/t2;t = t1*tan(dtheta)*(kappa*t2-kappac)-t3;t = t*ds^2/cos(dtheta);a = dds*t2+t;end

首先给出大致算法,算法思路是使用两个时间精度 \rm\tau Res,tRes ,首先使用 \rm \tau Res 来试探到达终点,如果因为所需花费过大不能达到终点。那就把此次计算得到的系数走 \rm tRes 时间。达到新的起点再做重复过程,知道到达终点。

%% % tb_ConstVeloLKA% Let say road is y = 1/100*x^2% Initial d = 1 s = 0 and we want vehicle to run @ 5m/s% Use cyclic replanning, time resolution @ 1ssyms x% Lane curve shapey = 1/100*x^2;global Ks Kdevs Kdevd tauRes stepResKs = 0.1;Kdevd = 2;Kdevs = 1;tauRes = 10;stepRes = 2;% Terminal vehicle velocityv_ref = 5;% Initial statexs0 = [0,1,0]';xd0 = [1,0,0]';xdi = xd0;di = xd0(1);xsi = xs0;si = 0;vi = xs0(2)-v_ref;siSet = [];psSet = [];pdSet = [];tauSet = [];i = 1;while 1if i > 1000breakendJmin = inf;for deltad = linspace(-di,di,11)xdi_tau = [deltad,0,0]';[c012,c345] = CalcCoef(tauRes,xdi,xdi_tau,0);pd = [c012;c345];pd = flipud(pd);Td = Kdevd*deltad^2;Jd = Costd(pd,Td);for deltav = linspace(-vi,vi,11)xsi_tau = [v_ref+deltav,0]';[c012,c345] = CalcCoef(tauRes,xsi,xsi_tau,2);ps = [c012;c345];ps = flipud(ps);Ts = Kdevs*deltav^2;Js = Costs(ps,Ts);Ji = Cost(Js,Jd);if Ji 规划的速度-t规划的s-t和d-t

https://www.shan-machinery.com