一種多分辨率障礙物環(huán)境下移動(dòng)機(jī)器人混合路徑規(guī)劃方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及移動(dòng)機(jī)器人技術(shù)領(lǐng)域，尤其是一種靜態(tài)、動(dòng)態(tài)障礙物并存，且障礙物分 布具有多分辨率特點(diǎn)的復(fù)雜情況下的移動(dòng)機(jī)器人混合路徑規(guī)劃方法。
【背景技術(shù)】
[0002] 路徑規(guī)劃技術(shù)一直是智能移動(dòng)機(jī)器人研究的核心內(nèi)容之一，其要求機(jī)器人根據(jù)預(yù) 先獲知或自身傳感器對(duì)環(huán)境的感知信息，自行規(guī)劃出一條從起點(diǎn)到終點(diǎn)，滿足一定準(zhǔn)則(路 徑長(zhǎng)度最短等）的無碰安全路徑。高效、可靠的路徑規(guī)劃技術(shù)是機(jī)器人完成具體任務(wù)的前提 和基本要求。
[0003] 根據(jù)環(huán)境信息獲取程度不同，路徑規(guī)劃方法可分為依靠先驗(yàn)已知信息的全局規(guī)劃 方法和依靠局部感知信息的局部規(guī)劃方法。在一些大規(guī)模地圖范圍的路徑規(guī)劃任務(wù)中，障 礙物環(huán)境往往具有多分辨率特點(diǎn)：即環(huán)境中障礙物分布疏密程度差異大、存在一些預(yù)知的 典型障礙物，如通過衛(wèi)星地圖等獲知的固定建筑等，局部還存在未探明的小的靜態(tài)或動(dòng)態(tài) 障礙物。此時(shí)傳統(tǒng)的全局規(guī)劃算法，如自由空間法等根據(jù)地圖幾何特征搜索路徑的方法，以 及粒子群算法、啟發(fā)式A*等智能算法，無法處理未知的靜態(tài)或動(dòng)態(tài)障礙物，而局部規(guī)劃算 法，如人工勢(shì)場(chǎng)法、滾動(dòng)窗口法等，由于缺乏已知全局信息的引導(dǎo)，往往計(jì)算復(fù)雜或易陷入 局部死循環(huán)。因而，采用全局路徑規(guī)劃和實(shí)時(shí)局部避障相結(jié)合的混合路徑規(guī)劃方法是滿足 移動(dòng)機(jī)器人高效、可靠、實(shí)時(shí)需求的。
[0004] 環(huán)境建模是機(jī)器人進(jìn)行全局路徑規(guī)劃的前提，其與路徑規(guī)劃的算法效率緊密相 關(guān)。常用的環(huán)境建模方法包括柵格法、鏈接圖法、等分坐標(biāo)變換法等，然而這些方法在針對(duì) 多分辨率的障礙物環(huán)境中缺乏靈活適應(yīng)性:柵格法需要根據(jù)最小的障礙物確定柵格的劃 分，造成極大的計(jì)算資源浪費(fèi);鏈接圖法根據(jù)障礙物頂點(diǎn)生成的網(wǎng)狀鏈接圖需要經(jīng)過復(fù)雜 處理才能定義種群的搜索空間；等分坐標(biāo)變換法以等分始末點(diǎn)連線的垂線確定種群的搜索 空間時(shí)，很難兼顧路徑解最優(yōu)性與計(jì)算資源的平衡，在障礙物稀疏處搜索空間太大浪費(fèi)計(jì) 算資源，障礙物密集處離散化的搜索空間太小難以規(guī)劃出最優(yōu)路徑。
[0005] 粒子群算法是解決機(jī)器人全局路徑規(guī)劃最常用的算法之一，它是由Kennedy和 Eberhart兩位博士根據(jù)自然界鳥群覓食的行為特點(diǎn)首次在1995年的國(guó)際會(huì)議"IEEE Interational Conference on Neural Network" 中的 "Particle Swarm Optimization" (粒子群優(yōu)化)一文中提出的一種智能搜索算法。粒子群中的每個(gè)粒子代表一個(gè)路徑解，粒 子的維數(shù)代表路徑點(diǎn)的個(gè)數(shù)。粒子在搜索空間內(nèi)展開隨機(jī)搜索，計(jì)算每個(gè)粒子所代表路徑 解的評(píng)價(jià)指標(biāo)(如路徑長(zhǎng)度等），選出粒子群中指標(biāo)最優(yōu)(長(zhǎng)度最短、安全系數(shù)最高等）的粒 子來更新每個(gè)粒子的歷史最優(yōu)路徑解和粒子群的全局最優(yōu)路徑解，以此引導(dǎo)當(dāng)前粒子快速 收斂到滿足路徑指標(biāo)要求的最優(yōu)路徑解。其具有結(jié)構(gòu)簡(jiǎn)單、參數(shù)易調(diào)節(jié)的優(yōu)點(diǎn)，但其仍存在 容易過早收斂到非最優(yōu)路徑解的不足。
[0006] 根據(jù)實(shí)時(shí)獲取的感知信息進(jìn)行局部動(dòng)態(tài)避障是移動(dòng)機(jī)器人路徑規(guī)劃的難點(diǎn)，要求 算法具有計(jì)算效率高、求解穩(wěn)定的優(yōu)點(diǎn)。人工勢(shì)場(chǎng)法由Khatib于1986年首次在國(guó)際期刊 "The International of Robotics Research" 中第五卷第一期的 "Real-time obstacle avoidance for manipulators and mobile robots"（移動(dòng)機(jī)器人和控制器的實(shí)時(shí)避障）一 文中提出以解決機(jī)器人路徑規(guī)劃和控制問題，其將機(jī)器人在環(huán)境中的運(yùn)動(dòng)視為一種虛擬人 工受力場(chǎng)中的運(yùn)動(dòng)。目標(biāo)對(duì)機(jī)器人產(chǎn)生引力，障礙物對(duì)其產(chǎn)生斥力，通過引力和斥力的合力 來改變機(jī)器人的運(yùn)動(dòng)方向和速度，驅(qū)使其躲避障礙物朝向目標(biāo)點(diǎn)位置移動(dòng)。其與滾動(dòng)窗口 法、行為法等相比算法簡(jiǎn)單、計(jì)算量小、產(chǎn)生的路徑平滑，但仍存在由于對(duì)避障距離缺乏預(yù) 估計(jì)，使機(jī)器人在無需避障時(shí)遠(yuǎn)離障礙導(dǎo)致路徑長(zhǎng)度過大，以及在特殊情況下陷入局部死 循環(huán)的問題。
[0007] 總體而言，現(xiàn)有技術(shù)在處理具有多分辨率障礙物的動(dòng)態(tài)環(huán)境中的路徑規(guī)劃中還存 在以下缺陷：（1)初始規(guī)劃存在盲目性。在具有未知障礙物的動(dòng)態(tài)環(huán)境中，只依靠實(shí)時(shí)獲取 的環(huán)境信息進(jìn)行規(guī)劃，缺乏全局信息引導(dǎo)，初始規(guī)劃存在盲目性。（2)全局路徑規(guī)劃的環(huán)境 建模方法缺乏靈活適應(yīng)性。已有的環(huán)境建模方法不能根據(jù)障礙物分布靈活確定路徑搜索空 間的大小，導(dǎo)致計(jì)算資源浪費(fèi)。（3)對(duì)未知的靜態(tài)或動(dòng)態(tài)障礙物的實(shí)時(shí)避障能力差。對(duì)于動(dòng) 態(tài)不確定運(yùn)動(dòng)的障礙物，由于對(duì)避障距離及角度缺乏預(yù)估可能導(dǎo)致機(jī)器人過度遠(yuǎn)離障礙 物，導(dǎo)致路徑長(zhǎng)度過大。對(duì)一些含有陷阱的特殊環(huán)境，可能使程序陷入死循環(huán)。

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

[0008] 本發(fā)明要解決的技術(shù)問題是提供一種多分辨率障礙物的動(dòng)態(tài)環(huán)境下移動(dòng)機(jī)器人 混合路徑規(guī)劃方法，將全局路徑規(guī)劃和局部動(dòng)態(tài)避障相結(jié)合，解決現(xiàn)有方法存在的初始規(guī) 劃存在盲目性、環(huán)境建模缺乏靈活適應(yīng)性、對(duì)未知?jiǎng)討B(tài)或靜態(tài)障礙物實(shí)時(shí)避障能力差的問 題。
[0009] 本發(fā)明解決以上技術(shù)問題采用如下技術(shù)方案：
[0010] 第一步:采用自適應(yīng)非均勻極徑的極坐標(biāo)建模方法對(duì)移動(dòng)機(jī)器人運(yùn)動(dòng)空間進(jìn)行環(huán) 境建模。
[0011] 自適應(yīng)非均勻極徑的極坐標(biāo)建模方法能夠根據(jù)障礙物的疏密程度靈活設(shè)定全局 路徑規(guī)劃方法的路徑搜索空間，同時(shí)相對(duì)傳統(tǒng)的笛卡兒坐標(biāo)建模，極坐標(biāo)系允許通過路徑 回退逃離某些凹形障礙物，具體步驟為：
[0012] 1.1在機(jī)器人運(yùn)動(dòng)空間內(nèi)建立笛卡爾坐標(biāo)系oxy，將機(jī)器人和預(yù)先探明的障礙物 信息在笛卡爾坐標(biāo)系〇xy上標(biāo)注，包括機(jī)器人起始位置S、目標(biāo)點(diǎn)位置G以及各障礙物的頂點(diǎn) 位置，從鍵盤接收已知的障礙物總數(shù)目〇b num。
[0013] 1.2以S為極點(diǎn)，SG射線為極軸，逆時(shí)針為正方向，建立機(jī)器人運(yùn)動(dòng)空間的極坐標(biāo) 系。將1.1中輸入的機(jī)器人和預(yù)先探明的障礙物信息轉(zhuǎn)化為極徑和極角表示的極坐標(biāo)信息。
[0014] 1.3確定初始全局路徑點(diǎn)的個(gè)數(shù)和搜索空間：
[0015] 1.3.1確定初始全局路徑點(diǎn)的極徑序列：如圖2所示，以SG的距離L為半徑，在極軸 兩側(cè)作四分之一圓。設(shè)置角度增量為S(Se[5,90]且90能被S整除）。從極點(diǎn)S出發(fā)，依次引出 極角為mS(m=-(90-S)/S，-(90-2S)/S，? ? ?，-1，〇，1，? ? ?，（90-2S)/S，（90-S)/S)、極徑（即始 末點(diǎn)總距離)為L(zhǎng)的1+180/S條向量，分別計(jì)算每條向量與每個(gè)障礙物各邊的交點(diǎn)，將所有向 量與障礙物的交點(diǎn)的極徑按照從小到大的順序排列，得到一個(gè)極徑序列。
[0016] 1.3.2對(duì)所得的極徑序列作以下處理:設(shè)置相鄰的初始全局路徑點(diǎn)之間允許的最 大差值dmax和最小差值dmir^dmax根據(jù)L和最少需要的全局路徑點(diǎn)個(gè)數(shù)來定，dmax為L(zhǎng)/(q+l)，q 為最少需要的全局路徑點(diǎn)個(gè)數(shù)，通常設(shè)2 < q < 5。^^根據(jù)L和最多需要的全局路徑點(diǎn)個(gè)數(shù)來 定，dminSL/(p+l)，p為最多需要的全局路徑點(diǎn)個(gè)數(shù)，通常設(shè)20 < p < 50。若極徑序列中相鄰 兩極徑的差值大于Dmax，則在極徑序列中這兩相鄰極徑之間插入兩相鄰極徑的平均值;若極 徑序列中兩相鄰極徑的差值小于D min，則在極徑序列中刪除兩相鄰極徑，在序列中插入兩相 鄰極徑的平均值，得到新的極徑序列Ditc^cb，. . .，dn]，n為最終確定的極徑個(gè)數(shù)。
[0017]第二步:采用重力粒子群搜索方法規(guī)劃出從起點(diǎn)到終點(diǎn)的初始全局路徑，機(jī)器人 沿此初始全局路徑邊行走邊進(jìn)行局部動(dòng)態(tài)避障。
[0018]相比孫波等2005年在"控制與決策"期刊第20卷第9期中"基于粒子群優(yōu)化算法的 移動(dòng)機(jī)器人全局路徑規(guī)劃"一文求解路徑規(guī)劃的粒子群算法，重力粒子群搜索算法采用碰 撞約束和路徑長(zhǎng)度優(yōu)化目標(biāo)相區(qū)分的支配關(guān)系評(píng)價(jià)路徑的優(yōu)劣，能夠快速在可行解范圍內(nèi) 展開搜索，同時(shí)采用重力吸引的思想充分利用計(jì)算的路徑評(píng)價(jià)值的定量信息，提高粒子群 算法收斂速度和搜索精度。重力粒子群搜索方法步驟如下：
[0019] 2.1初始化重力粒子群算法參數(shù)：
[0020] 2.1.1設(shè)置重力粒子群算法的參數(shù):包括粒子群的粒子數(shù)目N，一般取值為30到 200之間的整數(shù)。最大迭代代數(shù)Iter，一般取500到1000之間的整數(shù)。學(xué)習(xí)因子cl、c2,通常均 設(shè)置為2。加速度系數(shù)c3,通常設(shè)為1。初始化當(dāng)前代數(shù)t為0,慣性因子w為0.9。
[0021] 2.1.2初始化粒子群中的每個(gè)粒子。粒子群表示粒子1，2，3，...，N的集合。每個(gè)粒 子表示一個(gè)全局路徑點(diǎn)序列，由全局路徑點(diǎn)的極角序列和極徑序列組成。粒子的維數(shù)，即所 表示的初始全局路徑點(diǎn)的個(gè)數(shù)初始化為n。粒子k(k=l，2,…，N)的極徑序列編碼為Dk = [dS，. . .表示粒子k中第i個(gè)全局路徑點(diǎn)的極徑，取值范圍為（OJhDi， D2，. . .，Dn均初始化為D。粒子k的極角序列編碼為Xk= [0\，. . .，0ki. . .，0kn]。0ki表示粒子k中 第i個(gè)全局路徑點(diǎn)的極角，取值范圍為
^^知，...，XN中的每個(gè)元素均初始化為0。粒 子1，2,3, . . .，N的個(gè)體歷史最優(yōu)極角序列pbesti，pbest2, . . .，pbestN初始化為各自的極角 序列 Xl，X2，???，Xn，個(gè)體歷史取優(yōu)的路徑長(zhǎng)度flength (pbestl)，f length (pbest2)，???，flength (口匕6 8七~)均初始化為+ °°，個(gè)體歷史最優(yōu)的路徑碰撞約束(30(。&6 81：1)，(30(。&6 8七2)，...，(30 (pbestN)均初始化1，粒子群的全局最優(yōu)極角序列g(shù)best初始化為pbesti，gbest的路徑長(zhǎng)度 fiength(gbest)初始化為+〇〇，gbest的碰撞約束co(gbest)初始化為1。粒子k的速度序列編碼 為示粒子k的第i個(gè)全局路徑點(diǎn)的極角的變化速度，取值范圍 為
。對(duì)Vi，V2,. . .，Vn進(jìn)行初始化，方法是：
[0022] 2.1.2.1 初始化 k = l，i = l。
[0023] 2.1.2.2設(shè)置vS為
區(qū)間內(nèi)的