一種機(jī)器人礦井作業(yè)路徑規(guī)劃方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及礦井作業(yè)路徑規(guī)劃技術(shù)領(lǐng)域，具體涉及一種機(jī)器人礦井作業(yè)路徑規(guī)劃 方法。
【背景技術(shù)】
[0002] 煤礦礦井內(nèi)部結(jié)構(gòu)復(fù)雜，條件艱苦。有些作業(yè)環(huán)境不適合礦工長(zhǎng)期作業(yè)，另外在礦 難發(fā)生之后，由于往往對(duì)井下的情況未知，會(huì)給救援人員帶來(lái)很大的救援困難，不能夠抵達(dá) 救援地點(diǎn)，即使到達(dá)事故地點(diǎn)，自身也較為危險(xiǎn)。而機(jī)器人技術(shù)的發(fā)展，將會(huì)為井下作業(yè)帶 來(lái)方便。在機(jī)器人的研究領(lǐng)域中，路徑規(guī)劃是重要的研究方向之一。為機(jī)器人尋找一條最佳 的路徑規(guī)劃問(wèn)題，就是根據(jù)某一個(gè)或者某一些優(yōu)化要求及約束(如行走路線最短，行走時(shí)間 最短，能源消耗最少等方面），在機(jī)器人的工作空間中找到一條符合優(yōu)化要求及約束條件的 最好路徑。從優(yōu)化的角度來(lái)說(shuō)，這是一個(gè)帶有約束條件的函數(shù)優(yōu)化問(wèn)題。在整個(gè)過(guò)程中，路 徑規(guī)劃、定位、避障是一個(gè)需要綜合考慮的問(wèn)題。如何從起始點(diǎn)到目的地之間尋找到一條最 優(yōu)的無(wú)碰撞路徑，如何避免外界物體對(duì)機(jī)器人造成影響或使得影響最小，如何利用已知信 息構(gòu)建智能地圖，為機(jī)器人進(jìn)行導(dǎo)航，從而進(jìn)行更為優(yōu)化的行為決策，是機(jī)器人導(dǎo)航中需要 解決的問(wèn)題。

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

[0003] 本發(fā)明的目的是提供一種機(jī)器人礦井作業(yè)路徑規(guī)劃方法，它利用傳統(tǒng)的柵格策略 對(duì)周圍環(huán)境進(jìn)行建模，通過(guò)粒子群算法找出一條較為優(yōu)化的全局路徑。
[0004] 為了解決【背景技術(shù)】所存在的問(wèn)題，本發(fā)明的應(yīng)用步驟如下： 步驟一、機(jī)器人自由移動(dòng)空間建模 用柵格方法來(lái)對(duì)機(jī)器人移動(dòng)空間進(jìn)行建模，柵格的大小可以按照如下方法生成： (1) 在尋優(yōu)空間內(nèi)順序的選擇一個(gè)障礙物； (2) 若該障礙物是一個(gè)凸多邊形，則把該多邊形分割為互不相交的三角形;若該障礙物 不是凸多邊形，則以該多邊形的最大及最小橫縱坐標(biāo)構(gòu)建一個(gè)長(zhǎng)方形，并將長(zhǎng)方形分割為 兩個(gè)不相交的三角形； (3) 按照如下公式計(jì)算每一個(gè)三角形的面積
其中a，b分別是三角形的兩 邊，α是a，b所夾的角度； (4) 在尋優(yōu)空間中順序的處理下一個(gè)障礙物，若還有其他的障礙物，跳轉(zhuǎn)至第(2)步驟； (5) 計(jì)算所有障礙物的面積總和Sab;
為最大的柵格邊 長(zhǎng)，lmin為最小的柵格邊長(zhǎng)，1為最后確定的柵格邊長(zhǎng)； (7)算法終止 以上算法得到的是根據(jù)障礙物大小而計(jì)算的柵格大小，用同樣的方法再次計(jì)算機(jī)器人 本身的大小，最終的柵格大小為兩者中的最大值； 步驟二、粒子群算法在柵格空間中的規(guī)劃算法 1. 根據(jù)障礙物及機(jī)器人自身的大小計(jì)算柵格大小，建立機(jī)器人的環(huán)境模型； 2. 初始化粒子群算法的具體參數(shù)，包括種群規(guī)模，初始位置及初始粒子速度，慣性權(quán) 重，加速度系數(shù)以及最大的迭代數(shù)； 3. 對(duì)各個(gè)粒子的有效性進(jìn)行檢查； 4. 計(jì)算每一個(gè)粒子的適應(yīng)度，比較每一個(gè)粒子的適應(yīng)度與歷史最優(yōu)的適應(yīng)度，根據(jù)需 要，更新歷史最優(yōu)適應(yīng)度； 5. 更新粒子的速度和位置； 6. 檢查粒子有效性，若無(wú)效，需要重新產(chǎn)生新的粒子； 7. 更新系數(shù)，如慣性權(quán)重等； 8. 判斷是否滿足終止條件，如果滿足，程序終止;若不滿足，則跳轉(zhuǎn)至步驟4。
[0005] 采用上述結(jié)構(gòu)后，本發(fā)明有益效果為：它利用傳統(tǒng)的柵格策略對(duì)周圍環(huán)境進(jìn)行建 模，通過(guò)粒子群算法找出一條較為優(yōu)化的全局路徑。
【附圖說(shuō)明】
[0006] 圖1為粒子群算法中粒子移動(dòng)原理圖； 圖2為機(jī)器人實(shí)際工作環(huán)； 圖3為柵形圖格法得到的工作模型。
【具體實(shí)施方式】
[0007] 下面結(jié)合附圖對(duì)本發(fā)明作進(jìn)一步的說(shuō)明。
[0008] 為了使本發(fā)明的目的、技術(shù)方案及優(yōu)點(diǎn)更加清楚明白，以下結(jié)合附圖及具體實(shí)施 方式，對(duì)本發(fā)明進(jìn)行進(jìn)一步詳細(xì)說(shuō)明。應(yīng)當(dāng)理解，此處所描述的【具體實(shí)施方式】?jī)H用以解釋本 發(fā)明，并不用于限定本發(fā)明。
[0009] 如圖1-3所示，本【具體實(shí)施方式】采用如下技術(shù)方案:本發(fā)明的應(yīng)用步驟如下： 步驟一、機(jī)器人自由移動(dòng)空間建模 用柵格方法來(lái)對(duì)機(jī)器人移動(dòng)空間進(jìn)行建模，其中，柵格的大小由地圖中障礙物的疏密， 機(jī)器人自身大小等因素來(lái)決定的，柵格的大小可以按照如下方法生成： (1) 在尋優(yōu)空間內(nèi)順序的選擇一個(gè)障礙物；
(2) 若該障礙物是一個(gè)凸多邊形，則把該多邊形分割為互不相交的三角形;若該障礙物 不是凸多邊形，則以該多邊形的最大及最小橫縱坐標(biāo)構(gòu)建一個(gè)長(zhǎng)方形，并將長(zhǎng)方形分割為 兩個(gè)不相交的三角形； (3) 按照如下公式計(jì)算每一個(gè)三角形的面積 其中a，b分別是三角形的 兩邊，α是a，b所夾的角度； (4) 在尋優(yōu)空間中順序的處理下一個(gè)障礙物，若還有其他的障礙物，跳轉(zhuǎn)至第(2)步驟； (5) 計(jì)算所有障礙物的面積總和Sab;
.為最大的柵格 邊長(zhǎng)，1_為最小的柵格邊長(zhǎng)，1為最后確定的柵格邊長(zhǎng)； (7)算法終止 以上算法得到的是根據(jù)障礙物大小而計(jì)算的柵格大小，用同樣的方法再次計(jì)算機(jī)器人 本身的大小，最終的柵格大小為兩者中的最大值； 步驟二、粒子群算法在柵格空間中的規(guī)劃算法 1. 根據(jù)障礙物及機(jī)器人自身的大小計(jì)算柵格大小，建立機(jī)器人的環(huán)境模型； 2. 初始化粒子群算法的具體參數(shù)，包括種群規(guī)模，初始位置及初始粒子速度，慣性權(quán) 重，加速度系數(shù)以及最大的迭代數(shù)； 3. 對(duì)各個(gè)粒子的有效性進(jìn)行檢查； 4. 計(jì)算每一個(gè)粒子的適應(yīng)度，比較每一個(gè)粒子的適應(yīng)度與歷史最優(yōu)的適應(yīng)度，根據(jù)需 要，更新歷史最優(yōu)適應(yīng)度； 5. 更新粒子的速度和位置； 6. 檢查粒子有效性，若無(wú)效，需要重新產(chǎn)生新的粒子； 7. 更新系數(shù)，如慣性權(quán)重等； 8. 判斷是否滿足終止條件，如果滿足，程序終止;若不滿足，則跳轉(zhuǎn)至步驟4。
[0010] 本應(yīng)用中粒子群的基本原理為：1995年，James Kennedy博士和Russell Eberhart 博士提出了簡(jiǎn)單的鳥群的運(yùn)動(dòng)模型。通過(guò)計(jì)算機(jī)仿真，提出了著名的粒子群優(yōu)化算法 (Particle Swarm 0ptimization，PS0)。這是一種群體優(yōu)化算法，群體中設(shè)一個(gè)粒子的運(yùn)動(dòng) 通過(guò)幾條較為簡(jiǎn)單的規(guī)則來(lái)進(jìn)行描述[3]。這使得PS0算法簡(jiǎn)單有效并容易實(shí)現(xiàn)。關(guān)于粒子 群算法可以做如下描述，令目標(biāo)搜索空間為一個(gè)N維空間，粒子數(shù)量為m，則其中第i個(gè)粒子 在η維空間里的位置有如下定義:假設(shè)在一個(gè)N維的目標(biāo)搜索空間中，有m個(gè)粒子組成一個(gè)群 落，其中第i個(gè)粒子在η維空間里的位置為Xi=(xu，…，Xi m)，i = l，2,…，m;在粒子群中，每一 個(gè)粒子的速度為Vi = (vu，…，Vin)，i = l，2,…，n;其第d維分量的更新公式如下Vid=wX Vid+ Cin ( XidPbeSt-Xid ) +C2T2 X ( XidPbeSt-Xid ) , Xid = Xid+Vid ； 其中，vld是粒子i的速度向量的第d維分量;Xld是粒子i位置向量的第d維分量；;|@是 粒子i位置矢量的第d維分量所經(jīng)歷的最好的位置;是所有粒子在解空間中遍歷過(guò)的最 好的位置。rl，r2為0，1之間的隨機(jī)數(shù);cl，c2為加速度系數(shù);w為慣性權(quán)重。粒子移動(dòng)原理圖 如圖1所示。
[0011] 粒子群算法的實(shí)現(xiàn)步驟為： (1) 種群初始化:這一階段對(duì)粒子群進(jìn)行初始化，定義種群中個(gè)體數(shù)量，以及各個(gè)例子 的初始位置與速度； (2) 計(jì)算種群中每一個(gè)粒子的適應(yīng)度函數(shù)，對(duì)種群中的粒子進(jìn)行排序； (3) 比較各個(gè)粒子的適應(yīng)度函數(shù)，比較每一個(gè)粒子適應(yīng)度函數(shù)與Pbest。若粒子新的狀 態(tài)的適應(yīng)度函數(shù)優(yōu)于Pbes t i，貝lj更新Pbe s ti。
[0012] (4)比較每一個(gè)粒子適應(yīng)度函數(shù)與gbest。若粒子新的狀態(tài)的適應(yīng)度函數(shù)優(yōu)于 Pbesti，則更新 gbest。
[0013] (5)根據(jù)(3)，（ 4)式更新計(jì)算每一個(gè)粒子新的位置和速度； (6)若終止條件滿足，則程序終止，否則返回第(2)步。
[0014] 以上所述，僅用以說(shuō)明本發(fā)明的技術(shù)方案而非限制，本領(lǐng)域普通技術(shù)人員對(duì)本發(fā) 明的技術(shù)方案所做的其它修改或者等同替換，只要不脫離本發(fā)明技術(shù)方案的精神和范圍， 均應(yīng)涵蓋在本發(fā)明的權(quán)利要求范圍當(dāng)中。
【主權(quán)項(xiàng)】
1. 一種機(jī)器人礦井作業(yè)路徑規(guī)劃方法，其特征在于包括如下步驟： 步驟一、機(jī)器人自由移動(dòng)空間建模 用柵格方法來(lái)對(duì)機(jī)器人移動(dòng)空間進(jìn)行建模，所述柵格的大小可以按照如下方法生成： (1) 在尋優(yōu)空間內(nèi)順序的選擇一個(gè)障礙物； (2) 若該障礙物是一個(gè)凸多邊形，則把該多邊形分割為互不相交的三角形;若該障礙物 不是凸多邊形，則以該多邊形的最大及最小橫縱坐標(biāo)構(gòu)建一個(gè)長(zhǎng)方形，并將長(zhǎng)方形分割為 兩個(gè)不相交的三角形； (3) 按照如下公式計(jì)算每一個(gè)三角形的面積^ ?其中a，b分別是三角形的兩 邊，α是a，b所夾的角度； (4) 在尋優(yōu)空間中順序的處理下一個(gè)障礙物，若還有其他的障礙物，跳轉(zhuǎn)至第(2)步驟； (5) 計(jì)算所有障礙物的面積總和Sab; (6) 根據(jù)如下公式計(jì)算柵格粒違為最大的柵格邊長(zhǎng)，lmin為最小的柵格邊長(zhǎng)，1為最后確定的柵格邊長(zhǎng)； (7) 算法終止 以上算法得到的是根據(jù)障礙物大小而計(jì)算的柵格大小，用同樣的方法再次計(jì)算機(jī)器人 本身的大小，最終的柵格大小為兩者中的最大值； 步驟二、粒子群算法在柵格空間中的規(guī)劃算法 該算法包括一下步驟：1.根據(jù)障礙物及機(jī)器人自身的大小計(jì)算柵格大小，建立機(jī)器人的環(huán)境模型； 2 .初始化粒子群算法的具體參數(shù)，包括種群規(guī)模，初始位置及初始粒子速度，慣性權(quán) 重，加速度系數(shù)以及最大的迭代數(shù)；3. 對(duì)各個(gè)粒子的有效性進(jìn)行檢查；4. 計(jì)算每一個(gè)粒子的適應(yīng)度，比較每一個(gè)粒子的適應(yīng)度與歷史最優(yōu)的適應(yīng)度，根據(jù)需 要，更新歷史最優(yōu)適應(yīng)度；5. 更新粒子的速度和位置；6. 檢查粒子有效性，若無(wú)效，需要重新產(chǎn)生新的粒子；7. 更新系數(shù)，如慣性權(quán)重等；8. 判斷是否滿足終止條件，如果滿足，程序終止;若不滿足，則跳轉(zhuǎn)至步驟4。
【專利摘要】本發(fā)明公開了一種機(jī)器人礦井作業(yè)路徑規(guī)劃方法，它涉及礦井作業(yè)路徑規(guī)劃技術(shù)領(lǐng)域；本發(fā)明的應(yīng)用步驟包含：1機(jī)器人自由移動(dòng)空間建模，2粒子群算法在柵格空間中的規(guī)劃算法；另外本發(fā)明應(yīng)用中還涉及粒子群的基本原理和粒子群算法的實(shí)現(xiàn)方法。本發(fā)明有益效果為：它利用傳統(tǒng)的柵格策略對(duì)周圍環(huán)境進(jìn)行建模，通過(guò)粒子群算法找出一條較為優(yōu)化的全局路徑。
【IPC分類】G01C21/20
【公開號(hào)】CN105606103
【申請(qǐng)?zhí)枴緾N201610097053
【發(fā)明人】季云峰, 匡亮
【申請(qǐng)人】江蘇信息職業(yè)技術(shù)學(xué)院
【公開日】2016年5月25日
【申請(qǐng)日】2016年2月22日