[发明专利]一种机械臂力觉临场感的主从遥操作控制系统及方法

权利要求书

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)中，第一控制器产生的驱动力为：

fm=mm[x··ms+k1(x·ms-x·m)+k2(xms-xm)]+bmx·m-kmf(fms-f0)-fms-12(1+kmf)(m^x··ms+b^x·ms+c^xms)+λ2mmfms,]]>

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

fs=ms[x··ms+k1(x·ms-ξpx·s)+k2(xms-ξpxs)]+bsξpx·s-ksf(ξffe-fms)+fms-12(1+ksf)(m^x··ms+b^x·ms+c^xms)-λ2mmfms,]]>

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