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

摘要

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

主权项

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