[发明专利]一种增强现实一体机手柄跟踪算法

权利要求书

1.一种增强现实一体机手柄跟踪算法，其特征在于：其算法包括以下步骤：

A、将头盔IMU数据和位姿以及手柄MIU预积分数据均输入到头盔四目图像信息内；

B、对数据检测和处理后数据同步；

C、手柄光点检测并判断是否有初始位姿；

D、没有，则以两个光点做基遍历第三个光点，并筛选正确的匹配，同时，根据匹配解算位姿判断光点数量是否3；

E、当光点数量4时，采用位姿解算法1，其他情况，则采用位姿解算法2；

F、通过位姿解算法1和位姿解算法2筛选解算位姿，并在结束后进行重力筛选，当有3帧满足重力约束条件时，则冷启动成功；

G、当手柄光点检测并判断有初始位姿时，则判断状态是否稳定；

H、不稳定，则采用blob identify操作，并获取手柄3DOF信息后进行筛选位姿，且在筛选位姿成功后进行热启动；

I、当手柄光点检测并判断有初始位姿，且初始位姿状态稳定时进行位姿预测操作；

J、对极约束删除外点并采用t-PNP方式剔除外点，最后进行光电确认操作；

K、当匹配数量4时，采用求解旋转和平移方式；

L、当匹配数量不大于4时，采用固定旋转，只求解平移的方式；

M、计算视觉协方差并计算视觉协方差的熵，最后对视觉协方差发散验证和修正；

N、采用IMU Propagation方式获取Propagation结果和协方差，且获取Propagation结果和协方差以及获取视觉观测结果和协方差均进行ESKF融合(Error State KalmanFilter,误差状态卡尔曼滤波器)，并在状态更新后结束。

2.根据权利要求1所述的一种增强现实一体机手柄跟踪算法，其特征在于：所述当没有3帧满足重力约束条件时，则继续对手柄光点检测并判断是否有初始位姿。

3.根据权利要求1所述的一种增强现实一体机手柄跟踪算法，其特征在于：所述热启动失败后则进行冷启动，且热启动成功后进行位姿预测操作。

4.根据权利要求1所述的一种增强现实一体机手柄跟踪算法，其特征在于：所述状态更新后判断是否进入ESKF收敛状态，当进入ESKF收敛状态时，进行3DOF更新，当未进入ESKF收敛状态时，对纯旋转陀螺仪进行积分计算后结束。

5.根据权利要求1所述的一种增强现实一体机手柄跟踪算法，其特征在于：所述每一帧预积分数据的定义如下：

ω_m＝(ω₀+ω₁)×0.5

a_m＝(a₀+a₁)×0.5

△R＝△R×Exp(ω_m×△t)

△v＝△v+△R×a_m×△t

其中，ω₀,a₀,ω₁,a₁分别是前后两帧1000Hz的IMU的陀螺仪和加速度计的输出，△R和dv分别为预积分的旋转和平移结果，预积分的帧数目前可以设定为240Hz(4帧IMU输出做一次预积分)，180Hz(6帧IMU输出做一次预积分)和120Hz(8帧IMU数据做一次预积分)或者其他40的倍数，预积分数据的优势是可以克服原始数据难以在合法信道以高频率(300Hz)在手柄和头盔之间传输数据的弱点。