基于UKF的六维力传感器重力补偿算法研究

李非凡,罗杨宇,曹志勇,李 翔,程 光

(1.北京联合大学 城市轨道交通与物流学院,北京 100101;2.中国科学院自动化研究所,北京 100190;3.北京联合大学 前沿智能技术研究院,北京 100101)

0 引言

随着科学技术的发展,传感器技术已经广泛应用于机器人、航空航天、制造业、医疗设备等领域。在工业生产和科学研究中,力传感器发挥着关键作用,特别是在人机交互领域中,精确测量物体的力和扭矩是确保人机交互系统精准控制和操作的关键[1]。在力控制、力反馈中,精确的力测量可以确保人机交互系统的安全性和可靠性,并且可以提高人机交互系统的操作精度和效率[2]。在人机交互系统中,为了实时监测机器人末端工具所受的力,通常在末端工具与机器人之间安装六维力传感器,可实时测量工具所受的三维力和力矩。随着机器人的不断运动,位姿信息不断改变,受末端工具的重力影响,传感零位值不断变化,导致传感器测量上产生一定偏差,降低了测量精度,进而影响机器人的操作。因此,有必要对机器人运动中的六维力传感器进行重力补偿与校正,使传感器在机器人运动过程中也能检测出末端工具实际所受的外力大小,从而使系统能够更加精密地完成操作任务。六维力传感器具有高灵敏度和高精度,对环境中的干扰和噪声非常敏感[3]。因此,在处理六维力传感器数据时,还需要采用一些有效的滤波和校准技术,以便最大限度地减小测量误差和噪声。

常用的六维力传感器重力补偿和校正算法包括线性回归、最小二乘法、小波变换、神经网络和粒子群优化等方法[4]。其中,利用线性回归和最小二乘法计算传感器的偏移量和灵敏度系数来进行重力补偿和校正的方法具有计算简单、易于实现等优点。小波变换方法将传感器测量值分解为多个频段,并分别进行重力补偿和校正,从而有效地消除了高频噪声的影响[5]。神经网络和粒子群优化方法可以根据传感器的测量值和理论模型,通过自适应学习和优化来进行重力补偿和校正,因此,具有较好的准确性和鲁棒性。

这些算法在处理六维力传感器的重力补偿和校正时仍然存在不足之处。例如:算法的精度和准确性容易受到环境和传感器位置的影响[6];部分算法需要进行多次试验和复杂的参数调节,时间成本较高;部分算法对非线性和复杂的传感器模型的处理能力有限,需要进一步研究和优化[7]。为解决上述问题,本文使用 UKF 算法处理六维力传感器数据,以实现对六维力传感器数据的高精度估计。

1 UKF 原理与算法设计

1.1 UKF 原理

UKF 是一种基于卡尔曼滤波器的无模型参数滤波器,可以用于估算非线性系统的状态和参数。UKF算法是利用UT变换获取Sigma点集合,这些Sigma点通过非线性系统进行传播,以获得一组新的预测Sigma点,再将预测的Sigma点通过观测模型传递,以获得一组预测测量。这组预测测量可以用来计算卡尔曼增益并更新状态的均值和协方差矩阵,从而更新状态估计[8]。通过非线性函数传递,将非线性函数线性化问题转换成系统状态量概率密度分布的近似,然后基于Kalman算法框架进行滤波。相较于传统的卡尔曼滤波器,UKF 不需要对非线性系统进行线性化处理,能够更好地处理非线性系统和噪声,具有更好的鲁棒性和准确性[9]。

1.2 UKF 算法设计流程

UKF 处理六维力传感器数据的过程与一般的 UKF 处理过程类似,主要包括以下步骤:

1)状态变量的选取:确定状态向量,包括估计需要的物理量,即六维力数据。

2)建立状态转移函数:建立状态转移函数,将当前状态向量映射到下一时刻的状态向量。

3)建立测量函数:根据传感器的测量原理,建立测量函数,将状态向量映射到测量向量。

4)噪声模型:确定噪声模型,包括过程噪声和测量噪声,通常假设为高斯分布。

5)初始状态的设定:根据实际情况,设定初始状态向量和协方差矩阵。

6)Sigma 点的生成:根据当前状态向量和协方差矩阵,生成Sigma点集合。Sigma点的数量通常为2n+1个,其中n是状态向量的维数。

7)Sigma 点的传播:根据状态转移函数,将 Sigma 点集合进行状态转移,得到预测的Sigma点集合。这些点的传播过程采用无迹变换,以保持Sigma点的高斯分布特性。

8)预测状态向量和协方差矩阵:从预测Sigma点集合中,计算预测状态向量和协方差矩阵。

9)预测测量向量和协方差矩阵:将预测状态向量和协方差矩阵通过测量函数得到预测测量向量和协方差矩阵。

10)状态预测:使用当前状态的均值和协方差矩阵生成一组Sigma点,通过非线性系统进行传播,得到一组新的预测Sigma点,从预测的Sigma点计算出预测的状态均值和协方差矩阵。

11)测量预测:将预测的Sigma点通过观测模型传递,得到一组预测测量。这组预测测量可以用来计算卡尔曼增益。

12)计算卡尔曼增益:卡尔曼增益将预测的状态估计与实际观测进行比较。UKF使用基于预测状态的均值和协方差矩阵、测量的均值和协方差矩阵以及预测测量的协方差矩阵计算卡尔曼增益。

13)更新状态估计:通过卡尔曼增益和测量值来更新状态的均值和协方差矩阵,将产生一个新的状态估计值,该值可用于下一次迭代的预测步骤[10]。

2 基于六维力传感器的 UKF 算法设计

2.1 设置参数

2.1.1设置状态向量x

状态向量x包含了末端受力工具(机械手)在X、Y、Z3个方向上的位置、受力、力矩等信息,表示为

x=[fx,fy,fz,mx,my,mz]T。

(1)

其中,fx,fy,fz表示机器人在X、Y和Z轴上的受力,mx,my,mz表示机器人围绕X、Y和Z轴的力矩。

在 UKF 算法中,状态向量x的更新是通过对x进行预测和校正来实现的。预测阶段通过系统动态方程对x进行预测,校正阶段通过测量方程对预测值进行校正。

2.1.2设置状态噪声Q

状态噪声Q表示系统动态方程中的噪声,它可以通过状态转移矩阵G和通常假设为零均值的高斯白噪声QwGT计算得到:

Q=GQwGT。

(2)

其中,G是状态转移矩阵。Q是噪声方差矩阵。QwGT通常假设为零均值的高斯白噪声,即:

Qw～N(0,Qw)。

(3)

其中,Qw是噪声方差。

2.1.3设置测量噪声R

测量噪声R表示测量方程中的噪声,它可以通过测量转移矩阵G和测量方差R0计算得到:

R=GR0GT。

(4)

其中,G是测量转移矩阵,R0是测量方差矩阵。

2.1.4设置无迹变换参数α,β,κ

UKF 算法中的无迹变换(Unscented Transformation)将高斯分布的均值和协方差转换为一组样本点,用于对非线性函数进行逼近[11]。

无迹变换需要设置3个参数:α,β,κ。其中,α控制样本点离均值的距离,β用于处理高斯分布的不确定度,κ用于调整样本点的权重。六维力传感器UKF的算法取值为:α=0.001,β=2,κ=0。

2.2 预测阶段

预测阶段通过状态转移方程对状态向量x进行预测,得到预测状态向量xk|k-1和预测协方差矩阵Pk|k-1:

xk|k-1=f(xk|k-1),

(5)

(6)

其中,f是状态转移方程,Qk-1是状态噪声协方差矩阵。

2.3 校正阶段

校正阶段通过测量方程对预测状态向量xk|k-1进行校正,得到校正状态向量xk|k和校正协方差矩阵Pk|k:

yk=h(xk-1|k-1),

(7)

xk|k=xk|k-1+Kk(yk-h(xk|k-1)),

Pk|k=(I-KkHk)Pk|k-1。

其中,yk是测量向量,h是测量方程,Sk是测量残差的协方差,Rk是测量噪声协方差矩阵,Hk是测量转移矩阵。

在 UKF 算法中,根据预测状态向量xk|k-1和预测协方差矩阵Pk|k-1,可以通过无迹变换计算一组样本点,然后根据这些样本点和权重系数计算校正状态向量xk|k和校正协方差矩阵Pk|k。

根据预测状态向量xk|k-1和预测协方差矩阵Pk|k-1,计算一组无迹变换的样本点:

(8)

(9)

接下来计算预测测量向量yk|k-1和预测测量协方差矩阵Pyy,k|k-1:

(10)

(11)

最后,计算校正增益Kk、校正状态向量xk|k-1和校正协方差矩阵Pk|k:

xk|k=xk|k-1+Kk(yk-yk|k-1),

(12)

(13)

3 实验过程

本实验采用六维力传感器KWR 75,采样频率为 1 000 Hz,该传感器具有高灵敏度和高稳定性,能够实时采集目标物体在6个自由度方向上的受力信息。实验装置如图 1 所示,机械臂采用睿尔曼RM-65六自由度机械臂,末端工具采用因时仿人机械手。六维力传感器通过法兰安装在机械臂与机械手之间,通过传感器测量机械手与外界物体接触的力和力矩,利用 Visual Studio Python语言实时接收数据并代入UKF算法中,导出实验数据并绘制数据曲线图。实验通过采集末端机械手动态空载下六维力传感器的测量数据,利用 UKF 算法对数据进行处理,验证 UKF 算法的准确性和鲁棒性。

实验过程中,在末端工具不接触外力与外界的环境下,对机械臂进行加速度和角速度激励,记录测量到的六维力的值[12]。图 2～7 为力反馈的原始数据,横轴为时间,单位为s,纵轴为力或力矩,单位为 N 或 N·m 。其中,图2表示物体在X方向上的力,图 3 表示物体在Y方向上的力,图 4 表示物体在Z方向上的力,图 5 表示物体在X方向上的力矩,图 6 表示物体在Y方向上的力矩,图7表示物体在Z方向上的力矩。

在未对力反馈数据做任何处理时,由于末端机械手重力的影响,在空载情况下,即在末端机械手不与外界物体接触的情况下,六维力传感器传输的力反馈数据不为 0,误差较大,误差范围在 0～6 N之间。其中,X方向上力的误差范围在0～3 N之间,Y方向上力的误差范围在 0～2 N之间,Z方向上力的误差范围在0～6 N之间,X方向上力矩的误差范围在 0～1 N·m之间,Y方向上力矩的误差范围在 0～1 N·m之间,Z方向上力矩的误差范围在 0～1 N·m之间。

在上述实验过程中,输出同一时刻经UKF算法补偿后的六维力传感器数据,实验结果如图8～13所示。其中,图8表示传感器在X方向上所受的力,图9表示传感器在Y方向上所受的力,图10表示传感器在Z方向上所受的力,图11表示传感器在X方向上的力矩,图12表示传感器在Y方向上的力矩,图13表示传感器在Z方向上的力矩。

图1 实验装置图

图2 X轴力

图3 Y轴力

图4 Z轴力

图5 X轴力矩

图6 Y轴力矩

图7 Z轴力矩

图8 X轴受力

图9 Y轴受力

图10 Z轴受力

图11 X轴力矩

图12 Y轴力矩

图13 Z轴力矩

从图 8～13 中可以看出,利用 UKF 算法补偿处理后得到的力反馈数据的结果误差更小,接近于 0,误差范围在 0～0.175 N 之间,其中X方向上力的误差范围在0～0.162 N 之间,Y方向上力的误差范围在0～0.160 N 之间,Z方向上力的误差范围在0～0.227 N 之间,X方向上力矩的误差范围在0～0.010 N·m之间,Y方向上力矩的误差范围在0～0.010 N·m 之间,Z方向上力矩的误差范围在0～0.039 N·m之间。

从图 2～13 中可以看出:利用 UKF 算法处理后的数据结果偏差更小、更稳定。这表明 UKF 算法在处理六维力传感器数据时具有更高的精度和更好的鲁棒性,可以有效提高数据的准确性和可靠性。

4 结论

本文针对由机器人末端工具带来的重力影响导致六维力传感器数据不准确的问题,提出利用UKF算法对数据进行补偿处理。本文通过实验验证说明使用 UKF 算法处理后的力反馈数据误差更小、更平滑,偏差更小、更稳定。这表明UKF算法在处理六维力传感器数据时,能够有效提高数据的准确性和可靠性。本文通过实验验证了 UKF 算法在处理六维力传感器数据时的优越性。在未来研究中,我们可以进一步探索 UKF 算法在其他机器人中的应用,例如,机器人的姿态估计、轨迹跟踪等领域。同时,我们也可以探索使用其他更为先进的算法对六维力传感器数据进行处理和应用,以期进一步提高机器人系统的性能和精度。