Optimization-based dynamic motion planning and control for quadruped robots

Abstract

Fast trajectory planning and control frameworks improve locomotion robustness against disturbances and uncertainties. In this paper, dynamic motions are optimized under the constraint of a decoupled spring-loaded inverted pendulum model. Subsequently, a hierarchical Quadratic Programming whole-body controller is employed to execute the planned trajectories while ensuring compliance with all physical feasibility constraints. Both the motion planner and the whole-body controller operate within the same high-frequency control loop. Furthermore, the unified whole-body controller governs all gait phases, including flight phases. The proposed algorithms are evaluated through simulation and real experiments, showcasing dynamic gaits such as hopping, twist jumping, and trotting in challenging environments. The algorithms demonstrate resilience against external disturbances and environmental uncertainties.

Appendix

1.1 Dimension reduction

Equation (20) can be directly split into two equations

$$\begin{aligned}{} & {} \textbf{M}_f\ddot{\textbf{q}}+\textbf{h}_f=\textbf{J}_{c,f}^\top \varvec{\lambda } \end{aligned}$$

(38)

$$\begin{aligned}{} & {} \textbf{M}_a\ddot{\textbf{q}}+\textbf{h}_a=\varvec{\tau }+\textbf{J}_{c,a}^\top \varvec{\lambda } \end{aligned}$$

(39)

where $\textbf{M}_f\in \mathbb {R}^{6\times (6+n)}$, $\textbf{h}_f \in \mathbb {R}^6$ and $\textbf{J}_{c,f} \in \mathbb {R}^{3\,m \times 6}$ denote the floating base components of the dynamic matrices while $\textbf{M}_a\in \mathbb {R}^{n\times (6+n)}$, $\textbf{h}_a \in \mathbb {R}^n$ and $\textbf{J}_{c,a} \in \mathbb {R}^{3\,m \times n}$ represent the actuated components. Apparently $\varvec{\tau }$ is determined by Eq. (39) as follows

$$\begin{aligned} \varvec{\tau }=\textbf{M}_a\ddot{\textbf{q}}+\textbf{h}_a-\textbf{J}_{c,a}^\top \varvec{\lambda } \end{aligned}$$

(40)

Therefore, the decision variable can be reduced to $\textbf{y}=\begin{bmatrix}\ddot{\textbf{q}}^\top&\varvec{\lambda }^\top \end{bmatrix}^\top$, and the physical feasibility constraint Eq. (28) becomes

$$\begin{aligned} \begin{bmatrix} -\textbf{h}_f \\ dot{\textbf{J}}_{c}\dot{\textbf{q}} \\ -\mathbf {\infty } \\ \varvec{\tau }_{\textrm{min}}-\textbf{h}_a\end{bmatrix} \le \begin{bmatrix}\textbf{M}_f &{} -\textbf{J}_{c,f}^\top \\ \textbf{J}_c &{} \textbf{0} \\ \textbf{0} &{} \textbf{C}_{\textrm{in}} \\ \textbf{M}_a &{} -\textbf{J}_{c,a}^\top \end{bmatrix}\textbf{y} \le \begin{bmatrix} -\textbf{h}_f \\ dot{\textbf{J}}_c\dot{\textbf{q}} \\ \textbf{0} \\ \varvec{\tau }_{\textrm{max}}-\textbf{h}_a\end{bmatrix} \end{aligned}$$

(41)

The dimensions of the cost functions also need to be modified using the reduced decision variable.