Single Sequential Trajectory Optimization with Centroidal Dynamics and Whole-Body Kinematics for Vertical Jump of Humanoid Robot

Abstract

High vertical jumping motion, which enables a humanoid robot to leap over obstacles, is a direct reflection of its extreme motion capabilities. This article proposes a single sequential kino-dynamic trajectory optimization method to solve the whole-body motion trajectory for high vertical jumping motion. The trajectory optimization process is decomposed into two sequential optimization parts: optimization computation of centroidal dynamics and coherent whole-body kinematics. Both optimization problems converge on the common variables (the center of mass, momentum, and foot position) using cost functions while allowing for some tolerance in the consistency of the foot position. Additionally, complementarity conditions and a pre-defined contact sequence are implemented to constrain the contact force and foot position during the launching and flight phases. The whole-body trajectory, including the launching and flight phases, can be efficiently solved by a single sequential optimization, which is an efficient solution for the vertical jumping motion. Finally, the whole-body trajectory generated by the proposed optimized method is demonstrated on a real humanoid robot platform, and a vertical jumping motion of 0.5 m in height (foot lifting distance) is achieved.

1. Introduction

Humanoid robotics is a burgeoning research field that has great potential for technological advancement and human prosperity. Considerable research has been conducted on the walking and manipulation of humanoid robots, which enables a robot to move through complex terrains and perform dangerous and repetitive tasks [1,2,3,4]. To further explore the full potential of humanoid robots and equip them with the ability to execute highly dynamic tasks, researchers are delving deeper into the study of highly dynamic motions, such as vertical jumping.

A high vertical jump is a challenging task, and many researchers have been working on jumping motions using simple mechanisms or simulations [5,6,7,8,9,10,11,12]. In the last few years, few have realized vertical jumping of a real full-scale humanoid robot platform [13,14,15,16,17,18]. Apart from excellent actuation hardware optimization [14,15,19], dynamic motion control is the key to the success of vertical jumping. Among the current jumping motion control frameworks [13,15,16,17,18], a whole-body trajectory can generate an effective jumping motion during the launching phase [6,7,10,12,16,17], and online control algorithms are designed to compensate as much as possible for some motion errors caused by an inaccurate trajectory. Thus, a whole-body trajectory that satisfies whole-body dynamics constraints can reduce the design complexity of the control algorithms for vertical jump. Because of the complexity of the motions and the high degrees of freedom of a humanoid robot, trajectory optimization has emerged as the most widely used method for generating specified motion trajectories.

Trajectory optimization methods are not limited to the field of humanoid robot jumping and can be divided into three categories. (1) Direct use of the complete whole-body dynamics model for planning: This enables the robot to produce satisfactory trajectories for more complex motion tasks [20]. However, this method sometimes becomes intractable, especially when dealing with high-dimensional, complex robot models. (2) Reduced-order models, such as the spring-loaded inverted pendulum (SLIP), flywheel SLIP, and single rigid body model (SRBM), are used to plan jumping motions. Despite the success of SLIP, its point mass model completely ignores angular momentum [16,21]. Jump motions often involve body rotation, for which angular momentum is indispensable. To alleviate this issue, flywheel SLIP is a potential solution [12]. Furthermore, the assumption of massless legs enables the SRBM to successfully render gait and motion generation problems computationally tractable for quadruped robots [22,23,24]. Unfortunately, humanoid robots have more actuated joints in each leg than a quadruped robot, and hence, the assumption of massless legs is easily violated. (3) Another simplified model, known as kino-dynamic [25], uses centroidal dynamics to reduce the complexity of the whole-body dynamics while guaranteeing feasible kinematic motions. Centroidal dynamics efficiently introduce the angular momentum, which is beneficial for the generation of arbitrary jumping motions. Recently, an alternating kino-dynamic optimization planner has been proposed that iteratively solves two optimization subproblems (the centroidal dynamics and kinematics optimization) until they reach consensus, allowing it to effectively solve whole-body motions [10,26,27].

Specifically, alternating kino-dynamic optimization makes it easier to find a locally optimal solution. During one iteration process, one subproblem needs to perform tracking tasks for common variables (the center of mass (CoM); momentum, including both the angular and linear momentum; and foot position) that were optimized by the other subproblem. Each tracking task is treated equally and objectively. Therefore, it has to iterate many times to ensure the consistency of common variables, which is obviously time-consuming. For example, Budhiraja et al. [27] obtained the whole-body trajectory with high consistency using centroidal dynamics and kinematics after 10 iterations. Based on their work, we propose a single sequential kino-dynamic trajectory optimization method that is designed on the basis of an alternating kino-dynamic planner. This method adds cost functions to ensure that both subproblems come to an agreement on the common variables (CoM and momentum), but consistency in the foot position is not required. In this case, the whole-body trajectory is effectively generated by a single sequential optimization, which first solves the centroidal dynamics optimization and then solve the whole-body kinematics optimization, and it is undoubtedly faster than the alternating kino-dynamic optimization planner. Obviously, the optimization solution time will be further reduced so that the whole-body trajectory of jumping motions can be optimized online, and the jumping flexibility of the robot can be further improved.

In addition, the above studies [16,28] focused only on the trajectory optimization of the launching or landing phases, paying little attention to the flight phase. However, to achieve a high jump, the active retraction motion of the leg is essential in the flight phase, which is a problem that remains be solved. More importantly, when searching for the optimal motions for the launching phase and flight phase, it is necessary to know whether the foot is touching the environment and whether any forces are present. In other words, deciding which of the feet should be in contact at a given time is a matter of finding the contact schedule. A traditional approach is to use a soft-contact model, such as a spring–damper contact model [29], to approximate inherently hard contact surfaces. These virtual spring–damper models need to be exceptionally stiff to emulate a real surface. However, the abrupt contact force changes resulting from a foot hitting a rigid surface can impede the optimizer’s performance. Mixed-integer programming has proved to be an efficient tool to efficiently solve contact schedules, gait transitions, and motion [30,31]. It involves assigning contact to surfaces using mixed integers and can be combined with convex or mixed-integer convex dynamic models, and hence, the computation is large and complicated. In this article, the contact sequence can be defined in advance because a jumping motion with contact is relatively simple. Then, the contact sequence is combined with the linear complementarity condition [24,25,32], which can enforce that either the foot must be zero distance from the contact surface or the force must be zero.

In order to solve the above problems and obtain the whole-body trajectory of a vertical jumping motion, including the launching and flight phases, a single sequential kino-dynamic trajectory optimization method is proposed in this work. The brief structure of the optimization method is illustrated in Figure 1. As shown, the task description in the optimization framework includes the initial robot pose (CoM, joint position, and foot position) and final robot pose estimation, motion duration, knot point and re-specified contact sequence, which are important initialization variables for optimization problems. After inputting the initial robot pose, knot point, and other variables into the centroidal dynamics optimization, a set of CoM and momentum values are obtained that satisfy the centroidal dynamics and other feasible sets of dynamic constraints while minimizing the cost function ${\Phi }^{\mathrm{cen}}\left(·\right)$. The optimized CoM, momentum, and foot position are used as reference inputs for the whole-body kinematics optimization, and the initialization variables such as initial joint angle, the knot points, and other variables in the task description are also required. Then, the whole-body kinematics optimization is solved under the above inputs and kinematic constraints. More importantly, the cost function ${\Phi }^{\mathrm{kin}}\left(·\right)$ is deliberately added to ensure that both optimization problems come to an agreement on the common variables (CoM and momentum) without requiring consistency in the foot position. Moreover, cost functions ${\varphi }^{\mathrm{cen}}\left(·\right)$ and ${\varphi }^{\mathrm{kin}}\left(·\right)$ are used to penalize the state variables to discourage violent motions. Finally, a whole-body motion trajectory is obtained that satisfies both the centroidal dynamic and whole-body kinematic constraints, enabling the humanoid robot to achieve a high vertical jump. As for the landing phase, the landing controller designed for our team’s previous research [17] is adopted to ensure landing stability. The CoM trajectory that is solved by centroidal dynamics optimization is used as the reference input for the landing controller.

The contribution of this work is summarized as follows:

(1)

A single sequential kino-dynamic trajectory optimization framework is proposed to solve the optimal jumping motion problem. The whole-body trajectory is effectively generated by a single sequential optimization, which is an efficient solution.

(2)

This optimization framework can generate vertical jumping motions with launching and flight phases, which are essential for highly dynamic motions.

This paper is organized as follows. The single sequential kino-dynamic trajectory optimization method is introduced in Section 2. Section 3 discusses the simulation and experimental results. Finally, the conclusion is presented in Section 4.

2. Single Sequential Kino-Dynamic Trajectory Optimization

In this section, the single sequential kino-dynamic trajectory optimization method is introduced. The centroidal dynamics optimization and whole-body kinematics optimization are described separately.

2.1. Centroidal Dynamics Optimization

Suppose the contact surface between a humanoid robot (i.e., the robot model shown in Figure 1) and its environment is described by ${\mathbf{S}}_{N_s}$, which is a convex polygon with vertices ($c_{N_1}$, …, $c_{N_i}$, …, $c_{N_c}$) [33,34]. Moreover, the robot’s state can be represented by centroidal linear and angular momentums, and the rates of the linear and angular momentums are determined by the total external force and gravity. Therefore, the equations of motion of the whole robot are given by:

$$\dot{\mathbf{h}}=\left[\begin{array}{c}\dot{\mathbf{l}}\\ \dot{\mathbf{k}}\end{array}\right]=\left[\begin{array}{c}m\mathbf{g}\\ \mathbf{0}\end{array}\right]+\sum _{i=1}^{N_c}\left[\begin{array}{c}{\mathbf{f}}_i\\ \left({\mathbf{p}}_i-\mathbf{r}\right)\times {\mathbf{f}}_i\end{array}\right]$$

(1)

where m represents the mass of the robot.

Generally, researchers use the centroidal dynamics model (Equation (1)) to optimize the CoM and momentum and then combine it with online control algorithms to realize the desired motion [28,35]. To satisfy the real-time demands of an online algorithm, the above optimization approach prioritizes efficiency, often at the cost of trajectory accuracy. In our proposed framework, the centroidal trajectory is used as a reference input for the whole-body kinematic optimization. However, any error in the centroidal trajectory tends to become amplified when mapping the whole-body kinematics. This undoubtedly has a negative impact on highly dynamic jumping motions. Throughout this article, the higher-order dynamic expression of Equation (1) is referred to as the centroidal dynamics model to enhance trajectory accuracy.

The optimization variables for centroidal dynamics optimization include the position $\mathbf{r}$, velocity $\dot{\mathbf{r}}$, and acceleration $\ddot{\mathbf{r}}$ of the robot’s CoM; the angular momentum $\mathbf{k}$ and its time derivative $\dot{\mathbf{k}}$; the linear momentum $\mathbf{l}$ and its time derivative $\dot{\mathbf{l}}$; and the contact force ${\mathbf{f}}_i$ and its time derivative ${\dot{\mathbf{f}}}_i$. Here, we sample all these time-varying quantities at the knot point N, with time duration T between successive knot points. Additionally, the contact sequence ${\mathbf{c}}_i$ is pre-specified, which dictates the sequence in which contact points make and break contact with the ground (touch → off).

The centroidal dynamics optimization can be posed in the following manner:

$$\underset{\mathbf{h},{\mathbf{p}}_i}{min}\sum _{k=1}^N\left[{\varphi }_k^{\mathrm{cen}}\left(·\right)+{\Phi }_k^{\mathrm{cen}}\left(·\right)\right]$$

(2a)

$$\left[\begin{array}{c}\ddot{\mathbf{r}}\left[k\right]\\ \ddot{\mathbf{l}}\left[k\right]\\ \ddot{\mathbf{k}}\left[k\right]\end{array}\right]=\left[\begin{array}{c}\dot{\mathbf{l}}\left[k\right]/\phantom{\dot{\mathbf{l}}\left[k\right]\mathrm{m}}\mathrm{m}\\ \sum _{i=1}^{N_c}{\mathbf{f}}_i\left[k\right]\\ \sum _{i=1}^{N_c}\left[\begin{array}{c}\left({\dot{\mathbf{p}}}_i\left[k\right]-\dot{\mathbf{r}}\left[k\right]\right)\times {\mathbf{f}}_i\left[k\right]+\hfill \\ \left({\mathbf{p}}_i\left[k\right]-\mathbf{r}\left[k\right]\right)\times {\dot{\mathbf{f}}}_i\left[k\right]\hfill \end{array}\right]\end{array}\right]$$

(2b)

$$\dot{\mathbf{r}}\left[k+1\right]-\dot{\mathbf{r}}\left[k\right]=\ddot{\mathbf{r}}\left[k\right]\Delta t,\mathbf{r}\left[k+1\right]-\mathbf{r}\left[k\right]=\dot{\mathbf{r}}\left[k\right]\Delta t$$

(2c)

$$\dot{\mathbf{l}}\left[k+1\right]-\dot{\mathbf{l}}\left[k\right]=\ddot{\mathbf{l}}\left[k\right]\Delta t,\mathbf{l}\left[k+1\right]-\mathbf{l}\left[k\right]=\dot{\mathbf{l}}\left[k\right]\Delta t$$

(2d)

$$\dot{\mathbf{k}}\left[k+1\right]-\dot{\mathbf{k}}\left[k\right]=\ddot{\mathbf{k}}\left[k\right]\Delta t,\mathbf{k}\left[k+1\right]-\mathbf{k}\left[k\right]=\dot{\mathbf{k}}\left[k\right]\Delta t$$

(2e)

$${\dot{\mathbf{f}}}_i\left[k+1\right]-{\dot{\mathbf{f}}}_i\left[k\right]={\ddot{\mathbf{f}}}_i\left[k\right]\Delta t,{\mathbf{f}}_i\left[k+1\right]-{\mathbf{f}}_i\left[k\right]={\dot{\mathbf{f}}}_i\left[k\right]\Delta t$$

(2f)

$$\sqrt{{\left(f_i^x\left[k\right]\right)}^2+{\left(f_i^y\left[k\right]\right)}^2}\le \mu {\left(f_i^z\left[k\right]\right)}^2,f_i^z\left[k\right]\ge 0$$

(2g)

$$f_i^z\le f_{max}{\mathbf{c}}_i\left[k\right]$$

(2h)

$$\left({\mathbf{p}}_i^{xy}\left[k+1\right]-{\mathbf{p}}_i^{xy}\left[k\right]\right){\mathbf{c}}_i\left[k\right]=0$$

(2i)

$${\mathbf{p}}_i^z\left[k\right]-h_{terrain}\ge 0$$

(2j)

$${\ell }^{min}\le {∥\mathbf{p}\left[k\right]-\mathbf{r}\left[k\right]∥}^2\le {\ell }^{max}$$

(2k)

$$\mathcal{F}\left({\mathbf{p}}_i\left[k\right]\right)\in {\mathbf{S}}_{N_s}$$

(2l)

The quadratic costs (see Equation (2a)) include a running cost ${\varphi }_k^{\mathrm{cen}}\left(·\right)$ and consensus cost ${\Phi }_k^{\mathrm{cen}}\left(·\right)$. Running cost ${\varphi }_k^{\mathrm{cen}}\left(·\right)$ is composed of user-defined tack costs, which penalize control variable ${\stackrel{⃛}{\mathbf{f}}}_i$ to discourage violent motions. It can be defined as follows:

$${\varphi }_k^{\mathrm{cen}}\left(·\right)=\sum _{i=1}^{N_c}{∥{\stackrel{⃛}{\mathbf{f}}}_i\left[k\right]∥}_{{\mathbf{Q}}_{cf}}^2$$

(3)

where ${∥\mathbf{x}∥}_Q^2$ is an abbreviation of quadratic cost ${\mathbf{x}}^{\mathrm{T}}\mathbf{Q}\mathbf{x}$, $\mathbf{Q}\ge 0$, and ${\mathbf{Q}}_{cf}\in {\mathbb{R}}^{6\times 6}$ is a force weight matrix. Additionally, square bracket $\left[k\right]$ indicates the sampled value at knot point k.

Additionally, consensus cost ${\Phi }_k^{\mathrm{kin}}\left(·\right)$ can penalize deviations in the CoM and momentum from the given references. It is expressed as follows:

$${\Phi }_k^{\mathrm{cen}}\left(·\right)={∥\mathbf{r}\left[k\right]{-}^{\mathrm{ref}}\mathbf{r}\left[k\right]∥}_{{\mathbf{Q}}_{cr}}^2+{∥\mathbf{h}\left[k\right]{-}^{\mathrm{ref}}\mathbf{h}\left[k\right]∥}_{{\mathbf{Q}}_{ch}}^2+\sum _{i=1}^{Nc}\left({∥\mathbf{r}\left[k\right]-{\mathbf{p}}_i\left[k\right]∥}_{{\mathbf{Q}}_{ce}}^2\right)$$

(4)

where ${}_{}{}^{\mathrm{ref}}\mathbf{r}_{}^{}$ and ${}_{}{}^{\mathrm{ref}}\mathbf{h}_{}^{}$ are the references obtained by linear interpolation from the initial value and final value estimate, respectively. ${\mathbf{Q}}_{cr}\in {\mathbb{R}}^{3\times 3}$, ${\mathbf{Q}}_{ch}\in {\mathbb{R}}^{3\times 3}$, and ${\mathbf{Q}}_{ce}\in {\mathbb{R}}^{3\times 3}$ are weight matrices.

Centroidal dynamics optimization includes the following constraints:

Integration constraint: The constraints of the centroidal dynamics include dynamic (Equation (2b)) and integration constraints (Equation (2c,f)). Note that the forward-Euler integration [36] is adopted to approximate the time derivatives for the variables $\mathbf{r}$, $\dot{\mathbf{r}}$, $\mathbf{l}$, $\dot{\mathbf{l}}$ and $\mathbf{k}$, $\dot{\mathbf{k}}$, ${\mathbf{f}}_i$, ${\dot{\mathbf{f}}}_i$, and $\Delta t$ indicates time duration.

Frictional constraint: The contact force is limited to the inside of the friction cone (friction coefficient $\mu$) to prevent the contact point from sliding, as expressed in Equation (2g). Additionally, the normal forces of the contact force have maximum limits during the launching phase and are always equal to zero during the flight phase. After combining the contact sequence, the constraint on the normal force is expressed by Equation (2h).

Terrain constraint: Referring to the complementarity constraint [24,25], we also impose a constraint that specifies that the foot in contact with the ground does not slide during the launching phase. The tangential displacement between two knot points must be zero when the foot is in contact. This constraint disappears during the flight phase, as shown in Equation (2i,j); $h_{terrain}$ represents the height of the ground.

Distance constraint: The distance constraint (see Equation (2k)) between the CoM and foot position should be within the predefined range ${\ell }^{min}$ and ${\ell }^{max}$. The maximum value ${\ell }^{max}$ is used to limit the overall length of the robot when stretched, which prevents some joints from approaching singularity. Similarly, the minimum value is used to prevent the robot from contracting too much during the flight phase.

Vertices constraint: The relative positions between the vertices ($c_1$, …, $c_{N_c}$) of the foot contact surface ${\mathbf{S}}_{N_s}$ are fixed to form a virtual, fixed-shaped sole (Equation (2l)).

This optimization solves the momentum and contact forces in the launching and flight phases. However, it completely ignores the whole-body kinematics of the robot. How to map these to the whole-body trajectory after accurate centroidal trajectories have been obtained is worth researching.

2.2. Whole-Body Kinematics Optimization

In this section, the whole-body kinematics optimization is considered to be aligned with the centroidal dynamics optimization results, and the whole-body trajectory for the jumping motion is optimized.

Because both optimization problems want to be consistent in terms of common variables (CoM, momentum, and foot position), it is necessary to establish a kinematic model. This article mainly describes the calculation process of momentum, but the expressions of CoM and foot position are not described in detail. A humanoid robot can be modeled as a system consisting of $n+1$ interconnected links and n joints, as shown in Figure 2c. The symbol ${\sum }_{\mathrm{w}}$ represents the world coordinate frame, and ${\sum }_1$ represents the float base coordinate frame. The left joints of the robot are represented as ${\sum }_{2\sim 7}^{\mathrm{L}}$ and ${\sum }_{14}^{\mathrm{L}}$, and the right joints are represented as ${\sum }_{8\sim 13}^{\mathrm{R}}$ and ${\sum }_{15}^{\mathrm{R}}$. Through the recursive relationship between link j and its predecessor link $p\left(j\right)$, the spatial speed of link j is expressed as follows:

$${\mathbf{v}}_j=\left[\begin{array}{c}{\omega }_j\\ {\upsilon }_j\end{array}\right]={}_{}{}^j\mathbf{X}_{p\left(j\right)}^{}{\mathbf{v}}_{p\left(j\right)}+{\mathbf{\Phi }}_j{\dot{\mathbf{q}}}_j$$

(5)

where ${\omega }_j$ and ${\upsilon }_j$ represent the angular and linear velocities of link j, respectively, in the link coordinate frame; ${}_3{}^j\mathbf{X}_{p\left(j\right)}^2$ is a $6\times 6$ spatial transform that converts spatial motion vectors from the predecessor $p\left(j\right)$ to j coordinates. The matrix ${\mathbf{\Phi }}_j$ depends on the type of joint and has full column rank, ${\dot{\mathbf{q}}}_j$ signifies the velocity vector of link j relative to the velocity of its predecessor $p\left(j\right)$.

The spatial transform ${}_{ }{}^j\mathbf{X}_{p\left(j\right)}^{ }$ can be composed from the position vector ${ }^{{ }_{p\left(j\right)}}{\mathbf{p}}_j$, which originates from coordinate frame $p\left(j\right)$ to the j coordinate, and the $3\times 3$ rotation matrix ${ }^j{\mathbf{R}}_{p\left(j\right)}$ from coordinate frame $p\left(j\right)$ to j:

$${ }^i{\mathbf{X}}_{p\left(j\right)}=\left[\begin{array}{cc}{ }^i{\mathbf{R}}_{p\left(j\right)}& \mathbf{0}\\ { }^i{\mathbf{R}}_{p\left(j\right)}\mathbf{S}{\left({ }^{{ }_{p\left(j\right)}}{\mathbf{p}}_j\right)}^{\mathrm{T}}& { }^i{\mathbf{R}}_{p\left(j\right)}\end{array}\right]$$

(6)

where ${ }^{{ }_{p\left(j\right)}}{\mathbf{p}}_j$ represents the position vector from coordinate frame $p\left(j\right)$ to the j coordinate frame, and $\mathbf{S}\left({ }^{{ }_{p\left(j\right)}}{\mathbf{p}}_j\right)$ represents the transformation of the position vector ${ }^{{ }_{p\left(j\right)}}{\mathbf{p}}_j$ into a skew-symmetric matrix.

The spatial momentum of the each link can be calculated from the spatial velocity as follow:

$${\mathbf{h}}_j=\left[\begin{array}{c}{\mathbf{k}}_j\\ {\mathbf{l}}_j\end{array}\right]={\mathbf{I}}_j{\mathbf{v}}_j$$

(7)

where ${\mathbf{k}}_j$ is the angular momentum, ${\mathbf{l}}_j$ represents the linear momentum, and ${\mathbf{I}}_j$ is the spatial inertia, all in reference to coordinate frame j.

Gathering all of the link velocities and joint velocities together, the system Jacobian ${\mathbf{J}}_j$ can be defined to give the relationship between the two:

$${\mathbf{v}}_j={\mathbf{J}}_j\dot{\mathbf{q}}$$

(8)

The total momentum of a humanoid can be calculated by adding up all the angular and linear momentums contributed by the individual link segments, which can be described as follow:

$$\mathbf{h}=\sum _{j=1}^N{ }^i{\mathbf{X}}_G^T{\mathbf{h}}_j=\sum _{j=1}^N{ }^j{\mathbf{X}}_G^T{\mathbf{I}}_j{\mathbf{J}}_j\dot{\mathbf{q}}={\mathbf{A}}_G\dot{\mathbf{q}}$$

(9)

where ${ }^j{\mathbf{X}}_G$ denotes a transformation matrix that links the j coordinate from the centroidal frame, ${\mathbf{A}}_G$ is the centroidal angular momentum matrix, and $\dot{\mathbf{q}}$ denotes the joint velocity vector.

The total momentum of a humanoid robot is described as follows:

$$\mathbf{h}={\mathbf{A}}_{\mathbf{G}}\dot{\mathbf{q}}$$

(10)

Consequently, the kinematics centroidal momentum is obtained. Next, the whole-body kinematic optimization problem is described in detail. The robot’s states are used to determine decision variables, including joint position $\mathbf{q}$, joint velocity $\dot{\mathbf{q}}$, and joint acceleration $\ddot{\mathbf{q}}$, which are employed as the control values. Here, we utilize the same knot points N, time durations T, and contact sequence ${\mathbf{c}}_i$. The whole-body kinematic optimization is formulated as follows:

$$\underset{ \mathbf{q},\dot{\mathbf{q}},\ddot{\mathbf{q}}}{min}\sum _{k=1}^N\left[{\varphi }_k^{\mathrm{kin}}\left(·\right)+{\Phi }_k^{\mathrm{kin}}\left(\begin{array}{c}\mathbf{h}\left[k\right]-{ }^{\mathrm{cen}}\mathbf{h}\left[k\right]\\ {\mathbf{p}}_i\left[k\right]-{ }^{\mathrm{cen}}{\mathbf{p}}_i\left[k\right]\end{array}\right)\right]$$

(11a)

$$\mathbf{h}\left[k\right]={\mathbf{A}}_G\left(\mathbf{q}\left[k\right]\right)\dot{\mathbf{q}}\left[k\right]$$

(11b)

$$\dot{\mathbf{q}}\left[k+1\right]-\dot{\mathbf{q}}\left[k\right]=\ddot{\mathbf{q}}\left[k\right]\Delta t,\mathbf{q}\left[k+1\right]-\mathbf{q}\left[k\right]=\dot{\mathbf{q}}\left[k\right]\Delta t$$

(11c)

$${\dot{\mathbf{q}}}_{\min }\le \dot{\mathbf{q}}\left[k\right]\le {\dot{\mathbf{q}}}_{\max },{\mathbf{q}}_{\min }\le \mathbf{q}\left[k\right]\le {\mathbf{q}}_{\max }$$

(11d)

$${\dot{\mathbf{q}}}_{\mathrm{L}}\left[k\right]={\dot{\mathbf{q}}}_{\mathrm{R}}\left[k\right]$$

(11e)

$$\left({\mathbf{p}}_i^{xy}\left[k\right]-{\mathbf{p}}_i^{xy}\left[k\right]\right){\mathbf{c}}_i\left[k\right]=0$$

(11f)

$${\mathbf{p}}_i^z\left[k\right]-h_{terrain}\ge 0$$

(11g)

$${\ell }^{min}\le {∥\mathbf{p}\left[k\right]-\mathbf{r}\left[k\right]∥}_2\le {\ell }^{max}$$

(11h)

The quadratic cost (see Equation (11a)) includes a user-defined cost ${\varphi }_k^{\mathrm{kin}}\left(·\right)$ and consensus cost ${\Phi }_k^{\mathrm{kin}}$. In detail, the user-defined cost ${\varphi }_k^{\mathrm{kin}}\left(·\right)$ is used to minimize joint acceleration, which can be expressed as follows:

$${\varphi }_k^{\mathrm{kin}}\left(·\right)={∥\ddot{\mathbf{q}}\left[k\right]∥}_{{\mathbf{Q}}_q}^2$$

(12)

where ${\mathbf{Q}}_{\mathbf{q}}\in {\mathbb{R}}^{n+6\times n+6}$ denotes the weight matrix.

The consensus cost ${\Phi }_k^{\mathrm{kin}}$ penalizes deviations in the CoM position ${ }^{\mathrm{cen}}\mathbf{r}$, centroidal momentum ${ }^{\mathrm{cen}}\mathbf{h}$, and foot positions ${ }^{\mathrm{cen}}{\mathbf{p}}_i$ from the solution of the centroidal momentum, which enforces consistency between the dynamic and whole-body kinematic problems. It can be set as follows:

$${\Phi }_k^{\mathrm{kin}}\left(·\right)={∥\mathbf{h}\left[k\right]-{ }^{\mathrm{cen}}\mathbf{h}\left[k\right]∥}_{{\mathbf{Q}}_{fh}}^2+{∥\mathbf{r}\left[k\right]-{ }^{\mathrm{cen}}\mathbf{r}\left[k\right]∥}_{{\mathbf{Q}}_{fr}}^2+{∥{\mathbf{p}}_i\left[k\right]-{ }^{\mathrm{cen}}{\mathbf{p}}_i\left[k\right]∥}_{{\mathbf{Q}}_{fp}}^2$$

(13)

where $\mathbf{h}$, $\mathbf{r}$, and ${\mathbf{p}}_i$ are calculated by the floating base kinematics, and ${\mathbf{Q}}_{fh}\in {\mathbb{R}}^{6\times 6}$, ${\mathbf{Q}}_{fr}\in {\mathbb{R}}^{3\times 3}$, and ${\mathbf{Q}}_{fh}\in {\mathbb{R}}^{3\times 3}$ are weight matrices.

Integration constraint: The constraints of the whole-body kinematic include the dynamical constraint (see Equation (11b)) and integration constraint (see Equation (11c)).

Joint constraint: The joint position should be limited within ${\mathbf{q}}_{\min }$ and ${\mathbf{q}}_{\max }$ to avoid exceeding the joint position limits. Similarly, joint velocities should also be restricted (Equation (11d)). More importantly, ${\mathbf{q}}_{\mathrm{L}}$ represents the joint positions of the left leg and the left arm, and ${\mathbf{q}}_{\mathrm{R}}$ represents the joint positions of the right leg and the right arm. The joint velocities of ${\dot{\mathbf{q}}}_{\mathrm{L}}$ and ${\dot{\mathbf{q}}}_{\mathrm{R}}$ should be consistent because of the symmetry of the robot (Equation (11e)).

Terrain constraint: The tangential displacement between the two knot points must be zero when the foot does not contact the ground. This constraint disappears during the flight phase. Equation (11f,g) guarantee that the foot position remains above the terrain surface.

Distance constraint: The distance constraint is expressed in Equation (11h). By tracking the reference variables provided by the centroidal dynamics optimization, a high vertical jumping trajectory can be obtained through whole-body kinematic optimization.

3. Simulation and Experimental Results

3.1. Validation Setup

The humanoid robot platform, model parameters, and solution for the optimization problem are described below.

The proposed optimization method was demonstrated on a humanoid robot, as shown in Figure 2. This humanoid robot has a total mass of 42.6 kg, a height of 1.5 m, and 14 degrees of freedom. The hip and arm joints are driven by a quasi-drive actuator composed of a brushless DC motor and a harmonic reducer, whereas the knee and ankle joints are driven by a brushless DC motor with ball screw structures. These joints can smoothly and instantaneously switch between position mode and torque mode [17]. There are two six-axis force/torque sensors (M3714B2, Sunrise Instruments, Canton, MI, USA) that measure contact force/torque. Furthermore, a Modbus communication system is established using EtherCAT, with a control cycle time of 1 ms. The model parameters of the humanoid robot are derived from the robot CAD model. Considering the large number of robot connecting rods, the masses and inertias of these links are not listed separately.

The FMINCON function in MATLAB is used to construct and solve the single sequential kino-dynamic trajectory optimization problem for jumping behaviors [36]. This tool allows for flexibility in defining system dynamics, path constraints, and boundary constraints as per user requirements. Additionally, the optimization program is executed on a quadcore computer (Intel Core i7-7700, 3.60 GHz).

3.2. Numerical Optimization

Using the single sequential kino-dynamic trajectory optimization, a vertical jumping trajectory with a vertical height of 0.5 m (foot lifting distance) was generated that includes both the launching and flight phases. The discrepancies in consistency between the centroidal dynamics and whole-body kinematics, especially regarding momentum and foot position, are discussed in detail. In addition, the main parameters used in the optimization process can be found in

Table 1. Main parameters for the single sequential kino-dynamic trajectory optimization.

Main Parameter	Value
Knot points, N	20
Motion duration, T	1 s
Contact sequence of $c_1$, $c_2$, … and $c_8$	[$1,\dots ,1,0,0,0,0,0,0,0$]
Optimality tolerance	<1 × 10−5
${\mathbf{Q}}_{cf}$	diag (0.001, 0.001, 0.001, …, 0.001, 0.001, 0.001)
${\mathbf{Q}}_{cr}$	diag (40, 40, 40)
${\mathbf{Q}}_{ch}$	diag (30, 10, 10, 10, 50, 10)
${\mathbf{Q}}_{ce}$	diag (0.01, 0.01, 0.01, …, 0.01, 0.01, 0.01)
${\mathbf{Q}}_q$	diag (0.0001, 0.0001, 0.0001, …, 0.0001)
${\mathbf{Q}}_{fh}$	diag (1, 1, 1, 1, 1, 1)
${\mathbf{Q}}_{fr}$	diag (0.01, 0.01, 0.01)
${\mathbf{Q}}_{fp}$	diag (0.02, 0.02, 0.02, …, 0.02, 0.02, 0.02)
${\mathbf{Q}}_{fe}$	diag (0.01, 0, 0.001, …, 0.01, 0, 0.001)

.

Table 2. Numerical optimization of main parameters.

Optimization Project	Centroidal Dynamics	Whole-Body Kinematics
Number of variables	720	1200
Duration of motion	1 s	1 s
Number of equality constraints	1036	720
Number of inequality constraints	3102	4250
Solving time	0.230 s	3.2 s
Number of iterations	12	35

lists the main parameters for the numerical optimization, such as the number of variables and the number of equality constraints, and also presents the solving times for both optimization problems. The solving times for centroidal dynamics and whole-body kinematics optimization are 0.230 s and 3.2 s, respectively. In addition, the trend of the cost value with iterations is shown in Figure 3. As the number of iterations increases, the cost value tends to be stable, indicating that the optimization problem is approaching convergence. Therefore, it is evident that this optimization process is highly efficient and takes little time.

From Figure 4 and Figure 5, the consistency of the momentum and CoM at each knot is high. Ensuring the consistency of momentum and CoM is key when bridging the centroidal dynamic and whole-body kinematic optimization, and hence, the relevant weight matrices ${\mathbf{Q}}_{fr}$ and ${\mathbf{Q}}_{fh}$ in Equation (13) are adjusted to maintain momentum consistency. Notably, the consistency of the foot position is poor (see Figure 6), and even the motion trends in the x-direction are quite different. This observed phenomenon can be attributed to the centroidal dynamics model, which focuses primarily on the relative position between the CoM and the foot position without introducing whole-body kinematic constraints. Because the optimized foot position might not meet the whole-body kinematic constraints, it is treated merely as a reference trajectory during the whole-body optimization, and high consistency is not pursued.

Figure 6 and Figure 7 presents the positions and contact forces at the contact point $c_1$. When this point is touching the ground, there is obviously no position sliding and the normal contact force exists; during the flight phase, the contact force is zero and the position changes. These fully meet the complementarity conditions (see Equations (2h,i) and (11f)). This result verifies the effectiveness of the complementary conditions and its combination with the re-specified contact sequence. Finally, the whole-body trajectory including the launching and flight phases is obtained.

To sum up, the whole-body trajectory of the vertical jump can be optimized using a single sequential optimization, and it is an efficient solution while saving time. After solving the optimization problem, the state values and control values that satisfy the constraints of the centroidal dynamics and whole-body kinematics are obtained. Subsequently, the whole-body trajectory is derived by utilizing a spline function to interpolate the states $\mathbf{q}$ between two knot points, which were used for the simulation and experiment (see Figure 13).

3.3. Simulations

The effectiveness of the optimized trajectory was verified using CoppeliaSim dynamics software. During the launching and flight phases, the whole-body trajectory was performed in the joint position mode of the robot. Then, the joints smoothly switched from position mode to torque mode. For the landing phase, a landing controller that had been designed for our team’s previous research [17] was adopted. Here, the CoM trajectory was derived from the centroidal dynamics optimization.

Figure 8 shows a snapshot of the jumping simulation. The humanoid robot was able to achieve a stable launch as the planned jumping motion. Figure 9 illustrates the tracking of the CoM position and angular momentum in the launching phase. Obviously, the CoM tracks well in the z and x directions, and the maximum tracking error is less than 3 mm when taking off in the x direction, which is well within tolerance. In addition, the angular momentum around the y-axis also has a good tracking effect, and the angular momentum is close to the reference value at the moment of take-off.

Additionally, the robot has an obvious leg retraction motion in the first half of the flight phase, which results in a high jump (foot lifting distance). Figure 10 depicts the absolute positions of the CoM and foot as well as their relative distance. The CoM rises by 0.30 m, and the foot retracts concurrently by 0.18 m. This coordinated motion enables the robot to achieve a jump height of 0.48 m from the ground. According to Equation (4) in Section 2.2, the relative distance between the foot and CoM can be adjusted by changing the weight matrix ${\mathbf{Q}}_{ce}$. In addition, the arm’s motion can be leveraged more effectively by adjusting the weight matrix ${\mathbf{Q}}_q$, which prevents excessive trunk bending and ensures a stable posture during both the launching and flight phases.

3.4. Experiments

The same whole-body trajectory used in the simulation was also executed on a real humanoid robot, as depicted in Figure 11. Clearly, the robot can achieve stable launching similar to the simulation, then retract its legs during the flight phase to achieve a 0.5 m (foot lifting distance) vertical jump, and subsequently maintain a stable posture until landing.

Figure 12 illustrates tracking of the CoM position and momentum in the launching phase. The joint angle and velocity are filtered by a low-pass filter, and the CoM position and momentum data are effectively computed through the forward kinematic and momentum equation. The actual CoM position tracks well in the z direction. Moreover, the tracking effectiveness in the x direction is not satisfactory, but the general trend is roughly the same. More importantly, the actual momentum around the y-axis is tracked satisfactorily in the initial 0.4 s, and then the actual angular momentum rises rapidly to about 2.5 kg · m²/s and then rises sharply to 0.2 kg · m²/s at the moment of take-off.

The reason for this phenomenon may be poor tracking of the joint angle and velocity, particularly in the late launching phase. Taking the joint motion of the robot’s left part as an example, Figure 13 and Figure 14 illustrate, respectively, the tracking performance of the joint angle and velocity in the launching and flight phases. There are certain tracking errors in the angles and velocities of the hip, knee, and ankle joints, coupled with the fact that the actual contact force in the x-direction is much larger than desired (the desired contact forces at the contact surface vertices are converted into a six-dimensional force at the footplate [33], and the results are shown in Figure 15), which together make the robot move forward in the x-direction. As a result, the pressure center of the foot gradually moves toward the toes, causing the heel to lift. The heel is raised about 2∼3 cm in height, as can be seen in Figure 11. This may lead to fluctuation in the contact force and contact torque at about s, which, in turn, causes poor tracking of the angular momentum.

Figure 13. Joint angles in the launching and flight phases of the experiment.

Figure 13. Joint angles in the launching and flight phases of the experiment.

Figure 13 and Figure 14 also compare the optimized whole-body trajectory with those of our previous research [17]. (The joint trajectories collected in the dynamic simulation are combined with reaction momentum control (RMC) [37] to generate the whole-body trajectory for the launching and flight phases. All jump heights are 50 cm, and the joint trajectories are represented by the icon labeled ’TRO’.) The maximum speeds of the hips, knees, and ankles decrease by varying degrees, and the velocity curve is smoother. Therefore, the proposed optimization method not only generates more usable trajectories but also obtains them more quickly.

The experimental results show that the optimized trajectory successfully realizes the desired vertical jumping motion during the launching and flight phases, and hence, the effectiveness of the proposed method has been verified.

4. Conclusions

High vertical jumping motion directly reflects a humanoid robot’s extreme motion capabilities and enables it to leap over obstacles. This article proposed a novel sequential dynamic trajectory optimization method that sequentially optimizes the centroidal dynamics and whole-body kinematics to solve the whole-body trajectory for high vertical jump motions. The proposed method is an efficient solution strategy that significantly reduces the solving time: giving a total optimization time of approximately 3.43 s. To validate the effectiveness of the single sequential kino-dynamic trajectory optimization, the optimized whole-body trajectory was verified through simulation and experimentation on a humanoid robot platform. The results show that the CoM position and momentum during the launching phase are satisfactorily tracked; the humanoid robot can achieve a stable vertical jump of 0.5 m. After comparing the optimized trajectory with the trajectory collected in the dynamic simulation and RMC, we found that the maximum joint velocity decreases by varying degrees, demonstrating that the optimized trajectory is more suitable for jump motion planning. The proposed method can be applied to a bipedal robot, and it is suitable for humanoid robots, which have high degrees of freedom. In the future, the proposed method will be enhanced to achieve online whole-body trajectory optimization and provide more practical whole-body trajectories combined with environment-aware information. Additionally, we plan to achieve more agile kinds of jump motions, such as a back flip.