泊车路径规划——Reeds Shepp、应用_泊车r-s轨迹

目录

一、前言

二、舱泊概述

三、RS概述

四、RS基础实现

五、直线圆弧泊车实现

 5.1 垂直停车场景

  5.2 垂直泊车规划实现

---

一、前言

 随着自动驾驶程度不断提升，OEM提升汽车产品竞争力，越来越来的增加汽车的智能功能，其中AVP功能也越来越广泛的得到应用。AVP一般需要360及12超声波传感器硬件，结合感知算法在告诉高算力SOC处理后通过中控显示屏给用户直观体验。舱泊一体的核心技术包括停车位检测及路径规划。

停车位检测方法：基于自由空间（有参考车）、基于视觉识别停车线

路径规划方法：基于Reeds Shepp算法；根据停车位模块提供的停车信息规划出一条安全且易于控制的决策路径。

二、舱泊概述

2.1 智能座舱

智能座舱包括多个核心技术:

• 人机交互技术HMI：主要有语音、面部、手势识别，舍弃传统按键使用虚拟按键AVI对BCM进行控制，L2级自驾功能控制，如360，泊车等，实现人与汽车的智能交互
• 感知技术：感知是智能座舱的基础，包括摄像头、超声波、毫米波、激光、雨光等多种感知传感器实时探测行车环境，实现智能功能包括自动灯光、车道偏离、ACC、AEB、自动泊车、LCA、FCTA，DMS等
• 座舱设计技术：符合人体工程学、通风加热、具有记忆等功能

2.2 舱泊一体

        座舱域控制器上的算力有了富余，剩余的算力可满足一些基本泊车功能的应用需求，如360环视和自动泊车辅助APA等基础泊车功能；泊车功能会涉及到一些人机交互的设计，把泊车功能融入到座舱，座舱域控制器会得到更多的泊车信号，进而能够更好地去做泊车场景下的人机交互设计。把低速泊车功能融合到座舱，可以把原来泊车的控制器省掉，能够节省一定的成本。

2.3 行泊一体

行车辅助指的是在高速驾驶下通过感知器件提供的驾驶辅助，如车道偏离，盲点检测；泊车辅助

指的是在低俗行车时（R档）通过360，APA等提供预警或者自动泊车功能；以往来看这是两套ECU，但随着算力提升及降本，分布式向集成式发展（域控）等需求，行泊将使公用一套传感器，实现行泊一体。

三、RS概述

3.1基本描述

Dubins 曲线不考虑车辆后退，且不允许出现尖瓣, ReedSHeep 曲线的路径中，允许尖瓣存在,可以前进和后退，关于RS的资料很多，简单介绍为：

C:表示圆弧Cycle ， 可以表示左L或者R右，右上标+表示前进，-表示后退

S：表示直线line segment

|：表示反方向

Π/2：表示L或者R的弧长是Π/2，也就是90度半径为1的圆弧l=R*n/180*pai

•  任何长度小于 pi/8 的最佳路径一定是 CSC形式
• 容易得出结论，任何最佳路径一定是由 C 和 S的字段组成。使用一系列的特殊参数，可将所有有限的字段简化为 CCC 或 CSC 的形式

48 个字段，以及相应的缩写形式 C∣C和 C±，

3.2 运动学模型

 汽车二自由度运动学模型：

 积分获得路径点左边：

已知初始坐标点，任意时刻坐标为：

 进一步化简得：

3.3起始点表达

 平面两点的旋转矩阵：实际上旋转的角度为-θ

 缩放：

 3.4字段类型转换

时间变换：将计算出的曲线按照其运动方向进行取反，得到的新的曲线为原曲线相反的曲线；LR不变，正负号交换

反射变换：将计算的曲线按照其沿圆周运动方向取反，得到的曲线与原来的曲线长度相同的新曲线;正负号不变，LR交换

逆向变换：逆向变换通过将原路径按照相反方向行走

通过这些变换可以简化计算量。

3.5路径求解

主要根据化简后的结果与极坐标相互转换求解：

极坐标表达式为：

可以得到CSC的求解为：

3.6总结

• 三段圆弧组成CCC有12种曲线
• 两端圆弧加直线CSC有8种曲线
• 四段圆弧CCCC有8种曲线
• 三段圆弧加直线CCSC有16种曲线
• 四段圆弧加直线CCSCC有4种曲线

加起来共46个曲线字段，对于每一类曲线，先计算特定曲线类型，然后计算出类型变换的其他曲线，从中选择有解且长度最小的曲线，比较上述5类曲线的最小值即最优解。

四、RS基础实现

对Reeds-Shepp 曲线的Matlab实现 - 知乎 (zhihu.com)Matlab代码进行分析、理解和注释

%https://zhuanlan.zhihu.com/p/38940994
%optimal paths for a car that goes both forwards and backwards.pdf
%% Main run
path = FindRSPath(1,1,pi);
PlotPath(path);
%% CCSCC 4
function [isok,path] = CCSCC(x,y,phi)
    Lmin = inf;
    type = repmat([RSPathElem.RS_NOP],[1,5]);
    path = RSPath(type,0,0,0,0,0);
    [isok,t,u,v] = LpRmSLmRp(x,y,phi);
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(17,:),t,-pi/2,u,-pi/2,v);
        end
    end
    [isok,t,u,v] = LpRmSLmRp(x,y,phi); % timeflip
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(17,:),-t,pi/2,-u,pi/2,-v);
        end
    end
    [isok,t,u,v] = LpRmSLmRp(x,y,phi); % reflect
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(18,:),t,-pi/2,u,-pi/2,v);
        end
    end
    [isok,t,u,v] = LpRmSLmRp(x,y,phi); % timeflip + reflect
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(18,:),-t,pi/2,-u,pi/2,-v);
        end
    end
    if Lmin == inf
        isok = false;
    else
        isok = true;
    end
end
%% CCSC 16
function [isok,path] = CCSC(x,y,phi)
    Lmin = inf;
    type = repmat([RSPathElem.RS_NOP],[1,5]);
    path = RSPath(type,0,0,0,0,0);
    [isok,t,u,v] = LpRmSmLm(x,y,phi);
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(5,:),t,-pi/2,u,v,0);
        end
    end
    [isok,t,u,v] = LpRmSmLm(-x,y,-phi); % timeflip
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(5,:),-t,pi/2,-u,-v,0);
        end
    end
    [isok,t,u,v] = LpRmSmLm(x,-y,-phi); % reflect
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(6,:),t,-pi/2,u,v,0);
        end
    end
    [isok,t,u,v] = LpRmSmLm(-x,-y,phi); % timeflip + reflect
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(6,:),-t,pi/2,-u,-v,0);
        end
    end
    [isok,t,u,v] = LpRmSmRm(x,y,phi);
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(9,:),t,-pi/2,u,v,0);
        end
    end
    [isok,t,u,v] = LpRmSmRm(-x,y,-phi); % timeflip
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(9,:),-t,pi/2,-u,-v,0);
        end
    end
    [isok,t,u,v] = LpRmSmRm(x,-y,-phi); % reflect
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(10,:),t,-pi/2,u,v,0);
        end
    end
    [isok,t,u,v] = LpRmSmRm(-x,-y,phi); % timeflip + reflect
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(10,:),-t,pi/2,-u,-v,0);
        end
    end
    % backwards
    xb = x*cos(phi)+y*sin(phi);
    yb = x*sin(phi)-y*cos(phi);
    [isok,t,u,v] = LpRmSmLm(xb,yb,phi);
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(7,:),v,u,-pi/2,t,0);
        end
    end
    [isok,t,u,v] = LpRmSmLm(-xb,yb,-phi); % timeflip
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(7,:),-v,-u,pi/2,-t,0);
        end
    end
    [isok,t,u,v] = LpRmSmLm(xb,-yb,-phi); % reflect
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(8,:),v,u,-pi/2,t,0);
        end
    end
    [isok,t,u,v] = LpRmSmLm(-xb,-yb,phi); % timeflip + reflect
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(8,:),-v,-u,pi/2,-t,0);
        end
    end
    [isok,t,u,v] = LpRmSmRm(xb,yb,phi);
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(11,:),v,u,-pi/2,t,0);
        end
    end
    [isok,t,u,v] = LpRmSmRm(-xb,yb,-phi); % timeflip
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(11,:),-v,-u,pi/2,-t,0);
        end
    end
    [isok,t,u,v] = LpRmSmRm(xb,-yb,-phi); % reflect
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(12,:),v,u,-pi/2,t,0);
        end
    end
    [isok,t,u,v] = LpRmSmRm(-xb,-yb,phi); % timeflip + reflect
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(12,:),-v,-u,pi/2,-t,0);
        end
    end
    if Lmin == inf
        isok = false;
    else
        isok = true;
    end
end
 
%% CCCC 8
function [isok,path] = CCCC(x,y,phi)
    Lmin = inf;
    type = repmat([RSPathElem.RS_NOP],[1,5]);
    path = RSPath(type,0,0,0,0,0);
    [isok,t,u,v] = LpRupLumRm(x,y,phi);
    if isok
        L = abs(t)+2*abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(3,:),t,u,-u,v,0);
        end
    end
    [isok,t,u,v] = LpRupLumRm(-x,y,-phi); % timeflip
    if isok
        L = abs(t)+2*abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(3,:),-t,-u,u,-v,0);
        end
    end
    [isok,t,u,v] = LpRupLumRm(x,-y,-phi); % reflect
    if isok
        L = abs(t)+2*abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(4,:),t,u,-u,v,0);
        end
    end
    [isok,t,u,v] = LpRupLumRm(-x,-y,phi); % timeflip + reflect
    if isok
        L = abs(t)+2*abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(4,:),-t,-u,u,-v,0);
        end
    end
    [isok,t,u,v] = LpRumLumRp(x,y,phi);
    if isok
        L = abs(t)+2*abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(3,:),t,u,u,v,0);
        end
    end
    [isok,t,u,v] = LpRumLumRp(-x,y,-phi); % timeflip
    if isok
        L = abs(t)+2*abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(3,:),-t,-u,-u,-v,0);
        end
    end
    [isok,t,u,v] = LpRumLumRp(x,-y,-phi); % reflect
    if isok
        L = abs(t)+2*abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(4,:),t,u,u,v,0);
        end
    end
    [isok,t,u,v] = LpRumLumRp(-x,-y,phi); % timeflip + reflect
    if isok
        L = abs(t)+2*abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(4,:),-t,-u,-u,-v,0);
        end
    end
    if Lmin == inf
        isok = false;
    else
        isok = true;
    end
end
 
%% CCC 8
function [isok,path] = CCC(x,y,phi)
    Lmin = inf;
    type = repmat([RSPathElem.RS_NOP],[1,5]);
    path = RSPath(type,0,0,0,0,0);
    [isok,t,u,v] = LpRmL(x,y,phi);
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(1,:),t,u,v,0,0);
        end
    end
    [isok,t,u,v] = LpRmL(-x,y,-phi); % timeflip
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(1,:),-t,-u,-v,0,0);
        end
    end
    [isok,t,u,v] = LpRmL(x,-y,-phi); % reflect
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(2,:),t,u,v,0,0);
        end
    end
    [isok,t,u,v] = LpRmL(-x,-y,phi); % timeflip + reflect
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(2,:),-t,-u,-v,0,0);
        end
    end
    % backwards
    xb = x*cos(phi)+y*sin(phi);
    yb = x*sin(phi)-y*cos(phi);
    [isok,t,u,v] = LpRmL(xb,yb,phi);
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(1,:),v,u,t,0,0);
        end
    end
    [isok,t,u,v] = LpRmL(-xb,yb,-phi); % timeflip
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(1,:),-v,-u,-t,0,0);
        end
    end
    [isok,t,u,v] = LpRmL(xb,-yb,-phi); % reflect
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(2,:),v,u,t,0,0);
        end
    end
    [isok,t,u,v] = LpRmL(-xb,-yb,phi); % timeflip + reflect
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(2,:),-v,-u,-t,0,0);
        end
    end
    if Lmin == inf
        isok = false;
    else
        isok = true;
    end
end
%% CSC 12
function [isok,path] = CSC(x,y,phi)
    Lmin = inf;
    type = repmat([RSPathElem.RS_NOP],[1,5]);
    path = RSPath(type,0,0,0,0,0);
    [isok,t,u,v] = LpSpLp(x,y,phi);%L+S+L+
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(15,:),t,u,v,0,0);
        end
    end
    [isok,t,u,v] = LpSpLp(-x,y,-phi); % timeflip, LR不变，正负号取反，L-S-L-
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(15,:),-t,-u,-v,0,0);
        end
    end
    [isok,t,u,v] = LpSpLp(x,-y,-phi); % reflect,正负号不变，LR交换，R+S+R+
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(16,:),t,u,v,0,0);
        end
    end
    [isok,t,u,v] = LpSpLp(-x,-y,phi); % timeflp + reflect, R-L+R-
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(16,:),-t,-u,-v,0,0);
        end
    end
    [isok,t,u,v] = LpSpRp(x,y,phi);% L+S+R+
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(13,:),t,u,v,0,0);
        end
    end
    [isok,t,u,v] = LpSpRp(-x,y,-phi); % timeflip, L-S-R-
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(13,:),-t,-u,-v,0,0);
        end
    end
    [isok,t,u,v] = LpSpRp(x,-y,-phi); % reflect,R+S+L+
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(14,:),t,u,v,0,0);
        end
    end
    [isok,t,u,v] = LpSpRp(-x,-y,phi); % timeflip + reflect, R-S-L-
    if isok
        L = abs(t)+abs(u)+abs(v);
        if Lmin > L
            Lmin = L;
            path = RSPath(RSPathElem.Type(14,:),-t,-u,-v,0,0);
        end
    end
    if Lmin == inf
        isok = false;
    else
        isok = true;
    end
end
%% 特定曲线
% formula 8.1 ( u,t ) = R(x-sin(phi), y-1+cos(phi))
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
 
% formula 8.2 (u,t)=R(x+sin(phi),y-1-cos(phi))
function [isok,t,u,v] = LpSpRp(x,y,phi)
    [t1,u1] = cart2pol(x+sin(phi),y-1-cos(phi));%cart2pol - 将笛卡尔坐标转换为极坐标或柱坐标
    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 xi=x-sin(phi) eta = y-1+cos(phi) 
% 求解结果有争议https://blog.csdn.net/weixin_42301220/article/details/125382518
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);% A = arcsin(u1^2 / 4)  (pi/2=<A<= pi) u=M(A+theta)
        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 L+R+L-R-
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 L+R-L-R+
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 = -acos(rho);
        if u >= -pi/2
            [t,v] = tauOmega(u,u,xi,eta,phi);
            if t >=0 && v >=0
                isok = true;
                return
            end
        end
    end
    isok = false;
    t = 0;
    u = 0;
    v = 0;
end
 
% formula 8.9 L+R-(-pi/2)S-L-
function [isok,t,u,v] = LpRmSmLm(x,y,phi)
    xi = x-sin(phi);
    eta = y-1+cos(phi);
    [theta,rho] = cart2pol(xi,eta);
    if rho >= 2
        r = sqrt(rho^2-4);
        u = 2-r;
        t = mod2pi(theta+atan2(r,-2));
        v = mod2pi(phi-pi/2-t);
        if t >= 0 && u <= 0 && v <= 0
            isok = true;
            return
        end
    end
    isok = false;
    t = 0;
    u = 0;
    v = 0;
end
 
% formula 8.10 L+R-(-pi/2)S-R-
function [isok,t,u,v] = LpRmSmRm(x,y,phi)
    xi = x+sin(phi);
    eta = y-1-cos(phi);
    [theta,rho] = cart2pol(-eta,xi);
    if rho >= 2
        t = theta;
        u = 2-rho;
        v = mod2pi(t+pi/2-phi);
        if t >= 0 && u <= 0 && v <= 0
            isok = true;
            return
        end
    end
    isok = false;
    t = 0;
    u = 0;
    v = 0;
end
 
% formula 8.11 L+R-(-pi/2)R-L-(-pi/2)R+
function [isok,t,u,v] = LpRmSLmRp(x,y,phi)
    xi = x+sin(phi);
    eta = y-1-cos(phi);
    [~,rho] = cart2pol(xi,eta);
    if rho >= 2
        u = 4-sqrt(rho^2-4);
        if u <= 0
            t = mod2pi(atan2((4-u)*xi-2*eta,-2*xi+(u-4)*eta));
            v = mod2pi(t-phi);
            if t >= 0 && v >= 0
                isok = true;
                return
            end
        end
    end
    isok = false;
    t = 0;
    u = 0;
    v = 0;
end
 
%% utility
%各个阶段位姿角度变化值
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
%formula 8.6 omega and tao 
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
 
function path = FindRSPath(x,y,phi)
    rmin = 5; %minimum turning radius
    x = x/rmin;
    y = y/rmin;
    [isok1,path1] = CSC(x,y,phi);
    [isok2,path2] = CCC(x,y,phi);
    [isok3,path3] = CCCC(x,y,phi);
    [isok4,path4] = CCSC(x,y,phi);
    [isok5,path5] = CCSCC(x,y,phi);
    isoks = [isok1, isok2, isok3, isok4, isok5];
    paths = {path1, path2, path3, path4, path5};
    Lmin = inf;
    for i = 1:5
        if isoks(i) == true
            elem = paths{i};
            if Lmin > elem.totalLength
                Lmin = elem.totalLength;
                path = elem;
            end
        end
    end
end
 
function PlotPath(path)
    type = path.type;
%     x = [];
%     y = [];
    seg = [path.t,path.u,path.v,path.w,path.x];
    pvec = [0,0,0];
    rmin = 5;
    for i = 1:5        
        if type(i) == RSPathElem.RS_STRAIGHT
            theta = pvec(3);
            dl = rmin*seg(i);
            dvec = [dl*cos(theta), dl*sin(theta), 0];
            dx = pvec(1)+linspace(0,dvec(1));
            dy = pvec(2)+linspace(0,dvec(2));
%             x = [x,dx];
%             y = [y,dy];
            pvec = pvec+dvec;
        elseif type(i) == RSPathElem.RS_LEFT
            theta = pvec(3);
            dtheta = seg(i);
            cenx = pvec(1)-rmin*sin(theta);
            ceny = pvec(2)+rmin*cos(theta);
            t = theta-pi/2+linspace(0,dtheta);
            dx = cenx+rmin*cos(t);
            dy = ceny+rmin*sin(t);
%             x = [x,dx];
%             y = [y,dy];
            theta = theta+dtheta;
            pvec = [dx(end),dy(end),theta];
            dl = dtheta;
        elseif type(i) == RSPathElem.RS_RIGHT
            theta = pvec(3);
            dtheta = -seg(i);
            cenx = pvec(1)+rmin*sin(theta);
            ceny = pvec(2)-rmin*cos(theta);
            t = theta+pi/2+linspace(0,dtheta);
            dx = cenx+rmin*cos(t);
            dy = ceny+rmin*sin(t);
%             x = [x,dx];
%             y = [y,dy];
            theta = theta+dtheta;
            pvec = [dx(end),dy(end),theta];
            dl = -dtheta;
        else
            % do nothing
        end
        if dl > 0
            plot(dx,dy,'b');
        else
            plot(dx,dy,'r');
        end
        hold on
    end
    hold off
    axis equal
end

五、直线圆弧泊车实现

下图是车辆坐标系示意图，注意前轮转角为fai，根据该示意图要推出轨迹公式：

 车辆后轴中心的轨迹公式为：

可以看到后轴左右轮的轨迹参数是圆，与车速无关，和轴距L，转角，车宽相关。

 5.1 垂直停车场景

  5.2 垂直泊车规划实现

%% run
clc
clear
close all
car_s=[5,11];
P1=[10,10];
P2=[13,10];
vertical_planning(car_s,P1,P2);
%% main 计算ABC 坐标
function path = vertical_planning(xy_car,xyP1,xyP2)
%参数初始化
car_w=170;   %车宽
car_h=470;   %车长
L=270;       %轴距
%d0=150      %起点处，车身与车位线的平行距离
%R=600       %转弯半径（可由方向盘角度控制）
r=[500,600,700];
%R1=R2=R     %R1,R2分别为第一第二个圆弧的转弯半径
dis=100;     %后轮轴与车尾的距离
theta = 0;   %simple scene define
D = 0;       %simple scene define
phi_max = pi*(30/180);
R1 = (L/100)*cot(phi_max);
R2=R1;
d0 = 1;
%cal C point
xc = (xyP1(1)+xyP2(1))/2 + D*sin(theta);
yc = (xyP1(2)+xyP2(2))/2 - D*cos(theta);
%cal B point
yo2 = yc+R2*sin(theta);
xo2 = xc+R2*cos(theta);
alpha = asin((d0+R1+D)/(R1+R2));
xb = xo2 -R2*cos(alpha-theta);
yb = yo2 +R2*sin(alpha-theta);
%cal A point
xo1 = xo2 - (R1+R2)*cos(alpha-theta);
yo1 = yo2 + (R1+R2)*sin(alpha-theta);
ya = yo1-R1*cos(theta);
xa = xo1+R1*sin(theta);
 
path.o2a_x = xy_car(1):0.1:xa;
path.o2a_y = xy_car(2):0.1:ya;
 
 
plot(xy_car(1),xy_car(2),'o')
hold on
plot([xy_car(1),xa],[xy_car(2),ya])
plot(xyP1(1),xyP1(2),'o')
plot(xyP2(1),xyP2(2),'o')
plot([xyP1(1),xyP1(1)],[xyP1(2),xyP1(2)/2])
plot([xyP2(1),xyP2(1)],[xyP2(2),xyP2(2)/2])
xyP2
plot(xa,ya,'o')
plot(xb,yb,'o')
plot(xc,yc,'o')
 
ang = 0:pi/100:2*pi;
xc_a=xo1+R1*cos(ang);
yc_a=yo1+R1*sin(ang);
plot(xc_a,yc_a)
 
xc_b=xo2+R2*cos(ang);
yc_b=yo2+R2*sin(ang);
plot(xc_b,yc_b)
axis equal
end
 

参考：

Reeds-Shepp曲线学习笔记及相关思考_慕羽★的博客-CSDN博客

Reeds_Shepp_trunk.pdf (uran.ru)【自动驾驶】路径规划——ReedsShepp 曲线总结（python实现 | c++实现）_reedsshepp曲线_CHH3213的博客-CSDN博客Reeds_Shepp_trunk.pdf (uran.ru)《optimal paths for a car that goes both forwards and backwards.pdf》

平面旋转矩阵 - 知乎 (zhihu.com)

路径规划 - ReedsShepp 曲线 | Henry-Z (zgh551.github.io)