MatLab not plotting as expected and can't determine a fix.

This code is supposed to demonstrate a vehicle/cart moving towards a goal coordinate that is moving over time. How the cart moves is based upon it's location and heading in relation to the goal. The problem occurs when the y or x coordinate of the cart gets extremely close to the goal and the angle between the cart and goal suddenly becomes very large, causing it to rotate while the goal moves away. This creates a jarring collection lines that are along the path of the goal. I've been trying various different modifications to the algorithm (which is the only thing I'm changing to alter the path finding of the cart) but none have fixed this problem.

clear; clc; close;
%% ODE45
tspan=0:0.1:60;
u0=[10,7,5];
[tout,uout]=ode45(@xytheta,tspan,u0);
xg=1.25*cos(2*tspan');
yg=1.25*sin(2*tspan');
%% Plot x and y vs Time
figure(1);
hold on
plot(tout,uout(:,2),'--r');
plot(tout,uout(:,3),'-.b');
plot(tspan,yg,'g')
plot(tspan,xg,'m')
ylabel('Distance (m)');
xlabel('Time (s)');
title('X and Y vs Time');
legend('x distance','y distance');
hold off

%% Plot Theta vs Time
figure(2);
Hding=uout(:,1)*(180/pi);
plot(tout,Hding,'g');
ylabel('Heading (deg)');
xlabel('Time (s)');
title('Heading vs Time');

%% Plot Path of Vehicle
figure(3);
hold on
plot(uout(:,2),uout(:,3),'o','MarkerSize',3);
plot(xg,yg,'r');
xlabel('x Distance (m)');
ylabel('y Distance (m)');
title('Path of Vehicle');
axis equal

%% Function for ODE45
function dudt=xytheta(t,u)
D=0.2;
w=0.3;
R0=1.25;
wg=2;
xg=R0*cos(wg*t);
yg=R0*sin(wg*t);
L=[xg-u(2),yg-u(3)];
gamma=atan2(L(2),L(1));
Omega=gamma-u(1); %% This is the begging of the Algorithm
if Omega<-pi
    Omega=Omega+(2*pi);
elseif Omega>pi
    Omega=Omega-(2*pi);
end
if abs(L(1))>5||abs(L(2))>5 %The values may seem random as I have been trying a bunch of different combinations
    if Omega>(8*pi)/9
        PhiL=20;
        PhiR=-20;
    elseif Omega<(-pi*8)/9
        PhiL=-20;
        PhiR=20;
    elseif Omega<=(8*pi)/9&&Omega>(4*pi)/9
        PhiL=10;
        PhiR=-10;
    elseif Omega>=(-8*pi)/9&&Omega<(-4*pi)/9
        PhiL=-10;
        PhiR=10;
    elseif Omega<=(4*pi)/9&&Omega>(2*pi)/9
        PhiL=5;
        PhiR=-5;
    elseif Omega>=(-4*pi)/9&&Omega<(-2*pi)/9
        PhiL=-5;
        PhiR=5;
    elseif Omega<=(2*pi)/9&&Omega>(pi)/18
        PhiL=2;
        PhiR=-2;
    elseif Omega>=(-2*pi)/9&&Omega<(-pi)/18
        PhiL=-2;
        PhiR=2;
    else
        PhiL=100;
        PhiR=100;
    end
elseif (abs(L(1))<=5&&abs(L(2))<=5)&&(abs(L(1))>1||abs(L(2))>1)
    if abs(L(1))>2.5||abs(L(2))>2.5
        if Omega>(8*pi)/9
            PhiL=20;
            PhiR=-20;
        elseif Omega<(-pi*8)/9
            PhiL=-20;
            PhiR=20;
        elseif Omega<=(8*pi)/9&&Omega>(4*pi)/9
            PhiL=10;
            PhiR=-10;
        elseif Omega>=(-8*pi)/9&&Omega<(-4*pi)/9
            PhiL=-10;
            PhiR=10;
        elseif Omega<=(4*pi)/9&&Omega>(2*pi)/9
            PhiL=5;
            PhiR=-5;
        elseif Omega>=(-4*pi)/9&&Omega<(-2*pi)/9
            PhiL=-85;
            PhiR=5;
        elseif Omega<=(2*pi)/9&&Omega>(pi)/18
            PhiL=2;
            PhiR=-2;
        elseif Omega>=(-2*pi)/9&&Omega<(-pi)/18
            PhiL=-2;
            PhiR=2;
        else
            PhiL=120;
            PhiR=120;
        end
    elseif abs(L(1))<=2.5&&abs(L(2))<=2.5
        if Omega>(8*pi)/9
            PhiL=20;
            PhiR=-20;
        elseif Omega<(-pi*8)/9
            PhiL=-20;
            PhiR=20;
        elseif Omega<=(8*pi)/9&&Omega>(4*pi)/9
            PhiL=10;
            PhiR=-10;
        elseif Omega>=(-8*pi)/9&&Omega<(-4*pi)/9
            PhiL=-10;
            PhiR=10;
        elseif Omega<=(4*pi)/9&&Omega>(2*pi)/9
            PhiL=5;
            PhiR=-5;
        elseif Omega>=(-4*pi)/9&&Omega<(-2*pi)/9
            PhiL=-5;
            PhiR=5;
        elseif Omega<=(2*pi)/9&&Omega>(pi)/18
            PhiL=3;
            PhiR=-3;
        elseif Omega>=(-2*pi)/9&&Omega<(-pi)/18
            PhiL=-3;
            PhiR=3;
        else
            PhiL=120;
            PhiR=120;
        end
    end
elseif abs(L(1))<=1&&abs(L(2))<=1
    if abs(xg)>0.1&&abs(yg)>0.1
        if Omega>(8*pi)/9
            PhiL=20;
            PhiR=-20;
        elseif Omega<(-pi*8)/9
            PhiL=-20;
            PhiR=20;
        elseif Omega<=(8*pi)/9&&Omega>(4*pi)/9
            PhiL=10;
            PhiR=-10;
        elseif Omega>=(-8*pi)/9&&Omega<(-4*pi)/9
            PhiL=-10;
            PhiR=10;
        elseif Omega<=(4*pi)/9&&Omega>(2*pi)/9
            PhiL=2.5;
            PhiR=-2.5;
        elseif Omega>=(-4*pi)/9&&Omega<(-2*pi)/9
            PhiL=-2.5;
            PhiR=2.5;
        elseif Omega<=(2*pi)/9&&Omega>(pi)/18
            PhiL=3;
            PhiR=-3;
        elseif Omega>=(-2*pi)/9&&Omega<(-pi)/18
            PhiL=-3;
            PhiR=3;
        else
            PhiL=90;
            PhiR=90;
        end
    elseif abs(xg)<=0.1||abs(yg)<=0.1
        if Omega>(8*pi)/9
            PhiL=20;
            PhiR=-20;
        elseif Omega<(-pi*8)/9
            PhiL=-20;
            PhiR=20;
        elseif Omega<=(8*pi)/9&&Omega>(4*pi)/9
            PhiL=10;
            PhiR=-10;
        elseif Omega>=(-8*pi)/9&&Omega<(-4*pi)/9
            PhiL=-10;
            PhiR=10;
        elseif Omega<=(4*pi)/9&&Omega>(2*pi)/9
            PhiL=5;
            PhiR=-5;
        elseif Omega>=(-4*pi)/9&&Omega<(-2*pi)/9
            PhiL=-5;
            PhiR=5;
        elseif Omega<=(2*pi)/9&&Omega>(pi)/18
            PhiL=3;
            PhiR=-3;
        elseif Omega>=(-2*pi)/9&&Omega<(-pi)/18
            PhiL=-3;
            PhiR=3;
        else
            PhiL=15;
            PhiR=15;
        end
    end
end % This is the end of the Algorithm
dudt=[(D/w)*(PhiL-PhiR);(D/2)*((PhiL+PhiR)/2)*cos(u(1));(D/2)*((PhiL+PhiR)/2)*sin(u(1))];
end

[Steven Lord]

What is the mathematical form of the differential equations you're trying to solve? Your if / elseif / else blocks smell fishy to me.

in order to avoid the "circling the drain" behavior that I think you're concerned about, consider using an event function to stop the solver when you're "close enough" to the goal.

[Francisco J. Triveno Vargas]

My friend i change two things in your graphs

The first one

tspan=0:0.001:30;

and second one about the heading angle that in fact is between 0 and 360, for this reason it is possible to use the values between -180 and +180 degrees.

Hding=wrapTo180(uout(:,1)*(180/pi));

The results

Answer 1

[Rahul]

Hi Nicholas,

Assuming that you are trying to modify the cart's pathfinding algorithm to avoid erratic heading changes and jerky movements when it gets very close to the moving goal.

The issue you're experiencing seems to be emerging from how the cart interprets angular differences (Omega) and adjusts its heading. When the cart is close to the goal, the angular difference can change rapidly, which can lead to erratic movements, especially when the goal is dynamic.

To prevent large, sudden changes in orientation, you can introduce a gain factor to scale down Omega before using it for control logic, like a proportional control for the angular adjustment.

Kp = 0.5; % Proportional gain 
Omega = Kp * (gamma, u(1));

In the ‘xytheta’ function, cart behavior can be modified when it is near the goal. For instance, If the distance |L| between the cart and the goal is below a certain threshold (e.g., 0.1), you can stop or reduce the control inputs.

distance = norm(L); 
if distance < 0.1 
    PhiL = 0; % Stop motors 
    PhiR = 0; 
elseif distance < 1 
    PhiL = 20 * distance; % Scale speed based on proximity 
    PhiR = 20 * distance; 
end 

Since the goal is moving, you can use the goal's angular velocity (wg) and current position anticipate its future position:

prediction_horizon = 0.5; % Predict 0.5 seconds ahead 
xg_future = R0 * cos(wg * (t + prediction_horizon)); 
yg_future = R0 * sin(wg * (t + prediction_horizon)); 
L = [xg_future - u(2), yg_future - u(3)]; 

Further, in order to smooth out angular oscillations, you can implement a damping term based on the rate of change of Omega, as shown below:

Omega_damping = 0.1; % Damping factor 
Omega = Omega - Omega_damping * (Omega - previous_Omega); 
previous_Omega = Omega; % Track last Omega value

Final ODE function ‘xytheta’ would look like the following snippet, after the above-mentioned modifications:

function dudt = xytheta(t, u) 
    D = 0.2; 
    w = 0.3; 
    R0 = 1.25; 
    wg = 2; 
 
    % Predict goal position 
    prediction_horizon = 0.5; 
    xg = R0 * cos(wg * (t + prediction_horizon)); 
    yg = R0 * sin(wg * (t + prediction_horizon)); 
 
    L = [xg - u(2), yg - u(3)]; 
    gamma = atan2(L(2), L(1)); 
     
    % Smooth heading adjustment 
    Kp = 0.5; 
    Omega = Kp * (gamma - u(1)); 
    Omega = mod(Omega + pi, 2 * pi) - pi; % Wrap within [-pi, pi] 
 
    % Control logic 
    distance = norm(L); 
    if distance < 0.1 
        PhiL = 0; 
        PhiR = 0; 
    elseif distance < 1 
        PhiL = 20 * distance; 
        PhiR = 20 * distance; 
    else 
        PhiL = 100; 
        PhiR = 100; 
    end 
     
    % Dynamics 
    dudt = [(D / w) * (PhiL - PhiR); 
            (D / 2) * ((PhiL + PhiR) / 2) * cos(u(1)); 
            (D / 2) * ((PhiL + PhiR) / 2) * sin(u(1))]; 
end

To know more about the usage of ‘norm’ function, refer to the documentation link mentioned below:

https://in.mathworks.com/help/releases/R2023b/matlab/ref/norm.html

Best!