末端的瞬时姿态，再由储存在上位机中的末端工具的重力W 及重力臂矢量值，解算重力在力信号传感器坐标系{FT-Sensor}下S坐标轴向分力W及分 力矩的偏差值;在同一采样时刻下，上位机通过力信号控制器读取外力在力信号传感器坐 标系{FT-Sensor}下S坐标轴向瞬时分力W及分力矩值，与上述重力的偏差值矢量相减，得 到在该采样时刻下实际作用在力信号传感器上的外部作用力沿力信号传感器坐标系{FT-Sensor} S轴的分力与分力矩。
[0026] 在步骤6)中，位姿包括平移和旋转，相应地，力-位姿偏移控制算法包括力-平移偏 移量算法和力-旋转偏移量算法。力-位姿偏移控制算法采用分级控制策略，即设定多个力 阔值点，将外部作用力分为多个受力阔值区间，位姿偏移量分为多个随力递增的对应值，力 与位姿偏移量的曲线为阶梯形，在力阔值段内对应值不变;力-速控制算法，采用S型函数控 审IJ，即在一定的受力阔值区间内，速度值是所受外部合力绝对值的Sigmoid函数。
[0027] 在步骤7)中，位姿坐标点解算算法包括:力信号传感器解算出的位姿偏移量是基 于力信号传感器坐标系{FT-Sensor}的坐标偏移值，当前采样时刻的机械臂末端的预期位 姿坐标值是基于机械臂基坐标系{Robot}的位姿坐标值;上位机将上述基于力信号传感器 坐标系{FT-Sensor}的位姿偏移量转换成基于机械臂基坐标系{Robot}的位姿偏移量之后， 与当前采样时刻的预期位姿坐标值进行矢量相加，得到机械臂末端的下一采样时刻在机械 臂基坐标系{Robo t}的预期位姿坐标值。
[0028] 本发明的机械臂控制末端工具实现机械臂力控牵引中，上位机不指定机械臂末端 到达某个目标点，机械臂系统受外部作用力控制。首先上位机获取力信号传感器的力/力矩 数值，对数据进行末端工具重力补偿的预处理，保证末端工具在有外部作用力时上位机是 对消除末端工具的重力因素之后的实际作用的外部作用力的力/力矩值进行解算;然后，上 位机通过力-位及力-速控制算法与位姿坐标点解算算法，得到下一时刻的机械臂末端预期 位姿坐标值和速度值并控制机械臂末端运动，实现力控牵引；若上位机获取的力信号传感 器读数为零，即末端工具在无外部作用力时，末端工具不会因为自身重力影响导致误运动， 能够保持当前采样时刻的空间位姿状态。
[0029] 本发明的力控摆位的多自由度机械臂控制方法，包括W下步骤：
[0030] 1)上位机首先计划机械臂末端到达预期的目标点，确定目标点的位姿坐标值，在 无外部作用力作用时，机械臂末端按照预先设定的插补方式从起始点运动到目标点；
[0031] 2)开机通电，上位机对力信号控制器通讯模块、末端工具重力补偿模块、力-位及 力-速控制算法模块、位姿坐标解算模块、机械臂控制模块和图形用户界面模块进行初始 化，机械臂控制器与力信号控制器分别进行初始化，上位机通过机械臂控制器使机械臂末 端移动到初始位姿，在该位姿状态下对力信号控制器进行偏置校正；
[0032] 3)在采用插补方式设定的从起始点至目标点的运动轨迹上出现障碍物时，外部作 用力作用在末端工具上，W改变机械臂末端的运动轨迹；
[0033] 4)上位机在当前位姿状态下，通过先前设定的插补方式计算出机械臂末端下一时 刻的预期运动插补点的位姿坐标值；
[0034] 5)力信号传感器在当前时刻检测到外力，外力包括外部作用力和末端工具重力， 将外力信号转化为模拟电信号传输至力信号控制器；
[0035] 6)上位机向力信号控制器发送力信号请求指令，力信号控制器将模拟信号转化为 数字信号后传输至上位机；
[0036] 7)上位机的力信号控制器通讯模块接收数字信号得到外力的值；
[0037] 8)上位机向机械臂控制器发送机械臂状态请求指令，读取机械臂末端当前位姿， 并通过末端工具重力补偿模块，采用末端工具重力补偿算法，解算出实际作用在力信号传 感器上的外部作用力沿力信号传感器坐标系{FT-Sensor}S轴的分力与分力矩；
[0038] 9)力-位及力-速控制算法模块采用力-位姿偏移控制算法和力-速控制算法，将外 部作用力的信息转化为在力信号传感器坐标系{FT-Sensor}下的机械臂末端的位姿偏移量 与速度值；
[0039] 10)位姿坐标解算模块将在力信号传感器坐标系{FT-Sensor}的机械臂的位姿偏 移量与速度值转换成在机械臂基坐标系{Robot}下的机械臂末端的位姿偏移量与速度值， 并将下一时刻的预期运动插补点的位姿坐标值与位姿偏移量进行矢量相加，得到机械臂末 端下一时刻的实际运动插补点的位姿坐标值；
[0040] 11)机械臂控制模块按照下一时刻的实际运动插补点的位姿坐标值控制机械臂运 动，通过外部作用力实现对机械臂末端的运动轨迹的修正；
[0041] 12)当外部作用力消失后，机械臂末端能够W当前位姿点按照新的插补方式运动 到目标点，实现对目标点精确定位的力控摆位控制。
[0042] 本发明的优点：
[0043] 本发明的多自由度机械臂控制装置，包括上位机、多自由度机械臂系统W及六自 由度力传感器系统，在机械臂末端的法兰盘上用安装连接件W特定方式安装力信号传感 器，使传感器坐标系与机械臂末端坐标系巧nd}平行且Z轴共线同向，运样能够方便上位机 对力信息的处理解算，使力信号传感器检测到的外部合力方向与最终解算的机械臂末端的 运动方向一致。
[0044] 上位机分别与机械臂控制器和力信号控制器相联接，在同一采样时刻分别读取机 械臂末端的位姿信息与力信号传感器检测到的力/力矩信息，保证信息的同步采样。
[0045] 本发明的控制方法，能够在不改变机械臂现有组成结构与控制结构的基础上实现 基于外部作用力的反馈控制，将力反馈信息转化为机械臂末端的位姿偏移量信息，方便系 统集成与控制实施，同时力反馈信息的获取与位置信息解算均在笛卡尔坐标系下，方便数 据处理。通过设定基于分级控制策略的力-位姿偏移控制算法W及基于S型控制策略的力-速控制算法，使机械臂末端的位姿和运动速度随外部作用力而变化，得到更好的人际交互 效果，并能够在人的牵引停止时准确摆位。
[0046] 本发明的机械臂控制装置实现机械臂的力控牵引，既能够使末端工具按照操作人 员的意愿方便实时地实现牵引操作，同时在无外力作用时能够保持当前位姿，实现精确定 位;本发明的机械臂控制装置实现机械臂的力控摆位，能够使操作人员在机械臂向目标祀 点运动过程中通过施加外力改变机械臂末端的运动轨迹，实现机械臂运动中紧急避障。
【附图说明】
[0047] 图1为本发明的力控牵引和摆位的多自由度机械臂控制装置的原理框图；
[0048] 图2为本发明的力控牵引和摆位的多自由度机械臂控制装置的安装连接件的示意 图；
[0049] 图3为本发明的力控牵引和摆位的多自由度机械臂控制装置的末端工具对力信号 传感器检测的影响的示意图，其中，（a)是机械臂末端法向与重力方向垂直的机械臂末端位 姿，（b)是机械臂末端法向与重力方向呈一定角度的机械臂末端位姿；
[0050] 图4为本发明的力控牵引和摆位的多自由度机械臂控制装置的力信号传感器偏执 校正后对末端工具重力补偿算法的影响的示意图；
[0051] 图5为根据本发明的力控牵引和摆位的多自由度机械臂控制方法的一个实施例的 力-位姿偏移量算法和力-速控制算法对应的函数模型，其中，（a)为力-平移偏移量算法的 函数模型，（b)为力-旋转偏移量算法的函数模型，（C)为力-速控制算法的函数模型；
[0052] 图6为本发明的力控牵引和摆位的多自由度机械臂控制方法的流程图，其中，（a) 为六自由度力传感器系统的控制流程图，（b)为上位机的控制流程图，（C)为多自由度机械 臂系统的控制流程图；
[0053] 图7为本发明的力控摆位的多自由度机械臂控制方法的控制示意图。
【具体实施方式】
[0054] 下面结合附图，通过具体实施例，进一步阐述本发明。
[0055] 如图1所示，本实施例的力控牵引和摆位的多自由度机械臂控制装置包括：上位 机、多自由度机械臂系统和六自由度力传感器系统;其中，多自由度机械臂系统包括多自由 度机械臂和机械臂控制器;六自由度力传感器系统包括力信号传感器W及力信号控制器； 上位机包括力信号控制器通讯模块、末端工具重力补偿模块、力-位及力-速控制算法模块、 位姿坐标解算模块、机械臂控制模块和图形用户截面模块;上位机分别与机械臂控制器和 力信号控制器联接。
[0056] 在本实施例中，多自由度机械臂系统为六自由度关节机械臂系统，其执行机构为 具有六转动副的关节机械臂，能够在工作空间实现六自由度位姿定位，机械臂腕部=个转 动副的轴线相互垂直并且相交于一点，组成球副结构;控制机构为具有可教导、可编程控 制的机械臂控制器，其状态监测模块能够实时监测关节机械臂运动状态并响应上位机的状 态读取指令，运动控制模块能够从上位机得到运动指令，并控制机械臂末端进行运动到基 于机械臂基坐标系{Robot}下的位姿坐标点。
[0057] 如图2所示，安装连接件3的结构呈类圆柱体状，相对且平行的机械臂末端安装面 32和力信号传感器安装面31;机械臂末端安装面32的中屯、具有机械臂末端安装面定中屯、凸 台，边缘设置机械臂末端安装面定位销槽;力信号传感器安装面的中屯、具有力信号传感器 安装面定中屯、凸台33,边缘设置力信号传感器安装面定位销槽34。定中屯、凸台使机械臂末 端坐标系巧nd}原点、安装连接件的几何中屯、与力信号传感器坐标系{FT-Sensor}原点S点 共线，并使机械臂末端坐标系巧nd}Z轴与力信号传感器坐标系{FT-Sensor}Z轴共线且同 向；定位销使机械臂末