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

13 次查看（过去 30 天）
Nicholas
Nicholas 2024-5-2
回答： Rahul about 10 hours 前
Ran in:
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
  2 个评论
Steven Lord
Steven Lord 2024-5-2
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
Francisco J. Triveno Vargas 2024-7-18
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

请先登录，再进行评论。

请先登录，再回答此问题。

回答（1 个）

Rahul
Rahul about 10 hours 前
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!

类别

在 Help Center 和 File Exchange 中查找有关 Programming 的更多信息

标签

产品


版本

R2023b

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by