CN102922522A - 多自由度电液伺服遥操纵机械手力反馈控制方法 - Google Patents

Abstract

本发明涉及一种多自由度电液伺服遥操纵机械手力反馈控制方法，可广泛用于空间探索、海洋开发、原子能应用、军事战场、抢险救助等代替人于危险或有害环境中从事作业任务。该方法以主－从端关节空间角度偏差形成反馈力，以雅可比矩阵构成的多维位置约束空间和冗余自由度空间矩阵形成反馈力增益，在主动侧形成与从动侧相同的位置空间，为遥操作者了解作业反力和因干涉产生的力觉信息。该方法通过在主动侧建立与从动侧完全相同的位置空间约束，使操作者通过反馈力的大小、方向和发生干涉自由度信息，把握从动侧干涉情况，真实地获得力觉临场感。

Description

多自由度电液伺服遥操纵机械手力反馈控制方法

技术领域

本发明涉及一种多自由度电液伺服遥操纵机械手力反馈控制方法，广泛应用于航空航天、海洋开发、核能、抢险救助、医疗、虚拟现实、游戏业等领域。

背景技术

在对遥控作业的机械手进行遥操纵时，操作者不但需要为其提供视觉信息，而且需要了解作业反力和因障碍干涉产生的力觉信息。控制策略设计是遥操纵力反馈系统设计的关键环节，相同控制结构但控制策略不同的遥操纵系统的力反馈效果和操作性能会有较大差异。针对遥操纵机械手的力觉双向伺服控制系统，目前国际上已经尝试了多种主－从伺服控制策略，主要概括为“位置－力”及“位置－位置”等控制结构。这些力反馈控制方法具有统一的特征：

（1）采用主－从动侧的力和位移信息融合，构成了多种不同的力反馈双向控制策略。

（2）只考虑了反馈力的大小而忽略了方向，从而对多自由度主－从遥操纵系统操控性差。

（3）仅针对主－从动侧系统自由度数目和动力学特性相同的遥操纵控制系统。

通过试验发现这些方法存在如下问题：

（1）主－从多自由度系统力反馈效果差；当力与位移强耦合时，操作者不能较好地把握从动侧干涉和位移情况。

（2）对位置－力反馈型控制策略，从动侧与障碍物发生干涉的自由度为非末端安装检测力传感器的自由度，则操作者无法感知到该干涉力而造成操纵失败。

（3）力反馈控制策略通用性不强，无法满足广泛应用日益多样化的主－从遥操纵设备的需要。

“位置－位置”控制策略的主动侧反馈力由主－从位置差、检测力信号形成，即使干涉力发生在非力传感器安装自由度，由于有位置偏差存在，操作者仍然可以通过操纵手柄感知到反馈力。在保证系统稳定性的前提下，使用“位置－位置”控制结构实现了较为理想的力觉反馈。但该控制策略只考虑了反馈力的大小而忽略了方向，操作者无法真实获得从动侧干涉力方向信息。当操作者在操纵多自由度主－从系统作业时，由于从动侧力和位移的强耦合，从而无法准确感知从动侧干涉力和位移情况，造成操作失败。

发明内容

本发明的目的在于提供一种多自由度电液伺服遥操纵机械手力反馈控制方法，解决了现有技术存在的上述问题，使操作者更好地获得从动侧机械手与障碍物发生干涉力的大小、方向和发生干涉自由度信息。本发明采用“位置约束空间”双向伺服控制方案，打破了目前一直沿用的“位置－力”和“位置－位置”的双向伺服力反馈控制方法的惯例。主要思想为：主动侧操纵手柄不是从动侧力和位置相互作用的跟随，而是为操作者提供与从动侧完全相同约束的设备。当从动侧任意自由度与障碍物发生干涉时，在主动侧操纵手柄上能够形成与从动侧相同的位置空间，使操作者感知从动侧干涉力大小、方向和发生干涉的自由度信息。本发明的优点是可以使操作者更好地把握从动侧机械手与环境干涉情况与位置状况，可以获得真实的力感觉。本发明可广泛用于空间探索、海洋开发、原子能应用、军事战场、抢险救助等代替人于危险或有害环境中的重要作业任务，也可用于远程医疗、虚拟现实、游戏娱乐等领域中。

本发明的上述目的通过以下技术方案实现：

多自由度电液伺服遥操纵机械手力反馈控制方法，在主动侧设置与从动侧完全相同约束的设备，当从动侧任意自由度与障碍物发生干涉时，在主动侧操纵手柄上能够形成与从动侧相同的位置空间，使操作者感知从动侧干涉力大小、方向和发生干涉的自由度信息；设从动侧为6自由度机械手，公式（1）、（2）和（3）所示，操作空间向关节空间的力映射转换成广义约束在操纵手柄各自由度实现出来，使操作者完全把握从动侧各自由度干涉力大小、方向和障碍物作用的自由度，使操作者仿佛身临现场操作一样；

(F_R)_6×1=(R_m)_6×6·(q_d-q_s)_6×1+(R_G)_6×6·(X_d)_6×1(1)

R_m=[Φ(J₂,J₃,…J₆)^⊥,Φ(J₁,J₃,…J₆)^⊥,…..Φ(J₁,J₂,…J₅)^⊥](2)

R_G=[Φ(I-JJ⁺)](3)

式中: F_R为反馈力，q_d与q_s分别为主动侧操纵手柄受到操纵力后期望达到的关节角度空间和从动侧机械手实际达到的关节角度空间；R_m 为位置约束空间矩阵，R _G 为冗余自由度空间矩阵，J为从动侧机械手雅可比矩阵，J⁺为其广义逆矩阵，I为单位矩阵；公式（2）中“⊥”为正交补空间；

其具体实现步骤如下：

(1)当操作者在主动侧施加操纵力时，由各自由度液压缸上端的力传感器检测出操作者在该自由度上的作用力，并经放大电路放大，与从动侧机械手在对应自由度上的干涉力检测值经力调整器单元的比较运算后，经从动侧控制器单元转换输出驱动从动侧机械手运动产生位移；

(2)同时操纵力经主动侧控制器计算出操纵手柄在该操纵力作用下，各自由度期望位移，再经逆运动学解算出主动侧机械手理论上达到的关节空间角度；

(3)由步骤（1）产生的驱动力，经从动侧控制器得到由该驱动力使从动侧各自由度产生的位移，计算得到从动侧机械手实际达到的关节空间角度；与主动侧期望达到的关节空间角度比较运算，若两者之间有差值，即从动侧任一自由度与障碍物干涉，则将该差值与位置约束空间矩阵相乘，形成多维力反馈控制信号；

(4)多维控制信号控制伺服阀换向或改变流量，驱动操纵手柄各自由度作反向平移或转动，使操作者感受到从动侧干涉力；由于控制量为多维控制信号，操作者可同时感受到干涉力大小、方向和发生干涉的自由度。

本发明的有益效果在于：（1）提出以雅可比矩阵组成的约束空间矩阵代替数值增益，与关节空间角度偏差的乘积来形成反馈力，其包含了干涉力的大小、方向和发生干涉自由度信息，在主动侧形成与从动侧相同的位置空间，使操作者能够把握从动侧干涉情况，能更加真实地获得反馈力感觉。（2）通用性好，对自由度数目及运动学特性不同的主—从系统仍然适用。

附图说明

此处所说明的附图用来提供对本发明的进一步理解，构成本申请的一部分，本发明的示意性实例及其说明用于解释本发明，并不构成对本发明的不当限定。

图1为本发明的控制框图；

图2为本发明的控制系统示意图；

图3为本发明实施例中第一号液压缸位移曲线示意图；

图4为本发明实施例中第三号液压缸位移曲线示意图；

图5为本发明实施例中第一号液压缸反馈力曲线示意图；

图6为本发明实施例中第三号液压缸反馈力曲线示意图。

图中：1.主动侧操纵手柄；  2.上活动平台；  3.力传感器；4.下固定平台；   5.位移传感器；  6.伺服阀Ⅰ；  7.伺服阀Ⅱ；  8.A/D转换器Ⅰ；  9.A/D转换器Ⅱ；  10.A/D转换器Ⅲ； 11.A/D转换器Ⅳ； 12.D/A转换器Ⅰ；  13.D/A转换器Ⅱ；14.控制单元；  15.从动侧机械手；16.放大电路。

具体实施方式

下面结合附图进一步说明本发明的详细内容及其具体实施方式。

参见图1及图2所示，本发明的多自由度电液伺服遥操纵机械手力反馈控制方法，其主动侧操纵手柄1不是从动侧力和位置相互作用的跟随，而是为操作者提供与从动侧完全相同约束的设备。当从动侧任意自由度与障碍物发生干涉时，在主动侧操纵手柄上能够形成与从动侧相同的位置空间，使操作者感知从动侧干涉力大小、方向和发生干涉的自由度信息。因为从动侧机械手任意自由度都可能与环境障碍物发生干涉，操纵手柄的反馈力信息就应包括大小、方向和发生干涉自由度，这样有利于操作者及时把握从动机械手的当前位置及干涉情况。这一点对操作者看不到从动机械手工作现场时的未确定环境中的遥操纵尤其重要。

参见图1及图2所示，本发明所应用控制系统包括主动侧电液伺服控制六自由度斯图尔特（Stewart）并联机构操纵手柄、主动侧控制器、位置调整器、力调整器（位置约束空间和冗余自由度空间）、从动侧控制器、从动侧电液伺服控制与主动侧手柄同构并联机械手。当操纵者通过对主动侧操纵手柄1施力，可使手控器上活动平台2沿x轴、y轴和z轴方向做平移或绕x轴、y轴和z轴做回转运动；液压缸活塞杆端部装有拉压力传感器3，并通过万向节与上活动平台2相连；液压缸另一端通过万向节与下固定平台4相连，液压缸体与活塞杆间装有位移传感器5；主动侧操纵手柄1的位移经位移传感器5检测、A/D转换器Ⅲ10转换后的数据送入计算机的控制单元14，通过并联机构逆运动学计算出操纵手柄的位形；从动侧机械手的位移检测值经A/D转换器Ⅳ11转换后的数据也送入控制单元14，计算出从动侧机械手15的位形，主-从动侧位形的差值经位置约束空间计算出外部约束力。主动侧操纵手柄1的位移经位移传感器5检测、A/D转换器Ⅲ10转换后的数据送入计算机控制单元14，经位置调整器计算出主动侧操纵手柄期望达到的位移，该位移经冗余自由度空间计算冗余操纵力。外部约束力与冗余操纵力之和为主动侧操纵手柄1的反馈力。位置约束空间和冗余自由度空间均由从动侧机械手雅可比矩阵确定。主－从系统自由度数目及运动学特性不相同时，力反馈控制策略同样有效。操纵者在冗余自由度方向上驱动操纵杆，可感知冗余操纵力。

参见图1及图2所示，当操作者将操纵力加在主动侧操纵手柄1，使该主动侧操纵手柄1在X轴方向作平移运动时，由每个自由度液压缸上端的力传感器3检测出操作者在该自由度上的作用力，并经放大电路16放大，送入A/D转换器Ⅰ8中，进行A/D转换后的数据送入控制单元14中的力调整器单元，从动侧机械手15在对应自由度上的干涉力检测值通过A/D转换器Ⅱ9也送入控制单元14中的力调整器单元。在力调整器单元中，用主动侧操纵手柄1上的力减去从动侧机械手15干涉阻力，经过力调整单元做差值计算后，将数据送入控制单元14中的从动侧控制器单元，经D/A转换器Ⅱ13转换输出驱动从动侧机械手15运动。同时，主动侧操纵手柄1中的位移传感器5将沿X轴方向作平移测量值通过A/D转换器Ⅲ10后送入控制单元14中的位置调整器单元中。经状态观测器得到由该操纵力期望主动侧得到的位移，再经逆运动学解算出从动侧机械手理论上达到的关节空间角度。从动侧机械手15位移检测值经A/D转换器Ⅳ11转换后，得到从动侧机械手15实际达到的关节空间角度。将数据送入控制单元14中的位置调整器单元中，在位置调整器中，对从动侧机械手15实际到达的关节空间角度与主动侧操纵手柄1指令期望到达的关节空间角度进行差值计算后，将数据送入控制单元14中的主动侧控制器单元，若两者之间有差值，即从动侧任一自由度与障碍物干涉，则将该差值与位置约束空间（见公式2）矩阵相乘，形成多维控制信号，该信号经D/A转换器Ⅰ12转换后控制伺服阀Ⅰ、Ⅱ6、7换向或改变流量，驱动主动侧操纵手柄1在X轴方向作反向平移，使操作者感受到从动侧干涉力。由于其实多维控制信号，操作者同时能感受到干涉力大小、方向和发生干涉的自由度。

参见图1及图2所示，当操作者对主动侧操纵手柄1施力，使手控器沿Y、Z轴平移、回转或在X、Y和Z轴方向的复合运动时，其原理同上所述。

当主－从动侧系统自由度数目及运动学特性都不相同时，当操作者驱动主动侧操纵手柄1时，使主动侧操纵手柄1绕X轴方向做回转运动，经状态观测器得到由该操纵力期望主动侧得到的回转角度时，将数据送入控制单元14中的主动侧控制器单元，与冗余自由度空间（见公式3）矩阵相乘，形成冗余自由度约束力。由于主—从系统自由度数目不相同，当操作者在冗余自由度方向上驱动操纵杆时，从动侧机械手15保持不动，主动侧操纵手柄1可以操纵，同时操作者在冗余自由度方向上可以感知到反馈力。

参见图3至图6所示，为在从动侧手柄第三号液压缸前端放置一刚性硬物，阻挡从手在该自由度方向的运动，模拟接触硬物时的力反馈效果，试验曲线如图3至图6所示。

当对主动侧操纵手柄1施加操纵力，从动侧手柄先产生运动，主动侧操纵手柄1跟随从动侧手柄运动，主－从动侧手柄的位移方向一致，但从动侧手柄的运动相位超前于主动侧手柄。当第三号液压缸受到硬物阻挡时，操作者在主动侧操纵手柄1的第三号液压缸瞬间感觉反馈力，操作者在主动侧操纵手柄1的第一号液压缸始终感觉不到反馈力存在。

以上所述仅为本发明的优选实例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (1)

1.一种多自由度电液伺服遥操纵机械手力反馈控制方法，其特征在于：当从动侧任意自由度与障碍物发生干涉时，在主动侧操纵手柄上能够形成与从动侧相同的位置空间，使操作者感知从动侧干涉力大小、方向和发生干涉的自由度信息；设从动侧为6自由度机械手，公式（1）、（2）和（3）所示，操作空间向关节空间的力映射转换成广义约束在操纵手柄各自由度实现出来，使操作者完全把握从动侧各自由度干涉力大小、方向和障碍物作用的自由度，使操作者仿佛身临现场操作一样；

(F_R)_6×1=(R_m)_6×6·(q_d-q_s)_6×1+(R_G)_6×6·(X_d)_6×1(1)

R_m=[Φ(J₂,J₃,…J₆)^⊥,Φ(J₁,J₃,…J₆)^⊥,…..Φ(J₁,J₂,…J₅)^⊥](2)

R_G=[Φ(I-JJ⁺)](3)

式中：F_R为反馈力，q_d与q_s分别为主动侧操纵手柄受到操纵力后期望达到的关节角度空间和从动侧机械手实际达到的关节角度空间；R_m 为位置约束空间矩阵，R _G 为冗余自由度空间矩阵，J为从动侧机械手雅可比矩阵，J⁺为其广义逆矩阵，I为单位矩阵；公式（2）中“⊥”为正交补空间；

其具体实现步骤如下：

(1)当操作者在主动侧施加操纵力时，由各自由度液压缸上端的力传感器检测出操作者在该自由度上的作用力，并经放大电路放大，与从动侧机械手在对应自由度上的干涉力检测值经力调整器单元的比较运算后，经从动侧控制器单元转换输出驱动从动侧机械手运动产生位移；

(2)同时操纵力经主动侧控制器计算出操纵手柄在该操纵力作用下，各自由度期望位移，再经逆运动学解算出主动侧机械手理论上达到的关节空间角度；

(3)由步骤(1)产生的驱动力，经从动侧控制器得到由该驱动力使从动侧各自由度产生的位移，计算得到从动侧机械手实际达到的关节空间角度；与主动侧期望达到的关节空间角度比较运算，若两者之间有差值，即从动侧任一自由度与障碍物干涉，则将该差值与位置约束空间矩阵相乘，形成多维力反馈控制信号；

(4)多维控制信号控制伺服阀换向或改变流量，驱动操纵手柄各自由度作反向平移或转动，使操作者感受到从动侧干涉力；由于控制量为多维控制信号，操作者可同时感受到干涉力大小、方向和发生干涉的自由度。

Images