CN110736456A - 稀疏环境下基于特征提取的二维激光实时定位方法 - Google Patents

Abstract

本发明属于智能机器人技术领域，公开了一种稀疏环境下基于特征提取的二维激光实时定位方法，包括以下步骤：通过机器人激光数据计算机器人的预测位姿初值；根据最新接收的里程计数据和惯性导航数据校正该值；2)确定所有可能的候选位姿；每得到一帧激光数据后，计算各扫描角度的离散扫描数据；3)将激光点云数据分成不同的区块，利用霍夫变换提取区块内的直线、直角、圆弧和普通点，分别存放在不同的集合中；4)为不同集合中的点分配不同的权重，计算可能的候选位姿置信度，选择置信度分值最高的位姿作为最优位姿估计。采用本发明的方法，提高了特征标志障碍物在定位过程中的置信度，进而提高移动机器人的定位精度。

Description

稀疏环境下基于特征提取的二维激光实时定位方法

技术领域

本发明属于智能机器人技术领域，具体涉及一种稀疏环境下基于特征提取的二维激光实时定位方法。

背景技术

移动机器人定位是指机器人通过感知获取环境信息，经过相关的信息处理而确定自身及目标位姿的过程。根据机器人的定位过程可分为相对定位和绝对定位。

移动机器人的相对定位，假定机器人的初始位姿，采用相邻时刻的传感器信息对机器人的位置进行跟踪估计。相对定位法分为里程计法和惯性导航法。里程计法是在移动机器人的车轮上安装光电编码器，通过车轮转动的记录来实现对机器人的位姿跟踪。惯性导航法采用陀螺仪和加速度计来实现定位，使用陀螺仪测得旋转速度的值，使得加速度计测得加速度的值，机器人的位置信息通过将测量值进行一次积分和二次积分得到。

移动机器人的绝对定位，需要预先确定好环境模型或者通过外部传感器直接向机器人提供外界位置信息，计算机器人在全局坐标系中的位置。全局定位的主要方法有：(1)信标定位；(2)地图匹配定位；(3)GPS；(4)概率定位。信标定位利用人工路标或自然路标和三角原理进行定位。地图匹配定位首先利用传感器感知环境信息创建好地图，然后将当前地图与数据库中预先存储好的地图进行匹配，最后计算机器人在全局坐标系中的位姿。GPS是用于室外移动机器人导航与定位技术。概率定位即基于概率地图的定位，利用概率论来表示不确定性，将机器人方位表示为对所有可能的机器人位姿的一个概率分布，概率最大的位置就是机器人的位置。

中国专利申请CN201810048969.4公开了一种稀疏环境下的机器人实时定位方法。通过机器人激光数据计算机器人下一帧对应时间点的预测位姿初值，根据最新接收的里程计数据和惯性导航数据校正位姿初值。然后根据定位扫描窗口确定所有可能的候选位姿，每得到一帧激光数据后，计算各扫描角度下各激光反射点的坐标，并结合地图数据计算各扫描角度的离散扫描数据；最后计算每个候选位姿的置信度分值，取置信度分值最高的候选位姿作为下一帧数据对时间点位姿的最优估计。该方法虽然在一定程度上提高了变电站巡检机器人的定位精度和定位效率，但是将每个候选位姿对应的离散数据的置信度赋予相同的权值，无形中拉大了非特征标志障碍物的置信度，使得机器人的定位精度难以满足实际工程的需求，出现脱离导航路线的问题。

发明内容

本发明目的是：针对现有技术的不足，提供一种稀疏环境下基于特征提取的二维激光实时定位方法。本方法将不同特征类型的激光点云数据赋予不同的置信度权值，能够提高机器人的定位精度。

具体地说，本发明是采用以下技术方案实现的，包括以下步骤：

1)通过来自机器人的二维激光传感器的两帧激光数据中位姿计算机器人位移速度和旋转速度，并通过前一帧位姿、位移速度和旋转速度计算下一帧的预测位姿初值；

根据最新接收的里程计数据和最新接收的惯性导航数据对预测位姿初值进行校正得到位姿的估计值；

2)计算定位扫描窗口大小，根据定位扫描窗口大小确定各地图栅格上各扫描角度的位姿，作为所有可能的候选位姿；每得到一帧来自机器人的二维激光传感器的激光数据后，计算各扫描角度下各激光反射点在地图坐标系中的坐标；并结合地图数据计算各扫描角度下各激光反射点对应的地图栅格在地图坐标系中的坐标，作为各扫描角度的离散扫描数据；

3)将定位扫描窗口下的激光点云数据分成不同的区块，利用霍夫变换提取各个区块内的直线、直角、圆弧和普通点，并分别存放在不同的集合中；

4)为所述不同集合中的点分配不完全一致的权重，计算各可能的候选位姿的置信度，选择置信度分值最高的位姿作为机器人最优位姿估计。

进一步而言，所述步骤3)中，将定位扫描窗口下的激光点云数据分成不同的区块的具体方法为：

对于每一帧激光点云数据，计算连续的两个点的距离，如果该距离小于等于指定阈值，则这两个点属于同一个区块；如果该距离大于指定阈值，这两个点分别属于不同的区块。

进一步而言，所述阈值采用自适应变阈值分割方法，计算方式如下：

根据以下公式确定相邻扫描点的最小距离d：

d＝2Dsinθ

其中D为激光的有效测距距离，θ为二维激光传感器的分辨率；

根据以下公式确定自适应变阈值threshold：

threshold＝Sd/D

其中S为扫描点与距离传感器的距离，d为相邻扫描点的最小距离，D为激光的有效测距距离。

进一步而言，所述步骤3)中，将定位扫描窗口下的激光点云数据分成不同的区块还包括判断每个区块内的数据点的个数，当区块包含数据点的个数小于等于指定值时，舍弃该区块内数据点的步骤。

进一步而言，所述步骤3)中，利用霍夫变换提取各个区块内的直线点的具体方法为：

5-1)建立一个二维累加数组A(a,b)，其中，a是原始图像坐标系中区块R_i中经过点P(x_i,y_i)的直线斜率的可能取值范围的离散值，b是原始图像坐标系中区块R_i中经过点P(x_i,y_i)的直线截矩的可能取值范围的离散值；点P(x_i,y_i)经霍夫变换后在参数空间满足下式：

b＝-ax_i+y_i

5-2)按照以下过程寻找A(a,b)的最大峰值：

A(a,b)初始化为0；

对每一个激光点P(x_i,y_i)进行参数计算表决，即，将参数空间中每一个a的离散值代入上式中，从而计算出对应的b值，每计算出一对(a,b)，都将对应的数组元素A(a_i,b_j)加1，即A(a_i,b_j)＝A(a_i,b_j)+1；同时将由同一个(a_i,b_j)值确定的原始图像空间中的点存入另一个数组C(a_i,b_j)，作为(a_i,b_j)值所确定的直线上的共线点；

在参数计算表决结果中找到A(a,b)的最大峰值，所对应的a_i就是块R_i中共线点数目最多的直线方程的斜率，所对应的b_i就是块R_i中共线点数目最多的直线方程的截距；

5-3)继续寻找次峰值、第3峰值和第4峰值，直到找到所有的峰值点；

5-4)判断某一区块内各激光点云的数据是否满足直线特征，判断方式如下：

对于只存在最高峰值的区块，将该区块中直线上的共线点存入第一集合中；

对于存在多个峰值的区块，计算该区块中每条直线与其他直线上的点的距离，当最小距离大于指定阈值时，将各直线上的点存入第一集合中；

当最小距离小于指定阈值时，判断相应的两条直线是否垂直；若不垂直，则将这两条直线上的点存入第一集合中。

进一步而言，所述步骤3)中，利用霍夫变换提取各个区块内的直角点的具体方法为：

提取出所有激光点云的共线点，对于存在多个峰值的区块，计算该区块中每条直线与其他直线上的点的距离，当最小距离小于指定阈值时，判断相应的两条直线是否垂直，若垂直，则将这两条直线上的点存入第二集合中。

进一步而言，所述步骤3)中，利用霍夫变换提取各个区块内的圆弧点的具体方法为：

7-1)建立三维数组累加器A(m,n,r)，其中，m是原始图像坐标系中区块R_i中经过点P(x_i,y_i)的圆弧圆心的x轴坐标可能范围的离散值，n是原始图像坐标系中区块R_i中经过点P(x_i,y_i)的圆弧圆心的y轴坐标可能范围的离散值，r是原始图像坐标系中区块R_i中经过点P(x_i,y_i)的圆弧半径的可能取值范围的离散值；r的取值范围为r_min≤r≤r_max，x_j-r≤m≤x_j+r，x_j-r≤n≤x_j+r；r以指定步长遍历预设的[r_min,r_max]；对于每一个r，让m或n在取值范围内增加指定步长，每计算出一个(m,n,r)值，都将相应的数组元素A(m,n,r)加1；同时将由同一个(m,n,r)值确定的原始图像空间中的点存入另一个数组D，作为(m,n,r)值所确定的圆弧上的点；

7-2)计算结束后，找到A(m,n,r)的最大峰值所对应的圆弧，将该圆弧上的点存入第三集合中。

进一步而言，所述步骤3)中，所述普通点为剔除各个区块中属于第一集合、第二集合或第三集合的点之后，剩余的点；存在第四集合中。

进一步而言，所述步骤4)中，计算各可能的候选位姿的置信度的具体方法为：

根据每个候选位姿对应的扫描角度的离散扫描数据中各个地图栅格的置信度，计算每个候选位姿的置信度σ，公式如下：

其中，n₄为某个候选位姿对应的扫描角度的具有直角特征的离散扫描数据中的地图栅格的总数，n₃为某个候选位姿对应的扫描角度的具有圆弧特征的离散扫描数据中的地图栅格的总数，n₂为某个候选位姿对应的扫描角度的具有直线特征的离散扫描数据中的地图栅格的总数，n₁为某个候选位姿对应的扫描角度的普通离散扫描数据中的地图栅格的总数；p_xnyn是具有直角特征的离散扫描数据中的地图栅格的置信度，p_xiyi是具有圆弧特征的离散扫描数据中的地图栅格的置信度，p_xjyj是具有直线特征的离散扫描数据中的地图栅格的置信度，p_xkyk是普通离散扫描数据中的地图栅格的置信度；ω₁为直角点的比重，ω₂为圆弧点的比重，ω₃为共线点的比重、ω₄为普通点的比重，ω_i≥0(i＝1，2，3，4)且ω₁+ω₂+ω₃+ω₄>0；

根据每个候选位姿与位姿的估计值的位姿差来计算每个候选位姿对应的置信度权重ω，公式如下：

其中，x_offset是每个候选位姿与位姿的估计值pose_estimated间沿x轴的位移，y_offset是每个候选位姿与位姿的估计值pose_estimated间沿y轴的位移，trans_weight是位移权重，candiate.rotation是候选位姿与位姿的估计值pose_estimated间旋转角度，rot_weight是旋转角度权重；

将每个候选位姿的置信度σ与置信度权重ω的乘积作为当前位姿的置信度分值score，公式如下，

score＝σ*ω

选择置信度分值score最高的位姿作为最优位姿估计。

进一步而言，所述步骤2)中，在每得到一帧激光数据后，先检查激光数据的帧率是否满足阈值的要求，如果小于阈值则表示不满足阈值的要求，此时不使用该激光数据并上报告警，等待接收到下一帧激光数据。

进一步而言，所述步骤2)中，在每得到一帧激光数据后，如该激光数据的帧率满足阈值的要求，则先对该激光数据中的各激光反射点进行过滤，去掉各激光反射点中相距较近的点和较远的点，剩余的各激光反射点再用于后续各扫描角度下各激光反射点在地图坐标系中的坐标的计算。

进一步而言，所述步骤2)中，对于某个扫描角度的离散扫描数据而言，若其中有重复落在同一地图栅格位置的多个激光反射点，仅取其中一个激光反射点对应的地图栅格在地图坐标系中的坐标，用于后续步骤的置信度计算。

本发明的有益效果如下：采用本发明的稀疏环境下基于特征提取的二维激光实时定位方法，将不同特征类型的激光点云数据赋予不同的置信度权值，能够提高特征标志障碍物在定位过程中的置信度，进而提高机器人的定位精度。

附图说明

图1是本发明实施例的定位流程图。

图2是本发明实施例的定位扫描示意图。

图3是本发明实施例的激光点云特征提取流程图。

具体实施方式

下面结合实施例并参照附图对本发明作进一步详细描述。

实施例1：

本发明的一个实施例，介绍了一种稀疏环境下的机器人实时定位方法。

本实施例采用的机器人，其硬件系统传感器主要包括里程计、惯性导航单元、二维激光传感器，其中里程计可以用来校准位置的预测值，惯性导航单元可以用来校准线速度、角速度，二维激光传感器用来获得激光数据。机器人软件系统使用机器人操作系统ROS，这是一种常用的机器人软件平台，它能为异质计算机集群提供类似操作系统的功能，在ROS系统中包括实现定位功能的节点。当然，可以理解的是，本发明方法也可以通过其他机器人的软件系统加以实现，本实施例采用ROS系统仅作为一种实现方式。

获得各帧激光数据的时间点是计算位姿的时间点。假定机器人在时间点t的位姿是P(t)，前一个计算位姿的时间点为t-1，相应的位姿是P(t-1)，下一个计算位姿的时间点t+1，相应的的位姿是P(t+1)。通过时间点t和t-1之间的位姿差(包括位移和旋转角度)来计算机器人运动的线速度V(x,y)和角速度W。

线速度V(x,y)＝(t和t-1之间的位移)/时间差

角速度W＝(t和t-1之间的旋转角度)/时间差

由于巡检机器人的移动速度较慢，最大约为1米/秒，并且两个时间点之间的时间差很小，一般为ms级别。因此可以使用计算的速度(包括线速度和角速度)来预测机器人在时间点t+1的位姿P(t+1)。由于实际硬件的误差，机器人在时间点t+1的准确位姿与该预测位姿之间会有偏差。通过激光测量数据和预测位姿对应地图数据之间符合的程度来优化P(t+1)，最终获得最接近准确位姿的最优位置。

本实施例中的地图以及机器人的导航运动假定在一个二维平面以内，坐标系包括地图坐标系、机器人坐标系、激光传感器坐标系。

地图坐标系是全局坐标系，在地图构建结束以后确定。计算机器人位姿时使用的是地图坐标系。

机器人坐标系是以机器人为原点的坐标系，二维导航中，一般是以机器人的中心点为原点。

激光传感器坐标系是以激光传感器的中心位置为原点的坐标系，激光数据的位姿使用的是激光传感器坐标系。

需要把不同坐标系下面的数据转化到同一个坐标系，才能进行位姿比较和计算。不同坐标系间坐标值的转换可以通过ROS系统中TF模块(坐标转换模块)实现。

机器人在时间点t的位姿P(t)可以表示为机器人在地图坐标系中的位置(x,y)以及机器人与x轴间角度r的函数P(x,y,r)。

机器人定位过程中得到的激光数据中，包括不同特征类型的数据，例如变电站中的柱子对应的圆弧特征、墙对应的直线特征、墙角对应的直角特征、花草或不规则障碍物对应的普通点特征。根据经验，在这些特征中，最可信的是墙角，其次是柱子，再次是墙，最后是花草或不规则障碍物。因为直角有两个方向(维度)的特征，最能体现平移特征，而直线只有一个维度的特征，圆弧介于直角和直线之间。不同特征类型的数据分配与可信度相应的权重可以有效减少噪声误差，使得定位效果更好。

参照图1，本实施例的稀疏环境下的机器人实时定位算法，其实现的主要步骤如下：

一、预测位姿初值估算：

根据相邻两帧激光数据中位姿计算机器人位移速度和旋转速度，并通过前一帧位姿、位移速度和旋转速度计算下一帧的预测位姿初值；根据最新接收的里程计数据和最新接收的惯性导航数据对预测位姿初值进行校正得到位姿的估计值。详细步骤如下：

每得到一帧来自机器人的二维激光传感器的激光数据laserscan后，ROS系统对激光数据进行处理得到激光点云数据pointcloud，该激光点云数据pointcloud是每帧激光数据所包含的laserscan.size()个数据点信息的统称，反映了各激光反射点在激光传感器坐标系中的坐标。不同激光传感器的帧率、扫描角度、每帧包含的数据点数量不同，ROS系统在定位过程中使用激光点云数据来进行处理和计算。针对稀疏环境的特点，可以对激光点云数据pointcloud进行过滤，即去掉激光点云数据pointcloud中的各激光反射点中相应的噪点(相距较近的点和较远的点)，剩余作为有效点，可以提高定位估计的置信度。由于对每一帧激光数据对应的激光点云数据pointcloud进行过滤的结果不同，每一帧激光数据对应的有效点的值不同。例如，在laserscan.size()为1141的传感器上，有效点约为几百个。对于激光数据laserscan，要进行帧率的检测，即在每得到一帧激光数据后，检查激光数据laserscan的帧率是否小于阈值，如果小于阈值则上报告警并等待接收到下一帧激光数据后重新进行相应处理。例如，如果定义的帧率是25Hz，而得到相邻两帧激光数据的时间大于40ms，则激光数据laserscan不符合要求，可能激光传感器出现过热等故障，需要上报告警并等待下一帧激光数据。

通过P(t)和P(t-1)之间x轴和y轴方向的位移分别计算机器人沿x轴的速度velocity_x和沿y轴的速度velocity_y，通过P(t)和P(t-1)之间r的差值来计算机器人的旋转速度velocity_r。通过P(t)、velocity_x、velocity_y和velocity_r计算P(t+1)，得到机器人在时间点t+1的预测位姿P(t+1)，称为预测位姿初值pose_prediction。

当通过ROS系统中Odom节点获得更新的里程计数据odometerdata(包括位移和旋转角度)后，根据两帧里程计数据odometerdata之间的差异，计算出里程计数据的校正值odometer_correction，用于优化P(t+1)。

当通过ROS系统中的IMU节点获得更新的惯性导航单元数据后，根据线性加速度校正当前的位移速度velocity_x和velocity_y，根据角加速度校正当前的旋转速度velocity_r，将预测位姿初值pose_prediction更新为位姿的估计值pose_estimated。

二、定位窗口扫描：

在定位窗口扫描阶段，以位姿估算值为中心，计算定位扫描窗口大小，每得到一帧来自机器人的二维激光传感器的激光数据后，计算各扫描角度下各激光反射点在地图坐标系中的坐标；并结合地图数据计算各扫描角度下各激光反射点对应的地图栅格在地图坐标系中的坐标，所有可能的候选位姿。具体流程如下：

根据机器人位姿的估计值pose_estimated设置定位扫描窗口。定位窗口扫描时使用位移扫描参数linear_search_window和角度扫描参数，其中角度扫描参数包括扫描角度大小angular_search_window和扫描角度步长angular_step。位移扫描参数用于限定定位扫描窗口的位移范围为以位姿的估计值pose_estimated为中心，上下左右各偏离linear_search_window大小的正方形。角度扫描参数用于限定定位扫描窗口的角度范围为以位姿的估计值pose_estimated为中心角度，上下各偏离angular_search_window大小的角度。

定位扫描窗口大小确定定位扫描窗口内各地图栅格上各扫描角度的位姿，作为所有可能的候选位姿possible_candidates。

例如，参照图2，linear_search_window＝0.3米，angular_search_window＝4，angular_step＝0.2，地图分辨率0.1米*0.1米。以位姿的估计值pose_estimated为中心计算各栅格上各扫描角度的位姿时，因为在地图坐标系中可能位置有(2*0.3/0.1+1)²＝49个，即49个栅格位置；每个位置上包括2*4/0.2+1＝81个可能角度。最终可能的位姿就是49*81个位姿。

候选位姿是机器人可能的位置和朝向，也即激光传感器可能的位置和朝向。当激光传感器放置在不同位置和朝向即机器人采用不同的候选位姿时，同一激光点云数据映射到地图坐标系中会得到不同的一套坐标。在各套地图坐标中选择候选位姿置信度最高的那套坐标作为激光点云数据在地图坐标系的坐标。

确定各候选位姿对应的激光点云数据在地图坐标系中坐标的方法为：根据机器人的定位扫描窗口(通过位姿的估计值pose_estimated、角度扫描参数和位移扫描参数确定)和激光点云数据pointcloud计算各扫描角度的激光点云数据rotatedcloud，这个rotatedcloud反映了各激光反射点在地图坐标系中的坐标。根据位姿的估计值pose_estimated和各扫描角度的激光点云数据rotatedcloud计算各扫描角度下各激光反射点对应的地图栅格位置(即计算每个地图栅格在地图坐标系中的坐标)，作为各扫描角度的离散扫描数据discretizescans。对某个扫描角度的离散扫描数据discretizescans而言，若其中有重复落在同一地图栅格位置的多个激光反射点，仅取其中一个激光反射点对应的地图栅格在地图坐标系中的坐标，用于后续步骤的置信度计算。

三、激光点云数据几何特征提取：

将定位扫描窗口下的激光点云数据分成不同的区块，利用霍夫变换提取各个区块内的直线、直角、圆弧和普通点特征，详细流程如图3所示。

1、激光点云区域分割

对于每一帧距离数据，把激光点云分割成不同的区块。如果连续两个点的距离小于或等于一个阈值，这两个扫描点属于同一个区块。如果连续两个扫描点的距离大于一个阈值，数据帧就从这个地方分割开。

一帧距离数据分割成几个区块后，分割的区块表示为Ri(i＝1,2,3…,Q)，其中Q是分割的区块数，每一个区块包含Ni个点。由于扫描点的分布并不是均匀的，通常情况下，离传感器近的扫描点密度大一些，而远离传感器的扫描点密度小一些。若使用同一阈值进行分割的话，对于密度大的扫描点就难以区分出来，使用自适应阈值可以避免数据分割错误的问题。所以进行距离数据分割时，应用自适应变阈值分割方法。容易想到，还可以使用别的分割方法，如区域蔓延分割、基于最少切割的分割等，但是使用距离数据分割具有准确，算法简单的优点。具体分割步骤如下：

1)计算点P(x,y)与相邻点之间的距离D_j、判断D_j和阈值的关系，如果D_j大于阈值，则认为点P(x,y)是两个区域的分割点。自适应阈值threshold的计算方式如下：

根据以下公式确定相邻扫描点的最小距离d。

d＝2Dsinθ (1)

其中D为激光的有效测距距离，θ为二维激光传感器的分辨率。

根据以下公式确定自适应阈值threshold。

threshold＝Sd/D (2)

其中S为扫描点与距离传感器的距离，d为相邻扫描点的最小距离，D为激光的有效测距距离。

2)判断每个区域内的数据点的个数，如果某个区域包含数据点的个数小于等于k个，k∈[3,5]，那么该区域被视为噪声区域，舍弃这些噪声点。

2、激光点云几何特征提取

针对各个区块R_i，首先利用霍夫变换算法提取出激光点云中呈直线排列的点和直角排列的点，分别存入集合L和集合A，然后在未被标记的激光点云数据中提取出呈圆弧排列的点，存入为集合C。剩余的点存入集合D。具体步骤如下：

1)提取直线特征

在二维平面内，区块R_i中某点的坐标可表示为P(x_i,y_i)，经过点P(x_i,y_i)的直线可以表示为：

y_i＝ax_i+b (3)

其中，a为斜率，b为截矩。通过点P(x_i,y_i)的直线有无数条，分别对应于不同的a值和b值。对点P(x_i,y_i)进行霍夫变换，将x_i和y_i视为常数，而将原本的参数a和b看作变量，则公式(3)可以表示为如下方程：

b＝-ax_i+y_i (4)

公式(4)是原始图像坐标系(作为原始图像空间的坐标系)中的点P(x_i,y_i)在参数坐标系(作为参数空间的坐标系)的唯一直线方程。

具体计算时，可以将参数空间视为离散的。建立一个二维累加数组A(a,b)，该数组的第一维a的取值范围是原始图像坐标系中直线斜率的可能范围，第二维b的取值范围是原始图像坐标系中直线截矩的可能范围。理论上a和b的取值范围为[0，∞]，计算时设置为[0，a′]，a′为一个足够大的值，设置初始值为0，指定步长(如1或0.1)，得到取值范围内各离散取值，由a、b的离散取值建立二维累加数组A(a,b)。寻找A(a,b)的最大峰值的过程如下：

开始时，A(a,b)初始化为0，然后对每一个激光点P(x_i,y_i)进行参数计算表决，即，将参数空间中每一个a的离散取值代入式(4)中，从而计算出对应的b值，每计算出一对(a,b)，都将对应的数组元素A(a_i,b_j)加1，即A(a_i,b_j)＝A(a_i,b_j)+1；同时将这对(a_i,b_j)对应的原始图像空间的点(即由(a_i,b_j)确定的一条直线上的共线点)存入另一个数组C(a_i,b_j)。

在参数计算表决结果中找到A(a,b)的最大峰值，所对应的a_i,b_j就是区块R_i中共线点数目最多的直线方程的参数。

可以继续寻找次峰值和第3峰值和第4峰值等，直到找到所有的峰值点。它们对应于原始图像空间中共线点略少一些的直线，

激光点云的直线特征需满足以下条件：

·对于只存在最高峰值的区块，说明该区块中只存有一条直线，该直线上的共线点存入集合L中。

·对于存在多个峰值的区块，对于该区块中每条直线，计算其他直线上的点到该直线(后文称当前直线)的距离，当最小距离大于某一阈值T时，即认为该点所在的直线与当前直线不存在交点，即该区块中存在多条直线，将各直线上的点存入集合L中。

·对于存在多个峰值的区块，对于该区块中每条直线，计算其他直线上的点到该直线(后文称当前直线)的距离，当最小距离小于某一阈值T时，说明该点所在的直线与当前直线相交，并利用“相互垂直的两条直线的斜率等于-1”的数学知识判断两条是否垂直；若两条直线斜率的乘积不接近于-1，则说明这两条直线是相交但不垂直的，将各直线上的点存入集合L中。

2)提取直角特征

参照1)提取出所有激光点云的共线点，对于该区块中每条直线，计算其他直线上的点到该直线(后文称当前直线)的距离，当最小距离小于某一阈值T时，判断两条直线斜率的乘积是否接近-1，若接近于-1，则认为两条直线组成的夹角为直角，将两条直线上的点存入集合A中。

3)提取圆弧特征

将各个区块R_i中共线的点去除，剩余的某点P(x_i,y_j)，经过点P(x_i,y_j)的一簇圆弧可以表示为：

(x_j-m)²+(y_j-n)²＝r² (5)

其中，m为圆弧的圆心在原始图像坐标系中x轴的坐标，n为圆弧的圆心在原始图像坐标系中y轴的坐标，r为圆弧的半径。

那么，经过霍夫变换后，参数空间中该圆弧可以表示为(m,n,r)，即，原始图像空间中的一个圆弧对应参数空间中的一个点。建立三维数组累加器A(m,n,r)，设变电站柱子的半径的取值范围为r_min≤r≤r_max，x_j-r≤m≤x_j+r，x_j-r≤n≤x_j+r。r以步长1cm遍历[r_min,r_max]，根据工程中的实际情况，可以预设r_min＝8cm，r_max＝30cm。对于每一个r，让m，n在取值范围内按步长增加，每计算出一个(m,n,r)值，就对相应的数组元素A(m,n,r)加1。计算结束后，找到的最大的A(m,n,r)所对应的m，n，r就是所求的圆弧的参数。然后将圆弧上的点存入集合C中。

4)提取普通点

剔除各个区块R_i中属于集合A、集合L、集合C的点，将剩余的点存入集合D中。

四、置信度计算：

经过之前的步骤，此时，每个候选位姿possible_candidate对应的离散扫描数据已经被分类存放在集合A、集合L、集合C和集合D中。分别给直角点、圆弧点、共线点、普通点分配比重ω₁、ω₂、ω₃、ω₄，且满足ω_i≥0(i＝1，2，3，4)和ω₁+ω₂+ω₃+ω₄>0；优选地，ω₁＞ω₂>ω₃>ω₄，计算每个候选位姿的置信度σ，公式如下：

其中，n₄为某个候选位姿对应的扫描角度的直角离散扫描数据中的地图栅格的总数，n₃为某个候选位姿对应的扫描角度的圆弧离散扫描数据中的地图栅格的总数，n₂为某个候选位姿对应的扫描角度的共线离散扫描数据中的地图栅格的总数，n₁为某个候选位姿对应的扫描角度的普通离散扫描数据中的地图栅格的总数，

是不同特征类型下地图栅格的置信度(地图栅格的置信度值与地图构建过程相关，在定位过程中，为已经确定的值)，取值范围为0.1～0.9。

根据每个候选位姿possible_candidate与位姿的估计值pose_estimated的位姿差来计算每个候选位姿对应的置信度权重ω，公式如下：

其中，x_offset是每个候选位姿与位姿的估计值pose_estimated间沿x轴的位移，y_offset是每个候选位姿与位姿的估计值pose_estimated间沿y轴的位移，trans_weight是位移权重，candiate.rotation是候选位姿与位姿的估计值pose_estimated间旋转角度，rot_weight是旋转角度权重；

将每个候选位姿的置信度σ与置信度权重ω的乘积作为当前位姿的置信度分值score，公式如下，

score ＝σ *ω (8)

选择置信度分值score最高的位姿更新位姿的估计值pose_estimated，作为最优位姿估计，即作为P(t+1)，定位结束。

虽然本发明已以较佳实施例公开如上，但实施例并不是用来限定本发明的。在不脱离本发明之精神和范围内，所做的任何等效变化或润饰，同样属于本发明之保护范围。因此本发明的保护范围应当以本申请的权利要求所界定的内容为标准。

Claims (12)

1.一种稀疏环境下基于特征提取的二维激光实时定位方法，其特征在于，包括以下步骤：

1)通过来自机器人的二维激光传感器的两帧激光数据中位姿计算机器人位移速度和旋转速度，并通过前一帧位姿、位移速度和旋转速度计算下一帧的预测位姿初值；

根据最新接收的里程计数据和最新接收的惯性导航数据对预测位姿初值进行校正得到位姿的估计值；

2)计算定位扫描窗口大小，根据定位扫描窗口大小确定各地图栅格上各扫描角度的位姿，作为所有可能的候选位姿；每得到一帧来自机器人的二维激光传感器的激光数据后，计算各扫描角度下各激光反射点在地图坐标系中的坐标；并结合地图数据计算各扫描角度下各激光反射点对应的地图栅格在地图坐标系中的坐标，作为各扫描角度的离散扫描数据；

3)将定位扫描窗口下的激光点云数据分成不同的区块，利用霍夫变换提取各个区块内的直线、直角、圆弧和普通点，并分别存放在不同的集合中；

4)为所述不同集合中的点分配不完全一致的权重，计算各可能的候选位姿的置信度，选择置信度分值最高的位姿作为机器人最优位姿估计。

2.根据权利要求1所述的一种稀疏环境下基于特征提取的二维激光实时定位方法，其特征在于，所述步骤3)中，将定位扫描窗口下的激光点云数据分成不同的区块的具体方法为：

对于每一帧激光点云数据，计算连续的两个点的距离，如果该距离小于等于指定阈值，则这两个点属于同一个区块；如果该距离大于指定阈值，这两个点分别属于不同的区块。

3.根据权利要求2所述的一种稀疏环境下基于特征提取的二维激光实时定位方法，其特征在于，所述阈值采用自适应变阈值分割方法，计算方式如下：

根据以下公式确定相邻扫描点的最小距离d：

d＝2Dsinθ

其中D为激光的有效测距距离，θ为二维激光传感器的分辨率；

根据以下公式确定自适应变阈值threshold：

threshold＝Sd/D

其中S为扫描点与距离传感器的距离，d为相邻扫描点的最小距离，D为激光的有效测距距离。

4.根据权利要求1所述的一种稀疏环境下基于特征提取的二维激光实时定位方法，其特征在于，所述步骤3)中，将定位扫描窗口下的激光点云数据分成不同的区块还包括判断每个区块内的数据点的个数，当区块包含数据点的个数小于等于指定值时，舍弃该区块内数据点的步骤。

5.根据权利要求1所述的一种稀疏环境下基于特征提取的二维激光实时定位方法，其特征在于，所述步骤3)中，利用霍夫变换提取各个区块内的直线点的具体方法为：

5-1)建立一个二维累加数组A(a,b)，其中，a是原始图像坐标系中区块R_i中经过点P(x_i,y_i)的直线斜率的可能取值范围的离散值，b是原始图像坐标系中区块R_i中经过点P(x_i,y_i)的直线截矩的可能取值范围的离散值；点P(x_i,y_i)经霍夫变换后在参数空间满足下式：

b＝-ax_i+y_i

5-2)按照以下过程寻找A(a,b)的最大峰值：

A(a,b)初始化为0；

对每一个激光点P(x_i,y_i)进行参数计算表决，即，将参数空间中每一个a的离散值代入上式中，从而计算出对应的b值，每计算出一对(a,b)，都将对应的数组元素A(a_i,b_j)加1，即A(a_i,b_j)＝A(a_i,b_j)+1；同时将由同一个(a_i,b_j)值确定的原始图像空间中的点存入另一个数组C(a_i,b_j)，作为(a_i,b_j)值所确定的直线上的共线点；

在参数计算表决结果中找到A(a,b)的最大峰值，所对应的a_i就是块R_i中共线点数目最多的直线方程的斜率，所对应的b_i就是块R_i中共线点数目最多的直线方程的截距；

5-3)继续寻找次峰值、第3峰值和第4峰值，直到找到所有的峰值点；

5-4)判断某一区块内各激光点云的数据是否满足直线特征，判断方式如下：

对于只存在最高峰值的区块，将该区块中直线上的共线点存入第一集合中；

对于存在多个峰值的区块，计算该区块中每条直线与其他直线上的点的距离，当最小距离大于指定阈值时，将各直线上的点存入第一集合中；

当最小距离小于指定阈值时，判断相应的两条直线是否垂直；若不垂直，则将这两条直线上的点存入第一集合中。

6.根据权利要求5所述的一种稀疏环境下基于特征提取的二维激光实时定位方法，其特征在于，所述步骤3)中，利用霍夫变换提取各个区块内的直角点的具体方法为：

提取出所有激光点云的共线点，对于存在多个峰值的区块，计算该区块中每条直线与其他直线上的点的距离，当最小距离小于指定阈值时，判断相应的两条直线是否垂直，若垂直，则将这两条直线上的点存入第二集合中。

7.根据权利要求6所述的一种稀疏环境下基于特征提取的二维激光实时定位方法，其特征在于，所述步骤3)中，利用霍夫变换提取各个区块内的圆弧点的具体方法为：

7-1)建立三维数组累加器A(m,n,r)，其中，m是原始图像坐标系中区块R_i中经过点P(x_i,y_i)的圆弧圆心的x轴坐标可能范围的离散值，n是原始图像坐标系中区块R_i中经过点P(x_i,y_i)的圆弧圆心的y轴坐标可能范围的离散值，r是原始图像坐标系中区块R_i中经过点P(x_i,y_i)的圆弧半径的可能取值范围的离散值；r的取值范围为r_min≤r≤r_max，x_j-r≤m≤x_j+r，x_j-r≤n≤x_j+r；r以指定步长遍历预设的[r_min,r_max]；对于每一个r，让m或n在取值范围内增加指定步长，每计算出一个(m,n,r)值，都将相应的数组元素A(m,n,r)加1；同时将由同一个(m,n,r)值确定的原始图像空间中的点存入另一个数组D，作为(m,n,r)值所确定的圆弧上的点；

7-2)计算结束后，找到A(m,n,r)的最大峰值所对应的圆弧，将该圆弧上的点存入第三集合中。

8.根据权利要求7所述的一种稀疏环境下基于特征提取的二维激光实时定位方法，其特征在于，所述步骤3)中，所述普通点为剔除各个区块中属于第一集合、第二集合或第三集合的点之后，剩余的点；存在第四集合中。

9.根据权利要求1所述的一种稀疏环境下基于特征提取的二维激光实时定位方法，其特征在于，所述步骤4)中，计算各可能的候选位姿的置信度的具体方法为：

根据每个候选位姿对应的扫描角度的离散扫描数据中各个地图栅格的置信度，计算每个候选位姿的置信度σ，公式如下：

其中，n₄为某个候选位姿对应的扫描角度的具有直角特征的离散扫描数据中的地图栅格的总数，n₃为某个候选位姿对应的扫描角度的具有圆弧特征的离散扫描数据中的地图栅格的总数，n₂为某个候选位姿对应的扫描角度的具有直线特征的离散扫描数据中的地图栅格的总数，n₁为某个候选位姿对应的扫描角度的普通离散扫描数据中的地图栅格的总数；

是具有直角特征的离散扫描数据中的地图栅格的置信度，

是具有圆弧特征的离散扫描数据中的地图栅格的置信度，

是具有直线特征的离散扫描数据中的地图栅格的置信度，

是普通离散扫描数据中的地图栅格的置信度；ω₁为直角点的比重，ω₂为圆弧点的比重，ω₃为共线点的比重、ω₄为普通点的比重，ω_i≥0(i＝1，2，3，4)且ω₁+ω₂+ω₃+ω₄>0；

根据每个候选位姿与位姿的估计值的位姿差来计算每个候选位姿对应的置信度权重ω，公式如下：

其中，x_offset是每个候选位姿与位姿的估计值pose_estimated间沿x轴的位移，y_offset是每个候选位姿与位姿的估计值pose_estimated间沿y轴的位移，trans_weight是位移权重，candiate.rotation是候选位姿与位姿的估计值pose_estimated间旋转角度，rot_weight是旋转角度权重；

将每个候选位姿的置信度σ与置信度权重ω的乘积作为当前位姿的置信度分值score，公式如下，

score＝σ*ω

选择置信度分值score最高的位姿作为最优位姿估计。

10.根据权利要求1～9任一所述的稀疏环境下基于特征提取的二维激光实时定位方法，其特征在于，所述步骤2)中，在每得到一帧激光数据后，先检查激光数据的帧率是否满足阈值的要求，如果小于阈值则表示不满足阈值的要求，此时不使用该激光数据并上报告警，等待接收到下一帧激光数据。

11.根据权利要求10所述的稀疏环境下基于特征提取的二维激光实时定位方法，其特征在于，所述步骤2)中，在每得到一帧激光数据后，如该激光数据的帧率满足阈值的要求，则先对该激光数据中的各激光反射点进行过滤，去掉各激光反射点中相距较近的点和较远的点，剩余的各激光反射点再用于后续各扫描角度下各激光反射点在地图坐标系中的坐标的计算。

12.根据权利要求1～9任一所述的稀疏环境下基于特征提取的二维激光实时定位方法，其特征在于，所述步骤2)中，对于某个扫描角度的离散扫描数据而言，若其中有重复落在同一地图栅格位置的多个激光反射点，仅取其中一个激光反射点对应的地图栅格在地图坐标系中的坐标，用于后续步骤的置信度计算。

Images