Real-Time Model Predictive Control of Rigid Body Motion via Discretization Using the Cayley Map

This paper presents a fast nonlinear model predictive control method for rigid body dynamical systems such as spacecraft or aerial vehicles on the special Euclidean group SE(3). The focus of this research is on the real-time execution of the optimal control on a low-cost embedded computer. The position and orientation of a rigid body are expressed as a 6-dimensional vector via the Cayley map for SE(3). Based on the representation, the motion can be exactly discretized on the algebra se(3). As a result, coarse sampling intervals can be selected to reduce the number of prediction steps. Furthermore, the recursive discretization technique is applied to effectively reduce the decision variables of the nonlinear optimization problem by eliminating states from them. Simulation results on a Raspberry Pi single-board computer are given to prove that the present model predictive control method is feasible in real time. The effectiveness of the present controller is further verified by an experiment using a fully actuated hexarotor unmanned aerial vehicle.

Related papers:

title: Intrinsic Optimal Control for Mechanical Systems on Lie Group
abstract: The intrinsic infinite horizon optimal control problem of mechanical systems on Lie group is investigated. The geometric optimal control problem is built on the intrinsic coordinate-free model, which is provided with Levi-Civita connection. In order to obtain an analytical solution of the optimal problem in the geometric viewpoint, a simplified nominal system on Lie group with an extra feedback loop is presented. With geodesic distance and Riemann metric on Lie group integrated into the cost function, a dynamic programming approach is employed and an analytical solution of the optimal problem on Lie group is obtained via the Hamilton-Jacobi-Bellman equation. For a special case on , the intrinsic optimal control method is used for a quadrotor rotation control problem and simulation results are provided to show the control performance.

title: Proportional Derivative (PD) Control on the Euclidean Group
abstract: In this paper we study the stabilization problem for control 
systems defined on SE(3) (the special Euclidean group of rigid-body 
motions) and its subgroups. Assuming one actuator is available for each 
degree of freedom, we exploit geometric properties of Lie groups (and 
corresponding Lie algebras) to generalize the classical proportional derivative 
(PD) control in a coordinate-free way. For the SO(3) case, the 
compactness of the group gives rise to a natural metric structure and to a 
natural choice of preferred control direction: an optimal (in the sense of 
geodesic) solution is given to the attitude control problem. In the SE(3) 
case, no natural metric is uniquely defined, so that more freedom is left 
in the control design. Different formulations of PD feedback can be 
adopted by extending the SO(3) approach to the whole of SE(3) or by 
breaking the problem into a control problem on SO(3) x R^3. For the simple 
SE(2) case, simulations are reported to illustrate the behavior of the 
different choices. We also discuss the trajectory tracking problem and show 
how to reduce it to a stabilization problem, mimicking the usual 
approach in R^n. Finally, regarding the case of underactuated control 
systems, we derive linear and homogeneous approximating vector fields for 
standard systems on SO(3) and SE(3).

title: Joint Optimization of Geometric Control and Structure of a Fully-actuated Hexrotor based on an Analytic HJBE Solution
abstract: This paper proposes a joint optimization scheme of the structural design and control of a fully-actuated hexrotor unmanned aerial vehicle. The hexrotor dynamics is formulated on the special Euclidean group SE(3) which represents the position and attitude of a rigid body. An optimal control problem on SE(3) is then considered, and the optimal input and the associated viscosity solution of the Hamilton-Jacobi-Bellman equation are presented analytically. The solution, value function, expresses the minimum value of the cost function for a given initial state. The analytical form of the value function is then regarded as a function of structural variables, and the function is minimized by modifying the vehicle structure. A numerical example shows that the optimally-controlled optimal structure maximizes its dynamic manipulability measure. Moreover, the resulting structure and control system are shown to be energy-efficient.

title: Geometric Numerical Integration
abstract: The subject of this workshop was numerical methods that preserve geometric properties of the flow of an ordinary or partial differential 
equation. This was complemented by the question as to how structure preservation affects the long-time behaviour of numerical methods.

title: GEOMETRIC OPTIMAL CONTROL OF RIGID BODIES
abstract: This paper treats the geometric formulation of optimal control problems for rigid bodies and it presents computational procedures based on this geometric formulation that can be used for numerical solution of these optimal control problems. The dynamics of each rigid body is viewed as evolving on a configuration manifold that is a Lie group. Discrete time dynamics of each rigid body are developed that evolve on the configuration manifold according to a discrete version of Hamilton’s principle so that the computations preserve geometric features of the dynamics and guarantee evolution on the configuration manifold; these discrete-time dynamics are referred to as Lie group variational integrators. Rigid body optimal control problems are formulated as discrete-time optimization problems for discrete Lagrangian/Hamiltonian dynamics, to which standard numerical optimization algorithms can be applied. This general approach is illustrated by presentation of results for several different optimal control problems for a single rigid body and for multiple interacting rigid bodies. Although the emphasis in this work is on computational results, the philosophy and the approach follow the work on optimal control problems for rigid body kinematics by Roger Brockett and his colleagues in the 1970s. This version of the paper provides an overview of the concepts and contributions; the final version of the paper will include additional details on geometric optimal control of rigid bodies.

title: Discrete optimal control on lie groups and applications to robotic vehicles
abstract: This paper is concerned with optimal trajectory generation for robotic multi-body systems. The focus is on discrete optimal control methods which operate intrinsically in the state space system manifold and do not require coordinate charts or projections. This is accomplished by defining both the dynamics and the optimal control solution as sequences of vector fields mapping to curves on the Lie group through retraction maps, and defining variations and differentiation with respect to such vector fields. As a result, standard trajectory optimization methods can be easily extended to the Lie group setting without loss of efficiency. The methods are illustrated with three numerical examples: a quadrotor, an aerial vehicle with manipulators, and a simple nonholonomic system.

title: Discrete Geometric Optimal Control on Lie Groups
abstract: We consider the optimal control of mechanical systems on Lie groups and develop numerical methods that exploit the structure of the state space and preserve the system motion invariants. Our approach is based on a coordinate-free variational discretization of the dynamics that leads to structure-preserving discrete equations of motion. We construct necessary conditions for optimal trajectories that correspond to discrete geodesics of a higher order system and develop numerical methods for their computation. The resulting algorithms are simple to implement and converge to a solution in very few iterations. A general software implementation is provided and applied to two example systems: an underactuated boat and a satellite with thrusters.

title: Reconfigurable Fault-tolerant NMPC for Y6 Coaxial Tricopter with Complete Loss of One Rotor
abstract: Safe and reliable operation of unmanned aerial vehicles (UAVs) is a high priority as most of the UAVs must be operated in humanly populated environments. In this study, we exhibit a nonlinear model predictive control (NMPC) approach, as an active fault-tolerant control method, for a Y6 coaxial tricopter UAV in the presence of an actuator fault respecting the actuator constraints. A cascaded closed-loop control methodology, incorporating a reconfigurable low-level controller, is proposed which eradicates the need to design multiple controllers for normal and faulty operations. The efficacy of the presented methodology is illustrated for tracking an 8-shaped trajectory with a complete loss of one rotor. From the sequential failure cases of two different rotors, it is shown that even with a fault-detection delay of 0.5s, the maximum altitude divergence is 0.41m that smoothly diminishes over the span of few seconds. In addition to the adequate tracking performance, thanks to ACADO toolkit, a combined average execution time of 2.1 milliseconds is achieved which is promising for its real-time applicability for fast aerospace applications.

title: Online Nonlinear Model Predictive Control for tethered UAVs to perform a safe and constrained maneuver
abstract: This paper focuses on the design and implementation of a two layer hierarchical controller for a tethered aerial vehicle. In particular, a challenging problem of smoothly landing on a inclined plane is considered. Such problem yields nonlinear control objective and constraints which require advanced control techniques like Nonlinear Model Predictive Control (NMPC), that is known to be computationally demanding. A control architecture composed by two layers is proposed: an NMPC operating at 100Hz is employed to generate feasible trajectories on-line and a simplified local controller, working at 500Hz, is used to track them. The effectiveness of this hierarchical control structure is demonstrated by a closed-loop simulation. Moreover, the rejection of external disturbances, such as the wind, is a peculiar characteristic of the proposed control strategy.

title: Nonlinear Model Predictive Control with Actuator Constraints for Multi-Rotor Aerial Vehicles
abstract: In this paper we propose, test, and validate an online Nonlinear Model Predictive Control (NMPC) method applied to multi-rotor aerial systems with arbitrarily positioned and oriented rotors. This work brings into question some common modeling and control design choices that are typically adopted in order to guarantee robustness and reliability but which may severely limit the attainable performance. In particular the proposed method \emph{does not} resort to common simplifications such as: 1) linear model approximation, 2) cascaded control paradigm used to decouple the translational and the rotational dynamics of the rigid body, and 3) use of low level reactive trackers for stabilization, 4) unconstrained system or use of fictitious constraints. The method addresses simultaneously the problem of local reference trajectory planning and that of stabilizing the vehicle dynamics. Furthermore, by considering as control inputs the derivatives of the forces generated by the multi-rotor vehicle and by means of a novel actuator modeling approach, the method avoids conservative -- and often fictitious -- input/state saturations which are present, e.g., in cascaded approaches. The control algorithm is implemented using a state-of-the-art Real Time Iteration (RTI) scheme with partial sensitivity update method. The performances of the control system are finally validated by means of real-time simulations and in real experiments, with a large spectrum of multi-rotor systems: an \emph{under-actuated} quadrotor, a \emph{fully actuated} hexarotor, a multi-rotor with \emph{orientable} propellers, and a multi-rotor with an unexpected \emph{rotor failure}.

title: Towards model-predictive control for aerial pick-and-place
abstract: This paper considers pick-and-place tasks using aerial vehicles equipped with manipulators. The main focus is on the development and experimental validation of a nonlinear model-predictive control methodology to exploit the multi-body system dynamics and achieve optimized performance. At the core of the approach lies a sequential Newton method for unconstrained optimal control and a high-frequency low-level controller tracking the generated optimal reference trajectories. A low cost quadrotor prototype with a simple manipulator extending more than twice the radius of the vehicle is designed and integrated with an on-board vision system for object tracking. Experimental results show the effectiveness of model-predictive control to motivate the future use of real-time optimal control in place of standard ad-hoc gain scheduling techniques.

title: Robust obstacle avoidance for aerial platforms using adaptive model predictive control
abstract: This work addresses the problem of motion planning among obstacles for quadrotor platforms under external disturbances and with model uncertainty. A novel Nonlinear Model Predictive Control (NMPC) optimization technique is proposed which incorporates specified uncertainties into the planned trajectories. At the core of the procedure lies the propagation of model parameter uncertainty and initial state uncertainty as high-confidence ellipsoids in pose space. The quadrotor trajectories are then computed to avoid obstacles by a required safety margin, expressed as ellipsoid penetration while minimizing control effort and achieving a user-specified goal location. Combining this technique with online model identification results in robust obstacle avoidance behavior. Experiments in outdoor scenarios with virtual obstacles show that the quadrotor can avoid obstacles robustly, even under the influence of external disturbances.

title: A Mathematical Introduction to Robotic Manipulation
abstract: Brief History.  Outline of the Book. Bibliography.  Rigid Body Transformations. Rotational Motion in R3. Rigid Motion in R3. Velocity of a Rigid Body. Wrenches and Reciprocal Screws.  Forward Kinematics. Inverse Kinematics. The Manipulator Jacobian. Redundant and Parallel Manipulators.  Lagrange's Equations. Dynamics of Open-Chain Manipulators. Lyapunov Stability Theory. Position Control and Trajectory Tracking. Control of Constrained Manipulators.  Lagrange's Equations with Constraints. Robot Hand Dynamics. Redundant and Nonmanipulable Robot Systems. Kinematics and Statics of Tendon Actuation. Control of Robot Hands.  Controllability and Frobenius' Theorem. Examples of Nonholonomic Systems. Structure of Nonholonomic Systems.  Steering Model Control Systems Using Sinusoids. General Methods for Steering. Dynamic Finger Repositioning. FUTURE PROSPECTS: Robots in Hazardous Environments. Medical Applications for Multifingered Hands. Robots on a Small Scale: Microrobotics.  Lie Groups and Robot Kinematics. A Mathematica Package for Screw Calculus. Bibliography. Index Each chapter also includes a Summary, Bibliography, and Exercises

title: COMPUTING EXPONENTIALS OF SKEW-SYMMETRIC MATRICES AND LOGARITHMS OF ORTHOGONAL MATRICES
abstract: The authors show that there is a generalization of Rodrigues’ formula for computing the exponential map exp: so(n) →SO(n) from skewsymmetric matrices to orthogonal matrices when n ≥ 4, and give a method for computing some determination of the (multivalued) function log: SO(n) → so(n). The key idea is the decomposition of a skew-symmetric n×n matrix B in terms of (unique) skew-symmetric matrices B1,...,Bp obtained from the diagonalization of B and satisfying some simple algebraic identities. A subproblem arising in computing logR, where R ∈SO(n), is the problem of finding a skewsymmetric matrix B, given the matrix B 2 , and knowing that B 2 has eigenvalues −1 and 0. The authors also consider the exponential map exp: se(n) →SE(n), where se(n) is the Lie algebra of the Lie group SE(n) of (affine) rigid motions. The authors show that there is a Rodrigues-like formula for computing this exponential map, and give a method for computing some determination of the (multivalued) function log: SE(n) → se(n). This yields a direct proof of the surjectivity of exp: se(n) →SE(n).

title: Closed Form of the Baker-Campbell-Hausdorff Formula for the Lie Algebra of Rigid Body Displacements
abstract: This paper demonstrates the existence of the closed form of the Baker-Campbell-Hausdorff (BCH) formula for the Lie algebra of rigid body displacement. For this, the structure of the Lie group of the rigid body displacements \( S{\mathbb{E}}_{3} \) and the properties of its algebra Lie Open image in new window are used. Also, using the isomorphism between the Lie group \( S{\mathbb{E}}_{3} \) and the Lie group of the orthogonal dual tensors, a solution of this problem in dual algebra is given.

title: Nonlinear Model Predictive Control
abstract: In this chapter, we introduce the nonlinear model predictive control algorithm in a rigorous way. We start by defining a basic NMPC algorithm for constant reference and continue by formalizing state and control constraints. Viability (or weak forward invariance) of the set of state constraints is introduced and the consequences for the admissibility of the NMPC-feedback law are discussed. After having introduced NMPC in a special setting, we describe various extensions of the basic algorithm, considering time varying reference solutions, terminal constraints, and costs and additional weights. Finally, we investigate the optimal control problem corresponding to this generalized setting and prove several properties, most notably the dynamic programming principle.

title: On the geometric analysis of optimum trajectories for cooperating robots using dual quaternion coordinates
abstract: A geometric method for the dynamic analysis of trajectories for cooperating robot systems is presented. The technique uses the algebraic manifolds that arise from the image space of spatial displacements to do trajectory analysis. The geometric structure of these manifolds offers a convenient framework to study robot motion. In this technique, the dynamics of the robots and the workpiece are formulated in the operational space of the robot. The position of the robot is written in the image space of dual quaternion coordinates. Thus, the analysis is performed in the operational image space that provides an algebraically defined geometric structure upon which to examine the trajectories that the robot system follows. This analysis offers a tool for determining optimal trajectories for the robot system. The technique is demonstrated through the analysis of two planar three-revolute-joint (3R) robots manipulating a common object. A planar example was chosen because the results can be demonstrated graphically. The algebraic properties of the constraint manifolds can be used to extend the work to more general, multidimensional spatial robot problems.<<ETX>>

title: Quaternion kinematics for the error-state Kalman filter
abstract: A tutorial of quaternion algebra, especially suited for the error-state Kalman filter, with the aim of building Visual-Inertial SLAM and odometry systems.

title: Linear vs Nonlinear MPC for Trajectory Tracking Applied to Rotary Wing Micro Aerial Vehicles
abstract: Precise trajectory tracking is a crucial property for Micro Air Vehicles (MAVs) to operate in cluttered environment or under disturbances. In this paper we present a detailed comparison between two state-of-the-art model-based control techniques for MAV trajectory tracking. A classical Linear Model Predictive Controller (LMPC) is presented and compared against a more advanced Nonlinear Model Predictive Controller (NMPC) that considers the full system model. In a careful analysis we show the advantages and disadvantages of the two implementations in terms of speed and tracking performance. This is achieved by evaluating hovering performance, step response, and aggressive trajectory tracking under nominal conditions and under external wind disturbances.

title: Embedded Optimization Algorithms for Steering in Autonomous Vehicles based on Nonlinear Model Predictive Control
abstract: Steering control for autonomous vehicles on slippery road conditions, such as on snow or ice, results in a highly nonlinear and therefore challenging online control problem, for which nonlinear model predictive control (NMPC) schemes have shown to be a promising approach. NMPC allows to deal with the nonlinear vehicle dynamics as well as the system limitations and geometric constraints in a rather natural way, given a desired trajectory that can be provided by a supervisory algorithm for path planning. Our aim is to study the real-time feasibility of NMPC-based steering control on an embedded computer and the importance of the appropriate vehicle model selection, the optimization solver choice and control horizon length. The presented computation times have been obtained on a Raspberry Pi 2 model, as a proof of concept for a future real-world implementation on an embedded microprocessor.

title: Updating Quasi-Newton Matrices With Limited Storage
abstract: We study how to use the BFGS quasi-Newton matrices to precondition minimization methods for problems where the storage is critical. We give an update formula which generates matrices using information from the last m iterations, where m is any number supplied by the user. The quasi-Newton matrix is updated at every iteration by dropping the oldest information and replacing it by the newest informa- tion. It is shown that the matrices generated have some desirable properties. The resulting algorithms are tested numerically and compared with several well- known methods. 1.  For the problem of minimizing an unconstrained function / of n variables, quasi-Newton methods are widely employed (4). They construct a se- quence of matrices which in some way approximate the hessian of /(or its inverse). These matrices are symmetric; therefore, it is necessary to have n(n + l)/2 storage locations for each one. For large dimensional problems it will not be possible to re- tain the matrices in the high speed storage of a computer, and one has to resort to other kinds of algorithms. For example, one could use the methods (Toint (15), Shanno (12)) which preserve the sparsity structure of the hessian, or conjugate gradient methods (CG) which only have to store 3 or 4 vectors. Recently, some CG algorithms have been developed which use a variable amount of storage and which do not require knowledge about the sparsity structure of the problem (2), (7), (8). A disadvantage of these methods is that after a certain number of iterations the quasi-Newton matrix is discarded, and the algorithm is restarted using an initial matrix (usually a diagonal matrix). We describe an algorithm which uses a limited amount of storage and where the quasi-Newton matrix is updated continuously. At every step the oldest information contained in the matrix is discarded and replaced by new one. In this way we hope to have a more up to date model of our function. We will concentrate on the BFGS method since it is considered to be the most efficient. We believe that similar algo- rithms cannot be developed for the other members of the Broyden 0-class (1). Let / be the function to be nnnimized, g its gradient and h its hessian. We define

title: Classification and Structural Evaluation of Fully-Actuated Hexrotor UAVs
abstract: This paper presents a kind of taxonomy to classify and understand the acceleration characteristics of fully-actuated hexrotor unmanned aerial vehicles. First, two special classes of hexrotor structures are defined: symmetric-coplanar-tilted-rotor (SCTR) structure and parallel-coplanar-tilted-rotor (PCTR) structure. It is shown that many of structures from related research can be classified into the two types. Moreover, the acceleration characteristics of hexrotor structures can be analyzed and visualized using dynamic manipulability. The dynamic manipulability ellipsoids enable to evaluate the directional characteristics of the vehicle motion, and it is suited to the design of the fully-actuated hexrotor for each aerial manipulation task.

title: Optimal Control of a Rigid Body using Geometrically Exact Computations on SE(3)
abstract: Optimal control problems are formulated and efficient computational procedures are proposed for combined orbital and rotational maneuvers of a rigid body in three dimensions. The rigid body is assumed to act under the influence of forces and moments that arise from a potential and from control forces and moments. The key features of this paper are its use of computational procedures that are guaranteed to preserve the geometry of the optimal solutions. The theoretical basis for the computational procedures is summarized, and examples of optimal spacecraft maneuvers are presented

title: A continuation/GMRES method for fast computation of nonlinear receding horizon control
abstract: In this paper, a fast numerical algorithm for nonlinear receding horizon control is proposed. The control input is updated by a differential equation to trace the solution of an associated state-dependent two-point boundary-value problem. A linear equation involved in the differential equation is solved by the generalized minimum residual method, one of the Krylov subspace methods, with Jacobians approximated by forward differences. The error in the entire algorithm is analyzed and is shown to be bounded under some conditions. The proposed algorithm is applied to a two-link arm whose dynamics is highly nonlinear. Simulation results show that the proposed algorithm is faster than the conventional algorithms.

title: Variable sampling-time nonlinear model predictive control of satellites using magneto-torquers
abstract: Satellite control using magneto-torquers represents a control challenge combined with strong nonlinearity, variable dynamics and partial controllability. An automatic differentiation-based nonlinear model predictive control (NMPC) algorithm is developed in this work to tackle these issues. Based on the previously developed formulation of NMPC, a novel variable sampling-time scheme is proposed to provide a better trade-off between transient control performance and closed-loop stability. More specifically, a small sampling time is adopted to improve the response speed when the satellite is far away from the desired position, and a large sampling time is employed for the closed-loop stability when the satellite is around its equilibrium position. This scheme also significantly reduces the online computational burden associated with fixed sampling-time NMPC where a large prediction horizon has to be adopted in order to the ensure closed-loop stability. The proposed approach is demonstrated through nonlinear simulation of a specific satellite case with satisfactory results obtained.