买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

摘要：本申请提供一种基于迭代扩展卡尔曼滤波器的里程计算方法及装置，应用于视觉惯性里程计，视觉惯性里程计包括相机和IMU，该方法包括：对视觉惯性里程计进行动态初始化处理，得到视觉惯性里程计的初始化状态信息，并基于初始化状态信息建立初始空间三维地图；根据初始化状态信息、相机实时采集的图像数据和IMU实时采集的IMU数据进行状态传播，得到视觉惯性里程计的线性化模型；基于迭代扩展卡尔曼滤波器方法对线性化模型进行多次迭代更新处理，得到视觉惯性里程计的当前系统状态向量，当前系统状态向量用于指示视觉惯性里程计在初始空间三维地图中的位置和姿态。提高了视觉惯性里程计的精度。

主权项：1.一种基于迭代扩展卡尔曼滤波器的里程计算方法，其特征在于，应用于视觉惯性里程计，所述视觉惯性里程计包括相机和惯性测量单元IMU，所述方法包括：对视觉惯性里程计进行动态初始化处理，得到所述视觉惯性里程计的初始化状态信息，并基于所述初始化状态信息建立初始空间三维地图，所述初始化状态信息包括多张所述相机拍摄的初始图像对应的相机位姿以及各所述初始图像上的三维特征点的空间位置；根据所述初始化状态信息、所述相机实时采集的图像数据和所述IMU实时采集的IMU数据进行状态传播，得到所述视觉惯性里程计的线性化模型；基于迭代扩展卡尔曼滤波器方法对所述线性化模型进行多次迭代更新处理，得到所述视觉惯性里程计的当前系统状态向量，所述当前系统状态向量用于指示所述视觉惯性里程计在所述初始空间三维地图中的位置和姿态；其中，所述根据所述初始化状态信息、所述相机实时采集的图像数据和所述IMU实时采集的IMU数据进行状态传播，得到所述视觉惯性里程计的线性化模型，包括：根据所述IMU数据获取IMU测量模型；根据所述IMU测量模型和所述图像数据进行状态向量增广处理，得到所述线性化模型；其中，所述状态向量增广处理包括：更新状态向量与更新协方差矩阵；所述更新状态向量包括：更新SLAM点和更新MSCKF点；所述SLAM点指长期特征信息，所述SLAM点被加入到所述状态向量中；所述MSCKF点指短期特征信息，所述MSCKF点不被加入到所述状态向量中；所述SLAM点被加入到所述状态向量中的线性化的观测方程为： 所述MSCKF点不被加入到所述状态向量中的线性化的观测方程为： 其中，为所有特征观测对所有相机姿态的雅可比矩阵，Hf为残差对空间点的雅可比矩阵；为整个误差状态向量，表示当前IMU误差状态量，表示滑动窗口中所有相机位姿的误差状态量，表示所有SLAM点的误差状态量，n表示噪声。

全文数据：

权利要求：

百度查询： 中国科学院自动化研究所 基于迭代扩展卡尔曼滤波器的里程计算方法及装置