一种融合反光板和占据栅格的混合导航地图构建方法与流程

本发明属于自动控制技术领域，提供了一种融合反光板及占据栅格的混合导航地图构建方法。

背景技术：

随着社会的发展和技术的进步，移动机器人越来越深的介入到人类的日常生活中，例如家庭中的清洁机器人、工厂中的搬运机器人以及餐馆中的送餐机器人等。移动机器人想要实现上述功能必须准确的知道自身所在的位置，即实时定位。而移动机器人实时定位的先决条件是建立地图，这是机器人导航和其它智能行为的关键。激光雷达传感器因为测量精度高、不受光照条件影响等优点，被广泛的应用于移动机器人定位导航中。基于激光雷达的定位导航方法可以分为基于反光标识的定位导航方法和基于轮廓的定位导航方法，其中基于反光标识的激光雷达定位导航方法有着精度高、稳定性好等优点，但是有些场景可能会不方便布设反光标识，此外大量布设反光标识会增加成本。

技术实现要素：

本发明实施例提供一种融合反光板和占据栅格的混合导航地图构建方法，在不方便布置反光板或者是无需精准定位的区域，通过栅格地图来进行导航。

为了实现上述目的，本发明提供了一种融合反光板及占据栅格地图的混合导航地图构建方法，该方法包括如下步骤：

s1、检测当前帧中是否存至少3个反光板，若检测结果为是，则执行步骤s2，若检测结果为否，则执行步骤s3；

s2、基于反光板计算激光雷达在路标地图中的位姿，并基于激光雷达在路标地图中的位姿来更新路标地图及栅格地图；

s3、基于扫描数据与栅格地图的匹配来计算激光雷达在栅格地图中的位姿，并基于激光雷达在栅格地图中的位姿来更新栅格地图。

进一步的，路标地图的构建方法具体包括如下步骤：

s21、检测激光雷达在上一帧中是否定位成功；

s22、若检测结果为是，则基于动态匹配来提取当前帧中的已知反光板和未知反光板，若检测结果为否，则基于静态匹配来提取当前帧中的已知反光板和未知反光板；

s23、基于3个已知反光板来计算激光雷达当前在路标地图中的位姿，并保存雷达坐标系与路标坐标系之间的旋转矩阵r、平移向量t；

s24、基于雷达坐标系与路标坐标系之间的旋转矩阵r、平移向量t来计算未知反光板在路标地图中的坐标，并更新路标地图。

进一步的，步骤s2中的栅格地图更新方法具体如下：

s25、基于激光雷达当前帧在路标地图中的位姿，计算光束起点及终点在栅格地图中的坐标；

s26、对光束终点对所在的栅格进行占据更新，对从到所经过的栅格进行自由更新；

s27、计算占据更新栅格及自由更新栅格的栅格占据概率，并更新占据栅格地图。

进一步的，激光雷达在栅格地图中的定位方法具体包括如下步骤：

s31、检测激光雷达在上一帧中是否定位成功；

s32、若检测结果为是，则将扫描数据与栅格地图进行匹配，来计算激光雷达当前帧在栅格地图中的坐标。

进一步的，步骤s3中的栅格地图更新方法具体如下：

s33、基于激光雷达当前帧在栅格地图中的位姿，计算光束起点及终点在栅格地图中的坐标；

s34、对光束终点对所在的栅格进行占据更新，对从到所经过的栅格进行自由更新；

s35、计算占据更新栅格及自由更新栅格的栅格占据概率，并更新占据栅格地图。

本发明提供的一种融合反光板及占据栅格的混合导航地图构建方法具有如下有益技术效果：在定位精度要求不高或者是不便于布置反光板的区域，通过栅格地图进行定位，仅在需要精准定位的区域分布反光板，可以减少反光板的布置数量，减少成本，且普适应增强。

附图说明

图1为本发明实施例提供的融合反光板和占据栅格的混合导航地图构建方法流程图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

在本发明在无需精确定位或者是不便于布置反光板的区域使用栅格地图进行导航，在需要精确定位的区域使用反光板进行导航，将激光雷达水平放置，反光板布置于环境中需要精确定位的区域，且垂直于地面张贴，激光雷达的扫描线与反光板的中点大致对齐。

图1为本发明实施例提供的融合反光板和占据栅格的混合导航地图构建方法流程图，该方法包括如下步骤：

s1、提取当前帧中的反光板，并检测当前帧中是否存在至少3个已知反光板，若检测结果为是，则执行步骤s2，若检测结果为否，则执行步骤s3；

根据激光雷达探测距离与反光板反射强度之间的关系，筛选出反光板上的扫描点，利用单个反光板上的反射点集拟合得到反光板中心，检测反光板中线到激光雷达中心的距离及角度，计算反光板中心在激光雷达坐标系中的坐标。

s2、基于反光板计算激光雷达在路标地图中的位姿，并基于激光雷达在路标地图中的位姿来更新路标地图及栅格地图；

s3、基于扫描数据与栅格地图的匹配来计算激光雷达在栅格地图中的位姿，并基于激光雷达在栅格地图中的位姿来更新栅格地图。

在本发明实施例中，在路标地图构建方法具体包括如下步骤：

s21、检测路标地图中是否存在路标，若检测结果为否，则以激光雷达中心为路标地图的原点，计算当前帧中所有反光板在路标地图中的坐标，若检测结果为是，则执行步骤s12；

s22、检测激光雷达在上一帧中是否定位成功，若检测结果为是，则基于动态匹配来匹配出当前帧中的已知反光板和未知反光板，若检测结果为否，则基于静态匹配来匹配出当前帧中的已知反光板和未知反光板；

反光板即为路标地图中的路标，已知反光板是指已经纳入路标地图的反光板，未知反光板是指未纳入路标地图的反光板。

s23、检测当前帧是否存在至少3个已知反光板，若检测结果为是，则基于3个已知反光板来计算激光雷达在路标地图中的位姿，并保存雷达坐标系与路标坐标系之间的旋转矩阵r、平移向量t，若检测结果为否，则控制激光雷达重新扫描，执行步骤s1；

已知反光板在雷达坐标系中的局部坐标xloc、在反光板地图坐标系中的坐标x，从反光板的局部坐标到反光板地图坐标之间转换满足刚体变换，因此根据公式

x＝rxloc+t

可以计算出雷达坐标系与地图坐标系之间的旋转矩阵r、平移向量t，因为激光雷达的局部坐标为则激光雷达在地图中的坐标x^scan＝t，激光雷达相对于地图坐标系的旋转角θ可由旋转矩阵r分解得到。

s24、基于雷达坐标系与路标坐标系之间的旋转矩阵r、平移向量t来计算未知反光板在路标地图中的位置，并更新路标地图；

未知反光板在激光雷达坐标系中的坐标为x，则未知反光板在路标地图坐标系中的坐标为x＝rx+t，为了提高反光板位置计算的精确性，可以取50次计算的坐标平均值作为最终的反光板坐标。

在本发明实施例中，已知栅格地图尺寸为w*h，单个栅格的边长所代表的实际尺寸为l，设置栅格地图的原点为基于反光板的栅格地图更新方法具体如下：

s25、基于激光雷当前帧在路标地图中的位姿，计算光束起点及终点在栅格地图中的坐标；

已知激光雷达在路标地图中的位姿p(x,y,θ)，则激光雷达在栅格地图中的位置为激光雷达坐标系中的扫描点到栅格地图坐标系的转换矩阵为m，m的表达式具体如下：

其中p^m即是激光雷达的光束起点，已知第i个激光扫描点在激光雷达坐标系中的齐次坐标为pi(xi,yi,1)，则第i个光束的终点在栅格地图中的坐标为

s26、对光束终点对所在的栅格进行占据更新，对从到所经过的栅格进行自由更新；

已知一个光束的起点和终点k是光束经过的栅格的数量，可以利用优势对数积分法对光束经过栅格的概率进行更新。对所在的栅格进行占据更新，对从到所经过的栅格进行自由更新。设置自由更新的值oddsfree＝0.4，占据更新的值oddsocc＝0.6。

s27、计算占据更新栅格及自由更栅格的栅格占据概率，并更新栅格地图。

假定更新前的栅格占据概率为p，则更新前栅格的odds值为odds＝log(p/(1-p))，若当前栅格是进行自由更新，则令odds＝odds+oddsfree，若当前栅格是进行占据更新，令odds＝odds+oddsocc，更新后，栅格的占据概率为p＝exp(odds)/(exp(odds)+1)。

在本发明实施例中，当前帧中提取到的反光板数量少于3个，则基于栅格地图进行定位，其定位方法具体包括如下步骤：

s31、检测激光雷达在上一帧中是否定位成功；

s32、若检测结果为是，则将扫描数据与栅格地图进行匹配，来计算激光雷达当前帧在栅格地图中的坐标，若检测结果为否，则控制激光雷达重新扫描，执行步骤s1，激光雷达在栅格地图中的定位计算具体如下：

根据激光雷达上一时刻的位姿，将每一个扫描点映射到栅格地图上，基于所有的扫描点在栅格地图的，可以构造如下的目标函数：

si(ξ)表示将t时刻第i个激光点变换到栅格地图坐标系下，m(si(ξ))表示第i个激光点在栅格地图上的位置被占据的概率，这样就把位姿计算变换成最小二乘求解问题；利用双线性差值计算m(si(ξ))，并对m(si(ξ))求偏导，将位姿计算的最小二乘求解转换为迭代的位姿增量计算的问题，基于位姿增量来确定激光雷达当前帧在栅格地图中的定位。

在本发明实施例中，基于栅格定位的栅格地图更新方法，具体如下：

s33、基于激光雷当前帧在栅格地图中的位姿，计算光束起点及终点在栅格地图中的坐标；

已知激光雷达在上一帧中的位姿p′(x,y,θ)，则激光雷达在栅格地图中的位置为激光雷达坐标系中的扫描点到栅格地图坐标系的转换矩阵为m，m的表达式具体如下：

其中p^m即是激光雷达的光束起点，已知第i个激光扫描点在激光雷达坐标系中的齐次坐标为pi(xi,yi,1)，则第i个光束的终点在栅格地图中的坐标为

s34、对光束终点对所在的栅格进行占据更新，对从到所经过的栅格进行自由更新；

已知一个光束的起点和终点k是光束经过的栅格的数量，可以利用优势对数积分法对光束经过栅格的概率进行更新。对所在的栅格进行占据更新，对从到所经过的栅格进行自由更新。设置自由更新的值oddsfree＝0.4，占据更新的值oddsocc＝0.6。

s35、计算占据更新栅格及自由更栅格的栅格占据概率，并更新栅格地图。

假定更新前的栅格占据概率为p，则更新前栅格的odds值为odds＝log(p/(1-p))，若当前栅格是进行自由更新，则令odds＝odds+oddsfree，若当前栅格是进行占据更新，令odds＝odds+oddsocc，更新后，栅格的占据概率为p＝exp(odds)/(exp(odds)+1)。

本发明提供的一种融合反光板及占据栅格的混合导航地图构建方法具有如下有益技术效果：在定位精度要求不高或者是不便于布置反光板的区域，通过栅格地图进行定位，仅在需要精准定位的区域分布反光板，可以减少反光板的布置数量，减少成本，且普适应增强。

在本发明涉及的静态匹配及动态匹配可以采用现有的方法，或者是基于如下如下方法来实现静态匹配或动态匹配。

在本发明实施例中，设置反光板匹配允许的误差为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，则输出匹配表matchlistr1:ri,....；....；rn:rj,...；

s25、查找匹配表matchlist中的最优匹配组合，若最优匹配组合的匹配数目大于2，且macth_err最小，若最优匹配组合的匹配误差小于matchw，则认为反光板匹配成功，返回最优匹配组合，否则，匹配失败。

在本发明实施例中，静态匹配及动态匹配中匹配表中的最佳匹配查找方法具体如下：

s31、按照设定的顺序从每个反光板的可能匹配路标点中选择一个，得到一个匹配组合

已知当前扫描到n个反光板，每个反光板所有的可能匹配为r1:ri,....；....；rn:rj,...，设置当前最佳匹配的误差为minerr。

s32、删除匹配组合中错误匹配，获得匹配组合计算的匹配误差matcherr，若matcherr小于当前最佳匹配组合的匹配误差minerr，则将所述匹配组合设为当前最佳匹配，令minerr＝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之间至少有一个匹配错误，令匹配组合的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的匹配路标点设为空。

该反光板的匹配方法具有如下有益效果：分层进行匹配，提高了匹配的速度，提高匹配的可靠性。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。