本發(fā)明涉及一種基于3D立體感知技術(shù)的自主導(dǎo)航機器人系統(tǒng)，可使機器人在陌生環(huán)境中完成三維自主感知、三維目標(biāo)識別以及自主導(dǎo)航，屬于機器人技術(shù)領(lǐng)域。

背景技術(shù)：

近年來，國家在特殊環(huán)境下作業(yè)的機器人方面取得了一系列矚目的成果，國內(nèi)一些高校和研究機構(gòu)研制出了諸多危險環(huán)境作業(yè)機器人。上海交通大學(xué)等單位研制了國內(nèi)首臺ZXPJ01型消防機器人；北京航空航天大學(xué)開發(fā)了RT3-EOD、RAPTOR 排爆機器人；北京金吾高科技有限公司研制出JW901B型排爆機器人；中科院沈陽自動化所研制出“靈蜥”危險作業(yè)機器人；哈爾濱工業(yè)大學(xué)面向未來戰(zhàn)場需求和反恐作戰(zhàn)需要，研制了模塊化、多功能的地面無人作戰(zhàn)平臺；中科院沈陽自動化所研制了輪、腿和履帶復(fù)合型自主式移動機器人Climber等等。中科院沈陽自動化所研制出“靈蜥”危險作業(yè)機器人系列可以代替人進(jìn)行一定的工作，在大多數(shù)非結(jié)構(gòu)環(huán)境或危險、惡劣環(huán)境（環(huán)境狀況復(fù)雜多變，地面高低不平，存在坡、溝和障礙物）條件下實施反恐防暴作業(yè)。同時，該作業(yè)型機器人使用便捷的機械手（含工具）也是拓展其應(yīng)用范圍的重要因素。JW-901B排爆機器人可廣泛應(yīng)用于搜索、排爆、放射性物質(zhì)的排除，代替人去完成有危險的工作。JW-901B機器人的主要功能為抓取，優(yōu)于國內(nèi)外同類的各種機器人。

國外的危險環(huán)境作業(yè)機器人的發(fā)展較迅速，美國華盛頓州南部原子能研究中心根據(jù)核應(yīng)急環(huán)境的需求，設(shè)計了具有攀爬、去污、搬運功能的機器人組。該組機器人既可單獨工作，也可組網(wǎng)配合、協(xié)同工作。這套機器人具有磁性吸盤、高壓水槍、運動履帶等配套部件，有效地擴(kuò)展了輻射中心區(qū)的施工作業(yè)范圍。另外，美國還設(shè)計了一種機器人，可配合各種扳手部件，擰螺紋元件，倒推進(jìn)劑，清理不能自由倒入的推進(jìn)劑，最終完成強輻射環(huán)境中的自動拆卸任務(wù)。

未來世界智能機器人的發(fā)展趨勢將是著力解決小型機器人系統(tǒng)創(chuàng)新設(shè)計、無損檢測與故障診斷技術(shù)、多傳感器信息融合與智能預(yù)警策略、惡劣環(huán)境下的高穩(wěn)定遙操作或自動智能化技術(shù)、作業(yè)路徑自主導(dǎo)航及路徑優(yōu)化等關(guān)鍵技術(shù)難題，此外，從成本低、運動靈活、操作方便等角度考慮，將繼續(xù)向小型化、智能化、實用化方向發(fā)展。

目前，世界上已有60個國家裝備了軍用機器人，其中包括各種地面?zhèn)刹鞕C器人、空中偵察機器人、各種作戰(zhàn)機器人等，而軍用機器人的重難點技術(shù)也是機器人3D環(huán)境感知、目標(biāo)識別、自主導(dǎo)航。

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

為了解決上述問題，本發(fā)明提供一種基于3D立體感知技術(shù)的自主導(dǎo)航機器人系統(tǒng)，包括場景深度信息采集系統(tǒng)、移動機器人實時姿態(tài)估計系統(tǒng)、移動機器人三維場景重構(gòu)系統(tǒng)、復(fù)雜環(huán)境下三維目標(biāo)識別系統(tǒng)、移動機器人自主導(dǎo)航系統(tǒng)。通過場景深度信息采集系統(tǒng)獲取機器人所在環(huán)境中的場景深度信息，通過移動機器人實時姿態(tài)估計系統(tǒng)評估機器人實時姿態(tài)，通過三維場景重構(gòu)系統(tǒng)進(jìn)行實時三維重建，以獲得當(dāng)前環(huán)境三維信息，用以機器人進(jìn)行實時三維環(huán)境感知，通過三維目標(biāo)識別系統(tǒng)，識別復(fù)雜環(huán)境中的各目標(biāo)，以便機器人更好地感知環(huán)境、規(guī)劃路徑及完成相關(guān)作業(yè)，通過自主導(dǎo)航系統(tǒng)，使機器人可在陌生環(huán)境中進(jìn)行自主性路徑規(guī)劃。

本發(fā)明的技術(shù)方案如下：一種基于3D立體感知技術(shù)的自主導(dǎo)航機器人系統(tǒng)，主要工作流程包含以下步驟：

（1）場景深度信息采集：融合TOF攝像機與普通CCD陣列兩種攝像機優(yōu)勢，TOF攝像機提供的低分辨率深度圖, 研究低分辨率深度圖與立體視覺的匹配融合算法，通過立體視覺以及數(shù)據(jù)融合優(yōu)化的方法，在可見光攝像機視野下獲取高分辨率的深度圖。實現(xiàn)TOF攝像機與陣列立體視覺系統(tǒng)在相同的覆蓋空間上，在空間上達(dá)到嚴(yán)格一致；

（2）三維場景自主感知：場景感知可以表述為在場景數(shù)據(jù)獲取與表示的基礎(chǔ)上，結(jié)合視覺分析與模式識別等技術(shù)手段，從計算統(tǒng)計、結(jié)構(gòu)分析和語義表達(dá)等不同角度挖掘視覺數(shù)據(jù)中的特征與模式，從而實現(xiàn)對場景有效感知。所以，這里的“感知”不僅意味著對場景從局部到全局、從外觀到幾何形態(tài)的魯棒獲取，更是從低層數(shù)據(jù)到有含義實體的抽象模式的解釋，主要包含機器人對場景幾何結(jié)構(gòu)感知、機器人位姿感知以及機器人目標(biāo)物體感知。

場景幾何結(jié)構(gòu)感知是指三維場景重構(gòu)，就是重構(gòu)機器人所處的三維場景，并表示成機器人能夠的理解的結(jié)構(gòu)，即使機器人智能感知自己所處的環(huán)境；三維場景重構(gòu)技術(shù)是移動機器人三維環(huán)境感知的基礎(chǔ)，同時也是機器視覺領(lǐng)域長期以來非?；钴S的研究點。

機器人位姿感知就是計算移動機器人在場景中的位置及姿態(tài)，使機器人感知到自己在場景中位姿；本項目將采用在線深度圖序列實現(xiàn)移動機器人6DoF姿態(tài)估計及實時三維模型重構(gòu)，使移動機器人不僅能夠感知自身的姿態(tài)，而且可以感知到場景的三維幾何結(jié)構(gòu)。

場景目標(biāo)物體感知就是利用目標(biāo)物體的先驗知識，比如目標(biāo)物體模型等，采用模式識別算法訓(xùn)練目標(biāo)識別系統(tǒng)，從而使機器人能在場景中識別出目標(biāo)物體，即使機器人感知到場景中由哪些目標(biāo)組成。復(fù)雜環(huán)境下的目標(biāo)識別是機器視覺一個基礎(chǔ)而活躍的研究領(lǐng)域。目標(biāo)識別有著非常廣泛的應(yīng)用領(lǐng)域，比如機器人，智能監(jiān)控，自動化工業(yè)組裝及生物特征識別。在過去的幾十年，二維目標(biāo)識別得到了廣泛地研究并且在某些領(lǐng)域已經(jīng)得到相對成熟的應(yīng)用，比如人臉識別和行人檢測。與二維圖像對比，使用距離圖像（Range Images）在目標(biāo)識別方面能克服很多二維圖像目標(biāo)識別中存在的問題：

①距離圖像比相對于二維圖像不僅提供了足夠的紋理（Texture）信息，還額外包含了深度（Depth）信息；

從距離圖像提取得到的特征受尺度，旋轉(zhuǎn)及光照等因素的影響較??；

通過距離圖像計算而來的目標(biāo)三維姿態(tài)信息比二維圖像計算的結(jié)果更為精確。

（3）機器人自主導(dǎo)航：移動機器人自主導(dǎo)航，主要使用A線算法，A線算法是一種靜態(tài)路網(wǎng)中求解最短路徑最有效的直接搜索方法，也是解決許多搜索問題的有效算法。算法中的距離估算值與實際值越接近，最終搜索速度越快。對于機器人于未知環(huán)境中進(jìn)行三維環(huán)境感知及目標(biāo)識別，其龐大的信息量及其處理讓CPU吃力，故適用的算法要求在低的系統(tǒng)資源下，穩(wěn)定有效的完成路徑規(guī)劃，并在不斷運行中予以優(yōu)化。

本發(fā)明的技術(shù)方案如下：一種基于3D立體感知技術(shù)的自主導(dǎo)航機器人系統(tǒng)，該系統(tǒng)主要包括：

（1）場景深度信息采集系統(tǒng)：包括TOF相機與普通CCD陣列兩種攝像機，TOF相機提供低分辨率深度圖，CCD陣列提供立體視覺信息，通過立體視覺與低分辨率深度圖融合算法，獲取到高分辨率的深度圖；

（2）移動機器人實時姿態(tài)估計系統(tǒng)：通過上述場景深度信息采集系統(tǒng)獲取到機器人所在環(huán)境的三維場景信息，通過場景表面點云計算出表面法線圖，用以估計機器人實時姿態(tài)；

（3）移動機器人三維場景重構(gòu)系統(tǒng)：三維場景重構(gòu)主要指在機器人所處環(huán)境中進(jìn)行實時三維重建，以便于機器人進(jìn)行實時三維環(huán)境感知、目標(biāo)識別及自主導(dǎo)航；

（4）復(fù)雜環(huán)境下三維目標(biāo)識別系統(tǒng)：三維目標(biāo)識別主要指在復(fù)雜背景下，對機器人所在環(huán)境中特定目標(biāo)進(jìn)行識別，如作業(yè)目標(biāo)，障礙物等，便于機器人定位作業(yè)目標(biāo)及路徑規(guī)劃；

（5）移動機器人自主導(dǎo)航系統(tǒng)：移動機器人自主導(dǎo)航是基于三維場景感知及三維目標(biāo)識別基礎(chǔ)之上，完成機器人自主規(guī)劃作業(yè)路徑、自主避障及在作業(yè)過程中遇見動態(tài)障礙物時自主重優(yōu)化過程。

附圖說明

圖1為本發(fā)明基于3D立體感知技術(shù)的自主導(dǎo)航機器人系統(tǒng)的場景深度采集系統(tǒng)示意圖；

圖2為本發(fā)明基于3D立體感知技術(shù)的自主導(dǎo)航機器人系統(tǒng)的移動機器人姿態(tài)估計及三維場景重構(gòu)流程圖；

圖3為本發(fā)明基于3D立體感知技術(shù)的自主導(dǎo)航機器人系統(tǒng)的三維目標(biāo)識別系統(tǒng)框圖。

具體實施方式

下面結(jié)合附圖和具體實施方式對本發(fā)明的技術(shù)方案作進(jìn)一步詳細(xì)地說明。

參照圖1、圖2及圖3所示，一種基于3D立體感知技術(shù)的自主導(dǎo)航機器人系統(tǒng)，該系統(tǒng)主要包括場景深度信息采集系統(tǒng)、移動機器人實時姿態(tài)估計系統(tǒng)、移動機器人三維場景重構(gòu)系統(tǒng)、復(fù)雜環(huán)境下三維目標(biāo)識別系統(tǒng)、移動機器人自主導(dǎo)航系統(tǒng)。

TOF攝像機獲得低分辨率和高精度的深度數(shù)據(jù)用來指導(dǎo)同等或高分辨率的陣列立體視覺處理，獲取可靠的深度數(shù)據(jù)。TOF攝像機與普通CCD陣列融合的立體視覺系統(tǒng)、Manifold嵌入式計算機和千兆以太網(wǎng)交換機都安裝在移動機器人上，Manifold作為機器人平臺上的嵌入式計算機主要負(fù)責(zé)CCD陣列相機的深度圖計算及獲取TOF數(shù)據(jù)，并把它們的RGB圖像和深度圖像通過交換機傳輸?shù)胶笈_圖像工作站。

機器人位姿感知如圖2所示，利用系統(tǒng)輸入深度采集系統(tǒng)得到場景的深度圖序列；結(jié)合相機標(biāo)定參數(shù)及三角測量原理，將深度圖映射為場景表面點云并計算表面法線圖；運用連續(xù)的表面點云圖和法線圖拼接成一個全局的場景表面模型，得到當(dāng)前機器人相對全局模型姿態(tài)變化信息；在線深度圖序列可以實現(xiàn)移動機器人6DoF姿態(tài)估計及實時三維模型重構(gòu)，使移動機器人不僅能夠感知自身的姿態(tài)，而且可以感知到場景的三維幾何結(jié)構(gòu)。

在姿態(tài)估計的同時，將全局場景模型轉(zhuǎn)化為三角形網(wǎng)格的方式表示。針對表面拼接過程中ICP算法迭代效率低的問題，使用了一種在多個尺度表面下從粗到細(xì)地優(yōu)化迭代拼接過程的方法；為了解決ICP非線性優(yōu)化計算時間效率低下的問題，提出利用兩個連續(xù)表面之間相對運動較小的特性，將拼接過程中的非線性優(yōu)化問題近似轉(zhuǎn)變成線性優(yōu)化問題，從而提高優(yōu)化階段計算時間效率。最后，結(jié)合GPU并行計算能力，將以上步驟進(jìn)一步加速，實現(xiàn)了實時的移動機器人姿態(tài)估計及三維場景重構(gòu)。

機器人三維目標(biāo)識別及感知如圖3所示，這里所提到的三維場景表面是通過上一個模塊三維場景重建得到的場景表面，通常用偽灰度、點云或者多邊形網(wǎng)格等方式表示。多邊形網(wǎng)格表示是由一個的三維頂點坐標(biāo)矩陣和一個的三角面片頂點索引矩陣組成，即包含了頂點信息也包含了表面切片信息，較前兩種方式具有更強的表達(dá)能力，并且由于信息被壓縮的緣故易于在計算機上存儲。其中，n為頂點數(shù)，m為面片數(shù)。多邊形網(wǎng)格將離散的點云數(shù)據(jù)重新組織成多邊形而得到的場景表示，包含了大量直觀的表面信息。

在實際場景下，受被遮擋或者自遮擋的影響，一個視點下獲得的場景并無法包含三維目標(biāo)的全部形狀信息。法向量、曲率、主曲率、平均曲率、高斯曲率和形狀索引等微分幾何屬性作為局部表面的固有特性，構(gòu)成局部特征提取的理論基礎(chǔ)。首先利用曲面的平均曲率特性和高斯曲率特性計算曲面的形狀索引圖（Shape Index Map），然后計算形狀索引圖SIFT表面特征描述，最后匹配模型庫和場景特征描述從而完成三維目標(biāo)識別。

流程圖中的三維掃面具體是指待識別的實際物體的模型用三維多邊形網(wǎng)格表示，實際場景使用深度相機來獲取，目的是識別出深度相機中的三維目標(biāo)。在訓(xùn)練樣本階段，把三維網(wǎng)格表示的物體模型均勻地分割成點云，每個點云都表示物體的一個局部表面，用于模擬了深度相機的輸入，因為深度相機一次只能重建物體的一個局部表面。

具體做法是，虛擬一種深度相機，將它們均勻地布置在一個足夠包含整個模型的球體表面上，每個相機只獲得模型的一個局部點云視圖。實際過程中，為了得到這個球體，算法先從正二十面體的一個面開始，用4個等邊三角形分割這個三角形面。針對每一個面，重復(fù)前面的做法，直到達(dá)到想要的分割的到三角形數(shù)量。分割完三角形的數(shù)量代表了使用多少三角形來逼近球體。在得到的每一個三角形重心布置一個虛擬相機，通過采樣顯示卡中深度緩存數(shù)據(jù)來得到模型三維多邊形網(wǎng)格一個局部視圖。這個過程包含了兩個重要參數(shù)：一是模型球形的三角形數(shù)量；二是每個深度緩存的分辨率。參數(shù)1代表采樣的數(shù)量，參數(shù)2代表采樣的細(xì)節(jié)程度。當(dāng)每個模型的局部視圖都獲取完成對每個視圖計算它們的特征，產(chǎn)生模型庫特征描述集。同時，還可以保存每個采樣視圖的坐標(biāo)相對整體模型的剛體轉(zhuǎn)換矩陣，以便用于幾何連續(xù)性檢測。

當(dāng)獲得當(dāng)前場景與模型庫中每一個模型的特征描述之后，采用FLANN算法來快速計算場景特征與模型中的特征的最近鄰匹配并對應(yīng)分組。由于存在一個場景中包含多個目標(biāo)的情況，采用場景的特征描述與模型庫中的每一個目標(biāo)的特征匹配，這樣如果場景中包含了多個目標(biāo)也會被匹配出來。通過設(shè)置閾值剔除那些遠(yuǎn)離特征空間的對應(yīng)組。最后，對于每個對應(yīng)分組都將通過強制檢測它們之間的幾何連續(xù)性（geometric consistency）來剔除不符合要求的對應(yīng)組。通過假設(shè)模型中目標(biāo)與場景中的目標(biāo)之間的轉(zhuǎn)換是剛體的，模型庫中的每一個目標(biāo)的對應(yīng)集分成不同的子集組，每個子集保持場景中模型的一個特定的旋轉(zhuǎn)及平移矩陣。

以上所述，僅為本發(fā)明較佳的具體實施方式，本發(fā)明的應(yīng)用適用范圍不限于此，任何熟悉本技術(shù)領(lǐng)域的技術(shù)人員在本發(fā)明披露的技術(shù)范圍內(nèi)，可顯而易見地得到的技術(shù)方案的簡單變化或等效替換均落入本發(fā)明的應(yīng)用適用范圍內(nèi)。