本发明属于自动控制技术领域,提供了一种基于卡尔曼过滤的激光雷达定位方法。
背景技术:
随着社会的发展和技术的进步,移动机器人越来越深的介入到人类的日常生活中,例如家庭中的清洁机器人、工厂中的搬运机器人以及餐馆中的送餐机器人等。移动机器人想要实现上述功能必须准确的知道自身所在的位置,即实时定位。而移动机器人实现实时定位的先决条件是建立地图,这是机器人导航和其它智能行为的关键。移动机器人定位导航常用的传感器有磁导航、摄像机和激光雷达等。其中磁导航的导航路径单一,不利于扩展和更改,使用的局限性较大。基于视觉的定位导航方法受光源条件影响大,稳定性和精度较差,无法让移动机器人进行稳定、精密的操作。基于激光雷达的定位方法可以分为基于反光板的定位导航方法和基于轮廓的定位导航方法,其中基于轮廓的定位导航方法计算量大、精度较差,无法适应移动机器人精密操作的要求;基于反光板的激光雷达定位导航方法精度高、稳定性好。现有的基于反光板的激光雷达定位方法分为三边法和三角法,三边法在定位时将反光板的角度信息和距离信息分开计算,导致定位的精度降低;三角法定位计算复杂,而且定位精度对反光板的分布要求较高。
技术实现要素:
本发明实施例提供一种基于卡尔曼过滤的激光雷达定位方法,通过卡尔曼过滤对激光雷达位姿进行优化,获取更为精准的激光雷达位姿,即提高了定位精度。
为了实现上述目的,本发明提供了一种基于卡尔曼过滤的激光雷达定位方法,该方法包括如下步骤:
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的距离
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、删除匹配组合
本发明提供的基于卡尔曼过滤的激光雷达定位方法具有如下有益效果:
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。因为激光雷达的局部坐标为
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的距离
s24、检测当前l的取值是否等于z,若l≠z,则令l=l+1,执行步骤s23,若l=z,则检测当前k的取值是否为n,若k≠n,则令k=k+1,l=1,执行步骤s23,若k=n,则输出匹配表matchlistr1:ri,....;....;rn:rj,...;
s25、查找匹配表matchlist中的最优匹配组合,若最优匹配组合的匹配数目大于2,且macth_err最小,若最优匹配组合的匹配误差小于matchw,则认为反光板匹配成功,返回最优匹配组合,否则,匹配失败。
在本发明实施例中,静态匹配及动态匹配中匹配表中的最佳匹配查找方法具体如下:
s31、按照设定的顺序从每个反光板的可能匹配路标点中选择一个,得到一个匹配组合
已知当前扫描到n个反光板,每个反光板所有的可能匹配为r1:ri,....;....;rn:rj,...,设置当前最佳匹配的误差为minerr。
s32、删除匹配组合
在本发明实施例中,错误匹配的删除方法具体包括如下步骤:
s321、从匹配组合
s322、计算第i个反光板ri与剩余反光板rj(j≠i)间距离
s323、计算
s324、检测i是否等于n,若i≠n,则令i=i+1,并执行步骤s322,若i=n,则输出匹配组合
s325、将匹配组合
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,则输出匹配组合
s327、提取反光板ri、rj及两反光板ri、rj的剩余反光板rm(m≠i,j),剩余反光板中不包括匹配路标点为空的反光板;
s328、计算反光板ri与剩余反光板rm之间的距离
s329、比较sumi与sumj,若sumi<sumj,则保留反光板ri匹配的路标点,将反光板rj的匹配路标点设为空,若sumi>sumj,保留反光板rj匹配的路标点,将反光板ri的匹配路标点设为空;
s330、在匹配组合
s331、计算反光板ri与反光板rj之间的距离
s332、计算
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间的距离
s336、将sumi与sumj进行比较,若sumi<sumj,则保留反光板ri匹配的路标点,将反光板rj的匹配路标点设为空,sumi≥sumj,保留反光板rj匹配的路标点,将反光板ri的匹配路标点设为空。
该反光板的匹配方法具有如下有益效果:分层进行匹配,提高了匹配的速度,提高匹配的可靠性。
以上所述仅为本发明的较佳实施例而已,并不用以限制本发明,凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等,均应包含在本发明的保护范围之内。