Momentum Control of an Underactuated Flying Humanoid Robot

TL;DR

This paper introduces a control framework for underactuated flying humanoid robots, enabling stable flight, contact locomotion, and manipulation by controlling centroidal momentum, verified through simulations on the iCub robot model.

Contribution

It proposes a novel momentum control method for underactuated flying humanoid robots, integrating Whole-Body Control and Aerial Manipulation concepts.

Findings

Stability and convergence of robot momentum are proven using Lyapunov methods.

Simulations on iCub demonstrate the effectiveness of the control approach.

The framework enables trajectory tracking and orientation control in simulation.

Abstract

The paper takes the first step towards the de- velopment of a control framework for underactuated flying humanoid robots. These robots may thus have the capacities of flight, contact locomotion, and manipulation, and benefit from technologies and methods developed for Whole-Body Control and Aerial Manipulation. As in the case of quadrotors, we as- sume that the humanoid robot is powered by four thrust forces. For convenience, these forces are placed at the robot hands and feet. The control objective is defined as the asymptotic stabilization of the robot centroidal momentum. This objective allows us to track a desired trajectory for the robot center of mass and keep small errors between a reference orientation and the robot base frame. Stability and convergence of the robot momentum are shown to be in the sense of Lyapunov. Simulations carried out on a model of the humanoid robot iCub verify the soundness of the proposed approach.

Figures

Equations

$$M(q)\dot{\nu }+C(q,\nu )\nu +G(q)=\left[\begin{array}{c}{0}_6\\ \tau \end{array}\right]+\sum _{k=1}^m{J}_k^{\top }{F}_k,\text{vspace}-0.1cm$$

$$\sum J_k^{\top }{F}_k=\sum J_k^{\top }(q)\text{prescript}A{ı}_k(q)T_k:=f(q,T).$$

$$M(q)\dot{\nu }+C(q,\nu )\nu +G(q)$$

$${\dot{\mathrm{v}}}_B$$

$${\dot{\mathrm{v}}}_B=\mathrm{v}(q,\nu ,T,\tau ).$$

$$\tilde{\mathrm{h}}:=\mathrm{h}-{\mathrm{h}}_d(t).$$

$$\dot{\tilde{\mathrm{h}}}$$

$$\dot{T}$$

$$A(q)$$

$$r_i$$

$$\bar{S}(r_i)$$

$$0$$

$$\tilde{\xi }$$

$$F$$

$$\dot{I}$$

$$\dot{\tilde{\mathrm{h}}}$$

$$\dot{\tilde{\xi }}$$

$${\Lambda }_b$$

$$\Lambda$$

$${\tilde{S}}_i$$

$$\mathrm{h}$$

$$\delta$$

$$B$$

$$\tilde{K}$$

$$\delta +A{u}_1+B{u}_2=0_6,$$

$$\text{rank}(AB)=6,$$

$$\mathrm{h}$$

$${\mathrm{h}}_d$$

$$I(t)$$

$$\omega =-k{\left(\mathrm{skew}(R_d^{\top }R)\right)}^{\vee },k>0,$$

$$I$$

$$p:=-K_P^p(s-s_r),$$

$$(u_1^{\ast },u_2^{\ast })=\underset{(u_1,u_2)}{\mathrm{argmin}}$$

$$l{b}_1$$

$$\bar{M}$$

Full text

I Introduction

The general purpose of providing humanoid robots with some degree of locomotion and manipulation has driven most of the recent research in the humanoid robotics community. Legged and wheeled locomotion, for instance, have proven to be feasible on various humanoid platforms, which can now be envisioned as interfaces for user assistance in several domains (see, e.g., [1, 2]). The robot underactuation combined with the (usually) large number of the robot degrees-of-freedom are among the main difficulties for humanoid robot locomotion and manipulation control. This paper takes the first step towards the development of platforms having the capacities of flight, contact locomotion, and manipulation, and proposes a framework for underactuated flying humanoid robots. Hence, the proposed platforms belong to the domain of interests of both Whole-Body Loco-Manipulation and Aerial Manipulation [3, 4], the union of which may be referred to as Whole-Body Aerial Loco-Manipulation.

Humanoid and flying robot control has developed along different directions, and suffer from specific limitations.

Humanoid robot control is often adddressed assuming the robot attached to ground, and in this case the robot is referred to as  fixed-base [5]. To weaken this assumption, one can apply the Euler-Poincarè formalism that provides one with singularity free equations of motion for the humanoid robot [6, Chapter 13]. In this case, the robot is referred to as floating base. When considering these robot equations of motion, however, the mechanical system representing the humanoid robot is underactuated, which forbids full feedback linearization of the underlying system [7]. The system underactuation is usually dealt with by means of constraints arising from the contacts between the robot and the environment. From the control design perspective, instead, a common strategy for humanoid robots is based on the so called stack-of-task [8]. These strategies usually consider several control objectives organized in a hierarchical or weighted prioritization. Often, the high-priority task is the stabilization of the robot momentum [8, 9, 10], whose essence is that of controlling the robot center-of-mass and angular momentum while guaranteeing stable zero-dynamics [11]. Quadratic programming (QP) solvers can be used to monitor contact forces while achiving momentum control [12, 13].

Besides classical flight control [14], small and versatile aircraft – referred to as Vertical Take Off and Landing (VTOL) vehicles – have attracted the nonlinear control community effort of the last decade (see, e.g., [15, 16]). One of the main assumptions of these work is that the flying robot is powered by a body-fixed thrust force, and moves at relatively small velocities. This latter assumption renders the aerodynamic forces negligible when compared to gravity, and drag effects are but seldom taken into account [16]. Then, a common approach for VTOL position control is the so-called vectored-thrust control paradigm: the aircraft angular velocity is considered as a control input, and its main role is to align thrust and gravity forces.

We believe that there is a strong technological benefit in bringing humanoid and flying robots closer: a platform combining these two robot natures may have, at least theoretically, the capacities of flight, contact locomotion, and manipulation, eventually not all used at the same time. A first step in this direction has already been taken by the so-called Aerial Manipulation, which conceives platforms capable to fly and manipulate [4]. Most robots having these two capacities are composed of a VTOL equipped with one or several robotic arms [17, 18]. These robots, however, are not energetically efficient when flying in confined spaces, and an additional capacity of contact locomotion may lower energy consumption considerably. Also, the robot capacity of making contacts with the environment may robustify the achievement of manipulation tasks: an additional end-effector may be used to make a contact with the environment, thus robustifying the overall system against wind gusts.

This paper takes a further step in bringing humanoid and flying robots closer, and proposes a control architecture for flying humanoid robots. We assume that a humanoid robot is powered by four thrust forces installed at the robot end effectors, namely the robot hands and feet. Four turbo engines exemplify the robot actuation assumed in this paper. The control objective is the asymptotic stabilization of the robot centroidal momentum [19], which allows us to stabilize a reference trajectory for the center of mass and keep small, bounded errors between a desired orientation and the robot base frame. Reminiscent of the vectored-thrust control paradigm in VTOL control, we assume that the robot joint velocities can be considered as control input, and we then use them to align the effect of thrust forces with gravity effects. Aerodynamic effects are here neglected, and stability and convergence is shown to be in the sense of Lyapunov. A quadratic programming (QP) solver is employed to consider actuation limits. Finally, the robot joint torques that stabilize the (desired) joint velocities (in turn achieving momentum control) are generated via an high-gain control at the joint level. Simulation results performed on the humanoid robot iCub verify the soundness of the proposed approach.

The paper is structured as follows. Section II introduces notation, equations of motion, and robot actuation. Section III describes the strategy for momentum, center of mass position, and orientation control. Simulations verifying the soundness and robustness of the control laws are presented in Section IV. Remarks and perspectives conclude the paper.

II Background

II-A Notation

• •

$A$ denotes an inertial (absolute) frame, composed of a point (origin) and an orientation frame with its $z$ axis pointing against the gravity, and $g\in\mathbb{R}$ the norm of gravitational acceleration. Given two orientation frames $A$ and $B$, and vectors of coordinates expressed in these orientation frames, i.e. $\prescript{A}{}{o},\prescript{B}{}{p}\in\mathbb{R}^{3}$, respectively, the rotation matrix $\prescript{A}{}{R}_{B}$ is such that ${}^{A}p=^{A}R_{B}^{B}p+^{A}o_{B}$.

• •

$1_{n}\in\mathbb{R}^{n\times n}$ is the identity matrix of size $n$; $0_{m\times n}\in\mathbb{R}^{m\times n}$ is the zero matrix of size $m\times n$ and $0_{n}=0_{n\times 1}$.

• •

$S(x){\in}\mathbb{R}^{3\times 3}$ is the skew-symmetric matrix such that $S(x)y{=}x{\times}y$, with $\times$ the cross product operator in $\mathbb{R}^{3}$.

II-B Robot modelling

We assume that the humanoid robot is composed of $n{+}1$ rigid bodies – called links – connected by $n$ joints with one degree of freedom each. The configuration space of the multi-body system can then be characterized by the position and the orientation of a frame attached to a robot link – called base frame $B$ – and the joint configurations. More precisely, the robot configuration space is defined by: $\mathbb{Q}=\mathbb{R}^{3}\times SO(3)\times\mathbb{R}^{n}.$ An element of the set $\mathbb{Q}$ is a triplet $q=(\prescript{A}{}{o}_{B},\prescript{A}{}{R}_{B},s)$, where $(\prescript{A}{}{o}_{B},\prescript{A}{}{R}_{B})$ denotes the origin and orientation of the base frame expressed in the inertial frame, and $s$ – which characterizes the robot shape – denotes the joint angles. The velocity of the multi-body system can then be characterized by the set $\mathbb{V}$ defined by: $\mathbb{V}=\mathbb{R}^{3}\times\mathbb{R}^{3}\times\mathbb{R}^{n}$. An element of $\mathbb{V}$ is then $\nu=(\mathrm{v}_{B},\dot{s})$, where $\mathrm{v}_{B}=(^{A}\dot{o}_{B},^{A}\omega_{B})$ is the linear and angular velocity of the base frame w.r.t. the frame $A$, i.e. ${}^{A}\dot{R}_{B}=S(^{A}\omega_{B})^{A}{R}_{B}$.

Applying the Euler-Poincaré formalism [20, Ch. 13.5] to the robot yields the following equations of motion:

[TABLE]

where ${M},{C}\in\mathbb{R}^{n+6\times n+6}$ are the mass and Coriolis matrix, respectively, ${G}\in\mathbb{R}^{n+6}$ is the gravity vector, $\tau$ are the internal actuation torques, and $F_{k}\in\mathbb{R}^{3}$ is the $k$th of the $m$ external forces applied by the environment on robot. In particular, we assume that the application point of the external force $F_{k}$ is the origin of a frame $\mathcal{C}_{k}$, which is attached to the robot’s link where the force acts; the external force $F_{k}$ is expressed in a frame whose orientation coincides with that of the inertial frame $A$. The Jacobian ${J}_{k}={J}_{k}(q)$ is the map between the robot’s velocity ${\nu}$ and the linear velocity ${}^{A}\dot{o}_{\mathcal{C}_{k}}\in\mathbb{R}^{3}$ of the origin of $\mathcal{C}_{k}$, i.e. ${}^{A}\dot{o}_{\mathcal{C}_{k}}={J}_{k}(q){\nu}.$

II-C Robot actuation

We assume that the robot is powered by four thrust forces $T_{1},T_{2},T_{3},T_{4}\in\mathbb{R}$ that act along the directions $\prescript{A}{}{\imath}_{1},\prescript{A}{}{\imath}_{2},\prescript{A}{}{\imath}_{3},\prescript{A}{}{\imath}_{4}\in\mathbb{R}^{3}$, with $|\prescript{A}{}{\imath}_{i}|=1$, $\forall i\in\{1,2,3,4\}$, respectively. The application points $\prescript{A}{}{o}_{i}\in\mathbb{R}^{3}$ of the thrust forces are the origins of four frames attached to the robot end-effectors, e.g., the robot hands and foot. The thrust force directions, instead, move accordingly to the robot end-effectors, since thrust forces are assumed to be attached to the end-effector links. Figure 1 depicts the notation used for the robot actuation. Four turbo engines installed at the robot end-effectors exemplify the actuation assumed for the humanoid robot111 For the humanoid robot iCub, we are considering the turbo engine JETCAT P100 RX [21] (weight: 1050gr; length: 245mm; thrust: 100N @152000 1/min). Note that the weight of iCub is about 30 Kg, which means that four engines suffice for hovering when fuel is kept off board. . We also assume that each thrust force $T_{i}$ is measurable. This latter assumption holds, for instance, when force sensors are installed in series with the turbo engines.

By defining $T:=(T_{1},T_{2},T_{3},T_{4})$, the effects of the external thrust forces on the right hand side of the equations of motion (1) can be compactly written as follows:

[TABLE]

III Control Design

III-A Problem statement

In light of section II-C, the equations of motion

[TABLE]

are powered by $n+4$ control inputs. This in turn implies that controlling the entire robot configuration space $\mathbb{Q}=\mathbb{R}^{3}\times SO(3)\times\mathbb{R}^{n}$, which is of dimension222The set $\mathbb{Q}$ is a Lie Group. Hence, the dimension of $\mathbb{Q}$ is the dimension of the algebra associated with it. $n+6$, may not be straightforward being system (3) underactuated and thus not feedback linearisable. Let us remark that one may attempt at the stabilization of the configuration space $\mathbb{Q}$ by applying advanced techniques developed for underactuated systems evolving on Lie Groups [22, 23]. The application of these techniques, however, is beyond the scope of this paper.

Assume that the control objective is the asymptotic stabilization of a frame – i.e. origin and orientation  – associated with a robot link. Without loss of generality, assume that one wants to control the base frame of the humanoid robot. The equations of motion of the base frame are given by:

[TABLE]

with ${\mathrm{v}}_{B}\in\mathbb{R}^{6}$ the linear and angular velocity of the base frame, $J_{B}\in\mathbb{R}^{6\times n+6}$ the Jacobian of the base frame, and $\dot{\nu}$ the robot acceleration obtained from the robot equations of motion (3). More precisely, by using the equations (3), one can evaluate the robot acceleration $\dot{\nu}$ and substitute it into Eq. (4), thus obtaining an instantaneous relationship between the base acceleration $\dot{\mathrm{v}}_{B}$ and the control inputs $(\tau,T)$, i.e.

[TABLE]

Then, one may attempt at the control of a reference frame by performing feedback linearisation of the above function with proper feedback correction terms to achieve asymptotic stability. One of the main drawbacks of this approach is that the joint torques $\tau$ have little influence on the base acceleration $\dot{\mathrm{v}}_{B}$, which may render the associated control laws ill-posed especially close to constant reference position-and-orientation for the base frame. As a matter of fact, at low joint velocities, a first approximation for the robot equations of motion is given by the Newton-Euler equations, which corresponds to assuming that the robot behaves as a single rigid body. In this case, the dynamics $\dot{\mathrm{v}}_{B}$ can be obtained from the Newton-Euler equations, which clearly do not depend upon the (internal) joint torques $\tau$ but only on the four (external) thrust forces. Having only four inputs in this dynamics, one cannot perform feedback linearisation of the six-dimensional dynamics $\dot{\mathrm{v}}_{B}$.

Now, since the mass matrix $M(q)$ is positive definite, and thus invertible, the equations (3) point out that the joint dynamics $\ddot{s}$ can be feedback-linearized via a proper choice of the joint torques $\tau$. So, any differentiable, desired joint velocity $\dot{s}_{d}\in\mathbb{R}^{n}$ can be stabilized with any desired settling time. We then make the following assumption.

Assumption 1**.**

The joint velocity $\dot{s}:=u_{2}$ and the thrust forces rate-of-change, i.e. $\dot{T}:=(\dot{T}_{1},\dot{T}_{2},\dot{T}_{3},\dot{T}_{4}):=u_{1}$, can be chosen at will and then considered as control inputs.

In the language of Automatic Control, assuming $\dot{s}$ as control variable is a typical backstepping assumption. Then, the production of the joint torques associated with the desired joint velocities can be achieved via classical nonlinear control techniques [24, p. 589] or high-gain control.

III-B Control objective and centroidal momentum dynamics

This section shows that despite the aforementioned underactuation, it is possible to conceive control laws for the Newton-Euler equation of the multibody system without any approximation. Then, the control objective for the reminder of this section is defined as follows:

• •

Asymptotic stabilization of the robot centroidal momentum $\mathrm{h}\in\mathbb{R}^{6}$ about desired, smooth values $\mathrm{h}_{d}(t)\in\mathbb{R}^{6}$.

When complemented with integral correction terms, the laws for the momentum control allows us to:

$i)$ stabilize a reference trajectory $r(t)\in\mathbb{R}^{3}$ for the robot center of mass ${}^{A}c\in\mathbb{R}^{3}$;

$ii)$ keep small, bounded errors between a reference orientation $R_{d}(t)\in SO(3)$ and the robot base frame.

Note that the choice of the above objective renders the dimension of the control task equal to six. This means that under Assumption 1, we may attempt at achieving this task by means of the control inputs $(\dot{s},\dot{T})$. More precisely, let $\tilde{\mathrm{h}}\in\mathbb{R}^{3}$ denote the momentum error defined by

[TABLE]

Then, by recalling that the rate-of-change of the centroidal momentum equals the summation of all external wrenches acting on the robot [25], one has the following dynamics:

[TABLE]

with $e_{3}:=(0,0,1,0,0,0)^{\top}$,

[TABLE]

Eq. (6a) points out that the dynamics $\dot{\tilde{\mathrm{h}}}\in\mathbb{R}^{6}$ are underactuated even if one assumes that the thrust intensities $T\in\mathbb{R}^{4}$ can be considered as control input, and this renders the control of these dynamics not straightforward.

The equation (6a) highlights also that at the equilibrium configuration $(\tilde{\mathrm{h}},\dot{\tilde{\mathrm{h}}}){=}(0,0)$, the effect of the thrust intensities must oppose the gravity plus $\dot{\mathrm{h}}_{d}(t)$, i.e.

[TABLE]

The above equation is reminiscent of the so-called vectored-thrust control paradigm used in recent flight dynamics control techiniques. In this literature, in fact, the aircraft angular velocity is assumed as control input, and then exploited to align the thrust force against the effect of gravity and, in general, of external forces [16, 15]. This in turn emphasizes the role of the joint velocities $\dot{s}$ in establishing Eq. (11): they are in charge of aligning the total thrust force $A(q)T$ to the gravity and desired momentum rate-of-change effect.

More precisely and generally, define

[TABLE]

with $K_{P},K_{D}{\in}\mathbb{R}^{6\times 6}$ two symmetric positive definite matrices, and the variable $I(t)$ –representing the integral of $\tilde{\mathrm{h}}$– governed by $\dot{I}{=}\tilde{\mathrm{h}}$. Then, Eqs. (6a) can be rewritten as follows:

[TABLE]

with the matrix $A$ given by (8a),

[TABLE]

and $J_{r}$ the Jacobian matrix mapping the robot velocity $\nu$ into the velocities $\Omega:=(\dot{r}_{1},\omega_{1},\dot{r}_{2},\omega_{2},\dot{r}_{3},\omega_{3},\dot{r}_{4},\omega_{4})\in\mathbb{R}^{24}$, i.e. $\Omega=J_{r}(q)\nu,$ where $\omega_{i}\in\mathbb{R}^{3}$ is the angular velocity associated to the $i$th end-effector frame, i.e. $\dot{\prescript{A}{}{\imath}_{i}}=S(\omega_{i})\prescript{A}{}{\imath}_{i}$.

III-C Momentum control

In view of the dynamics (14a), the main role of the control inputs $(u_{1},u_{2})$ is to bring the variable $\tilde{\xi}$ to zero, which means that the effect of the thrust forces must oppose the apparent force $F$ (see Eq. (12a)). To introduce the control laws accomplishing this task and stabilizing the desired momentum $h_{d}(t)$, let us recall that the centroidal momentum $\mathrm{h}$ is linear versus the robot velocity $\nu$. Namely, there exists a Jacobian matrix $J_{\mathrm{h}}(q)\in\mathbb{R}^{6\times n+6}$ such that

[TABLE]

with $J^{b}_{\mathrm{h}}\in\mathbb{R}^{6\times 6}$ an invertible matrix, and $J^{s}_{\mathrm{h}}\in\mathbb{R}^{6\times n}$. The matrix $J_{\mathrm{h}}$ is usually referred to as centroidal momentum matrix. We can now present the control laws for system (14a).

Theorem 1**.**

Assume that Assumption 1 holds and define

[TABLE]

with $K_{O}\in\mathbb{R}^{6\times 6}$ a symmetric positive definite matrix.

If there exist smooth control inputs $(u_{1},u_{2}){\in}\mathbb{R}^{4+n}$ such that

[TABLE]

*then the closed loop equilibrium point $(I,\tilde{\mathrm{h}},\tilde{\xi})=(0,0,0)$ of system (14a) is globally asymptotically stable. *

The proof is given in the Appendix. The main condition of the above theorem is Eq. (24). Let us remark that the number of control inputs to satisfy this condition is $n+4$, which means that as long as

[TABLE]

one is left with a redundancy of dimension $n-2$ to render Eq. (24) satisfied. This redundancy is later exploited to attempt at the stabilization of the system zero dynamics, i.e. the evolution of system (3) at $(I,\tilde{\mathrm{h}},\tilde{\xi})=(0,0,0)$.

The explicit form of the state feedback laws, namely $(u_{1},u_{2})=C(q,\nu,t)$, is here omitted because the expression (24) is later exploited into the formulation of a quadratic programming problem, which allows us to take into account inequality constraints on the control inputs $(u_{1},u_{2})$.

Let us remark that the control laws satisfying Eq. (24) are similar to those obtained by applying pure-feedback linearization techniques with output $I$. In particular, the output $I$ is of relative degree equal to three, and one can then apply feedback linearization on the obtained dynamics. The laws deduced with this approach, however, usually forbid to choose control gains independently from each other when the relative degree is higher than two. Also, simulations we have performed tend to show that the law proposed here are more robust w.r.t. to modeling errors than those obtained from pure feedback linearization.

III-D Velocity and position control

The control laws satisfying Eq. (24) can also be used to stabilize a (smooth) desired velocity $v_{d}(t)\in\mathbb{R}^{3}$ for the robot center-of-mass. To this purpose, let us recall that the centroidal momentum $\mathrm{h}$ can be decomposed into a linear and angular component $h^{l},h^{\omega}\in\mathbb{R}^{3}$, respectively. Recall also that the linear component $h^{l}$ is given the robot center-of-mass velocity $\dot{{}^{A}c}$ times its mass $m$. In formule,

[TABLE]

Consequently, the control laws stabilizing a desired velocity $v_{d}$ for the robot center-of-mass are those satisfying Eq. (24) with the desired momentum $\mathrm{h}_{d}(t)$ defined as follows:

[TABLE]

with $h_{d}^{\omega}\in\mathbb{R}^{3}$ the desired angular momentum. Clearly, stability and convergence statements of Theorem 1 are retained under the same assumptions.

Analogously, one can exploit the control laws satisfying Eq. (24) to stabilize a (smooth) desired position $r(t)\in\mathbb{R}^{3}$ for the robot center-of-mass. In particular, it suffices to choose the desired momentum $\mathrm{h}_{d}$ as in Eq. (26) with $v_{r}=\dot{r}$ and to set the integral initial condition $I(0)$ such that

[TABLE]

Again, stability and convergence statements of Theorem 1 are retained under the same assumptions.

III-E Orientation control

This section proposes modifications to Eq. (24) for the control of the robot base frame $\prescript{A}{}{R}_{B}\in SO(3)$ towards desired values $R_{d}\in SO(3)$. Let us first make a short digression on orientation control on $SO(3)$.

The problem of stabilizing a desired orientation $R_{d}{\in}SO(3)$ for a rigid body orientation $R$ may not be straightforward. For instance, it is known that the topology of $SO(3)$ forbids the design of smooth controllers that globally asymptotically stabilize a reference orientation $R_{d}$ [26]. Then, quasi-global asymptotic stability is a common feature that orientation controllers guarantee. Just to recall a result:

Lemma 1** ( [27] p. 173).**

Let skew$(A){:=}\tfrac{1}{2}(A{-}A^{\top})$ for any matrix $A\in\mathbb{R}^{3}$, and the operator $(\cdot)^{\vee}$ defined by $x=S(x)^{\vee}$. Consider the orientation dynamics $\dot{R}=S(\omega)R,$ where $\omega\in\mathbb{R}^{3}$ is considered as control input. Assume that the control objective is the asymptotic stabilization of a (constant) desired attitude $R_{d}\in SO(3)$. Then,

[TABLE]

renders the equilibrium point $R=R_{d}$ quasi globally stable.

Let us recall that to evaluate the control torques generating the angular velocity (28), the correction terms (28) are first multiplied by the body inertia, and then complemented with additional velocity correction terms [27, p. 178].

The orientation control for the robot base frame proposed in this paper builds upon the above digression, and consists in modifying the term $I$ in (24) so as to take into account the orientation correction term (28). More precisely, recall that the robot centroidal angular momentum $h^{\omega}$ can be expressed in terms of the total robot inertia $\mathbb{\bar{I}}\in\mathbb{R}^{3\times 3}$ as follows, $h^{\omega}=\mathbb{\bar{I}}(q)\omega_{o},$ where $\omega_{o}\in\mathbb{R}^{3}$ is the so-called locked angualr velocity [19]: the terminology encompasses the fact that when joint velocities are blocked, $\dot{s}\equiv 0$, then $\omega_{o}$ corresponds to the angular velocity of the humanoid robot, which behaves as a rigid body. This suggests that the control laws (24) with

[TABLE]

can guarantee good tracking performance of the base frame $\prescript{A}{}{R}_{B}$ towards the reference orientation $R_{d}$. It is important to emphasize that the asymptotic stability of the equilibrium point containing $\prescript{A}{}{R}_{B}=R_{d}$ is not guaranteed in this case. Simulations that we have performed, however, tend to show that the suggested control law for the orientation control of the base frame can guarantee good tracking performance even if asymptotic stability is not guaranteed. Control laws ensuring stability properties of the equilibrium point containing $\prescript{A}{}{R}_{B}=R_{d}$ will be the subject of forthcoming studies.

III-F Orientation and position control

In light of the above, the control of the robot center-of-mass and base frame can be attempted by using the control laws satisfying Eq. (24) with $\mathrm{h}_{d}$ given by (26), $v_{r}=\dot{r}$, and

[TABLE]

III-G Zero dynamics and optimization problem

Now, assume that the humanoid robot center of mass and base frame are stabilized about the reference position and orientation, respectively. This constrains only six out of the $n+6$ degrees of freedom of the robot, thus leaving an $n$-dimensional * free-motion* of the system at the desired values: this free motion should be at least bounded. More precisely, this section discusses how to deal with the boundedness of the system zero dynamics by exploiting the input redundancy when satisfying Eq. (24).

Let us recall that stability and convergence in Theorem 1 are shown when Eq. (24) holds, i.e. $\delta+Au_{1}+Bu_{2}{=}0_{6}$. Finding the control inputs $(u_{1},u_{2})\in\mathbb{R}^{n+4}$ such that this equation holds in general leaves a $n-2$ dimensional input redundancy, which can be used for other purposes. We here use this redundancy so as the joint velocity $u_{2}=\dot{s}$ are as close as possible to a postural task of the following form:

[TABLE]

with $K^{p}_{P}\in\mathbb{R}^{n\times n}$ a symmetric positive definite matrix, and $s_{r}\in\mathbb{R}^{n}$ a reference position for the joint configuration. If $u_{2}=p$ the joint configurations tend to the reference value $s_{r}$, thus reducing the risk of unstable zero dynamics.

We combine the tasks of satisfying Eq. (24) and $u_{2}=p$ in a weighted optimization problem of the following form:

[TABLE]

where $\lambda_{m},\lambda_{p},\lambda_{s},\lambda_{T}$ are positive weighting constants, and $lb_{1},ub_{1}\in\mathbb{R}^{4}$ and $lb_{2},ub_{2}\in\mathbb{R}^{n}$ are the lower and upper bounds for the thrust-intensity variations $u_{1}$ and joint velocities $u_{2}$, respectively. Note that the cost function of the optimization problem (32a) contains also some regularization terms depending on $|u_{2}|^{2}$ and $|u_{1}|^{2}$.

III-H Torque control for joint velocity stabilisation

The solution to the problem (32a) is a pair $(u^{*}_{1},u^{*}_{2})$, namely the instantaneous rate-of-change of the thrust intensities $\dot{T}^{*}$ and the joint velocities $\dot{s}^{*}$. This latter value is then interpreted as a desired value to be stabilized by a torque-control law. More precisely, the route we follow is high-gain control for the stabilization of $\dot{s}^{*}$. Now, partition (3) as follows $M~{}=~{}\begin{pmatrix}\mathbb{I}&&\mathbb{F}\\ \mathbb{F}^{\top}&&\mathbb{H}\end{pmatrix}$ with $\mathbb{I}\in\mathbb{R}^{6\times 6}$, $\mathbb{F}\in\mathbb{R}^{6\times n}$, $\mathbb{H}\in\mathbb{R}^{n\times n}$, $b:=\begin{pmatrix}b_{b}\\ b_{s}\end{pmatrix}:=C(q,\nu)\nu+G(q)f:=\begin{pmatrix}f_{b}\\ f_{s}\end{pmatrix}$ with $b_{b},f_{b}{\in}\mathbb{R}^{6}$ and $b_{s},f_{s}{\in}\mathbb{R}^{n}$. Then, from Eq. (3), one gets

[TABLE]

In view of Eq. (34a), the stabilization of a desired joint velocity $u^{*}_{2}$ is then attempted by means of the following high-gain torque control law:

[TABLE]

IV Simulation Results

This section presents simulations results obtained by applying the control algorithm discussed in the previous section. These simulations have been carried out by using the humanoid robot iCub with 25 DoFs [28]. In particular, the robot joint angles $s\in\mathbb{R}^{25}$ can be partitioned into five robot parts, which correspond to the robot torso $s_{t}\in\mathbb{R}^{3}$, left arm $s_{la}\in\mathbb{R}^{5}$, right arm $s_{ra}\in\mathbb{R}^{5}$, left leg $s_{ll}\in\mathbb{R}^{6}$, and right leg $s_{rl}\in\mathbb{R}^{6}$, i.e. $s=(s_{t},s_{la},s_{ra},s_{ll},s_{rl})$.

IV-A Simulation environment and control parameters

The custom simulation environment is in charge of integrating the equations of motion (3) with the joint torques $\tau$ given by (36) and the thrust intensities $T$ provided by the time integration of $u^{*}_{1}$, which are in turn generated by (32a).

For time-integration purposes of (3), we parametrize $SO(3)$ by means of a quaternion representation $\mathcal{Q}\in\mathbb{R}^{4}$. The resulting state space system, which is integrated through time, is then: $\chi:=(^{A}o_{{B}},\mathcal{Q},s,\dot{p}_{{B}},\omega_{{B}},\dot{s})$, and its derivative is given by $\dot{\chi}=(^{A}\dot{o}_{{B}},\dot{\mathcal{Q}},\dot{s},\dot{\nu}).$ The constraints $|\mathcal{Q}|=1$ are enforced during the integration phase [29]. The system evolution is obtained by integrating the constrained dynamical system with the numerical integrator MATLAB ode15s thanks to our software abstraction interfaces – described in [30] – which provide us the elements of the equations of motion (36), e.g. the mass matrix.

The humanoid robot initial condition is $\chi(0)=(0_{3},1,0_{3},0_{3},0,25,0_{3},0,25,0_{3},0_{12})$, which corresponds to having the robot arms open ($25^{\circ}$ on the robot shoulders) and the base frame corresponding to the inertial frame. The robot legs are at the zero position, which corresponds to having the robot legs straight.

We then set the control gains as follows: for the momentum control described in Theorem 1, $K_{P}=(1_{3}5,0_{3};0_{3},1_{3}50)$, $K_{D}=2\sqrt{K_{P}}$, $K_{O}=10$; for the optimization problem (32a): $\lambda_{m}=50$, $\lambda_{p}=1$, $\lambda_{s}=50$, $\lambda_{T}{=}1$, $K^{p}_{P}{=}\text{diag}(1,1,1,5,5,5,10,1,5,5,5,10,\text{ones}(1,12))$, with limits $ub_{1}=-lb_{1}=45\ \text{ones}(4,1)\ [^{\circ}/s]$, $ub_{2}=-lb_{2}=100\ \text{ones}(25,1)\ [N/s]$; and for the high-gain torque control (36): $K^{s}_{I}=10^{3}\ \text{diag}(\text{ones}(25,1))$, $K^{s}_{P}=2\sqrt{K^{s}_{I}}$.

In the following simulations, noise is not considered. The main reason why is that given the robot actuation, we expect that the noise produced by turbo engines may be higher than that produced by the robot joint motors. Noise nature and amplitude produced by turbo engines when attached to the humanoid robot is not known for the time being. Hence, noise will be considered after proper experimental campaigns will be carried on to identify noise nature and amplitude.

IV-B *Simulation 1: piece-wise constant velocity trajectory without

orientation control*

The first simulation concerns the stabilization of a reference trajectory $r(t)\ [m/s]$ obtained by integrating the following piece-wise constant reference velocity:

[TABLE]

with $r(0)=0_{3}$. To stabilize the resulting trajectory, we then apply the algorithm (32a)-(36), with the integral term $I$ given by (27) and $h^{\omega}_{d}=0_{3}$. Data associated with this simulation are depicted in Figures 2–4. In particular, from top to bottom, Figure 2 depicts the tracking error of the robot center of mass, the angular momentum error, and the norm of the joint velocity error $u^{*}_{2}-\dot{s}$. Since all tracking errors converge to zero, the simulation results shown in Figure 2 verify the statements of Theorem 1 when combined with the high-gain control (36). The control gains $K_{P}$ and $K_{O}$ influence the settling time with which both tracking errors of the center-of-mass and the angular momentum tend to zero. Still in Figure 2, observe also that the norm of the velocity error $u^{*}_{2}-\dot{s}$ tends to zero meaning that the high-gain control (36) for joint velocity stabilization is working properly. The joint torques (36) to obtain this stabilization are depicted in Figure 3. Observe that despite the high-gain control chosen in this paper, the joint torques remain relatively small during the task $|u^{*}_{2}-\dot{s}|\rightarrow 0$.

The outcome $u^{*}_{1}$ of the optimization problem (32a) is depicted in Figures 4. Note how the optimizer tends to choose solutions that render equal all thrust intensities variations thanks to the term $\lambda_{T}$. The smaller this regularization term, the more different the rate-of-change $u^{*}_{1}$ of the jet intensities.

IV-C Simulation 2: helicoidal flight with orientation control and non-perfect control model to test controller robustness

The second simulation consists in tracking the reference trajectory given by

[TABLE]

with

[TABLE]

After $10\ [s]$, it represents a helicoidal trajectory of radius $2\ [m]$ with vertical speed of $1\ [m/s]$. Differently to the previous section, we here test the trajectory tracking controller with orientation control. So, we apply (32a)-(36) with the integral term $I$ given by (30) and $R_{d}$ obtained by rotating $\prescript{A}{}{R}_{B}(0)$ about the vertical axis of $60^{\circ}$. Then, the robot is expected to rotate about the vertical axis to reduce the orientation error. Also, to test the control robustness, the controller has been calculated with estimated parameters that differ from the real ones of $10\ \%$. More precisely, the controller is evaluated by over-estimating the real model, i.e. $\hat{M}(q)=1.1M(q)$, $\hat{C}(q,\nu)\nu+\hat{G}(q)=1.1(C(q,\nu)\nu+G(q))$, and $\hat{J}_{h}(q)=1.1J_{h}(q)$. As depicted in Figure 5, modeling errors lead to non convergence of the center-of-mass tracking error and orientation error to zero. However, relatively small bounded errors between actual and reference signals are kept, despite the controller (32a)-(36) (30) is not guaranteed to possess stability and convergence properties. This shows a degree of robustness versus modeling errors of the control laws presented in this paper. Let us remark that the orientation control performance degrades quickly for a time-varying reference orientation, and this calls for studies and extensions of the control laws presented in this paper for time-varying orientation tracking. Due to lack of space, plots depicting the control inputs are omitted.

V Conclusions

This paper has proposed extensions of the so-called vectored-thrust control paradigm used in VTOL control to the case of an underactuated flying humanoid robot. The main assumption is that the humanoid robot is powered by four thrust forces placed at the robot end effectors: these forces reduce but do not eliminate the well-known humanoid robot underactuation. Within this actuation framework, we have presented control laws guaranteeing stability and convergence properties for the robot centroidal momentum. Slight modifications to these laws allows us to track a desired reference trajectory for the center of mass and keep small tracking errors between a reference orientation and the robot base frame.

In this respect, future work consists in proposing control laws guaranteeing stability and convergence not only for the desired center of mass position and angular momentum, but also for the reference orientation of the base frame. Also, the model of the external forces acting on the robot neglects the aerodynamic phenomena. Hence, future work will also consist in extending vectored-thrust control paradigm with aerodynamic effects to the case of the considered flying humanoid [31, 32, 33].

Appendix: proof of Theorem 1

Consider the following Lyapunov function candidate

[TABLE]

Note that $V=0\iff(I,\tilde{h},\tilde{\xi})=0$. Direct calculations show $\dot{V}$ along the trajectories of system (14a) is given by:

[TABLE]

Now, it is clear that $\dot{V}\leq 0$ if the term in the parenthesis on the right hand side of the above equation is equal to $-\tilde{\xi}$. In view of (15a) and (12a), imposing ${-}\tilde{\xi}=\dot{\tilde{\xi}}+K^{-1}_{O}\tilde{\mathrm{h}}$ yields:

[TABLE]

By substituting $\tilde{\mathrm{h}}=\mathrm{h}-\mathrm{h}_{d}$ and $\mathrm{h}~{}=~{}J^{b}_{\mathrm{h}}(q)\mathrm{v}_{B}~{}+~{}J^{s}_{\mathrm{h}}(q)u_{2}$ in (42), one gets the condition (24) of Theorem 1. As a consequence, if (24) – and consequently (42) – are always satisfied, then $\dot{V}$ in Eq. (41) becomes $\dot{V}=-\tilde{\mathrm{h}}^{\top}K_{D}\tilde{\mathrm{h}}-\tilde{\xi}^{\top}K_{O}\tilde{\xi}\leq 0.$ And $\dot{V}\leq 0$ implies the stability of the equilibrium point and also global boundedness of the system trajectories.

Now, as long as (42) is satisfied, the closed loop dynamics is given by $\dot{\tilde{\xi}}=-\tilde{\xi}-K^{-1}_{O}\tilde{\mathrm{h}}$ and (14a) (15a): therefore, the closed loop dynamics is autonomous, and we can use LaSalle Theorem to conclude that $\dot{V}\rightarrow 0$. This implies that $\tilde{\mathrm{h}}\rightarrow 0$, $\tilde{\xi}\rightarrow 0$, and $\dot{\tilde{\xi}}\rightarrow 0$, $\dot{\tilde{\mathrm{h}}}\rightarrow 0$. By using these implications with (15a), namely $\dot{\tilde{\mathrm{h}}}=\tilde{\xi}-K_{D}\tilde{\mathrm{h}}-K_{P}I,$one obtains that $I\rightarrow 0$. Hence, the equilibrium point $(I,\tilde{\mathrm{h}},\tilde{\xi})=(0,0,0)$ is asymptotically stable. Global asymptotic stability comes from radial unboundedness of $V$.