Implementation and stability analysis of prioritized whole-body compliant controllers on a wheeled humanoid robot in uneven terrains

Abstract

In this work, we implement the floating base prioritized whole-body compliant control framework described in Sentis et al. (IEEE Transactions on Robotics 26(3):483–501, 2010) on a wheeled humanoid robot maneuvering in sloped terrains. We then test it for a variety of compliant whole-body behaviors including balance and kinesthetic mobility on irregular terrain, and Cartesian hand position tracking using the co-actuated (i.e. two joints are simultaneously actuated with one motor) robot’s upper body. The implementation serves as a hardware proof for a variety of whole-body control concepts that had previously been developed and tested in simulation. First, behaviors of two and three priority tasks are implemented and successfully executed on the humanoid hardware. In particular, first and second priority tasks are linearized in the task space through model feedback and then controlled through task accelerations. Postures, on the other hand, are shown to be asymptotically stable when using prioritized whole-body control structures and then successfully tested in the real hardware. To cope with irregular terrains, the base is modeled as a six degree of freedom floating system and the wheels are characterized through contact and rolling constraints. Finally, center of mass balance capabilities using whole-body compliant control and kinesthetic mobility are implemented and tested in the humanoid hardware to climb terrains with various slopes.

Appendix

1.1 Derivations of prioritized whole-body compliant control based on previous work (Sentis et al. 2010)

Rigid multibody dynamics can be developed using the Lagrangian formalism, which leads to the following equation of motion,

$$\begin{aligned} A(q) \, \ddot{q} + b(q,\dot{q}) + g(q) + J_\mathrm{constr}^{\,T}\, \lambda _\mathrm{constr}= U^{\,T}\tau _\mathrm{control},\nonumber \\ \end{aligned}$$

(62)

where $\{A, b, g\}$ are the inertial matrix, Coriolis-centrifugal, and gravity terms, $\tau _\mathrm{control} \in \mathbb{R }^{n_\mathrm{dofs}-(1+6)}$ is the vector of torques generated at the output of the motors (assuming completely rigid actuators), $\lambda _\mathrm{constr}$ are the Lagrangian multipliers associated with the tension forces on the biarticular transmission wire and the reaction forces on the wheel contacts against the ground. This Jacobian can be viewed as infinitesimal displacements of a coordinate $\delta _\mathrm{constr}$ defined as

$$\begin{aligned} \dot{\delta }_\mathrm{constr}\triangleq J_\mathrm{constr}\; \dot{q} = 0. \end{aligned}$$

(63)

Considering a rigid transmission in the biarticular joints and rigid contacts on the wheels, it is possible to obtain a model-based solution of the previous Lagrangian multipliers. By left multiplying Eq. (62) by $J_\mathrm{constr}\, A^{-1}$, applying the time derivatives of Eq. (63), i.e. ${\ddot{\delta }}_\mathrm{constr}= J_\mathrm{constr}{\ddot{q}} + \dot{J}_\mathrm{constr}\dot{q} = 0$, the Lagrangian multipliers can be solved as

$$\begin{aligned} \lambda _\mathrm{constr}= \overline{J}_\mathrm{constr}^{\,T}\left( U^{\,T}\tau _\mathrm{control}- b - g \right) + \varLambda _\mathrm{constr}\dot{J}_\mathrm{constr}\, \dot{q},\nonumber \\ \end{aligned}$$

(64)

where $\overline{J}_\mathrm{constr}\triangleq A^{-1}\, J_\mathrm{constr}^{\,T}\, \varLambda _\mathrm{constr}$ is the dynamically consistent generalized inverse of (12) and $\varLambda _\mathrm{constr}\triangleq (J_\mathrm{constr}A^{-1} J_\mathrm{constr}^{\,T})^{-1}$ is the joint inertial matrix projected into the manifold of the constraints.

Plugging the above Equation into (62) we remove the Lagrangian Multipliers term, yielding constrained dynamics

$$\begin{aligned}&A \, {\ddot{q}} + N_\mathrm{constr}^{\,T}\left( b + g \right) +J_\mathrm{constr}^{\,T}\varLambda _\mathrm{constr}\dot{J}_\mathrm{constr}\dot{q} \nonumber \\&\quad = \left( U N_\mathrm{constr}\right) ^{\,T}\tau _\mathrm{control}, \end{aligned}$$

(65)

where

$$\begin{aligned} N_\mathrm{constr}\triangleq I - \overline{J}_\mathrm{constr}J_\mathrm{constr}\end{aligned}$$

(66)

is the dynamically consistent null space of the constraint Jacobian.

It is of interest to formulate kinematic quantities in terms of actuated joints alone. In particular, we consider decomposing the generalized coordinates into actuated and floating / co-actuated portions, i.e.

$$\begin{aligned} q = D_\mathrm{kin} \begin{pmatrix} q_\mathrm{act}\\ q_\mathrm{unact}\end{pmatrix}, \end{aligned}$$

(67)

where $D_\mathrm{kin}$ is a matrix that distributes the joint coordinates of the actuated and floating / co-actuated joints to their respective positions in the robot’s kinematic chain, $q_\mathrm{act}$ are the actuated joints (i.e. attached to a motor), and $q_\mathrm{unact}$ correspond to the floating and co-actuated degrees of freedom.

Because of condition (63), we can state that joint velocities lie in the null space of the constraints, i.e. $\dot{q}^* \in \mathrm{Null}(J_\mathrm{constr})$, where $\dot{q}^{*}$ expresses the set of generalized velocities that fulfills all of the constraints (see Sentis (2007) for details). In other words, the constrained velocities can be expressed as the self motion manifold $\dot{q}^* = ( I - J_\mathrm{constr}^\# \, J_\mathrm{constr})\; \dot{q}$, where $J_\mathrm{constr}^\#$ is any right inverse of the constraints Jacobian (i.e. $J_\mathrm{constr}J_\mathrm{constr}^\# = I$) and $\dot{q}$ represents unconstrained velocities.

The constrained generalized velocities can be reconstructed from the velocities of the actuated DoFs alone using the following formula

$$\begin{aligned} \dot{q}^* = \overline{U N}_\mathrm{constr}\; \dot{q}_\mathrm{act}, \end{aligned}$$

(68)

where

$$\begin{aligned} \overline{U N}_\mathrm{constr}\triangleq A^{-1}\left( U N_\mathrm{constr}\right) ^{\,T}(\phi ^*)^+, \end{aligned}$$

(69)

is the the dynamically consistent generalized inverse of $U N_\mathrm{constr}, (.)^+$ is the Moore-Penrose pseudoinverse operator and

$$\begin{aligned} \phi ^* \triangleq UN_\mathrm{constr}A^{-1}(U N_\mathrm{constr})^{\,T}, \end{aligned}$$

(70)

is a projection of the inertia matrix in the reduced constrained manifold. Once more, see Sentis (2007) for a proof of (68).

Task entities to be controlled in the operational space can be expressed with a generic coordinate transformation $x_\mathrm{task}= T_\mathrm{task}(q) \in \mathbb{R }^{n_\mathrm{task}}$, where $T_\mathrm{task}$ is a kinematic transformation matrix, e.g. a homogeneous transformation for a Cartesian point, or a rotation and translation in the group $SE(3)$ for a spatial transformation of a frame, $n_\mathrm{task}$ is the number of degrees of freedom of the task coordinates, and $q$ is the generalized coordinates of the system including both the actuated and unactuated / co-actuated joints. It follows that instantaneous task kinematics can be expressed as $\dot{x}_\mathrm{task}= J_\mathrm{task}\; \dot{q}$, where $J_\mathrm{task}= \partial x_\mathrm{task}/\partial q \in \mathbb{R }^{n_\mathrm{task}\times n_\mathrm{dofs}}$ is the whole-body Jacobian matrix with respect to the generalized coordinates. The differential kinematics with respect to actuated joints alone can be expressed as

$$\begin{aligned} \dot{x}_\mathrm{task}= J_\mathrm{task}^* \; \dot{q}_\mathrm{act}, \end{aligned}$$

(71)

where $J_\mathrm{task}^*$ is the reduced constraint consistent Jacobian (a.k.a. Generalized Jacobian)

$$\begin{aligned} J_\mathrm{task}^* \triangleq J_\mathrm{task}\overline{UN}_\mathrm{constr}, \end{aligned}$$

(72)

with $J_\mathrm{task}^* \in \mathbb{R }^{n_\mathrm{task}\times n_\mathrm{act}}$, being a reduced form of the task Jacobian consistent with the general constraint conditions, and $n_\mathrm{act}$ being the number of degrees of freedom of the actuated joints. To prove (71), consider the constrained generalized velocities $\dot{q}^*$, and apply (68) to the instantaneous task kinematics , thus getting $\dot{x}_\mathrm{task}= J_\mathrm{task}\dot{q}^* = J_\mathrm{task}\overline{UN}_\mathrm{constr}\; \dot{q}_\mathrm{act}$.

Generalized Jacobians describe the relationship between instantaneous motions of the active hinges and the task frames. Additionally, they incorporate dynamic effects on the robot’s motion, such as the conservation of momenta (Umetami and Yoshida 1989; Dubowsky and Papadopoulos 1993) and the contact dynamics (Sentis et al. 2010). In the context of our robot Dreamer, they are effective at deriving controllers for constrained underactuated / co-actuated systems since they define the behavior of robots with respect to the actuated joints alone.

Let us consider the skill of touching objects using Dreamer’s right arm as shown in Fig. 10. We represent the skill using two coordinate systems,

$$\begin{aligned} x_\mathrm{skill}\triangleq \left\{ \begin{array}{l} x_\mathrm{hand}\\ x_\mathrm{posture}\\ \end{array}\right\} , \end{aligned}$$

(73)

where $x_\mathrm{hand}\in T(3)$ represents a coordinate attached to the hand midpoint, $T(3)$ is the group of translations and $x_\mathrm{posture}= q_\mathrm{act}\in \mathbb{R }^{n_\mathrm{act}}$ is the vector of actuated joints. For instance, the posture might consist of minimizing the joint posture with respect to a static pose frame.

In general, a prioritized task coordinate, $\mathrm{task}(k)$, can be kinematically characterized by its Generalized constrained Jacobian which in a general sense it is constrained both by the robot’s physical constraints and by the higher priority tasks, i.e.

$$\begin{aligned}&x_{\mathrm{task}(k)} = T_{\mathrm{task}(k)}(q),\end{aligned}$$

(74)

$$\begin{aligned}&\dot{x}_{\mathrm{task}(k)} = J_{\mathrm{task}(k)}^* \; \dot{q}_\mathrm{act}, \end{aligned}$$

(75)

where

$$\begin{aligned} J_{\mathrm{task}(k)}^* \triangleq J_{\mathrm{task}(k)} \overline{UN}_\mathrm{constr}N_{\mathrm{prec}(k)}^*, \end{aligned}$$

(76)

and $N_{\mathrm{prec}(k)}^*$ is the dynamically-consistent null-space of the higher priority tasks (i.e. preceding)

$$\begin{aligned} N_{\mathrm{prec}(k)}^* \triangleq I - \sum _{i=1}^{k-1} \overline{J}_{\mathrm{task}(i)}^{\;*} J_{\mathrm{task}(i)}^*. \end{aligned}$$

(77)

In the above derivations we have assumed that tasks with lower index $k$ have higher priority. As such, the priorities are established by projecting lower priority tasks on the null space of preceding tasks.

It can be demonstrated Sentis (2007), that the dynamics of the actuated joints can be expressed as

$$\begin{aligned} {\ddot{q}}_\mathrm{act}+ \phi ^* (b^* + g^*)= \phi ^* \tau _\mathrm{control}. \end{aligned}$$

(78)

where

$$\begin{aligned}&\phi ^* (b^*+g^*)\nonumber \\&\quad \triangleq U A^{-1}\left( N_\mathrm{constr}^{\,T}(b+g) + J_\mathrm{constr}^{\,T}\varLambda _\mathrm{constr}\dot{J}_\mathrm{constr}\dot{q}\right) . \end{aligned}$$

(79)

By left multiplying the above Equation by the generalized inverse

$$\begin{aligned} \overline{J}_{\mathrm{task}(k)}^{\;*} \triangleq \phi ^* \; J_{\mathrm{task}(k)}^{\,*T} (J_{\mathrm{task}(k)}^* \, \phi ^* \, J_{\mathrm{task}(k)}^{\,*T})^{-1}, \end{aligned}$$

(80)

and using the equality defined by differentiating (75), i.e. ${\ddot{x}}_{\mathrm{task}(k)} = J_{\mathrm{task}(k)}^* {\ddot{q}}_\mathrm{act}+ \dot{J}_{\mathrm{task}(k)}^* \dot{q}_\mathrm{act}$, the constrained task dynamics can be expressed as

$$\begin{aligned} \varLambda _{\mathrm{task}(k)}^* {\ddot{x}}_{\mathrm{task}(k)} + \mu _{\mathrm{task}(k)}^* + p_{\mathrm{task}(k)}^* = \overline{J}_{\mathrm{task}(k)}^{\,*T} \tau _\mathrm{control}, \end{aligned}$$

(81)

with $\{\varLambda _{\mathrm{task}(k)}^*, \mu _{\mathrm{task}(k)}^*, p_{\mathrm{task}(k)}^*\}$ being inertial, Coriolis-centrifugal, and gravitational terms respectively and not derived here.

If $J_{\mathrm{task}(k)}^*$ is full rank, the following control structure yields full control of the task’s closed loop dynamics,

$$\begin{aligned} \tau _\mathrm{control}= J_{\mathrm{task}(k)}^{*\,T} F_{\mathrm{task}(k)}. \end{aligned}$$

(82)

This statement can be proven by plugging the above torques into Eq. (81) and using the property of the generalized inverse $J_{\mathrm{task}(k)}^* \overline{J}_{\mathrm{task}(k)}^* = I$, thus getting

$$\begin{aligned} \varLambda _{\mathrm{task}(k)}^* {\ddot{x}}_{\mathrm{task}(k)} + \mu _{\mathrm{task}(k)}^* + p_{\mathrm{task}(k)}^* = F_{\mathrm{task}(k)}. \end{aligned}$$

(83)

The above transformation can be used to provide full control of the task accelerations ${\ddot{x}}_{\mathrm{task}(k)}$ whenever $\varLambda _\mathrm{task}(k)$ is full rank. This is the case when there is no conflict between the $k$-th task and higher priority ones. By defining the control input as

$$\begin{aligned} F_{\mathrm{task}(k)} = \varLambda _{\mathrm{task}(k)}^* u_{\mathrm{task}(k)} + \mu _{\mathrm{task}(k)}^* + p_{\mathrm{task}(k)}^*, \end{aligned}$$

(84)

where $u_{\mathrm{task}(k)}$ is an acceleration feedback control policy, the above controller yields closed loop dynamics

$$\begin{aligned} {\ddot{x}}_{\mathrm{task}(k)} = u_{\mathrm{task}(k)}. \end{aligned}$$

(85)

In our scheme, tasks are coordinates that need to track a position or force trajectory or converge to a goal. In contrast, postures are objectives that need to optimize a performance criterion in the null space of tasks. As shown in Eq. (73), a skill has one or more task coordinates to be controlled and one or more postures to be optimized in the redundant space. For the case of one task and one posture, we define the following prioritized control structure,

$$\begin{aligned} \tau _\mathrm{control}= J_\mathrm{task}^{\,*T} F_\mathrm{task}+ N_\mathrm{task}^{\,*T} \tau _\mathrm{posture}, \end{aligned}$$

(86)

where

$$\begin{aligned} N_\mathrm{task}^* \triangleq I - \overline{J}_\mathrm{task}^* J_\mathrm{task}^*, \end{aligned}$$

(87)

is the dynamically consistent null-space matrix of the reduced task Jacobian. An example of a three-priority controller is shown in Fig. 8.