基于力传感器重力补偿的机器人柔顺控制研究

0 引言

随着机器人技术的快速发展[1-3]，其应用领域也越来越广泛。传统的工业机器人大多是基于位置的运动控制，为了更好地满足工业需求，比如完成抛光、装配、打磨、拖动示教等[4-7]任务，需要给机器人加入力觉感知系统。

作为机器人的力觉感知系统，力传感器是实现柔顺控制的基础。关于力传感器的应用，相关学者进行了大量的理论算法推导和实践研究。文献[8]提出了基于最小二乘法的机器人动力学参数识别方法。文献[9]通过机器人各连杆的变换矩阵和不同坐标系下的力分量变换，解决了工具重力对力传感器读数的影响，但未考虑机器人底座安装角及力传感器零点数据对机器人受力感知的影响。文献[10]通过对力传感器的零点及机器人安装角等参数的求解，实现了机器人末端负载的受力感知。文献[11]提出，采用极大似然估计法降低了力传感器标定误差。但文献[10-11]只进行了机器人末端负载受力感知实验和力传感器的标定实验，并未对理论进行具体的实际应用。

针对机器人运动过程中的柔顺控制，国内外学者提出了阻抗控制[12-16]理论，其具体实现方式主要分为两类：阻抗控制和导纳控制。阻抗控制由基于位置控制的外环与力控制的内环构成，将位置偏差作为控制系统的输入，调整机器人各关节的扭矩，从而使机器人保持适当的接触力。导纳控制[17-18]则是基于位置控制的内环与力控制的外环，将力偏差作为控制系统的输入，调整机器人的末端位置，从而达到对力的控制效果。LEE等[19]提出，在不了解环境的情况下，通过在线改变目标刚度来调节所需的接触力。李正义等[20]针对环境的阻尼、刚度参数未知或变化的情况,提出一种机器人自适应阻抗控制方法，但并未考虑力传感器的测量误差对柔顺控制的影响。

本文采用对力传感器重力补偿的方法降低力传感器对力和力矩的测量误差，为机器人柔顺控制提供准确的受力感知。采集机器人在不同位姿下力传感器数据，计算机器人底座安装倾角、传感器零点数据和末端工具重力及重心在力传感器坐标系上的坐标，利用姿态变换矩阵，实现对力传感器的重力补偿。通过导纳控制方法，实现机器人对抓取物体在重力方向上的柔顺放置，以期使机器人能够更加准确、可靠地实现力和位置的双重控制。

1 理论模型建立

1.1 重力补偿算法

六维力传感器作为机器人的力觉感知系统，可以实时检测大小和方向不断变化的三维正交力(fx、fy、fz)和三维正交力矩(mx、my、mz)。六维力传感器测得的力与力矩由3部分构成，即力传感器的零点数据(系统本身误差)、末端工具重力产生的力与力矩、外界环境施加的力与力矩。为了更加精准地检测机器人末端工具所受的力与力矩，需考虑力传感器零点及末端工具重力产生的误差干扰。因为本文机器人工作在低加速工况下，所以只考虑标定工件与传感器敏感端的重力，忽略了其造成的惯性力，在需更高精度标定等惯性力不能忽略场合，需基于Kalman Filter的动态标定[21]。本文模型计算参考机器人静力学理论[22]。

1.1.1 机器人安装倾角与末端工具重力计算

力传感器的零点数据记为：F0=[fx0 fy0 fz0]T,M0=[mx0 my0 mz0]T，实测的力和力矩记为：F=[fx fy fz]T,M=[mx my mz]T，末端工具重力记为G，G在力传感器坐标系上产生的力和力矩记为：Gs=[gx gy gz]T，Ms=[mgx mgy mgz]T。本文坐标系的定义如图1所示，定义世界坐标系为O0X0Y0Z0，令其Z0轴方向竖直向上，即重力的反方向，机器人基坐标系为O1X1Y1Z1，力传感器坐标系为OsXsYsZs。世界坐标系O0X0Y0Z0通过先绕X0轴旋转U，再绕当前Y轴旋转V，即可得到机器人基坐标系O1X1Y1Z1，姿态变换矩阵可表示为

(1)

图1 坐标系示意图

Fig.1 Diagram of coordinate system

坐标系OsXsYsZs可由坐标系O1X1Y1Z1先绕Z1轴旋转α，再绕当前Y轴旋转β，最后绕当前X轴旋转γ得到，姿态变换矩阵可表示为

(2)

末端工具重力G在坐标系O0X0Y0Z0的分力表示为G0=[0 0 -G]T，通过位姿变换矩阵，可得到G在OsXsYsZs上的分力，计算公式为

(3)

力传感器的实际读数由两部分组成，即工具重力在力传感器上产生的力和力矩以及力传感器的零点数据，表示为

(4)

令

(5)

式(4)可整理为

(6)

式中 I——3×3的单位矩阵

选取N(N≥3)个不同姿态下的力传感器数据，代入式(6)，可得

(7)

其可表示为 F=Ra

(8)

其中

a=[ax ay az fx0 fy0 fz0]T

式(8)进一步整理得

a=(RTR)-1RTF

(9)

由式(9)可以计算出力传感器零点的分力数据fx0、fy0、fz0及常数ax、ay、az。

由式(5)可计算出末端工具的重力为

(10)

同时可得机器人底座的安装角U、V为

(11)

1.1.2 力传感器零点与末端工具重心坐标计算

机器人末端安装工具后，记工具重心在坐标系OsXsYsZs上的坐标为P=[px py pz]T，末端工具重力G在坐标系OsXsYsZs上产生的力与力矩的示意图如图2所示，其力矩计算公式为

(12)

图2 工具重力在力传感器坐标系上的示意图

Fig.2 Gravity of tool in coordinate system of force sensor

由式(4)可知

式(12)整理后得

(13)

令

(14)

整理式(13)得

(15)

选取N(N≥3)个不同姿态下的力传感器数据，代入式(15)，可得

(16)

式(16)可表示为

M=Fb

(17)

其中

整理式(17)可得

b=(FTF)-1FTM

(18)

由式(18)可得末端工具重心在坐标系OsXsYsZs上的坐标px、py、pz和常数bx、by、bz。

再由式(9)和式(14)得

(19)

至此，机器人底座安装倾角U和V、力传感器的零点数据F0=[fx0 fy0 fz0]T、M0=[mx0 my0 mz0]T、末端工具重力G及重心坐标P=[px py pz]T均已求出。

1.1.3 重力补偿值与负载重力计算

由式(5)可得工具重力在坐标系OsXsYsZs的力分量为

(20)

再由式(12)可得工具重力在OsXsYsZs上的力矩分量mgx、mgy、mgz。此时坐标系OsXsYsZs上需补偿的力和力矩可记为

(21)

经过重力补偿后，机器人末端工具受到的真实接触力和力矩为

(22)

由式(22)可计算出机器人末端工具抓取负载的真实重力，用G′表示，即

(23)

1.2 导纳控制

导纳控制将力作为输入，机器人的位移或者速度作为输出。通过对外界接触力的检测，控制机器人末端工具的位置和速度，其结构图如图3所示。图3中，F0为力的期望值，F′为经过重力补偿后得到的接触力，ΔF为力的偏差值，即纠正误差。导纳控制包括电纳控制和电导控制。

图3 导纳控制的结构图

Fig.3 Structure diagram of admittance control

电纳控制输出的是机器人位置信息，可表示为

ΔX=K1ΔF

(24)

其中

ΔX=[Δx Δy Δz Δu Δv Δw]T

K1=diag[k11 k12 k13 k14 k15 k16]

ΔF=[Δfx Δfy Δfz Δmx Δmy Δmz]T

式中 K1——ΔF对ΔX的反馈系数，为6×6对角矩阵

ΔX——机器人末端位姿的偏移量

电导控制输出的是机器人速度信息，可表示为

ΔV=K2ΔF

(25)

其中 K2=diag[k21 k22 k23 k24 k25 k26]

ΔV=[Δvx Δvy Δvz Δvu Δvv Δvw]T

式中 K2——ΔF对ΔV的反馈系数，为6×6对角矩阵

ΔV——机器人末端速度的偏移量

若同时转换为速度与位置控制则属于导纳控制，即

(26)

在完成实际任务的过程中，针对操作工件的几何特征和具体的任务要求，在导纳控制的基础上应增加一个选择矩阵S，即

(27)

其中，S为6×6对角矩阵，对角元素为0或1，0表示位置控制不受力偏差的影响，1表示位置控制受力偏差的影响，通过对S的合理设置，进行运动过程中力和位置的混合控制。

2 实验及数据分析

2.1 实验平台

采用的力传感器为Leptrino六维力传感器，fx、fy、fz的测量范围为:±250 N，mx、my、mz的测量范围为:±6 N·m，f过载范围为:±400%，m过载范围为:±200%。测量误差为：±1.5%。分辨率为：±1/2 000(DC)，输出频率为：1.2 kHz。实验采用EPSON C4-A901S型六自由度机器人，机器人末端工具为一个机械手爪，手爪固定在力传感器上，力传感器与机器人通过法兰联结，采用Kinect相机作为视觉采集器，与机器人分别固定在工作环境中，实验平台如图4所示。

图4 机器人实验平台

Fig.4 Robot experimental platform

1.海绵垫 2.机械手爪 3.六维力传感器 4.Kinect相机 5.EPSON机器人 6.控制主机

2.2 实验及统计分析

2.2.1 力传感器重力补偿实验及分析

为准确得到机器人底座安装倾角和力传感器零点数据，本文采集机器人末端在任意9种不同位姿(表1)下的力传感器数据，每种位姿连续采集500组力传感器数据，并对500组数据求取平均值，降低力传感器产生的读数误差，力传感器数据见表2，由表2知，不同位姿下力传感器数据有明显区别。

根据1.1.1节，对9组数据进行计算，得到机器人底座安装倾角为U=-0.204°,V=-0.129°。末端工具重力G=3.153 N，工具重力包含了安装在力传感器上的手爪重为以及力传感器敏感端的自身重力，因为力传感器本身无法拆卸，所以不能通过称重获得其敏感端重力。本文拆除安装在力传感器上的手爪，在完全空载情况下，再次采集9种位姿下力传感器数据，计算得力传感器敏感端重力为0.835 N。

表1 机器人末端位姿

Tab.1 Postures at end of robot (°)

表2 9种位姿下力传感器的读数

Tab.2 Force sensor readings in nine postures

根据1.1.2节，可得力传感器的零点数据为

手爪重心在力传感器坐标系的坐标(单位：m)。为进一步验证机器人在不同的负载情况下，位姿连续变化时的重力补偿效果，通过机器人抓取500 g负载，位姿由α=60°、β=-20°、γ=-180°连续变为α=85.294°、β=5.251°、γ=-154.938°，根据1.1.3节，再次进行实验验证，结果如图5所示，由图可知，测量值与补偿值对称分布在X轴两侧，误差与X轴几乎重合，误差很小，结果表明该方法能够在不同负载下精确地实现位姿连续变化过程中重力的补偿效果。

图5 位姿连续变化时的重力补偿效果

Fig.5 Gravity compensation effect charts of continuously changing posture

表3为力传感器在9种不同位姿下，力与力矩的相对补偿误差，相对补偿误差是误差与测量值绝对值的百分比。δ为9种位姿下相对补偿误差的平均值。根据表3可知，力的相对补偿误差平均值在2.98%以内，力矩的相对补偿误差平均值在3.89%以内，其中fy在位姿4和mx在位姿5的相对补偿误差较大，主要原因是该位姿下力传感器测得的力与力矩较小，导致在较小的误差下也会产生较大的相对误差。

表3 重力补偿误差分析结果

Tab.3 Gravity compensation error analysis results

为了进一步验证重力补偿算法的准确度，分别选取6种不同质量的负载，再次进行实验验证。仍采用表1的9种位姿，根据1.1.3节，得到6种负载重力的计算值，然后将6种负载重力的计算值与真实值作对比。负载的真实重力通过电子秤测得的质量乘以g(9.8 m/s2)得到，统计结果如表4所示，其中δG表示重力的相对误差，δG在3.86%以内，与之前重力补偿误差结果相近。

由于本实验所用的力传感器测量误差为±1.5%，所以力传感器自身的误差为3%左右，这部分误差无法消除。对比δG与δ的实验结果可知，本文所提出的重力补偿算法能够实现较好的重力补偿效果，可以在机器人柔顺控制时，提供更加精准的力觉感知。

表4 负载重力测量误差

Tab.4 Load gravity measurement errors

2.2.2 机器人柔顺放置实验及分析

机器人柔顺放置实验，是在实现了力传感器重力补偿的基础上，通过力传感器准确感知手爪抓取物体的重力，从而完成对抓取物体的柔顺放置。这种控制方式可以对物体轻拿轻放，针对搬运一些易碎、怕磕碰的物体具有良好的保护措施。

实验过程: 物体抓取系统[23]包括物体检测系统、物体定位系统和机器人运动系统。首先，通过Kinect相机进行物体检测，识别实验台上物体；然后，利用标定技术对物体定位；最后，驱动机器人完成物体的抓取与放置实验。采用无力控和力控两种方式进行实验。无力控实验是仅基于位置控制的放置实验，力控实验是以导纳控制为基础的柔顺放置实验。

图6为机器人在无力控条件下位移和力随时间的变化情况，放置平台为海绵垫和书本。由图6a知，物体在放置过程中，位移均匀下降；由图6b知，物体在接近放置平台时，力传感器感知重力迅速减小，放置平台为海绵垫时力传感器最终测得力为-1.467 N；当放置平台为书本时，力减小的速度更快，力传感器最终测得力为-2.534 N，因为非力控实验时，速度恒定，故不做分析。

图6 无力控实验位移、力随时间的变化情况

Fig.6 Changes of displacement and force with time without force control

仅基于位置控制的机器人运动系统，在放置过程中会产生较大的力冲击信号，放置平台质地越硬，力变化越快，产生的力冲击信号越大，无法实现对抓取物体的柔顺放置。

在对机器人进行力控实验时，主要研究被抓取物体的柔顺放置过程，因为物体在放置过程中只在重力方向上与接触平台有力信息的交流，除重力方向，其他方向并没有接触力，故导纳控制时，选择矩阵S的设置是仅对Z轴方向进行力控制即可，即研究的对象为重力方向上的力、位移和速度的变化情况，所以本文选择对Z轴方向的力控反馈系数做对比实验。根据式(24)和式(25)可得，选取系数k13和k23进行研究，对比实验采用控制变量法。

实验过程分为无重力补偿和有重力补偿两种实验方式，根据式(23)，可准确得到抓取物体的重力。将力传感器重力方向的期望力设置为零，考虑有测量误差，将区间设置为±0.05 N作为判断物体是否已经平稳放置的条件，放置平台为海绵垫。图7为力传感器感知重力、物体位移和速度随时间的变化情况。图7a、7b的力控反馈系数设置：固定k23为0.1 mm/(s·N)，k13作为变量。图7c的力控反馈系数设置：固定k13为0.1 mm/N,k23作为变量。

由图7有重力补偿时曲线可知，在物体放置过程中，力传感器感知重力逐渐减到零时，位移和速度也逐渐减为零。力控反馈系数影响力、位移和速度的变化，力控反馈系数越大，放置时间越短，但力和速度的波动情况也越明显，原因为接近放置平台时，反馈系数越大的调节程度也会越大。由图7a无重力补偿时力曲线可知，力传感器测得的力明显偏大，主要原因为该数据包含了未补偿的力传感器零点数据和末端工具重力在力传感器上的分力，故导致测得数据较大。由图7b无重力补偿时位移曲线可知，物体下降位移明显偏大，主要原因为力传感器测得的数据偏大，导致机器人对物体是否已平稳放置判断有误差。实验表明基于重力补偿的导纳控制能够更加准确地实现机器人对物体的柔顺放置。

图7 导纳控制时力、位移、速度随时间的变化曲线

Fig.7 Change curves of force, displacement and velocity with time under admittance control

3 结束语

提出了一种基于力传感器重力补偿的机器人柔顺控制方法。考虑机器人底座安装倾角、力传感器零点数据和末端工具重力在力传感器上产生的力与力矩对力传感器读数的影响，分别完成了力传感器重力补偿实验和机器人柔顺放置实验，并进行了数据分析及验证。力传感器重力补偿实验结果表明，力的相对补偿误差平均值在2.98%以内，力矩的相对补偿误差平均值在3.89%以内，手爪对所夹持物体重力感知的相对误差在3.86%以内，相对误差较小，且可以做到对不同负载质量进行实时性补偿。由机器人柔顺放置实验中力、位移和速度的曲线可知，当力传感器感知重力逐渐减为零时，机器人的位移和速度也逐渐减为零。实验结果表明，将力传感器的重力补偿应用于机器人导纳控制的方法能够提高机器人对外界环境感知的精准度，实现更精准的机器人柔顺控制。