[发明专利]一种融合惯性测量单元的单目同步定位与建图位姿解算方法

权利要求书

1.一种融合惯性测量单元的单目同步定位与建图的高精度位姿解算方法，至少包含以下步骤：

步骤1)视觉与惯性在前端的初始位姿估计；

步骤2)待求解的视觉惯性变量定义；

步骤3)视觉与惯性在后端的误差项及优化目标函数构造及位姿求解；

所述的步骤1)包括以下步骤：

步骤1.1前端初始位姿估计，特别是去畸变帧间位姿估计，至少包括：

步骤1.1.1对当前帧的像素点p_i投影得到对应点p_j，投影公式：

p_j表示像素点p_i投影后对应像素点，p_i表示当前图像帧中的第i个像素点，K表示相机内参矩阵，r²＝u²+v²表示像到光轴中心的距离，即像素点p_i在像素坐标系上横轴变量与纵轴变量的平方和，r⁴表示r²的平方，(·)^-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时刻的速度分量，_WB p_i^k表示IMU坐标系相对于世界坐标系上的从第i时刻到第k时刻的平移分量；表示IMU第i次采样得到的真实角速度测量值，表示IMU第i次采样得到的真实加速度测量值，g_W表示重力矢量，分别表示第i时刻时的陀螺仪偏差、加速度计偏差，η^gd、η^ad分别表示陀螺仪、加速度计的离散化噪声；

所述的步骤2)包括以下步骤：

步骤2.1根据步骤1.1.3中惯性测量单元模块的测量值的预积分所得的旋转分量、速度分量、平移分量以及惯性测量单元模块固有偏差b^g、b^a，定义k时刻下相机采集的图像帧状态x_k对应的优化变量为：

x_k＝[_Wp_k,_Wv_k,_Wq_k,b_g,b_a,s]

x_k表示k时刻时图像帧状态优化变量，_Wp_k表示在世界坐标系中在k时刻时的平移分量；_Wv_k表示在世界坐标系中在k时刻时的速度分量；_Wq_k表示在世界坐标系中在k时刻时的旋转分量；b_g表示陀螺仪偏差；b_a表示加速度计偏差；s表示尺度因子，使视觉光度误差项与惯性误差项中的位姿信息保持尺度一致性；

步骤2.2根据步骤1.1.1的视觉投影公式和步骤1.1.3的惯性测量单元预积分中的各个变量，定义融合惯性测量单元后的全局优化状态变量为

x＝[x₁,x₂,...,x_m,k₁,k₂,k_c,ρ₁,...,ρ_n]

x_k表示待求解的全局优化状态变量，x₁,x₂,...,x_m表示有m个图像帧状态优化变量，k₁,k₂表示径向畸变参数，k_c表示单目相机内参矩阵，ρ₁,...,ρ_n表示n个不同像素点对应的逆深度；

步骤2.3对步骤1.1.3的惯性测量单元预积分中的旋转分量、平移分量与步骤1.1.1的视觉投影公式中位姿的旋转分量、平移分量，定义尺度因子s，使得惯性测量单元预积分中的旋转分量、平移分量与视觉投影公式中位姿的旋转分量、平移分量之间满足：

s·_WCp＝_WBq_WBp-_WCq·_CBq·_CBp

s表示定义的尺度因子，_WCp表示相机坐标系C相对于世界坐标系W上的平移分量，_WBq表示IMU坐标系B相对于世界坐标系W上的旋转分量，_WBp表示IMU坐标系B相对于世界坐标系W上的平移分量，_WCq表示相机坐标系C相对于世界坐标系W上的旋转量，_CBq表示IMU坐标系B相对于相机坐标系C上的旋转分量，_CBp表示IMU坐标系B相对于相机坐标系C上的平移分量；

所述的步骤3)包括以下步骤：

步骤3.1给出视觉的光度误差项；

步骤3.2给出惯性测量单元的误差项；

步骤3.3优化目标函数求解。