1.一种基于激光视觉融合数据的大场景重定位方法,其特征在于,包括如下步骤:步骤1:创建激光点云地图;
利用车载激光雷达对大场景环境进行建图,生成激光点云地图;
步骤2:获取初始位姿估计值;
根据步骤1创建的激光点云地图,利用当前获取的点云帧,与地图中的特征点进行匹配,并进行优化,最终计算得到车辆当前位姿;
步骤3:根据视觉数据进行匹配获取位姿;
首先对图像信息提取特征点,计算描述子;根据当前图像已有的特征向量进行匹配,计算出图像与地图之间的相对位姿变换,并优化位姿以便获得更精确的位姿估计值;
步骤4:获取重定位位姿;
在步骤4中,具体包括如下步骤:
步骤4.1:将初始位姿估计值和匹配的位姿作为变量,利用初始位姿估计值与匹配位姿之间的约束条件将它们联合优化;
步骤4.2:利用重定位位姿优化问题中的约束条件对变量进行限制;
步骤4.3:将上述重定位位姿优化问题中的约束条件带入到优化问题中,采用优化算法求解得到最优的重定位位姿;
步骤4.4:将新的重定位位姿应用到地图中,更新地图中的点云数据和图像数据,使得地图与环境保持一致;
步骤4.5:根据重定位位姿的误差大小和优化收敛情况,判断重定位是否成功;如果重定位失败,则需要重新进行位姿估计和匹配;
在步骤4.3中,设计自适应MPC横向控制器,具体包括如下步骤:步骤4.3.1:将点云地图中的特征点与当前图像中的特征点进行匹配,得到匹配点对及其对应的特征描述子;
步骤4.3.2:通过这些匹配点对,可以构建初始位姿估计值与匹配位姿之间的约束条件,将其带入到优化问题中;
步骤4.3.3:将点云地图中的点投影到当前相机坐标系下,得到对应的二维像素坐标;通过这些投影点与当前图像中的特征点进行匹配,得到匹配点对并计算及其对应的相对位姿变换;将这些匹配点对的相对位姿变换带入到优化问题中,作为相机位姿与点云地图之间的约束条件;
步骤4.3.4:将以上约束带入到优化函数中,使用优化算法求解出最优的重定位位姿。
2.根据权利要求1所述的一种基于激光视觉融合数据的大场景重定位方法,其特征在于:在步骤1中,具体包括如下步骤:步骤1.1:使用激光雷达采集所需建立的大场景环境中的点云数据;
步骤1.2:对采集到的点云数据进行预处理,包括去噪、滤波以及聚类;
步骤1.3:将处理后的点云数据通过点云配准算法建立初始点云地图;
步骤1.4:随着车辆或机器人的移动,不断采集新的点云数据,并通过点云配准算法将其加入到已有的点云地图中;
步骤1.5:将新的点云数据与已有的点云地图进行融合和更新;
步骤1.6:在点云地图中设置关键帧;
步骤1.7:将更新后的点云地图保存下来。
3.根据权利要求1所述的一种基于激光视觉融合数据的大场景重定位方法,其特征在于:在步骤2中,具体包括如下步骤:步骤2.1:首先对激光数据进行去噪和滤波;
步骤2.2:从处理后的激光数据中提取特征点;
步骤2.3:将当前帧提取的特征点与地图中的特征点进行匹配,得到一个初步的位姿估计值;
步骤2.4:利用激光数据和位姿估计值,估计当前帧与上一帧之间的相对运动,以进一步优化位姿估计值;
步骤2.5:利用优化方法对位姿估计值进行优化。
4.根据权利要求3所述的一种基于激光视觉融合数据的大场景重定位方法,其特征在于:步骤2.2:从处理后的激光数据中提取特征点,具体包括如下步骤:步骤2.2.1:首先对激光点云数据进行体素网格化处理;
步骤2.2.2:根据曲率计算公式,选取左右多个点,计算每个体素中点云的曲率信息;
步骤2.2.3:对于每个点,根据其所在的体素中曲率的大小和法向量的方向,判断该点是否为曲率特征点。
5.根据权利要求1所述的一种基于激光视觉融合数据的大场景重定位方法,其特征在于:在步骤3中,具体包括如下步骤:步骤3.1:使用特征点检测算法ORB,从图像中提取出具有可重复性和区分性的特征点;
步骤3.2:对于提取出的特征点,通过使用特征描述算法ORB描述子,将其转化为可用于匹配的特征向量;
步骤3.3:将当前图像的特征向量与地图中已有的特征向量进行匹配,找到地图中与当前图像最相似的特征点;
步骤3.4:通过匹配的特征点,计算出当前图像与地图之间的相对位姿变换;
步骤3.5:将通过匹配的位姿与先前的位姿进行优化,通过使用图优化算法得到更加精确的位姿估计值;
步骤3.6:对优化后的位姿估计值进行误差判断和优化,判断匹配结果是否可靠,如果可靠,则更新当前位姿状态;如果不可靠,则需要重新匹配或使用其他传感器信息来辅助定位。