[发明专利]一种基于边角联合特征结构体的激光反射板定位方法

权利要求书

1.一种基于边角联合特征结构体的激光反射板定位方法，其特征在于：所述定位方法包括如下步骤：

步骤1已知参考地图上的反射板数据集Q_m{(x_m0，y_m0)，(x_m1，y_m1)，...，(x_mn，y_mn)}，及对应预生成的反射板特征结构体集合F_m，通过实时扫描激光提取的反射板数据集Q_r{(x_r0，y_r0)，(x_r1，y_r1)，...，(x_rk，y_rk)}，对其中的每个反射板数据，结合数据集中反射板间的距离和相互角度关系，生成反射板特征结构体FeatureMark，进而得到激光扫描反射板数据集对应的反射板特征结构体集合F_r，样例激光数据包含了激光传感器传感的估计位置(x_ini，y_ini，theta_ini)及反射板点集Q_r{(x_r0，y_r0)，(x_r1，y_r1)，(x_r2，y_r2)，(x_r3，y_r3)}，对其中的每个反射板，反射板r₀，利用r₀与同一激光数据中的其他反射板的距离与角度关系生成多个边角边特征单元，其中edge1表示由r₀到r₁的边特征，edge2表示由r₀到r₂的边特征，angle表示两条边的夹角，对于有n个邻域反射板的单个反射板，一共生成n*(n-1)/2个边角边特征单元，如r₀最终生成如反射板特征结构体f₀，其中包含3个边角边特征单元，对激光数据中的其他反射板(r₁，r₂，r₃)进行同样操作，得到激光反射板数据对应的反射板特征结构体集合F_r{f_r0，f_r1，f_r2，f_r3}，反射板数据Q_m同样通过这个过程预处理而得到了反射板特征结构体集合F_m{f_m0，f_m1，f_m2，f_m3，f_m4}；

步骤2通过步骤1对激光反射板数据处理获取激光实时扫描反射板特征结构体集合F_r{f_r0，f_r1，f_r2，f_r3}与参考地图预处理获取的反射板特征结构体集合F_m{f_m0，f_m1，f_m2，f_m3，f_m4}进行特征结构体FeatureMark的匹配操作，将匹配结果推入待处理反射板特征结构体匹配对集M_mp，反射板特征结构体间的匹配是通过逐个对反射板特征结构体内的边角边特征单元进行对比，计算其差值大小，统计合格的配对的边角边特征单元数量及其匹配平均误差；边角边特征单元的匹配计算流程如下：

首先计算两个边角边特征单元的角度差值，若差值在设定的阈值d_a范围内，则进行下一步边长差值匹配，若存在两条边的边长差值都在设定阈值eT范围内的情况，则认为两个边角边特征单元对应相等，记录匹配误差error；两个反射板特征结构体进行匹配时，逐个将它们各自包含的边角边特征单元按以上方法进行匹配，将匹配成功的单元配对，按照平均误差大小进行由小到大的排序；由于匹配时，会出现单个边角边特征单元有多个匹配成功对象的情况，此时，按照之前的从小到大的排序，对包含重复配对单元的对象组进行唯一性筛选，保留最优结果，然后计算成功配对的边角边特征单元数量suc_eAe_sum及多个边角边特征单元匹配平均误差average_error统计到反射板特征结构体匹配对结构；

步骤3对于步骤2最终得到的反射板特征结构体匹配对集M_mp，按照其中每个匹配对MatchPair的评分进行从优到次的排序，评分方式为匹配对中成功配对的边角边特征单元数量suc_eAe_sum大的评分高，suc_eAe_sum数量相同则对比匹配平均误差average_error的值，average_error值越小评分越高，在匹配对集中存在同一个反射板特征结构体匹配上多个其他对象的情况，这个时候按照评分排序筛选保留最优的唯一匹配对，样例激光扫描反射板数据与参考地图地标数据的匹配对排序结果，其中有两对重复对，筛选删除后保留得到包含四对匹配对的集合M_mp{(f_r1，f_m2)，(f_r2，f_m4)，(f_r0，f_m1)，(f_r3，f_m0)}；

步骤4、通过获取的匹配对的集合M_mp，得到激光数据与地图地标一一对应的反射板匹配点对，如从M_mp{(f_r1，f_m2)，(f_r2，f_m4)，(f_r0，f_m1)，(f_r3，f_m0)}中获取样例的匹配点对获得一一对应的准确匹配点对以后，考虑到实际布置的地图地标与激光扫描数据中反射板之间位置是一个连接固定的刚体结构，激光反射板数据两两之间形成的连接向量在激光坐标系中的姿态角与地图坐标系中对应地标连接形成的向量姿态角的角度差值，即为两个点集的转动角，经过旋转以后点集质心坐标的差值即为两个点集的平移量，至此，即得到两个点集之间的坐标变换关系；将有(n+1)对匹配点的集合Q_mp表示为点集变换的转角量θ通过公式(1)获取，之后平移量由式(2)和式(3)得到；

步骤5将激光传感器的初始位姿(x_ini，y_ini，theta_ini)，通过获取的激光扫描反射板数据与地图地标间的旋转平移量按照式(4)进行坐标变换，即得到准确的地图定位位姿(x_c，y_c，theta_c)。