CN109613549A - 一种基于卡尔曼过滤的激光雷达定位方法 - Google Patents
一种基于卡尔曼过滤的激光雷达定位方法 Download PDFInfo
- Publication number
- CN109613549A CN109613549A CN201811618459.2A CN201811618459A CN109613549A CN 109613549 A CN109613549 A CN 109613549A CN 201811618459 A CN201811618459 A CN 201811618459A CN 109613549 A CN109613549 A CN 109613549A
- Authority
- CN
- China
- Prior art keywords
- matching
- reflector
- road sign
- laser radar
- combination
- 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.)
- Granted
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/06—Systems determining position data of a target
- G01S17/42—Simultaneous measurement of distance and other co-ordinates
Landscapes
- Physics & Mathematics (AREA)
- Electromagnetism (AREA)
- Engineering & Computer Science (AREA)
- Computer Networks & Wireless Communication (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Optical Radar Systems And Details Thereof (AREA)
- Navigation (AREA)
Abstract
本发明适用于自动控制技术领域,提供了一种基于卡尔曼过滤的激光雷达定位方法,该方法包括如下步骤:S1、基于当前帧中的3个已知反光来计算激光雷达当前在路标地图中的位姿P(xp,yp,θp);S2、基于卡尔曼过滤对激光雷达的当前位姿进行优化,得到当前时刻的最优位姿P′(xp,yp,θp);S3、检测当前帧中是否存在未知反光板,若检测结果为是,则基于激光雷达当前的最优位姿P′(xp,yp,θp)来计算未知反光板在路标地图中的坐标,并更新路标地图。对计算的激光雷达位姿进行卡尔曼过滤,获取更为精准的激光雷达位姿,提高定位精度。
Description
技术领域
本发明属于自动控制技术领域,提供了一种基于卡尔曼过滤的激光雷达定位方法。
背景技术
随着社会的发展和技术的进步,移动机器人越来越深的介入到人类的日常生活中,例如家庭中的清洁机器人、工厂中的搬运机器人以及餐馆中的送餐机器人等。移动机器人想要实现上述功能必须准确的知道自身所在的位置,即实时定位。而移动机器人实现实时定位的先决条件是建立地图,这是机器人导航和其它智能行为的关键。移动机器人定位导航常用的传感器有磁导航、摄像机和激光雷达等。其中磁导航的导航路径单一,不利于扩展和更改,使用的局限性较大。基于视觉的定位导航方法受光源条件影响大,稳定性和精度较差,无法让移动机器人进行稳定、精密的操作。基于激光雷达的定位方法可以分为基于反光板的定位导航方法和基于轮廓的定位导航方法,其中基于轮廓的定位导航方法计算量大、精度较差,无法适应移动机器人精密操作的要求;基于反光板的激光雷达定位导航方法精度高、稳定性好。现有的基于反光板的激光雷达定位方法分为三边法和三角法,三边法在定位时将反光板的角度信息和距离信息分开计算,导致定位的精度降低;三角法定位计算复杂,而且定位精度对反光板的分布要求较高。
发明内容
本发明实施例提供一种基于卡尔曼过滤的激光雷达定位方法,通过卡尔曼过滤对激光雷达位姿进行优化,获取更为精准的激光雷达位姿,即提高了定位精度。
为了实现上述目的,本发明提供了一种基于卡尔曼过滤的激光雷达定位方法,该方法包括如下步骤:
S1、基于当前帧中的3个已知反光来计算激光雷达当前在路标地图中的位姿P(xp,yp,θp);
S2、基于卡尔曼过滤对激光雷达的当前位姿进行优化,得到当前时刻的最优位姿P′(xp,yp,θp);
S3、检测当前帧中是否存在未知反光板,若检测结果为是,则基于激光雷达当前的最优位姿P′(xp,yp,θp)来计算未知反光板在路标地图中的坐标,并更新路标地图。
进一步的,当前帧中的已知反光板的获取方法具体如下:
S4、检测激光雷达在上一帧中是否定位成功;
S5、若检测结果为是,则基于动态匹配来匹配出当前帧中的已知反光板和未知反光板,若检测结果为否,则基于静态匹配来匹配出当前帧中的已知反光板和未知反光板。
进一步的,在步骤S4之前还包括:
S6、检测当前帧中是否提取到至少3个反光板;
S7、若当前帧中存在至少3个反光板则,检测路标地图中是否存在路标,若检测结果为是,则执行步骤S1,若检测结果为否,则以激光雷达中心为路标地图中原点,计算当前帧中所有反光板在路标地图中的坐标。
进一步的,所述静态匹配具体包括如下步骤:
S11、提取路标地图中的m个路标点,计算两两路标点之间的距离,得到全局距离列表D;
S12、提取当前帧中的n个反光板,计算两两反光板之间的距离,得到局部距离列表d;
S13、选择全局距离列表中的第i行Di与局部距离列表中的第j行dj,计算Di行中第k元素与dj行中第l个元素的绝对差值,若绝对差值小于matchW,则将第k个路标点加入到第l个反光板的候选匹配中,将Di行中的每一个元素与dj行中的每一个元素的绝对差值与matchW进行比较,得到Di与dj的匹配表matchListi,j;
S14、查找匹配表matchListi,j中的最佳组合,输出最佳匹配组合的匹配数目macth_count和匹配误差macth_err,若macth_count≥3且macth_err小于当前全局最优匹配的误差best_err,用当前匹配组合替换全局最优的匹配组合,令best_err=macth_err;
S15、检测当前j的取值是否等于n,若j≠n,则令j=j+1,并执行步骤S13,若j=n,则检测当前i的取值是否为m,若i≠m,则令i=i+1,j=1,执行步骤S13,若i=m,则输出全局最优匹配,若全局最优匹配的best_err<matchW,则反光板匹配成功,返回全局最优匹配组合,若best_err≥matchW,则输出匹配失败。
进一步的,所述动态匹配具体包括如下步骤:
S21、计算路标地图中所有路标点距激光雷达中心的距离,提取距离大于最小探测距离distmin,且小于最大探测距离distmax的路标点,共z个;
S22、基于激光雷达在上一帧中的位姿,计算n个反光板在路标地图中的坐标;
S23、计算反光板rk与路标点Rl的距离若将路标点Rl加入到反光板rk的匹配列表中;
S24、检测当前l的取值是否等于z,若l≠z,则令l=l+1,执行步骤S23,若l=z,则检测当前k的取值是否为n,若k≠n,则令k=k+1,l=1,执行步骤S23,若k=n,则输出匹配表matchList;
S25、查找匹配表matchList中的最优匹配组合,最优匹配组合的匹配数目大于2,且匹配组合的macth_err最小,最优匹配组合的匹配误差小于matchW,则认为反光板匹配成功,返回最优匹配组合,否则,匹配失败。
进一步的,匹配表中的最优匹配组合的获取方法包括如下步骤:
S31、按照设定的顺序从每个反光板的可能匹配路标点中选择一个,得到一个匹配组合
S32、删除匹配组合中错误匹配,获得匹配组合计算的匹配误差matchErr,若matchErr小于当前最佳匹配组合的匹配误差min Err,则将所述匹配组合设为最佳匹配,令min Err=matchErr,返回步骤S31,直至遍历匹配表中所有的匹配组合,输出最佳匹配、最佳匹配的匹配数目及匹配误差。
本发明提供的基于卡尔曼过滤的激光雷达定位方法具有如下有益效果:
1.对计算的激光雷达位姿进行卡尔曼过滤,获取更为精准的激光雷达位姿,提高了定位精度,尤其至针对激光雷达扫描频率过大,即运动幅度过大的情况,可以极大的提高定位的精准度;
2.采用坐标转换法进行定位,提高了定位的精度。
附图说明
图1为本发明实施例提供的基于卡尔曼过滤的激光雷达定位方法流程图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白,以下结合附图及实施例,对本发明进行进一步详细说明。应当理解,此处所描述的具体实施例仅仅用以解释本发明,并不用于限定本发明。
在本发明中,将激光雷达水平放置,反光板垂直于地面张贴,激光雷达的扫描线与反光板的中点大致对齐,图1为本发明实施例提供的基于卡尔曼过滤的激光雷达定位方法流程图,该方法具体包括如下步骤:
S1、提取当前帧中的反光板;
基于激光雷达探测距离与反光板反射强度之间的关系,筛选出反光板上的扫描点,利用单个反光板上的反射点集拟合得到反光板中心,计算反光板中心到激光雷达中心的距离、及夹角,并计算反光板中心在激光雷达坐标系中的坐标。
S2、检测当前帧中是否至少存在3个反光板,若当前帧中不存在至少3个反光板,则控制激光雷达重新扫描,并返回步骤S1;若当前帧中存在至少3个反光板,则检测路标地图中是否存在路标,若检测结果为否,则以激光雷达的中心为坐标原点,建立路标坐标系,并计算当前帧中所有反光板在路标地图中的位置,检测结果为是,则执行步骤S3,
S3、检测激光雷达在上一帧中是否定位成功,若检测结果为是,则基于动态匹配来匹配出当前帧中的已知反光板和未知反光板,若检测结果为否,则基于静态匹配来匹配出当前帧中的已知反光板和未知反光板;
本发明中的反光板即为路标,已知反光板即为已经纳入路标地图中的反光板,未知反光板是指未纳入地图中的反光板,将未知反光板不断的纳入路标地图即为建立路标地图的过程,直至所有反光板全部纳入路标地图,则路标地图构建完毕;只有在首次构建路标地图时,或者是上一帧中扫描到的已知反光板数量少于3个时,采用静态匹配来提取当前帧中的已知反光板或未知反光板,否则均采用动态匹配来提取已知反光板和未知反光板,动态匹配过程相对于静态匹配过程的计算量小。
S4、检测当前帧中是否存在至少3个已知反光板,检测结果为否,则控制激光雷达重新扫描,并返回步骤S1,若检测结果为是,则基于三个已知反光板来计算激光雷达在路标地图中的位姿P(xp,yp,θp),其计算过程具体如下:
已知反光板在雷达坐标系中的局部坐标Xloc、以及在地图坐标系中的地图坐标Xmap,反光板的局部坐标到地图坐标之间转换满足刚体变换,因此根据公式
Xmap=RXloc+t
计算出雷达坐标系与地图坐标系之间的旋转矩阵R、平移向量t。因为激光雷达的局部坐标为则激光雷达在地图中的坐标激光雷达相对于地图坐标系的旋转角θ可由旋转矩阵R分解得到;
S5、基于卡尔曼过过滤对激光雷达当前的位姿P(xp,yp,θp)进行优化,获取激光雷达当前时刻的最优位姿P′(xp,yp,θp);
利用卡尔曼滤波对激光雷达当前计算得到的位姿数据进行滤波处理,得到更准确的位姿值。对于离散控制过程的系统,其系统状态和系统测量值可以进行以下表示:
X(k)=AX(k-1)+BU(k)+W(k)
Z(k)=HX(k)+V(k)
其中X(k)为k时刻的系统状态,A为状态转移矩阵,B为控制输入矩阵,U(k)为k时刻对系统的控制量,Z(k)为k时刻的测量值,H为系统测量矩阵,W(k)为系统过程噪声,V(k)为测量噪声。
S6、检测是否存在未知反光板,若检测结果为是,则控制激光雷达重新扫描,并执行步骤S1,若检测结果为否,基于雷达坐标系与地图坐标系之间的旋转矩阵R、平移向量t来计算未知反光板在路标地图中的坐标,并更新路标地图。
计算公式具体如下:
其中,(xloc,yloc)为未知反光板在激光雷达坐标系中的坐标。
本发明提供的基于卡尔曼过滤的激光雷达定位方法具有如下有益效果:
1.对计算的激光雷达位姿进行卡尔曼过滤,获取更为精准的激光雷达位姿,提高定位精度的时候,也提高了路标地图的精准度,尤其至针对激光雷达扫描频率过大,即运动幅度过大的情况,可以极大的提高定位的精准度;
2.采用坐标转换法进行定位,提高了定位的精度。
在本发明涉及的静态匹配及动态匹配可以采用现有的方法,或者是基于如下如下方法来实现静态匹配或动态匹配。
在本发明实施例中,设置反光板匹配允许的误差为matchW,定义全局最优匹配的误差best_err等于无穷大,本发明中的静态匹配具体包括如下步骤:
S11、根据路标地图中m个路标点R1(X1,Y1),...,Rm(Xm,Ym),计算两两路标点之间的距离,得到全局距离列表D1,2,...,D1,m;...;Dm,1,...,Dm,m-1;
其中,Dm,m-1表示路标点Rm与路标点Rm-1之间的距离,(Xm,Ym)为第m个路标点Rm在路标地图中的坐标;
S12、根据当前帧中的n个反光板r1(x1,y1),...,rn(xn,yn),计算两两反光板之间的距离,得到局部距离列表d1,2,....,d1,n;....;dn,1,...,dn,n-1;
dn,n-1表示反光板rn与反光板rn-1之间的距离,(xn,yn)为第n个反光板rn在激光雷达坐标系中的坐标,根据激光雷达探测距离与反光板反射强度之间的关系筛选出反光板上的扫描点,利用单个反光板上的反射点集拟合得到反光板中心,检测反光板中心距激光雷达中心的距离di及角度θi,用ri(di,θi)来表示反光板ri的中心距激光雷达中心的距离di及夹角θi,基于ri(di,θi)计算反光板中心在激光雷达坐标系中的坐标。
S13、选择全局距离列表中的第i行Di与局部距离列表中的第j行dj,假设路标地图中第i个路标点与当前帧中的第j个反光板匹配成功,将Di行中的每一个元素与dj行中的每一个元素进行对比,若Di行中的第k个元素,与dj行中的第l个元素满足abs(Di,k-dj,l)<matchW,则将第k个路标点加入到第l个反光板的候选匹配中,对比结束得到Di与dj的匹配表matchListi,j={r1:Ri,...;...;rn:Rj,...};
S14、对匹配表matchListi,j进行最佳匹配查找,得到当前最佳的匹配组合、匹配数目macth_count和匹配误差macth_err。若macth_count≥3且macth_err小于全局最优匹配的误差best_err,用当前匹配组合替换全局最优的匹配组合,令best_err=macth_err;
S15、检测当前j的取值是否等于n,若j≠n,则令j=j+1,并执行步骤S13,若j=n,则检测当前i的取值是否为m,若i≠m,则令i=i+1,j=1,执行步骤S13,若i=m,则输出全局最优匹配,若全局最优匹配的best_err<matchW,则反光板匹配成功,返回全局最优匹配组合,若best_err≥matchW,则输出匹配失败。
在本发明实施例中,反光板的动态匹配具体包括如下步骤:
在激光雷达在上一帧中的位姿获取成功,在上一帧中的位姿为pose(x,y,θ),基于动态匹配来寻找反光板对应的路标点,动态匹配的步骤如下:
S21、计算路标地图中所有路标点R1(X1,Y1),....,Rm(Xm,Ym)距激光雷达中心的距离,提取距离大于最小探测距离distmin,且小于最大探测距离distmax的路标点R1(X1,Y1),....,Rz(Xz,Yz)
S22、基于激光雷达在上一帧中的位姿pose(x,y,θ),计算n个反光板r1(x1,y1),...,rn(xn,yn)在路标地图中的坐标P(x,y),
S23、计算反光板rk与路标点Rl的距离若将路标点Rl加入到反光板rk的匹配列表中;
S24、检测当前l的取值是否等于z,若l≠z,则令l=l+1,执行步骤S23,若l=z,则检测当前k的取值是否为n,若k≠n,则令k=k+1,l=1,执行步骤S23,若k=n,则输出匹配表matchList r1:Ri,....;....;rn:Rj,...;
S25、查找匹配表matchList中的最优匹配组合,若最优匹配组合的匹配数目大于2,且macth_err最小,若最优匹配组合的匹配误差小于matchW,则认为反光板匹配成功,返回最优匹配组合,否则,匹配失败。
在本发明实施例中,静态匹配及动态匹配中匹配表中的最佳匹配查找方法具体如下:
S31、按照设定的顺序从每个反光板的可能匹配路标点中选择一个,得到一个匹配组合
已知当前扫描到n个反光板,每个反光板所有的可能匹配为r1:Ri,....;....;rn:Rj,...,设置当前最佳匹配的误差为min Err。
S32、删除匹配组合中错误匹配,获得匹配组合计算的匹配误差matchErr,若matchErr小于当前最佳匹配组合的匹配误差min Err,则将所述匹配组合设为当前最佳匹配,令min Err=matchErr,返回步骤S31,直至遍历匹配表中所有的匹配组合,输出最佳匹配、最佳匹配的匹配数目及匹配误差。
在本发明实施例中,错误匹配的删除方法具体包括如下步骤:
S321、从匹配组合中顺序提取n个反光板及其对应的路标点;
S322、计算第i个反光板ri与剩余反光板rj(j≠i)间距离以及路标点Ri与剩余路标点Rj(j≠i)间的距离路标点Ri为反光板ri的可能匹配路标点,路标点Rj为反光板ri的可能匹配路标点;
S323、计算与之差deta_dist,统计deta_dist小于matchW的个数macth_count_i,若macth_count_i≥3,则匹配组合的匹配数目macth_count加1,否则,将反光板ri的匹配路标点设为空;
S324、检测i是否等于n,若i≠n,则令i=i+1,并执行步骤S322,若i=n,则输出匹配组合即将更新为及匹配组合的数目macth_count,若macth_count<3,则返回步骤S321,若macth_count≥3,执行步骤S325;
S325、将匹配组合中的反光板ri与反光板rj进行比较,检测反光板ri与反光板rj是否匹配到同一路标点;
S326、若反光板ri与反光板rj未匹配到同一路标点,则检测j是否等于n,若j≠n,令j=j+1,执行步骤S325,若j=n,则检测i是否等于n,若i≠n,令i=i+1,j=i+1执行步骤S325,若i=n,则输出匹配组合若反光板ri与反光板rj匹配到同一路标点,则匹配组合的macth_count减去1,检测macth_count是否小于3,若macth_count<3,则返回步骤S321,若macth_count≥3,则执行步骤S327;
S327、提取反光板ri、rj及两反光板ri、rj的剩余反光板rm(m≠i,j),剩余反光板中不包括匹配路标点为空的反光板;
S328、计算反光板ri与剩余反光板rm之间的距离及路标点Ri与剩余路标点Rm间的距离计算距离差将距离差进行累积计算反光板rj与剩余反光板rm之间的距离计算路标点Rj与剩余路标点Rm间的距离计算距离差将距离差进行累积其中,路标点Ri为反光板ri在匹配组合中的匹配路标点,路标点Rm为反光板rm在该匹配组合中的匹配路标点;
S329、比较sumi与sumj,若sumi<sumj,则保留反光板ri匹配的路标点,将反光板rj的匹配路标点设为空,若sumi>sumj,保留反光板rj匹配的路标点,将反光板ri的匹配路标点设为空;
S330、在匹配组合中提取匹配路标点不为空的反光板r1,...,rs;
S331、计算反光板ri与反光板rj之间的距离及路标点Ri与路标点Rj之间的距离路标点Ri与路标点Rj分别为反光板ri与反光板rj在匹配组合中的匹配路标点;
S332、计算与的差值,若与的差值大于matchW,则执行步骤S333;若与的差值小于或等于matchW,检测j是否等于s,若j≠s,令j=j+1,执行步骤S331,若j=s,则检测i是否等于s,若i≠s,则令i=i+1,j=i+1,执行步骤S331,若i=s,则输出
S333、反光板ri与反光板rj之间至少有一个匹配错误,令匹配组合macthlisti2的macth_count减1,若macth_count<3,则返回步骤S321,若macth_count≥3,则执行步骤S334;
S334、提取反光板ri、rj及两反光板ri、rj的剩余反光板rm(m≠i,j),且剩余反光板中不包括匹配路标点为空的反光板;
S335、计算反光板ri与剩余反光板rm间的距离及路标点Ri与剩余路标点Rm间的距离计算距离差将距离差进行累积计算反光板rj与剩余反光板rm间的距离及路标点Rj与剩余路标点Rm间的距离计算距离差将距离差进行累积其中,路标点Ri为反光板ri在匹配组合中的匹配路标点,路标点Rm为反光板rm在匹配组合中的匹配路标点;
S336、将sumi与sumj进行比较,若sumi<sumj,则保留反光板ri匹配的路标点,将反光板rj的匹配路标点设为空,sumi≥sumj,保留反光板rj匹配的路标点,将反光板ri的匹配路标点设为空。
该反光板的匹配方法具有如下有益效果:分层进行匹配,提高了匹配的速度,提高匹配的可靠性。
以上所述仅为本发明的较佳实施例而已,并不用以限制本发明,凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等,均应包含在本发明的保护范围之内。
Claims (6)
1.一种基于卡尔曼过滤的激光雷达定位方法,其特征在于,所述方法包括如下步骤:
S1、基于当前帧中的3个已知反光板来计算激光雷达当前在路标地图中的位姿P(xp,yp,θp);
S2、基于卡尔曼过滤对激光雷达的当前位姿进行优化,得到当前时刻的最优位姿P′(xp,yp,θp);
S3、检测当前帧中是否存在未知反光板,若检测结果为是,则基于激光雷达当前的最优位姿P′(xp,yp,θp)来计算未知反光板在路标地图中的坐标,并更新路标地图。
2.如权利要求1所述基于卡尔曼过滤的激光雷达定位方法,其特征在于,当前帧中的已知反光板的获取方法具体如下:
S4、检测激光雷达在上一帧中是否定位成功;
S5、若检测结果为是,则基于动态匹配来匹配出当前帧中的已知反光板和未知反光板,若检测结果为否,则基于静态匹配来匹配出当前帧中的已知反光板和未知反光板。
3.如权利要求2所述基于卡尔曼过滤的激光雷达定位方法,其特征在于,在步骤S4之前还包括:
S6、检测当前帧中是否提取到至少3个反光板;
S7、若当前帧中存在至少3个反光板则,检测路标地图中是否存在路标,若检测结果为是,则执行步骤S1,若检测结果为否,则以激光雷达中心为路标地图中原点,计算当前帧中所有反光板在路标地图中的坐标。
4.如权利要求1所述基于卡尔曼过滤的激光雷达定位方法,其特征在于,所述静态匹配具体包括如下步骤:
S11、提取路标地图中的m个路标点,计算两两路标点之间的距离,得到全局距离列表D;
S12、提取当前帧中的n个反光板,计算两两反光板之间的距离,得到局部距离列表d;
S13、选择全局距离列表中的第i行Di与局部距离列表中的第j行dj,计算Di行中第k元素与dj行中第l个元素的绝对差值,若绝对差值小于matchW,则将第k个路标点加入到第l个反光板的候选匹配中,将Di行中的每一个元素与dj行中的每一个元素的绝对差值与matchW进行比较,得到Di与dj的匹配表matchListi,j;
S14、查找匹配表matchListi,j中的最佳组合,输出最佳匹配组合的匹配数目macth_count和匹配误差macth_err,若macth_count≥3且macth_err小于当前全局最优匹配的误差best_err,用当前匹配组合替换全局最优的匹配组合,令best_err=macth_err;
S15、检测当前j的取值是否等于n,若j≠n,则令j=j+1,并执行步骤S13,若j=n,则检测当前i的取值是否为m,若i≠m,则令i=i+1,j=1,执行步骤S13,若i=m,则输出全局最优匹配,若全局最优匹配的best_err<matchW,则反光板匹配成功,返回全局最优匹配组合,若best_err≥matchW,则输出匹配失败。
5.如权利要求1所述基于卡尔曼过滤的激光雷达定位方法,其特征在于,所述动态匹配具体包括如下步骤:
S21、计算路标地图中所有路标点距激光雷达中心的距离,提取距离大于最小探测距离distmin,且小于最大探测距离distmax的路标点,共z个;
S22、基于激光雷达在上一帧中的位姿,计算n个反光板在路标地图中的坐标;
S23、计算反光板rk与路标点Rl的距离若将路标点Rl加入到反光板rk的匹配列表中;
S24、检测当前l的取值是否等于z,若l≠z,则令l=l+1,执行步骤S23,若l=z,则检测当前k的取值是否为n,若k≠n,则令k=k+1,l=1,执行步骤S23,若k=n,则输出匹配表matchList;
S25、查找匹配表matchList中的最优匹配组合,最优匹配组合的匹配数目大于2,且匹配组合的macth_err最小,最优匹配组合的匹配误差小于matchW,则认为反光板匹配成功,返回最优匹配组合,否则,匹配失败。
6.如权利要求4或5所述基于卡尔曼过滤的激光雷达定位方法,其特征在于,匹配表中的最优匹配组合的获取方法包括如下步骤:
S31、按照设定的顺序从每个反光板的可能匹配路标点中选择一个,得到一个匹配组合
S32、删除匹配组合中错误匹配,获得匹配组合计算的匹配误差matchErr,若matchErr小于当前最佳匹配组合的匹配误差minErr,则将所述匹配组合设为最佳匹配,令minErr=matchErr,返回步骤S31,直至遍历匹配表中所有的匹配组合,输出最佳匹配、最佳匹配的匹配数目及匹配误差。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811618459.2A CN109613549B (zh) | 2018-12-28 | 2018-12-28 | 一种基于卡尔曼过滤的激光雷达定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811618459.2A CN109613549B (zh) | 2018-12-28 | 2018-12-28 | 一种基于卡尔曼过滤的激光雷达定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109613549A true CN109613549A (zh) | 2019-04-12 |
CN109613549B CN109613549B (zh) | 2023-04-07 |
Family
ID=66011843
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201811618459.2A Active CN109613549B (zh) | 2018-12-28 | 2018-12-28 | 一种基于卡尔曼过滤的激光雷达定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109613549B (zh) |
Cited By (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110082775A (zh) * | 2019-05-23 | 2019-08-02 | 北京主线科技有限公司 | 基于激光装置的车辆定位方法和系统 |
CN110471072A (zh) * | 2019-08-19 | 2019-11-19 | 华晟(青岛)智能装备科技有限公司 | 一种反光柱位置辨识方法及系统 |
CN111273304A (zh) * | 2019-12-31 | 2020-06-12 | 芜湖哈特机器人产业技术研究院有限公司 | 一种融合反光柱的自然定位方法及系统 |
CN111337877A (zh) * | 2020-03-19 | 2020-06-26 | 北京北特圣迪科技发展有限公司 | 一种反光板匹配定位方法 |
CN111679261A (zh) * | 2020-08-05 | 2020-09-18 | 湖北工业大学 | 一种基于反光板的激光雷达定位方法及系统 |
CN111856447A (zh) * | 2020-07-28 | 2020-10-30 | 三一机器人科技有限公司 | 一种雷达定位方法、装置及存储介质 |
CN113759928A (zh) * | 2021-09-18 | 2021-12-07 | 东北大学 | 用于复杂大尺度室内场景的移动机器人高精度定位方法 |
CN115220012A (zh) * | 2022-09-20 | 2022-10-21 | 成都睿芯行科技有限公司 | 一种基于反光板定位方法 |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2013071190A1 (en) * | 2011-11-11 | 2013-05-16 | Evolution Robotics, Inc. | Scaling vector field slam to large environments |
CN105953798A (zh) * | 2016-04-19 | 2016-09-21 | 深圳市神州云海智能科技有限公司 | 移动机器人的位姿确定方法和设备 |
CN106123890A (zh) * | 2016-06-14 | 2016-11-16 | 中国科学院合肥物质科学研究院 | 一种多传感器数据融合的机器人定位方法 |
CN107450577A (zh) * | 2017-07-25 | 2017-12-08 | 天津大学 | 基于多传感器的无人机智能感知系统和方法 |
CN108180917A (zh) * | 2017-12-31 | 2018-06-19 | 芜湖哈特机器人产业技术研究院有限公司 | 一种基于位姿图优化的顶标地图构建方法 |
CN108195377A (zh) * | 2017-12-22 | 2018-06-22 | 广东嘉腾机器人自动化有限公司 | 一种基于三角形周长匹配的反光板匹配算法 |
CN108362288A (zh) * | 2018-02-08 | 2018-08-03 | 北方工业大学 | 一种基于无迹卡尔曼滤波的偏振光slam方法 |
-
2018
- 2018-12-28 CN CN201811618459.2A patent/CN109613549B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2013071190A1 (en) * | 2011-11-11 | 2013-05-16 | Evolution Robotics, Inc. | Scaling vector field slam to large environments |
CN105953798A (zh) * | 2016-04-19 | 2016-09-21 | 深圳市神州云海智能科技有限公司 | 移动机器人的位姿确定方法和设备 |
CN106123890A (zh) * | 2016-06-14 | 2016-11-16 | 中国科学院合肥物质科学研究院 | 一种多传感器数据融合的机器人定位方法 |
CN107450577A (zh) * | 2017-07-25 | 2017-12-08 | 天津大学 | 基于多传感器的无人机智能感知系统和方法 |
CN108195377A (zh) * | 2017-12-22 | 2018-06-22 | 广东嘉腾机器人自动化有限公司 | 一种基于三角形周长匹配的反光板匹配算法 |
CN108180917A (zh) * | 2017-12-31 | 2018-06-19 | 芜湖哈特机器人产业技术研究院有限公司 | 一种基于位姿图优化的顶标地图构建方法 |
CN108362288A (zh) * | 2018-02-08 | 2018-08-03 | 北方工业大学 | 一种基于无迹卡尔曼滤波的偏振光slam方法 |
Non-Patent Citations (2)
Title |
---|
强敏利等: "IEKF滤波在移动机器人定位中的应用", 《电子技术应用》 * |
柳俊城等: "基于激光信息的移动机器人SLAM研究", 《自动化与仪表》 * |
Cited By (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110082775A (zh) * | 2019-05-23 | 2019-08-02 | 北京主线科技有限公司 | 基于激光装置的车辆定位方法和系统 |
CN110082775B (zh) * | 2019-05-23 | 2021-11-30 | 北京主线科技有限公司 | 基于激光装置的车辆定位方法和系统 |
CN110471072A (zh) * | 2019-08-19 | 2019-11-19 | 华晟(青岛)智能装备科技有限公司 | 一种反光柱位置辨识方法及系统 |
CN111273304A (zh) * | 2019-12-31 | 2020-06-12 | 芜湖哈特机器人产业技术研究院有限公司 | 一种融合反光柱的自然定位方法及系统 |
CN111273304B (zh) * | 2019-12-31 | 2022-05-24 | 芜湖哈特机器人产业技术研究院有限公司 | 一种融合反光柱的自然定位方法及系统 |
CN111337877A (zh) * | 2020-03-19 | 2020-06-26 | 北京北特圣迪科技发展有限公司 | 一种反光板匹配定位方法 |
CN111856447A (zh) * | 2020-07-28 | 2020-10-30 | 三一机器人科技有限公司 | 一种雷达定位方法、装置及存储介质 |
CN111679261A (zh) * | 2020-08-05 | 2020-09-18 | 湖北工业大学 | 一种基于反光板的激光雷达定位方法及系统 |
CN111679261B (zh) * | 2020-08-05 | 2021-04-27 | 湖北工业大学 | 一种基于反光板的激光雷达定位方法及系统 |
CN113759928A (zh) * | 2021-09-18 | 2021-12-07 | 东北大学 | 用于复杂大尺度室内场景的移动机器人高精度定位方法 |
CN115220012A (zh) * | 2022-09-20 | 2022-10-21 | 成都睿芯行科技有限公司 | 一种基于反光板定位方法 |
Also Published As
Publication number | Publication date |
---|---|
CN109613549B (zh) | 2023-04-07 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109613549A (zh) | 一种基于卡尔曼过滤的激光雷达定位方法 | |
CN109613550A (zh) | 一种基于反光板的激光雷达地图构建及定位方法 | |
CN110031817A (zh) | 一种激光雷达反光板的快速匹配方法 | |
CN109613547A (zh) | 一种基于反光板的占据栅格地图构建方法 | |
CN109631919B (zh) | 一种融合反光板和占据栅格的混合导航地图构建方法 | |
CN109613548A (zh) | 一种基于图优化的激光雷达路标地图构建方法 | |
Olson et al. | Robust range-only beacon localization | |
US20240036203A1 (en) | Methods and systems for geo-referencing mapping systems | |
CN110412635B (zh) | 一种环境信标支持下的gnss/sins/视觉紧组合方法 | |
CN105760811B (zh) | 全局地图闭环匹配方法及装置 | |
Wilbers et al. | Localization with sliding window factor graphs on third-party maps for automated driving | |
CN112268559B (zh) | 复杂环境下融合slam技术的移动测量方法 | |
CN110187375A (zh) | 一种基于slam定位结果提高定位精度的方法及装置 | |
CN110187372A (zh) | 一种低速无人车园区内组合导航方法及系统 | |
CN109382840A (zh) | 作业机器人定位方法及系统 | |
CN109613571B (zh) | 一种基于双极化天线的城市峡谷内卫星定位方法 | |
CN116358547B (zh) | 一种基于光流估计获取agv位置的方法 | |
Jia et al. | An indoor and outdoor seamless positioning system based on android platform | |
Ćwian et al. | GNSS-augmented lidar slam for accurate vehicle localization in large scale urban environments | |
US11812342B2 (en) | Cellular-based navigation method | |
CN114998540A (zh) | 一种智慧城市传感器探测主动同步定位与建图方法 | |
CN108171316A (zh) | 一种面向不动产测量的改进型惯性定位方法 | |
Ma et al. | Dimension-Expanded-Based Matching Method With Siamese Convolutional Neural Networks for Gravity-Aided Navigation | |
Lee et al. | Fusion of monocular vision and radio-based ranging for global scale estimation and drift mitigation | |
Iannucci et al. | Cross-Modal Localization: Using automotive radar for absolute geolocation within a map produced with visible-light imagery |
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 |