基于新的圖優(yōu)化方法的拓?fù)涞貓D創(chuàng)建*

黃宏前石朝俠王燕清

（1.南京理工大學(xué)計(jì)算機(jī)科學(xué)與工程學(xué)院 南京 210094）（2.南京曉莊學(xué)院信息工程學(xué)院 南京 211171）

1 引言

SLAM是移動機(jī)器人領(lǐng)域研究的熱點(diǎn)。機(jī)器人需要通過自主導(dǎo)航和決策，完成相應(yīng)的任務(wù)。而閉環(huán)檢測和檢測之后的地圖優(yōu)化也是相當(dāng)重要的一環(huán)。僅使用里程計(jì)航跡推算信息的SLAM系統(tǒng)，誤差隨著時(shí)間的積累增加導(dǎo)致創(chuàng)建的地圖誤差會越來越大。所以實(shí)現(xiàn)閉環(huán)檢測和地圖優(yōu)化是一個(gè)非常重要的內(nèi)容。

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

可見，圖優(yōu)化在地圖創(chuàng)建和優(yōu)化方面是很重要的一環(huán)。圖優(yōu)化的方法在大部分情況下是非常有效的。但是如果系統(tǒng)中僅有里程計(jì)航跡推算信息，是很難對地圖進(jìn)行調(diào)整和優(yōu)化。本文針對這個(gè)問題，提出了一種新的圖優(yōu)化方法，只采用里程計(jì)航跡推算信息，引入機(jī)械臂逆向運(yùn)動學(xué)模型減小位姿跟蹤和地圖創(chuàng)建誤差，并使用模擬退火算法實(shí)現(xiàn)變步長快速迭代，完成拓?fù)涞貓D的創(chuàng)建。

2 基于機(jī)械臂逆向運(yùn)動學(xué)的圖優(yōu)化方法

2.1 拓?fù)鋱D邊及節(jié)點(diǎn)定義

拓?fù)涞貓D是由一系列的邊和節(jié)點(diǎn)構(gòu)成的。在本文中，節(jié)點(diǎn)的定義為地圖中路口的位姿。在每一個(gè)位姿中，都有包含當(dāng)前路口的全局坐標(biāo)（x，y），當(dāng)前路口的分支個(gè)數(shù)branch_num、每一個(gè)分支相對當(dāng)前路口機(jī)器人的偏角大小orientation［branch_num］以及節(jié)點(diǎn)對應(yīng)的每一個(gè)分支所看到的圖像image［branch_num］。其中orientation里面存放的是當(dāng)前路口的所有分支相對于機(jī)器人的偏角，Image存放著當(dāng)前機(jī)器人在該路口中央所看到的各個(gè)分支的圖像。在本文中，存在四種類型的路口，分別是十字路口、T字路口、直角路口和單向路口。在每一種路口類型中，都會有當(dāng)前的位姿信息。機(jī)器人每到達(dá)一個(gè)路口節(jié)點(diǎn)之后，會使用環(huán)繞攝像頭來檢測消失點(diǎn)從而判斷路口分支的數(shù)量，然后在每一個(gè)分支場景中提取特征點(diǎn)作為當(dāng)前分支的場景描述，從而建立節(jié)點(diǎn)的拓?fù)鋱鼍暗貓D［12］。如圖1所示，圖中十字路口節(jié)點(diǎn)處的四處分支分別對應(yīng)四張?zhí)卣鲌鼍皥D。拓?fù)涞貓D中的邊信息，是兩個(gè)路口節(jié)點(diǎn)之間的路徑。從邊的信息可以得到相對應(yīng)的兩個(gè)節(jié)點(diǎn)的信息。

圖1 拓?fù)鋱鼍暗貓D

拓?fù)涞貓D是由節(jié)點(diǎn)和邊來表示，邊表示相連的兩個(gè)節(jié)點(diǎn)之間的有一條相通的路徑，這與機(jī)械臂模型很相似，機(jī)械臂模型由桿和關(guān)節(jié)連接而成?？梢越梃b機(jī)械臂運(yùn)動學(xué)規(guī)劃思想應(yīng)用到拓?fù)涞貓D的創(chuàng)建優(yōu)化中。所以本文引入了機(jī)械臂逆向運(yùn)動學(xué)的思想來對創(chuàng)建的拓?fù)涞貓D進(jìn)行優(yōu)化。

2.2 機(jī)械臂逆向運(yùn)動學(xué)

在本文中，拓?fù)涞貓D的優(yōu)化調(diào)整主要借鑒了機(jī)械臂運(yùn)動學(xué)的規(guī)劃控制原理。機(jī)械臂運(yùn)動學(xué)［13～16］主要包括兩個(gè)方面，一是正向運(yùn)動學(xué)，已知機(jī)械臂中所有桿的長度和各個(gè)關(guān)節(jié)的轉(zhuǎn)角，求末端結(jié)構(gòu)所處的一個(gè)位置；二是逆向運(yùn)動學(xué)，知道了末端結(jié)構(gòu)所處的位置，最終需要求出各個(gè)關(guān)節(jié)所轉(zhuǎn)動的角度分別是多少。在機(jī)械臂的運(yùn)動學(xué)中，討論和使用最多的還是逆向運(yùn)動學(xué)。

假設(shè)現(xiàn)在末端結(jié)構(gòu)的位置（x，y），那么根據(jù)機(jī)械臂的結(jié)構(gòu)，假設(shè)機(jī)械臂有N個(gè)關(guān)節(jié)，那么根據(jù)機(jī)器人運(yùn)動學(xué)正解的方法可以得出：

式中，x和y表示末端結(jié)構(gòu)的位置，θi表示各個(gè)關(guān)節(jié)所旋轉(zhuǎn)之后的角度，Li表示每一個(gè)機(jī)械臂桿的長度。如果將末端結(jié)構(gòu)x和y的改變量大小設(shè)置為ΔX，θi的變化量設(shè)置為Δq，那么上面式（1）、（2）可以寫為

其中J為

將機(jī)器人路徑看成機(jī)械臂，由于里程計(jì)的誤差，機(jī)器人生成的路徑中不僅方向會發(fā)生偏移，兩個(gè)路口之間的距離也會有偏差。所以，需要將每個(gè)節(jié)點(diǎn)路口的偏角θ和兩個(gè)路口節(jié)點(diǎn)之間距離L都當(dāng)成自變量來進(jìn)行求解。此時(shí)，式（4）中的雅可比矩陣就變?yōu)?/p>

自變量q變?yōu)?/p>

假設(shè)現(xiàn)在需要求雅格比矩陣中的某一列的微分，那么有

其中e表示末端結(jié)構(gòu)的位姿，qi表示某一列的旋轉(zhuǎn)角度和桿的伸縮長度。如果對qi增加微小的改變量，那么就能計(jì)算末端結(jié)構(gòu)的位移量：

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

求機(jī)械臂的逆解時(shí)，本文用到了最速下降法的思想，一是避免變量太多時(shí)解析法無法計(jì)算，二是可以沿著函數(shù)值下降最快的方向搜索。求取末端結(jié)構(gòu)的位姿，本文的誤差函數(shù)就是當(dāng)前值與目標(biāo)值的歐式距離：

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

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

3 模擬退火算法迭代策略

模擬退火算法［17］是啟發(fā)示算法的一種，使用貪心的算法，來搜索達(dá)到全局的最優(yōu)點(diǎn)。該算法來源于固體退火原理，溫度升高的時(shí)候，內(nèi)部分子的會變?yōu)闊o序狀；當(dāng)溫度下降下來的時(shí)候，分子又會取向于有序。模擬算法的基本思想的基本流程如下：

Step 1初始化溫度T，初始解狀態(tài)S，以及迭代次數(shù)L。

Step 2迭代L次，重復(fù)步驟3到步驟6的過程，直到搜索到最優(yōu)解。

Step 3在當(dāng)前解基礎(chǔ)上面產(chǎn)生新解S'。

Step 4計(jì)算當(dāng)前增量Δs=f（s’）-f（s），其中f（s）為模擬退火算法的代價(jià)函數(shù)，在本文中的代價(jià)函數(shù)f（s）指的是當(dāng)前位姿X與目標(biāo)位姿Xtarget的歐式距離大小。

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

Step 6如果滿足條件，則輸出當(dāng)前解作為最優(yōu)解，結(jié)束程序。

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

在利用梯度下降法去迭代結(jié)果的時(shí)候，本文中的步長可以使用模擬退火算法來實(shí)現(xiàn)變長，加快算法的速度。在該模擬退火算法的基礎(chǔ)上，每次在不接受新解的時(shí)候，會將當(dāng)前的步長α減半，進(jìn)行一個(gè)二分的折半搜索策略，可以加快搜索的進(jìn)度。

4 仿真實(shí)驗(yàn)及分析

4.1 實(shí)驗(yàn)平臺及數(shù)據(jù)

本文實(shí)驗(yàn)是在自主研發(fā)的一個(gè)仿真平臺上面實(shí)現(xiàn)的。該平臺是基于C++語言開發(fā)的一個(gè)機(jī)器人仿真平臺，地圖數(shù)據(jù)是本校的俯瞰圖，可以看到地圖的每一條道路和每一個(gè)路口地點(diǎn)，平臺上有機(jī)器人在地圖中真實(shí)的行走路徑，也有機(jī)器人行走時(shí)創(chuàng)建的理論地圖。每一個(gè)機(jī)器人都有自己的一個(gè)里程計(jì)信息。

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

4.2 創(chuàng)建地圖及閉環(huán)檢測

在創(chuàng)建拓?fù)涞貓D時(shí)，機(jī)器人每到達(dá)一個(gè)節(jié)點(diǎn)，就進(jìn)行閉合檢測。閉環(huán)的方法很多［18～19］，但本文中的閉環(huán)利用到了相鄰的兩個(gè)節(jié)點(diǎn)之間里程計(jì)的誤差范圍大小，根據(jù)當(dāng)前節(jié)點(diǎn)的位置，來推斷下一個(gè)節(jié)點(diǎn)所在的范圍，通過位姿的偏差范圍、分支個(gè)數(shù)以及當(dāng)前位姿相對于各個(gè)分支偏角的范圍來進(jìn)行判斷是否為之前訪問過的節(jié)點(diǎn)。如果確認(rèn)閉環(huán)，則更新回環(huán)中的所有節(jié)點(diǎn)信息；否則將該節(jié)點(diǎn)放進(jìn)自己的局部地圖庫中，繼續(xù)漫游。

4.3 實(shí)驗(yàn)仿真

在實(shí)驗(yàn)過程中，采取在線地圖優(yōu)化的一個(gè)策略。在試驗(yàn)中，對創(chuàng)建的拓?fù)涞貓D進(jìn)行了優(yōu)化，使用到了兩種機(jī)械臂路徑規(guī)劃的算法思想來進(jìn)行對比。一種是根據(jù)Cyclic Coordinate Descent（CCD）的啟發(fā)式迭代搜索算法［20］，通過每次只改變一個(gè)關(guān)節(jié)的參數(shù)來減少誤差，從該拓?fù)涞貓D的末端開始調(diào)整優(yōu)化，直到收斂或者誤差到達(dá)最小值。另外一種就是本文中提出來的基于機(jī)械臂逆運(yùn)動學(xué)方法的拓?fù)涞貓D優(yōu)化。結(jié)果如圖2、圖3所示。

圖2 拓?fù)涞貓D優(yōu)化對比（一）

圖3 拓?fù)涞貓D優(yōu)化對比（二）

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

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

本文中創(chuàng)建了兩個(gè)地圖，并且在機(jī)器人創(chuàng)建地圖的同時(shí)，對到達(dá)的路口節(jié)點(diǎn)進(jìn)行閉環(huán)檢測，然后對環(huán)中的所有節(jié)點(diǎn)進(jìn)行優(yōu)化調(diào)整。可以看出，對于實(shí)際創(chuàng)建的拓?fù)涞貓D，誤差的積累越來越大。在只使用里程計(jì)的情況下，利用本文提出來的機(jī)械臂優(yōu)化調(diào)整方法，可以調(diào)整優(yōu)化已閉環(huán)的拓?fù)涞貓D接近真實(shí)的拓?fù)涞貓D結(jié)構(gòu)。

4.4 機(jī)械臂方法各個(gè)位姿節(jié)點(diǎn)角度調(diào)整對比

圖5表示在一個(gè)有十個(gè)位姿節(jié)點(diǎn)閉環(huán)的拓?fù)涞貓D中，每條路徑調(diào)整的弧度大小。可以看出來，在調(diào)整過程中，雖然結(jié)果可能差不多，但是本文方法在兩個(gè)路口位姿節(jié)點(diǎn)之間的路徑調(diào)整的幅度會比另外一種方法更小，并且調(diào)整的誤差也會更加平均。

圖5 十個(gè)位姿節(jié)點(diǎn)的閉環(huán)調(diào)整角度大小

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

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

圖6 模擬退火變步長對比

5 結(jié)語

機(jī)器人的地圖創(chuàng)建和優(yōu)化是SLAM系統(tǒng)中重要的一環(huán)，特別是在閉環(huán)之后，誤差調(diào)整更為重要。對于單里程計(jì)的SLAM系統(tǒng)，里程計(jì)漂移造成了地圖創(chuàng)建偏差，本文針對該問題，提出了一種新的基于圖優(yōu)化方法的拓?fù)涞貓D創(chuàng)建，可以很好地解決地圖中各個(gè)節(jié)點(diǎn)之間的位姿誤差問題，能夠更好地將積累的誤差平均的分?jǐn)偟交丨h(huán)中的各個(gè)節(jié)點(diǎn)中，生成一個(gè)較為準(zhǔn)確的地圖。實(shí)驗(yàn)結(jié)果證明了該算法是有效可行的。

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