力控牵引和摆位的多自由度机械臂控制装置及其控制方法
【技术领域】
[0001] 本发明设及多自由度机械臂控制技术，具体设及力控牵引和摆位的多自由度机械 臂控制装置及其控制方法。
【背景技术】
[0002] 多自由度机械臂W其定位重复精度高、可编程、易操作等优点，已在工业设备制 造、航空航天、医疗卫生等领域得到广泛应用。工业多自由度机械臂系统常用的控制方法有 两种:一是采用"示教模式"，即操作人员首先通过机械臂教导器控制机械臂末端(常安装有 工具)沿一定的工作轨迹运动，并使机械臂控制器W预先确定的采样频率"记录"工作轨迹 中每个采样轨迹点对应的机械臂运动状态(包括瞬时速度、瞬时加速度、各关节的瞬时角度 信息等），在实际控制中机械臂控制器W预先选定的插补方式控制机械臂末端依次通过上 述采样轨迹点，复现示教时的工作运动轨迹，完成相应的工作任务;二是采用"离线编程模 式"，即在机械臂控制器中按照特定工作任务要求预先写入控制程序，在实际工况中机械臂 将按照程序中的步骤单步循环运行，实现预定工作任务。
[0003] 在一些应用场景中，有时需要机械臂末端能够按照操作人员的意愿方便地、实时 地调整空间位姿，例如医疗应用中的机械臂扶持、康复训练应用中的人机交互。在运些应用 中一方面在操作人员进行机械臂控制时，通过对末端工具施加作用力实现对末端工具的牵 引操作;另一方面在无外部控制输入时，机械臂能够克服自身及其工具重力等因素影响，保 持当前位姿，实现精确定位。另外，在预先给定机械臂末端目标点位姿信息时，有时候为了 机械臂紧急避障等目的，需要机械臂在运动过程中也能够通过外力控制实时更新运动轨 迹。
[0004] 现有的机械臂控制系统绝大多数不能实现外力作用下控制机械臂的运动，而只能 通过教导模式与离线编程模式两种控制方法预先规划好机械臂运动轨迹。在上述应用场景 中，应用两类控制模式实现机械臂控制所存在的不足主要体现在：
[0005] 1.示教模式下，特定工作任务中要求操作人员控制机械臂对部分位姿点精确到 达，同时也要考虑到各种可能引发安全隐患的因素，因此示教轨迹选取W及示教操作过程 较为费时。离线编程模式下，前期需要将工作任务转化为机器程序，后期根据实际工况条件 会有多次的程序修改与运行调试，系统维护成本也较高。
[0006] 2.当机械臂末端工作轨迹改变时，往往需要操作人员对运动轨迹进行重新示教或 者预编程，也使多自由度机械臂在个性化方案应用方面受到了极大限制。
[0007] 3.机械臂在运行过程中，无法通过检测外部作用力做出相应控制指令的调整，因 此无法实现力控牵引功能;在机械臂运动轨迹中意外遇到障碍物时，无法通过外力作用实 时更新机械臂的运动轨迹，容易造成意外事故。
[000引专利CN103347659A提出了一种传感器中继控制装置，能够通过对力传感器信息进 行解算控制机械臂各关节电动机的运动，但是也不能实现对机械臂系统的力控牵引功能。
【发明内容】
[0009] 基于W上现有技术中多自由度机械臂控制中存在的问题，本发明提出了一种力控 牵引和摆位的多自由度机械臂控制装置及其控制方法。
[0010] 本发明的一个目的在于提出一种力控牵引和摆位的多自由度机械臂控审曠置。
[0011] 本发明的力控牵引和摆位的多自由度机械臂控制装置包括:上位机、多自由度机 械臂系统和六自由度力传感器系统;其中，多自由度机械臂系统包括多自由度机械臂和机 械臂控制器;六自由度力传感器系统包括力信号传感器和力信号控制器;上位机包括力信 号控制器通讯模块、末端工具重力补偿模块、力-位及力-速控制算法模块、位姿坐标解算模 块、机械臂控制模块和图形用户界面模块;上位机分别与机械臂控制器和力信号控制器联 接;力信号传感器具有相对且平行的安装平面和工具平面，安装平面通过安装连接件固定 在机械臂末端，在工具平面上安装末端工具，保持末端工具与力信号传感器之间的相对固 定;机械臂末端坐标系巧nd}的Z轴与力信号传感器坐标系{FT-Sensor}的Z轴共线且同向， 机械臂末端坐标系巧nd}的Y轴与力信号传感器坐标系{FT-Sensor}的Y轴平行;外部作用力 作用在末端工具上，力信号传感器检测到外力，外力包括外部作用力和末端工具的重力，将 外力信号转化为模拟电信号传输至力信号控制器;力信号控制器将模拟信号转化为数字信 号后传输至上位机;上位机的力信号控制器通讯模块接收数字信号得到外力的值，末端工 具重力补偿模块解算出实际作用在力信号传感器上的外部作用力沿力信号传感器坐标系 IFT-SensoHS轴的分力与分力矩;力-位及力-速控制算法模块将外部作用力的信息转化 为在力信号传感器坐标系{FT-Sensor}下的机械臂末端的位姿偏移量与速度值;位姿坐标 解算模块将在力信号传感器坐标系{FT-Sensor}的机械臂末端的位姿偏移量与速度值转换 成在机械臂基坐标系{Robot}下的机械臂末端的位姿偏移量与速度值，并将当前采样时刻 解算得到的位姿偏移量与当前采样时刻的预期位姿坐标值进行矢量相加，得到机械臂末端 的下一采样时刻在机械臂基坐标系{Robot}下的预期位姿坐标值;机械臂控制模块按照机 械臂末端的下一采样时刻在机械臂基坐标系{Robot}下的预期位姿坐标值控制机械臂末端 运动，带动末端工具到达预期指定的位姿，实现基于力控牵引和摆位控制。
[0012] 多自由度机械臂系统包括多自由度机械臂和机械臂控制器;多自由度机械臂具有 多个移动副或旋转副组成的串联结构。机械臂控制器包括状态检测模块和运动控制模块。
[0013] 六自由度力传感器系统包括能够检测六自由度力/力矩分量信息的力信号传感器 W及能够处理六自由度力/力矩信息的力信号控制器。力信号控制器包括模数转换模块和 偏置校正模块。
[0014] 机械臂末端具有圆环状的法兰盘，法兰盘上设置有法兰盘定位销槽。力信号传感 器与机械臂末端相对的面为安装平面，安装平面具有圆环，圆环上设置有圆环定位销槽;力 信号传感器与末端工具相对的面为工具平面，在工具平面上直接固定安装末端工具。安装 连接件为可拆卸连接件，具有相对且平行的机械臂末端安装面和力信号传感器安装面;机 械臂末端安装面与机械臂末端固定并唯一确定安装连接件的方向；力信号传感器安装面与 力信号传感器固定并唯一确定力信号传感器的安装方向。机械臂末端安装面的中屯、具有机 械臂末端安装面定中屯、凸台，边缘设置机械臂末端安装面定位销槽;定中屯、凸台嵌在法兰 盘的圆环中，法兰盘定位销槽与机械臂末端安装面定位销槽通过定位销对齐并固定;力信 号传感器安装面的中屯、具有力信号传感器安装面定中屯、凸台，边缘设置力信号传感器安装 面定位销槽;力信号传感器安装面定中屯、凸台嵌在力信号传感器的圆环中，力信号传感器 上的圆环定位销槽与力信号传感器安装面定位销槽通过定位销对齐并固定。定中屯、凸台的 中屯、点与定位销的连线分别平行于机械臂末端坐标系的Y轴和力信号传感器坐标系{FT-Sensor}的Y轴;安装连接件通过定中屯、凸台和定位销，将机械臂末端与力信号传感器连接， 机械臂末端坐标系巧nd}的Z轴与力信号传感器坐标系{FT-Sensor}的Z轴共线且同向，机械 臂末端坐标系巧nd}的Y轴与力信号传感器坐标系{FT-Sensor}的Y轴平行，并唯一确定安装 方向；机械臂末端与力信号传感器通过安装连接件W唯一方式固定，使两坐标系XYZS轴两 两对应平行，两坐标系原点连线与两坐标系Z轴=者共线且同向。
[0015] 本发明的另一个目的在于提出一种力控牵引和摆位的多自由度机械臂的控制方 法。
[0016] 本发明的力控牵引的多自由度机械臂控制方法，包括W下步骤：
[0017] 1)开机通电，上位机对力信号控制器通讯模块、末端工具重力补偿模块、力-位及 力-速控制算法模块、位姿坐标解算模块、机械臂控制模块和图形用户界面模块进行初始 化，机械臂控制器与力信号控制器分别进行初始化，上位机通过机械臂控制器使机械臂末 端移动到初始位姿，在该位姿状态下对力信号控制器进行偏置校正；
[0018] 2)外部作用力作用在末端工具上，力信号传感器检测到外力，外力包括外部作用 力和末端工具的重力，将外力信号转化为模拟电信号传输至力信号控制器；
[0019] 3)上位机向力信号控制器发送力信号请求指令，力信号控制器将模拟信号转化为 数字信号后传输至上位机；
[0020] 4)上位机的力信号控制器通讯模块接收数字信号得到外力的值；
[0021] 5)上位机向机械臂控制器发送机械臂状态请求指令，读取机械臂当前位姿，并通 过末端工具重力补偿模块，采用末端工具重力补偿算法，解算出实际作用在力信号传感器 上的外部作用力沿力信号传感器坐标系{FT-Sensor}S轴的分力与分力矩；
[0022] 6)力-位及力-速控制算法模块采用力-位姿偏移控制算法和力-速控制算法，将外 部作用力的信息转化为在力信号传感器坐标系{FT-Sensor}下的机械臂末端的位姿偏移量 与速度值；
[0023] 7)位姿坐标解算模块采用位姿坐标点解算算法，将在力信号传感器坐标系{FT-Sensor}的机械臂末端的位姿偏移量转换成在机械臂基坐标系{Robot}下的机械臂末端的 位姿偏移量，用位姿偏移量修正当前采样时刻的预期位姿坐标值，得到机械臂末端的下一 采样时刻在机械臂基坐标系{Robot}下的预期位姿坐标值；
[0024] 8)机械臂控制模块按照机械臂末端的下一采样时刻在机械臂基坐标系{Robot}下 的预期位姿坐标值控制机械臂末端运动，带动末端工具到达预期指定的位姿，实现力控牵 引控制。
[0025] 其中，在步骤5)中，末端工具重力补偿算法具体包括:在每一采样时刻下，上位机 通过机械臂控制器得到机械臂