CN111307147B - 一种融合定位反光板与激光特征的agv高精度定位方法 - Google Patents

Abstract

本发明涉及一种融合定位反光板与激光特征的AGV高精度定位方法，包括建立激光地图，使反光板轮廓在地图上清晰可见，并采集反光板在地图上的位置，开始AMCL定位，将AMCL定位作为优化初始值，用以选取可能扫描得到的反光板的位置，按激光扫描得到离反光板的距离，反向求解及优化，得到AGV的精确位置，如果未能识别到反光板，则使用AMCL的定位结果作为AGV的位置估计。本发明不依赖于反光板的严格安装，且能获得比激光定位更好的准确性，可以使得工业AGV小车在生产环境中长期稳定地工作。

Description

一种融合定位反光板与激光特征的AGV高精度定位方法

技术领域

本发明涉及移动机器人自主定位领域，涉及一种融合定位反光板与激光特征的AGV高精度定位方法。

背景技术

AGV是Automated Guided Vehicle的缩写，即“自动导引运输车”，其装备有电磁或光学等自动导引装置，它能够沿规定的导引路径行驶，是具有安全保护以及各种移载功能的运输车。

定位技术是工业AGV在工厂环境中稳定作业的基础，定位是确定移动机器人在运行环境中相对于全局坐标的位置及航向，是AGV导航的最基本环节。

自主AGV能够自动将物品从一个点搬运到另一个点，在整个过程中不需人参与，可以代替人工叉车运输，减少工厂和仓库对劳动力的需求。同时，AGV具有柔性好、自动化智能化水平高的特点，可根据生产工艺流程等改变而改变，也可与其他设备自动对接，实现自动化智能化生产，因此，近年来AGV得到越来越多制造业的青睐。为了实现AGV自主导航，需要根据多种传感器识别多种环境信息，AGV通过环境感知确定前进方向中的可达区域和不可达区域，确定在环境中的相对位置，以及对动态障碍物运动进行预判，从而为局部路径规划提供依据。

目前AGV定位产品级的方法主要是激光导航定位和磁钉磁条导航定位，但由于这些方法中反光板和磁钉磁条安装较为复杂，对环境要求高，因此仍然具有改进的空间。依赖于反光板的定位方法对环境设置要求高，往往需要保证运行期间在每个点都至少能检测到3个以上的反光板，而采用基于纯激光的定位方法无法满足工业AGV的工作精度要求，且容易受定位漂移的影响导致累积定位误差。

发明内容

本发明的目的是提供一种融合定位反光板与激光特征的AGV高精度定位方法。

为达到上述目的，本发明采用的技术方案是：

一种融合定位反光板与激光特征的AGV高精度定位方法，包括以下步骤：

S1、建立激光地图，使反光板轮廓在地图上清晰可见，并采集反光板在地图上的位置，

S2、开始AMCL定位，将AMCL定位作为优化初始值，用以选取可能扫描得到的反光板的位置，

S3、按激光扫描得到离反光板的距离，反向求解及优化，得到AGV的精确位置，

S4、如果未能识别到反光板，则使用AMCL的定位结果作为AGV的位置估计。

优选地，所述AGV上设置包括激光雷达、IMU、里程计，通过所述激光雷达、IMU、里程计建立地图并定位。

进一步优选地，在S1中：采集反光板在地图上的位置为采集在地图坐标系下定位反光板圆心的位置{R₁，...，R_n}。

进一步优选地，在S2中：对AMCL定位包括以下步骤：

S2.1、初始化粒子，

S2.2、根据里程计和IMU的数据对粒子做预测更新，

S2.3、根据激光数据对粒子做测量更新，

S2.4、对粒子进行权重计算和重采样，

S2.5、输出粒子的最终位置。

进一步优选地，在S3中：反向求解及优化包括以下步骤：

S3.1、从雷达信息中提取反光板位置信息，

S3.2、基于AMCL当前位置估计进行反光板位置匹配，

S3.3、反向求解计算AGV相对于反光板的位置估计，

S3.4、通过加权融合的方式优化当前位姿。

进一步优选地，在S3.1中：提取反光板位置信息包括根据局部极大值检测，得出激光雷达坐标系下的反光板位置{R″₁，...，R″_n}，并通过坐标变换得到基础坐标系下的反光板位置{R′₁，...，R′_n}。

进一步优选地，在S3.2中：根据AMCL当前位置估计、反光板扫描结果，匹配注册反光板位置，从而获得精准的位置估计。

进一步优选地，AMCL当前位置估计为r′(x′，y′，α′)，注册反光板的绝对位置描述为R_i(x_i，y_i)，其中i＝1，...，n，进而计算要用以匹配的距离和角度/>用/>与检测到的d，θ，进行匹配，匹配条件是两者之差满足设定的阈值，满足上述条件则可以将检测到的反光板与注册反光板进行位置匹配。

进一步优选地，在S3.3中：进行反光板位置匹配后，根据注册值和雷达检测的角度距离信息估算AGV的精准位置。

进一步优选地，在S3.4中：得到基于反光板的AGV位置估计后，用求均值的方法优化得到位置，位姿角的计算可以任取一个反光板进行计算，通过反光板到AGV相对于地图坐标系的角度β进行反馈，从而反解得到AGV位姿角α，从而得到AGV的精准位姿r(r_x，r_y，r_α)。

由于上述技术方案运用，本发明与现有技术相比具有下列优点和效果：

1、本发明提出的定位方法融合了反光板特征与激光特征，基于反光板定位模型，在马尔科夫定位结果的基础上对定位结果进行了二次优化，提升了定位精度；

2、本发明提出的定位方法不依赖于环境特征，基于地图反馈的反光板标定方法使得算法的重定位误差较低；

3、本发明提出的定位方法不依赖于反光板的严格安装，AMCL可以保证在暂时无反光板的区域内也能保持较好的位置估计，从而可在工业无人AGV上长期稳定地使用。

附图说明

附图1为本实施例中系统定位算法流程图；

附图2为本实施例中使用的AGV示意图；

附图3为本实施例中示意地图与反光板注册点；

附图4为本实施例中反光板反向求解位置优化过程图；

附图5为本实施例中基于反光板模型的定位优化算法示意图。

具体实施方式

下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

在本发明的描述中，需要说明的是，术语“中心”、“上”、“下”、“左”、“右”、“竖直”、“水平”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”、“第三”仅用于描述目的，而不能理解为指示或暗示相对重要性。

在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。

如图1所示的一种融合定位反光板与激光特征的AGV高精度定位方法，包括以下步骤：

S1、建立激光地图，使反光板轮廓在地图上清晰可见，并采集反光板在地图上的位置。

如图2所示，本实施例所使用的AGV上搭载了激光雷达、IMU和轮式里程计，利用多传感器信息的融合感知环境，构建地图并定位。激光雷达可以提供距离信息和强度信息，距离信息用来构建环境地图，而强度信息则用来识别定位反光板，结合两者可以提供精准的定位。本实施例的实验环境是安装有定位反光板的室内环境，其地图建立与反光板标定包括以下步骤：

1.1：首先使用Google开源的Cartographer方法建立2D激光地图，设地图坐标系为{m}，

1.2：建立好地图后，在地图坐标系下采集定位反光板圆心的位置{R₁，...，R_n}，本实施例中n为3。

如图3所示，本实施例提出的反光板位置标定方法基于已经建立好的地图，在标定反光板位置时采集圆心处的坐标，图中灰色圆圈的部分为定位反光板所在的位置，计算时会将反光板的尺寸考虑在内，这样便于控制AGV的重定位误差。

S2、开始AMCL定位，将AMCL定位作为优化初始值，用以选取可能扫描得到的反光板的位置。

定位部分使用了Brian P.Gerkey提出的AMCL定位方法，AMCL定位即自适应蒙特卡罗定位，本实施例通过此方法得出AGV位置初始估计。其根据里程计，IMU和激光数据，采用粒子滤波的方式，计算出AGV的位置，主要算法步骤如下：

2.1：初始化粒子，

2.2：根据里程计和IMU的数据对粒子做预测更新，

2.3：根据激光数据对粒子做测量更新，

2.4：对粒子进行权重计算和重采样，

2.5：输出粒子的最终位置r′。

定义本实施例中的位置表达为R(X,Y,θ)，其中X,Y,θ分别指x轴数值，y轴数值和朝向角。本实施例中AMCL定位结果和精准位姿结果都采用这样的位置表达。

AMCL定位可以为本实施例提出的定位优化算法提供初始值r′(x′，y′，α′)而后根据反向求解优化AGV的位置估计。

S3、按激光扫描得到离反光板的距离，反向求解及优化，得到AGV的精确位置。

如图4所示，反向求解及优化位置估计包含以下步骤：

3.1：从雷达信息中提取反光板位置信息。

处理激光雷达的强度值数据，根据局部极大值检测的方法，得出激光雷达坐标系下的反光板位置{R″₁，...，R″_n}，并通过坐标变换得到基础坐标系下的反光板位置{R′₁，...，R′_n}。

设基础坐标系到激光雷达坐标系的偏移为{Δx，Δy}，反光板的相对位置由距离和角度描述，激光坐标系下的反光板位置为R″(d′，θ′)，基础坐标系下的反光板位置为R′(d，θ)。

雷达坐标系到基础坐标系的转换由以下公式计算：

3.2：基于AMCL当前位置估计进行反光板位置匹配。

根据AMCL的当前位置估计，以及反光板扫描结果，匹配注册反光板位置，从而获得更加精准的位置估计。

AMCL当前位置估计为r′(x′，y′，α′)，注册反光板的绝对位置描述为R_i(x_i，y_i)，其中i＝1，...，n，进而计算要用以匹配的距离和角度/>计算公式为：

然后用与检测到的d，θ，进行匹配，匹配条件是两者之差满足设定的阈值，本实施例中/>设置为0.2，/>设置为0.3：

满足上述条件则可以将检测到的反光板与注册反光板进行位置匹配。

3.3：反向求解计算AGV相对于反光板的位置估计。

进行反光板位置匹配后，则可以根据注册值和雷达检测的角度距离信息估算AGV的精准位置，定义反光板到AGV相对于地图坐标系的角度为β，β可由下面的公式进行计算：

β_i＝θ_i+α+π。

从而计算出基于注册反光板位置的AGV位置估计：

3.4：通过加权融合的方式优化当前位姿。

得到基于反光板的AGV位置估计后，便可用求均值的方法优化得到位置：

位姿角的计算可以任取一个反光板进行计算，通过β进行反馈，从而反解得到位姿角α，计算公式如下：

α＝β-θ_i-π。

从而得到AGV的精准位姿r(r_x，r_y，r_α)，其中r_α即为求得的α。

S4、如果未能识别到反光板，则使用AMCL的定位结果作为AGV的位置估计。

本实施例建议的操作流程需要注意的是：

1、在环境中任意配置反光板，在关键位置处可以多安装一些；

2、基于多传感器的数据建立并优化环境地图；

3、基于环境地图标定反光板位置，记录在配置文件里；

4、运行AMCL定位算法获得AGV的初始估计位置；

5、运行反光板优化定位算法，得到AGV的精准位置估计。

上述实施例只为说明本发明的技术构思及特点，其目的在于让熟悉此项技术的人士能够了解本发明的内容并据以实施，并不能以此限制本发明的保护范围。凡根据本发明精神实质所作的等效变化或修饰，都应涵盖在本发明的保护范围之内。

Claims (1)

1.一种融合定位反光板与激光特征的AGV高精度定位方法，其特征在于：包括以下步骤：

S1、建立激光地图，使反光板轮廓在地图上清晰可见，并采集反光板在地图上的位置，采集反光板在地图上的位置为采集在地图坐标系下定位反光板圆心的位置{R₁，...，R_n}，所述AGV上设置包括激光雷达、IMU、里程计，通过所述激光雷达、IMU、里程计建立地图并定位，

S2、开始AMCL定位，将AMCL定位作为优化初始值，用以选取可能扫描得到的反光板的位置，对AMCL定位包括以下步骤：

S2.1、初始化粒子，

S2.2、根据里程计和IMU的数据对粒子做预测更新，

S2.3、根据激光数据对粒子做测量更新，

S2.4、对粒子进行权重计算和重采样，

S2.5、输出粒子的最终位置，

定义反光板位置表达为R(X，Y，θ)，其中X，Y，θ分别指x轴数值，y轴数值和朝向角，AMCL定位为提出的定位优化算法提供初始值r′(x′，y′，α′)而后根据反向求解优化AGV的位置估计，

S3、按激光扫描得到离反光板的距离，反向求解及优化，得到AGV的精确位置，反向求解及优化包括以下步骤：

S3.1、从雷达信息中提取反光板位置信息，根据局部极大值检测，得出激光雷达坐标系下的反光板位置{R″₁，...，R″_n}，并通过坐标变换得到基础坐标系下的反光板位置{R′₁，...，R′_n}，设基础坐标系到激光雷达坐标系的偏移为{Δx，Δy}，反光板的相对位置由距离和角度描述，激光坐标系下的反光板位置为R″(d′，θ′)，基础坐标系下的反光板位置为R′(d，θ)，

雷达坐标系到基础坐标系的转换由以下公式计算：

S3.2、基于AMCL当前位置估计进行反光板位置匹配，

根据AMCL当前位置估计、反光板扫描结果，匹配注册反光板位置，从而获得精准的位置估计，AMCL当前位置的初始值为r′(x′，y′，α′)，注册反光板的绝对位置描述为R_i(x_i，y_i)，

其中i＝1，...，n，进而计算要用以匹配的距离和角度/>计算公式为：

然后用与检测到的d，θ，进行匹配，匹配条件是两者之差满足设定的阈值：

满足上述条件则可以将检测到的反光板与注册反光板进行位置匹配，

S3.3、反向求解计算AGV相对于反光板的位置估计，

进行反光板位置匹配后，则可以根据注册值和雷达检测的角度距离信息估算AGV的精准位置，定义反光板到AGV相对于地图坐标系的角度为β，β可由下面的公式进行计算：

β_i＝θ_i+α’+π，

从而计算出基于注册反光板位置的AGV位置估计：

S3.4、通过加权融合的方式优化当前位姿，

得到基于反光板的AGV位置估计后，便可用求均值的方法优化得到位置：

位姿角的计算可以任取一个反光板进行计算，通过β进行反馈，从而反解得到位姿角α，计算公式如下：

α＝β-θ_i-π，

从而得到AGV的精准位姿r(r_x，r_y，r_α)，其中r_α即为求得的α，

S4、如果未能识别到反光板，则使用AMCL的定位结果作为AGV的位置估计。