本發(fā)明屬于機(jī)器人技術(shù)領(lǐng)域，具體涉及一種基于xtion設(shè)備的多功能六足機(jī)器人遠(yuǎn)程控制系統(tǒng)。

背景技術(shù)：

六足機(jī)器人地形適用性較強(qiáng)，可用于復(fù)雜地形，適用范圍廣，可用于星球探測、災(zāi)后救援等領(lǐng)域，除此之外，還可用于家庭服務(wù)，如照顧老人、殘疾人?？傊?，六足機(jī)器人在生產(chǎn)、生活中有廣泛應(yīng)用。

六足機(jī)器人完成較復(fù)雜任務(wù)，其控制系統(tǒng)起重要作用，然而，現(xiàn)有操作系統(tǒng)對各功能的集成度不高，導(dǎo)致六足機(jī)器人的功能有限，無法完成復(fù)雜任務(wù)。如，現(xiàn)有六足機(jī)器人未將slam(simultaneouslocalizationandmapping，同時定位及地圖創(chuàng)建)、動作編輯、人機(jī)交互、承載能力等功能集合起來，從而使機(jī)器人不能完成某些復(fù)雜任務(wù)。另外，某些機(jī)器人操作系統(tǒng)雖然能完成一定較復(fù)雜任務(wù)，但使用特定控制系統(tǒng)，通用性差。

目前腿式機(jī)器人slam面臨實時性差，建圖精度低的缺點(diǎn)，并且，區(qū)別于輪式機(jī)器人，六足腿式機(jī)器人無法用輪式里程計計算自身位移，以輔助定位。因此，本發(fā)明中的slam模塊設(shè)計了基于多傳感器信息融合的slam系統(tǒng)，提高了感知的準(zhǔn)確性、快速性和可靠性。

技術(shù)實現(xiàn)要素：

本發(fā)明提供一種基于xtion設(shè)備的多功能六足機(jī)器人的遠(yuǎn)程控制系統(tǒng)，將六足機(jī)器人的slam、動作編輯、手勢識別和語音識別的功能集合在一起，增加了六足機(jī)器人的功能。為了提高系統(tǒng)的通用性，本發(fā)明基于ros(robotoperatingsystem,ros)，同時，將手勢識別、語音識別、動作編輯和slam結(jié)合在一起，以便更好完成任務(wù)。

一種基于xtion設(shè)備的多功能六足機(jī)器人的遠(yuǎn)程控制系統(tǒng)，其特征在于：包括無線網(wǎng)絡(luò)模塊、運(yùn)動控制單元、動作編輯模塊、slam模塊、手勢識別單元和語音識別模塊。

所述無線網(wǎng)絡(luò)模塊實現(xiàn)遠(yuǎn)程控制系統(tǒng)與六足機(jī)器人的控制主板通過pci-e接口通訊，節(jié)進(jìn)而實現(xiàn)遠(yuǎn)程控制系統(tǒng)對六足機(jī)器人的控制。

運(yùn)動控制單元包括運(yùn)動模式控制單元與舵機(jī)控制單元；其中，運(yùn)動模式控制單元用來控制六足機(jī)器人的運(yùn)行模式，包括輪行運(yùn)動、腿式行走和輪腿切換；舵機(jī)控制單元用來控制各個驅(qū)動舵機(jī)上電，進(jìn)而控制六足機(jī)器人開始行走、停止和運(yùn)動方向。

所述動作編輯模塊用來編輯六足機(jī)器人的運(yùn)動動作，包括機(jī)器人模型顯示子模塊、動作設(shè)定子模塊與操作子模塊與動作編輯界面；其中，機(jī)器人模型顯示子模塊用來實時顯示由機(jī)器人運(yùn)動狀態(tài)；動作設(shè)定子模塊包括條腿足底三軸方向位移設(shè)定、繞髖關(guān)節(jié)、膝關(guān)節(jié)和踝關(guān)節(jié)的旋轉(zhuǎn)角度設(shè)定與分段時間設(shè)定，實現(xiàn)對六足機(jī)器人的動作編輯；操作子模塊包括姿勢預(yù)覽操作、重置操作、開始控制線程操作、結(jié)束控制線程操作、添加姿勢操作、刪除姿勢操作、角度到姿勢轉(zhuǎn)換計算操作、生成子軌跡操作、生成軌跡操作、運(yùn)行軌跡操作、保存軌跡操作、刪除軌跡操作與下載軌跡操作。

所述手勢識別單元包括手勢控制單元與運(yùn)動控制單元，實現(xiàn)對腿輪復(fù)合機(jī)器人的手勢控制。

所述語音識別模塊中預(yù)定義有語音指令及與機(jī)器人動作的映射關(guān)系，通過語音控制六足機(jī)器人前進(jìn)、后退、左轉(zhuǎn)、右轉(zhuǎn)、停止、原地順時針旋轉(zhuǎn)和原地逆時針旋轉(zhuǎn)運(yùn)動。

所述的slam模塊用于六足機(jī)器人定位和創(chuàng)建環(huán)境地圖；slam模塊包括slam模型子模塊、二維slam子模塊和三維slam子模塊。

所述slam模型子模塊使用xtionprolive作為視覺傳感器，采集的環(huán)境信息作為主要信息源，以imu的慣性信息作為補(bǔ)償，建立機(jī)器人的運(yùn)動模型和相機(jī)模型，進(jìn)而設(shè)計多傳感器信息融合模型。

二維slam子模塊和三維slam子模塊均以機(jī)器人運(yùn)動模型、相機(jī)模型以及多傳感器信息融合模型為基礎(chǔ)，使用二維slam算法和三維slam算法構(gòu)建分別進(jìn)行二維slam和三維slam。

所述二維slam算法中，令機(jī)器人感知到的環(huán)境障礙物表面均與地面垂直，由此通過平面柵格地圖表示環(huán)境信息，采用二維激光掃描儀數(shù)據(jù)重建環(huán)境中物體表面，然后通過匹配算法將單次掃描結(jié)果依次拼接、融合，形成整個環(huán)境的地圖。

所述三維slam算法通過將兩幀二維圖像提取特征并匹配的方式映射到三維點(diǎn)集，據(jù)此計算運(yùn)動變換；然后采用圖優(yōu)化模型對估計的整體軌跡進(jìn)行優(yōu)化，輸出最終的地圖。

本發(fā)明的優(yōu)點(diǎn)在于：

1、本發(fā)明基于xtion設(shè)備的多功能六足機(jī)器人遠(yuǎn)程控制系統(tǒng)，基于ros，可移植性強(qiáng)；在六足機(jī)器人平臺集成了無線網(wǎng)絡(luò)連接、slam、動作編輯器、手勢識別和語音識別功能，使六足機(jī)器人能夠更好完成任務(wù)。

2、本發(fā)明基于xtion設(shè)備的多功能六足機(jī)器人遠(yuǎn)程控制系統(tǒng)，遠(yuǎn)程控制界面將無線網(wǎng)絡(luò)連接、slam、六足機(jī)器人動作編輯、手勢識別和語音識別集合在一起，便于操作，易于控制六足機(jī)器人。

3、本發(fā)明基于xtion設(shè)備的多功能六足機(jī)器人遠(yuǎn)程控制系統(tǒng)中，slam模塊基于ros，且綜合考慮處理速度和slam精度，使用基于imu和rgb-d相機(jī)的slam算法；且slam模塊不僅能用于六足機(jī)器人，還能用于四足機(jī)器人、輪式機(jī)器人、履帶式機(jī)器人等裝有微軟kinect或xtionprolive攝像頭的其他機(jī)器人。

5、本發(fā)明基于xtion設(shè)備的多功能六足機(jī)器人遠(yuǎn)程控制系統(tǒng)，設(shè)計了一種融合imu和rgbd多傳感器信息的slam算法，提高了rgbd攝像頭slam的精度。

6、本發(fā)明基于xtion設(shè)備的多功能六足機(jī)器人遠(yuǎn)程控制系統(tǒng)，包含二維slam和三維slam，用戶可根據(jù)需要選擇合適類型。

7、本發(fā)明基于xtion設(shè)備的多功能六足機(jī)器人遠(yuǎn)程控制系統(tǒng)中，動作編輯器模塊通過界面編輯六足機(jī)器人的動作，避免了復(fù)雜的編程操作，具有簡單直觀、操作方便的優(yōu)點(diǎn)，增強(qiáng)了交互的自然性。

8、本發(fā)明基于xtion設(shè)備的多功能六足機(jī)器人遠(yuǎn)程控制系統(tǒng)中，手勢識別單元可移植性強(qiáng)；手勢識別單元基于ros，手勢識別基于openni框架設(shè)計實現(xiàn)，可用于其他機(jī)器人控制系統(tǒng)。

9、本發(fā)明基于xtion設(shè)備的多功能六足機(jī)器人遠(yuǎn)程控制系統(tǒng)中，手勢識別單元使用手勢控制六足機(jī)器人，提高了人與六足機(jī)器人交互的自然性，符合人機(jī)交互的發(fā)展趨勢。

10、本發(fā)明基于xtion設(shè)備的多功能六足機(jī)器人遠(yuǎn)程控制系統(tǒng)中，語音識別模塊基于pocketsphinx，使用語音控制六足機(jī)器人運(yùn)動。

11、本發(fā)明基于xtion設(shè)備的多功能六足機(jī)器人遠(yuǎn)程控制系統(tǒng)中，語音識別模塊和手勢識別單元互相補(bǔ)充，用戶可根據(jù)實際情況，選擇手勢或語音控制六足機(jī)器人運(yùn)動。手勢和語音兩種交互模式互相補(bǔ)充，既增強(qiáng)了人與機(jī)器人交互的自然性，又提高了六足機(jī)器人的適用范圍。

附圖說明

圖1為本發(fā)明基于xtion設(shè)備的多功能六足機(jī)器人的遠(yuǎn)程控制系統(tǒng)所針對的六足機(jī)器人結(jié)構(gòu)示意圖；

圖2為本發(fā)明基于xtion設(shè)備的多功能六足機(jī)器人的遠(yuǎn)程控制系統(tǒng)所針對的六足機(jī)器人控制系統(tǒng)結(jié)構(gòu)框圖；

圖3為本發(fā)明基于xtion設(shè)備的多功能六足機(jī)器人的遠(yuǎn)程控制系統(tǒng)結(jié)構(gòu)框圖；

圖4為本發(fā)明基于xtion設(shè)備的多功能六足機(jī)器人的遠(yuǎn)程控制系統(tǒng)中slam模塊框圖；

圖5為本發(fā)明基于xtion設(shè)備的多功能六足機(jī)器人的遠(yuǎn)程控制系統(tǒng)中三維slam算法流程圖；

圖6為本發(fā)明基于xtion設(shè)備的多功能六足機(jī)器人的遠(yuǎn)程控制系統(tǒng)中三維slam算法中所應(yīng)用的位姿變換原理圖。

1-本體2-髖關(guān)節(jié)3-膝關(guān)節(jié)

4-踝關(guān)節(jié)5-輪子

具體實施方式

下面結(jié)合附圖和實施實例對本發(fā)明進(jìn)行詳細(xì)說明。

本發(fā)明一種基于xtion設(shè)備的多功能六足機(jī)器人的遠(yuǎn)程控制系統(tǒng)，用于實現(xiàn)搭載有xtion設(shè)備的六足機(jī)器人定位、導(dǎo)航、動作設(shè)計、手勢識別控制、語音識別控制，以及六足機(jī)器人的無線連接。

上述六足機(jī)器人本體1周向均布六條腿，每條腿有三個關(guān)節(jié)，即：髖關(guān)節(jié)2、膝關(guān)節(jié)3和踝關(guān)節(jié)4，各關(guān)節(jié)由驅(qū)動舵機(jī)驅(qū)動轉(zhuǎn)動，如圖1所示；且六足機(jī)器人腿部裝有輪子5，在行進(jìn)過程中可通過對各關(guān)節(jié)的控制，使六足機(jī)器人在移動過程中，可根據(jù)地形進(jìn)行輪行與腿行間的切換，具有很好的地形適應(yīng)性。本發(fā)明中六足機(jī)器人分層搭載有xtionprolive攝像頭、imu(慣性導(dǎo)航單元)、機(jī)載系統(tǒng)、舵機(jī)驅(qū)動板與電池盒。如圖2所示，攝像頭用來采集環(huán)境信息，并且用來獲取人體的信息，用于人機(jī)交互。攝像頭包含一個rgb相機(jī)及一個深度相機(jī)；且攝像頭兩側(cè)的麥克組成麥克陣列，能夠有效地獲取環(huán)境中的聲音信息，可用于實現(xiàn)語音控制等自然人機(jī)交互方式。imu用于獲得六足機(jī)器人的自身姿態(tài)，輔助定位和創(chuàng)建環(huán)境地圖。舵機(jī)驅(qū)動板用于控制六足機(jī)器人的6條腿各關(guān)節(jié)對應(yīng)的驅(qū)動舵機(jī)運(yùn)動。機(jī)載系統(tǒng)用于用戶管理、信息采集、地形構(gòu)建、任務(wù)管理、路徑規(guī)劃和處理傳感器信息；機(jī)載系統(tǒng)中的控制主板為控制核心，與攝像頭間通過usb接口通訊；與imu通過rs232通訊；與舵機(jī)驅(qū)動板通過i2c接口通訊，可實現(xiàn)對攝像頭與imu的數(shù)據(jù)采集及數(shù)據(jù)處理、以及對舵機(jī)驅(qū)動板的管理。電池盒內(nèi)置一塊12v鋰電池和一塊7.4v鋰電池，用于為六足機(jī)器人供電。

針對上述六足機(jī)器人，本發(fā)明設(shè)計的遠(yuǎn)程控制系統(tǒng)采用分布式架構(gòu)，包括無線網(wǎng)絡(luò)模塊、運(yùn)動控制單元、動作編輯模塊、slam模塊、手勢識別單元和語音識別模塊，如圖3所示。

所述無線網(wǎng)絡(luò)模塊用來實現(xiàn)六足機(jī)器人與六足機(jī)器人的控制主板通過pci-e接口通訊，實現(xiàn)遠(yuǎn)程控制系統(tǒng)對六足機(jī)器人的控制。

運(yùn)動控制單元包括運(yùn)動模式控制單元與舵機(jī)控制單元。其中，運(yùn)動模式控制單元用來控制六足機(jī)器人的運(yùn)行模式，包括輪行運(yùn)動、腿式行走和輪腿切換。舵機(jī)控制單元用來控制各個驅(qū)動舵機(jī)上電，進(jìn)而控制六足機(jī)器人開始行走、停止和運(yùn)動方向。

所述動作編輯模塊用來通過gui界面編輯六足機(jī)器人的運(yùn)動動作，包括機(jī)器人模型顯示子模塊、動作設(shè)定子模塊與操作子模塊與動作編輯界面，如圖2所示。

其中，機(jī)器人模型顯示子模塊通過ros(機(jī)器人操作系統(tǒng))中的rviz(三維可視化模擬工具)生成，用來實時顯示由機(jī)器人運(yùn)動狀態(tài)。

如圖2所示，動作設(shè)定子模塊包括6條腿足底位移(x、y、z三軸方向位移)設(shè)定、繞髖關(guān)節(jié)、膝關(guān)節(jié)和踝關(guān)節(jié)的旋轉(zhuǎn)角度設(shè)定與分段時間設(shè)定，實現(xiàn)對六足機(jī)器人的動作編輯。分段時間設(shè)定用于子軌跡的步長時間設(shè)定，不同子軌跡可設(shè)置為不同的分段時間。如圖2中的左上角的(0,63.5,-45.9,0,0,-100)分別為第一條腿的足底位移和繞髖關(guān)節(jié)、膝關(guān)節(jié)和踝關(guān)節(jié)的旋轉(zhuǎn)角度，其中0、0、-100分別為第一條腿x、y、z三軸的位移。0、63.5、-45.9分別為第一條腿繞髖關(guān)節(jié)、膝關(guān)節(jié)和踝關(guān)節(jié)的旋轉(zhuǎn)角度。

操作子模塊包括姿勢預(yù)覽操作、重置操作、開始控制線程操作、結(jié)束控制線程操作、添加姿勢操作、刪除姿勢操作、角度到姿勢轉(zhuǎn)換計算操作、生成子軌跡操作、生成軌跡操作、運(yùn)行軌跡操作、保存軌跡操作、刪除軌跡操作與下載軌跡操作，

其中，姿勢預(yù)覽操作用來使機(jī)器人模型顯示部分中顯示動作設(shè)定部分內(nèi)編輯好的動作。在用戶未編輯動作時，通過姿勢預(yù)覽操作可使機(jī)器人模型顯示部分顯示預(yù)設(shè)的六足機(jī)器人初始狀態(tài)(六足站立)。在用戶進(jìn)行姿勢預(yù)覽操作時，若無法正常在機(jī)器人模型顯示部分中顯示，則可通過重置操作，使機(jī)器人模型顯示部分顯示六足機(jī)器人初始狀態(tài)。

開始控制線程操作用來關(guān)聯(lián)動作設(shè)定子模塊和機(jī)器人模型顯示單元，使用戶可以通過動作設(shè)定子模塊為六足機(jī)器人編輯動作。若進(jìn)行重置操作后，無法正常在六足機(jī)器人模型顯示單元顯示六足機(jī)器人初始狀態(tài)，表明動作設(shè)定子模塊與機(jī)器人模型顯示子模塊未關(guān)聯(lián)，則通過開始控制線程操作進(jìn)行關(guān)聯(lián)即可。

結(jié)束控制線程操作用來結(jié)束動作設(shè)定子模塊和機(jī)器人模型顯示子模塊間的關(guān)聯(lián)；此時，用戶無法通過動作設(shè)定子模塊對六足機(jī)器人編輯動作。

添加姿勢操作用來生成動作設(shè)定子模塊中設(shè)定的六足機(jī)器人動作，且可為所設(shè)定的動作命名。

刪除姿勢操作用來刪除由添加姿勢單元所生成的機(jī)器人動作。

角度到姿勢轉(zhuǎn)換計算操作用來根據(jù)用戶在動作設(shè)定子模塊中設(shè)定的機(jī)器人某一關(guān)節(jié)角度計算機(jī)器人模型的姿勢，并在機(jī)器人模型顯示單元中進(jìn)行實時顯示。

生成子軌跡操作用來將添加姿勢操作所生成的動作生產(chǎn)一段動作序列。

生成軌跡單元用來將生成上述全部動作序列的整體軌跡。

運(yùn)行軌跡單元用來通過機(jī)器人模型顯示子模塊按照生成軌跡單元生成的整體軌跡，顯示出六足機(jī)器人的實際運(yùn)動。

保存軌跡單元，用來保存生成的整體軌跡。

刪除軌跡單元，用來刪除一段動作序列。

下載軌跡單元，用來載入之前保存的軌跡。

用戶可通過動作編輯界面查看編輯好的動作，且只需要點(diǎn)擊動作編輯器界面中編輯的動作，便可通過六足機(jī)器人模型實時看到相應(yīng)動作。

所述手勢識別單元包括手勢控制單元與運(yùn)動控制單元，實現(xiàn)對腿輪復(fù)合機(jī)器人的手勢控制。本發(fā)明中手勢識別單元基于linuxubuntu12.04上安裝的機(jī)器人操作系統(tǒng)(robotoperatingsystem,ros)的hydro版本設(shè)計，將手勢識別單元與運(yùn)動控制單元抽象為整個機(jī)器人操作系統(tǒng)中的不同節(jié)點(diǎn)，則手勢識別單元與運(yùn)動控制單元間的通信可看作不同結(jié)點(diǎn)間通信，故模塊間以ros(機(jī)器人操作系統(tǒng))中topic(話題)的形式通信，通信的內(nèi)容即為topic中的message(消息)。

上述手勢識別單元包括動態(tài)手勢識別子模塊和靜態(tài)手勢識別子模塊，用戶可根據(jù)需要選擇適當(dāng)?shù)氖謩葑R別方法，其中，動態(tài)手勢識別基于openni(opennaturalinteraction)軟件設(shè)計，用于實現(xiàn)動態(tài)手勢識別；openni為開發(fā)者提供一個多語言、跨平臺的框架，并提供其自然交互的api接口。openniapi(applicationprogramminginterface，應(yīng)用程序編程接口)便是一組編寫自然交互應(yīng)用的接口。openni的主要目的是形成一個標(biāo)準(zhǔn)應(yīng)用程序編程接口，以此來搭建視覺和音頻傳感器與視覺和音頻感知中間件通信的橋梁。nite是openni的中間層，主要用來實現(xiàn)手勢識別。nite主要通過手勢檢測器來實現(xiàn)，這些檢測器在nite程序中以類的形式出現(xiàn)，用來實現(xiàn)特定手勢的識別。本發(fā)明中的動態(tài)手勢識別子模塊基于nite設(shè)計實現(xiàn)，所使用手勢檢測器包括穩(wěn)定檢測器(steadydetector)、畫圓檢測器(circledetector)、推檢測器(pushdetector)、移動檢測器(swipedetector)。其中，穩(wěn)定檢測器用于識別當(dāng)前活躍的手部是否處于穩(wěn)定狀態(tài)，畫圓檢測器用于識別當(dāng)前活躍手部的畫圓運(yùn)動，推檢測器用于識別當(dāng)前活躍手的前推動作(即，手部垂直于身體前推)，移動檢測器用于識別當(dāng)前活躍手的擊打動作(即，手沿身體正面的平面運(yùn)動)。

在進(jìn)行手勢識別時，首先通過xtionprolive攝像頭采集動態(tài)手勢的rgb圖像信息；隨后基于openni軟件對采集的圖像進(jìn)行處理，提取手勢的運(yùn)動軌跡，并通過穩(wěn)定檢測器、畫圓檢測器、推檢測器、移動檢測器識別手靜止不動、畫圓、推和移動的手勢；隨后穩(wěn)定檢測器、畫圓檢測器、推檢測器、移動檢測器將各自的回調(diào)函數(shù)中輸出識別結(jié)果，并在機(jī)器人操作系統(tǒng)中以message的形式將識別結(jié)果發(fā)送至運(yùn)動控制單元。運(yùn)動控制單元根據(jù)識別結(jié)果，結(jié)合動態(tài)手勢識別單元內(nèi)預(yù)定義的動態(tài)手勢與機(jī)器人運(yùn)動的映射關(guān)系，控制機(jī)器人完成相應(yīng)的運(yùn)動。上述動態(tài)識別子模塊使用范圍較廣，可在室內(nèi)光線不好的情況下時使用；

所述靜態(tài)手勢識別子模塊基于彩色圖像，用于識別靜態(tài)手勢。通過靜態(tài)手勢識別子模塊，在進(jìn)行手勢識別時；首先通過xtionprolive攝像頭采集人體的rgb圖像信息，該圖像信息通過機(jī)器人操作系統(tǒng)中topic中的message發(fā)送至靜態(tài)手勢識別子模塊；隨后，利用靜態(tài)手勢識別子模塊內(nèi)預(yù)存的膚色模型和hog特征檢測出人臉部分，進(jìn)而根據(jù)人體結(jié)構(gòu)特征，檢測出人的身體部分，并由身體部分中分割出人的手部區(qū)域，最終提取已分割手部區(qū)域的圖像特征，進(jìn)行手勢的訓(xùn)練識別，并將識別結(jié)果發(fā)送運(yùn)動控制單元，根據(jù)識別結(jié)果，結(jié)合預(yù)存于的靜態(tài)手勢與機(jī)器人運(yùn)動的映射關(guān)系，控制機(jī)器人完成相應(yīng)的運(yùn)動。上述靜態(tài)手勢識別子模塊可在室內(nèi)或室外光照好的情況下使用。

所述語音識別模塊中預(yù)定義有語音指令及與機(jī)器人動作的映射關(guān)系，由此通過華碩攝像頭xtionprolive，基于ros，基于pocketsphinx，可通過語音控制六足機(jī)器人前進(jìn)、后退、左轉(zhuǎn)、右轉(zhuǎn)、停止、原地順時針旋轉(zhuǎn)和原地逆時針旋轉(zhuǎn)運(yùn)動。

所述的slam(simutaneouslocalizationandmapping，同步定位與地圖構(gòu)建系統(tǒng))模塊采用分布式架構(gòu)設(shè)計，用于六足機(jī)器人定位和創(chuàng)建環(huán)境地圖。本發(fā)明中設(shè)計slam模塊以六足機(jī)器人機(jī)載系統(tǒng)為核心，與遠(yuǎn)程pc客戶端間通過無線網(wǎng)絡(luò)通訊，實現(xiàn)人機(jī)交互，遠(yuǎn)程pc客戶端還承擔(dān)部分運(yùn)算任務(wù)。slam模塊中相機(jī)數(shù)據(jù)獲取、imu數(shù)據(jù)采集、數(shù)據(jù)融合處理、slam程序均在六足機(jī)器人機(jī)載系統(tǒng)中運(yùn)行。

本發(fā)明中結(jié)合六足機(jī)器人的特點(diǎn)，在已有算法基礎(chǔ)上，探索了多傳感器融合算法，設(shè)計slam模塊包括slam模型子模塊、二維slam子模塊和三維slam子模塊，如圖4所示。其中，slam模型子模塊采用了視覺傳感器和慣性導(dǎo)航單元，建立了混合式傳感器融合模型。

所述slam模型子模塊使用體感設(shè)備華碩xtionprolive作為視覺傳感器，采集的環(huán)境信息作為主要信息源，以imu的慣性信息作為補(bǔ)償，建立機(jī)器人的運(yùn)動模型和相機(jī)模型，進(jìn)而設(shè)計多傳感器信息融合模型。其中，機(jī)器人運(yùn)動模型用于建立機(jī)器人坐標(biāo)系與世界坐標(biāo)系的聯(lián)系；相機(jī)模型用于建立任意選取的點(diǎn)在世界坐標(biāo)系中的坐標(biāo)與圖像坐標(biāo)系中坐標(biāo)的變換關(guān)系，并進(jìn)行三維重建；本發(fā)明采用針孔相機(jī)模型建立rgb相機(jī)和紅外相機(jī)的數(shù)學(xué)模型。

二維slam子模塊和三維slam子模塊均以機(jī)器人運(yùn)動模型、相機(jī)模型以及多傳感器信息融合模型為基礎(chǔ)，使用二維slam算法和三維slam算法構(gòu)建分別進(jìn)行2維slam和3維slam，在保證一定精度的前提下，提高了實時性，滿足六足機(jī)器人的需要。

所述二維slam算法對環(huán)境進(jìn)行簡化，適用于簡單結(jié)構(gòu)化環(huán)境，如樓宇通道，具有計算量小、實時性好的特點(diǎn)，得到的是線特征。二維slam算法中，令機(jī)器人感知到的環(huán)境障礙物表面均與地面垂直，由此可通過平面柵格地圖即可表示環(huán)境信息。本發(fā)明中二維slam算法的采用二維激光掃描儀數(shù)據(jù)重建環(huán)境中物體表面，然后通過匹配算法將單次掃描結(jié)果依次拼接、融合起來，形成整個環(huán)境的地圖，具體方法如下：

(1)獲取深度數(shù)據(jù)；

根據(jù)相機(jī)模型，由深度相機(jī)進(jìn)行深度數(shù)據(jù)測量，則深度相機(jī)每次采集的深度數(shù)據(jù)為image＝{ii,j}，1≤i≤w,1≤j≤h；其中，w與h分別為深度圖像的寬度和高度；ii,j為第i行j列的圖像中像素深度值；

(2)虛擬二維激光；

根據(jù)步驟a中得到的深度數(shù)據(jù)結(jié)合相機(jī)模型可得到，虛擬的二維激光掃描數(shù)據(jù)源laser，

laser＝{lm|lm＝ii,jk^-1(i,j1)^t},1≤i≤w(1)

其中，lm為第m個虛擬二維激光掃描數(shù)據(jù)源。利用極坐標(biāo)轉(zhuǎn)換將laser轉(zhuǎn)換到極坐標(biāo)空間即可得到常見形式的二維激光掃描數(shù)據(jù)：

其中，xm、ym分別為lm在x、y軸上的分量；r為虛擬二維激光掃描數(shù)據(jù)源的掃描半徑；θ為設(shè)定的掃描角度。

(3)利用機(jī)器人運(yùn)動模型，通過基于rao-blackwellized的重要性重采樣粒子濾波算法進(jìn)行位姿求解和地圖計算，得到當(dāng)前機(jī)器人位姿條件下的地圖。所述基于rao-blackwellized的重要性重采樣粒子濾波算法中，每個粒子均維護(hù)一張地圖，且每個粒子中使用n個ekf維護(hù)路標(biāo)，粒子中包含了機(jī)器人位姿(x,y,θ)以及環(huán)境u；具體方法如下：

a、采樣；

從提議分布π(xt|z1:t,u1:t)中采樣，結(jié)合t-1時刻n個粒子的粒子集得到新的t時刻n個粒子的粒子集其中，xt為t時刻更新得到的位姿；z1:t為從開始到時刻t的imu采集的姿態(tài)數(shù)據(jù)；u1:t從開始到時刻t采用的ekf維護(hù)的環(huán)境特征采用的是ekf維護(hù)的環(huán)境特征。

b、通過基于rao-blackwellized的重要性重采樣粒子濾波算法將聯(lián)合概率分布分解成自身位姿和該位姿條件下的地圖，得到：

p(x1:t,m|z1:t,u1:t)＝p(m|x1:t,z1:t)p(x1:t|z1:t,u1:t)(4)

x1:t為從開始到t時刻機(jī)器人的軌跡；m為移動機(jī)器人地圖；p(x1:t,m|z1:t,u0:t)為機(jī)器人軌跡x1:t和地圖m的后驗概率；p(x1:t|z1:t,u0:t)為機(jī)器人軌跡x1:t的后驗概率；p(m|x1:t,z1:t)為地圖m的后驗概率。

(4)計算重要性權(quán)重系數(shù)。

根據(jù)下式計算每個粒子的重要性權(quán)重系數(shù)wⁿ。

由于提議分布與真實分布一般不一致，因此wⁿ≠1；重要性權(quán)重系數(shù)評價了該粒子所維護(hù)地圖的優(yōu)劣；

(5)重采樣，即用具有較高重要性權(quán)重系數(shù)的粒子替代重要性權(quán)重系數(shù)較小的粒子；

(6)地圖估計。

對于每個粒子來說，可用其軌跡和觀測信息z1:t計算相應(yīng)的從而對地圖進(jìn)行更新，至此完成二維地圖的創(chuàng)建。

雖然通過二維slam算法建立的二維地圖存在一定優(yōu)點(diǎn)，但多數(shù)情況無法有效描述環(huán)境信息，而本發(fā)明中三維slam子模塊在surf(speededuprobustfeatures，加速魯棒特征)匹配的基礎(chǔ)上，加入閉環(huán)檢測，利用rgb數(shù)據(jù)和深度數(shù)據(jù)得到相鄰兩幀的三維點(diǎn)集，通過icp算法進(jìn)行位姿估計，最后在后端采用圖優(yōu)化模型對位姿軌跡進(jìn)行優(yōu)化，輸出最終的地圖。

三維slam算法通過將兩幀二維圖像提取特征并匹配的方式映射到三維點(diǎn)集，據(jù)此計算運(yùn)動變換。然后采用圖優(yōu)化模型對估計的整體軌跡進(jìn)行優(yōu)化，輸出最終的地圖。三維slam算法分為前端與后端。前端部分通過對rgb-d相機(jī)數(shù)據(jù)和imu進(jìn)行數(shù)據(jù)融合形成visual-imuodometry子模塊，由該子模塊經(jīng)運(yùn)動變換，可得到機(jī)器人的位姿及其變換矩陣；后端采用圖優(yōu)化模型對建立的地圖和位姿矩陣進(jìn)行優(yōu)化，提高建圖精度及可靠性。最后在優(yōu)化結(jié)果的基礎(chǔ)上，重建地圖，。具體方法如下:

1)使用surf(speededuprobustfeatures)特征提取與匹配算法進(jìn)行特征提取和幀間特征匹配。

由于存在一定的誤匹配，為了提高后續(xù)處理速度，采用歐氏距離對得到的匹配進(jìn)行篩選，對匹配距離過大的匹配剔除。匹配剔除原則如下式：

dis(matchs)＞k·min(dis(matchs))(8)

其中dis表示歐氏距離；k為閾值，matchs為第s幀圖像匹配。上式將大于最優(yōu)匹配距離k倍的匹配剔除。依次提取一定數(shù)量的特征匹配作為最終匹配。

2)將步驟1)中得到的最終匹配結(jié)果與深度數(shù)據(jù)利用icp算法(iterativeclosestpoint迭代最近點(diǎn))求解位姿變換矩陣，通過兩個互斥通道進(jìn)行位姿融合。

當(dāng)環(huán)境本身存在較多紅外光或環(huán)境表面反射效應(yīng)強(qiáng)烈都會造成深度相機(jī)數(shù)據(jù)丟失。需要采用imu數(shù)據(jù)進(jìn)行融合。imu姿態(tài)數(shù)據(jù)通過互斥通道a、b進(jìn)行融合，即同一時間只存在a通道或b通道。其中a通道頻率高，b通道頻率低，兼顧了速度和精度。b通道使用的是拓展卡爾曼濾波(ekf)。

將位姿變換作為系統(tǒng)狀態(tài)量進(jìn)行傳感器信息融合。由貝葉斯濾波得：

根據(jù)感知模型p(ot|xt)、運(yùn)動模型p(xt|xt-1,vt-1)、機(jī)器人初始狀態(tài)bel(x0)即可遞歸求出機(jī)器人當(dāng)前狀態(tài)。式(9)中，ot為t時刻imu測量的數(shù)據(jù)；vt-1為前一時刻體感攝像頭xtionprolive采集的數(shù)據(jù)；h為常數(shù)，h＝p(ot|vt-1，ot-1，vt-2，...,o0)^-1。

將位姿變換作為系統(tǒng)狀態(tài)量進(jìn)行傳感器信息融合。在式(9)中，將視覺里程計得到的t作為vt，imu測量的姿態(tài)偏轉(zhuǎn)角作為觀測量ot，最終xt作為最終的位姿輸出。

由于機(jī)器人在運(yùn)動過程中可能會進(jìn)入環(huán)境中已經(jīng)創(chuàng)建地圖的部分，此時不適合直接匹配上一幀數(shù)據(jù)，因此需要做閉環(huán)檢測。

閉環(huán)檢測采用surf算法進(jìn)行匹配，匹配特征個數(shù)達(dá)到閾值即匹配成功。若匹配到已經(jīng)創(chuàng)建地圖的幀，則在其基礎(chǔ)上估計位姿，創(chuàng)建地圖；否則繼續(xù)匹配其他幀或接受新的數(shù)據(jù)幀。

在上述閉環(huán)檢測中，輸入為關(guān)鍵幀序列和待檢測幀。首先對末尾m幀進(jìn)行閉環(huán)檢測，若均未匹配成功，則進(jìn)行隨機(jī)匹配，在剩下的關(guān)鍵幀中隨機(jī)選取n幀進(jìn)行匹配。

3)后端圖優(yōu)化

后端采用圖優(yōu)化模型對建立的地圖和位姿矩陣進(jìn)行優(yōu)化，采用圖優(yōu)化庫g2o(generalgraphoptimization)對運(yùn)動估計進(jìn)行優(yōu)化。提高建圖精度及可靠性。最后在優(yōu)化結(jié)果的基礎(chǔ)上，重建環(huán)境地圖；本發(fā)明采用點(diǎn)云圖表示環(huán)境地圖。

對于生成的地圖，部分關(guān)鍵幀間只考慮了相鄰兩幀之間的位姿變換。由于采用依次疊加匹配，slam誤差累計將比較明顯，在大尺度環(huán)境下容易出現(xiàn)匹配失敗。

將機(jī)器人每幀數(shù)據(jù)對應(yīng)的位姿矩陣作為節(jié)點(diǎn)，幀之間對應(yīng)的位姿變換矩陣作為邊，建立slam圖優(yōu)化模型，對slam過程的位姿估計進(jìn)行優(yōu)化：

g＝(v,e)(10)

其中v＝{zt}為節(jié)點(diǎn)集合，其元素表示位姿矩陣；e＝{ett′}為邊集合，其元素表示位姿變換矩陣；如圖6所示，ett′表示從t到t′時刻節(jié)點(diǎn)z的變換矩陣，滿足以下條件：

zt′＝ett′zt′(11)

優(yōu)化模型的目標(biāo)函數(shù)為：

利用通用圖優(yōu)化框架g2o對建立的圖模型進(jìn)行優(yōu)化。g2o是一個優(yōu)化算法集，包含了如guassian-newton和levenberg-marquardt等常用算法。

將優(yōu)化后的關(guān)鍵幀fi加入全局地圖，完成對建立的地圖和位姿矩陣進(jìn)行優(yōu)化；

zt·ft→mmap(13)

上述二維slam計算量小、實時性好，但是環(huán)境的簡化，得到的是線特征，所建環(huán)境地圖為二維圖，但在多數(shù)應(yīng)用場景下，二維地圖無法有效描述環(huán)境特征，但三維slam相對速度較慢，二者各有優(yōu)缺點(diǎn)，用戶可根據(jù)需要選擇使用二維slam，還是三維slam。