等距曲线matlab实现_Reeds-Shepp 曲线的Matlab实现

Reeds-Shepp曲线是一种路线规划方法。假设车辆能以固定的半径转向，且车辆能够前进和后退，那么Reeds-Shepp曲线就是车辆在上述条件下从起点到终点的最短路径。该曲线不仅能保证车辆能够到达终点，而且能保证车辆的角度能在终点到达预期角度，比如在垂直泊车的过程中，开始车辆平行于道路，终点要求车辆垂直于道路，这就对车辆的终点位置和终点角度都提出了要求。

本文图片主要来自：

http://planning.cs.uiuc.edu/node822.html​planning.cs.uiuc.edu

首先看一下车辆模型，上图为车辆模型示意图。

该运动方在低速情况下运动轨迹为一个圆，方向盘转角

转到最大，其转向半径最小，假设最小转向半径为rmin。为了方便起见Reeds-Shepp中最小转向半径强制设置为1，如果车辆的实际最小转向半径不是1，可也通过适当放缩终点坐标来计算该曲线。比如如果一个车辆的最小转向半径为10，终点坐标为x,y，如果我们在计算曲线的时候将终点设为x/10,y/10，计算所得路径放大10倍曲线的转向半径就是10，终点也是x,y，所得曲线就是我们所期望曲线。下面曲线就是一条Reeds-Shepp曲线，R表示右打方向，+表示前进，图中从

到

，首先右打方向前进，再左打方向后退，最后右打方向前进到达终点。

下表是Reeds-Shepp曲线的基本操作方式，一共有48种操作模式，归为9种Base word，可详见OPTIMAL PATHS FOR A CAR THAT GOES BOTH FORWARDS AND BACKWARDS。但是在编程求解我们不需要每一种都分开求解，其中他们之间的对称性可以帮助我们减少工作量。例如求解到终点

的

后，我们可以用同样的方法来求解

但是需要将终点换为

，这种处理方法在文中叫timeflip，还有其他的f对称，这里不做介绍可以参看原文。

例如下面的代码是求解

路径的方法。

% formula 8.1
function [isok,t,u,v] = LpSpLp(x,y,phi)
    [t,u] = cart2pol(x-sin(phi),y-1+cos(phi));
    if t >= 0
        v = mod2pi(phi-t);
        if v >= 0
            isok = true;
            return
        end
    end
    isok = false;
    t = 0;
    u = 0;
    v = 0;
end

下面我列出文中用于求解路径的主要公式。

function v = mod2pi(x)
    v = rem(x,2*pi);
    if v < -pi
        v = v+2*pi;
    elseif v > pi
        v = v-2*pi;
    end
end

function [tau,omega] = tauOmega(u,v,xi,eta,phi)
    delta = mod2pi(u-v);
    A = sin(u)-sin(delta);
    B = cos(u)-cos(delta)-1;
    t1 = atan2(eta*A-xi*B,xi*A+eta*B);
    t2 = 2*(cos(delta)-cos(v)-cos(u))+3;
    if t2 < 0
        tau = mod2pi(t1+pi);
    else
        tau = mod2pi(t1);
    end
    omega = mod2pi(tau-u+v-phi);
end

% formula 8.2
function [isok,t,u,v] = LpSpRp(x,y,phi)
    [t1,u1] = cart2pol(x+sin(phi),y-1-cos(phi));
    if u1^2 >= 4
        u = sqrt(u1^2-4);
        theta = atan2(2,u);
        t = mod2pi(t1+theta);
        v = mod2pi(t-phi);
        if t >= 0 && v >= 0
            isok = true;
            return
        end
    end
    isok = false;
    t = 0;
    u = 0;
    v = 0;
end

% formula 8.3/8.4
function [isok,t,u,v] = LpRmL(x,y,phi)
    xi = x-sin(phi);
    eta = y-1+cos(phi);
    [theta,u1] = cart2pol(xi,eta);
    if u1 <= 4
        u = -2*asin(u1/4);
        t = mod2pi(theta+u/2+pi);
        v = mod2pi(phi-t+u);
        if t >= 0 && u <= 0
            isok = true;
            return
        end
    end
    isok = false;
    t = 0;
    u = 0;
    v = 0;
end

% formula 8.7
function [isok,t,u,v] = LpRupLumRm(x,y,phi)
    xi = x+sin(phi);
    eta = y-1-cos(phi);
    rho = (2+sqrt(xi^2+eta^2))/4;
    if rho <= 1
        u = acos(rho);
        [t,v] = tauOmega(u,-u,xi,eta,phi);
        if t >= 0 && v <= 0
            isok = true;
            return
        end
    end
    isok = false;
    t = 0;
    u = 0;
    v = 0;
end

% formula 8.8
function [isok,t,u,v] = LpRumLumRp(x,y,phi)
    xi = x+sin(phi);
    eta = y-1-cos(phi);
    rho = (20-xi^2-eta^2)/16;
    if rho >= 0 && rho <= 1
        u = -a