基于多传感器数据融合的手部空间姿态追踪补偿方法与流程

摘要: 本发明涉及手部姿态解算领域,特别是基于多传感器数据融合的手部空间姿态追踪补偿方法。背景技术:1、在虚拟现实、遥操作机器人控制、精细化工业装配及医疗康复领域,捕捉...
基于多传感器数据融合的手部空间姿态追踪补偿方法与流程

本发明涉及手部姿态解算领域,特别是基于多传感器数据融合的手部空间姿态追踪补偿方法。

背景技术:

1、在虚拟现实、遥操作机器人控制、精细化工业装配及医疗康复领域,捕捉高精度的手部空间三维位姿具有重要价值。现有的利用手部电磁可穿戴设备的捕捉方案中,电磁跟踪技术能够提供无遮挡的全向追踪,但在复杂的应用环境(如存在金属结构或外部强磁场干扰)中,电磁接收模块极易受到干扰,导致解算出的三维坐标与空间姿态发生低频漂移,严重影响系统的长期稳定性。

2、现有的补偿方案多采用惯性测量单元(imu)进行数据融合,但由于imu自身存在固有的积分累积误差,且无法感知手部与外部环境的物理交互边界。在实际操作中,手部执行抓取、按压等精细动作时,指尖常与刚性物体表面发生稳定接触。现有技术未能有效提取并利用这一自然交互过程中的“物理空间不变性约束”来对系统的全局位姿漂移进行闭环修正。

技术实现思路

1、本发明的主要目的是提出一种基于多传感器数据融合的手部空间姿态追踪补偿方法,能够有效消除低频累积漂移,拓展适用边界,减少对计算资源的占用。

2、本发明通过以下技术方案实现:

3、基于多传感器数据融合的手部空间姿态追踪补偿方法,包括如下步骤:

4、步骤s1、手部穿戴具有电磁接收模块和各指尖贴合面覆盖矩阵压力传感器的可穿戴设备,同步采集电磁接收模块的全局位姿数据与矩阵压力传感器的压力分布数据,并对电磁接收模块与矩阵压力传感器之间的空间位移偏置进行补偿标定,得到指尖在全局坐标系下的名义位置,其中,指尖指矩阵压力传感器中心;

5、步骤s2、持续根据压力分布数据计算对应指尖的总接触力和压力中心轨迹,在末端为当前时刻的滑动时间窗口内计算压力中心轨迹的位置方差,根据总接触力和位置方差判断当前时刻手指为接触状态或者无接触状态,接触状态分为完全刚性接触状态或者指尖滚动接触状态;

6、步骤s3、构建eskf模型,以指尖在全局坐标系下的三维位置误差、三维速度误差和三维姿态角误差、电磁低频位置漂移偏置和电磁姿态漂移偏置作为eskf模型的误差状态向量,接触状态时,eskf模型进行状态预测、物理约束观测更新,物理约束观测更新包括针对完全刚性接触状态构建零位移观测方程、针对指尖滚动接触状态构建微位移补偿观测方程;

7、步骤s4、根据eskf模型输出的误差状态向量,更新真实指尖空间位置、电磁低频位置漂移偏置,并在流行空间内完成姿态四元数更新,实现姿态追踪。

8、进一步的,所述步骤s1中,所述指尖在全局坐标系下的名义位置表示为,其中,为定义的全局坐标系,为定义的电磁接收模块局部坐标系,为电磁接收模块在全局坐标系下的名义测量位置,为电磁接收模块的姿态旋转矩阵,为已知的电磁接收模块中心到矩阵压力传感器中心的空间位移偏置向量。

9、进一步的,所述步骤s2中,当前时刻的总接触力表示为,压力中心在矩阵压力传感器坐标系下的二维坐标表示为,所述压力中心轨迹的位置方差表示为,其中,为矩阵压力传感器的阵列大小,为矩阵压力传感器阵列中第行、第列的传感单元在当前时刻的压力读数,,,为第行、第列的传感单元在矩阵压力传感器坐标系下的具体物理坐标,为滑动时间窗口长度,为滑动时间窗口内压力中心在矩阵压力传感器坐标系下的二维坐标的时间平均值,表示向量的二范数的平方。

10、进一步的,所述步骤s2中,若且时,判定当前时刻手指为完全刚性接触状态,若但且变化平滑,判定当前时刻手指为指尖滚动接触状态,若或者变化不平滑,判定当前时刻手指为未接触状态,其中,的取值范围为矩阵压力传感器满量程输出的1%至5%,的取值范围为0.5 mm2至4.0mm2,变化平滑指压力中心速度的一阶范数小于设定的速度阈值,且加速度的二阶范数小于设定的平滑阈值。

11、进一步的,所述步骤s3中,所述零位移观测方程表示为,其中,表示零位移观测残差,和分别为在当前时刻和前一时刻,指尖在全局坐标系下的名义位置,为在当前时刻的观测雅可比矩阵,为误差状态向量,为受压力传感器和电磁传感器精度限制的观测噪声向量。

12、进一步的,所述步骤s3中,所述微位移补偿观测方程表示为,其中,为微位移补偿观测残差,,为当前时刻相对于前一时刻、压力中心在矩阵压力传感器坐标系下的微位移,表示为。

13、进一步的,所述步骤s3中,所述eskf模型的状态预测方程为,所述eskf模型的误差状态更新方程为,所述eskf模型的后验误差协方差矩阵更新方程为,其中,为由上一时刻推算至当前时刻的先验误差协方差矩阵,为上一时刻的后验误差协方差矩阵,为已知的将上一时刻的误差状态线性映射至当前时刻的雅可比矩阵,为已知的包含时间步长与单位矩阵的常数稀疏矩阵,为根据电磁接收模块进行标定得到的过程噪声协方差矩阵,为eskf模型输出的误差状态向量,为卡尔曼增益矩阵,为观测残差,对于完全刚性接触状态,,对于滚动接触状态,,为eskf模型输出的后验误差协方差矩阵,为测量噪声协方差矩阵,为单位矩阵。

14、进一步的,所述步骤s4中,所述真实指尖空间位置表示为,所述电磁低频位置漂移偏置表示为,更新后的姿态四元数表示为,其中,、、分别为eskf模型输出的误差状态向量中的指尖在全局坐标系下的三维位置误差、电磁低频位置漂移偏置和电磁姿态漂移偏置,为更新前的电磁低频位置漂移偏置,为电磁传感器模块的名义姿态四元数,为流形空间下的四元数乘法运算符。

15、进一步的,所述步骤s1中,所述可穿戴设备为交互手套,交互手套的手背位置或者指节位置设置电磁发射模块,交互手套的各指尖背面设置电磁接收模块,交互手套的各指尖贴合面设置矩阵压力传感器。

16、进一步的,判定为非接触状态时,停止执行物理约束观测更新,同时持续利用上一接触阶段得到的电磁低频位置漂移偏置对指尖在全局坐标系下的名义位置进行开环修正,利用上一接触阶段估计的电磁姿态漂移偏置对名义姿态四元数进行补偿,得到去漂移后的姿态。

17、由上述对本发明的描述可知,与现有技术相比,本发明具有如下有益效果:

18、本发明打破了单一电磁追踪在磁场畸变区无法自愈的局限,有效消除低频累积漂移,抗干扰与自修复能力强;解决了现有方法的手指在接触但未完全静止(如指尖滚动)状态下的算法失效问题,实现高精度微动补偿,大幅拓展了本发明的适用边界;采用eskf(误差状态卡尔曼滤波)模型进行状态预测、物理约束观测更新,状态矩阵维度精简,避免了复杂的全局非线性优化,减少了对计算资源的占用,保障了姿态追踪的实时性。

1.基于多传感器数据融合的手部空间姿态追踪补偿方法,其特征在于:包括如下步骤:

2.根据权利要求1所述的基于多传感器数据融合的手部空间姿态追踪补偿方法,其特征在于:所述步骤s1中,所述指尖在全局坐标系下的名义位置表示为,其中,为定义的全局坐标系,为定义的电磁接收模块局部坐标系,为电磁接收模块在全局坐标系下的名义测量位置,为电磁接收模块的姿态旋转矩阵,为已知的电磁接收模块中心到矩阵压力传感器中心的空间位移偏置向量。

3.根据权利要求2所述的基于多传感器数据融合的手部空间姿态追踪补偿方法,其特征在于:所述步骤s2中,当前时刻的总接触力表示为,压力中心在矩阵压力传感器坐标系下的二维坐标表示为,所述压力中心轨迹的位置方差表示为,其中,为矩阵压力传感器的阵列大小,为矩阵压力传感器阵列中第行、第列的传感单元在当前时刻的压力读数,,,为第行、第列的传感单元在矩阵压力传感器坐标系下的具体物理坐标,为滑动时间窗口长度,为滑动时间窗口内压力中心在矩阵压力传感器坐标系下的二维坐标的时间平均值,表示向量的二范数的平方。

4.根据权利要求3所述的基于多传感器数据融合的手部空间姿态追踪补偿方法,其特征在于:所述步骤s2中,若且时,判定当前时刻手指为完全刚性接触状态,若但且变化平滑,判定当前时刻手指为指尖滚动接触状态,若或者变化不平滑,判定当前时刻手指为未接触状态,其中,的取值范围为矩阵压力传感器满量程输出的1%至5%,的取值范围为0.5 mm2至4.0mm2,变化平滑指压力中心速度的一阶范数小于设定的速度阈值,且加速度的二阶范数小于设定的平滑阈值。

5.根据权利要求4所述的基于多传感器数据融合的手部空间姿态追踪补偿方法,其特征在于:所述步骤s3中,所述零位移观测方程表示为,其中,表示零位移观测残差,和分别为在当前时刻和前一时刻,指尖在全局坐标系下的名义位置,为在当前时刻的观测雅可比矩阵,为误差状态向量,为受压力传感器和电磁传感器精度限制的观测噪声向量。

6.根据权利要求5所述的基于多传感器数据融合的手部空间姿态追踪补偿方法,其特征在于:所述步骤s3中,所述微位移补偿观测方程表示为,其中,为微位移补偿观测残差,,为当前时刻相对于前一时刻、压力中心在矩阵压力传感器坐标系下的微位移,表示为。

7.根据权利要求6所述的基于多传感器数据融合的手部空间姿态追踪补偿方法,其特征在于:所述步骤s3中,所述eskf模型的状态预测方程为,所述eskf模型的误差状态更新方程为,所述eskf模型的后验误差协方差矩阵更新方程为,其中,为由上一时刻推算至当前时刻的先验误差协方差矩阵,为上一时刻的后验误差协方差矩阵,为已知的将上一时刻的误差状态线性映射至当前时刻的雅可比矩阵,为已知的包含时间步长与单位矩阵的常数稀疏矩阵,为根据电磁接收模块进行标定得到的过程噪声协方差矩阵,为eskf模型输出的误差状态向量,为卡尔曼增益矩阵,为观测残差,对于完全刚性接触状态,,对于滚动接触状态,,为eskf模型输出的后验误差协方差矩阵,为测量噪声协方差矩阵,为单位矩阵。

8.根据权利要求7所述的基于多传感器数据融合的手部空间姿态追踪补偿方法,其特征在于:所述步骤s4中,所述真实指尖空间位置表示为,所述电磁低频位置漂移偏置表示为,更新后的姿态四元数表示为,其中,、、分别为eskf模型输出的误差状态向量中的指尖在全局坐标系下的三维位置误差、电磁低频位置漂移偏置和电磁姿态漂移偏置,为更新前的电磁低频位置漂移偏置,为电磁传感器模块的名义姿态四元数,为流形空间下的四元数乘法运算符。

9.根据权利要求1至8任一所述的基于多传感器数据融合的手部空间姿态追踪补偿方法,其特征在于:所述步骤s1中,所述可穿戴设备为交互手套,交互手套的手背位置或者指节位置设置电磁发射模块,交互手套的各指尖背面设置电磁接收模块,交互手套的各指尖贴合面设置矩阵压力传感器。

10.根据权利要求1至8任一所述的基于多传感器数据融合的手部空间姿态追踪补偿方法,其特征在于:判定为非接触状态时,停止执行物理约束观测更新,同时持续利用上一接触阶段得到的电磁低频位置漂移偏置对指尖在全局坐标系下的名义位置进行开环修正,利用上一接触阶段估计的电磁姿态漂移偏置对名义姿态四元数进行补偿,得到去漂移后的姿态。