一种融合惯性测量单元的单目同步定位与建图位姿解算方法技术

本发明专利技术涉及机器人感知领域，特别是一种融合惯性测量单元的单目同步定位与建图位姿解算方法。其特征是：至少包含以下步骤：步骤1)获取机器人视觉信息，依据机器人视觉信息和机器人惯性信息确定机器人位姿；步骤2)依据步骤1)给出机器人视觉信息坐标系C相对于世界坐标系W上的平移分量；步骤3)给出视觉与惯性在后端的误差项，优化目标函数构造及位姿求解。本发明专利技术具有良好的鲁棒性，建图精度和点云质量，可以求解更高精度的位姿，从而应用于在未知环境中行走的机器人时，能够获得精准的定位，使机器人在后续自动导航过程中可以实现精准壁障、精确规划最优路线等功能。

A method of monocular synchronous positioning and mapping position and attitude calculation based on inertial measurement unit

【技术实现步骤摘要】
一种融合惯性测量单元的单目同步定位与建图位姿解算方法
本专利技术涉及机器人感知领域，特别是一种融合惯性测量单元的单目同步定位与建图位姿解算方法。
技术介绍
在未知环境状态下移动机器人感知技术，其依据的核心为同步定位与地图构建SLAM(SimultaneousLocalizationandMapping,SLAM)，它是实现机器人自主移动的关键。视觉SLAM从匹配原理上分为直接法与间接法两类。其中，基于间接法的主流方法是RaúlMur-Artal和J.M.M.Montiel提出的ORB-SLAM，在PTAM的基础上提出了初始化算法自动计算初始位姿，提取高速的ORB(OrientedFASTandRotatedBRIEF，ORB)图像特征点并使用G2O(GeneralGraphOptimization，G2O)库进行全局误差最小化，使其更适应于大规模场景；添加了回环检测线程，引进本质图的概念来加速闭环校正过程。基于直接法的是EngelJ在2016年提出的DSO(DirectSparseOdometry，DSO)算法，添加了光度标定模型，直接采用像素点构造光度误差项参与优化，与直接法相比极本文档来自技高网...

【技术保护点】
1.一种融合惯性测量单元的单目同步定位与建图的高精度位姿解算方法，至少包含以下步骤：步骤1)视觉与惯性在前端的初始位姿估计；步骤2)待求解的视觉惯性变量定义；步骤3)视觉与惯性在后端的误差项及优化目标函数构造及位姿求解。

【技术特征摘要】
1.一种融合惯性测量单元的单目同步定位与建图的高精度位姿解算方法，至少包含以下步骤：步骤1)视觉与惯性在前端的初始位姿估计；步骤2)待求解的视觉惯性变量定义；步骤3)视觉与惯性在后端的误差项及优化目标函数构造及位姿求解。2.根据权利要求1所述一种融合惯性测量单元的单目同步定位与建图的高精度位姿解算方法，其特征是：所述的步骤1)包括以下步骤：步骤1.1前端初始位姿估计，特别是去畸变帧间位姿估计，至少包括：步骤1.1.1对当前帧的像素点pi投影得到对应点pj，投影公式：pj表示像素点pi投影后对应像素点，pi表示当前图像帧中的第i个像素点，K表示相机内参矩阵，r2＝u2+v2表示像到光轴中心的距离，即像素点pi在像素坐标系上横轴变量与纵轴变量的平方和，r4表示r2的平方，(·)-1表示取逆操作，exp(·)表示取指数操作，ξk,k+1表示k到k+1时刻位姿的李代数形式，(·)∧表示将六维向量转变成四维矩阵的从向量到矩阵操作；步骤1.1.2对投影公式进行非线性优化后得到位姿的更新量Δξ，利用更新量对步骤1.1.1中位姿的李代数形式ξk,k+1进行迭代更新：ξk,k+1_update←ξk,k+1_cur+Δξ，得到更新后的ξk,k+1_update就是前端初始位姿；步骤1.1.3对惯性测量单元(IMU)模块的实际测量值进行预积分：表示IMU坐标系相对于世界坐标系上的从第i时刻到第k时刻的旋转分量，表示IMU坐标系相对于世界坐标系上的从第i时刻到第k时刻的速度分量，表示IMU坐标系相对于世界坐标系上的从第i时刻到第k时刻的平移分量。ωi表示IMU第i次采样得到的真实角速度测量值，表示IMU第i次采样得到的真实加速度测量值，gW表示重力矢量，分别表示第i时刻时的陀螺仪偏差、加速度计偏差，ηgd、ηad分别表示陀螺仪、加速度计的离散化噪声。3.根据权利要求1所述一种融合惯性测量单元的单目同步定位与建图的高精度位姿解算方法，其特征是：所述的步骤2)包括以下步骤：步骤2.1根据步骤1.1.3中惯性测量单元(IMU)模块的测量值的预积分所得的旋转分量、速度分量、平移分量以及惯性测量单元(IMU)模块固有偏差bg、ba，定义k时刻下相机采集的图像帧状态xk对应的优化变量为：xk＝[Wpk,Wvk,Wqk,bg,ba,s]xk表示k时刻时图像帧状态优化变量，Wpk表示在世界坐标系中在k时刻时的平移分量；Wvk表示在世界坐标系中在k时刻时的速度分量；Wqk表示在世界坐标系中在k时刻时的旋转分量；bg表示陀螺仪偏差；ba表示加速度计偏差；s表示尺度因子，使视觉光度误差项与惯性误差项中的位姿信息保持尺度一致性；步骤2.2根据步骤1.1.1的视觉投影公式和步骤1.1.3的惯性测量单元(IMU)预积分中的各个变量，定义融合惯性测量单元后的全局优化状态变量为x＝[x1,x2,...,xm,k1,k2,kc,ρ1,...,ρn]xk表示待求解的全局优化状态变量，x1,x2,...,xm表示有m个图像帧状态优化变量，k1,k2表示径向畸变参数，kc表示单目相机内参矩阵，ρ1,...,ρn表示n个不同像素点对应的逆深度；步骤2.3对步骤1.1.3的惯性测量单元(IMU)预积分中的旋转分量、平移分量与步骤1.1.1的视觉投影公式中位姿的旋转分量、平移分量，定义尺度因子s，使得惯性测量单元(IMU)预积分中的旋转分量、平移分量与视觉视觉投影公式中位姿的旋转分量、平移分量之间满足：s·WCp＝WBqWBp-WCq·CBq·CBps表示定义的尺度因子，WCp表示相机坐标系C相对于世界坐标系W上的平移分量，WBq表示IMU坐标系B相对于世界坐标系W上的旋转分量，WBp表示IMU坐标系B相对于世界坐标系W上的平移分量，WCq表示相机坐标系C相对于世界坐标系W上的旋转量，CBq表示IMU坐标系B相对于相机坐标系C上的旋转分量，CBp表示IMU坐标系B相对于相机坐标系C上的平移分量。4.根...

【专利技术属性】
技术研发人员：孙伟，纪晓凤，陆伟，
申请(专利权)人：西安电子科技大学，
类型：发明
国别省市：陕西,61