CN109613549B - 一种基于卡尔曼过滤的激光雷达定位方法 - Google Patents

Abstract

本发明适用于自动控制技术领域，提供了一种基于卡尔曼过滤的激光雷达定位方法，该方法包括如下步骤：S1、基于当前帧中的3个已知反光来计算激光雷达当前在路标地图中的位姿P(x_p,y_p,θ_p)；S2、基于卡尔曼过滤对激光雷达的当前位姿进行优化，得到当前时刻的最优位姿P′(x_p,y_p,θ_p)；S3、检测当前帧中是否存在未知反光板，若检测结果为是，则基于激光雷达当前的最优位姿P′(x_p,y_p,θ_p)来计算未知反光板在路标地图中的坐标，并更新路标地图。对计算的激光雷达位姿进行卡尔曼过滤，获取更为精准的激光雷达位姿，提高定位精度。

Description

一种基于卡尔曼过滤的激光雷达定位方法

技术领域

本发明属于自动控制技术领域，提供了一种基于卡尔曼过滤的激光雷达定位方法。

背景技术

随着社会的发展和技术的进步，移动机器人越来越深的介入到人类的日常生活中，例如家庭中的清洁机器人、工厂中的搬运机器人以及餐馆中的送餐机器人等。移动机器人想要实现上述功能必须准确的知道自身所在的位置，即实时定位。而移动机器人实现实时定位的先决条件是建立地图，这是机器人导航和其它智能行为的关键。移动机器人定位导航常用的传感器有磁导航、摄像机和激光雷达等。其中磁导航的导航路径单一，不利于扩展和更改，使用的局限性较大。基于视觉的定位导航方法受光源条件影响大，稳定性和精度较差，无法让移动机器人进行稳定、精密的操作。基于激光雷达的定位方法可以分为基于反光板的定位导航方法和基于轮廓的定位导航方法，其中基于轮廓的定位导航方法计算量大、精度较差，无法适应移动机器人精密操作的要求；基于反光板的激光雷达定位导航方法精度高、稳定性好。现有的基于反光板的激光雷达定位方法分为三边法和三角法，三边法在定位时将反光板的角度信息和距离信息分开计算，导致定位的精度降低；三角法定位计算复杂，而且定位精度对反光板的分布要求较高。

发明内容

本发明实施例提供一种基于卡尔曼过滤的激光雷达定位方法，通过卡尔曼过滤对激光雷达位姿进行优化，获取更为精准的激光雷达位姿，即提高了定位精度。

为了实现上述目的，本发明提供了一种基于卡尔曼过滤的激光雷达定位方法，该方法包括如下步骤：

S1、基于当前帧中的3个已知反光来计算激光雷达当前在路标地图中的位姿P(x_p,y_p,θ_p)；

S2、基于卡尔曼过滤对激光雷达的当前位姿进行优化，得到当前时刻的最优位姿P′(x_p,y_p,θ_p)；

S3、检测当前帧中是否存在未知反光板，若检测结果为是，则基于激光雷达当前的最优位姿P′(x_p,y_p,θ_p)来计算未知反光板在路标地图中的坐标，并更新路标地图。

进一步的，当前帧中的已知反光板的获取方法具体如下：

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

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

进一步的，在步骤S4之前还包括：

S6、检测当前帧中是否提取到至少3个反光板；

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

进一步的，所述静态匹配具体包括如下步骤：

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

进一步的，匹配表中的最优匹配组合的获取方法包括如下步骤：

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(x_p,y_p,θ_p)，其计算过程具体如下:

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

X_map＝RX_loc+t

计算出雷达坐标系与地图坐标系之间的旋转矩阵R、平移向量t。因为激光雷达的局部坐标为

则激光雷达在地图中的坐标

激光雷达相对于地图坐标系的旋转角θ可由旋转矩阵R分解得到；

S5、基于卡尔曼过过滤对激光雷达当前的位姿P(x_p,y_p,θ_p)进行优化，获取激光雷达当前时刻的最优位姿P′(x_p,y_p,θ_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来计算未知反光板在路标地图中的坐标，并更新路标地图。

计算公式具体如下：

其中，(x_loc，y_loc)为未知反光板在激光雷达坐标系中的坐标。

本发明提供的基于卡尔曼过滤的激光雷达定位方法具有如下有益效果：

1.对计算的激光雷达位姿进行卡尔曼过滤，获取更为精准的激光雷达位姿，提高定位精度的时候，也提高了路标地图的精准度，尤其至针对激光雷达扫描频率过大，即运动幅度过大的情况，可以极大的提高定位的精准度；

2.采用坐标转换法进行定位，提高了定位的精度。

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

在本发明实施例中，设置反光板匹配允许的误差为matchW，定义全局最优匹配的误差best_err等于无穷大，本发明中的静态匹配具体包括如下步骤：

S11、根据路标地图中m个路标点R₁(X₁,Y₁),...,R_m(X_m,Y_m)，计算两两路标点之间的距离，得到全局距离列表D_1,2,...,D_1,m；...；D_m,1,...,D_m,m-1；

其中，D_m,m-1表示路标点R_m与路标点R_m-1之间的距离，(X_m,Y_m)为第m个路标点R_m在路标地图中的坐标；

S12、根据当前帧中的n个反光板r₁(x₁,y₁),...,r_n(x_n,y_n)，计算两两反光板之间的距离，得到局部距离列表d_1,2,....,d_1,n；....；d_n,1,...,d_n,n-1；

d_n,n-1表示反光板r_n与反光板r_n-1之间的距离，(x_n,y_n)为第n个反光板r_n在激光雷达坐标系中的坐标，根据激光雷达探测距离与反光板反射强度之间的关系筛选出反光板上的扫描点，利用单个反光板上的反射点集拟合得到反光板中心，检测反光板中心距激光雷达中心的距离d_i及角度θ_i，用r_i(d_i,θ_i)来表示反光板r_i的中心距激光雷达中心的距离d_i及夹角θ_i，基于r_i(d_i,θ_i)计算反光板中心在激光雷达坐标系中的坐标。

S13、选择全局距离列表中的第i行D_i与局部距离列表中的第j行d_j，假设路标地图中第i个路标点与当前帧中的第j个反光板匹配成功，将D_i行中的每一个元素与d_j行中的每一个元素进行对比，若D_i行中的第k个元素，与d_j行中的第l个元素满足abs(D_i,k-d_j,l)＜matchW，则将第k个路标点加入到第l个反光板的候选匹配中，对比结束得到D_i与d_j的匹配表matchList_i,j＝{r₁:R_i，...；...；r_n:R_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，则输出匹配失败。

在本发明实施例中，反光板的动态匹配具体包括如下步骤：

在激光雷达在上一帧中的位姿获取成功，在上一帧中的位姿为pose(x,y,θ)，基于动态匹配来寻找反光板对应的路标点，动态匹配的步骤如下：

S21、计算路标地图中所有路标点R₁(X₁,Y₁),....,R_m(X_m,Y_m)距激光雷达中心的距离，提取距离大于最小探测距离dist_min，且小于最大探测距离dist_max的路标点R₁(X₁,Y₁),....,R_z(X_z,Y_z)

S22、基于激光雷达在上一帧中的位姿pose(x,y,θ)，计算n个反光板r₁(x₁,y₁),...,r_n(x_n,y_n)在路标地图中的坐标P(x,y)，

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 r₁:R_i,....；....；r_n:R_j,...；

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

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

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

已知当前扫描到n个反光板，每个反光板所有的可能匹配为r₁:R_i,....；....；r_n:R_j,...，设置当前最佳匹配的误差为min Err。

S32、删除匹配组合

中错误匹配，获得匹配组合

计算

的匹配误差matchErr，若matchErr小于当前最佳匹配组合的匹配误差min Err，则将所述匹配组合

设为当前最佳匹配，令min Err＝matchErr，返回步骤S31，直至遍历匹配表中所有的匹配组合，输出最佳匹配、最佳匹配的匹配数目及匹配误差。

在本发明实施例中，错误匹配的删除方法具体包括如下步骤：

S321、从匹配组合

中顺序提取n个反光板及其对应的路标点；

S322、计算第i个反光板r_i与剩余反光板r_j(j≠i)间距离

以及路标点R_i与剩余路标点R_j(j≠i)间的距离

路标点R_i为反光板r_i的可能匹配路标点，路标点R_j为反光板r_i的可能匹配路标点；

S323、计算

与

之差deta_dist，统计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(m≠i,j)，剩余反光板中不包括匹配路标点为空的反光板；

S328、计算反光板r_i与剩余反光板r_m之间的距离

及路标点R_i与剩余路标点R_m间的距离

计算距离差

将距离差进行累积

计算反光板r_j与剩余反光板r_m之间的距离

计算路标点R_j与剩余路标点R_m间的距离

计算距离差

将距离差进行累积

其中，路标点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、在匹配组合

中提取匹配路标点不为空的反光板r₁,...,r_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、反光板r_i与反光板r_j之间至少有一个匹配错误，令匹配组合macthlist_i2的macth_count减1，若macth_count＜3，则返回步骤S321，若macth_count≥3，则执行步骤S334；

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

S335、计算反光板r_i与剩余反光板r_m间的距离

及路标点R_i与剩余路标点R_m间的距离

计算距离差

将距离差进行累积

计算反光板r_j与剩余反光板r_m间的距离

及路标点R_j与剩余路标点R_m间的距离

计算距离差

将距离差进行累积

其中，路标点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的匹配路标点设为空。

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

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

Claims (6)

1.一种基于卡尔曼过滤的激光雷达定位方法，其特征在于，所述方法包括如下步骤：

S1、基于当前帧中的3个已知反光板来计算激光雷达当前在路标地图中的位姿P(x_p,y_p,θ_p)；

S2、基于卡尔曼过滤对激光雷达的当前位姿进行优化，得到当前时刻的最优位姿P′(x_p,y_p,θ_p)；

S3、检测当前帧中是否存在未知反光板，若检测结果为是，则基于激光雷达当前的最优位姿P′(x_p,y_p,θ_p)来计算未知反光板在路标地图中的坐标，并更新路标地图。

2.如权利要求1所述基于卡尔曼过滤的激光雷达定位方法，其特征在于，当前帧中的已知反光板的获取方法具体如下：

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

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

3.如权利要求2所述基于卡尔曼过滤的激光雷达定位方法，其特征在于，在步骤S4之前还包括：

S6、检测当前帧中是否提取到至少3个反光板；

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

4.如权利要求2所述基于卡尔曼过滤的激光雷达定位方法，其特征在于，所述静态匹配具体包括如下步骤：

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，则输出匹配失败。

5.如权利要求2所述基于卡尔曼过滤的激光雷达定位方法，其特征在于，所述动态匹配具体包括如下步骤：

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

6.如权利要求4或5所述基于卡尔曼过滤的激光雷达定位方法，其特征在于，匹配表中的最优匹配组合的获取方法包括如下步骤：

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

S32、删除匹配组合

中错误匹配，获得匹配组合

计算

的匹配误差matchErr，若matchErr小于当前最佳匹配组合的匹配误差minErr，则将所述匹配组合

设为最佳匹配，令minErr＝matchErr，返回步骤S31，直至遍历匹配表中所有的匹配组合，输出最佳匹配、最佳匹配的匹配数目及匹配误差。

Images