專利名稱：一種基于虛擬場(chǎng)景的機(jī)器人避障路徑規(guī)劃方法
技術(shù)領(lǐng)域：
本發(fā)明涉及機(jī)器人避障路徑規(guī)劃技術(shù)。本方法主要涉及基于虛擬機(jī)器人的機(jī)器人避障路徑規(guī)劃方法，也可以用于實(shí)體機(jī)器人的避障路徑規(guī)劃。
背景技術(shù)：
避障路徑規(guī)劃是指給定環(huán)境的障礙條件以及起始位姿和目標(biāo)位姿，要求選擇一條從起始點(diǎn)到目標(biāo)點(diǎn)的路徑，使運(yùn)動(dòng)物體(機(jī)器人)能安全、無碰撞地通過所有的障礙。這種自主地躲避障礙物并完成作業(yè)任務(wù)是機(jī)器人研究的一個(gè)重要內(nèi)容。
目前，地面機(jī)械臂的障方法有基于C空間的自由空間法、基于直角坐標(biāo)空間的人工勢(shì)場(chǎng)法和關(guān)節(jié)空間法。
Lazona-Perze提出了基于C空間的自由空間法。以機(jī)械臂的關(guān)節(jié)軸為坐標(biāo)系建立C空間(Configuration pace)，將障礙物映射到C空間，形成C空間障礙，那么，在C空間內(nèi)C空間障礙的補(bǔ)集，則對(duì)應(yīng)自由空間，自由空間內(nèi)的點(diǎn)代表不與障礙物干澀的機(jī)器人構(gòu)型，而C空間障礙內(nèi)的點(diǎn)代表與障礙物干澀的機(jī)器人構(gòu)型。應(yīng)用啟發(fā)搜索算法，避障規(guī)劃就轉(zhuǎn)化為在自由空間內(nèi)尋找連接初始點(diǎn)和目標(biāo)點(diǎn)的路徑問題。缺點(diǎn)是C空間的建立比較困難，且計(jì)算量大。
Khatib在罰函數(shù)的基礎(chǔ)上引入了人工勢(shì)場(chǎng)(Artificial PotentialField)的概念，對(duì)障礙定義一個(gè)排斥場(chǎng)，此勢(shì)場(chǎng)在機(jī)器人上一點(diǎn)的梯度定義為排斥力；對(duì)目標(biāo)定義一個(gè)吸引勢(shì)場(chǎng)，機(jī)器人的運(yùn)動(dòng)是由兩種力的互相作用引起的，且受到運(yùn)動(dòng)學(xué)的約束。關(guān)節(jié)空間法是指將障礙物映射到關(guān)節(jié)空間，那么關(guān)節(jié)空間形成了一個(gè)可行域。優(yōu)點(diǎn)是比較直觀。缺點(diǎn)是根據(jù)關(guān)節(jié)空間規(guī)劃出的路徑點(diǎn)手爪的軌跡不規(guī)則。
與傳統(tǒng)規(guī)劃方法相比，空間機(jī)器人避碰路徑規(guī)劃算法還應(yīng)從以下幾方面考慮(1)安全性機(jī)器人整個(gè)手臂都有可能與障礙物體發(fā)生碰撞，因此對(duì)機(jī)器人的手臂進(jìn)行規(guī)劃時(shí)，應(yīng)與障礙物體保持一定距離，并對(duì)已經(jīng)規(guī)劃好的路徑，進(jìn)行機(jī)器人與障礙物體的碰撞檢驗(yàn)檢查，以免發(fā)生碰撞。(2)規(guī)劃效率為提高路徑規(guī)劃的效率，應(yīng)限制規(guī)劃算法搜索的空間，從而縮短機(jī)器人軌跡規(guī)劃的時(shí)間。
目前對(duì)空間機(jī)器人的避障路徑規(guī)劃極少。已有的方法也都只能進(jìn)行離線規(guī)劃，達(dá)不到在線規(guī)劃的實(shí)時(shí)性要求。

發(fā)明內(nèi)容
為了解決上述傳統(tǒng)問題，因此本發(fā)明的一個(gè)目的就是提出了一種基于虛擬機(jī)器人的機(jī)器人避障路徑規(guī)劃方法。
在本發(fā)明的一個(gè)方面中，基于虛擬場(chǎng)景的機(jī)器人避障路徑規(guī)劃方法，包括步驟(1)確定機(jī)械臂的初始狀態(tài) 和目標(biāo)狀態(tài) (2)確定時(shí)間步長(zhǎng)h和 (3)計(jì)算障礙物在水平面內(nèi)的障礙域HSSmin和HSSmax以及垂直平面內(nèi)的障礙域；(4)在由 確定的垂直平面內(nèi)順序調(diào)節(jié) (5)按靠近原則將 調(diào)節(jié)到180、0、-180度；(6)判斷若 而QQ1∈(HSSmin，HSSmax)或 或 時(shí)，進(jìn)入下一步，否則進(jìn)入第十步；(7)計(jì)算 的準(zhǔn)目標(biāo)位置QQ1；(8)將 由當(dāng)前位置調(diào)節(jié)到QQ1；(9)再由QQ1確定的垂直平面內(nèi)調(diào)節(jié) 使得當(dāng) 后L1，L2，L3將不會(huì)與障礙物相撞；(10)將 由當(dāng)前位置調(diào)到QQ1；(11)計(jì)算由QQ1確定的垂直平面下基座的障礙域；(12)按 的順序調(diào)節(jié)關(guān)節(jié)角到目標(biāo)關(guān)節(jié)角QQ5，QQ3，QQ2。若成功，則進(jìn)入下一步；(13)按 的順序調(diào)節(jié)關(guān)節(jié)角到目標(biāo)關(guān)節(jié)角QQ2，QQ3，QQ5。若成功，則進(jìn)入下一步；(14)調(diào)節(jié) 至目標(biāo)關(guān)節(jié)角QQ4；(15)調(diào)節(jié) 至目標(biāo)關(guān)節(jié)角QQ6；(16)規(guī)劃結(jié)束。
根據(jù)這個(gè)方面，在步驟(13)中，若不成功，則規(guī)劃失敗，進(jìn)入第十四步。
根據(jù)這個(gè)方面，在步驟(14)中，若不成功，則規(guī)劃失敗，進(jìn)入第十六步。


結(jié)合隨后的附圖，從下面的詳細(xì)說明中可顯而易見的得出本發(fā)明的上述及其他目的、特征及優(yōu)點(diǎn)。在附圖中圖1給出了對(duì)障礙物用規(guī)則體的包絡(luò)近似建模；圖2給出了投影到兩個(gè)平面示意圖；圖3給出了平面避障示意圖；圖4給出了平面避障示意圖；圖5給出了平面內(nèi)的障礙物；圖6給出了機(jī)械臂避障路徑規(guī)劃的兩岔樹；圖7給出了V平面內(nèi)zjojvj坐標(biāo)系下障礙物Wi確定的障礙域；圖8給出了由基座，六關(guān)節(jié)機(jī)械臂、以及方形障礙物組成的機(jī)器人系統(tǒng)的示意圖；圖9給出了機(jī)械臂避障流程圖。
具體實(shí)施例方式
針對(duì)目前C空間法的C空間的建立困難，而且計(jì)算量大，人工勢(shì)場(chǎng)法只是局部規(guī)劃，而空間機(jī)器人避障路徑規(guī)劃研究極少等特點(diǎn)，為了達(dá)到規(guī)劃的實(shí)時(shí)性和高效性的要求，我們提出了把三維問題轉(zhuǎn)化為二維問題解決六自由度空間機(jī)械臂避障路徑規(guī)劃問題。
本方法利用規(guī)則體的包絡(luò)對(duì)障礙物建模，并充分結(jié)合了關(guān)節(jié)空間法和C空間法的思想，在保持較高精度的前提下，通過在H和V平面上的投影確定障礙域，將三維避障路徑規(guī)劃問題轉(zhuǎn)化成兩個(gè)平面內(nèi)的二維問題。該方法計(jì)算量小、簡(jiǎn)單、直觀、易實(shí)現(xiàn)。
(1)利用規(guī)則體的包絡(luò)對(duì)障礙物建模三維障礙物一般具有不規(guī)則的幾何形狀，因此它在C空間法等方法中的精確的障礙域難以獲得。王偉[7]等人提出通過利用邊界點(diǎn)和特征點(diǎn)來確定障礙物在C空間下的障礙域，但是對(duì)于不規(guī)則幾何形狀的障礙物的邊界點(diǎn)過多，并且特征點(diǎn)不易確定，即使能確定，其計(jì)算量也相當(dāng)大，這將直接影響規(guī)劃速度，難以滿足實(shí)時(shí)性要求。因此本發(fā)明提出了利用障礙物規(guī)則體的包絡(luò)來近似建模，這種近似雖然擴(kuò)大了障礙域，但是使障礙域的描述大大簡(jiǎn)化，有效地提高了規(guī)劃的效率，并滿足安全性的要求。圖1(a)為在滿足精度的條件下障礙物采用最小外接球的近似，圖1(b)是對(duì)障礙物采用長(zhǎng)方體近似。
(2)把三維問題轉(zhuǎn)化為二維問題目前對(duì)障礙物的工作空間的建模主要利用C空間法，但是由于C空間法的建立非常困難，而且計(jì)算量隨著機(jī)械臂自由度的增加成指數(shù)被增長(zhǎng)，因此我們利用C空間法的思想，把三維的工作空間投影到兩個(gè)垂直的平面(H平面、V平面)內(nèi)解決。然后分別在H平面和V平面內(nèi)進(jìn)行路徑搜索。圖2為把機(jī)械臂和障礙物投影到H和V平面的示意圖。圖3和圖4為在V平面內(nèi)和H平面內(nèi)進(jìn)行搜索的示意圖。
(3)障礙域內(nèi)障礙物合并如圖2所示平面內(nèi)的障礙物可以描述為Wi(x，y，z，r)，其中i為障礙物序號(hào)，x，y，z為球心在基坐標(biāo)系中的坐標(biāo)，r為球的半徑。障礙物在平面上形成的障礙域?yàn)?αi，αi+δi)(αi+1，αi+1+δi+1)(αi+2，αi+2+δi+2)Λ。其中當(dāng)多個(gè)障礙域有重疊時(shí)，將這些障礙域進(jìn)行合并，經(jīng)過整理后的平面的障礙域?yàn)閅t=1(αt,αt+δt).]]>(4)搜索算法利用兩岔樹逆向?qū)?yōu)方法，如圖6所示。機(jī)械臂避障路徑規(guī)劃的兩岔樹共由T段組成，每段又分為g級(jí)。第i段通過調(diào)解每一級(jí)的關(guān)節(jié)角，避開第i段的障礙域，形成第i個(gè)中間規(guī)劃點(diǎn) 依此類推，最終到達(dá)機(jī)械臂目標(biāo)位姿 其中ΔθjL和ΔθjU為本節(jié)點(diǎn)到下一級(jí)節(jié)點(diǎn)的連接權(quán)值(在整個(gè)兩岔樹中，雖然表述相同，但由于父節(jié)點(diǎn)不同，所以其值不同)。在每一級(jí)的規(guī)劃中，若這個(gè)節(jié)點(diǎn)滿足機(jī)械臂關(guān)節(jié)角的硬約束時(shí)，此節(jié)點(diǎn)為實(shí)心節(jié)點(diǎn)，否則為空心節(jié)點(diǎn)?？招墓?jié)點(diǎn)沒有子節(jié)點(diǎn)，規(guī)劃時(shí)返回到父節(jié)點(diǎn)并沿另一岔規(guī)劃。
兩岔樹逆向?qū)?yōu)方法從最后一級(jí)子節(jié)點(diǎn)開始逆向返回到根節(jié)點(diǎn)，同時(shí)計(jì)算每一條路徑(每一條都是可行的避障規(guī)劃路徑)的權(quán)值 最后根據(jù)罰函數(shù) 確定一條“路徑最短”的最優(yōu)路徑。其中Eg×1＝[1，1，Λ，1]T。
如圖8所示，機(jī)器人系統(tǒng)由基座，六關(guān)節(jié)機(jī)械臂以及方形障礙物組成。
其中，A，B，C，D，E，F(xiàn)分別為基座及機(jī)械臂的安裝位置參數(shù)；L1，L2，L3為簡(jiǎn)化后機(jī)械臂參數(shù)；g為外延尺寸；LWH為障礙物參數(shù)，r為障礙物的外接球半徑(r=L2+W2+H2);]]>此外，機(jī)械臂的6關(guān)節(jié)角的硬約束條件為1)各個(gè)關(guān)節(jié)角的調(diào)節(jié)范圍是〔-180~180〕；根據(jù)當(dāng)前t時(shí)刻1)障礙物相對(duì)于基座的位置SSx，SSy，SSz；2)機(jī)械臂的6個(gè)初始關(guān)節(jié)角Q1，Q2，Q3，Q4，Q5，Q6；3)機(jī)械臂的6個(gè)目標(biāo)關(guān)節(jié)角QQ1，QQ2，QQ3，QQ4，QQ5，QQ6；4)規(guī)劃輸出的節(jié)點(diǎn)的時(shí)間步長(zhǎng)為0.25s；作如下規(guī)定規(guī)劃時(shí)間步長(zhǎng)為h＝0.25s；機(jī)械臂各個(gè)關(guān)節(jié)角在單位時(shí)間內(nèi)的運(yùn)動(dòng)步長(zhǎng)為 接下來，結(jié)合圖9，對(duì)基于虛擬機(jī)器人的機(jī)器人避障路徑規(guī)劃方法進(jìn)行描述。
第一步，確定機(jī)械臂的初始狀態(tài) 和目標(biāo)狀態(tài) 第二步，確定時(shí)間步長(zhǎng)h和 第三步，計(jì)算障礙物在水平面內(nèi)的障礙域(HSSmin，HSSmax)，以及垂直平面內(nèi)的障礙域。
第四步，在由 確定的垂直平面內(nèi)順序調(diào)節(jié) 直到11～13都在水平平面之上。
第五步，按靠近原則將調(diào)節(jié)到180、0、-180度。
第六步，判斷若 而QQ1∈(HSSmin，HSSmax)或 QQ1>HSSmax]]>或 HSSmax時(shí)，進(jìn)入下一步，否則進(jìn)入第十步。
第七步，計(jì)算 的準(zhǔn)目標(biāo)位置Q Q1。
第八步，將 由當(dāng)前位置調(diào)節(jié)到QQ1。
第九步，再由QQ1確定的垂直平面內(nèi)調(diào)節(jié) 使得當(dāng) 后L1，L2，L3將不會(huì)與障礙物相撞。
第十步，將 由當(dāng)前位置調(diào)到QQ1。
第十一步，計(jì)算由QQ1確定的垂直平面下基座的障礙域。
第十二步，按 的順序調(diào)節(jié)關(guān)節(jié)角到目標(biāo)關(guān)節(jié)角QQ5，QQ3，QQ2。若成功，則進(jìn)入下一步，否則，規(guī)劃失敗，進(jìn)入第十四步。
第十三步，按 的順序調(diào)節(jié)關(guān)節(jié)角到目標(biāo)關(guān)節(jié)角QQ2，QQ3，QQ5。若成功，則進(jìn)入下一步，否則，規(guī)劃失敗，進(jìn)入第十六步。
第十四步，調(diào)節(jié) 至目標(biāo)關(guān)節(jié)角QQ4。
第十五步，調(diào)節(jié) 至目標(biāo)關(guān)節(jié)角QQ6。
第十六步，規(guī)劃結(jié)束。
因此，基于虛擬場(chǎng)景的機(jī)器人避障相對(duì)于直接的機(jī)器人避障有消除遠(yuǎn)距離時(shí)延，為苛刻環(huán)境提供現(xiàn)場(chǎng)狀況等優(yōu)點(diǎn)。該規(guī)劃方法可提高規(guī)劃的安全性，實(shí)時(shí)性和高效性。
對(duì)于本領(lǐng)域的普通技術(shù)人員來說可顯而易見的得出其他優(yōu)點(diǎn)和修改。因此，具有更廣方面的本發(fā)明并不局限于這里所示出的并且所描述的具體說明及示例性實(shí)施例。因此，在不脫離由隨后權(quán)利要求及其等價(jià)體所定義的一般發(fā)明構(gòu)思的精神和范圍的情況下，可對(duì)其作出各種修改。
權(quán)利要求
1.一種基于虛擬場(chǎng)景的機(jī)器人避障路徑規(guī)劃方法，包括步驟(1)確定機(jī)械臂的初始狀態(tài) 和目標(biāo)狀態(tài) (2)確定時(shí)間步長(zhǎng)h和 (3)計(jì)算障礙物在水平面內(nèi)的障礙域HSSmin和HSSmax以及垂直平面內(nèi)的障礙域。(4)在由 確定的垂直平面內(nèi)順序調(diào)節(jié) (5)按靠近原則將 調(diào)節(jié)到180、0、-180度；(6)判斷若 而QQ1∈(HSSmin，HSSmax)或 QQ1>HSSmax]]>或 時(shí)，進(jìn)入下一步，否則進(jìn)入第十步；(7)計(jì)算 的準(zhǔn)目標(biāo)位置QQ1；(8)將 由當(dāng)前位置調(diào)節(jié)到QQ1；(9)再由QQ1確定的垂直平面內(nèi)調(diào)節(jié) 使得當(dāng) 后L1，L2，L3將不會(huì)與障礙物相撞；(10)將 由當(dāng)前位置調(diào)到QQ1；(11)計(jì)算由QQ1確定的垂直平面下基座的障礙域；(12)按 的順序調(diào)節(jié)關(guān)節(jié)角到目標(biāo)關(guān)節(jié)角QQ5，QQ3，QQ2。若成功，則進(jìn)入下一步；(13)按 的順序調(diào)節(jié)關(guān)節(jié)角到目標(biāo)關(guān)節(jié)角QQ2，QQ3，QQ5。若成功，則進(jìn)入下一步；(14)調(diào)節(jié) 至目標(biāo)關(guān)節(jié)角QQ4；(15)調(diào)節(jié) 至目標(biāo)關(guān)節(jié)角QQ6；(16)規(guī)劃結(jié)束。
2.根據(jù)權(quán)利要求1的方法，在步驟(13)中，若不成功，則規(guī)劃失敗，進(jìn)入第十四步。
3.根據(jù)權(quán)利要求1的方法，在步驟(14)中，若不成功，則規(guī)劃失敗，進(jìn)入第十六步。
全文摘要
本方法主要用于基于虛擬機(jī)器人的機(jī)器人避障路徑規(guī)劃，其利用規(guī)則體的包絡(luò)對(duì)障礙物建模，并充分結(jié)合了關(guān)節(jié)空間法和C空間法的思想，在保持較高精度的前提下，通過在H和V平面上的投影確定障礙域，將三維避障路徑規(guī)劃問題轉(zhuǎn)化成兩個(gè)平面內(nèi)的二維問題。該規(guī)劃方法可提高規(guī)劃的安全性，實(shí)時(shí)性和高效性。
文檔編號(hào)B25J9/16GK1883887SQ20061009100
公開日2006年12月27日 申請(qǐng)日期2006年7月7日 優(yōu)先權(quán)日2006年7月7日
發(fā)明者張珩, 陳靖波, 趙猛 申請(qǐng)人:中國科學(xué)院力學(xué)研究所