# A Novel Collision-Free Navigation Approach for Multiple Nonholonomic Robots Based on ORCA and Linear MPC

Authors: Run Mao; Hongli Gao; Liang Guo
Journal: Mathematical Problems in Engineering
 (2020)

---

## Abstract

In the study of collision-free navigation methods of multirobots, much attention has been paid to the constraints of external environment. However, most of the wheeled mobile robots are subjected to nonholonomic constraints. A collision between robots may occur if the nonholonomic constraints are neglected. This paper presents an improved approach to collision-free navigation for multi-nonholonomic robots. This approach combines the Optimal Reciprocal Collision Avoidance (ORCA) algorithm and Model Predictive Control (MPC) strategy. ORCA used a simple robot model, in which kinematics and dynamics are ignored. To cope with this problem, the MPC controller is introduced. In each ORCA step, the reference trajectory, reference control inputs, and “safe zones” are generated based on the new velocity. Consequently, the derived safe zone is transformed into the constraints of decision variables for a MPC controller. Finally, quadratic programming is used to solve the MPC problem by successive linearization of an error model of the mobile robot. Simulation results illustrate the effectiveness of the proposed method.


---

## Body

## 1. Introduction

Motion planning/collision avoidance is concerned with computing a path or trajectory between two configurations embedded in cost field, while taking into account motion constraints, static obstacles, and moving obstacles [1]. The problem has been well studied for one robot avoiding static or moving obstacles [2–4].Many classical approaches such as artificial potential field method [5], sampling-based algorithms [6–9], and dynamic window [10] are extended to dynamic environments [11]. These approaches assume the observed obstacles to be static in a period of time [12–14] and compute an immediate action to avert collisions with the consideration of kinematics and dynamics in many cases. When considering moving obstacles, such approaches typically repeatedly plan based on continuous cycle of sensing and acting. This may work well as if the obstacles move slower than the robot, but among fast obstacles, the static assumption will not lead a collision-free navigation. The methods such as multilayer fuzzy control method [15], reactive collision avoidance [16], and rolling window method [17] are also used in the dynamic environment. However, these methods based on a simple robot model ignore the nonholonomic constraints.The problem of collision avoidance becomes even more complicated when the obstacles are also decision-making agents instead of entities moving arbitrarily without considering their environment [18]. This problem has many applications in the areas of robotics, such as multirobots navigation and coordination in the Container Terminal Logistics System [19] and crowd simulation for computer graphics and VR [20]. In the context of collision avoidance for multiple robots, similar approaches to those for the single robot case can be applied [21–23]. However, the increase in robot density may lead to oscillations [24]. Many motion-planning methods of multiple point-mass robots have been developed [24–26]. It means that applying these methods into practice directly is difficult because the mobility of the robot is ignored. The Net-MPC strategy is also used to navigate in multirobots environment [27–30]. However, these methods relying on the communication between robots may increase the complexity of system.A particularly successful method for mobile robot real-time navigation is the ORCA algorithm [31]. It is an effective decentralized collision avoidance method for a large number of decision-making agents. The ORCA infers for each robot a half-plane of velocities that are allowed to be selected in order to guarantee collision avoidance without communication among the robots or central coordination. This method works well if the robots act on holonomic constraints, since the robots must move along the direction of a new selected velocity by ORCA immediately. For many nonholonomic constrained robots, such as car-like robots, the set of feasible velocities at any instant is a specific velocity in the direction of the rear wheels [32, 33]. One way to work around these constraints is to use the set of velocities that can be achieved over some time interval τ [26]. An optimal method for distributed collision avoidance among multiple nonholonomic robots is presented in [32]. Furthermore, Jeon and Lee defined the wheel velocity obstacle, which is a set of all the left and right wheel velocity pairs that induce collisions with obstacles within a given time horizon [34]. However, these methods can only guarantee collision-free with the instant of each step time. To alleviate these issues, we can select a small value for τ, but this has some implications for navigation: as τ decreases, the set of velocities that are being considered by the robot becomes smaller, and the robot can miss feasible controls.The Model Predictive Control (MPC) method is suitable for solving the multivariable constrained control problems due to its predictive nature. It means that the MPC method can handle the environment constraints and robot constraints in a single optimization problem [35, 36]. The applications of MPC for navigation of a single robot have been reported in [36–38].Motivated by the features of MPC and ORCA methods, in this paper, we propose a Discrete-ORCA-MPC combined approach for the problem where collisions need to be avoided among multiple nonholonomic mobile robots with decision-making ability. Concretely, the ORCA algorithm is used by each agent to generate independently its new velocity at each cycle of sensing (ORCA step timeτ). This new velocity is used to generate a discrete reference control input and trajectory. Meanwhile, a series of safe zones are constructed, which serve as constraints in a MPC problem. Unlike original ORCA, the construction of safe zones needs to know the new velocities of other agents in the neighbor region at each step. Furthermore, the proof for collision-free motion of this Discrete-ORCA-MPC approach is derived. Although Nonlinear Model Predictive Control (NMPC) has been well developed, the computational effort necessary is much higher than the linear model [39, 40]. This paper uses a successive linearization approach yielding a linear, time-varying description of the system which can be solved through linear MPC. Considering the control inputs as the decision variables, it is then used to transform the optimization problem into a Quadratic Programming (QP) problem. Since this is a convex problem, QP problems can be solved by commercial numerical solvers which lead to global optimal solutions. More importantly, it is easy to a real-time implementation. In addition, we focus on differential drive mobile robots (DDMRs) in the following work, even though our approach applies more generally for the class of feedback linearizable vehicles with nonholonomic kinematics, such as car-like robots or DDMR with trailer.The main features of the proposed Discrete-ORCA-MPC combined approach are summarized as follows:(1)
Different from the traditional methods such as artificial potential functions based on a point-mass robot model, the proposed method considers the nonholonomic constraints of robots. The point-mass robot model cannot guarantee collision-free navigation, since a car-like robot will actually need to follow an arc to achieve the selected velocity, and this arc may lead to a collision with other robot. Consequently, the proposed approach solves this problem by using a dynamic model to predict the future system.(2)
Different from the network-based method such as Distributed Model Predictive Control method, the proposed method does not rely on communications among the robots or central coordination. Therefore, it is effective for multirobots to avoid obstacles real-time without a real-time communication environment.The remainder of this paper is organized as follows: in the next section the brief definition of the ORCA is shown. The problem statement is illustrated in Section3. Section 4 puts out the kinematic model of DDMR and the linear MPC algorithm. Simulation results in MATLAB and C++ are shown in Section 5. Section 6 presents some conclusions.


## 2. Optimal Reciprocal Collision Avoidance (ORCA)

ORCA is a rigorous approach for reciprocaln-body collision avoidance that allows each agent to compute independently the optimal moving velocities in each step [31]. In this section, we review the concept of velocity obstacles and discuss its application to DDMR with nonholonomic constraints.


### 2.1. Definition of ORCA

ORCA provides a sufficient condition for multiple robots to avoid collisions among one another and thus can guarantee collision-free navigation [31]. Let there be a set of n disc-shaped robots (this disc-shaped assumption can easily be extended to translate polygons) moving in the plane ℝ2. Each robot has a current position p, a current velocity v, and a radius r. It is assumed that these parameters can be observed by other robots. Furthermore, each robot has a maximum speed vmax and a preferred velocity vpref (for instance, a velocity directed towards the robot’s goal with a magnitude equal to the robot’s preferred speed). These parameters are considered as internal state of the robot, which cannot therefore be observed by other robots. The brief definitions are presented in the following.LetDp,r denote an open disc of radius r centered at p:(1)Dp,r=qq−p<r.The velocity obstacleVOABτ is the set of all relative velocities of A with respect to B that will result in a collision at some moment before time τ. The geometric interpretation of velocity obstacles is shown in Figure 1. And it can be formally represented as(2)VOABτ=v∃t∈0,τ::tv∈DpB−pA,rA+rB,where pA and pB are the positions of agent A and agent B, respectively. rA (rB) is the radius of the safe zone of agent A (agent B) and is chosen slightly larger than the radius of the agent. Note that for agent B, its velocity obstacle is VOBAτ=−VOABτ.Figure 1
The velocity obstacle for a short time horizonVOABτ.LetvAopt and vBopt be the optimization velocities of A and B, respectively. Nominally, the optimization velocities are equal to the current velocities, such that the robots have to deviate as little as possible from their current trajectories to avoid collisions. Let u be the vector from vAopt−vBopt to the closest point on the boundary of the velocity obstacle:(3)u=argminv∈∂VOABτv−vAopt−vBopt−vAopt−vBopt,and let n be the outward normal of the boundary of VOABτ at point vAopt−vBopt+u. Consequently, u is the smallest change required to the relative velocity of A and B to avert collision with τ time. To share the responsibility of avoiding collisions among the robots in a fair way, The increment 1/2u means that agent A takes half the responsibility of avoiding potential collision with agent B, while the remaining half will be taken by agent B. Hence, the set ORCAABτ of permitted velocities for A is the half-plane pointing in the direction of n starting at the point vAopt+1/2u. More formally,(4)ORCAABτ=vv−vAopt+12un≥0.This set is illustrated in Figure2. By this definition, the chosen new relative velocities will not enter VOABτ, and consequently the velocities of the agent can be smoothed.Figure 2
VOABτ and ORCAABτ for agent A when vA−vB lies in VOABτ.Each robot A performs a continuous cycle of sensing and acting with time stepΔt. In each iteration, the robot acquires the radius, the current position, and the current optimization velocity of the other robots. Based on this information, the robot infers the permitted half-plane of velocities ORCAABτ with respect to each other robot B. The set of ORCA velocities for agent A with respect to all robots is the intersection of the half-planes of ORCAABτ induced by each other robot B:(5)ORCAAτ=D0,vAmax∩∪B≠AORCAABτ.Note thatD0,vAmax include the maximum speed constraint on the robot A.At last, the new velocityvAnew is selected from ORCAAτ, which is the closest to its preferred velocity vApref amongst all velocities inside the region of permitted velocities:(6)vAnew=argminv∈ORCAAτv−vApref.The ORCA algorithm in [31] used a simple robot model, in which kinematics are ignored. Consequently, the robot reaches its new position:(7)pAnew=pA+vAnewΔt.In many of these applications, the robot of interest is a car-like robot and subject to nonholonomic kinematic constraints [33]. Hence, the constant velocity assumption rarely holds [41].


### 2.2. Nonholonomic Constraints

ORCA is a velocity obstacle based algorithm, which is a first-order method since it does not integrate velocities to yield positions as functions of time [26]. It means that the control input to the robot is velocity. By considering nonholonomic kinematic constraints, the navigation problem of a differential drive robot generally followed these two steps: first, the velocity vH is generated by the ORCA based on the assumption that the robot is holonomic; second, the robot tracks this velocity by using the controller with nonholonomic constraints [34]. However, these will make a tracking error δ between the holonomic trajectory and the real trajectory inevitable, as shown in Figure 3.Figure 3
The holonomic trajectory is tracked by the DDMR moving along the nonholonomic trajectory with a tracking error.The real nonholonomic trajectory is defined by two sections: an arc of circumferenceAB covered with constant linear speed vt and decreasing angular velocity ωt, followed by a straight line path BC with constant speed vt. In this case, a mobile robot will actually need to follow an arc to achieve the selected velocity vH, and the set of ORCAAτ provide no guarantee of collision-free.


## 2.1. Definition of ORCA

ORCA provides a sufficient condition for multiple robots to avoid collisions among one another and thus can guarantee collision-free navigation [31]. Let there be a set of n disc-shaped robots (this disc-shaped assumption can easily be extended to translate polygons) moving in the plane ℝ2. Each robot has a current position p, a current velocity v, and a radius r. It is assumed that these parameters can be observed by other robots. Furthermore, each robot has a maximum speed vmax and a preferred velocity vpref (for instance, a velocity directed towards the robot’s goal with a magnitude equal to the robot’s preferred speed). These parameters are considered as internal state of the robot, which cannot therefore be observed by other robots. The brief definitions are presented in the following.LetDp,r denote an open disc of radius r centered at p:(1)Dp,r=qq−p<r.The velocity obstacleVOABτ is the set of all relative velocities of A with respect to B that will result in a collision at some moment before time τ. The geometric interpretation of velocity obstacles is shown in Figure 1. And it can be formally represented as(2)VOABτ=v∃t∈0,τ::tv∈DpB−pA,rA+rB,where pA and pB are the positions of agent A and agent B, respectively. rA (rB) is the radius of the safe zone of agent A (agent B) and is chosen slightly larger than the radius of the agent. Note that for agent B, its velocity obstacle is VOBAτ=−VOABτ.Figure 1
The velocity obstacle for a short time horizonVOABτ.LetvAopt and vBopt be the optimization velocities of A and B, respectively. Nominally, the optimization velocities are equal to the current velocities, such that the robots have to deviate as little as possible from their current trajectories to avoid collisions. Let u be the vector from vAopt−vBopt to the closest point on the boundary of the velocity obstacle:(3)u=argminv∈∂VOABτv−vAopt−vBopt−vAopt−vBopt,and let n be the outward normal of the boundary of VOABτ at point vAopt−vBopt+u. Consequently, u is the smallest change required to the relative velocity of A and B to avert collision with τ time. To share the responsibility of avoiding collisions among the robots in a fair way, The increment 1/2u means that agent A takes half the responsibility of avoiding potential collision with agent B, while the remaining half will be taken by agent B. Hence, the set ORCAABτ of permitted velocities for A is the half-plane pointing in the direction of n starting at the point vAopt+1/2u. More formally,(4)ORCAABτ=vv−vAopt+12un≥0.This set is illustrated in Figure2. By this definition, the chosen new relative velocities will not enter VOABτ, and consequently the velocities of the agent can be smoothed.Figure 2
VOABτ and ORCAABτ for agent A when vA−vB lies in VOABτ.Each robot A performs a continuous cycle of sensing and acting with time stepΔt. In each iteration, the robot acquires the radius, the current position, and the current optimization velocity of the other robots. Based on this information, the robot infers the permitted half-plane of velocities ORCAABτ with respect to each other robot B. The set of ORCA velocities for agent A with respect to all robots is the intersection of the half-planes of ORCAABτ induced by each other robot B:(5)ORCAAτ=D0,vAmax∩∪B≠AORCAABτ.Note thatD0,vAmax include the maximum speed constraint on the robot A.At last, the new velocityvAnew is selected from ORCAAτ, which is the closest to its preferred velocity vApref amongst all velocities inside the region of permitted velocities:(6)vAnew=argminv∈ORCAAτv−vApref.The ORCA algorithm in [31] used a simple robot model, in which kinematics are ignored. Consequently, the robot reaches its new position:(7)pAnew=pA+vAnewΔt.In many of these applications, the robot of interest is a car-like robot and subject to nonholonomic kinematic constraints [33]. Hence, the constant velocity assumption rarely holds [41].


## 2.2. Nonholonomic Constraints

ORCA is a velocity obstacle based algorithm, which is a first-order method since it does not integrate velocities to yield positions as functions of time [26]. It means that the control input to the robot is velocity. By considering nonholonomic kinematic constraints, the navigation problem of a differential drive robot generally followed these two steps: first, the velocity vH is generated by the ORCA based on the assumption that the robot is holonomic; second, the robot tracks this velocity by using the controller with nonholonomic constraints [34]. However, these will make a tracking error δ between the holonomic trajectory and the real trajectory inevitable, as shown in Figure 3.Figure 3
The holonomic trajectory is tracked by the DDMR moving along the nonholonomic trajectory with a tracking error.The real nonholonomic trajectory is defined by two sections: an arc of circumferenceAB covered with constant linear speed vt and decreasing angular velocity ωt, followed by a straight line path BC with constant speed vt. In this case, a mobile robot will actually need to follow an arc to achieve the selected velocity vH, and the set of ORCAAτ provide no guarantee of collision-free.


## 3. Problem Statement

In this section, we define a new concept called the safe zone. This is an extended generalization of the ORCA concept and seeks to address the difficulty of using ORCA with nonholonomic constrained agents. The proof of collision-free navigation of safe zone is also derived. Note that, the actions of ORCA are computed for each robot independently, without communication among the robots or central coordination. Unlike original ORCA algorithm, the construction of safe zone needs to know the new velocities of other agents in neighbor region at each step. This is not a severe limitation since the multiagent system has been studied extensively.


### 3.1. Safe Zone

We consider multiple mobile robots sharing a common workspace where the robots have the same equations of motion and state spaces. Let the state space of robot beX⊂ℝn. Let ℝd be the robot’s physical workspace. Let O⊂ℝd be the geometry of robot relative to its position. For simplicity, we restrict our analysis to circular robots and obstacles, which is not a severe limitation since general polygons can be represented by a number of circles [26]. We assume the position p∈ℝd of robot can be derived from its state x∈X by a potentially projection function q∈X⟶ℝd:(8)pt=qxt.The control input spaceU⊂ℝm is assumed to be convex. Let the continuous-time equation of motion for robot be given by a potentially nonlinear function f∈X×U⟶ℝn:(9)x˙t=fxt,ut,where xt is the state and ut is the control input at time t.Given a current statex0 of robot and some constant control input u0 at time 0, the state of the robot at a given time t>0 is given by(10)xt=gt,x0,u0,where g∈ℝ×X×U⟶X is the solution to the differential equation of equation (9), which can be obtained numerically.Consider two circular robotsA and B in an environment. Let circle A represent the robot, and let circle B represent the obstacle. Given the position of agent A at time t=0 and a control uA, let us denote the position agent A will be later undertaking uA for time interval t by pAt. Similarly, pBt represents the state of agent B undertaking uB for time interval t. Note that the control u is the control inputs to the robot that results in a change in the robot’s configuration. Then, the control obstacle can be defined as(11)uA,uB∃t∈0,τ::pAt−pBt<rA+rB,where pAt=qAgAt,xA0,uA and pBt=qBgBt,xB0,uB by using equations (8) and (10). As shown in Figure 4, vA0∉ORCAABτ, and symmetrically, vB0∉ORCABAτ. Robots A and B chose constant velocities vAH and vBH outside the obstacle velocity set and generate trajectory pAHt and pAHt, respectively. Then A and B take controls uA and uB (for instance, u=vH,kϕ−ϕd), which generate trajectory pAt and pBt (red dashed line), respectively. Note that this will lead to a collision, although the minimum distance between two holonomic trajectory (blue solid line) is greater than the radii sum of two robots (dmin>rA+rB).Figure 4
RobotsA and B collide in pAtcol and pBtcoal caused by the deviations for the nonholonomic constraints.In fact, the ORCA algorithm can be transformed using a uniform sampling scheme to deal with the nonholonomic kinematic constraints. As illustrated in Figure5, let pAH0,pAH1,…,pAHN and pBH0,pBH1,…,pBHN be the uniform sampling waypoints of holonomic trajectory pAHt and pBHt, respectively. The distance between pAk and pBk is lABk for k∈0,1,2,…,N. Note that pN is the terminal point of one step of ORCA; that is, pN=pτ. Hence, the collision avoidance set of ORCA algorithm can be reformulated as(12)CAABτ=uAs,uBs∀k∈0,1,…,N::lABk>rA+rB,where(13)lABk=pAk−pBk,uAs=uA0,uA1,…,uAN−1,uBs=uB0,uB1,…,uBN−1.Figure 5
The discrete trajectories of robotsA and B generated by ORCA.Letrsik be the safe zone radius of robot i at time k, and it can be defined as(14)rsik=12minj≠ilijk−ri−rj,where ri and rj are the radii of robots i and j. The constructive process of safe zone can geometrically be illustrated in Figure 6. Note that rsijk=lijk−ri−rj and b is side length of the maximum inscribed square. This square is the approximate safe zone for linear constraints (see next section). If robot i applies the control input uik−1 in time k−1 that “walk” to the safe zone rsik in time k will lead to collision avoidance.Figure 6
The safe zone for roboti with two neighbor robots j and k.


### 3.2. Proof of Collision-Free Navigation

Theorem 1.
If the safe zone of roboti at control step k is equal to the relative safe zone between robots i and j, the safe zone of robot j must have been less than or equal to the safe zone of robot i in this moment. Then, the mathematical relationship can be described by rsik=rsijk⇒rsjk≤rsik.Proof.
Ifrsjk>rsik⇒∃m::rsjmk=1/2minm≠jljmk−rj−rm>rsik=rsjik.
According to (14), rsjik≥1/2minm≠jljmk−rj−rm.
By using reduction to absurdity, Theorem1 is proved.
For roboti, there are n robots in its neighbor region. If the radius of safe zone at control step k is rsik=rsijk, it means that rsijk is the minimum dimension for robot i, but it does not mean that the rsijk is the minimum dimension for robot j. Theorem 1 gives the proof of this fact.Theorem 2.
In a multirobots environment, the collision-free navigation can be guaranteed when the safe zone constraints are maintained at every moment.Proof.
Theorem1⇒∀i,j∈Rs (robots set)::ri+rj+rsik+rsjk≤lijk.
Consider any pair of robots in one neighbor region, the sum of the radii of these two robots and their safe zones is less than the distance between these two robots. This theorem proves that the collision-free navigation for Discrete-ORCA can be guaranteed.


## 3.1. Safe Zone

We consider multiple mobile robots sharing a common workspace where the robots have the same equations of motion and state spaces. Let the state space of robot beX⊂ℝn. Let ℝd be the robot’s physical workspace. Let O⊂ℝd be the geometry of robot relative to its position. For simplicity, we restrict our analysis to circular robots and obstacles, which is not a severe limitation since general polygons can be represented by a number of circles [26]. We assume the position p∈ℝd of robot can be derived from its state x∈X by a potentially projection function q∈X⟶ℝd:(8)pt=qxt.The control input spaceU⊂ℝm is assumed to be convex. Let the continuous-time equation of motion for robot be given by a potentially nonlinear function f∈X×U⟶ℝn:(9)x˙t=fxt,ut,where xt is the state and ut is the control input at time t.Given a current statex0 of robot and some constant control input u0 at time 0, the state of the robot at a given time t>0 is given by(10)xt=gt,x0,u0,where g∈ℝ×X×U⟶X is the solution to the differential equation of equation (9), which can be obtained numerically.Consider two circular robotsA and B in an environment. Let circle A represent the robot, and let circle B represent the obstacle. Given the position of agent A at time t=0 and a control uA, let us denote the position agent A will be later undertaking uA for time interval t by pAt. Similarly, pBt represents the state of agent B undertaking uB for time interval t. Note that the control u is the control inputs to the robot that results in a change in the robot’s configuration. Then, the control obstacle can be defined as(11)uA,uB∃t∈0,τ::pAt−pBt<rA+rB,where pAt=qAgAt,xA0,uA and pBt=qBgBt,xB0,uB by using equations (8) and (10). As shown in Figure 4, vA0∉ORCAABτ, and symmetrically, vB0∉ORCABAτ. Robots A and B chose constant velocities vAH and vBH outside the obstacle velocity set and generate trajectory pAHt and pAHt, respectively. Then A and B take controls uA and uB (for instance, u=vH,kϕ−ϕd), which generate trajectory pAt and pBt (red dashed line), respectively. Note that this will lead to a collision, although the minimum distance between two holonomic trajectory (blue solid line) is greater than the radii sum of two robots (dmin>rA+rB).Figure 4
RobotsA and B collide in pAtcol and pBtcoal caused by the deviations for the nonholonomic constraints.In fact, the ORCA algorithm can be transformed using a uniform sampling scheme to deal with the nonholonomic kinematic constraints. As illustrated in Figure5, let pAH0,pAH1,…,pAHN and pBH0,pBH1,…,pBHN be the uniform sampling waypoints of holonomic trajectory pAHt and pBHt, respectively. The distance between pAk and pBk is lABk for k∈0,1,2,…,N. Note that pN is the terminal point of one step of ORCA; that is, pN=pτ. Hence, the collision avoidance set of ORCA algorithm can be reformulated as(12)CAABτ=uAs,uBs∀k∈0,1,…,N::lABk>rA+rB,where(13)lABk=pAk−pBk,uAs=uA0,uA1,…,uAN−1,uBs=uB0,uB1,…,uBN−1.Figure 5
The discrete trajectories of robotsA and B generated by ORCA.Letrsik be the safe zone radius of robot i at time k, and it can be defined as(14)rsik=12minj≠ilijk−ri−rj,where ri and rj are the radii of robots i and j. The constructive process of safe zone can geometrically be illustrated in Figure 6. Note that rsijk=lijk−ri−rj and b is side length of the maximum inscribed square. This square is the approximate safe zone for linear constraints (see next section). If robot i applies the control input uik−1 in time k−1 that “walk” to the safe zone rsik in time k will lead to collision avoidance.Figure 6
The safe zone for roboti with two neighbor robots j and k.


## 3.2. Proof of Collision-Free Navigation

Theorem 1.
If the safe zone of roboti at control step k is equal to the relative safe zone between robots i and j, the safe zone of robot j must have been less than or equal to the safe zone of robot i in this moment. Then, the mathematical relationship can be described by rsik=rsijk⇒rsjk≤rsik.Proof.
Ifrsjk>rsik⇒∃m::rsjmk=1/2minm≠jljmk−rj−rm>rsik=rsjik.
According to (14), rsjik≥1/2minm≠jljmk−rj−rm.
By using reduction to absurdity, Theorem1 is proved.
For roboti, there are n robots in its neighbor region. If the radius of safe zone at control step k is rsik=rsijk, it means that rsijk is the minimum dimension for robot i, but it does not mean that the rsijk is the minimum dimension for robot j. Theorem 1 gives the proof of this fact.Theorem 2.
In a multirobots environment, the collision-free navigation can be guaranteed when the safe zone constraints are maintained at every moment.Proof.
Theorem1⇒∀i,j∈Rs (robots set)::ri+rj+rsik+rsjk≤lijk.
Consider any pair of robots in one neighbor region, the sum of the radii of these two robots and their safe zones is less than the distance between these two robots. This theorem proves that the collision-free navigation for Discrete-ORCA can be guaranteed.


## 4. Discrete-ORCA-MPC Combined Collision Avoidance

In this section, the brief overview of the kinematic model of a DDMR is given. Then, a successive linearization approach is introduced, which yields a linear, time-varying description of the DDMR system. The optimal control input is solved through linear MPC. Meanwhile, the safe zone and control constraints are satisfied by transforming the optimization problem in a QP problem.


### 4.1. Kinematics and Dynamics of DDMR

As illustrated in Figure7, the configuration of a DDMR is given by the position of its center point p=x,y and its orientation ϕ. The configuration and high-level control input transition equations [42] are given as(15)x˙=vcosϕ,y˙=vsinϕ,ϕ˙=ω,where v=vr+vl/2 is the linear velocity and ω=vr−vl/b is the angular velocity. b is the distance between the rear wheels of the robot, and vr and vl are the signed speeds of the left and right wheels, respectively. Generally, vr and vl are bounded such that vr,vl∈−vmax,vmax.Figure 7
Structure of the differential drive mobile robot.In general, the kinematic model can be written in the form of (9):(16)x˙=fx,u,where x=x,y,ϕT describes the configuration of point P (shown in Figure 7) and u=v,ωT is the control input.Similarly, letxr=xr,yr,ϕrT and ur=vr,ωrT be the configuration and control input of reference system. Then, it can be described as(17)x˙r=fxr,ur.By expanding the right side of (16) in Taylor series around the reference point xr,ur, a linear model is obtained by discarding the high-order terms:(18)x˙=fxr,ur+fx′x−xr+fu′u−ur,where fx′ and fu′ are the Jacobians of f with respect to x and u, respectively, evaluated around the reference point xr,ur.Then, the subtraction of (17) from (18) results in(19)x˜.=fx′x˜+fu′u˜,where x˜=x−xr represents the error with respect to the reference robot configuration and u˜ is its error with respect to the reference input. Finally, the discrete-time system can be obtained:(20)x˜k+1=Akx˜k+Bku˜k,with(21)Ak=10−vrksinϕrkT01vrkcosϕrkT001,Bk=cosϕrkT0sinϕrkT00T,where T is the sampling period and k is the sampling time.In this paper, letvir=vinew and ϕir=arctanvinew be the reference velocity and direction of robot i. Then, the reference trajectory of robot i is obtained as (6):(22)Xir=xk+vircosϕirTyk+virsinϕirTϕirxk+2vircosϕirT2yk+virsinϕirTϕir⋮⋮⋮xk+NτvircosϕirTNτyk+virsinϕirTϕir,where Nτ=τ/T. It means that robot i generates Nτ waypoints in the direction of ϕir uniformly. In [40], we notice that the controllability matrix has full rank if either vr or ωr is nonzero. This implies the tracking of a reference trajectory being possible with linear MPC.In the field of mobile robotics, predictive approaches to trajectory tracking seem to be very promising because the reference trajectory is known beforehand [40]. The essence of MPC approach is to optimize predictions of process behavior over a sequence of future control inputs. At each sampling time, the model predictive controller generates an optimal control sequence by solving an optimization problem [39]. In this section, the trajectory-tracking control of a differentially driven mobile robot based on MPC is presented. The main idea is to minimize the future trajectory-following errors and control errors between the robot and reference waypoints generated by Discrete-ORCA.


### 4.2. MPC Algorithm

The ideal of MPC concept it to find the values of control variable that minimize the receding horizon quadratic cost function based on the predicted robot tracking error model (20). In general, MPC can be proven to be stable using a terminal cost and terminal constraints in its optimization problem [43]. The cost function at time k can be stated as(23)Φk=∑i=1hx˜Tk+ikQx˜k+ik+u˜Tk+i−1kRu˜k+i−1k,where h is the prediction horizon and Q and R are the weighting matrices (diagonal matrix with Q≥0 and R≥0). Note that, the angle error ϕ−ϕr must be wrapped in radians to −π,π. The notation amn indicates the value of a at the predicted time m based on the current time n. In order to reformulate the optimization problem in a usual quadratic programming form, the x^k+1 and u^k are defined as(24)x^k+1=x˜k+1kx˜k+2k⋮x˜k+hk,u˜k=u˜kku˜k+1k⋮u˜k+h−1k.Then, the original optimal problem (23) can be rewritten as(25)Φk=x^Tk+1Q^x^k+1+u^TkR^u^k,where(26)Q^=Q0⋯00Q⋯0⋮⋮⋱⋮00⋯Q,R^k=R0⋯00R⋯0⋮⋮⋱⋮00⋯R.Therefore, (20) can be recasted as(27)x^k+1=A^kx^k+B^ku^k,with(28)A^k=AkkAkkAk+1k⋮αk,0,B^=Bkk0⋯0Ak+1kBkkBk+1k⋯0⋮⋮⋱⋮αk,1Bkkαk,2Bk+1k⋯Bk+h−1k,where(29)αk,j=∏i=jh−1Ak+ik.Rearrange (25) and (27), the cost function (23) can be rewritten as(30)Φk=12u^TkHku^k+fTku^k+dk,where(31)Hk=2B^TkQ^B^k+R^,fk=2B^TkQ^A^kx^kk,dk=x˜TkkA^TkQ^A^kx^kk.TheH is a positive definite Hessian matrix. It is the quadratic part of the cost function while the vector f describes the linear part. Note that fk and dk contain x˜kk which is constant vector at time k. Hence, the optimization problem can be stated as to find u^∗k at each time step k minimizing the cost function (30):(32)u^∗k=argminu^Φk.The solution of (32) is a sequence of optimal control; that is u^∗k=u˜∗kk,u˜∗k+1k,…,u˜∗k+h−1k. The MPC scheme is implicitly given by the first control action of the sequence of optimal control, u˜∗kk.


### 4.3. Collision-Free Constraints

In real world applications, the robots are inevitably subject to physical limitations. Without consideration of these limitations while computing the optimal control inputs, the robot will suffer from unpredictable mismatch. Hence, the constraints of upper and lower bounds should add to the optimization problem (32). And the control inputs constraint can be written as(33)umink≤uk≤umaxk.Note that the free variable in the optimization (32) is u^k, so the constraint (33) must be rewritten as(34)u^mink≤u^k≤u^maxk,where(35)u^mink=uminkk−urkkumink+1k−urk+1k⋮umink+h−1k−urk+h−1k,u^maxk=umaxkk−urkkumaxk+1k−urk+1k⋮umaxk+h−1k−urk+h−1k.Furthermore, the collision avoidance requirement will be constructed as the constraint with respect to control inputs. Let us rewrite the safe zone (14) as(36)rsik+1=12minj≠ilijk+1−ri−rj.It means that the control inputs to roboti in time k must reach the safe zone rsik+1. To make a linear constraint, the approximate safe zone is constructed as a square which is the maximum inscribed square as illustrated in Figure 6. Hence, the side length of this square b can be denoted as(37)bk+1=12rsik+1.Then, the collision avoidance manoeuvre can be represented as(38)−12bk+1≤xk+1−xrk+1≤12bk+1,−12bk+1≤yk+1+yrk+1≤12bk+1,where xk+1 and yk+1 can be obtained from the discrete time form of the kinematic equation (15):(39)xk+1=xk+Tcosϕkvk,yk+1=yk+Tsinϕkvk.Note thatϕk+1 is not the consideration of the navigation. Remember that u=v,ωT, so we have v˜k=vk−vrk. Then rearranging (38) to (39) can obtain the collision avoidance constraint in each step time k:(40)blk≤v˜k≤buk,where(41)blk=min−1/2bk+1+xrk+1−xkTcosϕk,−1/2bk+1+yrk+1−ykTsinϕk,buk=min1/2bk+1+xrk+1−xkTcosϕk,1/2bk+1+yrk+1−ykTsinϕk.Note thatblk and buk are constants in each time step. Recalling (34), we can obtain(42)b˜lk≤v˜k≤b˜uk,with(43)b˜lk=minblk,vmink−vrk,b˜uk=minbuk,vmaxk−vrk.Finally, the constraint of free variableu^k for optimization problem (32) can be constructed by refreshing u^mink and u^maxk in (34) as(44)u^mink=uminkkumink+1k−urk+1k⋮umink+h−1k−urk+h−1k,u^maxk=umaxkkumaxk+1k−urk+1k⋮umaxk+h−1k−urk+h−1k,where(45)u˜minkk=b˜lkωmin−ωr,u˜maxkk=b˜ukωmax−ωr.


### 4.4. Algorithm

The overall approach is summarized in Algorithm1. In each ORCA step (τ), it has three things for every robot to do. First, robot i generates the discrete reference control inputs and trajectory. The number of waypoints is τ/T, and the corresponding reference control inputs are vinew,0. Second, robot i computes the safe zone rsi with respect to the neighbor region. Finally, robot i repeatedly computes the optimal control inputs at each sample time T to close the last waypoint. Then, it will go into the next ORCA step. This process will continue until every robot reaches its goal.Algorithm 1: Discrete-ORCA-MPC method for multi-nonholonomic robots collision avoidance.
Input:Rs: list of robots; O: list of obstacles; τ: time horizon of ORCA;h: predict horizon of MPC; T: sampling period.Output: optimal control u∗k(1)
repeat ORCA(2)
for each Ri∈Rsdo(3)
computevinew for Ri using ORCA (6)(4)
compute reference trajectoryXir using (22);(5)
compute safe zonersi for robot i at each way point using (14)(6)
end for(7)
fork=1;k<Nτ;k++do(8)
for each Ri∈Rsdo(9)
computeu˜i∗k for robot i using MPC (27)(10)
compute control inputuik=u˜i∗k+uirk;(11)
compute the configuration of next stepxik+1 using (15)(12)
end for(13)
end for(14)
until (all robots reach their goal zone)


## 4.1. Kinematics and Dynamics of DDMR

As illustrated in Figure7, the configuration of a DDMR is given by the position of its center point p=x,y and its orientation ϕ. The configuration and high-level control input transition equations [42] are given as(15)x˙=vcosϕ,y˙=vsinϕ,ϕ˙=ω,where v=vr+vl/2 is the linear velocity and ω=vr−vl/b is the angular velocity. b is the distance between the rear wheels of the robot, and vr and vl are the signed speeds of the left and right wheels, respectively. Generally, vr and vl are bounded such that vr,vl∈−vmax,vmax.Figure 7
Structure of the differential drive mobile robot.In general, the kinematic model can be written in the form of (9):(16)x˙=fx,u,where x=x,y,ϕT describes the configuration of point P (shown in Figure 7) and u=v,ωT is the control input.Similarly, letxr=xr,yr,ϕrT and ur=vr,ωrT be the configuration and control input of reference system. Then, it can be described as(17)x˙r=fxr,ur.By expanding the right side of (16) in Taylor series around the reference point xr,ur, a linear model is obtained by discarding the high-order terms:(18)x˙=fxr,ur+fx′x−xr+fu′u−ur,where fx′ and fu′ are the Jacobians of f with respect to x and u, respectively, evaluated around the reference point xr,ur.Then, the subtraction of (17) from (18) results in(19)x˜.=fx′x˜+fu′u˜,where x˜=x−xr represents the error with respect to the reference robot configuration and u˜ is its error with respect to the reference input. Finally, the discrete-time system can be obtained:(20)x˜k+1=Akx˜k+Bku˜k,with(21)Ak=10−vrksinϕrkT01vrkcosϕrkT001,Bk=cosϕrkT0sinϕrkT00T,where T is the sampling period and k is the sampling time.In this paper, letvir=vinew and ϕir=arctanvinew be the reference velocity and direction of robot i. Then, the reference trajectory of robot i is obtained as (6):(22)Xir=xk+vircosϕirTyk+virsinϕirTϕirxk+2vircosϕirT2yk+virsinϕirTϕir⋮⋮⋮xk+NτvircosϕirTNτyk+virsinϕirTϕir,where Nτ=τ/T. It means that robot i generates Nτ waypoints in the direction of ϕir uniformly. In [40], we notice that the controllability matrix has full rank if either vr or ωr is nonzero. This implies the tracking of a reference trajectory being possible with linear MPC.In the field of mobile robotics, predictive approaches to trajectory tracking seem to be very promising because the reference trajectory is known beforehand [40]. The essence of MPC approach is to optimize predictions of process behavior over a sequence of future control inputs. At each sampling time, the model predictive controller generates an optimal control sequence by solving an optimization problem [39]. In this section, the trajectory-tracking control of a differentially driven mobile robot based on MPC is presented. The main idea is to minimize the future trajectory-following errors and control errors between the robot and reference waypoints generated by Discrete-ORCA.


## 4.2. MPC Algorithm

The ideal of MPC concept it to find the values of control variable that minimize the receding horizon quadratic cost function based on the predicted robot tracking error model (20). In general, MPC can be proven to be stable using a terminal cost and terminal constraints in its optimization problem [43]. The cost function at time k can be stated as(23)Φk=∑i=1hx˜Tk+ikQx˜k+ik+u˜Tk+i−1kRu˜k+i−1k,where h is the prediction horizon and Q and R are the weighting matrices (diagonal matrix with Q≥0 and R≥0). Note that, the angle error ϕ−ϕr must be wrapped in radians to −π,π. The notation amn indicates the value of a at the predicted time m based on the current time n. In order to reformulate the optimization problem in a usual quadratic programming form, the x^k+1 and u^k are defined as(24)x^k+1=x˜k+1kx˜k+2k⋮x˜k+hk,u˜k=u˜kku˜k+1k⋮u˜k+h−1k.Then, the original optimal problem (23) can be rewritten as(25)Φk=x^Tk+1Q^x^k+1+u^TkR^u^k,where(26)Q^=Q0⋯00Q⋯0⋮⋮⋱⋮00⋯Q,R^k=R0⋯00R⋯0⋮⋮⋱⋮00⋯R.Therefore, (20) can be recasted as(27)x^k+1=A^kx^k+B^ku^k,with(28)A^k=AkkAkkAk+1k⋮αk,0,B^=Bkk0⋯0Ak+1kBkkBk+1k⋯0⋮⋮⋱⋮αk,1Bkkαk,2Bk+1k⋯Bk+h−1k,where(29)αk,j=∏i=jh−1Ak+ik.Rearrange (25) and (27), the cost function (23) can be rewritten as(30)Φk=12u^TkHku^k+fTku^k+dk,where(31)Hk=2B^TkQ^B^k+R^,fk=2B^TkQ^A^kx^kk,dk=x˜TkkA^TkQ^A^kx^kk.TheH is a positive definite Hessian matrix. It is the quadratic part of the cost function while the vector f describes the linear part. Note that fk and dk contain x˜kk which is constant vector at time k. Hence, the optimization problem can be stated as to find u^∗k at each time step k minimizing the cost function (30):(32)u^∗k=argminu^Φk.The solution of (32) is a sequence of optimal control; that is u^∗k=u˜∗kk,u˜∗k+1k,…,u˜∗k+h−1k. The MPC scheme is implicitly given by the first control action of the sequence of optimal control, u˜∗kk.


## 4.3. Collision-Free Constraints

In real world applications, the robots are inevitably subject to physical limitations. Without consideration of these limitations while computing the optimal control inputs, the robot will suffer from unpredictable mismatch. Hence, the constraints of upper and lower bounds should add to the optimization problem (32). And the control inputs constraint can be written as(33)umink≤uk≤umaxk.Note that the free variable in the optimization (32) is u^k, so the constraint (33) must be rewritten as(34)u^mink≤u^k≤u^maxk,where(35)u^mink=uminkk−urkkumink+1k−urk+1k⋮umink+h−1k−urk+h−1k,u^maxk=umaxkk−urkkumaxk+1k−urk+1k⋮umaxk+h−1k−urk+h−1k.Furthermore, the collision avoidance requirement will be constructed as the constraint with respect to control inputs. Let us rewrite the safe zone (14) as(36)rsik+1=12minj≠ilijk+1−ri−rj.It means that the control inputs to roboti in time k must reach the safe zone rsik+1. To make a linear constraint, the approximate safe zone is constructed as a square which is the maximum inscribed square as illustrated in Figure 6. Hence, the side length of this square b can be denoted as(37)bk+1=12rsik+1.Then, the collision avoidance manoeuvre can be represented as(38)−12bk+1≤xk+1−xrk+1≤12bk+1,−12bk+1≤yk+1+yrk+1≤12bk+1,where xk+1 and yk+1 can be obtained from the discrete time form of the kinematic equation (15):(39)xk+1=xk+Tcosϕkvk,yk+1=yk+Tsinϕkvk.Note thatϕk+1 is not the consideration of the navigation. Remember that u=v,ωT, so we have v˜k=vk−vrk. Then rearranging (38) to (39) can obtain the collision avoidance constraint in each step time k:(40)blk≤v˜k≤buk,where(41)blk=min−1/2bk+1+xrk+1−xkTcosϕk,−1/2bk+1+yrk+1−ykTsinϕk,buk=min1/2bk+1+xrk+1−xkTcosϕk,1/2bk+1+yrk+1−ykTsinϕk.Note thatblk and buk are constants in each time step. Recalling (34), we can obtain(42)b˜lk≤v˜k≤b˜uk,with(43)b˜lk=minblk,vmink−vrk,b˜uk=minbuk,vmaxk−vrk.Finally, the constraint of free variableu^k for optimization problem (32) can be constructed by refreshing u^mink and u^maxk in (34) as(44)u^mink=uminkkumink+1k−urk+1k⋮umink+h−1k−urk+h−1k,u^maxk=umaxkkumaxk+1k−urk+1k⋮umaxk+h−1k−urk+h−1k,where(45)u˜minkk=b˜lkωmin−ωr,u˜maxkk=b˜ukωmax−ωr.


## 4.4. Algorithm

The overall approach is summarized in Algorithm1. In each ORCA step (τ), it has three things for every robot to do. First, robot i generates the discrete reference control inputs and trajectory. The number of waypoints is τ/T, and the corresponding reference control inputs are vinew,0. Second, robot i computes the safe zone rsi with respect to the neighbor region. Finally, robot i repeatedly computes the optimal control inputs at each sample time T to close the last waypoint. Then, it will go into the next ORCA step. This process will continue until every robot reaches its goal.Algorithm 1: Discrete-ORCA-MPC method for multi-nonholonomic robots collision avoidance.
Input:Rs: list of robots; O: list of obstacles; τ: time horizon of ORCA;h: predict horizon of MPC; T: sampling period.Output: optimal control u∗k(1)
repeat ORCA(2)
for each Ri∈Rsdo(3)
computevinew for Ri using ORCA (6)(4)
compute reference trajectoryXir using (22);(5)
compute safe zonersi for robot i at each way point using (14)(6)
end for(7)
fork=1;k<Nτ;k++do(8)
for each Ri∈Rsdo(9)
computeu˜i∗k for robot i using MPC (27)(10)
compute control inputuik=u˜i∗k+uirk;(11)
compute the configuration of next stepxik+1 using (15)(12)
end for(13)
end for(14)
until (all robots reach their goal zone)


## 5. Experiments and Analysis

In this section, we conduct computer simulations to verify the proposed Discrete-ORCA-MPC algorithm. The following simulations are specifically designed to deal with multi-nonholonomic-agent navigation. These were carried out using MATLAB R2018a and Visual Studio 2017 on a laptop with Intel Quad Core i5 CPU and 16 GB RAM.To simplify the configuration, it is assumed that all the DDMR have the same parameters. The radius of robotsr=2 and the control input constraints are set as umin=0,−1 and umax=2,1, respectively. The preferred velocity vpref was chosen as the velocity directed towards the robot’s goal with a magnitude equal to 1. Meanwhile, the sampling time period of MPC is T=30ms and the time window of the ORCA algorithm is τ=300ms. In the MPC controller formulation, the prediction horizon h=6 and optimization problem is solved using interior point method. Furthermore, the free variable constraints are set as u˜min=−0.5,−0.5 and u˜max=0.5,0.5, respectively. To keep the robot close to the ORCA trajectory, the weighting matrix Q should be larger than R. In this paper, these are set as Q=3I3 and R=0.1I3.


### 5.1. Compare MPC and PID for 4 DDMRs

As mentioned in Section2.2, the original ORCA algorithm is a first-order method. In other words, the control input to the robot is reference velocity but not the reference trajectory. In this simulation, a first-order method based on ORCA is formulated to compare with the proposed Discrete-ORCA-MPC approach. This method uses a proportional controller to control the angular velocity, and the linear velocity is equal to the new velocity generated by ORCA. It can be formulated as(46)vikj=viknew,ωikj=minKgainϕiknew−ϕikj,ωmax,ϕiknew≥ϕikj,maxKgainϕiknew−ϕikj,ωmin,ϕiknew<ϕikj,where viknew and ϕiknew represent the 2-norm value and direction of the new velocity (equation (6)) of robot i generated by ORCA in step k; that is, viknew=viknew and ϕiknew=atan2viknew. vikj and ωikj are the control input to robot i in control step j of ORCA step k. The proportional controller gain is set as 3. As a consequence, the constraints of angular velocity are also added to the controller. Note that we have no intention of constructing a perfect controller to following the reference velocity vnew, since the tracking error to the holonomic trajectory always exists even for a “perfect” controller (see Figure 3). Furthermore, these tracking errors may lead to a collision. By contrast, the proposed MPC controller also has tracking errors; however, these tracking errors are formulated as constraints for the decision variables that the collision will not happen (see Theorem 2 and equation (45)).In this simulation, 4 DDMRs are initially located on the four corners of a square, that is, [−25, −25] (Robot 1), [25, 25] (Robot 2), [−25, 25] (Robot 3), and [25, −25] (Robot 4), as illustrated in Figure8. Robots 1, 2 and Robots 3, 4 at antipodal positions are expected to exchange positions with each other. Furthermore, the initial orientation of robots 1 to 4 is 1/4π, −3/4π, −1/4π, and 3/4π, respectively. Note that every 18 ORCA steps plot a circle as shown in Figure 8.Figure 8
Motion trajectory for (a) Discrete-ORCA-MPC and (b) PID.
(a)(b)Figure8 shows the motion trajectory of 4 DDMRs on the workspace without static obstacles based on the proposed Discrete-ORCA-MPC approach and a proportional controller. Comparing the trajectories of local enlarged drawing of Figures 8(a) and 8(b), at 37.5°s, Robot 2 −1.718,1.56 collides with Robot 3 −1.421,−2.082 in Figure 8(b) (PID) while the collision did not happen in Figure 8(b) (Discrete-ORCA-MPC). Figure 9 depicts the relative distance between robots over the time where dij represents the relative distance between robots i and j. These results demonstrate the proposed method can generate a collision-free navigation for nonholonomic mobile robot effectively.Figure 9
The relative distance between robots. (a) Discrete-ORCA-MPC. (b) PID.
(a)(b)Figure10 shows the control inputs (v and ω) to each robot. In most of the time of the navigation, the control inputs change a little with respect to time; however, these has been a great change near 28°s and 37°s. It can be explained that the robots took a sharp turn in 28°s and 37°s as shown in Figure 8. In particular, the rapid growth of angular velocity ω is inevitably accompanied by a decline in the linear velocity ω. In other words, the robot should slow down for taking a sharp turn. It also can be seen that the control inputs are inside the limits imposed by the constraints. In Figure 11, it demonstrates that the state errors are compared with the trajectory generated by ORCA. It can be seen that maximum absolute errors of the position xe,ye and orientation ϕe are 0.07 and 0.4, respectively. Obviously, position errors are much less than the orientation error ϕe. This is reasonable because the smaller xe,ye shows a better performance of MPC controller. This simulation demonstrates that the proposed Discrete-ORCA-MPC algorithm has the ability to cope with the collision avoidance problem of multi-nonholonomic-agent.Figure 10
Control inputs of the 4 robots with respect to time. (a) Robot 1 control inputs. (b) Robot 2 control inputs. (c) Robot 3 control inputs. (d) Robot 1 control inputs.
(a)(b)(c)(d)Figure 11
Robots state errors compared to the holonomic trajectory. (a) Robot 1 state inputs. (b) Robot 2 state inputs. (c) Robot 3 state inputs. (d) Robot 4 state inputs.
(a)(b)(c)(d)


### 5.2. 4 DDMRs with Static Obstacle

It is similar to the first simulation; 4 DDMRs are initially located on the four corners of a square also, that is, [−45,−45] (Robot 1), [45, 45] (Robot 2), [−45, 45] (Robot 3) and [45, −45] (Robot 4), as illustrated in Figure12. Unlike the first one, this simulation considers static obstacle avoidance. These four static obstacles are located at [−22.5, −22.5], [22.5, 22.5], [−22.5, 22.5], and [22.5, −22.5] with sides that are 15 dm in length. Figure 13 depicts the relative distance between robots over the time. Furthermore, the initial orientations of Robots 1 to 4 are 1/4π+0.1, −3/4π+0.1, −1/4π+0.1, and 3/4π+0.1, respectively. This is why the trajectories of these four robots are asymmetrical. More sharp turn can be seen compared to the first simulation which leads to more sudden change in control inputs as shown in Figure 14. Meanwhile, the constraints of control inputs umin,umax and the errors with respect to reference control inputs u^min,u^max are satisfied. In Figure 15, it can also be seen that the position errors xe,ye are much less than the orientation error ϕe. The maximum value of position errors is less than 0.1 which means that the performance of the obtained trajectory tracking is good enough.Figure 12
Motion trajectory of 4 DDMRs with static obstacles.Figure 13
The relative distance between robots.
(a)(b)(c)Figure 14
Control inputs of the 4 robots with respect to time (consider static obstacles). (a) Robot 1 control inputs. (b) Robot 2 control inputs. (c) Robot 3 control inputs. (d) Robot 4 control inputs.
(a)(b)(c)(d)Figure 15
Robots state errors compared to the holonomic trajectory generated by ORCA (consider static obstacles). (a) Robot 1 state errors. (b) Robot 2 state errors. (c) Robot 3 state errors. (d) Robot 1 state errors.
(a)(b)(c)(d)This simulation demonstrates that the proposed Discrete-ORCA-MPC algorithm has the ability to cope with the navigation problem of multi-nonholonomic-agent with static obstacles.


### 5.3. Performance Results

In order to test the performance of Discrete-ORCA-MPC, we carried on large-scale simulations to compare the mean computation times of our method with Priority-Based Noncooperative Distributed Model Predictive Control (PB-NC-DMPC) [44]. PB-NC-DMPC is an effective NC-DMPC strategy that deals with the problem of loss of the prediction consistency property while reducing the computation time as well. In the problem of collision avoidance of networked vehicles, PB-NC-DMPC showed better performance compared to NC-DMPC and Centralized MPC. The test scenario contains many robots whose goal is to move across a circle to the antipodal position. This scenario is the same as the published literature, Jur et al. [31]. In PB-NC-DMPC, we implemented the collision avoidance controller based on the same linear predictive model as (20). The controller parameters of PB-NC-DMPC are the same as Discrete-ORCA-MPC and remain unchanged for all robots in all simulations.Figure16 depicts the mean computation times of Discrete-ORCA-MPC and PB-NC-DMPC plotted over increasing number of DDMRs. It shows that running Discrete-ORCA-MPC is computationally cheaper than PB-NC-DMPC.Figure 16
Comparison of computation times.


## 5.1. Compare MPC and PID for 4 DDMRs

As mentioned in Section2.2, the original ORCA algorithm is a first-order method. In other words, the control input to the robot is reference velocity but not the reference trajectory. In this simulation, a first-order method based on ORCA is formulated to compare with the proposed Discrete-ORCA-MPC approach. This method uses a proportional controller to control the angular velocity, and the linear velocity is equal to the new velocity generated by ORCA. It can be formulated as(46)vikj=viknew,ωikj=minKgainϕiknew−ϕikj,ωmax,ϕiknew≥ϕikj,maxKgainϕiknew−ϕikj,ωmin,ϕiknew<ϕikj,where viknew and ϕiknew represent the 2-norm value and direction of the new velocity (equation (6)) of robot i generated by ORCA in step k; that is, viknew=viknew and ϕiknew=atan2viknew. vikj and ωikj are the control input to robot i in control step j of ORCA step k. The proportional controller gain is set as 3. As a consequence, the constraints of angular velocity are also added to the controller. Note that we have no intention of constructing a perfect controller to following the reference velocity vnew, since the tracking error to the holonomic trajectory always exists even for a “perfect” controller (see Figure 3). Furthermore, these tracking errors may lead to a collision. By contrast, the proposed MPC controller also has tracking errors; however, these tracking errors are formulated as constraints for the decision variables that the collision will not happen (see Theorem 2 and equation (45)).In this simulation, 4 DDMRs are initially located on the four corners of a square, that is, [−25, −25] (Robot 1), [25, 25] (Robot 2), [−25, 25] (Robot 3), and [25, −25] (Robot 4), as illustrated in Figure8. Robots 1, 2 and Robots 3, 4 at antipodal positions are expected to exchange positions with each other. Furthermore, the initial orientation of robots 1 to 4 is 1/4π, −3/4π, −1/4π, and 3/4π, respectively. Note that every 18 ORCA steps plot a circle as shown in Figure 8.Figure 8
Motion trajectory for (a) Discrete-ORCA-MPC and (b) PID.
(a)(b)Figure8 shows the motion trajectory of 4 DDMRs on the workspace without static obstacles based on the proposed Discrete-ORCA-MPC approach and a proportional controller. Comparing the trajectories of local enlarged drawing of Figures 8(a) and 8(b), at 37.5°s, Robot 2 −1.718,1.56 collides with Robot 3 −1.421,−2.082 in Figure 8(b) (PID) while the collision did not happen in Figure 8(b) (Discrete-ORCA-MPC). Figure 9 depicts the relative distance between robots over the time where dij represents the relative distance between robots i and j. These results demonstrate the proposed method can generate a collision-free navigation for nonholonomic mobile robot effectively.Figure 9
The relative distance between robots. (a) Discrete-ORCA-MPC. (b) PID.
(a)(b)Figure10 shows the control inputs (v and ω) to each robot. In most of the time of the navigation, the control inputs change a little with respect to time; however, these has been a great change near 28°s and 37°s. It can be explained that the robots took a sharp turn in 28°s and 37°s as shown in Figure 8. In particular, the rapid growth of angular velocity ω is inevitably accompanied by a decline in the linear velocity ω. In other words, the robot should slow down for taking a sharp turn. It also can be seen that the control inputs are inside the limits imposed by the constraints. In Figure 11, it demonstrates that the state errors are compared with the trajectory generated by ORCA. It can be seen that maximum absolute errors of the position xe,ye and orientation ϕe are 0.07 and 0.4, respectively. Obviously, position errors are much less than the orientation error ϕe. This is reasonable because the smaller xe,ye shows a better performance of MPC controller. This simulation demonstrates that the proposed Discrete-ORCA-MPC algorithm has the ability to cope with the collision avoidance problem of multi-nonholonomic-agent.Figure 10
Control inputs of the 4 robots with respect to time. (a) Robot 1 control inputs. (b) Robot 2 control inputs. (c) Robot 3 control inputs. (d) Robot 1 control inputs.
(a)(b)(c)(d)Figure 11
Robots state errors compared to the holonomic trajectory. (a) Robot 1 state inputs. (b) Robot 2 state inputs. (c) Robot 3 state inputs. (d) Robot 4 state inputs.
(a)(b)(c)(d)


## 5.2. 4 DDMRs with Static Obstacle

It is similar to the first simulation; 4 DDMRs are initially located on the four corners of a square also, that is, [−45,−45] (Robot 1), [45, 45] (Robot 2), [−45, 45] (Robot 3) and [45, −45] (Robot 4), as illustrated in Figure12. Unlike the first one, this simulation considers static obstacle avoidance. These four static obstacles are located at [−22.5, −22.5], [22.5, 22.5], [−22.5, 22.5], and [22.5, −22.5] with sides that are 15 dm in length. Figure 13 depicts the relative distance between robots over the time. Furthermore, the initial orientations of Robots 1 to 4 are 1/4π+0.1, −3/4π+0.1, −1/4π+0.1, and 3/4π+0.1, respectively. This is why the trajectories of these four robots are asymmetrical. More sharp turn can be seen compared to the first simulation which leads to more sudden change in control inputs as shown in Figure 14. Meanwhile, the constraints of control inputs umin,umax and the errors with respect to reference control inputs u^min,u^max are satisfied. In Figure 15, it can also be seen that the position errors xe,ye are much less than the orientation error ϕe. The maximum value of position errors is less than 0.1 which means that the performance of the obtained trajectory tracking is good enough.Figure 12
Motion trajectory of 4 DDMRs with static obstacles.Figure 13
The relative distance between robots.
(a)(b)(c)Figure 14
Control inputs of the 4 robots with respect to time (consider static obstacles). (a) Robot 1 control inputs. (b) Robot 2 control inputs. (c) Robot 3 control inputs. (d) Robot 4 control inputs.
(a)(b)(c)(d)Figure 15
Robots state errors compared to the holonomic trajectory generated by ORCA (consider static obstacles). (a) Robot 1 state errors. (b) Robot 2 state errors. (c) Robot 3 state errors. (d) Robot 1 state errors.
(a)(b)(c)(d)This simulation demonstrates that the proposed Discrete-ORCA-MPC algorithm has the ability to cope with the navigation problem of multi-nonholonomic-agent with static obstacles.


## 5.3. Performance Results

In order to test the performance of Discrete-ORCA-MPC, we carried on large-scale simulations to compare the mean computation times of our method with Priority-Based Noncooperative Distributed Model Predictive Control (PB-NC-DMPC) [44]. PB-NC-DMPC is an effective NC-DMPC strategy that deals with the problem of loss of the prediction consistency property while reducing the computation time as well. In the problem of collision avoidance of networked vehicles, PB-NC-DMPC showed better performance compared to NC-DMPC and Centralized MPC. The test scenario contains many robots whose goal is to move across a circle to the antipodal position. This scenario is the same as the published literature, Jur et al. [31]. In PB-NC-DMPC, we implemented the collision avoidance controller based on the same linear predictive model as (20). The controller parameters of PB-NC-DMPC are the same as Discrete-ORCA-MPC and remain unchanged for all robots in all simulations.Figure16 depicts the mean computation times of Discrete-ORCA-MPC and PB-NC-DMPC plotted over increasing number of DDMRs. It shows that running Discrete-ORCA-MPC is computationally cheaper than PB-NC-DMPC.Figure 16
Comparison of computation times.


## 6. Conclusions

In this paper, we have described a method, namely, Discrete-ORCA-MPC, for collision avoidance among multiple nonholonomic robots in planar environments. In particular, we extended the traditional ORCA method to a Discrete-ORCA strategy which can generate a uniform distribution of safe zones in the direction of selected new velocity by ORCA. The idea was to introduce the collision-free constraints to MPC optimization problem. The presented Discrete-ORCA-MPC method allows for smooth and safe navigation and good performance was observed in simulation experimental tests.The results of this paper show that our approach is capable of letting a homogeneous group of robots with nonlinear equations of motion safely avoid collisions at real-time computation rates. The collision-free navigation problem of nonhomogeneous group of robots will be solved through adjusting the safe zone. Many challenges for future development still remain. The future work should aim at further improving the adaptability in uncertain environment.



---

*Source: 4183427-2020-06-10.xml*