[发明专利]视觉惯性数据深度融合的定位方法及装置

权利要求书

1.一种视觉惯性数据深度融合的定位方法，其特征在于，包括以下步骤：

步骤S1：在初始时刻将系统置于静止状态，采集初始化时间段内加速度计的测量值和陀螺仪的测量值，以估计系统的初始状态；

步骤S2：在得到所述初始状态后，根据所述加速度计的测量值和所述陀螺仪的测量值对系统状态进行传播，并对系统的协方差矩阵进行更新，所述步骤S2进一步包括：使用所述加速度计的测量值和所述陀螺仪的测量值对所述系统的状态位置、速度和姿态进行传播，并且根据所述系统的初始状态构建协方差矩阵更新方程，以更新协方差；

步骤S3：在获取到图像以后，利用IMU辅助的外点剔除方法跟踪特征点，所述步骤S3进一步包括：采用IMU辅助的视觉跟踪算法，并利用IMU积分得到的相邻帧之间的旋转获取对积几何约束，以减少求解对积几何约束模型需要的点对数，并减少迭代次数；以及

步骤S4：对于跟踪失败的特征点，根据视觉测量信息构建视觉测量，并对所述系统状态进行更新，在所述步骤S4中，所述视觉测量构建的方法为利用所述特征点在满足预设条件的图像中的像素位置计算所述特征点的三维位置，并对视觉测量进行线性化，以将特征点相关误差进行边缘化；所述步骤S4进一步包括：对于所述跟踪失败的特征点，如果跟踪的帧数大于等于预设帧数，则利用其过去所有的多个观测以及估计的对应的相机位姿，并基于最小二乘的方式估计所述跟踪失败的特征点的位置。

2.一种视觉惯性数据深度融合的定位装置，其特征在于，包括：

估计模块，用于在初始时刻将系统置于静止状态，采集初始化时间段内加速度计的测量值和陀螺仪的测量值，以估计系统的初始状态；

传播模块，用于在得到所述初始状态后，根据所述加速度计的测量值和所述陀螺仪的测量值对系统状态进行传播，并对系统的协方差矩阵进行更新，所述传播模块进一步用于使用所述加速度计的测量值和所述陀螺仪的测量值对所述系统的状态位置、速度和姿态进行传播，并且根据所述系统的初始状态构建协方差矩阵更新方程，以更新协方差；

剔除模块，用于在获取到图像以后，利用IMU辅助的外点剔除方法跟踪特征点，所述剔除模块进一步用于采用IMU辅助的视觉跟踪算法，并利用IMU积分得到的相邻帧之间的旋转获取对积几何约束，以减少求解对积几何约束模型需要的点对数，并减少迭代次数；以及

构建模块，用于对于跟踪失败的特征点，根据视觉测量信息构建视觉测量，并对所述系统状态进行更新，其中，所述视觉测量构建的方法为利用所述特征点在满足预设条件的图像中的像素位置计算所述特征点的三维位置，并对视觉测量进行线性化，以将特征点相关误差进行边缘化；所述构建模块进一步用于对于所述跟踪失败的特征点，如果跟踪的帧数大于等于预设帧数，则利用其过去所有的多个观测以及估计的对应的相机位姿，并基于最小二乘的方式估计所述跟踪失败的特征点的位置。