一種基于蟻群算法的空間機(jī)械臂末端器避障路徑規(guī)劃的方法
【專利摘要】本發(fā)明設(shè)計(jì)了一種基于蟻群算法的空間機(jī)械臂末端器避障規(guī)劃的方法。該方法首先對空間機(jī)械臂和環(huán)境進(jìn)行建模，然后以起始點(diǎn)到目標(biāo)點(diǎn)之間的避障路徑既短又安全為目標(biāo)來定義適應(yīng)度函數(shù)，并對蟻群算法中的搜索策略、轉(zhuǎn)移規(guī)則和信息素更新方法進(jìn)行改進(jìn)，最后利用改進(jìn)的蟻群算法搜索最優(yōu)的避障路徑。本發(fā)明著重解決的問題是利用改進(jìn)的蟻群算法實(shí)現(xiàn)空間機(jī)械臂末端器的避障。
【專利說明】一種基于蟻群算法的空間機(jī)械臂末端器避障路徑規(guī)劃的方法

【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及一種屬于空間機(jī)械臂避障路徑規(guī)劃的方法，具體講是涉及一種基于蟻群算法的空間機(jī)械臂末端器避障路徑規(guī)劃的方法，其屬于宇航控制領(lǐng)域。

【背景技術(shù)】
[0002]空間機(jī)器人是由機(jī)器人本體(衛(wèi)星)和其搭載的機(jī)械臂組成。由于機(jī)器人本體內(nèi)攜帶氣體推進(jìn)器，它可以在空間微重力環(huán)境下自由飛行或浮游，從而擴(kuò)展了機(jī)器人的工作空間，因此它將代替宇航員從事各種艙外作業(yè)，成為今后空間機(jī)器人的主要研究方向之一?？臻g環(huán)境中的碎片、空間艙體外設(shè)試驗(yàn)裝置等都有可能成為空間機(jī)械臂在軌操作過程中的障礙，而機(jī)械臂與障礙物之間的碰撞不僅會(huì)干擾在軌操作任務(wù)的完成，甚至?xí)C(jī)械臂系統(tǒng)及操作人員造成傷害，因此空間機(jī)械臂的避障路徑規(guī)劃具有十分重要的研究意義。
[0003]空間機(jī)械臂避障規(guī)劃的目標(biāo)是為空間機(jī)械臂規(guī)劃出一條從起始點(diǎn)到目標(biāo)點(diǎn)避開所有障礙物并且滿足某項(xiàng)指標(biāo)最優(yōu)的路徑。針對路徑規(guī)劃問題，已經(jīng)采用的方法有C空間法、A*搜索法、改進(jìn)人工勢場法、遺傳算法、蟻群算法等，它們都存在一定的局限性。由于C空間法的計(jì)算時(shí)間要比機(jī)械臂的響應(yīng)時(shí)間長，因此限制了其在實(shí)際避障中的應(yīng)用算法的計(jì)算量會(huì)隨著維數(shù)的增加而急劇增加，其時(shí)空要求很難得到滿足；改進(jìn)人工勢場法非常適用于處理動(dòng)態(tài)避障，但其容易陷入局部最小點(diǎn)處；在復(fù)雜的環(huán)境中，遺傳算法很難找到一條滿足約束條件的可行路徑；蟻群算法具有較強(qiáng)的魯棒性，能夠進(jìn)行分布式計(jì)算，易于與其他方法相結(jié)合，但同時(shí)也存在一些缺陷，如收斂速度慢，易于陷入局部最優(yōu)。


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

[0004]鑒于現(xiàn)有技術(shù)存在的問題，本發(fā)明的目的在于提出一種基于改進(jìn)的蟻群算法的空間機(jī)械臂末端器避障路徑規(guī)劃的方法，著重解決蟻群算法收斂速度慢，易于陷入局部最優(yōu)的缺點(diǎn)，使機(jī)械臂末端器不僅能順利地避開所有障礙物，還能保證其運(yùn)動(dòng)的路徑比蟻群算法搜索到的路徑更短。
[0005]為了實(shí)現(xiàn)上述目的，本發(fā)明所采用的技術(shù)方案為基于改進(jìn)的蟻群算法的空間機(jī)械臂末端器避障路徑規(guī)劃的方法，其步驟如下:
[0006]步驟一:建立空間機(jī)械臂模型和環(huán)境模型；
[0007]根據(jù)廣義雅可比矩陣對空間機(jī)械臂建立運(yùn)動(dòng)學(xué)模型，然后采用等分空間方法在三維空間中建立路徑規(guī)劃所需的網(wǎng)格點(diǎn)，最后設(shè)計(jì)障礙物均為頂面和底面與XY面平行的不規(guī)則體。
[0008]步驟二:參數(shù)初始化；
[0009]需要初始化的參數(shù)包括:起始點(diǎn)和目標(biāo)點(diǎn)的坐標(biāo)；螞蟻的數(shù)量N ;迭代次數(shù)N。的初始值及最大值Nanax ;信息素衰減系數(shù)ζ和更新系數(shù)P。
[0010]步驟三:設(shè)計(jì)啟發(fā)式函數(shù)進(jìn)行路徑搜索，并更新局部信息素；
[0011]設(shè)啟發(fā)式函數(shù)為:
[0012]H(i，j，k) = D(i, j, k).S(i, j, k).Q(i, j, k)
[0013]在上式中，1、j和k分別表示劃分規(guī)劃空間AB⑶EFGH時(shí)沿邊AB、邊AD和AE的劃分序號。
[0014]D(i, j, k)表示兩點(diǎn)間的距離，促使螞蟻向離它近的點(diǎn)移動(dòng)，其表達(dá)式為:
[0015]D(i, j, k) = sqrt [ (xa_xb)2+ (ya-yb)2+ (za-zb)2]
[0016]其中，(x, y, z)表示規(guī)劃空間中的任意一點(diǎn)對應(yīng)的坐標(biāo)。a、b分別為當(dāng)前點(diǎn)和下一個(gè)點(diǎn)。
[0017]S(i, j, k)表示安全性因素，促使螞蟻選擇安全點(diǎn)，當(dāng)選擇點(diǎn)不可達(dá)到時(shí)，其表達(dá)式為:S(i，j，k) = O。
[0018]Q(i, j, k)為下一點(diǎn)到目標(biāo)點(diǎn)的路徑長度，促使螞蟻選擇距離目標(biāo)更近的點(diǎn)，表達(dá)式如下:
[0019]Q(i, j, k) = sqrt [ (xb_xd)2+ (yb-yd)2+ (zb-zd)2]
[0020]其中，b、d分別表示下一個(gè)點(diǎn)和目標(biāo)點(diǎn)。
[0021]如果一只螞蟻位于節(jié)點(diǎn)i，那么它根據(jù)下面的方程式選擇下一個(gè)要轉(zhuǎn)移的節(jié)點(diǎn)k:[ ]化 j arg max jr (? + ].//.V')// (i7 + l,//.v)} if q < q0
I jelse
[0023]其中，(a+l，u，v)表示規(guī)劃空間中要選擇的下一個(gè)點(diǎn)的坐標(biāo)，τ (a+1, u, v)為平面Π?+1上點(diǎn)P(a+1，u，v)的信息素值，H(a+1，u, v)為點(diǎn)Pi到平面ni+1內(nèi)的可行點(diǎn)集合的啟發(fā)信息值，q是在(0，I]區(qū)間均勻分布的隨機(jī)數(shù)，％是[0，I]區(qū)間內(nèi)的數(shù)，j是根據(jù)下面公式選擇出來的節(jié)點(diǎn):
[0024]

【權(quán)利要求】
1.一種基于蟻群算法的空間機(jī)械臂末端器避障規(guī)劃的方法，其特征在于，該方法包括以下步驟: 步驟一:建立空間機(jī)械臂模型和環(huán)境模型； 步驟二:參數(shù)初始化； 步驟三:設(shè)計(jì)啟發(fā)式函數(shù)進(jìn)行路徑搜索，并更新局部信息素； 啟發(fā)式函數(shù)為:
H(i，j，k) =D(i，j，k).S(i, j, k).Q(i, j, k) (I) 式(I)中，1、j和k分 別表示劃分規(guī)劃空間AB⑶EFGH時(shí)沿邊AB、邊AD和AE的劃分序號; D(i，j，k)表示兩點(diǎn)間的距離，促使螞蟻向離它近的點(diǎn)移動(dòng)，其表達(dá)式為:
D (i, j, k) = sqrt [ (xa-xb)2+ (ya-yb)2+ (za_zb)2] (2) S(i，j，k)表示安全性因素，促使螞蟻選擇安全點(diǎn)，當(dāng)選擇點(diǎn)不可達(dá)到時(shí)，其表達(dá)式為:S(i, j, k) = O ； Q(i, j，k)為下一點(diǎn)到目標(biāo)點(diǎn)的路徑長度，促使螞蟻選擇距離目標(biāo)更近的點(diǎn)，表達(dá)式如下:
Q(i，j，k) = sqrt [(xb-xd) 2+(yb_yd)2+(zb-zd)2] (3) 式(2)中，(x，y，z)表示規(guī)劃空間中的任意一點(diǎn)對應(yīng)的坐標(biāo)；a、b分別為當(dāng)前點(diǎn)和下一個(gè)占.式(3)中，b、d分別表示下一個(gè)點(diǎn)和目標(biāo)點(diǎn)； 步驟四:將搜索到的路徑的適應(yīng)度值進(jìn)行比較進(jìn)行路徑規(guī)劃，并進(jìn)行全局信息素更新； 在進(jìn)行路徑規(guī)劃時(shí)，取X軸為路徑規(guī)劃的主方向，將適應(yīng)度函數(shù)定義為:
式(4)中，η為種群的個(gè)數(shù)，i表示當(dāng)前點(diǎn)，i+1表示下一點(diǎn)；如果新路徑比已知最優(yōu)路徑的長度短，則用新路徑替代原來的最優(yōu)路徑； 步驟五:如果滿足迭代次數(shù)要求，則輸出最優(yōu)的避障路徑。
2.根據(jù)權(quán)利要求1所述的基于改進(jìn)的蟻群算法的空間機(jī)械臂末端器避障路徑規(guī)劃的方法，其特征在于:所述空間機(jī)械臂模型為跟據(jù)廣義雅可比矩陣對空間機(jī)械臂建立的運(yùn)動(dòng)學(xué)模型，所述環(huán)境模型是采用等分空間方法在三維空間中建立路徑規(guī)劃所需的網(wǎng)格點(diǎn)，并設(shè)計(jì)障礙物均為頂面和底面與XY面平行的不規(guī)則體。
3.根據(jù)權(quán)利要求1所述的基于改進(jìn)的蟻群算法的空間機(jī)械臂末端器避障路徑規(guī)劃的方法，其特征在于:所述步驟二中參數(shù)初始化的參數(shù)分別為:起始點(diǎn)和目標(biāo)點(diǎn)的坐標(biāo)、螞蟻的數(shù)量N、迭代次數(shù)N。的初始值及最大值Ncmax、信息素衰減系數(shù)ζ和更新系數(shù)P。
4.根據(jù)權(quán)利要求1所述的基于改進(jìn)的蟻群算法的空間機(jī)械臂末端器避障路徑規(guī)劃的方法，其特征在于:所述步驟三中路徑搜索為: 設(shè)一只螞蟻位于節(jié)點(diǎn)i，那么它根據(jù)下面的方程式選擇下一個(gè)要轉(zhuǎn)移的節(jié)點(diǎn)k:
式(5)中，(a+l，u，v)表示規(guī)劃空間中要選擇的下一個(gè)點(diǎn)的坐標(biāo)，τ (a+1, u, v)為平面Π?+1上點(diǎn)P(a+1，u，v)的信息素值，H(a+1，u, v)為點(diǎn)Pi到平面ni+1內(nèi)的可行點(diǎn)集合的啟發(fā)信息值，q是在(O，I]區(qū)間均勻分布的隨機(jī)數(shù)，％是[O，I]區(qū)間內(nèi)的數(shù)，j是根據(jù)下面公式選擇出來的節(jié)點(diǎn):
5.根據(jù)權(quán)利要求1所述的基于改進(jìn)的蟻群算法的空間機(jī)械臂末端器避障路徑規(guī)劃的方法，其特征在于:所述步驟三中局部信息素更新隨著螞蟻的搜索進(jìn)行，其更新公式為:
τ ijk = (1- ξ ) τ iJk (6) 式(6)中，Tijk為點(diǎn)(i, j，k)上所帶的信息素值，ζ為信息素的衰減系數(shù)。
6.根據(jù)權(quán)利要求1所述的基于改進(jìn)的蟻群算法的空間機(jī)械臂末端器避障路徑規(guī)劃的方法，其特征在于:所述步驟四中全局信息素更新公式如下:
τ ijk = (1-P ) τ iJk+p Δ τ iJk (7) 式(7)中:P為信息素更新系數(shù)，Δ τ iJk為
式(8)中，K是系數(shù)，length(m)為樂m只螞蟻經(jīng)過的路徑長度。
7.根據(jù)權(quán)利要求1所述的基于改進(jìn)的蟻群算法的空間機(jī)械臂末端器避障路徑規(guī)劃的方法，其特征在于:步驟五中，如果迭代次數(shù)NcXNemax,則轉(zhuǎn)步驟三，否則輸出最優(yōu)的避障路徑。
【文檔編號】G05D1/10GK104199292SQ201410391192
【公開日】2014年12月10日 申請日期:2014年8月11日 優(yōu)先權(quán)日:2014年8月11日
【發(fā)明者】張強(qiáng), 周東生, 王蘭 申請人:大連大學(xué)