基于ORB-SLAM2的温室移动机器人定位研究_李旭.pdf

农 业 机 械 学 报 第 55卷 增刊1 2 0 2 4年 12月 基于ORB SLAM2的温室移动机器人定位研究 李 旭 1 2 阳奥凯 1 刘 青 1 伍硕祥 1 刘大为 3 4 邬 备 1 2 谢方平 3 4 1 湖南农业大学机电工程学院 长沙 410128 2 湖南省智能农机装备重点实验室 长沙 410128 3 农业农村部 南方智能育秧 苗 重点实验室 益阳 413055 4 湖南省智能育秧育苗装备工程技术研究中心 益阳 413055 摘要 针对温室内道路环境复杂 且温室移动机器人无法使用GNSS进行定位的问题 本文开展了基于ORB SLAM2的温 室定位研究 首先 对Realsense D455型深度相机获取的温室彩色图像和深度信息进行预处理 通过图像金字塔和灰度质 心法实现ORB特征的尺度和旋转不变性 完成精确有效的特征点匹配 其次 利用跟踪线程参考关键帧跟踪 匀速模型跟 踪和重定位跟踪进行粗定位 再使用局部地图跟踪进行精定位 实现对相机位姿的精确求解 再次 结合局部建图线程 在完成关键帧插入 近期地图点筛选 新地图点筛选 新地图点重建 局部BA优化和局部关键帧筛选的基础上 应用共 视图方法建立地图点 最后 结合闭环线程 通过候选回环 计算相似变换 回环融合和位姿图优化对全图地图进行回环 修正 从而实现温室内的实时定位与建图 选取辣椒生长初期 中期和成熟期3种不同作物生长期的温室进行实机测试 算法生成的轨迹与实际轨迹基本契合 X轴的均方根误差分别为0 686 2 0 355 0 0 492 5 m 平均绝对误差分别为0 588 3 0 293 7 0 455 4 m Z轴的均方根误差分别为0 149 7 0 071 8 0 368 6 m 平均绝对误差分别为0 098 6 0 046 4 0 282 5 m 试验结果表明该方法可为温室移动机器人的定位与导航提供技术支撑 关键词 温室 机器人 建图 定位 深度相机 ORB SLAM2 中图分类号 S24 文献标识码 A 文章编号 1000 1298 2024 S1 0317 08 Greenhouse Mobile Robot Localization Based on ORB SLAM2 LI Xu 1 2 YANG Aokai 1 LIU Qing 1 WU Shuoxiang 1 LIU Dawei 3 4 WU Bei 1 2 XIE Fangping 3 4 1 College of Mechanical and Electrical Engineering Hunan Agricultural University Changsha 410128 China 2 Hunan Key Laboratory of Intelligent Agricultural Machinery Equipment Changsha 410128 China 3 Key Laboratory of Intelligent Seedling Cultivation Ministry of Agriculture and Rural Affairs Yiyang 413055 China 4 Hunan Research Center of Engineering Technology for Intelligent Seedling Equipment Yiyang 413055 China Abstract Aiming at the complex road environment in greenhouse and the problem that greenhouse mobile robots cannot use GNSS for localization research and experiments on greenhouse localization were carried out based on ORB SLAM2 Firstly the color image and depth information of greenhouse acquired by the depth camera Realsense D455 were preprocessed and the scale and rotation invariance of ORB features was achieved by the image pyramid and grayscale center of mass method to complete accurate and effective feature point matching Secondly coarse localization was done by using tracking thread reference key frame tracking homogeneous model tracking and repositioning tracking and then fine localization was done by using local map tracking to achieve an accurate solution for the camera pose Thirdly combining with the local map building thread applying the common view method to build up the map points based on the completion of the key frame insertion the recent map point screening the new map point screening the new map point reconstruction the local BA optimization and the local key frame screening Finally combined with the closed loop thread the full map was corrected by loopback correction through the candidate loopback computation of similarity transformation loopback fusion and position map optimization so as to realize the greenhouse in the real time localization and map building Three greenhouses with different crop growth conditions in the early middle and maturity stages of pepper growth were selected for real machine testing and the trajectories generated by the algorithm basically matched doi 10 6041 j issn 1000 1298 2024 S1 034 收稿日期 2024 07 29 修回日期 2024 09 30 基金项目 国家重点研发计划项目 2021YFD1600300 4 06 2020YFD1000300 作者简介 李旭 1981 男 副教授 博士 主要从事农业装置智能控制技术研究 E mail leexu 通信作者 刘大为 1983 男 副教授 博士 主要从事蔬菜生产全程机械化技术与装备研究 E mail liudawei8361 农 业 机 械 学 报 2 0 2 4年 the actual trajectories with the root mean square errors on the X axis of 0 686 2 m 0 355 0 m 0 492 5 m and the average absolute errors of 0 588 3 m 0 293 7 m and 0 455 4 m respectively and on the Z axis of 0 149 7 m 0 071 8 m 0 368 6 m and the average absolute errors of 0 098 6 m 0 046 4 m and 0 282 5 m respectively The experimental results showed that the method could provide technical support for the localization and navigation of greenhouse mobile robots Key words greenhouses robot mapping localization depth camera ORB SLAM2 0 引言 温室是一种用于种植作物的生产设施 可以提高生 产效率和品质 2022年我国温室面积约为186 2万公 顷 1 随着科技进步和农业生产技术不断发展 智能化 温室装备成为研究热点 2 由于温室作业环境温度和 湿度高 且作业劳动强度大 3 温室移动机器人在近年 来的研究和应用中取得了显著进展 4 温室内环境感知 是温室移动机器人实现导航和开展自主作业的关键 国内外学者围绕铺设轨道 循迹式和基于SLAM技 术的多源信息融合式3种技术开展了温室移动机器人的 定位与导航研究 5 OIKAWA等 6 提出一种预先获取 农作物的位置 提前铺设轨道 温室机器人根据轨道运 动并进行药剂喷洒作业的方法 该方法实现了精确有效 的喷洒作业且节省了时间 提高了效率 袁挺等 7 在硬 化且表面附着有浮土 泥块 植物残留物的水泥地路面 中央铺设红色引导线来提供引导信息 利用光照色彩稳 定性有效提取导航路径信息 导航线平均识别率达 93 9 贾士伟等 8 提出了一种基于激光测距仪的温室 移动机器人道路边缘检测和导航算法 利用道路区域与 作物区域的表面平整度差异实现道路边缘检测 利用基 准道路宽度帮助剔除伪道路区域 进而生成导航调速控 制指令 机器人偏离道路中心线的平均值为 1 270 7 cm 均方误差为2 677 2 侯加林等 9 使用激光雷达与 Cartographer算法结合实现了在温室环境下的二维栅格 地图构建与机器人实时定位功能 车载系统分别以0 2 0 5 0 8 m s的速度运行时 实际导航路径与目标路径的 平均偏差小于13 cm 标准差小于5 cm 吴雄伟等 10 提 出了一种基于无监督光流的视觉里程估计方法 通过引 入局部几何一致性约束和优化光流网络结构 并使用金 字塔层间知识自蒸馏技术 使得温室移动机器人在位姿 跟踪中的相对位移误差降低9 80 TAN等 11 提出了 一种基于多线激光雷达和自适应滤波点云投影 AF PCP SLAM算法的温室地图构建方法 该方法利用 Cartographer框架 改进了地图构建和定位精度 试验表 明 AF PCP算法绘图面积比Cartographer算法提高 155 7 显著降低了作物行长度误差和空隙长度 同时 定位误差降低79 9 综合研究现状 铺设轨道式和循 迹式具有可靠性强和稳定性好的优点 但会改变原有的 环境结构 缺少灵活性 限制了移动机器人的作业范围 基于SLAM技术的多源数据融合通过激光雷达 12 相 机 13 惯性传感器 14 等多种传感器信息实现自主导航 该方法不需要人为铺设轨道和路标 节约了人力物力成 本 保护了现场环境 移动机器人作业更灵活 本文为实现移动机器人在作物生长密集 空间狭窄 和道路起伏的温室内定位和导航 采用RGB D深度相 机开展基于ORB SLAM2算法的温室建图与实时定位研 究 以期为温室移动机器人的自主作业提供技术支撑 1 温室内ORB SLAM2建图与定位方法 1 1 算法框架 ORB SLAM2算法 15 是一种基于特征点的视觉 SLAM算法 该算法在ORB SLAM 16 纯单目系统的基础 上添加了对双目和RGB D相机的支持 同时增加了用 于专门处理双目和RGB D数据的输入与处理模块 并 且在原有跟踪线程 局部建图线程和闭环线程的基础上 增加了全局BA优化线程 ORB SLAM2系统框架如图1 所示 ORB SLAM2算法从输入图像中提取ORB特征点 Oriented FAST and rotated BRIEF 通过特征匹配估计 相机的相对位姿 单目相机使用运动恢复结构 Structure from motion SfM 进行初始化 而双目和 RGB D相机则直接利用深度信息生成三维点云并构建 初始地图 在实时追踪过程中 ORB SLAM2根据上一 帧的位姿预测当前帧的位姿 通过特征点与局部地图的 匹配修正位姿 同时 算法根据一定判定条件选择生成 并插入关键帧 利用三角测量方法生成新的三维地图 点 回环检测通过词袋模型查找与当前帧相似的先前 关键帧 识别是否存在回环 并通过姿态图优化消除漂 移误差 全局BA Bundle adjustment 进一步优化整个 地图的相机位姿与三维点位置 确保全局地图的一致性 和高精度的定位 1 2 视觉理论基础 丰富的图像信息是温室高精度定位与稠密地图构 建的基础 本文选用Realsense D455型深度相机来获取 温室的环境图像信息 如图2所示 搭载在移动机器人 上的相机在位姿1 位姿2 位姿N处获取温室的路标 点y 1 y 2 y n 的环境信息 17 系统根据视觉SLAM算法 来处理获取到的图像数据 根据相机的运动方程和观测 方程来估计相机的位姿 再通过坐标系转换关系确定移 动机器人的位姿信息 实现精准定位 318 增刊1 李旭 等 基于ORB SLAM2的温室移动机器人定位研究 深度相机的运动方程为 x k f x k 1 u k w k 1 其观测方程为 z k j h y j x k v k j 2 式中 x k 相机k时刻位置 u k 测量k时刻数据 w k 测量k时刻噪声 y j 相机观测到的路标点 v k j 观测k时刻路标点噪声 z k j 相机在x k 处对路标点y j 的观测数据 1 2 1 特征点提取 跟踪线程是ORB SLAM2系统的主线程 每帧图像 从相机输入到跟踪线程都要进行特征点提取 特征点提 取过程如图3所示 18 为了减少计算量 原始图像需要 通过灰度变换 在灰度图像上选取一个像素点P x 0 y 0 以像素点P为中心 选取半径为r的圆周上16个像 素点为领域 并设置阈值 若这16个像素点中有连续 12个点满足 x i y i x 0 y 0 3 式中 x i y i 点 x i y i 的像素值 则认为这个点为FAST特征点 为了使特征点具有旋转 不变性 需要计算出这个领域的灰度质心m来作为权重 中心 19 在一个图像块B中 定义图像块的矩为 m pq x y B x p y q I x y p q 0 1 4 通过矩找到灰度质心 m m 10 m 00 m 01 m 00 5 中心点P到灰度质心m的方向则为特征点的方向 最终 得到oFAST特征点 同时构建图像金字塔实现对不同尺度的图像处理 从而提升特征点匹配的效率和鲁棒性 提高算法在各种 1 2 P 3 4 5 6 7 8910 11 12 13 14 15 16 m 图3 oFAST特征提取过程 Fig 3 oFAST feature extraction process 普通帧 双目 或RGB D 输入预处理 位姿预测 局部地图追踪 新关 键帧挑选 关键帧插入 近期 地图点插入 新地 图云点重建 局部 BA优化 局部关键帧筛选 位姿图优化 回环融合 计算转移矩阵 查询数据库 回环修正 回环检测 全局 BA优化地图更新 全局 BA优化线程 视觉词袋 识别数据库 位置识别 生成树共视关系图 地图云点 关键帧 地图结构 跟踪线程 局 部 建 图 线 程 闭环线程 图1 ORB SLAM2系统框架图 Fig 1 System framework diagram of ORB SLAM2 y 1 y 2 y 3 y 4 3 2 1 图2 温室环境下相机运动状态 Fig 2 Camera motion state in greenhouse environment 319 农 业 机 械 学 报 2 0 2 4年 环境下的表现 构建图像金字塔如图4所示 不同尺度 图像进行融合可以获得更多的特征点 因此对同一图像 按照相同的比例进行扩大或者缩小 金字塔面积随着金 字塔层数的增加而减小 所能提取到的特征点数量也 越少 假设第0层图像尺寸为L b 则第k层分配的特征 点数为 N k N 1 s 2 1 s 2n s 2k 0 s 1 6 式中 N 特征点总数 s 缩放因子 n 金字塔图层数 N k 第k层分配的特征点数 1 2 2 局部地图构建 如何获取更精确的温室地图信息是本文研究的重点 内容之一 ORB SLAM2算法中跟踪线程挑选出来的关键 帧会存放在一个缓冲区内等待着局部建图线程对其进行 处理 20 首先在词袋模型中计算出表征向量 词袋模型 将关键帧更新到词袋模型的在线数据库中 随后将地图 中有共视关系但没有与该帧建立映射的点云建立关联 接着计算该关键帧与地图点已有关键帧的共视连接权 重 以将该关键帧添加到共视关系图中 最后将该关键帧 添加到地图结构中 完成关键帧的插入 在插入关键帧 的过程中 需要保留一些近期地图点 剔除质量差的点 云 对于每个新插入的关键帧 借助共视图关系与邻近 的关键帧进行匹配 将该新关键帧中还未映射到地图点 的特征点进行三角化重建 以生成新的地图点 相机成 像和地图点观测原理如图5针孔相机模型所示 21 空间上的一点P通过一个小孔将外部光线投影到 图像平面上 形成倒立的二维图像 通过三角形相似变 换 物理成像坐标系与像素坐标系的转换关系 将世界 坐标系中的三维点通过相机的外参转换到相机坐标系 然后利用内参进行投影 生成图像坐标 转换关系式为 Z u v 1 f x 0 C x 0 f y C y 0 0 1 X Y Z KP 7 式中 u v 像素坐标 Z 该点在相机坐标系下深度 X Y 世界坐标系下水平垂直方向的空间坐 标轴 C x C y 相机成像的主点坐标 f x f y 相机焦距 K 相机的内参数矩阵 P 该点在相机坐标系下坐标 由于相机是运动的 所以需要一个世界坐标系来描 述相机和物体的位置 设该点在相机坐标系下的坐标 为P c 在世界坐标系下的坐标为P w 则坐标P c 是由坐标 P w 通过旋转矩阵R和平移向量t得到的 则二者的转换 关系式为 Z u v 1 K RP W t KTP w 8 式中 T 变换矩阵 为了完成温室三维环境的构建 需要温室空间点的 深度即距离信息 深度相机相比于单目 双目相机具有 主动测量图像深度的优势 按照测量原理可以分为红外 结构光和TOF Time of flight 本文所选用的Realsense D455型深度相机采用的是TOF原理测量图像深度 原 理示意如图6所示 通过连续发射光脉冲 一般为不可见 光 到被测物体上 然后接收从物体反射回去的光脉冲 通过探测光脉冲的飞行 往返 时间来计算被测物体离 相机的距离 为光脉冲飞行的时间乘以光脉冲飞行速度 的一半 第 0层 第 1层 第 2层 第 3层 第 4层 图4 图像金字塔 Fig 4 Image pyramid X null null Z null Y X Y 相机坐标系 Oxyz O B A P P null Z X X 相似三角形 O W X W Y W Z W 世界坐标系 物理成像平面 像素平面 光心O M f焦距 图5 针孔相机模型 Fig 5 Pinhole camera model 发射器 检测器 物体 距离 图6 TOF原理示意图 Fig 6 TOF principle schematic 320 增刊1 李旭 等 基于ORB SLAM2的温室移动机器人定位研究 当缓冲区的关键帧都被处理完后 则将当前帧局部 的几个关键帧以及地图点放入局部BA中进行优化 局 部BA优化完成后对局部地图中的关键帧进行筛选 剔 除冗余的关键帧 22 通过处理关键帧 建立共视关系 生成新的地图点并进行局部优化来增强地图的精度和 一致性 1 2 3 回环修正 由于位置估计随着时间的变化会产生漂移 23 为了 纠正这种累计误差 ORB SLAM2算法通过闭环线程来 减少累计的定位误差并优化全局地图 在局部建图线 程中 筛选出来最后保留在地图中的关键帧都会被送到 闭环线程中 通过词袋模型 将数据库中与当前帧相似 度较高的帧挑选出来作为候选回环帧 再计算当前帧与 候选回环帧之间的变换关系 在ORB SLAM2算法的闭 环检测过程中 相似变换 Similarity transformation 用于 计算相机位姿和地图点之间的关系 以修正由于漂移引 起的误差 24 26 在闭环检测中 系统会识别出相机在新 的帧与旧的关键帧之间的相似场景 这时需要通过相似 变换来校正这些关键帧的相对位置和姿态 通过计算得 到旋转矩阵R 缩放因子s 以及平移向量t 可以构造出 最终的相似变换 将不同帧之间的地图点变换到同一坐 标系中 相似变换表示为 p i sRp i t 9 其中p i p i 表示不同帧之间对应的同一组地图点 如果通过计算得到变换关系并且二者之间有足够 多的共视点 则认为回环检测成功 通过回环融合修正 当前关键帧与获选回环帧及其邻近关键帧的累计误差 最后通过全局优化来修正与当前关键帧无共视关系的 帧 提升地图的全局一致性和定位精度 2 试验与分析 为验证基于ORB SLAM2的温室机器人定位的性能 和应用效果 结合温室移动机器人在真实场景开展实机 测试 试验场景如图7所示 试验平台计算机为 NVIDIA Jetson nano开发套件 操作系统为Ubuntu 18 04 采用的相机为Relsense D455型深度相机 相机外 部参数和标定后的内部参数分别如表1和表2所示 2 1 ORB SLAM2算法在温室应用性能评估 实机测试在湖南农业大学辣椒温室进行 将ORB SLAM2算法在温室机器人的移动底盘上进行测试 选取 如图8所示不同生长期的3个辣椒温室各规划一条矩形 闭环路径进行试验 根据特征点捕捉效果 关键帧数量 点云生成数量以及处理每帧所需时间等测试算法的应 用性能 不同辣椒生长期算法特征点捕捉效果如图9所示 图中的绿色框代表算法捕捉的特征点 从图9中可以看 图7 温室移动机器人作业场景 Fig 7 Greenhouse mobile robot operation scene 表1 相机外部参数 Tab 1 Camera external parameters 参数 数值 外形尺寸 mm mm mm 124 26 29 深度 m 0 6 6 深度 FOV 87 58 彩色 FOV 90 H 65 V 深度帧 率 f s 1 30 彩色帧 率 f s 1 30 表2 相机内部参数 Tab 2 Internal camera parameters 参数 数值 内参矩阵 384 255 6 0 326 467 9 0 385 990 9 241 153 0 0 0 1 畸变参数 K 1 0 050 4 K 2 0 046 8 P 1 0 P 2 0 重投影平均误差 0 208 5 a 初期 b 中期 c 成熟期 图8 辣椒不同生长期场景 Fig 8 Scenarios for different growth periods of chili peppers 321 农 业 机 械 学 报 2 0 2 4年 出 深度相机下的ORB SLAM2算法在不同作物长势的 情况下均可以达到良好的特征点提取和匹配效果 并且 特征点分布情况也较均匀 因此深度相机下的ORB SLAM2算法在温室内具有良好的特征点捕捉效果 操作温室移动机器人绕一垄作物闭环行走一周 生 成如图10所示的点云图 图中红色点为参考地图点 黑 色点为所有地图点 蓝色表示关键帧 绿色线表示关键 帧之间的共视关系 算法建图结果为 辣椒生长初期的 轨迹a建图时间为130 288 s 路径长度58 876 m 共获取 501个关键帧和21 064个点云 辣椒生长中期的轨迹b 建图时间为129 522 s 路径长度56 104 m 共获取536个 关键帧和26 606个点云 辣椒生长成熟期的轨迹c建图 时间230 963 s 路径长度57 569 m 共获取943个关键帧 和36 742个点云 结合数据分析 ORB SLAM2具有良好的建图效果 和轨迹生成的能力 且在温室内的建图能达到较好的回 环效果 处理3种不同作物生长期下ORB SLAM2算法得到 CameraTrajectory txt文件 统计算法处理每帧图像的时 间及所占的比例 结果如表3所示 算法在3种不同作物生长期的温室环境下处理每帧 图像的时间稳定在0 033 s和0 034 s 本次试验采集图 像的频率为30 Hz 即每隔0 033 3 s捕获1幅图像 与算 法处理每帧图像的时间一致 且ORB SLAM2算法平均 跟踪时间分别为0 086 7 0 088 17 0 100 0 s 说明基于 深度相机的ORB SLAM2算法可以满足温室内不同环境 下的实时定位与建图要求 2 2 ORB SLAM2算法在温室内定位试验 在不同生长期的3个辣椒温室分别选取一段矩形路 径 在路径上每隔0 5 m用石灰粉做一个标记点 如图11 所示 操作温室移动机器人沿标记点移动 形成用于评 估算法定位精度的实际轨迹 温室移动机器人移动过程中 搭载在温室移动机器 a 初期 b 中期 c 成熟期 图9 不同辣椒生长期算法特征点捕捉效果 Fig 9 Effectiveness of feature point capture by algorithms for different pepper growth periods a 初期 b 中期 c 成熟期 图10 不同辣椒生长期算法生成的点云图 Fig 10 Point clouds generated by algorithm for different pepper growth periods 图11 实际轨迹设定方法 Fig 11 Actual trajectory setting method 表3 算法处理每帧时间统计 Tab 3 Algorithmic processing of time per frame statistics 轨迹 a b c 总帧数 4 104 4 022 7 247 0 033 s处理帧 数占比 65 16 65 17 65 48 0 034 s处理帧 数占比 34 84 34 83 34 52 322 增刊1 李旭 等 基于ORB SLAM2的温室移动机器人定位研究 人上的深度相机录制视频获得数据集 通过算法分别获 得3种场景下移动机器人的算法轨迹 表4所示为辣椒 生长中期算法生成的部分轨迹坐标 算法轨迹与实际 轨迹对比图如图12所示 计算2个轨迹间X轴和Z轴的 均方根误差和平均绝对误差 其中X轴方向的均方根误 差分别为0 686 2 0 355 0 0 492 5 m 平均绝对误差分别 为0 588 3 0 293 7 0 455 4 m Z轴方向的均方根误差分 别为0 149 7 0 071 8 0 368 6 m 平均绝对误差分别为 0 098 6 0 046 4 0 282 5 m 3个不同轨迹的X轴均方根 误差均在0 7 m范围之内 平均绝对误差均在0 6 m范围 内 Z轴均方根误差均在0 4 m范围之内 平均绝对误差 均在0 3 m范围内 偏差主要产生在转弯位置及道路起 伏较大的位置 算法轨迹与真实轨迹基本契合 具有良 好的定位效果 3 结论 1 针对温室移动机器人定位技术存在的问题 提 出了基于深度相机的ORB SLAM2的温室定位方法 实 机测试试验表明该方法在温室不同环境内有着良好的 建图效果以及较高的定位精度 2 在辣椒生长初期 中期 成熟期3种不同长势环 境下都有着良好的特征点匹配效果 且分别能获取501 536 943个关键帧 生成210 64 260 66 367 42个点云 具有较好的建图效果 同时每帧图像处理的时间为 0 033 s和0 034 s 与采集视频数据集的频率一致 算法 的实时性良好 3 定位试验结果表明 X轴均方根误差分别为 0 686 2 0 355 0 0 492 5 m 平均绝对误差分别为 0 588 3 0 293 7 0 455 4 m Z轴均方根误差为0 149 7 0 071 8 0 368 6 m 平均绝对误差分别为0 098 6 0 046 4 0 282 5 m 算法轨迹与真实轨迹基本契合 参 考 文 献 1 孙国祥 黄银锋 汪小旵 等 基于LIO SAM建图和激光视觉融合定位的温室自主行走系统 J 农业工程学报 2024 40 3 表4 辣椒生长中期算法生成的部分轨迹坐标 Tab 4 Partial trajectory coordinates generated by algorithm during mid growth period of chili peppers 序号 1 2 3 4 5 6 7 8 9 10 T x 0 0 000 789 696 0 000 851 873 0 002 024 376 0 002 503 945 0 003 638 672 0 003 728 752 0 003 829 772 0 004 466 692 0 006 414 600 T y 0 0 000 465 213 0 000 534 035 0 000 505 984 0 000 978 224 0 001 571 808 0 002 505 891 0 002 324 495 0 003 230 116 0 003 694 547 T z 0 0 000 538 790 0 001 070 877 0 000 966 425 0 001 988 228 0 002 060 608 0 002 992 342 0 003 379 196 0 004 210 810 0 004 373 698 Q x 0 0 000 009 404 0 000 016 415 0 000 145 660 0 000 205 524 0 000 413 689 0 000 440 078 0 000 466 959 0 000 754 498 0 000 754 098 Q y 0 0 000 081 495 0 000 005 087 0 000 212 470 0 000 228 129 0 000 351 456 0 000 320 390 0 000 241 764 0 000 234 699 0 000 556 574 Q z 0 0 000 238 793 0 000 069 865 0 000 339 335 0 000 348 918 0 000 486 708 0 000 350 172 0 000 231 431 0 000 146 374 0 000 747 159 Q w 1 000 000 000 0 999 999 940 1 000 000 000 0 999 999 881 0 999 999 881 0 999 999 702 0 999 999 762 0 999 999 821 0 999 999 642 0 999 999 285 a 初期 b 中期 c 成熟期 图12 不同辣椒生长时期算法生成轨迹与实际轨迹对比 Fig 12 Comparison of algorithm generated trajectories and actual trajectories for different pepper growth periods 323 农 业 机 械 学 报 2 0 2 4年 227 239 SUN Guoxiang HUANG Yinfeng WANG Xiaochan et al Autonomous navigation system in a greenhouse using LIO SAM mapping and laser viosion fusion localization J Transations of the CSAE 2024 40 3 227 239 in Chinese 2 杨婧暄 我国农业大棚现状及未来发展趋势 J 种子科技 2023 41 14 142 144 3 毕松 张国轩 李志军 等 基于测距值修正的温室植保机器人定位方法 J 农业机械学报 2023 54 8 347 358 BI Song ZANG Guoxuan LI Zhijun et al Positioning method of greenhouse plant protection robot based on distance measurement value correction J Transactions of the Chinese Society for Agricultural Machinery 2023 54 8 347 385 in Chinese 4 刘成良 贡亮 苑进 等 农业机器人关键技术研究现状与发展趋势 J 农业机械学报 2022 53 7 1 22 55 LIU Chengliang GONG Liang YUAN Jin et al Current status and development trends of agricultural robots J Transactions of the Chinese Society for Agricultural Machinery 2022 53 7 1 22 55 in Chinese 5 高国琴 李明 基于K means算法的温室移动机器人导航路径识别 J 农业工程学报 2014 30 7 25 33 GAO Guoqin LI Ming Navigating path recognition for greenhouse mobile robot based on K means algorithm J Transactions of the CSAE 2014 30 7 25 33 in Chinese 6 OIKAWA Y OHYA A YOROZU A Pre planning of trajectory and orbiting around greenhouses to shorten operation time for fruit set reagent spraying robot C 2024 10th International Conference on Mechatronics and Robotics Engineering ICMRE IEEE 2024 105 109 7 袁挺 任永新 李伟 等 基于光照色彩稳定性分析的温室机器人导航信息获取 J 农业机械学报 2012 43 10 161 166 YUAN Ting REN Yongxin LI Wei et al Navigation information acquisition based on illumination chromaticity stability anal