[发明专利]一种基于激光雷达扫描匹配算法的移动机器人定位方法

权利要求书

1.一种基于激光雷达扫描匹配算法的移动机器人定位方法，其特征在于，该方法包括以下步骤：

步骤1.分别对激光雷达相邻两采样周期的扫描数据进行角点特征提取，得到参考角点特征点集P和待配准角点特征点集Q；

步骤2.对参考角点特征点集P和待配准角点特征点集Q进行关联特征点配对得到初步配对点集M，具体为：利用点到点的欧式距离，对于所有的点q_i∈Q，在参考角点特征点集P中找到距离点q_i最近的点p_i组成对应点对(q_i,p_i)，得到初步配对点集M＝{(mq_i,mp_i)}，其中i＝1,2,...,n；

步骤3.采用分步迭代的方法求解参考角点特征点集P和待配准角点特征点集Q之间的整体匹配参数旋转矩阵R和平移矩阵T，具体包括以下步骤：

Step31：对初步配对点集M中的集合MQ和集合MP分别进行去中心化处理：

mq_i′＝mq_i-C_mq

mp_i′＝mp_i-C_mq

其中，MQ＝{mq_i∈Q|i＝1,2,...,n}，MP＝{mp_i∈P|i＝1,2,...,n}，

Step32：根据Step31获得目标评价函数E′(R,T)：

当目标评价函数E′(R,T)的值最小时所对应的R为所求旋转矩阵R，其中：

上式中，Δθ表示在相邻采样周期内小车方向变化角度；

Step33：将求解出的旋转矩阵R带入下式求出平移矩阵T：

T＝MP-R×MQ；

步骤4.对设置匹配阈值，滤除旋转矩阵R和平移矩阵T之间的无效关联特征；

步骤5.计算机器人在相邻两采样周期内的运动增量以及完成当前时刻机器人位姿估计。

2.根据权利要求1所述的一种基于激光雷达扫描匹配算法的移动机器人定位方法，其特征在于，所述分步迭代的方法具体包括：

计算中间转换矩阵：将矩阵MQ′带入整体匹配参数旋转矩阵R计算得到MQ″＝R×MQ′，将整体匹配参数旋转矩阵R和平移矩阵T带入得集合MQ中得：MQ″′＝R×MQ+T；其中，MQ′代表去中心化的待配准角点特征集；

迭代终止条件设置：设最大迭代次数为num_max，旋转矩阵R和平移矩阵T最小判别阈值分别为thd_R和thd_T；设去中心化的参考角点集合MP′和经过一次旋转变换的中间待配准点集MQ″之间的最小二乘误差为集合MP与MQ″′之间的最小二乘误差为MQ″′表示初始待配准点集经过整体旋转和平移变换后的新的待配准点集；

迭代终止判断：

当迭代次数num_i＜num_max时,若d_{min_R}＜thd_R并且d_{min_T}＜thd_T，返回(R,T)结束迭代；

否则，Q＝MQ″′，返回步骤2继续进行迭代；

当num_i≥num_max时，返回(R,T)结束迭代。

3.根据权利要求1所述的一种基于激光雷达扫描匹配算法的移动机器人定位方法，其特征在于，所述步骤4的具体方法为：

利用整体匹配时得到的参数(R,T)，对集合MQ进行转换得MQ1＝R×MQ+T，对集合MQ1和MP进行关联特征点配对，得到配对集合M1，对于(mq_i,mp_i)∈M1，若||mq_i-mp_i||＞D_max，将(mq_i,mp_i)从集合M1中剔除，其中D_max表示两关联点之间的最大距离阈值。