Orchard robot navigation in unstructured environment based on density adaptive RANSAC
-
摘要:目的
提出一种基于多传感器融合的果园导航方案,解决果园机器人在GPS导航过程中受果树遮挡导致信号弱、定位效果差的问题。
方法通过16线激光雷达采集高精度的三维点云数据,利用Voxel grid filter滤波算法进行点云预处理,降低点云密度并去除离散点,将果树行通过欧几里类算法进行聚类,采用改进的随机采样一致性 (Random sample consensus, RANSAC) 算法拟合出果树行直线,根据平行直线的关系,推算得到导航线,并融合惯性测量单元(Inertial measurement unit, IMU)对果园机器人进行高精度定位。基于差速转向和纯追踪模型进行轨迹跟踪,实现果园机器人在果树行间自主导航以及自动换行的目标。
结果在将激光雷达和IMU的数据进行融合后,获取到果园机器人的准确位姿,当机器人以速度0.8 m/s在果园作业时,对比最小二乘法和传统RANSAC法产生的偏差,基于密度自适应RANSAC法产生的横向偏差不超过0.1 m、航向角偏差不超过1.5°,均为3种方法中的最小值。但当机器人速度增加到1.0 m/s时,各项偏差均明显增大。
结论本文提出的基于多传感器融合的果园机器人导航技术适用于大多数规范化果园,具有重要推广价值。
-
关键词:
- 农业机器人 /
- 自主导航 /
- 直线拟合 /
- 差速转向 /
- 改进RANSAC算法
Abstract:ObjectiveAn orchard navigation scheme based on multi-sensor fusion was proposed to solve the problems of weak signal and poor positioning effect caused by tree occlusion in the GPS navigation process of orchard robot .
MethodHigh-precision 3D point cloud data were collected by 16-line lidar, point cloud was preprocessed by Voxel grid filter algorithm, point cloud density was reduced and discrete points were removed, fruit tree rows were clustered by Euclidian algorithm, and the straight lines of fruit tree rows were fitted by improved random sampling consistency (RANSAC) algorithm. According to the relationship of parallel lines, the navigation line was calculated and integrated with inertial measurement unit (IMU) for high-precision positioning of orchard robot. Based on differential steering and pure tracking model, the goal of autonomous navigation and automatic line wrapping of orchard robot was realized.
ResultAfter the data fusion of lidar and IMU, the accurate position and pose of the robot were obtained. Compared with the deviation produced by the least square method and the traditional RANSAC method, the lateral deviation based on density adaptive RANSAC method was less than 0.1 m and the heading angle deviation was less than 1.5° when the robot was operating in the orchard at the speed of 0.8 m/s. The deviations were the minimum in the three methods. However, when the robot speed increased to 1.0 m/s, all the deviations increased obviously.
ConclusionThe orchard robot navigation technology based on multi-sensor fusion proposed in this paper is suitable for most standardized orchards and has important promotion value.
-
-
图 3 运动模型
v:线速度;vr:右侧轮速度;vl:左侧轮速度;$ \omega $:角速度;$ \theta $:姿态角;£:轮距;p:机器人中心位置;Ll、Lr:左、右两侧果树行
Figure 3. Motion model
v: Linear velocity; vr: Right wheels speed; vl: Left wheels speed; ω: Angular velocity; θ : Attitude angle; £: Wheelspan; p: Center position of robot; Ll, Lr: Fruit tree rows on the left and right sides
图 4 果园机器人轨迹跟踪示意图
K:目标轨迹;B:目标点;$ \alpha $:航向偏差;f:前视距离;d:横向偏差;A:圆弧轨迹对应的圆心;R:轨迹半径;C:OC边上的垂足;O:机器人中心位置
Figure 4. Orchard robot trajectory tracking diagram
K: Target trajectory; B: Target point; $ \alpha $: Course deviation; f: Forward-looking distance; d: Lateral deviation; A: Center of the arc trajectory; R: Trajectory radius; C: Vertical foot at the edge of OC; O: Robot center position
图 7 点云预处理
a:果园真实环境;b:原始点云;c:分割地面后的点云,其中,白色部分为地面点云,红色部分为非地面点云,即果树行点云
Figure 7. Point cloud pretreatment
a: Real environment of the orchard; b: Original point cloud; c: Point cloud after ground segmentation. In c, the white part is ground point cloud, and the red part is non-ground point cloud, namely, fruit tree row point cloud
图 8 拟合导航线
a:去除地面后的点云;b:聚类后的点云;c:生成导航线后的点云;d:存在离散果树的点云;c、d中,绿色直线为拟合得到的果树行直线,红色直线为推算得到的导航线
Figure 8. Fitting navigation lines
a: Point cloud after ground removal; b: Point cloud after clustering; c: Point cloud with the navigation line; d: Point clouds with discrete fruit trees. In c and d, the green line is the fruit tree line obtained by fitting, and the red line is the navigation line obtained by calculation
表 1 速度0.8 m/s下3种算法产生的偏差结果
Table 1 The deviation results generated by the three algorithms under the speed of 0.8 m/s
算法
Algorithm横向偏差/m
Lateral deviation平均横向偏差/m
Mean lateral deviation航向偏差/(°)
Course deviation平均航向偏差/(°)
Mean course deviationLSM算法
LSM algorithm0.20
0.18
0.23
0.21
0.190.202 2.1
1.8
2.2
1.7
2.01.96 传统RANSAC算法
Traditional RANSAC algorithm0.16
0.15
0.13
0.18
0.170.158 1.3
1.8
1.5
1.2
1.81.52 改进RANSAC算法
Improved RANSAC algorithm0.08
0.10
0.09
0.07
0.080.084 1.5
1.5
1.2
1.3
1.21.34 表 2 速度1.0 m/s下3种算法产生的偏差结果
Table 2 The deviation results generated by the three algorithms under the speed of 1.0 m/s
算法
Algorithm横向偏差/m
Lateral deviation平均横向偏差/m
Mean lateral deviation航向偏差/(°)
Course deviation平均航向偏差/(°)
Mean course deviationLSM算法
LSM algorithm0.25
0.28
0.26
0.23
0.240.252 2.3
2.5
2.6
2.4
2.72.50 传统RANSAC算法
Traditional RANSAC algorithm0.23
0.22
0.24
0.22
0.210.224 2.2
2.4
1.9
2.5
2.62.32 改进RANSAC算法
Improved RANSAC algorithm0.14
0.09
0.08
0.13
0.100.108 2.0
1.8
2.1
2.2
2.02.02 -
[1] 王祺, 王艳玲, 俄胜哲. 我国果园机械装备现状及发展思路[J]. 农业机械, 2019(1): 109-111. [2] LAN Y B, CHEN S D. Current status and trends of plant protection UAV and its spraying technology in China[J]. International Journal of Precision Agricultural Aviation, 2018, 1(1): 1-9.
[3] GONG J L, WANG M X, ZHANG Y F, et al. Flow and sound field analysis of agricultural ultrasonic atomizing nozzle[J]. International Journal of Precision Agricultural Aviation, 2019, 2(2): 32-37.
[4] 秦喜田, 刘学峰, 任冬梅, 等. 我国果园生产机械化现状及其发展趋势[J]. 农业装备与车辆工程, 2019, 57(S1): 35-38. [5] 张漫, 季宇寒, 李世超, 等. 农业机械导航技术研究进展[J]. 农业机械学报, 2020, 51(4): 1-18. doi: 10.6041/j.issn.1000-1298.2020.04.001 [6] 陈媛媛, 游炯, 幸泽峰, 等. 世界主要国家精准农业发展概况及对中国的发展建议[J]. 农业工程学报, 2021, 37(11): 315-324. doi: 10.11975/j.issn.1002-6819.2021.11.036 [7] 刘成良, 林洪振, 李彦明, 等. 农业装备智能控制技术研究现状与发展趋势分析[J]. 农业机械学报, 2020, 51(1): 1-18. doi: 10.6041/j.issn.1000-1298.2020.01.001 [8] 何勇, 蒋浩, 方慧, 等. 车辆智能障碍物检测方法及其农业应用研究进展[J]. 农业工程学报, 2018, 34(9): 21-32. doi: 10.11975/j.issn.1002-6819.2018.09.003 [9] 李道亮, 李震. 无人农场系统分析与发展展望[J]. 农业机械学报, 2020, 51(7): 1-12. doi: 10.6041/j.issn.1000-1298.2020.07.001 [10] 周建军, 周文彬, 刘建东, 等. 果园机器人自动导航技术研究进展[J]. 计算机与数字工程, 2019, 47(3): 571-576. [11] 李秋洁, 丁旭东, 邓贤. 基于激光雷达的果园行间路径提取与导航[J]. 农业机械学报, 2020, 51(S2): 344-350. doi: 10.6041/j.issn.1000-1298.2020.S2.040 [12] 莫冬炎, 杨尘宇, 黄沛琛, 等. 基于环境感知的果园机器人自主导航技术研究进展[J]. 机电工程技术, 2021, 50(9): 145-150. doi: 10.3969/j.issn.1009-9492.2021.09.038 [13] BLOK P M, SUH H K, VAN BOHEEMEN K, et al. Autonomous in-row navigation of an orchard robot with a 2D LIDAR scanner and particle filter with a laser-beam model[J]. Journal of Institute of Control, Robotics and Systems, 2018, 24(8): 726-735. doi: 10.5302/J.ICROS.2018.0078
[14] 姬长英, 周俊. 农业机械导航技术发展分析[J]. 农业机械学报, 2014, 45(9): 44-54. doi: 10.6041/j.issn.1000-1298.2014.09.008 [15] 赛炜, 孙忠涵. 基于激光雷达的机器人智能导航系统研究[J]. 激光杂志, 2019, 40(11): 182-186. [16] ZHANG H F, HONG Y, QIU J L. An off-policy least square algorithms with eligibility trace based on importance reweighting[J]. Cluster Computing, 2017, 20(4): 3475-3487. doi: 10.1007/s10586-017-1165-0
[17] FOTOUHI M, HEKMATIAN H, KASHANI-NEZHAD M A, et al. SC-RANSAC: Spatial consistency on RANSAC[J]. Multimedia Tools and Applications, 2019, 78(7): 9429-9461. doi: 10.1007/s11042-018-6475-6
[18] SANGAPPA H K, RAMAKRISHNAN K R. A probabilistic analysis of a common RANSAC heuristic[J]. Machine Vision and Applications, 2019, 30(1): 71-89. doi: 10.1007/s00138-018-0973-4
[19] 张华强, 王国栋, 吕云飞, 等. 基于改进纯追踪模型的农机路径跟踪算法研究[J]. 农业机械学报, 2020, 51(9): 18-25. doi: 10.6041/j.issn.1000-1298.2020.09.002