Robust Optimization-Based Control and Planning for Legged Robots

Task-Space Inverse Dynamics (TSID) is a well-known optimization-based technique for the control of highly-redundant mechanical systems, such as humanoid robots. One of its main flaws is that it does not take into account any of the uncertainties affecting these systems: poor torque tracking, sensor noises, delays and model uncertainties. As a consequence, the resulting control-state trajectories may be feasible for the ideal system, but not for the real one. We propose to improve the robustness of TSID by modeling uncertainties in the joint torques, either as Gaussian random variables or as bounded deterministic variables. Then we try to immunize the constraints of the system to any—or at least most—of the realizations of these uncertainties. When the resulting optimization problem is too computationally expensive for online control, we propose ways to approximate it that lead to resolution times below 1 ms. Extensive simulations in a realistic environment show that the proposed robust controllers greatly outperform the classic one, even when other unmodeled uncertainties affect the system (e.g. errors in the inertial parameters, delays in the velocity estimates).

This talk is about tube-based model predictive control (MPC) for both linear and nonlinear continuous-time dynamic systems that are affected by time-varying disturbances. We derive a min-max differential inequality describing the support function of positive robust forward invariant tubes, which can be used to construct a variety of conservative, but computationally tractable, tube-based model predictive controllers. The implementation of these robust MPC controllers is based on an automatic C-code generation strategy for real-time nonlinear model predictive control. We illustrate the performance of the proposed methods with applications in the field of robotics.

Model Predictive Control (MPC) is a high performance control technique that has seen wide-spread success due to its ability to control constrained systems. Standard MPC formulations make use of a nominal model of the system to optimize future control inputs over a finite horizon. While under certain assumptions this strategy offers inherent robustness properties, an explicit characterization of the theoretical properties is generally not available. Robust MPC techniques extend the theory of MPC and explicitly consider uncertainties in the MPC design to guarantee constraint satisfaction and stability. While this offers the potential to address the issue of uncertainties in a constrained, or safety-critical environment, its practical success is challenged by computational requirements and performance limitations.

The first part of the talk will provide an introduction to robust MPC, where we will review the main method classes and discuss the central underlying technique of constraint tightening. Questions of computation and robustness in real-time environments will be addressed. The second part of the talk will focus on the challenge of characterizing uncertainties and the impact on closed-loop performance. We will present some of our recent results on improving performance and robustness by identifying uncertainties online and show techniques for maintaining the desired theoretical properties when the design assumptions on uncertainties are violated.

References:

E. D. Klenske, M. N. Zeilinger, B. Schölkopf and P. Hennig. Gaussian Process-Based Predictive Control for Periodic Error Correction. IEEE Transactions on Control Systems Technology, (2016)
A. K. Akametalu, J. F. Fisac, J. H. Gillula, S. Kaynama, M. N. Zeilinger and C. J. Tomlin. Reachability-based safe learning with Gaussian processes. IEEE Conference on Decision and Control, (2014)
M. N. Zeilinger, M. Morari and C. N. Jones. Soft Constrained Model Predictive Control With Robust Stability Guarantees. IEEE Transactions on Automatic Control, (2014)

In this talk we describe a technique for robustly estimating the dynamic variables of an articulated rigid body with whole-body distributed sensing. Within this context, we define dynamic variables as those quantities that describe the system dynamic evolution, including (internal and external) forces and torques, positions, velocities and accelerations (of the joints and of the reference frames attached to the rigid bodies). The estimation is performed within a Bayesian framework and, in particular, the proposed algorithm computes the a-posteriori estimate given all of the available sensors. The sensors that are incorporated in the model include gyroscopes, accelerometers, joint torque sensors, encoders, 6-axes force/torque sensors and distributed pressure sensor (sometimes referred also as artificial skin). Computational efficiency is improved by exploiting the problem structure and specifically the sparsity in the Newton-Euler equations describing an articulated rigid body. Estimation accuracy is improved by estimating the covariance of the probability distributions describing the available sensor measurements. Experiments for validating the proposed theoretical framework are performed on the iCub humanoid robot. The robot is equipped with distributed force, torque, linear acceleration and angular velocity sensors. The results prove that the proposed procedure is capable of estimating the robot dynamic variables achieving a significant improvement with respect to available measurements. In particular the proposed approach is shown to be capable of estimating quantities which are not directly measured and is also capable of improving the estimation of those quantities which are directly measured and affected by systematic errors.

Model based multi-parametric programming provides a complete map of solutions of an optimization problem as a function of, unknown but bounded, parameters in the model, in a computationally efficient manner, without exhaustively enumerating the entire parameter space. In a Model-based Predictive Control (MPC) framework, multi-parametric programming can be used to obtain the governing control laws – the optimal control variables as an explicit function of the state variables. The main advantage of this approach is that it reduces repetitive on-line control and optimization to simple function evaluations, which can be implemented on simple computational hardware, such as a microchip, thereby opening avenues for many applications in chemical, energy, automotive, and biomedical equipment, devices and systems.

In this presentation, we will first provide a historical progress report of the key developments in multi-parametric programming and control along with recent theoretical and algorithmic advances in explicit robust multi-parametric programming and control for discrete and continuous linear, quadratic and mixed integer systems. We will also present an overview of PAROC, a prototype software system which allows for the representation, modelling and solution of integrated scheduling and control problems. Its main features include: (i) a high-fidelity dynamic model representation, also involving global sensitivity analysis, parameter estimation and mixed integer dynamic optimization capabilities; (ii) a suite/toolbox of model approximation methods; (iii) a host of multi-parametric programming solvers for mixed continuous/integer problems; (iv) a state-space modelling representation capability for scheduling and control problems; and (v) an advanced control toolkit for robust multi-parametric/explicit MPC and moving horizon reactive scheduling problems. Algorithms that enable the integration capabilities of the systems for design, scheduling and control are presented along with applications in sustainable energy systems, smart manufacturing and personalized healthcare engineering.

References:

Pistikopoulos, E. N.; Diangelakis, N. A.; Oberdieck, R.; Papathanasiou, M. M.; Nascu, I.; Sun, M. PAROC - an Integrated Framework and Software Platform for the Optimization and Advanced Model-Based Control of Process Systems. Chemical Engineering Science, (2015)
Bemporad, A.; Morari, M.; Dua, V.; Pistikopoulos, E. N. The explicit quadratic regulator for constrained systems. Automatica, (2002)
Oberdieck, R.; Pistikopoulos, E. N. Explicit hybrid model-predictive control: The exact solution. Automatica, (2015)
Kouramas, K. I.; Panos, C.; Faisca, N. P.; Pistikopoulos, E. N. An algorithm for robust explicit/multi-parametric model predictive control. Automatica, (2013)

Several research groups recently experimentally demonstrated that optimization-based controllers and planners relying heavily on dynamic models could lead to very high performance behaviors on legged robots. This constitutes an important step because 1) it demonstrates that these techniques are (to some extend) robust to dynamic model inaccuracies, sensor noise and actuator bandwidth limitations and 2) it opens the way to more general controller formulations through optimization. However, despite these recent advances, robots are still very limited in performance and often lack robustness in face of unexpected events. In this presentation I will show our current research efforts in control, planning and estimation to address some of these limitations. In particular, using experimental results on our humanoid robot, I will show how optimization-based algorithms that plan for the right physical quantities can lead to major performance improvements. For example, focusing on the control of contact interactions rather than kinematics can lead to controllers with low impedance and more robust behaviors. Moreover, I will argue that estimation and sensor fusion algorithms, while still under-used in our field, are a necessity to improve effective control authority on legged robots. Based on our experimental results with humanoid robots, I will also discuss current important algorithmic limitations that need to be addressed in the future.

References:

Alexander Herzog, Nicholas Rotella, Sean Mason, Felix Grimminger, Stefan Schaal, Ludovic Righetti. Momentum Control with Hierarchical Inverse Dynamics on a Torque-Controlled Humanoid. Autonomous Robots, 2015.
Alexander Herzog, Nicholas Rotella, Stefan Schaal, Ludovic Righetti. Trajectory generation for multi-contact momentum-control. Humanoids, 2015.
Alexander Herzog, Stefan Schaal, Ludovic Righetti. Structured contact force optimization for kino-dynamic motion generation. arXiv:1605.08571, 2016.
Nicholas Rotella, Michael Bloesch, Ludovic Righetti, Stefan Schaal. State Estimation for a Humanoid Robot . IROS, 2014.
Nicholas Rotella, Alexander Herzog, Stefan Schaal, Ludovic Righetti. Humanoid Momentum Estimation Using Sensed Contact Wrenches. Humanoids, 2015.
Brahayam Ponton, Stefan Schaal, Ludovic Righetti. Risk sensitive nonlinear optimal control with measurement uncertainty. arXiv:1605.04344, 2016.

A lot of progress has recently been made in dynamic motion planning for humanoid robots. However this work has remained limited to simulation. Here we show that executing the resulting trajectories on a Darwin-OP robot, even with local feedback derived from the optimizer, does not result in stable movements. We then develop a new trajectory optimization method, adapting our earlier CIO algorithm to plan through ensembles of perturbed models. This makes the plan robust to model uncertainty, and leads to successful execution on the robot. We obtain 90% success rate in (dynamic) forward walking, 95% success rate in sideways walking, 80% success rate in turning, and a similarly high success rate in getting up from the floor (the robot broke before we quantified the latter). Even though the planning is still done offline, this work represents a significant step towards automating the tedious scripting of complex movements.

In this talk, we present a new model of biped locomotion which is composed of three linear pendulums (one per leg and one for the whole upper body) to describe stance, swing and torso dynamics. In addition to double support, this model has different actuation possibilities in the swing hip and stance ankle which could be widely used to produce different walking gaits. Thanks to linearity properties, the proposed model can provide a computationally fast platform for model predictive controllers to predict the future and consider meaningful inequality constraints. The proposed model produces human-like torque and ground reaction force profiles and thus, compared to point-mass models, it is more promising for precise control of humanoid robots. We also talk about a transformation which is formulated as an online optimization to produce all joint angles and center of mass excursion details. The transformed 3LP is much more human like and realistic, compared to 3LP itself. The role of this new model and the transformation will be further discussed in conventional optimization-based control architectures.

Unmodeled environmental parameters and system dynamics make it hard to deploy trajectory optimization because solutions fail, even if modeled parameters are well-calibrated. In legged robots, small errors in the idealized dynamics model may cause a robot to fall, even though the masses, link lengths, and foot-terrain friction parameters of that model may be well measured. This talk will describe recently developed methods for fast trajectory optimization along a fixed path in the presence of frictional contact, as well as iterative methods for automatically learning how to compensate for unmodeled errors by adjusting constraint margins.

In this introductory talk we describe the main features of the polynomial moment-sum-of-squares approach a.k.a. the Lasserre hierarchy for nonlinear optimal control. First, the original nonconvex problem is lifted to a linear hence convex optimization problem in cones of occupation measures (classical objects in Markov decision processes and dynamical systems, but maybe less standard in control and robotics). Second, this infinite-dimensional linear optimization problem is truncated and solved approximately (and sometimes exactly) with a family of finite-dimensional primal-dual semidefinite optimization problems of increasing size, with convergence guarantees. We conclude the talk with a discussion on how the current practical limitations of this mathematical technology can be addressed to solve control problems relevant in robotics.

References:

lecture notes on polynomial optimization

In this talk, we discuss a method for efficient computation of feedback control policies and corresponding regions of attraction for rigid body robots subject to contact, including impacts and Coulomb friction. Robust control and planning of legged robots requires methods capable of addressing the complex dynamic interactions resulting from contact with the environment. Discontinuities, both in the vector field and state evolutions pose significant challenges for traditional algorithms dependent on local gradients. By leveraging the Measure Differential Inclusion (MDI) formulation of contact dynamics, methods based on sums-of-squares (SOS) can generate Lyapunov certificates of stability and positive invariance. Importantly, this approach avoids enumeration of hybrid modes and, instead, scales efficiently with the number of potential contacts. The approach is demonstrated on simple models of legged robots, including automated control design of a balancing robot.

References:

Michael Posa, Mark Tobenkin, and Russ Tedrake. Stability Analysis and Control of Rigid-Body Systems with Impacts and Friction. To appear in the IEEE Transactions on Control (TAC), 2016.
Michael Posa, Mark Tobenkin, and Russ Tedrake. Lyapunov analysis of rigid body systems with impacts and friction via sums-of-squares. Intl. Conf. on Hybrid Systems: Computation and Control (HSCC), 2013.

“DSOS and SDSOS optimization” techniques are more scalable alternatives to sum of squares (SOS) optimization which instead of semidefinite programming rely on linear and second order cone programming. They have been successfully used (for example) to find a stabilizing controller for a model of a humanoid robot with 30 state variables, and can handle quartic polynomial optimization problems with 50-70 variables in the order of a few minutes. In this talk, we review the theoretical and numerical aspects of these algorithms and present new directions for improving their approximation quality in an iterative fashion.