[发明专利]一种基于点线特征扫描匹配的移动机器人定位方法

权利要求书

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

步骤1：移动机器人通过搭载的激光雷达传感器对周围环境进行扫描，从激光雷达中获取二维点云数据，进行坐标转换和滤波的预处理：

步骤1.1：获取的二维点云数据表示为其中为激光雷达第i束激光在t时刻的观测量，d为所测得的障碍物的距离，η为激光雷达的扫描分辨率；通过公式(1)进行坐标转换，将得到的原始点云数据是从极坐标转换至直角坐标，得到

步骤1.2：对二维点云数据进行滤波处理：当一个扫描点到两相邻扫描点的距离皆大于阈值γ时，计算该扫描点到两相邻扫描点所连成线段的距离，当此距离大于阈值λ时，判定该扫描点为离群点，去除该扫描点；

步骤1.3：获取t时刻的一帧点云数据，进行坐标转换和滤波处理后得到点云数据其中n为此帧点云数据中所包含的扫描点个数；

步骤2：对点云数据P_t按扫描顺序依次从中取m个扫描点组成备选种子段，对各个备选种子段进行奇异值分解SVD，得到n-m+1个拟合残差平方和，并进行排序和筛选：

步骤2.1：从第i个扫描点开始，按顺序取m个扫描点，形成备选种子段按公式(2)进行奇异值分解SVD：

其中，u_i为左奇异矩阵；s_i为奇异值矩阵，且v_i为右奇异矩阵，为的均值，即

步骤2.2：对m个扫描点进行直线拟合时得到n-m+1个拟合残差平方和进行升序排序；设定阈值ε，舍弃E_i＞ε的备选种子段，保留剩下种子段用于区域生长算法，其中N为种子段的数量；

步骤3：将Seed中的种子段拟合直线，生长延伸得到直线特征，并进行合并操作：

步骤3.1：对于种子段使用总体最小二乘法拟合直线，由公式(3)计算得到直线方程y＝a_ix+b_i：

其中a_i和b_i分别表示直线的斜率和截距，和取自

步骤3.2：区域生长算法的生长条件设定为：种子段任一侧最邻近扫描点到拟合直线的距离小于阈值δ；

终止条件设定为：种子段两侧最邻近扫描点到拟合直线的距离大于阈值δ，或已到达点云边缘；

以生长条件和终止条件为判断条件对种子段进行生长延伸：

步骤3.2.1：种子段向后延伸；

在种子段S_i的邻域中取最邻近的后侧扫描点计算该扫描点到直线方程y＝a_ix+b_i的直线距离D_b：

若满足条件D_bδ，则将扫描点纳入种子段，将种子段向后扩张；再计算当前最邻近的后侧扫描点到直线方程y＝a_ix+b_i的直线距离D_b，若满足条件D_bδ则将纳入种子段；重复执行直至满足终止条件：当前最邻近的后侧扫描点到直线方程y＝a_ix+b_i的直线距离大于阈值δ，或该扫描点是点云边缘点，种子段停止向后延伸；

步骤3.2.2：种子段向前延伸；

在种子段S_i的邻域中取最邻近的后侧扫描点计算该扫描点到直线方程y＝a_ix+b_i的直线距离D_f：

若满足条件D_fδ，则将扫描点纳入种子段，将种子段向前扩张；再计算当前最邻近的前侧扫描点到直线方程y＝a_ix+b_i的直线距离D_f，满足条件D_fδ则将纳入种子段；重复执行直至满足终止条件：当前最邻近的前侧扫描点到直线方程y＝a_ix+b_i的直线距离大于阈值δ，或该扫描点是点云边缘点，种子段停止向前延伸；

当两侧都停止生长时，得到完整的种子段其中s_i和e_i分别表示种子段S_i中的首尾扫描点在P_t中的序号下标；对S_i采用公式(3)使用总体最小二乘法进行直线拟合，对拟合直线的斜率a_i和截距b_i进行更新；

步骤3.3：确定直线特征的两侧端点；

对于种子段S_i，取其中的首尾扫描点和向直线y＝a_ix+b_i作垂线，与直线相交于和两点，则将和作为直线特征首尾两侧的端点，并由公式(6)、(7)计算得到和的坐标：

则由种子段S_i得到的直线特征表示为其中为直线特征的重心，亦为中心，l_i表示直线特征的长度，即过坐标系原点向直线特征L_i作辅助垂线，r_i和θ_i分别表示辅助垂线在当前激光雷达坐标系下的长度以及与横轴x轴的夹角；

步骤3.4：所有种子段的直线特征提取完成之后，按种子段所包含的扫描点的获取顺序对所有直线特征重新排序，然后进行判别合并：

当前后两个直线特征为同一近似直线时，两者的辅助垂线也会近似重合，通过比较两辅助垂线的几何位置判断两直线特征是否可以进行合并，从而避免个别扫描点噪声过大而导致直线特征断开的情况，具体如下：

取L_i和L_i+1，i∈1,2,…,N-1，进行条件判断：

步骤3.4.1：判断条件：|r_i-r_i+1|Δr，其中Δr是两直线特征的辅助垂线之间的最大长度差，r_i+1表示过坐标系原点向直线特征L_i+1作的辅助垂线在当前激光雷达坐标系下的长度以及与横轴_x轴的夹角；

步骤3.4.2：判断条件：|θ_i-θ_i+1|Δθ，其中Δθ是两直线特征的辅助垂线之间的最大角度差，θ_i+1表示过坐标系原点向直线特征L_i+1作的辅助垂线在当前激光雷达坐标系下与横轴x轴的夹角；

当同时满足上述两个条件时，则将直线特征L_i和L_i+1合并形成L_i，并对L_i的参数进行更新，合并完成之后得到其中N₁为合并之后的直线特征数量，N₁≤N；

步骤4：角点特征提取；

步骤4.1：从中依次取相邻的两个直线特征L_i和L_i+1判断是否满足角点特征的条件：

步骤4.1.1：条件判断：|θ_i-θ_i+1|∈[90°-σ,90°+σ]，其中σ是用于判定角点的不确定性参数；

步骤4.1.2：条件判断：其中D_max是形成角点特征的直线特征相邻端点的最远距离；

步骤4.2，若同时满足上述步骤4.1.1和步骤4.1.2的条件，则认为直线特征L_i和L_i+1能延伸相交形成角点特征C_j，由公式(8)计算得到该角点特征的坐标：

其中a_i+1和b_i+1分别表示种子段S_i+1使用总体最小二乘法拟合直线的斜率和截距；则由直线特征L_i和L_i+1形成的角点特征表示为其中为角点特征的位置坐标；和是角点特征的两直角边在当前激光雷达坐标系下与横轴x轴的夹角，即对所有直线特征进行判断计算之后得到此帧点云的角点特征集合其中N₂为所提取的角点特征的数目；

步骤5：在移动机器人的运动过程中读取t+1时刻的激光雷达点云数据，进行步骤1至步骤4相同的处理操作来提取点线特征，从而得到t+1时刻点云数据中的直线特征集合和角点特征集合并按下列步骤进行匹配度计算，以寻找两帧点云之间的对应关系；匹配度分成直线匹配度和角点匹配度两部分；

步骤5.1：计算直线匹配度；

对于前后两帧点云的直线特征Line和Line'，设定固定大小的滑动窗口以确定需要进行匹配计算的直线特征对；

对于直线特征L_i∈Line和L_j'∈Line'，其直线匹配度为：

其中和分别为长度匹配度、位置匹配度和角度匹配度，且

其中DL_max为可匹配的两直线特征的最大重心距离差；Δθ_max为可匹配的两直线特征的最大夹角差；

步骤5和步骤6中的公式里带右上角标“＇”的变量均表示t+1时刻的激光雷达点云数据，变量定义与t时刻的相同；

步骤5.2：计算角点匹配度；

对于前后两帧点云的角点特征CP和CP'，使用相同的滑动窗口来确定需要进行匹配计算的角点特征对，取C_i∈CP和C_j'∈CP'，则此两角点特征的角点匹配度为：

其中为位置匹配度，和为角点特征两直角边的角度匹配度，且

其中DL_max和Δθ_max与公式(11)、(12)中参数相同，分别为可匹配的两直线特征的最大坐标距离差和最大夹角差；

步骤6：利用步骤5中得到的直线匹配度和角点匹配度进行移动机器人运动参数估计，计算前后两帧点云之间移动机器人的位姿变换；

移动机器人的运动分为平移和旋转，平移参数(x_t,y_t)和旋转参数θ_t计算过程表示为：

其中n₁，n₂分别为匹配度不为0的直线特征匹配对和角点特征匹配对的数量；表示直线特征匹配对(L_i,L_j')在参数计算中所占权重，表示角点特征匹配对(C_i,C_j')在参数计算中所占权重，

则前后两帧点云之间，即t时刻至t+1时刻，移动机器人位姿变换的旋转矩阵为平移矩阵为

2.根据权利要求1所述的一种基于点线特征扫描匹配的移动机器人定位方法，其特征在于，所述步骤1.2中判定为离群点的步骤如下：

步骤1.2.1：判断条件：其中γ为扫描点到相邻扫描点的最大距离阈值；

步骤1.2.2：判断条件：

步骤1.2.3：判断条件：其中分别为扫描点和相连所形成线段的斜率和截距；λ为扫描点到相邻扫描点所连成线段的最大距离阈值；

若同时满足上述三个条件，则认为为离群点，去除