CN119399282B 一种基于多源数据的定位与建图方法及系统 （中国海洋大学）

一种基于多源数据的定位与建图方法及系统2S203，基于状态向量方程和观测向量方程进行一阶S204，基于二值贝叶斯滤波原理构建激光雷达栅格i是i3在进行多源数据的处理过程中，同一栅格在激光雷达和深度相机不同传感器的探测ry为基于多源数据的栅格概率。S104：将当前点云根据计算出的变换矩阵进行3.根据权利要求1所述的基于多源数据的定位与建图方法，其特征在于，在步骤S201wk-1)k)wk-1和vk满足正态分布；xk=f(it-1,up-1,wk-1)+A(xr-t-1)+wwk-14称=f(⃞it-1,ur-1,0),称为当前状态估计状态变量；A为函数f对x的偏导数的雅克比4.根据权利要求3所述的基于多源数据的定位与建图方法，其特征在于，在步骤S203⃞=f(it-1,up-1,0)kT为在深度相机的外参矩阵约束下函数h对v的偏导数的5的概率p(mi)，概率值为1代表占用状态，概率值为0代表空闲状态，引入两者的比值的某一个栅格被占据的概率，p(z|mi＝0)为测量值到空闲状态的某一个栅格被占据的概采用PnP求解深度相机3D到2D点对的运动，通过最小化重投影误差方法得到最优的相机位6最小化重投影误差方法包括：重投影误差表示为实际观测值与投影坐标之间的欧氏距离，7两个将上述与两个上式表示重投影误差和相机位姿李代数一阶导数通过获取雅可比矩阵，完成对目标函数的线性化处理，再通过高斯J(ξ)TXJ(ξ)Δξ=_J(ξ)Te(ξ)扩展卡尔曼滤波算法融合模块(2)，用于利用扩展卡尔曼滤波算法融合移动机器人位深度相机数据处理模块(3)，用于深度相机数据处理：通过图像特征提取和描述算法8移动机器人的定位与建图过程。因而复杂环境下的同步定位与建图(SimultaneousLocalizationandMapping,SLAM)是当前移动机器人研究的热basedVisual_InertialSLAM他们在非线性优化问题中引入了IMU和图像关键点的紧密对象的激光映射模块和将视觉词袋与LIDAR_Iris特征相结合的并行全局和局部搜索回环9[0010]S3，深度相机数据处理：通过图像特征提取和描述算法ORB利用FAST角点的主方[0019]S202，在系统的上个后验状态估计状态变量it-i处进行一阶泰勒展开进行线性[0024]式中，为系统在k时刻的状态变量，f为状态向量运算函数，Xk-1为系统在于系统的不确定性或建模误差导致的状态变化中的随机扰动；[0028]在步骤S202中，在系统的上个后验状态估计状态变量it-i处进行一阶泰勒展开i=f(ir-1,为函数f对x的偏导数的雅克比矩阵为函数对的偏导数的雅可比矩阵则[0031]xr=+A(th-it-1)+wwh-1；为函数h对x的偏导数的雅可比矩阵v为函数h对v的偏导数的雅可比矩阵，;[0040]ip=f(i-1,ur-1,0)；机的外参矩阵约束下函数f对w的偏导数的雅[0052]P=(I-k;到测量值被占据的概率，为从空闲状态的某一个栅格到测量值被占据的概p(a)为扫描测量后得到的测量值被占据的概率，p(elm,=1)为测量值到占用状态的某一个栅格被占据的概率，plelm,=o)为测量值到空闲状态的某一个栅格i个栅格的状态；栅格的观测结果只有占据和空闲两种状态；默认栅格初始状态时占据和空闲的概率都为；;是在给定地图状态和之前所有观测数据z1:t-1到数据zt的概率，即观测模型；p(mlzrr-1)是在给定之前所有观测数据z1:t-1的条件[0144]式中，为系统在k时刻的状态变量，f为状态向量运算函数，Xk-1为系统在于系统的不确定性或建模误差导致的状态变化中的随机扰动；[0148]S202，在系统的上个后验状态估计状态变量it-i处进行一阶泰勒展开进行线性i=f(ir-1,为函数f对x的偏导数的雅克比矩阵为函数对的偏导数的雅可比矩阵则[0152]xr=+A(th-it-1)+wwh-1；为函数h对x的偏导数的雅可比矩阵v为函数h对v的偏导数的雅可比矩阵，;[0161]ip=f(i-1,ur-1,0)；机的外参矩阵约束下函数f对w的偏导数的雅[0173]P=(I-k;入来判断激光雷达栅格地图内每个栅格的障碍[0179]对于激光雷达栅格地图m上的某一个栅格，用p(m)用概率的形式表示表示到测量值被占据的概率，为从空闲状态的某一个栅格到测量值被占据的概p(a)为扫描测量后得到的测量值被占据的概率，p(elm,=1)为测量值到占用状态的某一个栅格被占据的概率，plelm,=o)为测量值到空闲状态的某一个栅格i个栅格的状态；栅格的观测结果只有占据和空闲两种状态；默认栅格初始状态时占据和空闲的概率都为[0195]S3，深度相机数据处理：通过图像特征提取和描述算法ORB利用FAST角点的主方法，它是FAST角点检测器和BRIEF描述子的结合体，并且增加了方向性。FAST（Features法是一种用于估计相机运动的方法，它利用已知的n个特征点在图像中的像素位置及其对应的3D空间点位置来估计相机的位姿。这种算法在视觉SLAM和三维重建等领域中广泛应法来估计相机的运动。PnP算法的核心在于利用已知的3D_2D点对应关系来求解相机的位；;是在给定地图状态和之前所有观测数据z1:t-1到数据zt的概率，即观测模型；p(mlzrr-1)是在给定之前所有观测数据z1:t-1的条件[0260]扩展卡尔曼