An NMPC Approach using Convex Inner Approximations for Online Motion Planning with Guaranteed Collision Avoidance
Abstract
Even though mobile robots have been around for decades, trajectory optimization and continuous time collision avoidance remain subject of active research. Existing methods trade off between path quality, computational complexity, and kinodynamic feasibility. This work approaches the problem using a nonlinear model predictive control (NMPC) framework, that is based on a novel convex inner approximation of the collision avoidance constraint. The proposed Convex Inner ApprOximation (CIAO) method finds kinodynamically feasible and continuous time collision free trajectories, in few iterations, typically one. For a feasible initialization, the approach is guaranteed to find a feasible solution, i.e. it preserves feasibility. Our experimental evaluation shows that CIAO outperforms state of the art baselines in terms of planning efficiency and path quality. Experiments on a robot with 12 states show that it also scales to highdimensional systems. Furthermore realworld experiments demonstrate its capability of unifying trajectory optimization and tracking for safe motion planning in dynamic environments.
tabular \makesavenoteenvtable
I Introduction
Several existing mobile robotics applications (e.g. intralogistic and service robotics) require robots to operate in dynamic environments among other agents, such as humans or other autonomous systems. In these scenarios, the reactive avoidance of unforeseen dynamic obstacles is an important requirement. Combined with the objective of reaching optimal robot behavior, this poses a major challenge for motion planning and control and remains subject of active research.
Recently several researchers have tackled the obstacle avoidance problem by formulating and solving optimization problems [?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?]. This approach is well suited for finding locally optimal solutions, but generally gives no guarantee of finding the global optimum. Most methods therefore rely on the initialization by an asymptotically optimal samplingbased planner [?, 1, 3]. A shortcoming of most common trajectory optimization methods is that they are incapable of respecting kinodynamic constraints, e.g. bounds on the acceleration, and typically lack a notion of time in their predictions, [?, ?, ?, ?]. These approaches are typically limited to the optimization of paths rather than trajectories and impose constraints by introducing penalties.
The increase of computing power and the availability of fast numerical solvers, as discussed in [?], has given rise to model predictive control (MPC) based approaches, e.g. [?, ?, ?, ?, ?]. In this framework, an optimal control problem (OCP) is solved in every iteration. These methods succeed in finding kinodynamically [?, ?, ?] or kinematically [?, ?, ?] feasible trajectories, but typically use penalty terms in the cost function that offer no safety guarantees [?] or require that obstacles are given as a set of convex hulls [?].
Contribution
This work presents CIAO, a nonlinear (NMPC) based approach to realtime collision avoidance for single body robots. It preserves feasibility across iterations and uses a novel, convex formulation of the collision avoidance constraint that is compatible with many implementations of the distance function, even discrete ones like distance fields. To the best of authors’ knowledge, CIAO is the first realtime capable NMPC approach that guarantees continuous time collision free trajectories and is agnostic of the distance function’s implementation. The method’s efficacy is demonstrated and evaluated in simulation and realworld using robots with nonlinear, constrained dynamics and state of the art baselines.
Structure
The paper is structured as follows: The related work is discussed in Section II and Section III introduces the problem we want to solve. Section IV details CIAO alongside some considerations on feasibility, safety and practical challenges. In Section V we detail how CIAO can be used for trajectory optimization and receding horizon control (RHC). The experiments and results are discussed in Section VI. A summary and an outlook is given in Section VII.
Ii Related Work
Trajectory optimization methods try to find timeoptimal and collisionfree robot trajectories by formulating and solving an optimization problem [?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?]. Classical approaches to obstacle avoidance include [?, ?, ?, ?, ?]. These approaches do neither produce optimal trajectories, nor unify planning and control, nor account for complex robot dynamics.
A simple and effective method that is still used in practice, is the elasticband algorithm [?]. The computed paths, however, are generally nonsmooth, i.e. they are not guaranteed to satisfy kinodynamic constraints, nor does this algorithm compute a velocity profile. Like Zhu et al. [?], our approach aims to fix this shortcoming, while building up on the notion of (circular) free regions. Instead of optimizing velocity profile and path length separately, as done in [?, ?], we optimize them jointly utilizing an nonlinear (NMPC) setup. Also Rösmann et al. [?] provide an approach which combines elasticband with an optimization algorithm. Contrarily to ours, their approach does not enforce obstacle avoidance as constraint, and requires a further controller for trajectory tracking.
Nowadays optimization based methods receive increasingly more attention. Popular methods include CHOMP [?], TrajOpt [?], OBCA [?], and GuSTO [?], which have been shown to produce smooth trajectories efficiently. They typically use a simplified system model to compute a path from the current to the goal state (or set), and further need an additional controller to steer the system along the precomputed path. The proposed method, CIAO, is MPCbased and provides algorithms for both trajectory optimization and RHC, simultaneously controlling the robot and optimizing its trajectory.
Also Neunert et al. [?] propose RHC to unify trajectory optimization and tracking, but their approach does not include a strategy for obstacle avoidance. They propose to solve an unconstrained nonlinear program (NLP) online, CIAO on the other hand is solving a constrained NLP online that enforces collision avoidance as constraint.
Frasch et al. [?] propose an MPC with boxconstraints to model obstacles and road boundaries. Liniger et al. [?] handle obstacles in a similar way, but apply contouring control, i.e. the approach steers a race car in a corridor around a predefined path. These frameworks do not consider arbitrarily placed obstacles, particularly no moving obstacles, which makes them unsuitable for many applications. In our approach we handle more complex obstacle definitions, modeled according to a generic nonlinear and nonconvex distance function. To this end, we propose a novel constraint formulation that is shown to be a convex inner approximation of the actual collision avoidance constraint.
Herbert et al. [?] propose a hybrid approach to safely avoid dynamic obstacles. The trajectory tracker does not consider the obstacles explicitly, but relies on the planning layer. The method proposed in this work presents a unified approach for all robot motion planing, control, and obstacle avoidance, using constrained NMPC.
The recently proposed method GuSTO [?] uses sequential convex programming (SCP), like other common algorithms, e.g. [?, ?, ?, ?]. SCP requires a full convexification of the originally nonlinear and nonconvex trajectory optimization problem. This is accomplished by linearizing the system model and incorporating paths constraints, including collision avoidance, as penalties in the objective function. In general these approximations may lead to infeasible, i.e. colliding or kinodynamically intractable trajectories. Typically several SCP iterations are required to find a feasible solution. CIAO, on the other hand, solves partially convexified NLPs, using a convex inner approximation of the collision avoidance constraint, and finds feasible solutions in less iterations, typically one. The individual iterations are computationally cheaper and feasibility is preserved. Since the dynamical model is accounted for by the NLPsolver, linearization errors are minimized.
Finally CIAO can be considered as a trust region method [?], where the nonlinear state constraints are approximated with a convex inner approximation.
Iii Problem Formulation
We want to find a kinodynamically feasible, collision free trajectory by formulating and solving a constrained OCP. Kinodynamic feasibility is ensured by using a dynamical model to simulate the robot’s behavior and collision avoidance is achieved constraining the robot to positions with a minimum distance to all obstacles.
The occupied set is defined as the set of points in the robot’s workspace that are occupied by obstacles. The Euclidean distance to the closest obstacle for any point is given by the distance function :
(1) 
For the sake of simplicity we assume that the robot’s shape is contained in an dimensional sphere with radius and center point . Collision avoidance can now be achieved by requesting that the distance function at the robot’s position is at least the minimum distance :
(2) 
which is equivalent to .
We assume to be fixed from now on, to ensure that points in the occupied set do not satisfy (2). A point that satisfies (2) is called ’free’. Further we define the safety margin as the area for which holds.
We can now formulate an OCP that enforces collision avoidance as path constraint: {mini} x (⋅), u (⋅)∫_0^T l( x (t), u (t), r(t)) dt + l_T( x (T), r(T)) \addConstraint x (0)= ¯ x _0 \addConstraint x (T)∈X_T \addConstraint˙x (t)= f( x (t), u (t)), t ∈[0, T] \addConstrainth( x (t), u (t))≤0, t ∈[0, T] \addConstraintd_ O ( p (t))≥ d , t ∈[0, T], where denotes the robot’s state, is the vector of controls, provides reference states and controls, is the length of the horizon in seconds, the function denotes the cost at time point and the terminal cost, is robot’s current state, and is the set of admissible terminal states. We use the common shorthand to denote the derivative with respect to time, i.e. . The function models the robot’s dynamics and implements a set of path constraints, e.g. physical limitations of the system, and denotes the robot’s position with a selector matrix chosen accordingly.
Iv Convex Inner Approximation (Ciao)
In this section we describe how we solve the OCP presented in (III) by adopting a convex inner approximation of the actual collision avoidance constraint presented in Sec. III.
First we discretize (III) using a direct multiple shooting scheme as proposed by [?]. The resulting NLP is a function of , , and the sampling time . Using the shorthand for cleaner notation, we discretize (III) as: {mini!} w J( w , r ) \addConstraint x _0  ¯ x _0=0 \addConstraint x _N∈X_T \addConstraint x _k+1  F( x _k, u _k; Δt)=0, k=0,…,N1 \addConstrainth( x _k, u _k)≤0, k=0,…,N \addConstraintd_ O ( p _k)≥ d , k=0,…,N, where is a vector of optimization variables that contains the stacked controls and states for all steps in the horizon, similarly contains the reference states and controls, is the discretized objective, with stage cost and terminal cost , and models the discretized robot dynamics. We denote the feasible set of (IV) by .
Iva Free Balls: A Convex Inner Approximation of the Obstacle Avoidance Constraint
The actual obstacle avoidance constraint formulated in Eq. (2) is generally nonconvex and nonlinear, which makes it illsuited for rapid optimization. We propose a convex inner approximation of the constraint that is based on the notion of free balls (FBs), as proposed in [?] and extended by [?]. For cleaner notation we first define the free set.
Definition 1
Let be the occupied set and be the minimum distance, then the free set is defined as
Remark: This definition implies that the free set and the occupied set are disjunct, i.e. .
We can now formulate an obstacle avoidance constraint by enforcing that the robot’s position lies within an dimensional ball formed around as shown in Fig. 2.
Definition 2
For an arbitrary free point we define the free ball as
We will now show that a free ball is a convex subset of the free set.
Lemma 1
Let be a free point, then the free ball is a convex subset of , i.e. .
We will prove this lemma in two steps. First, we observe that the free ball is a norm ball and therefore convex. Second, we show by contradiction that .
Suppose and such that . We now apply the triangle inequality and obtain , see Fig. 1(b). Using the distance function’s definition we get . Reordering yields , which shows that .
Based on Lem. 1 and Def. 2 we can approximate the collision avoidance constraint by This formulation is not differentiable in and might pose a problem for gradient based solvers. To prevent this case and linear independence constraint qualification (LICQ) violations at the only feasible point we assume This implies that both sides are grater , such that we can square both sides and get
(3) 
With the constraint formulated in (3) and assuming that LICQ holds for all free ball center points with , we can partially convexify the NLP (IV). We obtain the CIAONLP, which like (IV) depends on , , , and additionally the tuple of center points : {mini!}[0] w , s J( w , r ) + ∑_k=0^N μ_k ⋅s_k \addConstraint x _0 ¯ x _0=0 \addConstraint x _N ∈X_T \addConstraint x _k+1 F( x _k, u _k; Δt)=0, k=0,…,N1 \addConstrainth( x _k, u _k) ≤0, k=0,…,N \addConstraint∥ p _k  c _k ∥^2_2 ≤(d_ O ( c _k)  d _k)^2 + s_k, k=0,…,N \addConstraints_k ≥0, k=0,…,N. This reformulation of the actual NLP (IV) is called Convex Inner ApprOximation (CIAO). For numerical stability we include slack variables that are penalized. A point is only considered admissible if all slacks are zero, i.e. in a vector sense. To ensure that the slacks are only active for problems, that would be infeasible otherwise, the multipliers have to be chosen sufficiently large, i.e. for . The feasible set for optimization variables of this NLP depends on and is denoted as , recall that and .
Note that enters the NLP as a constant ( is a parameter not an optimization variable). Thereby CIAO is compatible any implementation of the distance function, even discrete ones.
Note that for a convex objective , a convex terminal set , affine dynamics , and convex path constraints , the CIAONLP (IVA) is convex. Further note that for a linearquadratic objective , affinequadratic path constraints , affine dynamics , and a terminal set that can be written as either (i) an affine equality or (ii) an affinequadratic inequality constraint, CIAONLP (IVA) is a quadratically constrained quadratic program (QCQP). If is also convex, it is a convex QCQP.
Lemma 2
IvB The Ciaoiteration
We will now introduce the CIAOiteration, as detailed in Alg. 1. It takes a two step approach that first formulates the CIAONLP (IVA) by finding a tuple of center points before solving it.
In Line 2 we find an initial tuple of center points . In practice the free balls resulting from these center points are very small and therefore very restrictive, which leaves little room for optimization, especially if the initial guess approaches obstacles closely. To overcome this problem we maximize free balls (FBs) (Line 3) by solving the following optimization problem for each and obtain an optimized center point :
(4) 
where is a given initial point, is the search direction with and is the step size. It yields a maximized free ball with radius and center point for each . The optimized center points are collected in the tuple . To ensure convergence of Alg.1 we require that the distance function is bounded, i.e. such that .
We will now show that the optimization problem (4) preserves feasibility of the initial guess by showing that includes , i.e. .
Lemma 3
For , and , holds with .
We will prove this by contradiction, assuming s.t. . Using Def. 2 we can rewrite this as . Applying the triangle inequality on the left side yields and based on our assumption holds. Inserting this gives and thus contradicts the condition .
To solve the line search problem (4) we propose to use the distance function’s normalized gradient as search direction. Starting from the step size is exponentially increased until a step size is found for which the constraint is violated. The optimal step size can now be found using the bisection method. solveNLP uses a suitable solver to solve (IVA), e.g. Ipopt [?], and computes a new trajectory (Line 4).
Lemma 4
For a feasible initial guess Alg. 1 finds a feasible point with .
IvC Continuous Time Collision Avoidance for Systems with Bounded Acceleration
The constraints formulated in (3) can be extended to the continuous time case. Using Lem. 1 we can write the continuous time collision avoidance constraint as
Assuming a double integrator model of the form with the shorthand for all and it can be written as
Using the triangle inequality we get
With and assuming that system’s total acceleration is bounded , which is a reasonable assumption for most physical systems, yields
We assume that velocities are bounded in all discretization points, i.e., for . Considering that and lie outside of the prediction horizon and that for , it is sufficient to consider the interval in each time step and get
IvD Rotational Invariance Trick
Modeling a robot’s kinematics and dynamics using Eulerangles and continuous variables for the orientation is intuitive, but leads to ambiguities and possibly singularities, e.g. the gimbal lock. Unit quaternions are widespread approach to circumvent these problems for 3D rotations. For the 2D case, however, they pose an avoidable overhead. In this case we use Eulerangles and continuous orientation variables for computational efficiency and the ’rotational invariance trick’ to compensate for the ambiguity. We measure the distance between of two orientations by
(6) 
Note that . We thereby avoid unnecessary rotations of the robot, which is relevant for example if the robot drives in a circle.
IvE Choosing the Objective Function
We use a quadratic cost function for reference tracking with regularization:
where leads to exponentially increasing stage cost and reduces oscillating behavior around the goal, denote reference state and controls at stage , augments the state by applying proper transformations where applicable, , and are positive definite matrices.
V CiaoBased Motion Planning
In this section we propose and detail two algorithms, one for pure trajectory optimization (Alg. 2) and the other for simultaneous trajectory optimization and tracking (Alg. 3).
Va Ciao for Trajectory Optimization
The proposed trajectory optimization algorithm (see Alg. 2) starts by computing a feasible initial guess and a reference trajectory (Lines 1–2).
In the general case of nonconvex scenarios, such as cluttered environments, feasible initializations can be obtained through a samplingbased motion planner [2, 4]. To monitor the progress the initial guess is copied (Line 4), before using it as initial guess for the CIAOiteration (Line 5). Lines 4–5 are repeated as long as the costfunction shows an improvement that exceeds a given threshold (Line 6). Finally the best known solution is returned (Line 7). For trajectory optimization the terminal constraint in (IVA) becomes an equality constraint, which enforces that the goal state is reached at the end of the horizon, i.e. .
VB CiaoNmpc
While Alg. 2 iteratively improves a trajectory that connects and , Alg. 3 uses a shorter, receding horizon. This can be considered the realtime iteration (RTI) version of Alg. 2. Therefore the trajectory computed by initialGuess (Line 1) is not required to reach the goal state . In many cases it is sufficient to choose , where is chosen, such that the robot remains in the current state .
While the robot has not reached the goal region (Line 2), it is iteratively steered to it (Lines 3–8). Each iteration starts by updating the robot’s current state . Based on the complexity of the scenario referenceTrajectory may return a guiding trajectory to the goal or just the goal state itself (Line 4). We run Alg. 1 to compute a new trajectory (Line 5), before sending the first control to the robot (Line 6). shiftTrajectory moves the horizon one step forward (Line 7).
To ensure recursive feasibility, which implies collision avoidance, the terminal constraint (IVA) is commonly chosen such that the robot comes to a full stop at the end of the horizon, i.e. , where is the matrix that selects the velocities from the state vector.
Vi Experiments and Discussion
To evaluate CIAO in terms of planning efficiency and final trajectory quality, we compare it against a set of baselines. We challenge CIAO by using nonlinear dynamics and a nonconvex cost function. Further we use a sampling based motion planner to initialize it with a collision free path that does not satisfy the robot’s dynamics. In this case a NLPsolver is required to solve the CIAONLP (IVA). We use the primaldual interior point solver Ipopt [?] with the linear solver MA27 [?] called through CasADi [?] (version 3.4.5).
For the evaluation, we consider three types of experiments:

numerical experiments to investigate the behavior of the free ball constraint against competing formulations;

a trajectory optimization benchmark to evaluate the quality of trajectories found by CIAO;

realworld experiments where CIAO is qualitatively compared to a state of the art baseline.
Via Comparison of constraint formulations
In a first set of experiments, the numerical performance of the free ball constraint formulation, as derived in Sec. IVA, is compared to common alternatives: the actual constraint as defined in Eq. (2) (actual), a linearization of the actual constraint (linear), and a logbarrier formulation (logbarrier). They differ only in the way the obstacle avoidance constraint (IV) is formulated. Our findings are reported in Tab. 2.
actual  linear  CIAO  logbarrier  
ms / iteration  2.00  0.72  0.70  2.23 
ms / step  40.78  13.13  17.26  50.34 
iterations / step  20.35  18.23  24.64  22.57 
time to goal [s]  14.35  14.39  18.67  (13.46) 
path length [m]  10.22  10.33  10.58  (9.25) 
max ms / step  448.94  230.07  179.26  
% timeouts  0  0  0  11.3 
The average computation time taken per MPCstep and per Ipopt iteration are given as ‘ms / step’ and ‘ms / iteration’ respectively, ‘iters / step’ are the average Ipopt iterations per MPCstep. The path quality is evaluated in terms of ‘time to goal‘ and ‘path length‘. Averages in the first five rows are taken over scenarios, for which the maximum CPU time of was not exceeded. The percentage of runs that exceed the CPU time is given by ‘% timeouts’. The maximum CPU time taken for a single MPCstep is given by ‘max ms / step’.
We observe that the actual constraint is producing both fastest and shortest paths. This path quality comes at comparatively high computational cost. Linearizing the actual constraint reduces the computational effort, while maintaining a high path quality. In contrast to CIAO, linearization is not an inner approximation and can lead to constraint violations that necessitate computationally expensive recovery iterations. This increases the overall computation time significantly and leads to a higher maximal computation time. At the cost of lower path quality, but a similar average computation time, CIAO overcomes this problem by preserving feasibility. This leads to a lower variance in the computation time, and allows for continuous time collision avoidance guarantees. A further advantage, which is relevant in practice, is that CIAO generalizes to not continuously differentiable distance function implementations, e.g. distance fields.
Including the collision avoidance constraint as a barrier term in the objective, i.e. by adding to the stage cost , is an alternative approach to enforce collision freedom. Our results suggest, however, that for our application it is least favorable among the considered options.
ViB Trajectory Optimization Benchmark
In a second set of experiments CIAO is compared to GuSTO [?] using the implementation publicly provided by the authors. In these experiments we consider a freeflying Astrobee Robot with 12 states and 6 controls, that has to be rotated and traversed from a start position on the bottom front left corner of a cube to a goal in the opposite corner. The room between start and goal point is cluttered with randomly placed static obstacles of varying sizes (between and meters). Fig. 1 shows some examples.
The results reported in Tab. II and Fig. 4 were performed in Julia on a MacBook Pro with an Intel Core i78559U clocked at . The SCPs formulated by GuSTO [?] are solved with Gurobi [?]. Both algorithms are provided with the same initial guess, which is computed based on a path found with RRT [?]. We used a horizon of equally split into steps, resulting in a sampling time of .
measure  CIAO  GuSTO 

Compute 
14.79211.966  131.367130.743 
Iterations  30.66015.886  4.5201.282 
Compute / Iteration [s]  0.4750.230  27.72922.096 
Linearization Error  4.66e143.67e15  4.10e061.28e06 
Since both GuSTO and CIAO use tailored cost functions we evaluate the computed trajectories using a common cost function , which is based on the state distance metric proposed by [?]: , with goal state and all weights of the distance metric chosen equal. The controls are evaluated separately and reported as control effort given by . The path quality is evaluated in terms of time to goal, path length, and clearance (minimum distance to the closest obstacle along the trajectory). The first two measures take the time and path length until the state distance metric falls below a threshold of , while the latter is evaluated on the entire trajectory. These three metrics are evaluated on an oversampled trajectory using a sampling time .
The results in Fig. 4 show that CIAO finds faster trajectories than GuSTO and thereby also achieves significantly lower cost. As depicted in Fig. 1 it maintains a larger distance to obstacles for higher speeds. This behavior allows for a higher average speed, at the cost of a higher control activation and slightly longer paths in comparison to GuSTO.
In our experiments both CIAO and GuSTO find solutions to all considered scenarios. As reported in Tab. II, CIAO (Alg. 2) requires more iterations to converge, but the individual iterations are cheaper. Moreover CIAO obtains a feasible trajectory after the first iteration and therefore could be terminated early, while GuSTO does not have this property and takes several iterations to find a feasible trajectory. Even though the dynamics are mostly linear we observe linearization errors for GuSTO, originating from the linear model they use.
In summary CIAO finds trajectories of higher quality than GuSTO at lower computational effort.
ViC RealWorld Experiments  Differential Drive Robot


To qualitatively assess the behavior of CIAONMPC (Alg. 3), it was tested in dynamic realworld scenarios with freely moving humans. A representative example is depicted in Fig. 5. Note that CIAO has no knowledge of the humans’ future movements. It is instead considering all humans as static obstacles in their current position.
As in Sec. VIA a differential drive robot is used, this time with a horizon of and a control frequency of resulting in a total of optimization variables (including slacks). For these experiments CIAO was implemented as a C++ ROSmodule, the distance function was realized as distance field based on the code by [?]. Initial guesses and reference paths were computed using an A* algorithm [?].
Since GuSTO is not suitable for receding horizon control (RHC), we used an extended version of the elasticband (EB) method [?]. To obtain comparable results, we used the same A* planner and localization method with both algorithms.
In Fig. 5, it can be seen that the free balls (FBs) (transparent circles) keep to the center of the canyonlike free space. The predicted trajectory (blue line) is deformed to stay inside the FBs. This is a predictive adaptation to the changed environment. For the shown, representative example in Fig. 5 the robot passed smoothly the group. Comparable scenarios were solved similarly by EB.
We observed that groups of people pose a particular challenge that could, however, be solved by both approaches. We note that the robot is moving a bit faster in proximity to people for EB, while CIAO adjusts to blocked paths a bit faster. The most significant difference between the methods is that CIAO combines rotation and backward/forward motion, while the EB rotates the robot on the spot. Both methods succeeded in steering the robot through the group safely, without a single collision.
Fig. 6 shows representative computation times obtained in simulation on a set of 14 scenarios involving nondeterministically moving virtual humans. The reported computation time accounts only for solving the CIAONLP and function evaluations in CasADi, the processing time required by preprocessing steps and other components is not included. High computation times originate from farfromoptimal initializations occurring when a new goal is set, i.e. around time , or if humans cross the planned path.
In summary CIAO and the elastic band (EB) approach show similar behavior. In contrast to EB, CIAO computes kinodynamically feasible and guaranteed continuous time collision free trajectories. Further it has a notion of time for the planned motion, such that predictions for dynamic environments can be incorporated in future work.
Vii Conclusions and Future Work
This work proposes CIAO, a new framework for trajectory optimization, that is based on a novel constraint formulation, that allows for NMPC based collision avoidance in realtime. We show that it reaches or exceeds state of the art performance in trajectory optimization at significantly lower computational effort, scales to high dimensional systems, and that it can be used for RHC style MPC of mobile robots in dynamic environments.
Future research will focus on extending CIAO to full body collision checking, guaranteed obstacle avoidance in dynamic environments, time optimal motion planning, and multi body robots. A second focus will lie on efficient numerical methods that exploit the structure and properties of the CIAONLP stated above.
Footnotes
 These experiments were conducted in simulation considering a robot with differential drive dynamics (5 states, 2 controls) and a prediction horizon of 50 steps, resulting in a total of 405 optimization variables.
in Table 2: not representative because complex scenarios with long transitions failed.  These timings are only indicative due to differences in implementation, a similar trend is confirmed in Fig. 6.
References
 (2014) A novel RRT extend function for efficient and smooth mobile robot motion planning. In 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 205–211. Cited by: §I.
 (2015) Distance metric learning for RRTbased motion planning with constanttime inference. In 2015 IEEE International Conference on Robotics and Automation (ICRA), pp. 637–643. Cited by: §VA.
 (2016) RRTbased nonholonomic motion planning using anyangle path biasing. In 2016 IEEE International Conference on Robotics and Automation (ICRA), pp. 2775–2781. Cited by: §I.
 (2017) Kinodynamic motion planning on gaussian mixture fields. In 2017 IEEE International Conference on Robotics and Automation (ICRA), pp. 6176–6181. Cited by: §VA.