本發(fā)明涉及智能電站巡檢智能體的定位與環(huán)境感知技術(shù),尤其是涉及一種多粒度智能體的定位與環(huán)境感知方法。
背景技術(shù):
要實(shí)現(xiàn)智能電站智能體自主巡檢,需要實(shí)現(xiàn)智能體的自主定位和對(duì)特定場(chǎng)景的物理環(huán)境進(jìn)行感知。其關(guān)鍵技術(shù)是同步定位與建圖(simultaneouslocalizationandmapping,簡(jiǎn)稱(chēng)slam).這種技術(shù)的核心是在無(wú)需任何先驗(yàn)知識(shí)的情況下實(shí)時(shí)、同步地對(duì)完全陌生的環(huán)境實(shí)現(xiàn)目標(biāo)的定位與環(huán)境模型構(gòu)建.由于光學(xué)傳感器相對(duì)廉價(jià)且能夠提供豐富的信息,也更容易模擬人類(lèi)認(rèn)知世界的過(guò)程,所以通常被作為slam的感知設(shè)備.在定位方面,以視覺(jué)里程計(jì)(visualodometry,簡(jiǎn)稱(chēng)vo)定位最為常見(jiàn).同時(shí),使用慣性測(cè)量單元(inertialmeasurementunit,簡(jiǎn)稱(chēng)imu)實(shí)時(shí)測(cè)量并更新相機(jī)的位姿信息并輸出運(yùn)動(dòng)增量用以配合解決視覺(jué)里程計(jì)的漂移誤差的方法,稱(chēng)之為視覺(jué)慣性導(dǎo)航(visualinternalodometry,簡(jiǎn)稱(chēng)vio)也逐步成為多傳感器集成時(shí)代的首選方案.通過(guò)基于濾波器如擴(kuò)展卡爾曼濾波器(extendkalmanfilter,簡(jiǎn)稱(chēng)ekf)方法或者基于關(guān)鍵幀的光束平差法(bundleadjustment,簡(jiǎn)稱(chēng)ba)姿態(tài)融合與優(yōu)化,移動(dòng)工具能夠?qū)崟r(shí)根據(jù)攝像機(jī)輸入的圖像幀來(lái)確定自身當(dāng)前的位置和朝向以及移動(dòng)的距離.以此數(shù)據(jù)作為依據(jù),可以對(duì)導(dǎo)航提供有效的數(shù)據(jù)支撐.
傳統(tǒng)環(huán)境模型多以單粒度模型為主,而單粒度模型自身存在各異構(gòu)圖像傳感器的固有不足,且受模型固有噪聲、隨機(jī)噪聲的影響較大.多粒度模型的主要困難則在于如何統(tǒng)一多粒度模型的表示使其具有統(tǒng)一的語(yǔ)義背景.本案方法使用擴(kuò)展卡爾曼濾波融合gps、慣性傳感器和圖像傳感器三者的數(shù)據(jù)用于定位,與此同時(shí)使用卡爾曼濾波融合通過(guò)單目、多目、紅外圖像傳感器生成的稀疏(sparse)、半稠密(semidense)、稠密(dense)環(huán)境模型,進(jìn)而同步生成融合后的統(tǒng)一多粒度概率八叉樹(shù)網(wǎng)格地圖,實(shí)現(xiàn)不同輔助狀態(tài)下所需場(chǎng)景粒度的導(dǎo)航.對(duì)于導(dǎo)航中的路徑規(guī)劃本發(fā)明提出了一種基于威脅度和d*算法的動(dòng)態(tài)路徑規(guī)劃算法,這種算法可以充分利用上述定位與建圖的信息并最終實(shí)現(xiàn)精確智能的自主導(dǎo)航.這種導(dǎo)航方法不但能夠在變電站環(huán)境下成功避開(kāi)障礙物并規(guī)劃路徑,幫助自主巡檢智能體完成從初始結(jié)點(diǎn)到目標(biāo)結(jié)點(diǎn)的路徑規(guī)劃,實(shí)現(xiàn)真正意義的無(wú)先驗(yàn)知識(shí)環(huán)境感知,與此同時(shí)還能夠在導(dǎo)航過(guò)程中規(guī)避隨機(jī)出現(xiàn)的危險(xiǎn).在保證實(shí)時(shí)性和可靠性的同時(shí)給自主巡檢智能體的環(huán)境感知提供了理論基礎(chǔ).相對(duì)于傳統(tǒng)成熟的gps導(dǎo)航,這種基于視覺(jué)信息的細(xì)粒度導(dǎo)航不但不受衛(wèi)星信號(hào)強(qiáng)度和磁場(chǎng)的干擾,可以在變電站環(huán)境中進(jìn)行,而且還具備應(yīng)對(duì)隨機(jī)產(chǎn)生移動(dòng)物體突發(fā)事件的能力.
技術(shù)實(shí)現(xiàn)要素:
本發(fā)明所要解決的技術(shù)問(wèn)題(發(fā)明目的)
傳統(tǒng)方式的室外建圖與導(dǎo)航一般都采用單一傳感器實(shí)現(xiàn)自身位置和環(huán)境的感知.例如gps傳感器獲取地理坐標(biāo)或者激光測(cè)距儀獲取當(dāng)前智能體和障礙物之間的距離.這種方式容易受到環(huán)境及天氣的限制.例如在變電站等強(qiáng)電磁場(chǎng)環(huán)境下gps模塊失效,在雷電暴雨等惡劣天氣條件下激光測(cè)距儀測(cè)量數(shù)據(jù)不準(zhǔn)確等問(wèn)題.而視覺(jué)傳感器獲取的圖像數(shù)據(jù)雖然可以規(guī)避以上問(wèn)題,但是其處理數(shù)據(jù)量大且對(duì)計(jì)算存儲(chǔ)資源的消耗可觀.為了解決單一傳感器在室外導(dǎo)航中的缺陷,本案提出了一種基于擴(kuò)展卡爾曼濾波的同步定位與建圖vio方法.對(duì)由不同傳感器對(duì)真實(shí)環(huán)境感知所生成的稀疏、半稠密、稠密環(huán)境模型提出一種統(tǒng)一的概率八叉樹(shù)表示,并通過(guò)卡爾曼濾波融合,避免了單一模型的固有噪聲和隨機(jī)噪聲.最后根據(jù)上述八叉樹(shù)網(wǎng)格地圖和本文定義的基于d*算法考慮威脅度的動(dòng)態(tài)路徑規(guī)劃模型,完成對(duì)巡檢智能體多粒度導(dǎo)航并實(shí)現(xiàn)了一個(gè)自主移動(dòng)工具原型.
本發(fā)明提供的完整技術(shù)方案(發(fā)明方案)
自主導(dǎo)航的基礎(chǔ)是高度擬合真實(shí)環(huán)境的三維模型.智能電站自主巡檢智能體通過(guò)各種異構(gòu)相機(jī)獲取外界圖像,得到的絕大部分環(huán)境視頻感知數(shù)據(jù)都來(lái)自于變電站設(shè)備、道路等復(fù)雜場(chǎng)景.傳統(tǒng)環(huán)境模型的建立多以單一分辨率進(jìn)行,生成的單粒度模型如稀疏、半稠密、稠密模型存在各異構(gòu)圖像傳感器的固有不足,且受模型固有噪聲、隨機(jī)噪聲的影響較大,難以全面的描述上述復(fù)雜場(chǎng)景.對(duì)于多粒度模型而言,建立的主要困難則在于如何統(tǒng)一各種粒度模型使其具有統(tǒng)一的語(yǔ)義背景.為了解決上述問(wèn)題,本案提出一種多粒度環(huán)境的概率八叉樹(shù)的統(tǒng)一表示,并根據(jù)室內(nèi)外感知特性選擇特定粒度的模型進(jìn)行融合.如:在室內(nèi)場(chǎng)景的定位和導(dǎo)航中,由于絕大多數(shù)障礙物距離與被導(dǎo)航對(duì)象的間距在紅外相機(jī)的有效工作范圍內(nèi),因此使用紅外深度相機(jī)獲取的深度稠密地圖和單目rgb相機(jī)獲取的圖像建立稀疏地圖;在室外場(chǎng)景的定位和導(dǎo)航中,為了克服隨機(jī)噪聲的影響和獲取較遠(yuǎn)障礙物的圖像信息,使用雙目相機(jī)和單目相機(jī)分別建立半稠密和稀疏地圖.另一方面,通過(guò)分析視覺(jué)里程計(jì)的數(shù)據(jù)融合方法,最終提出適用于室內(nèi)建圖導(dǎo)航的紅外稠密加單目稀疏模型和室外導(dǎo)航的單目半稠密加雙目稀疏模型實(shí)現(xiàn)定位、建圖和導(dǎo)航.整個(gè)導(dǎo)航方法的結(jié)構(gòu)如圖1所示.圖中硬件輸入部分為單目、雙目、紅外深度相機(jī).整個(gè)系統(tǒng)建立有世界坐標(biāo)系g,避障坐標(biāo)系m和工具坐標(biāo)系.其中世界坐標(biāo)系中建立的永久地圖用于回環(huán)檢測(cè)和重定位,避障坐標(biāo)系采用深度相機(jī)等傳感器建立的臨時(shí)稠密地圖配合視覺(jué)慣性里程計(jì)返回在該坐標(biāo)系下的相機(jī)位姿信息,用來(lái)規(guī)避路徑中存在的障礙物等.這兩個(gè)坐標(biāo)系通過(guò)在世界坐標(biāo)系中的基線幀在世界坐標(biāo)系和避障坐標(biāo)系之間相互轉(zhuǎn)換,并配合工具坐標(biāo)系上的定位與建圖并在gps導(dǎo)航的輔助下完成地圖和姿態(tài)的坐標(biāo)轉(zhuǎn)換.兩個(gè)坐標(biāo)系的相互配合使得世界坐標(biāo)系無(wú)需存儲(chǔ)耗費(fèi)巨大的稠密地圖,且能夠利用稠密地圖為避障提供最豐富的決策信息,這樣可以大大節(jié)省計(jì)算消耗并提高系統(tǒng)的時(shí)效性.
本案在深入研究現(xiàn)有算法的基礎(chǔ)上,提出了基于光學(xué)圖像的多粒度隨動(dòng)環(huán)境感知算法.該算法采集多種異構(gòu)圖像傳感器及相應(yīng)visualslam算法所生成的稀疏、半稠密、稠密多種粒度的異構(gòu)三維點(diǎn)云模型,同時(shí)提出一種概率八叉樹(shù)統(tǒng)一表示生成的若干三維模型.然后通過(guò)卡爾曼濾波,在相機(jī)運(yùn)動(dòng)期間不斷融合多種異構(gòu)點(diǎn)云的置信度并更新時(shí)態(tài)融合概率八叉樹(shù)模型(temporalfusedprobabilisticoctreemodel,簡(jiǎn)稱(chēng)tfpom),這種融合方法既可以保證環(huán)境模型的時(shí)空一致性又可以滿(mǎn)足后續(xù)增量更新的需求.同時(shí)利用一種有效的剪枝和歸并策略,在壓縮模型存儲(chǔ)空間的同時(shí)使環(huán)境模型能夠以任意粒度動(dòng)態(tài)擬合真實(shí)環(huán)境,最終實(shí)現(xiàn)魯棒的隨動(dòng)環(huán)境感知,實(shí)現(xiàn)多傳感器融合的多粒度導(dǎo)航.
在定位過(guò)程中本案使用slam思想實(shí)現(xiàn),姿態(tài)圖作為存儲(chǔ)定位與姿態(tài)約束信息的載體,便于ba的優(yōu)化.視覺(jué)里程計(jì)通過(guò)經(jīng)典的擴(kuò)展卡爾曼濾波方法實(shí)時(shí)得出較為精確的變換矩陣,加入姿態(tài)圖中并計(jì)算出6自由度的智能體姿態(tài),從而完成定位的過(guò)程.本案所用姿態(tài)圖的特點(diǎn)就是采用了在全局坐標(biāo)系中跟子坐標(biāo)系維持轉(zhuǎn)換關(guān)系的基線幀,所有當(dāng)前結(jié)點(diǎn)和地標(biāo)(landmark)位置都在局部障礙坐標(biāo)系的地圖中表示,這就使得可以通過(guò)與基準(zhǔn)幀之間的轉(zhuǎn)換量來(lái)完成子坐標(biāo)系和全局坐標(biāo)系的對(duì)齊操作,而不需要每次都更新地圖中的關(guān)鍵幀和地標(biāo).正由于此便可維持相對(duì)固定的參考地圖,當(dāng)新的建圖信息到來(lái)時(shí)再由局部地圖和參考地圖間的轉(zhuǎn)換量去更新全局地圖.
當(dāng)巡檢智能體的移動(dòng)路徑不包含細(xì)粒度的移動(dòng)姿態(tài)時(shí),例如指定了從變電站入口按照一定規(guī)則移動(dòng)到出口而不會(huì)給出需要如何轉(zhuǎn)彎和傾斜.這個(gè)時(shí)候才需要稠密建圖介入來(lái)完成位姿的估計(jì)和判斷實(shí)現(xiàn)這種細(xì)粒度地導(dǎo)航.而為了實(shí)現(xiàn)該粒度的建圖和導(dǎo)航,需要結(jié)合特征點(diǎn)立體匹配來(lái)預(yù)測(cè)相機(jī)姿態(tài).由于存儲(chǔ)稠密地圖需要耗費(fèi)巨大的空間和計(jì)算資源只有一小部分關(guān)鍵幀被保存下來(lái).這樣就只能根據(jù)對(duì)導(dǎo)航有關(guān)部分的信息來(lái)建立局部地圖.在圖像幀間和攝像機(jī)不同視角獲取的幀中包含有特征點(diǎn)、特征點(diǎn)描述子和跟蹤特征點(diǎn)時(shí)所用的三維地標(biāo),形成了一個(gè)由關(guān)鍵幀作為頂點(diǎn)和其他深度信息作為邊的姿態(tài)圖.
為了保持局部里程計(jì)框架的一致性,我們只對(duì)基準(zhǔn)坐標(biāo)系進(jìn)行修改.對(duì)已建地圖進(jìn)行重定位之前需要將局部地圖添加到參考地圖上.為了在通過(guò)光束平差法進(jìn)行圖優(yōu)化時(shí)能夠增量地進(jìn)行地圖重建,我們通過(guò)一個(gè)框架來(lái)實(shí)現(xiàn)基于事務(wù)的(transaction-based)多線程訪問(wèn)地圖更新.但是由于光束平差法本身的特點(diǎn)導(dǎo)致圖中頂點(diǎn)的位置會(huì)經(jīng)常出現(xiàn)大幅度變化,當(dāng)創(chuàng)建新的局部坐標(biāo)時(shí)僅采用最近一個(gè)參考坐標(biāo)作為參考.詳見(jiàn)公式(1)(2):
其中tva是執(zhí)行光束平差法之前參考坐標(biāo)系中最后一個(gè)頂點(diǎn)6自由度的偏移量,tvb是執(zhí)行之后的偏移量.tgml是相對(duì)于參考坐標(biāo)系的基準(zhǔn)幀偏移量而tgmr是相對(duì)于局部坐標(biāo)系的基準(zhǔn)幀偏移量更新值.
擁有一個(gè)執(zhí)行過(guò)光束平差固定參考坐標(biāo)的好處就是能夠通過(guò)定位來(lái)修正視覺(jué)慣導(dǎo)的漂移誤差.這是通過(guò)依靠在特征點(diǎn)描述子區(qū)域做近鄰匹配來(lái)實(shí)現(xiàn)的,隨后使用ransac機(jī)制的魯棒估計(jì)方法來(lái)剔除野點(diǎn).而當(dāng)前流程中所有內(nèi)點(diǎn)會(huì)被當(dāng)做邊約束加入到姿態(tài)圖的頂點(diǎn)之間,同時(shí)在地圖中三角測(cè)量特征點(diǎn)的三維坐標(biāo).相對(duì)于自主移動(dòng)工具的運(yùn)動(dòng)而言姿態(tài)漂移的發(fā)生是很緩慢的,我們并不需要按照姿態(tài)預(yù)測(cè)的頻率執(zhí)行重定位操作.只需要每到一個(gè)新的關(guān)鍵幀便逆著全局地圖執(zhí)行回環(huán)檢測(cè),通過(guò)對(duì)滑動(dòng)窗口大小的關(guān)鍵幀中關(guān)鍵幀和地標(biāo)的約束執(zhí)行非線性最小二乘優(yōu)化更新定位預(yù)測(cè).姿態(tài)圖頂點(diǎn)的位置和地圖中三角測(cè)量地標(biāo)的三維坐標(biāo)作為固定值保留下來(lái),而只有局部坐標(biāo)的基礎(chǔ)框架轉(zhuǎn)化是非固定值.唯一的殘余量是參考坐標(biāo)系中二維特征點(diǎn)和三維地標(biāo)間的重投影誤差.
在建圖過(guò)程中本案引入了八叉樹(shù)表示方法.八叉樹(shù)是一種將環(huán)境空間迭代細(xì)分為八維子空間的存儲(chǔ)表示方法.在環(huán)境感知中,八叉樹(shù)的葉子節(jié)點(diǎn)存儲(chǔ)所有的環(huán)境體素,非葉子節(jié)點(diǎn)稱(chēng)為內(nèi)點(diǎn)(innernode),在插入、剪枝、歸并或變更分辨率等樹(shù)操作后可能轉(zhuǎn)換為葉子節(jié)點(diǎn)或高層節(jié)點(diǎn),如圖2和圖3所示.環(huán)境空間內(nèi)的立體度量單位在轉(zhuǎn)換成三維模型中的一個(gè)體素后可以分為三種狀態(tài).其中一種稱(chēng)為未知狀態(tài),表示該體素在當(dāng)前時(shí)刻之前相機(jī)尚未捕捉到的環(huán)境區(qū)域中.剩余兩種相互制約狀態(tài)分別稱(chēng)為占有和空閑,表示該體素對(duì)應(yīng)的立體度量單位在當(dāng)前時(shí)刻被認(rèn)為有實(shí)體占有和被認(rèn)為無(wú)實(shí)體占有.
在實(shí)際對(duì)環(huán)境的感知過(guò)程中,受各種異構(gòu)相機(jī)內(nèi)部噪聲和環(huán)境外部噪聲產(chǎn)生共同影響,當(dāng)前時(shí)刻體素的狀態(tài)難以由三元狀態(tài)完全解釋.概率八叉樹(shù)在第i個(gè)葉子節(jié)點(diǎn)使用logodds值l(i),如式(3)所示:
表示對(duì)占有狀態(tài)的置信度.p(i)的初始值一般設(shè)置為0.5,表示處于置信度不明確狀態(tài).根據(jù)三元狀態(tài)的定義,該葉子節(jié)點(diǎn)空閑狀態(tài)的置信度為1-l(i),而未知狀態(tài)則會(huì)被預(yù)先標(biāo)識(shí).而這種置信度關(guān)系隨著相機(jī)移動(dòng)可能會(huì)產(chǎn)生三種狀態(tài)的置信度變化,例如狀態(tài)為未知的環(huán)境區(qū)域被攝像機(jī)在后續(xù)時(shí)刻捕捉到而轉(zhuǎn)換為其他兩種狀態(tài)或者非剛體環(huán)境中體素隨時(shí)態(tài)在占有和未知狀態(tài)相互轉(zhuǎn)換.概率八叉樹(shù)使用式(2)更新時(shí)間序列節(jié)點(diǎn)t上的logodds值l(i|z1:t),其中z1:t為傳感器在當(dāng)前時(shí)刻的累計(jì)度量,l(n|zt)根據(jù)異構(gòu)模型的固有置信特性設(shè)置,如式(4)所示.因?yàn)檫@種logit轉(zhuǎn)換可逆,更新過(guò)程可以被解釋為一種概率度量更新.
其中logodds值通過(guò)反logit變換,如式(5)所示:
轉(zhuǎn)換成p(n|z1:t),并由式(6)表示概率更新過(guò)程:
稀疏、半稠密和稠密模型經(jīng)過(guò)上述過(guò)程后轉(zhuǎn)化為統(tǒng)一的概率八叉樹(shù)表示.在相機(jī)時(shí)間序列中,若干稀疏、半稠密和稠密的概率八叉樹(shù)模型在同步定位與建圖的每個(gè)時(shí)刻不斷生成.在環(huán)境感知中,八叉樹(shù)的葉子節(jié)點(diǎn)存儲(chǔ)所有的環(huán)境體素,非葉子節(jié)點(diǎn)稱(chēng)為內(nèi)點(diǎn),在插入、剪枝、歸并或變更分辨率等樹(shù)操作后可能轉(zhuǎn)換為葉子節(jié)點(diǎn)或高層節(jié)點(diǎn),如圖2所示,相機(jī)在沿軌跡運(yùn)動(dòng)的過(guò)程中算法不斷以多粒度感知環(huán)境.由此,在時(shí)間序列中的每個(gè)時(shí)間節(jié)點(diǎn),由不同異構(gòu)模型所生成的若干概率八叉樹(shù)模型如何有機(jī)融合成統(tǒng)一的時(shí)態(tài)融合概率八叉樹(shù)模型成為了下一步需要解決的問(wèn)題.
本案提出一種基于卡爾曼濾波的置信融合方法,設(shè)第i個(gè)概率八叉樹(shù)模型的第j個(gè)葉子節(jié)點(diǎn)在k時(shí)刻的形式化表示為
置信度融合算法首先引入一個(gè)離散系統(tǒng)的控制模型和測(cè)量方程,分別如式(7),式(8)所示,其中
下一步使用系統(tǒng)的過(guò)程模型,如式(9)所示,預(yù)測(cè)下一時(shí)刻的系統(tǒng)狀態(tài).
更新?tīng)顟B(tài)
計(jì)算每個(gè)觀測(cè)值的卡爾曼增益,如式(11)所示.
結(jié)合觀測(cè)值更新?tīng)顟B(tài)估計(jì)
最后更新k+1時(shí)刻的誤差估計(jì)
為可視化算法的完整過(guò)程,對(duì)生成的點(diǎn)云地圖生成統(tǒng)一的概率八叉樹(shù)表示.接著對(duì)上步每個(gè)時(shí)間節(jié)點(diǎn)所生成的若干概率八叉樹(shù)融合.最后通過(guò)時(shí)間序列地不斷增量融合,完整的多粒度表示環(huán)境模型,如圖4所示.該方法的導(dǎo)航部分依賴(lài)于經(jīng)過(guò)以上定位和概率八叉樹(shù)建圖過(guò)程得到的包含了慣導(dǎo)單元測(cè)量數(shù)據(jù)的柵格地圖.傳統(tǒng)的dijkstra算法以出發(fā)點(diǎn)為中心向外層層擴(kuò)展直到終點(diǎn),能得到最短路徑最優(yōu)解,但是遍歷節(jié)點(diǎn)多,計(jì)算代價(jià)太大不適用于嵌入式設(shè)備.a*算法是靜態(tài)路網(wǎng)路徑規(guī)劃中最有效的方法,但是由于智能輔助系統(tǒng)的應(yīng)用環(huán)境是動(dòng)態(tài)的,例如會(huì)有人從攝像機(jī)前走過(guò)或者車(chē)輛掠過(guò),且定位和建圖過(guò)程也是動(dòng)態(tài)進(jìn)行的,因此這個(gè)柵格地圖是動(dòng)態(tài)變化的.因此只能選用d*算法作為路徑規(guī)劃的方法.而慣導(dǎo)單元測(cè)量后的數(shù)據(jù)經(jīng)過(guò)和視覺(jué)信息的融合還提供了障礙信息,因此在這個(gè)地圖中還有需要避開(kāi)的障礙物.這些障礙物有可能是行人、車(chē)輛或者建筑物等不同形狀和體積的物體,本文將這些不規(guī)則的障礙物簡(jiǎn)化為具有一定作用半徑的圓,而由于這些障礙有的是在緩慢移動(dòng)或者高低變化,因此對(duì)自主移動(dòng)工具的威脅也是不確定的,需要對(duì)每個(gè)障礙圓留有一定的余度.定義o是智能輔助系統(tǒng)受到的障礙代價(jià),r是障礙物的有效作用半徑,d是攝像機(jī)到障礙中心點(diǎn)的距離,s根據(jù)自主移動(dòng)工具特性設(shè)計(jì)的安全距離,威脅度建??捎檬?15)表示:
由于d*算法是在a*算法和dijkstra算法的基礎(chǔ)上建立起來(lái)適合含有不確定因素地圖的路徑規(guī)劃算法,在考慮威脅度時(shí)需要選取合適的代價(jià)函數(shù).我們令g(x,r)是點(diǎn)x到攝像機(jī)當(dāng)前位置的估計(jì)代價(jià),o(x,r)是x到目標(biāo)點(diǎn)r的障礙代價(jià).則代價(jià)函數(shù)c(x,r)可以用式(16)表示:
具體實(shí)施方法
下面結(jié)合附圖和實(shí)施例子對(duì)本發(fā)明做進(jìn)一步說(shuō)明。
如圖5所示為一個(gè)智能電站自主巡檢智能體的應(yīng)用實(shí)例,包括一個(gè)搭載nvidiadevelopmentkittx1嵌入式設(shè)備的四輪小車(chē),其中tx1負(fù)責(zé)運(yùn)行本發(fā)明方法并通過(guò)舵機(jī)控制輪軸實(shí)現(xiàn)自主移動(dòng)及導(dǎo)航.相關(guān)環(huán)境、運(yùn)動(dòng)感知傳感器包含紅外深度相機(jī)kinect(用于室內(nèi))、雙目攝像頭(用于室外)、板載單目攝像頭.在智能體向目標(biāo)節(jié)點(diǎn)移動(dòng)過(guò)程中該算法檢查最短路徑上下一節(jié)點(diǎn)的變化情況。這個(gè)變化是由slam建圖和慣導(dǎo)單元測(cè)量數(shù)據(jù)綜合而得的.但是d*算法對(duì)于距離遠(yuǎn)的最短路徑發(fā)生的變化并不敏感,由于慣導(dǎo)單元測(cè)量距離的局限不能感應(yīng)遠(yuǎn)距離的變化,因此這并不影響算法的正確執(zhí)行和路徑規(guī)劃.具體的路徑規(guī)劃算法如下所示:
1.創(chuàng)建兩個(gè)表open,close.open表保存所有已生成但未考察的節(jié)點(diǎn),close表保存已訪問(wèn)過(guò)的節(jié)點(diǎn).
2.用dijsktra算法從目標(biāo)節(jié)點(diǎn)d開(kāi)始向起始節(jié)點(diǎn)搜索.儲(chǔ)存路網(wǎng)中目標(biāo)節(jié)點(diǎn)到各個(gè)節(jié)點(diǎn)的最短路和該位置到目標(biāo)點(diǎn)的實(shí)際值h,并設(shè)置中間變量k存儲(chǔ)變化中h的最小值,當(dāng)前為k=h.
3.沿最短路徑開(kāi)始移動(dòng),在下一節(jié)點(diǎn)沒(méi)有變化時(shí)利用上一步計(jì)算出的最短路徑信息從出發(fā)點(diǎn)向后追蹤.定義x為下一節(jié)點(diǎn),y是當(dāng)前節(jié)點(diǎn).當(dāng)探測(cè)到x狀態(tài)發(fā)生改變時(shí),調(diào)整當(dāng)前位置y到目標(biāo)節(jié)點(diǎn)d的實(shí)際值h(y),由x節(jié)點(diǎn)到y(tǒng)節(jié)點(diǎn)的新代價(jià)權(quán)值c(x,y)與原權(quán)值h(x)相加得到.
4.使用a*算法遍歷y的子節(jié)點(diǎn)并放入close表中.令y的子節(jié)點(diǎn)為a,則h(a)=h(y)+c(y,a),其中h(a)為子節(jié)點(diǎn)a的h值,c(y,a)為y到子節(jié)點(diǎn)a的權(quán)重代價(jià).
5.從open表中取k值最小的節(jié)點(diǎn)y,遍歷y的子節(jié)點(diǎn)a并計(jì)算h(a)=h(y)+c(y,a).
6.如果a節(jié)點(diǎn)在open表中,向下繼續(xù)執(zhí)行.如果在close表中跳轉(zhuǎn)到步驟8,如果都不在則跳轉(zhuǎn)至步驟9.
7.如果計(jì)算所得a的權(quán)值h(a)小于open表中存儲(chǔ)的權(quán)重值,則更新open表中相應(yīng)的值為新計(jì)算的最小值.
8.如果計(jì)算所得a的權(quán)值h(a)小于close表中存儲(chǔ)的權(quán)重值,則更新close表中相應(yīng)的值為新計(jì)算的最小值并將a節(jié)點(diǎn)放入open表中.
9.將a節(jié)點(diǎn)插入open表中.
10.將y節(jié)點(diǎn)放入close表中.
11.輸出open表中所有節(jié)點(diǎn)并按照k值大小進(jìn)行排序.
12.使用第一步dijsktra計(jì)算出的最短路徑信息生成從當(dāng)前位置到目標(biāo)節(jié)點(diǎn)d的路徑.
在變電站真實(shí)場(chǎng)景,智能巡檢體通過(guò)一段包含較多障礙物的站內(nèi)崎嶇道路.行走的起始節(jié)點(diǎn)與終止節(jié)點(diǎn)間路線相對(duì)固定,但需要實(shí)現(xiàn)繞行直角轉(zhuǎn)彎并在極為狹窄的道路間行進(jìn).這對(duì)多傳感器數(shù)據(jù)融合是巨大的挑戰(zhàn).盡管本發(fā)明只對(duì)其中定位、建圖和導(dǎo)航的具體實(shí)施方式進(jìn)行了描述,但是本領(lǐng)域普通技術(shù)人員應(yīng)當(dāng)了解到本發(fā)明可以在不偏離其主旨與范圍內(nèi)以許多其他的形式實(shí)施。因此,所展示的例子與實(shí)施方式被視為示意性的而非限制性的,在不脫離如所附各權(quán)利要求所定義的本發(fā)明精神及范圍的情況下,本發(fā)明可能涵蓋各種修改與替換。