基于模糊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.