CN108253958A - 一种稀疏环境下的机器人实时定位方法 - Google Patents

Abstract

本发明属于智能机器人技术领域，公开了一种稀疏环境下的机器人实时定位方法，包括以下步骤：1)通过机器人激光数据计算机器人下一帧对应时间点的预测位姿初值；根据最新接收的里程计数据和惯性导航数据校正位姿初值；2)根据定位扫描窗口确定所有可能的候选位姿；每得到一帧激光数据后，计算各扫描角度下各激光反射点的坐标；并结合地图数据计算各扫描角度的离散扫描数据；3)计算每个候选位姿的置信度与置信度权重的乘积作为当前位姿的置信度分值，取置信度分值最高的候选位姿作为下一帧数据对应时间点位姿的最优估计。采用本发明的稀疏环境下的机器人实时定位方法，定位相关计算量大大降低，定位效率和定位精度得到明显提高。

Description

一种稀疏环境下的机器人实时定位方法

技术领域

本发明属于智能机器人技术领域，具体涉及一种稀疏环境下的机器人实时定位方法。

背景技术

传统的变电站巡检主要是通过人工方式，存在劳动强度大、检测质量分散、主观因素多等缺陷。近年来，变电站设备巡检机器人采用完全自主或遥控方式，代替巡检人员对变电站内室外设备的项目进行巡检，并对图像进行分析和判断，及时发现电力设备存在的问题，为无人值班变电站的推广应用提供了创新性的技术检测手段，提高了电网的可靠稳定运行水平。

在移动机器人应用中，基于传感器的定位一直以来都被认为是一个关键的问题。定位是一种在线空间状态估计，其中移动机器人需要在一个全局坐标系中估计其位置。定位问题有两种：全局定位和位置跟踪。位置跟踪是指，机器人知晓其起始位置，只需考虑其运动过程中位移和角度的理论值和测量值之间的偏差即可。机器人定位上的已有方法包括马尔可夫定位(Markov localization)、卡尔曼滤波(Kalman Filtering)、粒子滤波(Particle Filtering)、即时定位与地图构建(Simultaneous Localization AndMapping，简称SLAM)和多机器人定位(Multi-robot localization)。依据不同的环境特征、机器人传感器配置可以选择不同的定位方法。

目前的巡检机器人使用的定位方法主要是粒子滤波定位，粒子滤波基于蒙特卡洛思想。粒子滤波定位的目标是粒子收敛，确定最终位置。粒子滤波定位过程可以分为以下四个步骤：

1)初始化粒子群。可以理解为在地图空间内均匀的撒下粒子。

2)模拟粒子运动。可以用不同的方法(例如里程计和惯性导航单元的数据，包括线速度和角速度)来估计机器人可能运动到的位置。

3)计算粒子可信度。根据粒子位置的估计对应的地图特征与实际测量的地图特征匹配度计算粒子可信度，匹配度越高，可信度越高。

4)粒子群重采样。在可信度高的粒子附近增加粒子，在可信度很低的粒子附近减少或者去掉粒子，重采样之后保持粒子群数量基本不变。

变电站室外环境为稀疏环境，具有以下几个特征：

●面积较大：大部分变电站面积都在1万平方米以上；

●环境相对静止：变电站内人、车等障碍物出现频次低，环境变化小；

●环境特征点少：缺少特征标志的障碍物。

中国专利201710239729.8公开了一种适用于变电站环境下的巡检机器人定位方法，机器人处于静态环境时，采用的是蒙特卡洛定位方法，通过激光扫描环境信息和地图进行匹配；当机器人进入和建图时环境相比发生改变了的区域是，不再使用已有地图定位，采用SLAM算法实时建图并定位，当机器人离开该区域，再次进入静止环境时，再次切换回蒙特卡洛定位；两种定位方式之间的切换由一个定位管理节点实现。该方法虽然可以使机器人可在整个变电站环境下实现准确定位，但在变电站这样的环境下，由于变电站面积大，粒子数量多，计算量大，因此不可避免地会带来对硬件主控设备要求高、定位效率低、定位精度不高等问题。

发明内容

本发明目的是：针对现有技术的不足，提供一种稀疏环境下的机器人实时定位方法。本方法可以大大降低定位过程中的计算量，实现高效的稀疏环境实时定位。

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

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

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

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

3)计算各可能的候选位姿的置信度，选择置信度分值score最高的位姿作为机器人最优位姿估计。

上述技术方案的进一步特征在于，所述步骤3)中，计算各可能的候选位姿的置信度的具体方法为：

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

其中，m为每个候选位姿对应的扫描角度的离散扫描数据中的地图栅格的总数，设第n个地图栅格的地图坐标为(x_n,y_n)，则该地图栅格置信度是

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

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

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

score＝candidate_probability*candidate_weight

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

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

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

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

本发明的有益效果如下：采用本发明的稀疏环境下的机器人实时定位方法，定位相关计算量大大降低，定位效率和定位精度得到明显提高。

附图说明

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

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

具体实施方式

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

实施例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，本实施例的稀疏环境下的机器人实时定位算法，其实现的主要步骤如下：

1)预测位姿初值估算：

通过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。

2)定位窗口扫描：

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

根据机器人位姿的估计值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)置信度计算：

根据每个候选位姿possible_candidate对应的各个地图栅格的置信度(地图栅格的置信度值与地图构建过程相关，在定位过程中，为已经确定的值)，计算每个候选位姿的置信度candidate_probability，公式如下：

其中，m为某个候选位姿对应的扫描角度的离散扫描数据discretizescan中的地图栅格的总数，设其中第n个栅格的地图坐标为(x_n,y_n)，该栅格置信度是取值范围为0.1～0.9。

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

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

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

score＝candidate_probability*candidate_weight

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

下面具体描述一下ROS系统中实时定位的工作流程：

1)定位节点启动后，订阅/imu、/odom、/scan、/loc_cmd和/initialpose主题。其中/imu、/odom和/scan主题分别对应惯性导航单元、里程计和激光传感器的数据，/loc_cmd是系统的定位指令，/initialpose是机器人开始工作的位姿。

2)定位节点获得当前起始位置的位姿start_pose。

一般使用上次工作结束时保存在地图文件中的位姿。当机器人无法确定当前位置时，根据发布的/initialpose来更新。

3)处理接收到的不同主题数据。

定位节点通过/odm主题获得里程计数据odometerdata(位移和旋转角度)以后，进入步骤4)。

定位节点通过/imu主题获得惯性导航单元(IMU)数据以后，进入步骤5)。

定位节点通过/scan主题获得激光数据laserscan以后，进入步骤6)。

4)依据两帧odometerdata(位移和旋转角度)之间的差异，计算出校正值，保存为odometer_correction。

5)根据线性加速度校正当前的位移速度velocity_x和velocity_y，根据角加速度校正当前的velocity_r，将预测位姿初值pose_prediction更新为位姿的估计值pose_estimated。

6)执行以下步骤进行定位估计。

6-1)将激光数据laserscan转换为激光点云数据pointcloud。

6-2)激光帧率检查，如果帧率低于阀值，向ROS处理告警信息的模块发出警告信息，等待帧率恢复正常后重新进行定位估计。

6-3)根据上一时刻的位姿、当前时刻的位移速度和角速度，估算出当前位姿pose_estimated。

6-4)根据最新的odometer_correction校正pose_estimated。

6-5)根据pose_estimated设置定位扫描窗口，根据位移窗口大小参数linear_search_window、角度窗口大小参数angular_search_window和角度步长参数angular_step计算所有可能的候选位姿possible_candidates。

6-6)按照角度扫描步长angular_step，计算角度扫描窗口内每个角度点云数据rotatedscans。

6-7)根据rotatedscans以及当前pose_estimated的(x,y)值，计算rotatedscans中每个点对应的栅格地图坐标，保存在离散点云数据discretizescans中。

6-8)计算每个候选位姿的置信度以及对应的权重，计算置信度分值。选取置信度分值最高的位姿更新pose_estimated。位姿post_estimated作为当前位姿的最优估计。

6-9)发布当前位姿/pose。

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

Claims (5)

1.一种稀疏环境下的机器人实时定位方法，其特征在于，包括以下步骤：

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

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

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

3)计算各可能的候选位姿的置信度，选择置信度分值score最高的位姿作为机器人最优位姿估计。

2.根据权利要求1所述的一种稀疏环境下的机器人实时定位方法，其特征在于，所述步骤3)中，计算各可能的候选位姿的置信度的具体方法为：

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

其中，m为每个候选位姿对应的扫描角度的离散扫描数据中的地图栅格的总数，设第n个地图栅格的地图坐标为(x_n,y_n)，则该地图栅格置信度是

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

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

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

score＝candidate_probability*candidate_weight

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

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

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

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