Multi-robot motion planning: A modified receding horizon approach for reaching goal states

. This paper proposes the real-time implementation of an algorithm for collision-free motion planning based on a receding horizon approach, for the navigation of a team of mobile robots in presence of obstacles of diﬀerent shapes. The method is simulated with three robots. Impact of parameters is studied with regard to computation time, obstacle avoidance and travel time.


Introduction
The control of mobile robots is a long-standing subject of research in the robotics domain. A trending application of mobile robotic systems is their use in industrial supply-chains for processing orders and optimizing products' storage and distribution. Companies such as Amazon and the logistic provider IDEA Groupe employ mobile multi-robot systems (Kiva systems, Scallog respectively) for autonomously processing client orders [1,2]. Such logistics tasks became increasingly complex as sources of uncertainty, such as human presence, are admitted in the work environment.
One basic requirement for such mobile multirobot systems is the capacity of motion planning, that is, generating admissible configuration and input trajectories connecting two arbitrary states. For solving the motion planning problem, different constraints must be taken into account, in particular, the robot's kinematic and geometric constraints.
The first constraints derive directly from the mobile robot architecture implying, in particular, in nonholonomic constraints. Geometric constraints result from the need of preventing the robot to assume specific configurations in order to avoid collisions, communication lost, etc.
We are particularly interested in solving the problem of planning a trajectory for a team of nonholonomic mobile robots, in a partially known environment occupied by static obstacles, being efficient with respect to the travel time (amount of time to go from initial to goal configuration).
A great amount of work towards collision-free motion planning for cooperative multi-robot systems has been proposed. That work can be split into centralized and distributed approaches. Centralized approaches are usually formulated as an optimal control problem that takes all robots in the team into account at once. This produces solutions closer to the optimal one than distributed approaches. However, the computation time, security vulnerability and communication requirements can make it impracticable, specially for a great number of robots [3].
Distributed methods based in probabilistic [4] and artificial potential fields [5] approaches, for instance, are computationally fast. However, they deal with collision avoidance as a cost function to be minimized. But rather than having a cost that increases as paths leading to collision are considered, collision avoidance should to be considered as hard constraints of the problem.
Other distributed algorithms are based on receding horizon approaches. In [6] a brief comparison of the main distributed receding methods is made as well as the presentation of the base approach extended in our work. In this approach each robot optimizes only its own trajectory at each computation/update horizon. In order to avoid robot-to-robot collisions and lost of communication, neighbor robots exchange information about their intended trajectories before performing the update. Intended trajectories are computed by each robot ignoring constraints that take the other robots into account.
Identified drawbacks of this approach are the dependence on several parameters for achieving realtime performance and good solution optimality, the difficulty to adapt it for handling dynamic obstacles, the impossibility of bringing the robots to a precise goal state and the limited geometric representation of obstacles.
Therefore, in this paper, we propose a motion planning algorithm that extends the approach presented in [6]. In this modified algorithm goal states can be precisely reached and more complexes forms of obstacles handled. Furthermore, we perform an investigation about how the method's parameters José M. Mendes Filho, Eric Lucet http://robotics.fel.cvut.cz/demur15/

DEMUR'15
impact a set of performance criteria. Thus, this distributed algorithm is able to find collision-free trajectories and computes the corresponding angular and longitudinal velocities for a multi-robot system in presence of static obstacles perceived by the robots as they evolve in their environment. This paper is structured as follows. The second section states the problem to be resolved, pointing out the cost function for motion planning and all constraints that need to be respected by the computed solution. The third section explains the algorithm for resolving the motion planning problem and gives some remarks on how to resolve the constrained optimization problems associated with the method. The forth section is dedicated to the results found using this method and the analysis of the specific performance criteria and how they are impacted by the algorithm parameters. Finally, in last section we present our conclusions and perspectives.

Assumptions
In the development of the approach presented in this paper, the following assumptions are made: (1.) The motion of the multi-robot system begins at the instant t init and goes until the instant t f inal .
(2.) The team of robots consists of a set R of B nonholonomic mobile robots.
is geometrically represented by a circle of radius ρ b centered at (x b , y b ).

Constraints and cost functions
After giving the assumptions in the previous Subsection, we can define the constraints and the cost function for the multi-robot navigation.
(1.) The solution of the motion planning problem for the robot R b represented by the pair (q * b (t), u * b (t)) q * b (t) ∈ R n being the solution trajectory for the robot's configuration and u * b (t) ∈ R p the solution trajectory for the robot's input -must satisfy the robots kinematic model equation: (1) where f : R n × R p → R n the is the vector-valued function modeling the robot kinematics. (2.) The planned initial configuration and initial input for the robot R b must be equal to the initial configuration and initial input of R b : (3.) The planned final configuration and final input for the robot R b must be equal to the goal configuration and goal input for R b :  (5.) The cost for the multi-robot system navigation is defined as: is the integrated cost for one robot motion planning (see [6]). (6.) To ensure collision avoidance with obstacles, the euclidean distance between a robot and an obstacle For the circle representation of an obstacle the distance d(R b , O m ) is defined as: For the convex polygon representation, the distance was calculated using three different definitions, according to the Voronoi region [7] R b is located.

DEMUR'15
http://robotics.fel.cvut.cz/demur15/ Trajectory Generation Approach (7.) In order to prevent inter-robot collisions, the following constraint must be satisfied: is the set of robots that present a collision risk with R b . (8.) Finally, the need of a communication link between two robots (R b , R c ) yields to the following constraint:

Receding horizon approach
Since the environment is progressively perceived by the robots and new obstacles may appear as time passes, planning the whole motion from initial to goal configurations before the beginning of the motion is not a satisfying approach. Planning locally and replanning is more suitable for taking new information, as they come, into account. Besides, the computation cost of finding a motion plan using the first approach may be prohibited high if the planning complexity depends on the distance between start and goal configurations. Therefore, an on-line motion planner is proposed. In order to do so a receding horizon control approach [8] is used.
Two fundamental concepts of this approach are the planning horizon T p and update/computation horizon T c . T p is the timespan for which a solution will be computed and T c is the time horizon during which a plan is executed while the next plan, for the next timespan T p , is being computed. The problem of producing a motion plan during a T c time interval is called here a receding horizon planning problem.
For each receding horizon planning problem, the following steps are performed: Step 1. All robots in the team compute an intended solution trajectory (denoted (q b (t),û b (t))) by solving a constrained optimization problem.
Coupling constraints (9) and (10) that involve other robots in the team are ignored.
Step 2. Robots involved in a potential conflict (that is, risk of collision or lost of communication) update their trajectories computed during Step 1 by solving another constrained optimization problem that additionally takes into account coupling constraints (9) and (10). This is done by using the other robots' intended trajectories computed in the previous step as an estimate of those robots' final trajectories. If a robot is not involved in any conflict, Step 2 is not executed and its final solution trajectory is identical to the one estimated in Step 1.
All robots in the team use the same T p and T c for assuring synchronization when exchanging information about their positions and intended trajectories.
For each of these steps and for each robot in the team, one constrained optimization problem is resolved. The cost function to be minimized in those optimization problems is the geodesic distance of a robot's current configuration to its goal configuration. This assures that the robots are driven towards their goal.
This two step scheme is explained in details in [6,9] where constrained optimization problems associated to the receding horizon optimization problem are formulated.
However, constraints related to the goal configuration and goal input of the motion planning problem are neglected in their method. Constraints (4) and (5) are left out of the planning. For taking them into account, a termination procedure is proposed in the following that enables the robots to reach their goal state.

Motion planning termination
After stopping the receding horizon planning algorithm, we propose a termination planning that considers those constraints related to the goal state. This enables the robots to reach their goal states.
The criterion used to pass from the receding horizon planning to the termination planning is based on the distance between goal and current position of the robots. It is defined by the equation 11: This condition ensures that the termination plan will be planned for at least a d min distance from the robot's goal position. This minimal distance is assumed to be sufficient for the robot to reach the goal configuration. Before solving the termination planning problem new parameters for the solution representation and computation are calculated by taking into account the estimate remaining distance and the typical distance traveled for a T p planning horizon. This is done in order to rescale the configuration intended for a previous planning horizon not necessarily equal to the new one. Potentially, this rescaling will decrease the computation time for the termination planning.
The following pseudo code 1 summarizes the planning algorithm and the Figure 1 illustrates how plans would be generated through time by the algorithm.
In the pseudo code, we see the call of a PlanSec procedure. It corresponds to the resolution of the José M. Mendes Filho, Eric Lucet http://robotics.fel.cvut.cz/demur15/

DEMUR'15
receding horizon planning problem as defined in subsection 3.1.
PlanLastSec is the procedure solving the termination planning problem. This problem is similar to the receding horizon planning problems.
It also has the two steps presented before for computing an intended plan and for updating it, if need be, so conflicts are avoided.
The difference consists in how the optimization problems associated to it are defined.
The optimization problem defined in (12) and (13) is the problem solved at the first step. The optimization problem associated with the second step is defined (14) and (15). Besides, in both new constrained optimal problems, the planning horizon is not a fixed constant as before, instead it is a part of the solution to be found.
Then, for generating the intended plan the following is resolved: under the following constraints for τ k = kT c with k the number of receding horizon problems solved before the termination problem: And for generating the final solution: under the following constraints:

Flatness property
As explained in [9], all mobile robots consisting of a solid block in motion can be modeled as a flat system. RescaleRepresentation(· · · ) 10: T f ←PlanLastSec(· · · ) 11: end procedure This means that a change of variables is possible in a way that states and inputs of the kinematic model of the mobile robot can be written in terms of a new variable, called flat output (z), and its lth first derivatives. The value of l | l ≤ n depends on the kinematic model of the mobile robot. Therefore, the flat output can completely determine behavior of the system.
Searching for a solution to our problem in the flat space rather than in the actual configuration space of the system presents advantages. It prevents the need for integrating the differential equations of system (constraint 1) and reduces the dimension of the problem of finding an optimal admissible trajectory. After finding (optimal) trajectories in the flat space, it is possible to retrieve back the original configuration and input trajectories.

Parametrization of the flat output by
B-splines Another important aspect of this approach is the parametrization of the flat output trajectory. As done in [10], the use of B-spline functions present interesting properties.
• It is possible to specify a level of continuity C k when using B-splines without additional constraints.
• B-spline presents a local support -changes in parameters values have a local impact on the resulting curve.
The first property is very well suited for parametrizing DEMUR'15 http://robotics.fel.cvut.cz/demur15/ Trajectory Generation Approach the flat output since its lth first derivatives will be needed when computing the system actual state and input trajectories. The second property is important when searching for an admissible solution in the flat space; such parametrization is more efficient and well-conditioned than, for instance, a polynomial parametrization [10]. This choice for parameterizing the flat output introduces a new parameter to be set in the motion planning algorithm which is the number of nonnull knots intervals (denoted simply N knots ). This parameter plus the l value determines how many control points will be used for generating the B-splines.

Optimization solver
The optimization problems associated with finding the solution q * (t), u * (t) are solved using a numerical optimization solver.
For all time dependent constraints time sampling is used. This introduces a new parameter in the algorithm: the time sampling for optimization N s . Each constraint that must be satisfied ∀t ∈ (τ k , τ k + T f ) implies in N s equations.
The need of a solver that supports nonlinear equality and inequality constrains restricts the number of numerical optimization solvers to be considered.
For our initial implementation of the motion planning algorithm, the SLSQP optimizer stood out as a good option. Besides being able to handle nonlinear equality and inequality constrains, its availability in the minimization module of the open-source scientific package Scipy [11] helps to facilitate the motion planner implementation.
However, an error was experienced using this optimizer which uses the SLSQP Optimization subroutine originally implemented by Dieter Kraft [12]. As the cost function value becomes too high (typically for values greater than 10 3 ), the optimization algorithm finishes with the "Positive directional derivative for linesearch" error message. This appears to be a numerical stability problem experienced by other users as discussed in [13].
For working around this problem, we proposed a change in the objective functions of the receding horizon optimization problems. This change aims to keep the evaluated cost of the objective function around a known value when close to the optimal solution instead of having a cost depending on the goal configuration (which can be arbitrarily distant from the current position).
We simply exchanged the goal position point in the cost function by a new point computed as follows: Where p b,goal and p b (τ s−1 + T c ) are the positions associated with configurations q b,goal and q b (τ s−1 +T c ) respectively, α | α ≥ 1, α ∈ R is a constant for controlling how far from the current position the new point is placed, the product T p v b,max the maximum possible distance covered by R b during a planning horizon and s | s ∈ [0, k), s ∈ N the current receding horizon problem index.

Simulation results
Results and their analysis for the motion planner presented in the previous sections are presented here. The trajectory and velocities shown in Figures 2  and 3 illustrate a motion planning solution found for a team of three robots. They plan their motion in an environment where three static obstacles are present. Each point along the trajectory line of a robot represents the beginning of a T c update/computation horizon.
It is possible to see on those figures how the planner generates configuration and input trajectories satisfying the constraints associated with the goal states.
In particular, in Figure 2, the resulting plan is computed ignoring coupling constraints (Step 2 is never performed) and consequently two points of collision occur.
A collision-free solution is presented in Figure 3. Specially near the regions were collisions occurred a change in the trajectory is present from Figure 2 to Figure 3 to avoid collision. Complementary, changes in the robots (linear) velocities across charts in both figures can be noticed. Finally, the bottom charts show that the collisions were indeed avoided: inter-robot distances in Figure 3 are greater than or equal to zero all along the simulation.
For performing these two previous simulations, a reasonable number of parameters have to be set. These parameters can be categorized into two groups. Algorithm related parameters and the optimization solver related ones. Among the former group, the most important ones are: • The number of sample for time discretization (N s ); • The number of internal knots for the B-splines curves (N knots ); • The planning horizon for the sliding window (T p ); • The computation horizon (T c ).
• The detection radius of the robot (d sen ).
The latter kind depends on the numeric optimization solver adopted. However, since most of them are iterative methods, it is common to have at least a maximum number of iterations and a stop condition parameters.
This considerable number of parameters makes the search for a satisfactory set of parameters' values a laborious task.
Therefore, it is important to have a better understanding of how some performance criteria are impacted by the changes in algorithm parameters. Inter-robot distances throughout simulation

Parameters' impact
Three criteria considered important for the validation of this method were studied: Maximum computation time during the planning over the computation horizon (M CT /T c ratio); Obstacle penetration area (P ); Travel time (T tot ). Different parameters configuration and scenarios where tested in order to highlight how they influence those criteria.

4.1.1.
Maximum computation time over computation horizon M CT /T c The significance of this criterion lays in the need of assuring the real-time property of this algorithm. In a real implementation of this approach the computation horizon would have always to be superior than the maximum time took for computing a plan. Table 1 summarizes one of the scenarios studied for a single robot. Results obtained from simulations in that scenario are presented in Figure 4, for different parameters set.
Each dot along the curves corresponds to the Inter-robot distances throughout simulation Rather than observing the absolute values, it is interesting to analyze the impact of changes in the parameters values. In particular, an increasing number of N s increases M CT /T c for a given T c /T p . Similarly, an increasing of M CT /T c as the number of internal knots N knots increases from charts 4a to 4c is noticed.
Further analyses of those data show that finding the solution using the SLSPQ method requires O(N 3 knots ) and O(N s ) time. Although augmenting N knots can yield to an impractical computation time, typical N knots values did not need to exceed 10 in our simulations, which is a sufficiently small value.

DEMUR'15
http://robotics.fel.cvut.cz/demur15/ Trajectory Generation Approach  Another parameter having direct impact on the M CT /T c ratio is the detection radius of the robot's sensors. As the detection radius of the robot increases, more obstacles are seen at once which, in turn, increases the number of constraints in the optimization problems. The impact of increasing the detection radius d sen in the M CT /T c ratio can be seen in Figure 5 for a scenario with seven obstacles. The computation time stops increasing as soon as the

Obstacle penetration P
Obstacle penetration area P gives a metric for obstacle avoidance and consequently for the solution quality. A solution where the planned trajectory does not pass through an object at any instant of time gives P = 0. The solution quality decrease with increasing P. However, since time sampling is performed during the optimization, P is usually greater than zero. A way of assuring P = 0 would be to increase the obstacles radius computed by the robot's perception system by the maximum distance that the robot can run within the time spam T p /N s . However simple, this approach represents a loss of optimality and is not considered in this work. It is relevant then to observe the impact of the algorithm parameters in the obstacle penetration area. T c /T p ratio, N knots and d sen impact on this criteria is only significant for degraded cases, meaning that around typical values those parameters do not change P significantly. However, time sampling N s is a relevant parameter. Figure 6 shows the penetration area decreasing as the number of samples increases. Obstacle penetration decreasing as sampling increases

Travel time T tot
Another complementary metric for characterizing solution quality is the travel time T tot . Analyses of data from several simulations show a tendency that for a given value of N knots , N s and T c the travel time decreases as the planning horizon T p decreases. This can be explained by the simple fact that for a given T c , a more optimal solution (in terms of travel time) can be found if the planning horizon T p is smaller. José M. Mendes Filho, Eric Lucet http://robotics.fel.cvut.cz/demur15/

DEMUR'15
Another relevant observation is that the overall travel time is shorter for smaller N s 's. This misleading improvement does take into account the fact that the fewer the samples the greater will be the obstacle penetration area as shown previously in Figure 6. Furthermore, Figure 7 shows travel time invariance for changes in the detection radius far from degraded values that are too small. This points out that a local knowledge of the environment provides enough information for finding good solutions.

Conclusions
A distributed motion planner based on a receding horizon approach, modified for taking into account termination constraints, was proposed. Near the goal configuration neighborhood, the receding horizon approach is finished and a termination planning problem is solved for bringing the robots to their precise final state. The problem is stated as a constrained optimization problem. It minimizes the time for reaching a goal configuration through a collision-free trajectory securing communication between robots. Circle and convex polygon representation of obstacles are supported. Key techniques for implementing the motion planner are: system flatness property, B-spline parameterization of the flat output and SLSQP optimizer. Finally, solutions using this planner for different scenarios were generated in order to validate the method. Impact of different parameters on computation time and quality of the solution was analyzed. Future work will be performed in physics simulation environment where dynamics is taken into account as well as sensors models and communication latency.