專利名稱：預(yù)選擇最小距離指標(biāo)冗余機(jī)器人動(dòng)態(tài)避障方法
技術(shù)領(lǐng)域：
本發(fā)明涉及障礙物隨機(jī)運(yùn)動(dòng)的環(huán)境下，預(yù)選擇最小距離指標(biāo)冗余機(jī)器人動(dòng)態(tài)避障路徑規(guī)劃方法，在動(dòng)態(tài)避障規(guī)劃前便剔除安全桿件，并采用局部映射距離計(jì)算方法，得出實(shí)時(shí)最小距離。本發(fā)明無(wú)需計(jì)算障礙物到所有桿件的最小距離，提高了動(dòng)態(tài)避障的規(guī)劃效率，滿足實(shí)時(shí)性的要求。屬于智能機(jī)器人技術(shù)領(lǐng)域。
背景技術(shù)：
冗余機(jī)器人，是指關(guān)節(jié)空間維數(shù)η大于任務(wù)空間維數(shù)m，可以在自運(yùn)動(dòng)而不影響末端位姿的情況下實(shí)現(xiàn)多種二次目標(biāo)。由于任務(wù)環(huán)境越來(lái)越復(fù)雜，要求冗余機(jī)器人具有更高的智能性，其中動(dòng)態(tài)避障是機(jī)器人自主路徑規(guī)劃中一個(gè)重要的組成部分，日益成為人們關(guān)注的焦點(diǎn)。 目前，利用桿件與障礙物的距離信息完成避障規(guī)劃是學(xué)者們的研究重點(diǎn)。Maciejiewski和Khatib應(yīng)用人工勢(shì)場(chǎng)法,通過(guò)距離信息建立吸引勢(shì)場(chǎng)和排斥勢(shì)場(chǎng)完成避障。LALIBERTE等人針對(duì)人工勢(shì)場(chǎng)法容易發(fā)生局部極小的不足，提出了離散勢(shì)場(chǎng)法，從而減少了局部極小發(fā)生的可能性。CHO等人基于流體力學(xué)的勢(shì)流理論，提出了一種可以避免局部極小的新穎的速度排斥函數(shù)法。另外，Liegeois提出梯度投影法，利用最小距離指標(biāo)，優(yōu)化雅克比矩陣的零空間矢量，修改齊次解完成避障。IKADA等人提出一種潛在避障操作性能形狀指標(biāo)來(lái)控制機(jī)械臂達(dá)到易于避障的位形來(lái)進(jìn)行避障，同時(shí)保持末端軌跡的跟蹤。Kavraki等人提出了隨機(jī)路標(biāo)法，該方法通過(guò)環(huán)境距離檢測(cè)，搜索出一些能夠避開(kāi)障礙物的關(guān)節(jié)坐標(biāo)點(diǎn)來(lái)實(shí)現(xiàn)避障，但是隨著機(jī)構(gòu)的冗余度增加，計(jì)算量也在增加。避障指標(biāo)選取的合適與否將直接影響到避障效果的好壞，特別對(duì)于動(dòng)態(tài)避障，障礙物隨機(jī)運(yùn)動(dòng)，選擇合理的并能準(zhǔn)確快速描述障礙物與機(jī)械臂位置關(guān)系的指標(biāo)，對(duì)于保證避障規(guī)劃的實(shí)時(shí)性尤為重要。傳統(tǒng)的距離指標(biāo)，需要計(jì)算障礙物到每個(gè)桿件的最小距離，然后再取最小值。當(dāng)桿件處于某些特殊構(gòu)形時(shí)，障礙物到桿件最小距離的垂足可能會(huì)落到桿件的延長(zhǎng)線上，延長(zhǎng)線并不屬于機(jī)器人的一部分，對(duì)于該桿件計(jì)算的最小距離不準(zhǔn)確，這時(shí)需要在桿件上取多個(gè)標(biāo)志點(diǎn)并遍歷各個(gè)標(biāo)志點(diǎn)到障礙物的距離?？梢?jiàn)，傳統(tǒng)的距離避障指標(biāo)，一是準(zhǔn)確性不足，二是多個(gè)標(biāo)志點(diǎn)的計(jì)算，耗費(fèi)了大量的時(shí)間。對(duì)于傳統(tǒng)的距離指標(biāo)，可以完成靜態(tài)避障規(guī)劃，但是對(duì)于動(dòng)態(tài)避障規(guī)劃來(lái)說(shuō)，障礙物隨機(jī)運(yùn)動(dòng)，需要機(jī)器人在有限的時(shí)間內(nèi)完成障礙物檢測(cè)及關(guān)節(jié)的多種動(dòng)作，對(duì)避障的效率提出了更高的要求。

發(fā)明內(nèi)容
基于上述，本發(fā)明提出在障礙物隨機(jī)運(yùn)動(dòng)環(huán)境下，一種預(yù)選擇距離指標(biāo)冗余機(jī)器人動(dòng)態(tài)避障路徑規(guī)劃方法，該方法無(wú)需計(jì)算障礙物到所有桿件的最小距離，并且采用局部映射距離計(jì)算方法，提高了冗余機(jī)器人動(dòng)態(tài)避障的規(guī)劃效率。為了實(shí)現(xiàn)此目的，本發(fā)明所采取的技術(shù)方案是
(I)對(duì)于冗余機(jī)器人各個(gè)桿件及隨機(jī)運(yùn)動(dòng)障礙物建立相應(yīng)的包圍盒，減少基本幾何元素的數(shù)目和參與相交測(cè)試的包圍盒的數(shù)目。進(jìn)一步的，本發(fā)明選用OBB方向包圍盒建模，并設(shè)定包圍盒的方向和桿件的方向重合，包圍盒的長(zhǎng)度和桿件的長(zhǎng)度一致，寬度根據(jù)最小安全距離設(shè)定。面對(duì)全部關(guān)節(jié)都是旋轉(zhuǎn)關(guān)節(jié)的冗余機(jī)器人，當(dāng)桿件發(fā)生旋轉(zhuǎn)運(yùn)動(dòng)后，只需讓OBB盒的基底進(jìn)行同樣的旋轉(zhuǎn)進(jìn)行實(shí)時(shí)更新。對(duì)于形狀不一的動(dòng)態(tài)障礙物，采用球形包圍盒。(2)實(shí)時(shí)檢測(cè)運(yùn)動(dòng)障礙物與機(jī)器人桿件的相對(duì)位置關(guān)系，建立預(yù)選擇最小距離指標(biāo)Hd_min，其思想是針對(duì)不同的機(jī)器人構(gòu)型建立相應(yīng)的包圍盒，利用相交測(cè)試，在計(jì)算實(shí)時(shí)距離之前剔除其他安全桿件，只考慮相碰撞的桿件和障礙物的位置關(guān)系，并在桿件的局部坐標(biāo)系中計(jì)算距離值。如果在相交測(cè)試中有多個(gè)桿件相交，則Hd_min取相交距離中最小的。在避障過(guò)程中，要求Hd_min越大越好，說(shuō)明離障礙物越遠(yuǎn)，機(jī)器人越安全。 具體步驟分為2. I在機(jī)器人桿件的局部坐標(biāo)系中，利用分離軸定理進(jìn)行相交測(cè)試。2. 2利用局部映射計(jì)算最小距離，優(yōu)化傳統(tǒng)距離計(jì)算方法。進(jìn)一步的，所述2. I相交測(cè)試中，OBB包圍盒采用分離軸檢測(cè)方法，將障礙物的坐標(biāo)轉(zhuǎn)換到每個(gè)桿件的局部坐標(biāo)系中，相交測(cè)試在局部坐標(biāo)系下完成，分離軸減少到3個(gè)，即X、Y、z。如果在3個(gè)分離軸上，障礙物和機(jī)器人的投影全都重合，則判斷相交。如果其中有一個(gè)分離軸上的投影分離，則不會(huì)發(fā)生碰撞，記為安全桿件，不作為計(jì)算實(shí)時(shí)距離的有效桿件。設(shè)在桿件AB的局部坐標(biāo)系中，A端的坐標(biāo)為(xA’，yA’，zA’)，B端的坐標(biāo)為(xB’，yB’，zB’ )包圍盒長(zhǎng)度和寬度為2*d，高度為桿件長(zhǎng)度L ;障礙物球心半徑為R，中心坐標(biāo)為D1 (x1； y1； Z1),將桿件AB的包圍盒和球體分別向三個(gè)坐標(biāo)軸投影，當(dāng)
權(quán)利要求
1.一種預(yù)選擇最小距離指標(biāo)冗余機(jī)器人動(dòng)態(tài)避障方法，其特征在于包括以下步驟 (1)對(duì)于冗余機(jī)器人各個(gè)桿件及隨機(jī)運(yùn)動(dòng)障礙物建立相應(yīng)的OBB包圍盒，減少基本幾何元素的數(shù)目和參與相交測(cè)試的包圍盒的數(shù)目。
(2)實(shí)時(shí)檢測(cè)運(yùn)動(dòng)障礙物與機(jī)器人桿件的相對(duì)位置關(guān)系，將基坐標(biāo)系中的障礙物映射到桿件的局部坐標(biāo)系中，建立預(yù)選擇最小距離指標(biāo)Hd_min。
(3)設(shè)定和最小距離相關(guān)的避障增益和逃逸速度，利用冗余機(jī)器人自運(yùn)動(dòng)零空間映射矩陣實(shí)現(xiàn)逃逸速度動(dòng)態(tài)避障運(yùn)動(dòng)規(guī)劃。
2.如權(quán)利要求I所述的預(yù)選擇最小距離指標(biāo)冗余機(jī)器人動(dòng)態(tài)避障方法，其特征在于 預(yù)選擇最小距離指標(biāo)Hd_min的思想是針對(duì)不同的機(jī)器人構(gòu)型建立相應(yīng)的包圍盒，利用相交測(cè)試，在計(jì)算實(shí)時(shí)距離之前剔除安全桿件，只考慮相碰撞的桿件和障礙物的位置關(guān)系，并在桿件的局部坐標(biāo)系中計(jì)算距離值。如果在相交測(cè)試中有多個(gè)桿件相交，則Hd_min取相交距離中最小的。在避障過(guò)程中，要求Hd_min越大越好，說(shuō)明離障礙物越遠(yuǎn)，機(jī)器人越安全。
所述步驟(2)中建立預(yù)選擇最小距離指標(biāo)Hd_min的具體步驟是 ·2.I在機(jī)器人桿件的局部坐標(biāo)系中，利用分離軸定理進(jìn)行相交測(cè)試。 · 2.2利用局部映射計(jì)算最小距離，優(yōu)化傳統(tǒng)距離計(jì)算方法。
3.如權(quán)利要求3所述的預(yù)選擇最小距離指標(biāo)冗余機(jī)器人動(dòng)態(tài)避障方法，其特征在于 所述步驟2. I相交測(cè)試的依據(jù)是0ΒΒ包圍盒采用分離軸檢測(cè)方法，將障礙物的坐標(biāo)轉(zhuǎn)換到每個(gè)桿件的局部坐標(biāo)系中，相交測(cè)試在局部坐標(biāo)系下完成，分離軸減少到3個(gè)，即X、Y、Ζ。如果在3個(gè)分離軸上，障礙物和機(jī)器人的投影全都重合，則判斷相交。如果其中有一個(gè)分離軸上的投影分離，則不會(huì)發(fā)生碰撞，記為安全桿件，不作為計(jì)算實(shí)時(shí)距離的有效桿件。
設(shè)在桿件AB的局部坐標(biāo)系中，A端的坐標(biāo)為(V，yA，，zA’)，B端的坐標(biāo)為(xB，，yB，，zB’)包圍盒長(zhǎng)度和寬度為2*d，高度為桿件長(zhǎng)度L ;障礙物球心半徑為R，中心坐標(biāo)為D1U1,Y1, Z1),將桿件AB的包圍盒和球體分別向三個(gè)坐標(biāo)軸投影，當(dāng)
4.如權(quán)利要求3所述的預(yù)選擇最小距離指標(biāo)冗余機(jī)器人動(dòng)態(tài)避障方法，其特征在于 所述步驟2. 2局部映射最小距離計(jì)算的依據(jù)是在相交測(cè)試后，剔除不和障礙物發(fā)生碰撞的安全桿件，在此基礎(chǔ)上，在碰撞桿件的局部坐標(biāo)系中計(jì)算有效的最小距離。由于最小距離dmin是標(biāo)量，所以在任意坐標(biāo)系中它的值不變。碰撞點(diǎn)的求取，實(shí)質(zhì)是為了求得碰撞點(diǎn)的雅可比矩陣，利用矢量積法計(jì)算雅可比矩陣Jtl，則需要計(jì)算碰撞點(diǎn)到桿件末端的距離，也是標(biāo)量。所以不用轉(zhuǎn)換到基坐標(biāo)系中，直接在局部坐標(biāo)系中利用目標(biāo)點(diǎn)的坐標(biāo)值直接得出結(jié)果。
設(shè)定相碰撞桿件A端坐標(biāo)(xA’，yA’，zA’)，B端坐標(biāo)(xB’，yB’，zB’)，障礙物中心坐標(biāo)D1U1, y1； Z1)，碰撞點(diǎn)為P，障礙物距此桿件的最小距離為dmin，碰撞點(diǎn)距離末端關(guān)節(jié)A的長(zhǎng)度為PA。則P (χι，Ya，zA )dmin = ΥΓΥα^PA = I X1-X/
5.如權(quán)利要求I所述的預(yù)選擇最小距離指標(biāo)冗余機(jī)器人動(dòng)態(tài)避障方法，其特征在于所述步驟(3)中根據(jù)預(yù)選擇最小距離指標(biāo)Hd_min的建立，進(jìn)行逃逸速度冗余機(jī)器人動(dòng)態(tài)避障運(yùn)動(dòng)規(guī)劃具體步驟是.3.I通過(guò)預(yù)選擇最小距離指標(biāo)的建立得出有效的最小距離dmin，并和預(yù)設(shè)定的最小安全距離做比較。如果大于最小安全距離，則不開(kāi)啟動(dòng)態(tài)避障命令，繼續(xù)執(zhí)行末端規(guī)劃的軌跡。反之則開(kāi)啟動(dòng)態(tài)避障命令，并跟蹤末端軌跡。
.3.2建立和最小距離dmin相關(guān)的避障增益和逃逸速度，根據(jù)最小距離dmin的不同，而取相應(yīng)的避障增益和逃逸速度，已達(dá)到較好的避障效果。
.3.3計(jì)算機(jī)器人雅可比矩陣，碰撞點(diǎn)雅可比矩陣，明確逃逸方向。.3.4通過(guò)機(jī)器人逆運(yùn)動(dòng)學(xué)，利用冗余機(jī)器人自運(yùn)動(dòng)零空間映射矩陣進(jìn)行動(dòng)態(tài)避障路徑規(guī)劃； .3.5對(duì)角速度值積分,得到下一步關(guān)節(jié)角。
.3.6跟蹤末端軌跡，判斷是否到達(dá)終點(diǎn)。
全文摘要
本發(fā)明涉及障礙物隨機(jī)運(yùn)動(dòng)環(huán)境下，預(yù)選擇最小距離指標(biāo)冗余機(jī)器人動(dòng)態(tài)避障路徑規(guī)劃方法，包括步驟利用OBB包圍盒對(duì)冗余機(jī)器人進(jìn)行建模，建立預(yù)選擇最小距離指標(biāo)；通過(guò)相交測(cè)試，在計(jì)算實(shí)時(shí)距離之前剔除安全桿件；在相應(yīng)桿件的局部坐標(biāo)系中，采用局部映射距離計(jì)算方法，利用目標(biāo)點(diǎn)坐標(biāo)值直接得出實(shí)時(shí)最小距離并確定碰撞點(diǎn)位置；在此基礎(chǔ)上，建立和最小距離相關(guān)的避障增益和逃逸速度，利用冗余機(jī)器人零空間映射矩陣進(jìn)行避障路徑規(guī)劃。本發(fā)明在動(dòng)態(tài)避障規(guī)劃前便剔除安全桿件，去除了不必要的繁瑣計(jì)算并優(yōu)化距離計(jì)算方法，提高了動(dòng)態(tài)避障的規(guī)劃效率，滿足實(shí)時(shí)性的要求。
文檔編號(hào)G05D1/02GK102902269SQ20121035534
公開(kāi)日2013年1月30日 申請(qǐng)日期2012年9月21日 優(yōu)先權(quán)日2012年9月21日
發(fā)明者賈慶軒, 張倩茹, 高欣, 陳鋼, 宋荊洲 申請(qǐng)人:北京郵電大學(xué)