{'fraction_non_alphanumeric': 0.03622210223182146, 'fraction_numerical': 0.009719222462203024, 'mean_word_length': 4.717777206071521, 'pattern_counts': {'":': 0, '<': 0, '<?xml version=': 0, '>': 0, 'https://': 1, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 5, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
Least Conservative Linearized Constraint Formulation for Real-Time Motion Generation

Today robotics has shown many successful strategies to solve several navigation problems. However, moving into a dynamic environment is still a challenging task. This paper presents a novel method for motion generation in dynamic environments based on real-time nonlinear model predictive control (NMPC). At the core of our approach is a least conservative linearized constraint formulation built upon the real-time iteration (RTI) scheme with GaussNewton Hessian approximation. We demonstrate that the proposed constraint formulation is less conservative for planners based on Newton-type method than for those based on a fully converged NMPC method. Additionally, we show the performance of our approach in simulation, in a scenario where the Crazyflie nanoquadcopter avoids balls and reaches its desired goal in spite of the uncertainty about when the balls will be thrown. The numerical results validate our theoretical findings and illustrate the computational efficiency of the proposed scheme.


INTRODUCTION
Autonomous navigation in dynamic environments still poses an important challenge for robotics research. In contrast to static scenarios, where global path planning strategies are well-suited, in dynamic environments the decision about motion must be based on the online perception of the world and on the fast reaction-time behavior. Traditional motion planning methods typically rely on graphsearch methods (Dijkstra, 1959;Hart et al., 1968), combinatorial methods (Bhattacharya and Gavrilova, 2008), or sampling-based methods (LaValle, 2006;Kuffner and LaValle, 2000). Despite the effectiveness of those algorithms in finding a path between two given points, the quality of the path obtained may be far from optimal. Thus, the research to improve the path refinement led to a common subdivision of the problem into a global and local planner.
The local planner accounts for the dynamic constraints and generates sets of feasible local trajectories. Popular algorithms used in this layer are dynamic window approach (DWA) (Brock and Khatib, 1999) and timed elastic band (TEB) (Quinlan and Khatib, 1993). More recent motion planning methods focus, instead, on optimization-based strategies. Benjamin et al. (2019) develop the concept of linear interval programming functions, which is based on multi-objective optimization. In this case, the collision avoidance constraint is defined as a shrinking convex polygon around the obstacle. As the optimization problem seeks the set of so-called Pareto optimal solutions -which is rather demanding -its application in systems with tight runtime requirements is usually not possible. Another approach is risk-constrained model predictive control (Hakobyan et al., 2019), where expectation constraints are defined within a linearly constrained mixed-integer convex program. However, the efficiency of this method highly depends on the tightness of the continuous linear program relaxations. Other techniques involve manifold trajectory optimization (Watterson et al., 2020) and convex decomposition through interval allocation (Tordesillas et al., 2019), both exploiting the inflated ellipsoids concept for ensuring collision-free trajectories. These approaches have obtained promising results but the solutions achieved for local replanning often lead to optimal trajectories that are far too conservative.
Nonlinear model predictive control (NMPC) is the cornerstone of advanced process control and has gained increasing attention in the robotics community in recent years. It is a feedback control strategy based on the solution of finite-horizon optimal control problems (OCP) where nonlinear dynamics and constraints are taken into account. A great challenge associated with the employment of NMPC-based trajectory optimizers as local planners has been the availability of fast and reliable algorithms for online implementations. The advances in optimization strategies have enabled the development of a mature class of optimization algorithms such as sequential quadratic programming (SQP) and interior point (IP) methods. However, the real-time dilemma lies in the fact that while the SQP/IP iterations are being performed, the physical system evolves, and the information used to compute the state estimate becomes outdated (Gros et al., 2020). To tackle this issue, the real-time iteration (RTI) scheme is proposed in Diehl et al. (2005) which trades speed for accuracy while incorporating "globalization features" (Sathya et al., 2018). This paper presents a novel approach for real-time motion generation for robotic systems. More precisely, we exploit the algorithmic ideas of the RTI scheme with inexact Hessian to formulate a least conservative linearized collision avoidance constraint. We provide a theoretical analysis proving that the proposed constraint formulation is less conservative for planners based on Newton-type method than for those based on a fully converged NMPC approach. This analysis is further leveraged to overcome the numerical difficulties associated with the (local) feasibility of the optimization problem. Our framework uses the implementation of the RTI strategy by means of the high-performance software package acados (Verschueren et al., 2019). The quadratic program (QP) solver is HPIPM, which relies on the hardware-tailored linear algebra library BLASFEO . Furthermore, we use the Crazyflie nanoquadcopter as a relevant example in order to assess the performance and implementability of our realtime planner.
With respect to the challenges at hand, the main contributions of our work are twofold: • An NMPC-based algorithmic framework for real-time motion generation that is reliable and guarantees less conservative optimal trajectories for robotic systems.
• A numerical validation that shows the efficiency of the proposed approach.
This paper is outlined as follows: in Section 2 preliminary concepts on NMPC and the RTI scheme are presented. In Section 3, the proposed real-time planner is introduced. The general planner problem formulation is tailored to the Crazyflie nanoquadcopter example, and the generation of the global trajectories are described in Section 4. Section 5 reports the results of the closed-loop simulations and the discussion of our approach. In Section 6, the conclusions are summarized.

Nonlinear Model Predictive Control
NMPC is an optimization-based control technique that has gradually become known in the robotics community over the past few years. In order to use NMPC to control a system, one has to solve a nonlinear nonconvex optimal control problem (OCP). Throughout this paper, let us consider the following general NMPC tracking formulation as the OCP that needs to be solved numerically: min ξ0,...,ξ N , u0,...,u N −1 where ξ ∈ R n ξ and u ∈ R nu denote the state and input trajectories of the discrete-time system whose dynamics are described by F : R n ξ × R nu → R n ξ . The functions in the Lagrange and Mayer least-squares terms are denoted by η : R n ξ × R nu → R and η N : R n ξ → R, and will be weighted by the symmetric positive-definite matrices W and W N , respectively. Variablesη : R n ξ × R nu → R and η N : R n ξ → R denote the time-varying references. The functions g : R n ξ × R nu − → R nc and g N : R n ξ − → R nc, N define the path and terminal constraints. Finally, N and ξ 0 denote the horizon length and the current state of the system, respectively.
Problem (1) can be solved by a state-of-the-art optimization algorithm. Among others, two main classes of approaches stand out: SQP and IP methods. In our work, we use a variant of SQP for NMPC embedded applications, the RTI scheme. An explanation of the algorithm's main idea is presented in the next subsection.

The Real-Time Iteration Scheme
In real-time NMPC applications, the nonlinear program (NLP) (1) needs to be solved at every sampling instant under the available computational time. To that end, Diehl et al. (2005) propose an efficient Newton-type scheme that allows us to approximately solve the OCPs during the runtime of the real process, the so-called RTI scheme. In this strategy, only a single linearization and dense QP solve are carried out per sampling instant, leading to an approximate feedback control policy. Our approach uses the implementation of the RTI scheme by means of the high-performance software package acados. Thus, after linearization, the obtained QP for problem (1) used in acados reads as follows: The linearized dynamics and inequality constraints use the following matrices: where the superscript k denotes the linearization point for the states and inputs obtained at the previous QP iteration. The quadratic cost uses the exact gradients q i , r i , q N , the Gauss-Newton Hessian approximation H i , both as defined in . The Hessian and the gradient with respect to the slack variables s i are denoted by P i and p i , respectively. In acados, the slack variables are introduced and associated with the soft constraints on the outputs.

LEAST CONSERVATIVE REAL-TIME PLANNER
In this section, we detail and discuss the reformulations and parametrizations of QP (2) that are used in this paper.

Collision Avoidance Constraint and Physical Limits
To ensure kinodynamically feasible and collision-free trajectories, we consider a constrained OCP. Having the dynamic model of the robot as a constraint provides kinodynamically feasible state trajectories. Collision avoidance, on the other hand, is granted by constraining the robot's distance with respect to an obstacle o ∈ R n in the workspace W ∈ R n to be larger than the clearance d C ∈ R + . The obstacles, here considered dynamic, are described as point-mass models. Furthermore, we assume a single-body robot, which can be well approximated by a point if one defines the clearance so as take into account: i) the actual size of the robot, ii) the actual size of the obstacle, iii) a safety margin between them. When extending our approach to a multi-body robot system, one needs to consider the enlargement of the clearance. Ultimately, in our method, for each obstacle present in the workspace, an additional collision avoidance constraint must be included in the OCP.
To formulate the collision avoidance constraint, we will consider the distance function to be the Euclidean distance between the robot and an obstacle in the workspace d O : W − → R. It can be formally defined as the following: Definition 1. (Distance Function). Let us define the distance function for an arbitrary point p ∈ W with respect to an obstacle as Based on the aforementioned definition, the obstacle avoidance constraint can be concisely written as (3) Note that (3) is not differentiable at p = o because we can get different limits by choosing different paths to the origin. Hence, to tackle this problem, one can square both sides of the inequality, and recover the desirable property of being continuously differentiable everywhere. However, depending on the optimization algorithm behind the planner at hand, one constraint formulation can be more advantageous than the other.
For a planner whose optimization algorithm is based on a Newton-type method, e.g. with acados, note that only one system linearization and one QP (2) are performed per sampling time. Each QP corresponds to a linear approximation of the original NLP along a time-varying trajectory. In this configuration, squaring the Euclidean norm, as in (4), provides local trajectories that are overconservative. In a case where the linearization points are far away from the boundaries of the linearized feasible set of the original NLP, the use of (3) leads to linear constraints that are "least" conservative. We will formalize this statement in Proposition 2. In contrast, for a planner whose optimization algorithm is based on a fully converged NMPC method, e.g. with IPOPT (Wächter and Biegler, 2006), in general, no constraint formulation has a distinct advantage over the other. As in this case, the iterations continue until convergence or a certain termination conditions are reached, the feasible sets for (3) and (4)  Proof. Define c 1,lin (p, p) := c 1 (p) + ∂c1 ∂p (p −p) and c 2,lin (p, p) := c 2 (p) + ∂c2 ∂p (p −p). Due to convexity of c 1 (p) and c 2 (p), we have that, for anyp we have that 0 ≤ c 1,lin (p, p) ≤ c 1 (p) and 0 ≤ c 2,lin (p, p) ≤ c 2 (p) such that p ∈ H 1 (p) =⇒ p ∈ S and p ∈ H 2 (p) =⇒ p ∈ S, which proves the first statement. In order to prove that H 2 (p) ⊆ H 1 (p), we proceed as follows. Define the auxiliary functionsĉ 2 ,ĉ 1 : . Due to convexity in v ofĉ 1,lin and affinity in v ofĉ 2,lin we have that, for any d, and anyv, the following holds: min This last inequality shows that H 2 (p) ⊆ H 1 (p) concluding the proof.
The boundaries of the linearized feasible set, considering three distinct regular points p , for constraint formulations (3) and (4) can be seen in Fig. 1. We notice that the linearized constraint formulation defined by (3) provides a solution that is closer to the lower bound d C .
Moreover, in order to account for the limits on the actuator, we additionally impose box constraints on the control input of the form U := {u ∈ R nu : u min ≤ u ≤ u max }.

Constraints Violation
The constraint presented in (3) guarantees collision avoidance instantaneously since its evaluation occurs only at t i and t i+1 . This, however, does not ensure that there is no collision in-between shooting nodes, which may result in constraint violation. As a result, the predicted trajectory between time points may be seen penetrating the obstacle.
This issue can be addressed by expanding the obstacle, in all directions, by the maximum incursion distance (Kuwata, 2003), or by using the hyperplane separation theorem as in (Brossette and Wieber, 2017). In this paper, we do not use any special method to ensure collisionfree trajectories in-between successive positions of the robot, as it comes with a higher computational burden. Therefore, we rely on a reasonable choice for the size of the discretization time step.

Feasibility and Soft Constraints
One major drawback regarding the use of hard constraints is that they may render the optimization problem infeasible: this is especially true in the case of state constraints (Frison and Jørgensen, 2015), such as the one in (3). In a case where the input constraints represent a physical limitation, they have to be enforced all the time. State constraints, instead, are only often desired -although are not always physically necessary -and hence should be satisfied whenever possible.
A more systematic approach for dealing with infeasibility is to modify the cost function to include a penalization on the violation of the collision avoidance constraint. This is usually achieved by introducing slack variables associated with the soft constraint, and heavily penalizing them. The optimization algorithm searches for a solution that minimizes the original cost function while keeping the slack variables equal to zero whenever possible. Scokaert and Rawlings (1999) discuss the inclusion of a 1 -norm µ|| || 1 penalization term of the slack in order to obtain an exact penalty function -i.e. the controller violates the constraints only when it is necessary. A hazard with 1 penalty functions is their inherent non-smoothness at optimal points which may degrade the final solution accuracy. On the other hand, the 2 2 -norm µ|| || 2 2 benefits of being smooth and having "simple" derivatives. A major drawback to the 2 2 penalty function is the uneven way that it penalizes constraints, such that µ has to be very large to enforce asymptotic feasibility (Griffin and Kolda, 2010). In this work, the use of a 1 -norm is preferred as a means to obtain an exact penalty function.

System Model
Let {B} be the body-fixed frame located at the center of mass (CoM) of the nanoquadcopter which is aligned with the North-West-Up (NWU) inertial frame {I}.
Given a nonlinear dynamic model for a quadcopter in the form ofξ = f (ξ, u), let us then define a state vector such as ξ := (p, q, v b , ω) T ∈ R 13 , where the state variable vectors are presented in Table 1. The dynamics can be described using the Newton-Euler formulation as in (Luis and Ny, 2016), leading to the dynamic system used in this paper. Furthermore, assuming that the rotor inertia is considerably small such that it is possible to change the propeller's rotational speed Ω i instantaneously, we consider their collection as the control inputs of our system u := (Ω 1 , Ω 2 , Ω 3 , Ω 4 ) T ∈ R 4 .

Tailored Problem Formulation
In this section, we tailor the NMPC formulation (1) into the following soft-constrained problem with LCL constraint formulation: where ∈ R n is a vector that contains all slack variables over the horizon := ( 0 , . . . , N ) T , and µ is the penalty weight. Remark 3. In our approach, for each obstacle in the workspace, we explicitly add new constraints (6e)-(6f) to the NLP. In this paper, two dynamic obstacles are considered, thus n = 2 × N .

Reference Trajectory Generation
Two different global reference trajectories for the Crazyflie are considered. They have been generated offline by solving the OCP (1) for system (5) considering constraint (1d) of the form U using CasADi. We assume a prediction horizon of T = 3 s, N = 200 shooting nodes, discretizing the dynamics via ERK4 integration method. The resulting NLPs provide the optimal trajectories that drive the nanoquadcopter from the hovering state at p := (0, 0, 0.4) T to the final desired positions: p d 1 := (1, −1, 1) T and p d 2 := (0.5, −0.5, 1) T . We regulate for q d := (1, 0, 0, 0) T , so that the geodesic arc can be approximated by a straight line without major problems. As the obstacles are dynamic, the global references are unaware of them. Finally, we make use of the interior-point solver IPOPT, exploiting the just-in-time compiling function evaluations, to solve this optimization problem.

Implementation Details
We make use of acados Python template-based interface to generate a library which implements our tailored problem formulation (6). The QPs arising in this NMPC formulation are solved using the high-performance interior-point method (HPIPM) solver that is built on top of the linear algebra package BLASFEO. The major difference between existing high-performance implementations of BLAS-and LAPACK-like routines for embedded applications and BLASFEO is that the last is optimized for small to medium scale matrices . The NLPs results were obtained on an Intel Core i5 @2.6 GHz running macOS Catalina. The X64 INTEL HASWELL implementation of BLASFEO package has been used, which exploits a set of vectorized instructions for the target CPU. Additionally, for the NMPC solution, we use partial condensing HPIPMbased technique. This approach reformulates the large and sparse Hessian of problem (6) into a small and dense one which has a more suitable form for the QP solver.

Performance Evaluation
To evaluate the trajectory conservativeness introduced by the constraint approximation, we perform simulations comparing our real-time planner with LCL constraint formulation to its counterpart, a planner where constraint (6e) is defined by · 2 2 . The values of the slack penalties and weighting matrices used in both cases are those defined in (8). In simulation, the Crazyflie must travel from a hovering configuration to another while avoiding balls thrown at it. We assume that the obstacle's movement between two shooting nodes is ballistic. The results of our simulations are presented in Fig. 2 and Fig. 3. As expected, the planner with LCL constraint formulation generates trajectories that are significantly closer to the minimum clearance d C , as compared to its counterpart. In addition, we note that there is no violation of the constraints. However, the effort required to tune the slack penalties directly impacts the amount of constraint violation. Much of the difficulty exists because, in this particular problem, the optimal solution will frequently lie on the boundary of the feasible set. Therefore, imposing very strict penalties -in the case of soft constraintsincreases the difficulty in driving the solution towards the optimum while distancing the local trajectories from the lower bound. Conversely, if the penalty is not strict enough, then the search will tend to stall outside the feasible region and thereby violate the constraint. Although the tuning considered in this work has provided a remarkable closed-loop performance for both real-time planners -with no constraint violation -there is no guarantee that the violation will not occur. This is especially true in a realworld scenario, where the state estimation deals effectively with the uncertainty due to noisy sensor data and, to some extent, with random external factors, i.e. wind. One way to avoid this shortcoming and, therefore, recover feasibility is to retune the slack penalties of the soft-constrained problem.
To assess the computational complexity of the planner's algorithm, the average computational effort has been gathered. The values for final desired positions p d 1 and p d 2 are 4.76 ms and 7.97 ms, respectively. These results show the computational efficiency of the proposed scheme. This paper is accompanied by a video showcasing the simulations: https://youtu.be/ZRbGyikvsxw.

CONCLUSION
In this work, a new scheme for real-time motion generation for robotic systems has been presented. The key concepts that contribute to the effectiveness of our method stem from the algorithmic ideas tied to the RTI scheme to formulate a least conservative linearized constraint. We have shown a reformulation that guarantees less conservative trajectories for planners whose optimization algorithm is based on Newton-type method. Strategies to ensure both constraint satisfaction and feasibility of the optimization problem have also been discussed. Finally, our approach was tested in simulation, on a challenging example involving a high-dimensional quadcopter system, showing efficient computational performance. Future work will focus on real-world implementation.