[发明专利]一种基于激光和二维码融合的即时定位与地图构建方法

摘要

本发明提供一种基于激光和二维码融合的即时定位与地图构建方法，将里程计的行程与速度信息、单目相机采集的二维码、惯性测量单元的信息融合为状态向量，根据里程计的观测模型、惯性测量单元的观测模型以及单目相机的观测模型构建联邦卡尔曼滤波器的观测方程，在传统的里程计与惯性测量单元的基础上加入二维码定位信息，从而得到无人移动平台的状态向量的最优解；再利用二维码信息处理的快速性和精确性的特点，形成精确度较高的位姿预测信息，从而降低粒子滤波即时定位与地图构建方法中的粒子数量，由此能够提高低功率处理器，如无人移动平台的定位精度，保证无人移动平台在复杂环境的定位效果，完成地图构建，满足复杂环境的应用需求。

主权项

1.一种基于激光和二维码融合的即时定位与地图构建方法，应用于无人移动平台，其特征在于，所述无人移动平台包括处理模块、单目相机、激光雷达、里程计以及惯性测量单元；所述方法包括以下步骤：S1：所述单目相机采集地图待构建区域上预先铺设的二维码，其中，所述二维码包含表征二维码在地图待构建区域上的坐标的ID信息；S2：所述处理模块对二维码进行解码，得到二维码在地图待构建区域上的坐标，再根据二维码的坐标得到无人移动平台在世界坐标系下的初始全局坐标；S3：将无人移动平台在世界坐标系下的全局坐标、无人移动平台的航向角、无人移动平台的速度以及无人移动平台的角速度作为联邦卡尔曼滤波器的状态向量；其中，所述全局坐标由步骤S2中的初始全局坐标以及里程计测量的无人移动平台的行程与速度确定；S4：根据所述状态向量构建联邦卡尔曼滤波器的状态方程，根据里程计的观测模型、惯性测量单元的观测模型以及单目相机的观测模型构建联邦卡尔曼滤波器的观测方程；S5：根据联邦卡尔曼滤波算法对所述状态方程与观测方程进行求解，得到所述状态向量的最优解；S6：构建粒子滤波即时定位与地图构建算法的运动状态转移方程与运动观测方程，其中，当无人移动平台的速度小于设定阈值、无转弯或者运动路径上存在障碍物时，所述运动状态转移方程的控制向量为激光雷达对地图待构建区域进行特征匹配得到的位姿，当无人移动平台的速度不小于设定阈值、有转弯或者运动路径上不存在障碍物时，所述运动状态转移方程的控制向量为所述状态向量的最优解中包含的位姿；其中，所述位姿为所述全局坐标与无人移动平台的航向角；S7：对所述运动状态转移方程与运动观测方程进行迭代求解，得到地图待构建区域的全局地图。