Question

In Matlab Write a function dubinsLSR.m that computes a two-dimensional left-straightright (LSR) Dubins path solution. Your function should take the following input arguments (in this order): W0, W3, R. Your function should return Dubins waypoints W1, W2. R is turning radius, and each waypoint Wi must be a Matlab three-element row vector of Cartesian coordinates (xi , yi , ψi). All distance values are specified in meters, and headings ψi are in radians. In the LSR Dubins solution, the aircraft executes a left (counterclockwise) turn from W0 to W1, follows a straight tangent path to W2, then executes a right (clockwise) turn to final waypoint W3. Assume waypoints are sufficiently close to assume a “flat Earth”.

Answer 1

function path = dubins_curve(p1, p2, r, stepsize, quiet)
if nargin < 3
error('Function requires at least two inputs.');
elseif nargin < 4
stepsize = 0;
elseif nargin < 5
quiet = 0; %Default/undefined is not quiet
end
  
if ~quiet
close(findobj('type','figure','name','Dubins curve'));
tic;
end
  
   if(p1(3)~=p2(3))
T = [cos(p1(3)), sin(p1(3));...
cos(p2(3)), sin(p2(3)) ];
Y = [p1(1)*cos(p1(3)) + p1(2)*sin(p1(3)); ...
p2(1)*cos(p2(3)) + p2(2)*sin(p2(3)) ];
X = T \ Y;
if( norm(X-reshape(p1(1:2), [2,1]),2) == r ) && ( norm(X-reshape(p2(1:2),[2,1]),2) == r )
warning('p2 lies on the turning circle from the p2, dubins curve may be suboptimal');
end
end
  
  param = dubins_core(p1, p2, r);
if stepsize <= 0
stepsize = dubins_length(param)/1000;
end
path = dubins_path_sample_many(param, stepsize);
  
    if ~quiet
disp('dubins calculation time'); toc;
  tic; % most of the time is spent on plotting
figure('name','Dubins curve');
plot(path(:,1), path(:,2)); axis equal; hold on
scatter(p1(1), p1(2), 45, '*','r','LineWidth',1); hold on;
scatter(p2(1), p2(2), 45, 'square','b','LineWidth',1); hold on;
text(p1(1), p1(2),'start','HorizontalAlignment','center');
text(p2(1), p2(2),'end','VerticalAlignment','top');
disp('plot drawing time'); toc;
end
end

function path = dubins_path_sample_many( param, stepsize)
if param.flag < 0
path = 0;
return
end
length = dubins_length(param);
path = -1 * ones(floor(length/stepsize), 3);
x = 0;
i = 1;
while x <= length
path(i, :) = dubins_path_sample( param, x );
x = x + stepsize;
i = i + 1;
end
return
end

function length = dubins_length(param)
length = param.seg_param(1);
length = length + param.seg_param(2);
length = length + param.seg_param(3);
length = length * param.r;
end

function end_pt = dubins_path_sample(param, t)
if( t < 0 || t >= dubins_length(param) || param.flag < 0)
end_pt = -1;
return;
end

  tprime = t / param.r;

L_SEG = 1;
S_SEG = 2;
R_SEG = 3;

types = DIRDATA(param.type, :);
param1 = param.seg_param(1);
param2 = param.seg_param(2);
mid_pt1 = dubins_segment( param1, p_init, types(1) );
mid_pt2 = dubins_segment( param2, mid_pt1, types(2) );
  
if( tprime < param1 )
end_pt = dubins_segment( tprime, p_init, types(1) );
elseif( tprime < (param1+param2) )
end_pt = dubins_segment( tprime-param1, mid_pt1, types(2) );
else
end_pt = dubins_segment( tprime-param1-param2, mid_pt2, types(3) );
end

% scale the target configuration, translate back to the original starting point
end_pt(1) = end_pt(1) * param.r + param.p_init(1);
end_pt(2) = end_pt(2) * param.r + param.p_init(2);
end_pt(3) = mod(end_pt(3), 2*pi);
return;
end

function seg_end = dubins_segment(seg_param, seg_init, seg_type)
L_SEG = 1;
S_SEG = 2;
R_SEG = 3;
if( seg_type == L_SEG )
seg_end(1) = seg_init(1) + sin(seg_init(3)+seg_param) - sin(seg_init(3));
seg_end(2) = seg_init(2) - cos(seg_init(3)+seg_param) + cos(seg_init(3));
seg_end(3) = seg_init(3) + seg_param;
elseif( seg_type == R_SEG )
seg_end(1) = seg_init(1) - sin(seg_init(3)-seg_param) + sin(seg_init(3));
seg_end(2) = seg_init(2) + cos(seg_init(3)-seg_param) - cos(seg_init(3));
seg_end(3) = seg_init(3) - seg_param;
elseif( seg_type == S_SEG )
seg_end(1) = seg_init(1) + cos(seg_init(3)) * seg_param;
seg_end(2) = seg_init(2) + sin(seg_init(3)) * seg_param;
seg_end(3) = seg_init(3);
end
end