一种基于场景自适应的多模态融合SLAM方法

专利主权项内容

1.一种基于场景自适应的多模态融合SLAM方法，其特征在于：包括以下步骤：步骤一：输入传感器数据输入激光雷达、相机和IMU三个模态的传感器数据；步骤二：模态可靠性评估对于激光雷达传感器，设在相邻两帧之间存在对应的平面特征F，κ表示平面特征数目，平面特征F的单位法向量为n，计算二阶矩：pl, 1 : κpl, 1 : κ1 : κ将其进行特征值分解：式中，V表示列为特征向量v, v, v的正交方阵，Λ表示元素为对应的特征值λ, λ, λ的对角矩阵；123123特征值降序排序λ≥λ≥λ，定义激光雷达传感器的环境退化系数当ρ<1.0×10时，认为环境发生退化导致激光雷达传感器的可靠性降低；123-3对于相机传感器，通过跟踪相邻两帧之间的视觉特征点来估计机器人运动，当跟踪到的视觉特征点的数目小于50对时，认为环境发生退化导致相机传感器的可靠性降低；除了环境退化导致的传感器可靠性降低以外，对于激光雷达、相机和IMU的传感器，分别检测三种传感器的输出频率，当输出频率小于1Hz时，认为对应的传感器故障导致可靠性降低；步骤三：确定主/次模态传感器基于模态可靠性评估结果，主/次模态传感器的确定方法如下：(1)当激光雷达正常工作时，无论相机或者IMU的传感器可靠性降低与否，选择激光雷达传感器作为主模态传感器，其余传感器作为次模态传感器；(2)当激光雷达可靠性降低但相机正常工作时，无论IMU可靠性降低与否，选择相机传感器作为主模态传感器，其余传感器作为次模态传感器；(3)当激光雷达和相机的可靠性均降低，但IMU正常工作时，选择IMU传感器作为主模态传感器，其余传感器作为次模态传感器；步骤四：构建各个模态的里程计系统(1)激光雷达里程计系统：提取当前帧激光雷达点云中的点特征、线特征和面特征，在上一帧激光雷达点云中搜索和当前帧对应的点特征、线特征和面特征，计算这三种特征的匹配误差：式中，i、i、i分别表示当前帧的点特征、线特征和面特征，分别表示当前帧和上一帧之间点特征到点特征的误差、点特征到线特征的误差和点特征到面特征的误差，/>分别表示/>和/>的权重值，通过最小化匹配误差得到当前时刻机器人的位姿估计值；123(2)相机里程计系统：在当前k时刻的视觉关键帧M以及上一时刻的视觉关键帧M中提取视觉特征点，设M和M之间跟踪成功到的视觉特征点的数目为m，利用IMU提供的初始位姿参数T和相机与IMU的外参将M中观测到的视觉特征点p投影到M的图像坐标系下，计算视觉特征点的重投影误差：kk-1kk-1kk-1lk式中，l∈m表示视觉特征点索引，表示M中与p对应的视觉特征点的像素坐标，π表示相机投影模型，/>表示李群中对旋转向量的变换操作符，通过最小化重投影误差得到当前时刻机器人的位姿估计值；klc(3)IMU里程计系统：对于t时刻的IMU数据，定义IMU的积分误差如下：式中，ω、a分别表示t时刻角速度和线加速度的测量值，和/>分别表示t-1时刻和t时刻IMU的角速度随机游走和加速度随机游走，ω(t)和a(t)通过以下公式计算：ImImII式中，表示t时刻IMU相对于机器人初始位置坐标系的姿态，∨表示旋转矩阵从李群到李代数的变换，a(t)和g分别表示地球坐标系下的加速度测量值和地球重力加速度值，通过最小化积分误差得到当前时刻机器人的位姿估计值；GG步骤五：采用因子图优化方法计算机器人当前时刻的最终位姿将由主模态里程计系统得到的当前时刻机器人的位姿估计值作为因子图优化的节点X，i＝1, 2, ..., N，N表示窗口中的节点数目，其余两个次模态里程计系统得到的当前时刻机器人的位姿估计值作为节点之间的相对变换因子，构建因子图优化模型，计算机器人当前时刻的最终位姿X：i*式中，r表示主模态里程计的先验因子，和/>分别表示主模态里程计相对变换因子和次模态里程计相对变换因子，l＝1, 2表示次模态传感器的序号，/>和/>分别表示/>和的协方差，i表示在整个滑动窗口中的索引值；0步骤六：计算机器人下一时刻的最终位姿重复执行步骤一至步骤五，即可得到后续机器人下一时刻的最终位姿。 更多数据：搜索