基于模糊人工勢(shì)場(chǎng)法的機(jī)械臂避障達(dá)點(diǎn)研究

韓 峰，趙 京

（北京工業(yè)大學(xué) 機(jī)械工程與應(yīng)用電子技術(shù)學(xué)院，北京 100124）

0 引言

機(jī)器人的路徑規(guī)劃算法有RRT，A*等，還有一些人工智能算法。但是人工勢(shì)場(chǎng)法以其簡(jiǎn)單的特點(diǎn)獲得了廣泛的應(yīng)用。人工勢(shì)場(chǎng)法是由KHABIT[1]提出的。王俊龍等[2]利用虛擬斥力勢(shì)場(chǎng)法解決了人工勢(shì)場(chǎng)法中的局部極小問(wèn)題。唐彪等[3]利用添加虛擬目標(biāo)點(diǎn)來(lái)改進(jìn)人工勢(shì)場(chǎng)法。Wei Guan等[4]利用變步長(zhǎng)結(jié)合重構(gòu)的勢(shì)場(chǎng)函數(shù)來(lái)逃離局部極小點(diǎn)。楊小菊等[5]利用模糊控制算法進(jìn)行移動(dòng)機(jī)器人的避障研究，將傳感器采集到的信息反饋給模糊控制器，進(jìn)而不斷修正位姿進(jìn)行避障。Amal Karray等[6]利用自適應(yīng)模糊控制器實(shí)現(xiàn)移動(dòng)機(jī)械臂的軌跡追蹤和避障。

針對(duì)人工勢(shì)場(chǎng)法常見(jiàn)的局部極小問(wèn)題的解決方法大多集中在陷入極小問(wèn)題后如何逃離，本文通過(guò)添加模糊控制器控制引力與斥力勢(shì)場(chǎng)權(quán)重系數(shù)可以避免陷入局部極小，規(guī)避該問(wèn)題。

1 人工勢(shì)場(chǎng)法的原理

傳統(tǒng)的人工勢(shì)場(chǎng)法就是在笛卡爾空間中引入了引力勢(shì)場(chǎng)以及斥力勢(shì)場(chǎng)，引力勢(shì)場(chǎng)是由目標(biāo)點(diǎn)對(duì)機(jī)械臂的吸引產(chǎn)生的，而斥力勢(shì)場(chǎng)是由障礙物對(duì)機(jī)械臂的排斥所產(chǎn)生的。在這兩個(gè)合勢(shì)場(chǎng)的共同作用下，機(jī)械臂到達(dá)目標(biāo)點(diǎn)并實(shí)現(xiàn)了避障。設(shè)機(jī)械臂的當(dāng)前位置是P=[x，y，z]，目標(biāo)點(diǎn)的位置是Pg=[xg，yg，zg]，那么引力勢(shì)場(chǎng)的一般式如式(1)所示。

式(1)中ka表示引力勢(shì)場(chǎng)系數(shù)，Uatt就是引力勢(shì)場(chǎng)。

斥力勢(shì)場(chǎng)表示為式(2)所示。

式(2)中kr是斥力場(chǎng)系數(shù)，ρ（P0，P）是障礙物到機(jī)器人的距離，ρ0是斥力勢(shì)場(chǎng)的影響距離，是一個(gè)常數(shù)。

2 改進(jìn)算法

2.1 關(guān)節(jié)空間搜索

傳統(tǒng)的人工勢(shì)場(chǎng)法通常針對(duì)的是平面移動(dòng)機(jī)器人，在二維空間中受引力和斥力的合力作用。但這里是針對(duì)機(jī)械臂的路徑規(guī)劃，而機(jī)械臂是強(qiáng)耦合以及在三維空間運(yùn)動(dòng)的。如果在三維空間進(jìn)行路徑規(guī)劃就需要進(jìn)行反解，這就大大增加了計(jì)算量，同時(shí)容易出現(xiàn)奇異點(diǎn)的問(wèn)題。本文中是在關(guān)節(jié)空間進(jìn)行的規(guī)劃。關(guān)節(jié)空間搜索方法如下：

1）設(shè)當(dāng)前的關(guān)節(jié)位置為θ=（θ1，θ2，…，θn），n為機(jī)械臂關(guān)節(jié)數(shù)目。

2）設(shè)定步長(zhǎng)為σ，遍歷每個(gè)關(guān)節(jié)角的相鄰位置,每個(gè)關(guān)節(jié)角都有3個(gè)位置，則總共有3n種組合方式。

在手術(shù)室內(nèi),由經(jīng)管護(hù)士繼續(xù)以親切的語(yǔ)言和熱情的態(tài)度與其交流,與患者建立良好的護(hù)患關(guān)系,以減輕其因?yàn)榄h(huán)境改變而帶來(lái)的心理壓力。給患者介紹手術(shù)環(huán)境,手術(shù)過(guò)程,以及注意情況,多與患者溝通,適時(shí)給予安慰和鼓勵(lì),及時(shí)解決患者的心理問(wèn)題,提高患者對(duì)其自身疾病的認(rèn)知程度和應(yīng)對(duì)能力。消除患者對(duì)手術(shù)疼痛的恐懼感及焦躁的情緒,以積極平和的心態(tài)應(yīng)對(duì)術(shù)后疼痛等問(wèn)題,提高對(duì)治療的依從性。

3）計(jì)算每種組合的合勢(shì)能，最小的勢(shì)能所對(duì)應(yīng)的關(guān)節(jié)角就是下一步的關(guān)節(jié)角位置。

4）如此循環(huán)計(jì)算，直至到達(dá)目標(biāo)點(diǎn)。

在關(guān)節(jié)空間中搜索，將合勢(shì)場(chǎng)作為目標(biāo)函數(shù)，機(jī)械臂就向著合勢(shì)場(chǎng)最小的方向運(yùn)動(dòng)，該方式規(guī)避了反解等問(wèn)題。降低了計(jì)算的難度和強(qiáng)度。

2.2 勢(shì)場(chǎng)函數(shù)

針對(duì)人工勢(shì)場(chǎng)法中勢(shì)場(chǎng)函數(shù)中的參數(shù)引力與斥力勢(shì)場(chǎng)系數(shù)ka，kr不好確定的問(wèn)題，這里對(duì)勢(shì)場(chǎng)函數(shù)進(jìn)行了改進(jìn)，確定了參數(shù)的選取。

引力勢(shì)能：

式(3)中P是機(jī)械臂末端的位置，Pg是目標(biāo)點(diǎn)位置，Pc是機(jī)械臂末端初始位置。

斥力勢(shì)能：

3 模糊控制器設(shè)計(jì)

這里設(shè)計(jì)了一個(gè)模糊控制器來(lái)控制勢(shì)場(chǎng)函數(shù)中的引力勢(shì)場(chǎng)權(quán)重系數(shù)Wa和斥力勢(shì)場(chǎng)權(quán)重系數(shù)Wr。該模糊控制器將機(jī)械臂與障礙物的距離以及該距離的變化率作為輸入，權(quán)重系數(shù)Wa，Wr作為輸出。如圖1所示。

圖1 勢(shì)場(chǎng)權(quán)重模糊控制器

引入權(quán)重系數(shù)后，合勢(shì)場(chǎng)就如式(5)所示。根據(jù)機(jī)械臂與障礙物的距離以及距離變化率，模糊控制器控制兩權(quán)重系數(shù)的大小。

輸入為障礙物與機(jī)械臂距離，其論域是[100，1000]，其模糊語(yǔ)言變量為['VN'，'N'，'M'，'F'，'VF']，距離變化率的論域?yàn)閇-1，1]，模糊語(yǔ)言變量為['Negative'，'Zero'，'Positive']。圖2為機(jī)械臂與障礙物距離隸屬度函數(shù)，這里用的是三角形隸屬函數(shù)。圖3為機(jī)械臂與障礙物距離變化率隸屬度函數(shù)。

圖2 障礙物距離隸屬度函數(shù)

圖3 障礙物距離變化率隸屬度函數(shù)

輸出為引力勢(shì)場(chǎng)權(quán)重系數(shù)Wa和斥力勢(shì)場(chǎng)權(quán)重系數(shù)Wr。Wa其論域?yàn)閇5，1 0]，模糊語(yǔ)言變量為['VS'，'S'，'M'，'B'，'VB']；Wr論域?yàn)閇0，5]，模糊語(yǔ)言變量為['VS'，'S'，'M'，'B'，'VB']。圖4和圖5分別為Wa和Wr的隸屬度函數(shù)。模糊系統(tǒng)的Wa和Wr輸出曲面如圖6、圖7所示。

圖4 引力勢(shì)場(chǎng)權(quán)重系數(shù)隸屬度函數(shù)

圖5 斥力勢(shì)場(chǎng)權(quán)重系數(shù)隸屬度函數(shù)

圖6 引力勢(shì)場(chǎng)權(quán)重系數(shù)輸出曲面

圖7 斥力勢(shì)場(chǎng)權(quán)重系數(shù)輸出曲面

該模糊控制器的規(guī)則如表1所示。本文的模糊推理采用Mamdani法，反模糊化采用了重心法。采用的是IFTHEN條件語(yǔ)言，共有3×5=15條模糊控制規(guī)則。

表1 模糊控制規(guī)則

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

4.1 機(jī)械臂建模

這里用MATLAB進(jìn)行了仿真實(shí)驗(yàn)。選定的機(jī)械臂為kukaiiwa機(jī)械臂，如圖8所示。其DH參數(shù)如表2所示。這里仿真只用到該機(jī)械臂的前四個(gè)關(guān)節(jié)。

圖8 kukaiiwa機(jī)械臂

表2 kukaiiwaDH參數(shù)

圖9 機(jī)械臂前四個(gè)自由度DH參數(shù)模型

該kukaiiwa機(jī)械臂是七由度的，有肩部，肘部和腕部。這里只關(guān)注前四個(gè)自由度，腕部關(guān)節(jié)作為末端點(diǎn)。

式(6)、式(7)，式(8)中P，Pe，Ps分別是機(jī)械臂末端，肘部以及肩部的空間位置坐標(biāo)。

4.2 局部極小問(wèn)題

假設(shè)機(jī)械臂初始位置時(shí)θ=[0，0，0，0，0，0，0]，目標(biāo)點(diǎn)的坐標(biāo)為Pg=[400，400，400]，障礙物坐標(biāo)為P0=[200，200，500]，d0min=100，d0max=300。首先對(duì)傳統(tǒng)算法的進(jìn)行了仿真。圖10是傳統(tǒng)算法中機(jī)械臂的四個(gè)關(guān)節(jié)角位置圖。

圖10 傳統(tǒng)算法機(jī)械臂四個(gè)關(guān)節(jié)角位置

由圖可以看出機(jī)械臂關(guān)節(jié)角保持不變，機(jī)械臂末端并為達(dá)到目標(biāo)點(diǎn)。此時(shí)機(jī)械臂陷入了局部極小。同樣的任務(wù)對(duì)改進(jìn)算法進(jìn)行了仿真。關(guān)節(jié)角位置如圖11所示。

圖11 改進(jìn)算法機(jī)械臂四個(gè)關(guān)節(jié)角位置

圖12是機(jī)械臂末端與目標(biāo)點(diǎn)距離，由圖可以看出機(jī)械臂到達(dá)了目標(biāo)點(diǎn)。機(jī)械臂與障礙物距離如圖13所示，可以看出機(jī)械臂與障礙物距離大于最小安全閾值d0min，機(jī)械臂實(shí)現(xiàn)了避障達(dá)點(diǎn)。

圖12 機(jī)械臂末端與目標(biāo)點(diǎn)距離

圖13 機(jī)械臂與障礙物距離

5 物理樣機(jī)實(shí)驗(yàn)

這里使用了kukaiiwa機(jī)械臂進(jìn)行了實(shí)驗(yàn)驗(yàn)證。如圖14所示，機(jī)械臂實(shí)現(xiàn)了避障達(dá)點(diǎn)運(yùn)動(dòng)。

圖14 kukaiiwa機(jī)械臂避障實(shí)驗(yàn)

6 結(jié)語(yǔ)

使用模糊人工勢(shì)場(chǎng)法在關(guān)節(jié)空間進(jìn)行避障路徑規(guī)劃，對(duì)原算法的勢(shì)場(chǎng)函數(shù)進(jìn)行了改進(jìn)，同時(shí)加入了權(quán)重系數(shù)，利用模糊控制器控制該參數(shù)，解決了局部極小問(wèn)題。通過(guò)MATLAB仿真以及kukaiiwa機(jī)械臂的實(shí)驗(yàn)驗(yàn)證了方法的有效性。