A note on Lie Theory in Robotics

Why do I care?

Rotational objects are everywhere in robotics. In perception tasks, one has to reason about the pose of an object in space. In control, controlling an end-effector to move and rotate also involve rotation. And finally in planning, dynamics and kinematics of free-bodies (e.g. quadrotor) often involve the rotational momentum and velocity. No serious applications of robotics can ignore rotations.

Comparing to linear vector spaces, handling rotations is much more difficult because it lacks many useful properties that one might be used to. For instance, vector spaces are linear,

\begin{equation} \mathbf{x} \in \mathbb{V} \implies \alpha \mathbf{x} \in \mathbb{V}, \end{equation}

where \(\alpha\) is a scalar. And yet this is wrong for the space of rotational matrices \(SO(3)\) or quaternion \(\mathbb{S}\) due to the internal constraint that elements of these spaces have to satisfy.

Other properties that break down include commutativity, additivity, both of which are critical to how we think about abstract mathematical elements. If I can’t add them, what can I do with them?

Thus, one often has to derive, through much hardwork, special mathematical machinery to deal with a quaternion or rotational matrices. These are difficult and tedious, especially if one is not accustomed to it.

This is where Lie Group enters the picture:

A mathematical treament would require some understanding of topology and group theory 1983VeryBasicLieTheory.

Micro Lie theory

Lie group

A Lie group is both a group and a smooth manifold. Groups are mathematical structures that satisfy four axioms: closure under group operation, identiy, inverse and associativity.

A smooth manifold is a topological space that locally resembles a linear space. Now this can be somewhat confusing. I find the best way to see a manifold is to think of it as a sheet of paper in our 3 dimensional world. Let call this manifold \(\mathcal{M}\). We can see that:

Figure 1: A sphere in \(\mathbb{R}^3\) is a smooth manifold.

Figure 1: A sphere in \(\mathbb{R}^3\) is a smooth manifold.

Clearly there are a few difficulty with the physical model of the paper-manifold \(\mathcal{M}\). We can clearly talk about the points belonging to the manifold. And yet, how to combine, move and perform arimethics on theses points? How can we optimize for an optimal point \(m^* \in \mathcal{M}\) that optimizes some scalar objective?

A Lie group is a smooth manifold on which these questions have very concrete answers. There are a few Lie groups that we are interested in:

Group actions

A group action is an operation on a member of the Lie group with another member of a set \(\mathcal{V}\) \[ \cdot: \mathcal{M} \times \mathcal{V} \rightarrow \mathcal {V} \]

Tangent spaces and Lie algebra

Consider a function \(X(t) \in \mathcal{M}\), the set of all first-derivatives \(\partial \mathcal{X} / \partial t\) is the tangent space \(\mathcal{T}_{\mathcal{X}} \mathcal M\). The tangent space at the identity element \(\mathcal{E}\) is called the Lie algebra \(\mathrm{m}\) of the group \(\mathcal{M}\). Elements of the Lie algebra are distinguished with a hat, e.g, \(\mathbf{\tau}^{\wedge} \in \mathrm{m}\).

It is easy to see that tangent spaces have a different structure from the original Lie group. For example, take \(SO(3)\) the group of rotation matrices, the derivatives are 3-by-3 matrices, but does not belong to the group of rotational matrix \(SO(3)\). Similarly, the derivative of a unit quarternion is also not a unit quarternion.

However, the group operations (i.e., matrix multiplication, quaternion multiplication, complex product) are still defined between an element of the Lie algebra and an element of the Lie group. This observation is a bit empirical in nature; perharps a deeper study of Lie theory would give us a more principled answer. In any case, we shall freely use the group operation such as

\begin{equation*} \dot{X} X; \dot{X^{-1}} X \end{equation*}

In fact, as it turns out, tangent spaces have much simpler structure: Tangent spaces are actually linear vector spaces. We can see this by analyzing the tangent spaces of \(\mathcal {S}\); writing the unit norm constraint and taking the derivative:

\begin{align} \mathbf{q} \mathbf{q}^* &= 1 \\\
\dot {\mathbf{q}} \mathbf{q}^* + \mathbf{q} (\dot{\mathbf{q}})^* &= 0 \end{align}

It’s not hard to see that the set of derivatives \(\dot{\mathbf{q}}\) is a linear vector space. Further more, we can see that the product \(\dot{\mathbf{q}} \mathbf{q}^*\) is a pure imaginary quarternion:

\begin{equation*} \dot{\mathbf{q}} \mathbf{q}^* = 0 + \alpha_1 i + \alpha_2 j + \alpha_3 k. \end{equation*}

If \(\mathbf{q}\) is the identity quaternion \(1\), we thus see that the set of pure imaginary unit quaternions is the Lie algebra of \(\mathcal{S}\).

As a consequence, we can maps an element of the Lie algebra to an element of a tangent space easily. Consider a non-identity unit quaternion \(\mathbf{q}\) with derivative \(\dot{\mathbf{q}}\), we have:

\begin{align} \tau^{\wedge} &= \dot{\mathbf{q}} \mathbf{q}^* \\\
\tau^{\wedge} \mathbf{q} &= \dot{\mathbf{q}} \mathbf{q}^* \mathbf {q} = \dot{\mathbf{q}} \end{align}

Similarly, we can show that \(\mathbf{q}\tau^\wedge\) also belongs to \(T_\mathbf{q}M\).

The same derivation can be done for other Lie groups to show that the Lie algebra has a linear, Eucledian-like structure. For example, consider the rotational group \(SO(3)\), we can show that \(R \tau^\wedge\) and \(\tau^\wedge R\) belongs to \(T_R M\), for \(\tau^\wedge \in \mathrm{m}\) and that \(\mathrm{m}\) is the set of skew-symmetric matrices which is identified with \(\mathbb{R}^3\).

Consider now the set of rigid-body transformations \(SE(3)\), its Lie algebra is the set of matrices with the following form:

\begin{equation} \begin{bmatrix} 0 & - w_3 & w_2 & v_1 \\\
w_3 & 0 & -w_1 & v_2 \\\
-w_2 & w_1 & 0 & v_3 \\\
0 & 0 & 0 & 0 \end{bmatrix} \end{equation}

That is the set of 4-by-4 matrices with the top left 3-by-3 block being skew-symmetric and the last row is the zero vector. This is easily realized by noting that the top left 3-by-3 block of a homogeneous matrix is a rotational matrix.

Similarly, we also see that \(\mathbf{T} \tau^\wedge\) also belongs to \(T_\mathbf{T}M\) that noting that

\begin{equation} \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix} \begin{bmatrix} [w]_\times & v \\ 0 & 0 \end{bmatrix} = \begin{bmatrix} R [w]_\times & Rv \\ 0 & 0 \end{bmatrix} \end{equation}

Physically, the composition of \(X\) with an element of the Lie algebra can be interpretted as realizing a vector in the local frame of \(X\) in the global frame.

The Lie algebra is interesting because it is homemorphic to a linear vector space of the same dimensions as the Lie algebra. This is true for all Lie groups such as \(SO(3), SE(3), S\) (quarternion) and composition of those spaces (I assume this is a core theoretical result in Lie theory, as it is completely unclear to me why it might be true for all Lie groups. For the groups that I am interested in though, this can be verified quite easily.). This is seen very easily for these rotational spaces. For example, the Lie algebra of \(SO(3)\) has the form:

\begin{equation} [\mathbf{w}]_\times = \begin{bmatrix} 0 & - w_3 & w_2 \\\
w_3 & 0 & -w_1 \\\
-w_2 & w_1 & 0 \end{bmatrix} \end{equation}

which identifies with the 3-vector \([w_1, w_2, w_3]^T\). Similarly, the Lie algebra of the group of unit quarternions has the form

\begin{equation} 0 + w_1 i + w_2 j + w_3 j \end{equation}

and identifies with the same 3-vector.

The exponential map

Each element in the Lie algebra \(\mathrm{m}\) can be mapped to a element in the Lie group \(\mathcal{M}\) using the exponential mapping. It has been establish

For different Lie groups – \(SO(3), SE(3), S\) – the exponential map is defined differently. For instance, for \(SO(3)\) and \(SE(3)\), we can use the usual matrix exponential. For quaternion, we need to use the quaternion exponential and for complex number, we use Euler’s formula.

In notation, we shall say \(\tau^\wedge\), which is an element of the Lie algebra \(\mathrm{m}\), is mapped to \(X\) that belongs to the Lie group \(\mathcal{M}\).

\begin{align} X &= \exp{(\tau^\wedge)}& \\
&= \mathrm{Exp}{(\tau)}& \end{align}

The operations: \(\log, \mathrm{Log}\) are defined as inverses of these exponential functions.

Additive operations

Additive operations are important idea in the theory of Lie Group. These operations bring the Lie Group abstraction on par with vector space, as these allow our intuition on making a deviation in vector spaces work again.

We shall define here two pairs of operations: the right plus and right minus; the left plus and left minus. It shall be most convenient to look at the two plus operations. Similar to the previous section, let us consider \(X, X’ \in \mathcal{X}\) and \(\tau^\wedge \in \mathrm{m}\). Define the right plus operation

\begin{equation} X + \tau_1 := X \cdot \mathrm{Exp}(\tau_1) \in \mathcal{M}. \end{equation}

The left plus operation:

\begin{equation} \tau_2 + X := \mathrm{Exp}(\tau_2) \cdot X \in \mathcal{M}. \end{equation}

Here \(\tau_1\) and \(\tau_2\) are vector of the Lie algebra \(\mathrm{m}\).

Consider the Euclidean space \(\mathrm{R}^2\), it can be seen that the two operations equal due to commutativity.

Consider the group of rotational matrices \(SO(3)\), this is clearly not the case. Physically, the two operations represent different things. The right plus operation represents \(\mathrm{Exp}(\tau_1)\) as a rotation of \(\tau_1\) in the frame shifted from the origin a rotation \(X\). For the left plus operation, \(\mathrm{Exp}(\tau_2)\) is a rotation of \(\tau_2\) in the original world frame. Thus, for the same \(\tau\), the two operations definitely differ.

Adjoint action

Given \(\tau_1\), which value of \(\tau_2\) that equates the two plus operations? First, let us define the adjoint maps that map \(\tau_1\) to \(\tau_2\), that is:

\begin{equation} \tau_2^\wedge = \mathrm{Ad}_X(\tau_1^\wedge). \end{equation}

The interesting thing is there is a linear relationship between between \(\tau_1\) and \(\tau_2\), indeed, there exists an adjoint matrix in all Lie Groups that realizes the adjoint action.

\begin{equation} \tau_2 = \mathbf{Ad_X} \tau_1 \end{equation}

This relationship can be useful when we want to carry computations in which there are multiple perturbation sources. For instance, consider the following expression in SE(3)

\begin{equation} \tau_1 + T_1 + \tau_2 + T_2 + \tau_3 \end{equation}

Using the adjoint mappings, we can shift all three perturbation sources toward the left side or right side before doing subsequent optimization/processing.