Trajectory Generation for Mobile Robots in a Dynamic Environment using Nonlinear Model Predictive Control

—This paper presents an approach to collision-free, long-range trajectory generation for a mobile robot in an industrial environment with static and dynamic obstacles. For the long range planning a visibility graph together with A* is used to ﬁnd a collision-free path with respect to the static obstacles. This path is used as a reference path to the trajectory planning algorithm that in addition handles dynamic obstacles while complying with the robot dynamics and constraints. A Nonlinear Model Predictive Control (NMPC) solver generates a collision-free trajectory by staying close the initial path but at the same time obeying all constraints. The NMPC problem is solved efﬁciently by leveraging the new numerical optimization method Proximal Averaged Newton for Optimal Control (PANOC). The algorithm was evaluated by simulation in various environments and successfully generated feasible trajectories spanning hundreds of meters in a tractable time frame.


I. INTRODUCTION
Mobile robots that autonomously navigate in an environment with static and dynamic obstacles has to solve two main problems, (i) they have to find a global path from the source to the destination, and (ii) they have to locally plan a trajectory that avoids collision with moving obstacles, e.g., humans and forklifts. There exists a large number of approaches to do motion planning for autonomous vehicles. An overview of different motion planning techniques for autonomous vehicles are presented in [1,2]. The problem is typically divided into a global route planner and a local trajectory planner. For the route planner the task is to select a path from the source to destination. In a factory setting this typically involves following preferred road networks and avoiding static obstacles. The route planning problem can be formulated as minimum-cost path problems defined over a graph where a weight on an edge describe the distance between the nodes that are connected by the edge. The nodes can originate from a discretization of the 2-D space or from geometric methods where obstacles are models as polyhedrals [3]. In both these cases shortest path problems can be solved using A* [4] or approximate, but faster, variants like Anytime Dynamic A* [5].
Global route planners are able to cover large distances in a computationally efficient manner, which makes them suitable to use for scales encountered in a factory. However, route planners do, typically not, consider robot kinematics and dynamics, nor do they handle dynamic obstacles.
Local trajectory planners are concerned with generating a collision-free path for a part of the path between the source and destination. These planners generally consider vehicle kinematics and dynamics and constraints such as velocity and acceleration limits, i.e., they yield feasible trajectories for a given robot. A local trajectory planning may also take moving obstacles into account by generating a trajectory that avoids a collision between the robot and the moving obstacle. There are many approaches to trajectory planning, for example sampling-based planners like RRT [6], or planners based on interpolating curves that computes a smooth path given a set of way-points, see e.g. [7]. However, to handle situations with moving obstacles it is advantageous to use an on-line approach that computes control actions for the robot based on the observed and predicted future states of the environment. Optimization-based trajectory planning algorithms, such as Model-Predictive Control (MPC), are very promising due to two reasons, (i) they can naturally encode constraints on velocity, acceleration, and jerk as well as distance constraints to obstacles inside its formulation, and (ii) the algorithms for solving optimization problems has improved considerably over the last years, this together with the increased performance of common processors has made it feasible to solve complex optimization problems in realtime.
The method Proximal Averaged Newton-type method for Optimal Control (PANOC) was proposed in [8] and shows computation times multiple orders faster than interior point (IP) and sequential quadratic programming (SQP) for many applications. In [9], different solvers were compared for non-linear MPC (NMPC) obstacle avoidance, showing the superiority of PANOC when it comes to robustness and solving time. Further, the PANOC solver has successfully been used for real-time NMPC with obstacle avoidance in applications such as [9,10].
In [10] a novel framework was introduced for modeling generic, possibly non-convex, shapes of obstacles by describing them as a set of non-linear inequalities. However, as described by [9], the framework has the risk of getting stuck in local minima. To avoid this, [9] combined the modeling approach with several heuristics such as using graph search to guide the solution out from local minima. Nevertheless, none of these approaches are suitable to find long-range trajectories.
In order to generate trajectories at larger scales, we propose an approach leveraging the global route planners optimality and computational efficiency with the a local trajectory based planner based on an NMPC controller solving several NMPC problems in an iterative manner. Specifically, A* is used to find the optimal path while NMPC follows this path closely and generates a feasible trajectory. To solve the NMPC problems iteratively in a computationally efficient manner, the PANOC solver is used. Our contribution is a new combination of global and local path planning techniques, yielding an algorithm that can generate collisionfree trajectories using NMPC for distances larger than a few meters while being able to handle arbitrary shapes of static obstacles as well as avoiding collision with dynamic obstacles.

A. Algorithm overview
In Fig. 1, a flowchart of the proposed algorithm is shown. The algorithm mainly consists of two parts. First, A* is used to find waypoints for the shortest path. Secondly, an NMPC solver is used iteratively to follow said waypoints and generate a smooth trajectory. The A* algorithm can consider arbitrary polygon shapes as static obstacles, which are translated to simpler constraints in the NMPC setting. The reasoning is that this simplifies the optimization problem and thus should improve the robustness and computation time of the NMPC solver.
As input, the algorithm is fed a set of static obstacle polygons P static , an outer boundary polygon B, initial state x init and a target state x end , an illustrative example is shown in Fig. 2. To account for A* considering the robot to be a point object, all polygons in P static are inflated by half the robot width together with some additional safety margin. Similarly, B is deflated by the same amount. The set of all polygons is then converted to a visibility graph where the set of vertices also includes the starting and target positions extracted from x init and x end respectively. The waypoints found when running the A* algorithm on the visibility graph then consist of start and target position, as well as polygon vertices that must be cornered, e.g. the upper left polygon vertex in Fig. 2. Before starting the NMPC solver, the waypoints are correlated to the non-padded polygon vertices and their coordinates are saved as o static = [x obs , y obs ] T in O static , e.g. in Fig. 2

this corresponds to
Further, a sequence L with line segments of equal length is extracted from the waypoints.
Once the initial processing is finished, the trajectory X is initiated as the initial state. Next, the NMPC solver is called in an iterative manner. Let N denote the horizon used in the NMPC and let n obs,static and n obs,dynamic be the maximum number of static and dynamic obstacles considered in each NMPC iteration. In each iteration, the latest state in the trajectory is set as the current state and based on this the N coming line segments from L are extracted as well as Simulate 1 timestep the n obs,static closest obstacle coordinates in O static . Further, the predicted location and shape of n obs,dynamic dynamical obstacles for the next N time steps are extracted. Given these inputs, the NMPC solver is used to find a sequence of feasible actionsû (0:N −1) for a given motion model. The corresponding states x should be close to the line segments, i.e. have a low cross-track error to the desired path, keep at least a distance of r to the selected n obs,static obstacle points in O static , keep a sufficient distance to the n obs,dynamic dynamical obstacles and respect vehicle dynamics and constraints. From the sequenceû (0:N −1) , the first action is used to calculate the new state to append to the trajectory X . If the last state is not close to the target state the next iteration is started, i.e. the current state is updated and new line segments and obstacle locations are fed to the NMPC solver. As described above and shown in Fig. 2, obstacles are modeled in different ways in the visibility graph and the NMPC formulation. In the visibility graph, obstacles are allowed to have any polygon shape. For the NMPC formulation, these are translated such that the trajectory must keep a certain distance of at least r to polygon vertices that are along the A* path, e.g. in Fig. 2 r is shown as the radius of the red circle. To ensure that the trajectory does not overlap with polygons that are not modeled in the NMPC, e.g. the boundary in Fig. 2, the cross-track error to the reference path must be kept sufficiently small. While not shown in the figure, dynamic obstacles are modelled as ellipses. This is due to the fact that their true locations in general are not known beforehand, and ellipses can be used to model their position as a Gaussian distribution.

B. NMPC Formulation
The dynamics in the NMPC formulation are modelled using a discrete motion model for a differential drive robot as in (1), where i denotes the current trajectory time step and T s denotes the step size. The states in the model are the 2D coordinates x, y, and the heading θ. The inputs to the system are velocity v and angular velocity ω. Acceleration is not included in the motion model but the change in the input is bounded in the coming NMPC constraints, and thus acceleration is not neglected. Note that the proposed algorithm is not dependent on this specific motion model but the differential drive is used since it is illustrative for the mobile robot use-case.
The NMPC-controller calculates an optimal control sequence in regard to a cost function and constraints for the predicted states and control actions over a horizon of length N. Predicted states and inputs within the NMPC controller are denoted asx andû respectively and use time index j, while the states and control actions that define the generated trajectory are denoted x and u respectively and use time index i. The selected cost function consists of multiple parts, where J cte is the cross-track error (CTE) cost. For a statex j and a set of line segments l A* , d(x i , l A* ) finds the minimum distance between the state and any of the line segments. J v is a cost for difference between selected velocityv j and the reference velocity v r,j at each time instance. Finally, J acc is the cost for change in input i.e acceleration. For coherence between iterations, this cost is also applied tô u 0 − u i−1 where u i−1 is the last applied input from the previous iteration. Conventionally a cost for the predicted terminal state is included to provide incentive for the robot move forward, however this cost has been omitted as that incentive is provided by other costs. Q cte , R v and R d are tuning parameters that can be modified for desired controller behaviour.
Dynamical obstacles are modeled as ellipses with five time dependent parameters namely the coordinates o x,j and o y,j , width o w,j , height o h,j and heading o α,j . The reason width and height are time dependent is to potentially model uncertainty in obstacle location. The robot is at time j not allowed to violate the area D j occupied by all ellipses at time j. Let O dynamic,j denote the dynamic obstacle states at time j, then D j can be written as, These cost, constraints from obstacles and dynamics make up the optimization problem in (4).
Here O l is a subset of all static obstacles O static , specifically, the n obs,static closest ones as described earlier. To ensure coherence, the first state of the NMPC solverx 0 is set to the latest state from the trajectory generated so far x i . Finally, the constraint on ∆û =û j −û j−1 is also applied toû 0 − u i−1 , again to ensure coherence between iterations.
The optimization problem in (4) is solved using the PANOC optimizer, this yields a sequence of control actions. Note, that we, in this work, assume that no disturbances are present. The first control action is supplied to the motion model and the robots movements are simulated one time step ahead. This process will be iterated until the final state is reached as is shown in the flowchart in Fig. 1.

III. EVALUATION
The proposed solution was evaluated via simulation and the algorithm was tested on multiple constellations of layouts and obstacle configurations to ensure robustness. However, only the test-case shown in Fig. 3 will be considered in this section. Note that the dynamical obstacles are not shown in this figure as their positions vary with time, but are displayed in Fig. 5 to 7.
Throughout the evaluation, a differential drive motion model, as described in (1) was used to simulate the movement of the mobile robot and the following values were selected as the tuning profile. The cost for velocity deviation from the reference speed R v was set to R v = 10.0, the cost of deviation from the global path Q cte was set to Q cte = 200 and the cost for linear and angular acceleration was set to R d = diag( 10.0 5.0 ) respectively. Furthermore, the length of the receding horizon was set to N = 20 and the algorithm operated with a time-step size of T s = 0.2. Lastly, the linear and angular constraints imposed on the vehicle was chosen to reflect that of a mobile robot, which is shown in (5). . example shown in Fig. 3. By inspecting the generated linear and angular velocity profiles, shown in Fig. 4, it is clear that the NMPC-controller decreases the linear velocity as it increases the angular velocity. In other words it slows down when taking sharp corners to avoid deviating too much from the reference path. Furthermore, from Fig. 3 and 4, it is evident that the proposed approach computes a smooth trajectory, even around very sharp corners, that satisfies the non-holonomic motion constraints of the differential drive robot while keeping the deviation from the global reference path small. The time-lapse in Fig. 5 displays a dynamic obstacle crossing the global path perpendicularly as the mobile robot is to traverse it. As the NMPC-controller has access to the future positions of said obstacle it can calculate that decreasing the velocity and allowing the obstacle to pass before proceeding would enable it to maintain a small distance to the global path. In Fig. 6 the mobile robot is blocked by a slow-moving obstacle travelling along the same path. Here, the NMPCcontroller can not simply wait for the obstacle to pass but rather it can reason about the cost of deviating from the global path versus the cost of remaining behind the slowly moving obstacle. Depending on the tuning profile this can lead to different behaviors, however, using the profile described above leads to the overtaking that can be seen in the time-lapse in Fig. 6.
Finally, in Fig. 7 the mobile robot is on course for a collision with an oncoming obstacle. Even in this seemingly challenging case, where the obstacle is approaching with a relatively large velocity, the NMPC-controller can compute a smooth and collision-free trajectory by deviating from the global path. Together, the scenarios shown in Fig. 5 to 7 display that the NPMC and algorithm can handle a range of different interaction scenarios with dynamic obstacles. It further illustrates that the algorithm is robust not only to different static obstacle configuration but also different types of dynamic obstacle interactions.
However, while the algorithm can handle a large variety of scenarios, one of which is shown in Fig. 3, there are certain cases where the NMPC solver generates trajectories that intersect the padded obstacles. In Fig. 8, one such case is depicted, where the path is heading north through a narrow corridor before turning 90 degrees into a different narrow corridor. For the NMPC solver, the only obstacle constraint is to keep a distance of at least 0.5 meters to the polygon corner marked with a red circle. Since the solver has no notion of other obstacles, there is nothing restricting it to stay within the drivable area. To stay close to the reference speed of 1.5 m/s, while respecting constraints on the angular velocity, the trajectory must deviate from the desired path. While the  Fig. 8. Scenario for which the NMPC solver generated a trajectory intersecting with a padded obstacle. However, the safety margin ensures that the robot keeps sufficient distance to the original obstacle. The trajectory is going from the bottom to the right side.

IV. CONCLUSION
In this paper we propose a novel combination of already established techniques to perform long-range trajectory generation. We leverage the optimality and computational efficiency of the A* algorithm to generate a reference path, consisting of linearly connected waypoints, from which we form a cross-track error loss in the NMPC objective function. The recent development in numerical optimization methods, namely PANOC, enables our approach to leverage the NMPC in an iterative manner while maintaining computation time sufficiently low. The iterative usage of the NMPC allows us to generate reference trajectories that span over longer distances than that of what previous solutions showcase.
To improve the robustness of the proposed approach, we suggest further investigations within three areas. Firstly, as mentioned earlier, in [10] a framework was presented on how to model general obstacle shapes in the NMPC formulation. It might be of interest to combine this type of obstacle modeling with our algorithm for cases where our original approach fails. However, one has to investigate how a more complex obstacle modeling affects the run-time. Secondly, the presented approach does not work for all possible motion models. There are situations where the A* algorithm will produce paths that cannot be followed closely by some motion models, e.g. one describing the motion of a car. More work has to be done to find ways to adjust our approach such that it can handle all common motion models.