CN110706279A 基于全局地图与多传感器信息融合的全程位姿估计方法 （清华大学）

基于全局地图与多传感器信息融合的全程本发明提出一种基于全局地图与多传感器惯性里程计VIO系统的图像信息构造并维护局部地图；构建基于因子图的多传感器信息融合框架，利用因子图优化得到无人机系统对应VIO系的方式将无人机搭载的所有传感器的量测和全22)在步骤1)搭建完毕的无人机系统中，对各传3)利用各传感器获得载体无人机当前所处位姿的量测信息，并利用VIO系统得到的图最终对VIO系统即无人机系统在局部坐标系下的位姿估计为如下非线性最小二乘问4)构建基于因子图的多传感器信息融合框架，利用因子图优化得到无人机对应VIO系4-1)将VIO系统第t帧图像记为当前帧，选择全局坐标系下无人机系统状态变量3为在全局坐标系下对应VIO系统第t帧图像的无人机位姿，其中表示VIO系统第t帧图像下无人机在全局坐标系下的位置，a"表示VIO系统第t帧图像下无人机在全局4-2)以状态变量xt为因子图变量节点，以无人机系统上搭载的各传感器获得的测量信4-3)在所有传感器测量均为独立，且噪声满足高斯分布的条VIO系统第t帧图像对应的在全局坐标系下的其中，代表VIO系统第t帧图像对应的GPS的量测，为无人机由4全局地图因子为将特征点像素坐标与全局地图点云中对应的3D点按照当前估计的位其中，"由:经过基帧变换矩阵变换得到，利用VIO系统第t-1帧图像下基帧变换矩阵"r,将局部地图转换为全局地图，即i-"r:.8,式中i和i为和分别对应的齐次坐4-5)将因子图因子节点表达式代入式(1)进行求解，得到无人机系统对应VIO系统第t5)进行基帧变换，得到当前帧下局部坐标5SLAM(基于直接法的大范围单目即时定位和地图构建方法)基于直接法实现大规模环境下一种多传感器融合框架，采用VINS-MONO(一种鲁棒通用的单目视觉惯性状态估计器)的输6[0011]3)利用各传感器获得载体无人机当前所处位姿的量测信息，并利用VIO系统得到时的位置，为无人机在VIO系统第t帧图像时的速度，d:为无人机在,ie[,m]表示特征点i在局部坐标系中的三维坐[0017]最终对VIO系统即无人机系统在局部坐标系下的位姿估计为如下非线性最小二乘B和rC分别代表惯性测量的误差和视觉测量的误差，WB为惯性测量的协方自由度位姿实时估计，将四元数q:转换为旋转矩阵R',则VIO系统的位姿估计表示为[0020]4)构建基于因子图的多传感器信息融合框架，利用因子图优化得到无人机对应[0021]4-1)将VIO系统第t帧图像记为当前帧，选择全局坐标系下无人机系统状态变量7第t帧图像下无人机在全局坐标系下的位置，a"表示VIO系统第t帧图像下无人机在全局坐8[0042]全局地图因子为将特征点像素坐标与全局地图点云中对应的3D点按照当前估计矩阵"r:,将局部地图转换为全局地图，即i"=",,式中和为和分别对应的齐[0049]根据步骤4)的结果得到VIO系统第t帧图像下无人机系统在全局坐标系下的位姿9下的基帧变换矩阵(局部坐标系和全局坐标系之间的变换[0064]3)利用各传感器获得载体当前所处位姿的量测信息，并利用VIO系统得到的图像[0065]具体地，目前已存在成熟的VIO系统可以为载体提供无人机的局部位姿和局部地(路标点即为二维特征点转化为三维形式的点)的个数。最终对VIO系统(即无人机系统)在局部坐标系下的位姿估计可以看做如下非线性B和rC分别代表惯性测量的误差和视觉测量的误差，WB为惯性测量的协方并且VIO系统可以实现局部地图的构造和维护。[0071]4)构建基于因子图的多传感器信息融合框架，利用因子图优化得到无人机对应多传感器信息融合框架。图3所示即为本发明实施例中由全部变量节点构成的因子图示例[0074]4-1)将VIO系统第t帧图像记为当前帧，选择全局坐标系下无人机系统状态变量x-[:a:]为在全局坐标系下对应VIO系统第t帧图像的载体位姿，其中P,"表示VIO系统第t帧图像下无人机在全局坐标系下的位置，a"表示VIO系统第t帧图像下无人机在全局坐[0082]以使用ENU坐标为例，得到VIO系统第t帧图像对应的在ENU全局坐标系下的GPS量[0088]假设磁强计已经校准且没有误差，通过查表得到在ENU坐标系下局部区域的磁场[0096]全局地图因子为将特征点像素坐标与全局地图点云中对应的3D点按照当前估计像信息构造并维护局部地图，其中局部地图的三维点坐标即为利用上一个周期(即VIO系统第t-1帧图像下)步骤5)所求得的基帧变换矩阵"T,可以将局部地图转换为全局地图，即i"="',式中ir和为r和:分别对应的齐次坐标。[0101]4-5)将因子图因子节点表达式代入式(1)，进行求解，即可求得无人机系统对应VIO系统第t帧图像的最优状态变量，即输出在当前帧下对无人机系统的全局位姿估计结[0107]6)当相机获取新的一帧图像(t+1帧)时，将该新的图像作为新