基于手持导航器的协作机器人引导控制技术研究

洪 鹰，匡加伦，肖聚亮，王云鹏，赵 炜，王 健，张阳阳，孙誉博

基于手持导航器的协作机器人引导控制技术研究

洪 鹰1, 2，匡加伦1, 2，肖聚亮1, 2，王云鹏1, 2，赵 炜1, 2，王 健1, 2，张阳阳1, 2，孙誉博1, 2

(1. 天津大学机械工程学院，天津 300350；2. 天津大学机构理论与装备设计教育部重点实验室，天津 300350)

现今实际的制造生产工作给机器人的交互性和协作性提出了越来越高的要求，传统的基于机器人示教器控制的交互技术已经越来越难以满足这些需求．为此，提出了一种新型的引导控制技术，采用了基于力/力矩识别的手持导航器，并开发出了可以实现快、慢速控制相互转换的引导控制模式．首先，对导航器的原始数据进行读取和解析，获得其自身的坐标系，建立和机器人的映射关系，并通过标定得出其输出数据与所受力/力矩之间的定量关系．其次，为减少操控过程中操作者的生理抖动和噪声的影响，引入卡尔曼滤波来对其状态进行最优估计．然后，为实现多控制模式的转换，建立变导纳控制模型，通过最小二乘法获得交互力的变化率，从而确定人机交互过程中的导纳参数，进而选取符合操作者意图的控制模式．最后，通过实验有效地验证了该方法在人机交互应用中具备较高的可行性和通用性．

手持导航器；协作机器人；变导纳控制模型；引导控制

传统的基于机器人示教器控制的人机交互技术存在效率低下、专业门槛高等问题[1]，难以满足现今的工作生产需求，所以机器人交互技术正向着提高交互友好性的方向发展，通过将人置为核心，从原来的人适应机器过渡到机器适应人的阶段，并把人的感知和语言等能力同计算机强大的计算能力有机结合在一起，从而充分反映人的意识形态[2]，而引导控制技术就是其中一个发展方向[3-4]．

机器人引导控制是通过操作设备将人的意图转换为机器人的输入，因此获得人的意图是实现引导控制的前提．目前协作机器人引导控制方案主要有基于力矩补偿算法的无传感器控制方案和有传感器的控制方案，而有传感器的控制方案又可以分为基于多维力传感器和基于关节力矩传感器两种控制方案．黎意枫[5]采用基于模型辨识的方法对机器人进行力矩补偿，从而获得较为精准的动力学模型，最终完成了力矩补偿算法的引导控制．蔡力刚等[6]提出一种基于刚度控制的机器人零力跟踪模型，通过刚度矩阵将力信号转换成位置增量，输入到机器人位置控制器，并设计了一种轨迹预测引导示教方法．Lee等[7]在机器人末端设计了引导控制装置，用一个六维力传感器感知操作者施加的力，引导机器人移动，并用另一个六维力传感器检测末端执行器与工件的接触力，对末端的位姿进行修正．Lee等[8]也设计了两种不同的引导控制装置：一种采用滑动机构和位置传感器组合的方式通过算法得到末端执行器轴线方向的位移补偿量，来调整机器人位姿，在淬火工艺中得到应用；另一种采用装有2个力矩传感器的手柄来获取操作者的交互力和环境接触力，最终应用到了玻璃墙安装方向．Neto等[9]提出基于安装在人手臂上的加速度传感器的引导控制方法，通过人工神经网络算法辨识手臂的位姿，转换成机器人的输入来控制机器人．Park等[10]设计了一款各关节安装有关节力矩传感器的六自由度机器人来实现引导控制功能．

本文借鉴现有的协作机器人引导控制研究成果，认为基于多维力传感器的控制方案无需改变机器人的本体结构和底层控制算法，无论是串联机器人还是并联机器人均可适用，具有很高的通用性．但多维力传感器配套设备较多，机器人的运动容易受到线缆的束缚；且其多安装于机器人的末端，往往会和末端执行器的安装位置产生冲突，同时也无法实现远距离操控，所以本文提出采用基于力/力矩识别的手持导航器来控制机器人．该导航器结构简单，组件较少，无需担心安装问题，对环境要求较低；其与上位机采用无线通讯方式，所以可实现远、近距离两种引导控制方式；且导航器成本较低，易于推广和应用．本文的控制方案针对引导控制中如何切换控制模式的问题，开发了变导纳控制模型，可以根据操作者施加力的变化趋势，确定人机交互过程中的导纳参数，进而选择不同速度和精度的控制模式．而为了使控制过程更加柔顺，减少噪声和操作者抖动带来的误差影响，本文采用卡尔曼滤波对从导航器获取的数据进行处理.

本文的构成如下：第1部分是导航器概述与机器人的坐标映射；第2部分对手持导航器进行开发，包括对其进行标定，得出力/力矩与输出数据之间的定性关系；第3部分是应用卡尔曼滤波处理导航器获取的数据；第4部分建立变导纳控制模型；最后一部分是验证本文引导控制实用性的实验．

1 方案概述和坐标映射

本文采用的是实验室自行设计的手持导航器．该设备由导航器本体和无线USB接收器组成，具有6个自由度．如图1所示，操作者可以在导航器上的6个方向轻轻施加力或者力矩，使导航器顶部发生微小的平动或者转动，内部光学传感元件检测相应方向的形变量．

图1 手持导航器的6个自由度

导航器可以将操作者施加的力和力矩(F，F，F，M，M，M)转换为各个自由度上的输出数据，并把这些数据转换为速度值，发给机器人控制器，从而实现对机器人的控制．本文的机器人引导控制符合力控领域里的导纳控制模式，在固定导纳的控制模式下，由于在导航器上获取的输出数据和施加的力/力矩呈正比例关系，所以传给机器人的速度值就是操作者施加的交互力的线性反映．一方面，在机器人加速启动过程中，要求机器人能够快速启动并达到预定的速度值，而在减速停止过程中，则要求机器人快速地停止．在这种情况下，与施加力始终呈线性对应的速度显然不利于运动控制的快速性和柔顺性．另一方面，机器人控制过程中，会面临各种不同的任务和工作．有时需要机器人快速运动到指定位置或者灵活地完成某种动作，这类任务往往不需要较高的精度；而有时则需要机器人完成较为精准的操控，这种任务往往需要机器人在一个较为低速的情况下运动．若导纳参数能根据任务的不同进行动态规划调整，则控制性能明显好于固定导纳参数的控制模式[11]．

针对上述情况，本文建立了一个基于力/力矩变化率观测的变导纳控制模型，来应对控制过程中提出的不同的任务要求．图2展示了模型的整体流程．如图3所示，本文引导控制技术根据坐标系不同分为两种模式，分别是世界坐标系下的控制模式和工具坐标系下的控制模式．

图2 整体流程

图3 坐标映射

将手持导航器置于实验平台，机器人运动参考坐标系为机器人的基坐标系．从导航器到基坐标系映射的转换方程为

将手持导航器置于机器人末端，机器人运动参考坐标系为末端工具坐标系．从导航器到工具坐标系的转换方程为

将经过卡尔曼滤波器和变导纳控制模型处理后获得的导航器数据，分别与上述的转换矩阵相乘，即可得到不同坐标系下机器人的运动速度．

2 导航器标定

常见的力/力矩传感器标定方案要求严格，且大多都需要解耦[12]，所以对标定系统要求特别高，多采用“滑轮-砝码”标定方法，并定制专门的试验台[13]，单方向和复合方向加载力/力矩．而手持导航器的输出数据已经实现了解耦，无需再进行复合方向加载 力/力矩．

用推拉力计在导航器的顶部各方向加载力/力矩，计算机采集导航器输出的形变量数据，然后进行分析得出力/力矩与输出数据之间的关系．导航器的量程如表1所示．

表1 导航器各自由度的量程

Tab.1 Range of each DOF of the navigator

将标定得到的数据输入到MATLAB软件中，每个自由度上的输出数据被量化为范围在[－350，350]之间的整数，并作出单自由度方向加载的力/力矩与手持导航器输出数据所代表的形变量(，，，R，R，R)之间拟合曲线．图4分别为、、向拟合曲线，图5分别为R、R、R向拟合曲线．

从拟合曲线可以看出，各自由度力/力矩与输出数据在、、、R和R方向为分段的一次函数关系，在R方向为分段的二次函数关系．可以认为，手持导航器在各方向获取的数据值与施加的力/力矩呈正比例关系．因此，可以确定导航器能够线性地反映操作者施加的力和力矩．

图4 力拟合曲线

图5 力矩拟合曲线

3 卡尔曼滤波

为提高人机交互的稳定性和柔顺性，要求机器人运动的速度曲线尽可能平滑，较大的速度突变会导致机器人的抖动，影响人机交互的正常进行．为解决由于手持导航器的刚度不如多维力传感器、操作者的轻微抖动导致的较大数据偏差，以及测量时噪声的影响，且由于导航器测量时的系统噪声和测量噪声均符合高斯分布，力和力矩参数值都是线性的，所以本文采用卡尔曼滤波来消除这些误差．

卡尔曼滤波法是一种根据前一时刻的状态来推测当前时刻状态的算法[14]，其要求模型是线性随机微分系统，即

将手持导航器获取的交互力参数和力矩参数(FFFMMM)代入上述公式中，设置好相应的不确定度值，即可得到本时刻的最优估计参数，该值综合了本时刻的测量值和前一时刻的预测值，是经过消除误差影响后较为准确的数据．

4 变导纳控制模型

为了实现机器人在快速灵活和慢速精准这两种控制模式的随意切换以及实现控制过程中的快速启停，本文建立了变导纳控制模型．在人机交互领域，力控制器主要采用Hogan[15]提出的阻抗控制和导纳控制两种模型．阻抗控制是将位置作为系统输入，将力作为系统输出；而导纳控制则是将交互力作为系统输入，将机器人的位置或者速度作为系统输出．对于本文提出的引导控制模型而言，将人施加的力/力矩作为输入，进而控制机器人运动速度，符合上述的导纳控制模型．一维导纳控制模型可以描述为

对于关节坐标系而言，导纳模型可以改写为

当操作者操纵机器人时，人机交互的可操作性是由虚拟阻尼所决定的．当虚拟阻尼处于一个较小的数值时，操作者手部只需提供一个很小的力，即可操控机器人完成快速、灵活的运动．该方式带来的缺陷则是控制精度较低，可控性不高；而当虚拟阻尼处于一个较大数值时，操作者也相应地需要提供较大的力，来控制机器人完成精准且慢速的动作．在完成一项完整的工作时，往往需要这两种控制方式结合起来，因此该变导纳模型需要在不同的任务需求间进行快速和智能化地选择合适的虚拟阻尼参数．为实现上述功能，本文建立的虚拟阻尼与操作者施加力变化的关系模型如下：

根据最小二乘法，总误差为

5 实 验

5.1 协作机器人D-H参数

本实验采用的是实验室研发的10kg负载的串联协作机器人，其D-H参数如表2所示．

表2 机器人D-H参数

Tab.2 D-Hparameters of the robot

5.2 实验方案

实验1 本文将基于手持导航器的引导控制方法与本实验室研究的基于参数辨识算法的无传感器的引导控制方法进行对比[17]．如图6所示，要求机器人在相同的姿态下以相同的速度在、、方向上进行平稳运动，并使用推拉力计采集操作者对导航器和机器人末端所施加的力，进行多次实验并求平均值进行对比．基于手持导航器的引导控制为方案1，基于参数辨识的引导控制为方案2．

实验2 为验证变导纳模型的控制效果，如图7所示，操作者将手持导航器置于机器人的末端关节处进行引导式的人机交互．机器人的末端装有一个柱状零件，本实验要求该零件顶端沿着顶面边长为180mm的长方体模拟工件边缘进行平稳移动．机器人从工作空间的远端出发，先运动到模拟工件的一个顶点，然后沿着＋、＋、－和－的次序在工件的上表面边缘运动一周后回到起始的顶点，从而完成整个交互过程．实验要求机器人首先快速到达模拟工件的顶点，然后严格地沿其边缘进行稳定且低速的运动，不能有任何偏离预定轨迹的情况出现．这对机器人运动的柔顺性和可控性提出了较高的要求．由于该实验模拟了实际生产中产品涂胶的过程，所以要求机器人的速度平稳而且缓慢．为验证变导纳控制模型的实用性，本文还设计了固定导纳下的控制模式作为对比实验，在该模式下，导纳参数始终保持固定．

图7 实验环境

5.3 结果分析

表3所示为实验1中基于导航器引导控制的交互力和基于参数辨识的无传感器引导控制交互力的对比情况．可以看出，实验1各方向的交互力均小于实验2，因此本文提出的方法引导机器人运动更加容易，具有更高的灵活性．

表3 引导控制交互力对比

Tab.3 Comparison of guide control interaction force

实验2中的卡尔曼滤波器处理数据的效果如图8所示，实验过程中的速度分布如图9所示，固定导纳和变导纳控制模式的速度对比曲线如图10所示．

从图8中可以看出，在整个交互过程中，人手的抖动和环境噪声始终存在，严重影响了机器人控制.本实验所采用的卡尔曼滤波器，很大程度上消除了抖动和外界的噪声影响，使速度曲线更加顺滑，使得机器人运动得更加平稳和柔顺，提高了人机的交互性.

图8 卡尔曼滤波效果

图9是机器人在变导纳控制模型下各个运动阶段的速度分布情况．快速对齐阶段轨迹上的曲线反映了该阶段的速度变化情况，而工件各表面的速度曲线分别反映了在工件表面边缘运动的速度变化情况.

图9 速度分段示意

图10是完成整个控制过程后得到的速度曲线对比．图10(a)为固定导纳下的速度曲线，图10(b)是可变导纳下的速度曲线．固定导纳下，交互过程耗时319ms，而可变导纳模型下，交互过程总共用时334ms．本实验选取了世界坐标系下的速度变化曲线加以研究讨论．不难看出，在前80ms的运动过程，两种方案的运动速度情况几乎是一致的，这对应了实验要求中的机器人和工件顶点快速对齐的运动．在第2个运动阶段，要求机器人沿工件边缘运动得尽可能平稳和柔顺．但是从图10中可以看出，固定导纳控制下在80ms以后的速度曲线的波峰和波谷都存在较大的波动，而可变导纳控制下的波峰和波谷则相对较为稳定和平顺．说明相较于固定导纳控制，变导纳模型控制下的机器人在低速运动时更加稳定和柔顺．图10(b)还反映出，在80ms后的、向的正负方向加速过程中，加速曲线较为平缓，加速度较小，从而使变导纳模型自动切换到导纳较大的模式，实现低速平稳运动．从图10(b)中近乎直线的峰值和谷值可以看出，由于变导纳模型的使用，操作者可以缓慢提升导航器的形变量，最后始终保持在最大形变下进行慢速稳定的运动．而在固定导纳的方案里，操作者只能采取施加较小力的控制方式，但由于导航器的刚度较小，这就导致了操作者难以精准地控制力和形变量的大小，从而无法实现既低速又平稳的运动控制．

图10 速度曲线对比

6 结 语

对机器人而言，提高机器人的交互性和协作性一直是机器人研究的重要方向．本文以串联机器人的引导控制为研究对象，提出了一种基于手持导航器的引导控制技术，该技术将机器人末端与导航器实现运动映射，采用卡尔曼滤波器对噪声和抖动进行滤波处理，并建立变导纳控制模型，实现了机器人控制过程中快速运动和慢速精准运动模式之间的任意切换．实验结果表明，变导纳模型相较于固定导纳控制模型，在引导控制过程中能使机器人运动得更加柔顺和稳定；而且基于力、力矩变化率观测的变导纳模型可以按照操作者的意图快速选取相应的控制模式，对提高人机交互技术的多样性有着重要意义．

［1］ 黎润伟. 面向复杂曲面加工的工业机器人离线编程系统研究[D]. 广州：华南理工大学机械与汽车工程学院，2014.

Li Runwei. Research on Off-line Programming System of Industrial Robot for Complex Surface Processing[D]. Guangzhou：School of Mechanical and Automotive Engineering，South China University of Technology，2014(in Chinese).

［2］ 沈靖皓. 基于手势引导的工业机器人示教技术研究及应用[D]. 杭州：浙江大学机械工程学院，2017.

Shen Jinghao. Gesture Guided Industrial Robot Teaching Technology and Its Application[D]. Hangzhou：School of Mechanical Engineering，Zhejiang University，2017(in Chinese).

［3］ Asada H，Izumi H. Direct teaching and automatic program generation for the hybrid control of robot manipulators[C]//1987 IEEE International Conference on Robotics and Automation. New York，USA，1987：1401-1406.

［4］ Fukuda T，Nakaoka M，Ueyama T，et al. Direct teaching and error recovery method for assembly task based on a transition process of a constraint condition[C]//2001 IEEE International Conference on Robotics and Automation. New York，USA，2001：1518-1523.

［5］ 黎意枫. 免力矩传感器的机器人直接示教技术研究[D]. 哈尔滨：哈尔滨工业大学航天学院，2016.

Li Yifeng. Research on Force-Sensorless Direct Teaching of Robot[D]. Harbin：School of Astronautics，Harbin Institute of Technology，2016(in Chinese).

［6］ 蔡力钢，柳英杰，刘志峰，等. 基于刚度控制的六轴工业机器人零力跟踪模型[J]. 北京工业大学学报，2017，43(7)：1037-1044.

Cai Ligang，Liu Yingjie，Liu Zhifeng，et al. Force-free control model for six axis industrial robot based on stiffness control[J]. Journal of Beijing University of Technology，2017，43(7)：1037-1044(in Chinese).

［7］ Lee S H，Li C J，Kim D H，et al. The direct teaching and playback method for robotic deburring system using the adaptive force-control[C]//2009 IEEE International Symposium on Assembly and Manufacturing. New York，USA，2009：235-240.

［8］ Lee S，Song C，Kim K. Design of robot direct-teaching tools in contact with hard surface[C]//2009 International Symposium on Assembly and Manufacturing. New York，USA，2009：231-234.

［9］ Neto P，Pires J N，Moreira A P. Accelerometer-based control of an industrial robotic arm[C]// The 18th IEEE International Symposium on Robot and Human Interactive Communication. New York，USA，2009：1192-1197.

［10］ Park C，Kyung J H，Do M H，et al. Development of direct teaching robot system[C]//2011 8th International Conference on Ubiquitous Robots and Ambient Intelligence. New York，USA，2011：730-732.

［11］ Arimoto S，Han H Y，Cheah C C，et al. Extension of impedance matching to nonlinear dynamics of robotic tasks[J]. Systems & Control Letters，1999，36(2)：109-119.

［12］ 郑红梅. 机器人多维腕力传感器静、动态性能标定系统的研究[D]. 合肥：合肥工业大学机械工程学院，2007.

Zheng Hongmei. Research on the Static and Dynamic Performance Calibration System for Robots Multi-Axis Wrist Force Sensor[D]. Hefei：School of Mechanical Engineering，Hefei University of Technology，2007(in Chinese).

［13］ 张立建，胡瑞钦，易旺民，等. 基于六维力传感器的工业机器人末端负载受力感知研究[J]. 自动化学报，2017，43(3)：439-447.

Zhang Lijian，Hu Ruiqin，Yi Wangmin，et al. Research on force sensing for the end-load of industrial robot based on a 6-axis force/torque sensor[J]. Acta Automatica Sinica，2017，43(3)：439-447(in Chinese).

［14］ Kim M H，Lee S，Lee K C. Kalman predictive redundancy system for fault tolerance of safety-critical systems[J]. IEEE Transactions on Industrial Informatics，2010，6(1)：46-53.

［15］ Hogan N. Impedance control：An approach to manipulation[J]. Journal of Dynamic Systems Measurement and Control，1985，107：1-24.

［16］ 丁克良，沈云中，欧吉坤. 整体最小二乘法直线拟合[J]. 辽宁工程技术大学学报：自然科学版，2010，29(1)：44-47.

Ding Keliang，Shen Yunzhong，Ou Jikun. Methods of line-fitting based on total least-squares[J]. Journal of Liaoning Technical University：Natural Science，2010，29(1)：44-47(in Chinese).

［17］ Xiao Juliang，Zeng Fan，Zhang Qiulong，et al. Research on the forcefree control of cooperative robots based on dynamic parameters identification[J]. The Industrial Robot，2019，46(4)：499-509.

Research on the Guide Control Technology of a Cooperative Robot Based on a Handheld Navigator

Hong Ying1, 2，Kuang Jialun1, 2，Xiao Juliang1, 2，Wang Yunpeng1, 2，Zhao Wei1, 2，Wang Jian1, 2，Zhang Yangyang1, 2，Sun Yubo1, 2

(1. School of Mechanical Engineering，Tianjin University，Tianjin 300350，China；2. Key Laboratory of Mechanism Theory and Equipment Design of Ministry of Education，Tianjin University，Tianjin 300350，China)

Manufacturing has created stricter requirements for the interaction and cooperation of robots，which have been difficult for traditional robot interaction technologies to meet．In this paper，a new guide control technology is proposed that uses a handheld navigator based on force/torque recognition，and a guide control mode that realizes the conversion of fast and slow control was developed．The original data of the navigator was read and analyzed，its own coordinate system obtained，the mapping relationship with the robot established，and the quantitative relationship between the output data and the force/torque was obtained by calibration．Then，to reduce the influence of physiological jitter and noise of the operators in the process of manipulation，Kalman filter was introduced to optimize the state of operators．To realize the transformation of the multi-control mode，the variable admittance control model was established．The change rate of the interaction force was obtained using least square method，the admittance parameters in the process of the human-robot interaction were determined，and then the control mode in accordance with the operator’s intention was selected．Finally，the feasibility and universality of the method in a human-robot interaction application was experimentally verified．

handheld navigator；cooperative robot；variable admittance control model；guide control

TP242.2

A

0493-2137(2020)11-1183-08

10.11784/tdxbz201909001

2019-09-01；

2019-11-08.

洪 鹰（1962—  ），男，博士，副教授，hying1122@126.com.

肖聚亮，tjxjl@tju.edu.cn.

天津市智能制造重大专项资助项目(16ZXZNGX00140).

Supported by the Major Projects of Tianjin Intelligent Manufacturing Science and Technology(No.16ZXZNGX00140).

(责任编辑：金顺爱)