欢迎您访问《智慧农业(中英文)》官方网站! English
专题--丘陵山区智慧农业技术与机械

基于激光雷达与IMU融合的农业机器人定位方法

  • 刘洋 1 ,
  • 冀杰 1 ,
  • 潘登 , 2 ,
  • 赵立军 3 ,
  • 李明生 1
展开
  • 1. 西南大学 工程技术学院,重庆 400715,中国
  • 2. 中国汽车工程研究院股份有限公司,重庆 401122,中国
  • 3. 重庆文理学院智能制造工程学院,重庆 402160,中国
潘 登,硕士,工程师,研究方向为智能网联测评技术研究、测试工具链研发与测试系统开发。E-mail:

刘 洋,研究方向为智能网联车辆及农机的环境感知定位。E-mail:

收稿日期: 2024-01-10

  网络出版日期: 2024-05-23

基金资助

重庆市研究生科研创新项目(CYS23207)

重庆市科学技术局农业农村领域重点研发项目(cstc2021jscx-gksbX0003)

重庆市教育委员会科学技术研究项目(KJZD-M202201302)

重庆市科技局创新发展联合基金项目(CSTB2022NSCQ-LZX0024)

Localization Method for Agricultural Robots Based on Fusion of LiDAR and IMU

  • LIU Yang 1 ,
  • JI Jie 1 ,
  • PAN Deng , 2 ,
  • ZHAO Lijun 3 ,
  • LI Mingsheng 1
Expand
  • 1. College of Engineering and Technology, Southwest University, Chongqing 400715, China
  • 2. China Automotive Engineering Research Institute Co. Ltd. , Chongqing 401122, China
  • 3. School of Mechanical and Electrical Engineering, Chongqing University of Arts and Sciences, Chongqing 402160, China
PAN Deng, E-mail:

LIU Yang, E-mail:

Received date: 2024-01-10

  Online published: 2024-05-23

Supported by

Chongqing Graduate Research and Innovation Project(CYS23207)

Chongqing Science and Technology Bureau Key R&D Project in Agriculture and Rural Areas(cstc2021jscx-gksbX0003)

Chongqing Municipal Education Commission Science and Technology Research Project(KJZD-M202201302)

Chongqing Science and Technology Bureau Innovation and Development Joint Fund Project(CSTB2022NSCQ-LZX0024)

Copyright

copyright©2024 by the authors

摘要

[目的/意义] 精准可靠的定位技术是智能农业机器人开展自主导航作业的重要前提,而常用的全球卫星导航系统(Global Navigation Satellite System, GNSS)定位方法在农业环境中容易受到树木遮挡、电磁干扰等因素影响,因而,提出一种基于三维激光雷达(Light Detection and Ranging, LiDAR)与惯性测量单元(Inertial Measurement Unit, IMU)信息融合的农业机器人定位方法。 [方法] 首先,利用基于角度的聚类方法对激光雷达点云数据进行信息处理,并与三维正态分布变换(3D Normal Distribution Transform, 3D-NDT)定位算法相结合,在先验点云地图信息基础上实现基于激光雷达的实时定位;其次,为了克服单传感器定位方法的局限性,利用扩展卡尔曼滤波(Extended Kalman Filter, EKF)算法对激光雷达定位信息与IMU里程计信息进行融合,进一步提升农业机器人的定位精度。最后,分别在机器人操作系统(Robot Operating System, ROS)的Gazebo仿真环境中,以及真实作业场景中进行实验,验证提出的定位算法的有效性。 [结果和讨论] 融合定位方法在仿真环境中的纵向和横向平均定位误差分别为1.7和1.8 cm,而在实验中的纵向和横向平均定位误差分别为3.3和3.3 cm,均小于传统3D-NDT定位算法的定位误差。 [结论] 提出的融合定位方法能够满足农业机器人在弱GNSS环境下自主作业的定位要求,为农业机器人提供了一种新的定位方法。

本文引用格式

刘洋 , 冀杰 , 潘登 , 赵立军 , 李明生 . 基于激光雷达与IMU融合的农业机器人定位方法[J]. 智慧农业, 2024 , 6(3) : 94 -106 . DOI: 10.12133/j.smartag.SA202401009

Abstract

[Objective] High-precision localization technology serves as the crucial foundation in enabling the autonomous navigation operations of intelligent agricultural robots. However, the traditional global navigation satellite system (GNSS) localization method faces numerous limitations, such as tree shadow, electromagnetic interference, and other factors in the agricultural environment brings challenges to the accuracy and reliability of localization technology. To address the deficiencies and achieve precise localization of agricultural robots independent of GNSS, a localization method was proposed based on the fusion of three-dimensional light detection and ranging (LiDAR) data and inertial measurement unit (IMU) information to enhance localization accuracy and reliability. [Methods] LiDAR was used to obtain point cloud data in the agricultural environment and realize self-localization via point cloud matching. By integrating real-time motion parameter measurements from the IMU with LiDAR data, a high-precision localization solution for agricultural robots was achieved through a specific fusion algorithm. Firstly, the LiDAR-obtained point cloud data was preprocessed and the depth map was used to save the data. This approach could reduce the dimensionality of the original LiDAR point cloud, and eliminate the disorder of the original LiDAR point cloud arrangement, facilitating traversal and clustering through graph search. Given the presence of numerous distinct crops like trees in the agricultural environment, an angle-based clustering method was adopted. Specific angle-based clustering criteria were set to group the point cloud data, leading to the segmentation of different clusters of points, and obvious crops in the agricultural environment was effectively perceived. Furthermore, to improve the accuracy and stability of positioning, an improved three-dimensional normal distribution transform (3D-NDT) localization algorithm was proposed. This algorithm operated by matching the LiDAR-scanned point cloud data in real time with the pre-existing down sampled point cloud map to achieve real-time localization. Considering that direct down sampling of LiDAR point clouds in the agricultural environment could result in the loss of crucial environmental data, a point cloud clustering operation was used in place of down sampling operation, thereby improving matching accuracy and positioning precision. Secondly, to address potential constraints and shortcomings of using a single sensor for robot localization, a multi-sensor information fusion strategy was deployed to improve the localization accuracy. Specifically, the extended Kalman filter algorithm (EKF) was chosen to fuse the localization data from LiDAR point cloud and the IMU odometer information. The IMU provided essential motion parameters such as acceleration and angular velocity of the agricultural robot, and by combining with the LiDAR-derived localization information, the localization of the agricultural robot could be more accurately estimated. This fusion approach maximized the advantages of different sensors, compensated for their individual limitations, and improved the overall localization accuracy of the agricultural robot. [Results and Discussions] A series of experimental results in the Gazebo simulation environment of the robot operating system (ROS) and real operation scenarios showed that the fusion localization method proposed had significant advantages. In the simulation environment, the average localization errors of the proposed multi-sensor data fusion localization method were 1.7 and 1.8 cm, respectively, while in the experimental scenario, these errors were 3.3 and 3.3 cm, respectively, which were significantly better than the traditional 3D-NDT localization algorithm. These findings showed that the localization method proposed in this study could achieve high-precision localization in the complex agricultural environment, and provide reliable localization assistance for the autonomous functioning of agricultural robots. [Conclusions] The proposed localization method based on the fusion of LiDAR data and IMU information provided a novel localization solution for the autonomous operation of agricultural robots in areas with limited GNSS reception. Through the comprehensive utilization of multi-sensor information and adopting advanced data processing and fusion algorithms, the localization accuracy of agricultural robots could be significantly improved, which could provide a new reference for the intelligence and automation of agricultural production.

0 引 言

随着智慧农业的发展,越来越多的大田、果园等经典农业场景开始逐步实现信息化与智能化,农业机器人代替人工进行无人播种、智能采摘、自主导航等任务。其中,农业机器人自身的精确定位是实现这些任务的基础,而传统的农业机器人通常采用基于北斗卫星导航系统(BeiDou Navigation Satellite System, BDS)、全球定位系统(Global Positioning System, GPS)的全球卫星导航系统(Global Navigation Satellite System, GNSS)进行定位,但GNSS的定位信号容易受到树木遮蔽、电磁干扰以及电离层的影响,进而导致定位精度下降1-3。为了解决该问题,近年来各国学者不断提出基于激光雷达、摄像头、惯性测量单元(Inertial Measurement Unit, IMU)、超宽带技术(Ultra Wide Band, UWB)的全新定位方法,其中,激光雷达定位方法因其精度高、可靠性好、实时性强等优点,逐渐在农业机器人中得到应用和推广4-6
激光雷达定位方法是对激光雷达(Light Detection and Ranging, LiDAR)采集的前后两帧点云信息进行对比和匹配,得到位姿变换关系,进而推算出机器人的当前位置。目前常用匹配算法主要包括迭代最近点(Iterative Closest Point, ICP)算法7,以及正态分布变换(Normal Distribution Transform, NDT)算法8。为了扩大NDT算法的使用维度,Saarinen等9和Hu等10对NDT算法进行改进,建立了3D-NDT算法,提高了算法的效率与精度。另外,部分学者提出了基于先验点云地图的定位方法,将二维激光雷达扫描结果与先验点云地图进行匹配,从而得到当前位置11-13。但这些方法都受限于二维激光雷达信息较少,难以处理复杂场景。近年来,随着机器人计算性能提升,信息更加丰富的三维激光雷达也逐渐被应用到农业机器人的精确定位中。金泰宇和黄劲松14使用三维激光雷达提取点云中树木等目标,再与先验点云地图进行配准,实现了更加鲁棒的定位;部分学者则创新地利用了激光雷达点云的强度值信息与先验点云地图进行匹配,在复杂场景下实现了更加精确的自主定位15-18
虽然依靠激光雷达的定位算法已较为成熟,但在实际使用时仍会因为点云畸变、采样频率较低等问题影响定位精度。IMU因其采样频率高以及位姿解算简便等原因受到众多学者青睐,但因其可能存在较大的累积误差,故常与激光雷达融合进行定位。崔文等19提出一种基于先验点云地图的定位方法,通过误差卡尔曼滤波(Error-state Kalman Filter, ESKF)融合IMU与激光雷达定位信息,克服了无人车在转向时与地图匹配误差较大的问题。张文玥等20针对传统自适应蒙特卡洛(Adoptive Monte Carlo Localization, AMCL)算法在IMU与里程计上累积误差较大的问题,提出了一种基于松耦合结构的扩展卡尔曼滤波(Extended Kalman Filter, EKF)和AMCL多传感器融合定位方法,并结合激光雷达进行定位,提高了定位精度。涂远泯等21则通过联合无迹卡尔曼滤波(Unscented Kalman Filter, UKF)算法与粒子滤波对IMU、轮式里程计和激光雷达进行融合定位,将定位误差稳定在了2 cm左右。
上述研究方法中基于地图的定位算法通常都对激光雷达原始点云进行降采样,然后直接与先验点云地图进行匹配求解位姿。但在农业环境中,激光雷达扫描得到的点云数据一般来自于环境中比较明显的物体上,如树木等,直接对原始点云进行降采样可能会丢失这部分信息。针对这一问题,本研究对于智慧农业中常见的果园环境进行点云聚类,保留扫描点云中的果园环境特征,提高点云与先验点云地图匹配时的准确性。然后利用扩展卡尔曼滤波算法将3D-NDT定位结果与IMU里程计信息进行融合,修正3D-NDT定位算法中的缺陷,实现农业机器人在果园环境下的精确定位。

1 材料与方法

1.1 仿真与实验平台

本研究的仿真实验主要通过机器人操作系统(Robot Operating System, ROS)平台上的Gazebo软件搭建农业机器人模型及农业作业环境开展。其中,农业机器人模型的底盘采用差速移动模型,搭载VLP-16激光雷达与IMU用于仿真定位实验;在农业仿真场景中设置了丘陵地形、草地与少量低矮平房模拟真实环境,同时设置了较多树木用于模拟某些环境较为复杂的农业场景,如图1所示。仿真实验平台采用搭载AMD Ryzen7 4800 H处理器、16 G运行内存、NVIDIA GeForce RTX 2060显卡,操作系统为Ubuntu18.04的计算机。
图1 农业环境的仿真模型

Fig. 1 Simulation model of the agricultural environment

为进一步验证本研究所提融合定位算法的实际可行性,以自主研发的农业机器人(智能割草机)作为测试平台进行实验,其整体结构如图2所示。测试平台为后轮差速驱动,通过遥控设备发送脉冲宽度调制(Pulse Width Modulation, PWM)信号,在Arduino中经过处理,并通过RS232串口传递给JETSON XAVIER NX处理器,再通过串口协议向Keya双驱伺服驱动器发送控制指令可以远程控制驱动电机实现直行、转向等操作。实验所用激光雷达为Velodyne公司生产的16线激光雷达VLP-16;IMU为VectorNav Technologies公司生产的VN-200小型惯性导航系统;为验证定位结果准确性,以Witmotion公司生产的实时动态载波相位差分(Real-Time Kinematic, RTK)定位系统输出的定位结果作为基准,并将其与另外两种定位结果进行比较分析。
图2 智能割草机器人测试平台

Fig. 2 Test platform of intelligent mowing robot

1.2 激光雷达点云聚类

1.2.1 点云数据预处理

三维激光雷达扫描的原始点云数据量较大,直接对其进行数据处理会极大增加计算时间,从而影响农业机器人的作业实时性。本研究采用深度图方法对三维点云数据进行保存,根据激光雷达竖直和水平分辨率得到二维图像分辨率大小,通过公式(1)将激光雷达三维点云数据转换为点云到激光雷达的距离数据,并依次填入二维图像中得到深度图。其中,深度图的每一行和每一列分别代表水平和竖直方向上扫描得到的点云距离数据。通过该方法不仅可以将点云的三维数据转换为二维数据,还能将原本无序排列的点云有序化为二维数组的形式,简化了后续聚类点云的搜索方式,缩短聚类时间。
r a n g e = x 2 + y 2 + z 2
式中:x、y、z表示激光束打在环境物体上得到的激光点空间坐标值;rangeij)表示深度图中第i行,第j列的点到激光雷达的距离;i表示深度图形分辨率也即激光雷达的线束值,以16线激光雷达为例,此时i=16;j代表深度图列分辨率,与激光雷达水平分辨率有关,16线激光雷达中水平分辨率为0.2°,此时j=(360°÷0.2°)=1 800。深度图的示意图如图3所示。同理,也可以根据此变换关系从深度图中根据行列关系检索到原有点云数据,使深度图距离数据与点云三维数据建立联系。
图3 激光雷达点云的深度图

Fig. 3 Depth map of LiDAR point cloud

1.2.2 地面点云去除

在三维激光雷达原始点云中,地面点云数据占比较大,会增大数据处理运算量。另外,在聚类过程中不需要处理地面点云信息。因此,在进行点云聚类前需要先去除地面点云。由深度图原理可知,每一列数据代表竖直方向上扫描得到的点云值。假设激光雷达在竖直方向上扫描得到的相邻两点分别为A、B两点,坐标分别为 A ( x 0 , y 0 , z 0 ) B ( x 1 , y 1 , z 1 ),如图4所示。
图4 激光雷达地面线束示意图

Fig. 4 Ground scanning beams of LiDAR

A、B两点竖直方向的高度差 h z、水平距离差 d A B、两点连线与水平面的夹角 θ可以分别表示为公式(2)~公式(4)
h z = z 0 - z 1
d A B = ( x 0 - x 1 ) 2 + ( y 0 - y 1 ) 2
θ = a r c t a n ( h z / d A B )
式中:两点连线与水平面的夹角 θ可以用来评估作业地面的平坦程度,如果激光雷达水平安装且地面平坦,夹角 θ理论值应该为0°。考虑实际情况,设置地面角度阈值为10°,当 θ小于该阈值时,认为AB两点属于地面点。通过遍历深度图,可以得到分割出的地面点云结果(图5)。其中,彩色部分代表地面的点云信息。
图5 激光雷达的地面点云分割结果

Fig. 5 Segmentation result of LiDAR ground point cloud

1.2.3 点云聚类

农业机器人上的三维激光雷达点云聚类采用Bogoslavskyi和Stachniss22提出的基于角度的聚类方法。该方法对于点云间的角度关系较为敏感,特别适用于检测近似垂直于地面的物体。在农业环境下,大多数作物的树干都具有这类特征,能够在聚类过程中被准确识别,从而在一定程度上还原环境的真实性。该聚类算法原理如图6所示。
图6 角度聚类原理图

注: A点和B点为深度图中任意两个相邻点,O点为激光雷达坐标原点,连接OAOBAB形成一个三角形,设OA为较长边,过B点作OA垂线,垂足为C点,βOABA夹角,α为激光雷达水平分辨率或垂直分辨率对应角度。

Fig. 6 Schematic diagram of angular clustering

图6中得出β的表达式如公式(5)所示。
β = a r c t a n ( B C A C ) = a r c t a n ( O B s i n α O A - O B c o s α )
式中:OAOB的大小即为深度图中对应的距离值,可以通过查找深度图直接获得。设置β角的一个阈值γ,当β>γ时,认为A点与B点相对平坦,将其归为同一类别;当β<γ时,认为A点与B点之间的距离相差过大,将两者视为不同类别。
根据深度图的特点,采用基于图的广度优先搜索(Breadth First Search, BFS),从某一点开始对深度图中上下左右4个领域点进行遍历,在遍历完所有点云后得到最终的聚类结果,如图7所示。通过基于角度的点云聚类,可以将农业环境中许多明显的特征保留下来,如环境中的树、边界等。传统的3D-NDT点云匹配算法对激光雷达采集的点云数据进行简单降采样后,直接与先验点云地图匹配。而本研究方法将采集的点云数据先转换为聚类后的点云,再与点云地图进行匹配,可以充分利用更多的环境特征,提高定位系统的准确性。
图7 激光雷达点云的聚类结果

Fig. 7 Clustering result of LiDAR point cloud

1.2.4 改进的3D-NDT定位算法

3D-NDT匹配算法是一种通过概率密度函数将两帧点云进行匹配并求解变换位姿的点云匹配算法。该算法使用统计模型并通过最优化算法确定配准的最优转换参数,充分利用了点云的概率分布,鲁棒性好且实时性强23, 24。针对农业作业场景的特殊性,本研究利用第1—3节提出的聚类方法,对激光雷达扫描得到的原始点云进行聚类,保留环境中树木、灌木丛等明显特征,还原部分农业场景,再作为输入点云与先验点云地图进行匹配,提高算法的准确性。
基于先验点云地图的3D-NDT定位方法是3D-NDT匹配算法在定位技术上的应用,该定位方法将激光雷达收集到的每一帧点云与提前准备好的先验点云地图进行匹配计算求解位姿,实现自身定位。具体流程如下:将先验点云地图中的点云集合划分为多个网格,其中,某个网格中的点云集合为 X x i | x 1 , x 2 x n x i表示该集合中单个点云的三维坐标。假设该点云集合符合高斯分布,则通过公式(6)公式(7)计算三维网格中的点云对应的方差 μ与均值 Σ,并通过公式(8)构建概率密度函数 f ( x )
μ = 1 n i = 1 n x i
= 1 n - 1 i = 1 n ( x i - μ ) ( x i - μ ) T
f ( x ) = 1 ( 2 π ) 3 / 2 e x p [ - ( x - μ ) T - 1 ( x - μ ) 2 ]
不同于原始点云数据,使用概率密度函数 f ( x )表示网格中的点云信息时,通常将参考点云近似为一个局部表面,并反映该表面的平滑程度与方向信息。这样不仅增加了先验点云地图的信息丰富度,同时,使用概率密度还可降低三维数据的运算成本。
3D-NDT匹配算法的目标是通过调整位姿变换参数,使得位姿变换后的点云在网格概率密度函数中计算的概率最大,并输出此时的最佳位姿变换。在农业机器人的运动中,通常采用平移量以及欧拉角表示其位姿变换,主要包含XYZ坐标轴上的3个平移参数与3个旋转参数。该变换关系可以用位姿变换 p进行表示, p = [ t x , t y , t z , φ x , φ y , φ z ]。对激光雷达点云原始数据进行预处理和聚类,得到3D-NDT匹配算法的输入点云 Y y i   |   y 1 , y 2 , , y m,将输入点云 Y   y i   |   y 1 , y 2 , , y m按照位姿变换 p得到点云 Y '   y i '   |   y 1 ' , y 2 ' , , y m '并投影到网格中,如公式(9)~公式(12)所示。
y i ' = T ( p , y i ) = t x , y , z + R z R y R x y i
R x = 1 0 0 0 c o s φ x - s i n φ x 0 s i n φ x c o s φ x
R y = c o s φ y 0 s i n φ y 0 1 0 - s i n φ y 0 c o s φ y
R z = c o s φ z - s i n φ z 0 s i n φ z c o s φ z 0 0 0 1
式中: t x , y , z表示 y i的平移变换; R x R y R z则表示 y i的旋转变换。得到点云变换后的 y i ',通过概率密度函数 f ( x )计算出变换后的点云集合 Y i ' y i '   |   y 1 ' , y 2 ' y m '在该网格下的概率 S ( p ),如公式(13)所示。
S ( p ) = k = 1 m f ( T ( p , y i ) ) = k = 1 m 1 ( 2 π ) 3 / 2 e x p [ - ( y i ' - μ ) T - 1 ( y i ' - μ ) 2 ]
为了简化后续计算步骤,对公式(13)两边取对数得到公式(14)
l n S ( p ) = k = 1 m [ - ( y i ' - μ ) T - 1 ( y i ' - μ ) 2 ] + C
由于公式(14)中C为常数项,为使概率 S ( p )达到最大,设定优化目标如公式(15)所示
m i n k = 1 m ( y i ' - μ ) T - 1 ( y i ' - μ )
式中: y i '只与变换位姿 p相关,通过迭代优化修正变换位姿 p,获得概率 S ( p )最大或达到迭代次数时的匹配结果,即可得到当前最佳位姿变换,具体算法流程如图8所示。通过不断获取激光雷达输入的原始点云数据并得到聚类点云后,3D-NDT匹配算法可将聚类点云作为输入点云与先验点云地图进行匹配并计算出当前位姿变换情况,通过对位姿变换进行累积即可实现农业机器人的精确定位。
图8 改进3D-NDT算法流程图

Fig. 8 Flowchart of the improved 3D-NDT algorithm

图8可知,采用3D-NDT定位算法进行定位时,比较依赖于机器人的初始位姿,当初始位姿与地图难以匹配时,会造成较大累积误差。部分研究采用GPS及IMU信息作为初始位姿以减小累积误差。除此之外,对于一些复杂农业作业环境,在先验点云地图划分过程中,网格分辨率大小的设置往往难以兼顾全局,也会存在部分场景网格分辨率过小或过大的问题,从而影响点云匹配精度并产生累积误差。针对这一问题,本研究使用扩展卡尔曼滤波方法,在改进后3D-NDT定位算法的定位结果基础上,进一步融合IMU里程计信息,减小了累积误差产生的影响。

1.3 基于EKF的激光雷达与IMU融合定位

基于滤波的融合定位算法的核心思想是通过对系统的运动进行预测估计,再集合其他传感器所输出的观测值进行更新修正,实现对系统运动状态的最优估计,完成融合定位。其中,EKF使用泰勒展开将非线性系统线性化后再进行状态估计,适用于具有非线性动力学特性的农业机器人25-27。该方法将系统预测模型和观测模型等先验信息与真实观测数据相结合,获得系统状态的最优估计并不断更新状态估计和协方差矩阵,从而改进对状态的估计,并提高算法的准确性和稳定性。

1.3.1 基于EKF的最优估计流程

对非线性模型进行线性化处理之后,EKF算法的流程主要包括系统预测方程以及观测方程初始化、预测以及更新3个步骤。具体内容如下:
1)初始化。基于系统及传感器的实际工作情况,构建预测方程 f ( · )以及观测方程 h ( · ),结合系统状态量、观测量,通过预测与更新对估计结果进行修正。
2)预测。基于上一时刻的最优估计值 x ^ k - 1,利用预测方程计算当前时刻的先验估计值 x ^ k -,对当前状态量进行初步估计,并结合上一时刻的协方差矩阵 P k - 1推算当前时刻的先验协方差矩阵 P k -,为后续更新步提供计算基础。
3)更新。通过观测方程得到传感器对当前时刻的观测值 z k,并在将非线性系统线性化后,结合预测的先验协方差矩阵 P k -得到观测方程的卡尔曼增益 K k,进而得到当前时刻对应的最优估计值 x ^ k,完成基于EKF的数据融合。同时,也可以得到当前时刻的协方差矩阵 P k,以便实现对系统下一时刻的状态估计。
在上述步骤中,EKF算法充分利用了系统上一时刻的状态量及当前时刻的观测量,实现对农业机器人的精确定位。

1.3.2 基于EKF的传感器融合定位

通过激光雷达与先验点云地图的3D-NDT定位,能够实现农业机器人的初步定位。在此基础上,再使用EKF算法进一步融合IMU的里程计信息,从而弥补激光雷达定位中可能产生的定位误差,使农业移动机器人获得更高的定位精度。考虑激光雷达与IMU的采样频率差异,本研究采用IMU的状态估计作为农业机器人系统的预测部分,观测部分则使用激光雷达基于3D-NDT的定位结果来更新预测值,融合两者信息得到最优位姿。根据农业机器人的动力学模型及传感器特性,基于EKF建立对应预测方程与观测方程如公式(16)公式(17)所示。
x k = f ( x k - 1 , u k ) + w k
z k = h ( x k ) + v k
式中: x k k时刻的农业机器人位置; x k - 1 k - 1时刻的农业机器人位置的最优估计; u k为已知的系统输入量; f ( · )为非线性系统预测函数; w k为系统状态噪声; h ( · )为非线性系统观测函数; z k为激光雷达在 k时刻的定位信息; v k为系统观测噪声。根据农业机器人的实际使用情况,假设 w k v k都满足均值为0的高斯分布,则有公式(18)公式(19)
w k ~ N ( 0 , Q k )
v k ~ N ( 0 , R k )
式中: Q k R k分别对应系统状态噪声的协方差矩阵与测量噪声的协方差矩阵。对于非线性系统,对预测方程与观测方程使用一阶泰勒展开,忽略高阶项后得到线性预测方程与线性观测方程,如公式(20)公式(21)所示。
x ^ k - f ( x ^ k - 1 , u k ) + f x ( x k - 1 , x ^ k - 1 ) + w k
z k h ( x ^ k - ) + h x ( x k , x ^ k - ) + v k
式中: x ^ k -表示 k时刻位置的先验估计值; x ^ k - 1表示 k - 1时刻最优估计; f x表示预测函数的雅可比矩阵; h x表示观测方程的雅可比矩阵。在构建完成预测方程与观测方程并将其线性化后,可以通过预测步与更新步来估计最优状态。EKF算法预测步与更新步如公式(22)~公式(26)所示。
x ^ k - = f ( x ^ k - 1 , u k )
P k - = f x P k - 1 f x T + Q k
K k = P k - h x T ( h x P k - h x T + R k ) - 1
x ^ k = x ^ k - + K k ( z k - h ( x ^ k - ) )
P k = ( I - K k h x ) P k -
式中: P k -表示 k时刻协方差矩阵的先验估计值; P k - 1 k - 1时刻对应的协方差矩阵; K k k时刻的卡尔曼增益; x ^ k为EKF滤波后得到的 k时刻最优状态估计值,即农业机器人当前位置的最优估计值。公式(22)公式(23)表示EKF算法中的预测步,主要根据系统预测方程以及上一时刻的最优估计得到当前时刻的先验估计;公式(24)~公式(26)表示EKF算法中的更新步,通过观测方程以及各先验估计值完成对当前时刻各状态量的更新修正,在构建完成系统的观测方程与预测方程后,通过IMU里程计信息的预测以及激光雷达3D-NDT定位的更新,可以不断将传感器定位信息进行融合,得到更高精度的定位结果。基于EKF算法的传感器融合定位的整体流程如图9所示。
图9 EKF融合定位流程图

Fig. 9 Flowchart of the EKF location algorithm

2 结果与分析

2.1 仿真结果与分析

对本研究提出的融合定位算法进行仿真分析,在1.1节中已构建的农业仿真环境下,选择常见的转向路径和矩形路径作为轨迹,控制农业机器人以线速度0.8 m/s按两种轨迹行驶,分别通过传统3D-NDT定位方法与本研究提出的融合定位算法获取农业机器人的位置信息并与Gazebo自带的定位结果进行轨迹对比分析。结果如图10所示,其中,红色曲线代表Gazebo提供的定位信息,视为农业机器人的真实位置;蓝色曲线为3D-NDT算法定位结果;绿色曲线为本研究提出的EKF融合算法定位结果。
图10 仿真环境下农业机器人的定位轨迹

Fig. 10 Localization trajectory of agricultural robot in simulation

图10左侧整体图可知,在整个运动过程中,两种定位方法得到的定位结果与真实值之间总体相差不大,轨迹基本一致。而由图10右侧局部图可知,当农业机器人做直线运动时,两种定位方法的定位结果基本与真实值吻合,误差较小。但是,当出现转向行为时,采用3D-NDT算法得到的定位结果(蓝色)明显偏离真实定位值(红色),而EKF融合定位结果(绿色)与真实定位结果(红色)基本重合。可以看出,利用EKF算法融合两种定位信息后,对3D-NDT定位算法的一些缺陷进行了弥补,提高了定位精度。对两种算法的定位结果进行定量计算和比较,得到如图11所示的定位轨迹。
图11 仿真环境下农业机器人的定位结果

注: 农业机器人在仿真环境中的真实位置 P t r u e,传统3D-NDT算法得到的位置 P N D T,以及EKF融合定位算法得到的位置 P E K F,分别用红色、蓝色和绿色点表示。

Fig. 11 Localization result of agricultural robot in simulation environment

图11中各定位点分布情况可知,宏观上,绿色轨迹位于红色轨迹与蓝色轨迹之间,表示EKF融合定位算法所得的位置 P E K F相比于传统3D-NDT算法得到的位置 P N D T,更接近真实值 P t r u e,其主要原因在于发生转向行为时,3D-NDT算法会存在较大的定位误差;而微观上,对定位误差进一步分析,按照公式(27)公式(28)计算 P N D T以及 P E K F与真实值 P t r u e之间的误差绝对值。
e r r o r _ N D T = P N D T - P t r u e
e r r o r _ E K F = P E K F - P t r u e
考虑农业机器人在实际定位中主要依靠XY方向上的精度,因此主要对XY方向上的误差绝对值进行分析,结果如图12所示。其中,红色和紫色分别表示传统3D-NDT定位结果在XY方向上的误差绝对值;而绿色和蓝色则分别表示EKF融合定位结果在XY方向上的误差绝对值。具体误差绝对值见表1
图12 仿真环境下农业机器人的定位误差

Fig. 12 Localization error of the agricultural robot in simulation environment

表1 仿真环境下农业机器人的定位误差统计表

Table 1 Localization error statistical table of agricultural robot in simulation environment

路径 算法 X方向上的平均误差绝对值/cm Y方向上的平均误差绝对值/cm X方向上的最大误差绝对值/cm Y方向上的最大误差绝对值/cm
转向路径 3D-NDT定位算法 5.8 3.7 11.7 9.9
EKF融合定位算法 1.7 2.4 4.9 5.6
矩形路径 3D-NDT定位算法 6.2 3.2 15.3 10.0
EKF融合定位算法 1.7 1.1 4.6 4.7
综合两种工况 3D-NDT定位算法 6.0 3.5 15.3 10.0
EKF融合定位算法 1.7 1.8 4.9 5.6
算法性能提升/% 71.7 48.6
图12可以看出,在整个运动过程中,通过3D-NDT定位算法得到的位置误差绝对值波动较大,且较明显的数值波动也主要出现在转向处。这一现象与激光雷达扫描方式以及3D-NDT定位算法本身有关。在对输入点云的当前帧与上一帧进行匹配时,明显的转向行为可能会导致角度变化过大而发生畸变,使得点云匹配准确度下降,无法及时输出准确位姿并产生累积误差。另外,在激光雷达的采样频率较低的情况下,也会导致3D-NDT定位算法的误差绝对值曲线出现较大波动。通过EKF算法将3D-NDT定位算法得到的定位信息与IMU的里程计信息进行融合,通过对位姿进行修正可以减小该累积误差,减小误差绝对值曲线的波动,提高整个系统的定位准确性。
综合两种工况,传统3D-NDT定位算法在XY方向上的平均定位误差分别为6.0和3.5 cm;XY方向上的最大误差绝对值分别为15.3和10.0 cm。在此基础上,本研究提出的算法通过提前进行点云聚类提高了3D-NDT算法的匹配准确度,并使用EKF算法融合IMU里程计信息对位姿进行最优估计,在XY方向上的平均定位误差分别为1.7和1.8 cm;XY方向上的最大误差绝对值分别为4.9和5.6 cm。满足农业机器人作业需求的同时,提高了作业时的定位精度。

2.2 实验结果与分析

为验证提出的融合定位算法的实际工作性能,使用自主研发的农业机器人在如图13所示的环境中进行实验。
图13 农业机器人定位实验环境

Fig. 13 The environment of agricultural robot localization experiment

首先通过遥控器控制农业机器人在该区域运动,并利用激光雷达扫描和采集所有环境点云数据,然后通过SC-Lego-LOAM算法进行建图,生成先验点云地图,如图14所示。
图14 农业机器人定位实验环境的先验点云地图

Fig. 14 Prior point cloud map for environment of agricultural robot localization experiment

参考仿真软件中设置的转向路径与矩形路径,通过遥控器手动控制农业机器人以线速度0.8 m/s分别沿上述两种路径运动,输出的定位结果如图15所示,其中红色曲线为RTK获得的真实定位轨迹;蓝色曲线为3D-NDT算法定位轨迹;绿色曲线为本研究提出的EKF融合算法定位轨迹。
图15 实验环境下农业机器人的定位轨迹

Fig. 15 Localization trajectory of agricultural robot in experimental environment

图15可知,实验结果与仿真结果类似,相比于3D-NDT定位结果,EKF融合定位结果更接近真实值,且在农业机器人转向时,3D-NDT定位结果仍然会因为激光雷达扫描方式存在一定误差。对3D-NDT定位算法与EKF融合定位算法获得的两种定位结果进行定量分析,如图16所示。
图16 实验环境下农业机器人的定位结果

Fig. 16 Localization result of the agricultural robot in experimental environment

图16可知,相比于仿真环境,在真实环境中3D-NDT定位结果会得到一定程度的改善,其主要原因是真实环境中的各种线、面特征会更加明显与密集,可以提高点云匹配时的匹配精度。但3D-NDT定位方法在转向处仍然存在一定偏差,而在融合了IMU的里程计信息后可以有效改善定位精度,如图16所示,EKF融合定位结果(绿色)会更加接近由RTK提供的真实定位结果(红色)。
为了进一步分析定位误差,按照公式(27)公式(28)计算误差绝对值进行定量分析,结果如图17表2所示。
图17 实验环境下农业机器人的定位误差

Fig. 17 Localization error of agricultural robot in experimental environment

表2 实验环境下农业机器人的定位误差统计表

Table 2 Localization error statistical table of agricultural robot in experimental environment

路径 算法 X方向上的平均误差绝对值/cm Y方向上的平均误差绝对值/cm X方向上的最大误差绝对值/cm Y方向上的最大误差绝对值/cm
转向路径 3D-NDT定位算法 6.7 6.0 11.7 11.1
EKF融合定位算法 3.5 3.9 7.5 10.5
矩形路径 3D-NDT定位算法 7.3 5.0 16.8 13.2
EKF融合定位算法 3.0 2.6 8.4 8.7
综合两种工况 3D-NDT定位算法 7.0 5.5 16.8 13.2
EKF融合定位算法 3.3 3.3 8.4 10.5
算法性能提升/% 52.9 40.0
图17表2中数据分析可知,在实验时定位结果的误差波动多出现在转向处,这一现象与仿真结果相同,主要是由激光雷达硬件结构以及3D-NDT定位算法原理引起。虽然相较于仿真环境,真实场景下的特征会更多,有利于提高点云匹配的精度,但同时,在真实场景下路面会存在一定颠簸,也会导致误差绝对值曲线出现较大震荡。在本研究所采用的两种工况下,若仅使用激光雷达进行基于3D-NDT定位,在XY方向上的平均定位误差分别为7.0和5.5 cm;XY方向上的最大误差绝对值分别为16.8和13.2 cm,已经不能很好地满足农业机器人的定位需求。而本研究提出的融合定位算法在XY方向上的平均定位误差分别为3.3和3.3 cm;XY方向上的最大误差绝对值分别为8.4和10.5 cm。相较于传统3D-NDT定位算法,在X方向上定位精度提高了52.9%,在Y方向上提高了40.0%,并且降低了运动过程中的最大误差绝对值,能够有效提高激光雷达在农业环境中定位的精度。

3 结 论

针对智慧农业果园场景中GNSS信号较弱,可能导致定位精度下降这一问题,提出一种基于激光雷达与IMU融合的定位算法,通过仿真与实验对算法进行了验证与分析,主要结论如下。
1)本研究使用扩展卡尔曼滤波算法,在改进3D-NDT定位算法的基础上,进一步融合IMU里程计信息,实现农业机器人的高精度定位。
2)实验结果表明,与传统3D-NDT定位算法相比,提出的融合定位方法的定位精度在XY方向上分别提高52.9%和40.0%,且降低了运动过程中的最大误差,减小误差曲线波动的同时获得了更高的定位精度。
3)由于3D-NDT定位算法依赖于先验点云地图的准确性,且当点云角度变化较大时,定位精度会显著下降。
后续将继续优化所提出的融合定位算法,减少对于先验点云地图的过度依赖并结合其他定位方法,进一步提高农业机器人的定位精度。

利益冲突声明

本研究不存在研究者以及与公开研究成果有关的利益冲突。

1
YIN X, WANG Y X, CHEN Y L, et al. Development of autonomous navigation controller for agricultural vehicles[J]. International journal of agricultural and biological engineering, 2020, 13(4): 70-76.

2
柳景斌, 毛井锋, 吕海霞, 等. BDS/GPS组合定位可靠性分析与粗差探测研究[J]. 武汉大学学报(信息科学版), 2023, 48(2): 214-223.

LIU J B, MAO J F, LYU H X, et al. Reliability analysis and gross error detection of BDS/GPS combined positioning[J]. Geomatics and information science of Wuhan university, 2023, 48(2): 214-223.

3
YOUSUF S, KADRI M B. Information fusion of GPS, INS and odometer sensors for improving localization accuracy of mobile robots in indoor and outdoor applications[J]. Robotica, 2021, 39(2): 250-276.

4
THANPATTRANON P, AHAMED T, TAKIGAWA T. Navigation of autonomous tractor for orchards and plantations using a laser range finder: Automatic control of trailer position with tractor[J]. Biosystems engineering, 2016, 147: 90-103.

5
BLOK P M, VAN BOHEEMEN K, VAN EVERT F K, et al. Robot navigation in orchards with localization based on Particle filter and Kalman filter[J]. Computers and electronics in agriculture, 2019, 157(C): 261-269.

6
UNDERWOOD J P, JAGBRANT G, NIETO J I, et al. LiDAR-based tree recognition and platform localization in orchards[J]. Journal of field robotics, 2015, 32(8): 1056-1074.

7
BESL P J, MCKAY N D. Method for registration of 3-D shapes[C]// Proc SPIE 1611, sensor fusion IV: Control paradigms and data structures. Burlingame, California, USA: SPIE, 1992, 1611: 586-606.

8
BIBER P, STRASSER W. The normal distributions transform: a new approach to laser scan matching[C]// Proceedings 2003 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2003) (Cat. No.03CH37453. Piscataway, JerseyNew, USA: IEEE, 2003, 3: 2743-2748.

9
SAARINEN J, ANDREASSON H, STOYANOV T, et al. Normal Distributions Transform Occupancy Maps: Application to large-scale online 3D mapping[C]// 2013 IEEE International Conference on Robotics and Automation. Piscataway, New Jersey, USA: IEEE, 2013: 2233-2238.

10
HU F J, REN T J, SHI S B. Discrete point cloud registration using the 3D normal distribution transformation based newton iteration[J]. Journal of multimedia, 2014, 9(7): ID 934.

11
WANG Q S, ZHANG J. Point cloud registration algorithm based on combination of NDT and PLICP[C]// 2019 15th International Conference on Computational Intelligence and Security (CIS). Piscataway, New Jersey, USA: IEEE, 2019

12
AKAI N, MORALES L Y, TAKEUCHI E, et al. Robust localization using 3D NDT scan matching with experimentally determined uncertainty and road marker matching[C]// 2017 IEEE Intelligent Vehicles Symposium (IV). Piscataway, New Jersey, USA: IEEE, 2017: 1356-1363.

13
KALLASI F, RIZZINI D L, CASELLI S. Fast keypoint features from laser scanner for robot localization and mapping[J]. IEEE robotics and automation letters, 2016, 1(1): 176-183.

14
金泰宇, 黄劲松. 一种基于语义地图的激光雷达定位方法[J]. 测绘地理信息, 2022, 47(4): 28-32.

JIN T Y, HUANG J S. A LiDAR positioning methodology based on semantic map[J]. Journal of geomatics, 2022, 47(4): 28-32.

15
LEVINSON J, THRUN S. Robust vehicle localization in urban environments using probabilistic maps[C]// 2010 IEEE International Conference on Robotics and Automation. Piscataway, New Jersey, USA: IEEE, 2010: 4372-4378.

16
HATA A, WOLF D. Road marking detection using LiDAR reflective intensity data and its application to vehicle localization[C]// 17th International IEEE Conference on Intelligent Transportation Systems (ITSC). Piscataway, New Jersey, USA: IEEE, 2014: 584-589.

17
WAN G W, YANG X L, CAI R L, et al. Robust and precise vehicle localization based on multi-sensor fusion in diverse city scenes[C]// 2018 IEEE International Conference on Robotics and Automation (ICRA). Piscataway, New Jersey, USA: IEEE, 2018: 4670-4677.

18
邓泽武, 胡钊政, 周哲, 等. 融合激光雷达与双层地图模型的智能车定位[J]. 汽车工程, 2022, 44(7): 1018-1026.

DENG Z W, HU Z Z, ZHOU Z, et al. Intelligent vehicle positioning by fusing LiDAR and double-layer map model[J]. Automotive engineering, 2022, 44(7): 1018-1026.

19
崔文, 薛棋文, 李庆玲, 等. 基于三维点云地图和ESKF的无人车融合定位方法[J]. 工矿自动化, 2022, 48(9): 116-122.

CUI W, XUE Q W, LI Q L, et al. Unmanned vehicle fusion positioning method based on 3D point cloud map and ESKF[J]. Journal of mine automation, 2022, 48(9): 116-122.

20
张文玥, 娄小平, 陈福笛. 移动机器人多传感器融合定位仿真研究[J]. 计算机仿真, 2023, 40(3): 436-441.

ZHANG W Y, LOU X P, CHEN F D. Research on multi-sensor fusion localization simulation of mobile robot[J]. Computer simulation, 2023, 40(3): 436-441.

21
涂远泯, 刘飞飞, 曾波华, 等. 基于多传感器融合技术的移动机器人位姿估计方法研究[J]. 制造业自动化, 2023, 45(11): 137-141.

TU Y M, LIU F F, ZENG B H, et al. Research on pose estimation method of mobile robot based on multi-sensor fusion technology[J]. Manufacturing automation, 2023, 45(11): 137-141.

22
BOGOSLAVSKYI I, STACHNISS C. Fast range image-based segmentation of sparse 3D laser scans for online operation[C]// 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). New York, USA: ACM, 2016.

23
杨奇峰, 曲道奎, 徐方. 基于3D-NDT的移动机器人定位算法研究[J]. 控制工程, 2020, 27(4): 613-619.

YANG Q F, QU D K, XU F. Localization algorithm of outdoor mobile robot based on 3D-NDT[J]. Control engineering of China, 2020, 27(4): 613-619.

24
MILIOTO A, VIZZO I, BEHLEY J, et al. RangeNet ++: Fast and accurate LiDAR semantic segmentation[C]// 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). New York, USA: ACM, 2019: 4213-4220.

25
KALMAN R E. A new approach to linear filtering and prediction problems[J]. Journal of basic engineering, 1960, 82(1): 35-45.

26
GALATI R, REINA G, MESSINA A, et al. Survey and navigation in agricultural environments using robotic technologies[C]// 2017 14th IEEE International Conference on Advanced Video and Signal Based Surveillance (AVSS). Piscataway, New Jersey, USA: IEEE, 2017: 1-6.

27
YAN Y X, ZHANG B H, ZHOU J, et al. Real-time localization and mapping utilizing multi-sensor fusion and visual-IMU-wheel odometry for agricultural robots in unstructured, dynamic and GPS-denied greenhouse environments[J]. Agronomy, 2022, 12(8): ID 1740.

文章导航

/