基于新的圖優化方法的拓撲地圖創建*

黃宏前石朝俠王燕清

（1.南京理工大學計算機科學與工程學院 南京 210094）（2.南京曉莊學院信息工程學院 南京 211171）

1 引言

SLAM是移動機器人領域研究的熱點。機器人需要通過自主導航和決策，完成相應的任務。而閉環檢測和檢測之后的地圖優化也是相當重要的一環。僅使用里程計航跡推算信息的SLAM系統，誤差隨著時間的積累增加導致創建的地圖誤差會越來越大。所以實現閉環檢測和地圖優化是一個非常重要的內容。

在SLAM發展過程中，實現閉環檢測和地圖優化的方法很多，如圖優化理論算法［1～3］。圖通常是由節點和邊構成，在SLAM領域中，圖的節點通常是由機器人的位姿構成，圖中的邊通常是由當前時刻和上一時刻的位姿變換關系構成的，或者是由視覺里程計計算出來的位姿轉換矩陣構成的。圖構建好之后就需要優化節點位姿去滿足圖中邊的約束，這就是SLAM中主流的圖優化方法。圖優化方法應用在各個地方，Olson等［4］使用了隨機梯度下降的方法來求解優化的非線性方程，利用兩個位姿之間的邊和它們之間的變換關系來恢復機器人的軌跡，可以在初始值較差的情況下取得較好的結果。翁瀟文等［5］利用激光傳感器獲得觀測數據來建立邊，根據圖優化理論進行匹配優化求解出誤差最小的位姿。在三維的空間中［6～8］，利用相機模型，通過在相鄰圖像之間匹配獲得的位姿變換和提取圖像的特征點放在一起創建成圖進行優化，利用閉環檢測和捆綁約束BA的方法，可以消除由于里程計漂移造成的誤差問題。拓撲地圖因簡潔而不需要大量計算的特點，在很多方面也有應用。Hou等［9］將真實掃描的地圖表示為拓撲地圖，頂點表示面積，邊線表示通道，利用Voronoi圖生成表示環境的拓撲地圖。Sren等［10］利用拓撲地圖來檢測房間并且用與真實地面圖的拓撲進行匹配，形成映射，完成房間的檢測。在車輛領域，也可以使用一種基于拓撲地圖構建和場景識別方法來對車輛進行定位［11］，使用圖像序列生成拓撲圖，利用Extended-HCT方法進行語義描述和特征提取來對車輛進行識別和定位。

可見，圖優化在地圖創建和優化方面是很重要的一環。圖優化的方法在大部分情況下是非常有效的。但是如果系統中僅有里程計航跡推算信息，是很難對地圖進行調整和優化。本文針對這個問題，提出了一種新的圖優化方法，只采用里程計航跡推算信息，引入機械臂逆向運動學模型減小位姿跟蹤和地圖創建誤差，并使用模擬退火算法實現變步長快速迭代，完成拓撲地圖的創建。

2 基于機械臂逆向運動學的圖優化方法

2.1 拓撲圖邊及節點定義

拓撲地圖是由一系列的邊和節點構成的。在本文中，節點的定義為地圖中路口的位姿。在每一個位姿中，都有包含當前路口的全局坐標（x，y），當前路口的分支個數branch_num、每一個分支相對當前路口機器人的偏角大小orientation［branch_num］以及節點對應的每一個分支所看到的圖像image［branch_num］。其中orientation里面存放的是當前路口的所有分支相對于機器人的偏角，Image存放著當前機器人在該路口中央所看到的各個分支的圖像。在本文中，存在四種類型的路口，分別是十字路口、T字路口、直角路口和單向路口。在每一種路口類型中，都會有當前的位姿信息。機器人每到達一個路口節點之后，會使用環繞攝像頭來檢測消失點從而判斷路口分支的數量，然后在每一個分支場景中提取特征點作為當前分支的場景描述，從而建立節點的拓撲場景地圖［12］。如圖1所示，圖中十字路口節點處的四處分支分別對應四張特征場景圖。拓撲地圖中的邊信息，是兩個路口節點之間的路徑。從邊的信息可以得到相對應的兩個節點的信息。

圖1 拓撲場景地圖

拓撲地圖是由節點和邊來表示，邊表示相連的兩個節點之間的有一條相通的路徑，這與機械臂模型很相似，機械臂模型由桿和關節連接而成。可以借鑒機械臂運動學規劃思想應用到拓撲地圖的創建優化中。所以本文引入了機械臂逆向運動學的思想來對創建的拓撲地圖進行優化。

2.2 機械臂逆向運動學

在本文中，拓撲地圖的優化調整主要借鑒了機械臂運動學的規劃控制原理。機械臂運動學［13～16］主要包括兩個方面，一是正向運動學，已知機械臂中所有桿的長度和各個關節的轉角，求末端結構所處的一個位置；二是逆向運動學，知道了末端結構所處的位置，最終需要求出各個關節所轉動的角度分別是多少。在機械臂的運動學中，討論和使用最多的還是逆向運動學。

假設現在末端結構的位置（x，y），那么根據機械臂的結構，假設機械臂有N個關節，那么根據機器人運動學正解的方法可以得出：

式中，x和y表示末端結構的位置，θi表示各個關節所旋轉之后的角度，Li表示每一個機械臂桿的長度。如果將末端結構x和y的改變量大小設置為ΔX，θi的變化量設置為Δq，那么上面式（1）、（2）可以寫為

其中J為

將機器人路徑看成機械臂，由于里程計的誤差，機器人生成的路徑中不僅方向會發生偏移，兩個路口之間的距離也會有偏差。所以，需要將每個節點路口的偏角θ和兩個路口節點之間距離L都當成自變量來進行求解。此時，式（4）中的雅可比矩陣就變為

自變量q變為

假設現在需要求雅格比矩陣中的某一列的微分，那么有

其中e表示末端結構的位姿，qi表示某一列的旋轉角度和桿的伸縮長度。如果對qi增加微小的改變量，那么就能計算末端結構的位移量：

對于解析法，本文使用差分代替微分求解，那么式（5）變換為

求機械臂的逆解時，本文用到了最速下降法的思想，一是避免變量太多時解析法無法計算，二是可以沿著函數值下降最快的方向搜索。求取末端結構的位姿，本文的誤差函數就是當前值與目標值的歐式距離：

式中Xtarget表示目標點位姿，f（q）表示末端位置關于自變量q的函數。機械臂逆解中使用梯度下降法，該方法可以求得角度和桿伸縮的變化值Δq：

式中的α表示迭代搜索的步長。

3 模擬退火算法迭代策略

模擬退火算法［17］是啟發示算法的一種，使用貪心的算法，來搜索達到全局的最優點。該算法來源于固體退火原理，溫度升高的時候，內部分子的會變為無序狀；當溫度下降下來的時候，分子又會取向于有序。模擬算法的基本思想的基本流程如下：

Step 1初始化溫度T，初始解狀態S，以及迭代次數L。

Step 2迭代L次，重復步驟3到步驟6的過程，直到搜索到最優解。

Step 3在當前解基礎上面產生新解S'。

Step 4計算當前增量Δs=f（s’）-f（s），其中f（s）為模擬退火算法的代價函數，在本文中的代價函數f（s）指的是當前位姿X與目標位姿Xtarget的歐式距離大小。

Step 5若Δs＜0則接受當前解S'作為當前的新解；否則以一定概率P接受該解作為當前解。其中P為

Step 6如果滿足條件，則輸出當前解作為最優解，結束程序。

Step 7 T減少，并且保證T＞0。

在利用梯度下降法去迭代結果的時候，本文中的步長可以使用模擬退火算法來實現變長，加快算法的速度。在該模擬退火算法的基礎上，每次在不接受新解的時候，會將當前的步長α減半，進行一個二分的折半搜索策略，可以加快搜索的進度。

4 仿真實驗及分析

4.1 實驗平臺及數據

本文實驗是在自主研發的一個仿真平臺上面實現的。該平臺是基于C++語言開發的一個機器人仿真平臺，地圖數據是本校的俯瞰圖，可以看到地圖的每一條道路和每一個路口地點，平臺上有機器人在地圖中真實的行走路徑，也有機器人行走時創建的理論地圖。每一個機器人都有自己的一個里程計信息。

在實驗中設定的參數初值分別是α=0.9，迭代的總次數為50次，模擬退火算法代價函數初始值為1000。本文采取的方法與原模擬退火算法稍有不同。本文中如果當前解不滿足條件，則以當前的一定概率pcur接受該解，如果不接受該解，則將α的當前值做一個二分減半的操作，同時概率pcur是隨著代價函數的變化量和溫度T動態變化。來實現一個變步長的迭代算法。

4.2 創建地圖及閉環檢測

在創建拓撲地圖時，機器人每到達一個節點，就進行閉合檢測。閉環的方法很多［18～19］，但本文中的閉環利用到了相鄰的兩個節點之間里程計的誤差范圍大小，根據當前節點的位置，來推斷下一個節點所在的范圍，通過位姿的偏差范圍、分支個數以及當前位姿相對于各個分支偏角的范圍來進行判斷是否為之前訪問過的節點。如果確認閉環，則更新回環中的所有節點信息；否則將該節點放進自己的局部地圖庫中，繼續漫游。

4.3 實驗仿真

在實驗過程中，采取在線地圖優化的一個策略。在試驗中，對創建的拓撲地圖進行了優化，使用到了兩種機械臂路徑規劃的算法思想來進行對比。一種是根據Cyclic Coordinate Descent（CCD）的啟發式迭代搜索算法［20］，通過每次只改變一個關節的參數來減少誤差，從該拓撲地圖的末端開始調整優化，直到收斂或者誤差到達最小值。另外一種就是本文中提出來的基于機械臂逆運動學方法的拓撲地圖優化。結果如圖2、圖3所示。

圖2 拓撲地圖優化對比（一）

圖3 拓撲地圖優化對比（二）

雖然最后兩種方法調整的結果差不多，但是過程卻是不一樣。由圖4可知，在對十個位姿節點的閉環優化過程中，CCD方法、逆運動學方法以及加了模擬退火優化的逆運動學方法三種方法在原始圖、第5次迭代和第10次迭代的結果可知，可以看出，在第5次和第10次的迭代結果中，CCD調整優化方法幾乎是非常慢的并且調整的非常小，但本文使用的模擬退化算法變步長優化的機械臂逆運動學方法在第10次的時候可以快速的收斂到最后的結果，比沒有加模擬退退火的機械臂逆運動學方法更加的快速。

圖4 原始圖（左）、第5次迭代（中）、第10次迭代（右）中間結果

本文中創建了兩個地圖，并且在機器人創建地圖的同時，對到達的路口節點進行閉環檢測，然后對環中的所有節點進行優化調整。可以看出，對于實際創建的拓撲地圖，誤差的積累越來越大。在只使用里程計的情況下，利用本文提出來的機械臂優化調整方法，可以調整優化已閉環的拓撲地圖接近真實的拓撲地圖結構。

4.4 機械臂方法各個位姿節點角度調整對比

圖5表示在一個有十個位姿節點閉環的拓撲地圖中，每條路徑調整的弧度大小。可以看出來，在調整過程中，雖然結果可能差不多，但是本文方法在兩個路口位姿節點之間的路徑調整的幅度會比另外一種方法更小，并且調整的誤差也會更加平均。

圖5 十個位姿節點的閉環調整角度大小

4.5 模擬退火算法變步長對比

由圖6可知，在本文實驗下，使用了模擬退火算法實現變步長比固定步長的算法時間和迭代次數要短的多。雖然兩者前面幾次迭代都差不多一樣，但是在后面迭代的時候，可以明顯看出來變步長的算法更快的收斂。

圖6 模擬退火變步長對比

5 結語

機器人的地圖創建和優化是SLAM系統中重要的一環，特別是在閉環之后，誤差調整更為重要。對于單里程計的SLAM系統，里程計漂移造成了地圖創建偏差，本文針對該問題，提出了一種新的基于圖優化方法的拓撲地圖創建，可以很好地解決地圖中各個節點之間的位姿誤差問題，能夠更好地將積累的誤差平均的分攤到回環中的各個節點中，生成一個較為準確的地圖。實驗結果證明了該算法是有效可行的。

之后的研究中如果加入了場景匹配信息，那么將會極大地提高地圖創建的準確性。