一種基于多傳感器的自主避障導航系統(tǒng)的制作方法
【專利摘要】一種基于多傳感器的自主避障導航系統(tǒng)，包括傳感器處理層，局部算法處理層，錯誤探測層和中心處理層；傳感器處理層包括若干個傳感器處理節(jié)點，局部算法處理層包括若干個算法運算節(jié)點，錯誤探測層用于探測系統(tǒng)的硬件和軟件錯誤，確定上述傳感器處理節(jié)點和/或算法運算節(jié)點出現(xiàn)錯誤時，屏蔽出現(xiàn)錯誤的節(jié)點；中心處理層對局部算法處理層中若干個算法運算節(jié)點的結(jié)果進行全局優(yōu)化。本發(fā)明采用了非集中式的融合設計，極大提高了系統(tǒng)的魯棒性和容錯性。
【專利說明】
一種基于多傳感器的自主避障導航系統(tǒng)
技術(shù)領域
[0001]本發(fā)明屬于機器人和無人飛機自主避障導航技術(shù)領域，特別涉及一種可根據(jù)外在環(huán)境的變化自動選擇不同傳感器組合的自主避障導航系統(tǒng)設計及其方法。
【背景技術(shù)】
[0002]隨著科技的進步，機器人和無人飛機的應用領域越來越廣泛。它們不僅可以提供各種服務(如家用服務型機器人，掃地機等)，還可以在復雜危險的環(huán)境下執(zhí)行各種任務(如機器人進入日本福島核電站事故現(xiàn)場探查內(nèi)部的破壞情況，無人機航拍災后現(xiàn)場等)。然而，相當多的應用依然是在人工遠程遙控下完成的。要想機器人和無人機在復雜環(huán)境下完成復雜的任務，就必須要讓它們具備在未知環(huán)境下的自主避障導航能力。自上世紀60年代，美國斯坦福大學就開始研究人工智能技術(shù)應用于機器人的自主推理、規(guī)劃和控制，90年代以來，隨著各類高精度傳感器的產(chǎn)品化和信息處理技術(shù)的飛速發(fā)展，高適應性的移動機器人控制技術(shù)，真實環(huán)境下的自主導航等領域的研究進入到了新的階段。進入本世紀，隨著計算機技術(shù)，多傳感器集成和控制技術(shù)的進一步成熟，機器人和無人飛機在真實環(huán)境里的自主避障導航開始成為可能。
[0003]自主避障導航系統(tǒng)可以分解為三個基本的子系統(tǒng):繪圖和定位子系統(tǒng)，避障子系統(tǒng)和路徑規(guī)劃子系統(tǒng)。繪圖和定位模塊是通過機器自身攜帶的各種傳感器來感知周圍的環(huán)境，快速繪制周圍環(huán)境的地圖，并計算出自身相對于環(huán)境地圖的位置，速度和姿態(tài)等運動狀態(tài)的最佳估計。避障子系統(tǒng)可以通過機器所攜帶的傳感器來實時計算出自身到障礙物的距離，通過模糊邏輯或者人工智能的算法得到一個可靠的避障狀態(tài)估計。路徑規(guī)劃模塊有局部運算和全局運算兩個功能，局部運算可以為機器如何繞開障礙物提供最佳的路徑，全局運算則可以在周圍環(huán)境已知的情況下計算出到達目的地的最佳路徑。
[0004]機器人和無人機由于負載載荷，運動方式和使用環(huán)境不同，會采用不同的避障導航系統(tǒng)設計，主要是繪圖和定位子系統(tǒng)的不同設計。比如，機器人上可以搭載較重的激光雷達，而無人飛機負載不夠。又比如在室外環(huán)境下，可以采用全球衛(wèi)星定位導航系統(tǒng)(GNSS)來獲取絕對位置，而在室內(nèi)環(huán)境是無法使用的。同樣，在室內(nèi)環(huán)境里，可以采用射頻識別(RFID)技術(shù)來定位，而在室外環(huán)境里，這種方案是不現(xiàn)實的。
[0005]主流的避障導航系統(tǒng)會采用激光雷達，視覺傳感器，慣性導航傳感器，超聲波傳感器和RGB-D傳感器等。但是，這些傳感器都存在一定的局限性。
[0006]激光雷達可以快速獲得周圍環(huán)境的三維信息，廣泛用于各種尺度的三維模型重建。近年來，激光雷達也被用于機器人避障定位導航?；诩す饫走_的運動狀態(tài)估計是通過連續(xù)幀的點云匹配，從而計算出相對運動變化。但是，由于每次點云匹配的時候總是會帶有一些誤差，經(jīng)過一段時間之后，這種累計誤差會導致定位精度嚴重降低。為了解決這種問題，往往需要采用閉環(huán)檢測糾正技術(shù)(Loop Closure)。另外，激光雷達可以快速大范圍掃描周圍的環(huán)境，從而直接獲得與前方障礙物的距離，避障算法簡單有效。然而，激光雷達設備往往價格昂貴，體積大，重量沉，這大大限制了它在機器人和無人機避障定位導航上的應用。
[0007]視覺傳感器由于造價便宜，體積小，重量輕，被廣泛用于機器人和無人機的定位導航。視覺定位導航是通過對連續(xù)幀上的特征點跟蹤匹配，從而計算出相機的相對運動。這類基于視覺的運動估計技術(shù)又稱為視覺里程計技術(shù)，主要包括了單目視覺和雙目視覺。單目視覺的問題在于無法無法提供絕對的尺度信息，僅能計算出相對的運動，無法獲得實際的周圍環(huán)境的深度信息。雙目視覺中兩個相機間的基線提供了絕對尺度信息，從而可以計算出真實的深度信息，但其運算往往復雜。視覺里程計技術(shù)的關(guān)鍵在于可以穩(wěn)定可靠地跟蹤特征點，這就要求環(huán)境的光線條件穩(wěn)定和物體表面有豐富的紋理。這種要求大大限制了視覺里程計技術(shù)的應用場景。
[0008]慣性導航傳感器的價格變化范圍很大，在機器人和無人機上采用的往往是價格便宜，重量輕，體積小的MEMS慣性傳感器。慣性傳感器可以在短時間里提供可靠的加速度和角速度信息，從而計算出機器人和無人機的位置，速度和姿態(tài)。然而MEMS慣性傳感器也存在較大的隨機系統(tǒng)誤差和時間漂移量，因此必須和其它傳感器的測量值通過濾波技術(shù)來進行融合，以改正誤差，提尚運動狀態(tài)的估計精度。
[0009]超聲波傳感器價格便宜，體積小，重量輕，廣泛用于機器人和無人機避障。它是靠發(fā)射聲波信號，利用物體界面上超聲反射來檢測障礙物的。超聲波是具有一定方向性的波束，當超聲波傳感器和障礙物形成一定角度的時候，會發(fā)生鏡面反射，反射而產(chǎn)生所謂的幻影，這種多路徑效應會導致機器以為自身和障礙物還很遠，從而發(fā)生誤判。此外，聲波在空氣中傳播衰減很快，有效探測距離一般只有5-10米。聲波速度比光速慢許多，響應速度也較慢。
[0010]RGB-D傳感器是一種新型視覺設備，可以同時獲得周圍環(huán)境的彩色圖像和深度信息。它相對于激光雷達設備，價格便宜(大約是最便宜的激光雷達的1/20)，體積小，重量輕，這些優(yōu)勢推動其逐步取代激光雷達用于機器人和無人機的避障定位導航系統(tǒng)。近幾年來，RGB-D傳感器的室內(nèi)移動機器人避障定位導航研究論文發(fā)表。不過這些研究，還基本沿用了激光雷達定位導航的思路，相當于用了一個便宜的激光雷達。雖然有些論文里談到采用RGB相機輔助發(fā)現(xiàn)閉合路徑，總體來說，當前的RGB-D定位導航算法并沒有更深入地挖掘RGB傳感器和D傳感器的融合。
[0011]目前基于不同類型的傳感器，發(fā)展出了多種場景的繪圖定位技術(shù)，并用于不同的場景中。其中，最常用的就是二維柵格地圖。然而，二維柵格地圖有其局限性，它只包含了場景內(nèi)某高度上的橫截面信息，信息缺失將影響到機器人避障功能，并限制了機器人的自動導航。此外，這種地圖幾乎無法應用在無人機上。
[0012]當前，自主避障導航系統(tǒng)及方法的設計往往都局限于應用的場合，每個不同的應用場合，根據(jù)傳感器種類及搭配的不同，具體避障導航系統(tǒng)及方法也相應有區(qū)別，這樣導致需要設計多種不同的避障導航系統(tǒng)及方法;同時，如果某個傳感器故障，會導致避障導航系統(tǒng)及方法無法很好地適應，使得整個系統(tǒng)魯棒性較差。目前還沒有適應性好、容錯性高的通用自主避障導航系統(tǒng)及方法。

【發(fā)明內(nèi)容】

[0013]本發(fā)明針對當前自主避障導航系統(tǒng)設計的局限性，首次提出一種容錯性高的自適應環(huán)境變化的自主導航避障系統(tǒng)設計及方法。該系統(tǒng)采用非集中融合設計，包括傳感器數(shù)據(jù)處理層，局部算法處理層和主算法處理層。傳感器數(shù)據(jù)處理層，按每個傳感器的計算任務設計運算節(jié)點，所有的運算節(jié)點彼此獨立，可并行運算。局部算法處理層，針對所有可能的傳感器組合設計了不同的算法節(jié)點，各局部算法節(jié)點彼此獨立，可并行處理。局部算法器的結(jié)果傳遞給主算法器，做全局優(yōu)化。硬件上，本系統(tǒng)可以采用多個小型單機版電腦，比如樹莓派零，來執(zhí)行各節(jié)點的運算。本系統(tǒng)可以自動探測傳感器的硬件錯誤和軟件錯誤，從而選擇最可靠的傳感器組合和局部算法。這種設計具有高度可擴展性和容錯性，便于根據(jù)需要添加和去除某些傳感器，也能確保在某些傳感器失靈的情況下，依然完成自主避障導航的任務。
[0014]針對以上自主避障導航系統(tǒng)設計的局限性，本發(fā)明提供了靈活的容錯性高的系統(tǒng)設計?；诒景l(fā)明，增加和去除傳感器變得容易，即便在部分傳感器失靈情況下，系統(tǒng)仍然可以完成任務，這大大拓展了機器人與無人機自主避障系統(tǒng)的應用領域。
[0015]為實現(xiàn)上述目的，本發(fā)明采用以下的技術(shù)方案:
[0016]—種基于多傳感器的自主避障導航系統(tǒng)，包括
[0017]傳感器處理層，局部算法處理層，錯誤探測層和中心處理層；
[0018]傳感器處理層包括若干個傳感器處理節(jié)點，每個傳感器處理節(jié)點對多個傳感器中的一個傳感器的數(shù)據(jù)進行處理;每個傳感器處理節(jié)點彼此獨立，且分別處理不同的傳感器數(shù)據(jù)；
[0019]局部算法處理層包括若干個算法運算節(jié)點，所述若干個算法運算節(jié)點分別基于兩個或多個所述傳感器處理節(jié)點中的輸出數(shù)據(jù)進行運算;每個局部算法節(jié)點彼此獨立，且分別基于不同的輸出數(shù)據(jù)進行運算；
[0020]錯誤探測層用于探測系統(tǒng)的硬件和軟件錯誤，確定上述傳感器處理節(jié)點和/或算法運算節(jié)點出現(xiàn)錯誤時，屏蔽出現(xiàn)錯誤的節(jié)點；
[0021]中心處理層對局部算法處理層中若干個算法運算節(jié)點的結(jié)果進行全局優(yōu)化。
[0022]—種基于多傳感器的自主避障導航系統(tǒng)，具有定位繪圖子系統(tǒng)，所述定位繪圖子系統(tǒng)包括:傳感器處理層，局部算法處理層；
[0023]傳感器處理層包括若干個定位傳感器處理節(jié)點，每個定位傳感器處理節(jié)點對多個定位傳感器中的一個定位傳感器的數(shù)據(jù)進行處理;每個定位傳感器處理節(jié)點彼此獨立，且分別處理不同的定位傳感器數(shù)據(jù)；
[0024]局部算法處理層包括若干個算法運算節(jié)點，所述若干個算法運算節(jié)點分別基于兩個或多個所述定位傳感器處理節(jié)點中的輸出數(shù)據(jù)進行運算;每個局部算法節(jié)點彼此獨立，且分別基于不同的輸出數(shù)據(jù)進行運算。
[0025]所述定位傳感器包括圖像傳感器、深度傳感器和慣導傳感器。
[0026]所述定位傳感器處理節(jié)點包括基于慣性傳感器和圖像傳感器數(shù)據(jù)的擴展卡爾曼濾波器、基于慣性傳感器和深度傳感器數(shù)據(jù)的擴展卡爾曼濾波器，以及基于圖像傳感器和深度傳感器數(shù)據(jù)的擴展卡爾曼濾波器。
[0027]所述定位繪圖子系統(tǒng)還包括錯誤探測層和中心處理層;錯誤探測層用于探測定位繪圖子系統(tǒng)的硬件和軟件錯誤，確定上述定位傳感器處理節(jié)點和/或算法運算節(jié)點出現(xiàn)錯誤時，屏蔽出現(xiàn)錯誤的節(jié)點；中心處理層，基于局部算法處理層中若干個算法運算節(jié)點的結(jié)果產(chǎn)生多個點云，并對多個點云進行融合，獲得三維地圖。
[0028]—種基于多傳感器的自主避障導航系統(tǒng)，具有避障子系統(tǒng)，所述避障子系統(tǒng)包括:傳感器處理層，局部算法處理層；
[0029]傳感器處理層包括若干個障礙感知傳感器處理節(jié)點，每個障礙感知傳感器處理節(jié)點對多個障礙感知傳感器中的一個障礙感知傳感器的數(shù)據(jù)進行處理;每個障礙感知傳感器處理節(jié)點彼此獨立，且分別處理不同的障礙感知傳感器數(shù)據(jù)；
[0030]局部算法處理層包括若干個算法運算節(jié)點，所述若干個算法運算節(jié)點分別基于兩個或多個所述障礙感知傳感器處理節(jié)點中的輸出數(shù)據(jù)進行運算;每個局部算法節(jié)點彼此獨立，且分別基于不同的輸出數(shù)據(jù)進行運算。
[0031]所述障礙感知傳感器包括若干個紅外傳感器，若干個超聲波傳感器和深度傳感器。
[0032]所述障礙感知傳感器處理節(jié)點包括基于慣性傳感器和圖像傳感器數(shù)據(jù)的模糊邏輯控制器、基于慣性傳感器和深度傳感器數(shù)據(jù)的模糊邏輯控制器，以及基于圖像傳感器和深度傳感器數(shù)據(jù)的模糊邏輯控制器。
[0033]所述避障子系統(tǒng)還包括錯誤探測層和中心處理層;錯誤探測層用于探測避障子系統(tǒng)的硬件和軟件錯誤，確定上述障礙感知傳感器處理節(jié)點和/或算法運算節(jié)點出現(xiàn)錯誤時，屏蔽出現(xiàn)錯誤的節(jié)點；中心處理層，對局部算法處理層中若干個算法運算節(jié)點的結(jié)果進行優(yōu)化處理。
[0034]—種基于多傳感器的自主避障導航系統(tǒng)，包括上述定位繪圖子系統(tǒng)和上述避障子系統(tǒng)。
[0035]—種無人機或機器人，包括上述基于多傳感器的自主避障導航系統(tǒng)。
[0036]以上所述的自主避障導航系統(tǒng)設計的發(fā)明點在于:
[0037](I)對定位繪圖子系統(tǒng)和避障子系統(tǒng)采用了非集中式的融合設計，將傳感器數(shù)據(jù)處理和局部算法運算分開，極大提高了系統(tǒng)的魯棒性和容錯性。
[0038](2)在傳感器數(shù)據(jù)處理層，各傳感器數(shù)據(jù)處理節(jié)點可以并行運算。在局部算法處理層，各局部算法節(jié)點可以并行運算。
[0039](3)在錯誤探測層，系統(tǒng)可以自動探測到硬件和算法的錯誤，并及時屏蔽不可靠的局部節(jié)點，保證主算法處理層能夠正常運行。
[0040]本發(fā)明的有益效果為:采用了非集中式的融合設計，極大提高了系統(tǒng)的魯棒性和容錯性。該系統(tǒng)的設計靈活，添加或去掉某個傳感器時，只需要添加或去除對應的傳感器處理節(jié)點和算法節(jié)點即可。當傳感器信號出現(xiàn)錯誤時，也能夠自動方便地屏蔽掉該傳感器節(jié)點，而不影響系統(tǒng)正常運行，從而可以自適應不同的環(huán)境(例如，室內(nèi)室外白天黑夜的環(huán)境)，極大提高了機器人和無人機的應用領域。
【附圖說明】
[0041 ]下面根據(jù)附圖對本發(fā)明作進一步詳細說明。
[0042]圖1為本發(fā)明的非集中式融合系統(tǒng)結(jié)構(gòu)框圖
[0043]圖2為本發(fā)明的定位繪圖子系統(tǒng)的一種硬件俯視圖
[0044]圖3為本發(fā)明的一種可能的定位繪圖子系統(tǒng)示意圖
[0045]圖4(a)為本發(fā)明定位繪圖子系統(tǒng)所實現(xiàn)的MEMS+RGB的擴展卡爾曼濾波實現(xiàn)的流程圖
[0046]圖4(b)為本發(fā)明定位繪圖子系統(tǒng)所實現(xiàn)的MEMS+D的擴展卡爾曼濾波實現(xiàn)的流程圖
[0047]圖4(c)為本發(fā)明的定位繪圖子系統(tǒng)所實現(xiàn)的RGB+D的擴展卡爾曼濾波實現(xiàn)的流程
[0048]圖5為本發(fā)明的避障子系統(tǒng)的一種硬件俯視圖
[0049]圖6為本發(fā)明的一種可能的避障子系統(tǒng)示意圖
【具體實施方式】
[0050]—種自適應環(huán)境的自主避障導航系統(tǒng)的設計和實現(xiàn)方法，包括基于非集中式融合設計的定位繪圖子系統(tǒng)，避障子系統(tǒng)和路徑規(guī)劃子系統(tǒng)。
[0051 ]定位繪圖子系統(tǒng)用RGB-D傳感器和MEMS慣性導航傳感器快速繪制三維地圖并定位。該子系統(tǒng)可以探測MEMS傳感器，RGB即彩色相機和D即深度相機的硬件故障和數(shù)據(jù)錯誤，依此選擇合適的組合算法來實現(xiàn)定位繪圖。
[0052]避障子系統(tǒng)用紅外傳感器，超聲波傳感器和D傳感器來獲取自身到障礙物的距離。該子系統(tǒng)可以探測到各傳感器的硬件故障和數(shù)據(jù)錯誤，依此選擇合適的策略來確定自身與障礙物的距離狀態(tài)。
[0053]路徑規(guī)劃子系統(tǒng)實現(xiàn)了多個不同的算法，比如人工勢場路徑規(guī)劃。該子系統(tǒng)會根據(jù)自身位置與所用地圖的狀態(tài)來確定所采用的算法。
[0054]定位繪圖子系統(tǒng)的實現(xiàn)方案包括:
[0055]步驟一:傳感器數(shù)據(jù)處理層的實現(xiàn)，以每個傳感器的計算任務為基本數(shù)據(jù)處理單元，設計若干運算節(jié)點。在本實例中，采用了MEMS慣性導航傳感器，RGB即彩色相機和D即深度傳感器。
[0056]步驟二:局部濾波器處理層的設計，實現(xiàn)三個局部的擴展卡爾曼濾波器節(jié)點，SPMEMS+RGB，MEMS+D和RGB+D三個局部濾波器。每個濾波器都獨立并行計算，并得到運動模型的參數(shù)估計，即相對的位置移動和姿態(tài)變化。
[0057]步驟三:探測該子系統(tǒng)的硬件和軟件錯誤。硬件錯誤一般在傳感器數(shù)據(jù)讀取的時候就會發(fā)現(xiàn)。軟件錯誤是指數(shù)據(jù)處理節(jié)點和局部濾波器節(jié)點的錯誤。
[0058]步驟四:根據(jù)錯誤檢測的結(jié)果，來選擇合適的局部濾波器，把結(jié)果傳遞給主處理器做全局優(yōu)化。這里會最終確定最佳的運動狀態(tài)估計，即位置和姿態(tài)的估計?；诖?，構(gòu)建和更新三維地圖。
[0059]避障子系統(tǒng)的實現(xiàn)方案包括:
[0060]步驟一:傳感器數(shù)據(jù)處理層的實現(xiàn)，在本實例中，采用了紅外傳感器，超聲波傳感器和D傳感器。
[0061]步驟二:本實例對每個傳感器實現(xiàn)一個局部模糊控制器。對基于距離的隸屬函數(shù)，設置輸入的語言集合。對輸出數(shù)據(jù)的隸屬函數(shù)，設置輸出的語言集合。根據(jù)模糊邏輯控制理論，基于輸入和輸出的語言集合設計模糊邏輯規(guī)則。將不同傳感器的最佳探測距離范圍，轉(zhuǎn)化為一種可靠性(即概率)一并帶入到輸出數(shù)據(jù)。
[0062]步驟三:探測該子系統(tǒng)的硬件和軟件錯誤。
[0063]步驟四:根據(jù)錯誤探測的結(jié)果，把有效的輸出發(fā)送到主處理器，主處理根據(jù)輸出到可靠性做加權(quán)平均獲得最終的機器人和無人機的運動控制命令。
[0064]路徑規(guī)劃子系統(tǒng)的實現(xiàn)方案
[0065]對局部路徑規(guī)劃采用標準人工勢能算法(已知算法)。它的基本思想是將機器在周圍環(huán)境中的運動，設計成一種抽象的人造引力場中的運動。該經(jīng)典算法的優(yōu)勢在于運算速度快，規(guī)劃出來的路徑平滑且安全。當全局的三維地圖和目的地已知的情況下，直接采用基于最短路徑的柵格法(已知算法)。它的基本思想是把已知三維地圖抽象為一個二維的平面柵格，然后在該柵格上標記出目的地，再通過經(jīng)典的最短路徑算法來規(guī)劃路徑。
[0066]下面結(jié)合附圖對本發(fā)明做進一步的詳述。
[0067]本發(fā)明中的定位繪圖子系統(tǒng)和避障子系統(tǒng)都采用了非集中式融合設計。圖1為非集中式融合的設計圖，整個系統(tǒng)分為四個層，分別是傳感器處理層，局部算法處理層，錯誤探測層和中心處理層。傳感器處理層內(nèi)實現(xiàn)了若干個傳感器處理節(jié)點，用于對傳感器的數(shù)據(jù)進行處理，這些運算節(jié)點彼此獨立，可并行處理。局部算法處理層內(nèi)實現(xiàn)了若干個不同的算法，每個算法實現(xiàn)都是一個算法運算節(jié)點，這些局部算法節(jié)點彼此獨立，可并行處理。錯誤探測層主要是自動探測系統(tǒng)的硬件和軟件錯誤，一旦確定某個節(jié)點出現(xiàn)錯誤，可以及時屏蔽該節(jié)點而不至于影響整個系統(tǒng)的正常運行。中心處理層實現(xiàn)了一個中心算法，中心算法會對各局部子算法的結(jié)果做全局的優(yōu)化。下面具體介紹如何實施定位繪圖子系統(tǒng)和避障子系統(tǒng)。
[0068]定位繪圖子系統(tǒng)具體實施方案
[0069]圖2給出了一種可能的定位繪圖子系統(tǒng)的硬件實現(xiàn)，包括了RGB傳感器I，D深度傳感器2和MEMS慣導傳感器3。圖3給出了一種可能的非集中式融合的定位繪圖子系統(tǒng)設計方案。
[0070]首先是傳感器的數(shù)據(jù)處理層的設計，以每個傳感器的計算任務為基本數(shù)據(jù)處理單元，設計若干運算節(jié)點。MEMS運算節(jié)點任務是通過對加速度和角速度的積分獲得運動的速度，位置和姿態(tài)。RGB運算節(jié)點任務是執(zhí)行特征提取和跟蹤，并計算出相對運動的位置變化和姿態(tài)變化。D運算節(jié)點的任務是對連續(xù)深度幀的點云數(shù)據(jù)做ICP運算，從而獲得相對運動的位置變化和姿態(tài)變化。
[0071]然后，實現(xiàn)了 3個局部的擴展卡爾曼濾波器(EKF)，分別是MEMS+RGB，MEMS+D和RGB+DJEMS+RGB的擴展卡爾曼濾波實現(xiàn)的流程圖如圖4(a)所示，采用算法是單目相機和MEMS的擴展卡爾曼濾波。在RGB傳感器連續(xù)幀上提取和跟蹤特征點完成相對定向，即確定連續(xù)幀間的位置和姿態(tài)相對變化?；贛EMS傳感器數(shù)據(jù)處理節(jié)點提供的位置和姿態(tài)信息可以有效縮小連續(xù)幀上特征點的搜索空間，從而提高運算的效率。濾波器輸出的是對位置和姿態(tài)的最佳估計。MEMS+D的擴展卡爾曼濾波實現(xiàn)的流程圖如圖4(b)所示。首先，通過D傳感器的連續(xù)幀產(chǎn)生連續(xù)點云，然后通過ICP算法(已知算法)對連續(xù)點云進行匹配，其中MEMS傳感器數(shù)據(jù)處理節(jié)點所獲得的位置和姿態(tài)信息可以幫助提供ICP算法的初始條件，從而提高ICP算法的成功率。濾波器輸出的是對位置和姿態(tài)的最佳估計。RGB+D的擴展卡爾曼濾波實現(xiàn)的流程如圖4(c)所示。首先，RGB傳感器和D傳感器之間的幾何關(guān)系必須通過嚴格的檢較來確定，SP通過雙目相機檢較的方式來確定RGB和D傳感器之間的相對位移和姿態(tài)。通過這個關(guān)系，就確定了 RGB像平面空間和D像平面空間的映射關(guān)系。基于深度圖像的點云提取的特征(不局限于點特征)可以通過映射關(guān)系變換到RGB的像空間去，從而輔助RGB連續(xù)幀上對特征的提取和跟蹤，即深度信息輔助的特征融合。基于此，可以計算出連續(xù)幀的位置和姿態(tài)的相對變化，即對位置和姿態(tài)的估計。這3個局部算法器的輸出都是相對的位置變化和姿態(tài)變化。
[0072]接下來，探測錯誤層需要對傳感器數(shù)據(jù)處理層和局部算法處理層探測錯誤。硬件錯誤往往可以直接通過數(shù)據(jù)包錯誤來發(fā)現(xiàn)。軟件錯誤包括了原始數(shù)據(jù)處理節(jié)點中的錯誤和濾波器錯誤。RGB處理節(jié)點的錯誤可能是無法提取和跟蹤特征點，比如在黑暗的環(huán)境里或者周圍物體表面沒有任何紋理。D處理節(jié)點的錯誤可能獲得無效的深度信息，比如在戶外強烈陽光直射的情況下。這一類的數(shù)據(jù)錯誤可以通過閥值檢測來探測。
[0073]RGB數(shù)據(jù)錯誤可能的檢測方法:
[0074]對每幅影像灰度值做直方圖，對直方圖進行分析，其理想分布應該接近于平均分布。如果其分布過于集中于某個狹窄區(qū)域，比如全黑或者全白，則認為數(shù)據(jù)錯誤。
[0075]RGB圖像無法提取和跟蹤足夠的特征點，則認為數(shù)據(jù)錯誤。
[0076]基于RGB圖像的相對定向結(jié)果的協(xié)方差矩陣分析，方差過大，則認為數(shù)據(jù)錯誤。
[0077]D傳感器數(shù)據(jù)錯誤可能的檢測方法:
[0078]對每幀深度圖像的紅外灰度值做直方圖，與RGB直方圖測錯方法一樣。
[0079]ICP算法無法收斂，則認為數(shù)據(jù)出錯。
[0080]另外一類軟件錯誤是濾波器錯誤，濾波器錯誤的探測方法是基于數(shù)理統(tǒng)計的方法。這里僅結(jié)合本實例，給出一種基于x~2的檢測方法，并解釋。每個局部的擴展卡爾曼濾波實現(xiàn)是基于兩個傳感器的數(shù)據(jù)融合。結(jié)合本實例，觀測方程的觀測量就是由位置，速度和姿態(tài)組成的觀測向量。一般的觀測方程如下:
[0081 ] y_k = H.x_k+v_k，v_k?N(0，R_k)
[0082]在濾波器運行正常的情況下，殘差V應該是符合高斯正態(tài)分布，其數(shù)學期望為0，方差R，且協(xié)方差為O。
[0083]設計一個假設檢驗如下:
[0084]基于殘差的方差D{v_k}計算統(tǒng)計量:
[0085]Ω: =v_k'T D{v_k}' (-1 )v_k
[0086]假設檢驗:
[0087]H_0:E{v_k} =(W$H_a:E{v_k} #0
[0088]在假設條件H_0下，統(tǒng)計量Ω應該符合x~2—分布，據(jù)此，可判斷濾波器是否運行正常。
[0089]Ω>χ_(η，αΓ2 濾波器錯誤
[0090]Ω〈χ_(η，α)~2 濾波器正常
[0091]最后，根據(jù)錯誤檢測的結(jié)果，把有效的局部算法器的結(jié)果傳遞給主算法器做全局優(yōu)化。在沒有發(fā)生錯誤的情況下，三個局部算法器會把位置和姿態(tài)的最佳估計，傳遞給主算法器。主算法器會基于局部算法器的結(jié)果來產(chǎn)生3組不同的點云，并對它們再次通過ICP算法融合，從而獲得最終的三維地圖。當系統(tǒng)局部失靈的情況下，主算法器依然可以通過其它正常的局部算法器獲得位置和狀態(tài)的估計，并產(chǎn)生三維環(huán)境地圖。另外，無論采用哪種組合，總會有誤差存在，這種累積的誤差會導致整個三維地圖的精度隨著時間而下降。這個問題可以通過閉環(huán)路徑糾正來解決。在本實例中，主算法器會保留關(guān)鍵幀的數(shù)字影像和點云，通過比對當前幀的數(shù)據(jù)和關(guān)鍵幀數(shù)據(jù)，來判斷是否發(fā)現(xiàn)閉合路徑。一單發(fā)現(xiàn)閉合路徑，則采用ELCH算法(已知算法)做全局優(yōu)化，并更新地圖。
[0092]避障子系統(tǒng)具體實施方案
[0093]圖5給出了一種可能的避障子系統(tǒng)的硬件實現(xiàn)方案，包括了紅外傳感器陣列I，D深度傳感器2，超聲波傳感器陣列3和MEMS慣導傳感器4。紅外傳感器陣列包括了放置在機器人和無人機頭部左側(cè)，中間和右側(cè)的三個紅外傳感器。超聲波陣列包括了同樣位置放置的三個超聲波傳感器。D傳感器放置在中間的部位。這樣放置的目的是使整個前部的探測扇面角度可以達到180度。
[0094]圖6給出了一種可能的非集中式融合設計的避障子系統(tǒng)設計圖。
[0095]首先，是傳感器數(shù)據(jù)處理層的實現(xiàn)，3個紅外和3個超聲波傳感器分別測量自身到障礙物左側(cè)，中間和右側(cè)的距離。D深度傳感器可以給出整個前方覆蓋范圍的深度值，即到障礙物表面的距離(該實施例僅僅是為了說明的舉例，本領域技術(shù)人員可以理解，上述傳感器可以是其他類型傳感器，傳感器的數(shù)量也可以根據(jù)需要變化，并不限于此)。一共實現(xiàn)7個數(shù)據(jù)運算節(jié)點，它們可以并行運算，并提供出機器到障礙物的距離信息。
[0096]由于機器人和無人機的避障環(huán)境可以非常復雜，很難像定位繪圖子系統(tǒng)那樣基于運動模型建立精確的各種濾波器，采用模糊邏輯控制算法是更合適的選擇?；诜羌惺綌?shù)據(jù)融合的設計思路，對每一種傳感器會設計一個模糊邏輯控制器。按本實例，對紅外傳感器，超聲波傳感器和D傳感器設計并實現(xiàn)3個局部的模糊邏輯控制器。模糊控制器的輸入是距離信號，輸出的是移動機器人或者無人機的動作，暫規(guī)定前進或旋轉(zhuǎn)兩種運動模式。根據(jù)一定的控制策略，建立輸入輸出的模糊語言集合和模糊控制規(guī)則。根據(jù)輸入的距離數(shù)據(jù)建立隸屬度函數(shù)，其輸入模糊語言集合為{近，中，遠}。模糊控制器的輸出為移動機器人或者無人機的前進或者旋轉(zhuǎn)的運動命令，建立輸出變量的隸屬度函數(shù)，輸出的語言集合為{右轉(zhuǎn)，前進，左轉(zhuǎn)}。紅外傳感器根據(jù)其CCD的分辨率存在一個最近探測距離，對中遠距離的障礙物有比較高的測距精度。超聲波傳感器的探測距離在5-10米，因此比較適合中等距離的障礙物探測。D傳感器的有效范圍在3米以內(nèi)，適合近距離的障礙物探測。這些傳感器的特性需要充分體現(xiàn)在模糊控制器的設計中。即在設定規(guī)則的時候，會根據(jù)每個傳感器的最佳測距范圍，加權(quán)輸出，即對輸出的控制參數(shù)附加一個取值范圍在O到I間的可信度。
[0097]接下來，探測錯誤層探測硬件和軟件錯誤。硬件錯誤可以通過數(shù)據(jù)包錯誤發(fā)現(xiàn)。軟件錯誤主要就是由于各個傳感器的錯誤數(shù)據(jù)所引起的模糊控制器錯誤。假設不存在軟件錯誤的情況下，各模糊控制器所輸出的運動參數(shù)應該是非常接近的，如果彼此間的輸出出現(xiàn)較大區(qū)別的時候，以輸出的可信度為標準決定模糊控制器的錯誤。
[0098]最后，將各局部模糊控制器的輸出信息發(fā)送給主控制器，主控制會加權(quán)平均出合適的輸出信息。同時會結(jié)合由定位繪圖子系統(tǒng)所構(gòu)建的三維地圖來驗證該輸出運動命令是否可行。
[0099]在本實例中，路徑規(guī)劃子系統(tǒng)實現(xiàn)了人工勢場方法的局部路徑規(guī)劃和基于三維地圖的全局路徑規(guī)劃(已知算法)。
[0100]以上所描述的僅為本發(fā)明的一種實現(xiàn)實例，僅用于解釋本發(fā)明，而非針對本發(fā)明的限制。本發(fā)明的保護范圍并不局限于此。具體算法和所采用的傳感器都可以改變和優(yōu)化，但凡是基于本專利所提的系統(tǒng)設計和算法框架都因?qū)儆诒緦＠谋Ｗo范疇。
【主權(quán)項】
1.一種基于多傳感器的自主避障導航系統(tǒng)，其特征在于:包括 傳感器處理層，局部算法處理層，錯誤探測層和中心處理層； 傳感器處理層包括若干個傳感器處理節(jié)點，每個傳感器處理節(jié)點對多個傳感器中的一個傳感器的數(shù)據(jù)進行處理;每個傳感器處理節(jié)點彼此獨立，且分別處理不同的傳感器數(shù)據(jù)； 局部算法處理層包括若干個算法運算節(jié)點，所述若干個算法運算節(jié)點分別基于兩個或多個所述傳感器處理節(jié)點中的輸出數(shù)據(jù)進行運算;每個局部算法節(jié)點彼此獨立，且分別基于不同的輸出數(shù)據(jù)進行運算； 錯誤探測層用于探測系統(tǒng)的硬件和軟件錯誤，確定上述傳感器處理節(jié)點和/或算法運算節(jié)點出現(xiàn)錯誤時，屏蔽出現(xiàn)錯誤的節(jié)點； 中心處理層對局部算法處理層中若干個算法運算節(jié)點的結(jié)果進行全局優(yōu)化。2.—種基于多傳感器的自主避障導航系統(tǒng)，其特征在于:具有定位繪圖子系統(tǒng)，所述定位繪圖子系統(tǒng)包括:傳感器處理層，局部算法處理層； 傳感器處理層包括若干個定位傳感器處理節(jié)點，每個定位傳感器處理節(jié)點對多個定位傳感器中的一個定位傳感器的數(shù)據(jù)進行處理;每個定位傳感器處理節(jié)點彼此獨立，且分別處理不同的定位傳感器數(shù)據(jù)； 局部算法處理層包括若干個算法運算節(jié)點，所述若干個算法運算節(jié)點分別基于兩個或多個所述定位傳感器處理節(jié)點中的輸出數(shù)據(jù)進行運算;每個局部算法節(jié)點彼此獨立，且分別基于不同的輸出數(shù)據(jù)進行運算。3.一種如權(quán)利要求2所述的基于多傳感器的自主避障導航系統(tǒng)，其特征在于:所述定位傳感器包括圖像傳感器、深度傳感器和慣導傳感器。4.一種如權(quán)利要求2所述的基于多傳感器的自主避障導航系統(tǒng)，其特征在于:所述定位傳感器處理節(jié)點包括基于慣性傳感器和圖像傳感器數(shù)據(jù)的擴展卡爾曼濾波器、基于慣性傳感器和深度傳感器數(shù)據(jù)的擴展卡爾曼濾波器，以及基于圖像傳感器和深度傳感器數(shù)據(jù)的擴展卡爾曼濾波器。5.一種如權(quán)利要求2-4中所述的基于多傳感器的自主避障導航系統(tǒng)，其特征在于:所述定位繪圖子系統(tǒng)還包括錯誤探測層和中心處理層;錯誤探測層用于探測定位繪圖子系統(tǒng)的硬件和軟件錯誤，確定上述定位傳感器處理節(jié)點和/或算法運算節(jié)點出現(xiàn)錯誤時，屏蔽出現(xiàn)錯誤的節(jié)點；中心處理層，基于局部算法處理層中若干個算法運算節(jié)點的結(jié)果產(chǎn)生多個點云，并對多個點云進行融合，獲得三維地圖。6.一種基于多傳感器的自主避障導航系統(tǒng)，其特征在于:具有避障子系統(tǒng)，所述避障子系統(tǒng)包括:傳感器處理層，局部算法處理層； 傳感器處理層包括若干個障礙感知傳感器處理節(jié)點，每個障礙感知傳感器處理節(jié)點對多個障礙感知傳感器中的一個障礙感知傳感器的數(shù)據(jù)進行處理;每個障礙感知傳感器處理節(jié)點彼此獨立，且分別處理不同的障礙感知傳感器數(shù)據(jù)； 局部算法處理層包括若干個算法運算節(jié)點，所述若干個算法運算節(jié)點分別基于兩個或多個所述障礙感知傳感器處理節(jié)點中的輸出數(shù)據(jù)進行運算;每個局部算法節(jié)點彼此獨立，且分別基于不同的輸出數(shù)據(jù)進行運算。7.一種如權(quán)利要求6所述的基于多傳感器的自主避障導航系統(tǒng)，其特征在于:所述障礙感知傳感器包括若干個紅外傳感器，若干個超聲波傳感器和深度傳感器。8.一種如權(quán)利要求6所述的基于多傳感器的自主避障導航系統(tǒng)，其特征在于:所述障礙感知傳感器處理節(jié)點包括基于慣性傳感器和圖像傳感器數(shù)據(jù)的模糊邏輯控制器、基于慣性傳感器和深度傳感器數(shù)據(jù)的模糊邏輯控制器，以及基于圖像傳感器和深度傳感器數(shù)據(jù)的模糊邏輯控制器。9.一種如權(quán)利要求6-8中所述的基于多傳感器的自主避障導航系統(tǒng)，其特征在于:所述避障子系統(tǒng)還包括錯誤探測層和中心處理層;錯誤探測層用于探測避障子系統(tǒng)的硬件和軟件錯誤，確定上述障礙感知傳感器處理節(jié)點和/或算法運算節(jié)點出現(xiàn)錯誤時，屏蔽出現(xiàn)錯誤的節(jié)點；中心處理層，對局部算法處理層中若干個算法運算節(jié)點的結(jié)果進行優(yōu)化處理。10.—種基于多傳感器的自主避障導航系統(tǒng)，其特征在于:包括定位繪圖子系統(tǒng)、避障子系統(tǒng);所述定位繪圖子系統(tǒng)與權(quán)利要求2-5中任意一項自主避障導航系統(tǒng)中的定位繪圖子系統(tǒng)相同；所述避障子系統(tǒng)與權(quán)利要求6-9中任意一項自主避障導航系統(tǒng)中的避障子系統(tǒng)相同。
【文檔編號】G01C21/20GK105910604SQ201610351904
【公開日】2016年8月31日
【申請日】2016年5月25日
【發(fā)明人】李金輝, 巨輝, 胡進
【申請人】武漢卓拔科技有限公司