CN112612034A - 基于激光帧和概率地图扫描的位姿匹配方法 - Google Patents

Abstract

本发明公开了一种基于激光帧和概率地图扫描的位姿匹配方法，包括如下步骤：S1、里程计基于上一时刻(h‑1)的全局最优位姿Pose_h‑1估算时刻h的机器人位姿Pose_h″；S2、基于相关性扫描确定搜索窗口，基于概率地图在搜索窗口内搜索得分最高的局部最优位姿Pose_h′；S3、将h时刻的激光帧投影到概率地图的Pose_h′位姿处，得到概率地图残差项；S4、基于概率地图的残差项、位姿Pose_h″及位姿Pose_h″构建目标函数，基于高斯牛顿法对目标函数进行优化，获取机器人在时刻h的全局最优位姿Pose_h。提高了相关匹配的搜索速度，提高了移动机器人的定位效率。

Description

基于激光帧和概率地图扫描的位姿匹配方法

技术领域

本发明属于连读定位技术领域，更具体地，本发明涉及一种基于激光帧和概率地图扫描的位姿匹配方法。

背景技术

随着移动机器人在服务和仓储物流领域有着越来越广泛的应用，其自主定位导航技术也越发重要，现在主流的前沿技术就是基于激光雷达的自然导航。移动机器人需要实时获取定位数据，通过里程计数据，通过各种传感器数据，如激光帧和预存地图进行匹配，从而获得雷达的实时位姿，实现机器人的定位。激光帧是激光雷达测量的环境距离数据，这个测距精度和雷达测量的频率有关，频率越低，测距越精准。但是测量频率越低，会导致雷达的位姿不确定性增加，使得雷达和地图的匹配位姿数量增多，匹配速度减慢。

为了解决上述问题，现有的技术方案对2D地图按照固定间隔距离进行平均分块；对地图中的每一块进行假设推演，假设块的左上位置为机器人的初始位置,在每一块的基础上枚举方向；对于每一个假设,映射激光雷达的点云数据到地图像素上；建立匹配度模型来评判每一个假设的得分,对于每一个假设,累计激光雷达所有点云数据所对应的像素距离“障碍物像素”曼哈顿距离的和；选取和最小的块和方向作为机器人的起始位置和方向；在机器人移动中使用蒙特卡洛法跟踪其实时位置。然而该方案存在如下问题：

该方案初始将点云映射到栅格地图的每一个位置，导致搜索的候选项目数非常庞大，使得匹配计算速度很慢。该方案找最优位置的得分使用激光点和地图障碍点的距离去计算，这样会导致栅格的尺寸过大影响匹配得分精度，导致机器人定位出现较大的偏差。

发明内容

本发明提供了一种基于激光帧和概率地图扫描的位姿匹配方法，旨在改善上述问题。

本发明是这样实现的，一种基于激光帧和概率地图扫描的位姿匹配方法，所述方法具体包括如下步骤：

S1、里程计基于(h-1)时刻的全局最优位姿Pose_h-1估算h时刻的机器人位姿Pose_h″；

S2、基于相关性扫描确定搜索窗口，基于概率地图在搜索窗口内搜索得分最高的局部最优位姿Pose_h′；

S3、将h时刻的激光帧投影到概率地图的Pose_h′位姿处，得到概率地图的残差项p_err；

S4、基于概率地图的残差项、位姿Pose_h″及位姿Pose_h″构建目标函数，基于高斯牛顿法对目标函数进行优化，获取机器人在h时刻的全局最优位姿Pose_h。

进一步的，局部最优位姿Pose_h′的获取具体包括如下步骤：

S21、基于最大角度步进值θ_s在角度范围2*v_θ*Δh内采集角度值，将采集到的角度与候选位置组成位姿候选项，将h时刻的激光帧投影到概率地图的候选位姿处，一个位姿候选项对应一个候选位姿，获取每个位姿候选项的概率得分，输出最高得分的位姿候选项的角度值，即为相对最优角度；

S22、将相对于最优角度作为中心，基于参数shr_a来缩小当前角度范围，同时基于参数shr_r缩小角度步进值；

S23、判定当前的角度步进值是否小于雷达旋转的最高分辨率角度值θ，若检测结果为是，则将得分最该的候选位姿作为局部最佳位姿Pose_h′进行输出，若检测结果为否，则执行步骤S24；

S24、基于缩小后的角度步进值在缩小后的角度范围内采集角度值，将采集到的角度与候选位置组成位姿候选项，将h时刻的激光帧投影到概率地图的候选位姿处，获取每个位姿候选项的概率得分，输出最高得分的位姿候选项的角度值，更新相对最优角度，并执行步骤S22。

进一步的，机器人位姿Pose_h″的获取方法具体如下：

S11、基于里程计数据估算机器人在m时刻的速度vO，将里程计估算的速度vO和时间戳m存到全局变量的里程计速度列表中：

vO＝(vO_x,vO_y,vO_θ)＝(o_m-o_m-1)/Δm

o_m-1为m-1时刻的里程计数据，o_m为m时刻的里程计数据，Δm为m-1时刻到m时刻的时间差；

S12、取当前时刻h的激光帧，在里程计速度列表中寻找不超过且距时刻h最近的时刻m和对应的速度vO，里程计输出的h时刻位姿Pose_h″：

Pose_h″＝Pose_h-1+vO*(h-m)

其中，Pose_h-1为h-1时刻的机器人全局最优位姿。

进一步的，目标函数具体如下：

其中，w₁、w₂为权重值，Pose_h″为里程计输出的h时刻位姿，Pose_h′为h时刻的局部最优位姿，1-M(S(Pose_h′))为概率地图残差项。

本发明提供的基于激光帧和概率地图扫描的位姿匹配方法具有如下有益技术效果：

1)通过高频的里程计估算匹配的初始位姿，使得匹配的初值更精准，避免扫描匹配出现误匹配。

2)插值加速搜索局部最优位姿，大幅减少了匹配次数，提高了匹配效率，提高了移动机器人的定位效率。

3)输出位姿是以加速搜索局部最优位姿为初值，并满足里程计估算位姿和概率地图投影最优的多方约束，提高了输出位姿的可靠性和精度。

附图说明

图1为本发明实施例提供的基于激光帧和概率地图扫描的位姿匹配方法流程图。

具体实施方式

下面对照附图，通过对实施例的描述，对本发明的具体实施方式作进一步详细的说明，以帮助本领域的技术人员对本发明的发明构思、技术方案有更完整、准确和深入的理解。

图3为本发明实施例提供的基于激光帧和概率地图扫描的位姿匹配方法流程图，该方法具体包括如下步骤：

(1)雷达点云获取及滤波

一帧雷达数据的激光帧点有数千上万个，在匹配过程中很耗计算时间，使用体素滤波器，在保证点云微观特征的基础上降低点云的密度，提高匹配效率，处理之后将点云存到全局变量。

(2)里程计估算速度及位姿

里程计有累积误差，该线程只使用里程计增量，并估算移动机器人的线速度和角度。已知在m-1时刻获得里程计数据为o_m-1，m时刻里程计数据为o_m，由时间差Δm，那么可以得到里程计估计的机器人速度vO：

vO＝(vO_x,vO_y,vO_θ)＝(o_m-o_m-1)/Δm

处理之后将里程计估算的速度vO和时间戳m存到全局变量的里程计速度列表中。

取当前时刻h的激光帧，在里程计速度列表中寻找不超过且距时刻h最近的时刻m和对应的速度vO。已知h-1时刻的机器人全局最优位姿Pose_h-1，那么里程计输出的h时刻位姿Pose_h″为：

Pose_h″＝Pose_h-1+vO*(h-m)

(3)相关性窗口计算

已知机器人的最大平移速度和旋转速度v_max(v_x,v_y,v_θ)，那么在时刻h-1到时刻h的时段Δh内，AGV单向移动距离不超过(v_x*Δh,v_y*Δh)，旋转角度不超过v_θ*Δh。那么相关性匹配的窗口(即相关性窗口)范围应该是以h-1时刻机器人位姿Pose_h-1为中心，以x方向宽度为2*v_x*Δh，以y方向宽度为2*v_y*Δh的矩形窗口，以2*v_θ*Δh角度范围为搜索姿态角，搜索范围如下图1所示。

(4)插值加速搜索最佳位姿

a)计算雷达旋转的最高分辨率角度值θ

雷达旋转角度步进的最高角度分辨率是激光扫描的最远点旋转一个地图栅格尺寸的角度，那么由余弦定理可得最高角度分辨率的角度值：

其中：res是所建栅格地图的分辨率，max是激光雷达扫描的最远点距离。

b)计算搜索候选项数目，那么可得匹配位姿的总候选项数目:

其中，位置候选项数目为

角度候选项数目为

c)最大角度步进值计算

概率地图使用双三次插值计算概率，再考虑到激光点的跳动偏差，雷达扫描的最远点旋转可跨过多个地图栅格，在此设为grid_a，计算最大的角度步进值：

d)插值加速搜索

在第一层原始角度范围2*v_θ*Δh内使用最大的角度步进值θ_s插值寻找较优角度，后面每一层通过参数shr_a缩小搜索范围，同时通过参数shr_r缩小角度步进值。直到最后一层插值使用步骤a)得到的最小分辨率角度遍历搜索，最终得到最优角度。具体如下图2所示，该方法具体包括如下步骤：

S1、基于最大角度步进值θ_s在角度范围2*v_θ*Δh内采集角度，即为候选角度，基于设定的位移步长在相关性窗口内来获取候选位置，将候选角度与候选位置组成位姿候选项，将h时刻的激光帧投影到概率地图的候选位姿处，一个位姿候选项对应一个候选位姿，获取每个位姿候选项的概率得分，输出最高得分位姿候选项的角度值，即为相对最优角度，

S2、将相对于最优角度作为中心，基于参数shr_a来缩小当前角度范围，同时基于参数shr_r缩小角度步进值；

S3、判定当前的角度步进值是否小于雷达旋转的最高分辨率角度值θ，若检测结果为是，则将得分最该的候选位姿作为局部最佳位姿Pose_h′进行输出，若检测结果为否，则执行步骤S4；

S4、基于缩小后的角度步进值在缩小后的角度范围内采集角度值，将采集到的角度值与候选位置组成位姿候选项，将h时刻的激光帧投影到概率地图的候选位姿处，获取每个位姿候选项的概率得分，输出最高得分位姿候选项的角度值，更新相对最优角度，并执行步骤S2。

(5)优化得到最优位姿

将h时刻的激光帧投影到概率地图的Pose_h′位姿处，将激光帧的中心点投影至栅格地图的Pose_h′位姿处，得到概率地图的残差项p_err，表示如下：

其中，M(S(Pose_h′)表示将第i个激光点打到各栅格的概率值。

防止基于概率地图匹配出现局部极值，再使用里程计估算位姿Pose_h″进行限定，那么得到总的优化目标函数：

其中，w₁、w₂为权重值。由高斯牛顿法优化，其中变量为Pose_h′，优化迭代得到新位姿Pose_h，即移动机器人的在h时刻的全局最优位姿Pose_h。

上面结合附图对本发明进行了示例性描述，显然本发明具体实现并不受上述方式的限制，只要采用了本发明的方法构思和技术方案进行的各种非实质性的改进，或未经改进将本发明的构思和技术方案直接应用于其它场合的，均在本发明的保护范围之内。

Claims (4)

1.一种基于激光帧和概率地图扫描的位姿匹配方法，其特征在于，所述方法具体包括如下步骤：

S1、里程计基于(h-1)时刻的全局最优位姿Pose_h-1估算h时刻的机器人位姿Pose_h″；

S2、基于相关性扫描确定搜索窗口，基于概率地图在搜索窗口内搜索得分最高的局部最优位姿Pose_h′；

S3、将h时刻的激光帧投影到概率地图的局部最优位姿Pose_h′处，得到概率地图残差项；

S4、基于概率地图的残差项、位姿Pose_h″及位姿Pose_h″构建目标函数，基于高斯牛顿法对目标函数进行优化，获取机器人在h时刻的全局最优位姿Pose_h。

2.如权利要求1所述基于激光帧和概率地图扫描的位姿匹配方法，其特征在于，局部最优位姿Pose_h′的获取具体包括如下步骤：

S21、基于最大角度步进值θ_s在角度范围2*v_θ*Δh内采集候选角度，将候选选角度与候选位置组成位姿候选项，将h时刻的激光帧投影到概率地图的候选位姿处，获取每个位姿候选项的概率得分，输出最高得分的位姿候选项的角度值，即为相对最优角度；

S22、将相对于最优角度作为中心，基于参数shr_a来缩小当前角度范围，同时基于参数shr_r缩小角度步进值；

S23、判定当前的角度步进值是否小于雷达旋转的最高分辨率角度值θ，若检测结果为是，则将得分最该的候选位姿作为局部最佳位姿Pose_h′进行输出，若检测结果为否，则执行步骤S24；

S24、基于缩小后的角度步进值在缩小后的角度范围内采集角度值，将采集到的角度值与候选位置组成位姿候选项，将h时刻的激光帧投影到概率地图的候选位姿处，获取每个位姿候选项的概率得分，输出最高得分位姿候选项的角度值，更新相对最优角度，并执行步骤S22。

3.如权利要求1所述基于激光帧和概率地图扫描的位姿匹配方法，其特征在于，机器人位姿Pose_h″的获取方法具体如下：

S11、基于里程计数据估算机器人在m时刻的速度vO，将里程计估算的速度vO和时间戳m存到全局变量的里程计速度列表中：

vO＝(vO_x,vO_y,vO_θ)＝(o_m-o_m-1)/Δm

o_m-1为m-1时刻的里程计数据，o_m为m时刻的里程计数据，Δm为m-1时刻到m时刻的时间差；

S12、取当前时刻h的激光帧，在里程计速度列表中寻找不超过且距时刻h最近的时刻m和对应的速度vO，里程计输出的h时刻位姿Pose_h″：

Pose_h″＝Pose_h-1+vO*(h-m)

其中，Pose_h-1为h-1时刻的机器人全局最优位姿。

4.如权利要求1所述基于激光帧和概率地图扫描的位姿匹配方法，其特征在于，目标函数具体如下：

其中，w₁、w₂为权重值，Pose_h″为里程计输出的h时刻位姿，Pose_h′为h时刻的局部最优位姿，1-M(S(Pose_h′))为概率地图的残差项。

Images