韓 峰，趙 京

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

0 引言

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

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

1 人工勢場法的原理

傳統(tǒng)的人工勢場法就是在笛卡爾空間中引入了引力勢場以及斥力勢場，引力勢場是由目標(biāo)點(diǎn)對機(jī)械臂的吸引產(chǎn)生的，而斥力勢場是由障礙物對機(jī)械臂的排斥所產(chǎn)生的。在這兩個合勢場的共同作用下，機(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]，那么引力勢場的一般式如式(1)所示。

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

斥力勢場表示為式(2)所示。

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

2 改進(jìn)算法

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

傳統(tǒng)的人工勢場法通常針對的是平面移動機(jī)器人，在二維空間中受引力和斥力的合力作用。但這里是針對機(jī)械臂的路徑規(guī)劃，而機(jī)械臂是強(qiáng)耦合以及在三維空間運(yùn)動的。如果在三維空間進(jìn)行路徑規(guī)劃就需要進(jìn)行反解，這就大大增加了計算量，同時容易出現(xiàn)奇異點(diǎ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è)定步長為σ，遍歷每個關(guān)節(jié)角的相鄰位置,每個關(guān)節(jié)角都有3個位置，則總共有3n種組合方式。

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

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

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

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

2.2 勢場函數(shù)

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

引力勢能：

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

斥力勢能：

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

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

圖1 勢場權(quán)重模糊控制器

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

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

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

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

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

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

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

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

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

該模糊控制器的規(guī)則如表1所示。本文的模糊推理采用Mamdani法，反模糊化采用了重心法。采用的是IFTHEN條件語言，共有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ī)械臂的前四個關(guān)節(jié)。

圖8 kukaiiwa機(jī)械臂

表2 kukaiiwaDH參數(shù)

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

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

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

4.2 局部極小問題

假設(shè)機(jī)械臂初始位置時θ=[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。首先對傳統(tǒng)算法的進(jìn)行了仿真。圖10是傳統(tǒng)算法中機(jī)械臂的四個關(guān)節(jié)角位置圖。

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

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

圖11 改進(jìn)算法機(jī)械臂四個關(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)動。

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

6 結(jié)語

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