Applied Mathematics for Engineering Problems in Biomechanics and Robotics 2020
View this Special IssueResearch Article  Open Access
Run Mao, Hongli Gao, Liang Guo, "A Novel CollisionFree Navigation Approach for Multiple Nonholonomic Robots Based on ORCA and Linear MPC", Mathematical Problems in Engineering, vol. 2020, Article ID 4183427, 16 pages, 2020. https://doi.org/10.1155/2020/4183427
A Novel CollisionFree Navigation Approach for Multiple Nonholonomic Robots Based on ORCA and Linear MPC
Abstract
In the study of collisionfree 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 collisionfree navigation for multinonholonomic 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.
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], samplingbased 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 collisionfree 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 decisionmaking 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 motionplanning methods of multiple pointmass 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 NetMPC 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 realtime navigation is the ORCA algorithm [31]. It is an effective decentralized collision avoidance method for a large number of decisionmaking agents. The ORCA infers for each robot a halfplane 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 carlike 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 collisionfree 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 DiscreteORCAMPC combined approach for the problem where collisions need to be avoided among multiple nonholonomic mobile robots with decisionmaking 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 collisionfree motion of this DiscreteORCAMPC 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, timevarying 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 realtime 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 carlike robots or DDMR with trailer.
The main features of the proposed DiscreteORCAMPC combined approach are summarized as follows:(1)Different from the traditional methods such as artificial potential functions based on a pointmass robot model, the proposed method considers the nonholonomic constraints of robots. The pointmass robot model cannot guarantee collisionfree navigation, since a carlike 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 networkbased 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 realtime without a realtime 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 Section 3. 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 reciprocal 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 collisionfree navigation [31]. Let there be a set of discshaped robots (this discshaped assumption can easily be extended to translate polygons) moving in the plane . Each robot has a current position , a current velocity , and a radius . It is assumed that these parameters can be observed by other robots. Furthermore, each robot has a maximum speed and a preferred velocity (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.
Let denote an open disc of radius centered at :
The velocity obstacle is the set of all relative velocities of with respect to 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 aswhere and are the positions of agent A and agent B, respectively. () 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 , its velocity obstacle is .
Let and be the optimization velocities of and , 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 be the vector from to the closest point on the boundary of the velocity obstacle:and let be the outward normal of the boundary of at point . Consequently, is the smallest change required to the relative velocity of and to avert collision with time. To share the responsibility of avoiding collisions among the robots in a fair way, The increment means that agent takes half the responsibility of avoiding potential collision with agent B, while the remaining half will be taken by agent . Hence, the set of permitted velocities for is the halfplane pointing in the direction of starting at the point . More formally,
This set is illustrated in Figure 2. By this definition, the chosen new relative velocities will not enter , and consequently the velocities of the agent can be smoothed.
Each robot A performs a continuous cycle of sensing and acting with time step . 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 halfplane of velocities with respect to each other robot . The set of ORCA velocities for agent with respect to all robots is the intersection of the halfplanes of induced by each other robot :
Note that include the maximum speed constraint on the robot .
At last, the new velocity is selected from , which is the closest to its preferred velocity amongst all velocities inside the region of permitted velocities:
The ORCA algorithm in [31] used a simple robot model, in which kinematics are ignored. Consequently, the robot reaches its new position:
In many of these applications, the robot of interest is a carlike 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 firstorder 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 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.
The real nonholonomic trajectory is defined by two sections: an arc of circumference covered with constant linear speed and decreasing angular velocity , followed by a straight line path with constant speed . In this case, a mobile robot will actually need to follow an arc to achieve the selected velocity , and the set of provide no guarantee of collisionfree.
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 collisionfree 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 be . Let be the robot’s physical workspace. Let 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 of robot can be derived from its state by a potentially projection function :
The control input space is assumed to be convex. Let the continuoustime equation of motion for robot be given by a potentially nonlinear function :where is the state and is the control input at time t.
Given a current state of robot and some constant control input at time 0, the state of the robot at a given time is given bywhere is the solution to the differential equation of equation (9), which can be obtained numerically.
Consider two circular robots and in an environment. Let circle represent the robot, and let circle represent the obstacle. Given the position of agent A at time and a control , let us denote the position agent will be later undertaking for time interval by . Similarly, represents the state of agent undertaking for time interval . Note that the control is the control inputs to the robot that results in a change in the robot’s configuration. Then, the control obstacle can be defined aswhere and by using equations (8) and (10). As shown in Figure 4, , and symmetrically, . Robots and chose constant velocities and outside the obstacle velocity set and generate trajectory and , respectively. Then and take controls and (for instance, ), which generate trajectory and (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 ().
In fact, the ORCA algorithm can be transformed using a uniform sampling scheme to deal with the nonholonomic kinematic constraints. As illustrated in Figure 5, let and be the uniform sampling waypoints of holonomic trajectory and , respectively. The distance between and is for . Note that is the terminal point of one step of ORCA; that is, . Hence, the collision avoidance set of ORCA algorithm can be reformulated aswhere
Let be the safe zone radius of robot at time , and it can be defined aswhere and are the radii of robots and . The constructive process of safe zone can geometrically be illustrated in Figure 6. Note that and is side length of the maximum inscribed square. This square is the approximate safe zone for linear constraints (see next section). If robot applies the control input in time that “walk” to the safe zone in time will lead to collision avoidance.
3.2. Proof of CollisionFree Navigation
Theorem 1. If the safe zone of robot at control step is equal to the relative safe zone between robots and , the safe zone of robot must have been less than or equal to the safe zone of robot in this moment. Then, the mathematical relationship can be described by .
Proof. If .
According to (14), .
By using reduction to absurdity, Theorem 1 is proved.
For robot , there are robots in its neighbor region. If the radius of safe zone at control step is , it means that is the minimum dimension for robot , but it does not mean that the is the minimum dimension for robot . Theorem 1 gives the proof of this fact.
Theorem 2. In a multirobots environment, the collisionfree navigation can be guaranteed when the safe zone constraints are maintained at every moment.
Proof. Theorem 1 (robots set).
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 collisionfree navigation for DiscreteORCA can be guaranteed.
4. DiscreteORCAMPC 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, timevarying 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 Figure 7, the configuration of a DDMR is given by the position of its center point and its orientation . The configuration and highlevel control input transition equations [42] are given aswhere is the linear velocity and is the angular velocity. is the distance between the rear wheels of the robot, and and are the signed speeds of the left and right wheels, respectively. Generally, and are bounded such that .
In general, the kinematic model can be written in the form of (9):where describes the configuration of point (shown in Figure 7) and is the control input.
Similarly, let and be the configuration and control input of reference system. Then, it can be described as
By expanding the right side of (16) in Taylor series around the reference point , a linear model is obtained by discarding the highorder terms:where and are the Jacobians of with respect to and , respectively, evaluated around the reference point .
Then, the subtraction of (17) from (18) results inwhere represents the error with respect to the reference robot configuration and is its error with respect to the reference input. Finally, the discretetime system can be obtained:withwhere is the sampling period and is the sampling time.
In this paper, let and be the reference velocity and direction of robot . Then, the reference trajectory of robot is obtained as (6):where . It means that robot generates waypoints in the direction of uniformly. In [40], we notice that the controllability matrix has full rank if either or 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 trajectorytracking control of a differentially driven mobile robot based on MPC is presented. The main idea is to minimize the future trajectoryfollowing errors and control errors between the robot and reference waypoints generated by DiscreteORCA.
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 can be stated aswhere is the prediction horizon and and are the weighting matrices (diagonal matrix with and ). Note that, the angle error must be wrapped in radians to . The notation indicates the value of at the predicted time based on the current time . In order to reformulate the optimization problem in a usual quadratic programming form, the and are defined as
Then, the original optimal problem (23) can be rewritten aswhere
Therefore, (20) can be recasted aswithwhere
Rearrange (25) and (27), the cost function (23) can be rewritten aswhere
The is a positive definite Hessian matrix. It is the quadratic part of the cost function while the vector describes the linear part. Note that and contain which is constant vector at time . Hence, the optimization problem can be stated as to find at each time step minimizing the cost function (30):
The solution of (32) is a sequence of optimal control; that is . The MPC scheme is implicitly given by the first control action of the sequence of optimal control, .
4.3. CollisionFree 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
Note that the free variable in the optimization (32) is , so the constraint (33) must be rewritten aswhere
Furthermore, the collision avoidance requirement will be constructed as the constraint with respect to control inputs. Let us rewrite the safe zone (14) as
It means that the control inputs to robot in time must reach the safe zone . 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 can be denoted as
Then, the collision avoidance manoeuvre can be represented aswhere and can be obtained from the discrete time form of the kinematic equation (15):
Note that is not the consideration of the navigation. Remember that , so we have . Then rearranging (38) to (39) can obtain the collision avoidance constraint in each step time :where
Note that and are constants in each time step. Recalling (34), we can obtainwith
Finally, the constraint of free variable for optimization problem (32) can be constructed by refreshing and in (34) aswhere
4.4. Algorithm
The overall approach is summarized in Algorithm 1. In each ORCA step (), it has three things for every robot to do. First, robot generates the discrete reference control inputs and trajectory. The number of waypoints is , and the corresponding reference control inputs are . Second, robot computes the safe zone with respect to the neighbor region. Finally, robot repeatedly computes the optimal control inputs at each sample time to close the last waypoint. Then, it will go into the next ORCA step. This process will continue until every robot reaches its goal.

5. Experiments and Analysis
In this section, we conduct computer simulations to verify the proposed DiscreteORCAMPC algorithm. The following simulations are specifically designed to deal with multinonholonomicagent 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 robots and the control input constraints are set as and , respectively. The preferred velocity 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 and the time window of the ORCA algorithm is . In the MPC controller formulation, the prediction horizon and optimization problem is solved using interior point method. Furthermore, the free variable constraints are set as and , respectively. To keep the robot close to the ORCA trajectory, the weighting matrix should be larger than . In this paper, these are set as and .
5.1. Compare MPC and PID for 4 DDMRs
As mentioned in Section 2.2, the original ORCA algorithm is a firstorder method. In other words, the control input to the robot is reference velocity but not the reference trajectory. In this simulation, a firstorder method based on ORCA is formulated to compare with the proposed DiscreteORCAMPC 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 aswhere and represent the 2norm value and direction of the new velocity (equation (6)) of robot generated by ORCA in step ; that is, and . and are the control input to robot in control step of ORCA step . 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 , 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 Figure 8. 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 , , , and , respectively. Note that every 18 ORCA steps plot a circle as shown in Figure 8.
(a)
(b)
Figure 8 shows the motion trajectory of 4 DDMRs on the workspace without static obstacles based on the proposed DiscreteORCAMPC 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 collides with Robot 3 in Figure 8(b) (PID) while the collision did not happen in Figure 8(b) (DiscreteORCAMPC). Figure 9 depicts the relative distance between robots over the time where represents the relative distance between robots and . These results demonstrate the proposed method can generate a collisionfree navigation for nonholonomic mobile robot effectively.
(a)
(b)
Figure 10 shows the control inputs ( 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 and orientation are 0.07 and 0.4, respectively. Obviously, position errors are much less than the orientation error . This is reasonable because the smaller shows a better performance of MPC controller. This simulation demonstrates that the proposed DiscreteORCAMPC algorithm has the ability to cope with the collision avoidance problem of multinonholonomicagent.
(a)
(b)
(c)
(d)
(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 Figure 12. 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 , , , and , 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 and the errors with respect to reference control inputs are satisfied. In Figure 15, it can also be seen that the position errors are much less than the orientation error . The maximum value of position errors is less than 0.1 which means that the performance of the obtained trajectory tracking is good enough.
(a)
(b)
(c)
(a)
(b)
(c)
(d)
(a)
(b)
(c)
(d)
This simulation demonstrates that the proposed DiscreteORCAMPC algorithm has the ability to cope with the navigation problem of multinonholonomicagent with static obstacles.
5.3. Performance Results
In order to test the performance of DiscreteORCAMPC, we carried on largescale simulations to compare the mean computation times of our method with PriorityBased Noncooperative Distributed Model Predictive Control (PBNCDMPC) [44]. PBNCDMPC is an effective NCDMPC 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, PBNCDMPC showed better performance compared to NCDMPC 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 PBNCDMPC, we implemented the collision avoidance controller based on the same linear predictive model as (20). The controller parameters of PBNCDMPC are the same as DiscreteORCAMPC and remain unchanged for all robots in all simulations.
Figure 16 depicts the mean computation times of DiscreteORCAMPC and PBNCDMPC plotted over increasing number of DDMRs. It shows that running DiscreteORCAMPC is computationally cheaper than PBNCDMPC.
6. Conclusions
In this paper, we have described a method, namely, DiscreteORCAMPC, for collision avoidance among multiple nonholonomic robots in planar environments. In particular, we extended the traditional ORCA method to a DiscreteORCA 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 collisionfree constraints to MPC optimization problem. The presented DiscreteORCAMPC 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 realtime computation rates. The collisionfree 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.
Data Availability
The data used to support the finds of this study are available from the corresponding author upon request.
Conflicts of Interest
The authors declare that there are no conflicts of interest regarding the publication of this paper.
Acknowledgments
This work was carried out with the support of the Sichuan Science and Technology Program (no. 20ZDYF1891).
References
 J. AlonsoMora, P. Beardsley, and R. Siegwart, “Cooperative collision avoidance for nonholonomic robots,” IEEE Transactions on Robotics, vol. 34, no. 2, pp. 404–420, 2018. View at: Publisher Site  Google Scholar
 V. Kunchev, L. Jain, V. Ivancevic, and A. Finn, “Path planning and obstacle avoidance for autonomous mobile robots: a review,” in Proceedings of the International Conference on KnowledgeBased and Intelligent Information and Engineering Systems, pp. 537–544, Springer, Bournemouth, UK, October 2006. View at: Google Scholar
 A. Mukhtar, L. Xia, and T. B. Tang, “Vehicle detection techniques for collision avoidance systems: a review,” IEEE Transactions on Intelligent Transportation Systems, vol. 16, no. 5, pp. 2318–2338, 2015. View at: Publisher Site  Google Scholar
 S. Campbell, W. Naeem, and G. W. Irwin, “A review on improving the autonomy of unmanned surface vehicles through intelligent collision avoidance manoeuvres,” Annual Reviews in Control, vol. 36, no. 2, pp. 267–283, 2012. View at: Publisher Site  Google Scholar
 S. S. Ge and Y. J. Cui, “Dynamic motion planning for mobile robots using potential field method,” Autonomous Robots, vol. 13, no. 3, pp. 207–222, 2002. View at: Publisher Site  Google Scholar
 J. P. van den Berg and M. H. Overmars, “Roadmapbased motion planning in dynamic environments,” IEEE Transactions on Robotics, vol. 21, no. 5, pp. 885–897, 2005. View at: Publisher Site  Google Scholar
 S. Karaman and E. Frazzoli, “Samplingbased algorithms for optimal motion planning,” The International Journal of Robotics Research, vol. 30, no. 7, pp. 846–894, 2011. View at: Publisher Site  Google Scholar
 D. Hsu, R. Kindel, J.C. Latombe, and S. Rock, “Randomized kinodynamic motion planning with moving obstacles,” The International Journal of Robotics Research, vol. 21, no. 3, pp. 233–255, 2002. View at: Publisher Site  Google Scholar
 J. P. Gonzalez and M. Likhachev, “Searchbased planning with provable suboptimality bounds for continuous state spaces,” in Proceedings of the Fourth Annual Symposium on Combinatorial Search, Barcelona, Spain, July 2011. View at: Google Scholar
 J. Wang, S. Wu, H. Li, and J. Zou, “Path planning combining improved rapidlyexploring random trees with dynamic window approach in ROS,” in Proceedings of the 2018 13th IEEE Conference on Industrial Electronics and Applications (ICIEA), Wuhan, China, May 2018. View at: Publisher Site  Google Scholar
 X. Zhong, X. Zhong, and X. Peng, “Velocitychangespacebased dynamic motion planning for mobile robots navigation,” Neurocomputing, vol. 143, no. SI, pp. 153–163, 2014. View at: Publisher Site  Google Scholar
 J. Borenstein and Y. Koren, “The vector field histogram—fast obstacle avoidance for mobile robots,” Robotics and Automation, IEEE Transactions on, vol. 7, pp. 278–288, 1991. View at: Google Scholar
 D. Fox, W. Burgard, and S. Thrun, “The dynamic window approach to collision avoidance,” IEEE Robotics & Automation Magazine, vol. 4, no. 1, pp. 23–33, 1997. View at: Publisher Site  Google Scholar
 F. Kanehiro, F. Lamiraux, O. Kanoun, E. Yoshida, and J.P. Laumond, “A local collision avoidance method for nonstrictly convex polyhedra,” Proceedings of Robotics: Science and Systems, vol. 4, 2008. View at: Google Scholar
 H. A. Hagras, “A hierarchical type2 fuzzy logic control architecture for autonomous mobile robots,” IEEE Transactions on Fuzzy Systems, vol. 12, no. 4, pp. 524–539, 2004. View at: Publisher Site  Google Scholar
 G. Antonelli, F. Arrichiello, and S. Chiaverini, “Experiments of formation control with multirobot systems using the nullspacebased behavioral control,” IEEE Transactions on Control Systems Technology, vol. 17, no. 5, pp. 1173–1182, 2009. View at: Publisher Site  Google Scholar
 C. G. Zhang and Y. G. Xi, “Rolling path planning and safety analysis of mobile robot in dynamic uncertain environment,” Control Theory & Applications, vol. 20, pp. 37–44, 2003. View at: Google Scholar
 M. G. Mohanan and A. Salgoankar, “A survey of robotic motion planning in dynamic environments,” Robotics & Autonomous Systems, vol. 100, pp. 171–185, 2017. View at: Google Scholar
 Y. Dobrev, M. Vossiek, M. Christmann, I. Bilous, and P. Gulden, “Steady delivery: wireless local positioning systems for tracking and autonomous navigation of transport vehicles and mobile robots,” IEEE Microwave Magazine, vol. 18, no. 6, pp. 26–37, 2017. View at: Publisher Site  Google Scholar
 D. Thalmann, “Crowd simulation,” in Encyclopedia of Computer Graphics and Games, pp. 1–8, Springer, Berlin, Germany, 2016. View at: Google Scholar
 Z. Peng, S. Yang, G. Wen, A. Rahmani, and Y. Yu, “Adaptive distributed formation control for multiple nonholonomic wheeled mobile robots,” Neurocomputing, vol. 173, pp. 1485–1494, 2016. View at: Publisher Site  Google Scholar
 S. Liu, D. Sun, and C. Zhu, “A dynamic priority based path planning for cooperation of multiple mobile robots in formation forming,” Robotics and ComputerIntegrated Manufacturing, vol. 30, no. 6, pp. 589–596, 2014. View at: Publisher Site  Google Scholar
 Y. Zhang, Y. Wang, and Y. Yao, “Distributed adaptive sliding mode control for attitudes synchronization of multiple autonomous underwater vehicles,” Advances in Mechanical Engineering, vol. 9, no. 11, Article ID 1687814017737262, 2017. View at: Publisher Site  Google Scholar
 J. V. den Berg, M. Lin, and D. Manocha, “Reciprocal velocity obstacles for realtime multiagent navigation,” in Proceedings of the 2008 IEEE International Conference on Robotics and Automation, pp. 1928–1935, IEEE, Pasadena, CA, USA, May 2008. View at: Publisher Site  Google Scholar
 A. Prasad, B. Sharma, and J. Vanualailai, “A new stabilizing solution for motion planning and control of multiple robots,” Robotica, vol. 34, no. 5, pp. 1071–1089, 2016. View at: Publisher Site  Google Scholar
 P. Fiorini and Z. Shiller, “Motion planning in dynamic environments using velocity obstacles,” The International Journal of Robotics Research, vol. 17, no. 7, pp. 760–772, 1998. View at: Publisher Site  Google Scholar
 W. Ren, W. Randal, and Beard, Distributed Consensus in MultiVechicle Cooperative Control, Springer Science & Business Media, Berlin, Germany, 2008.
 G. Lou, W. Gu, Y. Xu, M. Cheng, and W. Liu, “Distributed MPCbased secondary voltage control scheme for autonomous droopcontrolled microgrids,” IEEE Transactions on Sustainable Energy, vol. 8, no. 2, pp. 792–804, 2017. View at: Publisher Site  Google Scholar
 R. Van Parys and G. Pipeleers, “Distributed mpc for multivehicle systems moving in formation,” Robotics and Autonomous Systems, vol. 97, p. 08, 2017. View at: Publisher Site  Google Scholar
 S. Yoo and B. Park, “Connectivity preservation and collision avoidance in networked nonholonomic multirobot formation systems: unified error transformation strategy,” Automatica, vol. 103, pp. 274–281, 2019. View at: Google Scholar
 J. Van Den Berg, S. J. Guy, M. Lin, and D. Manocha, “Reciprocal nbody collision avoidance,” in Robotics Research, pp. 3–19, Springer, Berlin, Germany, 2011. View at: Google Scholar
 J. AlonsoMora, A. Breitenmoser, M. Rufli, B. Paul, and S. Roland, “Optimal reciprocal collision avoidance for multiple nonholonomic robots,” in Distributed Autonomous Robotic Systems, pp. 203–216, Springer, Berlin, Germany, 2013. View at: Google Scholar
 D. Wilkie, J. Van Den Berg, and D. Manocha, “Generalized velocity obstacles,” in Proceedings of the 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 5573–5578, IEEE, St. Louis, MO, USA, October 2009. View at: Publisher Site  Google Scholar
 J. D. Jeon and H. B. Lee, “Wheel velocity obstacles for differential drive robot navigation,” Journal of Automation and Control Engineering, vol. 3, no. 5, 2015. View at: Google Scholar
 B. Alrifaee, J. Maczijewski, and D. Abel, “Sequential convex programming mpc for dynamic vehicle collision avoidance,” in Proceedings of the 2017 IEEE Conference on Control Technology and Applications (CCTA), pp. 2202–2207, IEEE, Mauna Lani, HI, USA, August 2017. View at: Publisher Site  Google Scholar
 S. M. Erlien, S. Fujita, and J. C. Gerdes, “Shared steering control using safe envelopes for obstacle avoidance and vehicle stability,” IEEE Transactions on Intelligent Transportation Systems, vol. 17, no. 2, pp. 441–451, 2016. View at: Publisher Site  Google Scholar
 O. Andersson, M. Wzorek, P. Rudol, and P. Doherty, “Modelpredictive control with stochastic collision avoidance using bayesian policy optimization,” in Proceedings of the 2016 IEEE International Conference on Robotics and Automation (ICRA), pp. 4597–4604, IEEE, Stockholm, Sweden, May 2016. View at: Publisher Site  Google Scholar
 H. Yu, J. Duan, S. Taheri, H. Cheng, and Z. Qi, “A model predictive control approach combined unscented kalman filter vehicle state estimation in intelligent vehicle trajectory tracking,” Advances in Mechanical Engineering, vol. 7, no. 5, Article ID 1687814015578361, 2015. View at: Publisher Site  Google Scholar
 F. Kuhne, W. F. Lages, and J. G. da Silva Jr., “Model predictive control of a mobile robot using linearization,” in Proceedings of Mechatronics and Robotics, pp. 525–530, Citeseer, New York, NY, USA, 2004. View at: Google Scholar
 G. Klančar and I. Škrjanc, “Trackingerror modelbased predictive control for mobile robots in real time,” Robotics and Autonomous Systems, vol. 55, no. 6, pp. 460–469, 2007. View at: Google Scholar
 M. Rufli, J. AlonsoMora, and R. Siegwart, “Reciprocal collision avoidance with motion continuity constraints,” IEEE Transactions on Robotics, vol. 29, no. 4, pp. 899–912, 2013. View at: Publisher Site  Google Scholar
 S. M. LaValle, Planning Algorithms, Cambridge University Press, Cambridge, UK, 2006.
 M. Jan, Predictive Control with Constraints, Prentice Hall, Upper Saddle River, NJ, USA, 2002.
 B. Alrifaee, F.J. Heßeler, and D. Abel, “Coordinated noncooperative distributed model predictive control for decoupled systems using graphs,” IFACPapersOnLine, vol. 49, no. 22, pp. 216–221, 2016. View at: Publisher Site  Google Scholar
Copyright
Copyright © 2020 Run Mao et al. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.