机械臂腕力传感器负载端重力补偿算法与仿真

刘岩 武俊峰 夏科睿 马常友

摘 要：针对机械臂运动时负载端姿态变化影响腕力传感器测量结果问题，提出一种腕力传感器负载端重力补偿算法。通过分析机械臂负载端不同姿态下的重力变化形式，采用实验方法测定负载端重力与重心参数。建立了腕力传感器重力补偿系统模型，结合机械臂运动学方程，推导出腕力传感器负载端重力补偿算法和机械臂末端作用力解算方法，并应用算例进行MATLAB与ADAMS联合仿真。仿真结果表明，本补偿算法能有效地消除负载端姿态变化影响，通过腕力传感器准确地测量机械臂所受的外力信息，验证了机械臂腕力传感器负载端重力补偿算法的正确性。

关键词：机械臂；腕力传感器；重力补偿；仿真

DOI：10.15938/j.jhust.2018.02.014

中图分类号： TP241.3

文献标志码： A

文章编号： 1007-2683（2018）02-0078-08

Abstract：Aiming at the problem that the measuring results of wrist force sensor can be influenced by load end′s attitude change when mechanical arm moves， this paper proposes a load gravity compensation algorithm for wrist force sensor.According to gravity variation of the load end in different mechanical arm posture， the relationship between the gravity of load end and the position of the center of gravity can be measured by experiment. We build a wrist force sensor gravity compensation model， and combining manipulator kinematics equation， we deduct a load gravity compensation algorithm for wrist force sensor and a calculation method for force acting on the end of mechanical arm. Specific examples have been tested by adopting the ADAMS and MATLAB joint simulation method. The results show that this compensation algorithm can effectively eliminate the influence of the attitude change of load end， the external force information of mechanical arm can be measured by wrist force sensor.And the results verify the correctness of the load gravity compensation algorithm for wrist force sensor.

Keywords：mechanical arm； the wrist sensor； gravity compensation； simulation

0 引 言

如今機器人广泛地应用到工业、军事、航天等自动化领域。为使机器人更好的顺应接触的作业环境，一般会在机器人上安装力传感器，通过采集的力信息作为反馈条件，结合柔顺控制算法完成作业。如工业机器人完成精密的装配、磨光、切削等任务时，用以修正机械臂末端的运动轨迹，使其达到理想效果[1-3]；再如操作员为了通过施加较小的力，直观调整机械臂末端的姿态，实现机械臂的随动[4-5]；还可以分析机核电站救灾机器人械臂作业时各关节受力情况，优化控制策略[6]。所以当机器人与外界环境交互时，不仅依靠力传感器的反馈信息实现机器人高精度的柔顺作业，同时还确保机器人本体的安全性。

目前，国内外学者对力传感器的性能指标做了大量的研究工作，包括力传感器的静态标定和动态标定，测量结果的准确性和可靠性都有一定的提高和改善[7-9]。但是当机械臂与六维力传感器配合使用时，安装的转接机构会对六维力传感器读数产生影响。如果机械臂的负载端重力较小，可以不考虑负载端重力对六维力传感器力和力矩读数的影响[5]。当机械臂负载端重力较大时，对力传感器读数产生的影响一般不可忽略。负载端对传感器的受力敏感程度也会因外力的作用位置不同分为3个作用区域[10]。在控制精密要求不高时，根据机械臂所受外界力的作用区域，忽略力或力矩的影响[4][11]。浙江大学吴文祥针对摩擦引起机器人低速时运动性能恶化、作业精度差的问题，采用自适应鲁棒控制算法补偿机械臂关节摩擦[12]。东南大学陈建元早些年利用增加姿态敏感单元的方法获取手爪空间姿态，在腕力传感器上增加三维直线加速度计，再通过信息处理来消除影响[13]。该方法适用于一般早期工业机械臂，如今不需要借助外界传感器获取机械臂末端姿态，此外对于机构精密的机械臂、内部线路集成化，安装配合也不利于实现。

本文以带有腕力六维力传感器的六自由机械臂模型为研究对象，在空间中多次测定不同姿态下六维力传感器数据，采用最小二乘法拟合数据，获得负载端的重力与重心参数。通过建立腕力传感器重力补偿系统模型，结合机械臂运动学方程，推导出腕力传感器负载端重力补偿算法，修正读数，进而得出机械臂末端作用力的解算方法，准确获取机械臂末端接触力，最后通过仿真验证机械臂腕力传感器负载端重力补偿算法的正确性。

1 传感器负载端重力补偿方法与测量

1.1 传感器负载端重力大小与重心位置测量方法

对传感器负载端重力进行补偿，需要明确腕力传感器负载端重力大小和重心位置。一般将六维力传感器安装在机械臂末端的腕部，再通过转接装置与执行器相连，转接机构的复杂性使得传感器负载端重力与重心位置难以确定。

2 仿真实验与分析

仿真实验包括两方面内容，首先根据机械臂运动学方程与轨迹规划算法，规划出机械臂末端运动曲线；然后应用具体算例，验证六维力传感器负载端重力补偿方法和机械臂末端作用力解算方法的正确性，为后续物理样机的程序开发打下基础。

2.1 仿真平台的搭建

多体系统动力学分析软件Adams具有强大的运动学和动力学仿真及运算功能，但是在Adams软件中很难精准建立较为复杂的三维模型，所以凭借SolidWorks软件精准、快速的建模功能，事先在SolidWorks软件中设计并建立六自由度机械臂与移动平台的三维实体模型[14-15]。将三维模型保存为x_t文件形式，导入Adams软件。六自由度机械臂的三维实体模型，如图4所示。

模型导入到Adams软件后，需要对每个零件的质量、材料等参数进行设置，与真实机械臂参数保持一致。机械臂底座中心点所在的坐标系为基坐标系，用固定副将机械臂底座固定在大地上。机械臂六个关节均为旋转关节采用转动副进行连接，其它机构均采用固定副连接，这样机械臂每两个零件之间都定义好了约束关系，确保机械臂运动方式正确。其中六维力传感器安装在机械臂腕部位置，末端装有两指夹持器。虚拟样机中的移动机械臂模型，如图5所示。

为了实现对机械臂运动控制，首先采用D-H模型表示机械臂六个关节坐标系，利用关节间坐标系的变换，求取机械臂正逆运动学方程[16-17]。并完成机械臂末端运动轨迹的规划，以及关节空间规划，实现了机械臂末端在笛卡尔空间的直线、圆弧等运动[18-20]。应用Matlab软件的强大的计算功能，建立M文件，设计并编写控制程序算法，包括逻辑控制程序、机械臂运动学、轨迹规划及六维力传感器负载端重力补偿等算法[21-22]。

2.2 機械臂运动轨迹规划与仿真

机械臂末端是否变姿态运动对负载端重力补偿起决定性作用，要验证补偿算法的正确性，需对机械臂规划一段变姿态的运动轨迹。本文在机械臂常作业的工作空间内，规划一段由直线和空间圆弧组成的变姿态的运动轨迹，并对运动轨迹进行位置和姿态插补算法。

首先，将该轨迹分两段分别进行的直线和圆弧位置插补运算：第一段轨迹由点P1到P2，进行直线位置插补运算；第二段轨迹由点P2、P3、P4组成，进行空间圆弧位置插补运算。在笛卡尔空间下各点的空间坐标参见表1。

运用Matlab软件完成机械臂末端的上述轨迹规划，在两段轨迹插入足够数量的点数，确保机械臂末端轨迹仿真运动的平滑。应用机械臂运动学逆解方程，在笛卡尔空间下求取运动轨迹中每一点坐标对应的六组关节角，确定6个关节随时间运动的角度，将角度拟合成曲线如图6所示，机械臂末端从点P1运动到P4，运动时间为40s，将6个关节随时间运动的关节角的函数录入到在Adams软件中，应用SPLINE函数作为机械臂各关节运动变化的规律曲线。

在软件中获取仿真中机械臂末端在基坐标系{O}下的运动轨迹的空间坐标值，与规划的轨迹进行比对，轨迹各轴误差小于0.01mm，可忽略不计。仿真轨迹的曲线如图7所示。

2.3 传感器负载端重力补偿仿真实验与分析

仿真需要明确负载端重力大小和重心位置，可利用Adams软件对负载端的质量及质心进行设定。在实际应用中，可参照1.1节介绍的方法进行计算。机械臂的负载端由末端执行器和转接件两部分组成，末端执行器质量m1=0.94kg，转接件质量m2=0.20kg，负载端总质量即两者质量总和约为m=1.14kg。负载端在坐标系{S}下的重心坐标为（10，10，32），单位mm。利用2.2节规划的直线和圆弧运动轨迹，令机械臂做空载运动，六维力传感器的数据如图8所示。

由图8可见，负载端重力对六维力传感器读数的影响，为了消除这种影响，需要在机械臂规划的轨迹运动过程中根据机械臂正运动学方程，确定机械臂末端每一时刻的姿态矩阵OSR。并将已知量负载端总质量、重心位置代入式（13），即可求解出运动过程中每一时刻传感器负载端重力补偿数值，与六维力传感器实际值作差，实时标定六维力传感器的零位值，标定的误差曲线如图9所示。

仿真结果表明，当机械臂空载运动时，传感器负载端重力补偿算法解算出的结果与负载端对传感器读数的影响结果仅存在很小的误差，力误差小于0.01N，力矩误差小于0.02N·m。其误差主要来源于仿真时机械臂运动轨迹插补精度，这个数据是仿真时，机械臂未抓取重物时任意方向的最大误差，补偿误差在允许范围内，所以传感器负载端重力补偿算法可有效地消除负载端对机械臂空载运动的影响。

2.4 机械臂末端作用力解算仿真实验与分析

机械臂末端作用力解算仿真实验，令机械臂末端夹持器抓取已知质量的重物，对采用负载端重力补偿算法前后两组仿真实验数据进行对比分析，验证是否可以直接获取机械臂抓取重物的重量大小。仿真算例中，机械臂夹持器抓取2kg的重物，其重力约为19.6N，在Adams仿真平台中，移动机械臂抓取重物示意图，如图10所示。

机械臂末端运动同样采用2.2节所规划的轨迹，图11（a）、（b）是在0～20s的时间内，机械臂抓取重物运动一段变姿态的直线轨迹；图11（c）、（d）是在20～40s时间内，机械臂抓取重物运动一段变姿态的圆弧轨迹。分别验证负载端重力补偿算法前后两组对比实验，待两组仿真实验运动完成后，在Adams软件中获取机械臂腕部六维力传感器力与力矩的测量值。

采用力传感器负载端重力补偿算法前后，六维力传感器的力和力矩数据曲线分别如图12（a）、图12（b）所示。在机械臂末端抓取重物变姿态的运动过程中，当没有进行负载端重力补偿时，由于六维力传感器负载端重物的干扰，不能准确的读出机械臂末端所受的外界力信息。

根据式（17）可以计算出机械臂末端作用力，为了直观的观察解算的的测力结果，将机械臂传感器的测试力信息转换到基坐标系{O}下，如图13所示。由图像可知，在基坐标系{O}下，Z轴方向的读数FZ=-19.6N，即机械臂抓取物体的重量，准确地获得了外界对机械臂末端的作用力。

3 结 论

本文对基于六自由度机械臂腕部六维力传感器的零位标定进行了研究，得出如下结论：

1）对机械臂负载端姿态变化对六维力传感器零位值的影响进行分析，采用实验方法可以得到机械臂负载端重力大小与重心位置。

2）建立了腕力传感器重力补偿系统模型，结合机械臂运动学，推导出腕力传感器负载端重力补偿算法和机械臂末端作用力解算方法。

3）搭建了移动机械臂运动学仿真平台，对采用机械臂负载端重力补偿算法前后进行仿真对比实验，验证了该算法的正确性。

本方法解决了机械臂负载端重力过大对腕部六维力传感器读数的影响。机械臂在角加速度较低的变姿态运动时，忽略运动时惯性力，消除负载端重力对力传感器的影响在原理上可行，并经过仿真验证了该方法的有效，可以普遍应用到基于力反馈的重型工业机器人及其他类似的作业环境中。

参 考 文 献：

[1]DIETRICH F， BUCHHOLZ D， WOBBE F， et al. On Contact Models for Assembly Tasks： Experimental Investigation Beyond the Peg-in-hole Problem on the Example of Force-torque Maps[C]// Intelligent Robots and Systems （IROS）， 2010 IEEE/RSJ International Conference on. IEEE， 2010： 2313-2318.

[2]缪新， 田威. 机器人打磨系统控制技术研究[J]. 中国空间科学技术， 2014（11）：8-14.

[3]LIANG W， SONG Y， LV H， et al. A Novel Control Method for Robotic Belt Grinding Based on SVM and PSO Algorithm[C]// International Conference on Intelligent Computation Technology and Automation（ICICTA）， IEEE， 2010（1）：258-261.

[4]林君健. 基于力傳感器的工业机器人主动柔顺装配系统研究[D]. 广州：华南理工大学，2013.

[5]BISCHOFF R， KURTH J， SCHREIBER G， et al. The KUKA-DLR Lightweight Robot Arm-a New Reference Platform for Robotics Research and Manufacturing[C]//Robotics （ISR）， 2010 41st international Symposium on and 2010 6th German Conference on Robotics （ROBOTIK）. VDE， 2010： 1-8.

[6]黎佳骏. 面向核电救灾的机械臂开门拧阀门力学测试平台研究[D]. 哈尔滨：哈尔滨工业大学，2014.

[7]XU K J， LI C， ZHU Z N. Dynamic Modeling and Compensation of Robot Six-axis Wrist Force/torque Sensor[J]. Instrumentation and Measurement. IEEE Transactions on， 2007，56（5）：2094-2100.

[8]姜力， 刘宏， 蔡鹤皋. 多维力/力矩传感器静态解耦的研究[J]. 仪器仪表学报， 2004，25（3）：284-287.

[9]HOWARD C Q， HANSEN C H. Six-axis Vibrational Power Transducer for Active Vibration Isolation[C]//Proceedings of acoustics 2005： Annual conference of the Australian Acoustical Society， 2005：4-6.

[10]王志军. 双层预紧式六维力传感器基础理论与应用研究[D]. 秦皇岛：燕山大学， 2012.

[11]干方建. 基于多维力传感器的机器人动态特性若干问题的研究[D]. 合肥：合肥工业大学， 2003.

[12]吴文祥， 朱世强， 王宣银， 等. 基于摩擦模糊建模与补偿的机器人低速控制[J]. 电机与控制学报， 2013， 17（8）： 46-53.

[13]陈建元. 一种可消除机器人手爪姿态变化对腕力传感器影响的方法探讨[J]. 机器人，1992，14（6）：29-33.

[14]马如奇， 郝双晖， 郑伟峰. 基于MATLAB与ADAMS的机械臂联合仿真研究[J]. 机械设计与制造， 2010， 4（4）： 93-95.

[15]宗晓萍， 李月月. 基于ADAMS和MATLAB 的机械臂控制仿真[J]. 微计算机信息， 2009 （35）： 29-30.

[16]秦利， 刘福才， 金振林， 等. 仿人机器人混联灵巧手臂设计及运动学逆解研究[J]. 电机与控制学报， 2014， 18（12）： 102-107.

[17]王帅， 梅涛， 赵江海. 新型六自由度机械臂的运动学分析与仿真验证[J]. 计算机工程与设计， 2014， 35（9）：3213-3218.

[18]卓扬娃， 白晓灿， 陈永明. 机器人的三种规则曲线插补算法[J]. 装备制造技术， 2009 （11）： 27-29.

[19]COSTANTINESCU D， CROFT E A. Smooth and Time-optimal Trajectory Planning for Industrial Manipulators Along Specified Paths[J]. Journal of Robotic Systems， 2000， 17（5）： 233-249.

[20]刘松国. 六自由度串联机器人运动优化与轨迹跟踪控制研究 [D]. 杭州：浙江大学， 2009.

[21]孙铭. 基于MATLAB和ADAMS的机械臂的轨迹规划与协调控制[D]. 北京：北京化工大学， 2015.

[22]许家忠， 王温强， 谢文博等. 故障机械臂模型重构后的轨迹规划与实验分析[J]. 哈尔滨理工大学学报， 2016， 21（2）： 1-7.

（编辑：温泽宇）