[发明专利]一种激光雷达反光板的快速匹配方法

权利要求书

1.一种激光雷达反光板的快速匹配方法，其特征在于，所述方法包括如下步骤：

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

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

所述静态匹配具体包括如下步骤：

S11、提取路标地图中的m个路标点，计算两两路标点之间的距离，得到全局距离列表D；

S12、提取当前帧中的n个反光板，计算两两反光板之间的距离，得到局部距离列表d；

S13、选择全局距离列表中的第i行D_i与局部距离列表中的第j行d_j，计算D_i行中第k元素与d_j行中第l个元素的绝对差值，若绝对差值小于matchW，则将第k个路标点加入到第l个反光板的候选匹配中，将D_i行中的每一个元素与d_j行中的每一个元素的绝对差值与matchW进行比较，得到D_i与d_j的匹配表matchList_i,j；

S14、查找匹配表matchList_i,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、计算路标地图中所有路标点距激光雷达中心的距离，提取距离大于最小探测距离dist_min，且小于最大探测距离dist_max的路标点，共z个；

S22、基于激光雷达在上一帧中的位姿，计算n个反光板在路标地图中的坐标；

S23、计算反光板r_k与路标点R_l的距离若将路标点R_l加入到反光板r_k的匹配列表中；

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，则认为反光板匹配成功，返回最优匹配组合，否则，匹配失败。

2.如权利要求1所述激光雷达反光板的快速匹配方法，其特征在于，匹配表中的最优匹配组合的获取方法包括如下步骤：

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

S32、删除匹配组合中错误匹配，获得匹配组合计算的匹配误差matchErr，若matchErr小于当前最佳匹配组合的匹配误差minErr，则将所述匹配组合设为最佳匹配，令minErr＝matchErr，返回步骤S31，直至遍历匹配表中所有的匹配组合，输出最佳匹配、最佳匹配的匹配数目及匹配误差。

3.如权利要求2所述激光雷达反光板的快速匹配方法，其特征在于，所述匹配组合中的错误匹配删除方法包括如下步骤：

S321、从匹配组合中顺序提取n个反光板及其对应的路标点；

S322、计算第i个反光板r_i与剩余反光板r_j间距离以及路标点R_i与剩余路标点R_j间的距离路标点R_i为反光板r_i的可能匹配路标点，路标点R_j为反光板r_i的可能匹配路标点；

S323、计算与之差统计deta_dist小于matchW的个数macth_count_i，若macth_count_i≥3，则匹配组合的匹配数目macth_count加1，否则，将反光板r_i的匹配路标点设为空；

S324、检测i是否等于n，若i≠n，则令i＝i+1，并执行步骤S322，若i＝n，则输出匹配组合及匹配组合的数目macth_count，若macth_count＜3，则返回步骤S321，若macth_count≥3，执行步骤S325；

S325、将匹配组合中的反光板r_i与反光板r_j进行比较，检测反光板r_i与反光板r_j是否匹配到同一路标点；

S326、若反光板r_i与反光板r_j未匹配到同一路标点，则检测j是否等于n，若j≠n，令j＝j+1，执行步骤S325，若j＝n，则检测i是否等于n，若i≠n，令i＝i+1，j＝i+1，执行步骤S325，若i＝n，则输出匹配组合若反光板r_i与反光板r_j匹配到同一路标点，则匹配组合的macth_count减去1，检测macth_count是否小于3，若macth_count＜3，则返回步骤S321，若macth_count≥3，则执行步骤S327；

S327、提取反光板r_i、r_j、及两反光板r_i、r_j的剩余反光板r_m，剩余反光板中不包括匹配路标点为空的反光板；

S328、计算反光板r_i与剩余反光板r_m之间的距离及路标点R_i与剩余路标点R_m间的距离计算距离差将距离差进行累积，获得累积距离差sum_i；计算反光板r_j与剩余反光板r_m之间的距离计算路标点R_j与剩余路标点R_m间的距离计算距离差将距离差进行累积，获得累积距离差sum_j，其中，路标点R_i为反光板r_i在匹配组合中的匹配路标点，路标点R_m为反光板r_m在该匹配组合中的匹配路标点；

S329、比较sum_i与sum_j，若sum_i＜sum_j，则保留反光板r_i匹配的路标点，将反光板r_j的匹配路标点设为空，若sum_i＞sum_j，保留反光板r_j匹配的路标点，将反光板r_i的匹配路标点设为空；

S330、在匹配组合中提取匹配路标点不为空的s个反光板；

S331、计算反光板r_i与反光板r_j之间的距离及路标点R_i与路标点R_j之间的距离路标点R_i与路标点R_j分别为反光板r_i与反光板r_j在匹配组合中的匹配路标点；

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、将匹配组合的macth_count减1，若macth_count＜3，则返回步骤S321，若macth_count≥3，则执行步骤S334；

S334、提取反光板r_i、r_j、及两反光板r_i、r_j的剩余反光板r_m，且剩余反光板中不包括匹配路标点为空的反光板；

S335、计算反光板r_i与剩余反光板r_m间的距离及路标点R_i与剩余路标点R_m间的距离计算距离差将距离差进行累积，获得累积距离差sum_i，计算反光板r_j与剩余反光板r_m间的距离及路标点R_j与剩余路标点R_m间的距离计算距离差将距离差进行累积，获得累积距离差sum_j，其中，路标点R_i为反光板r_i在匹配组合中的匹配路标点，路标点R_m为反光板r_m在匹配组合中的匹配路标点；

S336、将sum_i与sum_j进行比较，若sum_i＜sum_j，则保留反光板r_i匹配的路标点，将反光板r_j的匹配路标点设为空，sum_i≥sum_j，保留反光板r_j匹配的路标点，将反光板r_i的匹配路标点设为空。