Technical Implementations of the Sense of Balance

Abstract

Control algorithms for legged robots rely on accurate and fail-safe ego-motion estimation in order to keep balance and perform desired tasks. To this end, the robot must integrate the measurements from different sensor modalities into a single consistent state estimation. In particular, the estimation process must provide estimates of the gravity direction and the local velocities of the robot since those quantities are essential for stabilizing the system and to counteract external disturbances. In comparison to other types of robots, legged robots interact through intermittent contacts with the surrounding. This provides the system with an additional source of information which can be leveraged in order to improve the state estimation. Since there is no one-size-fits-all solution, the following chapter will provide an insight into the different concepts and algorithms by discussing state-of-the-art approaches and examples. This should enable the reader to design a tailored state estimation solution to his or her specific robot and environment.

Appendices

Appendix A: Handling 3D Rotations

A more detailed discussion on the following elaborations can be found in [2]. As members of the special orthogonal group SO(3), 3D rotations possess a multiplication operation (which is not commutative) but unfortunately do not have a concept of addition. Consequently the subtraction and differentiation, which are essential for most sensor fusion algorithms, do not exist either. In order to overcome this issue, the region around a specific rotation can be mapped to a 3D vector space (the so-called Lie algebra). This is often done by means of the exponential and logarithmic map at identity. There are different ways of selecting these maps and a common choice is:

$$\displaystyle \begin{aligned} \log: &SO(3) \to \mathbb{R}^3 {} \end{aligned} $$

(69)

$$\displaystyle \begin{aligned} &\boldsymbol{q}_{\mathcal{BI}} \mapsto \log(\boldsymbol{q}_{\mathcal{BI}}) = \boldsymbol{\varphi}_{\mathcal{BI}},\\ \exp: &\mathbb{R}^3 \to SO(3) {} \end{aligned} $$

(70)

$$\displaystyle \begin{aligned} & \boldsymbol{\varphi}_{\mathcal{BI}} \mapsto \exp(\boldsymbol{\varphi}_{\mathcal{BI}}) = \boldsymbol{q}_{\mathcal{BI}}, \end{aligned} $$

(71)

where $\boldsymbol {\varphi }_{\mathcal {BI}}$ is the passive rotation vector of the rotation parametrized by the unit quaternion $\boldsymbol {q}_{\mathcal {BI}}$.

These maps can now be used to define a boxplus operator and a boxminus operator as follows:

$$\displaystyle \begin{aligned} \boxplus : & SO(3) \times \mathbb{R}^{3} \rightarrow SO(3) {} \end{aligned} $$

(72)

$$\displaystyle \begin{aligned} & \boldsymbol{q}, \boldsymbol{\varphi} \mapsto \exp(\boldsymbol{\varphi}) \otimes \boldsymbol{q},\\ \boxminus : & SO(3) \times SO(3) \rightarrow \mathbb{R}^{3} {} \\ & \boldsymbol{q}, \boldsymbol{p} \mapsto \log(\boldsymbol{q} \otimes \boldsymbol{p}^{-1}).\end{aligned} $$

(73)

They represent a local concept of addition and subtraction and fulfill the axioms required by Hertzberg et al. [18]:

$$\displaystyle \begin{aligned} \boldsymbol{q} \boxplus \boldsymbol{0} &= \boldsymbol{q}, \end{aligned} $$

(74)

$$\displaystyle \begin{aligned} (\boldsymbol{q} \boxplus \boldsymbol{\varphi}) \boxminus \boldsymbol{q} &= \boldsymbol{\varphi}, \end{aligned} $$

(75)

$$\displaystyle \begin{aligned} \boldsymbol{q} \boxplus (\boldsymbol{p} \boxminus \boldsymbol{q}) &= \boldsymbol{p}. \end{aligned} $$

(76)

The regular addition and subtraction in the definition of differentials can now be replaced by the boxplus and boxminus operators in order to compute derivatives of terms involving 3D rotations. This yields the following set of derivatives for commonly encountered terms (refer to [2] for a derivation):

$$\displaystyle \begin{aligned} \partial/\partial t \left(\boldsymbol{q}_{\mathcal{B}\mathcal{I}}(t)\right) &= -{}_{\mathcal{B}}\boldsymbol{\omega}_{\mathcal{I}\mathcal{B}}(t), \end{aligned} $$

(77)

$$\displaystyle \begin{aligned} \partial/\partial \boldsymbol{q} \left(\boldsymbol{C}(\boldsymbol{q})\boldsymbol{r}\right) &= -\left(\boldsymbol{C}(\boldsymbol{q})\boldsymbol{r}\right)^\times, \end{aligned} $$

(78)

$$\displaystyle \begin{aligned} \partial/\partial \boldsymbol{q} \left(\boldsymbol{q}^{-1}\right) &= - \boldsymbol{C}(\boldsymbol{q})^{T}, {} \end{aligned} $$

(79)

$$\displaystyle \begin{aligned} \partial/\partial \boldsymbol{q} \left(\boldsymbol{q} \otimes \boldsymbol{p}\right) &= \boldsymbol{I}, \end{aligned} $$

(80)

$$\displaystyle \begin{aligned} \partial/\partial \boldsymbol{p} \left(\boldsymbol{q} \otimes \boldsymbol{p}\right) &= \boldsymbol{C}(\boldsymbol{q}), \end{aligned} $$

(81)

$$\displaystyle \begin{aligned} \partial/\partial \boldsymbol{\varphi} \left(\exp(\boldsymbol{\varphi})\right) &=: \boldsymbol{\varGamma}(\boldsymbol{\varphi}), \end{aligned} $$

(82)

$$\displaystyle \begin{aligned} \partial/\partial \boldsymbol{q} \left(\log(\boldsymbol{q})\right) &= \boldsymbol{\varGamma}^{-1}(\log(\boldsymbol{q})). \end{aligned} $$

(83)

Please note that these terms can vary based on the selected convention. The analytical forms of the rotation matrix C(φ) and of the exponential differential matrix Γ(φ) are given by:

$$\displaystyle \begin{aligned} \boldsymbol{C}(\boldsymbol{\varphi}) &= \boldsymbol{I} + \frac{\sin{}(\|\boldsymbol{\varphi}\|) \boldsymbol{\varphi}^\times}{\|\boldsymbol{\varphi}\|} + \frac{(1 - \cos{}(\|\boldsymbol{\varphi}\|)) \boldsymbol{\varphi}^{\times^2}}{\|\boldsymbol{\varphi}\|{}^2}, \end{aligned} $$

(84)

$$\displaystyle \begin{aligned} \boldsymbol{\varGamma}(\boldsymbol{\varphi}) &= \boldsymbol{I} + \frac{(1 - \cos{}(\|\boldsymbol{\varphi}\|)) \boldsymbol{\varphi}^\times}{\|\boldsymbol{\varphi}\|{}^2} + \frac{(\|\boldsymbol{\varphi}\| - \sin{}(\|\boldsymbol{\varphi}\|)) \boldsymbol{\varphi}^{\times^2}}{\|\boldsymbol{\varphi}\|{}^3}. \end{aligned} $$

(85)

Appendix B: Solving the Least Squares Problem for Multiple Point Feet

The goal is to solve the nonlinear least squares problem of Eq. (43) (Lagrangian form):

$$\displaystyle \begin{aligned} \min_{\boldsymbol{t}, \boldsymbol{q}, \lambda} \sum_k \bar{\boldsymbol{e}}_k^T(\boldsymbol{t},\boldsymbol{q}) \bar{\boldsymbol{e}}_k(\boldsymbol{t},\boldsymbol{q}) + \lambda (\boldsymbol{q}^T \boldsymbol{q} - 1), \end{aligned} $$

(86)

with the error term from Eq. (41)

$$\displaystyle \begin{aligned} \bar{\boldsymbol{e}}_k (\boldsymbol{t},\boldsymbol{q}) &= \ \bar{\boldsymbol{t}} \otimes \boldsymbol{q} - \bar{\boldsymbol{a}}_k \otimes \boldsymbol{q} + \boldsymbol{q} \otimes \bar{\boldsymbol{b}}_k, \end{aligned} $$

(87)

and where t is a 3D vector and q is a unit quaternion representing the incremental translation and rotation, respectively. Setting the derivatives with respect to t, q, and λ to zero results in the following set of equations:

$$\displaystyle \begin{aligned} \sum_k \bar{\boldsymbol{e}}_k^T(\boldsymbol{t},\boldsymbol{q}) \boldsymbol{R}(\boldsymbol{q}) \boldsymbol{S}^T &= \boldsymbol{0}, \end{aligned} $$

(88)

$$\displaystyle \begin{aligned} \sum_k \bar{\boldsymbol{e}}_k^T(\boldsymbol{t},\boldsymbol{q}) \left(\boldsymbol{L}(\bar{\boldsymbol{t}}) - \boldsymbol{L}(\bar{\boldsymbol{a}}_k) + \boldsymbol{R}(\bar{\boldsymbol{b}}_k)\right) + \lambda \boldsymbol{q}^T &= \boldsymbol{0}, {} \end{aligned} $$

(89)

$$\displaystyle \begin{aligned} \boldsymbol{q}^T \boldsymbol{q} - 1 &= 0. \end{aligned} $$

(90)

Expanding and transposing the first equations gives

$$\displaystyle \begin{aligned} \sum_k \boldsymbol{S} \boldsymbol{R}(\boldsymbol{q}^{-1}) \left(\bar{\boldsymbol{t}} \otimes \boldsymbol{q} - \bar{\boldsymbol{a}}_k \otimes \boldsymbol{q} + \boldsymbol{q} \otimes \bar{\boldsymbol{b}}_k\right) = \boldsymbol{0}, \end{aligned} $$

(91)

which can be transformed and simplified to

$$\displaystyle \begin{aligned} N \boldsymbol{t} - \sum_k \boldsymbol{a}_k + \sum_k \boldsymbol{C}(\boldsymbol{q}) \boldsymbol{b}_k = \boldsymbol{0}, \end{aligned} $$

(92)

where N is the number of point feet in contact with the ground. This can finally be rearranged to

$$\displaystyle \begin{aligned} \boldsymbol{t} &= \boldsymbol{a} - \boldsymbol{C}(\boldsymbol{q}) \boldsymbol{b}, \ \ \boldsymbol{a} = \frac{\sum_k \boldsymbol{a}_k}{N}, \ \ \boldsymbol{b} = \frac{\sum_k \boldsymbol{b}_k}{N}. {} \end{aligned} $$

(93)

This means that the translation is obtained from the vector that maps the mean of the rotated b_k to the mean of the a_k.

The quaternion form of t is

$$\displaystyle \begin{aligned} \bar{\boldsymbol{t}} &= \bar{\boldsymbol{a}} - \boldsymbol{q} \otimes \bar{\boldsymbol{b}} \otimes \boldsymbol{q}^{-1}, \end{aligned} $$

(94)

which can be inserted into the transposed form of Eq. (89):

$$\displaystyle \begin{aligned}\sum_k \left(\boldsymbol{L}(\bar{\boldsymbol{a}} - \boldsymbol{q} \otimes \bar{\boldsymbol{b}} \otimes \boldsymbol{q}^{-1}) - \boldsymbol{L}(\bar{\boldsymbol{a}}_k) + \boldsymbol{R}(\bar{\boldsymbol{b}}_k)\right)^T \\ \left(\boldsymbol{L}(\bar{\boldsymbol{a}} - \boldsymbol{q} \otimes \bar{\boldsymbol{b}} \otimes \boldsymbol{q}^{-1}) - \boldsymbol{L}(\bar{\boldsymbol{a}}_k) + \boldsymbol{R}(\bar{\boldsymbol{b}}_k)\right) \boldsymbol{q} + \lambda \boldsymbol{q} = \boldsymbol{0}. {}\end{aligned} $$

(95)

Furthermore, since

$$\displaystyle \begin{aligned} \boldsymbol{L}\left(\boldsymbol{q} \otimes \bar{\boldsymbol{b}} \otimes \boldsymbol{q}^{-1}\right) \boldsymbol{q} = \boldsymbol{R}\left(\bar{\boldsymbol{b}}\right) \boldsymbol{q}, \end{aligned} $$

(96)

and the summation of the second factor in Eq. (95) equals 0:

$$\displaystyle \begin{aligned} \sum_k \left(\boldsymbol{L}(\bar{\boldsymbol{a}}) - \boldsymbol{R}(\bar{\boldsymbol{b}}) - \boldsymbol{L}(\bar{\boldsymbol{a}}_k) + \boldsymbol{R}(\bar{\boldsymbol{b}}_k)\right) &= \boldsymbol{0}, \end{aligned} $$

(97)

Equation (95) can be transformed to:

$$\displaystyle \begin{aligned}- \sum_k \left(\boldsymbol{L}(\bar{\boldsymbol{a}}) - \boldsymbol{R}(\bar{\boldsymbol{b}}) - \boldsymbol{L}(\bar{\boldsymbol{a}}_k) + \boldsymbol{R}(\bar{\boldsymbol{b}}_k)\right) \\ \left(\boldsymbol{L}(\bar{\boldsymbol{a}}) - \boldsymbol{R}(\bar{\boldsymbol{b}}) - \boldsymbol{L}(\bar{\boldsymbol{a}}_k) + \boldsymbol{R}(\bar{\boldsymbol{b}}_k)\right) \boldsymbol{q} + \lambda \boldsymbol{q} = \ \boldsymbol{0}.\end{aligned} $$

(98)

This has the form of an Eigenvector problem for q:

$$\displaystyle \begin{aligned} \boldsymbol{B} \boldsymbol{q} - \lambda \boldsymbol{q} &= \boldsymbol{0}, \end{aligned} $$

(99)

with

$$\displaystyle \begin{aligned} \boldsymbol{B} = \sum_k (\boldsymbol{A}-\boldsymbol{A}_k)&(\boldsymbol{A}-\boldsymbol{A}_k) = \sum_k \boldsymbol{A}_k \boldsymbol{A}_k - \boldsymbol{A} \boldsymbol{A}, \end{aligned} $$

(100)

$$\displaystyle \begin{aligned} \boldsymbol{A} = \sum_k \boldsymbol{A}_k, &\ \ \ \ \boldsymbol{A}_k = \boldsymbol{L}(\bar{\boldsymbol{a}}_k) - \boldsymbol{R}(\bar{\boldsymbol{b}}_k). \end{aligned} $$

(101)