實(shí)現(xiàn)空間機(jī)械臂避障的路徑規(guī)劃方法
【專(zhuān)利摘要】本方法主要用于空間機(jī)械臂避障路徑規(guī)劃。包括以下步驟：建立窗口型(正方形)障礙，利用人工勢(shì)場(chǎng)方法找出一條機(jī)械臂末端器的避障路徑；通過(guò)求機(jī)械臂各連桿與窗口障礙各邊的距離的碰撞檢測(cè)來(lái)調(diào)整整個(gè)機(jī)械臂末端器軌跡，使得各個(gè)連桿均不與障礙物碰撞，從而得到一條能使整個(gè)機(jī)械臂(包括末端器和連桿)安全避障的末端器路徑；運(yùn)用基于廣義雅克比矩陣的空間機(jī)械臂運(yùn)動(dòng)學(xué)逆解對(duì)上述的末端器路徑上的位置和姿態(tài)進(jìn)行連續(xù)跟蹤。應(yīng)用該障礙回避方法可使其針對(duì)整個(gè)空間機(jī)械臂(包括末端器和連桿)進(jìn)行避障路徑規(guī)劃。
【專(zhuān)利說(shuō)明】實(shí)現(xiàn)空間機(jī)械臂避障的路徑規(guī)劃方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明屬于空間機(jī)械臂的路徑規(guī)劃領(lǐng)域，尤其涉及一種實(shí)現(xiàn)空間機(jī)械臂的避障路徑規(guī)劃方法。
【背景技術(shù)】
[0002]由于太空任務(wù)環(huán)境復(fù)雜，使用機(jī)械臂代替宇航員進(jìn)行工作是未來(lái)航空航天的主要發(fā)展方向，而空間機(jī)械臂的避障路徑規(guī)劃對(duì)機(jī)械臂在太空環(huán)境中的實(shí)際應(yīng)用起到至關(guān)重要的作用。
[0003]避障路徑規(guī)劃是指給定環(huán)境的障礙條件以及起始位姿和目標(biāo)位姿，要求選擇一條從起始點(diǎn)到目標(biāo)點(diǎn)的路徑，使機(jī)械臂能安全、無(wú)碰撞地通過(guò)所有的障礙。
[0004]關(guān)于機(jī)械臂的避障路徑規(guī)劃方法有很多，如滾動(dòng)路徑規(guī)劃法、Stentz A的D*算法、遺傳算法等，比較經(jīng)典的是用蟻群算法，其具有極強(qiáng)的魯棒性和搜索較好解的能力。但是，這種方法多用于2維平面或者2維等高平面，在3維空間中實(shí)現(xiàn)基于蟻群算法的避障需要增加大量的計(jì)算。而人工勢(shì)場(chǎng)法是以其快捷的環(huán)境描述形式在實(shí)時(shí)避障、運(yùn)動(dòng)規(guī)劃中得到了廣泛應(yīng)用。人 工勢(shì)場(chǎng)法由Khatib提出，他在文獻(xiàn)中構(gòu)造了人工感應(yīng)力函數(shù)。此后，人工勢(shì)場(chǎng)法得到不斷的改進(jìn)與完善。Khosla提出超二次函數(shù)(SuperquadricFunct1n)J^F同形狀障礙物斥力場(chǎng)統(tǒng)一逼近為球形障礙物的斥力場(chǎng)，又提出從電場(chǎng)、溫度場(chǎng)、流體場(chǎng)的角度，運(yùn)用調(diào)和函數(shù)(Harmonic Funct1n)來(lái)回避局部最小問(wèn)題。目前的研究主要集中在地面機(jī)器人的路徑規(guī)劃問(wèn)題以及對(duì)空間機(jī)械臂末端器避障路徑規(guī)劃的研究，而對(duì)整個(gè)空間機(jī)械臂(包括連桿和末端器)避障的研究還極為有限。受到人工勢(shì)場(chǎng)方法的啟發(fā)，本發(fā)明對(duì)其進(jìn)行擴(kuò)展，用以對(duì)空間機(jī)械臂整體進(jìn)行避障路徑規(guī)劃。
[0005]針對(duì)空間機(jī)械臂的控制，由于太空環(huán)境中動(dòng)量守恒，空間機(jī)械臂存在動(dòng)力學(xué)冗余特性，空間機(jī)械臂末端器的位姿不僅和關(guān)節(jié)角有關(guān)還和機(jī)械臂運(yùn)動(dòng)的歷史有關(guān)。所以我們使用廣義雅可比矩陣來(lái)描述空間機(jī)械臂的系統(tǒng)狀態(tài)。由于單純的計(jì)算逆廣義雅可比不可避免的會(huì)帶來(lái)奇異，導(dǎo)致奇異點(diǎn)附近的關(guān)節(jié)角速度無(wú)限大，所以本發(fā)明通過(guò)計(jì)算廣義雅可比的阻尼最小方差逆的方法來(lái)求解關(guān)節(jié)角，以對(duì)空間機(jī)械臂進(jìn)行運(yùn)動(dòng)控制。

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

[0006]本發(fā)明的目的在于:將2維人工勢(shì)場(chǎng)方法擴(kuò)展成適用于3維空間；改進(jìn)人工勢(shì)場(chǎng)方法使其能夠針對(duì)整個(gè)機(jī)械臂(包括末端器和連桿)進(jìn)行避障規(guī)劃并將上述方法與基于廣義雅可比的空間機(jī)械臂逆運(yùn)動(dòng)學(xué)方法相結(jié)合，進(jìn)行空間機(jī)械臂連續(xù)無(wú)碰撞路徑規(guī)劃。
[0007]首先建立窗口型(正方形)障礙，利用人工勢(shì)場(chǎng)方法找出一條機(jī)械臂末端器的避障路徑；然后通過(guò)求機(jī)械臂各連桿與窗口障礙各邊的距離的碰撞檢測(cè)來(lái)調(diào)整整個(gè)機(jī)械臂末端器軌跡，使得各個(gè)連桿均不與障礙物碰撞，從而得到一條使整個(gè)機(jī)械臂能安全避障的末端器路徑；最后運(yùn)用基于廣義雅克比矩陣的空間機(jī)械臂運(yùn)動(dòng)學(xué)逆解對(duì)所述的末端器路徑進(jìn)行連續(xù)跟蹤。下面敘述算法具體步驟:[0008]本發(fā)明的一種實(shí)現(xiàn)空間機(jī)械臂避障的路徑規(guī)劃方法，其特征在于，包括步驟:
[0009](I)建立中心點(diǎn)帶有引力場(chǎng)的窗口型(正方形)障礙，計(jì)算其幾何中心點(diǎn)，在該點(diǎn)上設(shè)置一個(gè)帶吸引力的虛擬點(diǎn)，該點(diǎn)吸引力與目標(biāo)點(diǎn)吸引力公式均為:
【權(quán)利要求】
1.實(shí)現(xiàn)空間機(jī)械臂避障的路徑規(guī)劃方法，其特征在于，包括步驟: (1)建立中心點(diǎn)帶有引力場(chǎng)的窗口型(正方形)障礙，計(jì)算其幾何中心點(diǎn)，在該點(diǎn)上設(shè)置一個(gè)帶吸引力的虛擬點(diǎn)，該點(diǎn)吸引力與目標(biāo)點(diǎn)吸引力公式均為:
2.實(shí)現(xiàn)空間機(jī)械臂避障的路徑規(guī)劃方法，其特征在于，根據(jù)權(quán)利要求1的方法，在步驟(6)中，若成功，輸出該路徑，若迭代η次仍不成功,輸出不存在這樣的軌跡。
【文檔編號(hào)】B25J9/16GK104029203SQ201410274263
【公開(kāi)日】2014年9月10日 申請(qǐng)日期:2014年6月18日 優(yōu)先權(quán)日:2014年6月18日
【發(fā)明者】張強(qiáng), 劉雙 申請(qǐng)人:大連大學(xué)