Finding initial values and parameters for a robot model to not fall over is a challenging task. This blog post describes a method from optimal control that finds values for the Unified Locomotion Template model such that it hops stable.
Introduction
Robots have a significant impact on our lives today and will play an ever-increasing role in the future. With this growing demand for robotic assistance, robots with legs are particularly interesting due to their excellent adaptability to the human environment. Still, building walking and hopping robots is a complex task. Therefore, the locomotion of humans and animals is studied and described in models. To analyze them, it is crucial to investigate stable cycles of the models. Therefore, this blog post describes a method on how to find stable gaits for the Unified Locomotion Template model.
Fundamentals
When it comes to the gaits of one-legged robots, terminology must first be clarified:
Every one-legged system goes through a stance phase and a flight phase while hopping forward. During the stance phase, the foot is attached to the ground. During the flight phase, the foot is in the air. The transition from stance phase to flight phase is called lift-off and from flight phase to stance phase touch-down. Starting from a fixed event, a gait cycle is defined until the event is reached again. Additionally, hopping models are hybrid systems. During the phases, the motion can be described as continuous dynamics. The transition events are discrete jumps. For example, the impact of the foot on the ground results in an instantaneous change in velocity.
Template models
Designing a robust, stable one-legged system is a complex task. Therefore, robotics uses concepts from nature. But modeling them is computationally expensive. Template models describe the behavior of a model leaving out all redundant information, like detailed mechanics or joint functions. Thus, template models reduce the complexity of the original model. Over the last years, various models have been developed to describe mainly the movement of the Center of Mass (CoM). Prominent examples are the Spring-Loaded Inverted Pendulum (SLIP) model proposed by Blickhan and the Virtual Pivot Point (VPP) controlled SLIP model as explained in Sharbafi et al..
Uniform Locomotion Template model
The different templates mainly describe only one certain part of the gait or need complex control mechanisms in a later step. To achieve a description of the whole gait cycle the Uniform Locomotion Template (ULT) model is developed by Ossadnik et al.. Different sub-functions from different template models are combined.
The ULT model consists of an upright trunk and a foot with non-negligible mass. The leg is modeled by a spring and is attached to a hip joint with a fixed offset to the center of mass.
Ten states and 14 parameters fully describe the motion of the ULT model. The state vector is given by the location of the center of mass and the foot, the trunk inclination angle, and their velocities. Summing up all forces and torques yields
\begin{equation}
\dot{\boldsymbol{x}} = \boldsymbol{f}(\boldsymbol{x}) =
\begin{bmatrix}
\dot{\boldsymbol{r}}_{CoM} \\
\dot{\boldsymbol{r}}_{foot} \\
{\dot{\theta}} \\
\ddot{\boldsymbol{r}}_{CoM} \\
\ddot{\boldsymbol{r}}_{foot} \\
{\ddot{\theta}} \\
\end{bmatrix}
=
\begin{cases}
\begin{bmatrix}
\dot{\boldsymbol{r}}_{CoM} \\
\boldsymbol{0} \\
{\dot{\theta}} \\
\frac{1}{m_{CoM}} \boldsymbol{F} + \boldsymbol{g} \\
\boldsymbol{0} \\
\frac{1}{J} (-\tau + ||\boldsymbol{r}_{leg}|| (F_y \sin(\theta) – F_x \cos(\theta))) \\
\end{bmatrix} & \text{stance phase} \\~\\
\begin{bmatrix}
\dot{\boldsymbol{r}}_{CoM} \\
\dot{\boldsymbol{r}}_{foot} \\
{\dot{\theta}} \\
\frac{1}{m_{CoM}} \boldsymbol{F} + \boldsymbol{g} \\
\frac{1}{m_{foot}} (- \boldsymbol{F} + \boldsymbol{F}_{\tau}) + \boldsymbol{g} \\
\frac{1}{J} (-\tau + ||\boldsymbol{r}_{leg}|| (F_y \sin(\theta) – F_x \cos(\theta))) \\
\end{bmatrix} & \text{flight phase}.
\end{cases}
\end{equation}
Furthermore, the model is controlled by a hip torque during the stance and flight phase. Additionally, during the flight phase, the leg length is changed.
The stance phase controller is equal to the Virtual Pivot Point Control. During the stance phase, an error-dependent setpoint controller is used.
Optimal control
In general, optimal control is a dynamic optimization problem that aims to find controls and states that minimize an objective function as explained in . Therefore, different families of methods like dynamic programming, indirect methods, and direct methods exist. Direct methods can be understood as a “first discretize, then optimize“-approach. In general, they transcribe the optimization problem into a Nonlinear Program (NLP). Due to its robustness and easy application, it is the most commonly used approach. et al.
To understand the ideas of direct methods, it is necessary to understand what an NLP is. The general constrained NLP is defined as a scalar objective function \(f\) that needs to be minimized with respect to the state vector \(\boldsymbol{x}\). Additionally, equality and inequality constraints have to be fulfilled by \(\boldsymbol{x}\). If the objective function and the constraining functions are nonlinear, the problem is called the Nonlinear Program. If the functions are additionally convex, every local minimum is a global minimum.
Single Shooting
Starting from a fixed initial guess, the dynamics are integrated to the end of the interval. The value from the integration and the required value then form a gap. The gap becomes a constraint in the NLP. Therefore, the NLP has only a few constraints. A disadvantage is that the NLP tends to become highly nonlinear. Especially for complex systems, it is thus difficult to find a solution.
Multiple Shooting
Multiple shooting fixes the hypersensitivity by dividing the interval into smaller sub-intervals called shoots. Instead of integrating the trajectory over the whole time horizon, the integration is now performed for every shoot starting from a fixed initial guess. At the end of every shot, the shooting gap has to be closed. This leads to a bigger NLP than the single shooting approach. But the reformulation of the function with more variables makes it less nonlinear. In summary, the main advantages of multiple shooting over single shooting are lower sensitivity and improved convergence properties.
Stability
In developing template models and legged robots, the stability of the gait is of particular interest. Therefore it is necessary to define stability mathematically like in Perko.
The Poincaré map, also called the first return map, is a tool to analyze a dynamic system by transforming a continuous-time system into a discrete-time system. Let \(\Gamma \) be a periodic orbit of a differential equation through the point x0 and \(\Gamma \) be an (n-1)-dimensional hyperplane transversal to \(\Sigma \) at \(\boldsymbol{x}_0\). Then, for any point \(\boldsymbol{x}_0\) in a sufficiently small neighborhood \(U \subset \Sigma\), the solution again crosses the hyperplane \(\Sigma\) at a point \(\boldsymbol{P}(\boldsymbol{x})\) because of the periodicity of the solution \(\boldsymbol{\phi}_t \). The hyperplane must always be crossed transversely and in the same direction. Therefore, the Poincaré map is defined as \(P: U \to \Sigma\)
\begin{equation}\label{eq:poincaremap}
\boldsymbol{P}(\boldsymbol{x}) = \boldsymbol{\phi}_\tau(\boldsymbol{x}_0) \ .
\end{equation}
The first time the curve intersects the hyperplane is denoted as \(\tau\), starting at the initial value \(\boldsymbol{x}_0\).
If a point is mapped onto itself by the Poincaré map, it is called a fixed point. Moreover, the stability of the fixed point also proves the stability of the solution. Therefore, the nonlinear system is linearized around the equilibrium point, which leads to
\begin{equation}\label{eq:taylorExpansion}
D\boldsymbol{P}(\boldsymbol{x}^*) = \frac{\partial \boldsymbol{P}(\boldsymbol{x}^*) }{\partial \boldsymbol{x}} \ .
\end{equation}
Applying Lyaunov’s indirect method then yields that the system is stable if all eigenvalues \(\lambda\) of the Jacobian of the Poincaré map \(D\boldsymbol{P}(\boldsymbol{x}^*)\) have a magnitude smaller than one. The eigenvalues are called Floquet multiplicators.
Problem formulation
The problem is mathematically formulated by
\begin{split} \text{find} \quad T_{stance}, \; T_{flight}, \; \boldsymbol{x}_i, \; \boldsymbol{p}& \quad \text{for i} \; \in \; [0, N] \\
\text{subject to} \quad \max(|\lambda(D\boldsymbol{P}(\boldsymbol{x}, \boldsymbol{p}))|) &< 1 \\
\quad \boldsymbol{x}(0) – \boldsymbol{x}(T) &= \boldsymbol{0} \quad \text{(periodicity)} \\
\dot{\boldsymbol{x}} – f(\boldsymbol{x}(t), \boldsymbol{p}) &= \boldsymbol{0} \quad \text{(differential equations)} \\
h(\boldsymbol{x}(t), \boldsymbol{p}) &\geq \boldsymbol{0} \quad \text{(inequality constraints)} \\
g(\boldsymbol{x}(t), \boldsymbol{p}) &= \boldsymbol{0} \quad \text{(equality constraints)}.
\end{split}
Parameters p, states x and time for the stance and flight phase are searched such that the following conditions are fulfilled:
- stability: as explained in the section about the mathematical definition of stability, all eigenvalues \(\lambda\) of the Jacobian of the Poincaré map have to lie within the unit cycle.
- periodicity: after one cycle the state vector has to be equal to the initial state vector to enforce a periodic behavior.
- differential equation: at every point, the differential equation of the ULT model has to be fulfilled.
- inequality constraints: the states and parameters of the ULT model are constrained by an upper and lower boundary. For example, the foot can not go through the ground. Therefore, the y-value of the foot always has to be greater than or equal to zero. The data for the boundaries are motivated by different biomechanical papers.
- equality constraints: the remaining equality constraints define that the shooting gap at the end of every sub-interval is closed.
Implementation
For the implementation, CasADi and NLOPT are used in MATLAB. CasADi is an open-source library for numerical optimization. It provides a symbolic framework to implement optimal control problems. Additionally, NLOPT provides a user-friendly interface for various algorithms to solve nonlinear optimization problems.
The problem is transcribed into an NLP with multiple shootings. As explained earlier the method is chosen for its robustness. The dynamics of the ULT model have discrete jumps in one period. Therefore, the stance phase and flight phase are handled separately and the lift-off and touch-down conditions are enforced at the corresponding time values. Since the times for the phases are not known in advance, they are determined by the optimization.
The resulting problem optimizes a non-differentiable, non-convex objective function.
Therefore, a two-level approach, similar to the one proposed by Mombauer et al., is chosen to solve the optimal control problem. The outer loop uses the Nelder-Mead algorithm, a direct simplex method for nonlinear optimization, provided by NLOPT. Parameters of the ULT model are adjusted until all Floquet multipliers lie within the unit cycle. The inner loop searches for initial values such that the periodicity condition is met. Multiple shooting is used to transcribe the problem into an NLP that in the next step is solved with the standard Interior-Point Optimizer. CasADi is used for the implementation. The outer loop starts with an initial guess for the parameters and performs one step of the direct search method. The parameters then are passed to the inner loop which then solves a complete optimization problem to find initial values that lead to periodic gaits. After that, the Floquet multipliers are computed. If all eigenvalues are smaller than one, the solver terminates. Otherwise, the outer loop again performs one step of the Nelder-Mead algorithm and the solver continues. The following figure shows the schematic illustration of the two-level solver.
Results
For different initial guesses, different stable solutions for the ULT model are found. Stability is proven by the Floquet multipliers and a visual investigation of the gait over 100 cycles. All multipliers lie within the unit cycle.
As expected, the simulation of the model shows that even after 100 steps the model does not fall over. In addition to that, the gait shows the periodicity of the state except for the x-values. As mentioned earlier, at the end of the period, the discrete jump of the velocity of the foot is visible.
Outlook
The article presents a method to find stable cycles for the ULT model. It is shown that with a two-level solver, periodicity and stability can be enforced by using a constant initial guess. Usually, not only stability but also robustness is an important criterion for legged robots. Therefore, a different objective function than the Floquet multipliers can be used. Hobbelen and Wisse describe a norm that correlates better with the actual disturbance rejection. The gait sensitivity norm (GSN) relates disturbances and gait indicators. For example, as a disturbance steps in the floor can be chosen. A sufficiently correlating gait indicator to this disturbance is the step time. By minimizing the dynamic response of the system in terms of the gait sensitivity norm, the robustness of the model increases.
With today’s increasing demand for robotic assistance, the gait of animals and humans has to be further analyzed to create stable and robust models that can serve as a basis for robots. Based on the above findings, further research on this topic is suggested to gain a deeper understanding of the development of legged robots.