【技术实现步骤摘要】
基于全局地图与多传感器信息融合的全程位姿估计方法
本专利技术涉及导航领域，具体涉及一种基于全局地图与多传感器信息融合的全程位姿估计方法。
技术介绍
在自动驾驶、搜救、侦查等领域，对无人系统的需求日益提升，而无人系统的定位问题是其中的基础。目前，存在很多采用相机和激光雷达等传感器进行无人系统的局部位姿估计的技术，并且其中也有和IMU(惯性测量单元)结合形成的组合导航系统，它们在局部区域内可以实现对无人系统精确的位姿估计。例如，慕尼黑工业大学于2014年提出的LSD-SLAM(基于直接法的大范围单目即时定位和地图构建方法)基于直接法实现大规模环境下的位姿确定和地图构建；苏黎世联邦理工学院于2015年提出的ROVIO(鲁棒的视觉惯性里程计)算法，是一种基于卡尔曼滤波算法的单目视觉惯性里程计。但是上述的方式在实际应用中仍然存在一些缺陷，一是没有固定的全局参考，即在相同环境下，从不同的起点出发也可能得到不同的位姿估计结果，不利于之后的重复利用；二是缺少全局度量，局部估计随着时间推移容易产生漂移，使得长时间、大区域的局部估计不够准确。<...
【技术保护点】
1.一种基于全局地图与多传感器信息融合的全程位姿估计方法，其特征在于，包括以下步骤：/n1)搭建包含各传感器的无人机系统；具体方法为：选定一台无人机，在无人机上分别搭载各传感器，包括：视觉惯性里程计VIO系统、GPS、磁强计和气压计；其中，所述VIO系统包含相机和惯性测量单元IMU，相机镜头方向与无人机前进方向一致；/n2)在步骤1)搭建完毕的无人机系统中，对各传感器进行标定，分别获得每个传感器对应的参数，包括：相机的内外参矩阵，IMU的内参数和测量噪声，相机坐标系和IMU坐标系之间的相对位姿关系，磁强计噪声和气压计噪声；/n标定完成后，对无人机系统进行初始化，包括：对各传...
【技术特征摘要】
1.一种基于全局地图与多传感器信息融合的全程位姿估计方法，其特征在于，包括以下步骤：
1)搭建包含各传感器的无人机系统；具体方法为：选定一台无人机，在无人机上分别搭载各传感器，包括：视觉惯性里程计VIO系统、GPS、磁强计和气压计；其中，所述VIO系统包含相机和惯性测量单元IMU，相机镜头方向与无人机前进方向一致；
2)在步骤1)搭建完毕的无人机系统中，对各传感器进行标定，分别获得每个传感器对应的参数，包括：相机的内外参矩阵，IMU的内参数和测量噪声，相机坐标系和IMU坐标系之间的相对位姿关系，磁强计噪声和气压计噪声；
标定完成后，对无人机系统进行初始化，包括：对各传感器信息输出的时间对齐，确定各传感器坐标系和载体无人机坐标系之间的位姿关系，确定无人机系统初始状态下的基帧变换矩阵；其中，全局坐标系为东北天ENU坐标系，局部坐标系为VIO系统第一帧图像所对应的相机位姿，VIO系统的本体坐标系即为无人机系统的本体坐标系；
3)利用各传感器获得载体无人机当前所处位姿的量测信息，并利用VIO系统得到的图像信息构造并维护局部地图；
选用局部坐标系下的无人机系统的状态变量为：
其中，χl为局部坐标系下的无人机系统状态变量，n为VIO系统的滑动窗口的大小，χt为载体无人机在VIO系统第t帧图像时对应的状态变量，为无人机在VIO系统第t帧图像时的位置，为无人机在VIO系统第t帧图像时的速度，为无人机在VIO系统第t帧图像时的姿态，上标l表示状态变量为相对于局部坐标系所得，ba为IMU中加速度计的噪声，bg为IMU中陀螺仪的噪声；
在相机帧中观测到特征之后，以表示特征点i在局部坐标系中的三维坐标，m为路标点的个数；
最终对VIO系统即无人机系统在局部坐标系下的位姿估计为如下非线性最小二乘问题：
其中，rB和rC分别代表惯性测量的误差和视觉测量的误差，WB为惯性测量的协方差，WC为视觉测量的协方差；对该非线性最小二乘问题求解，得到VIO系统在局部参考系中6自由度位姿实时估计，将四元数转换为旋转矩阵则VIO系统的位姿估计表示为
4)构建基于因子图的多传感器信息融合框架，利用因子图优化得到无人机对应VIO系统当前帧的最优状态变量；具体步骤如下：
4-1)将VIO系统第t帧图像记为当前帧，选择全局坐标系下无人机系统状态变量为在全局坐标系下对应VIO系统第t帧图像的无人机位姿，其中表示VIO系统第t帧图像下无人机在全局坐标系下的位置，表示VIO系统第t帧图像下无人机在全局坐标系下的姿态，上标w表示相对于全局坐标系；
4-2)以状态变量xt为因子图变量节点，以无人机系统上搭载的各传感器获得的测量...
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。