一种混合现实设备空间定位与位姿校正方法与流程

本发明涉及混合现实设备定位，具体涉及一种混合现实设备空间定位与位姿校正方法。

背景技术：

1、混合现实设备在室内环境中实现高精度空间定位和姿态跟踪是实现沉浸式交互体验的关键基础。目前主流方案采用视觉惯性里程计融合方法，即通过摄像头采集环境图像进行特征点匹配以估计运动轨迹，同时结合惯性测量单元提供的加速度和角速度信息进行短时积分补充。

2、以vins-mono、orb-slam3和msckf为代表的现代方案已普遍采用基于信息矩阵的自适应加权机制、滑动窗口非线性优化以及紧耦合融合架构，其融合权重本质上由协方差矩阵的逆决定，能够在视觉特征丰富时自动强化视觉贡献，在特征退化时自动降低视觉权重。

3、然而，上述系统均未引入电磁定位作为绝对参考源，且其自适应调整主要围绕视觉重投影误差和imu残差展开，尚未建立将视觉特征匹配内点率与惯性预积分残差纳入同一量纲的综合置信度评估体系，当环境干扰导致视觉匹配内点率下降或惯性预积分残差增大时，现有系统因缺乏能够同时响应多传感器置信度变化并动态调整补偿阈值的闭环调控机制，其融合结果在极端工况下仍可能出现精度退化。

4、此外，现有位姿校正方法通常基于预设误差模型进行补偿，缺乏对当前定位质量的动态评估和自适应调整能力，当环境干扰导致视觉匹配内点率下降或惯性预积分残差增大时，校正参数不能及时响应，最终影响混合现实设备在复杂动态环境中的长期稳定性和定位精度。

技术实现思路

1、为此，本发明提供一种混合现实设备空间定位与位姿校正方法，以解决现有技术中缺乏对当前定位质量的动态评估和自适应调整能力，当环境干扰导致视觉匹配内点率下降或惯性预积分残差增大时，校正参数不能及时响应，最终影响混合现实设备在复杂动态环境中的长期稳定性和定位精度的问题。

2、为了实现上述目的，本发明提供如下技术方案：

3、一种混合现实设备空间定位与位姿校正方法，包括以下步骤：

4、步骤s1，初始化设备参数，获取设备初始位置坐标和初始姿态角，设定视觉惯性融合权重初始值、误差补偿阈值和置信度判定门限；

5、步骤s2，多源数据同步采集，通过视觉传感器采集环境图像序列，通过惯性测量单元采集三轴加速度和三轴角速度，通过电磁定位模块采集电磁场强度数据；

6、步骤s3，视觉特征检测与匹配，对采集到的环境图像进行fast角点检测和brief描述子提取，将当前帧图像的特征点与前一帧图像的特征点进行汉明距离匹配，得到匹配点对集合；

7、步骤s4，惯性预积分，将惯性测量单元采集的角速度和加速度在相邻图像帧时间间隔内进行预积分，得到设备在局部坐标系下的相对旋转矩阵、相对平移向量和速度变化量；

8、步骤s5，融合位姿估计，根据匹配点对集合计算本质矩阵，分解得到视觉位姿变化量，将视觉位姿变化量与惯性预积分得到的相对位姿进行加权融合，得到初步估计位姿；

9、步骤s6，误差补偿量计算，根据电磁定位模块采集的电磁场强度数据代入基于毕奥-萨伐尔定律建立的解析磁场模型进行位置反算，得到磁场定位偏差向量，计算偏差向量的模长并与所述误差补偿阈值比较，若模长小于等于阈值则按比例补偿，若模长大于阈值则缩减补偿系数后进行抑制补偿，得到补偿后位姿；

10、步骤s7，位姿校正，将补偿后位姿与上一时刻校正位姿进行卡尔曼滤波更新，得到当前时刻校正位姿，并更新设备的空间位置坐标和姿态角；

11、步骤s8，置信度评估，根据当前时刻校正位姿的协方差矩阵、视觉特征匹配内点率和惯性预积分残差，计算当前位姿置信度；

12、步骤s9，自适应参数调整，将当前位姿置信度与所述置信度判定门限比较，若置信度低于门限，则增大视觉惯性融合权重中的视觉权重，并减小误差补偿阈值，返回步骤s2继续执行，否则输出当前时刻校正位姿作为最终定位结果。

13、优选地，所述步骤s2中，视觉传感器为双目相机或深度相机，惯性测量单元为六轴imu，电磁定位模块包括至少两个空间位置已知的低频交变磁场发生器以及固定于设备壳体上的三轴磁力计阵列，所述各低频交变磁场发生器以时分复用方式交替发射交变磁场信号，或各发生器分别以互不相同的频率连续发射；所述三轴磁力计阵列由至少三个三轴磁力计组成，各磁力计以非共线方式分布于设备壳体上的不同安装位置，任意两个磁力计之间的空间距离为已知固定值；所述多源数据同步采集采用硬件时钟同步方式，将视觉传感器曝光时刻、惯性测量单元采样时刻和电磁定位模块采样时刻统一至全局时间基准，同步时间误差小于1毫秒；对于每个图像帧曝光时刻，通过线性插值获取与该时刻严格对齐的惯性测量值和各磁力计的磁场测量值，插值所使用的邻近采样点数量为前后各两个。

14、优选地，所述步骤s3中，fast角点检测的检测阈值设置为15至25，brief描述子长度为256比特；汉明距离匹配时，设定匹配距离阈值，仅保留汉明距离小于匹配距离阈值的匹配点对，所述匹配距离阈值等于描述子长度乘以0.3；对于匹配点对集合，采用随机采样一致性算法剔除误匹配点对，随机采样一致性算法的迭代次数设置为200次，根据双目极线几何约束计算内点，得到内点匹配集合，并计算内点率，所述内点率为内点数量与匹配点对总数量的比值。

15、优选地，所述步骤s5中，视觉位姿变化量包括视觉旋转矩阵r_v和视觉平移向量t_v，惯性预积分得到的相对旋转矩阵r_i和相对平移向量t_i；加权融合的计算方式为：将视觉旋转矩阵和惯性预积分旋转矩阵分别转换为四元数表示，分别记为q_v和q_i，然后进行球面线性插值，插值系数为所述视觉惯性融合权重中的视觉权重α，惯性权重为1-α，得到融合旋转四元数q_f；融合平移向量采用线性加权：t_f=α×t_v+(1-α)×t_i，其中α取值范围为0.4至0.8，初始值设定为0.6；所述初步估计位姿包括由q_f转换得到的融合旋转矩阵以及由t_f确定的融合平移向量。

16、优选地，所述步骤s6中，磁场定位偏差的计算方式为：将当前磁场强度数据代入所述解析磁场模型进行位置反算，所述解析磁场模型基于毕奥-萨伐尔定律建立，根据各低频交变磁场发生器的空间坐标、线圈匝数、线圈半径和激励电流强度，计算空间中任意位置处的理论磁场矢量，通过将实测磁场矢量与理论磁场矢量进行最小误差匹配，得到位置坐标估计值p_m；将初步估计位姿中的位置坐标p_e与p_m做差，得到偏差向量δp=p_m-p_e；误差补偿采用比例补偿方式，补偿后位置p_c=p_e+β×δp，其中β为补偿系数，取值范围为0.5至0.9，初始值设定为0.7；补偿后姿态的调整方式为：根据磁场强度数据计算当前磁场方向，将当前磁场方向与预设基准方向的角度差值乘以0.1作为偏航角微调量，叠加至初步估计位姿中的偏航角，俯仰角和横滚角保持不变，从而得到补偿后位姿。

17、优选地，所述步骤s7中，卡尔曼滤波更新采用扩展卡尔曼滤波器，状态向量包括三维位置、三维姿态角、三维速度和三维角速度偏差，共12维；预测步骤利用惯性预积分得到的相对旋转矩阵、相对平移向量和速度变化量进行状态预测，并更新状态协方差矩阵；更新步骤将补偿后位姿中的位置坐标和姿态角作为观测值，计算卡尔曼增益，进行测量更新，得到当前时刻校正位姿；滤波过程中，当补偿后位姿与预测位姿的位置差异大于2倍于当前误差补偿阈值t或姿态角差异大于0.1弧度时，将补偿后位姿直接作为当前时刻校正位姿，并将滤波器协方差矩阵重置为初始协方差矩阵的1.5倍。

18、优选地，所述步骤s8中，位姿置信度c的计算公式为：c=w1×(1-trace(p)/trace(p0))+w2×r_in+w3×(1-res/res0)，其中p为当前时刻校正位姿的协方差矩阵，p0为初始协方差矩阵，trace表示矩阵的迹，r_in为视觉特征匹配内点率，res为惯性预积分残差，所述惯性预积分残差定义为惯性预积分得到的相对平移向量与视觉位姿变化量中的平移向量之间的差值范数，res0为预设残差基准值，w1、w2、w3为预设权重系数，w1+w2+w3=1，且w1取0.4，w2取0.3，w3取0.3；当c大于等于0.8时判定为高置信度，当c介于0.5和0.8之间时判定为中置信度，当c小于0.5时判定为低置信度。

19、优选地，所述步骤s9中，增大视觉惯性融合权重中的视觉权重的方式为：将视觉权重α更新为α_new=min(α+δ,0.95)，其中δ为步长增量，δ=0.05×(1-c)；减小误差补偿阈值的方式为：将补偿阈值t更新为t_new=t×(1-0.1×(1-c))，减小的t值将直接用于步骤s6中偏差向量模长的比较判定，阈值越小，对电磁定位补偿量的准入越严格，抑制补偿越早介入，从而在低置信度时有效遏制磁场异常引入的定位跳变；当置信度c大于等于0.8时，不进行调整；当置信度c介于0.5和0.8之间时，仅增大视觉权重而不调整补偿阈值；当置信度c小于0.5时，同时增大视觉权重并减小误差补偿阈值，并将调整后的参数用于下一轮迭代。

20、优选地，所述步骤s9中，若连续三次迭代后置信度仍低于0.5，则触发重定位流程：清空视觉特征匹配历史缓冲，重新执行步骤s3至步骤s7，并将视觉惯性融合权重中的视觉权重α临时减小至0.2，同时将补偿系数β临时增大至0.95，重定位期间重新提取的视觉特征仅用于与预设关键帧数据库的匹配尝试，不参与位姿融合，直至连续两帧图像的内点率回升至0.6以上，判定视觉观测恢复可信，然后逐步恢复α和β至初始值，所述初始值为α_0=0.6，β_0=0.7，恢复过程采用线性递增递减，每迭代一次恢复0.05；若重定位流程执行超过50帧内点率仍未回升至0.6，则持续保持α=0.2、β=0.95的配置，仅依靠惯性积分和电磁定位输出位姿，并在设备端输出定位降级提示信号；重定位流程触发期间，当前时刻校正位姿采用如下方式确定：当内点率低于0.6时，直接采用补偿后位姿，不进行卡尔曼滤波更新；当内点率回升至0.6及以上时，恢复正常卡尔曼滤波更新。

21、本发明具有如下优点：本发明通过多源数据同步采集、视觉惯性融合估计、电磁辅助误差补偿、位姿置信度评估以及自适应参数调整，实现高鲁棒性和高精度的空间定位与位姿校正。该方法包括初始化、多源数据同步采集、视觉特征检测与匹配、惯性预积分、融合位姿估计、误差补偿量计算、位姿校正、置信度评估和自适应参数调整九个核心步骤，能够根据实时置信度动态调整融合权重和补偿阈值，有效抑制单一传感器失效时的定位漂移，显著提升混合现实设备在复杂场景下的定位连续性和准确性；

22、通过多源传感器同步采集和分层融合策略，有效克服了单一视觉惯性系统在纹理稀疏或快速运动场景下的定位退化问题，电磁辅助补偿环节提供了绝对位置参考，能够显著抑制长时间运行中的累积漂移，使定位精度在室内复杂环境下保持在厘米级；引入基于协方差迹、内点率和预积分残差的综合置信度评估机制，能够实时量化当前位姿估计的可信程度，为后续自适应调整提供了客观判据，避免了固定权重融合在传感器性能波动时导致的性能劣化；

23、通过自适应参数调整和重定位流程，实现了融合权重、补偿阈值和补偿系数的动态优化，当置信度降低时自动增强视觉贡献并收紧误差容许范围，有效提升了系统对光照变化、遮挡和电磁干扰等突发情况的响应能力，保证了混合现实设备在长时间连续使用过程中的定位稳定性和用户体验一致性。此外，本发明所采用的算法计算开销小，易于在嵌入式移动平台上实时实现，具有较高的工程实用价值。