dubin_Reeds-Shepp_

例如下面的代码是求解 L^+S^+L^+ 路径的方法。 % 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 = -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 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 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 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 我们需要定义一些类来辅助之后的工作，第一类定义路径的类型，第二个类定义了对Reeds-Shepp曲线的描述，包括了每一段的长度和每一段的类型。 classdef RSPathElem enumeration RS_NOP, RS_LEFT, RS_STRAIGHT, RS_RIGHT end properties (Constant) Type = [ RSPathElem.RS_LEFT, RSPathElem.RS_RIGHT, RSPathElem.RS_LEFT, RSPathElem.RS_NOP, RSPathElem.RS_NOP ; %1 RSPathElem.RS_RIGHT, RSPathElem.RS_LEFT, RSPathElem.RS_RIGHT, RSPathElem.RS_NOP, RSPathElem.RS_NOP ; %2 RSPathElem.RS_LEFT, RSPathElem.RS_RIGHT, RSPathElem.RS_LEFT, RSPathElem.RS_RIGHT, RSPathElem.RS_NOP ; %3 RSPathElem.RS_RIGHT, RSPathElem.RS_LEFT, RSPathElem.RS_RIGHT, RSPathElem.RS_LEFT, RSPathElem.RS_NOP ; %4 RSPathElem.RS_LEFT, RSPathElem.RS_RIGHT, RSPathElem.RS_STRAIGHT, RSPathElem.RS_LEFT, RSPathElem.RS_NOP ; %5 RSPathElem.RS_RIGHT, RSPathElem.RS_LEFT, RSPathElem.RS_STRAIGHT, RSPathElem.RS_RIGHT, RSPathElem.RS_NOP ; %6 RSPathElem.RS_LEFT, RSPathElem.RS_STRAIGHT, RSPathElem.RS_RIGHT, RSPathElem.RS_LEFT, RSPathElem.RS_NOP ; %7 RSPathElem.RS_RIGHT, RSPathElem.RS_STRAIGHT, RSPathElem.RS_LEFT, RSPathElem.RS_RIGHT, RSPathElem.RS_NOP ; %8 RSPathElem.RS_LEFT, RSPathElem.RS_RIGHT, RSPathElem.RS_STRAIGHT, RSPathElem.RS_RIGHT, RSPathElem.RS_NOP ; %9 RSPathElem.RS_RIGHT, RSPathElem.RS_LEFT, RSPathElem.RS_STRAIGHT, RSPathElem.RS_LEFT, RSPathElem.RS_NOP ; %10 RSPathElem.RS_RIGHT, RSPathElem.RS_STRAIGHT, RSPathElem.RS_RIGHT, RSPathElem.RS_LEFT, RSPathElem.RS_NOP ; %11 RSPathElem.RS_LEFT, RSPathElem.RS_STRAIGHT, RSPathElem.RS_LEFT, RSPathElem.RS_RIGHT, RSPathElem.RS_NOP ; %12 RSPathElem.RS_LEFT, RSPathElem.RS_STRAIGHT, RSPathElem.RS_RIGHT, RSPathElem.RS_NOP, RSPathElem.RS_NOP ; %13 RSPathElem.RS_RIGHT, RSPathElem.RS_STRAIGHT, RSPathElem.RS_LEFT, RSPathElem.RS_NOP, RSPathElem.RS_NOP ; %14 RSPathElem.RS_LEFT, RSPathElem.RS_STRAIGHT, RSPathElem.RS_LEFT, RSPathElem.RS_NOP, RSPathElem.RS_NOP ; %15 RSPathElem.RS_RIGHT, RSPathElem.RS_STRAIGHT, RSPathElem.RS_RIGHT, RSPathElem.RS_NOP, RSPathElem.RS_NOP ; %16 RSPathElem.RS_LEFT, RSPathElem.RS_RIGHT, RSPathElem.RS_STRAIGHT, RSPathElem.RS_LEFT, RSPathElem.RS_RIGHT ; %17 RSPathElem.RS_RIGHT, RSPathElem.RS_LEFT, RSPathElem.RS_STRAIGHT, RSPathElem.RS_RIGHT, RSPathElem.RS_LEFT %18 ]; end end classdef RSPath properties type = repmat([RSPathElem.RS_NOP],[1,5]); t = 0; u = 0; v = 0; w = 0; x = 0; totalLength = 0; end methods function obj = RSPath(type,t,u,v,w,x) obj.type = type; obj.t = t; obj.u = u; obj.v = v; obj.w = w; obj.x = x; obj.totalLength = sum(abs([t,u,v,w,x])); end end end 之后我们需要将上面各公式的计算结果，利用我们前面所说的对称特性进行整合，得到每一种base word的最短路径。 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); 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 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 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 if isok L = abs(t)+abs(u)+abs(v); if Lmin > L Lmin = L; path = RSPath(RSPathElem.Type(16,:),-t,-u,-v,0