來源：arVix

作者：Joan Sola, Jeremie Deray, Dinesh Atchuthan

編譯 : 李璟 湖南大學(xué)

審核：wyc

github: https://github.com/artivis/manif

這是泡泡圖靈智庫(kù)推送的第618篇文章,，歡迎個(gè)人轉(zhuǎn)發(fā)朋友圈；其他機(jī)構(gòu)或自媒體如需轉(zhuǎn)載,，后臺(tái)留言申請(qǐng)授權(quán)

摘要

李群是一個(gè)古老抽象的數(shù)學(xué)對(duì)象,，可以追溯到十九世紀(jì)，多年后,，在現(xiàn)在的機(jī)器人領(lǐng)域中,，至少是在狀態(tài)估計(jì)領(lǐng)域尤其是針對(duì)導(dǎo)航的運(yùn)動(dòng)估計(jì)，其扮演了越來越重要的角色,。然而,，對(duì)于絕大多數(shù)機(jī)器人專家來說，李群是高度抽象的結(jié)構(gòu),，因此很難理解和使用,。在狀態(tài)估計(jì)中，我們不需要應(yīng)用其全部的理論,，因此需要從中挑選出我們需要的部分,。在這篇文章中，我們將展示最基本的李群相關(guān)理論,，旨在傳達(dá)一些清晰有用的想法，為機(jī)器人狀態(tài)估計(jì)領(lǐng)域留下一份重要的關(guān)于李群的資料,。盡管存在缺陷,，但這里包含的內(nèi)容在機(jī)器人估計(jì)算法中非常有用，特別是在SLAM,、視覺里程計(jì)等領(lǐng)域,。除此之外，我們還提供了幾個(gè)應(yīng)用示例以及常用的公式,，還展示了一個(gè)C++模板庫(kù),，它實(shí)現(xiàn)了本文描述的所有功能。

一,、背景與貢獻(xiàn)

Fig1. 該圖展示了李群與李代數(shù)之間的關(guān)系,。李代數(shù) (紅色平面)是李群的流形 （藍(lán)色表示）在幺元  處的切空間。通過指數(shù)映射,，通過李代數(shù)上原點(diǎn)的每條直線路徑 會(huì)產(chǎn)生圍繞流形的路徑 ,，該路徑沿著各自的測(cè)地線延伸。相反,，群中的每個(gè)元素在李代數(shù)中都有一個(gè)等價(jià)元素,。這種關(guān)系的意義是如此深刻,，以至于（幾乎）彎曲和非線性的群中的所有運(yùn)算在李代數(shù)中都是完全等價(jià)的，而李代數(shù)是一個(gè)線性向量空間,。這里需要說明的是 中的球體不是李群（我們只是用它來表示以使得其可以在紙上畫出來）,，但是 中的球體是，它代表單位四元數(shù),。

借助李群的理論,，我們可以建立一個(gè)嚴(yán)格的微積分語料庫(kù)來處理狀態(tài)估計(jì)問題中的不確定性、導(dǎo)數(shù)和積分,。典型地,，這些工作集中在旋轉(zhuǎn)和剛性運(yùn)動(dòng)流形上。第一次接觸李群時(shí),，嘗試從不同的角度去看待它是十分重要的,。從拓?fù)涞慕嵌葋碚f，如Fig1,，它描述了流形的形狀,，并直觀地展示了李群與其切空間的關(guān)系以及什么是指數(shù)映射。從代數(shù)的角度來說,，它涉及到群運(yùn)算以及其具體實(shí)現(xiàn),，通過代數(shù)性質(zhì)來推出封閉形式的公式或化簡(jiǎn)它們。從幾何的角度來說（在機(jī)器人學(xué)中非常有用）,，它將群元素與本體或參考系的位置,、速度、旋轉(zhuǎn)相聯(lián)系,。初始系可能被定義為李群中的幺元,，流形中的其它點(diǎn)代表著某一確定的局部系。通過這些類比,，李群中許多抽象的數(shù)學(xué)概念可以更加接近向量空間,、運(yùn)動(dòng)學(xué)和其他更經(jīng)典的領(lǐng)域中的直觀概念。李群絕不簡(jiǎn)單,，為了掌握李群的基本概念,，可以參考以下三份材料：1.Abbaspour的“Basic Lie theory”（400多頁(yè)）；2.Howe的“Very basic Lie theory”（24頁(yè)）,；3.Stillwell的“Naive Lie theory”（200多頁(yè)）,。本篇論文只有17頁(yè)，目的是進(jìn)一步簡(jiǎn)化李群理論,。

我們通過兩種方式來實(shí)現(xiàn)這一點(diǎn)：1.我們只選取了李群中很小的一部分,，但是這些內(nèi)容對(duì)于機(jī)器人中的估計(jì)問題（如慣性預(yù)積分、里程計(jì),、SLAM,、視覺伺服等）十分有用,，它使得優(yōu)化器嚴(yán)格和優(yōu)雅的設(shè)計(jì)成為可能；2.我們使用一種類似教學(xué)的方式,，用大量的冗余介紹來降低學(xué)習(xí)李群的門檻,。正文是通用的，我們通過插入一些例子來介紹一些基礎(chǔ)的概念,，同時(shí)用大量的圖表來再次解釋相同的概念,。我們特別關(guān)注雅可比的計(jì)算，這對(duì)于大多數(shù)優(yōu)化器來說是必要的,，也是設(shè)計(jì)新算法時(shí)許多麻煩的來源,。我們?cè)谝徽轮刑峁┝硕ㄎ慌c建圖的示例，基于李群完成了EKF和非線性優(yōu)化算法的設(shè)計(jì),。最后,，幾個(gè)附錄中包含機(jī)器人中常用的一些群的相關(guān)內(nèi)容：?jiǎn)挝粡?fù)數(shù)、四元數(shù),、2D\3D旋轉(zhuǎn)矩陣,、2D\3D剛性運(yùn)動(dòng)矩陣與平移群。

在這項(xiàng)工作中,，我們將位于 向量空間的李代數(shù)降為到二維平面進(jìn)行表示,，而且不會(huì)引入李括號(hào)的概念。之所以這樣做是因?yàn)樵谖覀冾A(yù)見的目標(biāo)應(yīng)用領(lǐng)域,，這些內(nèi)容不是必要的,。如果將其包括在內(nèi)，我們很難進(jìn)行清晰有用的描述,，讀者將不得不進(jìn)入數(shù)學(xué)概念,，這些概念由于其抽象性或微妙性而使得問題變的復(fù)雜。

二,、簡(jiǎn)明李群理論

A.李群

李群包含了群和光滑流形的概念：李群是一個(gè)光滑流行，它的元素滿足群的公理,。在將兩個(gè)概念結(jié)合在一起之前,，我們先簡(jiǎn)單介紹一下這兩個(gè)概念。

Fig2. 流形 與其向量空間 在處相切,。速度項(xiàng) 不屬于流形 但屬于其切向空間 ,。

一方面，可微或光滑流形是一個(gè)局部類似線性空間的拓?fù)淇臻g,。讀者應(yīng)當(dāng)能夠想象流形的形狀（Fig2）,它就像一個(gè)彎曲的,、光滑的（超）曲面，沒有邊緣或尖刺,，嵌在更高緯度的空間里,。在機(jī)器人學(xué)中,，我們認(rèn)為狀態(tài)向量在這個(gè)曲面上變化，這就是說,，流行由強(qiáng)加于狀態(tài)的約束來定義或描述,。例如，具有單位模長(zhǎng)約束的向量定義了半徑為1的球面流形,。流形的光滑性意味著在每個(gè)點(diǎn)上存在唯一的切空間,，這個(gè)空間是允許我們做微積分的線性或向量空間。

另一方面,，群由集合和運(yùn)算組成,，對(duì)于，滿足以下公理：

在李群中,，流形上的每個(gè)點(diǎn)看起來都是一樣的,，因此任意點(diǎn)的切空間都是相似的。群的結(jié)構(gòu)規(guī)定了群元素的運(yùn)算仍保持在流形上（封閉性,，等式1）,，存在一個(gè)特殊元素幺元（等式2）而且每個(gè)元素在流形上存在一個(gè)逆（等式3），因此存在一個(gè)特殊的切空間,，它是群在幺元處的切空間,，我們將之稱之為李群的李代數(shù)。在本文中,，為了簡(jiǎn)單起見,，我們將李群稱之為“流形”。

Fig3. 流形  是一個(gè)位于平面 上的單位圓（藍(lán)色),，其中包含所有的單位復(fù)數(shù)  ,。李代數(shù) 是由虛數(shù)構(gòu)成的直線 （紅色），與 同構(gòu),。切向向量（紅色線段）通過指數(shù)映射包裹在流形上產(chǎn)生圓上的?。ㄋ{(lán)色弧）,。 （箭頭）將虛數(shù)  中的元素映射到  中的元素,， 將  中的元素映射到虛數(shù)  中的元素。單位虛數(shù)在切空間中的增量通過復(fù)合和指數(shù)運(yùn)算表示（類似Fig4, 我們將其定義為 和 ）,。

Fig4. 流形  是四元數(shù)空間  中的一個(gè)三維單位球,，其中包含了所有的單位四元數(shù)  。李代數(shù)位于純虛四元數(shù)空間 ,，與超平面  （紅色網(wǎng)格）同構(gòu),。切向向量（紅色線段）通過指數(shù)映射包裹在流形上的大弧線或測(cè)地線（虛線上）。中間與右邊的圖顯示了這個(gè)測(cè)地線的側(cè)面切割（由圖可見與 非常相似）。 （箭頭）將  中的元素映射到  中的元素,， 將  中的元素映射到  中的元素,。四元數(shù)在切空間中的增量通過符號(hào) 和 表示。

李群的重要性之一在于其具有變換其他集合元素的能力,，例如旋轉(zhuǎn),、平移、縮放以及他們的組合,。這些被廣泛應(yīng)用關(guān)于機(jī)器人技術(shù),，包括2D和3D。

給定一個(gè)李群和一個(gè)集合,，我們將作用于記作,，其中：

將  定義為一種群作用，它必須滿足以下公理：

常見的例子包括旋轉(zhuǎn)矩陣構(gòu)成的群,，單位四元數(shù)構(gòu)成的群,，剛性運(yùn)動(dòng)構(gòu)成的群。它們對(duì)向量的作用滿足(詳細(xì)內(nèi)容見table1與附錄)：

群的合成可以視作群對(duì)自身的作用,，還有一些有趣的作用如伴隨作用,，將在后文中看到。

C.切向空間與李代數(shù)

Fig5. 如圖,，點(diǎn)   以一恒定角速度  運(yùn)動(dòng),，有 。在通過  和  的時(shí)候,，它們的速度分別位于不同的切向空間中： 和 ,。以  為例，在全局系下其速度為 ,，在局部系下有 ,。二者有如下關(guān)系： 。在  處,，二者相等：,。顯然，所有的切空間都是 ,，也就是李代數(shù),。這也是  在幺元處的結(jié)構(gòu)，這就是為什么李代數(shù)被定義為在幺元處的切空間,。

Fig6.流行與其幺元處切空間的映射關(guān)系。小寫 代表李代數(shù)與流行間的變換,，大寫 代表向量空間與流行之間的映射,。

 在流形 上移動(dòng)，其速度屬于在處的切向空間記為（Fig2）。流形的光滑性,，即沒有邊或尖峰,，意味著在每個(gè)點(diǎn)都存在唯一的切空間。這樣的切空間的結(jié)構(gòu)在任何地方都是一樣的,。

1）李代數(shù) :位于李群幺元處的切空間,，稱為李代數(shù)，記做：

每一個(gè)李群都有李代數(shù)與之相關(guān)聯(lián),，我們將通過以下事實(shí)將李群與李代數(shù)聯(lián)系起來（Fig1,，F(xiàn)ig6）：

• 李代數(shù)是一個(gè)向量空間。因此,，它的元素可以用中的向量來定義,，其中代表的自由度。
• 指數(shù)映射,，： ,，精確地將李代數(shù)中的元素      轉(zhuǎn)換為群中的元素，對(duì)數(shù)映射是逆運(yùn)算,。
• 通過線性變換,，可以將處切空間中的向量轉(zhuǎn)換到幺元處的切空間，這稱作伴隨變換,。

李代數(shù)可以局部定義為以為切點(diǎn)建立的坐標(biāo)系（Fig5）,。我們用來修飾李代數(shù)中的元素，用左上標(biāo)來指定其所屬切向空間,，如,，。李代數(shù)的結(jié)構(gòu)可以通過對(duì)群約束（逆）的時(shí)間微分來找到（例3,5）,，對(duì)于乘法群產(chǎn)生約束：,，則李代數(shù)具有如下形式：

2）笛卡爾向量空間 ：李代數(shù)中的元素具有不一般的結(jié)構(gòu)（斜對(duì)稱矩陣，虛數(shù),，純四元數(shù),，見table1）,但是對(duì)我們來說最關(guān)鍵的是它們可以表示為一些基元的線性組合，其中記做的生成元（它們是圍繞原點(diǎn)在第個(gè)方向上的導(dǎo)數(shù)）,。因此,，將李代數(shù)寫作中的向量是十分方便的，我們記做,。我們可以通過兩個(gè)互逆的映射,，通常稱作和，將轉(zhuǎn)換到,，反之亦然（fig6）,。

其中為的基向量（有）,。這意味著與向量空間同構(gòu)，記做或者,。對(duì)于我們來說,，比更加方便，這是因?yàn)樗鼈兡軌蚯度朐诟蟮臓顟B(tài)向量中,，更重要的是,，能夠利用矩陣算子進(jìn)行線性運(yùn)算。在本文中,，我們更傾向于使用,，因此我們定義的大多數(shù)算子和對(duì)象（特別是：伴隨矩陣、雅可比矩陣,、擾動(dòng)矩陣和協(xié)方差矩陣）都在上,。

D.指數(shù)映射

指數(shù)映射能夠讓我們精確地將李代數(shù)中的元素轉(zhuǎn)換到李群中（Fig1），這種操作通常稱作,。直觀來說,，指數(shù)映射將切平面中的元素沿著大弧線或測(cè)地線包裹在流行上（就想用繩子繞球，F(xiàn)ig1,，3,，4）。逆映射為,，即展開操作,。通過考慮關(guān)于時(shí)間的導(dǎo)數(shù)，可以很自然地推出指數(shù)映射,，由公式（9）：

對(duì)于常數(shù),，這是一個(gè)常微分方程，它的解是：

由于都是群中的元素,，那么一定也是群中的元素,，因此將李代數(shù)中的元素映射到群中，這就是指數(shù)映射,。為了提供更一般的指數(shù)映射的定義,，我們定義，那么即為李代數(shù)中的一點(diǎn),。通過指數(shù)映射和對(duì)數(shù)映射有：

通過泰勒級(jí)數(shù)展開,，得到乘法群中指數(shù)的封閉形式：

其關(guān)鍵性質(zhì)包括：

其中公式（20）由泰勒展開可以很容易得到。

1）大寫的指數(shù)映射 :

與小寫的指數(shù)映射的關(guān)系（Fig6）：

E.加減運(yùn)算

加減運(yùn)算在（平坦的）切向量空間中表示（彎曲的）流行上元素之間的增量,。分別用 和 表示,，在每一次合成都包含了操作。由于合成的非交換性,，根據(jù)操作數(shù)的順序,，它們被定義為右版本和左版本,。右操作如下：

由于（25）式中的出現(xiàn)在合成式的右邊，位于處的切空間：我們按照慣例稱在的局部系中（注意我們?cè)谧笊辖菢?biāo)注的參考系）,。左操作：

在式（27）中，位于左邊,，有：我們認(rèn)為為全局系下的表示,。

注意到左右可以通過操作數(shù)順序區(qū)分，卻無法區(qū)分,。因此,，在本文中，我們默認(rèn)為局部擾動(dòng),，即默認(rèn)使用右操作形式,。

F.伴隨和伴隨矩陣

Fig7.圖中有兩條路徑， 和 ,，連接了原點(diǎn)  和點(diǎn)  ,。他們分別是局部系和全局系下的增量與  的組合。由于非交換性,，  和  并不相等,。他們對(duì)應(yīng)的切空間中的向量  和  也不相等。它們通過線性變換相關(guān)聯(lián)：,，其中  是  在  處的伴隨矩陣,。

如果令（25）（27）中的為幺元，那么有,。這代表局部和全局切空間中元素的轉(zhuǎn)換關(guān)系（Fig7）,。由（20）（25）（27）有：

1) 伴隨： 我們定義流形在處的伴隨作用，記為：

因此,。這定義了群在其李代數(shù)上的伴隨作用,。伴隨有兩個(gè)有趣的性質(zhì)：

*2) 伴隨矩陣：*由于是線性的作用，因此我們能夠找到等價(jià)的矩陣算子  來映射位于笛卡爾切空間中的向量 和：

我們稱其為伴隨矩陣,?？梢酝ㄟ^對(duì)式（29）執(zhí)行來計(jì)算：

將右邊展開可以得到伴隨矩陣（見附錄和例6）。伴隨矩陣的其他性質(zhì)包括：

注意（33）（34）中左邊的計(jì)算通常比右邊更簡(jiǎn)單,。我們通常將伴隨矩陣用作將切空間處向量轉(zhuǎn)換到原點(diǎn)處切空間向量的方法,，即。

G.李群求導(dǎo)

在定義李群導(dǎo)數(shù)的眾多方式中,，我們關(guān)注于以雅克比矩陣映射切向空間向量的形式,。在本文中這就已經(jīng)足夠了，因?yàn)樵谶@些空間中,，不確定性和增量可以正確和輕松地定義,。使用這些雅克比矩陣,，李群中關(guān)于不確定性管理的公式將在很大程度上類似于向量空間中的公式。

下文描述的雅克比矩陣滿足鏈?zhǔn)椒▌t,，因此我們可以很容易地從關(guān)于逆,、合成、指數(shù),、作用 的部分雅克比推導(dǎo)出任意雅克比,。

1) 關(guān)于向量空間的雅克比： 對(duì)于多元函數(shù)，雅克比定義為的矩陣：

用下面這個(gè)形式定義雅克比矩陣很方便,，令,，其中是第列向量。列向量對(duì)應(yīng)：

其中是的第個(gè)自然基向量,。關(guān)于分子,，注意到（37）是當(dāng)在方向進(jìn)行擾動(dòng)時(shí)的變化，并且相應(yīng)的雅克比正好是,。

在本項(xiàng)工作中,，為了方便起見，我們引入了緊湊形式：

其中,。式（38）只是一個(gè)方便的符號(hào)（就像（35）一樣）,，因?yàn)?span style="">的除法是未定義的，正確的計(jì)算需要式（36）,。然而,，通過將分子展開乘關(guān)于的線性形式，并且將左邊認(rèn)為是雅克比矩陣,，則有：

其中關(guān)于的小量,，具有線性近似：

Fig8.函數(shù)  的右雅克比：基向量方向上的擾動(dòng)  通過 得到擾動(dòng)向量 ，,。通過變化  ,在  中的擾動(dòng)  （粗紅色線）在  上沿著測(cè)地線產(chǎn)生了一些路徑（藍(lán)色）,。在  中，由于的非線性,，從  中映射過來的路徑（藍(lán)色實(shí)線）通常不位于測(cè)地線（藍(lán)色虛線）上,。這些映射路徑投射至切向空間  ，產(chǎn)生平滑的曲線路徑（細(xì)紅色實(shí)線）,得到擾動(dòng)向量  ,。雅克比的列向量  ,。每一個(gè)  都會(huì)對(duì)應(yīng)得到  。因此,，雅克比矩陣將  線性映射到  ,。

*2) 關(guān)于李群的右雅克比：*受到式（38）的啟發(fā)，我們可以使用和定義流行上函數(shù)的雅克比（Fig8）,。用右版本代替,，我們可以得到一個(gè)類似于標(biāo)準(zhǔn)導(dǎo)數(shù)的形式：

進(jìn)一步得到：

我們稱之為的右雅克比,。注意（41c）是復(fù)雜函數(shù)的標(biāo)準(zhǔn)導(dǎo)數(shù)。把它寫成（41a）更加直觀：它是關(guān)于的導(dǎo)數(shù),，只不過我們使用正切空間中的無窮小變化來進(jìn)行表示,。事實(shí)上，多虧了右版本的運(yùn)算符,，和的變化能夠表示為局部切空間中的向量,，其中切空間分別處于和。因此,，這個(gè)導(dǎo)數(shù)是一個(gè)的雅克比矩陣，它代表局部切向空間之間的映射：,。類似于向量空間,，這個(gè)矩陣的列對(duì)應(yīng)于方向?qū)?shù)。

（42）表示在方向上變化時(shí)的變化,。它對(duì)應(yīng)的雅克比的列是,。

我們使用（41a）來計(jì)算雅克比。例如,，對(duì)于3D旋轉(zhuǎn),，有,，因此：

注意，當(dāng)函數(shù)從一個(gè)流形傳遞到另一個(gè)流形上時(shí)，（41a）中的也需要進(jìn)行改變：使用定義域的,，使用值域的。對(duì)于微小量,，如下近似成立：

Fig9.該圖展示了  中涉及的所有線性映射,。線性映射 ，構(gòu)成了循環(huán)映射（實(shí)線）,，即公式（46）,。交叉雅克比（虛線）構(gòu)成更多的循環(huán)映射，即式（47）（48）,。

3) 關(guān)于李群的左雅克比： 使用左版本的,，可以計(jì)算左雅克比：

注意此時(shí)，分子屬于,，因此左雅克比是映射全局切空間的矩陣：,。對(duì)于小量，下式成立：

由（32）（43）（40）可知左右雅克比之間通過伴隨矩陣相關(guān)聯(lián)（Fig9）：

4) 交叉左右雅克比： 同樣可以使用右加左減來定義雅克比,，反之亦然,。這在進(jìn)行局部切空間與全局切空間之間的映射時(shí)是很有用的。我們可以通過伴隨將其與其他雅克比聯(lián)系起來：

這里,，上下的注腳表示在什么參考系下進(jìn)行求導(dǎo),。相應(yīng)對(duì)于小量：

5) 流形中的不確定性,，協(xié)方差矩陣的傳播

Fig10.點(diǎn)  周圍的不確定性被表示為該點(diǎn)切向空間上的協(xié)方差（紅色）。使用  (式51),，切向空間中的概率橢圓包裹在流形上（藍(lán)色）,，從而說明了在群上的概率集中區(qū)域。

定義在點(diǎn)處切空間中的局部擾動(dòng),，使用右:

協(xié)方差矩陣在這個(gè)切空間中可以通過標(biāo)準(zhǔn)的期望算子進(jìn)行計(jì)算：

由此定義流形上的高斯變量,，如Fig10。注意,，盡管我們寫作,，但是協(xié)方差是指切空間中擾動(dòng)的協(xié)方差。由于切空間的維度與流行的自由度相匹配,，所以這些協(xié)方差的定義是明確的,。

擾動(dòng)也可以在全局參考系中表示，即在幺元處的切空間中,，使用左:

那么（52）中的協(xié)方差定義將使用左,，這提供了全局的協(xié)方差矩陣定義。例如,，對(duì)于已知水平面內(nèi)的3D旋轉(zhuǎn)可以關(guān)聯(lián)至協(xié)方差矩陣,。這是因?yàn)椤八健笔且粋€(gè)全局的定義，因此必須定義在全局系下,。但事實(shí)上,，全局和局部擾動(dòng)的協(xié)方差可以通過伴隨矩陣進(jìn)行關(guān)聯(lián)：

協(xié)方差通過函數(shù)的線性化進(jìn)行傳播：6) 流形上的離散積分Fig11.流形上的運(yùn)動(dòng)積分。每一次運(yùn)動(dòng)數(shù)據(jù)都產(chǎn)生 ,，包裹在流形上成為局部增量,，然后與 組合得到 。

指數(shù)映射以恒定速度在流形上進(jìn)行連續(xù)時(shí)間的積分,。對(duì)于非恒定速度通常通過在短時(shí)間內(nèi)認(rèn)為速度恒定對(duì)其進(jìn)行分段得到,，得到離散積分：同樣（Fig11），我們可以定義,，將積分構(gòu)造為離散的一系列切空間小量的“和”：,。遞歸的形式寫作：常見的例子是將三維角速度積分到旋轉(zhuǎn)矩陣或者積分到四元數(shù)。