相機IMU融合四部曲（三）:MSF詳細解讀與使用

相機IMU融合四部曲（三）:MSF詳細解讀與使用

極品巧克力

前言

經過前兩篇文章，《D-LG-EKF詳細解讀》和《偏差狀態四元數詳細解讀》，已經把相機和IMU融合的理論所有都推導一遍了。並且《偏差狀態四元數》還對實際操做中的可能遇到的一些狀況，進行指導。

這些理論都已經比較完整了，那麼，該如何在實際當中操做呢？該如何用到實際產品中呢？偏差狀態四元數，是有開源的程序的，可是它是集成在rtslam（ https://www.openrobots.org/wiki/rtslam/ ）裏面的，不方便提取出來使用。

但還有另一個開源的程序，ETH的MSF（https://github.com/ethz-asl/ethzasl_msf），它是獨立的一個開源程序，能夠比較方便地用在本身的工程裏面，而且它的理論與偏差狀態四元數很接近，稍微有點不一樣，因此MSF開源程序就成了一個不錯的選擇。

因此，我把MSF的程序所有都通讀一遍以後，結合程序來推導MSF的理論，總結成本文，包括MSF的實驗，與各位分享。

1.基本模型

基本模型以下圖所示。

MSF的可擴展性很好，程序裏能夠接入新的傳感器，好比GPS，激光雷達，碼盤等。

主要的理論仍是偏差狀態四元數裏面的理論。

相比於《卡爾曼四元數帶外參融合方法》，它的方法更好。

首先，它的核心狀態裏面沒有重力在世界座標系下的表示。由於它用的就是《偏差狀態四元數》裏面的5.3.1的方法，直接就是以水平座標系爲世界座標系。只是初始位置根據當前加速度計的測量值和理論重力計算出來，初始位置帶着一個協方差而已。由於反正最後都是要轉換到水平座標系下的，因此這種方法反而更加方便。

並且，IMU的世界座標系和相機的世界座標系，也不用綁在一塊兒，同時創建。能夠在IMU開啓一小段時間之後，相機再獲取它的世界座標系。但相隔時間應該仍是要儘可能短點，由於位移單靠IMU的加速度計的積分，時間久了會很不許確，若是有輪子碼盤的話，則又能夠好一些。調整的目的，多是由於世界座標系和相機的世界座標系，在時間戳上並不對應得很好，因此須要微調。

或者，爲了不這樣的麻煩，直接把IMU的世界座標系和相機的世界座標系，綁在一塊兒，同時創建。這樣子，。

也應該用ETH的手眼標定方法，借用棋盤格先標定好，再固定住。就直接用尺子量好，固定住。若是用雙目相機的話，就不用考慮了。

（若是加上輪速計的話，怎麼更新呢？它的做用與IMU是相同的，都是相對值，而不是絕對值。因此，它應該與IMU實時融合，在主狀態與相機融合後，IMU預測出一個位姿態，輪速計預測出一個位姿，而後二者融合出主狀態。只有主狀態才能與相機位姿融合。融合的話，能夠用偏差狀態的思想來融合。）

因此，參考《偏差狀態四元數詳細解讀》，一個簡單魯棒的IMU與相機融合的模型應該是這樣的。

核心狀態爲，。

核心狀態上的擾動爲，。

運動模型跟《偏差狀態四元數》裏面的同樣，

則相機位姿的觀測方程爲，

使用《偏差狀態四元數》裏面的公式，

其中，

首先，要計算第一項關於的雅克比，

則用表示上式，則雅克比計算以下，

而後，第二項，要轉成三個元素的形式，即角軸李代數的形式。

用matlab程序，解出上面的式子，以及關於的雅克比。

syms qicw qicx qicy qicz thetax thetay thetaz ...

qwiw qwix qwiy qwiz qzocw qzocx qzocy qzocz real

q1=[qicw,-qicx,-qicy,-qicz]';

q2=[1,-1/2*thetax,-1/2*thetay,-1/2*thetaz]';

q3=[qwiw,-qwix,-qwiy,-qwiz]';

q4=[qicw,qicx,qicy,qicz]';

q5=[qzocw,qzocx,qzocy,qzocz]';

temp = quaternion_mul(q1,q2);

temp = quaternion_mul(temp,q3);

temp = quaternion_mul(temp,q4);

temp = quaternion_mul(temp,q5);

temp

J=jacobian(2*temp(2:4,:),[thetax,thetay,thetaz])

simplify(J)

 

雅克比能夠是，把temp轉換成角軸，再關於求導。或者，角軸直接近似等於temp向量部分的2倍，再關於求導，像上面的matlab程序這樣。

這樣子計算雖然準確，可是太麻煩了。論文裏還用了近似的方法。爲了方便地求雅克比，認爲測量值近似爲預測值直接轉換出來，即，。

這時候，上式轉換成角軸，就是，向量部分的2倍，即，。因此，雅克比爲，

（這種用近似的方法，來算雅克比，雖然不如從原始公式上推導準確，可是能夠極大地簡化計算，也許能夠給D-LG-EKF裏面計算H矩陣時借鑑。）

因此，就獲得了雅克比矩陣。

而後，。就能夠計算了。其他的流程，就跟《偏差狀態四元數》裏面同樣了。

2.對延時的處理

前提條件是，各個傳感器的時間戳得是同一個時間源的，或者，時間戳很穩定，能夠經過一些方法把它們時間戳之間的對應關係找到。不一樣傳感器的時間戳能準確對應上。

而後，由於測量值有時候會過一段時間才處理完，因此，把濾波中的狀態都記錄起來，而後，當有測量值過來的時候，更新對應時刻的狀態。而後繼續日後預測。若是有多個不一樣傳感器的測量值，也是如此操做。

3.計算相對測量

若是要融合單目相機。考慮到單目尺度的狀況，怕有時候尺度會忽然發生變化。爲了應對這種狀況，就都計算相對測量，就是兩個時刻之間的相對位姿態，這樣子，這一段的尺度就能夠濾得比較準確。而後，再把優化後的值，加入到原來的狀態中，方法跟《偏差狀態四元數》中的傳播差很少，就是把新濾出來的這段位姿的均值和擾動，加到原先狀態的均值和擾動中去，整合出新的均值和擾動。

而若是要融合的是輪子碼盤的話，則沒必要用這樣的方法。由於雖然輪子碼盤也有尺度問題，可是尺度是較穩定的。

若是是雙目相機的話，也沒必要考慮這種尺度忽然變化的狀況。

4.準確地計算每幀的協方差

爲了更好地與GPS融合，就須要當前狀態須要有準確的協方差。

而之前計算出來的相機位姿的協方差是不許確的，沒有考慮到累積偏差形成的影響。形成了與IMU融合後的狀態協方差也是不許確的。得準確地計算出要融合的每幀圖像的協方差。這協方差，就是經過全局BA的方法，計算出來的。

但其實爲了簡化。若是真的要與GPS融合的話。

在即時建圖的狀況下，用上一段的方法。可是視覺的偏差累積仍是很可觀的，因此若是是遠距離的話，應該以GPS爲外界絕對測量，視覺只是用來計算相對測量的。只能用短距離的視覺相對測量。在主狀態以後，以此爲起點，視覺的相對測量值與IMU的相對測量值融合，融合出相對測量狀態，再把相對狀態的均值和擾動合併進主狀態以及主狀態擾動，主狀態再與GPS融合，融合出新的主狀態。融合的話，能夠用偏差狀態的思想來融合。

若是已經提早建圖，迴環檢測都作了，地圖點固定且準確了，則視覺協方差就是當前幀的協方差，沒必要經過全局BA算出。

5. MSF方法總結

MSF的方法考慮得很全面，這個理論框架，能夠用來融合多種傳感器。

我上面思考出來的方法，還考慮了與輪速計碼盤，GPS融合的具體操做狀況，之後須要時再用。

6.實驗與改進

MSF的安裝與跑例程，能夠參考這篇文章，

http://www.liuxiao.org/2016/07/ros-%E5%A4%9A%E4%BC%A0%E6%84%9F%E5%99%A8%E5%8D%A1%E5%B0%94%E6%9B%BC%E8%9E%8D%E5%90%88%E6%A1%86%E6%9E%B6-ethzasl-msf-framework-%E7%BC%96%E8%AF%91%E4%B8%8E%E4%BD%BF%E7%94%A8/。

而後，我本身用ORBSLAM2跑Euroc的V201數據集，生成軌跡數據，和IMU數據一塊兒送入到MSF中運行。爲何要跑標準數據集呢？由於標準數據集提供了IMU噪聲的真實參數，能夠直接拿來使用，並且有真實的軌跡，groundtruth，能夠用來評價融合結果的好壞。

但是運行結果卻老是發散，融合後的軌跡鋸齒很是嚴重。但是在理論上來說，它應該能取得比較好的效果的呀？因此猜想應該是程序與理論沒有對應上。

只好從程序裏去查問題的緣由。將程序裏的全部的計算過程與算法公式一一對應起來以後，最終發現，是由程序裏的2個地方致使的。

1.MSF程序有個隱含的假設，即圖像的世界座標系是水平的。而我送的是以第一幀爲世界座標系的，而V201的第一幀並非水平的。

2. 通過仔細推導程序的計算過程，發現MSF程序中的qwv，本質上是qvw，這個致使參數的初值給錯了。

將以上兩個問題改過來以後，MSF就能夠正常運行我本身提供的數據了。

其中，在y軸上的結果以下。

以上，黑色的線是真實值，綠色的線是ORBSLAM經過圖像計算出來的位姿，紅色的線是圖像位姿和IMU融合後的結果，線上的每個點都表明一個輸出數據。能夠看出，msf融合後的結果，不只能夠把位姿的輸出頻率提升到和IMU同樣的頻率，還可讓軌跡更加接近真實值。

可是，msf有一個缺點，那就是IMU的bias收斂得很慢，猜想是因爲近似形成的。以下圖所示。

上圖是加速度計的bias的收斂狀況，和陀螺儀的bias的收斂狀況。也許能夠經過修改這部分的公式，讓bias收斂得更快。

7.參考文獻

1. Lynen S, Achtelik M W, Weiss S, et al. A robust and modular multi-sensor fusion approach applied to MAV navigation[C]// Ieee/rsj International Conference on Intelligent Robots and Systems. IEEE, 2013:3923-3929.