Reeds-Shepp曲线简介
Reeds-Shepp曲线由 J.AReeds 和 L.A.Sheep 于 1990 年发表的论文 optimal path for a car that goes both forward and backwards中提出，是一种基于 Dubins 算法的改进算法， 相比于Dubins曲线只允许车辆向前运动，Reeds-Shepp曲线运动模型既允许车辆向前运动，也允许车辆向后运动。这就使得在某些情况下可以得出比 Dubins 曲线更优的解。J Reeds和L Shepp证明Reeds Shepp Car从起点到终点的最短路径一定是下面的word的其中之一。word中的"|"表示车辆运动朝向由正向转为反向或者由反向转为正向，带π/2下标表示该段轨迹的弧长/弧长对应的角度为π/2。带β下标的相邻两段轨迹的弧长/弧长对应的角度相等，加号代表前行，减号代表倒车
展开后对应以下48种组合，尽管HJ Sussman 和 G Tang在70多页的论文中已经证明L-R+L-和R-L+R-这两种是多余的，依然还有46种组合相比于Dubins曲线只有6种可能的组合，Reeds-Shepp要复杂很多。
Reeds-Shepp曲线轨迹求解
1、位姿变化
在求解轨迹前，为方便运算，需要先对当前位姿和目标位姿进行变换，假设起始姿态为qi=(xi, yi,ψi)，目标姿态为qg=(xg,yg, ψg)，车辆转弯半径为r=ρ。首先将向量qiqg,平移到原点(0,0)，平移量为(-x1，-y1)。则qi平移到(0,0)，qg平移到(x2-x1,y2 -y1)，然后再将向量qiqg旋转-ψ1度，（注意，是–ψ1度，很多参考资料里写的是ψ1度，是错误的），旋转后再将向量qiqg进行缩放，使其除以最小转弯半径ρ，这样就可以把最小转弯半径归一化为1，变换过程的公式如下所示：
2、六种基础运动公式
Reed-Shepp曲线允许进行倒车，因此车辆存在六种最基础的运动，即左转前进，右转前进ψ，直行前进，左转倒车，右转倒车，直行倒车，车辆从当前位姿（x，y，ψ）分别按照以上基础运动来运动弧长t（直行时代表直线距离）后对应的姿态如下所示：
现假设机器人位于S点处横纵坐标分别为x、y，与x轴夹角为ψ，因此，机器人在S点的状态可以表示为（x，y，ψ），执行L+基础运动前行左转弧长t后，到达G点。求机器人在G点的状态,根据第一个公式可得G点的状态如下：
以下示意图，图中大写字母是为了方便对图中的边和角进行描述，图中的小写字母是角度。
G点的第三个状态是与x轴的夹角，执行L+运动之前为ψ， 由于是向前左转，也就是逆时针运动，逆时针运动为正，又因为L+运动向前左转了弧长t，计算时会将最小转弯半径归一化为1，即上图中边FG和边FS的长度为1，弧长=弧长圆心角x半径，所以弧长t也对应转过的弧度制角度，因此G点与x轴的夹角为ψ+t
由上图容易知道，G点的横纵坐标Xg,Yg满足下式：
所以我们只要求得边GS的长度和∠GSA即可求得Xg,Yg，我们先来求边GS，现在我们已知∠GFS=t，边GF=边FS=1，由余弦公式得，边GS的长度为：
=2(1-cost)
由半角公式得，上式可化为：
所以：
我们再来求∠GSA，过程如下：
将求得的边GS和∠GSA带入，可得G点的横纵坐标Xg,Yg的表达式：
最后再运用一下如下所示的积化和差公式：
利用积化和差公式，我们可以对Xg,Yg的表达式进一步化简，如下：
到这里，我们已经得到了推导前给出的G点的状态表达式：
也就是，文章开头处给出的第一个Reeds-Shepp曲线的基础运动公式：
3、轨迹求解
假设经过位姿变化后的起点和目标点位姿分别为（0，0，0）和（x，y，ψ），假设要求解从起点到目标点的L+S+L+轨迹，则只需要将起点（0，0，0）代入到上面六种基础运动中的L+公式中，将结果再代入S+公式中，再将结果代入L+公式中，得到的结果与已知的目标点位姿（x，y，ψ）等价，因此可以得到3个等式，未知数便是基础运动公式中的t，代入了3次，即t1、t2、t3，或者可以用t、u、v来表示，如下面的公式所示：
同理，可以对其他的轨迹进行分析求解
4、类型变换
事实上，我们并不需要将48或者46种轨迹的求解公式都求出来，我们可以通过以下几种变换，以转换的形式以某种轨迹的求解公式来求其他轨迹的解，常见的变换有以下3种及其组合。
（1）时间变换 (timeflip)
时间变换通过交换轨迹字母上标符号+和- ，即车取反向的行进方向。举个例子， 从起点（0，0, 0）到目标点（x，y，ψ）的L-S-L-轨迹 可以通过从起点（0, 0，0）到点（-x，y，-ψ）的L+S+L+轨迹求得，因为两者关于x轴对称，通俗一点讲，当我们需要求解 从起点（0，0, 0）到目标点（x，y，ψ）的L-S-L-轨迹 的时候，我们可以选择求解从起点（0, 0，0）到点（-x，y，-ψ）的L+S+L+轨迹，由于两个轨迹关于x轴对称，将得到的L+S+L+轨迹关于x轴对称变化后，即可得到我们真正想要求的轨迹，通过以上的转换，我们不再需要求解L-S-L-轨迹的求解公式，就可以得到L-S-L-轨迹。
（2）反射变换 (reflect)
反射变换通过交换轨迹字母中的R和L ，即将R变成L，将L变成R，举个例子， 从起点（0，0, 0）到目标点（x，y，ψ）的R+S+R+轨迹 可以通过从起点（0, 0，0）到点（x，-y，-ψ）的L+S+L+轨迹求得，因为两者关于y轴对称。
（3）逆向变换 (backwards)
第三种转换关系：“backwards”，即将原曲线的路径逆序转换，得到的曲线与原来的曲线长度相同的新曲线。
“backwards”实例图如图所示，蓝色曲线L-S-R-L+与红色曲线 L+R-S-L-不具有以上两种的堆成关系，但是很直观的可以看出L-S-R-L+的逆序排列得到L+R-S-L-,两条路径曲线上的路径进行逆向排列。从起始点O(0,0,0)到目标点A(x,y,θ)的路径可以通过起始点O(0,0,0)到目标点B(x∗cosθ+y∗sinθ,x∗sinθ−y∗cosθ,θ)的路径圆周方向取反计算获得
通过时间变换，反射变换，逆向变换可将48种轨迹简化为9种，也就是说，所有的48种轨迹均可以通过以上9种轨迹的求解公式，直接或间接的求解出来
Reeds-sheep曲线MATLAB仿真
代码：
这里只放出了主要求解代码，详细源代码可去参考链接下载
CSC
function [isok,path,costFinal] = CSCtypeTraj(x,y,phi,start,endp,veh,Types) % X:矩阵旋转后x坐标,Y:矩阵旋转后y坐标,veh:车辆规格
% :角度差,start:起点,endp:终点,Types:路径类型
costFinal = inf;
type = repmat('N',[1,5]); %构造一个1*5数值全为N的向量
path = RSpathStruct(type,0,0,0,0,0);
% according to the formula 8.1 in the paper to calculate L+S+L+
[isok,t,u,v] = LpSpLp(x,y,phi);
if isok
pathTemp = RSpathStruct(Types(15,:),t,u,v,0,0); %将路径类型，角度，半径，航向角存入pathTemp结构体
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(t,u,v,0,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(15,:),t,u,v,0,0);
end
end
% according to the formula 8.1 in the paper and timeflip symmetry to calculate L-S-L-
[isok,t,u,v] = LpSpLp(-x,y,-phi);
if isok
pathTemp = RSpathStruct(Types(15,:),-t,-u,-v,0,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(-t,-u,-v,0,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(15,:),-t,-u,-v,0,0);
end
end
% according to the formula 8.1 in the paper and reflect symmetry to calculate R+S+R+
[isok,t,u,v] = LpSpLp(x,-y,-phi);
if isok
pathTemp = RSpathStruct(Types(16,:),t,u,v,0,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(t,u,v,0,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(16,:),t,u,v,0,0);
end
end
% according to the formula 8.1 in the paper and timeflp + reflect symmetry to calculate R-S-R-
[isok,t,u,v] = LpSpLp(-x,-y,phi);
if isok
pathTemp = RSpathStruct(Types(16,:),-t,-u,-v,0,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(-t,-u,-v,0,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(16,:),-t,-u,-v,0,0);
end
end
% according to the formula 8.1 in the paper calculate L+S+R+
[isok,t,u,v] = LpSpRp(x,y,phi);
if isok
pathTemp = RSpathStruct(Types(13,:),t,u,v,0,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(t,u,v,0,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(13,:),t,u,v,0,0);
end
end
% according to the formula 8.2 in the paper and timeflp symmetry to calculate L-S-R-
[isok,t,u,v] = LpSpRp(-x,y,-phi);
if isok
pathTemp = RSpathStruct(Types(13,:),-t,-u,-v,0,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(-t,-u,-v,0,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(13,:),-t,-u,-v,0,0);
end
end
% according to the formula 8.2 in the paper and reflect symmetry to calculate R+S+L+
[isok,t,u,v] = LpSpRp(x,-y,-phi);
if isok
pathTemp = RSpathStruct(Types(14,:),t,u,v,0,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(t,u,v,0,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(14,:),t,u,v,0,0);
end
end
% according to the formula 8.2 in the paper and timeflip + reflect symmetry to calculate R-S-L-
[isok,t,u,v] = LpSpRp(-x,-y,phi);
if isok
pathTemp = RSpathStruct(Types(14,:),-t,-u,-v,0,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(-t,-u,-v,0,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(14,:),-t,-u,-v,0,0);
end
end
if costFinal == inf
isok = false;
else
isok = true;
end
end
% formula 8.1
function [isok,t,u,v] = LpSpLp(x,y,phi)
[t,u] = cart2pol(x-sin(phi),y-1+cos(phi)); %将笛卡尔坐标转换为极坐标t为角度，u为半径，v为标准化航向角
% if t >= 0
v = limitAngleRange(phi-t);
% if t >= 0 && u >= 0 && v >= 0 % the sign represent forward or backward
isok = true;
return
% end
% end
% isok = false;
% t = 0;
% u = 0;
% v = 0;
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,~]= cart2pol(u,2);
% theta = atan2(2,u);
t = limitAngleRange(t1+theta);
v = limitAngleRange(t-phi);
% if t >= 0 && v >= 0 % the sign represent forward or backward
isok = true;
return
% end
end
isok = false;
t = 0;
u = 0;
v = 0;
end
CCC
function [isok,path,costFinal] = CCCtypeTraj(x,y,phi,start,endp,veh,Types)
costFinal= inf;
type = repmat('N',[1,5]);
path = RSpathStruct(type,0,0,0,0,0);
[isok,t,u,v] = LpRmL(x,y,phi);
% according to the formula 8.3 and 8.4 to calculate L+R-L+ and L+R-L-
if isok
pathTemp = RSpathStruct(Types(1,:),t,u,v,0,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(t,u,v,0,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(1,:),t,u,v,0,0);
end
end
% according to the formula 8.3 and 8.4 and timeflip symmetry to calculate L-R+L- and L-R+L+
[isok,t,u,v] = LpRmL(-x,y,-phi);
if isok
pathTemp = RSpathStruct(Types(1,:),-t,-u,-v,0,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(-t,-u,-v,0,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(1,:),-t,-u,-v,0,0);
end
end
% according to the formula 8.3 and 8.4 and reflect symmetry to calculate R+L-R+ and R+L-R-
[isok,t,u,v] = LpRmL(x,-y,-phi);
if isok
pathTemp = RSpathStruct(Types(2,:),t,u,v,0,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(t,u,v,0,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(2,:),t,u,v,0,0);
end
end
% according to the formula 8.3 and 8.4 and timeflip + reflect symmetry to calculate R-L+R- and R-L+R+
[isok,t,u,v] = LpRmL(-x,-y,phi); % reflect
if isok
pathTemp = RSpathStruct(Types(2,:),-t,-u,-v,0,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(-t,-u,-v,0,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(2,:),-t,-u,-v,0,0);
end
end
% backwards
xb = x*cos(phi)+y*sin(phi);
yb = x*sin(phi)-y*cos(phi);
% according to the formula 8.3 and 8.4 to calculate L-R-L+
[isok,t,u,v] = LpRmL(xb,yb,phi);
if isok
pathTemp = RSpathStruct(Types(1,:),v,u,t,0,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(v,u,t,0,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(1,:),v,u,t,0,0);
end
end
% according to the formula 8.3 and 8.4 and timeflip symmetry to calculate L+R+L-
[isok,t,u,v] = LpRmL(-xb,yb,-phi);
if isok
pathTemp = RSpathStruct(Types(1,:),-v,-u,-t,0,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(-v,-u,-t,0,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(1,:),-v,-u,-t,0,0);
end
end
% according to the formula 8.3 and 8.4 and reflect symmetry to calculate R-L-R+
[isok,t,u,v] = LpRmL(xb,-yb,-phi);
if isok
pathTemp = RSpathStruct(Types(2,:),v,u,t,0,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(v,u,t,0,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(2,:),v,u,t,0,0);
end
end
% according to the formula 8.3 and 8.4 and timeflip + reflect symmetry to calculate R+L+R-
[isok,t,u,v] = LpRmL(-xb,-yb,phi);
if isok
pathTemp = RSpathStruct(Types(2,:),-v,-u,-t,0,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(-v,-u,-t,0,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(2,:),-v,-u,-t,0,0);
end
end
if costFinal == inf
isok = false;
else
isok = true;
end
end
% improvement 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 = limitAngleRange(theta+u/2+pi);
v = limitAngleRange(phi-t+u);
% if t >= 0 && u <= 0
isok = true;
return
% end
end
isok = false;
t = 0;
u = 0;
v = 0;
end
CCCC
function [isok,path,costFinal] = CCCCtypeTraj(x,y,phi,start,endp,veh,Types)
costFinal = inf;
type = repmat('N',[1,5]);
path = RSpathStruct(type,0,0,0,0,0);
[isok,t,u,v] = LpRupLumRm(x,y,phi);
% according to the formula 8.7 in the paper to calculate L+R+L-R-
if isok
pathTemp = RSpathStruct(Types(3,:),t,u,-u,v,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(t,u,-u,v,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(3,:),t,u,-u,v,0);
end
end
% according to the formula 8.7 in the paper and timeflip symmetry to calculate L-R-L+R+
[isok,t,u,v] = LpRupLumRm(-x,y,-phi);
if isok
pathTemp = RSpathStruct(Types(3,:),-t,-u,u,-v,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(t,u,-u,v,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(3,:),-t,-u,u,-v,0);
end
end
% according to the formula 8.7 in the paper and reflect symmetry to calculate R+L+R-L-
[isok,t,u,v] = LpRupLumRm(x,-y,-phi);
if isok
pathTemp = RSpathStruct(Types(4,:),t,u,-u,v,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(t,u,-u,v,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(4,:),t,u,-u,v,0);
end
end
% according to the formula 8.7 in the paper and timeflip + reflect symmetry to calculate R-L-R+L+
[isok,t,u,v] = LpRupLumRm(-x,-y,phi);
if isok
pathTemp = RSpathStruct(Types(1,:),-t,-u,u,-v,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(-t,-u,u,-v,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(4,:),-t,-u,u,-v,0);
end
end
[isok,t,u,v] = LpRumLumRp(x,y,phi);
% according to the formula 8.8 in the paper to calculate L+R-L-R+
if isok
pathTemp = RSpathStruct(Types(3,:),t,-u,-u,v,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(t,-u,-u,v,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(3,:),t,-u,-u,v,0);
end
end
[isok,t,u,v] = LpRumLumRp(-x,y,-phi);
% according to the formula 8.8 in the paper and timeflip symmetry to calculate L-R+L+R-
if isok
pathTemp = RSpathStruct(Types(3,:),-t,u,u,-v,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(-t,u,u,-v,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(3,:),-t,u,u,-v,0);
end
end
% according to the formula 8.8 in the paper and reflect symmetry to calculate R+L-R-L+
[isok,t,u,v] = LpRumLumRp(x,-y,-phi);
if isok
pathTemp = RSpathStruct(Types(4,:),t,-u,-u,v,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(t,-u,-u,v,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(4,:),t,-u,-u,v,0);
end
end
% according to the formula 8.8 in the paper and timeflip + reflect symmetry to calculate R-L+R+L-
[isok,t,u,v] = LpRumLumRp(-x,-y,phi);
if isok
pathTemp = RSpathStruct(Types(4,:),-t,u,u,-v,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(-t,u,u,-v,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(4,:),-t,u,u,-v,0);
end
end
if costFinal == inf
isok = false;
else
isok = true;
end
end
% formula 8.7 in the paper, tauOmega() is formula 8.6
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 >= 0 && rho <= 1
u = acos(rho);
[t,v] = tauOmega(u,-u,xi,eta,phi);
% if t >= 0 && v <= 0 % the sign represent forward or backward
isok = true;
return
% end
end
isok = false;
t = 0;
u = 0;
v = 0;
end
% formula 8.8 in the paper
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.6
function [tau,omega] = tauOmega(u,v,xi,eta,phi)
delta = limitAngleRange(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)-2*cos(v)-2*cos(u))+3;
if t2 < 0
tau = limitAngleRange(t1+pi);
else
tau = limitAngleRange(t1);
end
omega = limitAngleRange(tau-u+v-phi);
end
CCSC
function [isok,path,costFinal] = CCSCtypeTraj(x,y,phi,start,endp,veh,Types)
costFinal = inf;
type = repmat('N',[1,5]);
path = RSpathStruct(type,0,0,0,0,0);
% according to the formula 8.9 in the paper to calculate L+R-S-L-
[isok,t,u,v] = LpRmSmLm(x,y,phi);
if isok
pathTemp = RSpathStruct(Types(5,:),t,-pi/2,u,v,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(t,-pi/2,u,v,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(5,:),t,-pi/2,u,v,0);
end
end
% according to the formula 8.9 in the paper and timeflip symmetry to calculate L-R+S+L+
[isok,t,u,v] = LpRmSmLm(-x,y,-phi);
if isok
pathTemp = RSpathStruct(Types(5,:),-t,pi/2,-u,-v,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(-t,pi/2,-u,-v,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(5,:),-t,pi/2,-u,-v,0);
end
end
% according to the formula 8.9 in the paper and reflect symmetry to calculate R+L-S-R-
[isok,t,u,v] = LpRmSmLm(x,-y,-phi);
if isok
pathTemp = RSpathStruct(Types(6,:),t,-pi/2,u,v,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(t,-pi/2,u,v,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(6,:),t,-pi/2,u,v,0);
end
end
% according to the formula 8.9 in the paper and timeflip + reflect symmetry to calculate R-L+S+R+
[isok,t,u,v] = LpRmSmLm(-x,-y,phi);
if isok
pathTemp = RSpathStruct(Types(6,:),-t,pi/2,-u,-v,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(-t,pi/2,-u,-v,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(6,:),-t,pi/2,-u,-v,0);
end
end
% according to the formula 8.10 in the paper to calculate L+R-S-R-
[isok,t,u,v] = LpRmSmRm(x,y,phi);
if isok
pathTemp = RSpathStruct(Types(9,:),t,-pi/2,u,v,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(t,-pi/2,u,v,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(9,:),t,-pi/2,u,v,0);
end
end
% according to the formula 8.10 in the paper and timeflip symmetry to calculate L-R+S+R+
[isok,t,u,v] = LpRmSmRm(-x,y,-phi);
if isok
pathTemp = RSpathStruct(Types(9,:),-t,pi/2,-u,-v,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(-t,pi/2,-u,-v,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(9,:),-t,pi/2,-u,-v,0);
end
end
% according to the formula 8.10 in the paper and reflect symmetry to calculate R+L-S-L-
[isok,t,u,v] = LpRmSmRm(x,-y,-phi);
if isok
pathTemp = RSpathStruct(Types(10,:),t,-pi/2,u,v,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(t,-pi/2,u,v,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(10,:),t,-pi/2,u,v,0);
end
end
% according to the formula 8.10 in the paper and timeflip + reflect symmetry to calculate R-L+S+L+
[isok,t,u,v] = LpRmSmRm(-x,-y,phi);
if isok
pathTemp = RSpathStruct(Types(10,:),-t,pi/2,-u,-v,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(-t,pi/2,-u,-v,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(10,:),-t,pi/2,-u,-v,0);
end
end
if costFinal == inf
isok = false;
else
isok = true;
end
end
% formula 8.9 in the paper
function [isok,t,u,v] = LpRmSmLm(x,y,phi)
xi = x+sin(phi);
eta = y-1-cos(phi);
% xi = x-sin(phi);
% eta = y-1+cos(phi);
[theta,rho] = cart2pol(-eta,xi);
% [theta,rho] = cart2pol(xi,eta);
if rho >= 2
[theta1,~] = cart2pol(sqrt(rho^2-4),-2);
t = limitAngleRange(theta-theta1);
u = 2-theta1;
v = limitAngleRange(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 in the paper
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 = limitAngleRange(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
CSCC
% CSCC is the opposite direction of the CCSC
function [isok,path,costFinal] = CSCCtypeTraj(x,y,phi,start,endp,veh,Types)
costFinal = inf;
type = repmat('N',[1,5]);
path = RSpathStruct(type,0,0,0,0,0);
% backwards
xb = x*cos(phi)+y*sin(phi);
yb = x*sin(phi)-y*cos(phi);
% according to the formula 8.9 in the paper to calculate L-S-R-L+
[isok,t,u,v] = LpRmSmLm(xb,yb,phi);
if isok
pathTemp = RSpathStruct(Types(7,:),v,u,-pi/2,t,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(v,u,-pi/2,t,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(7,:),v,u,-pi/2,t,0);
end
end
% according to the formula 8.9 in the paper and timeflip symmetry to calculate L-S-R-L+
[isok,t,u,v] = LpRmSmLm(-xb,yb,-phi);
if isok
pathTemp = RSpathStruct(Types(7,:),-v,-u,pi/2,-t,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(-v,-u,pi/2,-t,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(7,:),-v,-u,pi/2,-t,0);
end
end
% according to the formula 8.9 in the paper and reflect symmetry to calculate R-S-L-R+
[isok,t,u,v] = LpRmSmLm(xb,-yb,-phi);
if isok
pathTemp = RSpathStruct(Types(8,:),v,u,-pi/2,t,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(v,u,-pi/2,t,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(8,:),v,u,-pi/2,t,0);
end
end
% according to the formula 8.9 in the paper and timeflip + reflect symmetry to calculate R+S+L+R-
[isok,t,u,v] = LpRmSmLm(-xb,-yb,phi);
if isok
pathTemp = RSpathStruct(Types(8,:),-v,-u,pi/2,-t,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(-v,-u,pi/2,-t,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(8,:),-v,-u,pi/2,-t,0);
end
end
% according to the formula 8.10 in the paper to calculate R-S-R-L+
[isok,t,u,v] = LpRmSmRm(xb,yb,phi);
if isok
pathTemp = RSpathStruct(Types(11,:),v,u,-pi/2,t,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(v,u,-pi/2,t,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(11,:),v,u,-pi/2,t,0);
end
end
% according to the formula 8.10 in the paper and to timeflip symmetry calculate R+S+R+L-
[isok,t,u,v] = LpRmSmRm(-xb,yb,-phi);
if isok
pathTemp = RSpathStruct(Types(11,:),-v,-u,pi/2,-t,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(-v,-u,pi/2,-t,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(11,:),-v,-u,pi/2,-t,0);
end
end
% according to the formula 8.10 in the paper and to reflect symmetry calculate L-S-L-R+
[isok,t,u,v] = LpRmSmRm(xb,-yb,-phi);
if isok
pathTemp = RSpathStruct(Types(12,:),v,u,-pi/2,t,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(v,u,-pi/2,t,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(12,:),v,u,-pi/2,t,0);
end
end
% according to the formula 8.10 in the paper and to timeflip + reflect symmetry calculate L+S+L+R-
[isok,t,u,v] = LpRmSmRm(-xb,-yb,phi);
if isok
pathTemp = RSpathStruct(Types(12,:),-v,-u,pi/2,-t,0);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(-v,-u,pi/2,-t,0);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(12,:),-v,-u,pi/2,-t,0);
end
end
if costFinal == inf
isok = false;
else
isok = true;
end
end
% formula 8.9 in the paper
function [isok,t,u,v] = LpRmSmLm(x,y,phi)
xi = x+sin(phi);
eta = y-1-cos(phi);
% xi = x-sin(phi);
% eta = y-1+cos(phi);
[theta,rho] = cart2pol(-eta,xi);
% [theta,rho] = cart2pol(xi,eta);
if rho >= 2
[theta1,~] = cart2pol(sqrt(rho^2-4),-2);
t = limitAngleRange(theta-theta1);
u = 2-theta1;
v = limitAngleRange(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 in the paper
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 = limitAngleRange(t+pi/2-phi);
% if t >= 0 && u <= 0 && v <= 0
isok = true;
return
% end
end
isok = false;
t = 0;
u = 0;
v = 0;
endCCSCC
function [isok,path,costFinal] = CCSCCtypeTraj(x,y,phi,start,endp,veh,Types)
costFinal = inf;
type = repmat('N',[1,5]);
path = RSpathStruct(type,0,0,0,0,0);
[isok,t,u,v] = LpRmSLmRp(x,y,phi);
% according to the formula 8.11 to calculate L+R-S-L-R+
if isok
pathTemp = RSpathStruct(Types(17,:),t,-pi/2,u,-pi/2,v);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(t,-pi/2,u,-pi/2,v);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(17,:),t,-pi/2,u,-pi/2,v);
end
end
% according to the formula 8.11 and timeflip symmetry to calculate L+R-S-L-R+
[isok,t,u,v] = LpRmSLmRp(-x,y,-phi);
if isok
pathTemp = RSpathStruct(Types(17,:),-t,pi/2,-u,pi/2,-v);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(-t,pi/2,-u,pi/2,-v);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(17,:),-t,pi/2,-u,pi/2,-v);
end
end
% according to the formula 8.11 and reflect symmetry to calculate R+L-S-R-L+
[isok,t,u,v] = LpRmSLmRp(x,-y,-phi);
if isok
pathTemp = RSpathStruct(Types(18,:),t,-pi/2,u,-pi/2,v);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(t,-pi/2,u,-pi/2,v);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(18,:),t,-pi/2,u,-pi/2,v);
end
end
% according to the formula 8.11 and timeflip + reflect symmetry to calculate R-L+S+R+L-
[isok,t,u,v] = LpRmSLmRp(-x,-y,phi);
if isok
pathTemp = RSpathStruct(Types(18,:),-t,pi/2,-u,pi/2,-v);
[traj_x,traj_y,traj_th] = trajPointGet(pathTemp,start,veh);
logi_final = reachGoalJudge(traj_x,traj_y,traj_th,endp);
costTot = trajCostGet(-t,pi/2,-u,pi/2,-v);
if costFinal > costTot && logi_final == 1
costFinal = costTot;
path = RSpathStruct(Types(18,:),-t,pi/2,-u,pi/2,-v);
end
end
if costFinal == inf
isok = false;
else
isok = true;
end
end
% improvement formula 8.11
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 = limitAngleRange(atan2((4-u)*xi-2*eta,-2*xi+(u-4)*eta));
v = limitAngleRange(t-phi);
% if t >= 0 && v >= 0
isok = true;
return
% end
% end
end
isok = false;
t = 0;
u = 0;
v = 0;
end
% the formula 8.11 in the paper
% formula 8.11
% function [isok,t,u,v] = LpRmSLmRp(x,y,phi)
% xi = x+sin(phi);
% eta = y-1-cos(phi);
% [theta,rho] = cart2pol(xi,eta);
% if rho >= 2
% t = limitAngleRange(theta - acos(-2/rho));
% if t > 0
% u = 4 - (xi + 2 * cos(t))/sin(t);
% v = limitAngleRange(t-phi);
% % if t > 0 && v >= 0
% isok = true;
% return
% % end
% end
% end
% isok = false;
% t = 0;
% u = 0;
% v = 0;
% end免责声明
本文章中的资料来源于互联网，这里只进行了总结，目的是为了更好的体验，资料查找更高效，如果资料作者有任何异议，都欢迎与我联系沟通删除。
参考链接
https://github.com/BaiChenuXu/ReedsSheppCurve-Application-Improvement
4728
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
