CN110108269A - 基于多传感器数据融合的agv定位方法 - Google Patents

Abstract

本发明公开了一种基于多传感器数据融合的AGV定位方法，在AGV车体上安装IMU姿态传感器、激光雷达和摄像头，并在工作环境进行标志铺设，AGV运动过程中，采用IMU姿态传感器和基于历史定位结果得到位姿估计，将其作为激光雷达扫描匹配定位的初始值，由激光雷达得到位姿估计；然后通过摄像头拍摄工作环境图像，从中识别出标志，结合标志位姿对AGV进行位姿估计，然后通过动态加权的方式对基于激光雷达的位姿估计和基于摄像头的位姿估计进行数据融合得到最终位姿。采用本发明可以提高二维平面AGV定位的稳定性和精确度。

Description

基于多传感器数据融合的AGV定位方法

技术领域

本发明属于机器人控制技术领域，更为具体地讲，涉及一种基于多传感器数据融合的AGV定位方法。

背景技术

AGV(Automated Guided Vehicles，自动导引车)是一种被广泛应用的智能轮式工业用移动机器人，通常具有承重、环境识别、自主导航、避障等功能，以其功能灵活、运行稳定、高度智能化等方面的优势，被广泛应用于智能制造、物流分拣、仓库存储、港口运输等行业。为生产运输过程带来了巨大的便利，具有良好的应用和发展前景。

在AGV设备的各项科学技术中，定位导航技术是实现AGV自动化、智能化运行的前提条件，也是AGV技术发展程度和发展水平的最重要标志。要实现精确可靠的定位，首先需要准确地感知环境信息。人类完整地感知环境需要眼、耳、鼻、舌、身等多种感官同时工作，各感官信息相互结合才能实现，机器依靠单一传感器只能获取特定的环境特征或运动状态的部分信息，为了更完整、更准确地进行定位，采用多种传感器组合感知环境是非常有必要的。同时，多个传感器之间也并非完全相互独立的，如何有效地进行传感器资源分配与管理，以及通过多源数据进行采集、识别、相关、组合和估计进而达到更加精确的姿态估计，具有极大的研究价值。此外，采用多传感器可以增强AGV***的环境感知维度，对抵抗环境干扰、提升***稳定性也是很有价值的。

现阶段也有很多研究者提出了各种各样的多传感器组合定位方案，但是在这些方案中引入的其它传感器很多都是用于对SLAM(simultaneous localization and mapping，即同时定位与地图构建)定位过程的优化，虽然可以在一定程度上提高AGV的构图定位效率和稳定性，但是在定位精度方面始终不能突破激光雷达自身的精度极限。许多研究者都认为基于视觉的SLAM方法是未来的SLAM技术的突破点，因为摄像头通常具有更高精度的环境分辨能力，但是目前相关算法都还不够成熟，并且完整的视觉SLAM过程又需要很高的计算能力，目前工业设备上的处理器还很难达到这样的计算能力。

发明内容

本发明的目的在于克服现有技术的不足，提供一种基于多传感器数据融合的AGV定位方法，结合IMU姿态传感器、激光雷达和摄像头，提高二维平面AGV定位的稳定性和精确度。

为实现上述发明目的，本发明基于多传感器数据融合的AGV定位方法的具体步骤包括：

S1：在AGV车体上安装IMU姿态传感器、激光雷达和摄像头，其中IMU姿态传感器安装于AGV车体中心，以其中心位姿坐标作为AGV的位姿坐标，激光雷达安装于AGV车体前方，摄像头安装位置可根据AGV工作环境中铺设的标志进行确定，须保证摄像头能够有效识别各个标志；

S2：在AGV工作环境中铺设若干标志，在AGV运行的每一个时刻至少有一个标志在AGV上搭载的摄像头视野范围之内，基于所铺设标志建立全局坐标系，并建立包含每个标志在全局坐标系中位姿坐标的标志坐标数据库；

S3：在AGV运动过程中，采用IMU姿态传感器获取当前定位周期的AGV姿态信息，记所得到的水平面方向上的旋转角度为w_est，利用之前定位周期所得到的AGV在全局坐标系下的位姿中的位置坐标，通过航迹推算得到当前定位周期AGV的位置估计[x_est,y_est]，即可得到AGV当前定位周期的位姿估计P_est＝[x_est,y_est,w_est]；

S4：将步骤S3得到的位姿估计P_est＝[x_est,y_est,w_est]作为激光雷达扫描匹配定位的初始值，通过激光雷达扫描匹配得到位姿估计P_scan＝[x_scan,y_scan,w_scan]；

S5：摄像头通过标志识别和畸变矫正获取到当前视野中标志的像素坐标P_pixel＝[x_pixel,y_pixel,w_pixel]，然后通过摄像头像素距离与空间实际距离的映射关系，得到在水平投影平面上标志相对摄像头视野中心的实际位姿坐标P_csign＝[x_csign,y_csign,w_csign]；

根据P_csign和P_scan计算得到标志的估计位姿坐标P_ms＝[x_ms,y_ms,0]：

其中，L为AGV车身中心与摄像头视野中心在水平面上的投影距离，θ为AGV在全局坐标系中的角度姿态，采用以下方法确定：

求取w_csign和w_scan的差值λ＝|(-w_csign)-w_scan|，如果差值λ小于预设的阈值，令令θ＝w_scan，否则令θ＝-w_csign；

根据估计位姿坐标P_ms＝[x_ms,y_ms,0]，从标志坐标数据库搜索最接近的标志，确定所识别标志在全局坐标系中的准确位姿坐标P_fixed＝[x_fixed,y_fixed,0]；

S6：根据P_csign和P_fixed计算得到摄像头视野中心在全局坐标系的位姿坐标P_camera＝[x_camera,y_camera,w_camera]：

P_camera＝[x_camera,y_camera,w_camera]＝[x_fixed-x_csigncosθ+y_csignsinθ,y_fixed-x_csignsinθ-y_csigncosθ,θ]

进而得到AGV的全局位姿坐标P_cagv＝[x_cagv,y_cagv,w_cagv]：

P_cagv＝[x_cagv,y_cagv,w_cagv]＝[x_camera(1-Lcosθ),y_camera(1-Lsinθ),θ]；

S7：计算摄像头所识别到的标志面积S_size，并以此为依据将P_scan和P_cagv两个数据进行动态加权融合，从而得到最终的AGV位姿P_out＝[x_out,y_out,w_out]，完成AGV定位，融合公式如下：

其中S_q为标志的期望面积，[a,b]为预设的摄像头标志识别数据有效的阈值范围。

本发明基于多传感器数据融合的AGV定位方法，在AGV车体上安装IMU姿态传感器、激光雷达和摄像头，并在工作环境进行标志铺设，AGV运动过程中，采用IMU姿态传感器和基于历史定位结果得到位姿估计，将其作为激光雷达扫描匹配定位的初始值，由激光雷达得到位姿估计；然后通过摄像头拍摄工作环境图像，从中识别出标志，结合标志位姿对AGV进行位姿估计，然后通过动态加权的方式对基于激光雷达的位姿估计和基于摄像头的位姿估计进行数据融合得到最终位姿。采用本发明可以提高二维平面AGV定位的稳定性和精确度。

附图说明

图1是本发明基于多传感器数据融合的AGV定位方法的具体实施方式流程图；

图2是本发明AGV的传感器配置示例图；

图3是本实施例中工作环境设置示意图。

具体实施方式

下面结合附图对本发明的具体实施方式进行描述，以便本领域的技术人员更好地理解本发明。需要特别提醒注意的是，在以下的描述中，当已知功能和设计的详细描述也许会淡化本发明的主要内容时，这些描述在这里将被忽略。

实施例

图1是本发明基于多传感器数据融合的AGV定位方法的具体实施方式流程图。如图1所示，本发明基于多传感器数据融合的AGV定位方法具体步骤包括：

S101：传感器配置：

在AGV车体上安装IMU(Inertial measurement unit，惯性测量单元)姿态传感器、激光雷达和摄像头，其中IMU姿态传感器安装于AGV车体中心，以其中心位姿坐标作为AGV的位姿坐标，激光雷达安装于AGV车体前方，摄像头安装位置可根据AGV工作环境中铺设的标志进行确定，须保证摄像头能够有效识别各个标志。图2是本发明AGV的传感器配置示例图。

S102：AGV工作环境设置：

在AGV工作环境中铺设若干标志，在AGV运行的每一个时刻至少有一个标志在AGV上搭载的摄像头视野范围之内，基于所铺设标志建立全局坐标系，并建立包含每个标志在全局坐标系中位姿坐标的标志坐标数据库。

所铺设的这些标志可以是铺设在地面的视觉可见标志，也可以是天花板上的电灯等标志物。为了使摄像头能够判别当前标志的方向，标志优选为由两部分不同图像特征组成的标志图像。若采用电灯等不具备明确方向特征的标志摄像头将无法判断标志的方向，但是水平面上的X、Y定位坐标依然有效，仍然可以实现定位。在铺设标志时，一般采用阵列分布，优选设置相邻标志之间的行、列间距相同，即标志在工作环境内均匀分布。图3是本实施例中工作环境设置示意图。如图3所示，本实施例中的标志由两种不同颜色矩形构成，且各个标志均匀分布，相邻标志之间横纵方向上的间距都为S，并以左下角第一个标志的中心为坐标原点建立全局坐标系，并且设置每个标志方向都与全局坐标系中X轴的方向一致。

S103：基于IMU姿态传感器和历史定位结果进行位姿估计：

在AGV运动过程中，采用IMU姿态传感器获取当前定位周期的AGV姿态信息，记所得到的水平面方向上的旋转角度为w_est，利用之前定位周期所得到的AGV在全局坐标系下的位姿中的位置坐标，通过航迹推算得到当前定位周期AGV的位置估计[x_est,y_est]，即可得到AGV当前定位周期的位姿估计P_est＝[x_est,y_est,w_est]。

本实施例中，航迹推算的具体方法为：

其中，Δt_k、Δt_k-1分别表示当前第k个定位周期和第k-1个定位周期的时间长度，表示第k-1个定位周期AGV在全局坐标系下的位姿中的位置坐标，表示第k-2个定位周期AGV在全局坐标系下的位姿中的位置坐标，显然分别为第k-1个定位周期AGV在X轴和Y轴上的运行速率。

由于本发明中AGV的定位结果是结合摄像头得到的，当摄像头正常工作时，由于摄像头标志识别的定位结果是没有累积误差的。因此在基于IMU姿态传感器进行位姿估计时，引入历史定位结果，可以减少累积误差。

S104：基于激光雷达进行定位：

将步骤S103得到的位姿估计P_est＝[x_est,y_est,w_est]作为激光雷达扫描匹配定位的初始值，通过激光雷达扫描匹配得到位姿估计P_scan＝[x_scan,y_scan,w_scan]。一般来说，激光雷达在扫描匹配定位过程中会同时进行环境二维地图的构建。

S105：标志识别与定位：

摄像头通过标志识别和畸变矫正获取到当前视野中标志的像素坐标P_pixel＝[x_pixel,y_pixel,w_pixel]，然后通过摄像头像素距离与空间实际距离的映射关系，得到在水平投影平面上标志相对摄像头视野中心的实际位姿坐标P_csign＝[x_csign,y_csign,w_csign]。记摄像头所拍摄的图像(即视野范围)的分辨率为m×n，对应实际空间面积为W×H，则标志相对摄像头视野中心的实际位姿坐标P_csign获取方式如下：

其中：

上标T表示转置。

将P_csign与步骤S104中得到的P_scan联合起来，可以得到一个不太准确的当前标志的估计位姿坐标P_ms＝[x_ms,y_ms,0]，具体方法如下：

其中，L为AGV车身中心与摄像头视野中心在水平面上的投影距离(如图2)，θ为AGV在全局坐标系中的角度姿态，由于本实施例中使用的标志识别方法角度识别精度较低，可以先由w_csign判断w_scan有无明显失真，进行判断时可以先求取二者的差值，由于w_csign和w_scan方向相反，所以差值λ＝|(-w_csign)-w_scan|，然后判断差值λ是否小于预设的阈值，如果是，则认为w_scan没有明显失真，并令θ＝w_scan，否则认为w_scan有明显失真，令θ＝-w_csign，取反是因为w_csign是标志相对AGV的角度，而θ是AGV相对于标志(也是全局地图)的角度。

根据估计位姿坐标P_ms＝[x_ms,y_ms,0]，从标志坐标数据库搜索最接近的标志，进而确定所识别标志在全局坐标系中的准确位姿坐标P_fixed＝[x_fixed,y_fixed,0]。

由于本实施例中标志是等间距均匀铺设的，因此也可以直接根据估计位姿坐标P_ms＝[x_ms,y_ms,0]计算出其准确位姿坐标P_fixed＝[x_fixed,y_fixed,0]，计算方法如下：

上式中，L表示AGV车身中心与摄像头视野中心在水平面上的投影距离，S表示相邻标志之间的间隔距离，表示向下取整。

S106：基于摄像头进行定位：

将步骤S105中得到的P_csign与P_fixed联合起来，就可以获取到摄像头视野中心在全局坐标系的位姿坐标P_camera＝[x_camera,y_camera,w_camera]，具体的计算公式如下：

P_camera＝[x_camera,y_camera,w_camera]＝[x_fixed-x_csigncosθ+y_csignsinθ,y_fixed-x_csignsinθ-y_csigncosθ,θ]

由于本发明中摄像头在AGV上的位姿是固定的，所以就可以得到AGV的全局位姿坐标P_cagv＝[x_cagv,y_cagv,w_cagv]：

P_cagv＝[x_cagv,y_cagv,w_cagv]＝[x_camera(1-Lcosθ),y_camera(1-Lsinθ),θ]。

S107：AGV定位：

计算摄像头所识别到的标志面积S_size，并以此为依据将P_scan和P_cagv两个数据进行动态加权融合，从而得到最终的AGV位姿P_out＝[x_out,y_out,w_out]，完成AGV定位，融合公式如下：

其中S_q为标志的期望面积，[a,b]为预设的摄像头标志识别数据有效的阈值范围，即当标志面积S_size超过此范围，则认为摄像头标志的识别数据无效。

为了更好地说明本发明的技术效果，采用图2所示的AGV和图3所示的工作环境对本发明进行实验验证，验证发现，当时，局部位置定位精度为±1cm，当S_size∈[a,b]时，局部位置定位精度可以达到±0.3cm，可以有效满足实际需要。

尽管上面对本发明说明性的具体实施方式进行了描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。

Claims (4)

1.一种基于多传感器数据融合的AGV定位方法，其特征在于，包括以下步骤：

S1：在AGV车体上安装IMU姿态传感器、激光雷达和摄像头，其中IMU姿态传感器安装于AGV车体中心，以其中心位姿坐标作为AGV的位姿坐标，激光雷达安装于AGV车体前方，摄像头安装位置可根据AGV工作环境中铺设的标志进行确定，须保证摄像头能够有效识别各个标志；

S2：在AGV工作环境中铺设若干标志，在AGV运行的每一个时刻至少有一个标志在AGV上搭载的摄像头视野范围之内，基于所铺设标志建立全局坐标系，并建立包含每个标志在全局坐标系中位姿坐标的标志坐标数据库；

S3：在AGV运动过程中，采用IMU姿态传感器获取当前定位周期的AGV姿态信息，记所得到的水平面方向上的旋转角度为w_est，利用之前定位周期所得到的AGV在全局坐标系下的位姿中的位置坐标，通过航迹推算得到当前定位周期AGV的位置估计[x_est,y_est]，即可得到AGV当前定位周期的位姿估计P_est＝[x_est,y_est,w_est]；

S4：将步骤S3得到的位姿估计P_est＝[x_est,y_est,w_est]作为激光雷达扫描匹配定位的初始值，通过激光雷达扫描匹配得到位姿估计P_scan＝[x_scan,y_scan,w_scan]；

S5：摄像头通过标志识别和畸变矫正获取到当前视野中标志的像素坐标P_pixel＝[x_pixel,y_pixel,w_pixel]，然后通过摄像头像素距离与空间实际距离的映射关系，得到在水平投影平面上标志相对摄像头视野中心的实际位姿坐标P_csign＝[x_csign,y_csign,w_csign]；

根据P_csign和P_scan计算得到标志的估计位姿坐标P_ms＝[x_ms,y_ms,0]：

其中，L为AGV车身中心与摄像头视野中心在水平面上的投影距离，θ为AGV在全局坐标系中的角度姿态，采用以下方法确定：

求取w_csign和w_scan的差值λ，λ＝|(-w_csign)-w_scan|，如果差值λ小于预设的阈值，令θ＝w_scan，否则令θ＝-w_csign；

根据估计位姿坐标P_ms＝[x_ms,y_ms,0]，从标志坐标数据库搜索最接近的标志，确定所识别标志在全局坐标系中的准确位姿坐标P_fixed＝[x_fixed,y_fixed,0]；

S6：根据P_csign和P_fixed计算得到摄像头视野中心在全局坐标系的位姿坐标P_camera＝[x_camera,y_camera,w_camera]：

P_camera＝[x_camera,y_camera,w_camera]

＝[x_fixed-x_csigncosθ+y_csignsinθ,y_fixed-x_csignsinθ-y_csigncosθ,θ]

进而得到AGV的全局位姿坐标P_cagv＝[x_cagv,y_cagv,w_cagv]：

P_cagv＝[x_cagv,y_cagv,w_cagv]＝[x_camera(1-Lcosθ),y_camera(1-Lsinθ),θ]；

S7：计算摄像头所识别到的标志面积S_size，并以此为依据将P_scan和P_cagv两个数据进行动态加权融合，从而得到最终的AGV位姿P_out＝[x_out,y_out,w_out]，完成AGV定位，融合公式如下：

其中S_q为标志的期望面积，[a,b]为预设的摄像头标志识别数据有效的阈值范围。

2.根据权利要求1所述的AGV定位方法，其特征在于，所述步骤S2中的标志由两种不同颜色矩形构成，且各个标志均匀分布。

3.根据权利要求1所述的AGV定位方法，其特征在于，所述步骤S3中航迹推算的具体方法为：

其中，Δt_k、Δt_k-1分别表示当前第k个定位周期和第k-1个定位周期的时间长度，表示第k-1个定位周期AGV在全局坐标系下的位姿中的位置坐标，表示第k-2个定位周期AGV在全局坐标系下的位姿中的位置坐标。

4.根据权利要求1所述的AGV定位方法，其特征在于，所述步骤S5中实际位姿坐标P_csign＝[x_csign,y_csign,w_csign]的获取方法为：

记摄像头所拍摄的图像(即视野范围)的分辨率为m×n，对应实际空间面积为W×H，则标志相对摄像头视野中心的实际位姿坐标P_csign获取方式如下：

其中：

上标T表示转置。