Reactive Collision Avoidance Control for Nonholonomic Vehicles and Obstacles of Arbitrary Shape

Rodríguez-Seda, Erick J.

doi:10.1115/1.4065644

Graphical Abstract Figure

Abstract

Potential field-based collision avoidance algorithms for mobile robots frequently assume vehicles and obstacles to have circular or spherical shapes. This assumption not only simplifies the analysis but also limits the mobility of agents in confined spaces, particularly for vehicles with elongated or irregular shapes. To increase mobility, this letter presents a decentralized collision avoidance framework for nonholonomic systems of unicycle type that considers the non-circular shape and relative orientation of vehicles and obstacles. The framework builds on the concepts of potential field and avoidance functions. However, it proposes using a non-constant minimum safe distance radius that changes based on the shape, relative position, and relative orientation of agents. The control framework is proven to guarantee collision avoidance at all times and is shown, via simulation, to increase the ability of agents to navigate through narrow spaces safely.

1 Introduction

A critical task of mobile robots is the ability to follow a desired trajectory or path while avoiding collisions with other vehicles and obstacles. Methods for collision avoidance include the use of velocity obstacle [1] and its variants [2,3], the concept of collision cones [4,5], the use of potential field functions [6–9], and the implementation of barrier functions [10,11], to name a few (see Refs. [12–14] for comprehensive reviews). A common assumption among most of these methods is the generalization of agents (namely, vehicles and obstacles) as having circular or spherical shapes. Although this assumption simplifies the analysis and implementation, it is also overly conservative, particularly for agents with elongated shapes. For instance, consider the two vehicles in Fig. 1, which have nearly rectangular shapes. The circular body assumption will require both vehicles to keep a distance from each other equal to or larger than the summation of their circumradii, $r_{i j}^{max} = h_{i} + h_{j}$ ⁠, regardless of their relative orientation. That is, the circular shape assumes the worst-case scenario and makes the vehicles occupy a larger space than necessary, which can impede the motion of agents in cluttered spaces and narrow corridors [15]. Therefore, for confined environments, it is more suitable to design avoidance control strategies that take the vehicle’s and the obstacle’s shape into consideration.

Fig. 1

View large Download slide

Safe minimum distance for two vehicles of rectangular shape with circumradii $h_{i}$ and $h_{j}$ ⁠. Traditional approaches assume circular shapes and enforce a constant minimum distance equal to or greater than $r_{i j}^{max} = h_{i} + h_{j}$ ⁠, which is the minimum safe distance when the agents are parallel to each other and diagonally opposed. This results in an excessive separation at any other configuration.

Examples of control algorithms that explicitly consider the non-circular shape of agents include methods based on reachability concepts. For instance, Ref. [4] introduces a collision cone approach for dynamic obstacles of arbitrary shape, which is later extended to obstacles of deformable shape [15] and to nonholonomic vehicles [5]. In Ref. [16], a distance-projection method is formulated using reachable sets that take into account the shape and kinematics of vehicles and obstacles. A common drawback of the aforementioned methods is that, with a large number of dynamic obstacles, they either cannot guarantee collision avoidance or the problem can quickly become intractable.

Collision avoidance with objects of non-circular shape has also been approached using potential field functions, which have the typical advantage of generating methods that can be rigorously proven to be safe for an arbitrarily large number of obstacles. However, most of these approaches rely on computing the closest distance to the obstacle [6] or a set of points in its boundary [17], which can generate non-smooth control inputs, be computationally expensive, or not suitable for non-convex obstacles. Others have approximated the shape of elongated agents using ellipsoids [7,18,19], which can still introduce conservatism for objects of irregular shape. Another common approach is representing the obstacles as an assemblage of $n$ -dimensional small spheres [20] and ellipsoids [18]. However, these methods would require a collision avoidance action for each individual surface.

Recently, gradient-based optimization methods have been proposed using signed distance functions [21,22] and control barrier functions [10,11,23]. For instance, the work in Ref. [21] introduces an optimization scheme to determine collision-free trajectories by approximating the gradient of signed distance functions to obstacles of primitive shapes. Similarly, Ref. [11] introduces the use of control barrier functions to guarantee the safety of vehicles with limited actuation. Yet, many of these approaches do not yield controllers in closed form or may yield discontinuous control inputs due to the approximations.

In this letter, we present a decentralized, reactive potential field-based collision avoidance method for an arbitrarily large group of nonholonomic vehicles of unicycle type and arbitrary shape. We use the concept of avoidance functions presented in Ref. [7] and extend it to vehicles and obstacles of arbitrary shape by introducing a non-constant minimum safe distance. The minimum safe distance (or avoidance radius) is a function of the relative orientation between the obstacle and the vehicle, as well as their shapes. Unlike previous potential field-based frameworks, ours only requires a single function between pairs of agents and generates analytical, smooth control inputs. To prevent deadlocks, a common drawback of potential field methods [17], we include an almost everywhere continuous and smooth control perturbation aimed to break symmetries in the potential field. The overall control strategy is rigorously proven to avoid collision among cooperative vehicles and static obstacles at all times. We validate the control approach via a numerical example with vehicles and obstacles of rectangular shape. To the best of our knowledge, this is the first time that a time-varying avoidance radius or minimum safe distance is assumed within the avoidance functions framework [7].

2 Problem Formulation

2.1 Multi-Vehicle Dynamics.

We consider the task of safely coordinating the motion of

N

vehicles with nonholonomic dynamic equations given by

\begin{matrix} {\dot{x}}_{i} (t) = v_{i} (t) \cos ϕ_{i} (t), m_{i} {\dot{v}}_{i} (t) = f_{i} (t), \\ {\dot{y}}_{i} (t) = v_{i} (t) \sin ϕ_{i} (t), J_{i} {\dot{ω}}_{i} (t) = τ_{i} (t), \\ {\dot{ϕ}}_{i} (t) = ω_{i} (t) \end{matrix}

(1)

where

x_{i} (t)

and

y_{i} (t)

are the position coordinates,

ϕ_{i} (t)

is the orientation,

v_{i} (t)

and

ω_{i} (t)

are the linear and angular velocities,

m_{i}

is the mass,

J_{i}

is the inertia, and

f_{i} (t)

and

τ_{i} (t)

are the control force and torque inputs for the ith robot (see Fig. 1). In what follows, we will omit the time argument of signals unless deemed necessary.

It is well known that the position and orientation of cannot be simultaneously stabilized at a desired value using a continuous static state feedback control law [24]. Therefore, we will opt to control a reference point in front of

(x_{i}, y_{i})

⁠, see Fig. 1, given by

z_{i, 1} = x_{i} + L_{i} \cos ϕ_{i}, z_{i, 2} = y_{i} + L_{i} \sin ϕ_{i}

(2)

where

L_{i} > 0

is a constant parameter [25] and

z_{i} = [z_{i, 1}, z_{i, 2}]^{T}

is the Cartesian position of the reference point. If we take the first and second derivatives of and apply the following control inputs to

[\begin{matrix} f_{i} \\ τ_{i} \end{matrix}] = [\begin{matrix} \frac{\cos ϕ_{i}}{m_{i}} & \frac{\sin ϕ_{i}}{m_{i}} \\ \frac{- \sin ϕ_{i}}{J_{i} L_{i}} & \frac{\cos ϕ_{i}}{J_{i} L_{i}} \end{matrix}] [\begin{matrix} u_{i, 1} + v_{i} ω_{i} \sin ϕ_{i} + L_{i} ω_{i}^{2} \cos ϕ_{i} \\ u_{i, 2} - v_{i} ω_{i} \cos ϕ_{i} + L_{i} ω_{i}^{2} \sin ϕ_{i} \end{matrix}]

(3)

one can show that reduces to

{\ddot{z}}_{i} = u_{i}

(4a)

{\dot{ϕ}}_{i} = [\begin{matrix} - \frac{\sin ϕ_{i}}{L_{i}} \frac{\cos ϕ_{i}}{L_{i}} \end{matrix}] {\dot{z}}_{i}

(4b)

where

u_{i} = [u_{i, 1}, u_{i, 2}]^{T}

is the control input for the linearized system. While the internal dynamics can only be shown to be Lagrange stable [12], the linear dynamics of the reference point are controllable. That is, for any desired trajectory

z_{i}^{d} \in R^{2}

⁠, one can design a state feedback control law

u_{i}

such that

z_{i} \to z_{i}^{d}

as

t \to \infty

⁠.

2.2 Minimum Safe Distance.

Herein, we would like the vehicles to maintain a safe distance from each other as well as obstacles. Accordingly, we define the minimum safe distance between the ith vehicle and jth agent as

r_{i j} : = r_{i j} (z_{i}, z_{j}, ϕ_{i}, ϕ_{j}) = r_{j i} (z_{j}, z_{i}, ϕ_{j}, ϕ_{i})

(5)

where

r_{i j}

takes into account the shape, relative position, and orientation of agents, as illustrated in Fig. 1. We say that a collision between agents takes place if

‖ z_{i} - z_{j} ‖ \leq r_{i j}

for some time

t

⁠. It is assumed that

r_{i j}

is time continuous differentiable and symmetric (i.e.,

r_{i j} = r_{j i}

⁠), yet it may differ among different pairs of agents (i.e.,

r_{i j} ≢ r_{i k}

for

j \neq k

⁠). An example of a smooth analytical expression for vehicles and obstacles with rectangular shapes is given in Sec. 4.

2.3 Control Objective.

The ith vehicle’s ultimate control objective is to follow a desired trajectory, given as $z_{i}^{d} \in R^{2}$ ⁠, while maintaining a safe distance $r_{i j}$ from other vehicles and obstacles at all times. To this end, we assume that each vehicle can detect, either via communication or onboard sensors, the relative position and orientation of other agents within a bounded detection radius $R$ ⁠, where $R > sup_{i, j \neq i} {r_{i j}} = r_{i j}^{max}$ ⁠. Then, we can formulate the control objective as follows. Design a control strategy $u_{i}$ such that $z_{i} \to z_{i}^{d}$ as $t \to \infty$ and $‖ z_{i} - z_{j} ‖ > r_{i j}$ $\forall i, j \neq i, t \geq 0$ ⁠.

3 Trajectory Tracking With Collision Avoidance Control

3.1 Avoidance Functions.

For the purpose of avoidance control, we use the concept of avoidance functions. Similar to Ref. [7], we define an avoidance function between the ith vehicle and jth agent as

V_{i j} : = V_{i j} (z_{i}, z_{j}, r_{i j}) = {(min {0, \frac{{‖ z_{i} - z_{j} ‖}^{2} - R_{i j}^{2}}{{‖ z_{i} - z_{j} ‖}^{2} - r_{i j}^{2}}})}^{2}

(6)

where the reaction distance (i.e., the distance at which the vehicle starts avoiding the obstacle) is defined as

R_{i j} = r_{i j} + Δ_{R}

for some positive constant

Δ_{R} \in (0, R - r_{i j}^{max})

⁠. One can show that

V_{i j}

is almost everywhere continuously differentiable, that

\frac{\partial V_{i j}}{\partial z_{i}} = \frac{\partial V_{j i}}{\partial z_{i}}, \frac{\partial V_{i j}}{\partial z_{i}} = - \frac{\partial V_{i j}}{\partial z_{j}}, \frac{\partial V_{i j}}{\partial r_{i j}} = \frac{\partial V_{j i}}{\partial r_{i j}}

(7)

and that

\frac{\partial V_{i j}^{T}}{\partial z_{i}} = \frac{4 (R_{i j} - r_{i j}) ({‖ z_{i} - z_{j} ‖}^{2} - R_{i j}^{2}) (z_{i} - z_{j})}{({‖ z_{i} - z_{j} ‖}^{2} - r_{i j}^{2})^{3}}

(8)

\frac{\partial V_{i j}}{\partial r_{i j}} = \frac{4 Δ_{R} (R_{i j}^{2} - {‖ z_{i} - z_{j} ‖}^{2}) (r_{i j} R_{i j} + {‖ z_{i} - z_{j} ‖}^{2})}{({‖ z_{i} - z_{j} ‖}^{2} - r_{i j}^{2})^{3}}

(9)

if

r_{i j} < ‖ z_{i} - z_{j} ‖ \leq R_{i j}

⁠, undefined if

‖ z_{i} - z_{j} ‖ = r_{i j}

⁠, and zero otherwise. In addition, due to the symmetry of the avoidance functions, we have that

\begin{aligned} \sum_{i = 1}^{N} & \sum_{j \in N_{i}} (\frac{\partial V_{i j}}{\partial z_{i}} {\dot{z}}_{i} + \frac{\partial V_{i j}}{\partial r_{i j}} \frac{\partial r_{i j}}{\partial z_{i}} {\dot{z}}_{i} + \frac{\partial V_{i j}}{\partial r_{i j}} \frac{\partial r_{i j}}{\partial ϕ_{i}} {\dot{ϕ}}_{i}) \\ = \frac{1}{2} \sum_{i = 1}^{N} \sum_{j \in N_{i}} (\frac{\partial V_{i j}}{\partial z_{i}} {\dot{z}}_{i} + \frac{\partial V_{i j}}{\partial r_{i j}} \frac{\partial r_{i j}}{\partial z_{i}} {\dot{z}}_{i} + \frac{\partial V_{i j}}{\partial r_{i j}} \frac{\partial r_{i j}}{\partial ϕ_{i}} {\dot{ϕ}}_{i} \\ + \frac{\partial V_{i j}}{\partial z_{j}} {\dot{z}}_{j} + \frac{\partial V_{i j}}{\partial r_{i j}} \frac{\partial r_{i j}}{\partial z_{j}} {\dot{z}}_{j} + \frac{\partial V_{i j}}{\partial r_{i j}} \frac{\partial r_{i j}}{\partial ϕ_{j}} {\dot{ϕ}}_{j}) = \frac{1}{2} \sum_{i = 1}^{N} \sum_{j \in N_{i}} {\dot{V}}_{i j} \end{aligned}

(10)

Note that different to other definitions of avoidance functions [7,8], the ones defined here use a non-constant safe distance

r_{i j}

and a non-constant reaction distance

R_{i j}

⁠. It is worth mentioning that the work in Refs. [26,27] uses a time-varying velocity-based reaction distance,

R_{i j}

⁠.

3.2 Control Law.

To achieve the tracking and collision avoidance objective, we propose the control input to be given by

u_{i} = {\ddot{z}}_{i}^{d} + K_{v} ({\dot{z}}_{i}^{d} - {\dot{z}}_{i}) + K_{p} (z_{i}^{d} - z_{i}) + u_{i}^{a} + u_{i}^{p}

(11a)

\begin{aligned} u_{i}^{a} & = - \sum_{j \in N_{i}} (\frac{\partial V_{i j}^{T}}{\partial z_{i}} + \frac{\partial V_{i j}}{\partial r_{i j}} {\frac{\partial r_{i j}}{\partial z_{i}}}^{T} + \frac{1}{L_{i}} \frac{\partial V_{i j}}{\partial r_{i j}} \frac{\partial r_{i j}}{\partial ϕ_{i}} [\begin{matrix} - \sin ϕ_{i} \\ \cos ϕ_{i} \end{matrix}]) \end{aligned}

(11b)

u_{i}^{p} = \frac{λ_{1} R ((π / 2) - ϑ_{i}) u_{a} \cos (σ_{i} t)}{‖ u_{i}^{a} ‖ + {‖ {\dot{z}}_{i} ‖}^{λ_{2}} + λ_{3}}

(11c)

where

K_{v}

⁠,

K_{p}

⁠,

λ_{*}

⁠, and

σ_{i}

are all positive constants,

N_{i}

is the set of neighbors for the ith agent,

ϑ_{i}

is the angle between

u_{i}^{a}

and

{\dot{z}}_{i}^{d} - {\dot{z}}_{i}

⁠, and

R (\cdot)

is the

2 \times 2

rotational matrix. The first three terms in comprise the trajectory tracking control law, with

K_{v}

and

K_{p}

regulating the convergence rate (to be shown next). The term

u_{i}^{a}

is the cooperative collision avoidance strategy and

u_{i}^{p}

is an optional control perturbation aimed to reduce the occurrence of unwanted local minima (i.e., deadlocks). Note that

u_{i}^{a}

is only active when another vehicle or obstacle is within the reaction distance

R_{i j}

⁠. In addition, note that

u_{i}^{p}

is upper bounded by

λ_{1}

⁠, almost everywhere continuous (except at

u_{i}^{a} = 0

or

{\dot{z}}_{i}^{d} - {\dot{z}}_{i} = 0

⁠), null if there is no collision threat, and always perpendicular to

{\dot{z}}_{i}^{d} - {\dot{z}}_{i}

⁠. To prove the latter, it is sufficient to show that

({\dot{z}}_{i}^{d} - {\dot{z}}_{i})^{T} R (π / 2) - ϑ_{i}) u_{i}^{a} = 0

⁠. Therefore, let

{\dot{z}}_{i}^{d} - {\dot{z}}_{i} = [a, b]^{T}

and

u_{i}^{a} = [c, d]^{T}

⁠. Then,

\begin{aligned} [a b] R (\frac{π}{2} - ϑ_{i}) [\begin{matrix} c \\ d \end{matrix}] = [a b] [\begin{matrix} \sin ϑ_{i} & - \cos ϑ_{i} \\ \cos ϑ_{i} & \sin ϑ_{i} \end{matrix}] [\begin{matrix} c \\ d \end{matrix}] \\ = \sin ϑ_{i} (a c + b d) - \cos ϑ_{i} (a d - b c) \\ = \frac{(a d - b c) (a c + b d)}{‖ [a b]^{T} ‖ ‖ [c d]^{T} ‖} - \frac{(a c + b d) (a d - b c)}{‖ [a b]^{T} ‖ ‖ [c d]^{T} ‖} = 0 \end{aligned}

(12)

where we used the dot and cross product formulas for finding

ϑ_{i}

⁠. Finally, the cosine function in

u_{i}^{p}

aims to keep the perturbation persistently exciting, changing the direction of the perturbation vector at a frequency given by

σ_{i}

⁠.

Similar to Ref. [8], we now make the following assumption about the desired trajectory.

Assumption 1

The desired trajectory satisfies the following constraint: ${\dot{z}}_{i}^{d T} u_{i}^{a} \leq K_{v} {‖ {\dot{z}}_{i}^{d} - {\dot{z}}_{i} ‖}^{2}$ for all $t \geq 0$ ⁠.

Note that Assumption 1 is trivially satisfied when the desired trajectory is constant ${\dot{z}}_{i}^{d} = {\ddot{z}}_{i}^{d} = 0$ ⁠. In the case of non-constant trajectory, we can assume that the ith robot can momentarily freeze the desired trajectory while trying to resolve a collision conflict, similar to Ref. [8], or when ${\dot{z}}_{i}^{d T} u_{i}^{a} > K_{v} {‖ {\dot{z}}_{i}^{d} - {\dot{z}}_{i} ‖}^{2}$ ⁠.

Collision avoidance

Theorem 1

Consider the system in(1) with control law (3)and(11). Suppose Assumption1holds. If $‖ z_{i} (0) - z_{j} (0) ‖ > r_{i j}$ $\forall i, j \neq i$ ⁠, then $‖ z_{i} (t) - z_{j} (t) ‖ > r_{i j}$ $\forall t \geq 0$ ⁠.

Proof

Consider the following Lyapunov function:

W_{c} = \frac{1}{2} \sum_{i = 1}^{N} (K_{p} {‖ z_{i}^{d} - z_{i} ‖}^{2} + {‖ {\dot{z}}_{i}^{d} - {\dot{z}}_{i} ‖}^{2} + \sum_{j \in N_{i}} V_{i j})

(13)

Taking its time derivative yields

\begin{aligned} {\dot{W}}_{c} & = \sum_{i = 1}^{N} (K_{p} (z_{i}^{d} - z_{i})^{T} ({\dot{z}}_{i}^{d} - {\dot{z}}_{i}) + ({\dot{z}}_{i}^{d} - {\dot{z}}_{i})^{T} ({\ddot{z}}_{i}^{d} - u_{i})) \\ + \frac{1}{2} \sum_{i = 1}^{N} \sum_{j \in N_{i}} (\frac{\partial V_{i j}}{\partial z_{i}} {\dot{z}}_{i} + \frac{\partial V_{i j}}{\partial z_{i}} {\dot{z}}_{j} + \frac{\partial V_{i j}}{\partial r_{i j}} \frac{\partial r_{i j}}{\partial z_{i}} {\dot{z}}_{i} \\ + \frac{\partial V_{i j}}{\partial r_{i j}} \frac{\partial r_{i j}}{\partial z_{j}} {\dot{z}}_{j} + \frac{\partial V_{i j}}{\partial r_{i j}} \frac{\partial r_{i j}}{\partial ϕ_{i}} {\dot{ϕ}}_{i} + \frac{\partial V_{i j}}{\partial r_{i j}} \frac{\partial r_{i j}}{\partial ϕ_{j}} {\dot{ϕ}}_{j}) \end{aligned}

(14)

and substituting for and , one obtains that

\begin{aligned} {\dot{W}}_{c} & = \sum_{i = 1}^{N} (- K_{v} {‖ {\dot{z}}_{i}^{d} - {\dot{z}}_{i} ‖}^{2} - {\dot{z}}_{i}^{d T} u_{i}^{a} - \sum_{j \in N_{i}} \frac{\partial V_{i j}}{\partial z_{i}} {\dot{z}}_{i} \\ - \sum_{j \in N_{i}} \frac{\partial V_{i j}}{\partial r_{i j}} \frac{\partial r_{i j}}{\partial z_{i}} {\dot{z}}_{i} - \sum_{j \in N_{i}} \frac{\partial V_{i j}}{\partial r_{i j}} \frac{\partial r_{i j}}{\partial ϕ_{i}} \underset{= {\dot{ϕ}}_{i}}{\underset{⏟}{{[\begin{matrix} - \sin ϕ_{i} \\ \cos ϕ_{i} \end{matrix}]}^{T} \cdot \frac{{\dot{z}}_{i}}{L_{i}}}}) \\ + \sum_{i = 1}^{N} \sum_{j \in N_{i}} (\frac{\partial V_{i j}}{\partial z_{i}} {\dot{z}}_{i} + \frac{\partial V_{i j}}{\partial r_{i j}} \frac{\partial r_{i j}}{\partial z_{i}} {\dot{z}}_{i} + \frac{\partial V_{i j}}{\partial r_{i j}} \frac{\partial r_{i j}}{\partial ϕ_{i}} {\dot{ϕ}}_{i}) \\ = \sum_{i = 1}^{N} (- K_{v} {‖ {\dot{z}}_{i}^{d} - {\dot{z}}_{i} ‖}^{2} - {\dot{z}}_{i}^{d T} u_{i}^{a}) \leq 0 \end{aligned}

(15)

where we used Assumption 1. Since

{\dot{W}}_{c} \leq 0

⁠, we have that

W_{c}

is bounded for all

t \geq 0

⁠, that is,

W_{c} (t) \leq W_{c} (0) \leq \infty

⁠. Now, suppose that for some pair

i, j \neq i

we have that

‖ z_{i} (t) - z_{j} (t) ‖ \to r_{i j}

⁠. The latter would imply that

W_{c} \to \infty

⁠, which is a contradiction. Therefore, we have that

‖ z_{i} (t) - z_{j} (t) ‖ > r_{i j}

\forall i, j \neq i

and

t \geq 0

⁠.

Trajectory tracking

Theorem 2

Assume that $\exists t_{0} \geq 0$ such that $‖ z_{i} (t) - z_{j} (t) ‖ \geq R_{i j}$ $\forall j \neq i$ and $\forall t \geq t_{0}$ ⁠. Then, $(z_{i} (t), {\dot{z}}_{i} (t))$ converges to $(z_{i}^{d} (t), {\dot{z}}_{i}^{d} (t))$ exponentially as $t \to \infty$ ⁠.

Proof

Let

{\tilde{z}}_{i} = z_{i}^{d} - z_{i}

and consider the following Lyapunov function:

W_{T} = K_{p} {‖ {\tilde{z}}_{i} ‖}^{2} + \frac{1}{2} {‖ {\dot{\tilde{z}}}_{i} ‖}^{2} + \frac{1}{2} {‖ {\dot{\tilde{z}}}_{i} + K_{v} {\tilde{z}}_{i} ‖}^{2}

(16)

which is lower and upper bounded by

α_{1} {‖ [{\tilde{z}}_{i}^{T}, {\dot{\tilde{z}}}_{i}^{T}] ‖}^{2} \leq W_{T} \leq α_{2} {‖ [{\tilde{z}}_{i}^{T}, {\dot{\tilde{z}}}_{i}^{T}] ‖}^{2}

(17)

for

α_{1} = min {1 / 2, K_{p}} < α_{2} = max {3 / 2, K_{p} + K_{v}^{2}}

⁠. Taking its time derivative yields

{\dot{W}}_{T} = 2 K_{p} {\tilde{z}}_{i}^{T} {\dot{\tilde{z}}}_{i} + {\dot{\tilde{z}}}_{i}^{T} {\ddot{\tilde{z}}}_{i} + ({\dot{\tilde{z}}}_{i} + K_{v} {\tilde{z}}_{i})^{T} ({\ddot{\tilde{z}}}_{i} + K_{v} {\dot{\tilde{z}}}_{i})

(18)

Now, the assumption that

\exists t_{0} \geq 0

such that

‖ z_{i} (t) - z_{j} (t) ‖ \geq R_{i j} \forall j \neq i

and

\forall t \geq t_{0}

implies that

u_{i}^{a} = u_{i}^{p} = 0 \forall t \geq t_{0}

⁠. Then, returning to yields that

{\dot{W}}_{T} = - K_{p} K_{v} {‖ {\tilde{z}}_{i} ‖}^{2} - K_{v} {‖ {\dot{\tilde{z}}}_{i} ‖}^{2} \leq - α_{3} {‖ [{\tilde{z}}_{i}^{T}, {\dot{\tilde{z}}}_{i}^{T}] ‖}^{2}

(19)

where

α_{3} = min {K_{v}, K_{p} K_{v}}

⁠. Applying [28, Theorem 4.10], one can conclude that the pair

{\tilde{z}}_{i} (t)

and

{\dot{\tilde{z}}}_{i} (t)

converge to zero exponentially, for all

t \geq t_{0}

⁠, i.e.,

‖ [\begin{matrix} {\tilde{z}}_{i} (t) \\ {\dot{\tilde{z}}}_{i} (t) \end{matrix}] ‖ \leq \sqrt{\frac{α_{2}}{α_{1}}} \cdot ‖ [\begin{matrix} {\tilde{z}}_{i} (t_{0}) \\ {\dot{\tilde{z}}}_{i} (t_{0}) \end{matrix}] ‖ e^{- (α_{3} (t - t_{0}) / 2 α_{2})}, t \geq t_{0}

(20)

Theorem 2 guarantees the exponential convergence of agents to the desired trajectory after the resolution of all conflicts. Yet, similar to other potential field-based methods, deadlocks can still occur if a conflict persists. The following section addresses this issue.

3.3 Deadlocks.

A common drawback of potential field methods is the occurrence of deadlocks, that is, when a vehicle cannot reach its desired destination due to persistent interaction with other agents. These situations are due to symmetries between the tracking and avoidance control, i.e., when $u_{i}^{a} \to - {\ddot{z}}_{i}^{d} - K_{v} ({\dot{z}}_{i}^{d} - {\dot{z}}_{i}) - K_{p} (z_{i}^{d} - z_{i})$ ⁠. For damped systems with constant desired trajectories, this condition reduces to $u_{i}^{a} \to - K_{p} (z_{i}^{d} - z_{i})$ [29]. To break the symmetry and aid in conflict resolution, we propose adding the perpendicular perturbation (11c). Note that $u_{i}^{p} \approx 0$ when the vehicle is still in motion (particularly for large $λ_{2}$ ⁠) and is only active when the avoidance control is active (i.e., under the presence of a collision threat). In Ref. [9], it is shown that these perpendicular perturbations can aid in conflict resolution. Finally, it is worth mentioning that different from Refs. [9,29], the perturbation control proposed in (11c) is almost everywhere continuous and smooth.

Even with this control perturbation, we cannot guarantee that the vehicles will always converge to $z_{i}^{d}$ ⁠, particularly under the presence of static obstacles. For instance, vehicles may be trapped between large obstacles, walls, or dead-end corridors, which is a common drawback of all previous approaches [9]. In decentralized scenarios where agents have limited information about their surroundings, such as the problem presented herein, the vehicles may need to apply other heuristic measures (e.g., exploration).

4 Numerical Example

In this section, we present an example with vehicles and obstacles of rectangular shape.

4.1 Minimum Safe Distance.

Assume the shapes of the ith and jth vehicles can be approximated by rectangles with length

ℓ_{i}

⁠,

ℓ_{j}

and width

w_{i}

⁠,

w_{j}

⁠. Without loss of generality, let their lengths be aligned with the

x

-axis (as shown in Fig. 1). Define the following orientation-dependent functions:

β_{i j} = \frac{ℓ_{i}}{2} + \frac{ℓ_{j}}{2} \sqrt{ε^{2} + \cos^{2} {\tilde{ϕ}}_{i j}} + \frac{w_{j}}{2} \sqrt{ε^{2} + \sin^{2} {\tilde{ϕ}}_{i j}}

(21a)

γ_{i j} = \frac{w_{i}}{2} + \frac{ℓ_{j}}{2} \sqrt{ε^{2} + \sin^{2} {\tilde{ϕ}}_{i j}} + \frac{w_{j}}{2} \sqrt{ε^{2} + \cos^{2} {\tilde{ϕ}}_{i j}}

(21b)

where

ε > 0

is a small constant chosen for smoothness and

{\tilde{ϕ}}_{i j} = ϕ_{i} - ϕ_{j}

is the relative orientation. Let

θ_{i j} = atan 2 (z_{j, 2} - z_{i, 2}, z_{j, 1} - z_{i, 1})

represent the angle between

z_{i}

and

z_{j}

⁠. Then, the equation for a rectangle with sides

β_{i j}

and

γ_{i j}

in polar coordinates

ρ_{i j}

and

θ_{i j}

⁠, rotated by

ϕ_{i}

and centered at

z_{i}

⁠, can be approximated by

ζ_{i j} = \sqrt{ε^{2} + (γ_{i j} \cos (θ_{i j} - ϕ_{i}) + β_{i j} \sin (θ_{i j} - ϕ_{i}))^{2}}

(22a)

η_{i j} = \sqrt{ε^{2} + (γ_{i j} \cos (θ_{i j} - ϕ_{i}) - β_{i j} \sin (θ_{i j} - ϕ_{i}))^{2}}

(22b)

ρ_{i j} = \frac{2 β_{i j} γ_{i j}}{ζ_{i j} + η_{i j} - 2 ε}

(22c)

Following the same procedure for the jth agent and using the continuous differentiable approximation of the minimum function [30], one can obtain a smooth function for

r_{i j}

r_{i j} = r_{j i} = δ \sqrt{\frac{2}{ρ_{i j}^{- δ} + ρ_{j i}^{- δ}}}

(23)

where

δ \geq 2

⁠. Choosing smaller

ε \to 0

and larger

δ \to \infty

yields more compact envelopes. Figure 2 illustrates

r_{i j}

for different relative orientations between two vehicles of different sizes. The solid line represents the minimum safe distance that the center of the jth agent can come from the ith vehicle, which is generally shorter than the constant minimum distance scenario.

Fig. 2

View large Download slide

Comparison between the proposed minimum safe distance framework (solid line) and the conservative use of a constant radius $r_{i j}^{max}$ (in dashed line) for different relative orientations. The ith vehicle is represented by the gray rectangles. The jth vehicle is illustrated in different relative positions and orientations by the light rectangular shapes. The vehicles have dimensions $ℓ_{i} = 5 w_{i}$ ⁠, $ℓ_{j} = 3 w_{j},$ and $w_{i} = w_{j}$ ⁠.

4.2 Simulations.

We consider a scenario of

N = 4

identical vehicles with nonlinear dynamics given by . The vehicles are assumed to have rectangular shapes with physical and control parameters given as:

m_{i} = 1 kg

⁠,

J_{i} = 1 kg \cdot m^{2}

⁠,

ℓ_{i} = 2 m

⁠,

w_{i} = 1 m

⁠,

L_{i} = 2 / 3 m

⁠,

R = 6 m

⁠,

K_{p} = 0.5 N / m

⁠,

K_{v} = 1 N \cdot s / m

⁠,

λ_{1} = 5

⁠,

λ_{2} = 3

⁠, and

λ_{3} = 1

⁠. The vehicles are tasked with following trajectories

z_{i}^{d} (t) = [z_{i, 1}^{d} (t), z_{i, 2}^{d} (t)]^{T}

defined as

z_{i, 1}^{d} (t) = 0.9 (t - 20) \forall i \in {1, 2, 3, 4}

(24a)

z_{1, 2}^{d} (t) = - z_{2, 2}^{d} (t) = - \frac{{(z_{2, 1}^{d})}^{3}}{9^{3}}, z_{3, 2}^{d} (t) = - z_{4, 2}^{d} (t) = - \frac{{(z_{4, 1}^{d})}^{3}}{12^{3}}

(24b)

which all pass through a single small static obstacle and between two large static obstacles (or walls) whose shapes are approximated by an arrangement of

2 \times 2 m^{2}

squares. Figure 3(top) illustrates the desired trajectories, the vehicles’ initial configurations, and the location of static obstacles.

Fig. 3

View large Download slide

Sequential motion of vehicles with constant minimum safe distance $r_{i j}^{max}$ ⁠. The top figure illustrates the initial configurations and desired trajectories. Other figures illustrate the sequential motion plotted every $0.5 s$ ⁠. Obstacles are represented by gray squares.

We first simulate the system assuming the conventional potential field-based approach of a constant minimum safe distance. Since the circumradius for the vehicles and the obstacles is $\sqrt{1.25} m$ and $\sqrt{2} m$ ⁠, respectively, we choose $r_{i j} = r_{i j}^{max} = 2.24 m$ as the minimum safe distance with other vehicles and $r_{i j} = r_{i j}^{max} = 2.53 m$ with obstacles. We also choose $Δ_{R} = 2 m$ ⁠, for a constant reaction distance of $R_{i j} \in {4.24 m, 4.53 m}$ ⁠. The system’s response can be seen in Fig. 3, where all agents navigate safely but cannot reach the other side of the domain despite the walls having a separation of $4 m$ ⁠. Figure 4 illustrates the position errors, which keep increasing over time.

Fig. 4

View large Download slide

Position errors when using a constant minimum safe distance

The response of the multi-vehicle system with the proposed control strategy is illustrated in Fig. 5, where we approximated the rectangular shapes using (22) and (23), for $ε = 0.01$ and $δ = 6$ ⁠. Observe that all vehicles are able to track the desired trajectory and transit through a narrow passage safely. In addition, Fig. 6 shows that the agents eventually converge to the desired trajectory once collision threats are over. Finally, the control input force and torque are given in Figs. 7 and 8, respectively, where it can be observed that the control inputs are bounded and continuous.

Fig. 5

View large Download slide

Sequential motion of vehicles with proposed time-varying orientation-based minimum safe distance $r_{i j}$

Fig. 6

View large Download slide

Position errors when using proposed time-varying orientation-based minimum safe distance

Fig. 7

View large Download slide

Control input force when using proposed time-varying orientation-based minimum safe distance

Fig. 8

View large Download slide

Control input torque when using proposed time-varying orientation-based minimum safe distance

5 Conclusions

This letter presents a decentralized, reactive collision avoidance framework for nonholonomic systems of unicycle type and arbitrary shape. The framework is built upon the concept of avoidance functions but exploits the use of a novel non-constant minimum safe distance radius that explicitly considers the shape of vehicles and obstacles, as well as their relative position and orientation. In contrast to previous potential field-based methods, the proposed framework only requires a single function between pairs of agents and generates analytical, smooth control inputs. The overall control strategy is rigorously proven to avoid collisions at all times and to converge exponentially to the desired trajectory once all conflicts have been resolved. To the authors’ knowledge, this is the first provably safe approach to use a time-varying avoidance radius or minimum safe distance within the avoidance functions framework.

Conflict of Interest

There are no conflicts of interest.

Data Availability Statement

The datasets generated and supporting the findings of this article are obtainable from the corresponding author upon reasonable request.

References

1.

Fiorini

,

P.

, and

Shiller

,

Z.

,

1998

, “

Motion Planning in Dynamic Environments Using Velocity Obstacles

,”

Int. J. Rob. Res.

,

17

(

7

), pp.

760

–

772

.

Google Scholar

Crossref

2.

van den Berg

,

J.

,

Snape

,

J.

,

Guy

,

S. J.

, and

Manocha

,

D.

,

2011

, “

Reciprocal Collision Avoidance With Acceleration-Velocity Obstacles

,”

IEEE International Conference on Robotics and Automation

,

Shanghai, China

,

May 9–13

, pp.

3475

–

3482

.

Google Scholar

Crossref

3.

Guo

,

K.

,

Wang

,

D.

,

Fan

,

T.

, and

Pan

,

J.

,

2021

, “

VR-ORCA: Variable Responsibility Optimal Reciprocal Collision Avoidance

,”

IEEE Rob. Autom. Lett.

,

6

(

3

), pp.

4520

–

4527

.

Google Scholar

Crossref

4.

Chakravarthy

,

A.

, and

Ghose

,

D.

,

1998

, “

Obstacle Avoidance in a Dynamic Environment: A Collision Cone Approach

,”

IEEE Trans. Syst. Man Cybern. A Syst. Humans

,

28

(

5

), pp.

562

–

574

.

Google Scholar

Crossref

5.

Haraldsen

,

A.

,

Wiig

,

M. S.

, and

Pettersen

,

K. Y.

,

2023

, “

Dynamic Obstacle Avoidance for Nonholonomic Vehicles Using Collision Cones: Theory and Experiments

,”

Proceedings of IEEE Conference on Control Technology and Applications

,

Bridgetown, Barbados

,

Aug. 16–18

, pp.

46

–

52

.

Google Scholar

Crossref

6.

Khatib

,

O.

,

1986

, “

Real-Time Obstacle Avoidance for Manipulators and Mobile Robots

,”

Int. J. Rob. Res.

,

5

(

1

), pp.

90

–

98

.

Google Scholar

Crossref

7.

Stipanović

,

D. M.

,

Hokayem

,

P. F.

,

Spong

,

M. W.

, and

Šiljak

,

D.

,

2007

, “

Cooperative Avoidance Control for Multiagent Systems

,”

ASME J. Dyn. Syst. Meas. Control

,

129

(

5

), pp.

699

–

707

.

Google Scholar

Crossref

8.

Mastellone

,

S.

,

Stipanović

,

D. M.

,

Graunke

,

C. R.

,

Intlekofer

,

K. A.

, and

Spong

,

M. W.

,

2008

, “

Formation Control and Collision Avoidance for Multi-agent Non-holonomic Systems: Theory and Experiments

,”

Int. J. Rob. Res.

,

27

(

1

), pp.

107

–

126

.

Google Scholar

Crossref

9.

Zhang

,

W.

,

Rodríguez-Seda

,

E. J.

,

Deka

,

S.

,

Amrouche

,

M.

,

Zhou

,

D.

,

Stipanovic

,

D.

, and

Leitmann

,

G.

,

2029

, “

Avoidance Control With Relative Velocity Information for Lagrangian Dynamics

,”

J. Intell. Rob. Syst.

,

99

, pp.

229

–

244

.

Google Scholar

Crossref

10.

Glotfelter

,

P.

,

Cortés

,

J.

, and

Egerstedt

,

M.

,

2017

, “

Nonsmooth Barrier Functions With Applications to Multi-robot Systems

,”

IEEE Contr. Syst. Lett.

,

1

(

2

), pp.

310

–

315

.

Google Scholar

Crossref

11.

Chen

,

Y.

,

Singletary

,

A.

, and

Ames

,

A. D.

,

2021

, “

Guaranteed Obstacle Avoidance for Multi-Robot Operations With Limited Actuation: A Control Barrier Function Approach

,”

IEEE Control Syst. Lett.

,

5

(

1

), pp.

127

–

132

.

Google Scholar

Crossref

12.

Rodríguez-Seda

,

E. J.

,

Tang

,

C.

,

Spong

,

M. W.

, and

Stipanović

,

D. M.

,

2014

, “

Trajectory Tracking With Collision Avoidance for Nonholonomic Vehicles With Acceleration Constraints and Limited Sensing

,”

Int. J. Rob. Res.

,

33

(

12

), pp.

1569

–

1592

.

Google Scholar

Crossref

13.

Huang

,

S.

,

Teo

,

R. S. H.

, and

Tan

,

K. K.

,

2019

, “

Collision Avoidance of Multi Unmanned Aerial Vehicles: A Review

,”

Annu. Rev. Control

,

48

, pp.

147

–

164

.

Google Scholar

Crossref

14.

Tang

,

J.

,

Lao

,

S.

, and

Wan

,

Y.

,

2022

, “

Systematic Review of Collision-Avoidance Approaches for Unmanned Aerial Vehicles

,”

IEEE Syst. J.

,

16

(

3

), pp.

4356

–

4367

.

Google Scholar

Crossref

15.

Sunkara

,

V.

,

Chakravarthy

,

A.

, and

Ghose

,

D.

,

2019

, “

Collision Avoidance of Arbitrarily Shaped Deforming Objects Using Collision Cones

,”

IEEE Rob. Autom. Lett.

,

4

(

2

), pp.

2156

–

2163

.

Google Scholar

Crossref

16.

Minguez

,

J.

, and

Montano

,

L.

,

2009

, “

Extending Collision Avoidance Methods to Consider the Vehicle Shape, Kinematics, and Dynamics of a Mobile Robot

,”

IEEE Trans. Rob.

,

25

(

2

), pp.

367

–

381

.

Google Scholar

Crossref

17.

Borenstein

,

J.

,

1994

, “

Real-Time Obstacle Avoidance for Non-point Mobile Robots

,”

Proceedings of World Conference on Robotics Research

,

Pittsburgh, PA

,

Sept. 17–19, 1991

.

18.

Wu

,

Z.

,

Hu

,

G.

,

Feng

,

L.

,

Wu

,

J.

, and

Liu

,

S.

,

2016

, “

Collision Avoidance for Mobile Robots Based on Artificial Potential Field and Obstacle Envelope Modelling

,”

Assembly Autom.

,

36

(

3

), pp.

318

–

332

.

Google Scholar

Crossref

19.

Braquet

,

M.

, and

Bakolas

,

E.

,

2022

, “

Vector Field-Based Collision Avoidance for Moving Obstacles With Time-Varying Elliptical Shape

,”

Proceedings of Modeling, Estimation, and Control Conference

,

Jersey City, NJ

,

Oct. 2–5

, pp.

587

–

592

.

Google Scholar

Crossref

20.

Xie

,

H.

,

Patel

,

R.

,

Kalaycioglu

,

S.

, and

Asmer

,

H.

,

1998

, “

Real-Time Collision Avoidance for a Redundant Manipulator in an Unstructured Environment

,”

Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems

,

Victoria, BC, Canada

,

Oct. 17

, Vol.

3

, pp.

1925

–

1930

.

Google Scholar

Crossref

21.

Zimmermann

,

S.

,

Busenhart

,

M.

,

Huber

,

S.

,

Poranne

,

R.

, and

Coros

,

S.

,

2022

, “

Differentiable Collision Avoidance Using Collision Primitives

,”

IEEE/RSJ International Conference on Intelligent Robots and Systems

,

Kyoto, Japan

,

Oct. 23–27

, p.

8086

–

8093

.

Google Scholar

Crossref

22.

Zhang

,

X.

,

Liniger

,

A.

, and

Borrelli

,

F.

,

2021

, “

Optimization-Based Collision Avoidance

,”

IEEE Trans. Control Syst. Technol.

,

29

(

3

), pp.

972

–

983

.

Google Scholar

Crossref

23.

Hu

,

Y.

,

Fu

,

J.

, and

Wen

,

G.

,

2023

, “

Decentralized Robust Collision-Avoidance for Cooperative Multirobot Systems: A Gaussian Process-Based Control Barrier Function Approach

,”

IEEE Trans. Control Network Syst.

,

10

(

2

), pp.

706

–

717

.

Google Scholar

Crossref

24.

Brockett

,

R. W.

,

1983

, “

Asymptotic Stability and Feedback Stabilization

,”

Differential Geometric Control Theory

,

R. W.

Brockett

,

R. S.

Millman

, and

H. J.

Sussmann

, eds.,

Birkhauser

,

Boston, MA

, pp.

181

–

191

.

25.

Sarkar

,

N.

,

Yun

,

X.

, and

Kumar

,

V.

,

1994

, “

Control of Mechanical Systems With Rolling Constraints: Application to Dynamic Control of Mobile Robots

,”

Int. J. Rob. Res.

,

13

(

1

), pp.

55

–

69

.

Google Scholar

Crossref

26.

Rodríguez-Seda

,

E. J.

, and

Stipanović

,

D. M.

,

2020

, “

Cooperative Avoidance Control With Velocity-Based Detection Regions

,”

IEEE Control Syst. Lett.

,

4

(

2

), pp.

432

–

437

.

Google Scholar

Crossref

27.

Rodríguez-Seda

,

E. J.

,

2024

, “

Decentralized Low-Energy Avoidance Control Framework for Multiple Mobile Agents Using Irregular Observations

,”

IEEE Trans. Control Syst. Technol.

,

32

(

5

), pp.

1027

–

1039

.

28.

Khalil

,

H. K.

,

2002

,

Nonlinear Systems

,

Prentice Hall

,

Upper Saddle River, NJ

.

29.

Rodríguez-Seda

,

E. J.

, and

Stipanović

,

D. M.

,

2013

, “

Guaranteed Collision Avoidance With Discrete Observations and Limited Actuation

,”

Advances in Intelligent Vehicles

,

Y.

Chen

, and

L.

Li

, eds.,

Intelligent Systems, Academic Press

,

Boston, MA

, pp.

89

–

110

.

30.

Stipanović

,

D. M.

,

Tomlin

,

C. J.

, and

Leitmann

,

G.

,

2012

, “

Monotone Approximations of Minimum and Maximum Functions and Multi-Objective Problems

,”

Appl. Math. Optim.

,

66

(

3

), pp.

455

–

473

.

Google Scholar

Crossref

Reactive Collision Avoidance Control for Nonholonomic Vehicles and Obstacles of Arbitrary Shape

Abstract

1 Introduction

2 Problem Formulation

2.1 Multi-Vehicle Dynamics.

2.2 Minimum Safe Distance.

2.3 Control Objective.

3 Trajectory Tracking With Collision Avoidance Control

3.1 Avoidance Functions.

3.2 Control Law.

Collision avoidance

Trajectory tracking

3.3 Deadlocks.

4 Numerical Example

4.1 Minimum Safe Distance.

4.2 Simulations.

5 Conclusions

Conflict of Interest

Data Availability Statement

References

Get Email Alerts

Cited By

ASME Journals

ASME Conference Proceedings

ASME eBooks

Resources

Opportunities

Reactive Collision Avoidance Control for Nonholonomic Vehicles and Obstacles of Arbitrary Shape

Abstract

1 Introduction

2 Problem Formulation

2.1 Multi-Vehicle Dynamics.

2.2 Minimum Safe Distance.

2.3 Control Objective.

3 Trajectory Tracking With Collision Avoidance Control

3.1 Avoidance Functions.

3.2 Control Law.

Collision avoidance

Trajectory tracking

3.3 Deadlocks.

4 Numerical Example

4.1 Minimum Safe Distance.

4.2 Simulations.

5 Conclusions

Conflict of Interest

Data Availability Statement

References

Get Email Alerts

Cited By

Related Articles

Related Proceedings Papers

Related Chapters

ASME Journals

ASME Conference Proceedings

ASME eBooks

Resources

Opportunities

This Feature Is Available To Subscribers Only