CN104827458A - 一种机械臂力觉临场感的主从遥操作控制系统及方法 - Google Patents

Abstract

本发明公开了一种机械臂力觉临场感的主从遥操作控制系统及方法，系统包括主机械臂控制系统、主控制系统和从机械臂控制系统，通过该系统的控制策略性能稳定可靠、信息处理平滑稳定，满足主从控制作业任务的要求。本发明的构思是在主从机器人上安装无线通信模块，实现控制指令与传感器信号的无线传输，通过主从隔离，实现主从的跟踪与主手很好的力觉感；本发明通过主从控制的方式完成复杂作业，并通过各种传感器采集的信息，提高其操作性能。

Description

一种机械臂力觉临场感的主从遥操作控制系统及方法

技术领域

本发明涉及一种机械臂力觉临场感的主从遥操作控制系统及方法。

背景技术

随着对海洋、宇宙资源的开发，核电站废料的清理，高压带电作业、显微外科手术等这些高危、高压、强辐射、精细工作需求的不断提高，主从遥操作的研究显得更加迫切。

对于难以接近或者对人有害的危险的非结构化环境，操作者通过控制位于安全场所的主手来远距离控制从手进入危险区进行作业，从手跟随主手运动的同时将与环境的作用力传递给操作者。只有给操作者提供真实的视觉和力觉信息，操作者才能完成复杂精细的作业任务。

现有的主从机器人系统存在以下缺点：

(1)采用双向位置反馈来实现位置跟踪和力觉再现，但由于主从手间无法消除的位置误差，使系统在自由空间运动也显得十分“迟滞”，直接影响系统的操作性能；

(2)利用将主从手两侧的位置和力信息采用不同融合方式，实现多传感器的信息融合，构成不同的控制策略，但是，当从机器人的力直接作用在主机器人上，当从机器人从自由运动到突然接触较大刚体的物体时，会对主机器人有较大的冲击。

发明内容

本发明为了解决上述问题，提出了一种机械臂力觉临场感的主从遥操作控制系统及方法，本系统在主从机器人上安装无线通信模块，实现控制指令与传感器信号的无线传输，实现主从隔离，该控制策略能够实现主从的跟踪与主机械臂很好的力觉感，通过主从控制的方式完成复杂作业，并通过各种传感器采集的信息，提高其操作性能。

为了实现上述目的，本发明采用如下技术方案：

一种机械臂力觉临场感的主从遥操作控制系统，包括主机械臂控制系统、主控制系统和从机械臂控制系统，其中：

所述主机械臂控制系统包括编码器、压力传感器、力矩电机、马达驱动器和第一无线模块，所述编码器连接第一无线模块，所述第一无线模块与马达驱动器连接，马达驱动器连接安装于主机械臂各关节处的力矩电机，第一无线模块与主机械臂上安设的压力传感器连接；

所述从机械臂控制系统包括电位器、压力传感器、伺服电机、马达驱动器和第四无线模块，所述电位器安装于从机械臂的各个关节处，且连接第四无线模块，所述第四无线模块与马达驱动器连接，马达驱动器连接伺服电机，第四无线模块与从机械臂上的压力传感器连接；

所述主控制系统，包括第二无线模块、第三无线模块、第一控制器、第二控制器、位置比较模块、PID控制器和压力比较模块，其中，第二无线模块与所述第一无线模块通信，第三无线模块连接所述第四无线模块，压力比较模块连接第二无线模块、第三无线模块和第一控制器，所述位置比较模块连接PID控制器、第二无线模块和第三无线模块，PID控制器连接第二控制器，第二控制器连接第三无线模块。

所述主机械臂控制系统安装有急停、校对的按钮。

所述主机械臂与从机械臂的结构一致，且两者尺寸呈比例关系。

所述编码器内置于主机械臂上，采集主机械臂的位置、速度信号，并对其进行滤波处理后，传输给第一无线模块。

所述电位器安装于从机械臂的各个关节上，采集从机械臂的位置、速度信号，并对其进行滤波处理后，传输给第四无线模块。

基于上述主从遥操作控制系统的控制方法，包括以下步骤：

(1)操作机械臂运动时，主机械臂上的压力传感器检测到作用力为f₀，同时主机械臂编码器检测主机械臂产生的位移x_m，通过第一无线模块传输给压力比较模块和位置比较模块，从机械臂的压力传感器检测从机械臂与环境的作用力f_e，从机械臂的移动位移x_s，并通过第四无线模块传输给压力比较模块和位置比较模块；

(2)压力比较模块得到力误差信号传输到第一控制器，位置比较模块得到位置误差信号后结合主、从机械臂尺寸比例，将处理结果传输给PID控制器，PID控制器对其进行调节后将调节信号传输给第一控制器和第二控制器，第一控制器和第二控制器根据调节信号产生相应的驱动力，分别通过第二无线模块和第三无线模块传输给第一无线模块和第四无线模块；

(3)主机械臂控制系统和从机械臂控制系统的马达驱动器接受各自的驱动信号，驱动力矩电机、伺服电机工作，使主、从机械臂执行相应运动。

所述步骤(2)中，压力比较模块得出力误差信号为f₀-f_e。

所述步骤(2)中，位置比较模块得出位置误差信号x_m-x_s。

所述步骤(2)中，第一控制器产生的驱动力为：

$\begin{array}{c}{f}_m=m_m[{\stackrel{\·\·}{x}}_{\mathrm{ms}}+k_1\left({\stackrel{\·}{x}}_{\mathrm{ms}}{\stackrel{\·}{x}}_m\right)+k_2(x_{\mathrm{ms}}-x_m)]+b_m{\stackrel{\·}{x}}_m-k_{\mathrm{mf}}(f_{\mathrm{ms}}-f_0)-f_{\mathrm{ms}}\\ -\frac{1}{2}(1+k_{\mathrm{mf}})(\hat{m}{\stackrel{\·\·}{x}}_{\mathrm{ms}}+\hat{b}{\stackrel{\·}{x}}_{\mathrm{ms}}+\hat{c}{x}_{\mathrm{ms}})+\frac{\mathrm{\λ}}{2}{m}_m{f}_{\mathrm{ms}},\end{array}$

第二控制器产生驱动力为：

$\begin{array}{c}{f}_s=m_s[{\stackrel{\·\·}{x}}_{\mathrm{ms}}+k_1({\stackrel{\·}{x}}_{\mathrm{ms}}-{\mathrm{\ξ}}_p{\stackrel{\·}{x}}_s)+k_2(x_{\mathrm{ms}}-{\mathrm{\ξ}}_p{x}_s)]+b_s{\mathrm{\ξ}}_p{\stackrel{\·}{x}}_s-k_{\mathrm{sf}}({\mathrm{\ξ}}_f{f}_e-f_{\mathrm{ms}})+f_{\mathrm{ms}}\\ -\frac{1}{2}(1+k_{\mathrm{sf}})(\hat{m}{\stackrel{\·\·}{x}}_{\mathrm{ms}}+\hat{b}{\stackrel{\·}{x}}_{\mathrm{ms}}+\hat{c}{x}_{\mathrm{ms}})-\frac{\mathrm{\λ}}{2}{m}_m{f}_{\mathrm{ms}},\end{array}$

其中x_m、x_s为主、从机械臂的位置向量，m_m、b_m为主机械臂的质量和阻尼矩阵，m_s、b_s为从机械臂的质量和阻尼矩阵，f₀为施加给主机械臂的力，f_e为外界环境给从机械臂的反作用，f_m、f_s分别表示主、从机械臂的驱动力； $x_{\mathrm{ms}}=\frac{1}{2}(x_m+x_s),f_{\mathrm{ms}}=\frac{1}{2}(f_0+f_e),$ 是主、从机械臂之间阻抗模型的质量、阻尼矩阵和刚度，ξ_p＝α，ξ_f＝α²，α为主、从机械臂的尺寸比例系数，k₁，k₂，k_mf，k_sf和λ均表示参数。

本发明的有益效果为：

(1)本发明采用力反馈的技术，提高对环境的感知能力，增强操作者的临场感；

(2)本发明采用主从式控制结构，避免了运动学逆解的奇异解，运动灵活；

(3)本发明采用无线通信，实现了主从分离，适用于危险作业环境；

(4)本发明的算法经过PID处理，使位置误差信号更加平滑，便于控制器处理；实现主从位置误差和力的偏差信号都经过第一控制器处理后传到主机械臂中，提高力反馈的精度和避免从机械臂碰到刚性物体时对主机械臂的冲击。

附图说明

图1是本发明的结构总框图；

图2是本发明的主从控制方法模型图。

具体实施方式：

下面结合附图与实施例对本发明作进一步说明。

如图1所示，一种机械臂力觉临场感的主从遥操作控制系统，包括主机械臂控制系统、主控制系统和从机械臂控制系统，其中：

所述主机械臂控制系统包括编码器、压力传感器、力矩电机、马达驱动器和第一无线模块，所述编码器连接第一无线模块，所述第一无线模块与马达驱动器连接，马达驱动器连接安装于主机械臂各关节处的力矩电机，第一无线模块与主机械臂上安设的压力传感器连接；

所述从机械臂控制系统包括电位器、压力传感器、伺服电机、马达驱动器和第四无线模块，所述电位器安装于从机械臂的各个关节处，且连接第四无线模块，所述第四无线模块与马达驱动器连接，马达驱动器连接伺服电机，第四无线模块与从机械臂上的压力传感器连接；

主控制系统，包括第二无线模块、第三无线模块、第一控制器、第二控制器、位置比较模块、PID控制器和压力比较模块，其中，第二无线模块与所述第一无线模块通信，第三无线模块连接所述第四无线模块，压力比较模块连接第二无线模块、第三无线模块和第一控制器，所述位置比较模块连接PID控制器、第二无线模块和第三无线模块，PID控制器连接第二控制器，第二控制器连接第三无线模块。

主机械臂与从机械臂的结构一致，且两者尺寸呈比例关系。

编码器内置于主机械臂上，采集主机械臂的位置、速度信号，并对其进行滤波处理后，传输给第一无线模块。

电位器安装于从机械臂的各个关节上，采集从机械臂的位置、速度信号，并对其进行滤波处理后，传输给第四无线模块。

主机械臂有急停、校对的按钮，在发生紧急情况下可以停止从机械臂的运动。

电位器采用5KΩ的360度旋转电位器，传感器精度1‰，输出信号-10V～+10V。

第一控制器与第二控制器采用TMS320F28335芯片，处理速度快。

力矩电机采用步进式力矩电机，转矩范围0.061-0.123N.m，堵转电流0.3A，转矩灵敏度0.028N.m；所述各电位器采用5KΩ的360度旋转电位器，传感器精度1‰，输出信号-10V～+10V。

无线模块的通信距离1000m，通信频率900MHz，抗干扰能力强，通信距离远。

图2所示一种机械臂力觉临场感的主从遥操作控制方法，具体步骤如下：

步骤1：当操作者操作主机械臂时，主机械臂的压力传感器检测到作用力f₀，同时主机械臂编码器检测主机械臂产生的位移x_m，从机械臂的压力传感器检测从机械臂与环境的作用力f_e，从机械臂的电位器检测移动位移x_s。

步骤2：f₀、f_e通过无线模块传到压力比较模块，得出力误差信号f₀-f_e传到第一控制器；x_m、x_s通过无线模块传到位置比较模块，得出位置误差信号x_m-x_s，处理后传到PID控制模块，进行PID处理后，将信号传给第一控制器和第二控制器。第一控制器产生的驱动力为：

$\begin{array}{c}{f}_m=m_m[{\stackrel{\·\·}{x}}_{\mathrm{ms}}+k_1\left({\stackrel{\·}{x}}_{\mathrm{ms}}{\stackrel{\·}{x}}_m\right)+k_2(x_{\mathrm{ms}}-x_m)]+b_m{\stackrel{\·}{x}}_m-k_{\mathrm{mf}}(f_{\mathrm{ms}}-f_0)-f_{\mathrm{ms}}\\ -\frac{1}{2}(1+k_{\mathrm{mf}})(\hat{m}{\stackrel{\·\·}{x}}_{\mathrm{ms}}+\hat{b}{\stackrel{\·}{x}}_{\mathrm{ms}}+\hat{c}{x}_{\mathrm{ms}})+\frac{\mathrm{\λ}}{2}{m}_m{f}_{\mathrm{ms}},\end{array}$

第二控制器产生驱动力为：

$\begin{array}{c}{f}_s=m_s[{\stackrel{\·\·}{x}}_{\mathrm{ms}}+k_1({\stackrel{\·}{x}}_{\mathrm{ms}}-{\mathrm{\ξ}}_p{\stackrel{\·}{x}}_s)+k_2(x_{\mathrm{ms}}-{\mathrm{\ξ}}_p{x}_s)]+b_s{\mathrm{\ξ}}_p{\stackrel{\·}{x}}_s-k_{\mathrm{sf}}({\mathrm{\ξ}}_f{f}_e-f_{\mathrm{ms}})+f_{\mathrm{ms}}\\ -\frac{1}{2}(1+k_{\mathrm{sf}})(\hat{m}{\stackrel{\·\·}{x}}_{\mathrm{ms}}+\hat{b}{\stackrel{\·}{x}}_{\mathrm{ms}}+\hat{c}{x}_{\mathrm{ms}})-\frac{\mathrm{\λ}}{2}{m}_m{f}_{\mathrm{ms}},\end{array}$

其中x_m、x_s为主从机械臂的位置向量，m_m、b_m为主机械臂的质量和阻尼矩阵，m_s、b_s为从机械臂的质量和阻尼矩阵，f₀为操作者施加给主机械臂的力，f_e为外界环境给从机械臂的反作用，f_m、f_s分别表示主从机械臂的驱动力。 $x_{\mathrm{ms}}=\frac{1}{2}(x_m+x_s),f_{\mathrm{ms}}=\frac{1}{2}(f_0+f_e),$ 是主从机械臂之间阻抗模型的质量、阻尼矩阵和刚度，ξ_p＝α，ξ_f＝α²，k₁，k₂，k_mf，k_sf和λ均表示参数。

步骤3：第一控制器根据位置误差信号和压力误差信号得到主机械臂的驱动力f_m，通过无线模块传输到主机械臂力矩电机中使操作者感受到反馈力。

步骤4：第二控制器根据位置误差信号得到从机械臂的驱动力f_s，通过无线模块传输到从机械臂驱动电机中促使从机械臂电机运动。促使从机械臂跟随主机械臂的运动。

所述的位置误差信号经过PID控制模块，使位置误差信号更加平滑，便于控制器处理。

所述的算法可以提高力反馈的精度和避免从机械臂碰到刚性物体时对主机械臂的冲击。

上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (9)

1.一种机械臂力觉临场感的主从遥操作控制系统，其特征是：包括主机械臂控制系统、主控制系统和从机械臂控制系统，其中：

所述主机械臂控制系统包括编码器、压力传感器、力矩电机、马达驱动器和第一无线模块，所述编码器连接第一无线模块，所述第一无线模块与马达驱动器连接，马达驱动器连接安装于主机械臂各关节处的力矩电机，第一无线模块与主机械臂上安设的压力传感器连接；

所述从机械臂控制系统包括电位器、压力传感器、伺服电机、马达驱动器和第四无线模块，所述电位器安装于从机械臂的各个关节处，且连接第四无线模块，所述第四无线模块与马达驱动器连接，马达驱动器连接伺服电机，第四无线模块与从机械臂上的压力传感器连接；

所述主控制系统，包括第二无线模块、第三无线模块、第一控制器、第二控制器、位置比较模块、PID控制器和压力比较模块，其中，第二无线模块与所述第一无线模块通信，第三无线模块连接所述第四无线模块，压力比较模块连接第二无线模块、第三无线模块和第一控制器，所述位置比较模块连接PID控制器、第二无线模块和第三无线模块，PID控制器连接第二控制器，第二控制器连接第三无线模块。

2.如权利要求1所述的一种机械臂力觉临场感的主从遥操作控制系统，其特征是：所述主机械臂控制系统安装有急停、校对的按钮。

3.如权利要求1所述的一种机械臂力觉临场感的主从遥操作控制系统，其特征是：所述主机械臂与从机械臂的结构一致，且两者尺寸呈比例关系。

4.如权利要求1所述的一种机械臂力觉临场感的主从遥操作控制系统，其特征是：所述编码器内置于主机械臂上，采集主机械臂的位置、速度信号，并对其进行滤波处理后，传输给第一无线模块。

5.如权利要求1所述的一种机械臂力觉临场感的主从遥操作控制系统，其特征是：所述电位器安装于从机械臂的各个关节上，采集从机械臂的位置、速度信号，并对其进行滤波处理后，传输给第四无线模块。

6.如权利要求1-5中任一项所述的主从遥操作控制系统的控制方法，其特征是：包括以下步骤：

(1)操作机械臂运动时，主机械臂上的压力传感器检测到作用力为f₀，同时主机械臂编码器检测主机械臂产生的位移x_m，通过第一无线模块传输给压力比较模块和位置比较模块，从机械臂的压力传感器检测从机械臂与环境的作用力f_e，从机械臂的移动位移x_s，并通过第四无线模块传输给压力比较模块和位置比较模块；

(2)压力比较模块得到力误差信号传输到第一控制器，位置比较模块得到位置误差信号后结合主、从机械臂尺寸比例，将处理结果传输给PID控制器，PID控制器对其进行调节后将调节信号传输给第一控制器和第二控制器，第一控制器和第二控制器根据调节信号产生相应的驱动力，分别通过第二无线模块和第三无线模块传输给第一无线模块和第四无线模块；

(3)主机械臂控制系统和从机械臂控制系统的马达驱动器接受各自的驱动信号，驱动力矩电机、伺服电机工作，使主、从机械臂执行相应运动。

7.如权利要求6所述的控制方法，其特征是：所述步骤(2)中，压力比较模块得出力误差信号为f₀-f_e。

8.如权利要求6所述的控制方法，其特征是：所述步骤(2)中，位置比较模块得出位置误差信号x_m-x_s。

9.如权利要求7或8所述的控制方法，其特征是：所述步骤(2)中，第一控制器产生的驱动力为：

$\begin{array}{c}{f}_m=m_m[{\stackrel{\·\·}{x}}_{\mathrm{ms}}+k_1({\stackrel{\·}{x}}_{\mathrm{ms}}-{\stackrel{\·}{x}}_m)+k_2(x_{\mathrm{ms}}-x_m)]+b_m{\stackrel{\·}{x}}_m-k_{\mathrm{mf}}(f_{\mathrm{ms}}-f_0)-f_{\mathrm{ms}}\\ -\frac{1}{2}(1+k_{\mathrm{mf}})(\hat{m}{\stackrel{\·\·}{x}}_{\mathrm{ms}}+\hat{b}{\stackrel{\·}{x}}_{\mathrm{ms}}+\hat{c}{x}_{\mathrm{ms}})+\frac{\mathrm{\λ}}{2}{m}_m{f}_{\mathrm{ms}}\end{array},$

第二控制器产生驱动力为：

$\begin{array}{c}{f}_s=m_s[{\stackrel{\·\·}{x}}_{\mathrm{ms}}+k_1({\stackrel{\·}{x}}_{\mathrm{ms}}-{\mathrm{\ξ}}_p{\stackrel{\·}{x}}_s)+k_2(x_{\mathrm{ms}}-{\mathrm{\ξ}}_p{x}_s)]+b_s{\mathrm{\ξ}}_p{\stackrel{\·}{x}}_s-k_{\mathrm{sf}}({{\mathrm{\ξ}}_{f}f}_e-f_{\mathrm{ms}})+f_{\mathrm{ms}}\\ -\frac{1}{2}(1+k_{\mathrm{sf}})(\hat{m}{\stackrel{\·\·}{x}}_{\mathrm{ms}}+\hat{b}{\stackrel{\·}{x}}_{\mathrm{ms}}+\hat{c}{x}_{\mathrm{ms}})-\frac{\mathrm{\λ}}{2}{m}_m{f}_{\mathrm{ms}}\end{array},$

其中x_m、x_s为主、从机械臂的位置向量，m_m、b_m为主机械臂的质量和阻尼矩阵，m_s、b_s为从机械臂的质量和阻尼矩阵，f₀为施加给主机械臂的力，f_e为外界环境给从机械臂的反作用，f_m、f_s分别表示主、从机械臂的驱动力； $x_{\mathrm{ms}}=\frac{1}{2}(x_m+x_s),f_{\mathrm{ms}}=\frac{1}{2}(f_0+f_e),$ 是主、从机械臂之间阻抗模型的质量、阻尼矩阵和刚度，ξ_p＝α，ξ_f＝α²，α为主、从机械臂的尺寸比例系数，k₁，k₂，k_mf，k_sf和λ均表示参数。