基于改进ORB-SLAM2的果园喷药机器人定位与稠密建图算法.pdf

doi:10.6041-1298.2023.07.0052023年月第54卷第7 期农报业学机械基于改进ORB-SLAM2的果园喷药机器人定位与稠密建图算法丛佩超崔利营万现全李佳星刘俊杰张欣（广西科技大学机械与汽车工程学院，柳州5456 16）摘要：针对果园喷药机器人视觉导航过程中定位精度低、地图构建效果差等问题，本文提出一种新的视觉定位与稠密建图算法。该算法基于ORB-SLAM2算法架构，首先，通过优化FAST角点、描述子阈值，并采取图像金字塔法与高斯滤波算法，剔除劣质ORB特征点，以提升图像关键顿质量和特征匹配精度。其次，引入稠密建图线程，利用点云恢复算法、统计滤波方法形成点云队列，采取点云拼接技术与体素滤波算法输出稠密点云地图，并在ORB-SLAM2算法的ROS节点中增加关键顿输出接口与位姿发布话题，通过NeedNewKeyFrame函数选取ORB-SLAM2算法所生成的关键顿，减少系统计算量。最终，由RGB-D相机实现果园喷药机器人的精准定位与稠密建图。为验证本文算法的有效性与实用性，进行TUM数据集仿真分析与真实场景测试，结果表明：相较ORB-SLAM2算法，本文算法的绝对轨迹平均误差降低44.0 1%、相对轨迹平均误差降低7.93%，0 RB特征点匹配数量平均提升19.03%，定位精度与运行轨迹效果均有显著提升，此外，还能获取较高精度的果园喷药机器人工作场景信息。本文算法可为果园喷药机器人的自主导航提供理论基础。关键词：果园；喷药机器人；ORBSLA M 2；高斯滤波；精准定位；稠密建图中图分类号：TP242.6文献标识码：A文章编号：10 0 0-12 98（2 0 2 3）0 7-0 0 45-11OSID:口Localization and Dense Mapping Algorithm for Orchard Spraying RobotBased on Improved ORB-SLAM2CONG PeichaoCUI LiyingWANXianquanLI JiaxingLIU JunjieZHANG Xin(School of Mechanical and Automotive Engineering,Guangxi University of Science and Technology,Liuzhou 545616,China)Abstract:In view of low localization accuracy and poor map construction during the visual navigation fororchard spraying robot,a visual localization and dense mapping algorithm was proposed.The algorithmwas based on the ORB-SLAM2 algorithm architecture,firstly,through the optimization of FAST cornerpoints,descriptor thresholds,and adopting the image pyramid method and Gaussian filtering algorithm,poor quality ORB feature points were eliminated to improve the image key frame quality and featurematching accuracy.Secondly,the dense map building thread was introduced,the point cloud recoveryalgorithm and statistical filtering method were used to form the point cloud queue,the point cloudstitching technology and voxel filtering algorithm were adopted to output the dense point cloud maps,andthe key frame output interface and position publishing topic were added in the ROS node of ORB-SLAM2 algorithm,and then the key frame generated by ORB-SLAM2 algorithm was selected through theNeedNewKeyFrame function to reduce the system computation.Finally,the RGB-D camera was used torealize the precise positioning and dense mapping of the orchard spraying robot.In order to verify theeffectiveness and practicality of the algorithm,simulation analysis of TUM dataset and real scenariotesting were conducted.The results showed that compared with that of ORB-SLAM2 algorithm,theabsolute trajectory average error of this algorithm was reduced by 44.01%,the relative trajectory averageerror was reduced by 7.93%,the average number of ORB feature point matching was increased by19.03%,and the positioning accuracy and running trajectory effect were improved significantly.Inaddition,the working scene information of orchard spraying robot can be obtained with high accuracy.The algorithm can provide a theoretical basis for the autonomous navigation of orchard spraying robot.Key words:orchard;spraying robot;ORB-SLAM2;Gaussian filtering;precise positioning;densemapping收稿日期：2 0 2 2-11-0 22修回日期：2 0 2 3-0 1-2 0基金项目：中央引导地方科技发展专项资金项目（桂科ZY19183003）和广西重点研发计划项目（桂科AB20058001）作者简介：丛佩超（198 0 一），男，副教授，博士，主要从事移动机器人自主导航定位研究，E-mail：c p c l z x 16 3.c o m46农2023年机业报学械0引言当前，我国水果品类丰富、产量稳居世界第一，但其种植区域多分布在丘陵地区。受地形条件影响，果园的农药喷施还以人工操作为主，该方式存在成本高、效率低、危害身体健康、环境污染大等问题。为解决上述问题，用于除草、喷药、采摘等工作的果园机器人不断涌现，其相关技术的研究已成为机器人领域的热点问题【1-2 果园喷药机器人是将环境感知、目标识别、导航定位和运动控制等技术高度集成的智能化系统，精准定位技术是其中的关键问题之一。目前广泛使用的定位方法有：全球导航卫星系统（Clobal navigationsatellite system，G NSS）、激光雷达定位、视觉定位3-4。室外场景下CNSS 的应用最为广泛，其无遮挡情况下的定位精度可达厘米级5-7 。虽然卫星定位在室外空旷环境下的表现良好，但在枝叶茂密的果园环境中，遮挡造成的卫星信号丢失，会严重影响定位精度3。激光雷达定位精度高、抗干扰能力强“-。激光雷达虽能获得较高精度的位置信息，但在运动过程中易产生点云畸变和误匹配，且多线激光雷达价格较高10 1。视觉定位通过单目、双目和RGB-D相机不仅能够获得丰富的位置信息，而且其硬件成本低廉，功耗低、体积小、安装方便，适用于大范围的工作场景，被广泛地应用于移动机器人的导航问题。文献12-14 研究的视觉定位技术通常先由相机获取环境的目标信息，再通过图像分割等方法对目标图像进行处理，并利用Hough变换拟合导航路线，提高导航的定位精度。然而，果园喷药机器人的作业环境复杂多样，茂密的枝叶易导致图像分割失败，继而影响果园喷药机器人导航过程中的定位精度。视觉同时定位与建图（Simultaneouslocalizationandmapping，SLA M）技术可较好地解决复杂场景下的定位与建图问题，已成为当前农业机器人领域的研究热点今为止，比较经典的视觉SLAM算法有MonoSLAML15、PT A M 16 、SVO 17 、O RB-SLA M 18 等。其中，MUR-ARTAL研究团队18 提出的ORB-SLAM系统，利用单目相机完成了全自动初始化，其在室内、外环境下均可实时运行，且具有较好的鲁棒性，与当时其它单目SLAM算法相比，实现了前所未有的性能。之后，该团队19 又提出了一种基于特征点法的ORBSLA M 2 算法，它主要由跟踪、局部建图、回环检测三大线程组成，在大场景下实现了移动机器人的高精度定位与稀疏点云地图构建。李盛辉等2 0 提出了一种基于全景相机的视觉SLAM算法，该方法可应用于真实农业环境下的智能车辆自主导航，其定位平均误差0.10 8 m。李月华等2 1 提出了一种基于人工信标修正的视觉SLAM算法，该算法利用双目相机实现了工厂环境下自动引导车的自主定位，其定位误差不超过0.1m。黎达等2 2 提出了一种双目视觉SLAM算法，利用地面、树木等周围环境的纹理信息，可实时确定相机传感器的空间位置和姿态,其均方根误差不超过0.9m。董蕊芳等2 3提出了一种改进型的ORB-SLAM2算法，通过融合环境的3D、2 D 占据信息、路标点空间位置以及视觉特征等信息，实现了轮式机器人在森林公园内的准确定位，其定位误差不超过1.43m。上述视觉SLAM方法在农、林等复杂环境下的定位虽然取得了一定的效果，但相应的精度还有很大提升空间，特别是针对稠密果园环境的视觉定位和建图方法仍然比较欠缺2 4。因此,研究适用于果园喷药机器人的定位与稠密建图方法，具有重要的现实意义2 5。基于以上分析，为进一步改善果园喷药应用场景下机器人定位与建图能力，本文提出一种改进的ORBSLA M 2 定位与稠密建图算法，以ORB-SLAM2为基础，围绕图像像素亮度的差异性，引入噪声抑制机制与高斯滤波，优化ORB特征点的提取过程；借助局部建图线程的优质关键顿，运用点云滤波处理与位姿处理技术，构建稠密建图线程。1算法框架及视觉SLAM理论基础1.1算法框架ORBSLA M 2 算法是基于特征点法的实时SLAM开源算法，但其构建的三维稀疏点云地图仅保留图像的部分ORB特征点，且建立的地图表征不够充分，缺少占据信息，难以描绘地图中的障碍物，因此，ORB-SLAM2算法仅适用于机器人的空间定位2 3。此外，果园环境复杂多样、纹理缺失，也易造成果园农药喷洒机器人的视觉定位精度不足，无法满足路径规划等任务的需求2 6 。针对上述问题，本文提出基于改进ORB-SLAM2算法的果园喷药机器人定位与稠密建图算法，如图1所示。该算法基于ORB-SLAM2算法框架，保留原有的局部建图与回环检测线程19，在跟踪线程中优化FAST角点、描述子阈值，利用高斯滤波对图像进行处理，并采用图像下采样方法形成高斯图像金字塔，实现ORB特征提取过程的优化，以降低劣质特征点数量，提高算法的关键帧质量及特征匹配准确率，从而改善算法的定位精度。此外，本文算法增加稠密建图线程,将相机周围场景以三维稠密点云地图的形式呈现，实现稠密地图的构建功能，以备果园喷药机器人后续47丛佩超等：基于改进ORB-SLAM2的果园喷药机器位与稠密建图算法第7 期跟踪线程优化ORB特征提取新关优化FAST地图关键顿跟踪相机图像高斯图像键顿角点、描述初始化重定位跟踪下采样计算金字塔决策子阅值稠密建图线程局部建图线程优质关键顿点云滤波点云位姿处理关键顿插人选择关键顿插人处理地图更新构建稠密地图地图点决策选择关键顿回环检测线程全局三维稠密局部BA优化点云地图回环检测回环检测回环校正BA图1基于改进ORB-SLAM2算法的视觉定位与稠密建图算法框架Fig.1Visual localization and dense mapping algorithm framework based on improved ORB-SLAM2 algorithm自主导航。1.2视觉SLAM理论基础为实现果园喷药机器人的高精度定位与三维稠密地图构建，需有丰富的图像信息作为保证。基于经济性考虑，本文采用AstraRGB-D相机。果园喷药机器人通过RCB-D相机获取环境信息，如图2所示，机器人在posel、p o s e 2、p o s e N处观测果园环境中路标点yi、y 2、Yn。通过视觉SLAM算法处理图像数据，根据相机的运动方程、观测方程估计相机位姿，再借助坐标系转换关系确定机器人的位置与姿态，以实现其自身的精准定位2 7 posepose2posel图2果园场景下的相机运动状态Fig.2Camera motion state in orchard sceneRGB一D相机的运动方程为x,=f(xt-1,P,q.)(1)其观测方程为zt.;=h(yi,x,w.)(2)式中相机t时刻位置Pi测量t时刻数据q测量t时刻噪声W.观测t时刻路标点噪声相机观测到的路标点相机在x，处对路标点y，的观测数据AstraRGB-D相机由RGB彩色相机、红外传感器和接近传感器3部分组成，结构如图3所示。该相机通过搭载的光学相机和红外相机获取RGB图像，利用红外结构光原理获取目标图像对应的深度信息，为三维稠密地图重建提供丰富的三维场景点云数据，再由相机点云坐标系转换为全局点云坐标系，最终实现环境信息重建（2 8 1234图3AstraRGB-D结构Fig.3AstraRGB-D structure14.红外传感器2.接近传感器3.RGB彩色相机1、4.红外传感器2.接近传感器3.RGB彩色相机点云坐标系变换关系式为Woi=T.W(3)CCij式中T.ci第i个关键帧位姿W第i个关键顿的坐标系点云W全局坐标系点云2同时定位与三维稠密地图构建2.1ORB特征点提取优化ORB特征由OrientedFAST角点、BRIEF描述子组成，其不仅计算速率和精度较高，还具备旋转、光照和尺度的不变性，以及匹配速度快、实时性高等特点。因此，ORB特征点提取在算法中起着至关重要的作用，其结果将直接影响定位的精度及建图效果2 9。但ORBSLA M 2 提取的特征点分布不均匀，且输出特征点重叠较多、质量较差。为解决该问题，本文提出一种如图4所示的特征点提取算法，其主要由5部分组成：FAST角点提取、BRIEF描述子计算、旋转角度计算、图像金字塔层数优化、高斯处理。2023年农48报机业学械FASTBRIEF描旋转角度图像金字塔高斯角点述子计算计算层数优化处理提取图4特征点提取流程图Fig.4Flowchart of feature point extraction本文算法根据像素亮度不同进行FAST角点提取与描述子计算，引人噪声抑制机制，以优化ORB特征点的处理流程，如图5所示。首先，在图像中选取像素块P，假设其像素亮度为Lp，并设置亮度阈值为A。以P为中心，在半径为r的圆上选取16 个像素点，若存在连续12 个像素点的亮度不在区间Lp-A。,Lp+A。之内，则像素块P可列为优质特征点。对图中所有像素均重复以上步骤，筛选出图像中所有的优质特征点。其次，利用OpenCV库中的imread函数检测出OrientedFAST角点位置，依据角点位置计算BRIEF描述子，设置其提取数量阈值范围为A,，A,，并在描述子阈值范围内选取关键点，且剔除范围以外的关键点，以提高SLAM系统特征点的质量。最后，采用OpenCV库中的BruteForceMatcher函数结合汉明距离对2 幅图像的描述子进行匹配，计算相邻点之间的极小值距离，若描述子间汉明距离大于2 倍极小值，判断其为误匹配。此时，运用随机采样一致性算法减少误匹配，抑制噪声，剔除外点，以提高系统定位精度。图5ORB特征点提取Fig.5ORB feature point extractionORB一SLAM2算法构建图像金字塔是对原图像进行多尺度结构表达。首先，按照图像比例因子进行逐级向下采样；其次，采样后的图像按不同分辨率由高到低向上排列，进行不同层的角点匹配，从而解决尺度不变性问题。但当描述子数量较少时，易出现特征点提取与匹配精度下降的问题30 。为解决上述问题，通过优化图像金字塔层级，对图像进行高斯滤波，利用OpenCV的pyrDown函数形成高斯图像金字塔，如图6 所示。高斯图像金字塔从第K层生成第K+1层，利用高斯滤波对第K层进行卷积，删除所有的偶数行与列，具体计算过程为Fk=pyrDown(Fk+1)(4)式中Fk/Fk+1第K、K+1次下采样图像pyrDown-下采样函数第3层第K+2层第2 层第K+1层第1层第K层第0 屋第K-1层图6高斯图像金字塔Fig.6Gaussian image pyramid所形成的高斯图像金字塔可用于构建图像的尺度，避免引人其它噪声，从而提高本文算法的特征点提取与匹配精度2.2三维稠密地图构建针对ORBSLA M 2 所建立地图的表征不够充分、缺少占据信息问题，本文在ORB-SLAM2架构基础上添加稠密地图构建线程，如图1、7 所示，该线程以局部建图线程的优质关键帧为基础，构建相机周围果园环境的三维稠密点云地图。首先，基于RCB-D相机的成像原理，如图8 所示，运用点云恢复算法、统计滤波方法，将输人关键顿的2 D像素坐标转换为3D点云坐标，从而形成像素点云队列31其计算过程为,x=(u-q)z/pxy=(u-q,)z/p,(5)Lz=d/n式中PxPy水平与垂直方向的相机焦距l9,像素坐标与成像平面的平移量（x,y,z）一一图像坐标系坐标(u,u）一一图像坐标d一一深度相机测量出的像素点距离n实际距离和测得距离d的比例系数点云恢复点云滤波处理优质体素滤波局部关键统计点云融合点云三维地图点云顿插滤波队列拼接点云更新人地图位姿处理相机位姿全局三维稠密位姿处理位姿队列点云地图L图7稠密建图流程图Fig.7Dense build flowchart同时，运用位姿处理技术，将相机位姿生成位姿队列。其次，点云队列与位姿队列采取点云融合拼接算法，进行点云地图拼接，完成新旧点云地图叠加，点云拼接公式为=R+t(6)WCLLZ式中R33旋转矩阵wCt-31平移向量49SLAM2的果园喷药机器位与稠密建图算法丛佩超等：基于改进ORB第7 期现实场景中的点RGB图像P(x,y,z)相机的局部三维坐标系RGB图像中的对应点YAXP(u,u)相机位置Z焦距图像中心点及其坐标系深度图：钅针孔相机成像原理图Fig.8Imaging principle of pinhole camera由于高采样频率、运动速度的约束，相邻关键顿之间存在大量的余信息，导致系统运算速度较慢，本文通过体素滤波处理掉该穴余信息，以获得新的局部点云地图。最后，进行点云地图更新，输出三维稠密点云地图，并将稠密建图部分编写成独立的pointcloud_mapping功能包。生成的稠密点云地图可通过Octomap_Server转换成八叉树地图，为果园喷药机器人在复杂场景下的自主导航提供环境地图 31 3稠密建图线程若单独运行，系统需重新进行特征点提取与匹配等工作，会产生大量重复计算。为减少稠密建图计算量，本文基于ORB-SLAM2系统的ROS节点进行改进。ORB-SLAM2系统主要分为3个节点：相机驱动节点、位姿估计节点、建图节点，如图9所示。相机驱动节点采集视觉传感器数据，位姿估计节点提供相机位姿（TCW），建图节点接收图像数据和位姿数据，进行点云拼接34。本文在ORB-SLAM2的建图节点中增加KeyFrame输出接口与位姿发布话题，通过TrackRGBD接口函数调用KeyFrameDataBase库，逐层添加关键顿状态标志位，利用Tracking：T r a c k 函数计算位姿，NeedNewKeyFrame函数选取ORBSLA M 2 生成的优质关键帧，作为稠密建图线程的输入，从而减少处理帧数，节约运算资源，保证系统的实时性，关键帧选取过程如图10 所示相机相机驱动RCB图像位姿估计节点图像节点深度图像点云地图构建建图节点TCW图9基于ROS的ORB-SLAM2系统节点框架Fig.9Nodeframework of ORB-SLAM2systembased on ROS3仿真与实验为验证本文算法的有效性与实用性，分别进行数据集仿真与真实场景实验。仿真分析选用自带标KeyFrame1KeyFrame2连续关键顿KeyFrame3图10关键顿选取示意图Fig.10KeyFrame selection准轨迹与比较工具的TUM数据集，将本文算法与ORB一SLAM2进行对比，验证其定位精度与稠密建图效果。果园真实场景选取广西壮族自治区柳州市某处柑橘种植场地，利用果园喷药机器人实体样机的移动底盘进行实物验证。上述仿真、真实场景实验的计算机采用Intelcorei7-6700HQ处理器、32GB运行内存的ThinkPadT460P型便携式计算机,其操作系统为Ubuntu18.04。3.1数据集仿真分析TUM数据集为Kinect视觉传感器采集的标准数据集，该数据集中包含彩色图、深度图及相机运动过程中的真实轨迹（groundtruth）2 3。仿真分析利用TUM数据集的freiburgl_room序列进行验证，该序列具有场景大、运动回环等特点，适于评估SLAM系统的位姿估计与回环检测能力。基于上述序列分别测试ORB-SLAM2及本文所提算法，其运行效果如图11所示。由图11可知，本文算法相较ORB-SLAM2可实时地构建三维稠密点云地图。(a)ORB-SLAM2算法(b)本文算法图11运行效果对比Fig.11Running effect comparison diagrams借助测评工具EVO对图11得到的运行结果进行处理，以评估上述两种SLAM算法的定位精度。首先，将两种SLAM算法运行后的估计位姿KeyFrameTrajectory与数据集真实位姿（g r o u n d t r u t h）进行对比，结果如图12 所示；其次，分别获取两种SLAM算法对应的绝对轨迹误差（APE，图13）、相对轨迹误差（RPE，图14）；最后，运行两种算法各15次，求取相应的绝对轨迹误差与相对轨迹误差平均值，结果如表1、2 所示。其中，采用最大误差（max）、平均误差（mean）、误差中值（median）、最小误差（min）、均方根误差（RMSE）、方差（SSE）与标准差（STD）作为算法的评价指标。50农2023年机业报学械真实轨迹真实轨迹真实轨迹-ORB-SLAM2KeyFrame轨迹ORB-SLAM2KeyFrame轨迹ORB-SLAM2KeyFrame轨迹本文算法KeyFrame轨迹2本文算法KeyFrame轨迹本文算法KeyFrame轨迹1.70.51.6三1三1.50.0.511.4-1.01.3-11.2102030405010203040501020304050时间/s时间/s时间s真实轨迹真实轨迹真实轨迹-ORB-SLAM2KeyFrame轨迹ORB-SLAM2KeyFrame轨迹-ORB-SLAM2KeyFrame轨迹一本文算法KeyFrame轨迹40本文算法KeyFrame轨迹本文算法KeyFrame轨迹（。）/电(。/电山10(。)/（f-6050100-800-100-120-5-100-140-10102030405010203040501020304050时间/s时间/s时间/s图12真实位姿与估计位姿对比Fig.12Comparison of real pose and estimated pose绝对轨迹误差0.175平均误差误差中值0.150均方根误差标准差0.1250.1000.0750.0500.0250010203040t/s(a)ORB-SLAM2算法0.12绝对轨迹误差平均误差0.10误差中值均方根误差标准差0.08w/dy0.060.040.020010203040t/s(b)本文算法图13绝对轨迹误差对比Fig.13Absolute trajectory error comparison charts0.175绝对轨迹误差0.150平均误差误差中值0.125均方根误差标准差0.1000.0750.0500.0250010203040t/s(a)ORB-SLAM2算法0.16绝对轨迹误差0.14平均误差误差中值0.12均方根误差标准差0.100.080.060.040.020010203040t/s(b)本文算法图14相对轨迹误差对比Fig.14Relative trajectory error comparison charts表1丝绝对轨迹误差平均值Tab.1Average absolute trajectory error算法最大值/m平均误差/m误差中值/m最小值/m均方根误差/m方差/m标准差/mORB-SLAM20.192.7750.075 6660.0673750.006 9010.0853301.4489590.039 445本文算法0.109 1060.043 6120.042 3620.009 8750.048 4190.489 9760.021 032表2相对轨迹误差平均值Tab.2Average relative trajectory error算法最大值/m平均误差/m误差中值/m最小值/m均方根误差/m方差/m2标准差/mORB-SLAM20.1696100.0205700.014 1960.001 0990.0293920.1710520.020995本文算法0.1560940.018 9380.014 1920.001 5720.026 4850.145 9040.018 51551丛佩超等：基于改进ORBSLAM2的果园喷药机器人定位与稠密建图算法第7 期图12 表明：本文算法相比ORB-SLAM2算法的估计位姿更接近真实位姿。观察图13、14可知，本文算法相较ORB-SLAM2的单误差波动、绝对和相对轨迹误差范围更小；此外，分析表1、2 可知，本文算法的绝对和相对轨迹误差平均值均有所下降。其中，绝对轨迹平均误差0.0 436 12 m，降低44.01%；均方根误差0.0 48 419m，降低43.2 6%；标准差0.0 2 10 32 m，降低46.6 8%。相对轨迹平均误差0.0 18 938 m，降低7.93%；均方根误差0.0 2 6 48 5m，降低9.8 9%；标准差0.0 18 515m，降低11.8 1%。数据集仿真结果表明：本文改进算法的估计位姿更接近真实位姿，定位精度也较ORBSLA M 2 算法有明显提升。3.2果园喷药机器人定位与建图实验为进一步验证本文算法在真实场景下的定位与稠密建图效果,以柑橘果园为应用场景，选取36 m16m的测试范围，如图15所示。将ORBSLA M 2及本文算法分别在果园喷药机器人实体样机的移动底盘上加以测试。移动底盘搭载AstraRGB-D相机，主控为ThinkPadT460P型便携式计算机，如图16 所示。3.2.1算法性能测试在果园内的不同工况下进行多次实验，根据特征点匹配数量、算法初始化时间、关键顿匹配数量测试算法性能。图15真实果园场景Fig.15Real of orchard scenery3图16实验平台Fig.16Experimental platform1.ThinkPadT460P型便携式计算机2.Astra RCB-D相机3.移动底盘两种算法在运行过程中，通过调整机器人位置改变相机在环境中的位姿，每种算法进行2 0 次特征点匹配实验，实验过程及结果如图17、18 所示。实验结果表明，在相同场景下本文算法匹配到的特征点数量普遍高于ORB-SLAM2算法，特征点数量平均提升19.0 3%图17特征点匹配实验（部分）Fig.17Feature point matching experiment(partial)52农2023年机报学业械500-ORB-SLAM2算法450一本文算法40035030025020002468101214161820实验序号图18特征点匹配数量结果Fig.18Number of feature point matching results在晴天与阴雨天两种工况下对同一场景进行30次初始化时间与关键顿匹配数量测试，分别计算其平均值，结果如表3、4所示。实验表明，本文算法30次初始化时间的均值为15.19s。在晴朗天气条件下初始化时间较短，两类算法同一工况下的平均初始化时间相差不到4.3s。本文算法通过优化ORB特征点的提取过程，滤除劣质特征点，减少了关键帧数量，其平均值为10 5顿，保证了本文算法在两种工况下关键的平均值均低于ORB-SLAM2算法。表3初始化时间平均值Tab.3Average initialization timeS算法晴天阴雨天ORB-SLAM210.0311.97本文算法14.3216.06表4关键顿匹配平均数量Tab.4Average number of KeyFrame matches算法晴天阴雨天ORB-SLAM2128116本文算法1091013.2.2果园喷药机器人定位实验通过实验平台直线运动与回环运动输出的结果，验证本文算法的定位精度。首先，在果园内选取一条长10 m、宽1.8 m的直线通道，道路两边种满果树；然后，实验平台以速度0.6 m/s沿红色直线喷药通道行驶，如图19 所示；本文算法的运行效果如图2 0 所示。图19实验平台直线行驶Fig.19Experimental platform travels in straight line图2 0 直线运动过程Fig.20Linear motion process上述测试过程中获取的实验数据使用EVO轨迹功能进行处理，直线运动中部分轨迹坐标信息如表5所示,其中，T，、T,、T,为当前关键顿位置坐标，Q、Q,、Q.、Q 为位姿四元数，整理后的实验结果如图2 1所示。通过观察图2 1直线运动轨迹与分析表5实验数据可知，本文通过优化跟踪线程中ORB特征点的提取过程，将劣质ORB特征滤除，不但提高了算法生成的关键帧质量，且增强了关键顿选取的可靠性与特征匹配的准确性，从而使本文算法的直线运动表5直线运动轨迹坐标Tab.5Trajectory coordinates of linear motion序号TTT.QQQ.Q10000001.000000020.000 963 4-0.00201250.011 25430.008 451 1-0.002 797 40.000 002 10.999.960 43-0.0012150-0.00211420.01309460.008 878 10.003 039 00.00004180.999 956 04-0.0022855-0.00464910.02803580.015 207 20.005 748 5-0.00018590.999 867 85-0.0024260-0.00643410.048 465 10.023 191 90.007 155 20.002 386 90.999 702 660.002427 4-0.00925480.07426840.029 670 70.009 658 40.006 625 10.999 491 17-0.002.9715-0.006179 60.11050750.009 431 9-0.009 213 30.012.091 80.999 840.08-0.004594 9-0.00523540.14333100.003738 4-0.01185840.011469 00.99985699-0.005918 6-0.01094770.168 04590.026 456 20.012 634 60.011997 60.999 498 110-0.00653050.01288750.19110980.0384140-0.01562710.01885120.998961953丛佩超等：基于改进ORBSLAM2的果园喷药机器人定位与稠密建图算法第7 期86三42关键顿0当前局部地图点-202。当前相机y/m（a）直线轨迹点云(b）直线轨迹坐标图2 1直线运动轨迹Fig.21Linearmotion trajectory估计位姿较接近喷药机器人实际的直线运动轨迹。利用回环检测消除果园喷药机器人运行过程中的累计误差，以提高其定位精度。在真实果园场景内选取长10 m、宽3.6 m的场地进行回环实验，实验过程与结果如图2 2、2 3所示。观察图2 3可知，通过喷药机器人在果园内的闭环运动，本文算法的估计轨迹坐标基本重合，具有良好的回环检测性能。虽然，室外地形复杂，光线不均会导致部分轨迹位姿产生漂移，但本文算法仍可有效绘制果园喷药机器人的位姿及轨迹，表明其在回环运动时的定位效果较为理想。通过上述实验分析可知，本文算法具有较好的定位精度和鲁棒性。图2 2回环运动过程Fig.22Process of loop movement6三420-202x/m（a）回环轨迹点云(b）回环轨迹坐标图2 3回环运动轨迹Fig.23Loop motion path3.2.3果园喷药机器人稠密建图实验在真实果园场景内选取两条直线喷药道路，场地长30 m、宽5.4m。实验平台以速度0.6 m/s沿着喷药路径运动，利用本文算法进行实时三维稠密地图构建，得到的点云地图如图2 4a所示，将其与真实场景（图2 4b）进行对比，以验证本文算法的稠密建图效果。(a）稠密地图构建(b）果园真实场景图2 4稠密地图构建Fig.24Dense map construction观察图2 4可知，本文算法构建出的三维稠密点云地图清晰地展现了真实果园场景内草地与果树的三维空间位置，且未出现重叠、变形等情况，与现实场景基本吻合。因此，本文算法具备较强的三维稠密点云地图构建能力。综上可知，相较ORBSLA M 2 算法，本文所提算法的绝对轨迹、相对轨迹误差均有所下降，定位精度也获得提升。在真实果园场景下，本文算法特征匹配数量增多，初始化时间接近ORBSLA M 2 算法，且在直线运动与回环运动中均实现了有效定位，室外场景也能构建出有效的三维稠密点云地图，可为果园喷药机器人的导航定位打下坚实基础。4结论（1）通过改进FAST角点、描述子阅值，利用图像金字塔法与高斯滤波算法优化图像特征点的提取过程，剔除劣质ORB特征点，提高了关键帧匹配的可靠性，并通过实验证明了本文算法在freiburg1_room序列中比ORB-SLAM2的运行轨迹效果更优，定位精度更高，其绝对轨迹的平均误差、均方根误差与相对轨迹的平均误差、均方根误差分别降低44.01%、43.2 6%、7.93%