基于模糊PD算法的三自由度機(jī)械臂遙操作雙邊控制

（西南科技大學(xué) 特殊環(huán)境機(jī)器人技術(shù)四川省重點(diǎn)實(shí)驗(yàn)室，綿陽 621010）

遙操作雙邊控制是典型的人在回路的人機(jī)交互控制系統(tǒng)[1]。由于結(jié)合了人類對復(fù)雜任務(wù)的決策能力和機(jī)器人在危險(xiǎn)環(huán)境中的作業(yè)能力，遙操作系統(tǒng)可以完成復(fù)雜的任務(wù)，已逐漸成為機(jī)器人領(lǐng)域的研究熱點(diǎn)[2]。

在雙邊控制系統(tǒng)中，主要解決穩(wěn)定性、透明性、跟蹤性3個(gè)問題[3]。以前的研究如文獻(xiàn)[3]主要從單自由度主從機(jī)械臂線性控制的角度，在主端輸入正弦或者余弦控制信號，忽略了在實(shí)際使用中需要對多自由度機(jī)械臂的控制，以及操作者的非線性和不同環(huán)境對從端機(jī)械臂的影響。最近也出現(xiàn)了簡單變參數(shù)的線性PD控制器，使得操作性有所提高，但是線性控制器對于非線性系統(tǒng)的控制還是受到很大的制約[2，4]。單從單自由度主從機(jī)械臂線性控制的角度去解決問題會出現(xiàn)在實(shí)際環(huán)境中操作者操作主端機(jī)械臂時(shí)從端機(jī)械臂無法實(shí)時(shí)跟隨，不同熟練程度的操作者操作主端機(jī)械臂會影響系統(tǒng)性能[5-6]。

本文提出一種基于三自由度模糊PD雙邊控制方法。在仿真中將主從三自由度機(jī)械臂與Matlab聯(lián)合進(jìn)行了實(shí)時(shí)仿真，能真實(shí)地研究環(huán)境、操作者對系統(tǒng)性能的影響，用模糊優(yōu)化的方法對雙邊系統(tǒng)的ΔKs、ΔKd參數(shù)進(jìn)行不斷地優(yōu)化，使系統(tǒng)能夠適應(yīng)操作者和不同的環(huán)境，解決操作者手部生理顫抖和多自由度從機(jī)械臂與不同工作環(huán)境相接觸時(shí)的非線性問題。

1 系統(tǒng)模型建立

1.1 三自由度機(jī)械臂數(shù)學(xué)模型

Pantom Omni是一個(gè)具有六自由度的力覺、觸覺設(shè)備，其中3個(gè)自由度具有力反饋、角度傳感器，3個(gè)自由度具有角度傳感器。如圖1所示，建立Pantom Omni基坐標(biāo)系P0、末端點(diǎn)坐標(biāo)系P，機(jī)械臂末端點(diǎn)姿態(tài)與關(guān)節(jié)角關(guān)系如式（1）所示。設(shè)置2臂為L1，設(shè)置3臂為L2，分別從俯視圖（圖2）和正視圖（圖3）建立末端點(diǎn)坐標(biāo)和關(guān)節(jié)角的函數(shù)關(guān)系[7]，L4=l1+A，A=0.035 m，L3=0.025 m。

圖1 Pantom Omni模型Fig.1 Pantom Omni model

圖2 Pantom Omni俯視圖Fig.2 Pantom Omni top view

圖3 Pantom Omni正視圖Fig.3 Pantom Omni elevation view

從圖2、圖3中可以得到末端點(diǎn)x、y、z與各連桿關(guān)系 （末端點(diǎn)坐標(biāo)P（x，y，z）與基坐標(biāo)P0（X，Y，Z），圖3相當(dāng)于將末端點(diǎn)坐標(biāo)通過平移變換到基坐標(biāo)）如式（2）～式（4）所示：

關(guān)節(jié)角度與末端點(diǎn)空間姿態(tài)函數(shù)關(guān)系：

由圖2可以求解θ1：

由式（7）～式（12）可得θ2：

由式（13）～式（15）可求得θ3：

1.2 雙邊控制系統(tǒng)模型

雙邊控制系統(tǒng)動力學(xué)模型為[3，8，9]

式中：fh為操作者施加在主端機(jī)械臂上的力；τm為主端電機(jī)產(chǎn)生的驅(qū)動力；mm為主從端的轉(zhuǎn)動慣量；bm、bs為主從端的阻尼系數(shù)；τs為從端電機(jī)產(chǎn)生的驅(qū)動力；fe為環(huán)境對從端機(jī)械臂施加的力。從端自由環(huán)境時(shí)，環(huán)境對從端的作用力 fe≈0，Pantom Omni電機(jī)輸入輸出關(guān)系為

式中：fs為電機(jī)產(chǎn)生的電磁轉(zhuǎn)矩；Cs為電機(jī)轉(zhuǎn)矩系數(shù)；us（t）為控制電機(jī)驅(qū)動電壓。由式（17）和式（18）可得從端機(jī)械臂輸入電壓與輸出角度的微分方程和傳遞函數(shù)：

2 模糊PD控制方法分析

2.1 模糊PD控制器

在Matlab中搭建雙邊控制系統(tǒng)的仿真平臺，設(shè)計(jì)模糊PD控制器[5，10]，控制器結(jié)構(gòu)如圖4所示。系統(tǒng)輸入量為機(jī)械臂每個(gè)關(guān)節(jié)的角度值θ1、θ2、θ3?？刂葡到y(tǒng)根據(jù)系統(tǒng)輸入的目標(biāo)量對主從機(jī)械臂各個(gè)關(guān)節(jié)的電機(jī)的電壓Ui（i=1、2、3）進(jìn)行快速調(diào)節(jié)。閉環(huán)反饋為通過編碼器實(shí)時(shí)采集的各個(gè)關(guān)節(jié)的角度。

圖4 模糊PD控制結(jié)構(gòu)Fig.4 Fuzzy PD control structure

Matlab中設(shè)計(jì)模糊PD子系統(tǒng)，模糊PD子系統(tǒng)的輸入為變量誤差e=Xin-Xout和誤差變化率 ec= e（k）-e（k-1）。模糊PD的模糊推理系統(tǒng)包括2個(gè)輸入變量e、ec，2個(gè)輸出變量ΔKs、ΔKd。本文選用的是三角型隸屬函數(shù)，在閉環(huán)系統(tǒng)的動態(tài)響應(yīng)曲線上采集需要的參考點(diǎn)，根據(jù)參考點(diǎn)上的誤差和誤差變化率的方向設(shè)計(jì)模糊規(guī)則表，校正后的模糊控制表如表1、表2所示。

表1 ΔKs的模糊規(guī)則表Tab.1 Fuzzy rule table of ΔKs

表2 ΔKd的模糊規(guī)則表Tab.2 Fuzzy rule table of ΔKd

用最大隸屬度函數(shù)法解模糊輸出參數(shù) ΔKs、ΔKd。PD初始參數(shù)ΔKs0、ΔKd0由Ziegler-Nichols設(shè)定公式得到，在計(jì)算值的基礎(chǔ)上根據(jù)響應(yīng)曲線得出比較理想的初始參數(shù)值。由式（21）和式（22）計(jì)算出Ks、Kd控制參數(shù)，調(diào)節(jié)從端機(jī)械臂各個(gè)關(guān)節(jié)角度。

由式（23）～式（26）可得三自由度機(jī)械臂各個(gè)關(guān)節(jié)的角度輸出，通過機(jī)械臂建模中關(guān)節(jié)空間與笛卡爾空間的函數(shù)關(guān)系可以計(jì)算出機(jī)械臂末端點(diǎn)位姿。

3 建立實(shí)驗(yàn)仿真平臺

本實(shí)驗(yàn)使用的實(shí)驗(yàn)平臺由 2臺力反饋設(shè)備Pantom Omni組成，如圖5所示，每臺力反饋設(shè)備具有六自由度，使用具有力覺反饋的3個(gè)關(guān)節(jié)做為控制對象。2臺設(shè)備機(jī)構(gòu)一致不會出現(xiàn)操作空間異構(gòu)的情況，便于對雙邊控制實(shí)驗(yàn)進(jìn)行分析。實(shí)驗(yàn)中操作者操作主端機(jī)械臂（Master）對從端機(jī)械臂（Slave）進(jìn)行控制，通過對末端點(diǎn)姿態(tài)和關(guān)節(jié)角的跟隨曲線分析本文設(shè)計(jì)的雙邊控制系統(tǒng)的穩(wěn)定性、透明性、跟隨性。

圖5 Pantom Omni雙邊控制實(shí)驗(yàn)系統(tǒng)Fig.5 Pantom Omni bilateral control experiment system

在仿真過程中使用PHANSIM TOOLKIT Matlab函數(shù)模塊進(jìn)行試驗(yàn)[11]。PHANSIM TOOLKIT將Pantom Omni開發(fā)工具 OpenHaptics Toolkit與 Matlab SFunction結(jié)合，封裝成 PHANSIM Library，使用slblocks.m 將 PHANSIM Library加 入 Simulink Library，圖6所示為主端機(jī)械臂與Matlab交互過程，對于從端機(jī)械臂工作流程同樣適用。在PHANSIM TOOLKIT中使用PHANToM Teleoperation Block函數(shù)塊，可以將主從機(jī)械臂Pantom Omni的關(guān)節(jié)角數(shù)據(jù)通過網(wǎng)絡(luò)將數(shù)據(jù)采集到Matlab仿真平臺中，在Matlab Simulink中實(shí)時(shí)記錄主從機(jī)械臂運(yùn)動時(shí)各個(gè)關(guān)節(jié)角的跟隨情況。通過建立的機(jī)械臂關(guān)節(jié)空間和末端點(diǎn)的數(shù)學(xué)模型，計(jì)算出機(jī)械臂末端點(diǎn)姿態(tài)，實(shí)時(shí)繪制主從機(jī)械臂末端點(diǎn)姿態(tài)曲線。對T時(shí)間段主從機(jī)械臂關(guān)節(jié)角和末端點(diǎn)姿態(tài)跟隨曲線進(jìn)行分析，測試本文設(shè)計(jì)的雙邊模糊PD控制系統(tǒng)的性能。

圖6 Pantom Omni與Matlab聯(lián)合實(shí)時(shí)仿真工作流程Fig.6 Pantom Omni and Matlab joint real-time simulation work flow chart

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

完成平臺設(shè)計(jì)后，設(shè)置好Matlab仿真步驟，操作者操作主端機(jī)械臂控制從端機(jī)械臂跟隨主端在材質(zhì)不平的桌面上做圓周運(yùn)動。通過主端機(jī)械臂控制從端機(jī)械臂在凹凸不平的桌面上做圓周運(yùn)動，可以分析環(huán)境和操作者的操作對系統(tǒng)的影響，由于是人在回路的實(shí)時(shí)仿真不能做到優(yōu)化前和優(yōu)化后運(yùn)動軌跡一致。實(shí)驗(yàn)仿真曲線和誤差曲線如圖7～圖12所示。

圖7 未經(jīng)過優(yōu)化的主從機(jī)械臂末端點(diǎn)跟隨實(shí)驗(yàn)曲線Fig.7 Master-slave following curve without optimization

圖8 經(jīng)過模糊PD算法優(yōu)化的主從機(jī)械臂末端點(diǎn)跟隨實(shí)驗(yàn)曲線Fig.8 Master-slave following curve with the optimization of the fuzzy PD algorithm

圖9 經(jīng)過模糊PD算法優(yōu)化的主從機(jī)械臂末端點(diǎn)誤差曲線Fig.9 Master-slave follow error curve with the optimization of the fuzzy PD algorithm

圖10 未優(yōu)化的主從機(jī)械臂關(guān)節(jié)角跟隨實(shí)驗(yàn)曲線Fig.10 Master-slave angle tracking curve without optimization

圖11 經(jīng)過模糊PD算法優(yōu)化的主從機(jī)械臂關(guān)節(jié)角跟隨實(shí)驗(yàn)Fig.11 Master-slave angle tracking curve with the optimization of the fuzzy PD algorithm

圖12 經(jīng)過模糊PD算法優(yōu)化的主從機(jī)械臂關(guān)節(jié)角誤差曲線Fig.12 Master-slave angle tracking curve with the optimization of the fuzzy PD algorithm

對比發(fā)現(xiàn)優(yōu)化后的系統(tǒng)主、從機(jī)械臂末端點(diǎn)在50 s內(nèi)能夠完成在空間中末端點(diǎn)跟隨、關(guān)節(jié)角跟隨。主從機(jī)械臂末端點(diǎn)誤差范圍、各個(gè)關(guān)節(jié)角誤差范圍如表3、表4所示。

表3 主從機(jī)械臂末端點(diǎn)X、Y、Z軸誤差范圍Tab.3 Error range of X，Y and Z axes at the end points of the master-slave manipulator

表4 主從機(jī)械臂關(guān)節(jié)角誤差范圍Tab.4 Error range of the master-slave manipulator angle

由于多關(guān)節(jié)控制存在重力及摩擦力等因素，從上述分析中可以看出關(guān)節(jié)2和Y軸的誤差出現(xiàn)了很大的偏差。同時(shí)從圖中可以看出在1 s左右操作者剛接觸設(shè)備和4.5 s操作者停止操作時(shí)會對系統(tǒng)產(chǎn)生很大的干擾。在仿真中系統(tǒng)是一個(gè)人在回路的系統(tǒng)，操作者是一個(gè)非線性因素，因?yàn)椴煌僮髡咚邮艿挠?xùn)練程度不同，操作的熟練程度不同，人體肌肉的反映情況不同，直接影響系統(tǒng)的跟隨性、透明性。圖7、圖8可以看出跟隨曲線出現(xiàn)了抖動現(xiàn)象，主從機(jī)械臂末端點(diǎn)和關(guān)節(jié)角出現(xiàn)了大幅偏差，這種現(xiàn)象產(chǎn)生的原因主要是從端接觸的環(huán)境和操作者手部顫抖。從表3、表4中可以計(jì)算出優(yōu)化后主從機(jī)械臂末端點(diǎn)和關(guān)節(jié)角的平均誤差分別為-4.347 mm～1.603 mm，0.047 rad～0.021 rad。對比文獻(xiàn)[2]中單自由度、正弦輸入信號從端的角度誤差，可以看出3自由度機(jī)械臂的角度跟隨誤差增大了0.02 rad。因?yàn)楸疚挠懻摿瞬僮髡邔Χ嘧杂啥葯C(jī)械臂的方法，沒有進(jìn)行標(biāo)準(zhǔn)信號的輸入，操作者操作過程中顫抖頻率在0～3 Hz，同時(shí)多機(jī)械臂的重力及摩擦力會對系統(tǒng)控制精度產(chǎn)生影響。本文討論的方法對操作者非線性因素有了一定的削弱，同時(shí)改變了以往的線性信號輸入、單自由度雙邊系統(tǒng)研究方法，針對實(shí)際應(yīng)用設(shè)計(jì)實(shí)驗(yàn)，系統(tǒng)的誤差隨著參數(shù)的優(yōu)化有了進(jìn)一步降低，系統(tǒng)在非線性干擾的情況下能夠進(jìn)行實(shí)時(shí)準(zhǔn)確的跟隨，操作者能夠?qū)崟r(shí)感受到力覺反饋信息。

5 結(jié)語

本文對由操作者、三自由度機(jī)械臂組成的雙邊控制系統(tǒng)進(jìn)行了控制系統(tǒng)設(shè)計(jì)和優(yōu)化。針對傳統(tǒng)的單自由度、線性信號的輸入的系統(tǒng)，完成了多自由度、非線性實(shí)驗(yàn)系統(tǒng)的搭建。采用模糊PD控制的方式對傳統(tǒng)的單自由度、線性信號輸入的遙操作雙邊PD控制系統(tǒng)進(jìn)行優(yōu)化。設(shè)計(jì)好模糊PD控制器，將Pantom Omni設(shè)備與Matlab連接進(jìn)行優(yōu)化前后的對比試驗(yàn)，通過對對比試驗(yàn)數(shù)據(jù)的分析得出雙邊控制系統(tǒng)性能有了進(jìn)一步的提升。經(jīng)過對控制系統(tǒng)控制參數(shù)Ks、Kd進(jìn)行模糊優(yōu)化后，三自由度機(jī)械臂雙邊控制系統(tǒng)保持了良好的穩(wěn)定性、跟隨性、透明性。下一步希望在多自由度機(jī)械臂主從控制中對重力、摩擦力、初始時(shí)刻和結(jié)束時(shí)刻等擾動因素產(chǎn)生的影響進(jìn)行分析，解決仿真圖中出現(xiàn)的擾動，優(yōu)化系統(tǒng)指標(biāo)。

[1]宋愛國.力覺臨場感遙操作機(jī)器人（1）：技術(shù)發(fā)展與現(xiàn)狀[J].南京信息工程大學(xué)學(xué)報(bào)：自然科學(xué)版，2013，5（1）：1-19.

[2]李永明，孫富春，劉華平，等.基于分布估計(jì)算法的遙操作模糊雙邊控制[J].系統(tǒng)仿真學(xué)報(bào)，2012，24（4）：810-815.

[3]鄧啟文.空間機(jī)器人遙操作雙邊控制技術(shù)研究[D].湖南：國防科學(xué)技術(shù)大學(xué)，2006.

[4]彭晉.基于虛擬力反饋的康復(fù)訓(xùn)練控制技術(shù)研究[D].哈爾濱：哈爾濱工程大學(xué)，2013.

[5]Z Zhang.A combined approach for master-slave robotic surgery system control based on jacobian and PD algorithm[J].IEEE International Conference on Information Science&Technology，2014（4）：718-721.

[6]W PoNgaen.Neuro-fuzzy in six DOF tele-robotic control[J].IEEE International Conference on Mechatronics& Automation，2008（8）：959-964.

[7]AJ Silva.PHANToM OMNI haptic device：kinematic and manipulability[J].Electronics，Robotics&Automotive Mechanics Conference，2009（9）：193-198.

[8]Xian Yang，Qin Huangdao，Chang Chunhua，et al.A new masterslave torque design for teleoperation system by T-S fuzzy approach[J].IEEE Transactions on Control Systems Technology，2015，23（4）：1611-1619.

[9]Tian Qiu.Design and control of a low cost 6 DOF master controller[J].IEEE International Conference on Robotics&Automation，2014：13-18.

[10]李曉丹.模糊 PID控制器的設(shè)計(jì)研究[D].天津：天津大學(xué)，2005.

[11]A Mohammadi，M Tacakoli，A Jazayeri，PHANTSIM：a simulink toolkit for the sensable PHANToM haptic devices[C]//Proceedings of the 23rd CANCAM，Canada，2011：787-790.