A note on the predictive control of non‐holonomic systems and underactuated vehicles in the presence of drift

Motion planning and control of non‐holonomic systems is challenging. Only very recently, it has become clear how model predictive controllers for such systems can be generally furnished in the driftless case, where the key is to design a cost function conforming to the geometry arising from the non‐holonomic constraints. However, in some applications, one cannot neglect drift since the time needed to accelerate is non‐negligible, for example, when operating vehicles with high inertia or at high velocities. Therefore, this contribution extends our previous work on the class of driftless non‐holonomic systems to systems with simple kinds of actuator dynamics that allow to represent the boundedness of acceleration in the model. Moreover, we show in a prototypical example of a simple boat‐like vehicle model that a similar procedure can also work for systems that are not non‐holonomic but still under‐actuated. While the contribution is rather technical in nature, to the knowledge of the authors, it is the first time that MPC controllers with theoretical guarantees are proposed for these kinds of models. Moreover, we expect that the resulting controllers are directly of practical value since even the simpler driftless models are employed successfully in various approaches to motion planning.


INTRODUCTION
Non-holonomic vehicles are widely used in transportation, logistics, and service robotics.Due to their importance, they are subject to automation.But it is well known that motion planning and control for such systems is very challenging.Even for the arguably simplest non-holonomic system, the differential-drive robot, there is no static, continuous statefeedback asymptotically stabilizing a given setpoint.Moreover, linearizations about a given setpoint are not controllable, so that, even locally, linearization is of limited use for control design.In addition, often in control and motion planning, one needs to measure the distance of the system to the goal or setpoint.Yet, due to the underlying geometry, it is not trivial to measure distance in a meaningful manner for these systems [1].These aspects also complicate the successful design of model predictive controllers.It has been known for a few years that, for predictive controllers without terminal ingredients, irrespective of the prediction horizon, quadratic costs do not work for the differential-drive vehicle [2].This is unusual since quadratic costs are the predominant, canonical choice when designing stabilizing predictive controllers.In the literature [3], a special quartic-quadratic cost function was proven to work specifically for the differential-drive vehicle.More generally, in our recent work [4], a condition is set forward on when quadratic costs do not work for the class of driftless underactuated controllable systems.Moreover, therein, a design procedure applicable to controllable driftless non-holonomic systems is proposed that yields predictive controllers for which there exist finite prediction horizons so that the closed loop is provably asymptotically stable.Driftlessness means that the system state stays stationary immediately when the input is set to zero.In the case of a vehicle, this presumes that the vehicle's velocity can be directly manipulated in a discontinuous manner.While clearly being unphysical due to inertia, empirical evidence shows that, for many applications, presuming driftlessness is not a too egregious presumption, see for instance the hardware experiments conducted in previous work [4][5][6].After all, for example, when precisely parking vehicles such as cars, velocities are usually small.Still, in more general cases, it may be that the time needed to brake or accelerate cannot be neglected so that actuation dynamics need to be considered.Therefore, in this work, we extend the MPC design process from previous work [4] to a certain class of models with drift, allowing to capture the boundedness of vehicle accelerations.To the knowledge of the authors, this is the first time that MPC controllers without terminal ingredients but with provable guarantees are proposed for non-holonomic vehicles modeled as second-order systems.In addition, novelly, we provide an example that shows that also underactuated, but holonomic systems may be dealt with in a similar manner.The paper is organized as follows.Section 2 presents the essential theoretical preliminaries, based on which Section 3 represents the key theoretical contribution of the paper by showing how MPC controllers for non-holonomic systems with drift may be furnished, first generally and then at the example of concrete vehicle models widely used in applications.Section 4 shows in an exemplary fashion how these considerations may be extended beyond the non-holonomic case.Finally, Section 5 discusses numerical results for the controllers designed in the contribution's examples, before a summary and an outlook are given in Section 6.

PRELIMINARIES
First, a quick recapitulation of the design steps for driftless systems and the corresponding theoretical background is prudent.Due to the limited scope of this contribution, the reader is kindly referred to the work [4] for full detail.
If a system is homogeneous in the sense of [7,Definition 2.3] or if it can be approximated homogeneously in the sense of [7,Definition 4.1] with degree of homogeneity  ≤ 0, then the MPC theory presented in [7] allows the derivation of a tailored, mixed-exponent cost function that allows to prove local asymptotic stability of the closed loop assuming that the prediction horizon is chosen sufficiently large.Models of common non-holonomic systems are not directly homogeneous, and, hence, an approximation needs to be derived.As shown in [4], using the differential-geometric procedure from [1, Section 2.1.3],one can obtain (nilpotent) homogeneous system approximations fitting to the needs of [7].To obtain the approximation, the system is expressed in so-called privileged coordinates which, in a sense, represent the system so that the resulting coordinates are ordered with regard to their ease of manipulation, as some coordinates can be influenced directly by actuation whereas others cannot.A remaining manual design step to ascertain the desired formal closed-loop properties is to prove that there is an appropriate upper estimate of the residual of the approximant as required in [7, Definition 4.1], which is straightforward for typical driftless vehicle models.The obtained cost functions are amenable to mechanical interpretation, with the apparent complexity of the cost function growing with the system's degree of nonholonomy, see [4].Crucially, the construction procedure to obtain a homogeneous approximation assumes that the system is driftless.Before overcoming that obstacle, this paper's notation needs to be clarified, followed by the employed definition of homogeneity that conforms to [7,Definition 2.3].
Subsequently, time dependencies of quantities may be omitted notationally if no ambiguities arise.Moreover, the shorthand notation ℝ  >0 ∶= { ∈ ℝ  |   > 0 ∀  ∈ {1, … , }} will be used, and diag( 1 , … ,   ) ∈ ℝ × ,  ∈ ℕ, shall denote the diagonal matrix with the diagonal entries  1 to   from top to bottom.Generally, vectors are represented by lower-case bold symbols whereas matrices are represented by capital bold symbols.Scalars are printed in normal typeface.
In the following, we will show that the design procedure from [4] naturally extends to specific kinds of non-holonomic systems with drift.

TOWARD MPC OF NON-HOLONOMIC SYSTEMS WITH DRIFT
The key idea to proceed is to re-use the homogeneous approximation of the kinematics to construct a homogeneous approximation of the system with drift.Without loss of generality the setpoint to be asymptotically stabilized shall be the origin.The approximation for the system with drift can then be constructed as follows from the approximation of the driftless system.
Remark 3.3 (System class).The class of system models that can be described by ( 5) can be thought of as driftless nonholonomic systems augmented by simple (homogeneous) actuator dynamics that introduce drift.This allows MPC controllers to appropriately plan motion including acceleration and deceleration, potentially improving accuracy of motion planning over the usage of purely kinematic, driftless models.General second-order models of non-holonomic systems can be of considerably more intricate form since they take into account effects beyond actuator dynamics, such as gyroscopic forces, which, generally, cannot be captured by models of the form (5). The more sophisticated character of the equations that would result in this more general case confound a direct transfer of results from the kinematics to the full non-holonomic dynamics.
Remark 3.4 (Privileged coordinates).As delineated in [4], to find a homogeneous approximation of a given driftless system, it may be necessary to first express the system, and therefore perform the approximation, in terms of the privileged coordinates , see also [1].In that case, the results of this paper are to be applied to the system in privileged coordinates.
Here, due to the required conciseness, we refrain from a full introduction of privileged coordinates and merely state that this paper's results remain applicable in that scenario when using the appropriate coordinates.
Given a sampling time   > 0, a prediction horizon  >   , and a control system of the form (1) that has an (, , 0)homogeneous approximation in the sense from Proposition 3.1, a model predictive controller is established by solving, in each time instant  ∶=   ,  ∈ ℕ 0 , the optimal control problem minimize where ∶ ℝ   × ℝ   → ℝ is the so-called stage cost and the notation (⋅ | ) marks quantities planned at time  over the prediction horizon for a given closed input constraint set  ⊂ ℝ   with  ∈ int( ).The controller works in the usual receding horizon fashion so that the optimal control input is applied on the interval [,  +   ), that is, () ∶=  ⋆ ( | ) for  ∈ [,  +   ).We tacitly assume the existence of an optimal control input  ⋆ , and, following [7], set the stage cost to where  may be set to  ∶= 2 ∏   =1   to obtain even exponents for all terms.
Theorem 3.5.Consider a system of the form (5), where the underlying kinematic model of the form (3) meets the assumptions from Proposition 3.1 and where the homogeneously approximated system ẋ = h(, ) is globally asymptotically null controllable.Then, for any sufficiently large prediction horizon , the MPC controller using the optimal control problem (6) with stage cost according to (7) locally asymptotically stabilizes the origin.
Proof.The assumptions, through Proposition 3.1, fulfill all prerequisites of [7,Theorem 4.4], which provides local asymptotic stability in the theorem's setting and, hence, proves the theorem's assertion.□

Application to the differential-drive vehicle with drift
While simple, before moving to more intricate dynamics, it is worth to inspect the differential-drive vehicle due to its popularity in applications, e.g., in service robotics.The kinematics of the driftless differential-drive vehicle can be written as where  and  denote the position of the vehicle's center of mass in the --plane,  its orientation as an angle measured relative to the positive -axis, and where  1 is the translational and  2 the angular velocity of the vehicle, see [4].A homogeneous approximation around the origin is given through (, ) with ℎ 1 =  1 , ℎ 2 =  1 , ℎ 3 =  2 .Fitting homogeneity ] ,  = 0, and the residuum  meets the requirements, see [7, Proposition 4.6].
According to Proposition 3.1, the system has the homogeneous approximation ẋ = h(, ) ] , which has the homogeneity ] ,  = 0. System ( 9) is a model for a differential-drive vehicle that considers inertia effects, where the mass of the vehicle is  and the moment of inertia about its center of mass is , and where the input  contains the acting propulsion force (in forward direction) and moment (about the robot's center of mass).Hence, inserting the homogeneity parameters into (7), a functioning stage cost is given by (, ) =  4  1 +  2 2 +  4  3 +  4 4 +  4 5 +  4  1 +  4 2 .

Application to a car-like vehicle with actuator dynamics
A model of a rear wheel-driven kinematic car, which is of interest due to its higher degree of non-holonomy, can be given by where the steering angle  is confined to (−∕2, ∕2),  and  denote the position of the center point of the rear axle, and  is the wheelbase, see [5].As before,  encodes the vehicle's orientation.The model can be understood as a simplified kinematic model for a car driving in a plane.Simplifications include that rear and front axle are collapsed to the center line of the car and that the wheels roll without slipping.A homogeneous approximation around the origin is given by q = (, ) with ℎ 1 =  1 , ℎ 2 =  1 , ℎ 3 =  1 ∕, ℎ 4 =  2 .Concretely, condition (2) is satisfied, for example, for  = [ ] , and  = 0.The residuum is given by  1 (, ) = (cos() − 1) 1 ,  2 (, ) = (sin() − ) 1 ,  3 (, ) = (tan() − ) 1 ∕,  4 (, ) = 0.One by one, the coordinates of the residuum vector can be upper bounded, yielding where the inequality in (13) holds when  ≤ ∕3.Moreover, trivially, the fourth coordinate yields | 4 | = 0.For instance, when setting the constants  = 2,  = 1,  = max{1∕2, 1∕}, the requirements on the residuum are met.
A model of the considered vehicle with actuator dynamics can be written in the form where the input  contains the resulting propulsion force acting on the vehicle as well as the steering torque, and where   ,   are positive constants that correspond to the inverse of actuation inertia.Hence, using Proposition 3.1, the vehicle dynamics with actuation dynamics can be approximated homogeneously with the parameters r = ] , and degree of homogeneity 0. Consequently, a fitting stage cost is given by (, ) =  12 1 +

BEYOND NON-HOLONOMY: PURE UNDER-ACTUATION
The approach set forward so far gives a clear indication how to consider actuation dynamics in predictive controllers for non-holonomic systems, where drift can only occur as long as it conforms with the system's non-holonomic constraints.However, there are also systems that are under-actuated but where the kinematics is not non-holonomic.Since such systems have less structure, it is most likely harder to give universal insight.Still, subsequently, we show at an example of practical value that the same idea presented before can also be applied to under-actuated but holonomic systems since a system structure similar to before arises.A prototypical example is a simple model of a boat actuated by two propellers that are mounted on the left and right sides of the boat's center line, see [8].Since hydrodynamic interactions between the boat-like vehicle and water are not accounted for, the model is a better fit for light boats in shallow water.Given the boat's mass  and the relevant moment of inertia  about the center of mass, the equations of motion can be given in the form where the generalized coordinates are chosen to [    ] =∶  and describe the position of the center of mass and the orientation.Input  1 is the resulting propulsion force and  2 the resulting moment around the center of mass.Using the state vector  ∶= [   q ] , the state-space dynamics is given by ẋ = which is of similar, although not identical, structure to (5).Hence, unsurprisingly, we find a zero-degree homogeneous approximation ẋ = (, ) of system (16) around the origin with (, ) = [  4  5  6  1 ∕  3  1 ∕  2 ∕ ] .The homogeneity condition (2) holds true, e.g., for  ∶= 0,  1 =  4 =  1 ∶= 1,  2 =  5 =  3 +  1 ∶= 2,  3 =  6 =  2 ∶= 1.Moreover, the required upper estimates for the coordinates of the residuum vector are straightforward to obtain.For coordinates 1, 2, 3, and 6, the residuum is trivially zero, whereas the upper estimates for the coordinates 4 and 5 can be obtained analogously as for the residuum of the differential-drive vehicle since the corresponding expressions have iden- 2+ , where the requirements are met, for example, when the constants are set to  = 2,  = 1,  = 1∕(2 ).Hence, according to [7], there exists a prediction horizon so that the corresponding MPC controller of form (6) locally asymptotically stabilizes the system when using the stage cost (, ) =  4  1 +

NUMERICAL RESULTS
To test the applicability of the theoretically sound controllers, for all three introduced vehicles with drift, a typical parking scenario is tested in a Matlab-based simulation.The optimal control problems are formulated using Casadi [9] and solved with Ipopt [10], where the problems are time-discretized before optimization, assuming a zero-order hold on the control input.The parameters chosen for Ipopt are geared toward a highly accurate solution, with convergence tolerances set to 10 −13 to reduce potential effects of inaccurate solutions.For differential-drive and boat-like vehicles, F I G U R E 1 Optimal value functions over time for the different vehicles as resulting from a simulated parking maneuver.
the sampling time is set to   = 0.1 s, whereas it is set to   = 0.25 s for the car-like vehicle.For the former two systems, the stage costs are directly chosen as written previously, whereas for the latter, the scaled stage-cost (, ) = 10 12  12 1 + 10 ) is employed to improve numeric accuracy.Otherwise, the large-exponent terms, close to the origin, quickly reach machine accuracy, negatively affecting the parking accuracy reachable with limited machine accuracy.This scaling can also be interpreted as a scaling of the system's states and inputs.For differential-drive ( = 2 kg,  = 1 kg m 2 ), car-like (  = 0.1 kg −1 ,   = 10 kg −1 m −2 ,  = 1 m), and boat-like vehicle ( = 10 kg,  = 2 kg m 2 ), the prediction horizons are set to  = 60   ,  = 400   , and  = 240   , respectively.The inputs are bounded so that the absolute values of  1 and  2 may not exceed 0.2 N and ∕5 N m for the differential-drive vehicle, as well as 5 N and 2.5 N m for the car-like and 1.5 N and 0.5 N m for the boat-like vehicles, respectively.Moreover, for the car-like vehicle, an additional state constraint on the steering angle  is added to the MPC problem (6), so that its absolute value is bounded by ∕3 rad.Not only does this fit better to real-world cars but it also avoids the singularity appearing in the model through the tangent.In all three scenarios, the vehicles start at the position  = −1.5 m,  = 0.75 m and all angles and velocities are initially set to zero.The optimal value functions (i.e., the optimal cost values) over time are shown in Figure 1.
As can be seen, all value functions converge nicely until values in the order of magnitude of machine accuracy are reached.For the differential-drive vehicle, sub-millimeter and sub-degree accuracy in the parking pose is first reached at  = 26.51s, that is, at that time, the position deviation is below one millimeter in the and -directions individually.In the same manner, the car-like vehicle first arrives at sub-millimeter position and sub-degree orientation accuracy at  = 125.72 s.The boat-like vehicle first reaches sub-millimeter and sub-degree pose accuracy at about  = 34.06s.Naturally, a wider spread of different exponents makes it numerically more difficult to bring all coordinates individually similarly close to the setpoint.Therefore, the intuitively and geometrically arising difficulty levels for parking the different vehicles are also reflected numerically.In all cases, the velocities in the specified time instances are small enough so that the nonholonomic vehicles could come to a standstill within one sampling interval while respecting the input bounds.This applies similarly to the boat-like vehicle, where the (not immediately influentiable) lateral drift is already below one millimiter per second at the specified time.Still, the controllers continue to actuate the vehicles to reach lower cost values.While the mixed-exponent cost functions can be challenging numerically, in real-world tests, the accuracy of models, actuators, and sensors may be much more limiting than numerics.

SUMMARY AND OUTLOOK
This paper furnished a design procedure for model predictive controllers with provable closed-loop guarantees for non-holonomic vehicles with actuator dynamics and provided an example how MPC controllers can also be designed successfully for under-actuated but holonomic systems.As yet, however, there is no generalizable estimate of the minimum prediction horizon necessary to obtain closed-loop guarantees; it is merely clear that such a horizon exists.To work toward an estimate, future work may formalize an observed relationship of the employed stage cost to a distance measure fitting to non-holonomic systems.Concretely, by building upon [7], the proof mechanism underlying this paper uses the homogeneity parameters in the stage cost (7).Interestingly, for all kinematic systems appearing in this paper and in our previous work [4], the homogeneity parameters   of the homogeneous approximation in privileged coordinates can be chosen equal to the weights   calculated to reformulate the kinematic system in privileged coordinates, cf.[1,4].In that case, the stage cost (7) is closely related to an estimate of the sub-Riemannian distance, which is a meaningful distance measure for non-holonomic systems (and other systems underlying sub-Riemannian geometry), see [

Definition 2 . 1 (
Homogeneity).The control system ẋ() = ((), ()) [1,Theorem 2.1].Indeed, when setting  ∶= 1, the state-dependent part of the stage cost is directly an established distance estimate for the system, compare[1, Eq. (2.5)].Potentially, this observed relationship might also be useful to design fitting mixed-exponent cost functions without the intermediate step of a homogeneous approximation.This work was supported by the Deutsche Forschungsgemeinschaft (DFG, German Research Foundation) under Grants 501890093 (SPP 2353) and 433183605, and through Germany's Excellence Strategy (Project PN4-4 Theoretical Guarantees for Predictive Control in Adaptive Multi-Agent Scenarios) under Grant EXC 2075-390740016.Open access funding enabled and organized by Projekt DEAL.
A C K N O W L E D G M E N T S