Optimal motion planning of hopping robot based on pseudospectral method during flight phase

<!-- metadata commented in wiki content


==Optimal Motion Planning of Hopping Robot Based on Pseudospectral Method during Flight Phase==

<div class="center" style="width: auto; margin-left: auto; margin-right: auto;">
NI Yingge<sup>1</sup><big>, </big>MENG Xiangyan<sup>2</sup></div>

<div class="center" style="width: auto; margin-left: auto; margin-right: auto;">
<span style="text-align: center; font-size: 75%;">（<sup>1</sup>School of Aircraft Engineering, Xi’an Aeronautical Institute, Xi’an 710077,China</span></div>

<div class="center" style="width: auto; margin-left: auto; margin-right: auto;">
<span style="text-align: center; font-size: 75%;"><sup>2</sup>Xi’an Founder Robot Co., Ltd, Xi’an 710072, China）</span></div>
-->
==Abstract==

The energy optimal motion planning of a hopping robot with three links is investigated in the flight phase. Firstly, the conservation equation of angular momentum of the hopping robot in the flight phase is established which is a nonholonomic constraint. Then the energy consumption of the robot in the flight phase is selected as the optimization goal. Given the initial and terminal positions, the Gaussian pseudospectrum method is used to solve the optimal control problem. The simulation results show that the initial angular momentum has a great influence on the performance of the hopping robot. With the zero initial angular momentum, although the flight time can be selected arbitrarily, the greater the flight time, the smaller the energy consumption, the force required by the robot is greater. Thus, it is necessary to select an appropriate value.

'''Keywords''': Hopping robot, flight phase, nonholonomic constraint, optimal motion planning

==1. Introduction==

Compared with wheeled or tracked robots, hopping robots move in a jumping manner and are capable of crossing obstacles which are several times their own size. They use discrete landing points to make contact with the ground and have strong adaptability to complex and unstructured terrain. The study of hopping robots has become a research focus in recent years [1]. Among them, Professor Raibert’s team has made groundbreaking research in the field of hopping robots, and their designed model of a single legged telescopic hopping robot provides a reference for subsequent research [2-6].

Before controlling the robot, it is necessary to plan the motion trajectory of the hopping robot [7-11]. Vermeulen [7] planned the robot based on its target motion parameters and used a quintic polynomial to describe the robot’s trajectory. Wu et al. [9]studied hopping robots with flat feet, using Bezier curves to represent the trajectory of active joints, and planning the trajectory based on minimizing driving energy.

In the flight phase, the robot foot is released from the ground. At this time, the only external force on the robot is gravity. The robot meets the principle of conservation of angular momentum, and is a nonholonomic constraint system. Due to the non integrability of nonholonomic constraints, its motion planning problem is much more complex than that of general systems. Rehman et al. [12] used a time-varying feedback control strategy to plan the motion during the flight phase. Guo et al. [13] used the direct single shot method to plan the jumping gait of a robot with four links, and its flight phase was almost passive.

The most commonly used motion planning methods for nonholonomic system can be divided into two categories, namely, the direct method and the indirect method [14-16]. The indirect method is based on the maximum (minimum) value principle, which transforms the optimal motion planning problem into a two-point boundary value problem. Its advantage is that the local optimal solution can be found, but it is difficult to guess the initial solution, and the radius of convergence is small. The direct method uses the parametric method to transform the optimal motion planning problem of the continuous system into a nonholonomic motion planning problem, and then obtains the optimal motion trajectory by solving the non motion planning problem. This method does not need to solve the first order optimal condition, and the radius of convergence is large. Pseudospectral method is the most widely used method among them.

In this paper, the optimal motion planning of a hopping robot with three links in the flight phase is studied. First, the constraint equation of the hopping robot in the flight phase is established according to the conservation principle of angular momentum. Then, the energy optimal motion planning problem is transformed into a nonholonomic motion planning problem by using the Gaussian pseudospectral method, and the problem is solved. The simulation results demonstrate the effectiveness of this method. Because the initial angular momentum has a great impact on the hopping robot. This paper finally analyzes the impact of the initial angular momentum on the hopping performance of the robot.

==2. Dynamic model==

The kinematics model of the robot in the flight phase is shown in [[#img-1|Figure 1]]. The hopping robot consists of three components: body represented by 1, legs denoted by 2, and feet, namely, 3. In order to simplify the analysis, it is assumed that the motion of the hopping robot is limited to the sagittal plane, without considering the lateral motion. Assuming the length of each member of the robot is  <math>l_i\left(i=1,2,3\right)</math>, the mass is mi, the position of the centroid Si of the body is  <math>AS_i</math>, and  <math>l_{Si}={\alpha }_il_i</math> (<math>{\alpha }_i</math> is the proportion factor of each centroid position), and  <math>0<{\alpha }_i<{1}</math>. The rotational inertia of each member around the center of mass is  <math>I_i</math>.

Based on a floating coordinate with hip joint position A as the origin, the dynamic model is established. The generalized coordinate of the robot is  <math>x={\left[{\theta }_0,{\theta }_{12},{\theta }_{{23}}\right]}^{T}</math>, where  <math>{\theta }_{1}</math> is the absolute coordinate of the body,  <math>{\theta }_{{12}}</math> is the relative angle between the body and legs (counterclockwise is positive), namely,  <math>{\theta }_{{12}}={\theta }_{1}-{\theta }_{2}</math>, <math>{\theta }_{{23}}</math> is the relative angle between the legs and feet, i.e.  <math>{\theta }_{{23}}={\theta }_{2}-{\theta }_\mbox{3}</math>.

<div id='img-1'></div>
{| class="wikitable" style="margin: 0em auto 0.1em auto;border-collapse: collapse;width:auto;" 
|-style="background:white;"
|style="text-align: center;padding:10px;"| [[File:Review_582677700696_5947_ffig1.png]]
|-
| style="background:#efefef;text-align:left;padding:10px;font-size: 85%;"| '''Figure 1'''. Model of robot during flight phase
|}


During the flight phase, the influence of air drag and internal friction of the robot on the robot’s hopping motion is ignored, and the robot’s total center of mass moves in a parabolic motion with the gravity. If the flight time is <math display="inline"> T </math>, then the horizontal and vertical motion of the center of mass can be written as

{| class="formulaSCP" style="width: 100%; text-align: center;" 
|-
| 
{| style="text-align: center; margin:auto;" 
|-
| <math>\left\{ \begin{array}{c}
x_c\left(t\right)=x_c^{to}+{\dot{x}}_c^{to}t\\
y_c\left(t\right)=y_c^{to}+{\dot{y}}_c^{to}t-\frac{1}{2}gt^2
\end{array}\right.</math>
|}
| style="width: 5px;text-align: right;white-space: nowrap;" | (1)
|}

where,  <math>x_{c}{}^{{to}}</math> and  <math>y_{c}{}^{{to}}</math> are the horizontal and vertical positions of the robot’s total center of mass at the flight time, respectively.  <math>{\dot{x}}_{c}^{to}</math> and  <math>{\dot{y}}_{c}^{to}</math> are the horizontal and vertical velocities of the total center of mass at the flight time, respectively. The position of the robot at the touchdown is completely determined by the flight conditions.

The rotation of the robot can be expressed by the angular momentum of the robot with regarding to the total center of mass. Angular momentum can be calculated as follows:

{| class="formulaSCP" style="width: 100%; text-align: center;" 
|-
| 
{| style="text-align: center; margin:auto;" 
|-
| <math>{\mu }_G=\sum_{i=1}^3\left({\overline{GG}}_i\times m_i{\overset{\bullet}{\overline{GG}}}_i +
I_i{\dot{\theta }}_i\right)</math>
|}
| style="width: 5px;text-align: right;white-space: nowrap;" | (2)
|}

where,  <math>{\overline{GG}}_i=\boldsymbol{r}_i-\boldsymbol{r}_G</math>,  <math>i={1},...,{3}</math>,  <math>r_{i}</math> is the position of the centroid of each component, and  <math>\boldsymbol{r}_G</math> is the position of the total centroid.

The only external force on the robot in the flight phase is gravity. So the robot meets the principle of conservation of angular momentum about the total center of mass. Supposing that the angular momentum of the robot at the initial moment of the flight phase is <math display="inline">P_{0}</math>, one can obtain:

{| class="formulaSCP" style="width: 100%; text-align: center;" 
|-
| 
{| style="text-align: center; margin:auto;" 
|-
| <math>\begin{array}{c}
{\mu }_G={\Delta }_1{\dot{\theta }}_1+{\Delta }_2{\dot{\theta }}_{12}+{\Delta }_3{\dot{\theta }}_{23} =P_0
\end{array}</math>
|}
| style="width: 5px;text-align: right;white-space: nowrap;" | (3)
|}

where  <math>{\Delta }_i</math> is a function of joint rotation angle. The equation above indicates that the system is a nonholonomic system with Pfaffian constraints, and it has a cyclic coordinate  <math>{\theta }_{1}</math>.

During the flight phase, the robot is only equipped with actuators in the hip and ankle joints. So the joint angular velocity  <math>{\dot{\theta }}_{{12}}</math> and  <math>{\dot{\theta }}_{23}</math> are selected as the control inputs for the system. Eq.(3) can be written in state space form. When  <math>P_0</math> do not equal 0, it is an affine nonlinear system with three states and two inputs and drift terms

{| class="formulaSCP" style="width: 100%; text-align: center;" 
|-
| 
{| style="text-align: center; margin:auto;" 
|-
| <math> \dot{x}=f\left(x\right)+g\left(x\right)u =\left[\begin{array}{c}
-P_0/{\Delta }_1\\
0\\
0
\end{array}\right]+\left[\begin{array}{c}
-{\Delta }_2/{\Delta }_1\\
1\\
0
\end{array}\right]u_1+ </math> <math>\left[\begin{array}{c}
-{\Delta }_3/{\Delta }_1\\
0\\
1
\end{array}\right]u_2 </math>
|}
| style="width: 5px;text-align: right;white-space: nowrap;" | (4)
|}

where， <math>\boldsymbol{u}={\left[\begin{array}{cc}
u_1 & u_2
\end{array}\right]}^{T}={\left[\begin{array}{cc}
{\dot{\theta }}_{12} & {\dot{\theta }}_{23}
\end{array}\right]}^{T}</math>.

When the initial angular momentum of the system is not zero,  <math>f\left(x\right)\not =0</math>, then the system has no balance point.

The optimal trajectory planning of a robot refers to the initial position  <math>x_0={\left[{\theta }_0,{\theta }_{\mbox{12}},{\theta }_{\mbox{23}}\right]}^\mbox{T}</math> and terminal position  <math>x_T={\left[q_{0d},q_{1d},q_{2d}\right]}^T</math> of the given robot, by searching for a set of control inputs  <math>u\left(t\right)</math> to enable the robot to move along a path that satisfies incomplete constraints. Meanwhile, the robot can move from the initial position  <math>x_0</math> to the terminal position  <math>x_T</math> within a given time <math display="inline"> T </math>, and achieve an extreme performance index.

In robot systems, due to the limited energy that robots can carry, in order to make the robot move for a longer time, it is necessary to consider the energy consumption of the robot. According to the minimum energy consumption theorem, in this paper the energy consumed by the hopping robot during flight phase is selected as the optimal control objective. Due to the fact that the usual driving method for robots is motor, and the energy consumed by the motor is proportional to its velocity, the performance index function can be expressed as follows:

{| class="formulaSCP" style="width: 100%; text-align: center;" 
|-
| 
{| style="text-align: center; margin:auto;" 
|-
| <math>J={\int }_0^T\boldsymbol{u}^2\left(t\right)dt</math>
|}
| style="width: 5px;text-align: right;white-space: nowrap;" | (5)
|}


In the flight phase, the optimal planning problem of a hopping robot can be described as: finding suitable control input variables to achieve the minimum performance index, i.e. Eq.(5). Meanwhile, the state variable and initial time  <math>t_0</math> and flight time <math display="inline"> T </math> satisfy the following constraints:

{| class="formulaSCP" style="width: 100%; text-align: center;" 
|-
| 
{| style="text-align: center; margin:auto;" 
|-
| <math>\left\{ \begin{array}{c}
x=f(x)+g(x)u\\
x(t_0)=x_0\\
x(T)=x_T
\end{array}\right.</math>
|}
| style="width: 5px;text-align: right;white-space: nowrap;" | (6)
|}

where  <math>t_0</math> and T are constant.

==3. Pseudospectral method for solving optimal control problems==

In recent years, the pseudospectral method has become one of the important methods for solving optimal control problems [12]. The basic principle of the pseudospectral method is to discretize the continuous optimal control problem at the orthogonal collocation points, and approximate the state and control variables through the global interpolation polynomial. Thus the optimal control problem can be transformed into a nonlinear programming problem (NLP).

According to the different orthogonal collocation points, there are four widely used pseudospectral methods: Gaussian pseudospectral method, Radau pseudospectral method, Legendre pseudospectral method and Chebyshev pseudospectral method. Legendre pseudospectral method has the advantages of convergence speed of exponential function, insensitivity to initial value guess, and large radius of convergence. So Legendre pseudospectral method is adopted in this paper.

The Legendre pseudospectral method uses the roots of orthogonal polynomials as collocation points and global orthogonal polynomials as finite bases. The system’s state and control variables are discretized at the Lendre Guass Lobatto (LGL) point. The Lagrange interpolation polynomial is used to approximate the state and control variables, obtaining the discrete dynamic equations at the points. Thus, the differential operation in the state equation and the integral operation in the performance index function are transformed into algebraic operation, and finally the optimal control problem is transformed into a nonlinear programming problem with the state variables and control variables at the points as the parameters to be optimized.

'''Dynamic equation''':

{| class="formulaSCP" style="width: 100%; text-align: left;" 
|-
| 
{| style="text-align: center; margin:auto;width: 100%;" 
|-
| style="text-align: center;" | <math>\dot{x}\left(t\right)=f\left(x\left(t\right),u\left(t\right),t\right)</math>
|}
| style="width: 5px;text-align: right;white-space: nowrap;" |(7)
|} 

'''Boundary condition''':

{| class="formulaSCP" style="width: 100%; text-align: left;" 
|-
| 
{| style="text-align: center; margin:auto;width: 100%;" 
|-
| style="text-align: center;" | <math>\varphi \left(x\left(t_0\right),x\left(t_f\right),t_0,t_f\right)=0</math>
|}
| style="width: 5px;text-align: right;white-space: nowrap;" |(8)
|}

'''Inequality path constraint'''

{| class="formulaSCP" style="width: 100%; text-align: left;" 
|-
| 
{| style="text-align: center; margin:auto;width: 100%;" 
|-
| style="text-align: center;" | <math>G\left(x\left(t\right),u\left(t\right),t\right)\leq 0</math>
|}
| style="width: 5px;text-align: right;white-space: nowrap;" |(9)
|}

where  <math>J\in R</math> is the performance indicator,  <math>\varphi \in R</math> is the Mayer type performance indicator,  <math>G\in R</math> is the Lagrange type performance indicator, and  <math>f\left(\cdot \right)</math> is the state equation function vector, <math>\phi</math> is the initial and terminal constraint function vector, and  <math>C</math> is the equation and inequality path constraint function vector.

The calculation process of Legendre pseudospectral method is as follows: first, the value range of the original planning problem is mapped from  <math>t\in \left[t_0,T_f\right]</math> to the distribution interval of discrete points in the pseudospectral method through time domain transformation  <math>\tau \in \left[-{1},{1}\right]</math>.

The Legendre orthogonal polynomial on the interval  <math>\left[-\mbox{1},\mbox{1}\right]</math> is

{| class="formulaSCP" style="width: 100%; text-align: left;" 
|-
| 
{| style="text-align: center; margin:auto;width: 100%;" 
|-
| style="text-align: center;" | <math>L_n\left(\tau \right)=\frac{1}{2^nn!}\frac{d^n}{dx^n}{\left({\tau }^2-1\right)}^n</math>
|}
| style="width: 5px;text-align: right;white-space: nowrap;" |(10)
|}

where  <math>L_n\left(\tau \right)</math> represents n-order Legendre polynomials.

In order to achieve better interpolation approximation,  <math>{\tau }_0=-{1}</math> ,  <math>{\tau }_{N}={1}</math> and <math>n-1</math> zeros  <math>{\tau }_i</math>  <math>\left({i}={1},{ 2},\ldots \ldots ,{ N-1}\right)</math> of the first derivative of the Legendre polynomial  <math>{\dot{L}}_n\left(\tau \right)</math> is defined as an LGL point, there are <math>N+1</math> control points, denoted as  <math>{\tau }_i</math>  <math>\left({i}={1},{2},\ldots \ldots ,{N}\right)</math>.

Using <math>n+1</math> LGL points as interpolation points, n-order Lagrangian interpolation polynomials is constructed to approximate the continuous state variable  <math>x\left(t\right)</math> and control variable  <math>u\left(t\right)</math>, and obtain the following equations:

{| class="formulaSCP" style="width: 100%; text-align: left;" 
|-
| 
{| style="text-align: center; margin:auto;width: 100%;" 
|-
| style="text-align: center;" | <math> x\left(\tau \right)\approx X\left(\tau \right)=\sum_{i=0}^N{\Phi }_i\left(\tau \right)X_i  </math>
|}
| style="width: 5px;text-align: right;white-space: nowrap;" |(11)
|}

{| class="formulaSCP" style="width: 100%; text-align: left;" 
|-
| 
{| style="text-align: center; margin:auto;width: 100%;" 
|-
| style="text-align: center;" | <math>  >u\left(\tau \right)\approx U\left(\tau \right)=\sum_{i=0}^N{\Phi }_i\left(\tau \right)U_i </math>
|}
| style="width: 5px;text-align: right;white-space: nowrap;" |(12)
|}

where, <math>{\Phi }_i\left(\tau \right)</math> is the <math>n</math>-order Lagrangian interpolation basis function,

{| class="formulaSCP" style="width: 100%; text-align: left;" 
|-
| 
{| style="text-align: center; margin:auto;width: 100%;" 
|-
| style="text-align: center;" | <math> {\Phi }_i\left(\tau \right)=\frac{1}{n\left(n+1\right)L_n\left({\tau }_i\right)}\frac{\left({\tau }^2-1\right){\dot{L}}_n\left({\tau }_i\right)}{\tau -{\tau }_i}  </math>
|}
| style="width: 5px;text-align: right;white-space: nowrap;" |(13)
|}

where,  <math>{i}={1},{2},\ldots \ldots ,n</math>,  <math>{\Phi }_i\left(\tau \right)</math> satisfies the relationship  <math>{\Phi }_i\left(\tau \right)={\delta }_{ij}</math>, if  <math>i=j</math>, there is  <math>{\delta }_{ij}={1}</math>； if  <math>i\not =j</math>,  <math>{\delta }_{ij}={0}</math>.

After parameterizing the state variable through interpolation polynomials, the differential operation of the state equation can be approximated as the differential operation of the interpolation basis function, and the derivative of the state variable  <math>x\left(\tau \right)</math> can be approximated as follows:

{| class="formulaSCP" style="width: 100%; text-align: left;" 
|-
| 
{| style="text-align: center; margin:auto;width: 100%;" 
|-
| style="text-align: center;" | <math> \dot{x}\left({\tau }_k\right)\approx {\dot{X}}_k=\sum_{i=0}^N{\dot{L}}_i\left({\tau }_k\right)X_i=</math><math>\sum_{i=0}^ND_{ki}X_i  </math>
|}
| style="width: 5px;text-align: right;white-space: nowrap;" |(14)
|}

where  <math>k=0,{1},\cdots \cdots ,N</math>,  <math>D</math> is the <math>(n+1)</math> differential matrix, representing the differential value of Lagrange basis function at each LGL control point,  <math>D_{ki}</math> is the <math>(k, i)</math>th element. One can obtain

{| class="formulaSCP" style="width: 100%; text-align: left;" 
|-
| 
{| style="text-align: center; margin:auto;width: 100%;" 
|-
| style="text-align: center;" | <math>D_{ki}=\left\{\frac{L_N\left({\boldsymbol{\tau }}_k\right)}{L_N\left({\boldsymbol{\tau }}_j\right)}\cdot \frac{1}{{\boldsymbol{\tau }}_k-{\boldsymbol{\tau }}_j},\quad k\not =j\frac{N\left(N+1\right)}{4},\quad k=j=0\frac{N\left(N+1\right)}{4},\quad k=j=N0,\mbox{ }\mbox{ }\mbox{other}\right.</math>
|}
| style="width: 5px;text-align: right;white-space: nowrap;" |(15)
|}


Now, the constraints of the state equation can be transformed into discrete state equations at N+1 LGL control points.

<div style="text-align: right; direction: ltr; margin-left: 1em;">
 <math>\sum_{i=0}^ND_{ki}X_i-\frac{t_f-t_0}{2}f\left(X_k,U_k,{\tau }_k\right)=</math><math>0</math> （16）</div>

The boundary and path constraints after discretization can be described as follows:

<div style="text-align: right; direction: ltr; margin-left: 1em;">
 <math>\phi \left(\boldsymbol{X}_0,\boldsymbol{X}_N,{\tau }_0,{\tau }_N\right)=</math><math>0</math> （17）</div>

<div style="text-align: right; direction: ltr; margin-left: 1em;">
 <math>\boldsymbol{C}\left(\boldsymbol{X}_k,\boldsymbol{U}_N,{\tau }_k\right)\leq 0</math> （18）</div>

where  <math>k=\mbox{ }0,\mbox{ 1},\mbox{ }\cdots \cdots \mbox{ },N</math> .

Through Gauss-Lobatto numerical integration, the integral term in the performance index function is transformed into the following algebraic expression:

<div style="text-align: right; direction: ltr; margin-left: 1em;">
 <math>\begin{array}{c}
J=\frac{t_f-t_0}{2}{\int }_{-1}^1\langle \boldsymbol{u}\left(\tau \right),\boldsymbol{u}\left(\tau \right)\rangle d\tau \\
=\frac{t_f-t_0}{2}\sum_{i=0}^Nw_i\langle \boldsymbol{U}_i,\boldsymbol{U}_i\rangle 
\end{array}</math> （19）</div>

where,  <math>w_i\left(i=\mbox{ }0,\mbox{ 1},\mbox{ }\cdots \cdots ,N\right)</math> is Gauss weights，defining

<div style="text-align: right; direction: ltr; margin-left: 1em;">
 <math>w_i={\int }_{-1}^1{\boldsymbol{\Phi }}_i\left(\tau \right)d\tau =</math><math>\frac{2}{N\left(N+1\right)}\frac{1}{{\left[L_N\left({\tau }_i\right)\right]}^2}</math> （20）</div>

Through the above discretization and approximation processing, the optimal control problem can be further described by Eqs.(7-9) as follows: solving the values of the state variable  <math>X_\mbox{i}</math> at N+1 interpolation points  <math>X=\mbox{ }(X_0,X_\mbox{1},\cdots \cdots ,X_N)</math> and the control variable  <math>U_i</math> at N+1 interpolation points  <math>U=\mbox{ }(U_0,U_\mbox{1},\cdots \cdots ,U_N)</math> , so that the performance index function takes the minimum value and satisfies the constraint conditions of Eqs. (16-18).

For the energy optimal path planning problem of the hopping robot in this paper, given the initial position <math>X\left(t_0\right)</math> and terminal position  <math>X\left(T_f\right)</math> of the system, two sets of unknown coefficients  <math>X=\mbox{ }(X_0,X_\mbox{1},\cdots \cdots ,X_N)</math> and  <math>U=\mbox{ }(U_0,U_\mbox{1},\cdots \cdots ,U_N)</math> are solved, resulting in a minimal discrete performance index function.

<div style="text-align: right; direction: ltr; margin-left: 1em;">
 <math>J=\frac{t_f-t_0}{2}\sum_{i=0}^3{\Vert \boldsymbol{U}_i\Vert }^2w_i</math> （21）</div>

Further, the following discrete system dynamic equation constraints can be satisfied:

<div style="text-align: right; direction: ltr; margin-left: 1em;">
 <math>\sum_{i=0}^ND_{ki}X_{1i}=\frac{t_f-t_0}{2}\left(f(X_k)+\right. </math><math>\left. g(X_k)U_k\right)</math> （22a）</div>

<div style="text-align: right; direction: ltr; margin-left: 1em;">
 <math>\sum_{i=0}^ND_{ki}X_{2i}=\frac{t_f-t_0}{2}\left(f(X_k)+\right. </math><math>\left. g(X_k)U_k\right)</math> （22b）</div>

<div id="_GoBack" style="text-align: right; direction: ltr; margin-left: 1em;">
 <math>\sum_{i=0}^ND_{ki}X_{3i}=\frac{t_f-t_0}{2}\left(f(X_k)+\right. </math><math>\left. g(X_k)U_k\right)</math> （22c）</div>

where  <math>k=\mbox{ }0,\mbox{ 1},\cdots \cdots ,N</math> ,  <math>f\left(X_k\right)</math> , and <math>g\left(X_k\right)</math> are elements in the state equation of the hopping robot system.

In actual robot control, there are limitations on motor torque and speed, so there are upper and lower limits on the actual control input of the system, which can be expressed as discrete inequality constraints as shown in the following equation:

{| class="formulaSCP" style="width: 100%; text-align: center;" 
|-
| 
{| style="text-align: center; margin:auto;" 
|-
| <math>\vert U_{ik}\vert \leq U_{max}</math>
|}
| style="width: 5px;text-align: right;white-space: nowrap;" | (23)
|}


where  <math>i=\mbox{ 1},\mbox{ 2},\mbox{ 3}</math> , <math>k=\mbox{ }0,\mbox{ 1},\cdots \cdots ,N</math> ,  <math>U_{\mbox{max}}>\mbox{ }0</math> is the upper limit of the control input.

===4 Results===

The physical parameters of the robot based on the hopping pattern of the kangaroo is determined as shown in Table 1.

<div class="center" style="width: auto; margin-left: auto; margin-right: auto;">
Table 1 Physical parameters</div>

{| style="width: 68%;margin: 1em auto 0.1em auto;border-collapse: collapse;" 
|-
|  colspan='2'  style="border-top: 1pt solid black;border-bottom: 1pt solid black;text-align: center;vertical-align: top;"|<span style="text-align: center; font-size: 75%;">Component numb</span>er  <math>i</math> 
|  colspan='2'  style="border-top: 1pt solid black;border-bottom: 1pt solid black;text-align: center;vertical-align: top;"|<span style="text-align: center; font-size: 75%;">1</span>
|  style="border-top: 1pt solid black;border-bottom: 1pt solid black;text-align: center;vertical-align: top;"|<span style="text-align: center; font-size: 75%;">2</span>
|  style="border-top: 1pt solid black;border-bottom: 1pt solid black;text-align: center;vertical-align: top;"|<span style="text-align: center; font-size: 75%;">3</span>
|-
|  style="border-top: 1pt solid black;text-align: center;vertical-align: top;"|<span style="text-align: center; font-size: 75%;">Length of r</span>od  <math>l_i/\left(\mbox{m}\right)</math> 
|  colspan='3'  style="border-top: 1pt solid black;text-align: center;vertical-align: top;"|<span style="text-align: center; font-size: 75%;">0.11</span>
|  style="border-top: 1pt solid black;text-align: center;vertical-align: top;"|<span style="text-align: center; font-size: 75%;">0.26</span>
|  style="border-top: 1pt solid black;text-align: center;vertical-align: top;"|<span style="text-align: center; font-size: 75%;">0.174</span>
|-
|  colspan='3'  style="text-align: center;vertical-align: top;"|Centroid moment separation  <math>l_{Si}/\left(\mbox{m}\right)</math> 
|  style="vertical-align: top;"|<span style="text-align: center; font-size: 75%;">0.11</span>
|  style="text-align: center;vertical-align: top;"|<span style="text-align: center; font-size: 75%;">0.105</span>
|  style="text-align: center;vertical-align: top;"|<span style="text-align: center; font-size: 75%;">0.082</span>
|-
|  style="text-align: center;vertical-align: top;"|Mass of rod  <math>m_i/\left(\mbox{kg}\right)</math> 
|  colspan='3'  style="text-align: center;vertical-align: top;"|<span style="text-align: center; font-size: 75%;">4.24</span>
|  style="text-align: center;vertical-align: top;"|<span style="text-align: center; font-size: 75%;">0.06</span>
|  style="text-align: center;vertical-align: top;"|<span style="text-align: center; font-size: 75%;">0.14</span>
|-
|  style="border-bottom: 1pt solid black;text-align: center;vertical-align: top;"|moment of inertia  <math>J_i/(\mbox{kg}\cdot \mbox{m}^\mbox{2})</math> 
|  colspan='3'  style="border-bottom: 1pt solid black;text-align: center;vertical-align: top;"|<span style="text-align: center; font-size: 75%;">0.034</span>
|  style="border-bottom: 1pt solid black;text-align: center;vertical-align: top;"|<span style="text-align: center; font-size: 75%;">0.0033</span>
|  style="border-bottom: 1pt solid black;text-align: center;vertical-align: top;"|<span style="text-align: center; font-size: 75%;">0.0038</span>
|}


The initial and terminal positions of the given motion planning of the hopping robot system are <math>x_0={\left[-5^{\circ },-{25}^{\circ },-{18}^{\circ }\right]}^\mbox{T}</math> and  <math>x_\mbox{T}={\left[-5^{\circ },-{125}^{\circ },-{35}^{\circ }\right]}^\mbox{T}</math> , respectively. The flight time  <math>T=0.\mbox{5}</math> ,  <math>P_0=0.0\mbox{2}</math> , and the control constraints are  <math>u_{\mbox{1max}}=\mbox{6rad}/\mbox{s}</math> ,  <math>u_{\mbox{2max}}=\mbox{6rad}/\mbox{s}</math> . The simulation results are shown in Figs.2-4. It can be seen that the hopping robot can complete the motion planning requirements and reach the preset goal position within the given time.

<div class="center" style="width: auto; margin-left: auto; margin-right: auto;">
[[File:Review_582677700696_2756_ffig2.png]]
 </div>

<div class="center" style="width: auto; margin-left: auto; margin-right: auto;">
Fig.2 Posture diagram during flight phase</div>

From Fig.3, it can be seen that the joint trajectory of the foot shows a trend of first rising and then falling, which means that the foot lifts inward and then quickly swings downwards during most of the flight phase, preparing to touch the ground. The range of changes in the ankle joint is the largest among the three curves, approximately from 48 ° to 205°. The range of body swing is relatively small, ranging from -16° to 5°, and the changes are relatively gentle.

<div class="center" style="width: auto; margin-left: auto; margin-right: auto;">
[[File:Review_582677700696_2716_ffig3.png]]
 </div>

<div class="center" style="width: auto; margin-left: auto; margin-right: auto;">
Fig.3 Joint angles of the hopping robot</div>

Fig.4 is the control input for the hopping robot, which is the input angular velocity of the hip and ankle joints. It can be seen that when the control input exceeds the speed limit, the limit value of the control input is taken as a straight line.

<div class="center" style="width: auto; margin-left: auto; margin-right: auto;">
[[File:Review_582677700696_8446_ffig4.png]]
 </div>

<div class="center" style="width: auto; margin-left: auto; margin-right: auto;">
Fig.4 Control input of the hip point</div>

====1）The effect of initial angular momentum on hopping performance====

In Ref.[6], the stability of the hopping robot’s flight movement was investigated. It was proposed that in the flight stage, when the total angular momentum regarding the robot’s center of mass was very small, the flight movement was in a stable state. Since the angular momentum regarding the robot’s center of mass is conserved in the flight phase, the angular momentum of the robot in flight is determined by the initial angular momentum  <math>P_0</math> .  <math>P_0</math> has a significant impact on the robot’s movement during the flight phase.

As can be seen from Fig.5, with the increase of the initial angular momentum, the maximum allowable flight stage continues to decrease. When  <math>P_0<0.01</math> , the maximum duration shows an exponential decreasing trend. Due to the decrease in the duration of the flight phase, the height of the hopping (the maximum height of the robot’s center of mass during the flight phase) decreases, and the foot may collide with the ground. When  <math>P_0=0</math> , the flight phase can be arbitrarily taken. It can be seen from Fig.6 that with the increase of the initial angular momentum, the performance index, i.e. energy consumption, increases accordingly, and the change of energy consumption and initial angular momentum is approximately in a straight line. It is can be drawn a conclusion that the existence of initial angular momentum is a very unfavorable factor for motion planning. Therefore, in the control process, the initial angular momentum at the take-off should be zero as far as possible.

<div class="center" style="width: auto; margin-left: auto; margin-right: auto;">
[[File:Review_582677700696_2565_ffig5.png]]
</div>

<div class="center" style="width: auto; margin-left: auto; margin-right: auto;">
Fig.5 Maximum flight time vs. initial angular momentum</div>

<div class="center" style="width: auto; margin-left: auto; margin-right: auto;">
[[File:Review_582677700696_7008_ffig6.png]]
 </div>

<div class="center" style="width: auto; margin-left: auto; margin-right: auto;">
Fig.6 Performance index vs. initial angular momentum</div>

====2）The effect of flight time on hopping performance====

In this section, the impact of flight time on hopping performance is discussed when initial angular momentum  <math>\mbox{P}_0</math> equals 0. Fig.7 shows the curve of performance indicator  <math>J</math> as a function of flight time. When the initial angular momentum is zero, the flight time can be arbitrarily selected. It can be seen that as the flight time increases, the performance indicator  <math>J</math> decreases, that is, the energy consumption decreases, and changes sharply within 0.5 seconds. As time increases, the robot has sufficient time to change the position of the joint angle, resulting in a smaller input and lower energy consumption. But at the same time, as time increases, the height of the hopping increases, and the vertical velocity of the center of mass at take-off also increases, requiring the robot to provide greater force . Therefore, it is necessary to comprehensively measure and select the appropriate time.

<div class="center" style="width: auto; margin-left: auto; margin-right: auto;">
[[File:Review_582677700696_6393_ffig7.png]]
 </div>

<div id="_Ref22139998" class="center" style="width: auto; margin-left: auto; margin-right: auto;">
Fig.7 Performance indicator ''J'' vs. flight time ''T''</div>

From Fig.8, it can be seen that as the flight time decreases, the movement distance of the hopping robot’s center of mass in the horizontal direction decreases, and the height in the vertical direction decreases, resulting in the robot’s foot height being less than 0 in the first 0.02 seconds, indicating a collision between the foot and the ground. Fig.9 shows the posture diagram. It can been seen that at the end of the flight phase the foot of the hopping robot touch the ground. It indicates that the flight phase is over. The negative height of the robot’s foot during flight phase indicates a collision between the robot’s and the ground.

<div class="center" style="width: auto; margin-left: auto; margin-right: auto;">
[[File:Review_582677700696_9139_ffig8.png]]
 </div>

<div class="center" style="width: auto; margin-left: auto; margin-right: auto;">
Fig.8 Trajectory of tip of the foot at different flight times</div>

<div class="center" style="width: auto; margin-left: auto; margin-right: auto;">
[[File:Review_582677700696_3895_ffig9.png]]
 </div>

<div class="center" style="width: auto; margin-left: auto; margin-right: auto;">
Fig.9 Posture diagram during flight phase at  <math>T=0.\mbox{1}</math> </div>

5 Conclusion

In this paper the energy optimal motion planning problem of the hopping robot with three links in the flight phase is investigated. First, the conservation equation of angular momentum of the hopping robot in the flight phase is established which is a non holonomic constraint. Then, the energy consumption of the robot during the flight phase is selected as the optimization goal. Given the initial and terminal positions, the Gaussian pseudospectral method is used to solve the optimal control problem. The conclusion can be drawn as follows:

1) The initial angular momentum has a great impact on the performance of the hopping robot. The larger the initial angular momentum is, the smaller the maximum allowable flight time of the robot will be. When the initial angular momentum is zero, the flight time can be chosen arbitrarily;

2) When the initial angular momentum is zero, the longer the flight time is, the smaller the energy consumption is, and the greater the flight distance and height are. However, the higher the velocity of the robot in the vertical direction of the center of mass at flight, the greater the force required by the robot. The smaller the flight time, the smaller the flight distance and altitude, and the less force required for flight. However, the energy consumption increases, and the robot’s foot may collide with the ground. So it is necessary to comprehensively measure and select a suitable time for flight.

===References===

<span id='_Ref85812059'></span>[1] Raibet, M H, Legged robots that balance. Cambridge: the MIT Press. 1986.

[2] Ahmadi, M,Buehler, M, Controlled passive dynamic running experiments with the ARL-Monopod II. IEEE Transactions on Robotics, 2006, 22(5): 974-986.

[3] Hyon, S H,Emura, T,Mita, T, Dynamics-based control of a one-legged hopping robot. Proceedings of the Institution of Mechanical Engineers, Part I: Journal of Systems and Control Engineering, 2003, 217(2): 83-98.

[4] Haewon, P,Patrick, M W,Sangbae, K, High-speed bounding with the MIT Cheetah 2: Control design and experiments. International Journal of Robotics Research, 2017, 36(2): 167-192.

[5] Terry, P,Piovan, G,Byl, K, Towards precise control of hoppers: Using high order partial feedback linearization to control the hopping robot FRANK. IEEE 55<sup>th</sup> Conference on Decision and Control (CDC) ARIA Resort & Casino, Las Vegas, USA: 2016, 6669-6675.

<span id='_Ref85812063'></span> [6] Haldane, D W,Yim, J K,Fearing, R S, Repetitive extreme-acceleration (14-g) spatial jumping with Salto-1P. 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) 2017, 3345-3351.

<span id='_Ref85812378'></span>[7] Vermeulen, J, Trajectory generation for planar hopping and walking robots: An objective parameter and angular momentum approach. McGill University, Department of Mechanical Engineering, 2004.

<span id='_Ref85812199'></span>[8] Li, Z,Huang, Q,Li, K, et al., Stability criterion and pattern planning for humanoid running. Proceedings of the 2004 IEEE International Conference on Robotics & Automation. 2004, 1059-1064.

<span id='_Ref85812961'></span>[9] Wu, T Y,Yeh, T J,Hsu, B H, Trajectory planning of a One-Legged robot performing a stable hop. The International Journal of Robotics Research, 2011, 30(8): 1072-1091.

[10] Shabestari, S S,Emami, M R, Gait planning for a hopping robot. Robotica, 2016, 34(8): 1822-1840.

<span id='_Ref85812382'></span>[11] Ahn, D H,Cho, B K, Optimal periodic hopping trajectory generation for legged robots. IEEE/ASME International Conference on Advanced Intelligent Mechatronics (AIM), Auckland, New Zealand: 2018, 1263-1268.

<span id='_Ref85814151'></span>[12] Rehman, F,Ahmed, M M,Memon, N Met al., Time-varying stabilizing control for a hopping robot model during the flight phase. Islamabad: 2006. 400-403.

<span id='_Ref85814567'></span>[13] Guo, Q,Macnab, C,Pieper, J, Hopping with nearly-passive flight phases. 2008 IEEE Conference on Robotics, Automation and Mechatronics, Chengdu, China: 2008, 743-748.

<span id='_Ref85817660'></span>[14] YONG Enmi, CHEN Lei, TANG Guojin. A survey of numerical methods for trajectory optimization of spacecraft. Journal of Astronautics, 2008, 29(2): 397-406. (In Chinese)

[15] XU Shaobing, LI Shengbo, CHENG Bo. Theory and application of Legendre pseudo-spectral method for solving optimal control problem. Control and Decision, 2014(12): 2113-2120. (In Chinese)

<span id='_Ref85817664'></span>[16] GE Xingsheng, CHEN Kaijie. Path planning of free floating space robot using Legendre pseudospectral method. Chinese Journal of Theoretical and Applied Mechanics, 2016, 48(4): 823-831. (In Chinese)