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

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

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

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

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

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

Pantom Omni是一個具有六自由度的力覺、觸覺設(shè)備，其中3個自由度具有力反饋、角度傳感器，3個自由度具有角度傳感器。如圖1所示，建立Pantom Omni基坐標(biāo)系P0、末端點坐標(biāo)系P，機(jī)械臂末端點姿態(tài)與關(guān)節(jié)角關(guān)系如式（1）所示。設(shè)置2臂為L1，設(shè)置3臂為L2，分別從俯視圖（圖2）和正視圖（圖3）建立末端點坐標(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中可以得到末端點x、y、z與各連桿關(guān)系 （末端點坐標(biāo)P（x，y，z）與基坐標(biāo)P0（X，Y，Z），圖3相當(dāng)于將末端點坐標(biāo)通過平移變換到基坐標(biāo)）如式（2）～式（4）所示：

關(guān)節(jié)角度與末端點空間姿態(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)境時，環(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è)計模糊PD控制器[5，10]，控制器結(jié)構(gòu)如圖4所示。系統(tǒng)輸入量為機(jī)械臂每個關(guān)節(jié)的角度值θ1、θ2、θ3。控制系統(tǒng)根據(jù)系統(tǒng)輸入的目標(biāo)量對主從機(jī)械臂各個關(guān)節(jié)的電機(jī)的電壓Ui（i=1、2、3）進(jìn)行快速調(diào)節(jié)。閉環(huán)反饋為通過編碼器實時采集的各個關(guān)節(jié)的角度。

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

Matlab中設(shè)計模糊PD子系統(tǒng)，模糊PD子系統(tǒng)的輸入為變量誤差e=Xin-Xout和誤差變化率 ec= e（k）-e（k-1）。模糊PD的模糊推理系統(tǒng)包括2個輸入變量e、ec，2個輸出變量ΔKs、ΔKd。本文選用的是三角型隸屬函數(shù)，在閉環(huán)系統(tǒng)的動態(tài)響應(yīng)曲線上采集需要的參考點，根據(jù)參考點上的誤差和誤差變化率的方向設(shè)計模糊規(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è)定公式得到，在計算值的基礎(chǔ)上根據(jù)響應(yīng)曲線得出比較理想的初始參數(shù)值。由式（21）和式（22）計算出Ks、Kd控制參數(shù)，調(diào)節(jié)從端機(jī)械臂各個關(guān)節(jié)角度。

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

3 建立實驗仿真平臺

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

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

在仿真過程中使用PHANSIM TOOLKIT Matlab函數(shù)模塊進(jì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中實時記錄主從機(jī)械臂運動時各個關(guān)節(jié)角的跟隨情況。通過建立的機(jī)械臂關(guān)節(jié)空間和末端點的數(shù)學(xué)模型，計算出機(jī)械臂末端點姿態(tài)，實時繪制主從機(jī)械臂末端點姿態(tài)曲線。對T時間段主從機(jī)械臂關(guān)節(jié)角和末端點姿態(tài)跟隨曲線進(jìn)行分析，測試本文設(shè)計的雙邊模糊PD控制系統(tǒng)的性能。

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

4 仿真與實驗分析

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

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

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

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

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

圖11 經(jīng)過模糊PD算法優(yōu)化的主從機(jī)械臂關(guān)節(jié)角跟隨實驗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ī)械臂末端點在50 s內(nèi)能夠完成在空間中末端點跟隨、關(guān)節(jié)角跟隨。主從機(jī)械臂末端點誤差范圍、各個關(guān)節(jié)角誤差范圍如表3、表4所示。

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

5 結(jié)語

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

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

[2]李永明，孫富春，劉華平，等.基于分布估計算法的遙操作模糊雙邊控制[J].系統(tǒng)仿真學(xué)報，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è)計研究[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.