CN111383282B - 位姿信息确定方法及装置 - Google Patents

位姿信息确定方法及装置 Download PDF

Info

Publication number
CN111383282B
CN111383282B CN201811639192.5A CN201811639192A CN111383282B CN 111383282 B CN111383282 B CN 111383282B CN 201811639192 A CN201811639192 A CN 201811639192A CN 111383282 B CN111383282 B CN 111383282B
Authority
CN
China
Prior art keywords
key frame
time
moment
imu
determining
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201811639192.5A
Other languages
English (en)
Other versions
CN111383282A (zh
Inventor
彭真
宋江新
毛慧
浦世亮
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hangzhou Hikvision Digital Technology Co Ltd
Original Assignee
Hangzhou Hikvision Digital Technology Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Hangzhou Hikvision Digital Technology Co Ltd filed Critical Hangzhou Hikvision Digital Technology Co Ltd
Priority to CN201811639192.5A priority Critical patent/CN111383282B/zh
Publication of CN111383282A publication Critical patent/CN111383282A/zh
Application granted granted Critical
Publication of CN111383282B publication Critical patent/CN111383282B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T19/00—Manipulating 3D models or images for computer graphics
    • G06T19/006—Mixed reality
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/50—Depth or shape recovery
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods

Landscapes

  • Engineering & Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • Theoretical Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Automation & Control Theory (AREA)
  • Computer Graphics (AREA)
  • Computer Hardware Design (AREA)
  • General Engineering & Computer Science (AREA)
  • Software Systems (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本申请公开了一种位姿信息确定方法及装置，属于机器视觉领域。所述方法包括：先根据当前时刻与上一关键帧时刻之间的IMU测量信息，以及IMU的相邻两个测量时刻之间的时长，确定当前时刻与上一关键帧时刻之间的IMU预积分测量信息。然后根据至少两个第一图像、存储的每个三维图像拍摄部件包括的第一摄像头的内参、该IMU预积分测量信息、当前时刻与上一关键帧时刻之间的时长、存储的上一关键帧时刻该可移动设备的系统状态等，即可确定当前时刻可移动设备的位姿信息。本申请通过将至少两个三维图像拍摄部件与IMU进行联合，共同对可移动设备的位姿信息进行确定，提高了确定位姿信息的准确性。

Description

位姿信息确定方法及装置
技术领域
本申请涉及机器视觉领域，特别涉及一种位姿信息确定方法及装置。
背景技术
对于机器人、AR(Augmented Reality，增强现实)设备和无人机等可移动设备来说，这些可移动设备可以通过视觉导航技术在陌生的环境中，依靠可移动设备中安装的三维图像拍摄部件拍摄的图像来确定位姿信息(位置信息和姿态信息)，以实现导航定位。比如，机器人可以通过三维图像拍摄部件拍摄的图像来确定自身在环境中的位置，从而实现自主导航定位并进行移动；AR设备可以通过三维图像拍摄部件拍摄的图像将虚拟物体在现实环境中进行叠加，从而实现实时导航定位。
目前，可移动设备中安装的三维图像拍摄部件包括两个摄像头。通过这两个摄像头可以在可移动设备移动的过程中不断地采集的图像。之后，提取不同时刻采集的图像中的特征点，并对不同时刻采集的图像中的特征点进行匹配，然后根据同一特征点在不同时刻采集的图像中的相对运动变化来确定可移动设备的位姿信息。然而，当出现三维图像拍摄部件的摄像头受到遮挡、三维图像拍摄部件所处环境的光线较暗或者三维图像拍摄部件的拍摄范围中出现快速移动的物体等情况时，三维图像拍摄部件可能无法采集到纹理较为清楚的图像，进而导致确定出的位姿信息较为不准确。
申请内容
本申请实施例提供了一种位姿信息确定方法及装置，可以解决相关技术中因三维图像拍摄部件可能无法采集到纹理较为清楚的图像，进而导致确定出的位姿信息较为不准确的问题。所述技术方案如下：
第一方面，提供了一种位姿信息确定方法，应用于可移动设备，所述可移动设备包括至少两个三维图像拍摄部件和一个惯性测量单元IMU，所述方法包括：
根据当前时刻与上一关键帧时刻之间的IMU测量信息，以及第一时长，确定当前时刻与上一关键帧时刻之间的IMU预积分测量信息，所述上一关键帧时刻是指每个三维图像拍摄部件最近一次拍摄的能够作为关键帧图像的图像的拍摄时刻，所述第一时长是指所述IMU的相邻两个测量时刻之间的时长；
根据至少两个第一图像、存储的每个三维图像拍摄部件包括的第一摄像头的内参、每个三维图像拍摄部件相对于所述IMU的旋转平移变换矩阵、所述IMU预积分测量信息、当前时刻与上一关键帧时刻之间的时长、存储的上一关键帧时刻所述可移动设备的系统状态，以及所述可移动设备的实际重力向量，确定当前时刻所述可移动设备的系统状态，所述至少两个第一图像是指所述至少两个三维图像拍摄部件包括的第一摄像头当前时刻分别拍摄的图像，所述系统状态包括所述可移动设备的位姿信息和速度，以及所述IMU的漂移误差。
可选地，所述根据至少两个第一图像、存储的每个三维图像拍摄部件包括的第一摄像头的内参、每个三维图像拍摄部件相对于所述IMU的旋转平移变换矩阵、所述IMU预积分测量信息、当前时刻与上一关键帧时刻之间的时长、存储的上一关键帧时刻所述可移动设备的系统状态，以及所述可移动设备的实际重力向量，确定当前时刻所述可移动设备的系统状态，包括：
确定所述至少两个第一图像包括的特征点的图像坐标，以及与每个第一图像包括的特征点对应的地图点的三维位置信息；
根据所述至少两个第一图像包括的特征点的图像坐标、与每个第一图像包括的特征点对应的地图点的三维位置信息、存储的每个三维图像拍摄部件包括的第一摄像头的内参，以及每个三维图像拍摄部件相对于所述IMU的旋转平移变换矩阵，构建第一重投影误差约束代价函数；
根据所述IMU预积分测量信息、当前时刻与上一关键帧时刻之间的时长、存储的上一关键帧时刻所述可移动设备的系统状态，以及所述可移动设备的实际重力向量，构建IMU运动预测代价函数；
最小化所述第一重投影误差约束代价函数与所述IMU运动预测代价函数之和，以得到当前时刻所述可移动设备的系统状态。
可选地，所述IMU包括陀螺仪和加速度计，所述IMU测量信息包括所述陀螺仪测量得到的角速度和所述加速度计测量得到的加速度，所述IMU预积分测量信息包括相对姿态增量、相对速度增量和相对位置增量；
所述根据当前时刻与上一关键帧时刻之间的IMU测量信息，以及第一时长，确定当前时刻与上一关键帧时刻之间的IMU预积分测量信息，包括：
根据当前时刻与上一关键帧时刻之间所述陀螺仪测量得到的角速度、上一关键帧时刻所述陀螺仪的漂移误差和所述第一时长，确定当前时刻与上一关键帧时刻之间的相对姿态增量，以及上一关键帧时刻与至少一个测量时刻中每个测量时刻之间的相对姿态增量，所述至少一个测量时刻是指当前时刻与上一关键帧时刻之间的测量时刻；
根据上一关键帧时刻与所述至少一个测量时刻中每个测量时刻之间的相对姿态增量、当前时刻与上一关键帧时刻之间所述加速度计测量得到的加速度、上一关键帧时刻所述加速度计的漂移误差和所述第一时长，确定当前时刻与上一关键帧时刻之间的相对速度增量；
根据上一关键帧时刻与所述至少一个测量时刻中每个测量时刻之间的相对姿态增量、上一关键帧时刻与所述至少一个测量时刻中每个测量时刻之间的相对速度增量、当前时刻与上一关键帧时刻之间所述加速度计测量得到的加速度、上一关键帧时刻所述加速度计的漂移误差和所述第一时长，确定当前时刻与上一关键帧时刻之间的相对位置增量。
可选地，所述根据当前时刻与上一关键帧时刻之间的IMU测量信息，以及第一时长，确定当前时刻与上一关键帧时刻之间的IMU预积分测量信息之前，所述方法还包括：
每隔所述第一时长获取一个IMU数据包，所述IMU数据包至少包括IMU测量信息和对应的测量时间戳，其中，每个三维图像拍摄部件每隔第二时长拍摄一次图像，所述第二时长大于所述第一时长，且每隔所述第二时长获取的IMU数据包还包括触发累加序号；
当每个三维图像拍摄部件当前时刻拍摄到图像时，确定拍摄累加序号，并根据所述拍摄累加序号从获取的IMU数据包中获取目标IMU数据包，所述目标IMU数据包中的触发累加序号与所述拍摄累加序号相同；
将所述目标IMU数据包中的测量时间戳与存储的时间偏置之和，确定为每个三维图像拍摄部件当前时刻拍摄的图像的拍摄时间戳；
从获取的IMU数据包中，获取测量时间戳处于上一关键帧时刻拍摄的图像的拍摄时间戳与当前时刻拍摄的图像的拍摄时间戳之间的IMU测量信息，以得到当前时刻与上一关键帧时刻之间的IMU测量信息。
可选地，所述根据至少两个第一图像、存储的每个三维图像拍摄部件包括的第一摄像头的内参、每个三维图像拍摄部件相对于所述IMU的旋转平移变换矩阵、所述IMU预积分测量信息、当前时刻与上一关键帧时刻之间的时长、存储的上一关键帧时刻所述可移动设备的系统状态，以及所述可移动设备的实际重力向量，确定当前时刻所述可移动设备的系统状态之后，所述方法还包括：
确定所述可移动设备当前时刻能够追踪到的地图点的比例，所述比例是指与所述至少两个第一图像包括的特征点对应的地图点的总量占所述至少两个第一图像包括的特征点的总量的比例；
当所述可移动设备当前时刻能够追踪到的地图点的比例小于比例阈值，或者当前时刻与上一关键帧时刻之间的时长大于时长阈值时，将每个三维图像拍摄部件当前时刻拍摄的图像确定为关键帧图像。
可选地，所述将每个三维图像拍摄部件当前时刻拍摄的图像确定为关键帧图像之后，所述方法还包括：
根据每个三维图像拍摄部件对应的至少一对特征点的图像坐标、存储的每个第一摄像头的焦距，以及每个三维图像拍摄部件包括的两个摄像头之间的基线距离，确定每个第一图像包括的特征点的深度，一对特征点是指一个三维图像拍摄部件包括的两个摄像头当前时刻拍摄的图像中对应同一实物点的两个特征点；
根据当前时刻所述可移动设备的位姿信息、每个三维图像拍摄部件相对于所述IMU的旋转平移变换矩阵、每个第一摄像头的内参，以及每个第一图像包括的特征点的图像坐标和深度，确定每个第一图像包括的特征点对应的地图点的三维位置信息；
根据每个第一图像包括的特征点对应的地图点的三维位置信息，对已构建的地图进行更新。
可选地，所述根据至少两个第一图像、存储的每个三维图像拍摄部件包括的第一摄像头的内参、每个三维图像拍摄部件相对于所述IMU的旋转平移变换矩阵、所述IMU预积分测量信息、当前时刻与上一关键帧时刻之间的时长、存储的上一关键帧时刻所述可移动设备的系统状态，以及所述可移动设备的实际重力向量，确定当前时刻所述可移动设备的系统状态之前，所述方法还包括：
确定所述可移动设备在N个关键帧时刻的位姿信息，以及所述N个关键帧时刻中相邻两个关键帧时刻之间的IMU预积分测量信息，所述N个关键帧时刻是指每个三维图像拍摄部件在当前时刻之前拍摄的能够作为关键帧图像的图像的拍摄时刻，所述N为大于或等于4的正整数；
根据所述可移动设备在所述N个关键帧时刻的位姿信息、所述N个关键帧时刻中相邻两个关键帧时刻之间的时长、所述N个关键帧时刻中相邻两个关键帧时刻之间的IMU预积分测量信息，以及惯性坐标系的重力方向向量，确定所述IMU在第N个关键帧时刻的漂移误差和所述可移动设备的实际重力向量；
根据所述可移动设备在第N-1个关键帧时刻的位姿信息、所述可移动设备的实际重力向量、第N-1个关键帧时刻与第N-2个关键帧时刻之间的IMU预积分测量信息、第N-1个关键帧时刻与第N-2个关键帧时刻之间的时长、第N-1个关键帧时刻与第N个关键帧时刻之间的时长和所述IMU在第N-1个关键帧时刻测量得到的IMU测量信息，确定所述可移动设备在第N个关键帧时刻的速度。
可选地，所述IMU包括陀螺仪和加速度计，所述IMU测量信息包括所述陀螺仪测量得到的角速度和所述加速度计测量得到的加速度，所述IMU预积分测量信息包括相对姿态增量、相对速度增量和相对位置增量；
所述根据所述可移动设备在所述N个关键帧时刻的位姿信息、所述N个关键帧时刻中相邻两个关键帧时刻之间的时长、所述N个关键帧时刻中相邻两个关键帧时刻之间的IMU预积分测量信息，以及惯性坐标系的重力方向向量，确定所述IMU在第N个关键帧时刻的漂移误差和所述可移动设备的实际重力向量，包括：
根据所述可移动设备在所述N个关键帧时刻的姿态信息和所述N个关键帧时刻中相邻两个关键帧时刻之间的相对姿态增量，构建漂移误差代价函数；
最小化所述漂移误差代价函数，以得到所述陀螺仪在第N个关键帧时刻的漂移误差；
根据所述可移动设备在所述N个关键帧时刻的位姿信息、所述N个关键帧时刻中相邻两个关键帧时刻之间的时长、所述N个关键帧时刻中相邻两个关键帧时刻之间的相对位置增量和相对速度增量，以及所述惯性坐标系的重力方向向量，确定所述加速度计在第N个关键帧时刻的漂移误差和所述可移动设备的实际重力向量。
可选地，所述根据所述可移动设备在所述N个关键帧时刻的位姿信息、所述N个关键帧时刻中相邻两个关键帧时刻之间的时长、所述N个关键帧时刻中相邻两个关键帧时刻之间的相对位置增量和相对速度增量，以及所述惯性坐标系的重力方向向量，确定所述加速度计在第N个关键帧时刻的漂移误差和所述可移动设备的实际重力向量，包括：
根据所述可移动设备在所述N个关键帧时刻的位姿信息、所述N个关键帧时刻中相邻两个关键帧时刻之间的时长，以及所述N个关键帧时刻中相邻两个关键帧时刻之间的相对位置增量和相对速度增量，确定所述可移动设备的估计重力向量；
根据所述可移动设备的估计重力向量和所述惯性坐标系的重力方向向量，确定所述惯性坐标系到世界坐标系的旋转变换矩阵；
根据所述惯性坐标系到世界坐标系的旋转变换矩阵、所述惯性坐标系的重力方向向量、所述N个关键帧时刻中相邻两个关键帧时刻之间的时长、所述可移动设备在所述N个关键帧时刻的位姿信息，以及所述N个关键帧时刻中相邻两个关键帧时刻之间的相对位置增量和相对速度增量，确定所述加速度计在第N个关键帧时刻的漂移误差和所述可移动设备的重力方向修正量；
根据所述可移动设备的重力方向修正量、所述惯性坐标系到世界坐标系的旋转变换矩阵和所述惯性坐标系的重力方向向量，确定所述可移动设备的实际重力向量。
可选地，所述根据所述可移动设备在第N-1个关键帧时刻的位姿信息、所述可移动设备的实际重力向量、第N-1个关键帧时刻与第N-2个关键帧时刻之间的IMU预积分测量信息、第N-1个关键帧时刻与第N-2个关键帧时刻之间的时长、第N-1个关键帧时刻与第N个关键帧时刻之间的时长和所述IMU在第N-1个关键帧时刻测量得到的IMU测量信息，确定所述可移动设备在第N个关键帧时刻的速度，包括：
根据所述可移动设备在第N-1个关键帧时刻的位姿信息、第N-1个关键帧时刻与第N-2个关键帧时刻之间的时长、所述可移动设备的实际重力向量、第N-1个关键帧时刻与第N-2个关键帧时刻之间的相对位置增量，确定所述可移动设备在第N-1个关键帧时刻的速度；
根据所述可移动设备在第N-1个关键帧时刻的速度、第N-1个关键帧时刻与第N个关键帧时刻之间的时长和所述加速度计在第N-1个关键帧时刻测量得到的加速度，确定所述可移动设备在第N个关键帧时刻的速度。
可选地，所述确定所述可移动设备在N个关键帧时刻的位姿信息，包括：
对于所述N个关键帧时刻中的任一关键帧时刻A，根据所述关键帧时刻A对应的初始关键帧图像包括的特征点的图像坐标、与所述关键帧时刻A对应的初始关键帧图像包括的特征点对应的地图点的三维位置信息、存储的每个第一摄像头的内参，以及每个三维图像拍摄部件相对于所述IMU的旋转平移变换矩阵，构建第二重投影误差约束代价函数，所述关键帧时刻A对应的初始关键帧图像是指所述至少两个三维图像拍摄部件包括的第一摄像头在所述关键帧时刻A分别拍摄的图像；
最小化所述第二重投影误差约束代价函数，以得到所述可移动设备在所述关键帧时刻A的位姿信息。
第二方面，提供了一种位姿信息确定装置，应用于可移动设备，所述可移动设备包括至少两个三维图像拍摄部件和一个惯性测量单元IMU，所述装置包括：
第一确定模块，用于根据当前时刻与上一关键帧时刻之间的IMU测量信息，以及第一时长，确定当前时刻与上一关键帧时刻之间的IMU预积分测量信息，所述上一关键帧时刻是指每个三维图像拍摄部件最近一次拍摄的能够作为关键帧图像的图像的拍摄时刻，所述第一时长是指所述IMU的相邻两个测量时刻之间的时长；
第二确定模块，用于根据至少两个第一图像、存储的每个三维图像拍摄部件包括的第一摄像头的内参、每个三维图像拍摄部件相对于所述IMU的旋转平移变换矩阵、所述IMU预积分测量信息、当前时刻与上一关键帧时刻之间的时长、存储的上一关键帧时刻所述可移动设备的系统状态，以及所述可移动设备的实际重力向量，确定当前时刻所述可移动设备的系统状态，所述至少两个第一图像是指所述至少两个三维图像拍摄部件包括的第一摄像头当前时刻分别拍摄的图像，所述系统状态包括所述可移动设备的位姿信息和速度，以及所述IMU的漂移误差。
可选地，所述第二确定模块包括：
第一确定子模块，用于确定所述至少两个第一图像包括的特征点的图像坐标，以及与每个第一图像包括的特征点对应的地图点的三维位置信息；
第一构建子模块，用于根据所述至少两个第一图像包括的特征点的图像坐标、与每个第一图像包括的特征点对应的地图点的三维位置信息、存储的每个三维图像拍摄部件包括的第一摄像头的内参，以及每个三维图像拍摄部件相对于所述IMU的旋转平移变换矩阵，构建第一重投影误差约束代价函数；
第二构建子模块，用于根据所述IMU预积分测量信息、当前时刻与上一关键帧时刻之间的时长、存储的上一关键帧时刻所述可移动设备的系统状态，以及所述可移动设备的实际重力向量，构建IMU运动预测代价函数；
第一最小化运算子模块，用于最小化所述第一重投影误差约束代价函数与所述IMU运动预测代价函数之和，以得到当前时刻所述可移动设备的系统状态。
可选地，所述IMU包括陀螺仪和加速度计，所述IMU测量信息包括所述陀螺仪测量得到的角速度和所述加速度计测量得到的加速度，所述IMU预积分测量信息包括相对姿态增量、相对速度增量和相对位置增量；
所述第一确定模块包括：
第二确定子模块，用于根据当前时刻与上一关键帧时刻之间所述陀螺仪测量得到的角速度、上一关键帧时刻所述陀螺仪的漂移误差和所述第一时长，确定当前时刻与上一关键帧时刻之间的相对姿态增量，以及上一关键帧时刻与至少一个测量时刻中每个测量时刻之间的相对姿态增量，所述至少一个测量时刻是指当前时刻与上一关键帧时刻之间的测量时刻；
第三确定子模块，用于根据上一关键帧时刻与所述至少一个测量时刻中每个测量时刻之间的相对姿态增量、当前时刻与上一关键帧时刻之间所述加速度计测量得到的加速度、上一关键帧时刻所述加速度计的漂移误差和所述第一时长，确定当前时刻与上一关键帧时刻之间的相对速度增量；
第四确定子模块，用于根据上一关键帧时刻与所述至少一个测量时刻中每个测量时刻之间的相对姿态增量、上一关键帧时刻与所述至少一个测量时刻中每个测量时刻之间的相对速度增量、当前时刻与上一关键帧时刻之间所述加速度计测量得到的加速度、上一关键帧时刻所述加速度计的漂移误差和所述第一时长，确定当前时刻与上一关键帧时刻之间的相对位置增量。
可选地，所述装置还包括：
第一获取模块，用于每隔所述第一时长获取一个IMU数据包，所述IMU数据包至少包括IMU测量信息和对应的测量时间戳，其中，每个三维图像拍摄部件每隔第二时长拍摄一次图像，所述第二时长大于所述第一时长，且每隔所述第二时长获取的IMU数据包还包括触发累加序号；
第二获取模块，用于当每个三维图像拍摄部件当前时刻拍摄到图像时，确定拍摄累加序号，并根据所述拍摄累加序号从获取的IMU数据包中获取目标IMU数据包，所述目标IMU数据包中的触发累加序号与所述拍摄累加序号相同；
第三确定模块，用于将所述目标IMU数据包中的测量时间戳与存储的时间偏置之和，确定为每个三维图像拍摄部件当前时刻拍摄的图像的拍摄时间戳；
第三获取模块，从获取的IMU数据包中，获取测量时间戳处于上一关键帧时刻拍摄的图像的拍摄时间戳与当前时刻拍摄的图像的拍摄时间戳之间的IMU测量信息，以得到当前时刻与上一关键帧时刻之间的IMU测量信息。
可选地，所述装置还包括：
第四确定模块，用于确定所述可移动设备当前时刻能够追踪到的地图点的比例，所述比例是指与所述至少两个第一图像包括的特征点对应的地图点的总量占所述至少两个第一图像包括的特征点的总量的比例；
第五确定模块，用于当所述可移动设备当前时刻能够追踪到的地图点的比例小于比例阈值，或者当前时刻与上一关键帧时刻之间的时长大于时长阈值时，将每个三维图像拍摄部件当前时刻拍摄的图像确定为关键帧图像。
可选地，所述装置还包括：
第六确定模块，用于根据每个三维图像拍摄部件对应的至少一对特征点的图像坐标、存储的每个第一摄像头的焦距，以及每个三维图像拍摄部件包括的两个摄像头之间的基线距离，确定每个第一图像包括的特征点的深度，一对特征点是指一个三维图像拍摄部件包括的两个摄像头当前时刻拍摄的图像中对应同一实物点的两个特征点；
第七确定模块，用于根据当前时刻所述可移动设备的位姿信息、每个三维图像拍摄部件相对于所述IMU的旋转平移变换矩阵、每个第一摄像头的内参，以及每个第一图像包括的特征点的图像坐标和深度，确定每个第一图像包括的特征点对应的地图点的三维位置信息；
更新模块，用于根据每个第一图像包括的特征点对应的地图点的三维位置信息，对已构建的地图进行更新。
可选地，所述装置还包括：
第八确定模块，用于确定所述可移动设备在N个关键帧时刻的位姿信息，以及所述N个关键帧时刻中相邻两个关键帧时刻之间的IMU预积分测量信息，所述N个关键帧时刻是指每个三维图像拍摄部件在当前时刻之前拍摄的能够作为关键帧图像的图像的拍摄时刻，所述N为大于或等于4的正整数；
第九确定模块，用于根据所述可移动设备在所述N个关键帧时刻的位姿信息、所述N个关键帧时刻中相邻两个关键帧时刻之间的时长、所述N个关键帧时刻中相邻两个关键帧时刻之间的IMU预积分测量信息，以及惯性坐标系的重力方向向量，确定所述IMU在第N个关键帧时刻的漂移误差和所述可移动设备的实际重力向量；
第十确定模块，用于根据所述可移动设备在第N-1个关键帧时刻的位姿信息、所述可移动设备的实际重力向量、第N-1个关键帧时刻与第N-2个关键帧时刻之间的IMU预积分测量信息、第N-1个关键帧时刻与第N-2个关键帧时刻之间的时长、第N-1个关键帧时刻与第N个关键帧时刻之间的时长和所述IMU在第N-1个关键帧时刻测量得到的IMU测量信息，确定所述可移动设备在第N个关键帧时刻的速度。
可选地，所述IMU包括陀螺仪和加速度计，所述IMU测量信息包括所述陀螺仪测量得到的角速度和所述加速度计测量得到的加速度，所述IMU预积分测量信息包括相对姿态增量、相对速度增量和相对位置增量；
所述第九确定模块包括：
第三构建子模块，用于根据所述可移动设备在所述N个关键帧时刻的姿态信息和所述N个关键帧时刻中相邻两个关键帧时刻之间的相对姿态增量，构建漂移误差代价函数；
第二最小化运算模块，用于最小化所述漂移误差代价函数，以得到所述陀螺仪在第N个关键帧时刻的漂移误差；
第五确定子模块，用于根据所述可移动设备在所述N个关键帧时刻的位姿信息、所述N个关键帧时刻中相邻两个关键帧时刻之间的时长、所述N个关键帧时刻中相邻两个关键帧时刻之间的相对位置增量和相对速度增量，以及所述惯性坐标系的重力方向向量，确定所述加速度计在第N个关键帧时刻的漂移误差和所述可移动设备的实际重力向量。
可选地，所述第五确定子模块包括：
第一确定单元，用于根据所述可移动设备在所述N个关键帧时刻的位姿信息、所述N个关键帧时刻中相邻两个关键帧时刻之间的时长，以及所述N个关键帧时刻中相邻两个关键帧时刻之间的相对位置增量和相对速度增量，确定所述可移动设备的估计重力向量；
第二确定单元，用于根据所述可移动设备的估计重力向量和所述惯性坐标系的重力方向向量，确定所述惯性坐标系到世界坐标系的旋转变换矩阵；
第三确定单元，用于根据所述惯性坐标系到世界坐标系的旋转变换矩阵、所述惯性坐标系的重力方向向量、所述N个关键帧时刻中相邻两个关键帧时刻之间的时长、所述可移动设备在所述N个关键帧时刻的位姿信息，以及所述N个关键帧时刻中相邻两个关键帧时刻之间的相对位置增量和相对速度增量，确定所述加速度计在第N个关键帧时刻的漂移误差和所述可移动设备的重力方向修正量；
第四确定单元，用于根据所述可移动设备的重力方向修正量、所述惯性坐标系到世界坐标系的旋转变换矩阵和所述惯性坐标系的重力方向向量，确定所述可移动设备的实际重力向量。
可选地，所述第十确定模块包括：
第六确定子模块，用于根据所述可移动设备在第N-1个关键帧时刻的位姿信息、第N-1个关键帧时刻与第N-2个关键帧时刻之间的时长、所述可移动设备的实际重力向量、第N-1个关键帧时刻与第N-2个关键帧时刻之间的相对位置增量，确定所述可移动设备在第N-1个关键帧时刻的速度；
第七确定子模块，用于根据所述可移动设备在第N-1个关键帧时刻的速度、第N-1个关键帧时刻与第N个关键帧时刻之间的时长和所述加速度计在第N-1个关键帧时刻测量得到的加速度，确定所述可移动设备在第N个关键帧时刻的速度。
可选地，所述第八确定模块包括：
第四构建子模块，用于对于所述N个关键帧时刻中的任一关键帧时刻A，根据所述关键帧时刻A对应的初始关键帧图像包括的特征点的图像坐标、与所述关键帧时刻A对应的初始关键帧图像包括的特征点对应的地图点的三维位置信息、存储的每个第一摄像头的内参，以及每个三维图像拍摄部件相对于所述IMU的旋转平移变换矩阵，构建第二重投影误差约束代价函数，所述关键帧时刻A对应的初始关键帧图像是指所述至少两个三维图像拍摄部件包括的第一摄像头在所述关键帧时刻A分别拍摄的图像；
第三最小化运算模块，用于最小化所述第二重投影误差约束代价函数，以得到所述可移动设备在所述关键帧时刻A的位姿信息。
第三方面，提供一种可移动设备，所述可移动设备包括：
处理器；
用于存储处理器可执行指令的存储器；
其中，所述处理器被配置为执行上述第一方面所述的任一项方法的步骤。
第四方面，提供一种计算机可读存储介质，所述计算机可读存储介质上存储有指令，所述指令被处理器执行时实现上述第一方面所述的任一项方法的步骤。
第五方面，提供了一种包括指令的计算机程序产品，当其在计算机上运行时，使得计算机执行上述第一方面任一项所述方法的步骤。
本申请实施例提供的技术方案至少可以带来以下有益效果：
先根据当前时刻与上一关键帧时刻之间的IMU测量信息，以及IMU的相邻两个测量时刻之间的时长，确定当前时刻与上一关键帧时刻之间的IMU预积分测量信息。然后根据至少两个第一图像、存储的每个三维图像拍摄部件包括的第一摄像头的内参、每个三维图像拍摄部件相对于IMU的旋转平移变换矩阵、该IMU预积分测量信息、当前时刻与上一关键帧时刻之间的时长、存储的上一关键帧时刻该可移动设备的系统状态，以及该可移动设备的实际重力向量，确定当前时刻该可移动设备的系统状态。由于系统状态包括可移动设备的位姿信息和速度，以及IMU的漂移误差，因此，在确定当前时刻可移动设备的系统状态之后，即可确定当前时刻可移动设备的位姿信息。由于本申请实施例是将至少两个三维图像拍摄部件与IMU进行联合，以共同对可移动设备的位姿信息进行确定，避免了因三维图像拍摄部件可能无法采集到纹理较为清楚的图像，而导致确定出的位姿信息较为不准确的问题，进而提高了确定位姿信息的准确性。
附图说明
为了更清楚地说明本申请实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本申请实施例提供的一种可移动设备的结构示意图。
图2是本申请实施例提供的一种嵌入式硬件板卡的结构示意图。
图3是本申请实施例提供的第一种位姿信息确定方法的流程图。
图4是本申请实施例提供的第二种位姿信息确定方法的流程图。
图5是本申请实施例提供的第三种位姿信息确定方法的流程图。
图6是本申请实施例提供的一种位姿信息确定装置框图。
图7是本申请实施例提供的另一种可移动设备的结构示意图。
具体实施方式
这里将详细地对示例性实施例进行说明，其示例表示在附图中。下面的描述涉及附图时，除非另有表示，不同附图中的相同数字表示相同或相似的要素。以下示例性实施例中所描述的实施方式并不代表与本申请相一致的所有实施方式。相反，它们仅是与本申请的一些方面相一致的装置和方法的例子。
在对本申请实施例进行详细的解释说明之前，先对本申请实施例的实施环境进行介绍：
本申请实施例提供的一种实施环境包括可移动设备。可移动设备用于采集图像和IMU(Inertial Measurement Unit，惯性测量单元)测量信息，并根据采集到的图像和IMU测量信息确定可移动设备的位姿信息，以对可移动设备的位姿信息进行实时更新。其中，可移动终设备可以是扫地机、机器人、无人车等。
在本申请实施例中，可以在可移动设备中设置至少两个三维图像拍摄部件。图1是一种可移动设备的结构示意图。参见图1，可移动设备包括三个三维图像拍摄部件，每个三维图像拍摄部件包括两个摄像头，共六个摄像头，即，标号1-6对应的部件。每个三维图像拍摄部件还可以包括ISP(Image Signal Processing，图像信号处理)，ISP用于对每个三维图像拍摄部件拍摄的图像进行预处理。另外，在图1所示的可移动设备中，可移动设备还包括其他的部件。具体为，标号7对应的嵌入式硬件板卡。标号8对应的金属底板，在金属底板的上方可以设置有摄像头和嵌入式硬件板卡。在金属底板上还可以设置有如标号9-11对应的三个走线孔，走线孔可以用于将摄像头和嵌入式硬件板卡的相关电线输出到可移动设备的底座的下面。在金属底板上还可以设置有如标号12-17对应的六个矩形凹槽，矩形凹槽用于将摄像头固定在金属底板上。
六个摄像头可以是独立的工业摄像头，摄像头的种类可以包括普通的针孔摄像头、鱼眼摄像头等。两个相邻且摆放位置平行的摄像头为一对双目摄像头(三维图像拍摄部件)。不同对双目摄像头之间的视角可以重合也可以不重合。
金属底板可以设置为三角形，共有三条边，三对双目摄像头分别被设置在三条边中的一条边上，且一对双目摄像头中的两个摄像头拍摄方向一致。每对双目摄像头的基线距离可以设置为12厘米。可移动设备可以包括三个三维图像拍摄部件，任意两个三维图像拍摄部件与可移动设备的预设参考中心的连线之间的夹角等于120度，即不同对双目摄像头中相邻的两个摄像头的夹角可以设置为120度。
图2是嵌入式硬件板卡的结构示意图，参见图2，嵌入式硬件板卡包括IMU、CPU(Central Processing Unit，中央处理器)、电气引脚、数据传输接口和电源输入接口。电源输入接口用于通过外部电源为可移动设备供电。
IMU可以每隔第一时长测量一次数据，即IMU测量信息。其中，IMU包括陀螺仪和加速度计。陀螺仪用于测量可移动设备的角速度，加速度计用于测量可移动设备的加速度。
CPU可以每隔第二时长通过上述电气引脚向至少两个三维图像拍摄部件发送一次触发信号，以触发至少两个三维图像拍摄部件拍摄图像，并可以通过数据传输接口接收至少两个三维图像拍摄部件拍摄的图像。具体地，IMU在每次测量得到IMU测量信息时，向CPU发送测量得到的IMU测量信息以及一个中断信号。CPU接收到该测量得到的IMU测量信息以及该中断信号之后，根据该中断信号可以判断当前时刻与上一触发信号的发送时刻之间的时长是否达到第二时长，如果是，则CPU可以通过电气引脚向至少两个三维图像拍摄部件发送触发信号，以触发至少两个三维图像拍摄部件拍摄图像。另外，CPU还可以在接收到中断信号时，将当前时刻对应的时间戳作为IMU测量信息的测量时间戳进行记录。再者，CPU在向至少两个三维图像拍摄部件发送触发信号时，可以记录触发累加序号。其中，触发累加序号是指可移动设备的CPU触发至少两个三维图像拍摄部件拍摄图像的次数，简单地，可以将可移动设备的CPU向至少两个三维图像拍摄部件发送触发信号的数量确定为触发累加序号。之后CPU可以将该IMU测量信息、该测量时间戳以及该触发累积序号作为一个IMU数据包进行打包并存储。如果当前时刻与上一触发信号的发送时刻之间的时长没有到达第二时长，则CPU可以将该IMU测量信息以及该测量时间戳作为一个IMU数据包进行打包并存储。
至少两个三维图像拍摄部件被CPU触发后，开始进行曝光，并将曝光后的图像通过数据传输接口发送给CPU，当然，也可以经过至少两个三维图像拍摄部件中的ISP预处理，之后再通过数据传输接口发送给CPU。当CPU接收到至少两个三维图像拍摄部件拍摄的图像时，可以记录拍摄累加序号。其中拍摄累加序号可以是指CPU接收到至少两个三维图像拍摄部件拍摄的图像的次数，且至少两个三维图像拍摄部件在同一时刻拍摄的图像算作一次。比如，至少两个三维图像拍摄部件在4个时刻分别进行了图像拍摄。那么，CPU在每接收到一个时刻发送的图像时，即可将拍摄累加序号增加1。这样，CPU在接收到4个时刻拍摄的图像时，即可确定拍摄累加序号为4。
接下来对本申请实施例提供的位姿信息确定方法进行详细地解释说明。
图3是本申请实施例提供的一种位姿信息确定方法的流程图，该方法应用于可移动设备，该可移动设备包括至少两个三维图像拍摄部件和一个IMU，参见图3，该方法包括：
步骤301：根据当前时刻与上一关键帧时刻之间的IMU测量信息，以及第一时长，确定当前时刻与上一关键帧时刻之间的IMU预积分测量信息，该上一关键帧时刻是指每个三维图像拍摄部件最近一次拍摄的能够作为关键帧图像的图像的拍摄时刻，第一时长是指IMU的相邻两个测量时刻之间的时长。
步骤302：根据至少两个第一图像、存储的每个三维图像拍摄部件包括的第一摄像头的内参、每个三维图像拍摄部件相对于IMU的旋转平移变换矩阵、该IMU预积分测量信息、当前时刻与上一关键帧时刻之间的时长、存储的上一关键帧时刻该可移动设备的系统状态，以及该可移动设备的实际重力向量，确定当前时刻该可移动设备的系统状态，该至少两个第一图像是指该至少两个三维图像拍摄部件包括的第一摄像头当前时刻分别拍摄的图像，该系统状态包括该可移动设备的位姿信息和速度，以及IMU的漂移误差。
在本申请实施例中，先根据当前时刻与上一关键帧时刻之间的IMU测量信息，以及IMU的相邻两个测量时刻之间的时长，确定当前时刻与上一关键帧时刻之间的IMU预积分测量信息。然后根据至少两个第一图像、存储的每个三维图像拍摄部件包括的第一摄像头的内参、每个三维图像拍摄部件相对于IMU的旋转平移变换矩阵、该IMU预积分测量信息、当前时刻与上一关键帧时刻之间的时长、存储的上一关键帧时刻该可移动设备的系统状态，以及该可移动设备的实际重力向量，确定当前时刻该可移动设备的系统状态。由于系统状态包括可移动设备的位姿信息和速度，以及IMU的漂移误差，因此，在确定当前时刻可移动设备的系统状态之后，即可确定当前时刻可移动设备的位姿信息。由于本申请实施例是将至少两个三维图像拍摄部件与IMU进行联合，以共同对可移动设备的位姿信息进行确定，避免了因三维图像拍摄部件可能无法采集到纹理较为清楚的图像，而导致确定出的位姿信息较为不准确的问题，进而提高了确定位姿信息的准确性。
可选地，该根据至少两个第一图像、存储的每个三维图像拍摄部件包括的第一摄像头的内参、每个三维图像拍摄部件相对于IMU的旋转平移变换矩阵、该IMU预积分测量信息、当前时刻与上一关键帧时刻之间的时长、存储的上一关键帧时刻该可移动设备的系统状态，以及该可移动设备的实际重力向量，确定当前时刻该可移动设备的系统状态，包括：
确定该至少两个第一图像包括的特征点的图像坐标，以及与每个第一图像包括的特征点对应的地图点的三维位置信息；
根据该至少两个第一图像包括的特征点的图像坐标、与每个第一图像包括的特征点对应的地图点的三维位置信息、存储的每个三维图像拍摄部件包括的第一摄像头的内参，以及每个三维图像拍摄部件相对于IMU的旋转平移变换矩阵，构建第一重投影误差约束代价函数；
根据该IMU预积分测量信息、当前时刻与上一关键帧时刻之间的时长、存储的上一关键帧时刻该可移动设备的系统状态，以及该可移动设备的实际重力向量，构建IMU运动预测代价函数；
最小化第一重投影误差约束代价函数与IMU运动预测代价函数之和，以得到当前时刻该可移动设备的系统状态。
可选地，IMU包括陀螺仪和加速度计，该IMU测量信息包括陀螺仪测量得到的角速度和加速度计测量得到的加速度，该IMU预积分测量信息包括相对姿态增量、相对速度增量和相对位置增量；
该根据当前时刻与上一关键帧时刻之间的IMU测量信息，以及第一时长，确定当前时刻与上一关键帧时刻之间的IMU预积分测量信息，包括：
根据当前时刻与上一关键帧时刻之间陀螺仪测量得到的角速度、上一关键帧时刻陀螺仪的漂移误差和第一时长，确定当前时刻与上一关键帧时刻之间的相对姿态增量，以及上一关键帧时刻与至少一个测量时刻中每个测量时刻之间的相对姿态增量，该至少一个测量时刻是指当前时刻与上一关键帧时刻之间的测量时刻；
根据上一关键帧时刻与该至少一个测量时刻中每个测量时刻之间的相对姿态增量、当前时刻与上一关键帧时刻之间加速度计测量得到的加速度、上一关键帧时刻加速度计的漂移误差和第一时长，确定当前时刻与上一关键帧时刻之间的相对速度增量；
根据上一关键帧时刻与该至少一个测量时刻中每个测量时刻之间的相对姿态增量、上一关键帧时刻与该至少一个测量时刻中每个测量时刻之间的相对速度增量、当前时刻与上一关键帧时刻之间加速度计测量得到的加速度、上一关键帧时刻加速度计的漂移误差和第一时长，确定当前时刻与上一关键帧时刻之间的相对位置增量。
可选地，该根据当前时刻与上一关键帧时刻之间的IMU测量信息，以及第一时长，确定当前时刻与上一关键帧时刻之间的IMU预积分测量信息之前，该方法还包括：
每隔第一时长获取一个IMU数据包，该IMU数据包至少包括IMU测量信息和对应的测量时间戳，其中，每个三维图像拍摄部件每隔第二时长拍摄一次图像，第二时长大于第一时长，且每隔第二时长获取的IMU数据包还包括触发累加序号；
当每个三维图像拍摄部件当前时刻拍摄到图像时，确定拍摄累加序号，并根据该拍摄累加序号从获取的IMU数据包中获取目标IMU数据包，目标IMU数据包中的触发累加序号与该拍摄累加序号相同；
将目标IMU数据包中的测量时间戳与存储的时间偏置之和，确定为每个三维图像拍摄部件当前时刻拍摄的图像的拍摄时间戳；
从获取的IMU数据包中，获取测量时间戳处于上一关键帧时刻拍摄的图像的拍摄时间戳与当前时刻拍摄的图像的拍摄时间戳之间的IMU测量信息，以得到当前时刻与上一关键帧时刻之间的IMU测量信息。
可选地，该根据至少两个第一图像、存储的每个三维图像拍摄部件包括的第一摄像头的内参、每个三维图像拍摄部件相对于IMU的旋转平移变换矩阵、该IMU预积分测量信息、当前时刻与上一关键帧时刻之间的时长、存储的上一关键帧时刻该可移动设备的系统状态，以及该可移动设备的实际重力向量，确定当前时刻该可移动设备的系统状态之后，该方法还包括：
确定该可移动设备当前时刻能够追踪到的地图点的比例，该比例是指与该至少两个第一图像包括的特征点对应的地图点的总量占该至少两个第一图像包括的特征点的总量的比例；
当该可移动设备当前时刻能够追踪到的地图点的比例小于比例阈值，或者当前时刻与上一关键帧时刻之间的时长大于时长阈值时，将每个三维图像拍摄部件当前时刻拍摄的图像确定为关键帧图像。
可选地，该将每个三维图像拍摄部件当前时刻拍摄的图像确定为关键帧图像之后，该方法还包括：
根据每个三维图像拍摄部件对应的至少一对特征点的图像坐标、存储的每个第一摄像头的焦距，以及每个三维图像拍摄部件包括的两个摄像头之间的基线距离，确定每个第一图像包括的特征点的深度，一对特征点是指一个三维图像拍摄部件包括的两个摄像头当前时刻拍摄的图像中对应同一实物点的两个特征点；
根据当前时刻该可移动设备的位姿信息、每个三维图像拍摄部件相对于IMU的旋转平移变换矩阵、每个第一摄像头的内参，以及每个第一图像包括的特征点的图像坐标和深度，确定每个第一图像包括的特征点对应的地图点的三维位置信息；
根据每个第一图像包括的特征点对应的地图点的三维位置信息，对已构建的地图进行更新。
可选地，该根据至少两个第一图像、存储的每个三维图像拍摄部件包括的第一摄像头的内参、每个三维图像拍摄部件相对于IMU的旋转平移变换矩阵、该IMU预积分测量信息、当前时刻与上一关键帧时刻之间的时长、存储的上一关键帧时刻该可移动设备的系统状态，以及该可移动设备的实际重力向量，确定当前时刻该可移动设备的系统状态之前，该方法还包括：
确定该可移动设备在N个关键帧时刻的位姿信息，以及该N个关键帧时刻中相邻两个关键帧时刻之间的IMU预积分测量信息，该N个关键帧时刻是指每个三维图像拍摄部件在当前时刻之前拍摄的能够作为关键帧图像的图像的拍摄时刻，N为大于或等于4的正整数；
根据该可移动设备在该N个关键帧时刻的位姿信息、该N个关键帧时刻中相邻两个关键帧时刻之间的时长、该N个关键帧时刻中相邻两个关键帧时刻之间的IMU预积分测量信息，以及惯性坐标系的重力方向向量，确定IMU在第N个关键帧时刻的漂移误差和该可移动设备的实际重力向量；
根据该可移动设备在第N-1个关键帧时刻的位姿信息、该可移动设备的实际重力向量、第N-1个关键帧时刻与第N-2个关键帧时刻之间的IMU预积分测量信息、第N-1个关键帧时刻与第N-2个关键帧时刻之间的时长、第N-1个关键帧时刻与第N个关键帧时刻之间的时长和IMU在第N-1个关键帧时刻测量得到的IMU测量信息，确定该可移动设备在第N个关键帧时刻的速度。
可选地，IMU包括陀螺仪和加速度计，该IMU测量信息包括陀螺仪测量得到的角速度和加速度计测量得到的加速度，该IMU预积分测量信息包括相对姿态增量、相对速度增量和相对位置增量；
该根据该可移动设备在该N个关键帧时刻的位姿信息、该N个关键帧时刻中相邻两个关键帧时刻之间的时长、该N个关键帧时刻中相邻两个关键帧时刻之间的IMU预积分测量信息，以及惯性坐标系的重力方向向量，确定IMU在第N个关键帧时刻的漂移误差和该可移动设备的实际重力向量，包括：
根据该可移动设备在该N个关键帧时刻的姿态信息和该N个关键帧时刻中相邻两个关键帧时刻之间的相对姿态增量，构建漂移误差代价函数；
最小化该漂移误差代价函数，以得到陀螺仪在第N个关键帧时刻的漂移误差；
根据该可移动设备在该N个关键帧时刻的位姿信息、该N个关键帧时刻中相邻两个关键帧时刻之间的时长、该N个关键帧时刻中相邻两个关键帧时刻之间的相对位置增量和相对速度增量，以及该惯性坐标系的重力方向向量，确定加速度计在第N个关键帧时刻的漂移误差和该可移动设备的实际重力向量。
可选地，该根据该可移动设备在该N个关键帧时刻的位姿信息、该N个关键帧时刻中相邻两个关键帧时刻之间的时长、该N个关键帧时刻中相邻两个关键帧时刻之间的相对位置增量和相对速度增量，以及该惯性坐标系的重力方向向量，确定加速度计在第N个关键帧时刻的漂移误差和该可移动设备的实际重力向量，包括：
根据该可移动设备在该N个关键帧时刻的位姿信息、该N个关键帧时刻中相邻两个关键帧时刻之间的时长，以及该N个关键帧时刻中相邻两个关键帧时刻之间的相对位置增量和相对速度增量，确定该可移动设备的估计重力向量；
根据该可移动设备的估计重力向量和该惯性坐标系的重力方向向量，确定该惯性坐标系到世界坐标系的旋转变换矩阵；
根据该惯性坐标系到世界坐标系的旋转变换矩阵、该惯性坐标系的重力方向向量、该N个关键帧时刻中相邻两个关键帧时刻之间的时长、该可移动设备在该N个关键帧时刻的位姿信息，以及该N个关键帧时刻中相邻两个关键帧时刻之间的相对位置增量和相对速度增量，确定加速度计在第N个关键帧时刻的漂移误差和该可移动设备的重力方向修正量；
根据该可移动设备的重力方向修正量、该惯性坐标系到世界坐标系的旋转变换矩阵和该惯性坐标系的重力方向向量，确定该可移动设备的实际重力向量。
可选地，该根据该可移动设备在第N-1个关键帧时刻的位姿信息、该可移动设备的实际重力向量、第N-1个关键帧时刻与第N-2个关键帧时刻之间的IMU预积分测量信息、第N-1个关键帧时刻与第N-2个关键帧时刻之间的时长、第N-1个关键帧时刻与第N个关键帧时刻之间的时长和IMU在第N-1个关键帧时刻测量得到的IMU测量信息，确定该可移动设备在第N个关键帧时刻的速度，包括：
根据该可移动设备在第N-1个关键帧时刻的位姿信息、第N-1个关键帧时刻与第N-2个关键帧时刻之间的时长、该可移动设备的实际重力向量、第N-1个关键帧时刻与第N-2个关键帧时刻之间的相对位置增量，确定该可移动设备在第N-1个关键帧时刻的速度；
根据该可移动设备在第N-1个关键帧时刻的速度、第N-1个关键帧时刻与第N个关键帧时刻之间的时长和加速度计在第N-1个关键帧时刻测量得到的加速度，确定该可移动设备在第N个关键帧时刻的速度。
可选地，该确定该可移动设备在N个关键帧时刻的位姿信息，包括：
对于该N个关键帧时刻中的任一关键帧时刻A，根据该关键帧时刻A对应的初始关键帧图像包括的特征点的图像坐标、与该关键帧时刻A对应的初始关键帧图像包括的特征点对应的地图点的三维位置信息、存储的每个第一摄像头的内参，以及每个三维图像拍摄部件相对于IMU的旋转平移变换矩阵，构建第二重投影误差约束代价函数，该关键帧时刻A对应的初始关键帧图像是指该至少两个三维图像拍摄部件包括的第一摄像头在该关键帧时刻A分别拍摄的图像；
最小化第二重投影误差约束代价函数，以得到该可移动设备在该关键帧时刻A的位姿信息。
上述所有可选技术方案，均可按照任意结合形成本申请的可选实施例，本申请实施例对此不再一一赘述。
本申请实施例提供了一种位姿信息确定方法的流程图，该方法应用于可移动设备，该可移动设备包括至少两个三维图像拍摄部件和一个IMU。本实施例将对图3所示的实施例进行展开说明，该方法包括如下几个步骤。
首先，参见图4，在确定可移动设备的位姿信息之前，可以对可移动设备中的至少两个三维图像拍摄部件和IMU进行联合初始化，以确定可移动设备在初始状态下的系统状态。之后，便可根据可移动设备在初始状态下的系统状态来实时地确定可移动设备的位姿信息。也即是，也可以在每次启动可移动设备时进行联合初始化，获取可移动设备在初始状态下的系统状态并进行存储，之后在对可移动设备的位姿信息进行实时确定时就可以直接根据存储的可移动设备在初始状态下的系统状态，来实时地确定可移动设备的位姿信息。下面通过步骤401-步骤403对该联合初始化的过程进行说明。
步骤401：确定可移动设备在N个关键帧时刻的位姿信息，以及N个关键帧时刻中相邻两个关键帧时刻之间的IMU预积分测量信息，N为大于或等于4的正整数。
需要说明的是，N个关键帧时刻是指每个三维图像拍摄部件在当前时刻之前拍摄的能够作为关键帧图像的图像的拍摄时刻。并且N个关键帧时刻中相邻两个关键帧时刻之间可能存在至少一个非关键帧时刻，即每个三维图像拍摄部件拍摄的不能作为关键帧图像的图像的拍摄时刻。示例性地，每个三维图像拍摄部件在时刻1和时刻4拍摄到的图像都可以作为关键帧图像，时刻1和时刻4之间的时刻拍摄到的图像不能作为关键帧图像，即，时刻2和时刻3拍摄到的图像不能作为关键帧图像，那么，时刻1和时刻4可以称为关键帧时刻，时刻2和时刻3可以称为非关键帧时刻。
值得注意的是，为了实现联合初始化，需要可移动设备运行一段时间，并通过这段时间内拍摄到的图像和测量到的IMU测量信息来进行联合初始化。其中，N个关键帧时刻可以是从这段时间内的第一个关键帧时刻开始确定的N个关键帧时刻。
其中，位姿信息可以包括位置信息和姿态信息，IMU可以包括陀螺仪和加速度计。N个关键帧时刻中相邻两个关键帧时刻之间的IMU预积分测量信息是指N个关键帧时刻中相邻两个关键帧时刻之间的相对姿态增量、相对位置增量和相对速度增量。N个关键帧时刻中相邻两个关键帧时刻之间的IMU预积分测量信息可以通过相邻两个关键帧时刻之间的IMU测量信息来确定，其中，IMU测量信息包括陀螺仪测量得到的角速度，以及加速度计测量得到的加速度。
接下来，通过如下步骤(1)-步骤(5)对上述步骤401的操作进行说明：
步骤(1)：对于N个关键帧时刻中的任一关键帧时刻A，根据关键帧时刻A对应的初始关键帧图像包括的特征点的图像坐标、与关键帧时刻A对应的初始关键帧图像包括的特征点对应的地图点的三维位置信息、存储的每个第一摄像头的内参，以及每个三维图像拍摄部件相对于IMU的旋转平移变换矩阵，构建第二重投影误差约束代价函数。
需要说明的是，关键帧时刻A对应的初始关键帧图像是指至少两个三维图像拍摄部件包括的第一摄像头在关键帧时刻A分别拍摄的图像。关键帧时刻A对应的初始关键帧图像的个数可以是至少两个。其中，第一摄像头可以是三维图像拍摄部件中位于左侧的摄像头。
另外，特征点指的是图像灰度值发生剧烈变化的点或者在图像边缘上曲率较大的点。初始关键帧图像包括的特征点的图像坐标可以是特征点在初始关键帧图像的像素坐标系下的像素坐标。该像素坐标系为二维坐标系，该像素坐标为二维坐标，也即特征点的图像坐标为二维坐标。
关键帧时刻A对应的初始关键帧图像包括的特征点可以通过ORB(Oriented FASTand Rotated BRIEF，一种快速特征点提取和描述的算法)、DAISY(一种特征点提取算法)和SIFT(Scale-invariant feature transform，尺度不变特征变换)等算法来确定。另外，在确定出关键帧时刻A对应的初始关键帧图像包括的特征点之后，还可以通过上述方法确定出特征点对应的描述子，其中，描述子用于描述特征点的属性，每个特征点对应唯一一个描述子。
需要说明的是，地图点的三维位置信息是指已构建的地图中的地图点在世界坐标系下的三维世界坐标，其中，世界坐标系是可移动设备的绝对坐标系，世界坐标系可以根据使用需求预先进行设置。
其中，已构建的地图中还可以包括地图点对应的描述子。这样，可以将关键帧时刻A对应的初始关键帧图像包括的特征点对应的描述子和已构建的地图中的地图点的描述子进行对比，从而在已构建的地图中确定描述子与特征点的描述子相同或者差值在预设阈值范围内的地图点，作为该特征点对应的地图点。该预设阈值范围可以预先进行设置，本申请实施例对此不做具体限定。
另外，每个三维图像拍摄部件包括的第一摄像头的内参可以通过对每个三维图像拍摄部件包括的每个摄像头进行内参标定来获得。可选地，可以通过张正友标定法来对每个三维图像拍摄部件包括的每个摄像头进行内参标定。
再者，每个三维图像拍摄部件相对于IMU的旋转平移变换矩阵，可以通过对每个三维图像拍摄部件与IMU之间进行外参标定来获得。当对每个三维图像拍摄部件与IMU之间进行外参标定时，可以将每个三维图像拍摄部件看做一个整体，那么这个整体相对于IMU就会存在一个旋转平移变换矩阵。可选地，可以通过Kalibr算法来对每个三维图像拍摄部件与IMU之间进行外参标定。
由于至少两个三维图像拍摄部件中的每个三维图像拍摄部件相对于IMU都存在一个旋转平移变换矩阵。这样的条件下，每个三维图像拍摄部件在关键帧时刻A都对应着一个重投影误差约束代价函数，将每个三维图像拍摄部件对应的重投影误差约束代价函数相加，即可得到第二重投影误差约束代价函数。
在一种可能的实现方式中，重投影误差约束代价函数可以通过如下公式一表示，为了便于描述，以至少两个三维图像拍摄部件中的任一三维图像拍摄部件B为例，对关键帧时刻A的重投影误差约束代价函数进行说明，其中，三维图像拍摄部件B包括第一摄像头和第二摄像头。
公式一：
其中，M为三维图像拍摄部件B对应的初始关键帧图像包括的特征点中与已构建的地图中的地图点对应的特征点的个数，m为M中的任一数值，也即是公式一中的为三维图像拍摄部件B对应的初始关键帧图像包括的特征点中与已构建的地图中的地图点对应的第m个特征点的图像坐标，pm为与/>对应的地图点的三维世界坐标，当pm对应的地图点只能在三维图像拍摄部件B中的第一摄像头观测到时/>当pm对应的地图点在三维图像拍摄部件B中的第一摄像头和第二摄像头都能观测到时/>π为三维图像拍摄部件B对应的初始关键帧图像包括的特征点中与已构建的地图中的地图点对应的第m个特征点的投影函数，/>为三维图像拍摄部件B相对于IMU的旋转平移变换矩阵的逆矩阵，/>为关键帧时刻A可移动设备的位姿矩阵的逆矩阵，其中/>为公式一中的未知量。
需要说明的是，Twb为关键帧时刻A可移动设备的位姿矩阵，而位姿信息通常可以通过位姿矩阵来表示，位置信息通常可以通过位置矩阵来表示，姿态信息通常可以通过姿态矩阵来表示，也即，位姿矩阵可以包括位置矩阵和姿态矩阵。因此，Twb也可以通过如下公式二来表示：
公式二：
其中，Rwb为关键帧时刻A可移动设备的姿态矩阵，pwb为关键帧时刻A可移动设备的位置矩阵。
步骤(2)：最小化第二重投影误差约束代价函数，以得到可移动设备在关键帧时刻A的位姿信息。
需要说明的是，在一种可能的实现方式中，最小化第二重投影误差约束代价函数可以通过高斯-牛顿迭代算法来实现，本申请实施例对高斯-牛顿迭代算法不做详细阐述。
另外，由于关键帧时刻A为N个关键帧时刻中的任一关键帧时刻，因此，不仅可以通过上述步骤(1)-步骤(2)来确定可移动设备在关键帧时刻A的位姿信息，还可以通过上述步骤(1)-步骤(2)来确定可移动设备在其他关键帧时刻的位姿信息，本申请实施例对其他关键帧时刻的位姿信息的确定过程不进行详细阐述。
下面对确定N个关键帧时刻中相邻两个关键帧时刻之间的IMU预积分测量信息的过程进行说明。
步骤(3)：根据相邻两个关键帧时刻之间陀螺仪测量得到的角速度、相邻两个关键帧时刻中前一个关键帧时刻陀螺仪的漂移误差和第一时长，确定相邻两个关键帧时刻之间的相对姿态增量，以及相邻两个关键帧时刻中前一个关键帧时刻与至少一个测量时刻中每个测量时刻之间的相对姿态增量，该至少一个测量时刻是指相邻两个关键帧时刻之间的测量时刻，相邻两个关键帧时刻中前一个关键帧时刻是指相邻两个关键帧时刻中时间较早的一个关键帧时刻，第一时长是IMU的相邻两个测量时刻之间的时长。
需要说明的是，陀螺仪的漂移误差可以是由于供电电源电压不稳、电子学元件老化、光电倍增管暗电流大和环境温度变化等原因造成的不可避免的误差，且陀螺仪的漂移误差在可移动设备的初始状态就存在，并随着时间而缓慢变化。
另外，相邻两个关键帧时刻之间的相对姿态增量，以及相邻两个关键帧时刻中前一个关键帧时刻与至少一个测量时刻中每个测量时刻之间的相对姿态增量可以通过如下公式三来确定。由于一个关键帧时刻也是一个测量时刻，且相邻两个关键帧时刻之间可能存在至少一个非关键帧时刻，也即是，相邻两个关键帧时刻之间可能存在至少一个测量时刻，因此，步骤401中是对可移动设备在运行过程中的所有测量时刻按照顺序进行编号，并不对关键帧时刻单独按照另一顺序进行编号。也即是，公式三中的i表示作为关键帧时刻的第i个测量时刻，公式三中的j表示作为关键帧时刻的第j个测量时刻，第i个测量时刻与第j个测量时刻是相邻的两个关键帧时刻，且第i个测量时刻早于第j个测量时刻，对于步骤401后续出现的第i个测量时刻与第j个测量时刻均是如此。
公式三：
其中，ΔRij为第i个测量时刻与第j个测量时刻之间的相对姿态增量，即，相邻两个关键帧时刻之间的相对姿态增量，为第i个测量时刻可移动设备的姿态矩阵的转置矩阵，Rj为第j个测量时刻可移动设备的姿态矩阵，Exp为以自然常数e为底的指数函数，/>为陀螺仪在第k个测量时刻测量得到的角速度，/>为第i个测量时刻陀螺仪的漂移误差，Δt为IMU的相邻两个测量时刻之间的时长，即第一时长。
例如，相邻的第i个测量时刻与第j个测量时刻之间间隔了两个测量时刻，将作为关键帧时刻的第i个测量时刻记为1，那么作为关键帧时刻的第j个测量时刻即为4，则通过上述公式三可以将第i个测量时刻与第j个测量时刻之间的相对位姿增量，即相邻两个关键帧时刻之间的相对位姿增量表示为：
且通过上述公式三可以将第i个测量时刻与间隔的两个测量时刻中每个测量时刻之间的相对姿态增量，即相邻两个关键帧时刻中前一个关键帧时刻与间隔的两个测量时刻中每个测量时刻之间相对姿态增量表示为：
步骤(4)：根据相邻两个关键帧时刻中前一个关键帧时刻与该至少一个测量时刻中每个测量时刻之间的相对姿态增量、相邻两个关键帧时刻之间加速度计测量得到的加速度、相邻两个关键帧时刻中前一个关键帧时刻加速度计的漂移误差和第一时长，确定相邻两个关键帧时刻之间的相对速度增量。
需要说明的是，加速度计的漂移误差可以是由于供电电源电压不稳、电子学元件老化、光电倍增管暗电流大和环境温度变化等原因造成的不可避免的误差，且加速度计的漂移误差在可移动设备的初始状态就存在，并随着时间而缓慢变化。
在一种可能的实现方式中，相邻两个关键帧时刻之间的相对速度增量可以通过如下公式四来确定：
公式四：
其中，Δvij为第i个测量时刻与第j个测量时刻之间的相对速度增量，即相邻两个关键帧时刻之间的相对速度增量，ΔRik为第i个测量时刻与第k个测量时刻之间的相对姿态增量，为第k个测量时刻加速度计测量得到的加速度，/>为第i个测量时刻加速度计的漂移误差，Δt为第一时长。
例如，相邻两个关键帧时刻之间间隔了两个测量时刻，将作为关键帧时刻的第i个测量时刻记为1，那么作为关键帧时刻的第j个测量时刻即为4，则通过上述公式四可以将第i个测量时刻与第j个测量时刻之间的相对位姿增量，即相邻两个关键帧时刻之间的相对速度增量表示为：
其中，ΔR11为一个3×3的单位矩阵。
步骤(5)：根据相邻两个关键帧时刻中前一个关键帧时刻与该至少一个测量时刻中每个测量时刻之间的相对姿态增量、相邻两个关键帧时刻中前一个关键帧时刻与该至少一个测量时刻中每个测量时刻之间的相对速度增量、相邻两个关键帧时刻之间加速度计测量得到的加速度、相邻两个关键帧时刻中前一个关键帧时刻加速度计的漂移误差和第一时长，确定相邻两个关键帧时刻之间的相对位置增量。
需要说明的是，在一种可能的实现方式中，相邻两个关键帧时刻之间的相对位置增量可以通过如下公式五来确定：
公式五：
其中，Δpij为第i个测量时刻与第j个测量时刻之间的相对位置增量，即相邻两个关键帧时刻之间的相对位置增量，Δvik为第i个测量时刻与第k个测量时刻之间的相对速度增量。
例如，相邻两个关键帧时刻之间间隔了两个测量时刻，将作为关键帧时刻的第i个测量时刻记为1，那么作为关键帧时刻的第j个测量时刻即为4，则通过上述公式五可以将第i个测量时刻与第j个测量时刻之间的相对位置增量，即相邻两个关键帧时刻之间的相对位置增量表示为：
其中，Δv11为零，Δv12和Δv13可以通过公式四确定出。
至此，可以根据上述步骤(1)-步骤(5)将可移动设备在N个关键帧时刻的位姿信息，以及N个关键帧时刻中相邻两个关键帧时刻之间的IMU预积分测量信息确定出来，由于N是大于或等于4的正整数，所以，可以确定出多个IMU预积分测量信息。例如当N等于4时，可以将该4个关键帧时刻中第一个关键帧时刻与第二个关键帧时刻、第二个关键帧时刻与第三个关键帧时刻，以及第三个关键帧时刻与第四个关键帧时刻之间的IMU预积分测量信息通过上述步骤(1)-步骤(5)分别确定出来。
根据上述步骤401确定的可移动设备在该N个关键帧时刻的位姿信息，以及该N个关键帧时刻中相邻两个关键帧时刻之间的IMU预积分测量信息之后，可以通过如下步骤402继续对该N个关键帧时刻中第N个关键帧时刻的漂移误差和可移动设备的实际重力向量进行确定。
步骤402：根据可移动设备在N个关键帧时刻的位姿信息、N个关键帧时刻中相邻两个关键帧时刻之间的时长、N个关键帧时刻中相邻两个关键帧时刻之间的IMU预积分测量信息，以及惯性坐标系的重力方向向量，确定IMU在第N个关键帧时刻的漂移误差和可移动设备的实际重力向量。
需要说明的是，惯性坐标系是指对牛顿运动定律适用的定坐标系，在一种可能的实现方式中，惯性坐标系的坐标原点可以是世界坐标系的原点，惯性坐标系的z轴可以指向地心，惯性坐标系的重力方向向量可以是与惯性坐标系中的z轴平行，且在x轴和y轴没有分量的向量，也即惯性坐标系的重力方向向量可以表示为[0,0,1]。
另外，基于上述描述，IMU包括陀螺仪和加速度计，因此，IMU在N个关键帧时刻的漂移误差是指陀螺仪在N个关键帧时刻的漂移误差和加速度计在N个关键帧时刻的漂移误差。
再者，可移动设备的实际重力向量是该可移动设备在世界坐标系下的重力向量。
其中，步骤402的操作可以通过如下步骤(1)-步骤(3)来实现：
步骤(1)：根据可移动设备在N个关键帧时刻的姿态信息和N个关键帧时刻中相邻两个关键帧时刻之间的相对姿态增量，构建漂移误差代价函数。
在一种可能的实现方式中，漂移误差代价函数可以通过如下公式六来表示：
公式六：
其中，上述步骤401是对可移动设备在运行过程中的所有测量时刻按照顺序编号，并不对关键帧时刻单独按照另一顺序编号，而在步骤402中，为了便于描述，对N个关键帧又重新编号，也即是，ΔRi,i+1为N个关键帧时刻中第i个关键帧时刻和第i+1个关键帧时刻之间的相对姿态增量，为N个关键帧时刻中第i个关键帧时刻和第i+1个关键帧时刻之间的相对姿态增量对bg的雅克比函数，bg为陀螺仪的漂移误差，/>为N个关键帧时刻中第i+1个关键帧时刻的姿态矩阵的转置矩阵，Ri为N个关键帧时刻中第i个关键帧时刻的姿态矩阵。其中，bg为公式六中的未知量。
需要说明的是，在进行联合初始化时，可以认为陀螺仪在N个关键帧时刻的漂移误差都相同，因此，在上述公式六中未对陀螺仪的漂移误差所处的时刻进行区分。
步骤(2)：最小化漂移误差代价函数，以得到陀螺仪在第N个关键帧时刻的漂移误差。
需要说明的是，对漂移误差代价函数的最小化，可以通过高斯-牛顿迭代算法来实现。
步骤(3)：根据可移动设备在N个关键帧时刻的位姿信息、N个关键帧时刻中相邻两个关键帧时刻之间的时长、N个关键帧时刻中相邻两个关键帧时刻之间的相对位置增量和相对速度增量，以及惯性坐标系的重力方向向量，确定加速度计在第N个关键帧时刻的漂移误差和可移动设备的实际重力向量。
其中，步骤(3)的操作可以通过如下步骤a-步骤d来实现：
步骤a：根据可移动设备在N个关键帧时刻的位姿信息、N个关键帧时刻中相邻两个关键帧时刻之间的时长，以及N个关键帧时刻中相邻两个关键帧时刻之间的相对位置增量和相对速度增量，确定可移动设备的估计重力向量。
需要说明的是，在一种可能的实现方式中，估计重力向量可以是在忽略加速度计的漂移误差的条件下确定出来的。可移动设备的估计重力向量可以通过如下公式七至公式九来确定，即，按照公式七至公式九，通过对N个关键帧时刻中每相邻的三个关键帧时刻之间的数据进行整合，然后将整合后得到的至少两组数据进行SVD(Singular ValueDecomposition，奇异值分解)，从而确定出可移动设备的估计重力向量。为了方便描述，将公式中相邻的三个关键帧时刻记为第1个关键帧时刻、第2个关键帧时刻和第3个关键帧时刻，实际应用中不限于这三个关键帧时刻：
公式七：A(i)g＝B(i)
公式八：
公式九：B(i)＝(p3-p2)Δt12-(p2-p1)Δt23+R1Δp12Δt23-R2Δp23Δt12-R1Δv12Δt12Δt23
其中，I3×3为一个三行三列的单位矩阵，Δt12为N个关键帧时刻中第1个关键帧时刻与第2个关键帧时刻之间的时长，Δt23为N个关键帧时刻中第2个关键帧时刻与第3个关键帧时刻之间的时长，p1为N个关键帧时刻中第1个关键帧时刻可移动设备的位置矩阵，p2为N个关键帧时刻中第2个关键帧时刻可移动设备的位置矩阵，p3为N个关键帧时刻中第3个关键帧时刻可移动设备的位置矩阵，p12为N个关键帧时刻中第1个关键帧时刻与第2个关键帧时刻之间的相对位置增量，p23为N个关键帧时刻中第2个关键帧时刻与第3个关键帧时刻之间的相对位置增量，R1为N个关键帧时刻中第1个关键帧时刻可移动设备的姿态矩阵，R2为N个关键帧时刻中第2个关键帧时刻可移动设备的姿态矩阵，Δv12为N个关键帧时刻中第1个关键帧时刻与第2个关键帧时刻之间的相对速度增量。
例如，当N个关键帧时刻中N为4时，这4个关键帧时刻中，第1个关键帧时刻、第2个关键帧时刻和第3个关键帧时刻可以通过上述公式七至公式九整合出一组数据；第2个关键帧时刻、第3个关键帧时刻和第4个关键帧时刻可以通过上述公式七至公式九整合出一组数据，将这两组数据进行SVD，就可以确定出可移动设备的估计重力向量。
步骤b：根据可移动设备的估计重力向量和惯性坐标系的重力方向向量，确定惯性坐标系到世界坐标系的旋转变换矩阵。
需要说明的是，在一种可能的实现方式中，惯性坐标系到世界坐标系的旋转变换矩阵可以根据如下公式十来确定：
公式十：
其中，RWI为惯性坐标系到世界坐标系的旋转变换矩阵，为惯性坐标系到世界坐标系的旋转轴的单位向量，θ为惯性坐标系到世界坐标系的旋转轴的旋转角度。
其中，和θ可以分别根据如下公式十一和公式十三来确定：
公式十一：
公式十二：
公式十三：
其中，g*为可移动设备的估计重力向量，|g*|为可移动设备的估计重力向量的模，为可移动设备的估计重力向量的单位向量，gI为惯性坐标系的重力方向向量的转置。可选地，gI可以为[0,0,1]T。
步骤c：根据惯性坐标系到世界坐标系的旋转变换矩阵、惯性坐标系的重力方向向量、N个关键帧时刻中相邻两个关键帧时刻之间的时长、可移动设备在N个关键帧时刻的位姿信息，以及N个关键帧时刻中相邻两个关键帧时刻之间的相对位置增量和相对速度增量，确定加速度计在第N个关键帧时刻的漂移误差和可移动设备的重力方向修正量。
需要说明的是，在一种可能的实现方式中，重力方向修正量可以根据如下公式十四来表示，可移动设备的实际重力向量可以根据如下公式十五来表示：
公式十四：δθ＝[δθx,δθy,0]T
公式十五：gW＝RWIgIG
其中，δθ为可移动设备的重力方向修正量，δθx为可移动设备的重力方向修正量在x轴的分量，δθy为可移动设备的重力方向修正量在y轴的分量，gW为可移动设备的实际重力向量，G为重力加速度，即9.8m/s2(米/平方秒)。
另外，加速度计在第N个关键帧时刻的漂移误差和可移动设备的重力方向修正量可以根据如下公式十六至公式十九来确定，与步骤a类似地，按照公式十六至公式十九，通过对N个关键帧时刻中每相邻的三个关键帧时刻之间的数据进行整合，然后将整合后得到的至少两组数据进行SVD，从而确定出加速度计在第N个关键帧时刻的漂移误差和可移动设备的重力方向修正量，为了方便描述，将公式中相邻的三个关键帧时刻记为第1个关键帧时刻、第2个关键帧时刻和第3个关键帧时刻，实际应用中不限于这三个关键帧时刻：
公式十六：/>
公式十七：
公式十八：
公式十九：
其中，δθ(1:2)为取δθ中第一行和第二行的元素组成的矩阵，为N个关键帧时刻中第2个关键帧时刻与第3个关键帧时刻之间的相对位置增量对ba的雅克比函数，/>为N个关键帧时刻中第1个关键帧时刻与第2个关键帧时刻之间的相对速度增量对ba的雅克比函数，/>为N个关键帧时刻中第1个关键帧时刻与第2个关键帧时刻之间相对位置增量对ba的雅克比函数。
例如，当N个关键帧时刻中N为4时，这4个关键帧时刻中，第1个关键帧时刻、第2个关键帧时刻和第3个关键帧时刻可以通过上述公式十六至公式十九整合出一组数据；第2个关键帧时刻、第3个关键帧时刻和第4个关键帧时刻可以通过上述公式十六至公式十九整合出一组数据，将这两组数据进行SVD，就可以确定出加速度计在第N个关键帧时刻的漂移误差和可移动设备的重力方向修正量。
值得注意的是，加速度计在第N个关键帧时刻的漂移误差，可以作为加速度计在该联合初始化阶段的N个关键帧时刻中每个关键帧时刻的漂移误差。
步骤d：根据可移动设备的重力方向修正量、惯性坐标系到世界坐标系的旋转变换矩阵和惯性坐标系的重力向量，确定可移动设备的实际重力向量。
由于惯性坐标系到世界坐标系的旋转变换矩阵只有两个自由度，因此，为了提高可移动设备的实际重力向量的准确性，可以定义可移动设备的重力方向修正量。之后，根据可移动设备的重力方向修正量、惯性坐标系到世界坐标系的旋转变换矩阵和惯性坐标系的重力向量，确定的可移动设备的实际重力向量的准确性较高。
需要说明的是，在一种可能的实现方式中，可移动设备的实际重力向量可以通过如下公式二十来确定：
公式二十：gW＝RWIExp(δθ)gIG≈RWIgIG-RWI(gI)×Gδθ
其中，(gI)×为gI的反对称矩阵。
将步骤a至步骤c确定出的惯性坐标系到世界坐标系的旋转变换矩阵RWI和可移动设备的重力方向修正量δθ，以及惯性坐标系的重力向量gI和重力加速度G代入公式二十即可确定出可移动设备的实际重力向量gW。
步骤403：根据可移动设备在第N-1个关键帧时刻的位姿信息、可移动设备的实际重力向量、第N-1个关键帧时刻与第N-2个关键帧时刻之间的IMU预积分测量信息、第N-1个关键帧时刻与第N-2个关键帧时刻之间的时长、第N-1个关键帧时刻与第N个关键帧时刻之间的时长和IMU在第N-1个关键帧时刻测量得到的IMU测量信息，确定可移动设备在第N个关键帧时刻的速度。
具体地，步骤403的操作可以通过如下步骤(1)-步骤(2)来实现：
步骤(1)：根据可移动设备在第N-1个关键帧时刻的位姿信息、第N-1个关键帧时刻与第N-2个关键帧时刻之间的时长、可移动设备的实际重力向量、第N-1个关键帧时刻与第N-2个关键帧时刻之间的相对位置增量，确定可移动设备在第N-1个关键帧时刻的速度。
需要说明的是，在一种可能的实现方式中，可移动设备在第N-1个关键帧时刻的速度可以根据如下公式二十一来确定，为了便于描述，将公式二十一中的i确定为N-2，则j即为N-1：
公式二十一：
其中，pj为N个关键帧中第j个关键帧时刻可移动设备的位置矩阵，pi为N个关键帧中第i个关键帧时刻可移动设备的位置矩阵，Δpij为N个关键帧中第i个关键帧时刻与第j个关键帧时刻之间的相对位置增量，vi为N个关键帧中第i个关键帧时刻可移动设备的速度矩阵，Δtij为N个关键帧中第i个关键帧时刻与第j个关键帧时刻之间的时长。其中vi为公式二十一中的未知量。
当公式二十一中的i为N-2时，可以确定出可移动设备在第N-1个关键帧时刻的速度。
步骤(2)：根据可移动设备在第N-1个关键帧时刻的速度、第N-1个关键帧时刻与第N个关键帧时刻之间的时长和加速度计在第N-1个关键帧时刻测量得到的加速度，确定可移动设备在第N个关键帧时刻的速度。
需要说明的是，在一种可能的实现方式中，可移动设备在第N个关键帧时刻的速度可以根据如下公式二十二来确定：
公式二十二：v(t+ΔtN-1,N)＝v(t)+aw(t)ΔtN-1,N
对于确定可移动设备在第N个关键帧时刻的速度而言，公式二十二可以转变为如下公式二十三：
公式二十三：
其中，vN为第N个关键帧时刻可移动设备的速度，vN-1为第N-1个关键帧时刻可移动设备的速度，为第N-1个关键帧时刻可移动设备的加速度真值，ΔtN-1,N为第N-1个关键帧时刻与第N个关键帧时刻之间的时长。其中，vN和/>为公式二十三中的未知量。
其中，可以通过如下公式二十四来确定：
公式二十四：
其中，为第N-1个关键帧时刻加速度计测量得到的加速度，/>为第N-1个关键帧时刻可移动设备的姿态矩阵的转置矩阵，/>为第N个关键帧时刻加速度计的漂移误差，为第N个关键帧时刻加速度计的测量白噪声。其中/>和/>为公式二十四中的未知量。
在通过公式二十四确定时，可以将/>设置为零，继而可以确定出/>然后再将确定出的/>代入公式二十三中即可确定出确定可移动设备在第N个关键帧时刻的速度。
至此，对可移动设备中的至少两个三维图像拍摄部件和IMU进行联合初始化的过程已经完成，在该联合初始化过程中，确定出了可移动设备在N个关键帧时刻的位姿信息、IMU在第N个关键帧时刻的漂移误差、可移动设备的实际重力向量以及可移动设备在第N个关键帧时刻的速度。对于第N个关键帧时刻之后的任一测量时刻，即当前时刻，可以根据第N个关键帧时刻可移动设备的位姿信息、速度、IMU的漂移误差以及当前时刻与第N个关键帧时刻之间的IMU预积分测量信息，通过如下步骤501-步骤504确定出当前时刻可移动设备的位姿信息、速度以及IMU的漂移误差。但由于联合初始化的过程可能是在当前时刻较早之前进行的，所以当前时刻的上一个关键帧时刻可能不是该N个关键帧时刻中的第N个关键帧时刻，因此，对于步骤501-步骤504中提到的上一关键帧时刻不限于是该联合初始化中第N个关键帧时刻。
参见图5，在对可移动设备中的至少两个三维图像拍摄部件和IMU进行联合初始化的过程进行说明之后，接下来通过步骤501-步骤504对可移动设备的位姿信息的实时更新过程进行说明。
步骤501：根据当前时刻与上一关键帧时刻之间的IMU测量信息，以及第一时长，确定当前时刻与上一关键帧时刻之间的IMU预积分测量信息。
需要说明的是，上一关键帧时刻是指每个三维图像拍摄部件最近一次拍摄的能够作为关键帧图像的图像的拍摄时刻。
具体地，根据当前时刻与上一关键帧时刻之间陀螺仪测量得到的角速度、上一关键帧时刻陀螺仪的漂移误差和第一时长，确定当前时刻与上一关键帧时刻之间的相对姿态增量，以及上一关键帧时刻与至少一个测量时刻中每个测量时刻之间的相对姿态增量，至少一个测量时刻是指当前时刻与上一关键帧时刻之间的测量时刻。根据上一关键帧时刻与该至少一个测量时刻中每个测量时刻之间的相对姿态增量、当前时刻与上一关键帧时刻之间加速度计测量得到的加速度、上一关键帧时刻加速度计的漂移误差和第一时长，确定当前时刻与上一关键帧时刻之间的相对速度增量。根据上一关键帧时刻与该至少一个测量时刻中每个测量时刻之间的相对姿态增量、上一关键帧时刻与该至少一个测量时刻中每个测量时刻之间的相对速度增量、当前时刻与上一关键帧时刻之间加速度计测量得到的加速度、上一关键帧时刻加速度计的漂移误差和第一时长，确定当前时刻与上一关键帧时刻之间的相对位置增量。
需要说明的是，由于当前时刻与上一关键帧时刻之间可能存在至少一个测量时刻，也可能不存在测量时刻，当当前时刻与上一关键帧时刻之间存在至少一个测量时刻时，确定当前时刻与上一关键帧时刻之间的IMU预积分测量信息的步骤，与上述联合初始化过程中确定相邻两个关键帧时刻之间的IMU预积分测量信息的类似。当当前时刻与上一关键帧时刻之间不存在测量时刻时，确定当前时刻与上一关键帧时刻之间的IMU预积分测量信息的步骤，与上述联合初始化过程中确定第i个测量时刻与第i+1个测量时刻之间的IMU预积分测量信息的类似，所以此处对确定当前时刻与上一关键帧时刻之间的IMU预积分测量信息的过程不再做详细说明。
进一步地，在步骤501之前，还可以包括如下步骤(1)-步骤(4)：
步骤(1)：可移动设备每隔第一时长获取一个IMU数据包，IMU数据包至少包括IMU测量信息和对应的测量时间戳，其中，每个三维图像拍摄部件每隔第二时长拍摄一次图像，第二时长大于第一时长，且可移动设备获取的IMU数据包还包括触发累加序号。
需要说明的是，测量时间戳是能表示IMU测量信息在某个特定时间之前已经存在的、完整的、可验证的数据，通常测量时间戳是一个字符序列，唯一地标识某一刻的时间。在本申请实施例中，IMU测量信息对应的测量时间戳用于指示IMU测量信息的测量时刻。
步骤(2)：当每个三维图像拍摄部件当前时刻拍摄到图像时，可移动设备确定拍摄累加序号，并根据拍摄累加序号从获取的IMU数据包中获取目标IMU数据包，目标IMU数据包中的触发累加序号与拍摄累加序号相同。
步骤(3)：可移动设备将目标IMU数据包中的测量时间戳与存储的时间偏置之和，确定为每个三维图像拍摄部件当前时刻拍摄的图像的拍摄时间戳。
需要说明的是，由于至少两个三维图像拍摄部件被可移动设备触发后，需要进行曝光一段时间才能拍摄到图像，所以当可移动设备接收到每个三维图像拍摄部件当前时刻拍摄的图像对应的时间要比该图像对应的IMU数据包中的测量时间戳对应的时间晚，这两个时间之间的差值即为时间偏置。
可选地，时间偏置可以通过对至少两个三维图像拍摄部件进行外参标定来获得。
步骤(4)：可移动设备从获取的IMU数据包中，获取测量时间戳处于上一关键帧时刻拍摄的图像的拍摄时间戳与当前时刻拍摄的图像的拍摄时间戳之间的IMU测量信息，以得到当前时刻与上一关键帧时刻之间的IMU测量信息。
在确定当前时刻与上一关键帧时刻之间的IMU预积分测量信息之后，可以根据至少两个第一图像、存储的每个三维图像拍摄部件包括的第一摄像头的内参、每个三维图像拍摄部件相对于IMU的旋转平移变换矩阵、当前时刻与上一关键帧时刻之间的IMU预积分测量信息、当前时刻与上一关键帧时刻之间的时长、存储的上一关键帧时刻可移动设备的系统状态，以及可移动设备的实际重力向量，确定当前时刻可移动设备的系统状态。接下来将分成两个步骤进行说明。
步骤502：确定至少两个第一图像包括的特征点的图像坐标，以及与每个第一图像包括的特征点对应的地图点的三维位置信息，根据至少两个第一图像包括的特征点的图像坐标、与每个第一图像包括的特征点对应的地图点的三维位置信息、存储的每个三维图像拍摄部件包括的第一摄像头的内参，以及每个三维图像拍摄部件相对于IMU的旋转平移变换矩阵，构建第一重投影误差约束代价函数。
需要说明的是，至少两个第一图像是指当前时刻至少两个三维图像拍摄部件包括的第一摄像头拍摄的图像。
另外，特征点的图像坐标、与每个第一图像包括的特征点对应的地图点的三维位置信息、存储的每个三维图像拍摄部件包括的第一摄像头的内参，以及每个三维图像拍摄部件相对于IMU的旋转平移变换矩阵已在上述步骤401中做过类似说明，本申请实施例对此不再赘述。
再者，第一重投影误差约束代价函数与上述步骤401中的第二重投影误差约束代价函数类似，本申请实施例对此不再赘述。
步骤503：根据当前时刻与上一关键帧时刻之间的IMU预积分测量信息、当前时刻与上一关键帧时刻之间的时长、存储的上一关键帧时刻可移动设备的系统状态，以及可移动设备的实际重力向量，构建IMU运动预测代价函数，系统状态包括可移动设备的位姿信息和速度，以及IMU的漂移误差。
需要说明的是，在一种可能实现方式中，IMU运动预测代价函数可以通过如下公式二十五至公式二十九，为了方便描述，公式二十五至公式二十九中的i表示上一关键帧时刻，公式中的j表示当前时刻。
公式二十五：
公式二十六：
公式二十七：
公式二十八：
公式二十九：
其中，∑pRv为关于p、R和v的协方差矩阵，∑b为关于bg和ba的协方差矩阵，Δtij为当前时刻与上一关键帧时刻之间的时长，为上一关键帧时刻陀螺仪的漂移误差，/>为当前时刻陀螺仪的漂移误差，/>上一关键帧时刻加速度计的漂移误差，/>当前时刻加速度计的漂移误差，Ri为上一关键帧时刻可移动设备的姿态矩阵，Rj为当前时刻可移动设备的姿态矩阵，ΔRi,j为当前时刻与上一关键帧时刻之间的相对姿态增量，/>为当前时刻与上一关键帧时刻之间的相对姿态增量相对于上一关键帧时刻陀螺仪的漂移误差的雅克比函数，vi为上一关键帧时刻可移动设备的速度，vj为当前时刻可移动设备的速度，Δvij为当前时刻与上一关键帧时刻之间的相对速度增量，/>为当前时刻与上一关键帧时刻之间的相对速度增量相对于上一关键帧时刻陀螺仪的漂移误差的雅克比函数，/>为当前时刻与上一关键帧时刻之间的相对速度增量相对于上一关键帧时刻加速度计的漂移误差的雅克比函数，pi为上一关键帧时刻可移动设备的位置矩阵，pj为当前时刻可移动设备的位置矩阵，Δpij为当前时刻与上一关键帧时刻之间的相对位置增量，/>为当前时刻与上一关键帧时刻之间的相对位置增量相对于上一关键帧时刻陀螺仪的漂移误差的雅克比函数，/>为当前时刻与上一关键帧时刻之间的相对位置增量相对于上一关键帧时刻加速度计的漂移误差的雅克比函数。其中/>Rj、vj和pj公式二十五至公式二十九中的未知量。
步骤504：最小化第一重投影误差约束代价函数与IMU运动预测代价函数之和，以得到当前时刻可移动设备的系统状态。
需要说明的是，最小化第一重投影误差约束代价函数与IMU运动预测代价函数之和可以通过高斯-牛顿迭代法实现，通过该高斯-牛顿迭代法可以确定出当前时刻可移动设备的系统状态。
值得注意的是，可移动设备在对可移动设备的位姿信息进行确定后，可移动设备可以根据该位置信息进行地图的实时更新，以及确定与周围障碍物之间的相对位置等，其中，在确定与周围障碍物之间的相对位置后可以使可移动设备能够有效地避开障碍物进行移动。
进一步地，在步骤504之后，还可以通过如下步骤(1)-步骤(2)来判断至少两个三维图像拍摄部件当前时刻拍摄的图像是否可以作为关键帧图像，从而便于后续对可移动设备的位姿进行更新：
步骤(1)：确定可移动设备当前时刻能够追踪到的地图点的比例。
需要说明的是，可移动设备当前时刻能够追踪到的地图点是指可移动设备中至少两个三维图像拍摄部件共同可以追踪到的地图点。也即是，可移动设备当前时刻能够追踪到的地图点的比例是指与至少两个第一图像包括的特征点对应的地图点的总量占至少两个第一图像包括的特征点的总量的比例。
步骤(2)：当可移动设备当前时刻能够追踪到的地图点的比例小于比例阈值，或者当前时刻与上一关键帧时刻之间的时长大于时长阈值时，将每个三维图像拍摄部件当前时刻拍摄的图像确定为关键帧图像。
需要说明的是，比例阈值和时长阈值可以预先进行设置，本申请实施例对此不做具体限定。例如，比例阈值可以为75％等，时长阈值可以为2s(秒)等。
更进一步地，由于地图是通过关键帧图像中的特征点来构建的，因此，在确定至少两个三维图像拍摄部件当前时刻拍摄的图像可以作为关键帧图像之后，还可以通过如下步骤A-步骤C对已构建的地图进行更新：
步骤A：根据每个三维图像拍摄部件对应的至少一对特征点的图像坐标、存储的每个第一摄像头的焦距，以及每个三维图像拍摄部件包括的两个摄像头之间的基线距离，确定每个第一图像包括的特征点的深度。
需要说明的是，一对特征点是指一个三维图像拍摄部件包括的两个摄像头当前时刻拍摄的图像中对应同一实物点的两个特征点。
另外，特征点的深度可以是特征点在第一摄像头的相机坐标系中的深度，其中，相机坐标系是以相机光心为原点的三维坐标系。
在一种可能的实现方式中，每个第一图像包括的特征点的深度可以通过如下公式三十来确定，为了方便描述，将以至少两个三维图像拍摄部件中的任一三维图像拍摄部件B为例，对通过公式三十确定第一图像包括的特征点的深度进行说明。其中，三维图像拍摄部件B包括两个摄像头，分别为第一摄像头和第二摄像头，第一摄像头当前时刻拍摄的图像称为第一图像B，第二摄像头当前时刻拍摄的图像称为第二图像B：
公式三十：
其中，d为第一图像B包括的特征点的深度，f为三维图像拍摄部件B中的第一摄像头的焦距，b为三维图像拍摄部件B中两个摄像头之间的基线距离，u为第一图像B中包括的特征点在像素坐标系中u轴方向的坐标，u*第二图像B中包括的特征点在像素坐标系中u轴方向的坐标。f为第一摄像头的焦距，且为第一摄像头的内参中的一个元素，b可以通过对第一个三维图像拍摄部件进行外参标定来获得。
步骤B：根据当前时刻可移动设备的位姿信息、每个三维图像拍摄部件相对于IMU的旋转平移变换矩阵、每个第一摄像头的内参，以及每个第一图像包括的特征点的图像坐标和深度，确定每个第一图像包括的特征点对应的地图点的三维位置信息。
需要说明的是，在一种可能的实现方式中，每个第一图像包括的特征点对应的地图点的三维位置信息可以通过如下公式三十一来确定，其中，每个第一图像包括的特征点对应的地图点的三维位置信息可以是该地图点的三维世界坐标。为了方便描述，将以至少两个三维图像拍摄部件中的任一三维图像拍摄部件B为例，对通过公式三十一确定每个第一图像包括的特征点对应的地图点的三维位置信息度进行说明。其中，三维图像拍摄部件B包括两个摄像头，分别为第一摄像头和第二摄像头，第一摄像头当前时刻拍摄的图像称为第一图像B：
公式三十一：
其中，um为第一图像B包括的特征点中与已构建的地图中的地图点对应的第m个特征点的在像素坐标系中u轴方向的坐标，vm为第一图像B包括的特征点中与已构建的地图中的地图点对应的第m个特征点的在像素坐标系中v轴方向的坐标，pm为已构建的地图中的地图点中与第一图像B包括的特征点对应的第m个地图点的三维世界坐标，Twb为当前时刻可移动设备的位姿矩阵，Tbc为三维图像拍摄部件B相对于IMU的旋转平移变换矩阵，K-1为三维图像拍摄部件B包括的第一摄像头的内参。其中pm为公式三十一中的未知量。
步骤C：根据每个第一图像包括的特征点对应的地图点的三维位置信息，对已构建的地图进行更新。
需要说明的是，对已构建的地图进行更新可以是将步骤B中确定的每个第一图像包括的特征点对应的地图点的三维位置信息添加到已构建的地图中。
在本申请实施例中，先根据至少两个第一图像包括的特征点的图像坐标、与每个第一图像包括的特征点对应的地图点的三维位置信息、存储的每个三维图像拍摄部件包括的第一摄像头的内参，以及每个三维图像拍摄部件相对于IMU的旋转平移变换矩阵，构建第一重投影误差约束代价函数。然后根据当前时刻与上一关键帧时刻之间的IMU测量信息，以及IMU的相邻两个测量时刻之间的时长，确定当前时刻与上一关键帧时刻之间的IMU预积分测量信息。之后再根据当前时刻与上一关键帧时刻之间IMU预积分测量信息、当前时刻与上一关键帧时刻之间的时长、存储的上一关键帧时刻可移动设备的系统状态，以及可移动设备的实际重力向量，构建IMU运动预测代价函数。由于系统状态包括可移动设备的位姿信息和速度，以及IMU的漂移误差，因此，在最小化第一重投影误差约束代价函数与IMU运动预测代价函数之和，得到当前时刻可移动设备的系统状态之后，即可确定当前时刻可移动设备的位姿信息。由于本申请实施例是将至少两个三维图像拍摄部件与IMU进行联合，以共同对可移动设备的位姿信息进行确定，避免了因三维图像拍摄部件可能无法采集到纹理较为清楚的图像，而导致确定出的位姿信息较为不准确的问题，进而提高了确定位姿信息的准确性。
图6是本申请实施例提供的一种位姿信息确定装置的结构示意图，应用于可移动设备，该可移动设备包括至少两个三维图像拍摄部件和一个惯性测量单元IMU。参见图6，该装置包括：第一确定模块601和第二确定模块602。
第一确定模块602，用于根据当前时刻与上一关键帧时刻之间的IMU测量信息，以及第一时长，确定当前时刻与上一关键帧时刻之间的IMU预积分测量信息，所述上一关键帧时刻是指每个三维图像拍摄部件最近一次拍摄的能够作为关键帧图像的图像的拍摄时刻，所述第一时长是指所述IMU的相邻两个测量时刻之间的时长；
第二确定模块602，用于根据至少两个第一图像、存储的每个三维图像拍摄部件包括的第一摄像头的内参、每个三维图像拍摄部件相对于所述IMU的旋转平移变换矩阵、所述IMU预积分测量信息、当前时刻与上一关键帧时刻之间的时长、存储的上一关键帧时刻所述可移动设备的系统状态，以及所述可移动设备的实际重力向量，确定当前时刻所述可移动设备的系统状态，所述至少两个第一图像是指所述至少两个三维图像拍摄部件包括的第一摄像头当前时刻分别拍摄的图像，所述系统状态包括所述可移动设备的位姿信息和速度，以及所述IMU的漂移误差。
可选地，第二确定模块602包括：
第一确定子模块，用于确定所述至少两个第一图像包括的特征点的图像坐标，以及与每个第一图像包括的特征点对应的地图点的三维位置信息；
第一构建子模块，用于根据所述至少两个第一图像包括的特征点的图像坐标、与每个第一图像包括的特征点对应的地图点的三维位置信息、存储的每个三维图像拍摄部件包括的第一摄像头的内参，以及每个三维图像拍摄部件相对于所述IMU的旋转平移变换矩阵，构建第一重投影误差约束代价函数；
第二构建子模块，用于根据所述IMU预积分测量信息、当前时刻与上一关键帧时刻之间的时长、存储的上一关键帧时刻所述可移动设备的系统状态，以及所述可移动设备的实际重力向量，构建IMU运动预测代价函数；
第一最小化运算子模块，用于最小化所述第一重投影误差约束代价函数与所述IMU运动预测代价函数之和，以得到当前时刻所述可移动设备的系统状态。
可选地，所述IMU包括陀螺仪和加速度计，所述IMU测量信息包括所述陀螺仪测量得到的角速度和所述加速度计测量得到的加速度，所述IMU预积分测量信息包括相对姿态增量、相对速度增量和相对位置增量；
第一确定模块601包括：
第二确定子模块，用于根据当前时刻与上一关键帧时刻之间所述陀螺仪测量得到的角速度、上一关键帧时刻所述陀螺仪的漂移误差和所述第一时长，确定当前时刻与上一关键帧时刻之间的相对姿态增量，以及上一关键帧时刻与至少一个测量时刻中每个测量时刻之间的相对姿态增量，所述至少一个测量时刻是指当前时刻与上一关键帧时刻之间的测量时刻；
第三确定子模块，用于根据上一关键帧时刻与所述至少一个测量时刻中每个测量时刻之间的相对姿态增量、当前时刻与上一关键帧时刻之间所述加速度计测量得到的加速度、上一关键帧时刻所述加速度计的漂移误差和所述第一时长，确定当前时刻与上一关键帧时刻之间的相对速度增量；
第四确定子模块，用于根据上一关键帧时刻与所述至少一个测量时刻中每个测量时刻之间的相对姿态增量、上一关键帧时刻与所述至少一个测量时刻中每个测量时刻之间的相对速度增量、当前时刻与上一关键帧时刻之间所述加速度计测量得到的加速度、上一关键帧时刻所述加速度计的漂移误差和所述第一时长，确定当前时刻与上一关键帧时刻之间的相对位置增量。
可选地，该装置还包括：
第一获取模块，用于每隔所述第一时长获取一个IMU数据包，所述IMU数据包至少包括IMU测量信息和对应的测量时间戳，其中，每个三维图像拍摄部件每隔第二时长拍摄一次图像，所述第二时长大于所述第一时长，且每隔所述第二时长获取的IMU数据包还包括触发累加序号；
第二获取模块，用于当每个三维图像拍摄部件当前时刻拍摄到图像时，确定拍摄累加序号，并根据所述拍摄累加序号从获取的IMU数据包中获取目标IMU数据包，所述目标IMU数据包中的触发累加序号与所述拍摄累加序号相同；
第二确定模块，用于将所述目标IMU数据包中的测量时间戳与存储的时间偏置之和，确定为每个三维图像拍摄部件当前时刻拍摄的图像的拍摄时间戳；
第三获取模块，从获取的IMU数据包中，获取测量时间戳处于上一关键帧时刻拍摄的图像的拍摄时间戳与当前时刻拍摄的图像的拍摄时间戳之间的IMU测量信息，以得到当前时刻与上一关键帧时刻之间的IMU测量信息。
可选地，该装置还包括：
第四确定模块，用于确定所述可移动设备当前时刻能够追踪到的地图点的比例，所述比例是指与所述至少两个第一图像包括的特征点对应的地图点的总量占所述至少两个第一图像包括的特征点的总量的比例；
第五确定模块，用于当所述可移动设备当前时刻能够追踪到的地图点的比例小于比例阈值，或者当前时刻与上一关键帧时刻之间的时长大于时长阈值时，将每个三维图像拍摄部件当前时刻拍摄的图像确定为关键帧图像。
可选地，该装置还包括：
第六确定模块，用于根据每个三维图像拍摄部件对应的至少一对特征点的图像坐标、存储的每个第一摄像头的焦距，以及每个三维图像拍摄部件包括的两个摄像头之间的基线距离，确定每个第一图像包括的特征点的深度，一对特征点是指一个三维图像拍摄部件包括的两个摄像头当前时刻拍摄的图像中对应同一实物点的两个特征点；
第七确定模块，用于根据当前时刻所述可移动设备的位姿信息、每个三维图像拍摄部件相对于所述IMU的旋转平移变换矩阵、每个第一摄像头的内参，以及每个第一图像包括的特征点的图像坐标和深度，确定每个第一图像包括的特征点对应的地图点的三维位置信息；
更新模块，用于根据每个第一图像包括的特征点对应的地图点的三维位置信息，对已构建的地图进行更新。
可选地，该装置还包括：
第八确定模块，用于确定所述可移动设备在N个关键帧时刻的位姿信息，以及所述N个关键帧时刻中相邻两个关键帧时刻之间的IMU预积分测量信息，所述N个关键帧时刻是指每个三维图像拍摄部件在当前时刻之前拍摄的能够作为关键帧图像的图像的拍摄时刻，所述N为大于或等于4的正整数；
第九确定模块，用于根据所述可移动设备在所述N个关键帧时刻的位姿信息、所述N个关键帧时刻中相邻两个关键帧时刻之间的时长、所述N个关键帧时刻中相邻两个关键帧时刻之间的IMU预积分测量信息，以及惯性坐标系的重力方向向量，确定所述IMU在第N个关键帧时刻的漂移误差和所述可移动设备的实际重力向量；
第十确定模块，用于根据所述可移动设备在第N-1个关键帧时刻的位姿信息、所述可移动设备的实际重力向量、第N-1个关键帧时刻与第N-2个关键帧时刻之间的IMU预积分测量信息、第N-1个关键帧时刻与第N-2个关键帧时刻之间的时长、第N-1个关键帧时刻与第N个关键帧时刻之间的时长和所述IMU在第N-1个关键帧时刻测量得到的IMU测量信息，确定所述可移动设备在第N个关键帧时刻的速度。
可选地，所述IMU包括陀螺仪和加速度计，所述IMU测量信息包括所述陀螺仪测量得到的角速度和所述加速度计测量得到的加速度，所述IMU预积分测量信息包括相对姿态增量、相对速度增量和相对位置增量；
所述第九确定模块包括：
第三构建子模块，用于根据所述可移动设备在所述N个关键帧时刻的姿态信息和所述N个关键帧时刻中相邻两个关键帧时刻之间的相对姿态增量，构建漂移误差代价函数；
第二最小化运算子模块，用于最小化所述漂移误差代价函数，以得到所述陀螺仪在第N个关键帧时刻的漂移误差；
第五确定子模块，用于根据所述可移动设备在所述N个关键帧时刻的位姿信息、所述N个关键帧时刻中相邻两个关键帧时刻之间的时长、所述N个关键帧时刻中相邻两个关键帧时刻之间的相对位置增量和相对速度增量，以及所述惯性坐标系的重力方向向量，确定所述加速度计在第N个关键帧时刻的漂移误差和所述可移动设备的实际重力向量。
可选地，所述第五确定子模块包括：
第一确定单元，用于根据所述可移动设备在所述N个关键帧时刻的位姿信息、所述N个关键帧时刻中相邻两个关键帧时刻之间的时长，以及所述N个关键帧时刻中相邻两个关键帧时刻之间的相对位置增量和相对速度增量，确定所述可移动设备的估计重力向量；
第二确定单元，用于根据所述可移动设备的估计重力向量和所述惯性坐标系的重力方向向量，确定所述惯性坐标系到世界坐标系的旋转变换矩阵；
第三确定单元，用于根据所述惯性坐标系到世界坐标系的旋转变换矩阵、所述惯性坐标系的重力方向向量、所述N个关键帧时刻中相邻两个关键帧时刻之间的时长、所述可移动设备在所述N个关键帧时刻的位姿信息，以及所述N个关键帧时刻中相邻两个关键帧时刻之间的相对位置增量和相对速度增量，确定所述加速度计在第N个关键帧时刻的漂移误差和所述可移动设备的重力方向修正量；
第四确定单元，用于根据所述可移动设备的重力方向修正量、所述惯性坐标系到世界坐标系的旋转变换矩阵和所述惯性坐标系的重力方向向量，确定所述可移动设备的实际重力向量。
可选地，所述第十确定模块包括：
第六确定子模块，用于根据所述可移动设备在第N-1个关键帧时刻的位姿信息、第N-1个关键帧时刻与第N-2个关键帧时刻之间的时长、所述可移动设备的实际重力向量、第N-1个关键帧时刻与第N-2个关键帧时刻之间的相对位置增量，确定所述可移动设备在第N-1个关键帧时刻的速度；
第七确定子模块，用于根据所述可移动设备在第N-1个关键帧时刻的速度、第N-1个关键帧时刻与第N个关键帧时刻之间的时长和所述加速度计在第N-1个关键帧时刻测量得到的加速度，确定所述可移动设备在第N个关键帧时刻的速度。
可选地，所述第八确定模块包括：
第四构建子模块，用于对于所述N个关键帧时刻中的任一关键帧时刻A，根据所述关键帧时刻A对应的初始关键帧图像包括的特征点的图像坐标、与所述关键帧时刻A对应的初始关键帧图像包括的特征点对应的地图点的三维位置信息、存储的每个第一摄像头的内参，以及每个三维图像拍摄部件相对于所述IMU的旋转平移变换矩阵，构建第二重投影误差约束代价函数，所述关键帧时刻A对应的初始关键帧图像是指所述至少两个三维图像拍摄部件包括的第一摄像头在所述关键帧时刻A分别拍摄的图像；
第三最小化运算子模块，用于最小化所述第二重投影误差约束代价函数，以得到所述可移动设备在所述关键帧时刻A的位姿信息。
在本申请实施例中，先根据当前时刻与上一关键帧时刻之间的IMU测量信息，以及IMU的相邻两个测量时刻之间的时长，确定当前时刻与上一关键帧时刻之间的IMU预积分测量信息。然后根据至少两个第一图像、存储的每个三维图像拍摄部件包括的第一摄像头的内参、每个三维图像拍摄部件相对于IMU的旋转平移变换矩阵、该IMU预积分测量信息、当前时刻与上一关键帧时刻之间的时长、存储的上一关键帧时刻该可移动设备的系统状态，以及该可移动设备的实际重力向量，确定当前时刻该可移动设备的系统状态。由于系统状态包括可移动设备的位姿信息和速度，以及IMU的漂移误差，因此，在确定当前时刻可移动设备的系统状态之后，即可确定当前时刻可移动设备的位姿信息。由于本申请实施例是将至少两个三维图像拍摄部件与IMU进行联合，以共同对可移动设备的位姿信息进行确定，避免了因三维图像拍摄部件可能无法采集到纹理较为清楚的图像，而导致确定出的位姿信息较为不准确的问题，进而提高了确定位姿信息的准确性。
需要说明的是：上述实施例提供的位姿信息确定装置在确定可移动设备的位姿信息时，仅以上述各功能模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能模块完成，即将装置的内部结构划分成不同的功能模块，以完成以上描述的全部或者部分功能。另外，上述实施例提供的位姿信息确定装置与位姿信息确定方法实施例属于同一构思，其具体实现过程详见方法实施例，这里不再赘述。
图7是本申请实施例提供的一种可移动设备的结构示意图，该可移动设备700可因配置或性能不同而产生比较大的差异，可以包括一个或一个以上处理器(centralprocessing units，CPU)701和一个或一个以上的存储器702，其中，所述存储器702中存储有至少一条指令，所述至少一条指令由该处理器701加载并执行，以实现上述实施例中位姿信息确定方法。当然，该可移动设备700还可以具有有线或无线网络接口、键盘以及输入输出接口等部件，以便进行输入输出，该可移动设备700还可以包括其他用于实现设备功能的部件，在此不做赘述。
在示例性实施例中，还提供了一种计算机可读存储介质，例如包括指令的存储器，上述指令可由可移动设备中的处理器执行以完成上述实施例中位姿信息确定方法。例如，所述计算机可读存储介质可以是ROM、随机存取存储器(RAM)、CD-ROM、磁带、软盘和光数据存储设备等。
本领域普通技术人员可以理解实现上述实施例的全部或部分步骤可以通过硬件来完成，也可以通过程序来指令相关的硬件完成，所述的程序可以存储于一种计算机可读存储介质中，上述提到的存储介质可以是只读存储器，磁盘或光盘等。
以上所述仅为本申请的较佳实施例，并不用以限制本申请，凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。

Claims (20)

1.一种位姿信息确定方法，应用于可移动设备，其特征在于，所述可移动设备包括至少两个三维图像拍摄部件和一个惯性测量单元IMU，每个三维图像拍摄部件包括一个双目摄像头，所述双目摄像头由两个相邻且摆放位置平行的摄像头组成，所述方法包括：
根据当前时刻与上一关键帧时刻之间的IMU测量信息，以及第一时长，确定当前时刻与上一关键帧时刻之间的IMU预积分测量信息，所述上一关键帧时刻是指每个三维图像拍摄部件最近一次拍摄的能够作为关键帧图像的图像的拍摄时刻，所述第一时长是指所述IMU的相邻两个测量时刻之间的时长，所述IMU包括陀螺仪和加速度计，所述IMU测量信息包括所述陀螺仪测量得到的角速度和所述加速度计测量得到的加速度，所述IMU预积分测量信息包括相对姿态增量、相对速度增量和相对位置增量；
根据至少两个第一图像、存储的每个三维图像拍摄部件包括的第一摄像头的内参、每个三维图像拍摄部件相对于所述IMU的旋转平移变换矩阵、所述IMU预积分测量信息、当前时刻与上一关键帧时刻之间的时长、存储的上一关键帧时刻所述可移动设备的系统状态，以及所述可移动设备的实际重力向量，确定当前时刻所述可移动设备的系统状态，所述至少两个第一图像是指所述至少两个三维图像拍摄部件包括的第一摄像头当前时刻分别拍摄的图像，所述实际重力向量是所述可移动设备在世界坐标系下的重力向量，所述系统状态包括所述可移动设备的位姿信息和速度，以及所述IMU的漂移误差；
其中，所述根据至少两个第一图像、存储的每个三维图像拍摄部件包括的第一摄像头的内参、每个三维图像拍摄部件相对于所述IMU的旋转平移变换矩阵、所述IMU预积分测量信息、当前时刻与上一关键帧时刻之间的时长、存储的上一关键帧时刻所述可移动设备的系统状态，以及所述可移动设备的实际重力向量，确定当前时刻所述可移动设备的系统状态之前，所述方法还包括：
确定所述可移动设备在N个关键帧时刻的位姿信息，以及所述N个关键帧时刻中相邻两个关键帧时刻之间的IMU预积分测量信息，所述N个关键帧时刻是指每个三维图像拍摄部件在当前时刻之前拍摄的能够作为关键帧图像的图像的拍摄时刻，所述N为大于或等于4的正整数；
根据所述可移动设备在所述N个关键帧时刻的姿态信息和所述N个关键帧时刻中相邻两个关键帧时刻之间的相对姿态增量，构建漂移误差代价函数；最小化所述漂移误差代价函数，以得到所述陀螺仪在第N个关键帧时刻的漂移误差；根据所述可移动设备在所述N个关键帧时刻的位姿信息、所述N个关键帧时刻中相邻两个关键帧时刻之间的时长、所述N个关键帧时刻中相邻两个关键帧时刻之间的相对位置增量和相对速度增量，以及惯性坐标系的重力方向向量，确定所述加速度计在第N个关键帧时刻的漂移误差和所述可移动设备的实际重力向量。
2.如权利要求1所述的方法，其特征在于，所述根据至少两个第一图像、存储的每个三维图像拍摄部件包括的第一摄像头的内参、每个三维图像拍摄部件相对于所述IMU的旋转平移变换矩阵、所述IMU预积分测量信息、当前时刻与上一关键帧时刻之间的时长、存储的上一关键帧时刻所述可移动设备的系统状态，以及所述可移动设备的实际重力向量，确定当前时刻所述可移动设备的系统状态，包括：
确定所述至少两个第一图像包括的特征点的图像坐标，以及与每个第一图像包括的特征点对应的地图点的三维位置信息；
根据所述至少两个第一图像包括的特征点的图像坐标、与每个第一图像包括的特征点对应的地图点的三维位置信息、存储的每个三维图像拍摄部件包括的第一摄像头的内参，以及每个三维图像拍摄部件相对于所述IMU的旋转平移变换矩阵，构建第一重投影误差约束代价函数；
根据所述IMU预积分测量信息、当前时刻与上一关键帧时刻之间的时长、存储的上一关键帧时刻所述可移动设备的系统状态，以及所述可移动设备的实际重力向量，构建IMU运动预测代价函数；
最小化所述第一重投影误差约束代价函数与所述IMU运动预测代价函数之和，以得到当前时刻所述可移动设备的系统状态。
3.如权利要求1所述的方法，其特征在于，所述根据当前时刻与上一关键帧时刻之间的IMU测量信息，以及第一时长，确定当前时刻与上一关键帧时刻之间的IMU预积分测量信息，包括：
根据当前时刻与上一关键帧时刻之间所述陀螺仪测量得到的角速度、上一关键帧时刻所述陀螺仪的漂移误差和所述第一时长，确定当前时刻与上一关键帧时刻之间的相对姿态增量，以及上一关键帧时刻与至少一个测量时刻中每个测量时刻之间的相对姿态增量，所述至少一个测量时刻是指当前时刻与上一关键帧时刻之间的测量时刻；
根据上一关键帧时刻与所述至少一个测量时刻中每个测量时刻之间的相对姿态增量、当前时刻与上一关键帧时刻之间所述加速度计测量得到的加速度、上一关键帧时刻所述加速度计的漂移误差和所述第一时长，确定当前时刻与上一关键帧时刻之间的相对速度增量；
根据上一关键帧时刻与所述至少一个测量时刻中每个测量时刻之间的相对姿态增量、上一关键帧时刻与所述至少一个测量时刻中每个测量时刻之间的相对速度增量、当前时刻与上一关键帧时刻之间所述加速度计测量得到的加速度、上一关键帧时刻所述加速度计的漂移误差和所述第一时长，确定当前时刻与上一关键帧时刻之间的相对位置增量。
4.如权利要求1所述的方法，其特征在于，所述根据当前时刻与上一关键帧时刻之间的IMU测量信息，以及第一时长，确定当前时刻与上一关键帧时刻之间的IMU预积分测量信息之前，所述方法还包括：
每隔所述第一时长获取一个IMU数据包，所述IMU数据包至少包括IMU测量信息和对应的测量时间戳，其中，每个三维图像拍摄部件每隔第二时长拍摄一次图像，所述第二时长大于所述第一时长，且每隔所述第二时长获取的IMU数据包还包括触发累加序号；
当每个三维图像拍摄部件当前时刻拍摄到图像时，确定拍摄累加序号，并根据所述拍摄累加序号从获取的IMU数据包中获取目标IMU数据包，所述目标IMU数据包中的触发累加序号与所述拍摄累加序号相同；
将所述目标IMU数据包中的测量时间戳与存储的时间偏置之和，确定为每个三维图像拍摄部件当前时刻拍摄的图像的拍摄时间戳；
从获取的IMU数据包中，获取测量时间戳处于上一关键帧时刻拍摄的图像的拍摄时间戳与当前时刻拍摄的图像的拍摄时间戳之间的IMU测量信息，以得到当前时刻与上一关键帧时刻之间的IMU测量信息。
5.如权利要求1所述的方法，其特征在于，所述根据至少两个第一图像、存储的每个三维图像拍摄部件包括的第一摄像头的内参、每个三维图像拍摄部件相对于所述IMU的旋转平移变换矩阵、所述IMU预积分测量信息、当前时刻与上一关键帧时刻之间的时长、存储的上一关键帧时刻所述可移动设备的系统状态，以及所述可移动设备的实际重力向量，确定当前时刻所述可移动设备的系统状态之后，所述方法还包括：
确定所述可移动设备当前时刻能够追踪到的地图点的比例，所述比例是指与所述至少两个第一图像包括的特征点对应的地图点的总量占所述至少两个第一图像包括的特征点的总量的比例；
当所述可移动设备当前时刻能够追踪到的地图点的比例小于比例阈值，或者当前时刻与上一关键帧时刻之间的时长大于时长阈值时，将每个三维图像拍摄部件当前时刻拍摄的图像确定为关键帧图像。
6.如权利要求5所述的方法，其特征在于，所述将每个三维图像拍摄部件当前时刻拍摄的图像确定为关键帧图像之后，所述方法还包括：
根据每个三维图像拍摄部件对应的至少一对特征点的图像坐标、存储的每个第一摄像头的焦距，以及每个三维图像拍摄部件包括的两个摄像头之间的基线距离，确定每个第一图像包括的特征点的深度，一对特征点是指一个三维图像拍摄部件包括的两个摄像头当前时刻拍摄的图像中对应同一实物点的两个特征点；
根据当前时刻所述可移动设备的位姿信息、每个三维图像拍摄部件相对于所述IMU的旋转平移变换矩阵、每个第一摄像头的内参，以及每个第一图像包括的特征点的图像坐标和深度，确定每个第一图像包括的特征点对应的地图点的三维位置信息；
根据每个第一图像包括的特征点对应的地图点的三维位置信息，对已构建的地图进行更新。
7.如权利要求1-6任一所述的方法，其特征在于，所述根据至少两个第一图像、存储的每个三维图像拍摄部件包括的第一摄像头的内参、每个三维图像拍摄部件相对于所述IMU的旋转平移变换矩阵、所述IMU预积分测量信息、当前时刻与上一关键帧时刻之间的时长、存储的上一关键帧时刻所述可移动设备的系统状态，以及所述可移动设备的实际重力向量，确定当前时刻所述可移动设备的系统状态之前，所述方法还包括：
根据所述可移动设备在第N-1个关键帧时刻的位姿信息、所述可移动设备的实际重力向量、第N-1个关键帧时刻与第N-2个关键帧时刻之间的IMU预积分测量信息、第N-1个关键帧时刻与第N-2个关键帧时刻之间的时长、第N-1个关键帧时刻与第N个关键帧时刻之间的时长和所述IMU在第N-1个关键帧时刻测量得到的IMU测量信息，确定所述可移动设备在第N个关键帧时刻的速度。
8.如权利要求1所述的方法，其特征在于，所述根据所述可移动设备在所述N个关键帧时刻的位姿信息、所述N个关键帧时刻中相邻两个关键帧时刻之间的时长、所述N个关键帧时刻中相邻两个关键帧时刻之间的相对位置增量和相对速度增量，以及所述惯性坐标系的重力方向向量，确定所述加速度计在第N个关键帧时刻的漂移误差和所述可移动设备的实际重力向量，包括：
根据所述可移动设备在所述N个关键帧时刻的位姿信息、所述N个关键帧时刻中相邻两个关键帧时刻之间的时长，以及所述N个关键帧时刻中相邻两个关键帧时刻之间的相对位置增量和相对速度增量，确定所述可移动设备的估计重力向量；
根据所述可移动设备的估计重力向量和所述惯性坐标系的重力方向向量，确定所述惯性坐标系到世界坐标系的旋转变换矩阵；
根据所述惯性坐标系到世界坐标系的旋转变换矩阵、所述惯性坐标系的重力方向向量、所述N个关键帧时刻中相邻两个关键帧时刻之间的时长、所述可移动设备在所述N个关键帧时刻的位姿信息，以及所述N个关键帧时刻中相邻两个关键帧时刻之间的相对位置增量和相对速度增量，确定所述加速度计在第N个关键帧时刻的漂移误差和所述可移动设备的重力方向修正量；
根据所述可移动设备的重力方向修正量、所述惯性坐标系到世界坐标系的旋转变换矩阵和所述惯性坐标系的重力方向向量，确定所述可移动设备的实际重力向量。
9.如权利要求7所述的方法，其特征在于，所述根据所述可移动设备在第N-1个关键帧时刻的位姿信息、所述可移动设备的实际重力向量、第N-1个关键帧时刻与第N-2个关键帧时刻之间的IMU预积分测量信息、第N-1个关键帧时刻与第N-2个关键帧时刻之间的时长、第N-1个关键帧时刻与第N个关键帧时刻之间的时长和所述IMU在第N-1个关键帧时刻测量得到的IMU测量信息，确定所述可移动设备在第N个关键帧时刻的速度，包括：
根据所述可移动设备在第N-1个关键帧时刻的位姿信息、第N-1个关键帧时刻与第N-2个关键帧时刻之间的时长、所述可移动设备的实际重力向量、第N-1个关键帧时刻与第N-2个关键帧时刻之间的相对位置增量，确定所述可移动设备在第N-1个关键帧时刻的速度；
根据所述可移动设备在第N-1个关键帧时刻的速度、第N-1个关键帧时刻与第N个关键帧时刻之间的时长和所述加速度计在第N-1个关键帧时刻测量得到的加速度，确定所述可移动设备在第N个关键帧时刻的速度。
10.如权利要求7所述的方法，其特征在于，所述确定所述可移动设备在N个关键帧时刻的位姿信息，包括：
对于所述N个关键帧时刻中的任一关键帧时刻A，根据所述关键帧时刻A对应的初始关键帧图像包括的特征点的图像坐标、与所述关键帧时刻A对应的初始关键帧图像包括的特征点对应的地图点的三维位置信息、存储的每个第一摄像头的内参，以及每个三维图像拍摄部件相对于所述IMU的旋转平移变换矩阵，构建第二重投影误差约束代价函数，所述关键帧时刻A对应的初始关键帧图像是指所述至少两个三维图像拍摄部件包括的第一摄像头在所述关键帧时刻A分别拍摄的图像；
最小化所述第二重投影误差约束代价函数，以得到所述可移动设备在所述关键帧时刻A的位姿信息。
11.一种位姿信息确定装置，应用于可移动设备，其特征在于，所述可移动设备包括至少两个三维图像拍摄部件和一个惯性测量单元IMU，每个三维图像拍摄部件包括一个双目摄像头，所述双目摄像头由两个相邻且摆放位置平行的摄像头组成，所述装置包括：
第一确定模块，用于根据当前时刻与上一关键帧时刻之间的IMU测量信息，以及第一时长，确定当前时刻与上一关键帧时刻之间的IMU预积分测量信息，所述上一关键帧时刻是指每个三维图像拍摄部件最近一次拍摄的能够作为关键帧图像的图像的拍摄时刻，所述第一时长是指所述IMU的相邻两个测量时刻之间的时长，所述IMU包括陀螺仪和加速度计，所述IMU测量信息包括所述陀螺仪测量得到的角速度和所述加速度计测量得到的加速度，所述IMU预积分测量信息包括相对姿态增量、相对速度增量和相对位置增量；
第二确定模块，用于根据至少两个第一图像、存储的每个三维图像拍摄部件包括的第一摄像头的内参、每个三维图像拍摄部件相对于所述IMU的旋转平移变换矩阵、所述IMU预积分测量信息、当前时刻与上一关键帧时刻之间的时长、存储的上一关键帧时刻所述可移动设备的系统状态，以及所述可移动设备的实际重力向量，确定当前时刻所述可移动设备的系统状态，所述至少两个第一图像是指所述至少两个三维图像拍摄部件包括的第一摄像头当前时刻分别拍摄的图像，所述实际重力向量是所述可移动设备在世界坐标系下的重力向量，所述系统状态包括所述可移动设备的位姿信息和速度，以及所述IMU的漂移误差；
所述装置还包括：
第八确定模块，用于确定所述可移动设备在N个关键帧时刻的位姿信息，以及所述N个关键帧时刻中相邻两个关键帧时刻之间的IMU预积分测量信息，所述N个关键帧时刻是指每个三维图像拍摄部件在当前时刻之前拍摄的能够作为关键帧图像的图像的拍摄时刻，所述N为大于或等于4的正整数；
第九确定模块，用于根据所述可移动设备在所述N个关键帧时刻的姿态信息和所述N个关键帧时刻中相邻两个关键帧时刻之间的相对姿态增量，构建漂移误差代价函数；最小化所述漂移误差代价函数，以得到所述陀螺仪在第N个关键帧时刻的漂移误差；根据所述可移动设备在所述N个关键帧时刻的位姿信息、所述N个关键帧时刻中相邻两个关键帧时刻之间的时长、所述N个关键帧时刻中相邻两个关键帧时刻之间的相对位置增量和相对速度增量，以及惯性坐标系的重力方向向量，确定所述加速度计在第N个关键帧时刻的漂移误差和所述可移动设备的实际重力向量。
12.如权利要求11所述的装置，其特征在于，所述第二确定模块包括：
第一确定子模块，用于确定所述至少两个第一图像包括的特征点的图像坐标，以及与每个第一图像包括的特征点对应的地图点的三维位置信息；
第一构建子模块，用于根据所述至少两个第一图像包括的特征点的图像坐标、与每个第一图像包括的特征点对应的地图点的三维位置信息、存储的每个三维图像拍摄部件包括的第一摄像头的内参，以及每个三维图像拍摄部件相对于所述IMU的旋转平移变换矩阵，构建第一重投影误差约束代价函数；
第二构建子模块，用于根据所述IMU预积分测量信息、当前时刻与上一关键帧时刻之间的时长、存储的上一关键帧时刻所述可移动设备的系统状态，以及所述可移动设备的实际重力向量，构建IMU运动预测代价函数；
第一最小化运算子模块，用于最小化所述第一重投影误差约束代价函数与所述IMU运动预测代价函数之和，以得到当前时刻所述可移动设备的系统状态。
13.如权利要求11所述的装置，其特征在于，所述第一确定模块包括：
第二确定子模块，用于根据当前时刻与上一关键帧时刻之间所述陀螺仪测量得到的角速度、上一关键帧时刻所述陀螺仪的漂移误差和所述第一时长，确定当前时刻与上一关键帧时刻之间的相对姿态增量，以及上一关键帧时刻与至少一个测量时刻中每个测量时刻之间的相对姿态增量，所述至少一个测量时刻是指当前时刻与上一关键帧时刻之间的测量时刻；
第三确定子模块，用于根据上一关键帧时刻与所述至少一个测量时刻中每个测量时刻之间的相对姿态增量、当前时刻与上一关键帧时刻之间所述加速度计测量得到的加速度、上一关键帧时刻所述加速度计的漂移误差和所述第一时长，确定当前时刻与上一关键帧时刻之间的相对速度增量；
第四确定子模块，用于根据上一关键帧时刻与所述至少一个测量时刻中每个测量时刻之间的相对姿态增量、上一关键帧时刻与所述至少一个测量时刻中每个测量时刻之间的相对速度增量、当前时刻与上一关键帧时刻之间所述加速度计测量得到的加速度、上一关键帧时刻所述加速度计的漂移误差和所述第一时长，确定当前时刻与上一关键帧时刻之间的相对位置增量。
14.如权利要求11所述的装置，其特征在于，所述装置还包括：
第一获取模块，用于每隔所述第一时长获取一个IMU数据包，所述IMU数据包至少包括IMU测量信息和对应的测量时间戳，其中，每个三维图像拍摄部件每隔第二时长拍摄一次图像，所述第二时长大于所述第一时长，且每隔所述第二时长获取的IMU数据包还包括触发累加序号；
第二获取模块，用于当每个三维图像拍摄部件当前时刻拍摄到图像时，确定拍摄累加序号，并根据所述拍摄累加序号从获取的IMU数据包中获取目标IMU数据包，所述目标IMU数据包中的触发累加序号与所述拍摄累加序号相同；
第三确定模块，用于将所述目标IMU数据包中的测量时间戳与存储的时间偏置之和，确定为每个三维图像拍摄部件当前时刻拍摄的图像的拍摄时间戳；
第三获取模块，从获取的IMU数据包中，获取测量时间戳处于上一关键帧时刻拍摄的图像的拍摄时间戳与当前时刻拍摄的图像的拍摄时间戳之间的IMU测量信息，以得到当前时刻与上一关键帧时刻之间的IMU测量信息。
15.如权利要求11所述的装置，其特征在于，所述装置还包括：
第四确定模块，用于确定所述可移动设备当前时刻能够追踪到的地图点的比例，所述比例是指与所述至少两个第一图像包括的特征点对应的地图点的总量占所述至少两个第一图像包括的特征点的总量的比例；
第五确定模块，用于当所述可移动设备当前时刻能够追踪到的地图点的比例小于比例阈值，或者当前时刻与上一关键帧时刻之间的时长大于时长阈值时，将每个三维图像拍摄部件当前时刻拍摄的图像确定为关键帧图像。
16.如权利要求15所述的装置，其特征在于，所述装置还包括：
第六确定模块，用于根据每个三维图像拍摄部件对应的至少一对特征点的图像坐标、存储的每个第一摄像头的焦距，以及每个三维图像拍摄部件包括的两个摄像头之间的基线距离，确定每个第一图像包括的特征点的深度，一对特征点是指一个三维图像拍摄部件包括的两个摄像头当前时刻拍摄的图像中对应同一实物点的两个特征点；
第七确定模块，用于根据当前时刻所述可移动设备的位姿信息、每个三维图像拍摄部件相对于所述IMU的旋转平移变换矩阵、每个第一摄像头的内参，以及每个第一图像包括的特征点的图像坐标和深度，确定每个第一图像包括的特征点对应的地图点的三维位置信息；
更新模块，用于根据每个第一图像包括的特征点对应的地图点的三维位置信息，对已构建的地图进行更新。
17.如权利要求11-16任一所述的装置，其特征在于，所述装置还包括：
第十确定模块，用于根据所述可移动设备在第N-1个关键帧时刻的位姿信息、所述可移动设备的实际重力向量、第N-1个关键帧时刻与第N-2个关键帧时刻之间的IMU预积分测量信息、第N-1个关键帧时刻与第N-2个关键帧时刻之间的时长、第N-1个关键帧时刻与第N个关键帧时刻之间的时长和所述IMU在第N-1个关键帧时刻测量得到的IMU测量信息，确定所述可移动设备在第N个关键帧时刻的速度。
18.如权利要求11所述的装置，其特征在于，所述第九确定模块包括：
第一确定单元，用于根据所述可移动设备在所述N个关键帧时刻的位姿信息、所述N个关键帧时刻中相邻两个关键帧时刻之间的时长，以及所述N个关键帧时刻中相邻两个关键帧时刻之间的相对位置增量和相对速度增量，确定所述可移动设备的估计重力向量；
第二确定单元，用于根据所述可移动设备的估计重力向量和所述惯性坐标系的重力方向向量，确定所述惯性坐标系到世界坐标系的旋转变换矩阵；
第三确定单元，用于根据所述惯性坐标系到世界坐标系的旋转变换矩阵、所述惯性坐标系的重力方向向量、所述N个关键帧时刻中相邻两个关键帧时刻之间的时长、所述可移动设备在所述N个关键帧时刻的位姿信息，以及所述N个关键帧时刻中相邻两个关键帧时刻之间的相对位置增量和相对速度增量，确定所述加速度计在第N个关键帧时刻的漂移误差和所述可移动设备的重力方向修正量；
第四确定单元，用于根据所述可移动设备的重力方向修正量、所述惯性坐标系到世界坐标系的旋转变换矩阵和所述惯性坐标系的重力方向向量，确定所述可移动设备的实际重力向量。
19.如权利要求17所述的装置，其特征在于，所述第十确定模块包括：
第六确定子模块，用于根据所述可移动设备在第N-1个关键帧时刻的位姿信息、第N-1个关键帧时刻与第N-2个关键帧时刻之间的时长、所述可移动设备的实际重力向量、第N-1个关键帧时刻与第N-2个关键帧时刻之间的相对位置增量，确定所述可移动设备在第N-1个关键帧时刻的速度；
第七确定子模块，用于根据所述可移动设备在第N-1个关键帧时刻的速度、第N-1个关键帧时刻与第N个关键帧时刻之间的时长和所述加速度计在第N-1个关键帧时刻测量得到的加速度，确定所述可移动设备在第N个关键帧时刻的速度。
20.如权利要求17所述的装置，其特征在于，所述第八确定模块包括：
第四构建子模块，用于对于所述N个关键帧时刻中的任一关键帧时刻A，根据所述关键帧时刻A对应的初始关键帧图像包括的特征点的图像坐标、与所述关键帧时刻A对应的初始关键帧图像包括的特征点对应的地图点的三维位置信息、存储的每个第一摄像头的内参，以及每个三维图像拍摄部件相对于所述IMU的旋转平移变换矩阵，构建第二重投影误差约束代价函数，所述关键帧时刻A对应的初始关键帧图像是指所述至少两个三维图像拍摄部件包括的第一摄像头在所述关键帧时刻A分别拍摄的图像；
第三最小化运算子模块，用于最小化所述第二重投影误差约束代价函数，以得到所述可移动设备在所述关键帧时刻A的位姿信息。
CN201811639192.5A 2018-12-29 2018-12-29 位姿信息确定方法及装置 Active CN111383282B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811639192.5A CN111383282B (zh) 2018-12-29 2018-12-29 位姿信息确定方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811639192.5A CN111383282B (zh) 2018-12-29 2018-12-29 位姿信息确定方法及装置

Publications (2)

Publication Number Publication Date
CN111383282A CN111383282A (zh) 2020-07-07
CN111383282B true CN111383282B (zh) 2023-12-01

Family

ID=71219308

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811639192.5A Active CN111383282B (zh) 2018-12-29 2018-12-29 位姿信息确定方法及装置

Country Status (1)

Country Link
CN (1) CN111383282B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112700546A (zh) * 2021-01-14 2021-04-23 视辰信息科技(上海)有限公司 一种室外大规模三维地图的构建系统与方法
CN114323010B (zh) * 2021-12-30 2024-03-01 北京达佳互联信息技术有限公司 初始特征确定方法、装置、电子设备及存储介质
CN116740382B (zh) * 2023-05-08 2024-02-20 禾多科技（北京）有限公司 障碍物信息生成方法、装置、电子设备和计算机可读介质

Citations (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2010238096A (ja) * 2009-03-31 2010-10-21 Ntt Docomo Inc 端末装置、拡張現実感システム及び端末画面表示方法
CN102937450A (zh) * 2012-10-31 2013-02-20 北京控制工程研究所 一种基于陀螺测量信息的相对姿态确定方法
CN103278177A (zh) * 2013-04-27 2013-09-04 中国人民解放军国防科学技术大学 基于摄像组网测量的惯性测量组合标定方法
CN104756039A (zh) * 2012-11-02 2015-07-01 高通股份有限公司 在世界坐标系中使用移动装置中的加速度计估计重力向量
CN105698765A (zh) * 2016-02-22 2016-06-22 天津大学 双imu单目视觉组合测量非惯性系下目标物位姿方法
CN106052683A (zh) * 2016-05-25 2016-10-26 速感科技（北京）有限公司 机器人运动姿态估计方法
WO2017022033A1 (ja) * 2015-07-31 2017-02-09 富士通株式会社 画像処理装置、画像処理方法および画像処理プログラム
CN107316319A (zh) * 2017-05-27 2017-11-03 北京小鸟看看科技有限公司 一种刚体追踪的方法、装置和系统
WO2018028649A1 (zh) * 2016-08-10 2018-02-15 纳恩博（北京）科技有限公司 一种移动装置及其定位方法、计算机存储介质
CN107747941A (zh) * 2017-09-29 2018-03-02 歌尔股份有限公司 一种双目视觉定位方法、装置及系统
CN107869989A (zh) * 2017-11-06 2018-04-03 东北大学 一种基于视觉惯导信息融合的定位方法及系统
CN108036785A (zh) * 2017-11-24 2018-05-15 浙江大学 一种基于直接法与惯导融合的飞行器位姿估计方法
CN108629793A (zh) * 2018-03-22 2018-10-09 中国科学院自动化研究所 使用在线时间标定的视觉惯性测程法与设备
CN108731664A (zh) * 2018-05-18 2018-11-02 深圳清创新科技有限公司 机器人状态估计方法、装置、计算机设备和存储介质
CN108805987A (zh) * 2018-05-21 2018-11-13 中国科学院自动化研究所 基于深度学习的混合跟踪方法及装置

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9766074B2 (en) * 2008-03-28 2017-09-19 Regents Of The University Of Minnesota Vision-aided inertial navigation
EP2309225A4 (en) * 2008-07-01 2014-10-15 Topcon Corp POSITION MEASUREMENT METHOD, POSITION MEASUREMENT DEVICE AND PROGRAM
US9213889B2 (en) * 2013-03-28 2015-12-15 The Regents Of The University Of Michigan Athlete speed prediction method using data from attached inertial measurement unit
US20160260250A1 (en) * 2015-03-05 2016-09-08 Dejan Jovanovic Method and system for 3d capture based on structure from motion with pose detection tool
US9933262B2 (en) * 2015-07-27 2018-04-03 Caterpillar Inc. Positioning system having a master-slave configuration
US20170336220A1 (en) * 2016-05-20 2017-11-23 Daqri, Llc Multi-Sensor Position and Orientation Determination System and Device
US10371530B2 (en) * 2017-01-04 2019-08-06 Qualcomm Incorporated Systems and methods for using a global positioning system velocity in visual-inertial odometry

Patent Citations (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2010238096A (ja) * 2009-03-31 2010-10-21 Ntt Docomo Inc 端末装置、拡張現実感システム及び端末画面表示方法
CN102937450A (zh) * 2012-10-31 2013-02-20 北京控制工程研究所 一种基于陀螺测量信息的相对姿态确定方法
CN104756039A (zh) * 2012-11-02 2015-07-01 高通股份有限公司 在世界坐标系中使用移动装置中的加速度计估计重力向量
CN103278177A (zh) * 2013-04-27 2013-09-04 中国人民解放军国防科学技术大学 基于摄像组网测量的惯性测量组合标定方法
WO2017022033A1 (ja) * 2015-07-31 2017-02-09 富士通株式会社 画像処理装置、画像処理方法および画像処理プログラム
CN105698765A (zh) * 2016-02-22 2016-06-22 天津大学 双imu单目视觉组合测量非惯性系下目标物位姿方法
CN106052683A (zh) * 2016-05-25 2016-10-26 速感科技（北京）有限公司 机器人运动姿态估计方法
WO2018028649A1 (zh) * 2016-08-10 2018-02-15 纳恩博（北京）科技有限公司 一种移动装置及其定位方法、计算机存储介质
CN107316319A (zh) * 2017-05-27 2017-11-03 北京小鸟看看科技有限公司 一种刚体追踪的方法、装置和系统
CN107747941A (zh) * 2017-09-29 2018-03-02 歌尔股份有限公司 一种双目视觉定位方法、装置及系统
CN107869989A (zh) * 2017-11-06 2018-04-03 东北大学 一种基于视觉惯导信息融合的定位方法及系统
CN108036785A (zh) * 2017-11-24 2018-05-15 浙江大学 一种基于直接法与惯导融合的飞行器位姿估计方法
CN108629793A (zh) * 2018-03-22 2018-10-09 中国科学院自动化研究所 使用在线时间标定的视觉惯性测程法与设备
CN108731664A (zh) * 2018-05-18 2018-11-02 深圳清创新科技有限公司 机器人状态估计方法、装置、计算机设备和存储介质
CN108805987A (zh) * 2018-05-21 2018-11-13 中国科学院自动化研究所 基于深度学习的混合跟踪方法及装置

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
Benjamin H. Groh等.IMU-based pose determination of scuba divers' bodies and shanks.《2015 IEEE 12th International Conference on Wearable and Implantable Body Sensor Networks (BSN)》.2015,全文. *
王婷婷等.基于双目视觉的运动目标检测跟踪与定位.《机械与电子》.2015,全文. *
邹谦.基于图优化SLAM的移动机器人导航方法研究.《中国优秀硕士学位论文全文数据库（电子期刊）信息科技辑》.2018,全文. *

Also Published As

Publication number Publication date
CN111383282A (zh) 2020-07-07

Similar Documents

Publication Publication Date Title
CN107747941B (zh) 一种双目视觉定位方法、装置及系统
CN110009681B (zh) 一种基于imu辅助的单目视觉里程计位姿处理方法
CN110246147B (zh) 视觉惯性里程计方法、视觉惯性里程计装置及移动设备
CN107990899B (zh) 一种基于slam的定位方法和系统
CN111354042B (zh) 机器人视觉图像的特征提取方法、装置、机器人及介质
CN108592950B (zh) 一种单目相机和惯性测量单元相对安装角标定方法
WO2020237574A1 (zh) 相机的内参及相对姿态的标定方法、装置、无人机和存储装置
CN111383282B (zh) 位姿信息确定方法及装置
JP3880702B2 (ja) 画像のオプティカルフロー検出装置及び移動体の自己位置認識システム
CN107357286A (zh) 视觉定位导航装置及其方法
CN109073407B (zh) 惯性测量单元的漂移标定方法、设备及无人飞行器
CN111754579B (zh) 多目相机外参确定方法及装置
CN112184824B (zh) 一种相机外参标定方法、装置
JP2020506487A (ja) シーンから深度情報を取得するための装置および方法
CN108871311B (zh) 位姿确定方法和装置
CN110969663A (zh) 一种相机外部参数的静态标定方法
CN108933896B (zh) 基于惯性测量单元的全景视频稳像的方法及系统
WO2018142533A1 (ja) 位置姿勢推定装置および位置姿勢推定方法
CN110375732A (zh) 基于惯性测量单元和点线特征的单目相机位姿测量方法
CN111489288A (zh) 一种图像的拼接方法和装置
CN107749069A (zh) 图像处理方法、电子设备和图像处理系统
CN113436267B (zh) 视觉惯导标定方法、装置、计算机设备和存储介质
CN110310243B (zh) 一种无人机摄影测量的图像校正方法、系统和存储介质
Huttunen et al. A monocular camera gyroscope
CN112629565B (zh) 像机与惯性测量单元旋转关系校准方法、装置和设备

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant