How can i write a loop to calculate repulsive and damping forces for a drone guidance while avoiding multiple obstacles collision using artificial potential method??

% Repulsive force (Fr)
    Fr = zeros(2,1);
    for i = 1:nobs
       rho_obs = sqrt((x-xo(i))^2+(y-yo(i))^2);
        if rho_obs <= rho_0(i)
           Fr(1,1) = Fr(1,1) + Kr(i) * (1/rho_obs - 1/rho_0(i)) * 1/rho_obs * (x-xo(i)); % Equation for repulsive force
           Fr(2,1) = Fr(2,1) + Kr(i) * (1/rho_obs - 1/rho_0(i)) * 1/rho_obs * (y-yo(i)); % Equation for repulsive force
        elseif rho_obs > rho_0(i)
            Fr(1,1) = Fr(1,1);
            Fr(2,1) = Fr(2,1);
        end
    end
    Frc = [Frc Fr];
% Damping force (Fd)
    Fd = zeros(2,1);
    Fd(1,1) = -Kv * vx;
    Fd(2,1) = -Kv * vy;

    Fdc = [Fdc Fd];

Answer 1

[Jaynik]

Hi Joan,

As per my understanding, I see that you have already implemented the internal loop to calculate the repulsive and damping forces. I am assuming that the code is a part of a bigger loop where the values of "x" and "y" are updated based on the drone's trajectory.

I see that "rho_obs" is the Euclidean distance from each obstacles, "Kr" is the scaling factor to adjust the repulsive force and "Kv" is the damping coefficient. As per my understanding the loop you have written should work correctly. However, for the repulsive force equation, as per the Artificial Potential Method, the expression should look like this:

Fr(1,1) = Fr(1,1) + Kr(i) * (1/rho_obs - 1/rho_0(i)) * 1/rho_obs^2 * (x - xo(i)) / rho_obs;
Fr(2,1) = Fr(2,1) + Kr(i) * (1/rho_obs - 1/rho_0(i)) * 1/rho_obs^2 * (y - yo(i)) / rho_obs;

The revised expression also includes the component of the repulsive force for the i-th obstacle. To provide you with more accurate assistance, it would be helpful if you could share the full code.

[Joan]

Hi Jaykin. Thank you!