2D path-planning with Waypoint Guidance

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

[Shahnil Sadiq Noorani]

hi please help me in this problem

Answer 1

[Luis Pedro Cobos]

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.

Answer 2

[ROHIT LAD]

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

Answer 3

[ayoub khelifati]

could you give me the title of this book

[AMIR SAEED AMIR SAEED]

if somebody knows the book title, kindly share

[emily buck]

It's from Unmanned Aircraft Systems, by Ella Atkins, Anibal Ollero, Antonios Tsourdos.