基于改進(jìn)人工勢(shì)場(chǎng)法的路徑規(guī)劃導(dǎo)航系統(tǒng)及方法
【專(zhuān)利摘要】本發(fā)明公開(kāi)了一種基于改進(jìn)人工勢(shì)場(chǎng)的煤礦救災(zāi)機(jī)器人路徑規(guī)劃導(dǎo)航系統(tǒng)及方法，將障礙物填充法和擾動(dòng)勢(shì)場(chǎng)法相結(jié)合，障礙物填充法通過(guò)對(duì)凹障礙物進(jìn)行虛擬填充，構(gòu)造填充勢(shì)場(chǎng)函數(shù)，產(chǎn)生相應(yīng)的排斥勢(shì)場(chǎng)函數(shù)；擾動(dòng)勢(shì)場(chǎng)通過(guò)在引力勢(shì)場(chǎng)中加入擾動(dòng)勢(shì)場(chǎng)，改變引力勢(shì)場(chǎng)函數(shù)，使救災(zāi)機(jī)器人在陷入局部極小點(diǎn)時(shí)，自主走出局部極小點(diǎn)；兩種方法相結(jié)合，可以很好地幫助井下救災(zāi)機(jī)器人逃離局部極小點(diǎn)，重新規(guī)劃路徑，避免再次陷入其他局部極小點(diǎn)，成功完成救災(zāi)救險(xiǎn)任務(wù)，算法簡(jiǎn)潔并且有很高的可靠性。
【專(zhuān)利說(shuō)明】
基于改進(jìn)人工勢(shì)場(chǎng)法的路徑規(guī)劃導(dǎo)航系統(tǒng)及方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及一種機(jī)器人路徑規(guī)劃導(dǎo)航系統(tǒng)及方法，具體是應(yīng)用煤礦救災(zāi)機(jī)器人 的，采用改進(jìn)人工勢(shì)場(chǎng)多技術(shù)融合的路徑規(guī)劃方法。
【背景技術(shù)】
[0002] 煤礦井下礦難發(fā)生時(shí)，采用機(jī)器人進(jìn)行探測(cè)和救援是可靠有效的途徑，針對(duì)井下 機(jī)器人的關(guān)鍵問(wèn)題，如供電、防爆、機(jī)器人步履機(jī)械柔性等研究，通過(guò)科研人員長(zhǎng)期不懈的 努力，已取得了長(zhǎng)足的進(jìn)步。煤礦井下環(huán)境特殊，巷道相互交叉，空間狹小封閉，機(jī)器人路徑 規(guī)劃一直是制約機(jī)器人在煤礦井下應(yīng)用的難題，救災(zāi)機(jī)器人作為一種移動(dòng)機(jī)器人，其路徑 規(guī)劃的水平一定程度上決定了救災(zāi)的效率。礦井救災(zāi)機(jī)器人的任務(wù)是，在一定具有動(dòng)態(tài)和 靜態(tài)的未知環(huán)境中，尋找一條沒(méi)有碰撞的最佳路徑，同時(shí)要滿足相應(yīng)參數(shù)的優(yōu)化，例如，最 短路徑，最短時(shí)間，最低能耗等。傳統(tǒng)的人工勢(shì)場(chǎng)是由Khabit 1986年提出來(lái)的一種虛擬牛 頓引力的方法，起初是為了解決機(jī)器人在抓取物體的時(shí)候，其手臂可以不觸及工作臺(tái)，后來(lái) 運(yùn)用到機(jī)器人運(yùn)動(dòng)避障中。其主要思想是將機(jī)器人、目標(biāo)點(diǎn)和障礙物簡(jiǎn)化成點(diǎn)，目標(biāo)點(diǎn)對(duì)機(jī) 器人產(chǎn)生引力，障礙物對(duì)機(jī)器人產(chǎn)生斥力，在整個(gè)環(huán)境中，通過(guò)合力的作用控制機(jī)器人運(yùn) 動(dòng)，進(jìn)行路徑規(guī)劃。人工勢(shì)場(chǎng)法是傳統(tǒng)路徑規(guī)劃方法中較為成熟且簡(jiǎn)潔高效的算法，其主要 思路是通過(guò)目標(biāo)點(diǎn)對(duì)機(jī)器人產(chǎn)生引力，障礙物對(duì)機(jī)器人產(chǎn)生斥力，以及機(jī)器人之間相互作 用力，形成一個(gè)智能的人工勢(shì)場(chǎng)，對(duì)機(jī)器人進(jìn)行實(shí)時(shí)的路徑進(jìn)行規(guī)劃。但傳統(tǒng)的人工勢(shì)場(chǎng)存 在局部極小點(diǎn)的問(wèn)題，當(dāng)障礙物在目標(biāo)點(diǎn)附近時(shí)，還會(huì)出現(xiàn)機(jī)器人在目標(biāo)點(diǎn)周?chē)鹗?，?dǎo)致 機(jī)器人無(wú)法到目標(biāo)點(diǎn)的問(wèn)題。
[0003] 目前，根據(jù)人工勢(shì)場(chǎng)的局部極小點(diǎn)問(wèn)題，常用的解決辦法有：障礙物填充法，探測(cè) 法，機(jī)器人加速度參考法，距離參數(shù)參考法，等勢(shì)場(chǎng)線法，F(xiàn)ollow wall等。
[0004] 障礙物填充法:通過(guò)對(duì)凹形障礙物進(jìn)行虛擬填充，產(chǎn)生新的斥力勢(shì)場(chǎng)函數(shù)，避免 機(jī)器人再次陷入凹形障礙物的局部極小點(diǎn)，缺點(diǎn)是很難及時(shí)判斷障礙物的形狀，往往會(huì)判 斷失誤導(dǎo)致避障失敗。
[0005] 探測(cè)法:通過(guò)給機(jī)器人輸入常見(jiàn)產(chǎn)生局部極小點(diǎn)的障礙物模型，利用機(jī)器人自帶 的探測(cè)裝置，探測(cè)前方是否存在可能產(chǎn)生局部極小點(diǎn)的障礙物，提前進(jìn)行路徑規(guī)劃避障;缺 點(diǎn)是由于機(jī)器人的探測(cè)裝置探測(cè)范圍有限，碰到煤礦井下的大型障礙物，往往無(wú)法提前探 測(cè)到完整的障礙物，最終導(dǎo)致避障失敗。
[0006] 機(jī)器人加速度參考法:機(jī)器人加速度是反映機(jī)器人是否將要進(jìn)入局部極小點(diǎn)的重 要參數(shù)，將機(jī)器人的加速度參數(shù)加入到斥力函數(shù)中，通過(guò)機(jī)器人加速度大小來(lái)隨時(shí)調(diào)節(jié)斥 力函數(shù)，避免機(jī)器人陷入局部極小點(diǎn)，從而完成避障;缺點(diǎn)是算法比較復(fù)雜，往往會(huì)因?yàn)榧?速度誤把目標(biāo)位置當(dāng)做局部極小點(diǎn)來(lái)處理。
[0007] 距離參數(shù)參考法:通過(guò)將機(jī)器人與目標(biāo)位置的距離信息引入到斥力函數(shù)中，通過(guò) 兩者距離信息調(diào)節(jié)斥力函數(shù)，避免機(jī)器人陷入局部極小點(diǎn)，從而完成避障;缺點(diǎn)是要時(shí)刻監(jiān) 測(cè)機(jī)器人與目標(biāo)點(diǎn)的位置，算法的復(fù)雜程度增大，算法運(yùn)行效率低，魯棒性差。
[0008] 等勢(shì)場(chǎng)線法:在機(jī)器人進(jìn)行路徑規(guī)劃之前，對(duì)已知的環(huán)境信息進(jìn)行模型構(gòu)建，通過(guò) 等勢(shì)場(chǎng)線，來(lái)確定整個(gè)環(huán)境中勢(shì)場(chǎng)最低的位置，包括局部極小點(diǎn)和目標(biāo)位置，避免機(jī)器人陷 入局部極小點(diǎn)，從而完成避障；缺點(diǎn)是由于要對(duì)環(huán)境信息已知，并且要構(gòu)造環(huán)境模型，不適 于用在礦難后未知的煤礦井下環(huán)境中。
[0009] Follow wall:當(dāng)機(jī)器人到達(dá)局部極小點(diǎn)的時(shí)候，選擇一個(gè)合適的方向，讓機(jī)器人 沿著障礙物的邊緣直到機(jī)器人跳出局部極小點(diǎn)，從而完成避障;缺點(diǎn)是算法復(fù)雜程度高，算 法內(nèi)存占用率高，效率低。
[0010] 可見(jiàn)現(xiàn)有的導(dǎo)航技術(shù)在煤礦井下非結(jié)構(gòu)化環(huán)境中應(yīng)用還需改進(jìn)，因此，研究適合 煤礦井下救災(zāi)機(jī)器人的導(dǎo)航系統(tǒng)及方法具有重要的意義。

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

[0011] 為了解決煤礦井下救災(zāi)機(jī)器人在人工勢(shì)場(chǎng)法中局部極小點(diǎn)問(wèn)題，本發(fā)明提供了一 種基于填充障礙物和擾動(dòng)勢(shì)場(chǎng)融合的煤礦井下救災(zāi)機(jī)器人路徑規(guī)劃技術(shù)。本發(fā)明與以往的 機(jī)器人路徑規(guī)劃技術(shù)相比，采用了原有的填充障礙物法，結(jié)合擾動(dòng)勢(shì)場(chǎng)法，改進(jìn)了救災(zāi)機(jī)器 人在凹形障礙物中逃離局部極小點(diǎn)的性能，該算法能有效的幫助機(jī)器人逃離局部極小點(diǎn)， 并避免機(jī)器人連續(xù)陷入局部極小點(diǎn)，提高了算法的魯棒性和準(zhǔn)確性，降低了算法的復(fù)雜度。
[0012] 本發(fā)明采用的技術(shù)方案如下：
[0013] 本發(fā)明提供了一種基于改進(jìn)人工勢(shì)場(chǎng)的煤礦井下救災(zāi)機(jī)器人的路徑規(guī)劃方法，該 規(guī)劃方法是將原有的障礙物填充法和擾動(dòng)勢(shì)場(chǎng)相結(jié)合，幫助機(jī)器人逃離局部極小點(diǎn)，重新 規(guī)劃避障路徑，避免再次陷入局部極小點(diǎn)；
[0014] 所述局部極小點(diǎn)是指機(jī)器人在人工勢(shì)場(chǎng)法的路徑規(guī)劃方法下，由于引力勢(shì)場(chǎng)和斥 力勢(shì)場(chǎng)的共同作用，在未到達(dá)目標(biāo)的位置，由于所受勢(shì)場(chǎng)合力為零，導(dǎo)致陷入某個(gè)區(qū)域無(wú)法 繼續(xù)完成路徑規(guī)劃；
[0015] 所述障礙物填充法是通過(guò)救災(zāi)機(jī)器人的探測(cè)裝置，對(duì)凹形障礙物進(jìn)行虛擬填充， 使其最終形成較為規(guī)則的障礙物，避免產(chǎn)生局部極小點(diǎn)；
[0016] 所述擾動(dòng)勢(shì)場(chǎng)法是當(dāng)機(jī)器人陷入局部極小點(diǎn)時(shí)，通過(guò)在總勢(shì)場(chǎng)函數(shù)中引入擾動(dòng)勢(shì) 場(chǎng)函數(shù)，改變總勢(shì)場(chǎng)函數(shù)，幫助井下救災(zāi)機(jī)器人走出局部極小點(diǎn)。
[0017] 根據(jù)本發(fā)明所述的基于改進(jìn)人工勢(shì)場(chǎng)的煤礦井下救災(zāi)機(jī)器人的路徑規(guī)劃方法，所 述路徑規(guī)劃方法將已知的障礙物填充法與新提出的擾動(dòng)勢(shì)場(chǎng)法相結(jié)合，當(dāng)機(jī)器人陷入局部 極小點(diǎn)時(shí)，首先通過(guò)增加擾動(dòng)勢(shì)場(chǎng)函數(shù)幫助救災(zāi)機(jī)器人走出局部極小點(diǎn)，此時(shí)進(jìn)行凹形障 礙物填充，構(gòu)造填充勢(shì)場(chǎng)函數(shù)，隨著機(jī)器人逐步走出凹形障礙物，填充面積逐漸增大，此時(shí) 斥力勢(shì)場(chǎng)函數(shù)也逐漸增大，總勢(shì)場(chǎng)函數(shù)發(fā)生改變，救災(zāi)機(jī)器人路徑重新規(guī)劃，直到凹形障礙 物被全部填充，機(jī)器人繞開(kāi)凹形障礙物，最終到達(dá)目標(biāo)位置。
[0018] 根據(jù)本發(fā)明所述的基于改進(jìn)人工勢(shì)場(chǎng)的煤礦井下救災(zāi)機(jī)器人的路徑規(guī)劃方法，所 述擾動(dòng)勢(shì)場(chǎng)函數(shù)與機(jī)器人到目標(biāo)點(diǎn)的直線距離存在正比例關(guān)系，擾動(dòng)勢(shì)場(chǎng)函數(shù)只有在救災(zāi) 機(jī)器人陷入局部極小點(diǎn)時(shí)才啟動(dòng)加入總勢(shì)場(chǎng)函數(shù)，其他時(shí)刻機(jī)器人只按照目標(biāo)點(diǎn)的引力函 數(shù)和障礙物的斥力函數(shù)進(jìn)行路徑規(guī)劃。
[0019] 根據(jù)本發(fā)明所述的基于改進(jìn)人工勢(shì)場(chǎng)的煤礦井下救災(zāi)機(jī)器人的路徑規(guī)劃方法，所 述障礙物將產(chǎn)生新的斥力勢(shì)場(chǎng)函數(shù)，填充范圍隨救災(zāi)機(jī)器人遠(yuǎn)離局部極小點(diǎn)逐步增大，填 充勢(shì)場(chǎng)斥力函數(shù)與機(jī)器人到局部極小點(diǎn)的距離成反比例關(guān)系。
[0020]根據(jù)本發(fā)明所述的基于改進(jìn)人工勢(shì)場(chǎng)的煤礦井下救災(zāi)機(jī)器人的路徑規(guī)劃方法，所 述路徑規(guī)劃方法當(dāng)救災(zāi)機(jī)器人在煤礦井下遇到凹障礙物時(shí)，具體工作步驟如下：
[0021 ]步驟一:將已知的煤礦井下全局信息輸入到路徑規(guī)劃系統(tǒng)中；
[0022]步驟二:探測(cè)障礙物信息，融合已知的煤礦井下全局信息，構(gòu)建障礙物斥力勢(shì)場(chǎng)模 型和目標(biāo)點(diǎn)引力勢(shì)場(chǎng)模型，對(duì)路徑進(jìn)行初步規(guī)劃；
[0023]步驟三:救災(zāi)任務(wù)開(kāi)始，救災(zāi)機(jī)器人開(kāi)始向目標(biāo)點(diǎn)運(yùn)行，直到陷入凹形障礙物局部 極小點(diǎn);此時(shí)機(jī)器人所受合力為零，到達(dá)整個(gè)規(guī)劃路徑中的勢(shì)場(chǎng)最低點(diǎn)；
[0024] 步驟四：開(kāi)始逃離局部極小點(diǎn)，啟動(dòng)擾動(dòng)勢(shì)場(chǎng)函數(shù)，此時(shí)總勢(shì)場(chǎng)函數(shù)改變，機(jī)器人 出現(xiàn)震蕩，暫時(shí)逃離局部極小點(diǎn)；
[0025] 步驟五:為了避免救災(zāi)機(jī)器人再次陷入凹形障礙物的其他局部極小點(diǎn)，開(kāi)始進(jìn)行 凹形障礙物填充，填充范圍隨救災(zāi)機(jī)器人逃離局部極小點(diǎn)逐步擴(kuò)大；
[0026] 步驟六:等待一個(gè)掃描周期過(guò)后，判斷機(jī)器人是否已逃離局部極小點(diǎn)，如果仍陷入 局部極小點(diǎn)，回到步驟二對(duì)現(xiàn)階段各類(lèi)模型進(jìn)行重新建立，重新進(jìn)行路徑規(guī)劃;如果逃離局 部極小點(diǎn)，按照現(xiàn)有勢(shì)場(chǎng)模型繼續(xù)進(jìn)行救災(zāi)任務(wù)；
[0027] 步驟七:機(jī)器人到達(dá)目標(biāo)位置，路徑規(guī)劃結(jié)束。
[0028] 根據(jù)本發(fā)明所述的基于改進(jìn)人工勢(shì)場(chǎng)的煤礦井下救災(zāi)機(jī)器人的路徑規(guī)劃方法，所 述的步驟二中，障礙物斥力勢(shì)場(chǎng)模型為：
[0029] Fatt (X) = -gradUatt (X) = -Katt | X~Xg (1)
[0030] 目標(biāo)點(diǎn)引力勢(shì)場(chǎng)函數(shù)模型為：
[0032]根據(jù)本發(fā)明所述的基于改進(jìn)人工勢(shì)場(chǎng)的煤礦井下救災(zāi)機(jī)器人的路徑規(guī)劃方法，所 述的步驟四中，擾動(dòng)勢(shì)場(chǎng)函數(shù)模型為：
[0034]根據(jù)本發(fā)明所述的基于改進(jìn)人工勢(shì)場(chǎng)的煤礦井下救災(zāi)機(jī)器人的路徑規(guī)劃方法，所 述步驟五中，填充障礙物斥力勢(shì)場(chǎng)模型為：
[0036] 與現(xiàn)有技術(shù)相比，本發(fā)明的優(yōu)點(diǎn)在于：
[0037] (1)本發(fā)明采用填充法和擾動(dòng)勢(shì)場(chǎng)法相融合的煤礦井下救災(zāi)機(jī)器人進(jìn)行路徑規(guī) 劃，充分利用了填充法對(duì)凹形障礙物斥力勢(shì)場(chǎng)模型的改變，結(jié)合擾動(dòng)勢(shì)場(chǎng)法幫助救災(zāi)機(jī)器 人逃離局部極小點(diǎn)，并避免其再次陷入其他局部極小點(diǎn)。
[0038] (2)本發(fā)明采用了正壓兼本質(zhì)安全型的混合防爆設(shè)計(jì)，混合防爆設(shè)計(jì)的質(zhì)量輕，既 保證了導(dǎo)航系統(tǒng)的安全，又使機(jī)器人具有良好的運(yùn)動(dòng)性能。
[0039] (3)本發(fā)明采用了改進(jìn)填充法，解決了由于機(jī)器人本身探測(cè)范圍有限帶來(lái)的對(duì)凹 形障礙物的探測(cè)失敗，從而避免了陷入局部極小點(diǎn)而無(wú)法逃脫。
[0040] (4)本發(fā)明在采用擾動(dòng)勢(shì)場(chǎng)法時(shí)，提出了新的擾動(dòng)勢(shì)場(chǎng)函數(shù)KdlsP2(X，X g)，將機(jī)器人 和目標(biāo)點(diǎn)距離信息導(dǎo)入到勢(shì)場(chǎng)函數(shù)中，提高了擾動(dòng)函數(shù)的靈活性。
【附圖說(shuō)明】
[0041] 圖1是局部極小點(diǎn)的位置示意圖；
[0042]圖2是子目標(biāo)點(diǎn)的選擇范圍示意圖；
[0043]圖3a~圖3c是凹形障礙物的局部極小點(diǎn)示意圖；
[0044] 圖4a和圖4b是機(jī)器人探測(cè)范圍不意圖；
[0045] 圖5是填充障礙物示意圖；
[0046] 圖6a~圖6c是算法運(yùn)行軌跡仿真示意圖；
[0047] 圖7是仿真算法運(yùn)行時(shí)間對(duì)比示意圖；
[0048]圖8是仿真算法內(nèi)存使用率對(duì)比不意圖；
[0049] 圖9是算法流程圖。
【具體實(shí)施方式】
[0050] 在傳統(tǒng)人工勢(shì)場(chǎng)函數(shù)中，環(huán)境模型的總勢(shì)場(chǎng)如下：
[0051] Utotal = Uatt+Urep (5)
[0052] 當(dāng)機(jī)器人在路徑規(guī)劃中，陷入局部極小點(diǎn)，采用增加擾動(dòng)勢(shì)場(chǎng)來(lái)使機(jī)器人逃離局 部極小點(diǎn)，對(duì)擾動(dòng)勢(shì)場(chǎng)的定義如下：
[0054]其中，Kdls是擾動(dòng)勢(shì)場(chǎng)常數(shù);P(X，Xg)是機(jī)器人距離目標(biāo)點(diǎn)的距離;P a為機(jī)器人是否 到達(dá)目標(biāo)點(diǎn)的評(píng)判距離。
[0055] 引入擾動(dòng)勢(shì)場(chǎng)后，機(jī)器人陷入局部極小點(diǎn)時(shí)，環(huán)境模型的總勢(shì)場(chǎng)如下：
[0056] Utotal = Uatt+Urep+Udis (7)
[0057] 如果救災(zāi)機(jī)器人陷入局部極小點(diǎn)，在勢(shì)場(chǎng)中加入擾動(dòng)勢(shì)場(chǎng)，幫助救災(zāi)機(jī)器人逃離 局部極小點(diǎn)，基于以上的模型，在救災(zāi)機(jī)器人進(jìn)行避障時(shí)，在每個(gè)采樣周期，探測(cè)救災(zāi)機(jī)器 人可以到達(dá)的范圍，尋找到整個(gè)可探范圍內(nèi)的勢(shì)場(chǎng)最低點(diǎn)，將其作為子目標(biāo)點(diǎn)，采用矢量法 將力進(jìn)行合成，判斷機(jī)器人是否處在局部極小點(diǎn)，如果處在局部極小點(diǎn)，勢(shì)場(chǎng)強(qiáng)度之和包含 擾動(dòng)勢(shì)場(chǎng)。假定機(jī)器人移動(dòng)的最大速度為V max，采樣周期為to,則機(jī)器人可到達(dá)的范圍就是 以當(dāng)前位置為圓心，以Vmaxto為半徑的圓，為了保證機(jī)器人運(yùn)動(dòng)的平穩(wěn)性和在運(yùn)動(dòng)過(guò)程中的 可靠性，控制機(jī)器人在運(yùn)動(dòng)過(guò)程中的速度目標(biāo)點(diǎn)選定在半徑在的環(huán)形區(qū)域。子目標(biāo)點(diǎn)選擇 還與機(jī)器人在加速度方向有關(guān)系，而當(dāng)機(jī)器人處在局部極小點(diǎn)時(shí)是靜止的，由于加入了擾 動(dòng)勢(shì)場(chǎng)，機(jī)器人才會(huì)走出局部極小點(diǎn)，所以機(jī)器人的加速度方向與擾動(dòng)勢(shì)場(chǎng)力的方向相同， 所以選定加速度方向的延長(zhǎng)線與環(huán)形區(qū)域的交接處為子目標(biāo)點(diǎn)，如圖2所示，其中機(jī)器人移 動(dòng)的角度θΕ(〇,2π)，陰影部分是子目標(biāo)點(diǎn)選擇的區(qū)域。
[0058]本發(fā)明提出了一種當(dāng)救災(zāi)機(jī)器人在煤礦井下遇到凹障礙物時(shí)，逃離局部極小點(diǎn)的 方法，將煤礦井下巷道地理信息預(yù)裝在井下救災(zāi)機(jī)器人導(dǎo)航系統(tǒng)中，巷道本身對(duì)機(jī)器人來(lái) 說(shuō)并不是障礙，井下的移動(dòng)救生艙、礦車(chē)、機(jī)械設(shè)備等對(duì)機(jī)器人來(lái)說(shuō)都是需要避開(kāi)的障礙。 如圖3所示，展示了兩種在礦井下巷道中的凹障礙物，當(dāng)機(jī)器人碰到類(lèi)似的障礙物時(shí)，很難 及時(shí)判別前方是否存在局部極小點(diǎn)，所以即使運(yùn)用障礙物填充法，也未必能逃避出局部極 小點(diǎn)。
[0059]如圖4a和圖4b所示，救災(zāi)機(jī)器人在礦井下巷道移動(dòng)，圖4a中救災(zāi)機(jī)器人探測(cè)到前 方是兩個(gè)障礙物，此時(shí)探測(cè)角α<180°，但救災(zāi)機(jī)器人的探測(cè)范圍有限，事實(shí)情況是兩者屬 于同一個(gè)障礙物，當(dāng)機(jī)器人運(yùn)動(dòng)到圖4b中的位置時(shí)，機(jī)器人已經(jīng)陷入凹形障礙物中，原探 測(cè)角α>180°，機(jī)器人探測(cè)到其他障礙物，并判斷出之前探測(cè)的兩個(gè)障礙物屬于同一個(gè)障礙 物，此時(shí)機(jī)器人已經(jīng)逐步陷入局部極小點(diǎn)，最終導(dǎo)致目標(biāo)不可達(dá)。如圖3c所示，遇到這樣的 障礙物，機(jī)器人并不會(huì)陷入局部極小點(diǎn)。但是在礦難發(fā)生時(shí)，障礙物種類(lèi)多，單單只考慮一 種情況是不足的，很難實(shí)際運(yùn)用在發(fā)生過(guò)礦難的巷道中。
[0060] 本發(fā)明將擾動(dòng)勢(shì)場(chǎng)的方法和之前提出的障礙物填充法結(jié)合，考慮到煤礦井下多變 的障礙物情況，主要思想是當(dāng)救災(zāi)機(jī)器人陷入局部極小點(diǎn)之后，通過(guò)擾動(dòng)勢(shì)場(chǎng)來(lái)使機(jī)器人 逃離局部極小點(diǎn)，并且運(yùn)用障礙物填充來(lái)填充產(chǎn)生局部極小點(diǎn)的障礙物，構(gòu)造填充后的障 礙物斥力函數(shù)，使機(jī)器人逃離局部極小點(diǎn)的同時(shí)，避免機(jī)器人再次陷入局部極小點(diǎn)，如圖5 所示。
[0061] 在這種情況下，可能產(chǎn)生局部極小點(diǎn)的區(qū)域被填充，從而形成一個(gè)面積更大的障 礙物，被填充的區(qū)域的斥力勢(shì)場(chǎng)為：
其中：klcical表示填充區(qū)域斥力勢(shì)場(chǎng)比例系數(shù);x-Xlcical表示機(jī)器人退出局部極小點(diǎn)時(shí)與 局部極小點(diǎn)的距離;Pi為填充區(qū)域的影響范圍。
[0063] 原有的填充障礙物方法可以很好的提前避免救災(zāi)機(jī)器人陷入局部極小點(diǎn)，但是在 仿真結(jié)果中可以看到，該方法運(yùn)用的前提是機(jī)器人的探測(cè)范圍要大于障礙物的長(zhǎng)度。如圖 4a所示，當(dāng)機(jī)器人探測(cè)的范圍小于障礙物的長(zhǎng)度，則無(wú)法提前判斷是否存在凹型障礙物，故 結(jié)合上文中提到的擾動(dòng)勢(shì)場(chǎng)法，當(dāng)機(jī)器人可探測(cè)范圍探測(cè)不出有凹型障礙物時(shí)，為避免機(jī) 器人陷入局部極小點(diǎn)，增加擾動(dòng)市場(chǎng)，幫助機(jī)器人逃離局部極小點(diǎn)。
[0064] 當(dāng)救災(zāi)機(jī)器人陷入局部極小點(diǎn)時(shí)，通過(guò)增加擾動(dòng)勢(shì)場(chǎng)，使機(jī)器人逃離局部極小點(diǎn)， 在上述情形下，擾動(dòng)勢(shì)場(chǎng)的方向應(yīng)該是機(jī)器人原來(lái)前進(jìn)方向的反方向，只有增加反方向的 擾動(dòng)勢(shì)場(chǎng)才能使機(jī)器人擺脫局部極小點(diǎn)，否則會(huì)繼續(xù)前行，可能出現(xiàn)碰撞到障礙物或者局 部震蕩的情況，此時(shí)機(jī)器人受到的總勢(shì)場(chǎng)為公式(9)中所示，子目標(biāo)點(diǎn)選擇的范圍如圖2所 示，若障礙物處于一二象限，則子目標(biāo)點(diǎn)的位置應(yīng)選擇在三四象限。通過(guò)增加擾動(dòng)勢(shì)場(chǎng)，機(jī) 器人逃離局部極小點(diǎn)。為了避免再次陷入局部極小點(diǎn)，進(jìn)行凹障礙物的填充，填充之后的 勢(shì)場(chǎng)強(qiáng)度如下：
[0065] Utotal - Uatt+Urep+Udis+Ulocal (9)
[0066] 當(dāng)機(jī)器人完全退出可能產(chǎn)生局部極小點(diǎn)的凹型區(qū)域如圖4b所示，即障礙物凹形區(qū) 域被填充完畢。此時(shí)，撤銷(xiāo)擾動(dòng)勢(shì)場(chǎng)，機(jī)器人此時(shí)受到的勢(shì)場(chǎng)和為：
[0067 ] Utotal = Uatt+Urep+Ulocal (10) 本發(fā)明提出的是一種煤礦井下救災(zāi)機(jī)器人避障的路徑規(guī)劃的方法，為了驗(yàn)證算法的可 行性和優(yōu)化性，選取了礦井下障礙物情況進(jìn)行了不失一般性的仿真，仿真均在Matlab7.0.4 的環(huán)境下進(jìn)行，分別對(duì)機(jī)器人路徑軌跡和算法的空間復(fù)雜度進(jìn)行了仿真。本發(fā)明運(yùn)用ZY08-C_G型避障小車(chē)，結(jié)合Visual C++進(jìn)彳丁編程，對(duì)實(shí)物進(jìn)彳丁了實(shí)驗(yàn)。
[0068] 本發(fā)明選取了兩種大小不同，所占面積不同的障礙物，采用了擾動(dòng)勢(shì)場(chǎng)的方法進(jìn) 行了仿真，如圖6a、圖6b和圖6c所示，設(shè)定圖6a中目標(biāo)位置為(0.45，0.2)，圖6b中目標(biāo)點(diǎn)位 置為(0.45,0.05)。在圖6a中，當(dāng)機(jī)器人陷入局部極小點(diǎn)后，在勢(shì)場(chǎng)函數(shù)中加入了擾動(dòng)勢(shì)場(chǎng)， 對(duì)路徑進(jìn)行了重新規(guī)劃，但經(jīng)過(guò)兩次重新規(guī)劃?rùn)C(jī)器人并沒(méi)有能逃離出局部極小點(diǎn)，而是再 次陷入了另外的一個(gè)局部極小點(diǎn)，最終沒(méi)能走出凹形障礙物;從圖6b中看到，機(jī)器人同樣是 經(jīng)歷了兩次局部極小點(diǎn)，但是最終通過(guò)擾增加動(dòng)勢(shì)場(chǎng)的方法走出局部極小點(diǎn)，成功避障凹 形障礙物;我們可以清楚地看到圖6a中的機(jī)器人陷入局部極小點(diǎn)時(shí)，機(jī)器人距離目標(biāo)點(diǎn)的 位置遠(yuǎn)小于圖6b中機(jī)器人距離目標(biāo)點(diǎn)的位置。由公式(7)對(duì)擾動(dòng)勢(shì)場(chǎng)的定義可知，P(X，X g) 在圖6b更大，擾動(dòng)勢(shì)場(chǎng)模型產(chǎn)生的斥力更大，更容易偏離原來(lái)的軌跡，便于重新規(guī)劃路徑。 但是圖6b中機(jī)器人避障花費(fèi)了更多的時(shí)間和路程，增加了算法的難度，算法的可靠性下降。
[0069] 圖6c中是結(jié)合了擾動(dòng)勢(shì)場(chǎng)法和填充障礙物法結(jié)合之后的結(jié)果，從圖6c可以清楚地 看到，當(dāng)機(jī)器人陷入局部極小點(diǎn)時(shí)，通過(guò)擾動(dòng)勢(shì)場(chǎng)使機(jī)器人逃離局部極小點(diǎn)，由于填充了障 礙物，似的勢(shì)場(chǎng)函數(shù)中，斥力勢(shì)場(chǎng)增大，從而使得機(jī)器人的軌跡相對(duì)于之前的軌跡更為陡 峭，并且隨著填充面積的增大，機(jī)器人逐漸逃離凹形障礙物，重新規(guī)劃路徑后很快到達(dá)目標(biāo) 點(diǎn)，路徑更加清晰。
[0070] 在全屬性運(yùn)行的狀態(tài)下，本發(fā)明對(duì)兩種不同的算法進(jìn)行了空間復(fù)雜度的對(duì)比，通 過(guò)對(duì)空間復(fù)雜度的對(duì)比，可以得知不同算法的運(yùn)行時(shí)間，算法運(yùn)行時(shí)內(nèi)存占用率情況等，這 都是來(lái)比較算法優(yōu)化性能的重要參數(shù)。根據(jù)算法空間復(fù)雜度的對(duì)比，可以比較幾種不同算 法的優(yōu)化性能，從中選擇最優(yōu)的算法，對(duì)救災(zāi)機(jī)器人最優(yōu)路徑的選擇有重要的作用。仿真結(jié) 果如下：隨著仿真樣本數(shù)量的增加，比較兩種算法的運(yùn)行時(shí)間，如圖7所示。隨著樣本數(shù)量的 增加，比較兩種算法運(yùn)行時(shí)系統(tǒng)內(nèi)存占用率情況，如圖8所示。其中，1表示擾動(dòng)勢(shì)場(chǎng)法的仿 真曲線，2表示擾動(dòng)勢(shì)場(chǎng)結(jié)合填充法的仿真曲線。
[0071 ]結(jié)果分析：由算法運(yùn)行時(shí)間和系統(tǒng)內(nèi)存占用率分析結(jié)果可知，兩種算法在仿真樣 本數(shù)目500000時(shí)，算法運(yùn)行時(shí)間和系統(tǒng)內(nèi)存占用率在性能上差異均不大。但隨著仿真樣本 數(shù)量增加，擾動(dòng)勢(shì)場(chǎng)結(jié)合填充法無(wú)論是在運(yùn)行時(shí)間還是內(nèi)存占用率上，都有很大的優(yōu)勢(shì)。在 運(yùn)行時(shí)間上，當(dāng)仿真樣本數(shù)目一定時(shí)，擾動(dòng)勢(shì)場(chǎng)結(jié)合填充法的算法時(shí)間要比單一的擾動(dòng)勢(shì) 場(chǎng)法少很多;在內(nèi)存使用率上，擾動(dòng)勢(shì)場(chǎng)法的內(nèi)存使用率逼近59.5%，而擾動(dòng)勢(shì)場(chǎng)結(jié)合填充 法的內(nèi)存使用率逼近43.5%，后者內(nèi)存占用率更低。
[0072] 本發(fā)明采用ZY08-C-G型避障小車(chē)進(jìn)行實(shí)物實(shí)驗(yàn)，結(jié)合Visual C++編程進(jìn)行現(xiàn)場(chǎng)模 擬避障，避障小車(chē)運(yùn)行工作流程圖如圖9所示。
[0073] 本發(fā)明的實(shí)物實(shí)驗(yàn)運(yùn)用Visual C++對(duì)避障小車(chē)寫(xiě)入編程，并對(duì)小車(chē)進(jìn)行了實(shí)際避 障實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果證明，小車(chē)很好地可以完成避障任務(wù)，并且最終到達(dá)我們?cè)O(shè)定的目標(biāo)點(diǎn)。
[0074] 本發(fā)明根據(jù)煤礦井下救災(zāi)機(jī)器人路徑規(guī)劃中人工勢(shì)場(chǎng)法存在的局部極小點(diǎn)問(wèn)題， 通過(guò)提出擾動(dòng)勢(shì)場(chǎng)的概念，結(jié)合原有文獻(xiàn)的填充障礙物的方法，解決了煤礦井下救災(zāi)機(jī)器 人陷入凹形障礙物的局部極小點(diǎn)問(wèn)題。當(dāng)救災(zāi)機(jī)器人陷入井下凹形障礙物的局部極小點(diǎn) 時(shí)，通過(guò)在勢(shì)場(chǎng)函數(shù)中增加擾動(dòng)勢(shì)場(chǎng)，可以使礦井救災(zāi)機(jī)器人使機(jī)器人成功逃離局部極小 點(diǎn)，但是根據(jù)仿真結(jié)果可知：
[0075] (1)救災(zāi)機(jī)器人可能會(huì)再次陷入同一障礙物的其他局部極小點(diǎn)，此時(shí)結(jié)合原有文 獻(xiàn)提出的填充障礙物法，形成新的斥力勢(shì)場(chǎng)函數(shù)。
[0076] (2)當(dāng)機(jī)器人陷入局部極小點(diǎn)時(shí)，先通過(guò)擾動(dòng)勢(shì)場(chǎng)幫助機(jī)器人逃離局部極小點(diǎn)，再 通過(guò)填充障礙物法幫助機(jī)器人重新規(guī)劃路徑，不僅可以避免救災(zāi)機(jī)器人再次陷入局部極 小點(diǎn)，并且可以完成路徑規(guī)劃，成功實(shí)施救災(zāi)任務(wù)。
[0077] 經(jīng)過(guò)實(shí)物實(shí)驗(yàn)，通過(guò)ZY08-C-G型避障小車(chē)進(jìn)行實(shí)物實(shí)驗(yàn)，證明了實(shí)驗(yàn)的可行性。從 控制的角度講，該方法可以很好地運(yùn)用到整個(gè)系統(tǒng)中去而不影響系統(tǒng)的穩(wěn)定性。
【主權(quán)項(xiàng)】
1. 一種基于改進(jìn)人工勢(shì)場(chǎng)的煤礦井下救災(zāi)機(jī)器人的路徑規(guī)劃方法，其特征在于，該規(guī) 劃方法是將擾動(dòng)勢(shì)場(chǎng)法和原有的障礙物填充法相結(jié)合，幫助機(jī)器人逃離局部極小點(diǎn)，重新 規(guī)劃避障路徑，避免再次陷入局部極小點(diǎn)； 所述局部極小點(diǎn)是指機(jī)器人在人工勢(shì)場(chǎng)法的路徑規(guī)劃方法下，由于引力勢(shì)場(chǎng)和斥力勢(shì) 場(chǎng)的共同作用，在未到達(dá)目標(biāo)的位置，由于所受勢(shì)場(chǎng)合力為零，導(dǎo)致陷入某個(gè)區(qū)域無(wú)法繼續(xù) 完成路徑規(guī)劃； 所述障礙物填充法是通過(guò)救災(zāi)機(jī)器人的探測(cè)裝置，對(duì)凹形障礙物進(jìn)行虛擬填充，使其 最終形成較為規(guī)則的障礙物，避免產(chǎn)生局部極小點(diǎn)； 所述擾動(dòng)勢(shì)場(chǎng)法是當(dāng)機(jī)器人陷入局部極小點(diǎn)時(shí)，通過(guò)在總勢(shì)場(chǎng)函數(shù)中引入擾動(dòng)勢(shì)場(chǎng)函 數(shù)，改變總勢(shì)場(chǎng)函數(shù)，幫助井下救災(zāi)機(jī)器人走出局部極小點(diǎn)。2. 根據(jù)權(quán)利要求1所述的基于改進(jìn)人工勢(shì)場(chǎng)的煤礦井下救災(zāi)機(jī)器人的路徑規(guī)劃方法， 其特征在于，所述路徑規(guī)劃方法將已知的障礙物填充法與新提出的擾動(dòng)勢(shì)場(chǎng)法相結(jié)合，當(dāng) 機(jī)器人陷入局部極小點(diǎn)時(shí)，首先通過(guò)增加擾動(dòng)勢(shì)場(chǎng)函數(shù)幫助救災(zāi)機(jī)器人走出局部極小點(diǎn)， 此時(shí)進(jìn)行凹形障礙物填充，構(gòu)造填充勢(shì)場(chǎng)函數(shù)，隨著機(jī)器人逐步走出凹形障礙物，填充面積 逐漸增大，此時(shí)斥力勢(shì)場(chǎng)函數(shù)也逐漸增大，總勢(shì)場(chǎng)函數(shù)發(fā)生改變，救災(zāi)機(jī)器人路徑重新規(guī) 劃，直到凹形障礙物被全部填充，機(jī)器人繞開(kāi)凹形障礙物，最終到達(dá)目標(biāo)位置。3. 根據(jù)權(quán)利要求2所述的基于改進(jìn)人工勢(shì)場(chǎng)的煤礦井下救災(zāi)機(jī)器人的路徑規(guī)劃方法， 其特征在于，所述擾動(dòng)勢(shì)場(chǎng)函數(shù)與機(jī)器人到目標(biāo)點(diǎn)的直線距離存在正比例關(guān)系，擾動(dòng)勢(shì)場(chǎng) 函數(shù)只有在救災(zāi)機(jī)器人陷入局部極小點(diǎn)時(shí)才啟動(dòng)加入總勢(shì)場(chǎng)函數(shù)，其他時(shí)刻機(jī)器人只按照 目標(biāo)點(diǎn)的引力函數(shù)和障礙物的斥力函數(shù)進(jìn)行路徑規(guī)劃。4. 根據(jù)權(quán)利要求2所述的基于改進(jìn)人工勢(shì)場(chǎng)的煤礦井下救災(zāi)機(jī)器人的路徑規(guī)劃方法， 其特征在于，所述障礙物將產(chǎn)生新的斥力勢(shì)場(chǎng)函數(shù)，填充范圍隨救災(zāi)機(jī)器人遠(yuǎn)離局部極小 點(diǎn)逐步增大，填充勢(shì)場(chǎng)斥力函數(shù)與機(jī)器人到局部極小點(diǎn)的距離成反比例關(guān)系。5. 根據(jù)權(quán)利要求1所述的基于改進(jìn)人工勢(shì)場(chǎng)的煤礦井下救災(zāi)機(jī)器人的路徑規(guī)劃方法， 其特征在于，所述路徑規(guī)劃方法當(dāng)救災(zāi)機(jī)器人在煤礦井下遇到凹形障礙物時(shí)，具體工作步 驟如下： 步驟一:將已知的煤礦井下全局信息輸入到路徑規(guī)劃系統(tǒng)中； 步驟二:探測(cè)障礙物信息，融合已知的煤礦井下全局信息，構(gòu)建障礙物斥力勢(shì)場(chǎng)模型和 目標(biāo)點(diǎn)引力勢(shì)場(chǎng)模型，對(duì)路徑進(jìn)行初步規(guī)劃； 步驟三:救災(zāi)任務(wù)開(kāi)始，救災(zāi)機(jī)器人開(kāi)始向目標(biāo)點(diǎn)運(yùn)行，直到陷入凹形障礙物局部極小 點(diǎn);此時(shí)機(jī)器人所受合力為零，到達(dá)整個(gè)規(guī)劃路徑中的勢(shì)場(chǎng)最低點(diǎn)； 步驟四：開(kāi)始逃離局部極小點(diǎn)，啟動(dòng)擾動(dòng)勢(shì)場(chǎng)函數(shù)，此時(shí)總勢(shì)場(chǎng)函數(shù)改變，機(jī)器人出現(xiàn) 震蕩，暫時(shí)逃離局部極小點(diǎn)； 步驟五：為了避免救災(zāi)機(jī)器人再次陷入凹形障礙物的其他局部極小點(diǎn)，開(kāi)始進(jìn)行凹形 障礙物填充，填充范圍隨救災(zāi)機(jī)器人逃離局部極小點(diǎn)逐步擴(kuò)大； 步驟六:等待一個(gè)掃描周期過(guò)后，判斷機(jī)器人是否已逃離局部極小點(diǎn)，如果仍陷入局部 極小點(diǎn)，回到步驟二對(duì)現(xiàn)階段各類(lèi)模型進(jìn)行重新建立，重新進(jìn)行路徑規(guī)劃;如果逃離局部極 小點(diǎn)，按照現(xiàn)有勢(shì)場(chǎng)模型繼續(xù)進(jìn)行救災(zāi)任務(wù)； 步驟七:機(jī)器人到達(dá)目標(biāo)位置，路徑規(guī)劃結(jié)束。6. 根據(jù)權(quán)利要求5所述的基于改進(jìn)人工勢(shì)場(chǎng)的煤礦井下救災(zāi)機(jī)器人的路徑規(guī)劃方法， 其特征在于，所述的步驟二中，障礙物斥力勢(shì)場(chǎng)模型為： Fatt(X) =-gradUatt(X) =-Katt | X~Xg 目標(biāo)點(diǎn)引力勢(shì)場(chǎng)函數(shù)模型為： r - -^ 、.-- --υ r-υ7. 根據(jù)權(quán)利要求5所述的基于改進(jìn)人工勢(shì)場(chǎng)的煤礦井下救災(zāi)機(jī)器人的路徑規(guī)劃方法， 紅亦平_靳彳太的來(lái)聰un由油熱·1??撒蹈刑％:8. 根據(jù)權(quán)利要求5所述的基于改進(jìn)人工勢(shì)場(chǎng)的煤礦井下救災(zāi)機(jī)器人的路徑規(guī)劃方法， 其特征在于，所述步驟五中，填充障礙物斥力勢(shì)場(chǎng)模型為：
【文檔編號(hào)】G05D1/02GK105867365SQ201610136552
【公開(kāi)日】2016年8月17日
【申請(qǐng)日】2016年3月11日
【發(fā)明人】田子建, 高學(xué)浩
【申請(qǐng)人】中國(guó)礦業(yè)大學(xué)(北京), 高學(xué)浩