I try to understand some MATLAB code (see below) i found from the web for inter-robot collision avoidance. But I'm not really understand, how the theory behind this code work. Can anyone help me explained the theory behinds this code?

% Collision avoidance using distance-based velocity scaling (scale by alpha)
% along the vector between an agent and the agent nearest to it
%        1 |          _______
%  alpha   |        /
%          |      /
%        0 |___ /
%          0   R2    R1    ++++
%            distance
%  u: velocity
%  p: position

    function u = avoidCollisions(states, n, u)

        R1 = COLLISION_RAD*3;
        R2 = COLLISION_RAD;
        dist = Inf;
        % checking for the nearest robot
        for j = 1:N
            d = norm(states(n).p-states(j).p);
            if (j ~= n) && (d < dist)
                k = j;
                dist = d;
        pDiff = states(n).p - states(k).p;

        if dist > R2
            alpha = (dist-R2)/(R1-R2);
            alpha = 0;
        if dot(u, pDiff) < 0
            v2 = project(u, [pDiff(2); -pDiff(1)]);
            u = alpha*u + (1-alpha)*v2;   

% Vector projection of b onto a
    function v = project(b, a)
        v = dot(a, b)/sum(a.^2)*a;
  • $\begingroup$ It would be helpful to have line numbering $\endgroup$
    – N. Staub
    Apr 24 '18 at 17:58

General Idea

This assume that each agent as a velocity trajectory it is following, to avoid collision this velocity is going to be scaled by a factor $\alpha$ computed at each step. At every step, the distance to the nearest agent is computed, then as the illustration at the beginning of the code suggests

  1. if the distance is superior than $R_1$ then $\alpha=1$, i.e. no scaling of the velocity;
  2. if the distance is smaller than $R_2$ then $\alpha=0$, i.e. the agent stops;
  3. if the distance is between the two radius then a linear scaling is applied

This means that when the agents are sufficiently far away the collision avoidance has not impact on them. If they come close in a certain neighborhood they will slow down, and if they are to close they will stop.


From the codes it seems that the parameters of the function avoidCollisions are:

  • states: the state of all agent
  • n: the index of the considered agent
  • u: the desired velocity of this agent

It also seems that this is applied to 2D cases.

In the code once the smallest closest agent is found (index k) the distance is stored (dist) and also the relative coordinates of the agent n w.r.t. to agent k (pDiff). The value of $\alpha$ is computed as described below put leaving the possibility to have it greater than 1 if the distance is greater than $R_1$. Then the code check if the desired velocity is orthogonal to the position of the agent k relatively to agent n, if this is the case n is not going toward k so no collision avoidance is necessary (not that if pDiff was the relative position of agent k w.r.t. agent n, the sign would be more intuitive). Then $v_2$ is introduced, it is the projection a vector orthogonal to the position of agent k relative to agent n, and the velocity vector u. This way the velocity u is scaled by alpha only in the direction non-orthogonal to the relative position.

Note that $\alpha$ should be forced to remain less than or equal to 1.

  • $\begingroup$ Thank you N. Staub for prompt reply and very clear explanation. I got the idea now. However, you said that in the point no 2: "if the distance is smaller than R2 then α=0, i.e. the agent stops" but if see from the code the velocity: u = alpha*u + (1-alpha)*v2, will not be zero if α=0 unless v2 is zero. Is this correct or I missed something here? $\endgroup$
    – Mei
    Apr 24 '18 at 19:09
  • $\begingroup$ In point 2 it was the general idea from the sketch in the code comment, then in implementation there is a refinement that is that alpha is applied only along the direction toward the other agent, while in the direction orthogonal there is no scaling. $\endgroup$
    – N. Staub
    Apr 25 '18 at 6:07
  • $\begingroup$ Thank you very much N. Staub. I think it is clear to me now. $\endgroup$
    – Mei
    Apr 25 '18 at 9:27
  • $\begingroup$ Sorry N. Staub. I have one more question. If seen from the v2 = project(u, [pDiff(2); -pDiff(1)]), why does the [pDiff(2); -pDiff(1)] is used instead of [pDiff(1); pDiff(2)]? Why there is negative sign there? $\endgroup$
    – Mei
    Apr 26 '18 at 9:49
  • $\begingroup$ v2 is the projection of the velocity orthogonal to the direction $\endgroup$
    – N. Staub
    Apr 26 '18 at 10:19

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.