Simultaneous Position and Orientation Planning of Nonholonomic Multi-Robot Systems: A Dynamic Vector Field Approach

Motion planning is a fundamental problem in the field of robotics and control, which aims at finding paths or trajectories to guide mobile robots moving from initial conditions to their respective destinations, while avoiding collisions with obstacles and other robots. One of the most typical robotic systems is the nonholonomic mobile robot, which is also referred to as the unicycle-type vehicle. Although neglected by a number of persons, two facts regarding the nonholonomic mobile robot should be mentioned above all. One is that the theoretical model of such a kind of robot is essentially a rigid body rather than a point of mass [3], since the robot states include both position and orientation (or attitude). Different from a 3-degree-of-freedom (DOF) rigid body moving freely on a plane, such a robot has no lateral velocity due to the nonholonomic constraint, which naturally brings about the other fact that the nonholonomic mobile robot is indeed an underactuated system [4]. To be more specific, the robot has three DOFs (two translation DOFs and one rotation DOF), while possessing only two control inputs (one linear velocity and one angular velocity). These two facts make the motion planning for nonholonomic mobile robots more challenging and demanding.

Although a variety of methodologies have been proposed for motion planning, such as roadmap [18, 1, 22], cell decomposition [6, 35, 8], sampling-based algorithm [17, 14, 30], they cannot be applied to nonholonomic mobile robots resulting from omitting the kinematics or only considering the model of the point of mass. Regarding the nonholonomic model, some researchers employ the optimization-based methods [13, 11, 36, 2, 25, 7, 37, 23]. For example, Bloch et al. in [2] formulate the problem as dynamic interpolation on Riemannian manifolds and provide the necessary conditions for optimality. Li et al. in [25] propose a prioritized optimization method so as to compute the planning results efficiently. In [7], Cichella et al. utilize the Bernstein polynomials to transform the motion planning into a discrete optimization approximately. There is no denying that optimization methods have the advantage of easily handling unicycle models, because the nonholonomic constraints can always be included as the optimization constraints. Nevertheless, the feasibility of such optimization problems are unable to be guaranteed, or they suffer from extremely heavy computational burden. Another drawback is that the control inputs derived from optimization are open-loop, relying only on time, thereby are not robust to disturbances.

Given above shortages, researchers are motivated to investigate feedback motion planning algorithms for mobile robots. A typical closed-loop methodology is the velocity vector field, where a velocity vector related to the state is defined at every point in the configuration space and the integral curve of the vector field converges to the goal point. The most common vector field is defined by the gradient of a potential function, also referred to as the potential field [10, 12, 34, 16, 21, 33], but one of its inherent limitations is the possible local minina other than the desired state [20]. Particular forms of potential function overcome such a drawback, that is, harmonic function [19, 9, 28] and navigation function [32, 27, 24]. However, the former has demanding computational complexity related to PDEs, while the latter is difficult to implement since a lower bound to ensure no local minima is unknown in advance.

As a matter of fact, the velocity vector field does not necessarily have to be given by a potential gradient. Instead, it can be defined directly over the configuration space. To the best of our knowledge, few works focus on motion planning via non-gradient-based vector fields, except two seminal papers by Lindemann et al. [26] and Panagou [31]. In [26], the environment with obstacles is decomposed into convex polytopes, in which simple local vector fields are defined and smoothly blended to form a global vector field convergent to the desired point. The author in [31] proposes a family of 2-D analytic vector fields, which exhibit different patterns (such as attractive or repulsive) by choosing the value of an parameter, so that the overall vector field can finally be obtained by a suitable blending design.

Regardless of open-loop or feedback algorithms, the aforementioned results rarely consider a significant yet implicit fact typically occurring in real-world scenarios. Actually, apart from the position constraints, the orientation of a robot is also usually required to reach a desired direction at the terminal time. For instance, in the multi-robot surveillance mission, the final orientation of each robot should point to a certain direction so as to obtain the largest overall surveillance area. Similarly, the attitudes of missiles are generally specified in the terminal guidance so as to realize a better performance of coordinated attack. Thus, these practical demands strongly motivate us that it is necessary to incorporate the orientation constraints into the motion planning. Additionally, such a motivation is further strengthened from a theoretical point of view. As mentioned above, the model of a nonholonomic mobile robot is a rigid body. Then, serving as a state, the attitude angle should also be specified to be a desired value, similar to the position, rather than being left randomly, which can be referred to as full-state motion planning. Since the nonholonomic mobile robot is an underactuated system, the biggest challenge lies in how to achieve full-state planning by fewer control inputs.

Motivated by above discussions, in this paper we consider the simultaneous position and orientation planning of nonholonomic multi-robot systems by designing a non-gradient-based velocity vector field. To begin with, the mobile robot is modelled as a rigid body with nonholonomic constraints rather than a point of mass. More importantly, besides the desired position constraint, we take into account the orientation constraint at the final time instant as well. Next, different from common static vector fields on a 2-D plane, we propose a novel dynamic vector field (DVF) in the sense that the dynamics of the attitude angle are introduced into the vector field. This implies that the velocity direction at a certain point is decided by not only the position but also the orientation of robot that passes such a point. Thus, by moving along the integral curve of the DVF, the robot can reach the specified position at the terminal time, and meanwhile the attitude angle can converge to the desired value following the orientation dynamics. Subsequently, the control inputs or velocities are designed based on the DVF, where the feedback of an extra angle in the body-fixed frame is brought in as an additional angular velocity, with the result that the robot orientation can be tuned along the direction of the DVF to deal with the lack of lateral velocity. Moreover, under the frame work of the DVF, the problems of obstacle avoidance and mutual-robot-collision avoidance are studied by proposing a circular vector field, where robots can move along the tangential directions of the round obstacles so as to evade collisions. Note that such two kinds of collision avoidance problems are both able to be solved by one circular vector field, which greatly simplifies the design of planned trajectories.

Apart from the dynamical characteristics, the following merits also distinguish our method from the existing motion planning results which also utilize non-gradient-based vector fields. As opposed to [26], the proposed dynamic vector field is unnecessary to be derived based on a cell decomposition of the whole environment, then advanced high-level discrete motion planning is not required in this paper. In contrast to [31], the dynamic vector field is global over the state space in the sense that the initial and final configurations can both be chosen arbitrarily, while the vector field in [31] has a separatrix (or mirror line) where the integral curves can become divergent possibly.

The organization of this paper is given below. Section II provides mathematical preliminaries and formulates the motion planning problem. Section III proposes the dynamic vector field approach, under which the problems of obstacle avoidance and collision avoidance are solved by designing a circular vector field in Section IV and Section V, respectively. Section VI gives several numerical simulation examples to verify the effectiveness of the proposed vector fields. Finally, conclusions end the whole paper in Section VII.

Several commonly-used notations are defined in advance. The identity matrix in $R^n$ is denoted by $I_n$. The base vectors of $R^3$ are denoted by $e_1,e_2,e_3$. The symbol $0_{m\times n}$ represents a matrix in $R^{m\times n}$ with all zero components. The Euclidean norm of a vector is denoted by $\parallel \cdot \parallel$. Moreover, variables denoting vectors and matrices are written in bold, while those denoting scalars are not.

As mentioned in the Introduction, the model of the nonholonomic mobile robot is a rigid body. Thus, before providing the nonholonomic model, we firstly consider a fully actuated planar rigid body moving in a 2-D Euclidean space. Let $F_E$ denote the earth-fixed frame, and let $F_B$ represent the body-fixed frame, which is attached to the center of mass of the rigid body. The position of the rigid body in $F_E$ is given by a vector $p=[xy{]}^T\in R^2$, and the attitude is specified by a rotation matrix $R\in R^{2\times 2}$, which depicts the rotation of $F_B$ relative to $F_E$. Herein, the rotation matrix $R$ can be parameterized by a scalar $\theta \in [-\pi ,\pi ]$, that is,

where $\theta$ is interpreted as the attitude angle of the rigid body. Let $\omega \in R$ and $v=[v^x{v}^y{]}^T\in R^2$ denote the rigid body’s angular velocity and linear velocity in the body-fixed frame $F_B$. Then, the kinematics of the fully actuated rigid body can be given by

Regarding nonholonomic mobile robots, due to no side slip of the wheels, the robot cannot move sideways. In other words, the velocity along the $Y_B$-axis of the body-fixed frame $F_B$ is always zero, that is, $v_y=0$. Such a constraint is named the nonholonomic constraint, since its reformulation in the earth-fixed frame $F_E$ which is a differential equation given by

cannot be integrated to be an algebraic equation. Thus, according to (2), the kinematic model of a nonholonomic mobile robot degenerates to

Given the fact that the nonholonomic mobile robot moves in an environment possibly populated with obstacles, we assume that each obstacle can be bounded by a circular region with radius $r_o>0$. Moreover, suppose that multiple nonholonomic mobile robots move in a common workspace simultaneously. Therefore, the potential collisions with obstacles and among robots should both be taken into account in the motion planning process. To acquire the information of obstacles and other robots positions, the robot $i$ ($i=1,\cdots ,N$) is assumed to have a circular sensing range with radius $R_s$, which can be defined by

where $p_i$ is the current position vector of the robot $i$. Then, once obstacles and other robots appear into the sensing region $S_i$, the robot $i$ can obtain the position information of them.

The motion planning problem for multiple nonholonomic mobile robots can be formulated below.

Problem Statement: Regarding $N$ nonholonomic mobile robots described by (3), design linear velocity $v_{ix}$ and angular velocity ${\omega }_i$ for the robot $i$ ($i=1,\cdots ,N$), such that each controlled trajectory of the robots, which starts from the initial condition $(x_{i0},y_{i0},{\theta }_{i0})$, can reach the specified destination $(x_{id},y_{id},{\theta }_{id})$, and meanwhile avoid the obstacles and mutual collisions with other robots.

In this section, we construct a non-gradient-based vector field to solve the motion planning problem for a nonholonomic mobile robot in an obstacle-free environment. With the loss of generality, we assume that the destination of the robot $(x_d,y_d,{\theta }_d)$ is chosen as $(0,0,0)$. For the sake of simplicity, we firstly rewrite the kinematics (2) and (3) in a more compact form.

Define the following matrix

which is uniquely decided by the rotation matrix $R$ and the position vector $p$, and we call $h$ the configuration of the rigid body. Similarly, the velocity can be formulated in a matrix as

where the hat operator $\hat{\cdot }$ defines a map from a scalar to a skew symmetric matrix in $R^{2\times 2}$. Therefore, based on (4) and (5), the fully actuated kinematic model (2) can be redefined by

where the configuration $h$ and the velocity $\eta$ serve as the state and control input, respectively. Correspondingly, the nonholonomic kinematic model can be rewritten as (6) with an additional nonholonomic constraint

where $e_i$ ($i=1,2,3$) are standard basis in $R^3$.

To design the vector field, a transformation is introduced for the configuration $h$. By utilizing the matrix logarithmic map proposed in [4, 5], we obtain the logarithm of configuration $h$ as follows

where $\hat{\theta }$ is a $2\times 2$ skew symmetric matrix with respect to $\theta$, similar to the form of $\hat{\omega }$ in (5), and the vector $\phi (x,y,\theta )$ is given by

Once defining a matrix

the formula (8) can be simplified as

where $p$ is the position vector. Then, we refer to ${\rm Y}$ given in (7) as the transformed configuration, which is uniquely defined by the attitude angle $\theta$ and position vector $p$. According to [5], under the transformed configuration ${\rm Y}$, the kinematics (6) can be reformulated as

where the transformed configuration ${\rm Y}$ is the state, the velocity $\eta$ is the control input, and $M\left({\rm Y}\right)\in R^{3\times 3}$ is a state-dependent matrix satisfying

We recommend [5] for readers who are interested in more information about the kinematics (11) and matrix $M\left({\rm Y}\right)$.

Now, we propose a vector field ${\Gamma }_d:R^2\times S\to R^2$, whose components ${\Gamma }_d^x$ and ${\Gamma }_d^y$ are given by

where $\theta$ serve as an internal parameter satisfying

The convergence of ${\Gamma }_d$ is provided in the following theorem.

In the following, we extend the dynamic vector field ${\Gamma }_d$ to an arbitrarily-specified final state $(x_d,y_d,{\theta }_d)$. Based on (4), once there holds $(x,y,\theta )=(0,0,0)$, the corresponding configuration matrix $h$ becomes a $3\times 3$ identity matrix $I_3$. Thus, the dynamic vector field ${\Gamma }_d$ given in (13), (14) can drive $h$ to $I_3$, indeed. Let $h_d$ denote the corresponding configuration matrix of the final state $(x_d,y_d,{\theta }_d)$. Then, the problem now becomes how to define a dynamic vector field ${\tilde{\Gamma }}_d$ which can drive the configuration $h$ to $h_d$. Motivated by the fact that ${\Gamma }_d$ achieves $h\to I_3$,, we define a new configuration

which contains the information of the desired final state $h_d$. The parameterized description of $\tilde{h}$, denoted by $(\tilde{x},\tilde{y},\tilde{\theta })$, can be given by $\tilde{x}=(x-x_d)\cos {\theta }_d+(y-y_d)\sin {\theta }_d$, $\tilde{y}=-(x-x_d)\sin {\theta }_d+(y-y_d)\cos {\theta }_d$, $\tilde{\theta }=\theta -{\theta }_d$. Based on above definitions, we present the following theorem.

Having obtained the dynamic vector field ${\tilde{\Gamma }}_d$, we can further design the control inputs $\omega$ and $v_x$. It should be noted that the dynamic vector field ${\tilde{\Gamma }}_d$ in (19), (14) is defined in the earth-fixed frame $F_E$. In contrast, the control inputs are the velocities given in the body-fixed frame $F_B$. Thus, for the purpose of a simple controller design, we transform the dynamic vector field ${\tilde{\Gamma }}_d$ from the earth-fixed frame $F_E$ to the body-fixed frame $F_B$. Let ${\tilde{\Gamma }}_d^B$ denote the dynamic vector field in $F_B$. Then, ${\tilde{\Gamma }}_d^B$ can be obtained by

where $R$ is the rotation matrix defined in (1). Thus, based on (20), it will be straightforward to design the motion planning controller of fully actuated rigid bodies, which can be obtained by making the body velocity $\eta$ equivalent to ${\tilde{\Gamma }}_d^B$, that is

where $k_v,k_{\omega }$ are positive scalars.

However, regarding nonholonomic mobile robots, there does not exist any control input along the direction of $v_y$. Hence, only $v_x$ and $\omega$ can be used to make the robot “follow” the dynamic vector field ${\tilde{\Gamma }}_d^B$. To this end, we introduce an additional rotation in $\omega$, which is a negative feedback relevant to the angle between the orientation of the robot and the direction of ${\tilde{\Gamma }}_d^B$. Specifically, as shown in Fig. 2, the orientation of the robot is along the $X_B$-axis of $F_B$, but the integral curve of the vector field moves along the direction of ${\tilde{\Gamma }}_d^B$. The angle between these two directions which is denoted by $\delta$ can be given by

where the negative sign means $\delta$ rotates along a counter direction with respect to the attitude angle $\theta$. Note that for nonholonomic mobile robots, the linear velocity $v_x$ has the same direction as the orientation. Therefore, in order to make the robot “follow” the vector field, the orientation of the robot should be rotate to the direction of ${\tilde{\Gamma }}_d^B$. Then, we can construct an additional angular velocity, which is an negative feedback of $\delta$, so as to rotate the robot orientation to the direction of ${\tilde{\Gamma }}_d^B$. Thus, based on (21), the motion planning controller for nonholonomic mobile robots is designed to be

where $k_v,k_{\omega },k_a$ are all positive scalars.

This section focuses on the motion planning problem involving obstacle avoidance. Instead of using a repulsive vector field directly, we design a circular vector field around the obstacle, and then blend it with the dynamic vector field which is convergent to the desired configuration.

Consider a circular obstacle with radius $r_o$ located at $p_o=[x_o{y}_o{]}^T$. Suppose that the obstacle avoidance vector field lies in the following area

where $R_s$ is the radius of the sensing region $S_i$. Once the robot enters the area $D$, we define a repulsive vector

which points from the obstacle to the nonholonomic mobile robot. Then, the distance between obstacle and robot can be obtained by $d=\parallel {\Gamma }_r\parallel$. Let ${\Gamma }_r^{\perp }$ denote the a vector satisfying the following two conditions

where $v$ is the velocity vector in the body-fixed frame $F_B$ of the nonholonomic mobile robot. Intuitively, the vector ${\Gamma }_r^{\perp }$ is perpendicular to ${\Gamma }_r$ and projected positively onto the velocity direction $v$. According to such a definition, we can derive that the vector ${\Gamma }_r^{\perp }$ always points to a collision-free direction with respect to the obstacle, since ${\Gamma }_r^{\perp }$ will be tangent to a certain circle located at $p$ with radius $r$ ($r_o<r\le R_o$). Thus, all the vectors ${\Gamma }_r^{\perp }$ in the area $D$ can constitute a circular vector field surrounding the obstacle.

Nevertheless, it is fairly unreasonable to construct the obstacle avoidance vector field only by ${\Gamma }_r^{\perp }$, since there must exist some situations where the nonholonomic mobile robot will never collide with the obstacle although moving in the area $D$. In such cases, it is unnecessary to construct obstacle avoidance vector field for the robot with the help of ${\Gamma }_r^{\perp }$. Otherwise, the results would become rather conservative. Therefore, in the following, we will design the obstacle avoidance vector fields in the area $D$, denoted by ${\Gamma }_o$, according to different situations of the robot with respect to the obstacle. The detailed construction of ${\Gamma }_o$ are provided below.

Assume that the robot has entered the area $D$, that is, the distance $d$ satisfying $r_o<d\le R_o$. Let ${\theta }_r$ represent the angle between the velocity direction $v$ and the line segment connecting robot and obstacle, as shown in Fig. 3, which can be computed by

Hence, according to the value of ${\theta }_r$, three different cases can be proposed as follows.

1. If ${\theta }_r\ge \frac{\pi }{2}$, as shown in Case 1 of Fig. 3, the robot does not have the risk of colliding with the obstacle. Then, in this case, the obstacle avoidance vector field can absolutely be chosen as the convergent dynamic vector field in the free space, i.e., ${\Gamma }_o={\tilde{\Gamma }}_d$.

2. If $0<{\theta }_r<\frac{\pi }{2}$, as shown in Case 2 of Fig. 3, it is possible that the robot will collide with the obstacle in a future time. Thus, we set the obstacle avoidance vector field to be ${\Gamma }_r^{\perp }$, i.e., ${\Gamma }_o={\Gamma }_r^{\perp }$, which is defined in (25). Then, the robot will turn to the direction that is perpendicular to the radius of the obstacle so as to achieve the obstacle avoidance.

3. Except for above two cases, the rest one is ${\theta }_r=0$, as shown in Case 3 of Fig. 3. Generally, in such a case, it is referred to as the deadlock of a robot, which corresponds to a local minima of an artificial potential. Once ${\theta }_r=0$, it can be derived that $⟨{\Gamma }_r^{\perp },v⟩=0$, indicating the vector ${\Gamma }_r^{\perp }$ is perpendicular to the velocity $v$, so that we cannot define the obstacle avoidance vector field by checking whether the projection of ${\Gamma }_r^{\perp }$ onto $v$ is positive, as given in (25). Actually, the reason why we choose the ${\Gamma }_r^{\perp }$ positively projected onto $v$ is that such a vector can make the robot turn a smaller attitude angle for obstacle avoidance than the negatively projected one. However, when ${\theta }_r=0$, the rotation angle from $v$ to ${\Gamma }_r^{\perp }$ is always $\frac{\pi }{2}$, either clockwise or anticlockwise. Thus, we can directly choose one of these two rotation directions to define the obstacle avoidance vector field ${\Gamma }_o$, and in this paper, ${\Gamma }_o$ is defined by rotating ${\Gamma }_r$ clockwise through $\frac{\pi }{2}$, that is, ${\Gamma }_o=R_{\frac{\pi }{2}}^T{\Gamma }_r$, where $R_{\frac{\pi }{2}}$ is rotation matrix given in (1) with $\theta =\frac{\pi }{2}$.

To summarize, based on above definitions under different situations, the obstacle avoidance vector field can be given by

Therefore, we can present the dynamic vector fields with obstacle avoidance in $R^2$ as follows

Note that the vector field given in (27) would be discontinuous at $d=R_o$. To make the vector field continuous, we introduce the following transition function

where $ϵ$ is a small positive constant. It is obvious that the transition function $\varsigma$ continuously varies from $0$ to $1$ as the distance $d$ varies from $R_o+ϵ$ to $R_o$. Therefore, the vector fields proposed in (27) can be revised to be a continuous form, that is

We summarize above results into the following theorem.

Although Theorem 3 is derived based on only one obstacle, it can be simply extended to the case of multiple obstacles. Regarding $M$ obstacles, the dynamic vector field is designed to be

where ${\tilde{\Gamma }}_d$ is the convergent dynamic vector field, ${\Gamma }_{oi}$ is the obstacle avoidance vector field around the $i$th obstacle, ${\varsigma }_i$ is the transition function regarding the $i$th obstacle.

Having obtained the vector field in (33), it would not be difficult to design the controller for the nonholonomic mobile robot. Similar to (20), we transform the vector field ${\Gamma }_P$ into the body-fixed frame $F_B$ by

Then, inspired by (23), the controller can be given by

where $k_v,k_{\omega },k_a$ are all positive scalars.

In this section, we consider the problem of collision avoidance among multiple nonholonomic mobile robots during the motion planning. For simplicity, the collision avoidance between two robots is taken into account at first.

Motivated by the circular vector field presented in the obstacle avoidance, intuitively, we can introduce a virtual obstacle between two robots so that they are able to avoid each other by avoiding the virtual obstacle. To be more specific, as shown in Fig. 4(a), regarding two robots positioned at $p_i$ and $p_j$, when there is a potential collision risk in their sensing ranges, a virtual obstacle is set on the line segment $\overline{p_i{p}_j}$. According to the obstacle avoidance vector field in Section IV, the robots will turn to the direction of ${\Gamma }_{ri}^{\perp }$ and ${\Gamma }_{rj}^{\perp }$, respectively, as the green vectors in Fig. 4(a), which are projected positively along $v_i$ and $v_j$, respectively. Then, two robots will follow the vectors ${\Gamma }_{ri}^{\perp }$ and ${\Gamma }_{rj}^{\perp }$ to avoid the virtual obstacle clockwise. In this way, the collision avoidance between two robots can be realized completely.

Nevertheless, in Fig. 4(a), it should be noted that the velocities $v_i$ and $v_j$ lie in the different sides of the line segment $\overline{p_i{p}_j}$. Once $v_i$ and $v_j$ lie in the same side of $\overline{p_i{p}_j}$, as given in Fig. 4(b), the obstacle avoidance vectors ${\Gamma }_{ri}^{\perp }$ and ${\Gamma }_{rj}^{\perp }$ which are projected positively along $v_i$ and $v_j$ will point to the opposite rotation directions around the virtual obstacle, i.e., clockwise and anticlockwise, respectively. Then, two mobile robots will turn to such directions and cause the collision eventually. Therefore, the obstacle avoidance vector field cannot be extended to collision avoidance straightforwardly by introducing an virtual obstacle between two robots.

Based on abovementioned analysis, the key point of the collision avoidance is how to define the direction of vectors ${\Gamma }_{ri}^{\perp }$ and ${\Gamma }_{rj}^{\perp }$ so as to make them both clockwise or both anticlockwise. To solve this problem, we still consider two robots located at $p_i$ and $p_j$, and suppose that they have been into each other’s sensing range, i.e., $d_{ij}=\parallel p_i-p_j\parallel <R_s$. In addition, we assume that the distance threshold for starting collision avoidance denoted by $R_c$ satisfies $R_c<R_s$. That is to say, as shown in Fig. 5, when $d_{ij}\le 2R_c$, two robots are supposed to have a potential risk of collision, and the vector fields should be converted from the mode of target navigation to collision avoidance.