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

（西南科技大學 特殊環境機器人技術四川省重點實驗室，綿陽 621010）

遙操作雙邊控制是典型的人在回路的人機交互控制系統[1]。由于結合了人類對復雜任務的決策能力和機器人在危險環境中的作業能力，遙操作系統可以完成復雜的任務，已逐漸成為機器人領域的研究熱點[2]。

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

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

1 系統模型建立

1.1 三自由度機械臂數學模型

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

關節角度與末端點空間姿態函數關系：

由圖2可以求解θ1：

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

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

1.2 雙邊控制系統模型

雙邊控制系統動力學模型為[3，8，9]

式中：fh為操作者施加在主端機械臂上的力；τm為主端電機產生的驅動力；mm為主從端的轉動慣量；bm、bs為主從端的阻尼系數；τs為從端電機產生的驅動力；fe為環境對從端機械臂施加的力。從端自由環境時，環境對從端的作用力 fe≈0，Pantom Omni電機輸入輸出關系為

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

2 模糊PD控制方法分析

2.1 模糊PD控制器

在Matlab中搭建雙邊控制系統的仿真平臺，設計模糊PD控制器[5，10]，控制器結構如圖4所示。系統輸入量為機械臂每個關節的角度值θ1、θ2、θ3。控制系統根據系統輸入的目標量對主從機械臂各個關節的電機的電壓Ui（i=1、2、3）進行快速調節。閉環反饋為通過編碼器實時采集的各個關節的角度。

圖4 模糊PD控制結構Fig.4 Fuzzy PD control structure

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

表1 ΔKs的模糊規則表Tab.1 Fuzzy rule table of ΔKs

表2 ΔKd的模糊規則表Tab.2 Fuzzy rule table of ΔKd

用最大隸屬度函數法解模糊輸出參數 ΔKs、ΔKd。PD初始參數ΔKs0、ΔKd0由Ziegler-Nichols設定公式得到，在計算值的基礎上根據響應曲線得出比較理想的初始參數值。由式（21）和式（22）計算出Ks、Kd控制參數，調節從端機械臂各個關節角度。

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

3 建立實驗仿真平臺

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

圖5 Pantom Omni雙邊控制實驗系統Fig.5 Pantom Omni bilateral control experiment system

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

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

4 仿真與實驗分析

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

圖7 未經過優化的主從機械臂末端點跟隨實驗曲線Fig.7 Master-slave following curve without optimization

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

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

圖10 未優化的主從機械臂關節角跟隨實驗曲線Fig.10 Master-slave angle tracking curve without optimization

圖11 經過模糊PD算法優化的主從機械臂關節角跟隨實驗Fig.11 Master-slave angle tracking curve with the optimization of the fuzzy PD algorithm

圖12 經過模糊PD算法優化的主從機械臂關節角誤差曲線Fig.12 Master-slave angle tracking curve with the optimization of the fuzzy PD algorithm

對比發現優化后的系統主、從機械臂末端點在50 s內能夠完成在空間中末端點跟隨、關節角跟隨。主從機械臂末端點誤差范圍、各個關節角誤差范圍如表3、表4所示。

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

表4 主從機械臂關節角誤差范圍Tab.4 Error range of the master-slave manipulator angle

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

5 結語

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

[1]宋愛國.力覺臨場感遙操作機器人（1）：技術發展與現狀[J].南京信息工程大學學報：自然科學版，2013，5（1）：1-19.

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

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

[4]彭晉.基于虛擬力反饋的康復訓練控制技術研究[D].哈爾濱：哈爾濱工程大學，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控制器的設計研究[D].天津：天津大學，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.