一种基于激光和二维码融合的即时定位与地图构建方法技术

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

A Method of Real-time Location and Map Construction Based on Laser and Two-Dimensional Code Fusion

The invention provides a real-time positioning and map construction method based on laser and two-dimensional code fusion, which integrates the travel and velocity information of odometer, the two-dimensional code collected by monocular camera and the information of inertial measurement unit into a state vector, and constructs the view of federated Kalman filter according to the observation model of odometer, the observation model of inertial measurement unit and the observation model of monocular camera. Based on the traditional odometer and inertial measurement unit, two-dimensional code positioning information is added to the equation to obtain the optimal solution of the state vector of the unmanned mobile platform, and then the fast and accurate information processing of the two-dimensional code is used to form the position and attitude prediction information with high accuracy, thus reducing the number of particles in the particle filter instant positioning and map building method. This can improve the positioning accuracy of high and low power processors, such as unmanned mobile platforms, ensure the positioning effect of unmanned mobile platforms in complex environments, complete map building, and meet the application requirements of complex environments.

【技术实现步骤摘要】
一种基于激光和二维码融合的即时定位与地图构建方法
本专利技术属于即时定位与地图构建
，尤其涉及一种基于激光和二维码融合的即时定位与地图构建方法。
技术介绍
无人移动平台，例如自动导引运输车(AGV，AutomatedGuidedVehicle)已经普遍的运用于多个行业，其应用环境也逐渐趋于复杂化，表现在同一导航范围内可能出现各种不同的场景，如：较大范围的空旷路面、复杂排列的货物架等。同时，在实际运用中，高速运行的工业自动导引运输车常常搭载着低功耗处理器。因此，根据目前自动导引运输车的实际应用需求提出合适的导航技术，保证其导航效果，是自动导引运输车能够广泛运用的关键。现有技术中主有激光即时定位与地图构建和惯性器件相结合的大型叉车搬运AGV，视觉传感器和惯性器件融合的物流机器人；其中，大型叉车搬运AGV利用激光即时定位与地图构建(SLAM，SimultaneousLocalizationAndMapping)进行地图构建、惯性器件辅助定位完成导航任务，虽然有效的保证了导航的精度，但是该导航技术应用于空旷或转弯的场合容易导致定位失败；物流机器人仅仅通过识别二维码标签完成定位导航，简洁的算法虽然保证了物流机器人高速运行时的定位效果，但是应用场合单一，无法满足复杂环境的应用需求。
技术实现思路
为解决上述问题，本专利技术提供一种基于激光和二维码融合的即时定位与地图构建方法，能够完成效果较好且精度较高的轻量级定位与地图构建，满足复杂环境的应用需求。一种基于激光和二维码融合的即时定位与地图构建方法，应用于无人移动平台，所述无人移动平台包括处理模块、单目相机、激光雷达、里程计以及惯性测量单元；所述方法包括以下步骤：S1：所述单目相机采集地图待构建区域上预先铺设的二维码，其中，所述二维码包含表征二维码在地图待构建区域上的坐标的ID信息；S2：所述处理模块对二维码进行解码，得到二维码在地图待构建区域上的坐标，再根据二维码的坐标得到无人移动平台在世界坐标系下的初始全局坐标；S3：将无人移动平台在世界坐标系下的全局坐标、无人移动平台的航向角、无人移动平台的速度以及无人移动平台的角速度作为联邦卡尔曼滤波器的状态向量；其中，所述全局坐标由步骤S2中的初始全局坐标以及里程计测量的无人移动平台的行程与速度确定，无人移动平台的航向角与角速度通过惯性测量单元获取，无人移动平台的速度由里程计获取；S4：根据所述状态向量构建联邦卡尔曼滤波器的状态方程，根据里程计的观测模型、惯性测量单元的观测模型以及单目相机的观测模型构建联邦卡尔曼滤波器的观测方程；S5：根据联邦卡尔曼滤波算法对所述状态方程与观测方程进行求解，得到所述状态向量的最优解；S6：构建粒子滤波即时定位与地图构建算法的运动状态转移方程与运动观测方程，其中，当无人移动平台的速度小于设定阈值、无转弯或者运动路径上存在障碍物时，所述运动状态转移方程的控制向量为激光雷达对地图待构建区域进行特征匹配得到的位姿，当无人移动平台的速度不小于设定阈值、有转弯或者运动路径上不存在障碍物时，所述运动状态转移方程的控制向量为所述状态向量的最优解中包含的位姿；其中，所述位姿为所述全局坐标与无人移动平台的航向角；S7：对所述运动状态转移方程与运动观测方程进行迭代求解，得到地图待构建区域的全局地图。进一步地，所述根据二维码的坐标得到无人移动平台在世界坐标系下的初始全局坐标，具体为：通过计算机视觉的方法获取二维码中心点到单目相机镜头中心点的变换矩阵，单目相机镜头中心点到无人移动平台中心点的变换矩阵；根据二维码在地图待构建区域上的坐标以及两个变换矩阵，得到无人移动平台在世界坐标系下的初始全局坐标。进一步地，步骤S4所述根据所述状态向量构建联邦卡尔曼滤波器的状态方程，根据里程计的观测模型、惯性测量单元的观测模型以及单目相机的观测模型构建联邦卡尔曼滤波器的观测方程，具体包括以下步骤：S401：假设k时刻，状态向量为Xc(k)＝[xk,yk,θk,vk,ωk]T，其中，xk,yk为无人移动平台在世界坐标系下的全局坐标，θk表示无人移动平台的航向角，vk表示无人移动平台的速度，ωk表示无人移动平台的角速度，T为转置；在联邦卡尔曼滤波过程的预测阶段，所述无人移动平台的速度、角速度以及航向角为设定的预期值，在联邦卡尔曼滤波过程的更新阶段，无人移动平台的速度通过里程计获取，无人移动平台的角速度与航向角通过惯性测量单元获取；S402：根据状态向量构建联邦卡尔曼滤波器的状态方程Xc(k+1)＝f(Xc(k))+Wk：其中，Wk为联邦卡尔曼滤波器的过程噪声，Δt为k+1时刻和k时刻的时间间隔，f(Xc(k))为非线性状态转移函数；S403：将联邦卡尔曼滤波器拆分为第一子滤波器与第二子滤波器，其中，第一子滤波器的观测方程为Z1(k+1)＝H1Xc(k)+V1(k)，具体的：其中，Zodom为里程计的观测模型，ZMEMS为惯性测量单元的观测模型，V1(k)为里程计与惯性测量单元的量测噪声，H1为第一子滤波器的观测矩阵；第二子滤波器的观测方程为Z2(k+1)＝H2Xc(k)+V2(k)，具体的：其中，Ztag为单目相机的观测模型，V2(k)为单目相机与惯性测量单元的量测噪声，H2为第二子滤波器的观测矩阵。进一步地，步骤S5所述的对所述状态方程与观测方程进行迭代求解，得到所述状态向量的最优解，具体包括以下步骤：S501：根据信息总分配原则，将联邦卡尔曼滤波器的过程噪声Wk的协方差Q(k)、联邦卡尔曼滤波器的估计误差协方差P(k)分配到第一子滤波器与第二子滤波器：其中，l＝1,2，β1+β2＝1，Ql(k)为各子滤波器的k时刻量测噪声的协方差，Pl(k)为各子滤波器k时刻的估计误差协方差，为假定的k时刻状态向量Xc(k)的全局最优解，为假定的各子滤波器k时刻状态向量的全局最优解；S502：根据状态方程Xc(k+1)＝f(Xc(k))+Wk对无人移动平台k+1时刻的状态进行预测，得到各子滤波器k+1时刻的状态向量的预测值与估计误差协方差的预测值，具体的：Pl(k+1,k)＝ФPl(k)ФT+Ql(k)其中，Ф为联邦卡尔曼滤波器状态方程的转移矩阵，通过求解非线性状态转移函数f(Xc(k))的雅可比矩阵得到，为各子滤波器k+1时刻的状态向量的预测值，Pl(k+1,k)为各子滤波器k+1时刻的估计误差协方差的预测值；S503：根据预测值与预测值Pl(k+1,k)，对各子滤波器的状态向量和估计误差协方差进行更新：P-1l(k+1)＝P-1l(k+1,k)+HlTR-1l(k+1)Hl其中，Kl(k+1)为k+1时刻各子滤波器的卡尔曼滤波增益，为各子滤波器k+1时刻的状态向量，P-1l(k+1)为各子滤波器k+1时刻的估计误差协方差的倒数，Zl(k+1)为各子滤波器的观测方程，Hl为各子滤波器的观测矩阵，Rl(k+1)为各子滤波器k+1时刻量测噪声的协方差，R-1l(k+1)为各子滤波器k+1时刻量测噪声的协方差的倒数；S504：分别将各子滤波器k+1时刻的状态向量、k+1时刻的估计误差协方差的倒数进行融合，得到联邦卡尔曼滤波器k+1时刻状态向量的最优解进一步地，步骤S7所述的对所述运动状态转移方程与运动观测方程进行迭代求解，得到地图待构建区域的全本文档来自技高网...

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

【技术特征摘要】
1.一种基于激光和二维码融合的即时定位与地图构建方法，应用于无人移动平台，其特征在于，所述无人移动平台包括处理模块、单目相机、激光雷达、里程计以及惯性测量单元；所述方法包括以下步骤：S1：所述单目相机采集地图待构建区域上预先铺设的二维码，其中，所述二维码包含表征二维码在地图待构建区域上的坐标的ID信息；S2：所述处理模块对二维码进行解码，得到二维码在地图待构建区域上的坐标，再根据二维码的坐标得到无人移动平台在世界坐标系下的初始全局坐标；S3：将无人移动平台在世界坐标系下的全局坐标、无人移动平台的航向角、无人移动平台的速度以及无人移动平台的角速度作为联邦卡尔曼滤波器的状态向量；其中，所述全局坐标由步骤S2中的初始全局坐标以及里程计测量的无人移动平台的行程与速度确定；S4：根据所述状态向量构建联邦卡尔曼滤波器的状态方程，根据里程计的观测模型、惯性测量单元的观测模型以及单目相机的观测模型构建联邦卡尔曼滤波器的观测方程；S5：根据联邦卡尔曼滤波算法对所述状态方程与观测方程进行求解，得到所述状态向量的最优解；S6：构建粒子滤波即时定位与地图构建算法的运动状态转移方程与运动观测方程，其中，当无人移动平台的速度小于设定阈值、无转弯或者运动路径上存在障碍物时，所述运动状态转移方程的控制向量为激光雷达对地图待构建区域进行特征匹配得到的位姿，当无人移动平台的速度不小于设定阈值、有转弯或者运动路径上不存在障碍物时，所述运动状态转移方程的控制向量为所述状态向量的最优解中包含的位姿；其中，所述位姿为所述全局坐标与无人移动平台的航向角；S7：对所述运动状态转移方程与运动观测方程进行迭代求解，得到地图待构建区域的全局地图。2.如权利要求1所述的一种基于激光和二维码融合的即时定位与地图构建方法，其特征在于，所述根据二维码的坐标得到无人移动平台在世界坐标系下的初始全局坐标，具体为：通过计算机视觉的方法获取二维码中心点到单目相机镜头中心点的变换矩阵，单目相机镜头中心点到无人移动平台中心点的变换矩阵；根据二维码在地图待构建区域上的坐标以及两个变换矩阵，得到无人移动平台在世界坐标系下的初始全局坐标。3.如权利要求1所述的一种基于激光和二维码融合的即时定位与地图构建方法，其特征在于，步骤S4所述根据所述状态向量构建联邦卡尔曼滤波器的状态方程，根据里程计的观测模型、惯性测量单元的观测模型以及单目相机的观测模型构建联邦卡尔曼滤波器的观测方程，具体包括以下步骤：S401：假设k时刻，状态向量为Xc(k)＝[xk,yk,θk,vk,ωk]T，其中，xk,yk为无人移动平台在世界坐标系下的全局坐标，θk表示无人移动平台的航向角，vk表示无人移动平台的速度，ωk表示无人移动平台的角速度，T为转置；在联邦卡尔曼滤波过程的预测阶段，所述无人移动平台的速度、角速度以及航向角为设定的预期值，在联邦卡尔曼滤波过程的更新阶段，无人移动平台的速度通过里程计获取，无人移动平台的角速度与航向角通过惯性测量单元获取；S402：根据状态向量构建联邦卡尔曼滤波器的状态方程Xc(k+1)＝f(Xc(k))+Wk：其中，Wk为联邦卡尔曼滤波器的过程噪声，Δt为k+1时刻和k时刻的时间间隔，f(Xc(k))为非线性状态转移函数；S403：将联邦卡尔曼滤波器拆分为第一子滤波器与第二子滤波器，其中，第一子滤波器的观测方程为Z1(k+1)＝H1Xc(k)+V1(k)，具体的：其中，Zodom为里程计的观测模型，ZMEMS为惯性测量单元的观测模型，V1(k)为里程计与惯性测量单元的量测噪声，H1为第一子滤波器的观测矩阵；第二子滤波器的观测方程...

【专利技术属性】
技术研发人员：肖烜，覃梓雨，王晨，刘康妮，刘俐伶，
申请(专利权)人：北京理工大学，
类型：发明
国别省市：北京,11