黎宇晨，蔣強(qiáng)，王洪參

（1.沈陽(yáng)理工大學(xué)，遼寧 沈陽(yáng) 110159；2.沈陽(yáng)信東機(jī)電有限公司，遼寧 沈陽(yáng) 110000）

0 引言

由于本文研究?jī)?nèi)容為狹小空間中的機(jī)械臂的運(yùn)動(dòng)軌跡規(guī)劃，對(duì)于粒子群算法，其在搜索路徑的過程中對(duì)于離散的優(yōu)化問題處理不佳，容易在搜索全局最優(yōu)路徑時(shí)陷入局部的最優(yōu)，無法全面考慮，因此可能會(huì)對(duì)實(shí)驗(yàn)結(jié)果造成誤差；而對(duì)于傳統(tǒng)A*算法，是對(duì)于節(jié)點(diǎn)和節(jié)點(diǎn)以外的點(diǎn)以最優(yōu)的方式進(jìn)行搜索的一種搜索方式，目的以最小的代價(jià)值完成相應(yīng)的目標(biāo)[1]；但傳統(tǒng)A*算法在搜索過程中規(guī)避障礙物的時(shí)候沒有考慮到經(jīng)過障礙物頂點(diǎn)時(shí)候的情況，容易造成不安全的因素，進(jìn)而產(chǎn)生誤差。

為解決上述問題，本文提出了結(jié)合碰撞檢測(cè)的三維A*算法，相比較于傳統(tǒng)的A*算法，在考慮了規(guī)避障礙物頂點(diǎn)的情況下同時(shí)加快了搜索的效率，通過在節(jié)點(diǎn)和節(jié)點(diǎn)之間引入目標(biāo)函數(shù)，來判斷下一個(gè)點(diǎn)是否可以作為最優(yōu)節(jié)點(diǎn)，有著最優(yōu)節(jié)點(diǎn)的指標(biāo)，進(jìn)而設(shè)定一條最優(yōu)的路徑；在保留局部最優(yōu)的同時(shí)計(jì)算周圍的最優(yōu)，進(jìn)而達(dá)到路徑的最優(yōu)，考慮了全局，更加的具有效率性以及精準(zhǔn)性。

1 三維A*算法的提出

1.1 傳統(tǒng)A*算法

A*算法，其本質(zhì)上是通過代價(jià)函數(shù)來在眾多節(jié)點(diǎn)中尋找最優(yōu)節(jié)點(diǎn)進(jìn)而形成路徑的一種搜索方法；在搜索的過程中不需要搜索全部節(jié)點(diǎn)，而是在初始點(diǎn)的周圍開始逐步搜索，該算法所涉及公式為：

1.2 三維A*算法及碰撞模型的建立

三維A*算法，相比于傳統(tǒng)A*算法在搜索過程中以及搜索路徑最優(yōu)的方面更加的趨于完善，對(duì)于路徑中的障礙物這一因素考慮了在搜索途中經(jīng)過障礙物頂點(diǎn)的情況，可以在總代價(jià)最小的前提下通過軌跡規(guī)劃來盡可能避開障礙物的頂點(diǎn)，因此本文利用三維A*算法來搜尋在已知障礙物條件下的機(jī)械手的最佳路徑[4]。

其中i和i+1分別表示節(jié)點(diǎn)和后續(xù)節(jié)點(diǎn)，并在此基礎(chǔ)上引入了子條件對(duì)于三個(gè)坐標(biāo)軸xi、yi、zi后一個(gè)節(jié)點(diǎn)與前一個(gè)節(jié)點(diǎn)之間的距離關(guān)系，并在h（n）這一估計(jì)值的基礎(chǔ)上利用懲罰函數(shù)來做出相應(yīng)的約束限制，進(jìn)而求出最優(yōu)的估計(jì)值；使機(jī)械臂在正常運(yùn)行情況下以最優(yōu)的路徑到達(dá)實(shí)驗(yàn)預(yù)期的目標(biāo)點(diǎn)。

當(dāng)機(jī)械臂運(yùn)動(dòng)時(shí)，將機(jī)械臂采用有向包圍盒OBB進(jìn)行包圍，過程中所有的障礙物也一并用有向包圍盒OBB包圍，雖然有向包圍盒的計(jì)算量略大，但包圍性是極高的，有利于實(shí)驗(yàn)的準(zhǔn)確性。

如圖1，表示了在密閉的實(shí)驗(yàn)箱中移動(dòng)的機(jī)械手和障礙物的投影情況，實(shí)驗(yàn)箱用簡(jiǎn)易的框架來表示，左側(cè)為機(jī)械手投影，右側(cè)為障礙物投影，以二維的形式來顯示出來，進(jìn)一步通過二維的形式來分析之間的碰撞情況[5]。對(duì)于機(jī)械手和障礙物兩個(gè)投影，首先要考慮的是邊的問題，在移動(dòng)的過程中，由于兩個(gè)剛體均采用了OBB有向包圍盒的形式，均是立方體，在靠近的過程中，考慮最近的邊即可，如果最近的邊在軌跡運(yùn)動(dòng)中沒有發(fā)生交集，那么此軌跡就是安全的，否則發(fā)生碰撞。

圖1 包圍盒邊的關(guān)系

同時(shí)還可以通過點(diǎn)的角度來考慮是否發(fā)生了碰觸，可以通過投影點(diǎn)的距離來判斷是否發(fā)生了碰撞；如圖2，為投影面及投影點(diǎn)的表示。

圖2 包圍盒點(diǎn)的關(guān)系

2 實(shí)驗(yàn)仿真

首先確定軌跡規(guī)劃的起始點(diǎn)和目標(biāo)點(diǎn)，在確定了尋路的起始點(diǎn)，目標(biāo)點(diǎn)的情況下，假定每一小塊立方體區(qū)域都有一個(gè)F值，該值代表了在當(dāng)前路徑下選擇走該塊區(qū)域的代價(jià)，進(jìn)而利用了三維A*算法代價(jià)少擇優(yōu)的方法來判定路徑；載入圖形數(shù)據(jù)，分別設(shè)定起始點(diǎn)和目標(biāo)點(diǎn)的值，并將空間矢量化且位置初始化，定義了兩點(diǎn)的位置和方向，進(jìn)而也定義了障礙物的坐標(biāo)和高度，在封閉環(huán)境內(nèi)設(shè)定多個(gè)障礙物，均采用了OBB有向包圍盒進(jìn)行包圍[6]。

其次針對(duì)機(jī)械手，也同樣采用OBB有向包圍盒進(jìn)行包圍，設(shè)定空間中坐標(biāo)軸的距離，進(jìn)而在封閉空間內(nèi)擬定多個(gè)障礙物，因?yàn)榭梢詫⒄系K物以O(shè)BB包圍盒的形式表示，因此在設(shè)計(jì)的時(shí)候也同樣直接用包圍盒來檢測(cè)碰撞。

如圖3，表示了在同樣的障礙物環(huán)境下的情況，傳統(tǒng)A*算法與三維A*算法在起始點(diǎn)和目標(biāo)點(diǎn)確定條件下路徑的不同，前方黑色線為三維A*算法規(guī)劃路線，后方藍(lán)色線為傳統(tǒng)A*算法規(guī)劃路線，由于其在傳統(tǒng)A*算法基礎(chǔ)上引入了z軸，因此形成了一個(gè)立體的三維空間，所搜尋節(jié)點(diǎn)如上圖均有顯示，并且可以更加直觀的分析所規(guī)劃的路徑，通過找尋局部最優(yōu)的同時(shí)來規(guī)劃全局最優(yōu)的路徑，并在懲罰函數(shù)的采用下剔除了無關(guān)節(jié)點(diǎn)的搜尋，提升了效率，進(jìn)而搜索系統(tǒng)涉及的代價(jià)也更小，也同時(shí)盡可能避開了障礙物的頂點(diǎn)不安全因素，保障了安全性。如表1。

圖3 相同障礙物環(huán)境下傳統(tǒng)A*算法與三維A*算法的路徑對(duì)比圖

表1 傳統(tǒng)A*算法與三維A*算法的對(duì)比

對(duì)于兩個(gè)剛體來說，基于OBB-OBB檢測(cè)，其原則上依舊為空間中點(diǎn)與線段的聯(lián)系，如果在某一時(shí)刻，最近的點(diǎn)超過了一個(gè)限度，那么就會(huì)導(dǎo)致發(fā)生碰撞，考慮到箱內(nèi)近乎真空的環(huán)境，如果中途意外的發(fā)生零件的損壞或者空中漂浮物的情況，進(jìn)而導(dǎo)致箱內(nèi)的障礙物意外的發(fā)生變換，進(jìn)而導(dǎo)致碰撞；此時(shí)機(jī)械手為了保證設(shè)備安全，設(shè)定了原路返回，更加確保了實(shí)驗(yàn)設(shè)備的安全性。

3 結(jié)語(yǔ)

本文在外太空空間站的環(huán)境下，以狹小工作箱內(nèi)的機(jī)械手為實(shí)驗(yàn)對(duì)象，通過三維A*算法來深入研究了機(jī)械手在密閉工作箱中的軌跡規(guī)劃，可以根據(jù)障礙物包絡(luò)體的復(fù)雜程度不同來相應(yīng)的規(guī)劃路線，有著極高的效率性，拓寬了算法的適用范圍，也同時(shí)增強(qiáng)了算法可靠性；并在搜索規(guī)劃路徑的過程中引入碰撞檢測(cè)算法，將工作箱側(cè)壁和上壁障礙物和機(jī)械手分別擬作OBB有向包圍盒進(jìn)行檢測(cè)，完全適用于與三維A*算法的相結(jié)合，在保證路徑擇優(yōu)的基礎(chǔ)上極大的提高了效率性及其安全性。