2D path-planning with Waypoint Guidance
조회 수: 11 (최근 30일)
이전 댓글 표시
I'm trying to implement a a 2D waypoint guidance for a UAV to follow in a 5000x5000 window. I've set up Waypoints at [(0,5000),(0,0),(400,0),(400,5000),(800,5000) ...] where (0,5000) is also the starting point. So it's gonna do the "lawnmower".
The following pictures are my references, so the "target" in pic1 will be my waypoints (the path in the picture is just an example though and those are not the points I've chosen).
The problem I'm having is that in order for this to work, gamma > lambda. But I'm using the x-axis as my reference for the angles so this isn't always true and I'm not sure how to solve this. (I made an attempt, but it's useless) If the UAV is at a close distance to the reference point, the reference point updates till the next one. Please see code for details. I'm using a fixed speed, v=40, and the Kinematic 2D model I'm using is attached, see figure.
if true
clear all; close all;
x0=0;y=5000;yaw=3*pi/2;v=40; % init
x(1,1) = x0; x(2,1) = y; % start pos
x(3,1) = yaw; % heading
xr=[0,0,400,400,800,800,1200,1200,1600,1600]; %waypoints x-coordinate
yr=[y,0,0,y,y,0,0,y,y,0]; %waypoints y-coordinate
n = 5000; % nbr of iter
Ts=.2; % sampling time
figure(1)
plot(x(1,1),x(2,1),'.');
hold on;
ref=1;
K=3; % K>2 is a constant (not sure how to pick this?)
for k = 2:n
r=sqrt((x(1,k-1)-xr(ref))^2+(x(2,k-1)-yr(ref))^2)
if r<10
ref=ref+1;
r=sqrt((x(1,k-1)-xr(ref))^2+(x(2,k-1)-yr(ref))^2);
end
if r>=10
ang=atan((yr(ref)-x(2,k-1))/(xr(ref)-x(1,k-1))); % angle to target
if ang<0
ang=ang+2*pi;
elseif ang>2*pi;
ang=ang-2*pi;
end
if x(3,k-1)<0
x(3,k-1)=x(3,k-1)+2*pi;
elseif x(3,k-1)>2*pi;
x(3,k-1)=x(3,k-1)-2*pi;
end
if ang>x(3,k-1)
lambda=-x(3,k-1)+ang;
else
lambda=x(3,k-1)-ang;
end
u=-K*v*sin(lambda)/r;
else
u=0;
end
if abs(cos(x(3,k-1))) < 1e-3
x(1,k) = x(1,k-1)+Ts*v*0;
if sin(x(3,k-1)) <0
dir=-1;
else
dir=1;
end
x(2,k) = x(2,k-1)+Ts*v*dir;
else
x(1,k) = x(1,k-1)+Ts*v*cos(x(3,k-1));
x(2,k) = x(2,k-1)+Ts*v*sin(x(3,k-1));
end
x(3,k) = x(3,k-1)+Ts*u;
plot(x(1,k),x(2,k),'r.');
hold on;
end
end
답변 (3개)
Luis Pedro Cobos
2017년 5월 14일
Hello:
Did you managed to solve it? I have a very similar problem I am using the X axis as my moving forward, in paper my turning angles work. In real life they don't.
댓글 수: 0
ROHIT LAD
2017년 10월 9일
편집: ROHIT LAD
2017년 10월 9일
Here are the changes that I made. If you maintain all the angles i.e. ang, lamda and gamma positive, it should work.
Cheers!!!
for k = 2:n
r=sqrt((x(1,k-1)-xr(ref))^2+(x(2,k-1)-yr(ref))^2);
if r<10
ref=ref+1;
r=sqrt((x(1,k-1)-xr(ref))^2+(x(2,k-1)-yr(ref))^2);
end
ang=atan2((yr(ref)-x(2,k-1)),(xr(ref)-x(1,k-1))); % angle to target
if ang < 0
ang=ang+2*pi; % always keep ang > 0 and < 360
end
if x(3,k-1)<0 % always keep gamma > 0 and < 360
x(3,k-1)=x(3,k-1)+2*pi;
elseif x(3,k-1)>2*pi;
x(3,k-1)=x(3,k-1)-2*pi;
end
lambda = x(3,k-1)-ang;
u=-K*v*sin(lambda)/r;
x(1,k) = x(1,k-1)+Ts*v*cos(x(3,k-1));
x(2,k) = x(2,k-1)+Ts*v*sin(x(3,k-1));
x(3,k) = x(3,k-1)+Ts*u;
plot(x(1,k),x(2,k),'r.');
hold on;
end
댓글 수: 0
참고 항목
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!