CN109211251B - 一种基于激光和二维码融合的即时定位与地图构建方法 - Google Patents
一种基于激光和二维码融合的即时定位与地图构建方法 Download PDFInfo
- Publication number
- CN109211251B CN109211251B CN201811110672.2A CN201811110672A CN109211251B CN 109211251 B CN109211251 B CN 109211251B CN 201811110672 A CN201811110672 A CN 201811110672A CN 109211251 B CN109211251 B CN 109211251B
- Authority
- CN
- China
- Prior art keywords
- mobile platform
- unmanned mobile
- filter
- sub
- map
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
- G01C21/30—Map- or contour-matching
- G01C21/32—Structuring or formatting of map data
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3407—Route searching; Route guidance specially adapted for specific applications
- G01C21/343—Calculating itineraries, i.e. routes leading from a starting point to a series of categorical destinations using a global route restraint, round trips, touristic trips
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明提供一种基于激光和二维码融合的即时定位与地图构建方法,将里程计的行程与速度信息、单目相机采集的二维码、惯性测量单元的信息融合为状态向量,根据里程计的观测模型、惯性测量单元的观测模型以及单目相机的观测模型构建联邦卡尔曼滤波器的观测方程,在传统的里程计与惯性测量单元的基础上加入二维码定位信息,从而得到无人移动平台的状态向量的最优解;再利用二维码信息处理的快速性和精确性的特点,形成精确度较高的位姿预测信息,从而降低粒子滤波即时定位与地图构建方法中的粒子数量,由此能够提高低功率处理器,如无人移动平台的定位精度,保证无人移动平台在复杂环境的定位效果,完成地图构建,满足复杂环境的应用需求。
Description
技术领域
本发明属于即时定位与地图构建技术领域,尤其涉及一种基于激光和二维码融合的即时定位与地图构建方法。
背景技术
无人移动平台,例如自动导引运输车(AGV,Automated Guided Vehicle)已经普遍的运用于多个行业,其应用环境也逐渐趋于复杂化,表现在同一导航范围内可能出现各种不同的场景,如:较大范围的空旷路面、复杂排列的货物架等。同时,在实际运用中,高速运行的工业自动导引运输车常常搭载着低功耗处理器。因此,根据目前自动导引运输车的实际应用需求提出合适的导航技术,保证其导航效果,是自动导引运输车能够广泛运用的关键。
现有技术中主有激光即时定位与地图构建和惯性器件相结合的大型叉车搬运AGV,视觉传感器和惯性器件融合的物流机器人;其中,大型叉车搬运AGV利用激光即时定位与地图构建(SLAM,Simultaneous Localization And Mapping)进行地图构建、惯性器件辅助定位完成导航任务,虽然有效的保证了导航的精度,但是该导航技术应用于空旷或转弯的场合容易导致定位失败;物流机器人仅仅通过识别二维码标签完成定位导航,简洁的算法虽然保证了物流机器人高速运行时的定位效果,但是应用场合单一,无法满足复杂环境的应用需求。
发明内容
为解决上述问题,本发明提供一种基于激光和二维码融合的即时定位与地图构建方法,能够完成效果较好且精度较高的轻量级定位与地图构建,满足复杂环境的应用需求。
一种基于激光和二维码融合的即时定位与地图构建方法,应用于无人移动平台,所述无人移动平台包括处理模块、单目相机、激光雷达、里程计以及惯性测量单元;
所述方法包括以下步骤:
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时刻的估计误差协方差的预测值;
P-1 l(k+1)=P-1 l(k+1,k)+Hl T R-1 l(k+1)Hl
其中,Kl(k+1)为k+1时刻各子滤波器的卡尔曼滤波增益,为各子滤波器k+1时刻的状态向量,P-1 l(k+1)为各子滤波器k+1时刻的估计误差协方差的倒数,Zl(k+1)为各子滤波器的观测方程,Hl为各子滤波器的观测矩阵,Rl(k+1)为各子滤波器k+1时刻量测噪声的协方差,R-1 l(k+1)为各子滤波器k+1时刻量测噪声的协方差的倒数;
进一步地,步骤S7所述的对所述运动状态转移方程与运动观测方程进行迭代求解,得到地图待构建区域的全局地图,具体为:
S701:将所述运动观测方程转换为似然函数;
S702:将地图待构建区域平均划分多个栅格,采用激光雷达扫描地图待构建区域的局部区域,将局部区域中存在障碍物的栅格设置为被占用,将局部区域中没有障碍物的栅格设置为可通行,从而得到局部栅格地图,其中,局部栅格地图作为初始全局地图;
S703:根据步骤S6中运动状态转移方程的控制向量生成建议分布函数以及步骤S2中无人移动平台在世界坐标系下的初始全局坐标,预测下一时刻无人移动平台可能出现的位置坐标;
S704:利用步骤S701的似然函数获取步骤S703中下一时刻无人移动平台可能出现的位置坐标的权重;
S705:将下一时刻无人移动平台可能出现的位置坐标的加权和,作为下一时刻无人移动平台最有可能出现的位置坐标,并将下一时刻无人移动平台最有可能出现的位置坐标对应的局部地图更新步骤S702中的初始全局地图;
S706:将下一时刻无人移动平台最有可能出现的位置坐标代替步骤S703中无人移动平台在世界坐标系下的初始全局坐标,重新执行步骤S703~S705,以此类推,直到得到地图待构建区域的全局地图。
有益效果:
本发明提供一种基于激光和二维码融合的即时定位与地图构建方法,将里程计的行程与速度信息、单目相机采集的二维码、惯性测量单元的信息融合为状态向量,根据里程计的观测模型、惯性测量单元的观测模型以及单目相机的观测模型构建联邦卡尔曼滤波器的观测方程,在传统的里程计与惯性测量单元的基础上加入二维码定位信息,也就是说,本发明利用多种数据融合技术提高数据处理的鲁棒性,得到无人移动平台的状态向量的最优解;再利用二维码信息处理的快速性和精确性的特点,形成精确度较高的位姿预测信息,从而降低粒子滤波即时定位与地图构建方法中的粒子数量,即下一时刻无人移动平台可能出现的位置坐标的数量,由此能够提高低功率处理器,如无人移动平台的定位精度,保证无人移动平台在复杂环境的定位效果,完成地图构建,满足复杂环境的应用需求;
此外,由于移动平台运行速度很快,或遇到转弯、空旷的路况时,激光雷达会匹配失效,本发明此时采用精度高于里程计获得的位姿精度的状态向量最优解得到的位姿来代替激光雷达对地图待构建区域进行特征匹配得到的位姿,作为粒子滤波即时定位与地图构建算法的运动状态转移方程中的控制向量,可以降低由于激光雷达匹配失效而导致的定位误差和建图误差。
附图说明
图1为本发明提供的一种基于激光和二维码融合的即时定位与地图构建方法的流程图。
具体实施方式
为了使本技术领域的人员更好地理解本申请方案,下面将结合本申请实施例中的附图,对本申请实施例中的技术方案进行清楚、完整地描述。
参见图1,该图为本实施例提供的一种基于激光和二维码融合的即时定位与地图构建方法的流程图。
一种基于激光和二维码融合的即时定位与地图构建方法,应用于无人移动平台,所述无人移动平台包括处理模块、单目相机、激光雷达、里程计以及惯性测量单元。
可选的,单目相机和激光雷达安装在无人移动平台的车头上,里程计安装在无人移动平台的车轮上,惯性测量单元安装在无人移动平台的中心点位置。
所述方法包括以下步骤:
S1:单目相机采集地图待构建区域预先铺设的二维码,其中,所述二维码包含表征二维码在地图待构建区域上的坐标的ID信息。
需要说明的是,二维码预先铺设在地图待构建区域上,二维码之间的间隔可以根据地图待构建区域是否有拐弯、障碍物而设定,例如,对于没有拐弯且没有障碍物的区域,二维码之间的距离可以为1~2m,而对于有拐弯或者存在障碍物的地方,二维码之间的间距可以更为紧密一些,如取0.5~1m。
S2:处理模块对二维码的图像进行解码,得到二维码在地图待构建区域上的坐标,再根据二维码的坐标得到无人移动平台在世界坐标系下的初始全局坐标。
需要说明的是,虽然在整个地图待构建区域铺设了很多二维码,单目相机只要采集到其中一个二维码,即可确定无人移动平台在世界坐标系下的初始全局坐标。
进一步地,所述根据二维码的坐标得到无人移动平台在世界坐标系下的初始全局坐标,具体为:
S201:通过计算机视觉的方法获取二维码中心点到单目相机镜头中心点的变换矩阵,单目相机镜头中心点到无人移动平台中心点的变换矩阵。
S202:根据二维码在地图待构建区域上的坐标以及两个变换矩阵,得到无人移动平台在世界坐标系下的初始全局坐标。
S3:将无人移动平台在世界坐标系下的全局坐标、无人移动平台的航向角、无人移动平台的速度以及无人移动平台的角速度作为联邦卡尔曼滤波器的状态向量;其中,所述全局坐标由步骤S2中的初始全局坐标以及里程计测量的无人移动平台的行程与速度确定,无人移动平台的航向角与角速度通过惯性测量单元获取,无人移动平台的速度由里程计获取。
需要说明的是,里程计能够实时获取无人移动平台的行程与速度,结合无人移动平台的初始全局坐标,能够确定无人移动平台在世界坐标系下的实时位置,也就是得到无人移动平台在世界坐标系下的全局坐标。
S4:根据所述状态向量构建联邦卡尔曼滤波器的状态方程,根据里程计的观测模型、惯性测量单元(Inertial Measure-mentUnit,IMU)的观测模型以及单目相机的观测模型构建联邦卡尔曼滤波器的观测方程。
可选的,惯性测量单元可以采用微机电***-惯性测量单元(Micro-Electro-Mechanical System-Inertial Measure-ment Unit,MEMS-IMU)实现。
需要说明的是,虽然里程计获取的位置信息、惯性测量单元获取的航向角和角速度是连续的,但长时间累计会产生误差;单目相机获取的二维码的位置信息是准确的,但二维码是离散分布的,想要得到无人移动平台连续且准确的位置信息,则需要根据里程计的观测模型、惯性测量单元的观测模型以及单目相机的观测模型构建联邦卡尔曼滤波器的观测方程,用离散而准确的二维码位置信息去修正连续而有累计误差的里程计的位置信息、惯性测量单元获取的航向角和角速度,从而得到一个融合二维码、里程计以及惯性测量单元的结果的状态向量的最优解。
进一步地,联邦卡尔曼滤波器的状态方程与观测方程的构建方法具体包括以下步骤:
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为第一子滤波器的观测矩阵;其中,惯性测量单元安装在无人移动平台的中心点位置。
需要说明的是,观测矩阵H1的表达式具体为:
观测矩阵H1的前四行对应里程计的观测模型Zodom,观测矩阵H1的前四行与状态向量Xc(k)相乘,则取出状态向量Xc(k)中的无人移动平台在世界坐标系下的全局坐标xk,yk,无人移动平台的速度vk,无人移动平台的角速度ωk。
同理,观测矩阵H1的后两行对应惯性测量单元的观测模型ZMEMS,观测矩阵H1的后两行与状态向量Xc(k)相乘,则取出状态向量Xc(k)中的无人移动平台的航向角θk,无人移动平台的角速度ωk。
第二子滤波器的观测方程为Z2(k+1)=H2Xc(k)+V2(k),具体的:
其中,Ztag为单目相机的观测模型,ZMEMS为惯性测量单元的观测模型,V2(k)为单目相机与惯性测量单元的量测噪声,H2为第二子滤波器的观测矩阵。
需要说明的是,观测矩阵H2的表达式具体为:
观测矩阵H2的前两行对应单目相机的观测模型Ztag,观测矩阵H2的前两行与状态向量Xc(k)相乘,则取出状态向量Xc(k)中的无人移动平台在世界坐标系下的全局坐标xk,yk。
同理,观测矩阵H2的后两行对应惯性测量单元的观测模型ZMEMS,观测矩阵H2的后两行与状态向量Xc(k)相乘,则取出状态向量Xc(k)中的无人移动平台的航向角θk,无人移动平台的角速度ωk。
S5:根据联邦卡尔曼滤波算法对所述状态方程与观测方程进行求解,得到所述状态向量的最优解。
进一步地,所述状态向量的最优解的求解过程包括以下步骤:
S501:根据信息总分配原则,将联邦卡尔曼滤波器的过程噪声Wk的协方差Q(k)、联邦卡尔曼滤波器的估计误差协方差P(k)分配到第一子滤波器与第二子滤波器:
其中,l=1,2,β1+β2=1,Ql(k)为各子滤波器的k时刻量测噪声的协方差,Pl(k)为各子滤波器k时刻的估计误差协方差,为假定的k时刻状态向量Xc(k)的全局最优解,为假定的各子滤波器k时刻状态向量的全局最优解;其中,在初始时刻,联邦卡尔曼滤波器的状态向量初始值为X0,初始估计误差协方差为P0;
需要说明的是,β1与β2的取值越大,说明对应的子滤波器可信度越高;在本实施例中,由于第一子滤波器的观测方程由里程计的观测模型和惯性测量单元的观测模型构建,第二子滤波器的观测方程由单目相机的观测模型和惯性测量单元的观测模型构建,而单目相机采集的二维码的定位比里程计实现的定位更准确,可信度更高;因此,第二子滤波器的权重β2要大于第一子滤波器的权重β1。
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时刻的估计误差协方差的预测值;
P-1 l(k+1)=P-1 l(k+1,k)+Hl T R-1 l(k+1)Hl
其中,Kl(k+1)为k+1时刻各子滤波器的卡尔曼滤波增益,为各子滤波器k+1时刻的状态向量,P-1 l(k+1)为各子滤波器k+1时刻的估计误差协方差的倒数,Zl(k+1)为各子滤波器的观测方程,Hl为各子滤波器的观测矩阵,Rl(k+1)为各子滤波器k+1时刻量测噪声的协方差,R-1 l(k+1)为各子滤波器k+1时刻量测噪声的协方差的倒数;
S6:构建粒子滤波即时定位与地图构建算法的运动状态转移方程与运动观测方程,其中,当无人移动平台的速度小于设定阈值、无转弯或者运动路径上存在障碍物时,所述运动状态转移方程的控制向量为激光雷达对地图待构建区域进行特征匹配得到的位姿,当无人移动平台的速度不小于设定阈值、有转弯或者运动路径上不存在障碍物时,所述运动状态转移方程的控制向量为所述状态向量的最优解中包含的位姿;所述位姿包括所述全局坐标与无人移动平台的航向角。
需要说明的是,当无人移动平台运行速度较慢或没有遇到空旷的路况时,采用激光雷达匹配得到的位姿作为控制向量。当移动平台运行速度很快,或遇到转弯、空旷的路况时,激光雷达会匹配失效,此时采用步骤S5中的状态向量最优解来得到的位姿来确定控制向量,由于步骤S5中确定的状态向量最优解精度高于里程计获得的位姿精度,因此可以降低由于激光雷达匹配失效而导致的定位误差和建图误差。
S7:对所述运动状态转移方程与运动观测方程进行迭代求解,得到地图待构建区域的全局地图。
进一步地,对所述运动状态转移方程与运动观测方程进行迭代求解,具体包括以下步骤:
S701:将所述运动观测方程转换为似然函数。
S702:将地图待构建区域平均划分多个栅格,采用激光雷达扫描地图待构建区域的局部区域,将局部区域中存在障碍物的栅格设置为被占用,将局部区域中没有障碍物的栅格设置为可通行,从而得到局部栅格地图,其中,局部栅格地图作为初始全局地图。
S703:根据步骤S6中运动状态转移方程的控制向量生成建议分布函数以及步骤S2中无人移动平台在世界坐标系下的初始全局坐标,预测下一时刻无人移动平台可能出现的位置坐标。
S704:利用步骤S701的似然函数获取步骤S703中下一时刻无人移动平台可能出现的位置坐标的权重。
需要说明的是,所述似然函数的作用是获取无人移动平台可能出现的位置坐标的权重,其中,似然函数计算得到的概率越高,就代表对应的权重越大,激光雷达扫描到地图待构建区域中的障碍物的概率也越高。
S705:将下一时刻无人移动平台可能出现的位置坐标的加权和,作为下一时刻无人移动平台最有可能出现的位置坐标,并将下一时刻无人移动平台最有可能出现的位置坐标对应的局部地图更新步骤S702中的初始全局地图。
S706:将下一时刻无人移动平台最有可能出现的位置坐标代替步骤S703中无人移动平台在世界坐标系下的初始全局坐标,重新执行步骤S703~S705,以此类推,直到得到地图待构建区域的全局地图。
需要说明的是,下一时刻无人移动平台最有可能出现的位置坐标对应的局部地图,指的是无人移动平台在最有可能出现的位置坐标上,激光雷达扫描到的局部区域的地图,即局部区域中哪些栅格存在障碍物,哪些栅格没有障碍物。
需要说明的是,本实施例还可以根据单目相机的视野范围,将面积较大的未知区域划分为多个地图待构建区域,也就是将单目相机的视野范围作为一个地图待构建区域,则无人移动平台构建完当前地图待构建区域的全局地图后,所述单目相机重新采集另一个地图待构建区域上预先铺设的二维码,重复执行步骤S1~S7,直到遍历未知区域上所有的二维码或者无人移动平台的运动轨迹遍历整个未知区域,最终得到整个未知区域的全局地图。
由此可见,本实施例提供的方法能够在低功率处理器上利用激光SLAM和视觉二维码定位融合的算法来实现无人移动平台的导航。将激光SLAM算法和二维码定位算法进行优化,使得高速运行的无人移动平台能够实现快速定位;利用里程计、单目相机、惯性测量单元以及激光雷达等多传感器融合技术提高无人移动平台的定位精度,保证无人移动平台在复杂环境的定位效果。
当然,本发明还可有其他多种实施例,在不背离本发明精神及其实质的情况下,熟悉本领域的技术人员当然可根据本发明作出各种相应的改变和变形,但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。
Claims (4)
1.一种基于激光和二维码融合的即时定位与地图构建方法,应用于无人移动平台,其特征在于,所述无人移动平台包括处理模块、单目相机、激光雷达、里程计以及惯性测量单元;
所述方法包括以下步骤:
S1:所述单目相机采集地图待构建区域上预先铺设的二维码,其中,所述二维码包含表征二维码在地图待构建区域上的坐标的ID信息;
S2:所述处理模块对二维码进行解码,得到二维码在地图待构建区域上的坐标,再根据二维码的坐标得到无人移动平台在世界坐标系下的初始全局坐标;
S3:将无人移动平台在世界坐标系下的全局坐标、无人移动平台的航向角、无人移动平台的速度以及无人移动平台的角速度作为联邦卡尔曼滤波器的状态向量;其中,所述全局坐标由步骤S2中的初始全局坐标以及里程计测量的无人移动平台的行程与速度确定;
S4:根据所述状态向量构建联邦卡尔曼滤波器的状态方程,根据里程计的观测模型、惯性测量单元的观测模型以及单目相机的观测模型构建联邦卡尔曼滤波器的观测方程;
S5:根据联邦卡尔曼滤波算法对所述状态方程与观测方程进行求解,得到所述状态向量的最优解;
S6:构建粒子滤波即时定位与地图构建算法的运动状态转移方程与运动观测方程,其中,当无人移动平台的速度小于设定阈值、无转弯或者运动路径上存在障碍物时,所述运动状态转移方程的控制向量为激光雷达对地图待构建区域进行特征匹配得到的位姿,当无人移动平台的速度不小于设定阈值、有转弯或者运动路径上不存在障碍物时,所述运动状态转移方程的控制向量为所述状态向量的最优解中包含的位姿;其中,所述位姿为所述全局坐标与无人移动平台的航向角;
S7:对所述运动状态转移方程与运动观测方程进行迭代求解,得到地图待构建区域的全局地图,具体为:
S701:将所述运动观测方程转换为似然函数;
S702:将地图待构建区域平均划分多个栅格,采用激光雷达扫描地图待构建区域的局部区域,将局部区域中存在障碍物的栅格设置为被占用,将局部区域中没有障碍物的栅格设置为可通行,从而得到局部栅格地图,其中,局部栅格地图作为初始全局地图;
S703:根据步骤S6中运动状态转移方程的控制向量生成建议分布函数以及步骤S2中无人移动平台在世界坐标系下的初始全局坐标,预测下一时刻无人移动平台可能出现的位置坐标;
S704:利用步骤S701的似然函数获取步骤S703中下一时刻无人移动平台可能出现的位置坐标的权重;
S705:将下一时刻无人移动平台可能出现的位置坐标的加权和,作为下一时刻无人移动平台最有可能出现的位置坐标,并将下一时刻无人移动平台最有可能出现的位置坐标对应的局部地图更新步骤S702中的初始全局地图;
S706:将下一时刻无人移动平台最有可能出现的位置坐标代替步骤S703中无人移动平台在世界坐标系下的初始全局坐标,重新执行步骤S703~S705,以此类推,直到得到地图待构建区域的全局地图。
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)=H1 Xc(k)+V1(k),具体的:
其中,Zodom为里程计的观测模型,ZMEMS为惯性测量单元的观测模型,V1(k)为里程计与惯性测量单元的量测噪声,H1为第一子滤波器的观测矩阵;
第二子滤波器的观测方程为Z2(k+1)=H2 Xc(k)+V2(k),具体的:
其中,Ztag为单目相机的观测模型,V2(k)为单目相机与惯性测量单元的量测噪声,H2为第二子滤波器的观测矩阵。
4.如权利要求3所述的一种基于激光和二维码融合的即时定位与地图构建方法,其特征在于,步骤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时刻的估计误差协方差的预测值;
Kl(k+1)=Pl(k+1,k)Hl T[HlPl(k+1,k)Hl T+Rl(k+1)]-1
P-1 l(k+1)=P-1 l(k+1,k)+Hl TR-1 l(k+1)Hl
其中,Kl(k+1)为k+1时刻各子滤波器的卡尔曼滤波增益,为各子滤波器k+1时刻的状态向量,P-1 l(k+1)为各子滤波器k+1时刻的估计误差协方差的倒数,Zl(k+1)为各子滤波器的观测方程,Hl为各子滤波器的观测矩阵,Rl(k+1)为各子滤波器k+1时刻量测噪声的协方差,R-1 l(k+1)为各子滤波器k+1时刻量测噪声的协方差的倒数;
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811110672.2A CN109211251B (zh) | 2018-09-21 | 2018-09-21 | 一种基于激光和二维码融合的即时定位与地图构建方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811110672.2A CN109211251B (zh) | 2018-09-21 | 2018-09-21 | 一种基于激光和二维码融合的即时定位与地图构建方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109211251A CN109211251A (zh) | 2019-01-15 |
CN109211251B true CN109211251B (zh) | 2022-01-11 |
Family
ID=64985500
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201811110672.2A Active CN109211251B (zh) | 2018-09-21 | 2018-09-21 | 一种基于激光和二维码融合的即时定位与地图构建方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109211251B (zh) |
Families Citing this family (20)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109725327B (zh) * | 2019-03-07 | 2020-08-04 | 山东大学 | 一种多机构建地图的方法及*** |
CN109900280B (zh) * | 2019-03-27 | 2020-12-11 | 浙江大学 | 一种基于自主导航的畜禽信息感知机器人与地图构建方法 |
CN110161527B (zh) * | 2019-05-30 | 2020-11-17 | 华中科技大学 | 一种基于rfid和激光雷达的三维地图重构***及方法 |
CN110285806A (zh) * | 2019-07-05 | 2019-09-27 | 电子科技大学 | 基于多次位姿校正的移动机器人快速精确定位算法 |
CN110647609B (zh) * | 2019-09-17 | 2023-07-18 | 上海图趣信息科技有限公司 | 视觉地图定位方法及*** |
CN110861082B (zh) * | 2019-10-14 | 2021-01-22 | 北京云迹科技有限公司 | 辅助建图方法、装置、建图机器人及存储介质 |
CN110823224B (zh) * | 2019-10-18 | 2021-10-08 | 中国第一汽车股份有限公司 | 一种车辆定位方法及车辆 |
CN111006655B (zh) * | 2019-10-21 | 2023-04-28 | 南京理工大学 | 机场巡检机器人多场景自主导航定位方法 |
CN111060135B (zh) * | 2019-12-10 | 2021-12-17 | 亿嘉和科技股份有限公司 | 一种基于局部地图的地图修正方法及*** |
CN111337011A (zh) * | 2019-12-10 | 2020-06-26 | 亿嘉和科技股份有限公司 | 一种基于激光和二维码融合的室内定位方法 |
CN113124850B (zh) * | 2019-12-30 | 2023-07-28 | 北京极智嘉科技股份有限公司 | 机器人、地图生成方法、电子设备及存储介质 |
CN111242996B (zh) * | 2020-01-08 | 2021-03-16 | 郭轩 | 一种基于Apriltag标签与因子图的SLAM方法 |
CN111624618A (zh) * | 2020-06-09 | 2020-09-04 | 安徽意欧斯物流机器人有限公司 | 融合激光slam和二维码导航的定位方法及搬运平台 |
CN111679677B (zh) * | 2020-06-24 | 2023-10-03 | 浙江华睿科技股份有限公司 | Agv的位姿调整方法、装置、存储介质、电子装置 |
CN111766603B (zh) * | 2020-06-27 | 2023-07-21 | 长沙理工大学 | 基于AprilTag码视觉辅助定位的移动机器人激光SLAM方法、***、介质及设备 |
CN113947097B (zh) * | 2020-07-15 | 2024-04-09 | 花瓣云科技有限公司 | 一种二维码识别的方法及电子设备 |
CN112254729A (zh) * | 2020-10-09 | 2021-01-22 | 北京理工大学 | 一种基于多传感器融合的移动机器人定位方法 |
CN113405544B (zh) * | 2021-05-08 | 2024-02-09 | 中电海康集团有限公司 | 一种移动机器人的建图与定位方法及*** |
CN114323020B (zh) * | 2021-12-06 | 2024-02-06 | 纵目科技(上海)股份有限公司 | 车辆的定位方法、***、设备及计算机可读存储介质 |
CN114236564B (zh) * | 2022-02-23 | 2022-06-07 | 浙江华睿科技股份有限公司 | 动态环境下机器人定位的方法、机器人、装置及存储介质 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101893443A (zh) * | 2010-07-08 | 2010-11-24 | 上海交通大学 | 道路数字正射影像地图的制作*** |
CN104777835A (zh) * | 2015-03-11 | 2015-07-15 | 武汉汉迪机器人科技有限公司 | 一种全向自动叉车及3d立体视觉导航定位方法 |
CN105607635A (zh) * | 2016-01-05 | 2016-05-25 | 东莞市松迪智能机器人科技有限公司 | 自动导引车全景光学视觉导航控制***及全向自动导引车 |
CN106154287A (zh) * | 2016-09-28 | 2016-11-23 | 深圳市普渡科技有限公司 | 一种基于双轮里程计与激光雷达的地图构建方法 |
CN107463173A (zh) * | 2017-07-31 | 2017-12-12 | 广州维绅科技有限公司 | 仓储agv导航方法及装置、计算机设备及存储介质 |
CN108253961A (zh) * | 2016-12-29 | 2018-07-06 | 北京雷动云合智能技术有限公司 | 一种基于imu的轮式机器人定位方法 |
-
2018
- 2018-09-21 CN CN201811110672.2A patent/CN109211251B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101893443A (zh) * | 2010-07-08 | 2010-11-24 | 上海交通大学 | 道路数字正射影像地图的制作*** |
CN104777835A (zh) * | 2015-03-11 | 2015-07-15 | 武汉汉迪机器人科技有限公司 | 一种全向自动叉车及3d立体视觉导航定位方法 |
CN105607635A (zh) * | 2016-01-05 | 2016-05-25 | 东莞市松迪智能机器人科技有限公司 | 自动导引车全景光学视觉导航控制***及全向自动导引车 |
CN106154287A (zh) * | 2016-09-28 | 2016-11-23 | 深圳市普渡科技有限公司 | 一种基于双轮里程计与激光雷达的地图构建方法 |
CN108253961A (zh) * | 2016-12-29 | 2018-07-06 | 北京雷动云合智能技术有限公司 | 一种基于imu的轮式机器人定位方法 |
CN107463173A (zh) * | 2017-07-31 | 2017-12-12 | 广州维绅科技有限公司 | 仓储agv导航方法及装置、计算机设备及存储介质 |
Non-Patent Citations (5)
Title |
---|
A sliding-window visual-IMU odometer based on tri-focal tensor geometry;Jwu-Sheng Hu 等;《2014 IEEE International Conference on Robotics and Automation (ICRA)》;20140929;第3963-3968页 * |
Enhancement of mobile robot localization using extended Kalman filter;Mohammed Faisal 等;《Advances in Mechanical Engineering》;20161121;第1-11页 * |
基于二维码和角点标签的无人小车室内定位算法研究实现;尚明超;《中国优秀硕士学位论文全文数据库》;20180515;第1-84页 * |
基于图优化SLAM的移动机器人导航方法研究;邹谦;《中国优秀硕士学位论文全文数据库》;20161215;第1-68页 * |
基于激光Slam的仓储搬运AGV定位技术研究;郝岩;《中国优秀硕士学位论文全文数据库》;20171215;第1-96页 * |
Also Published As
Publication number | Publication date |
---|---|
CN109211251A (zh) | 2019-01-15 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109211251B (zh) | 一种基于激光和二维码融合的即时定位与地图构建方法 | |
US10983217B2 (en) | Method and system for semantic label generation using sparse 3D data | |
JP7086111B2 (ja) | 自動運転車のlidar測位に用いられるディープラーニングに基づく特徴抽出方法 | |
Scherer et al. | River mapping from a flying robot: state estimation, river detection, and obstacle mapping | |
CN111402339B (zh) | 一种实时定位方法、装置、***及存储介质 | |
JP6855524B2 (ja) | 低速特徴からのメトリック表現の教師なし学習 | |
JP2021139898A (ja) | 測位方法、装置、計算装置、コンピュータ可読記憶媒体及びコンピュータプログラム | |
EP3497405A1 (en) | System and method for precision localization and mapping | |
US11158065B2 (en) | Localization of a mobile unit by means of a multi hypothesis kalman filter method | |
CN111260751B (zh) | 基于多传感器移动机器人的建图方法 | |
CN114111774B (zh) | 车辆的定位方法、***、设备及计算机可读存储介质 | |
CN114325634A (zh) | 一种基于激光雷达的高鲁棒性野外环境下可通行区域提取方法 | |
CN114323033A (zh) | 基于车道线和特征点的定位方法、设备及自动驾驶车辆 | |
CN112254728A (zh) | 基于关键路标增强ekf-slam全局优化的方法 | |
CN117685953A (zh) | 面向多无人机协同定位的uwb与视觉融合定位方法及*** | |
Mutz et al. | What is the best grid-map for self-driving cars localization? An evaluation under diverse types of illumination, traffic, and environment | |
Xu et al. | Dynamic vehicle pose estimation and tracking based on motion feedback for LiDARs | |
CN117387604A (zh) | 基于4d毫米波雷达和imu融合的定位与建图方法及*** | |
CN113093759A (zh) | 基于多传感器信息融合的机器人编队构造方法及*** | |
CN116774247A (zh) | 基于ekf的多源信息融合的slam前端策略 | |
Youssefi et al. | Visual and light detection and ranging-based simultaneous localization and mapping for self-driving cars | |
CN113379915B (zh) | 一种基于点云融合的行车场景构建方法 | |
CN115345944A (zh) | 外参标定参数确定方法、装置、计算机设备和存储介质 | |
CN113034538B (zh) | 一种视觉惯导设备的位姿跟踪方法、装置及视觉惯导设备 | |
CN112304321B (zh) | 一种基于视觉和imu的车辆融合定位方法及车载终端 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |