【技术实现步骤摘要】
本专利技术属于定位，更具体地，本专利技术涉及一种融合二维码定位的视觉惯性slam方法及系统。
技术介绍
1、随着科技的进步和市场的发展，为了减少生产成本、提高生产效率，视觉惯性传感器方案逐渐成为无人车实现室内定位、建图、导航的传感器融合方案之一。相比于雷达，其具备成本低，具有丰富的视觉信息，输出里程计频率高等优点，但是，具有定位精度低的不足。
2、无人车在自动化仓储物流、家庭服务等方面的应用越来越广泛，其中定位建图相关的slam系统是无人车的重要组成模块。现有的室内无人车slam解决方案大都成本高，需要多个2d激光雷达或3d激光雷达传感器，进行定位、避障，甚至需要在特殊使用场景下安装反光柱，来提高系统精度。
3、申请号：2022116027297，专利名称为：一种基于地平面假设的视觉惯导slam方法，该方案在室内采用相机惯性测量单元(以下简称imu)进行定位。该技术方案存在如下两个缺点：
4、1)视觉惯性方案只能达到分米级别定位，定位精度不高。
5、2)初始化需要识别大小已知标志物并分离地面特征，是冗余且不必要的。
技术实现思路
1、本专利技术提供一种融合二维码定位的视觉惯性slam方法，旨在改善上述问题。
2、本专利技术是这样实现的，一种融合二维码定位的视觉惯性slam方法，所述方法包括如下步骤：
3、s1、检测当前图像帧中是否存在二维码；
4、s2、若检测结果为否，则基于相邻关键图像帧的视觉重投
5、s3、若检测结果为是，则相邻关键图像帧的视觉重投影残差、imu数据帧间预积分约束及二维码残差来估计相机在地图坐标系下的位姿；
6、s4、基于相机在地图坐标系下的位姿将特征点及二维码投影至世界坐标系下，进行全局点云地图的构建。
7、进一步的，在当前图像帧中不存在二维码时，相机当前时刻在地图坐标系下位姿获取过程具体如下：
8、s21、检测当前图像帧为是否图像关键帧，若检测结果为是，执行步骤s22，若检测结果为否，则将当前图像帧直接放入滑动窗内；
9、s22、检测滑动窗内的图像关键帧是否达到数量阈值，若检测结果为是，则丢弃滑动窗内距当前时间最长的图像关键帧及之前数据帧的同时，将当前图像关键帧放入滑动窗内，若检测结果为否，则直接将当前图像关键帧放入滑动窗内；
10、s23、基于滑动窗内相邻图像关键帧的视觉重投影残差及imu数据帧间预积分约束来估计相机在地图坐标系下的位姿。
11、进一步的，若满足如下任一一个条件，则当前图像帧为图像关键帧；
12、条件(1)当前图像帧与相邻关键图像帧的距离大于距离阈值；
13、条件(2)当前图像帧与相邻关键图像帧的角度偏差大于角度偏差阈值。
14、进一步的，基于视觉重投影残差及imu数据帧间预积分约束构建最大后验估计方程，采用高斯牛顿法求解该方程的最小二乘解x，获取当前滑动窗内所有图像关键帧的位姿，包括当前关键图像帧的位姿，其中，最大后验估计方程如下所示：
15、
16、其中，x表示滑动窗内所有图像关键帧的位姿序列，rp、hp表示自边缘化的先验信息，表示滑动窗内相邻图像关键帧间的imu数据帧集合，表示滑动窗内所有图像关键帧的集合，ρ表示核函，表示从imu数据帧bk到imu数据帧bk+1的变化权重，表示滑动窗内第l个图像关键帧cl到第j个图像关键帧cj的变化权重。
17、进一步的，基于滑动窗中图像关键帧的视觉重投影残差表示如下：
18、
19、其中，分别表示基于预估位姿x将滑动窗内观测到特征点的第l个图像关键帧cl转换至第j个图像关键帧cj中获得特征点的像素坐标，第j个图像关键帧cj为当前滑动窗内观测到特征点的第一个图像关键帧，为从滑动窗内第j个图像关键帧cj中读取到的特征点的像素坐标，表示预估值，包括
20、进一步的，获取当前滑动窗内第j-1帧图像关键帧与第j帧图像关键帧间的imu数据帧，建立相邻imu数据帧间的预积分约束，所有相邻imu数据帧间的预积分约束的累加即为第j-1帧图像关键帧到第j帧图像关键帧间预积分约束，第k个imu数据帧bk与第k+1个imu数据帧bk+1间预积分约束具体如下：
21、
22、其中，表示从imu数据帧bk到imu数据帧bk+1的位移变化，表示从imu数据帧bk到imu数据帧bk+1的速度变化；表示从imu数据帧bk到imu数据帧bk+1的位姿变化；δba、δbg分别表示从imu数据帧bk到imu数据帧bk+1下的加速度、陀螺仪零偏变化；表示将世界坐标系转到imu数据帧bk下的旋转矩阵；分别表示为世界坐标系下imu数据帧bk+1、imu数据帧bk的位置；gw表示世界坐标系下重力向量；分别表示世界坐标系下imu数据帧bk到imu数据帧bk+1的速度；为预积分项，表示传感器采集值，由组成；分别表示世界坐标系下imu数据帧bk到imu数据帧bk+1的姿态四元数；分别表示imu数据帧bk+1、imu数据帧bk的加速度零偏变化；分别表示imu数据帧bk+1、imu数据帧bk的陀螺仪零偏变化；δtk表示从imu数据帧bk到imu数据帧bk+1的时间间隔；包括[x0,x1,...,xk,...,xn,λ1，...,λi,...λm]，xk表示滑动窗内第k个图像关键帧的位姿，n表示滑动窗内的图像关键帧数量，λi表示滑动窗内所有特征点的逆深度，m表示滑动窗内的特征点数量。
23、进一步的，在当前图像帧中不存在二维码时，相机当前时刻在地图坐标系下位姿获取过程具体如下：
24、s31、检测当前图像帧为是否图像关键帧，若检测结果为是，执行步骤s32，若检测结果为否，则将当前图像帧直接放入滑动窗内；
25、s22、检测滑动窗内的图像关键帧是否达到数量阈值，若检测结果为是，则丢弃滑动窗内距当前时间最长的图像关键帧及之前数据帧的同时，将当前图像关键帧放入滑动窗内，若检测结果为否，则直接将当前图像关键帧放入滑动窗内；
26、s23、基于滑动窗内相邻图像关键帧的视觉重投影残差、imu数据帧间预积分约束及二维码约束来估计相机在地图坐标系下的位姿。
27、进一步的，将满足下面任意一个条件的图像帧即为图像关键帧；
28、条件(1)当前图像帧与相邻关键图像帧的距离大于距离阈值；
29、条件(2)当前图像帧与相邻关键图像帧的角度偏差大于角度偏差阈值；
30、条件(3)当前图像帧第一次检测到二维码。
31、进一步的，基于滑动窗内相邻图像关键帧的视觉重投影残差、imu数据帧间预积分约束及二维码约束构建最大后验估计方程，采用高斯牛顿法求解该方程的最小二乘解x，获取当前滑动窗内所有图像关键帧的位姿，包括当前关键图像帧的位姿，其中，最大后验估计方程如下所示：...
【技术保护点】
1.一种融合二维码定位的视觉惯性SLAM方法，其特征在于，所述方法包括如下步骤：
2.如权利要求1所述融合二维码定位的视觉惯性SLAM方法，其特征在于，在当前图像帧中不存在二维码时，相机当前时刻在地图坐标系下位姿获取过程具体如下：
3.如权利要求2所述融合二维码定位的视觉惯性SLAM方法，其特征在于，若满足如下任一一个条件，则当前图像帧为图像关键帧；
4.如权利要求2所述融合二维码定位的视觉惯性SLAM方法，其特征在于，基于视觉重投影残差及IMU数据帧间预积分约束构建最大后验估计方程，采用高斯牛顿法求解该方程的最小二乘解x，获取当前滑动窗内所有图像关键帧的位姿，包括当前关键图像帧的位姿，其中，最大后验估计方程如下所示：
5.如权利要求4所述融合二维码定位的视觉惯性SLAM方法，其特征在于，基于滑动窗中图像关键帧的视觉重投影残差表示如下：
6.如权利要求4所述融合二维码定位的视觉惯性SLAM方法，其特征在于，获取当前滑动窗内第j-1帧图像关键帧与第j帧图像关键帧间的IMU数据帧，建立相邻IMU数据帧间的预积分约束，所有相邻
7.如权利要求1所述融合二维码定位的视觉惯性SLAM方法，其特征在于，在当前图像帧中不存在二维码时，相机当前时刻在地图坐标系下位姿获取过程具体如下：
8.如权利要求7所述融合二维码定位的视觉惯性SLAM方法，其特征在于，将满足下面任意一个条件的图像帧即为图像关键帧；
9.如权利要求7所述融合二维码定位的视觉惯性SLAM方法，其特征在于，基于滑动窗内相邻图像关键帧的视觉重投影残差、IMU数据帧间预积分约束及二维码约束构建最大后验估计方程，采用高斯牛顿法求解该方程的最小二乘解x，获取当前滑动窗内所有图像关键帧的位姿，包括当前关键图像帧的位姿，其中，最大后验估计方程如下所示：
10.一种融合二维码定位的视觉惯性SLAM系统，其特征在于，所述系统包括：
...【技术特征摘要】
1.一种融合二维码定位的视觉惯性slam方法，其特征在于，所述方法包括如下步骤：
2.如权利要求1所述融合二维码定位的视觉惯性slam方法，其特征在于，在当前图像帧中不存在二维码时，相机当前时刻在地图坐标系下位姿获取过程具体如下：
3.如权利要求2所述融合二维码定位的视觉惯性slam方法，其特征在于，若满足如下任一一个条件，则当前图像帧为图像关键帧；
4.如权利要求2所述融合二维码定位的视觉惯性slam方法，其特征在于，基于视觉重投影残差及imu数据帧间预积分约束构建最大后验估计方程，采用高斯牛顿法求解该方程的最小二乘解x，获取当前滑动窗内所有图像关键帧的位姿，包括当前关键图像帧的位姿，其中，最大后验估计方程如下所示：
5.如权利要求4所述融合二维码定位的视觉惯性slam方法，其特征在于，基于滑动窗中图像关键帧的视觉重投影残差表示如下：
6.如权利要求4所述融合二维码定位的视觉惯性slam方法，其特征在于，获取当前滑动窗内第j-1帧图像关键帧与第j帧图像关键...
【专利技术属性】
技术研发人员：赵法龙，陈智君，郑亮，郝奇，方靖森，曹雏清，赵立军，
申请(专利权)人：长三角哈特机器人产业技术研究院，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。