首選不直接對(duì)什么是李群什么是李代數(shù)進(jìn)行闡述，目前的資料太過同化了吧！在我們上初中的時(shí)候我們知道要想求取函數(shù)的最值需要求取導(dǎo)數(shù)，而在優(yōu)化問題中也需要求取導(dǎo)數(shù)（梯度下降、高斯牛頓、L-M法）?，F(xiàn)在已經(jīng)學(xué)到應(yīng)用數(shù)學(xué)的時(shí)候了，讓我們來看下slam是怎么的優(yōu)化問題？如何求導(dǎo)？我研究的是三維激光slam，就以三維激光slam為例子？二維激光slam，視覺slam都是如此。假如我們有兩個(gè)傳感器，一個(gè)是三維激光一個(gè)是IMU。使用IMU可以預(yù)測(cè)到機(jī)器人的運(yùn)動(dòng)，我們稱其為運(yùn)動(dòng)方程，而使用三維激光雷達(dá)通過一些算法（ICP或者NDT）也可以得到機(jī)器人的運(yùn)動(dòng)，稱為觀測(cè)方程。那到底聽這兩個(gè)傳感器誰的？那么可以構(gòu)建不同優(yōu)化問題，批量估計(jì)的最小二乘問題或者增量估計(jì)的濾波方法。不管哪種方法最終都逃不脫式1-1的形式，后續(xù)會(huì)對(duì)的形式進(jìn)行討論，首先我們來明確下待優(yōu)化的變量X是什么？我們都知道在slam問題中首先需要建立世界坐標(biāo)系，即你建立的地圖的坐標(biāo)原點(diǎn)需要指定，在你不指定的時(shí)候，目前是建圖和定位的起始幀的坐標(biāo)為世界坐標(biāo)系的原點(diǎn)。其次需要明白的是相機(jī)也有自己的坐標(biāo)系，但是他是運(yùn)動(dòng)的。當(dāng)相機(jī)下的一個(gè)坐標(biāo)點(diǎn)需要轉(zhuǎn)換到世界坐標(biāo)系下，你該怎么做，是不是需要乘以一個(gè)旋轉(zhuǎn)平移矩陣R。那么當(dāng)這個(gè)點(diǎn)就是相機(jī)坐標(biāo)系的原點(diǎn)時(shí)，那么旋轉(zhuǎn)平移矩陣是不是就代表著機(jī)器人的運(yùn)動(dòng)，因此待優(yōu)化變量是個(gè)矩陣，而且是正交單位陣。此時(shí)你已經(jīng)知道了slam問題即定位問題是個(gè)矩陣了，那為什么我說他是正交單位陣那？首先需要你知道的是我們?cè)趯W(xué)習(xí)矩陣的時(shí)候都是先學(xué)習(xí)的坐標(biāo)系然后再學(xué)習(xí)的向量，因此我們需要先改變一個(gè)概念，坐標(biāo)系并不只有像墻角那樣垂直的坐標(biāo)系，其次向量是真實(shí)存在的，但是向量的是唯一的，但是向量的坐標(biāo)是不一定的，因?yàn)樽鴺?biāo)系是由線性不相關(guān)的向量構(gòu)成的向量組，也可以稱為基底，一般選擇正交的單位陣，符合人類的直覺。選擇不正交且不是單位陣的矩陣作為坐標(biāo)系嗎？我猜你在定位問題中不會(huì)。如公式1-2。是一個(gè)坐標(biāo)系的基底。是在該基底下向量的坐標(biāo)，其含義是在三個(gè)向量的加和構(gòu)成了需要表示的向量。同理在基底下的坐標(biāo)表示為。因?yàn)槎际菍?duì)同一向量的表示因此二者相等。那么可由式1-3得出。觀察是基底向量的內(nèi)積留下的是余弦函數(shù)，將看成是世界坐標(biāo)系下的原點(diǎn)，此時(shí)R代表了旋轉(zhuǎn)。表示了旋轉(zhuǎn)的方向，因此R稱為方向余弦矩陣也稱為旋轉(zhuǎn)矩陣。是機(jī)器人姿態(tài)的變換，是slam中待優(yōu)化的變量，也是我們實(shí)時(shí)要知道的值，這樣才可以知道機(jī)器人是如何旋轉(zhuǎn)的，至此我們就明白了待優(yōu)化的變量不僅是個(gè)矩陣而且是個(gè)單位正交陣。而且這個(gè)旋轉(zhuǎn)矩陣相乘，仍然是旋轉(zhuǎn)矩陣，這個(gè)性質(zhì)挺有意思吧，整數(shù)相加仍然是整數(shù)和其類似，數(shù)學(xué)中把像旋轉(zhuǎn)平移矩陣和乘法，做完運(yùn)算后還是旋轉(zhuǎn)平移矩陣的稱為群（這是群的封閉性，即運(yùn)算完還是自己。我認(rèn)為集合和運(yùn)算在滿足封閉性后，很大概率上就是群，當(dāng)然嚴(yán)格定義共需要滿足4個(gè)條件，自行查找吧），至此我們可以得出slam中待優(yōu)化的變量是矩陣，是正交矩陣，還是群。并且這個(gè)群的乘法代表著旋轉(zhuǎn)變換，而且旋轉(zhuǎn)變換是連續(xù)的，平滑的，旋轉(zhuǎn)多微小的變化都可以，因此又稱為李群。OK,這便是待優(yōu)化的變量。

那么在優(yōu)化過程中我們需要求導(dǎo)，根據(jù)導(dǎo)數(shù)的定義，我們知道我去待優(yōu)化變量是需要做減法的如式2-1所示，而矩陣做減法沒有意義，這該如何是好？以下公式是前人想到的啊，我也不知道是怎么想到的，兩個(gè)字就是牛逼。已知任意旋轉(zhuǎn)矩陣滿足式2-2.對(duì)式2-2求導(dǎo)得整理得有一個(gè)結(jié)論，任意一個(gè)向量都可以用一個(gè)反對(duì)稱矩陣表示同時(shí)任意一個(gè)反對(duì)稱矩陣都可以用一個(gè)向量表示。這是結(jié)論，不允許你問為什么。根據(jù)結(jié)論式2-4可以表示為式2-5等式兩邊右乘R(t)，由于R為正交陣得到式2-6通過上式的推導(dǎo)可以看出反對(duì)稱矩陣對(duì)應(yīng)的向量是矩陣的導(dǎo)數(shù)，由于旋轉(zhuǎn)矩陣是關(guān)于時(shí)間變量連續(xù)的，故待優(yōu)化變量也成了時(shí)間t的函數(shù)。以直代曲的逼近過程是可以將導(dǎo)數(shù)看成常數(shù)故得2-7.解式2-7的微分方程得式2-8當(dāng)t=0時(shí)刻時(shí)，矩陣沒有變化因此帶入2-8可得最終的式2-9即c=0;通過推導(dǎo)將矩陣變換看成時(shí)間的導(dǎo)數(shù)，并推導(dǎo)出旋轉(zhuǎn)變換矩陣與指數(shù)函數(shù)的相關(guān)。那矩陣的乘法也就變成了指數(shù)的乘法，指數(shù)的乘法的運(yùn)算規(guī)則是加法，所以這是不是可以對(duì)旋轉(zhuǎn)變換矩陣進(jìn)行求導(dǎo)了，答;必須滴！那這個(gè)就成為李代數(shù)，每個(gè)李群都有與之對(duì)應(yīng)的李代數(shù)，李代數(shù)描述了李群的局部性質(zhì)，這里對(duì)矩陣以直代曲就是李代數(shù)最直觀的體現(xiàn)，在t=0時(shí)刻的泰勒展開就是李代數(shù)和李群最直觀的聯(lián)系如式2-10.后文我還會(huì)繼續(xù)深究，起始此向量是旋轉(zhuǎn)向量對(duì)應(yīng)的向量空間。后面細(xì)究。先有個(gè)直觀感受。設(shè),并設(shè)此時(shí)旋轉(zhuǎn)矩陣為。按照導(dǎo)數(shù)定義,可以把在t=0附近進(jìn)行一階泰勒展開:回到剛才，有了指數(shù)的加法是否可以得到式2-11或者式2-12

答不可以，只有高中學(xué)的是標(biāo)量才可以，當(dāng)是向量的時(shí)候不可以，BCH這三人得出了是標(biāo)量時(shí)的公式2-13

忽略高次項(xiàng)可得結(jié)論式2-14，想知道為什么需要自己推導(dǎo)。

和2-12差不多少了，只是多了一個(gè)雅可比矩陣或者（二者取其一）雅克比的具體表達(dá)如式2-15

至此優(yōu)化問題出現(xiàn)的對(duì)矩陣求導(dǎo)無法滿足導(dǎo)數(shù)定義的問題通過李代數(shù)將旋轉(zhuǎn)變換矩陣通過指數(shù)函數(shù)進(jìn)行表示，而指數(shù)函數(shù)有加法，可以滿足導(dǎo)數(shù)定義。

相機(jī)坐標(biāo)系的坐標(biāo)原點(diǎn)在世界坐標(biāo)系下的坐標(biāo)點(diǎn)P,通過算法得到R,現(xiàn)在要對(duì)R求導(dǎo)。如式3-1通過李代數(shù)的替換為式3-2根據(jù)導(dǎo)數(shù)定義對(duì)3-2式進(jìn)行展開第2行為線性近似,第3行為泰勒展開舍去高階項(xiàng)后的近似,4行和5是將反對(duì)稱符號(hào)看作外積（兩個(gè)向量的外積可以化作反對(duì)稱矩陣和另外一個(gè)向量的乘積）,交換之后變號(hào)。不過問題在于需要求取這可是很麻煩的如式2-14。不過不要被導(dǎo)數(shù)定義定死了，導(dǎo)數(shù)的定義用一句話來講是函數(shù)的微小變化和自變量微小變化的比值。按照如下想法可以得如下推導(dǎo)過程。這沒有了雅可比多簡(jiǎn)潔啊，這個(gè)求法稱為擾動(dòng)模型。

1. 李代數(shù)

這里就不仔細(xì)推導(dǎo)了，具體推導(dǎo)過程是將泰勒展開，最終可得這個(gè)向量是旋轉(zhuǎn)向量，什么是旋轉(zhuǎn)向量，除了上文推導(dǎo)出的旋轉(zhuǎn)矩陣即方向余弦矩陣，還有旋轉(zhuǎn)向量，方向余弦矩陣用了9個(gè)變量表示roll pitch yaw冗余了，使用旋轉(zhuǎn)向量即方向表示旋轉(zhuǎn)的向量的繞軸，大小代表轉(zhuǎn)過的角度。這樣不冗余了。而旋轉(zhuǎn)向量和方向余弦矩陣可以通過羅德里格旋轉(zhuǎn)表示。這里需要自行查找資料，其實(shí)推導(dǎo)不難。只需記住結(jié)論：李代數(shù)是旋轉(zhuǎn)向量組成的空間。最后提示一下，本文只提起了旋轉(zhuǎn)矩陣R,但是機(jī)器人的運(yùn)動(dòng)自由度有六個(gè)，我沒有提及平移變換，而平移和旋轉(zhuǎn)組成的4*4齊次矩陣依然是和旋轉(zhuǎn)矩陣一個(gè)性質(zhì)，也是一個(gè)群，也有對(duì)應(yīng)的李群，方法一樣的，可以替換的。