智能手机单目和imu融合的稳定运动跟踪方法和装置的制造方法
【专利摘要】本发明公开了一种智能手机单目和IMU融合的稳定运动跟踪方法和装置，属于AR/VR运动跟踪技术领域，所述方法包括：利用ORB算法对获取的图像进行处理，之后进行3D重构，得到初始地图点，完成地图初始化；使用ORB算法实时匹配和并行局部关键帧建图的方式进行视觉跟踪，得到视觉位姿；获取IMU在三维空间产生的加速度和角速度值，并对加速度和角速度值进行积分运算，得到IMU位姿预测结果；对视觉位姿和IMU位姿预测结果进行卡尔曼融合，依据融合后得到的位姿信息进行运动跟踪。与现有技术相比，本发明具有能够获取更加稳定的运动跟踪方式，并实现尺度的实时在线估计的特点。
【专利说明】
智能手机单目和IMU融合的稳定运动跟踪方法和装置
技术领域
[0001] 本发明设及移动通信领域，特别是指一种智能手机单目和IMU融合的稳定运动跟 踪方法和装置。
【背景技术】
[0002] 随着VR技术的发展，利用先进的运动跟踪技术是其应用的先决技术条件之一，在 此技术基础上可W实现更好的交互和更佳的沉浸感。当前移动VR主要是使用手柄来进行交 互，在交互过程中仅仅使用手机的巧螺仪来进行旋转跟踪，由于手机巧螺仪本身的偏差和 噪音的影响，导致旋转估计不准，重复精度较差；当用户坐着站起来向前移动时，如果不使 用手柄交互的话，虚拟场景保持固定不动，像什么也没发生一样，交互体验不好;而当用户 坐着沉浸在虚拟环境中，下意识站起来尝试移动，虚拟场景没有发生任何变化，沉浸感消 失。
[0003] 运动跟踪技术是要测量、跟踪、记录物体在=维空间中的运动轨迹，其主要是通过 传感器技术获取运动场景的信息，并实时计算得到被跟踪物体在空间的姿态，被广泛应用 于机器人导航、无人机导航和无人车自动驾驶导航等领域。2004年Nister首次提出视觉里 程计(Visual Odometry,VO)的概念W来，基于视觉里程计的方法已成为实时姿态估计和运 动跟踪的主流。它通过估计相机在空间的增量运动，在时间空间确定相机的运动轨迹。而视 觉惯性里程计(ViSiia 1 IMU Odome化y，VI0)融合了相机和惯性传感器的信息，主要是巧螺 仪和加速度计，给出了一个优势互补的方案。例如，一个单一的相机可W估计相对位置，但 它不能提供绝对尺度，无法得到物体大小或两个物体之间的实际距离，且摄像头采样帖率 一般较低且图像传感器的噪音相对较大，使得其在运动跟踪的过程中对环境的适应性较 差。惯性传感器可W提供绝对尺度，并W较高的采样频率测量，从而提高了设备移动快速时 的鲁棒性。然而，自带的低成本惯性传感器，相比基于相机的位置估计容易出现较大的漂 移，无法实现稳定的运动跟踪。
【发明内容】
[0004] 本发明要解决的技术问题是提供一种能够获取更加稳定的运动跟踪方式，并实现 尺度的实时在线估计的智能手机单目和IMU融合的稳定运动跟踪方法和装置。
[0005] 为解决上述技术问题，本发明提供技术方案如下：
[0006] -种智能手机单目和IMU融合的稳定运动跟踪方法，包括：
[0007] 利用ORB算法对获取的图像进行处理，之后进行3D重构，得到初始地图点，完成地 图初始化；
[000引使用ORB算法实时匹配和并行局部关键帖建图的方式进行视觉跟踪，得到视觉位 姿；
[0009]获取IMU在S维空间产生的加速度和角速度值，并对加速度和角速度值进行积分 运算，得到IMU位姿预测结果；
[0010] 对视觉位姿和IMU位姿预测结果进行卡尔曼融合，依据融合后得到的位姿信息进 行运动跟踪。
[0011] 进一步的，所述利用ORB算法对获取的图像进行处理，之后进行3D重构，得到初始 地图点，完成地图初始化包括：
[0012] 对获取的第一帖图像使用ORB算法提取特征点并计算描述子，将第一帖记为关键 帖，标记相机的绝对位姿；
[0013] 相机平移一端距离后，再对对获取的图像采用ORB算法提取特征点并计算描述子， 与第一帖图像特征点进行匹配，将第二帖记为关键帖，并计算出第二帖下相机相对于第一 帖的相对位姿；
[0014] 对匹配成功的特征点集进行3D重构，得到初始地图点。
[0015] 进一步的，所述计算出第二帖下相机相对于第一帖的相对位姿包括：
[0016] 根据第一帖和第二帖图像上的对应匹配特征点集，计算出两帖图像间的基础矩 阵；
[0017] 根据基础矩阵和相机的内参数，计算得到本质矩阵；
[0018] 对本质矩阵采用奇异值分解，得到所述第二帖下相机相对于第一帖的相对位姿。
[0019] 进一步的，所述使用ORB算法实时匹配和并行局部关键帖建图的方式进行视觉跟 踪，得到视觉位姿包括：
[0020] 对图像的当前帖采用ORB算法栅格化提取图像特征点和计算描述子；
[0021] 采用恒定速度运动模型，估计当前帖对应相机位姿，将上一帖图像所有地图点投 影到当前图像帖上，进行特征点匹配，并将匹配成功的上一帖地图点赋值给当前帖相应的 特征点；
[0022] 采用LM算法和化ber估计更新当前帖位姿和当前帖地图点；
[0023] 根据更新后的位姿，将局部关键帖所有地图点投影到当前图像帖上，并进行特征 点匹配，匹配成功后，将匹配成功的所有地图点赋值给当前帖相应的特征点，并使用LM算法 和化ber估计重新更新当前帖位姿和当前帖地图点。
[0024] 进一步的，所述使用ORB算法实时匹配和并行局部关键帖建图的方式进行视觉跟 踪，得到视觉位姿还包括：
[0025] 根据时间间隔情况和/或当前帖地图点个数来判断是否需要增加关键帖，如果离 上次增加关键帖超过一定时间后或者当前帖的地图点数小于阔值，则增加新的关键帖；
[0026] 判断当前帖是否为新的关键帖，如果是，则增加新的地图点，将新的关键帖无地图 点的所有特征点与局部关键帖中的所有特征点进行特征点匹配，匹配成功后3D重构得到新 的地图点；
[0027] 局部光束平差优化，修正累积误差，得到优化后的位姿和地图点。
[00%] -种智能手机单目和IMU融合的稳定运动跟踪装置，包括：
[0029] 地图初始化模块，用于利用ORB算法对获取的图像进行处理，之后进行3D重构，得 到初始地图点，完成地图初始化；
[0030] 视觉跟踪模块，用于使用ORB算法实时匹配和并行局部关键帖建图的方式进行视 觉跟踪，得到视觉位姿；
[0031] IMU位姿计算模块：用于获取IMU在S维空间产生的加速度和角速度值，并对加速 度和角速度值进行积分运算，得到IMU位姿预测结果；
[0032] 融合模块:用于对视觉位姿和IMU位姿预测结果进行卡尔曼融合，依据融合后得到 的位姿信息进行运动跟踪。
[0033] 进一步的，所述地图初始化模块还用于：
[0034] 对获取的第一帖图像使用ORB算法提取特征点并计算描述子，将第一帖记为关键 帖，标记相机的绝对位姿；
[0035] 相机平移一端距离后，再对对获取的图像采用ORB算法提取特征点并计算描述子， 与第一帖图像特征点进行匹配，将第二帖记为关键帖，并计算出第二帖下相机相对于第一 帖的相对位姿；
[0036] 对匹配成功的特征点集进行3D重构，得到初始地图点。
[0037] 进一步的，所述计算出第二帖下相机相对于第一帖的相对位姿包括：
[0038] 根据第一帖和第二帖图像上的对应匹配特征点集，计算出两帖图像间的基础矩 阵；
[0039] 根据基础矩阵和相机的内参数，计算得到本质矩阵；
[0040] 对本质矩阵采用奇异值分解，得到所述第二帖下相机相对于第一帖的相对位姿。 [0041 ]进一步的，所述视觉跟踪模块还用于：
[0042] 对图像的当前帖采用ORB算法栅格化提取图像特征点和计算描述子；
[0043] 采用恒定速度运动模型，估计当前帖对应相机位姿，将上一帖图像所有地图点投 影到当前图像帖上，进行特征点匹配，并将匹配成功的上一帖地图点赋值给当前帖相应的 特征点；
[0044] 采用LM算法和化ber估计更新当前帖位姿和当前帖地图点；
[0045] 根据更新后的位姿，将局部关键帖所有地图点投影到当前图像帖上，并进行特征 点匹配，匹配成功后，将匹配成功的所有地图点赋值给当前帖相应的特征点，并使用LM算法 和化ber估计重新更新当前帖位姿和当前帖地图点。
[0046] 进一步的，所述视觉跟踪模块还用于：
[0047] 根据时间间隔情况和/或当前帖地图点个数来判断是否需要增加关键帖，如果离 上次增加关键帖超过一定时间后或者当前帖的地图点数小于阔值，则增加新的关键帖；
[0048] 判断当前帖是否为新的关键帖，如果是，则增加新的地图点，将新的关键帖无地图 点的所有特征点与局部关键帖中的所有特征点进行特征点匹配，匹配成功后3D重构得到新 的地图点；
[0049] 局部光束平差优化，修正累积误差，得到优化后的位姿和地图点。
[0050] 本发明具有W下有益效果：
[0051] 本发明中，先对地图初始化，初始化成功后获取图像进行连续跟踪并进行位姿估 计；同时，获取IMU数据进行积分预测位姿;在扩展卡尔曼滤波化Xtended Kalman Filter, EKF)框架下进行数据融合得到稳定的位姿估计。针对当前移动VR的运动跟踪问题，本发明 W移动端自带的相机和IMU，通过使用VIO在EKF框架下结合视觉测量和惯性传感器测量，能 够准确地估计位姿和绝对尺度。实现移动VR的快速且稳定的运动跟踪方法。与现有技术相 比，本发明具有能够获取更加稳定的运动跟踪方式，并实现尺度的实时在线估计的特点。
【附图说明】
[0052] 图1为本发明的智能手机单目和IMU融合的稳定运动跟踪方法的流程示意图；
[0053] 图2为本发明的智能手机单目和IMU融合的稳定运动跟踪方法的视觉位姿估计流 程示意图；
[0054] 图3为本发明的智能手机单目和IMU融合的稳定运动跟踪方法的视觉位姿和IMU位 姿卡尔曼融合原理示意图；
[0055] 图4为本发明的智能手机单目和IMU融合的稳定运动跟踪方法的坐标系统示意图；
[0056] 图5为本发明的智能手机单目和IMU融合的稳定运动跟踪方法的单目视觉和IMU系 统不意图；
[0057] 图6为本发明的智能手机单目和IMU融合的稳定运动跟踪方法的技术方案总流程 框图；
[0058] 图7为本发明的智能手机单目和IMU融合的稳定运动跟踪装置的结构示意图。
【具体实施方式】
[0059] 为使本发明要解决的技术问题、技术方案和优点更加清楚，下面将结合附图及具 体实施例进行详细描述。
[0060] -方面，本发明提供一种智能手机单目和IMU融合的稳定运动跟踪方法，如图1所 示，包括：
[0061] 步骤SlOl:利用ORB算法对获取的图像进行处理，之后进行3D重构，得到初始地图 点，完成地图初始化；
[0062] 本步骤中，进行地图初始化的目的是构建初始的=维点云。由于不能仅仅从单个 帖得到深度信息，因此需要从图像序列中选取两帖或两帖W上的图像，估计相机姿态并重 建出初始的=维点云。本步骤中，采用两个关键帖，一个是初始的关键帖（初始帖），另一个 是运动一定角度后的关键帖（结束帖），对初始帖到结束帖之间进行关键点的匹配，然后对 匹配成功的特征点集进行3D重构，最后完成地图初始化。
[0063] 步骤S102:使用ORB算法实时匹配和并行局部关键帖建图的方式进行视觉跟踪，得 到视觉位姿；
[0064] 本步骤中，地图初始化成功后基于视觉进行运动跟踪。考虑到移动端较弱的计算 能力，使用ORB算法的实时匹配和位姿估计，W及并行局部关键帖的维护和建图的方式进行 视觉跟踪，进而得到视觉位姿。其中，ORB算法实时匹配和位姿估计为跟踪线程，局部关键帖 的维护和建图为局部关节帖线程。
[0065] 步骤S103:获取IMU在=维空间产生的加速度和角速度值，并对加速度和角速度值 进行积分运算，得到IMU位姿预测结果；
[0066] 本步骤中，设及的IMU(Ine;rtial measurement unit,简称IMU，惯性测量单元）是 测量物体=轴姿态角速度(或角速率)和加速度的装置。一般的，一个IMU包含了 =个单轴的 加速度计和=个单轴的巧螺，加速度计用于检测物体在载体坐标系统独立=轴的加速度信 号，巧螺用于检测载体相对于导航坐标系的角速度信号，本步骤中，IMU能在3个垂直轴方向 产生加速度和角速度值，进行积分对位姿预测，而移动设备内的单目视觉传感器可W提供 没有尺度的3D位置W及位姿的测量值。在前后相邻帖之间获取IMU数据进行位姿预测，后一 帖视觉位姿估计作为测量值进行更新。
[0067] 步骤S104:对视觉位姿和IMU位姿预测结果进行卡尔曼融合，依据融合后得到的位 姿信息进行运动跟踪；
[0068] 本步骤中，为获取稳定的跟踪位姿，充分利用视觉和IMU的传感器得到的信息，本 发明通过使用卡尔曼融合方法，将视觉图像得到的视觉位姿和IMU积分得到的位姿预测结 果进行融合，W实现两异类传感器的信息互补和目标状态估计，从而获取融合后更为准确、 可靠的位姿。进而，依据融合后的位姿信息进行运动跟踪。
[0069] 本发明中，先对地图初始化，初始化成功后获取图像进行连续跟踪并进行位姿估 计；同时，获取IMU数据进行积分预测位姿;在扩展卡尔曼滤波化Xtended Kalman Filter, EKF)框架下进行数据融合得到稳定的位姿估计。针对当前移动VR的运动跟踪问题，本发明 W移动端自带的相机和IMU，通过使用VIO在EKF框架下结合视觉测量和惯性传感器测量，能 够准确地估计位姿和绝对尺度。实现移动VR的快速且稳定的运动跟踪方法。与现有技术相 比，本发明具有能够获取更加稳定的运动跟踪方式，并实现尺度的实时在线估计的特点。
[0070] 作为本发明的一种改进，利用ORB算法对获取的图像进行处理，之后进行3D重构， 得到初始地图点，完成地图初始化包括：
[0071] 对获取的第一帖图像使用ORB算法提取特征点并计算描述子，将第一帖记为关键 帖，标记相机的绝对位姿；
[0072] 相机平移一端距离后，再对对获取的图像采用ORB算法提取特征点并计算描述子， 与第一帖图像特征点进行匹配，将第二帖记为关键帖，并计算出第二帖下相机相对于第一 帖的相对位姿；
[0073] 对匹配成功的特征点集进行3D重构，得到初始地图点。
[0074] 针对本发明的运种改进，本发明提供一种完整具体的实施例如下：
[007引1.采集第一帖图像使用具有局部不变性的特征算法（Oriented FAST and Rotated BRIEF,ORB)提取特征点并计算描述子，第一帖为关键帖，标记相机绝对位姿为 [R(o,k) I t(o,k)]，下标(0，k)表示第帥贞的绝对位姿，则[R(o,o) 11化0)] = [110];
[0076] 2.平移一段距离后，再采集图像使用ORB算法提取特征点并计算描述子。与第一帖 图像特征点匹配成功后，该帖也记为关键帖。并计算出第二帖下相机相对于第一帖的相对 位姿为[R化 1) I t(0,i)] = [R| t];
[0077] 3.匹配成功特征点集进行3D重构，得到初始地图点。
[0078] 本实施例中，采用ORB算法提取特征并直接匹配估计位姿，ORB算法是将FAST角点 检测与BRIEF特征描述结合一种算法改进，兼顾了单目视觉跟踪过程中的效率和精度。
[0079] 作为本发明的进一步改进，计算出第二帖下相机相对于第一帖的相对位姿包括：
[0080] 根据第一帖和第二帖图像上的对应匹配特征点集，计算出两帖图像间的基础矩 阵；
[0081 ]根据基础矩阵和相机的内参数，计算得到本质矩阵；
[0082] 对本质矩阵采用奇异值分解，得到第二帖下相机相对于第一帖的相对位姿。
[0083] 针对本发明的进一步改进，本发明提供的完整具体的实施例如下：
[0084] ①平移一段距离后，对第二帖图像使用ORB算法提取特征点并计算描述子，与第一 帖图像特征点匹配成功后，得到两关键帖上的对应匹配特征点集记为(Xl，Xr);
[0085] ②根据XlTFXr = O,计算出基础矩阵F;
[0086] ③由基础矩阵F和本质矩阵E的相互关系:E = KlTFKr,其中化l，Kr)分别相机的内参 数，该内参数可事先标定且Kl = Kr。得到本质矩阵E，本质矩阵只与相机的外参有关，而与相 机内参无关；
[0087] ④根据E=[t]xR，其中[t]x为平移量t = (tx，ty，tz)T的反对称矩阵，R为旋转矩阵。 对矩阵E利用奇异值分解（Singular化Iue Decomposition,SVD)，可W计算出R,t，则第二 帖下相机相对于第一帖的相对位姿为[R化1) I t(0,i)] = [R11]。
[0088] 本实施例在相机在运动过程中，可W依次获取每帖图片所对应的一系列相对位 姿。
[0089] 作为本发明的更进一步的改进，使用ORB算法实时匹配和并行局部关键帖建图的 方式进行视觉跟踪，得到视觉位姿包括：
[0090] 对图像的当前帖采用ORB算法栅格化提取图像特征点和计算描述子；
[0091] 采用恒定速度运动模型，估计当前帖对应相机位姿，将上一帖图像所有地图点投 影到当前图像帖上，进行特征点匹配，并将匹配成功的上一帖地图点赋值给当前帖相应的 特征点；
[0092] 采用LM算法和化ber估计更新当前帖位姿和当前帖地图点；
[0093] 根据更新后的位姿，将局部关键帖所有地图点投影到当前图像帖上，并进行特征 点匹配，匹配成功后，将匹配成功的所有地图点赋值给当前帖相应的特征点，并使用LM算法 和化ber估计重新更新当前帖位姿和当前帖地图点。
[0094] 针对上述本发明的更进一步改进，如图2所示，对于图像的当前帖（第Ik帖图像）， 跟踪步骤的具体的实施例如下：
[00M] (1)使用ORB算法栅格化(将一幅图像均分成一系列大小相同的栅格）区域提取图 像特征点和计算描述子，栅格化提取可W保证图像上的特征点提取分布均匀，提高后续跟 踪的稳定性和精度；
[0096] (2)采用恒定速度运动模型，估计当前帖对应相机位姿。将上一帖图像Ik-I所有地 图点投影到当前图像帖上。特征点匹配，并将匹配成功的上一帖地图点赋值给当前帖相应 的特征点；
[0097] (3)使用LM化evenberg-Ma巧Uar化)算法和化ber估计更新当前帖位姿和当前帖地 图点；
[0098] (4)根据更新后的位姿，将局部关键帖所有地图点（该地图点不包含（2)中的地图 点）投影到当前图像帖上，并进行特征点匹配。匹配成功后，将匹配成功的所有地图点赋值 给当前帖相应的特征点。并使用LM算法和化ber估计重新更新当前帖位姿[R(日,k) 11(日,k)巧口 当前帖地图点。
[0099] 本实施例中，使用ORB算法实时匹配和并行局部关键帖建图的方式进行视觉跟踪， 进而得到视觉位姿。其中，ORB算法实时匹配和位姿估计为跟踪线程，局部关键帖的维护和 建图为局部关节帖线程。本实施例中，跟踪线程与局部关键帖线程并行处理，效率高能实现 实时跟踪。
[0100] 作为本发明的一种改进，使用ORB算法实时匹配和并行局部关键帖建图的方式进 行视觉跟踪，得到视觉位姿还可W包括：
[0101] 根据时间间隔情况和/或当前帖地图点个数来判断是否需要增加关键帖，如果离 上次增加关键帖超过一定时间后或者当前帖的地图点数小于阔值，则增加新的关键帖；
[0102] 判断当前帖是否为新的关键帖，如果是，则增加新的地图点，将新的关键帖无地图 点的所有特征点与局部关键帖中的所有特征点进行特征点匹配，匹配成功后3D重构得到新 的地图点；
[0103] 局部光束平差优化，修正累积误差，得到优化后的位姿和地图点。
[0104] 针对运种改进，本发明提供的完整具体的实施例如下：
[0105] 1)增加新的关键帖，从时间维度和当前帖地图点个数来判断是否需要增强关键 帖。当离上次增加关键帖超过一定时间后则或者当前帖的地图点数小于阔值时增加新的关 键帖；
[0106] 2)如果当前帖为新的关键帖，增加新的地图点。将新的关键帖无地图点的所有特 征点与局部关键帖中的所有特征点进行特征点匹配，匹配成功后3D重构得到新的地图点；
[0107] 3)为了保证跟踪的效率和跟踪的连续性，控制局部关键帖的数量，当关键帖数量 大于阔值时，删除最早加入局部关键帖中的关键帖；
[0108] 4)局部光束平差(Bundle Adjustment)优化，修正累积误差。得到优化后的位姿和 地图点。
[0109] 本实施例中，步骤1)至4)可W放在局部关键帖线程(局部关键帖线程为上述实施 例中（4))中并行处理，提高效率。重复上述实施例中的（1)至(4)，W及1)至4)能够实现连续 跟踪。
[0110] 本实施例中，即能保证跟踪的连续性又能减少需要处理的关键帖数量，减少了处 理时间，提高了运动跟踪的效率。
[0111] 本发明中，视觉位姿和IMU位姿进行卡尔曼融合过程可W采用本领域技术人员公 知的多种方法实现，优选的，可W参考如下实施例进行：
[0112] 视觉和IMU融合流程示意图，如图3所示，为了方便描述，定义下标w，i，v，c分别表 示世界坐标系，IMU坐标，视觉坐标系和相机坐标系。坐标系定义，如图4所示；
[0113] 步骤1:假设惯性测量包括一个特定的偏差b和高斯白噪声n，则实际的角速度CO和 实际加速度a如下：
[0114] O = c〇m-bu-nua = am-ba-na
[0115] 其中下标m表示测量值，动态偏差b能被表示为一个随机过程：
[0116]
[0117]滤波器的状态包括IMU在世界坐标系中的位置托，W及世界坐标系相对IMU坐标系 的速度义和姿态四元素 ft。同时，还有巧螺仪和加速度计的偏差bu，baW及视觉尺度因子入。 W及标定所得的IMU和相机间的旋转关系给，平移关系转。由此可W得到一个包含24个元素 状态向量X，如附图5预测模块所示。
[011 引
[0119]步骤2:在上述的状态表达描述中，我们使用四元素对姿态进行描述。在运种情况 下，我们使用四元素误差来表示误差和其协方差，运样能增加数值稳定性W及在最小表达。 所W，我们定义了22元素的错误状杰向量。
[0120]
[0121] 考虑到估计值i和其真值X，例如X = X-克，除了四元素误差外，我们对所有状态变 量使用i亥节決.置由m元素指差吿公责.
[0122]
[0123] 由此，可W得到连续错误状态的线性化方程：
[0124]
[01巧]其中，，!=[记，相，苗，<,f是噪声向量。在当前的解决方法中，我们对算法的速度尤 其关屯、，为此，在两相邻状态的积分时间内，我们假设Fc和Gc是恒定值。为了对其进行离散化 表不：
[0126]
[0127] 同时，可W通过巧分得到离散时间的协方差矩阵Qd:
[012 引
[0129] 由计算所得Fd和Qd,根据卡尔曼滤波，计算得到状态协方差矩阵：
[0130] Pk+i Ik = FdPk I kFd^+Qd
[0131] 步骤3:对于相机的位置测量A，我们根据单相机的视觉跟踪获取的位姿[R(0,k) t(o,k)]，（故和为相机位姿的位置向量和旋转四元数描述。进而得到其对应的测量位置。 由此得到如下测量模型：
[0132]
[0133] 其中，勺是IMU在世界坐标系下的姿态，Cfg ,是视觉坐标系相对世界坐标系的旋 转。
[0134] 步骤4:定义位置测景误差模型
[0135]
[0136] 定义旋转测量误差模型
[0137]
[0138] 其中，是分别是误差状态量毎;，和的错误测量矩阵。最后，测量矩阵可 W被累加为：
[0139]
[0140] 步骤5:当我们获取到测量矩阵邸寸，我们可W根据卡尔曼滤波器的步骤进行更新， 如图5中的更新模块所示。
[0141] 计算残差向量=
[0142] 计算跟新量:s = HPHT+R;
[0143] 计算卡尔曼增益:K = PHTs一 1;
[0144] 计算矫正量:丢=巧。根据矫正量去，我们可W计算X状态的更新量。误差状态四元 素可由如下式进行更新：
[0145] Pbi I k+i = (I d-KH) Pk+i I k (I d-KH) T+K 服 T
[0146] 通过上述单目跟踪和IMU融合，得到移动端稳定的姿态输出，进而实现稳定的运动 跟踪，本发明实施例的技术方案总流程框图，如图6所示。
[0147] 上述实施例仅仅是本发明的视觉位姿和IMU位姿进行卡尔曼融合的一个举例，除 此实施例W外，还可W采用本领域技术人员公知的其它方法，也能够实现本发明的技术效 果。
[0148] 在本发明各方法实施例中，各步骤的序号并不能用于限定各步骤的先后顺序，对 于本领域普通技术人员来讲，在不付出创造性劳动的前提下，对各步骤的先后变化也在本 发明的保护范围之内。
[0149] 另一方面，与上述方法对应地，本发明还提供一种智能手机单目和IMU融合的稳定 运动跟踪装置，如图7所示，包括：
[0150] 地图初始化模块11，用于利用ORB算法对获取的图像进行处理，之后进行3D重构， 得到初始地图点，完成地图初始化；
[0151] 视觉跟踪模块12,用于使用ORB算法实时匹配和并行局部关键帖建图的方式进行 视觉跟踪，得到视觉位姿；
[0152] IMU位姿计算模块13:用于获取IMU在S维空间产生的加速度和角速度值，并对加 速度和角速度值进行积分运算，得到IMU位姿预测结果；
[0153] 融合模块14:用于对视觉位姿和IMU位姿预测结果进行卡尔曼融合，依据融合后得 到的位姿信息进行运动跟踪。
[0154] 与现有技术相比，本发明具有能够获取更加稳定的运动跟踪方式，并实现尺度的 实时在线估计的特点。
[01W]作为本发明的一种改进，地图初始化模块11还用于：
[0156] 对获取的第一帖图像使用ORB算法提取特征点并计算描述子，将第一帖记为关键 帖，标记相机的绝对位姿；
[0157] 相机平移一端距离后，再对对获取的图像采用ORB算法提取特征点并计算描述子， 与第一帖图像特征点进行匹配，将第二帖记为关键帖，并计算出第二帖下相机相对于第一 帖的相对位姿；
[0158] 对匹配成功的特征点集进行3D重构，得到初始地图点。
[0159] 本发明中，采用ORB算法提取特征并直接匹配估计位姿，ORB算法是将FAST角点检 测与BRIEF特征描述结合一种算法改进，兼顾了单目视觉跟踪过程中的效率和精度。
[0160] 作为本发明的进一步改进，计算出第二帖下相机相对于第一帖的相对位姿包括：
[0161] 根据第一帖和第二帖图像上的对应匹配特征点集，计算出两帖图像间的基础矩 阵；
[0162] 根据基础矩阵和相机的内参数，计算得到本质矩阵；
[0163] 对本质矩阵采用奇异值分解，得到第二帖下相机相对于第一帖的相对位姿。
[0164] 本发明在相机在运动过程中，可W依次获取每帖图片所对应的一系列相对位姿。
[0165] 作为本发明的更进一步改进，视觉跟踪模块12还用于：
[0166] 对图像的当前帖采用ORB算法栅格化提取图像特征点和计算描述子；
[0167] 采用恒定速度运动模型，估计当前帖对应相机位姿，将上一帖图像所有地图点投 影到当前图像帖上，进行特征点匹配，并将匹配成功的上一帖地图点赋值给当前帖相应的 特征点；
[016引采用LM算法和化ber估计更新当前帖位姿和当前帖地图点；
[0169] 根据更新后的位姿，将局部关键帖所有地图点投影到当前图像帖上，并进行特征 点匹配，匹配成功后，将匹配成功的所有地图点赋值给当前帖相应的特征点，并使用LM算法 和化ber估计重新更新当前帖位姿和当前帖地图点。
[0170] 本发明中，使用ORB算法实时匹配和并行局部关键帖建图的方式进行视觉跟踪，进 而得到视觉位姿。其中，ORB算法实时匹配和位姿估计为跟踪线程，局部关键帖的维护和建 图为局部关节帖线程。本发明中，跟踪线程与局部关键帖线程并行处理，效率高能实现实时 跟踪。
[0171] 作为本发明的一种改进，视觉跟踪模块12还用于：
[0172] 根据时间间隔情况和/或当前帖地图点个数来判断是否需要增加关键帖，如果离 上次增加关键帖超过一定时间后或者当前帖的地图点数小于阔值，则增加新的关键帖；
[0173] 判断当前帖是否为新的关键帖，如果是，则增加新的地图点，将新的关键帖无地图 点的所有特征点与局部关键帖中的所有特征点进行特征点匹配，匹配成功后3D重构得到新 的地图点；
[0174] 局部光束平差优化，修正累积误差，得到优化后的位姿和地图点。
[0175] 本发明中，即能保证跟踪的连续性又能减少需要处理的关键帖数量，减少了处理 时间，提高了运动跟踪的效率。
[0176] W上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员 来说，在不脱离本发明所述原理的前提下，还可W作出若干改进和润饰，运些改进和润饰也 应视为本发明的保护范围。
【主权项】
1. 一种智能手机单目和MU融合的稳定运动跟踪方法，其特征在于，包括： 利用ORB算法对获取的图像进行处理，之后进行3D重构，得到初始地图点，完成地图初 始化； 使用ORB算法实时匹配和并行局部关键帧建图的方式进行视觉跟踪，得到视觉位姿； 获取MU在三维空间产生的加速度和角速度值，并对加速度和角速度值进行积分运算， 得到IMU位姿预测结果； 对视觉位姿和IMU位姿预测结果进行卡尔曼融合，依据融合后得到的位姿信息进行运 动跟踪。2. 根据权利要求1所述的智能手机单目和IMU融合的稳定运动跟踪方法，其特征在于， 所述利用ORB算法对获取的图像进行处理，之后进行3D重构，得到初始地图点，完成地图初 始化包括： 对获取的第一帧图像使用ORB算法提取特征点并计算描述子，将第一帧记为关键帧，标 记相机的绝对位姿； 相机平移一端距离后，再对对获取的图像采用ORB算法提取特征点并计算描述子，与第 一帧图像特征点进行匹配，将第二帧记为关键帧，并计算出第二帧下相机相对于第一帧的 相对位姿； 对匹配成功的特征点集进行3D重构，得到初始地图点。3. 根据权利要求2所述的智能手机单目和IMU融合的稳定运动跟踪方法，其特征在于， 所述计算出第二帧下相机相对于第一帧的相对位姿包括： 根据第一帧和第二帧图像上的对应匹配特征点集，计算出两帧图像间的基础矩阵； 根据基础矩阵和相机的内参数，计算得到本质矩阵； 对本质矩阵采用奇异值分解，得到所述第二帧下相机相对于第一帧的相对位姿。4. 根据权利要求1-3中任一所述的智能手机单目和MU融合的稳定运动跟踪方法，其特 征在于，所述使用ORB算法实时匹配和并行局部关键帧建图的方式进行视觉跟踪，得到视觉 位姿包括： 对图像的当前帧采用ORB算法栅格化提取图像特征点和计算描述子； 采用恒定速度运动模型，估计当前帧对应相机位姿，将上一帧图像所有地图点投影到 当前图像帧上，进行特征点匹配，并将匹配成功的上一帧地图点赋值给当前帧相应的特征 占. 采用LM算法和Huber估计更新当前帧位姿和当前帧地图点； 根据更新后的位姿，将局部关键帧所有地图点投影到当前图像帧上，并进行特征点匹 配，匹配成功后，将匹配成功的所有地图点赋值给当前帧相应的特征点，并使用LM算法和 Huber估计重新更新当前帧位姿和当前帧地图点。5. 根据权利要求4所述的智能手机单目和IMU融合的稳定运动跟踪方法，其特征在于， 所述使用ORB算法实时匹配和并行局部关键帧建图的方式进行视觉跟踪，得到视觉位姿还 包括： 根据时间间隔情况和/或当前帧地图点个数来判断是否需要增加关键帧，如果离上次 增加关键帧超过一定时间后或者当前帧的地图点数小于阈值，则增加新的关键帧； 判断当前帧是否为新的关键帧，如果是，则增加新的地图点，将新的关键帧无地图点的 所有特征点与局部关键帧中的所有特征点进行特征点匹配，匹配成功后3D重构得到新的地 图点； 局部光束平差优化，修正累积误差，得到优化后的位姿和地图点。6. -种智能手机单目和頂U融合的稳定运动跟踪装置，其特征在于，包括： 地图初始化模块，用于利用ORB算法对获取的图像进行处理，之后进行3D重构，得到初 始地图点，完成地图初始化； 视觉跟踪模块，用于使用ORB算法实时匹配和并行局部关键帧建图的方式进行视觉跟 踪，得到视觉位姿； IMU位姿计算模块：用于获取IMU在三维空间产生的加速度和角速度值，并对加速度和 角速度值进行积分运算，得到MU位姿预测结果； 融合模块:用于对视觉位姿和IMU位姿预测结果进行卡尔曼融合，依据融合后得到的位 姿信息进行运动跟踪。7. 根据权利要求6所述的智能手机单目和IMU融合的稳定运动跟踪装置，其特征在于， 所述地图初始化模块还用于： 对获取的第一帧图像使用ORB算法提取特征点并计算描述子，将第一帧记为关键帧，标 记相机的绝对位姿； 相机平移一端距离后，再对对获取的图像采用ORB算法提取特征点并计算描述子，与第 一帧图像特征点进行匹配，将第二帧记为关键帧，并计算出第二帧下相机相对于第一帧的 相对位姿； 对匹配成功的特征点集进行3D重构，得到初始地图点。8. 根据权利要求7所述的智能手机单目和IMU融合的稳定运动跟踪装置，其特征在于， 所述计算出第二帧下相机相对于第一帧的相对位姿包括： 根据第一帧和第二帧图像上的对应匹配特征点集，计算出两帧图像间的基础矩阵； 根据基础矩阵和相机的内参数，计算得到本质矩阵； 对本质矩阵采用奇异值分解，得到所述第二帧下相机相对于第一帧的相对位姿。9. 根据权利要求6-8中任一所述的智能手机单目和MU融合的稳定运动跟踪装置，其特 征在于，所述视觉跟踪模块还用于： 对图像的当前帧采用ORB算法栅格化提取图像特征点和计算描述子； 采用恒定速度运动模型，估计当前帧对应相机位姿，将上一帧图像所有地图点投影到 当前图像帧上，进行特征点匹配，并将匹配成功的上一帧地图点赋值给当前帧相应的特征 占. 采用LM算法和Huber估计更新当前帧位姿和当前帧地图点； 根据更新后的位姿，将局部关键帧所有地图点投影到当前图像帧上，并进行特征点匹 配，匹配成功后，将匹配成功的所有地图点赋值给当前帧相应的特征点，并使用LM算法和 Huber估计重新更新当前帧位姿和当前帧地图点。10. 根据权利要求9所述的智能手机单目和IMU融合的稳定运动跟踪装置，其特征在于， 所述视觉跟踪模块还用于： 根据时间间隔情况和/或当前帧地图点个数来判断是否需要增加关键帧，如果离上次 增加关键帧超过一定时间后或者当前帧的地图点数小于阈值，则增加新的关键帧； 判断当前帧是否为新的关键帧，如果是，则增加新的地图点，将新的关键帧无地图点的 所有特征点与局部关键帧中的所有特征点进行特征点匹配，匹配成功后3D重构得到新的地 图点； 局部光束平差优化，修正累积误差，得到优化后的位姿和地图点。
【文档编号】G01C21/16GK105953796SQ201610346493
【公开日】2016年9月21日
【申请日】2016年5月23日
【发明人】邓欢军, 方维, 李 根, 乔羽, 古鉴
【申请人】北京暴风魔镜科技有限公司