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

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?

Answer 1

[Balavignesh]

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:

• 'gradient' function: https://www.mathworks.com/help/matlab/ref/gradient.html
• Preallocation: https://www.mathworks.com/help/matlab/matlab_prog/preallocating-arrays.html

Hope that helps!

Balavignesh.