一种基于视觉里程计的松耦合多传感器融合定位算法的制作方法

技术特征：
1.一种基于视觉里程计的松耦合多传感器融合定位算法，其特征在于，包括：步骤s1：获取从slo
‑
vlp计算得到的移动终端观测位姿s
t
(x
t
,y
t
,z
t
，θ
t
)，其中偏航角θ
t
通过激光雷达匹配器进行校正；步骤s2：将步骤s1中由slo
‑
vlp计算得到的移动终端位姿作为自适应蒙特卡罗定位算法中粒子的初始化位姿，由自适应蒙特卡罗定位算法估计的位姿记作(x
i
,y
i
)；步骤s3：获取由视觉里程计基于orb特征匹配的移动终端的位姿p
t
；步骤s4：将步骤s1、s2获得的观测位姿作为卡尔曼滤波算法的观测值以校正由步骤s3获得的视觉里程计预测的移动终端的位姿p
t
。2.根据权利要求1所述的一种基于视觉里程计的松耦合多传感器融合定位算法，其特征在于，所述步骤s1中，获取从slo
‑
vlp计算并得到校正后的移动终端观测位姿s
t
(x
t
,y
t
,z
t
，θ
t
)的计算方法包括：步骤s11：调制led灯光，用于传递led灯体id以及获得与所述灯体id相对应的led灯体位姿信息；步骤s12：获取某一led灯体在世界坐标系中的位姿(x
i
，y
i
,z
i
)，并计算位于移动终端上的相机图像中心在世界坐标系中的位姿p(x
s
，y
s
,z
s
)；步骤s13：通过位于移动终端上的相机的坐标系与该移动终端坐标系的坐标系变换关系计算得到该移动终端的姿态p
t
(x
t
,y
t
,z
t
)；步骤s14：由移动终端上的视觉里程计获取估计的偏航角γ
odom
、激光雷达匹配器检测的的相对方向变换关系γ
lidar
→
map
,可以得到校正后的偏航角θ
t
＝γ＝γ
odom
γ
lidar
→
map
；并由步骤s12获得的p可得到移动终端的slo
‑
vlp位姿s
t
(x
t
,y
t
,z
t
，θ
t
)。3.根据权利要求1所述的一种基于视觉里程计的松耦合多传感器融合定位算法，其特征在于，所述步骤s2中，获取由自适应蒙特卡罗定位算法得到的位姿(x
i
,y
i
)，其方法包括：步骤s21：将权利要求2获得的slo
‑
vlp位姿s
t
作为自适应蒙特卡罗定位算法滤波器粒子的初始化位姿；步骤s22：把视觉里程计传感器获取的位姿信息和激光雷达数据输入到自适应蒙特卡罗定位算法定位器，输出移动终端在地图坐标系上的位姿(x
i
,y
i
)。4.根据权利要求1所述的一种基于视觉里程计的松耦合多传感器融合定位算法，其特征在于，所述步骤s3中，获取由视觉里程计基于orb特征匹配的移动终端的位姿的方法包括：步骤s31：特征检测：从相机拍摄的图片中进行兴趣点检测，提取关键点；步骤s32：计算由步骤s31获得的关键点的描述子，根据描述子进行特征点匹配；步骤s33：根据对极几何约束，并由八点法估计出本质矩阵e，并由e通过奇异值分解svd恢复旋转矩阵和平移矩阵r和t；步骤s34：由步骤s33获得的r,t估计相机的相对运动，从而估计得到相机的当前位姿；步骤s35：由相机坐标系与移动终端坐标系的tf变换关系可以计算得到移动终端的位姿5.根据权利要求1所述的一种基于视觉里程计的松耦合多传感器融合定位算法，其特征在于，所述步骤s4中，步骤s1、s2获得的观测位姿作为卡尔曼滤波算法的观测值以校正由
步骤s3获得的视觉里程计预测的移动终端的位姿的方法如下：由slo
‑
vlp计算的观测位姿和激光雷达的观测位姿可以获得位姿观测值z
t
,并通过卡尔曼滤波器对所述步骤s3获得的位姿进行校正，得到最终的移动终端的位姿p
t
。

技术总结
本发明提供了一种基于视觉里程计的松耦合多传感器融合定位算法，包括视觉里程计、RSE照相机、激光雷达；激光雷达可以补偿累积误差，视觉里程计不受车轮在恶劣环境等特殊情况下(如不平整的路面、水面或沙漠)打滑的影响，而VLP测量可以提供高精度的姿态初始化或姿态校准，通过视觉里程计的状态预测和激光雷达扫描仪和VLP的测量更新，从而实现高精度定位；本发明在减少传统轮式里程计本身的漂移的同时，将可观测的LED数量的要求放宽到零。本发明的融合可见光、激光雷达、视觉里程计多传感器的定位方法使移动终端能够最大限度地提高其对环境的感知意识，并获得足够的测量值，在室内定位和导航方面更加精确。位和导航方面更加精确。位和导航方面更加精确。


技术研发人员：陈颖聪 关伟鹏 梁婉琳
受保护的技术使用者：佛山市广工大数控装备技术发展有限公司
技术研发日：2021.09.29
技术公布日：2021/12/13