MA-DV²F: A Multi-Agent Navigation Framework using Dynamic Velocity Vector Field

We aim to solve the task of multi-agent navigation in unconstrained environments. Given $N_{veh}$ dynamic vehicles and $N_{obs}$ static obstacles in the scene, the task is to ensure each vehicle reaches its desired destination while avoiding collision with other agents. The state vector for ego-vehicle $i$ ($1\leq i\leq N_{veh}$) at current time $t$ can be represented as $\mathbf{s}_{t}^{({i})}=[x_{t},y_{t},\theta_{t},v_{t},x_{tar},y_{tar},\theta_{tar}]^{T}$, where $x_{t}$ and $y_{t}$ shows the position, $\theta_{t}$, the orientation and $v_{t}$ the speed at current time $t$. Meanwhile, $x_{tar}$ and $y_{tar}$ are co-ordinates of the target position, and $\theta_{tar}$ is the target orientation. Each ego-vehicle is controlled by a control vector $\mathbf{c}_{t}^{({i}_{veh})}=[p_{t},\varphi_{t}]^{T}$, where $p_{t}\in[-P,P]$ and $\varphi_{t}\in[-\Phi,\Phi]$ are the pedal acceleration and steering angle, limited in magnitude by $P$ and $\Phi$ respectively. The obstacle $k$ ($1\leq k\leq N_{obs}$) is represented by a state vector $\mathbf{s}^{k}=[x,y,r]^{T}$, where $x$ and $y$ is the position, and $r$ is the radius of the circle circumscribing all vertices of the obstacle. The kinematics of the vehicles is modeled using the bicycle model [28].

It describes how the equations of motion can be updated in time increments of $\Delta t$ assuming no slip condition, valid under low or moderate vehicle speed $v$ when making turns. Note that $\beta$ and $\gamma$ are the tuneable hyper-parameters modeling the environment friction and vehicle wheelbase respectively.

We create a velocity vector field for each vehicle, which in turn can dynamically generate reference control variables. Generation of the velocity field can be divided into two stages:

• •

  Estimation of reference orientation of ego-vehicle for every position in the map (See Subsection III-B).

• •

  Estimation of reference speed for the corresponding positions. (See Subsection III-C).

The orientation which a vehicle should possess at any position on the map is referred to as the Reference Orientation and is represented as a unit vector. The black arrows in Fig. 1 show the reference orientation for the black ego vehicle at different positions in the map. It can be seen that the arrows attract the ego-vehicle towards its target while repelling it away from the other vehicles and obstacles. Note that the current orientation of the ego-vehicle is not aligned with the reference orientation at its existing position. Therefore, we ought to find the control variables which align the ego-vehicle with the reference orientation. Apart from the reference orientation at each position in the map, the ego-vehicle also has a reference speed which should be attained by the control variables. Lastly, note that apart from the black vehicle, a separate reference orientation map is also created for the blue and red vehicles present in Fig. 1 (not shown in the Figure).

Hence, our task of multi-agent navigation simplifies to finding the reference orientation maps and corresponding reference speed for each vehicle independently. In the subsequent sub-section, we describe how the reference orientation and speed are estimated.

We first define some frequently used functions and vectors before we begin discussion of reference orientation estimation. The vector function $\mathbf{f_{uni}}(\mathbf{a})$ is the function which takes a non-zero vector $\mathbf{a}$ as input and divides by its magnitude to convert it into a unit vector.

Other scalar functions include $f_{sgn}(a)$ and $f_{pos}(a)$ which both output 1 if the scalar input $a$ is positive. However, $f_{sgn}(a)$ outputs -1, while $f_{pos}(a)$ outputs 0 when $a$ is negative. We now define the vector from the next position $(x^{(i)}_{t+1},y^{(i)}_{t+1})$ of ego vehicle $i$ to

• •

  its target position $(x^{(i)}_{tar},y^{(i)}_{tar})$ as $\mathbf{X}^{(i)}_{tar}=[x^{(i)}_{tar}-x^{(i)}_{t+1},y^{(i)}_{tar}-y^{(i)}_{t+1}]^{T}$

• •

  the position $(x^{(k)}_{obs},y^{(k)}_{obs})$ of the static obstacle $k$ as $\mathbf{X}^{(i)}_{obs_{k}}=[x^{(k)}_{obs}-x^{(i)}_{t+1},y^{(k)}_{obs}-y^{(i)}_{t+1}]^{T}$

• •

  next position $(x^{(j)}_{t+1},y^{(j)}_{t+1})$ of another vehicle $j$ as $\mathbf{X}^{(i)}_{veh_{j}}=[x^{(j)}_{t+1}-x^{(i)}_{t+1},y^{(j)}_{t+1}-y^{(i)}_{t+1}]^{T}$

The unit vector along the Z-axis is $\mathbf{Z}=[0,0,1]^{T}$, while the unit orientation vector of the ego vehicle at the current target states are given by $\mathbf{U}^{(i)}_{t}=[\cos(\theta^{(i)}_{t}),\sin(\theta^{(i)}_{t})]^{T}$ and $\mathbf{U}^{(i)}_{tar}=[\cos(\theta^{(i)}_{tar}),\sin(\theta^{(i)}_{tar})]^{T}$ respectively.

For the ego vehicle $i$, the target reaching component of the reference orientation $\mathbf{u}^{(i)}_{tar}$ is defined as:

where $r_{p}$ is the parking threshold (black dotted circle in Fig. 1) and $0.5\cdot v^{2}_{d}$ is the marginal parking threshold (shaded blue region in Fig. 1). $v_{d}$ is the default reference speed, estimation of which is explained in Subsection III-C. $\epsilon_{p}$ is the threshold above which an additional term is introduced when the vehicle is within the parking threshold.

Equation 2 shows that when the ego-vehicle $i$ is far away from the parking threshold, the reference orientation is in the direction of $\mathbf{X}^{(i)}_{tar}$. However, as the ego-vehicle approaches the target and the velocity is high, then the vehicle might overshoot the target and enter the shaded marginal parking threshold region. In this case the direction of the orientation is flipped using $f_{sgn}({\mathbf{X}_{tar}^{(i)}}^{T}.\mathbf{U}^{(i)}_{t})$. This is to prevent the vehicle from moving in circles. A detailed explanation of this is given in the supplementary file on the project page: https://yininghase.github.io/MA-DV2F/supplementary.pdf.

Equation 2 also shows for the condition when the distance $\|\mathbf{X}^{(i)}_{tar}\|_{2}$ falls below the parking threshold $r_{p}$, and is closer to the target. The ego vehicle should be guided to not only reach the position of the target ($\mathbf{X}^{(i)}_{tar}=0$) but also be aligned with the target orientation ($\mathbf{U}^{(i)}_{tar}=\mathbf{U}^{(i)}_{t}$). The balancing act between these two conditions is handled by the $\lambda^{(i)}_{tar}$ term in Equation 2. Like previously, $f_{sgn}({\mathbf{X}_{tar}^{(i)}}^{T}\cdot\mathbf{U}^{(i)}_{tar})$ in $\lambda^{(i)}_{tar}$ makes sure the reference and target orientations are in the same directions when parking. The $f_{pos}(\|\mathbf{X}^{(i)}_{tar}\|_{2}-\epsilon_{p})$ term in $\lambda^{(i)}_{tar}$ is designed to expedite the parking behavior when the vehicle position is exactly on the left or the right side of the target and the vehicle orientation is parallel to the target orientation.

Besides reaching the target, the ego vehicle $i$ should also avoid collision on its way to the target. This is realized by the collision avoiding component $\mathbf{u}^{(i)}_{coll}$ which comprises of collision avoidance between the ego vehicle $i$ and either static obstacle $k$ ($\mathbf{u}^{(i)}_{obs_{k}}$) or with another vehicle $j$ ($\mathbf{u}^{(i)}_{veh_{j}}$).
The equation for determining ($\mathbf{u}^{(i)}_{obs_{k}}$) is given by:

where $r^{(k)}_{obs}$ is the radius of the static object $k$, $r_{veh}$ is the radius of the smallest circle enclosing the ego-vehicles, $r_{c}$ is the static component, while $|v^{(i)}_{t}|$ is the speed-based dynamic safety margin for collision avoidance between ego vehicle $i$ and obstacle $k$. Higher the vehicle speed $|v^{(i)}_{t}|$, larger is this margin $r_{c}+|v^{(i)}_{t}|$. When the distance between the ego vehicle $i$ and static object $k$ ($\|\mathbf{X}^{(i)}_{obs_{k}}\|_{2}-r_{obs}^{(k)}-r_{veh}$) is below the collision avoidance margin $r_{c}+|v^{(i)}_{t}|$, then $\alpha^{(i)}_{obs_{k}}\leq 0$, and the reference orientation is modified to prevent collision with the static obstacle. Under this condition, the first term $\mathbf{f_{uni}}(\mathbf{X}^{(i)}_{obs_{k}})\cdot\alpha^{(i)}_{obs_{k}}$ will guide the vehicle to drive away from the static obstacle. However, driving away is not enough as this might cause a bottleneck in cases where the obstacle is symmetrically collinear between the ego-vehicle and its target. We would additionally like the ego-vehicle to drive around the obstacle to reach the target for which the term $\mathbf{R}^{(i)}_{obs_{k}}\cdot\beta^{(i)}_{obs_{k}}$ is relevant. If the ego-vehicle is between the obstacle and target then $\beta^{(i)}_{obs_{k}}=0$ (since $f_{pos}({\mathbf{X}^{(i)}_{tar}}^{T}\cdot\mathbf{X}_{obs_{k}}^{(i)})=0$) and hence there is no need for the vehicle to drive around the obstacle. However, if that is not the case, then an additional component is added whose direction is given by $\mathbf{R}^{(i)}_{obs_{k}}$ and magnitude ($\beta^{(i)}_{obs_{k}}$) is proportional to how far the vehicle is away from the obstacle. $\mathbf{R}^{(i)}_{obs_{k}}$ is given as the cross product between $\mathbf{Z}$ and $\mathbf{X}^{(i)}_{obs_{k}}$ with a zero being appended to the third dimension of $\mathbf{X}^{(i)}_{obs_{k}}$ which originally lies in the 2D space. The vector resulting from the cross-product is perpendicular to $\mathbf{X}_{obs_{k}}^{(i)}$ which causes the vehicles to move in the clockwise direction around the obstacle as shown in Fig. 2.

Likewise, the component for avoiding collision between the ego vehicle $i$ and another vehicle $j$ ($\mathbf{u}^{(i)}_{veh_{j}}$) is similar to Equation 3 except that the static obstacle radius $r_{obs}^{(k)}$ will be replaced by the other vehicle’s radius $r_{veh}$ in the $\alpha^{(i)}_{veh_{j}}$ term. Secondly, the speed based dynamic margin is $|v^{(j)}_{t}|+|v^{(i)}_{t}|$ rather than just $|v^{(i)}_{t}|$.

Finally, the ideal reference orientation vector $\mathbf{\hat{u}}^{(i)}_{t+1}$ ($\mathbf{u_{ref}}$ in Fig. 1) is:

From this, the corresponding ideal reference orientation angle $\hat{\theta}^{(i)}_{t+1}$ for ego vehicle $i$ can be calculated by applying $arctan2$ to $\mathbf{\hat{u}}^{(i)}_{t+1}$. However, kinematic constraints arising from the motion model (Equation 1) may prevent the vehicle from immediately attaining the ideal reference orientation $\hat{\theta}^{(i)}_{t+1}$ in the next time step. Therefore, we instead use $\theta^{(i)}_{t+1}$ referred to as the real orientation. It is the reachable orientation closest to $\hat{\theta}^{(i)}_{t+1}$. The unit vector corresponding to this real reference orientation angle $\theta^{(i)}_{t+1}$ for ego vehicle $i$ is $\mathbf{u}^{(i)}_{t+1}$ ($\mathbf{u_{real}}$ in Fig. 1) = $[\cos(\theta^{(i)}_{t+1}),\sin(\theta^{(i)}_{t+1})]^{T}$.

The reference speed $v^{(i)}_{t+1}$ is chosen after determination of the reference orientation, which depends on the situation the vehicle is in. Fig. 3(b) shows a scenario wherein a vehicle is at the same location next to an obstacle but with opposite orientations. This determines if the velocity should move the car forward or backward.

We use the logical or ($\vee$) and logical and ($\wedge$) operators [29] to describe the criteria for collision avoidance between ego vehicle $i$ and static obstacle $k$ under such situations as:

where $\alpha_{obs_{k}}^{(i)}$ is same as defined in Equation 3, $\epsilon_{c}$ is the an additional tolerance for the collision checking region. $F^{(i)}_{obs_{k}}$ equalling $true$ indicates the ego vehicle $i$ is forbidden to drive forwards towards obstacle $k$. This happens when the angle between the reference orientation ($\mathbf{{u}^{(i)}_{t+1}}$) and the vector from the ego vehicle to the obstacle ($\mathbf{X}^{(i)}_{obs_{k}}$) is less than $90^{\circ}$ i.e. $\gamma^{(i)}_{obs_{k}}={\mathbf{u}^{(i)}_{t+1}}^{T}\cdot\mathbf{X}^{(i)}_{obs_{k}}>0$. Likewise, $B^{(i)}_{obs_{k}}$ equaling $true$ happens when $\gamma^{(i)}_{obs_{k}}={\mathbf{u}^{(i)}_{t+1}}^{T}\cdot\mathbf{X}^{(i)}_{obs_{k}}<0$ indicating that the ego vehicle $i$ is forbidden to drive backwards.

Similarly for the case of ego vehicle $i$ and another vehicle $j$, the conditions for preventing the ego-vehicle from moving forward $F^{(i)}_{veh_{j}}$ or backward $B^{(i)}_{veh_{j}}$ are the same as described in Equation 6 except that $\mathbf{X}^{(i)}_{veh_{j}}$ replaces $\mathbf{X}^{(i)}_{obs_{k}}$.

Summarising the results above, the magnitude and sign of the velocity depends on the combination of these boolean variables i.e. $F^{(i)}_{obs_{k}}$, $F^{(i)}_{veh_{j}},B^{(i)}_{obs_{k}}$ and $B^{(i)}_{veh_{j}}$ as follows:

The first 3 conditions check whether or not there is a potential for collision. The velocity is $+v_{d}$, when the ego-vehicle is prevented from moving backwards ($B^{(i)}_{coll}$) but allowed to move forward ($\neg F^{(i)}_{coll}$) and $-v_{d}$ when vice versa. The reference velocity is zero when the ego-vehicle is prevented from moving either forward ($F^{(i)}_{coll}$) or backward ($B^{(i)}_{coll}$). In all other cases, the velocity is $v_{tar}^{(i)}$ defined as:

where $\epsilon_{p}$ and $\epsilon_{o}$ are the acceptable position and orientation tolerances for deciding to stop the vehicle at the target. When the ego vehicle is within the parking radius, it reduces its speed as it gets closer to the target position and orientation. This is achieved by the multiplier $\lambda^{(i)}_{p}$, which is proportional to $\bar{\lambda}^{(i)}_{p}$ when the vehicle state is very close to the target state and $\sqrt{\bar{\lambda}^{(i)}_{p}}$ when farther away from the tolerance. The square root accelerates the vehicle approaching its target when there is still some distance between the current state and target state, i.e. $\neg((\|\mathbf{X}^{(i)}_{tar}\|_{2}<\epsilon_{p})\wedge(|\theta^{(i)}_{tar}-\theta^{(i)}_{t+1}|<\epsilon_{o}))$. When the vehicle is already very close to the target state, i.e. ($\|\mathbf{X}^{(i)}_{tar}\|_{2}<\epsilon_{p})\wedge(|\theta^{(i)}_{tar}-\theta^{(i)}_{t+1}|<\epsilon_{o}$), the ratio $\bar{\lambda}^{(i)}_{p}$ prevents the vehicle from shaking forward and backward. For $\bar{\lambda}^{(i)}_{p}$, the first term: $\frac{1}{r_{p}}\cdot\|\mathbf{X}^{(i)}_{tar}\|_{2}$, decreases linearly with the ego vehicle distance to its target, and the second term, i.e. $\frac{1}{v_{d}}\cdot|\theta^{(i)}_{tar}-\theta^{(i)}_{t+1}|$, reduces linearly with the angle difference between the reference $\theta^{(i)}_{t+1}$ and target $\theta^{(i)}_{tar}$ orientation angles. However, we do not allow the reference parking speed $v_{tar}^{(i)}$ to be any higher than the default reference speed $v_{d}$. So, $\bar{\lambda}^{(i)}_{p}$ is clipped to a maximum of $1$. $\xi^{(i)}_{p}$ controls whether the ego vehicle moves forwards or backwards by checking ${\mathbf{u}^{(i)}_{t+1}}^{T}\cdot\mathbf{X}^{(i)}_{tar}$. Originally, the vehicle should move forwards when facing the target, i.e. ${\mathbf{u}^{(i)}_{t+1}}^{T}\cdot\mathbf{X}^{(i)}_{tar}>0$, and move backwards when backing toward the target, i.e. ${\mathbf{u}^{(i)}_{t+1}}^{T}\cdot\mathbf{X}^{(i)}_{tar}<0$. However, to prevent the vehicle from changing direction at high frequency within short traveling distances, we set a margin allowing the vehicle to keep its previous direction when $|{\mathbf{u}^{(i)}_{t+1}}^{T}\cdot\mathbf{X}^{(i)}_{tar}|\leq 0.25$.

When the ego vehicle $i$ is out of the parking area of radius $r_{p}$, the reference speed $v_{t+1}^{(i)}$ takes the forn $v_{d}\cdot f_{sgn}({\mathbf{u}^{(i)}_{t+1}}^{T}\cdot\mathbf{\hat{u}}^{(i)}_{t+1})$, where the reference speed $v_{t+1}^{(i)}$ takes the default value $v_{d}$, but changes to negative, i.e. $-v_{d}$, when ${\mathbf{u}^{(i)}_{t+1}}^{T}\cdot{\mathbf{\hat{u}}^{(i)}_{t+1}}<0$.

The final ideal reference speed $\hat{v}^{(i)}_{t+1}$ for ego vehicle $i$ is:

Similar to the reference orientation, the ideal reference speed $\hat{v}^{(i)}_{t+1}$ may not achievable due to the limitation of the maximum pedal command. The real reference speed $v^{(i)}_{t+1}$ is therefore the reachable speed value closest to $\hat{v}^{(i)}_{t+1}$.

These reference control commands can either be directly used to control the vehicles, or as labels for training the GNN model in a self-supervised manner using the architecture of [12].

To assess the performance of our MA-DV²F approach and its learning based counterpart (self-supervised GNN model), we compare with other recent learning and search/optimization based algorithms in challenging, collision prone test cases. The recent learning based approaches include [12] which is a GNN model trained in a supervised manner and an extension of [7] i.e. GCBF+ [8], an RL-inspired GNN model incorporating safety constraints. Meanwhile, the two search/optimization based algorithms used for comparison include CL-MAPF [9] and CSDO [10]. The test dataset comprises of challenging, collision prone scenarios with the number of vehicles ranging from 10 to 50 and static obstacles chosen between 0 and 25. Note that, all the static obstacles in the test cases are fixed to be circles with fixed radius because of the limitation of the CL-MAPF and CSDO environment setting and because it circumscribes an obstacle of arbitrary shape, allowing for safer behaviour. For the GCBF+, we use the DubinsCar model because it has vehicle kinematics most similar to ours. As GCBF+ has a completely different working domain with different map and agent sizes, we scale our test cases when running GCBF+ under the rule that the ratio of the map size and the agent size remains equal across all the algorithms. Details regarding the generation of test samples are given in the supplementary file on the project page.

Table I shows the evaluation results of the different methods across the different vehicle-obstacle combinations described earlier against the success rate metric [8]. It measures the percentage of vehicles that successfully reach their targets within a tolerance without colliding with other objects along the way. The results show that our MA-DV²F outperforms other algorithms achieving almost 100% success rate across all vehicle-obstacle combinations. Even the self-supervised GNN model performs far better than other algorithms when scaling the number of vehicles. Only the CSDO algorithm has slightly better results than our GNN model when the number of agents are low (20). However, CSDO’s performance drops drastically as the number of agents are increased under these challenging conditions. Note that the GCBF+ pipeline only checks whether the agents reach their target positions but ignores the target orientations as shown in the project page: https://yininghase.github.io/MA-DV2F/#MC which explains why it has such poor performance. Therefore, we additionally evaluate GCBF+ by ignoring the orientation and only considering the position. Results of which are shown in the second last column. Even then, the GCBF+, does not match the performance of MA-DV²F.

GCBF+ has a higher safe rate than reach rate. This is not because the vehicles fail to reach their target, but rather because they reach the target at an incorrect orientation. This is aligned with the intuition described in IV-A and is further corroborated by the fact that when orientation is ignored in the evaluation and only position is considered, the reach rate jumps drastically. Nevertheless, it is still much lower than both our MA-DV²F and its GNN self-supervised counterpart. They are the only 2 methods which maintain a consistent high performance against both metrics across all vehicle-obstacle combinations.

Lastly, note that the runtime analysis is not done for the learning based methods since, it is dependent on the GPU rather than the algorithm itself. Therefore, for the same model architecture, the runtime will be the same for all learning based algorithms.

In this Subsection, we further elaborate some the design choices for Equation 2 used to determine the reference orientation. The Equation is repeated here again for clarity:

The reference steering angles $\varphi^{(i)}_{t}$ and reference pedal acceleration $p^{(i)}_{t}$ mentioned in Section III cannot only be used to control the vehicles directly, but also as labels to supervise training of the GNN model. This approach can therefore additionally be used to train a learning based Graphical Neural Network (GNN) controller in a self-supervised manner using these control labels and network architecture given in [12]. However, [12] requires running an optimization based procedure offline to collect enough training data with ground truth labels. This is a computationally expensive and slow process when the number of agents are large. In contrast, our self-supervised learning method is capable of directly training the model online during the simulation process without collecting ground truth labels beforehand. Note that in [12], all the vehicle nodes in the GNN model will have incoming edges from any other vehicle or obstacle nodes. In this case, the number of edges in the graph will grow quadratic to the number of agents(neighbouring vehicles/obstacles), leading to a heavy computational burden when when scaling to more agents. Thus, we remove edges between the neighboring agent $j$ or $k$ and the ego vehicle $i$ if the distance $\|X^{(i)}_{j}\|$ between them is greater than the threshold given by:

where $\alpha_{obs_{k}}^{(i)}=\|\mathbf{X}^{(i)}_{obs_{k}}\|_{2}-r_{obs}^{(k)}-r_{veh}-(r_{c}+|v^{(i)}_{t}|)$ and $\alpha_{veh_{j}}^{(i)}=\|\mathbf{X}^{(i)}_{veh_{j}}\|_{2}-2\cdot r_{veh}-(r_{c}+|v^{(i)}_{t}|)$ are defined same as in Equation 3 mentioned in Section III-B, $C_{tar}$ measures the distance of the ego vehicle $i$ to its target, and $C_{obs}$ and $C_{veh}$ evaluate the collision risk of the ego vehicle. The gradient of this vehicle state cost should alone be enough to guide the GNN model in learning to reach the target while avoiding collisions. However, in practice, the GNN does not converge to the optimal due to high non-linearities in Equation 12. Therefore, we combine this equation with the labels obtained from the dynamic velocity vector field which expedites the model training.

The overall loss function used in this pipeline is defined as follows: