Code covered by the BSD License

# Approximate Steering of a Unicycle Under Bounded Model Perturbation Using Ensemble Control

### Aaron Becker (view profile)

Steers unicycle despite model perturb. scaling speed & turning rate by unknown, bounded constant.

EnsembleControlUnicycleUnderBoundedModelPerturbation
function EnsembleControlUnicycleUnderBoundedModelPerturbation
% Code by Tim Bretl and Aaron Becker
% (http://rms.ae.illinois.edu/index.php/people/faculty/tim-bretl/)
%
%  This Matlab code implements algorithm for the paper:
% Becker, A.; Bretl, T.;
% "Approximate Steering of a Unicycle Under Bounded Model Perturbation Using Ensemble Control"
% Robotics, IEEE Transactions on , vol.28, no.3, pp.580-591, June 2012
% doi: 10.1109/TRO.2011.2182117
% URL: http://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=6157635&isnumber=6210446
% preprint at http://rms.ae.illinois.edu/papers/Becker2012.pdf
% ABSTRACT:
% This controller considers the problem of steering a
% nonholonomic unicycle despite model perturbation that scales
% both the forward speed and the turning rate by an unknown but
% bounded constant. We model the unicycle as an ensemble control
% system, show that this system is ensemble controllable, and derive
% an approximate steering algorithm that brings the unicycle to
% within an arbitrarily small neighborhood of any given Cartesian
% position. We apply our work to a differential-drive robot with
% unknown but bounded wheel radius, and validate our approach
% with hardware experiments

%User specifies the ORDER, the goal location (DX, DY), and the spread of the
% ensemble

DX = 1;
DY = 0;
phi = pi/2;
ORDER = 0:6;  %set this to a range to see the effect of different orders of control

set(0,'defaultaxesfontsize',14);
set(0,'defaulttextfontsize',14);
clc; format compact;
close all;
mycolor = ['r','b','g','m','c'];

for order = ORDER
% implements algorithm in Fig. 5, our approximate steering algorithm.
% It acts only to scale the primitive generated by the subroutine COMPUTEPRIMITIVE, which need only be called
% once for given \phi and k. We recommend choosing \phi = \pi/2 and the smallest
% integer k such that \delta^{k-1} < \mu for a given tolerance \mu > 0.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%IF YOU DO NOT HAVE SYMBOLIC TOOLBOX:
% use getprimitives.  Otherwise, use generateABmatrices
%[a,b,N] = getprimitives(order)
[a,b,N] = getprimitiveSYMBOLICTOOLBOX(phi,order);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
a = [a; 0];  %#ok<AGROW>
b = [0; b]; %#ok<AGROW>

ap = (DX*a+DY*b)/2;
bp = (DX*a-DY*b)/2;

% % Distance travelled
% sum(abs(ap)+abs(bp))
vmax = 1;
wheelbase = 0.1;
wmax = 1/wheelbase;

figure(1);
set(gcf,'name','State and Error Trajectories','Position', [695 402 931 559])
% this generates Figure 2, showing an ensemble with  \epsilon \in [0.8, 1.2] moving a unit distance in the
% $x$ direction achieving 4th-order error in $|\epsilon - 1|$, which corresponds to a
% maximum error bound of 0.2^3 = 0.008. Thin red lines show the path followed
% for particular values of \epsilon. The actual robot follows only one of these paths.
% Thick black lines show the entire ensemble at instants of time.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% CREATE FIGURE
k = 5;
e = 1e-4;
n = 100;

deltah = e.^(1./(abs(linspace(0,k,n))));
delta = [-fliplr(deltah(2:end)) deltah];
eC = (1+delta)';
[xC,yC,thetaC,tfinal] = GetTrajectory(ap,bp,eC,vmax,wmax,phi,N,5e-1); %#ok<NASGU>
deltah = e.^(1./(abs(0:4:k)));
delta = [-fliplr(deltah(2:end)) deltah];
eP = (1+delta)';
[xP,yP,thetaP,tfinal] = GetTrajectory(ap,bp,eP,vmax,wmax,phi,N,1e-3); %#ok<NASGU,ASGLU>
% tfinal

subplot(1,3,1:2);
cla;
axis equal;
hold on;
axis(1.5*[-1 1 -1 1]);
set(gca,'box','on','xtick',[],'ytick',[]);

for i=1:length(thetaC)
plot(xC(:,i),yC(:,i),'k.-','linewidth',1);
end
plot(xP',yP','r-');
plot([0 DX],[0 DY],'bo','linewidth',1,'markersize',6);
xlabel('x')
ylabel('y')
title('Path of Ensemble')
drawnow;
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

subplot(1,3,3);
distErrs = ((xC(:,end)-DX).^2 + (yC(:,end)-DY).^2).^.5;
plot(eC,distErrs,'color',mycolor(mod(order, numel(mycolor))+1),'linewidth',2);
hold on
%plot(eC,xC(:,end)-DX,'k-',eC,yC(:,end)-DY,'r-');
axis([min(eC) max(eC) -1e-4  1e-2]);
xlabel('\epsilon (unknown parameter)')
ylabel('Error (Distance)')
title('Error')
%     tSTO(order+1) = tfinal;

end

% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % CREATE MOVIE of the ensemble moving to a goal position
dt = 5e-3;
%
delta = e.^(1./(abs(linspace(0,k,n))));
delta = [-fliplr(delta(2:end)) delta];
eC = (1+delta)';
[xC,yC,thetaC] = GetTrajectory(ap,bp,eC,vmax,wmax,phi,N,dt); %#ok<NASGU>
delta = e.^(1./(abs(0:k)));
delta = [-fliplr(delta(2:end)) delta];
eP = (1+delta)';
[xP,yP,thetaP] = GetTrajectory(ap,bp,eP,vmax,wmax,phi,N,dt);
%
figure(2);
cla;
set(gcf,'position',[50 450 640 640]);
axis equal;
hold on;
axis(2*[-1 1 -1 1]);
set(gca,'units','pixels','position',[10 10 620 620]);
set(gca,'box','on','xtick',[],'ytick',[]);

plot([0 DX],[0 DY],'go','linewidth',1,'markersize',6);
hC = plot(0,0,'k','linewidth',1);
hP = plot(0,0,'r.');
hPdir = zeros(1,length(eP));
for i=1:length(eP)
hPdir(i) = plot(0,0,'b');
end
R = 5e-2;
%whos
for i=1:size(xP,2)
set(hC,'xdata',xC(:,i),'ydata',yC(:,i));
set(hP,'xdata',xP(:,i),'ydata',yP(:,i));
for j=1:length(eP)
set(hPdir(j),'xdata',xP(j,i)+R*[0 cos(eP(j)*thetaP(i))]);
set(hPdir(j),'ydata',yP(j,i)+R*[0 sin(eP(j)*thetaP(i))]);
end
drawnow;
% % FOR SAVING THE MOVIE:
%     F = getframe;
%     fname = sprintf('frames/frame-%010d.jpg',i);
%     imwrite(F.cdata,fname,'jpg','quality',100);
end
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [x,y,h,tfinal] = GetTrajectory(ap,bp,myEpsilon,vmax,wmax,dh,N,dt)
n = length(myEpsilon);
q = zeros(2*n+1,1);
t = 0;
[T,v,w] = getInputs(ap,bp,N,dh,vmax,wmax);
for i=1:length(T)
[t,q] = applyInputs(t,q,T(i),v(i),w(i),myEpsilon,dt);
end
x = q(2:n+1,:);
y = q(n+2:end,:);
h = q(1,:);
tfinal = t(end);

function [t,v,w] = getInputs_OneDirection(b,N,dh,vmax,wmax)
% STRAIGHTS
v1 = sign(b)*vmax;
w1 = zeros(size(v1));
t1 = abs(b)/vmax;
% TURNS
v2 = zeros(N+1,1);
w2 = [wmax*ones(N,1); -wmax];
t2 = [(abs(dh)/wmax)*ones(N,1); N*abs(dh)/wmax];
% PUT TOGETHER
v = reshape([v1 v2]',1,[])';
w = reshape([w1 w2]',1,[])';
t = reshape([t1 t2]',1,[])';

function [t,v,w] = getInputs(bP,bN,N,dh,vmax,wmax)
[tP,vP,wP] = getInputs_OneDirection(bP,N,dh,vmax,wmax);
[tN,vN,wN] = getInputs_OneDirection(bN,N,dh,vmax,wmax);
t = [tP; tN];
v = [vP; vN];
w = [wP; -wN];

function [t,q] = applyInputs(t0,q0,T,v,w,e,dt)
if (T==0)
t = t0;
q = q0;
return;
end
% ASSUMES THAT V AND W ARE CONSTANT (SCALARS).
% Get the number of sampled points along the time horizon.
m = 1+ceil(T/dt);
% Get the number of sampled points along the ensemble curve.
n = length(e);
% Get the vector of times.
t = linspace(0,T,m);
% Get the initial conditions.
h0 = q0(1,end);
x0 = repmat(q0(2:n+1,end),1,m);
y0 = repmat(q0(n+2:end,end),1,m);
% Propagate.
h = h0+w*t;
t = repmat(t,n,1);
e = repmat(e,1,m);
arg = (e.*t)*(w/2);
s = v*t.*e.*mysinc(arg);
x = x0+(s.*cos((e*h0)+arg));
y = y0+(s.*sin((e*h0)+arg));
% Pack everything back together.
t = [t0(1:end-1) t0(end)+t(1,:)];
q = [q0(:,1:end-1) [h; x; y]];

function y=mysinc(x)
y = sinc(x/pi);

function [a,b,N] = getprimitives(order) %#ok<DEFNU>
% returns inputs for Fig 4, COMPUTEPRIMITIVE(\phi; k)
% does not require Symbolic Toolbox
switch (order)
case 0
a = 1;
b = 1;
case 1
a = [1; 2/pi];
b = [1; 1/pi];
case 2
a = [1+(2/pi^2); 2/pi; 2/pi^2];
b = [(9/8)+(1/pi^2); 1/pi; (1/8)+(1/pi^2)];
case 3
a = [1 + (2/pi^2); 3*(8+3*pi^2)/(4*pi^3); 2/pi^2; (24+pi^2)/(12*pi^3)];
b = [(9/8)+(1/pi^2); (6+4*pi^2)/(3*pi^3); (1/8)+(1/pi^2); (6+pi^2)/(6*pi^3)];
case 4
a = [1+(6/pi^4)+(5/(2*pi^2)); (3*(8+3*pi^2)/(4*pi^3)); 8*(3+pi^2)/(3*pi^4); (24+pi^2)/(12*pi^3); (12+pi^2)/(6*pi^4)];
b = [(75/64)+(2/pi^4)+(17/(12*pi^2)); (6+4*pi^2)/(3*pi^3); (25/128)+(3/(pi^4))+(13/(8*pi^2)); (6+pi^2)/(6*pi^3); (3/128)+(1/pi^4)+(5/(25*pi^2))];
otherwise
error('order %d not supported by getprimitives',order);
end
N = order+1;

function [a,b,N]=getprimitiveSYMBOLICTOOLBOX(phi,order)
% returns inputs for Fig 4, COMPUTEPRIMITIVE(\phi; k)
% this function requires Symbolic Toolbox
% A(i,j) = 1/factorial(i-1)  (partial(i-1) (myEps Cos( myEps(j-1)*phi) /
% partial myEps^(i-1) | myEps = 1)
syms myEps;
N = order+1;
A = zeros(N, N);
B = zeros(N, N);
for i = 1:N
fact = 1/factorial(i-1);
for j = 1:N
A(i,j) = fact*subs(diff(myEps*cos(myEps*(j-1)*phi), sym('myEps'), i-1),myEps,1);  %(evaluate partial at myEps = 1)
B(i,j) = fact*subs(diff(myEps*sin(myEps*(j  )*phi), sym('myEps'), i-1),myEps,1);
end
end

% build the r and s matrices
r = zeros(N,1);
r(1) = 1;
s = r;
% solve for the a,b matrices
a = A\r;
b = B\s;