CN105583824A - 力控牵引和摆位的多自由度机械臂控制装置及其控制方法 - Google Patents

Abstract

本发明公开了一种力控牵引和摆位的多自由度机械臂控制装置及其控制方法。本发明在机械臂末端的法兰盘上用安装连接件以特定方式安装力信号传感器，同步采样机械臂末端位姿信息和力/力矩信息；本发明基于外部作用力的反馈控制，将力反馈信息转化为机械臂末端的位姿偏移量信息；通过力-位姿偏移控制算法以及力-速控制算法，使机械臂末端的位姿和速度随外部作用力而变化，并能够在人的牵引停止时准确摆位；本发明既能够使末端工具按照操作人员的意愿方便实时地实现牵引操作，在无外力作用时能够保持当前位姿，实现精确定位，同时使操作人员在机械臂向目标靶点运动过程中通过施加外力改变机械臂末端的运动轨迹，实现机械臂运动中紧急避障。

Description

力控牵引和摆位的多自由度机械臂控制装置及其控制方法

技术领域

本发明涉及多自由度机械臂控制技术，具体涉及力控牵引和摆位的多自由度机械臂控制装置及其控制方法。

背景技术

多自由度机械臂以其定位重复精度高、可编程、易操作等优点，已在工业设备制造、航空航天、医疗卫生等领域得到广泛应用。工业多自由度机械臂系统常用的控制方法有两种：一是采用“示教模式”，即操作人员首先通过机械臂教导器控制机械臂末端(常安装有工具)沿一定的工作轨迹运动，并使机械臂控制器以预先确定的采样频率“记录”工作轨迹中每个采样轨迹点对应的机械臂运动状态(包括瞬时速度、瞬时加速度、各关节的瞬时角度信息等)，在实际控制中机械臂控制器以预先选定的插补方式控制机械臂末端依次通过上述采样轨迹点，复现示教时的工作运动轨迹，完成相应的工作任务；二是采用“离线编程模式”，即在机械臂控制器中按照特定工作任务要求预先写入控制程序，在实际工况中机械臂将按照程序中的步骤单步循环运行，实现预定工作任务。

在一些应用场景中，有时需要机械臂末端能够按照操作人员的意愿方便地、实时地调整空间位姿，例如医疗应用中的机械臂扶持、康复训练应用中的人机交互。在这些应用中一方面在操作人员进行机械臂控制时，通过对末端工具施加作用力实现对末端工具的牵引操作；另一方面在无外部控制输入时，机械臂能够克服自身及其工具重力等因素影响，保持当前位姿，实现精确定位。另外，在预先给定机械臂末端目标点位姿信息时，有时候为了机械臂紧急避障等目的，需要机械臂在运动过程中也能够通过外力控制实时更新运动轨迹。

现有的机械臂控制系统绝大多数不能实现外力作用下控制机械臂的运动，而只能通过教导模式与离线编程模式两种控制方法预先规划好机械臂运动轨迹。在上述应用场景中，应用两类控制模式实现机械臂控制所存在的不足主要体现在：

1.示教模式下，特定工作任务中要求操作人员控制机械臂对部分位姿点精确到达，同时也要考虑到各种可能引发安全隐患的因素，因此示教轨迹选取以及示教操作过程较为费时。离线编程模式下，前期需要将工作任务转化为机器程序，后期根据实际工况条件会有多次的程序修改与运行调试，系统维护成本也较高。

2.当机械臂末端工作轨迹改变时，往往需要操作人员对运动轨迹进行重新示教或者预编程，也使多自由度机械臂在个性化方案应用方面受到了极大限制。

3.机械臂在运行过程中，无法通过检测外部作用力做出相应控制指令的调整，因此无法实现力控牵引功能；在机械臂运动轨迹中意外遇到障碍物时，无法通过外力作用实时更新机械臂的运动轨迹，容易造成意外事故。

专利CN103347659A提出了一种传感器中继控制装置，能够通过对力传感器信息进行解算控制机械臂各关节电动机的运动，但是也不能实现对机械臂系统的力控牵引功能。

发明内容

基于以上现有技术中多自由度机械臂控制中存在的问题，本发明提出了一种力控牵引和摆位的多自由度机械臂控制装置及其控制方法。

本发明的一个目的在于提出一种力控牵引和摆位的多自由度机械臂控制装置。

本发明的力控牵引和摆位的多自由度机械臂控制装置包括：上位机、多自由度机械臂系统和六自由度力传感器系统；其中，多自由度机械臂系统包括多自由度机械臂和机械臂控制器；六自由度力传感器系统包括力信号传感器和力信号控制器；上位机包括力信号控制器通讯模块、末端工具重力补偿模块、力-位及力-速控制算法模块、位姿坐标解算模块、机械臂控制模块和图形用户界面模块；上位机分别与机械臂控制器和力信号控制器联接；力信号传感器具有相对且平行的安装平面和工具平面，安装平面通过安装连接件固定在机械臂末端，在工具平面上安装末端工具，保持末端工具与力信号传感器之间的相对固定；机械臂末端坐标系{End}的Z轴与力信号传感器坐标系{FT-Sensor}的Z轴共线且同向，机械臂末端坐标系{End}的Y轴与力信号传感器坐标系{FT-Sensor}的Y轴平行；外部作用力作用在末端工具上，力信号传感器检测到外力，外力包括外部作用力和末端工具的重力，将外力信号转化为模拟电信号传输至力信号控制器；力信号控制器将模拟信号转化为数字信号后传输至上位机；上位机的力信号控制器通讯模块接收数字信号得到外力的值，末端工具重力补偿模块解算出实际作用在力信号传感器上的外部作用力沿力信号传感器坐标系{FT-Sensor}三轴的分力与分力矩；力-位及力-速控制算法模块将外部作用力的信息转化为在力信号传感器坐标系{FT-Sensor}下的机械臂末端的位姿偏移量与速度值；位姿坐标解算模块将在力信号传感器坐标系{FT-Sensor}的机械臂末端的位姿偏移量与速度值转换成在机械臂基坐标系{Robot}下的机械臂末端的位姿偏移量与速度值，并将当前采样时刻解算得到的位姿偏移量与当前采样时刻的预期位姿坐标值进行矢量相加，得到机械臂末端的下一采样时刻在机械臂基坐标系{Robot}下的预期位姿坐标值；机械臂控制模块按照机械臂末端的下一采样时刻在机械臂基坐标系{Robot}下的预期位姿坐标值控制机械臂末端运动，带动末端工具到达预期指定的位姿，实现基于力控牵引和摆位控制。

多自由度机械臂系统包括多自由度机械臂和机械臂控制器；多自由度机械臂具有多个移动副或旋转副组成的串联结构。机械臂控制器包括状态检测模块和运动控制模块。

六自由度力传感器系统包括能够检测六自由度力/力矩分量信息的力信号传感器以及能够处理六自由度力/力矩信息的力信号控制器。力信号控制器包括模数转换模块和偏置校正模块。

机械臂末端具有圆环状的法兰盘，法兰盘上设置有法兰盘定位销槽。力信号传感器与机械臂末端相对的面为安装平面，安装平面具有圆环，圆环上设置有圆环定位销槽；力信号传感器与末端工具相对的面为工具平面，在工具平面上直接固定安装末端工具。安装连接件为可拆卸连接件，具有相对且平行的机械臂末端安装面和力信号传感器安装面；机械臂末端安装面与机械臂末端固定并唯一确定安装连接件的方向；力信号传感器安装面与力信号传感器固定并唯一确定力信号传感器的安装方向。机械臂末端安装面的中心具有机械臂末端安装面定中心凸台，边缘设置机械臂末端安装面定位销槽；定中心凸台嵌在法兰盘的圆环中，法兰盘定位销槽与机械臂末端安装面定位销槽通过定位销对齐并固定；力信号传感器安装面的中心具有力信号传感器安装面定中心凸台，边缘设置力信号传感器安装面定位销槽；力信号传感器安装面定中心凸台嵌在力信号传感器的圆环中，力信号传感器上的圆环定位销槽与力信号传感器安装面定位销槽通过定位销对齐并固定。定中心凸台的中心点与定位销的连线分别平行于机械臂末端坐标系的Y轴和力信号传感器坐标系{FT-Sensor}的Y轴；安装连接件通过定中心凸台和定位销，将机械臂末端与力信号传感器连接，机械臂末端坐标系{End}的Z轴与力信号传感器坐标系{FT-Sensor}的Z轴共线且同向，机械臂末端坐标系{End}的Y轴与力信号传感器坐标系{FT-Sensor}的Y轴平行，并唯一确定安装方向；机械臂末端与力信号传感器通过安装连接件以唯一方式固定，使两坐标系XYZ三轴两两对应平行，两坐标系原点连线与两坐标系Z轴三者共线且同向。

本发明的另一个目的在于提出一种力控牵引和摆位的多自由度机械臂的控制方法。

本发明的力控牵引的多自由度机械臂控制方法，包括以下步骤：

1)开机通电，上位机对力信号控制器通讯模块、末端工具重力补偿模块、力-位及力-速控制算法模块、位姿坐标解算模块、机械臂控制模块和图形用户界面模块进行初始化，机械臂控制器与力信号控制器分别进行初始化，上位机通过机械臂控制器使机械臂末端移动到初始位姿，在该位姿状态下对力信号控制器进行偏置校正；

2)外部作用力作用在末端工具上，力信号传感器检测到外力，外力包括外部作用力和末端工具的重力，将外力信号转化为模拟电信号传输至力信号控制器；

3)上位机向力信号控制器发送力信号请求指令，力信号控制器将模拟信号转化为数字信号后传输至上位机；

4)上位机的力信号控制器通讯模块接收数字信号得到外力的值；

5)上位机向机械臂控制器发送机械臂状态请求指令，读取机械臂当前位姿，并通过末端工具重力补偿模块，采用末端工具重力补偿算法，解算出实际作用在力信号传感器上的外部作用力沿力信号传感器坐标系{FT-Sensor}三轴的分力与分力矩；

6)力-位及力-速控制算法模块采用力-位姿偏移控制算法和力-速控制算法，将外部作用力的信息转化为在力信号传感器坐标系{FT-Sensor}下的机械臂末端的位姿偏移量与速度值；

7)位姿坐标解算模块采用位姿坐标点解算算法，将在力信号传感器坐标系{FT-Sensor}的机械臂末端的位姿偏移量转换成在机械臂基坐标系{Robot}下的机械臂末端的位姿偏移量，用位姿偏移量修正当前采样时刻的预期位姿坐标值，得到机械臂末端的下一采样时刻在机械臂基坐标系{Robot}下的预期位姿坐标值；

8)机械臂控制模块按照机械臂末端的下一采样时刻在机械臂基坐标系{Robot}下的预期位姿坐标值控制机械臂末端运动，带动末端工具到达预期指定的位姿，实现力控牵引控制。

其中，在步骤5)中，末端工具重力补偿算法具体包括：在每一采样时刻下，上位机通过机械臂控制器得到机械臂末端的瞬时姿态，再由储存在上位机中的末端工具的重力以及重力臂矢量值，解算重力在力信号传感器坐标系{FT-Sensor}下三坐标轴向分力以及分力矩的偏差值；在同一采样时刻下，上位机通过力信号控制器读取外力在力信号传感器坐标系{FT-Sensor}下三坐标轴向瞬时分力以及分力矩值，与上述重力的偏差值矢量相减，得到在该采样时刻下实际作用在力信号传感器上的外部作用力沿力信号传感器坐标系{FT-Sensor}三轴的分力与分力矩。

在步骤6)中，位姿包括平移和旋转，相应地，力-位姿偏移控制算法包括力-平移偏移量算法和力-旋转偏移量算法。力-位姿偏移控制算法采用分级控制策略，即设定多个力阈值点，将外部作用力分为多个受力阈值区间，位姿偏移量分为多个随力递增的对应值，力与位姿偏移量的曲线为阶梯形，在力阈值段内对应值不变；力-速控制算法，采用S型函数控制，即在一定的受力阈值区间内，速度值是所受外部合力绝对值的Sigmoid函数。

在步骤7)中，位姿坐标点解算算法包括：力信号传感器解算出的位姿偏移量是基于力信号传感器坐标系{FT-Sensor}的坐标偏移值，当前采样时刻的机械臂末端的预期位姿坐标值是基于机械臂基坐标系{Robot}的位姿坐标值；上位机将上述基于力信号传感器坐标系{FT-Sensor}的位姿偏移量转换成基于机械臂基坐标系{Robot}的位姿偏移量之后，与当前采样时刻的预期位姿坐标值进行矢量相加，得到机械臂末端的下一采样时刻在机械臂基坐标系{Robot}的预期位姿坐标值。

本发明的机械臂控制末端工具实现机械臂力控牵引中，上位机不指定机械臂末端到达某个目标点，机械臂系统受外部作用力控制。首先上位机获取力信号传感器的力/力矩数值，对数据进行末端工具重力补偿的预处理，保证末端工具在有外部作用力时上位机是对消除末端工具的重力因素之后的实际作用的外部作用力的力/力矩值进行解算；然后，上位机通过力-位及力-速控制算法与位姿坐标点解算算法，得到下一时刻的机械臂末端预期位姿坐标值和速度值并控制机械臂末端运动，实现力控牵引；若上位机获取的力信号传感器读数为零，即末端工具在无外部作用力时，末端工具不会因为自身重力影响导致误运动，能够保持当前采样时刻的空间位姿状态。

本发明的力控摆位的多自由度机械臂控制方法，包括以下步骤：

1)上位机首先计划机械臂末端到达预期的目标点，确定目标点的位姿坐标值，在无外部作用力作用时，机械臂末端按照预先设定的插补方式从起始点运动到目标点；

2)开机通电，上位机对力信号控制器通讯模块、末端工具重力补偿模块、力-位及力-速控制算法模块、位姿坐标解算模块、机械臂控制模块和图形用户界面模块进行初始化，机械臂控制器与力信号控制器分别进行初始化，上位机通过机械臂控制器使机械臂末端移动到初始位姿，在该位姿状态下对力信号控制器进行偏置校正；

3)在采用插补方式设定的从起始点至目标点的运动轨迹上出现障碍物时，外部作用力作用在末端工具上，以改变机械臂末端的运动轨迹；

4)上位机在当前位姿状态下，通过先前设定的插补方式计算出机械臂末端下一时刻的预期运动插补点的位姿坐标值；

5)力信号传感器在当前时刻检测到外力，外力包括外部作用力和末端工具重力，将外力信号转化为模拟电信号传输至力信号控制器；

6)上位机向力信号控制器发送力信号请求指令，力信号控制器将模拟信号转化为数字信号后传输至上位机；

7)上位机的力信号控制器通讯模块接收数字信号得到外力的值；

8)上位机向机械臂控制器发送机械臂状态请求指令，读取机械臂末端当前位姿，并通过末端工具重力补偿模块，采用末端工具重力补偿算法，解算出实际作用在力信号传感器上的外部作用力沿力信号传感器坐标系{FT-Sensor}三轴的分力与分力矩；

9)力-位及力-速控制算法模块采用力-位姿偏移控制算法和力-速控制算法，将外部作用力的信息转化为在力信号传感器坐标系{FT-Sensor}下的机械臂末端的位姿偏移量与速度值；

10)位姿坐标解算模块将在力信号传感器坐标系{FT-Sensor}的机械臂的位姿偏移量与速度值转换成在机械臂基坐标系{Robot}下的机械臂末端的位姿偏移量与速度值，并将下一时刻的预期运动插补点的位姿坐标值与位姿偏移量进行矢量相加，得到机械臂末端下一时刻的实际运动插补点的位姿坐标值；

11)机械臂控制模块按照下一时刻的实际运动插补点的位姿坐标值控制机械臂运动，通过外部作用力实现对机械臂末端的运动轨迹的修正；

12)当外部作用力消失后，机械臂末端能够以当前位姿点按照新的插补方式运动到目标点，实现对目标点精确定位的力控摆位控制。

本发明的优点：

本发明的多自由度机械臂控制装置，包括上位机、多自由度机械臂系统以及六自由度力传感器系统，在机械臂末端的法兰盘上用安装连接件以特定方式安装力信号传感器，使传感器坐标系与机械臂末端坐标系{End}平行且Z轴共线同向，这样能够方便上位机对力信息的处理解算，使力信号传感器检测到的外部合力方向与最终解算的机械臂末端的运动方向一致。

上位机分别与机械臂控制器和力信号控制器相联接，在同一采样时刻分别读取机械臂末端的位姿信息与力信号传感器检测到的力/力矩信息，保证信息的同步采样。

本发明的控制方法，能够在不改变机械臂现有组成结构与控制结构的基础上实现基于外部作用力的反馈控制，将力反馈信息转化为机械臂末端的位姿偏移量信息，方便系统集成与控制实施，同时力反馈信息的获取与位置信息解算均在笛卡尔坐标系下，方便数据处理。通过设定基于分级控制策略的力-位姿偏移控制算法以及基于S型控制策略的力-速控制算法，使机械臂末端的位姿和运动速度随外部作用力而变化，得到更好的人际交互效果，并能够在人的牵引停止时准确摆位。

本发明的机械臂控制装置实现机械臂的力控牵引，既能够使末端工具按照操作人员的意愿方便实时地实现牵引操作，同时在无外力作用时能够保持当前位姿，实现精确定位；本发明的机械臂控制装置实现机械臂的力控摆位，能够使操作人员在机械臂向目标靶点运动过程中通过施加外力改变机械臂末端的运动轨迹，实现机械臂运动中紧急避障。

附图说明

图1为本发明的力控牵引和摆位的多自由度机械臂控制装置的原理框图；

图2为本发明的力控牵引和摆位的多自由度机械臂控制装置的安装连接件的示意图；

图3为本发明的力控牵引和摆位的多自由度机械臂控制装置的末端工具对力信号传感器检测的影响的示意图，其中，(a)是机械臂末端法向与重力方向垂直的机械臂末端位姿，(b)是机械臂末端法向与重力方向呈一定角度的机械臂末端位姿；

图4为本发明的力控牵引和摆位的多自由度机械臂控制装置的力信号传感器偏执校正后对末端工具重力补偿算法的影响的示意图；

图5为根据本发明的力控牵引和摆位的多自由度机械臂控制方法的一个实施例的力-位姿偏移量算法和力-速控制算法对应的函数模型，其中，(a)为力-平移偏移量算法的函数模型，(b)为力-旋转偏移量算法的函数模型，(c)为力-速控制算法的函数模型；

图6为本发明的力控牵引和摆位的多自由度机械臂控制方法的流程图，其中，(a)为六自由度力传感器系统的控制流程图，(b)为上位机的控制流程图，(c)为多自由度机械臂系统的控制流程图；

图7为本发明的力控摆位的多自由度机械臂控制方法的控制示意图。

具体实施方式

下面结合附图，通过具体实施例，进一步阐述本发明。

如图1所示，本实施例的力控牵引和摆位的多自由度机械臂控制装置包括：上位机、多自由度机械臂系统和六自由度力传感器系统；其中，多自由度机械臂系统包括多自由度机械臂和机械臂控制器；六自由度力传感器系统包括力信号传感器以及力信号控制器；上位机包括力信号控制器通讯模块、末端工具重力补偿模块、力-位及力-速控制算法模块、位姿坐标解算模块、机械臂控制模块和图形用户截面模块；上位机分别与机械臂控制器和力信号控制器联接。

在本实施例中，多自由度机械臂系统为六自由度关节机械臂系统，其执行机构为具有六转动副的关节机械臂，能够在工作空间实现六自由度位姿定位，机械臂腕部三个转动副的轴线相互垂直并且相交于一点，组成球副结构；控制机构为具有可教导、可编程控制的机械臂控制器，其状态监测模块能够实时监测关节机械臂运动状态并响应上位机的状态读取指令，运动控制模块能够从上位机得到运动指令，并控制机械臂末端进行运动到基于机械臂基坐标系{Robot}下的位姿坐标点。

如图2所示，安装连接件3的结构呈类圆柱体状，相对且平行的机械臂末端安装面32和力信号传感器安装面31；机械臂末端安装面32的中心具有机械臂末端安装面定中心凸台，边缘设置机械臂末端安装面定位销槽；力信号传感器安装面的中心具有力信号传感器安装面定中心凸台33，边缘设置力信号传感器安装面定位销槽34。定中心凸台使机械臂末端坐标系{End}原点、安装连接件的几何中心与力信号传感器坐标系{FT-Sensor}原点三点共线，并使机械臂末端坐标系{End}Z轴与力信号传感器坐标系{FT-Sensor}Z轴共线且同向；定位销使机械臂末端坐标系{End}Y轴与力信号传感器坐标系{FT-Sensor}Y轴共线且同向。通过定中心凸台与定位销，安装连接件以唯一安装方式固定在机械臂末端，并通过螺钉固定；力信号传感器同样以唯一安装方式固定在安装连接件上，并通过螺钉固定。

上位机作为多自由度机械臂控制装置的主控系统，分别联接机械臂控制器与力信号控制器。上位机包括力信号控制器通讯模块、算法模块、机械臂控制模块和图形用户界面模块。力信号控制器通讯模块向力信号控制器发送数据请求指令，并读取力/力矩数据值；机械臂控制模块能够读取机械臂末端反馈的运动状态，并将计算得到的下一采样时刻位姿坐标值写入机械臂控制器；算法模块包括末端工具重力补偿模块、力-位及力-速控制算法模块以及位姿坐标解算模块。上位机读取基于力信号传感器坐标系{FT-Sensor}的三坐标轴向分力与分力矩数据后，首先对末端工具的重力进行补偿，补偿后的数据为末端工具实际所受的外部作用力；然后通过力-位姿偏移控制算法依次将三坐标轴向分力与分力矩对应转换成三坐标轴向平移偏移分量与旋转偏移分量，同时通过力-速控制算法得到运动的速度值；最后，将上述六个分量值组成的矢量信息转换成基于机械臂基坐标系{Robot}的对应矢量值，并与预期位姿坐标值进行矢量相加，得到下一时刻基于机械臂基坐标系{Robot}的机械臂末端预期位姿坐标值。

如图3所示，末端工具1安装在力信号传感器2上，力信号传感器2通过安装连接件3固定在机械臂末端的法兰盘41上，机械臂4具有机械臂关节43，机械臂4通过机械臂基座42安装固定。力信号传感器读数易受末端工具重力的影响，因此需要进行末端工具重力补偿，消除力信号传感器数值中的重力偏差值。在任一姿态下，上位机得到力信号传感器检测到当前外力与外力矩读数F、M，减去该姿态下由末端工具重力引起的外力与外力矩偏差值F'、M'，得到末端工具实际受到的外部作用力与力矩F_out、M_out。用公式表示为：

F_out＝F-F'(1)

M_out＝M-M'(2)

对于末端工具以任意姿态刚性固定在力信号传感器上，对其读数影响可以用两个特征来衡量：重力G与重力臂S。工具安装固定后，末端工具重力G与重力臂S方向与大小唯一确定。重力G在力信号传感器坐标系{FT-Sensor}三个轴向分力只与机械臂姿态有关而与重力臂S无关，三个轴向分力矩则受重力G与重力臂S共同影响。假设力信号传感器的Z轴在竖直向下姿态时检测到的三轴力矢量为F₀，则重力补偿的公式为：

M'＝F'_i×S,i＝x/y/z(5)

表示力信号传感器坐标系{FT-Sensor}变换到机械臂基坐标系{Robot}的齐次变换矩阵。

力信号传感器每次通电后都需要进行偏置校正以保证力检测读数的准确性。考虑到偏置校正是将当前姿态下力信号传感器的读数归零，若末端工具不拆卸，则力信号传感器偏置校正后的重力检测情况如图4所示。假设末端工具沿着如图所示坐标系的Y轴转动，其重力分量只在XOZ轴进行转换，Y轴分力始终为0N。在A'点力信号传感器偏置校正，则检测到的X、Y、Z三轴重力分量均为0N；在B'点检测到X轴分力大小(Gcosθ)N，Z轴分力大小(G(sinθ-1))N；在C'点Z轴重力分量(-2G)N，X、Y轴分量为0N，其中，θ为力信号传感器坐标系{FT-Sensor}Z轴正方向与机器人基坐标系{Robot}XY平面之间的夹角。

通过读取当前机械臂位姿状态，得到机械臂末端坐标系{End}相对于机械臂基坐标系{Robot}的转换矩阵由于传感器坐标系与机械臂末端坐标系{End}平行，因此旋转矩阵为单位阵，有：

因此，力信号传感器在Z轴与重力方向平行的姿态进行偏置校正后，力信号传感器三轴末端工具重力补偿算法为：

Fx_out＝Fx-R₃₁·(-G)(7)

Fy_out＝Fy-R₃₂·(-G)(8)

Fz_out＝Fz-R₃₃·(-G)-(-G)(9

如果能够通过机械测量与计算的方式得到末端工具的重力矩S，可以通过式(5)计算末端工具的重力在某一机械臂位姿下力信号传感器对坐标系{FT-Sensor}三坐标轴向分力矩，写成矩阵形式为：

如果预先无法得到重力臂S，也可以通过设置训练点计算出重力臂S在力信号传感器坐标系{FT-Sensor}三坐标轴向分量的大小。式(10)方程组中只有两个相互独立的方程，因此需要两个位姿训练点的力/力矩组成下式方程组：

计算出一组重力臂S。选择多个训练位姿点计算S并求平均值，能够得到较为准确的重力臂值。在实际机械臂力控牵引和摆位中，应用式(10)计算每一机械臂位姿状态下的重力矩补偿值。

端末工具重力补偿后得到的值Fout与Mout即是末端工具实际所受到的外部作用力和力矩大小。在该具体例中，上位机选择分级控制的力-位姿偏移量算法和S型力-速控制算法，将作用力与作用力矩转化为基于力信号传感器坐标系{FT-Sensor}三坐标轴向平移偏移量与旋转偏移量，并更新机械臂末端在下一采样时刻内的速度值。控制算法的函数对应关系分别如式(12)～(13)所示。力-位姿偏移量算法包括力-平移偏移量算法，如图5(a)所示，以及力-旋转偏移量算法，如图5(b)所示；力-速控制算法的函数模型如图5(c)所示。

上位机得到末端工具重力补偿后的外部作用力在力信号传感器坐标系{FT-Sensor}三坐标轴向分力与分力矩值，随后将各轴向分力与分力矩的绝对值依次按照式(12)计算对应平移/旋转偏移标量值△P(△x,△y,△z,△Rx,△Ry,△Rz)。然后确定分力与分力矩的方向，当某一轴向分力/分力矩为负，表示沿力信号传感器坐标轴负方向的分量，此时将对应平移/旋转偏移标量值取反，与分力/分力矩方向对应。

用俯仰-偏航-翻滚(RPY,Raw-Pitch-Roll)坐标变换矩阵来描述机械臂末端坐标系{End}的当前位姿坐标系noa三坐标轴经过一定的旋转、平移操作之后得到新位姿坐标系n₁o₁a₁三坐标轴的变换关系。假设当前位姿坐标系noa沿n轴旋转γ弧度，沿o轴旋转β弧度，沿a轴旋转α弧度，偏移d在三轴的分量分别为d_n、d_o、d_a，变换后得到的新位姿坐标系n₁o₁a₁，用分别表示机械臂末端坐标系{End}当前采样时刻以及下一采样时刻预期位姿坐标值的变换矩阵，用表示由上位机计算得到的位姿偏移量相对于当前机械臂末端坐标系{End}的变换矩阵。所以有：(注：C＝cos()、S＝sin()，下同)

当前位姿坐标系noa右乘RPY变换矩阵△T，表示沿当前位姿坐标系noa进行旋转与平移变换，得到新位姿坐标系n₁o₁a₁。P变量与坐标变换矩阵△T之间存在下式转换关系：

因此，上位机经过工具重力补偿及力控函数解算后，将力信号传感器信息转换成位姿偏移量△P(△x,△y,△z,△Rx,△Ry,△Rz)，再根据式(15)转化为在当前机械臂末端坐标系{End}下相对位姿变换矩阵再通过上述计算末端工具重力补偿时得到的在当前采样时刻机械臂末端坐标系{End}相对于机械臂基坐标系{Robot}的旋转变换矩阵根据式(14)即解算出机械臂末端下一采样时刻在机械臂基坐标系{Robot}下{Robot}的预期位姿坐标点对应的变换矩阵。

图6和图7分别为具体应用例的控制流程图与实例示意图。按照的插补方式设定机械臂末端从起始点A至目标点Z的运动轨迹。上位机先对各功能模块进行初始化设置，力信号控制器系统对六自由度力信号传感器进行偏置校正，机械臂控制器对机械臂设定初始移动速度，随后两个子系统进入循环等待阶段。

上位机对力信号控制器进行数据读取，得到在起始点A的力信号传感器读数F_A，随后上位机读取当前机械臂位姿信息，并利用储存在寄存器中的工具重力G根据式(6)～(10)计算工具前位姿下的偏差值并予以补偿，因为在起始点A初始没有外部接触力信息，因此补偿后的力信号大小是只包含系统扰动的随机值，设定力控死区阈值大于此随机值，因此通过力-位姿偏移量算法后得到下一采样时刻的位姿偏移量为0，机械臂末端的位姿保持不变。上位机继续力控循环环节。

当某一循环时刻初，力信号传感器检测到操作人员手对末端工具的外部作用力Fout，合力方向指向目标点，此时上位机得到的力信息，进行末端工具重力补偿之后，根据力-位姿偏移量算法和力-速控制算法得到位姿偏移量△P，再经式(14)、(15)对偏移量坐标变换后得到在机械臂基座坐标系{Robot}下的下一时刻的预期运动插补点的位姿坐标值P_B，并写入机械臂控制器，控制机械臂末端运动至B点。在机械臂运动过程中，上位机时刻通过读取机械臂控制器中机械臂末端的运动状态，当检测到机械臂末端的运动减速时，上位机继续力控循环，否则等待。

在B点同理通过上位机对力反馈信息解算得到目标点对应的下一时刻的预期运动插补点的位姿坐标值P_final，并写入机械臂控制器，控制机械臂末端运动至目标点。

最后需要注意的是，公布实施例的目的在于帮助进一步理解本发明，但是本领域的技术人员可以理解：在不脱离本发明及所附的权利要求的精神和范围内，各种替换和修改都是可能的。因此，本发明不应局限于实施例所公开的内容，本发明要求保护的范围以权利要求书界定的范围为准。

Claims (10)

1.一种力控牵引和摆位的多自由度机械臂控制装置，其特征在于，所述控制装置包括：上位机、多自由度机械臂系统和六自由度力传感器系统；其中，所述多自由度机械臂系统包括多自由度机械臂和机械臂控制器；所述六自由度力传感器系统包括力信号传感器和力信号控制器；所述上位机包括力信号控制器通讯模块、末端工具重力补偿模块、力-位及力-速控制算法模块、位姿坐标解算模块、机械臂控制模块和图形用户界面模块；所述上位机分别与机械臂控制器和力信号控制器联接；所述力信号传感器具有相对且平行的安装平面和工具平面，所述安装平面通过安装连接件固定在机械臂末端，在工具平面上安装末端工具，保持末端工具与力信号传感器之间的相对固定；机械臂末端坐标系的Z轴与力信号传感器坐标系的Z轴共线且同向，机械臂末端坐标系的Y轴与力信号传感器坐标系的Y轴平行；外部作用力作用在末端工具上，力信号传感器检测到外力，外力包括外部作用力和末端工具的重力，传输至力信号控制器；力信号控制器将模拟信号转化为数字信号后传输至上位机；上位机的力信号控制器通讯模块接收数字信号得到外力的值，末端工具重力补偿模块解算出实际作用在力信号传感器上的外部作用力沿力信号传感器坐标系三轴的分力与分力矩；力-位及力-速控制算法模块将外部作用力的信息转化为在力信号传感器坐标系下的机械臂末端的位姿偏移量和速度值；位姿坐标解算模块将在力信号传感器坐标系下机械臂末端的位姿偏移量与速度值转换成在机械臂基坐标系下机械臂末端的位姿偏移量与速度值，并将当前采样时刻解算得到的位姿偏移量与当前采样时刻的预期位姿坐标值进行矢量相加，得到机械臂末端下一采样时刻在机械臂基坐标系下的预期位姿坐标值；机械臂控制模块按照机械臂末端的下一采样时刻在机械臂基坐标下的预期位姿坐标值控制机械臂末端运动，带动末端工具到达预期指定的位姿，实现基于力控牵引和摆位控制。

2.如权利要求1所述的控制装置，其特征在于，所述安装连接件为可拆卸连接件，具有相对且平行的机械臂末端安装面和力信号传感器安装面；机械臂末端安装面与机械臂末端固定并唯一确定安装连接件的方向；力信号传感器安装面与力信号传感器固定并唯一确定力信号传感器的安装方向。

3.如权利要求2所述的标定方法，其特征在于，所述机械臂末端安装面的中心具有机械臂末端安装面定中心凸台，边缘设置机械臂末端安装面定位销槽；定中心凸台嵌在法兰盘的圆环中，法兰盘定位销槽与机械臂末端安装面定位销槽通过定位销对齐并固定；力信号传感器安装面的中心具有力信号传感器安装面定中心凸台，边缘设置力信号传感器安装面定位销槽；力信号传感器安装面定中心凸台嵌在力信号传感器的圆环中，力信号传感器上的圆环定位销槽与力信号传感器安装面定位销槽通过定位销对齐并固定。

4.如权利要求1所述的控制装置，其特征在于，所述多自由度机械臂系统包括多自由度机械臂和机械臂控制器；多自由度机械臂具有多个移动副或旋转副组成的串联结构；机械臂控制器包括状态检测模块和运动控制模块。

5.如权利要求1所述的控制装置，其特征在于，所述六自由度力传感器系统包括能够检测六自由度力/力矩分量信息的力信号传感器以及能够处理六自由度力/力矩信息的力信号控制器；力信号控制器包括模数转换模块和偏置校正模块。

6.一种力控牵引的多自由度机械臂的控制方法，其特征在于，所述控制方法包括以下步骤：

1)开机通电，上位机对力信号控制器通讯模块、末端工具重力补偿模块、力-位及力-速控制算法模块、位姿坐标解算模块、机械臂控制模块和图形用户界面模块进行初始化，机械臂控制器与力信号控制器分别进行初始化，上位机通过机械臂控制器使机械臂末端移动到初始位姿，在该位姿状态下对力信号控制器进行偏置校正；

2)外部作用力作用在末端工具上，力信号传感器检测到外力，外力包括外部作用力和末端工具的重力，传输至力信号控制器；

3)上位机向力信号控制器发送力信号请求指令，力信号控制器将模拟信号转化为数字信号后传输至上位机；

4)上位机的力信号控制器通讯模块接收数字信号得到外力的值；

5)上位机向机械臂控制器发送机械臂状态请求指令，读取机械臂当前位姿，并通过末端工具重力补偿模块，采用末端工具重力补偿算法，解算出实际作用在力信号传感器上的外部作用力沿力信号传感器坐标系三轴的分力与分力矩；

6)力-位及力-速控制算法模块采用力-位姿偏移控制算法和力-速控制算法，将外部作用力的信息转化为在力信号传感器坐标系下的机械臂末端的位姿偏移量与速度值；

7)位姿坐标解算模块采用位姿坐标点解算算法，将在力信号传感器坐标系的机械臂末端的位姿偏移量与速度值转换成在机械臂基坐标系下的机械臂末端的位姿偏移量与速度值，用位姿偏移量修正当前采样时刻的预期位姿坐标值，得到机械臂末端的下一采样时刻在机械臂基坐标系下的预期位姿坐标值；

8)机械臂控制模块按照机械臂末端的下一采样时刻在机械臂基坐标系下预期位姿坐标值控制机械臂末端运动，带动末端工具到达预期指定的位姿，实现力控牵引控制。

7.如权利要求6所述的控制方法，其特征在于，在步骤5)中，末端工具重力补偿算法具体包括：在每一采样时刻下，上位机通过机械臂控制器得到机械臂末端的瞬时姿态，再由储存在上位机中的末端工具的重力以及重力臂矢量值，解算重力在力信号传感器坐标系下三坐标轴向分力以及分力矩的偏差值；在同一采样时刻下，上位机通过力信号控制器读取外力在力信号传感器坐标系下三坐标轴向瞬时分力以及分力矩值，与上述重力的偏差值矢量相减，得到在该采样时刻下实际作用在力信号传感器上的外部作用力沿力信号传感器坐标系三轴的分力与分力矩。

8.如权利要求6所述的控制方法，其特征在于，在步骤6)中，力-位姿偏移控制算法采用分级控制策略，即设定多个力阈值点，将外部作用力分为多个受力阈值区间，位姿偏移量分为多个随力递增的对应值，力与位姿偏移量的曲线为阶梯形，在力阈值段内对应值不变；力-速控制算法，采用S型函数控制，即在一定的受力阈值区间内，速度值是所受外部合力绝对值的Sigmoid函数。

9.如权利要求6所述的控制方法，其特征在于，在步骤7)中，位姿坐标点解算算法包括：力信号传感器解算出的位姿偏移量是基于力信号传感器坐标系的坐标偏移值，当前采样时刻的机械臂末端的预期位姿坐标值是基于机械臂基坐标系的位姿坐标值；上位机将上述基于力信号传感器坐标系的位姿偏移量转换成基于机械臂基坐标系的位姿偏移量之后，与当前采样时刻的预期位姿坐标值进行矢量相加，得到机械臂末端的下一采样时刻在机械臂基坐标系的预期位姿坐标值。

10.一种力控摆位的多自由度机械臂的控制方法，所述控制方法包括以下步骤：

1)上位机首先计划机械臂末端到达预期的目标点，确定目标点的位姿坐标值，在无外部作用力作用时，机械臂末端按照预先设定的插补方式从起始点运动到目标点；

2)开机通电，上位机对力信号控制器通讯模块、末端工具重力补偿模块、力-位及力-速控制算法模块、位姿坐标解算模块、机械臂控制模块和图形用户界面模块进行初始化，机械臂控制器与力信号控制器分别进行初始化，上位机通过机械臂控制器使机械臂末端移动到初始位姿，在该位姿状态下对力信号控制器进行偏置校正；

3)在采用插补方式设定的从起始点至目标点的运动轨迹上出现障碍物时，外部作用力作用在末端工具上，以改变机械臂末端的运动轨迹；

4)上位机在当前位姿状态下，通过先前设定的插补方式计算出机械臂末端下一时刻的预期运动插补点的位姿坐标值；

5)力信号传感器在当前时刻检测到外力，外力包括外部作用力和末端工具重力，将力信号转化为模拟电信号传输至力信号控制器；

6)上位机向力信号控制器发送力信号请求指令，力信号控制器将模拟信号转化为数字信号后传输至上位机；

7)上位机的力信号控制器通讯模块接收数字信号得到外力的值；

8)上位机向机械臂控制器发送机械臂状态请求指令，读取机械臂当前位姿，并通过末端工具重力补偿模块，采用末端工具重力补偿算法，解算出实际作用在力信号传感器上的外部作用力沿力信号传感器坐标系三轴的分力与分力矩；

9)力-位及力-速控制算法模块采用力-位姿偏移控制算法和力-速控制算法，将外部作用力的信息转化为在力信号传感器坐标系下的机械臂末端的位姿偏移量与速度值，并将在力信号传感器坐标系的机械臂的位姿偏移量与速度值转换成在机械臂基坐标系下的机械臂末端的位姿偏移量与速度值；

10)位姿坐标解算模块将下一时刻的预期运动插补点的位姿坐标值与位姿偏移量进行矢量相加，得到机械臂末端下一时刻的实际运动插补点的位姿坐标值；

11)机械臂控制模块按照下一时刻的实际运动插补点的位姿坐标值控制机械臂运动，通过外部作用力实现对机械臂末端的运动轨迹的修正；

12)当外部作用力消失后，机械臂末端能够以当前位姿点按照新的插补方式运动到目标点，实现对目标点精确定位的力控摆位控制。