Whole Body Model Predictive Control with a Memory of Motion: Experiments on a Torque-Controlled Talos

This paper presents the first successful experiment implementing whole-body model predictive control with state feedback on a torque-control humanoid robot. We demonstrate that our control scheme is able to do whole-body target tracking, control the balance in front of strong external perturbations and avoid collision with an external object. The key elements for this success are threefold. First, optimal control over a receding horizon is implemented with Crocoddyl, an optimal control library based on differential dynamics programming, providing state-feedback control in less than 10 msecs. Second, a warm start strategy based on memory of motion has been implemented to overcome the sensitivity of the optimal control solver to initial conditions. Finally, the optimal trajectories are executed by a low-level torque controller, feedbacking on direct torque measurement at high frequency. This paper provides the details of the method, along with analytical benchmarks with the real humanoid robot Talos

Fast Second-order Cone Programming for Safe Mission Planning

This paper considers the problem of safe mission planning of dynamic systems operating under uncertain environments. Much of the prior work on achieving robust and safe control requires solving second-order cone programs (SOCP). Unfortunately, existing general purpose SOCP methods are often infeasible for real-time robotic tasks due to high memory and computational requirements imposed by existing general optimization methods. The key contribution of this paper is a fast and memory-efficient algorithm for SOCP that would enable robust and safe mission planning on-board robots in real-time. Our algorithm does not have any external dependency, can efficiently utilize warm start provided in safe planning settings, and in fact leads to significant speed up over standard optimization packages (like SDPT3) for even standard SOCP problems. For example, for a standard quadrotor problem, our method leads to speedup of 1000x over SDPT3 without any deterioration in the solution quality. Our method is based on two insights: a) SOCPs can be interpreted as optimizing a function over a polytope with infinite sides, b) a linear function can be efficiently optimized over this polytope. We combine the above observations with a novel utilization of Wolfe's algorithm to obtain an efficient optimization method that can be easily implemented on small embedded devices. In addition to the above mentioned algorithm, we also design a two-level sensing method based on Gaussian Process for complex obstacles with non-linear boundaries such as a cylinder

Reliable Trajectories for Dynamic Quadrupeds using Analytical Costs and Learned Initializations

Dynamic traversal of uneven terrain is a major objective in the field of legged robotics. The most recent model predictive control approaches for these systems can generate robust dynamic motion of short duration; however, planning over a longer time horizon may be necessary when navigating complex terrain. A recently-developed framework, Trajectory Optimization for Walking Robots (TOWR), computes such plans but does not guarantee their reliability on real platforms, under uncertainty and perturbations. We extend TOWR with analytical costs to generate trajectories that a state-of-the-art whole-body tracking controller can successfully execute. To reduce online computation time, we implement a learning-based scheme for initialization of the nonlinear program based on offline experience. The execution of trajectories as long as 16 footsteps and 5.5 s over different terrains by a real quadruped demonstrates the effectiveness of the approach on hardware. This work builds toward an online system which can efficiently and robustly replan dynamic trajectories.Comment: Video: https://youtu.be/LKFDB_BOhl

Model predictive control of a collaborative manipulator considering dynamic obstacles

Collaborative robots have to adapt its motion plan to a dynamic environment and variation of task constraints. Currently, they detect collisions and interrupt or postpone their motion plan to prevent harm to humans or objects. The more advanced strategy proposed in this article uses online trajectory optimization to anticipate potential collisions, task variations, and to adapt the motion plan accordingly. The online trajectory planner pursues a model predictive control approach to account for dynamic motion objectives and constraints during task execution. The prediction model relates reference joint velocities to actual joint positions as an approximation of built-in robot tracking controllers. The optimal control problem is solved with direct collocation based on a hypergraph structure, which represents the nonlinear program and allows to efficiently adapt to structural changes in the optimization problem caused by moving obstacles. To demonstrate the effectiveness of the approach, the robot imitates pick-and-place tasks while avoiding self-collisions, semistatic, and dynamic obstacles, including a person. The analysis of the approach concerns computation time, constraint violations, and smoothness. It shows that after model identification, order reduction, and validation on the real robot, parallel integrators with compensation for input delays exhibit the best compromise between accuracy and computational complexity. The model predictive controller can successfully approach a moving target configuration without prior knowledge of the reference motion. The results show that pure hard constraints are not sufficient and lead to nonsmooth controls. In combination with soft constraints, which evaluate the proximity of obstacles, smooth and safe trajectories are planned

Nonlinear model predictive low-level control

This dissertation focuses on the development, formalization, and systematic evaluation of a novel nonlinear model predictive control (MPC) concept with derivative-free optimization. Motivated by a real industrial application, namely the position control of a directional control valve, this control concept enables straightforward implementation from scratch, robust numerical optimization with a deterministic upper computation time bound, intuitive controller design, and offers extensions to ensure recursive feasibility and asymptotic stability by design. These beneficial controller properties result from combining adaptive input domain discretization, extreme input move-blocking, and the integration with common stabilizing terminal ingredients. The adaptive discretization of the input domain is translated into time-varying finite control sets and ensures smooth and stabilizing closed-loop control. By severely reducing the degrees of freedom in control to a single degree of freedom, the exhaustive search algorithm qualifies as an ideal optimizer. Because of the exponentially increasing combinatorial complexity, the novel control concept is suitable for systems with small input dimensions, especially single-input systems, small- to mid-sized state dimensions, and simple box-constraints. Mechatronic subsystems such as electromagnetic actuators represent this special group of nonlinear systems and contribute significantly to the overall performance of complex machinery. A major part of this dissertation addresses the step-by-step implementation and realization of the new control concept for numerical benchmark and real mechatronic systems. This dissertation investigates and elaborates on the beneficial properties of the derivative-free MPC approach and then narrows the scope of application. Since combinatorial optimization enables the straightforward inclusion of a non-smooth exact penalty function, the new control approach features a numerically robust real-time operation even when state constraint violations occur. The real-time closed-loop control performance is evaluated using the example of a directional control valve and a servomotor and shows promising results after manual controller design. Since the common theoretical closed-loop properties of MPC do not hold with input moveblocking, this dissertation provides a new approach for general input move-blocked MPC with arbitrary blocking patterns. The main idea is to integrate input move-blocking with the framework of suboptimal MPC by defining the restrictive input parameterization as a source of suboptimality. Finally, this dissertation extends the proposed derivative-free MPC approach by stabilizing warm-starts according to the suboptimal MPC formulation. The extended horizon scheme divides the receding horizon into two parts, where only the first part of variable length is subject to extreme move-blocking. A stabilizing local controller then completes the second part of the prediction. The approach involves a tailored and straightforward combinatorial optimization algorithm that searches efficiently for suboptimal horizon partitions while always reproducing the stabilizing warm-start control sequences in the nominal setup

A Study Model Predictive Control for Spark Ignition Engine Management and Testing

Pressure to improve spark-ignition (SI) engine fuel economy has driven thedevelopment and integration of many control actuators, creating complex controlsystems. Integration of a high number of control actuators into traditional map basedcontrollers creates tremendous challenges since each actuator exponentially increasescalibration time and investment. Model Predictive Control (MPC) strategies have thepotential to better manage this high complexity since they provide near-optimal controlactions based on system models. This research work focuses on investigating somepractical issues of applying MPC with SI engine control and testing.Starting from one dimensional combustion phasing control using spark timing(SPKT), this dissertation discusses challenges of computing the optimal control actionswith complex engine models. A nonlinear optimization is formulated to compute thedesired spark timing in real time, while considering knock and combustion variationconstraints. Three numerical approaches are proposed to directly utilize complex high-fidelity combustion models to find the optimal SPKT. A model based combustionphasing estimator that considers the influence of cycle-by-cycle combustion variations isalso integrated into the control system, making feedback and adaption functions possible.An MPC based engine management system with a higher number of controldimensions is also investigated. The control objective is manipulating throttle, externalEGR valve and SPKT to provide demanded torque (IMEP) output with minimum fuelconsumption. A cascaded control structure is introduced to simplify the formulation and solution of the MPC problem that solves for desired control actions. Sequential quadratic programming (SQP) MPC is applied to solve the nonlinear optimization problem in real time. A real-time linearization technique is used to formulate the sub-QP problems with the complex high dimensional engine system. Techniques to simplify the formulation of SQP and improve its convergence performance are also discussed in the context of tracking MPC. Strategies to accelerate online quadratic programming (QP) are explored. It is proposed to use pattern recognition techniques to “warm-start” active set QP algorithms for general linear MPC applications. The proposed linear time varying (LTV) MPC is used in Engine-in-Loop (EIL) testing to mimic the pedal actuations of human drivers who foresee the incoming traffic conditions. For SQP applications, the MPC is initialized with optimal control actions predicted by an ANN. Both proposed MPC methods significantly reduce execution time with minimal additional memory requirement