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_0$ 到 $c_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

查看网友的精彩评论

推荐阅读