How to calculate the gradient at a point on a grid in a potential field with decimal coordinates?

6 次查看（过去 30 天）
Astik Srivastava
Astik Srivastava 2022-5-5
回答： Balavignesh 2023-12-6
I am using matlab for planning a path around dynamic obstacles using a time varying potential field. The code looks as follows:
%dALF-LQR algorithm for co-operative obstacle avoidance and path planning in a dynamic
%environment for micro multirotor UAV.
%Basic algorithm for obstacle aviodance
%og mpc = 5.*exp(-((x-1).^2+(y-3).^2)/0.5);
[x,y] = meshgrid(linspace(-50,50,100));
x_pos = -20;
y_pos = -20;
x_stat = -10;
y_stat = -5;
x_goal = 20;
y_goal = 20;
k_att = 1;
k_rep = 15;
t = 0:0.01:2;
theta_1 = 0;
theta_2 = 0;
path = [];
dt = 0.01;
a = 0.05;
b = 0.09;
%gif('obstacle_avoidance_quiver_morphed.gif')
for i = 2:length(t)
%sqrt((x-10-v3(1)).^2+(y-10-v3(2)).^2)
path(i,1) = x_pos;
path(i,2) = y_pos;
v_ob1 = [7;9];
v_ob2 = [0;-25];
v_wp = [10;-28];
v1 = [7.*t(i),9.*t(i)]; %velocity of obstacle 1
v2 = [0.*t(i),-25.*t(i)]; %velocity of obstacle 2
v3 = [-10.*t(i),2.*t(i)]; %velocity of waypoint
%morphed potential function
%z = 30.*(a.*((x-2-v1(1)).*cos(theta_1)+(y-2-v1(2)).*sin(theta_1)).^2+b.*((x-2-v1(1)).*sin(theta_1)-(y-2-v1(2)).*cos(theta_1)).^2+1).^(-1) + k_rep.*(a.*((x-15-v2(1)).*cos(theta_2)+(y-15-v2(2)).*sin(theta_2)).^2+b.*(((x-15-v2(1)).*sin(theta_2)-(y-15-v2(2)).*cos(theta_2)).^2+1)).^(-1) + k_att.*sqrt((x-x_goal-v3(1)).^2+(y-y_goal-v3(2)).^2)+ 15.*(0.5.*((x-x_stat).^2+(y-y_stat).^2)+1).^(-1);
%non-morphed potential
z = k_rep.*(0.5.*((x-2-v1(1)).^2+(y-2-v1(2)).^2)+1).^(-1) + k_rep.*(0.5.*((x-15-v2(1)).^2+(y-15-v2(2)).^2)+1).^(-1) + k_att.*sqrt((x-x_goal-v3(1)).^2+(y-y_goal-v3(2)).^2)+ k_rep.*(0.5.*((x-x_stat).^2+(y-y_stat).^2)+1).^(-1);
[f1,f2] = gradient(z);
f3 = -1.*f1;
f4 = -1.*f2;
fy = -k_rep.*(2.*(y_pos-2-v1(2)))./((x_pos-2-v1(1)).^2+(y_pos-2-v1(2)).^2+1).^2 -k_rep.*(2.*(y_pos-15-v2(2)))./((x_pos-15-v2(1)).^2+(y_pos-15-v2(2)).^2+1).^2 + k_att.*(y_pos-y_goal-v3(2))./(sqrt((x_pos-x_goal-v3(1)).^2+(y_pos-y_goal-v3(2)).^2))-15.*(2.*(y_pos-y_stat))./((x_pos-x_stat).^2+(y_pos-y_stat).^2+1).^2; %manually calculated y-gradient of z
fx = -k_rep.*(2.*(x_pos-2-v1(1)))./((x_pos-2-v1(1)).^2+(y_pos-2-v1(2)).^2+1).^2 -k_rep.*(2.*(x_pos-15-v2(1)))./((x_pos-15-v2(1)).^2+(y_pos-15-v2(2)).^2+1).^2 + k_att.*(x_pos-x_goal-v3(1))./(sqrt((x_pos-x_goal-v3(1)).^2+(y_pos-y_goal-v3(2)).^2))-15.*(2.*(x_pos-x_stat))./((x_pos-x_stat).^2+(y_pos-y_stat).^2+1).^2; %manually calculated x-gradient of z
y_pos = y_pos - fy.*t(i);
x_pos = x_pos - fx.*t(i);
z_pos = k_rep.*(0.5.*((x_pos-2-v1(1)).^2+(y_pos-2-v1(2)).^2)+1).^(-1) + k_rep.*(0.5.*((x_pos-15-v2(1)).^2+(y_pos-15-v2(2)).^2)+1).^(-1) + k_att.*sqrt((x_pos-x_goal-v3(1)).^2+(y_pos-y_goal-v3(2)).^2)+ 15.*(0.5.*((x_pos-x_stat).^2+(y_pos-y_stat).^2)+1).^(-1);
x_goal1 = x_goal + v3(1);
y_goal1 = y_goal + v3(2);
v_uav = [(path(i,1)-path(i-1,1))./dt;(path(i,2)-path(i-1,2))./dt];
v_rel1 = v_uav-v_ob1;
v_rel2 = v_uav-v_ob2;
theta_1 = atan(v_rel1(2)./v_rel1(1));
theta_2 = atan(v_rel2(2)./v_rel2(1));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
quiver(x,y,f3,f4)
%surfc(x,y,z)
%contour(z)
hold on
%plot3(x_pos,y_pos,z_pos,'o','MarkerFaceColor','y') %for surface plot
plot3(x_pos,y_pos,0,'o','MarkerFaceColor','y') %for quiver plot
%view(60,60)
plot3(x_goal1,y_goal1,0,'o','MarkerFaceColor','b')
hold off
drawnow
pause(0.01)
%gif
end
here, for the vanilla potential field, I am unable to use the 'gradient' function as matlab is saving the gradients for each (x,y) coordinate in a matrix on indices (x,y). The problem is that since the coordinates of the particle can be in decimals, I cannot access the gradients for these decimal points, resulting in error:
Index in position 2 is invalid. Array indices must be positive integers or logical values.
Does anyone have clue on how can I access gradient for decimal coordinates?

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

回答（1 个）

Balavignesh
Balavignesh 2023-12-6
Ran in:
Hello Astik,
It seems that you are facing challenges accessing gradients for decimal coordinates. This issue may arise due to the nature of the 'gradient' function in MATLAB, which returns numerical gradients at specified grid points. It is crucial to ensure that the grid points align precisely with the coordinates at which you intend to evaluate the gradients.
To overcome the obstacle of accessing gradients for decimal coordinates, you may consider employing a technique known as 'Custom Gradient Calculation'. This method involves estimating the gradients at decimal coordinates based on neighboring integer grid points, utilizing the central difference method. Additionally, I have preallocated the 'path' array to enhance computation speed.
The following code is provided to offer a clearer understanding of this concept:
[x, y] = meshgrid(linspace(-50, 50, 100));
x_pos = -20;
y_pos = -20;
x_stat = -10;
y_stat = -5;
x_goal = 20;
y_goal = 20;
k_att = 1;
k_rep = 15;
t = 0:0.01:2;
theta_1 = 0;
theta_2 = 0;
path = zeros(length(t), 2); % Preallocate the path array
dt = 0.01;
a = 0.05;
b = 0.09;
% Define grid spacing
dx = x(1, 2) - x(1, 1);
dy = y(2, 1) - y(1, 1);
for i = 2:length(t)
% Calculate the potential field
v1 = [x_stat, y_stat];
v2 = [x_pos, y_pos];
v3 = [x_goal, y_goal];
z = k_att * sqrt((x - x_goal).^2 + (y - y_goal).^2) - k_rep * sqrt((x - x_stat).^2 + (y - y_stat).^2);
% Find the nearest grid indices for x_pos and y_pos
x_index = round((x_pos - x(1, 1)) / dx) + 1;
y_index = round((y_pos - y(1, 1)) / dy) + 1;
% Estimate gradients at the nearest grid points using central difference method
if x_index > 1 && x_index < size(x, 2) && y_index > 1 && y_index < size(y, 1)
dz_dx = (z(y_index, x_index + 1) - z(y_index, x_index - 1)) / (2 * dx);
dz_dy = (z(y_index + 1, x_index) - z(y_index - 1, x_index)) / (2 * dy);
else
% Handle boundary cases by using forward/backward differences
dz_dx = (z(y_index, x_index + 1) - z(y_index, x_index)) / dx;
dz_dy = (z(y_index + 1, x_index) - z(y_index, x_index)) / dy;
end
% Update position using the calculated gradients
fy = -dz_dy;
fx = -dz_dx;
y_pos = y_pos - fy * t(i);
x_pos = x_pos - fx * t(i);
% Update path
path(i, 1) = x_pos;
path(i, 2) = y_pos;
% Update theta_1 and theta_2
theta_1 = theta_1 + a * fy * dt;
theta_2 = theta_2 + b * fx * dt;
% Update the plot
plot(path(:,1), path(:,2), 'b');
hold on
plot(x_pos, y_pos, 'ro');
plot(x_stat, y_stat, 'go');
plot(x_goal, y_goal, 'mo');
hold off
axis([-50 50 -50 50]);
drawnow;
% Calculate the new velocities
v = [cos(theta_1), sin(theta_1)] + [cos(theta_2), sin(theta_2)];
end
Kindly have a look at the following documentation links to have more information on:
Hope that helps!
Balavignesh.

类别

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

标签

产品


版本

R2021a

Community Treasure Hunt

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

Start Hunting!

Translated by