[發(fā)明專利]一種基于蟻群算法的空間機(jī)械臂末端器避障路徑規(guī)劃的方法

在審

• 申請(qǐng)?zhí)枺?/td>: 201410391192.3
• 申請(qǐng)日: 2014-08-11
• 公開（公告）號(hào): CN104199292A
• 公開（公告）日: 2014-12-10
• 發(fā)明（設(shè)計(jì)）人: 張強(qiáng);周東生;王蘭
• 申請(qǐng)（專利權(quán)）人: 大連大學(xué)
• 主分類號(hào): G05B13/04
• 分類號(hào): G05B13/04;G05D1/10
• 代理公司: 大連創(chuàng)達(dá)專利代理事務(wù)所(普通合伙) 21237
• 代理人: 趙英杰

技術(shù)領(lǐng)域

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

背景技術(shù)

空間機(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í)?duì)機(jī)械臂系統(tǒng)及操作人員造成傷害，因此空間機(jī)械臂的避障路徑規(guī)劃具有十分重要的研究意義。

空間機(jī)械臂避障規(guī)劃的目標(biāo)是為空間機(jī)械臂規(guī)劃出一條從起始點(diǎn)到目標(biāo)點(diǎn)避開所有障礙物并且滿足某項(xiàng)指標(biāo)最優(yōu)的路徑。針對(duì)路徑規(guī)劃問題，已經(jīng)采用的方法有C空間法、A^*搜索法、改進(jìn)人工勢(shì)場(chǎng)法、遺傳算法、蟻群算法等，它們都存在一定的局限性。由于C空間法的計(jì)算時(shí)間要比機(jī)械臂的響應(yīng)時(shí)間長(zhǎng)，因此限制了其在實(shí)際避障中的應(yīng)用；A^*算法的計(jì)算量會(huì)隨著維數(shù)的增加而急劇增加，其時(shí)空要求很難得到滿足；改進(jìn)人工勢(shì)場(chǎng)法非常適用于處理動(dòng)態(tài)避障，但其容易陷入局部最小點(diǎn)處；在復(fù)雜的環(huán)境中，遺傳算法很難找到一條滿足約束條件的可行路徑；蟻群算法具有較強(qiáng)的魯棒性，能夠進(jìn)行分布式計(jì)算，易于與其他方法相結(jié)合，但同時(shí)也存在一些缺陷，如收斂速度慢，易于陷入局部最優(yōu)。

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

鑒于現(xiàn)有技術(shù)存在的問題，本發(fā)明的目的在于提出一種基于改進(jìn)的蟻群算法的空間機(jī)械臂末端器避障路徑規(guī)劃的方法，著重解決蟻群算法收斂速度慢，易于陷入局部最優(yōu)的缺點(diǎn)，使機(jī)械臂末端器不僅能順利地避開所有障礙物，還能保證其運(yùn)動(dòng)的路徑比蟻群算法搜索到的路徑更短。

為了實(shí)現(xiàn)上述目的，本發(fā)明所采用的技術(shù)方案為基于改進(jìn)的蟻群算法的空間機(jī)械臂末端器避障路徑規(guī)劃的方法，其步驟如下：

步驟一：建立空間機(jī)械臂模型和環(huán)境模型；

根據(jù)廣義雅可比矩陣對(duì)空間機(jī)械臂建立運(yùn)動(dòng)學(xué)模型，然后采用等分空間方法在三維空間中建立路徑規(guī)劃所需的網(wǎng)格點(diǎn)，最后設(shè)計(jì)障礙物均為頂面和底面與XY面平行的不規(guī)則體。

步驟二：參數(shù)初始化；

需要初始化的參數(shù)包括：起始點(diǎn)和目標(biāo)點(diǎn)的坐標(biāo)；螞蟻的數(shù)量N；迭代次數(shù)N_c的初始值及最大值N_cmax；信息素衰減系數(shù)ζ和更新系數(shù)ρ。

步驟三：設(shè)計(jì)啟發(fā)式函數(shù)進(jìn)行路徑搜索，并更新局部信息素；

設(shè)啟發(fā)式函數(shù)為：

H(i,j,k)＝D(i,j,k)·S(i,j,k)·Q(i,j,k)

在上式中，i、j和k分別表示劃分規(guī)劃空間ABCDEFGH時(shí)沿邊AB、邊AD和AE的劃分序號(hào)。

D(i,j,k)表示兩點(diǎn)間的距離，促使螞蟻向離它近的點(diǎn)移動(dòng)，其表達(dá)式為：

D(i,j,k)＝sqrt[(x_a-x_b)²+(y_a-y_b)²+(z_a-z_b)²]

其中，(x,y,z)表示規(guī)劃空間中的任意一點(diǎn)對(duì)應(yīng)的坐標(biāo)。a、b分別為當(dāng)前點(diǎn)和下一個(gè)點(diǎn)。

S(i,j,k)表示安全性因素，促使螞蟻選擇安全點(diǎn)，當(dāng)選擇點(diǎn)不可達(dá)到時(shí)，其表達(dá)式為：S(i,j,k)＝0。

Q(i,j,k)為下一點(diǎn)到目標(biāo)點(diǎn)的路徑長(zhǎng)度，促使螞蟻選擇距離目標(biāo)更近的點(diǎn)，表達(dá)式如下：

Q(i,j,k)＝sqrt[(x_b-x_d)²+(y_b-y_d)²+(z_b-z_d)²]

其中，b、d分別表示下一個(gè)點(diǎn)和目標(biāo)點(diǎn)。

如果一只螞蟻位于節(jié)點(diǎn)i，那么它根據(jù)下面的方程式選擇下一個(gè)要轉(zhuǎn)移的節(jié)點(diǎn)k：