CN116839570B - 一种基于传感器融合目标检测的作物行间作业导航方法 - Google Patents

一种基于传感器融合目标检测的作物行间作业导航方法 Download PDF

Info

Publication number
CN116839570B
CN116839570B CN202310865172.4A CN202310865172A CN116839570B CN 116839570 B CN116839570 B CN 116839570B CN 202310865172 A CN202310865172 A CN 202310865172A CN 116839570 B CN116839570 B CN 116839570B
Authority
CN
China
Prior art keywords
point
target
navigation
coordinate system
angle
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202310865172.4A
Other languages
English (en)
Other versions
CN116839570A (zh
Inventor
李宁
刘立超
方旭
马立
丁宵
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Anhui Agricultural University AHAU
Original Assignee
Anhui Agricultural University AHAU
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Anhui Agricultural University AHAU filed Critical Anhui Agricultural University AHAU
Priority to CN202310865172.4A priority Critical patent/CN116839570B/zh
Publication of CN116839570A publication Critical patent/CN116839570A/zh
Application granted granted Critical
Publication of CN116839570B publication Critical patent/CN116839570B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1656—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with passive imaging devices, e.g. cameras
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
    • G06V10/762—Arrangements for image or video recognition or understanding using pattern recognition or machine learning using clustering, e.g. of similar faces in social networks
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
    • G06V10/77—Processing image or video features in feature spaces; using data integration or data reduction, e.g. principal component analysis [PCA] or independent component analysis [ICA] or self-organising maps [SOM]; Blind source separation
    • G06V10/774—Generating sets of training patterns; Bootstrap methods, e.g. bagging or boosting
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
    • G06V10/82—Arrangements for image or video recognition or understanding using pattern recognition or machine learning using neural networks
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/20—Special algorithmic details
    • G06T2207/20081—Training; Learning
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/20—Special algorithmic details
    • G06T2207/20084—Artificial neural networks [ANN]

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Evolutionary Computation (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Health & Medical Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Computing Systems (AREA)
  • Databases & Information Systems (AREA)
  • General Health & Medical Sciences (AREA)
  • Medical Informatics (AREA)
  • Software Systems (AREA)
  • Multimedia (AREA)
  • Electromagnetism (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种基于传感器融合目标检测的作物行间作业导航方法，属于数字化农业机械技术领域；本发明提出一种基于传感器融合目标检测的作物行间作业导航方法，运用计算机视觉以及深度学习算法使用相机识别农作物，再利用激光雷达进行基于点云聚类的目标检测与障碍物分类，并融合二者检测结果。基于点云聚类的雷达目标检测方法，相较于常用的基于特征提取的激光雷达目标检测方法在运算量大大减少的情况下，可以在果园环境中达到接近效果，且可以完成不需要预先进行学习的多种类障碍物自动检测识别效果。

Description

一种基于传感器融合目标检测的作物行间作业导航方法
技术领域
本发明涉及数字化农业机械技术领域，特别是涉及一种基于传感器融合目标检测的作物行间作业导航方法。
背景技术
随着农业机械技术的发展，农业机械向自动化、智能化方向的发展成为当今农业研究的热点。一种在田间环境下，精度较高、使用便捷，鲁棒性好的农业机械全自动驾驶系统，在农业生产中有着非常重要的作用，它可以降低人工成本，提高作业效率，带来良好的经济效益。农业机械在作物行间导航需要识别作物行，由于GNSS传感器信号在树木遮挡情况下易丢失，泥土与天气会影响相机的图像数据质量以及高线束激光雷达数据量过大且成本高昂等问题，导致单一传感器难以实现果园精确的环境感知。融合目标检测算法中通常使用基于特征的雷达目标检测算法与视觉融合，这种方法的运算量要求较大，同时农业机械常用的PID控制算法与MPC路径跟踪算法也需要较高算力，不适用于农业机械算力较低的嵌入式设备。
为了解决上述问题，本发明提出了一种基于传感器融合目标检测的作物行间作业导航方法。
发明内容
本发明的目的在于提供一种基于传感器融合目标检测的作物行间作业导航方法以解决背景技术中所提出的问题。
为了实现上述目的，本发明采用了如下技术方案：
一种基于传感器融合目标检测的作物行间作业导航方法，所述方法基于传感器融合目标检测的作物行间作业导航系统进行，所述系统包括有行进设备、相机、激光雷达、IMU传感器、轮胎转向角传感器和总控制器；
所述方法具体包括以下步骤：
S1、在行进设备上设置相机的拍摄角度以及激光雷达的位置和探测角度；
S2、初始化相机、激光雷达和IMU传感器；
S3、联合标定相机、激光雷达和IMU传感器的坐标系；
S4、利用视觉目标检测算法识别目标作物；
S5、进行点云预处理，基于密度法对激光雷达点云进行聚类，通过体积阈值判断检测目标与障碍物；
S6、利用S5中所得的目标检测结果进行决策融合，获取目标点的相对坐标；
S7、行进过程中，对环境进行识别，依据环境作物位置自动切换行走模式；
S8、利用IMU传感器获取行进设备的机械运动状态；
S9、计算S8中所得运动状态与规划路径的差值，进行滑模控制跟踪，完成作物行间作业导航工作。
优选地，S3中所述联合标定具体包括以下内容：
S3.1、相机坐标系标定：对相机进行标定，获取相机的内参；
S3.2、相机坐标系与激光雷达坐标系联合标定：
将相机坐标系表示为OC-XCYCZC，将激光雷达坐标系表示为OL-XLYLZL，将激光雷达坐标系与摄像机坐标系之间的位姿关系分解为绕坐标原点O的旋转和平移，具体计算公式如下：
其中，R为旋转矩阵，代表激光雷达坐标系与摄像机坐标系之间的角位移关系；T为平移矩阵，表示激光雷达坐标系原点在摄像机坐标系中的坐标，代表激光雷达坐标系与摄像机坐标系之间的相对位置关系；所述旋转矩阵R和平移矩阵T通过空间点匹配方案获取；
S3.3、激光雷达坐标系与IMU坐标系联合标定：
将IMU坐标系表示为OU-XUYUZU，IMU坐标系与激光雷达坐标系方向一致，因此转换关系为：
通过卷积运算方法消除IMU坐标系与激光雷达坐标系的位置偏差，获得旋转矩阵R’和平移矩阵T’。
优选地，所述S4具体包括以下内容：
S4.1、采集多种环境、条件下的目标农作物图像，构建数据集，并将数据集划分为训练集和验证集；
S4.2、基于S4.1中所得训练集，采用随机梯度下降法对视觉目标检测网络进行训练，并利用验证集对训练后的网络进行验证；当验证集的分类精度停止增加时，则降低学习率，直至通过调整学习率不再提高识别率为止；
S4.2、使用训练权重文件对环境进行检测，获取环境中目标图像信息，赋予目标2D包围框与标签，获得目标在图像坐标系位置信息。
优选地，所述S5具体包括以下内容：
S5.1、保留激光雷达正前方前云，去除地面点云，通过质心体素进行降采样处理，根据目标种植特征与机械运行速度进行点云分割；
S5.2、基于点云密度算法对点云进行处理，具体聚类原理如下：
S5.2.1、初始化邻域c、阈值ρ，将整体点云集合记为D，将其变量M赋值为J，聚类编号N＝0；
S5.2.2、从D集合中取一点a，如果M＝F，则选择其它点；若M＝K，则判断其是否为核心点；其中，F表示未访问，K表示已访问；
S5.2.3、若该点半径L范围内点云数量小于ρ，则其为噪声点，计N＝-1；若点云数量大于ρ，寻找其密度可达点，并对其编号N＝N+1，计其中所有点M＝K；
S5.2.4、重复S5.2.2、S5.2.3，直至所有点云M＝K，则聚类完成，输出所有点云簇Ni；
S5.3、聚类完成后，通过点云簇边界点赋予其垂直于地面的四条框线，根据其地面点赋予其底面与顶面上的包围线，由此获取其3D包围框；
S5.4、通过3D包围框的三维坐标获得其横向宽度以及纵向高度，并计算其点云簇体积；
S5.5、根据目标作物的大致宽度、高度设定一检测阈值，然后根据检测阈值对已聚类点云进行分类，在阈值范围内的点云簇赋予目标标签；非阈值内目标不赋予标签作为障碍物；过滤小于行进机械轮廓通过性最小离地间隙，接近角大小的点云簇。
优选地，所述S6具体包括以下内容：
S6.1、获取激光雷达数据发布帧率与相机数据发布帧率，取其帧率较低者帧率作为同步频率基准，通过二者数据时间戳，将相近时间数据按照同步频率进行匹配，同时删除多余数据；通过旋转矩阵R’和平移矩阵T’，修正激光雷达与相机之间的位置偏差与角度偏差；
S6.2、将带有标签的激光雷达3D包围框与目标检测包围框空间位置进行投影匹配，获得二者在三维空间中位置关系，将二者位置关系分为独立与相交，并求取相交3D包围框与目标检测包围框二者目标框交并比；
S6.3、通过目标框位置关系对检测目标真实位置进行判断，获取所有不相交包围框，认定其皆为单独目标，输出其包围框为真实框；获取所有目标包围框交并比小于重合阈值的包围框，认定其也为单独目标，输出其包围框为真实框；获取所有目标包围框交并比大于重合阈值的包围框，以其二维包围框作为真实框，删除与之相交且交并比大于重合阈值的3D包围框；
S6.4、将上述所有真实框的几何中心位置作为导航点，输出其导航点坐标(xn，yn，zn)。
优选地，所述S7具体包括以下内容：
S7.1：
S7.1.1、系统启动并初始化，对环境进行识别，将前方的目标按照角度划分为0～90°点云集合Q，以及270～360°点云集合P，用来区分左右目标；当点云集合I或J其中一个没有目标导航点时，机器开始单行识别开始作业；
S7.1.2、识别ln值最小点Qn或Pn，记录其坐标为O(X0，Y0，Z0)，取雷达坐标A点为原点，则导航点坐标为(r-X0，Y，0)，坐标系以激光雷达坐标系表示r为A点据障碍物点X轴距离；
S7.1.3、重复S7.1.2寻找下一导航点，直至驶入行末，进行行末转向，开始作业；
S7.2：当机械行走至行末，需要转入下一行以继续作业；
S7.2.1、当目标点Qn，Pn从雷达视野中消失，且点云集合Q与P都无目标导航点时，系统认为已行走至行末，并进行行末转向作业；
S7.2.2、若开始作业时，取以变量i，若点云集合P没有目标则计i＝0，若点云集合Q没有目标则i＝1；
S7.2.3、当i＝0时，机器向前行走一段路径r，其长度为导航点B至目标点Qn直线距离，完成后向右转90°，行走一段路径2r，再向右旋转90°，此时转向完成，并继续进行行间行走作业，i＝i+1；
当i＝1时，则行走距离为B至点Pn距离，转向方向向左，且转向完成后i＝i-1；
S7.2.4、在完成第一次转向作业后，其后转向算法行走距离r’取目标点坐标Qn(xq，yq，zq)，Pn(xp，yp，zp)在水平方向上投影的x轴距离，即PnQn在x轴上的投影长度；
S7.3：
S7.3.1、行驶至行间后将前方的目标按照角度划分为0°～90°点云集合Q，以及270°～360°点云集合P，用来区分左右目标；
S7.3.2、根据目标检测结果，输出目标检测的边界框或者雷达的3D包围框中心点与雷达之间距离l，目标中心点作为标记点n；
S7.3.3、选取0°～90°，270°～360°中标记点中ln值最小的点记为Qn，Pn，将其作为导航点；
S7.3.4、获得Qn，Pn坐标(xr，yr，zr)，(xl，yl，zl)，雷达坐标点为原点A并求出其中点在雷达高度上的投影B(xm，ym，0)作为导航点，计算公式如下：
S7.3.5、雷达坐标A(X，Y，Z)导航点B(xm，ym，Z)的连线AB即为路径，重复以上步骤，直至行走至行末；
S7.3.6、当单侧目标Pn出现漏检时，其左侧目标点位置向前移动至点Pn’，其导航点从B点向前移动至B’点，若再次出现漏检，则目标点继续前移至点Pn”，导航点向前移动至点B”；
S7.3.7、当双侧第一组目标Qn，Pn都出现漏检时，系统直接选取下一组导航点Qn’，Pn’，其导航点从B点向前移动至B”点；
S7.3.8、基于上述内容，得出原始路径AB与漏检路径AB’、AB”形成锐角三角形，若漏检数目增加，则∠BAB”＜∠BAB’，即偏航角随漏检距离增加而减小，此路径更加贴近原始路径。
S7.4：
S7.4.1、在行末检测完成之后，进行环境目标检测，当i＝1，点云集合P没有导航点，Q有导航点；或i＝1，点云集合Q没有导航点，P有导航点时，认为此行为结束行；
S7.4.2、当行进至某一点时，无法检测到任何导航点，则继续向前行走距离r，若仍无目标则停止运动；
S7.5：
在上述任一步骤行驶时，当行进路径中出现一无标签3D包围框，且其没有大于重合阈值的2D包围框，则不将其作为导航点，将其作为障碍物处理，然后采用基于6阶贝塞尔曲线的绕障路径对其进行避障路径规划。
优选地，所述S8具体包括以下内容：
S8.1、导航标定线的角度和方向控制轮式机械的电机控制器，通过控制前轮转向角和电机转速来控制植保机械的转向和移动；车轮转角和车身转角之间的关系式为：
x’(t)＝v(t)cosφ(t) (10)
y’(t)＝v(t)sinφ(t) (11)
S＝W/tanα (13)
式中，α为前轮转角，为航向角，S为转弯半径，W为前后轮轴距，上述方程表示输出位置和输入控制变量速度和前轮偏角之间的关系；
S8.2、通过标定矩阵，将雷达坐标系与IMU坐标系偏差修正，利用IMU三轴加速度信息获得瞬时速度，通过IMU三轴角度信息获得机械运行时的俯仰角、翻滚角、航向角信息。
S8.3、将底盘角速度表示为ω，整体速度表示为v，左侧轮速度表示为v1，右侧轮速度表示为v2，左右轮间距表示为U，可得任意时刻运动状态公式为：
v＝v1+v2/2 (14)
ω＝v1-v2/U (15)
S8.4、当规划路径坐标为(x，y)，航向角为θ时，则轴向偏移距离(x’，y’)，航向角偏移为θ’，取位姿控制参数m＝(xr，yr，θr)T，速度控制参数n＝(vr，wr)T，角度角速度关系如下所示：
联立公式(16)与公式(17)计算得角度控制方程：
优选地，所述S9具体包括以下内容：
S9.1、使用Sigmoid滑模切换函数对其进行设计，设S1为x’切换函数，S1为θ’切换函数：
上式中，k为大于0常数，α为角度参数，c为大于0常数，vr为驱动轮速度；
S9.2、跟踪路径时发送速度指令[vd，wd]T，则小车左右侧驱动轮驱动轮的角速度ω1，ω2，具体公式如下：
ω1＝(vd(t)-wd])r-1 (20)
ω2＝(vd(t)+wd])r-1 (21)
通过如上控制参数与IMU数据获得当前行进修正参数，从而进行路径跟踪。
与现有技术相比，本发明提供了一种基于传感器融合目标检测的作物行间作业导航方法，具备以下有益效果：
(1)本发明所提出的一种基于传感器融合目标检测的作物行间作业导航方法，运用计算机视觉以及深度学习算法使用相机识别农作物，再利用激光雷达进行基于点云聚类的目标检测与障碍物分类，并融合二者检测结果。基于点云聚类的雷达目标检测方法，相较于常用的基于特征提取的激光雷达目标检测方法在运算量大大减少的情况下，可以在果园环境中达到接近效果，且可以完成不需要预先进行学习的多种类障碍物自动检测识别效果。
(2)本发明提出一种融合方法对目标检测结果进行决策融合以获取目标点的相对坐标，该设计在低运算量的情况下提高了目标检测效果，具有较好的性能，由于异质传感器独立工作，在某一传感器失效的情况下，其余传感器可以独立运行，并可以完成部分作业，极大的提高了系统的稳定性与鲁棒性。将激光雷达和相机数据发送给总控制器，总控制器对数据进行融合转换分析，在作物行间规划出一条导航路径，路径规划方法基于导航点位置判断，当传感器发生失效时，传统基于导航线提取的路径规划方法会发生偏移，本方法原理上可以保证当临近导航点丢失时可以根据后方导航点不间断规划路径，且偏移量较小，同时多导航点丢失时本算法仍可以继续进行路径规划，且偏移量不会随导航点丢失增大。
(3)本发明根据IMU数据与激光雷达数据调整轮胎的转速以调整植保机械的运动状态，根据导航标定线的角度和方向，通过滑模控制方程控制植保机械的轮胎转速，通过控制前轮转角来控制植保机械的移动和转向。相比较于使用较多的PID控制跟踪与纯跟踪方法，滑模控制法的性质更适用于本非线性模型，且参数设定方法较PID更为简单，运算量低，调节跟踪速度快，更适用于农业机械中的嵌入式设备。
(4)本发明中行走模式切换基于环境感知算法自主实现，根据前方左右点云目标数量判断当前行走模式并主动切换，提高了本系统的自动化程度，降低了使用复杂程度。
综上所述，本发明所提出的基于传感器融合目标检测的作物行间作业导航方法运算量需求低，精度高，自动化程度高，适合于农业生产中进行推广。
附图说明
图1为本发明实施例1中提到的系统结构示意图；
图2为本发明实施例1中多传感器标定示意图；
图3为本发明实施例1中点云聚类原理图；
图4为本发明实施例1中行走模式切换识别示意图；
图5为本发明实施例1中导航点修正路径规划示意图；
图6为本发明实施例2中试验平台示意图；
图7为本发明实施例2中ROS软件模块框架示意图；
图8为本发明实施例2中下位机开发板示意图；
图9为本发明实施例2中试验场景图；
图10为本发明实施例2中行侧行走路径示意图；
图11为本发明实施例2中行侧行走IMU数据图；
图12为本发明实施例2中行间行走路径示意图；
图13为本发明实施例2中行间行走IMU数据图；
图14为本发明实施例2中行末转向路径示意图；
图15为本发明实施例2中行末转向IMU数据图；
图16为本发明实施例2中避障作业检测示意图；
图17为本发明实施例2中避障作业路径示意图；
图18为本发明实施例2中避障作业IMU数据图。
附图标记说明：
1、总控制器；2、IMU传感器；3、相机；4、激光雷达；5、轮胎转角传感器。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。
实施例1：
本发明提出一种基于传感器融合目标检测的作物行间作业导航方法，方法基于传感器融合目标检测的作物行间作业导航系统进行，请参阅图1，系统包括有行进设备、相机3、激光雷达4、IMU传感器2、轮胎转向角传感器5和总控制器1；
方法具体包括以下步骤：
S1、在行进设备上设置相机的拍摄角度以及激光雷达的位置和探测角度；
S2、初始化相机、激光雷达和IMU传感器；
S3、联合标定相机、激光雷达和IMU传感器的坐标系；
其中，联合标定具体包括以下内容：
S3.1、相机坐标系标定：对相机进行标定，获取相机的内参；
S3.2、相机坐标系与激光雷达坐标系联合标定：
将相机坐标系表示为OC-XCYCZC，将激光雷达坐标系表示为OL-XLYLZL，将激光雷达坐标系与摄像机坐标系之间的位姿关系分解为绕坐标原点O的旋转和平移，具体计算公式如下：
其中，R为旋转矩阵，代表激光雷达坐标系与摄像机坐标系之间的角位移关系；T为平移矩阵，表示激光雷达坐标系原点在摄像机坐标系中的坐标，代表激光雷达坐标系与摄像机坐标系之间的相对位置关系；所述旋转矩阵R和平移矩阵T通过空间点匹配方案获取；
S3.3、激光雷达坐标系与IMU坐标系联合标定：
将IMU坐标系表示为OU-XUYUZU，IMU坐标系与激光雷达坐标系方向一致，因此转换关系为：
通过卷积运算方法消除IMU坐标系与激光雷达坐标系的位置偏差，获得旋转矩阵R’和平移矩阵T’；
S4、利用视觉目标检测算法识别目标作物；
具体包括以下内容：
S4.1、采集多种环境、条件下的目标农作物图像，构建数据集，并将数据集划分为训练集和验证集；
S4.2、基于S4.1中所得训练集，采用随机梯度下降法对视觉目标检测网络进行训练，并利用验证集对训练后的网络进行验证；当验证集的分类精度停止增加时，则降低学习率，直至通过调整学习率不再提高识别率为止；
S4.2、使用训练权重文件对环境进行检测，获取环境中目标图像信息，赋予目标2D包围框与标签，获得目标在图像坐标系位置信息；
S5、进行点云预处理，基于密度法对激光雷达点云进行聚类，通过体积阈值判断检测目标与障碍物；
S5具体包括以下内容：
S5.1、保留激光雷达正前方前云，去除地面点云，通过质心体素进行降采样处理，根据目标种植特征与机械运行速度进行点云分割；
S5.2、基于点云密度算法对点云进行处理，具体聚类原理如下：
S5.2.1、初始化邻域c、阈值ρ，将整体点云集合记为D，将其变量M赋值为J，聚类编号N＝0；
S5.2.2、从D集合中取一点a，如果M＝F，则选择其它点；若M＝K，则判断其是否为核心点；其中，F表示未访问，K表示已访问；
S5.2.3、若该点半径L范围内点云数量小于ρ，则其为噪声点，计N＝-1；若点云数量大于ρ，寻找其密度可达点，并对其编号N＝N+1，计其中所有点M＝K；
S5.2.4、重复S5.2.2、S5.2.3，直至所有点云M＝K，则聚类完成，输出所有点云簇Ni；
S5.3、聚类完成后，通过点云簇边界点赋予其垂直于地面的四条框线，根据其地面点赋予其底面与顶面上的包围线，由此获取其3D包围框；
S5.4、通过3D包围框的三维坐标获得其横向宽度以及纵向高度，并计算其点云簇体积；
S5.5、根据目标作物的大致宽度、高度设定一检测阈值，然后根据检测阈值对已聚类点云进行分类，在阈值范围内的点云簇赋予目标标签；非阈值内目标不赋予标签作为障碍物；过滤小于行进机械轮廓通过性最小离地间隙，接近角大小的点云簇；
S6、利用S5中所得的目标检测结果进行决策融合，获取目标点的相对坐标；
具体包括以下内容：
S6.1、获取激光雷达数据发布帧率与相机数据发布帧率，取其帧率较低者帧率作为同步频率基准，通过二者数据时间戳，将相近时间数据按照同步频率进行匹配，同时删除多余数据；通过旋转矩阵R’和平移矩阵T’，修正激光雷达与相机之间的位置偏差与角度偏差；
S6.2、将带有标签的激光雷达3D包围框与目标检测包围框空间位置进行投影匹配，获得二者在三维空间中位置关系，将二者位置关系分为独立与相交，并求取相交3D包围框与目标检测包围框二者目标框交并比；
S6.3、通过目标框位置关系对检测目标真实位置进行判断，获取所有不相交包围框，认定其皆为单独目标，输出其包围框为真实框；获取所有目标包围框交并比小于重合阈值的包围框，认定其也为单独目标，输出其包围框为真实框；获取所有目标包围框交并比大于重合阈值的包围框，以其二维包围框作为真实框，删除与之相交且交并比大于重合阈值的3D包围框；
S6.4、将上述所有真实框的几何中心位置作为导航点，输出其导航点坐标(xn，yn，zn)；
S7、行进过程中，对环境进行识别，依据环境作物位置自动切换行走模式；
具体包括以下内容：
S7.1：
S7.1.1、系统启动并初始化，对环境进行识别，将前方的目标按照角度划分为0～90°点云集合Q，以及270～360°点云集合P，用来区分左右目标；当点云集合I或J其中一个没有目标导航点时，机器开始单行识别开始作业；
S7.1.2、识别ln值最小点Qn或Pn，记录其坐标为O(X0，Y0，Z0)，取雷达坐标A点为原点，则导航点坐标为(r-X0，Y，0)，坐标系以激光雷达坐标系表示r为A点据障碍物点X轴距离；
S7.1.3、重复S7.1.2寻找下一导航点，直至驶入行末，进行行末转向，开始作业；
S7.2：当机械行走至行末，需要转入下一行以继续作业；
S7.2.1、当目标点Qn，Pn从雷达视野中消失，且点云集合Q与P都无目标导航点时，系统认为已行走至行末，并进行行末转向作业；
S7.2.2、若开始作业时，取以变量i，若点云集合P没有目标则计i＝0，若点云集合Q没有目标则i＝1；
S7.2.3、当i＝0时，机器向前行走一段路径r，其长度为导航点B至目标点Qn直线距离，完成后向右转90°，行走一段路径2r，再向右旋转90°，此时转向完成，并继续进行行间行走作业，i＝i+1；
当i＝1时，则行走距离为B至点Pn距离，转向方向向左，且转向完成后i＝i-1；
S7.2.4、在完成第一次转向作业后，其后转向算法行走距离r’取目标点坐标Qn(xq，yq，zq)，Pn(xp，yp，zp)在水平方向上投影的x轴距离，即PnQn在x轴上的投影长度；
S7.3：
S7.3.1、行驶至行间后将前方的目标按照角度划分为0°～90°点云集合Q，以及270°～360°点云集合P，用来区分左右目标；
S7.3.2、根据目标检测结果，输出目标检测的边界框或者雷达的3D包围框中心点与雷达之间距离l，目标中心点作为标记点n；
S7.3.3、选取0°～90°，270°～360°中标记点中ln值最小的点记为Qn，Pn，将其作为导航点；
S7.3.4、获得Qn，Pn坐标(xr，yr，zr)，(xl，yl，zl)，雷达坐标点为原点A并求出其中点在雷达高度上的投影B(xm，ym，0)作为导航点，计算公式如下：
S7.3.5、雷达坐标A(X，Y，Z)导航点B(xm，ym，Z)的连线AB即为路径，重复以上步骤，直至行走至行末；
S7.3.6、当单侧目标Pn出现漏检时，其左侧目标点位置向前移动至点Pn’，其导航点从B点向前移动至B’点，若再次出现漏检，则目标点继续前移至点Pn”，导航点向前移动至点B”；
S7.3.7、当双侧第一组目标Qn，Pn都出现漏检时，系统直接选取下一组导航点Qn’，Pn’，其导航点从B点向前移动至B”点；
S7.3.8、基于上述内容，得出原始路径AB与漏检路径AB’、AB”形成锐角三角形，若漏检数目增加，则∠BAB”＜∠BAB’，即偏航角随漏检距离增加而减小，此路径更加贴近原始路径。
S7.4：
S7.4.1、在行末检测完成之后，进行环境目标检测，当i＝1，点云集合P没有导航点，Q有导航点；或i＝1，点云集合Q没有导航点，P有导航点时，认为此行为结束行；
S7.4.2、当行进至某一点时，无法检测到任何导航点，则继续向前行走距离r，若仍无目标则停止运动；
S7.5：
在上述任一步骤行驶时，当行进路径中出现一无标签3D包围框，且其没有大于重合阈值的2D包围框，则不将其作为导航点，将其作为障碍物处理，然后采用基于6阶贝塞尔曲线的绕障路径对其进行避障路径规划；
S8、利用IMU传感器获取行进设备的机械运动状态；
具体包括以下内容：
S8.1、导航标定线的角度和方向控制轮式机械的电机控制器，通过控制前轮转向角和电机转速来控制植保机械的转向和移动；车轮转角和车身转角之间的关系式为：
x’(t)＝v(t)cosφ(t) (10)
y’(t)＝v(t)sinφ(t) (11)
S＝W/tanα (13)
式中，α为前轮转角，为航向角，S为转弯半径，W为前后轮轴距，上述方程表示输出位置和输入控制变量速度和前轮偏角之间的关系；/>
S8.2、通过标定矩阵，将雷达坐标系与IMU坐标系偏差修正，利用IMU三轴加速度信息获得瞬时速度，通过IMU三轴角度信息获得机械运行时的俯仰角、翻滚角、航向角信息。
S8.3、将底盘角速度表示为ω，整体速度表示为v，左侧轮速度表示为v1，右侧轮速度表示为v2，左右轮间距表示为U，可得任意时刻运动状态公式为：
v＝v1+v2/2 (14)
ω＝v1-v2/U (15)
S8.4、当规划路径坐标为(x，y)，航向角为θ时，则轴向偏移距离(x’，y’)，航向角偏移为θ’，取位姿控制参数m＝(xr，yr，θr)T，速度控制参数n＝(vr，wr)T，角度角速度关系如下所示：
联立公式(16)与公式(17)计算得角度控制方程：
S9、计算S8中所得运动状态与规划路径的差值，进行滑模控制跟踪，完成作物行间作业导航工作；
具体包括以下内容：
S9.1、使用Sigmoid滑模切换函数对其进行设计，设S1为x’切换函数，S1为θ’切换函数：
上式中，k为大于0常数，α为角度参数，c为大于0常数，vr为驱动轮速度；
S9.2、跟踪路径时发送速度指令[vd，wd]T，则小车左右侧驱动轮驱动轮的角速度ω1，ω2，具体公式如下：
ω1＝(vd(t)-wd])r-1 (20)
ω2＝(vd(t)+wd])r-1 (21)
通过如上控制参数与IMU数据获得当前行进修正参数，从而进行路径跟踪。
综上所述，本发明所提出的一种基于传感器融合目标检测的作物行间作业导航方法，运用计算机视觉以及深度学习算法使用相机识别农作物，再利用激光雷达进行基于点云聚类的目标检测与障碍物分类，并融合二者检测结果。基于点云聚类的雷达目标检测方法，相较于常用的基于特征提取的激光雷达目标检测方法在运算量大大减少的情况下，可以在果园环境中达到接近效果，原理如图3所示。且可以完成不需要预先进行学习的多种类障碍物自动检测识别效果。同时本方法中的决策级融合算法在低运算量的情况下提高了目标检测效果，具有较好的性能，由于异质传感器独立工作，在某一传感器失效的情况下，其余传感器可以独立运行，并可以完成部分作业，极大的提高了系统的稳定性与鲁棒性。将激光雷达和相机数据发送给总控制器，总控制器对数据进行融合转换分析，在作物行间规划出一条导航路径，路径规划方法基于导航点位置判断，当传感器发生失效时，传统基于导航线提取的路径规划方法会发生偏移，本方法原理上可以保证当临近导航点丢失时可以根据后方导航点不间断规划路径，且偏移量较小，同时多导航点丢失时本算法仍可以继续进行路径规划，且偏移量不会随导航点丢失增大，如图5所示。再根据IMU数据与激光雷达数据调整轮胎的转速以调整植保机械的运动状态，根据导航标定线的角度和方向，通过滑模控制方程控制植保机械的轮胎转速，通过控制前轮转角来控制植保机械的移动和转向。相比较于使用较多的PID控制跟踪与纯跟踪方法，滑模控制法的性质更适用于本非线性模型，且参数设定方法较PID更为简单，运算量低，调节跟踪速度快，更适用于农业机械中的嵌入式设备。
本方法中行走模式切换基于环境感知算法自主实现，根据前方左右点云目标数量判断当前行走模式并主动切换，提高了本系统的自动化程度，降低了使用复杂程度，如图4所示。整体来说，本系统结构简单，方法运算量需求低，精度高，自动化程度高，适合于农业生产中进行推广。
实施例2：
基于实施例1但有所不同之处在于，为验证本发明所提出的基于传感器融合目标检测的作物行间作业导航方法，搭建软硬件试验平台，进行田间试验，通过试验数据分析系统的可行性与性能。
本发明采用硬件平台结合目标检测、路径规划、路径跟踪等算法开展田间实机试验，并对机器实际运行情况进行记录。通过履带机的田间行走试验，进行图像、雷达与IMU数据采集，并通过IMU数据分析其运行状况。根据其行走效果，对本发明设计的导航方法进行评价。试验地点选择在合肥高新技术农业园。试验平台为3YZ-80A型自走式履带热雾机，具体包括如下内容。
(1)硬件平台构建
本试验平台结构如图6所示。3YZ-80A型履带热雾机，其行走模块采用单片机控制，由于单片机性能较弱，所以只负责驱动轮电机控制，并且多传感器之间无法直接通讯，工作过程相对独立，所以需要上位机对其余部件进行数据处理以及统一调度工作，试验平台与传感器安装位置如图6所示。
试验平台包括ZED2i相机、Velodyne16线机械激光雷达、基于STM32单片机YS-F4Pro开发板、上位机电脑、3YZ-80A型自走式履带热雾机底盘、12V锂电池包、48V锂电池。其中ZED相机包括摄像头传感器以及IMU传感器。传感器通过可调角度的支架固定于履带底盘前部，传感器之间相对位置和角度可以在一定范围内调节，其相对于底盘为刚性连接。
上位机运行ROS系统的x86架构计算机，通过RJ45接口连接雷达控制盒，雷达由12V电池包进行供电，ZED相机通过USB接口进行连接与供电。上位机安装Ubuntu系统，通过ROS系统与传感器进行实时通讯以及对多传感器数据进行同步、融合，以及进行目标识别，点云处理、路径规划、路径跟踪等操作。上位机电脑通过USB转TTL与单片机进行串口通讯，上位机通过serial函数向下位机发送驱动轮角速度信息，下位机接受信息来对底盘速度进行控制。
(2)ROS环境搭建
ROS(Robot Operating System)是一个基于Linux等系统开发的，用于机器人开发的软硬件集成框架。其提供了一套完整开发工具与软件库，包含了软件包管理、设备控制驱动、相关开发函数以及多种进程间通信方式。由于其开源、模块化设计、跨平台支持等特点，可以帮助开发者快速搭建机器人应用，在机器人控制、机器人感知、运动规划、导航等领域被广泛使用，本发明软件均基于ROSMelodic开发，具体节点功能如下：
1.传感器启动节点，启动传感器并发布硬件数据；
2.标定与联合标定节点，订阅节点1的话题，获取传感器标定矩阵，此节点只需要标定时启动一次；
3.数据同步节点，订阅节点1的话题，根据时间戳相近关系，去除多余数据，并发布同步后的数据话题；
4.激光雷达点云数据处理节点，订阅节点3的话题，对其进行分割与降采样处理，发布处理后的点云数据；
5.视觉目标检测节点，订阅节点3的话题，通过深度学习权重文件对相机数据进行目标检测并发布有BoundingBox的图片；
6.点云聚类识别节点，订阅节点4的话题，发布聚类后有标签的目标与无标签的障碍点云簇；
7.融合节点，订阅节点5与节点6的话题，通过节点2的标定矩阵融合识别目标点，并输出目标点坐标；
8.路径规划节点，订阅节点7的话题，根据路径规划策略识别当前运行行并输出相应路径；
9.目标跟踪节点，订阅节点1与节点8的话题，通过节点2的标定矩阵确定IMU相对目标路径位置与方向的偏差，通过滑膜控制方程调整履带驱动轮转速，并发布转速数据；
10.上位机下位机通讯节点，订阅节点9的话题，通过Serial类对象向下位机发送轮速数据。
通过以上节点实现对作业机械的自动驾驶控制。其框架概况如图7所示。
(3)ROS单片机通讯
本试验控制底盘电机的下位机选用基于STM32F407芯片的YS-F4Pro开发板，其拥有性能强、储存空间大、按键丰富，拓展性强等多种优势。同时其接口丰富，不仅包含常见的RS232、RS485串口接口、CAN总线接口、USB接口、RJ45网线接口，还预留步进电机、无刷电机、编码器等外设的接口，因此本开发板满足本试验所需接口以及性能要求。
上位机与STM32单片机通讯有两种方式，第一种是将单片机作为ROS系统下一个节点，通过话题或者服务直接发送消息，但是此方式需要用专用硬件，需要额外成本；第二种方法为创建一个ROS节点，其通过ROS标准通讯模式，获取运算节点数据，并通过串口向单片机发送数据，本方式基于软件实现。本发明采用第二种方法，具体实现方式如下：
1.通过TTL串口数据线连接上位机与下位机；
2.创建一个ROS节点订阅路径跟踪节点发出的数据，并实例化一个Serial类对象，并设置串口的参数(波特率、数据位、停止位等)；
4.在ROS节点中，使用Serial类的write函数，将需要发送的数据转化为字节数组的形式发送到STM32。
5.在STM32上，通过读取串口接收缓存区，获取ROS路径跟踪节点发送的数据，并根据发送的数据对底盘电机进行控制。
下位机实物如图8所示。
(4)田间试验
4.1)试验场景
试验地点在合肥高新技术农业园，南区苗木区域进行试验，此处有灌木，乔木等多种不同树种，根据其平均直径确定点云聚类宽度阈值。将无人车放在作物行间，使其自动行驶，行驶同时使用ROSbag记录相机、激光雷达与IMU数据，通过实际行走路径与试验数据拟合路径程判断其导航效果可靠性。试验环境如图9所示。
目标检测算法对比试验已在第三章进行，进行田间试验主要实验目的是目标检测算法、路径规划与路径跟踪算法搭载于试验平台后，实际运行时的性能。如上图所示，本实验环境中，树木行较为明显，树木间距较均匀，地面有杂草、泥土块与石块，可以模拟真实果园环境。
4.2)田间行走及数据采集
本发明算法识别目标主要为树干部分，不受季节与树木长势影响。根据实际测量，田间树木行间距平均3.2米，同行植株间距3米，为验证实际算法效果，期望试验效果为履带机可以在各环节正常识别，并在设定速度下安全运行且不会偏移路径或产生碰撞，通过观察底盘行走时大致路径对算法可行性进行判断。
试验分为四种工况，行侧行走，行间行走，行末转向以及避障作业。在机器人行走时，录制bag，记录相机、激光雷达与IMU传感器数据，相机与激光雷达数据用来进行第三章的算法测试，融合数据用来拟合算法路径，IMU数据用来拟合出实际路径。
(5)试验数据结果与分析
5.1)评价标准
为了获得履带热雾机在田间行驶过程精度，使机器从树木行开始处出发，本试验设定行走速度为0.6m/s，在行走过程中，以200Hz的频率记录IMU数据，拟合实际行走路径，分析行走路径与规划路径偏差值的最大值、平均值、方差判，以及行驶过程中IMU记录的数据来判断其行走性能与稳定新。
5.2)试验结果分析与评价
通过IMU与激光雷达殿宇数据建立点云地图，通过四种工况下的路径在地图中与目标或障碍物的相对位置；以及履带热雾机三个方向上角速度数据，角速度数据可以判断出应对运行跳动以及偏转时算法恢复跟踪的能力，由以上两种方法判断小车田间行走以及路径调整效果。路径图中，绿色线为IMU拟合实际行走路线，黄色线为算法拟合路径线。
1.行侧行走
截取行侧行走数据，通过IMU数据进行路径线拟合，实际路径与规划路径如图10所示，由于地面不平整，拟合出的路径并不平滑。根据点云地图可以看出，实际行走路径基本符合障碍物分布情况，但中后段偏差值较大。
结合图11所示IMU数据，如表1展示的实际行走路径偏差值、可以看出，在三个方向上运行状况都有所波动，但每一段波动都会在不到1s时间内得到修正，由于单行树木的目标点较少，此工况下定位点的改变会导致履带机的运行产生较大波动。结合路径与IMU数据可以看出，在第一次切换目标点时，产生了较大幅度的偏移，第二次切换目标点时也产生了波动，但从Z轴数据上看，此处有路面起伏。由于本段路面起伏较大，定位目标点只在右侧，导致中后段实际路径与规划路径偏差值较大，本段数据波动值也较大，但系统通过调整仍可以完成行走作业，并未发生失效，在此工况下仍具有可靠性。
表1行侧行走路径偏差
2.行间行走
行间行走由于有两个方向上的目标点作为参照物，整体路径较为平滑，且左右偏移程度小，实际路径与规划路径如图12所示。
通过表2所示路径偏差数据与图13所示IMU数据可以看出，两次较大波动发生在中后段以及结束段，根据Z轴数据可以看出，此两处地面均有较大波动，且最大偏差值15.49cm相较于3.2m的平均行宽仍在可接受范围内，且本工况下平均偏差值小，方差值较小，行走效果较为稳定，本工况下行走效果较好。
表2行间行走路径偏差
3.行末转向
在转向作业中，波动程度较小，除地面起伏影响外波动程度小，路径也并未产生明显拐点，实际路径与规划路径如图14所示。
从表3所示路径偏差数据与图15所示IMU数据，可以看出，波动主要发生在直线曲线变换时，主要原因是运动方向改变，但控制回复程度快，即使是较大波动，恢复通常在1-2s内，对整体运行影响较小，此外还有三次X轴方向上波动，但从Z轴可以看出，此处路面起伏程度大，对行驶会造成影响。此路段由于在树林边缘，整体路面条件较其他位置更差，虽然行走路径波动次数多，但工况下曲线路径为预先规划路径，因此曲线行驶时路径较为平稳，最大偏差值小，平均偏差与方差值均较小，因此行末转向算法在本环境下可靠性较好。
表3行末转向路径偏差
4.避障作业
避障任务中，小车可以准确识别右侧石块障碍物，如图16所示并主动绕障碍，由于地面凹凸在绕障后半段未能完全按照原始路径进行行驶，但其仍通过调整完成行进，偏差值如表4所示，实际路径与规划路径如图17所示。
表4避障作业路径偏差
通过表4所示路径偏差值数据与图18的IMU数据可以看出，此段路面起伏程度大，小车Z轴方向上跳动大，X轴与Y轴方向上基本平稳，在避障结束时，产生较大波动，反映在路径图中则是后半段路径的平滑性小于前半段路径，且避障结束后路面跳动导致避障实际路径产生拐点，三方向上IMU数据都出现较大波动。但避障结束后仍可以根据树木确定目标点从而继续进行正确方向上的行走作业，由于本段路径较短，偏差值也较小，因此本避障算法有效。
综上所述，路面凹凸与作业状态切换都会引起实际行走路径的不平顺与运动速度的跳动，但控制算法调整实时性好，传感器精度足够，履带机可以及时调整路径。从结果中可以看出，四种工况下，系统都可以及时修正路面起伏与运动状况改变导致的运动状况波动，保证路径跟踪的成功，本系统可以保证履带机在0.6m/s的设定速度下完成自动驾驶、自动换行与自动避障作业，达到果园植保机械作业要求。
以上，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。

Claims (6)

1.一种基于传感器融合目标检测的作物行间作业导航方法，其特征在于，所述方法基于传感器融合目标检测的作物行间作业导航系统进行，所述系统包括有行进设备、相机、激光雷达、IMU传感器、轮胎转向角传感器和总控制器；
所述方法具体包括以下步骤：
S1、在行进设备上设置相机的拍摄角度以及激光雷达的位置和探测角度；
S2、初始化相机、激光雷达和IMU传感器；
S3、联合标定相机、激光雷达和IMU传感器的坐标系；
S4、利用视觉目标检测算法识别目标作物；
S5、进行点云预处理，基于密度法对激光雷达点云进行聚类，通过体积阈值判断检测目标与障碍物；
S6、利用S5中所得的目标检测结果进行决策融合，获取目标点的相对坐标；具体包括以下内容：
S6.1、获取激光雷达数据发布帧率与相机数据发布帧率，取其帧率较低者帧率作为同步频率基准，通过二者数据时间戳，将相近时间数据按照同步频率进行匹配，同时删除多余数据；通过旋转矩阵R’和平移矩阵T’，修正激光雷达与相机之间的位置偏差与角度偏差；
S6.2、将带有标签的激光雷达3D包围框与目标检测包围框空间位置进行投影匹配，获得二者在三维空间中位置关系，将二者位置关系分为独立与相交，并求取相交3D包围框与目标检测包围框二者目标框交并比；
S6.3、通过目标框位置关系对检测目标真实位置进行判断，获取所有不相交包围框，认定其皆为单独目标，输出其包围框为真实框；获取所有目标包围框交并比小于重合阈值的包围框，认定其也为单独目标，输出其包围框为真实框；获取所有目标包围框交并比大于重合阈值的包围框，以其二维包围框作为真实框，删除与之相交且交并比大于重合阈值的3D包围框；
S6.4、将上述所有真实框的几何中心位置作为导航点，输出其导航点坐标（xn，yn，zn）；
S7、行进过程中，对环境进行识别，依据环境作物位置自动切换行走模式；
S8、利用IMU传感器获取行进设备的机械运动状态；
S9、计算S8中所得运动状态与规划路径的差值，进行滑模控制跟踪，完成作物行间作业导航工作，具体包括以下内容：
S9.1、使用Sigmoid滑模切换函数对其进行设计，设S 1为x’切换函数，S 2为θ’切换函数：
（19）
上式中，k为大于0常数，α为角度参数，τ为大于0常数，v r为驱动轮速度;
S9.2、跟踪路径时发送速度指令[v d，w d]T，则小车左右侧驱动轮的角速度ω 1，ω 2，具体公式如下：
ω 1=(v d(t)-w d])r -1(20)
ω 2=(v d(t)+w d])r -1(21)
式中，r表示行走距离；通过如上控制参数与IMU数据获得当前行进修正参数，从而进行路径跟踪。
2.根据权利要求1所述的一种基于传感器融合目标检测的作物行间作业导航方法，其特征在于，S3中所述联合标定具体包括以下内容：
S3.1、相机坐标系标定：对相机进行标定，获取相机的内参；
S3.2、相机坐标系与激光雷达坐标系联合标定：
将相机坐标系表示为O C-X C Y C Z C，将激光雷达坐标系表示为O L-X L Y L Z L，将激光雷达坐标系与摄像机坐标系之间的位姿关系分解为绕坐标原点O的旋转和平移，具体计算公式如下：
（1）
其中，R为旋转矩阵，代表激光雷达坐标系与摄像机坐标系之间的角位移关系；T为平移矩阵，表示激光雷达坐标系原点在摄像机坐标系中的坐标，代表激光雷达坐标系与摄像机坐标系之间的相对位置关系；所述旋转矩阵R和平移矩阵T通过空间点匹配方案获取；
S3.3、激光雷达坐标系与IMU坐标系联合标定：
将IMU坐标系表示为O U-X U Y U Z U，IMU坐标系与激光雷达坐标系方向一致，因此转换关系为：
（2）
通过卷积运算方法消除IMU坐标系与激光雷达坐标系的位置偏差，获得旋转矩阵R’和平移矩阵T’。
3.根据权利要求1所述的一种基于传感器融合目标检测的作物行间作业导航方法，其特征在于，所述S4具体包括以下内容：
S4.1、采集多种环境、条件下的目标农作物图像，构建数据集，并将数据集划分为训练集和验证集；
S4.2、基于S4.1中所得训练集，采用随机梯度下降法对视觉目标检测网络进行训练，并利用验证集对训练后的网络进行验证；当验证集的分类精度停止增加时，则降低学习率，直至通过调整学习率不再提高识别率为止；
S4.2、使用训练权重文件对环境进行检测，获取环境中目标图像信息，赋予目标2D包围框与标签，获得目标在图像坐标系位置信息。
4.根据权利要求1所述的一种基于传感器融合目标检测的作物行间作业导航方法，其特征在于，所述S5具体包括以下内容：
S5.1、保留激光雷达正前方前云，去除地面点云，通过质心体素进行降采样处理，根据目标种植特征与机械运行速度进行点云分割；
S5.2、基于点云密度算法对点云进行处理，具体聚类原理如下：
S5.2.1、初始化邻域c、阈值ρ，将整体点云集合记为D，将其变量M赋值为J，聚类编号N=0；
S5.2.2、从D集合中取一点a，如果M=F，则选择其它点；若M=K，则判断其是否为核心点；其中，F表示未访问，K表示已访问；
S5.2.3、若该点半径L范围内点云数量小于ρ，则其为噪声点，计N=−1；若点云数量大于ρ，寻找其密度可达点，并对其编号N=N+1，计其中所有点M=K；
S5.2.4、重复S5.2.2、S5.2.3，直至所有点云M=K，则聚类完成，输出所有点云簇Ni；
S5.3、聚类完成后，通过点云簇边界点赋予其垂直于地面的四条框线，根据其地面点赋予其底面与顶面上的包围线，由此获取其3D包围框；
S5.4、通过3D包围框的三维坐标获得其横向宽度以及纵向高度，并计算其点云簇体积；
S5.5、根据目标作物的大致宽度、高度设定一检测阈值，然后根据检测阈值对已聚类点云进行分类，在阈值范围内的点云簇赋予目标标签；非阈值内目标不赋予标签作为障碍物；过滤小于行进机械轮廓通过性最小离地间隙，接近角大小的点云簇。
5.根据权利要求1所述的一种基于传感器融合目标检测的作物行间作业导航方法，其特征在于，所述S7具体包括以下内容：
S7.1：
S7.1.1、系统启动并初始化，对环境进行识别，将前方的目标按照角度划分为0~90°点云集合Q，以及270~360°点云集合P，用来区分左右目标；当点云集合I或J其中一个没有目标导航点时，机器开始单行识别开始作业；
S7.1.2、识别l n值最小点Qn或Pn，记录其坐标为O(X 0，Y 0，Z 0)，取雷达坐标A点为原点，则导航点坐标为(r-X 0，Y，0)，坐标系以激光雷达坐标系表示r为A点据障碍物点X轴距离；
S7.1.3、重复S7.1.2寻找下一导航点，直至驶入行末，进行行末转向，开始作业；
S7.2：当机械行走至行末，需要转入下一行以继续作业；
S7.2.1、当目标点Qn，Pn从雷达视野中消失，且点云集合Q与P都无目标导航点时，系统认为已行走至行末，并进行行末转向作业；
S7.2.2、若开始作业时，取以变量i，若点云集合P没有目标则计i=0，若点云集合Q没有目标则i=1；
S7.2.3、当i=0时，机器向前行走一段路径r，其长度为导航点B至目标点Qn直线距离，完成后向右转90°，行走一段路径2r，再向右旋转90°，此时转向完成，并继续进行行间行走作业，i=i+1；
当i=1时，则行走距离为B至点Pn距离，转向方向向左，且转向完成后i=i-1；
S7.2.4、在完成第一次转向作业后，其后转向算法行走距离r’取目标点坐标Qn(x q，y q，z q)，Pn(x p，y p，z p)在水平方向上投影的x轴距离，即PnQn在x轴上的投影长度；
S7.3：
S7.3.1、行驶至行间后将前方的目标按照角度划分为0°～90°点云集合Q，以及270°～360°点云集合P，用来区分左右目标；
S7.3.2、根据目标检测结果，输出目标检测的边界框或者雷达的3D包围框中心点与雷达之间距离l，目标中心点作为标记点n；
S7.3.3、选取0°～90°，270°～360°中标记点中l n值最小的点记为Q n，Pn，将其作为导航点；
S7.3.4、获得Q n，P n坐标(x r，y r，z r)，(x l，y l，z l)，雷达坐标点为原点A并求出其中点在雷达高度上的投影B(x m，y m，0)作为导航点，计算公式如下：
(9)
S7.3.5、雷达坐标A(X，Y，Z)导航点B(x m，y m，Z)的连线AB即为路径，重复以上步骤，直至行走至行末；
S7.3.6、当单侧目标Pn出现漏检时，其左侧目标点位置向前移动至点Pn’，其导航点从B点向前移动至B’点，若再次出现漏检，则目标点继续前移至点Pn’’，导航点向前移动至点B’’;
S7.3.7、当双侧第一组目标Q n，Pn都出现漏检时，系统直接选取下一组导航点Q n’，P n’，其导航点从B点向前移动至B’’点;
S7.3.8、基于上述内容，得出原始路径AB与漏检路径AB’、AB’’形成锐角三角形，若漏检数目增加，则∠BAB’’＜∠BAB’，即偏航角随漏检距离增加而减小，此路径更加贴近原始路径；
S7.4：
S7.4.1、在行末检测完成之后，进行环境目标检测，当i=1，点云集合P没有导航点，Q有导航点；或i=1，点云集合Q没有导航点，P有导航点时，认为此行为结束行；
S7.4.2、当行进至某一点时，无法检测到任何导航点，则继续向前行走距离r，若仍无目标则停止运动；
S7.5：
在上述S7.1-S7.4中行驶时，当行进路径中出现一无标签3D包围框，且其没有大于重合阈值的2D包围框，则不将其作为导航点，将其作为障碍物处理，然后采用基于6阶贝塞尔曲线的绕障路径对其进行避障路径规划。
6.根据权利要求1所述的一种基于传感器融合目标检测的作物行间作业导航方法，其特征在于，所述S8具体包括以下内容：
S8.1、导航标定线的角度和方向控制轮式机械的电机控制器，通过控制前轮转向角和电机转速来控制植保机械的转向和移动；车轮转角和车身转角之间的关系式为：
x’(t)=v(t)cosφ(t)（10）
y’(t)=v(t)sinφ(t)（11）
φ’(t)=v(t)tanα(t)/W（12）
S=W/tanα（13）
式中，α为前轮转角，φ为航向角，S为转弯半径，W为前后轮轴距，式（10）-（13）表示输出位置和输入控制变量速度和前轮偏角之间的关系；
S8.2、通过标定矩阵，将雷达坐标系与IMU坐标系偏差修正，利用IMU三轴加速度信息获得瞬时速度，通过IMU三轴角度信息获得机械运行时的俯仰角、翻滚角、航向角信息；
S8.3、将底盘角速度表示为ω，整体速度表示为v，左侧轮速度表示为v 1，右侧轮速度表示为v 2，左右轮间距表示为U，可得任意时刻运动状态公式为：
v=v 1+v 2/2（14）
ω=v 1-v 2/U（15）
S8.4、当规划路径坐标为(x，y)，航向角为θ时，则轴向偏移距离(x’，y’)，航向角偏移为θ’，取位姿控制参数m=(x r，y r，θ r)T，速度控制参数n=(v r，w r)T，角度角速度关系如下所示：
（16）
（17）
联立公式（16）与公式（17）计算得角度控制方程：
（18）。
CN202310865172.4A 2023-07-13 2023-07-13 一种基于传感器融合目标检测的作物行间作业导航方法 Active CN116839570B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310865172.4A CN116839570B (zh) 2023-07-13 2023-07-13 一种基于传感器融合目标检测的作物行间作业导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310865172.4A CN116839570B (zh) 2023-07-13 2023-07-13 一种基于传感器融合目标检测的作物行间作业导航方法

Publications (2)

Publication Number Publication Date
CN116839570A CN116839570A (zh) 2023-10-03
CN116839570B true CN116839570B (zh) 2023-12-01

Family

ID=88166923

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310865172.4A Active CN116839570B (zh) 2023-07-13 2023-07-13 一种基于传感器融合目标检测的作物行间作业导航方法

Country Status (1)

Country Link
CN (1) CN116839570B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117383460B (zh) * 2023-12-13 2024-03-12 甘肃送变电工程有限公司 一种全电动控制的gis设备安装用升降式搬运平台
CN118010009B (zh) * 2024-04-10 2024-06-11 北京爱宾果科技有限公司 一种教育机器人在复杂环境下的多模态导航系统

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110155163A (zh) * 2019-06-05 2019-08-23 福州大学 一种无人驾驶转向系统及车辆
CN112347840A (zh) * 2020-08-25 2021-02-09 天津大学 视觉传感器激光雷达融合无人机定位与建图装置和方法
CN113255481A (zh) * 2021-05-11 2021-08-13 北方工业大学 一种基于无人巡逻车的人群状态检测方法
CN113566833A (zh) * 2021-07-28 2021-10-29 上海工程技术大学 一种多传感器融合的车辆定位方法及系统
CN113671994A (zh) * 2021-09-01 2021-11-19 重庆大学 基于强化学习的多无人机与多无人船巡检控制系统
WO2022022694A1 (zh) * 2020-07-31 2022-02-03 北京智行者科技有限公司 自动驾驶环境感知方法及系统
CN114781514A (zh) * 2022-04-22 2022-07-22 山东大学 一种融合注意力机制的漂浮物目标检测方法及系统
CN115330866A (zh) * 2022-08-09 2022-11-11 杭州云深处科技有限公司 一种融合激光雷达和深度相机点云的建图定位方法
CN116229408A (zh) * 2022-11-22 2023-06-06 重庆邮电大学 一种图像信息与激光雷达点云信息融合的目标识别方法

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8126642B2 (en) * 2008-10-24 2012-02-28 Gray & Company, Inc. Control and systems for autonomously driven vehicles
CN111352112B (zh) * 2020-05-08 2022-11-29 泉州装备制造研究所 基于视觉、激光雷达和毫米波雷达的目标检测方法
CN111713245B (zh) * 2020-05-11 2022-06-21 江苏大学 山地果园仿形自主避障割草机及其控制方法

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110155163A (zh) * 2019-06-05 2019-08-23 福州大学 一种无人驾驶转向系统及车辆
WO2022022694A1 (zh) * 2020-07-31 2022-02-03 北京智行者科技有限公司 自动驾驶环境感知方法及系统
CN112347840A (zh) * 2020-08-25 2021-02-09 天津大学 视觉传感器激光雷达融合无人机定位与建图装置和方法
CN113255481A (zh) * 2021-05-11 2021-08-13 北方工业大学 一种基于无人巡逻车的人群状态检测方法
CN113566833A (zh) * 2021-07-28 2021-10-29 上海工程技术大学 一种多传感器融合的车辆定位方法及系统
CN113671994A (zh) * 2021-09-01 2021-11-19 重庆大学 基于强化学习的多无人机与多无人船巡检控制系统
CN114781514A (zh) * 2022-04-22 2022-07-22 山东大学 一种融合注意力机制的漂浮物目标检测方法及系统
CN115330866A (zh) * 2022-08-09 2022-11-11 杭州云深处科技有限公司 一种融合激光雷达和深度相机点云的建图定位方法
CN116229408A (zh) * 2022-11-22 2023-06-06 重庆邮电大学 一种图像信息与激光雷达点云信息融合的目标识别方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
农业机械导航技术研究进展;张漫等;《农业机械学报》;第51卷(第4期);全文 *
基于信息融合的农业自主车辆障碍物检测方法;薛金林等;《农业机械学报》;第49卷;全文 *
果园作业机器人自主导航控制系统研究与设计;冀鹏;《中国优秀硕士学位论文全文数据库 信息科技辑》;全文 *

Also Published As

Publication number Publication date
CN116839570A (zh) 2023-10-03

Similar Documents

Publication Publication Date Title
CN116839570B (zh) 一种基于传感器融合目标检测的作物行间作业导航方法
Badue et al. Self-driving cars: A survey
Royer et al. Outdoor autonomous navigation using monocular vision
JP5227065B2 (ja) 三次元機械地図、三次元機械地図生成装置、ナビゲーション装置及び自動運転装置
CN109556615A (zh) 基于自动驾驶的多传感器融合认知的驾驶地图生成方法
CN107600067A (zh) 一种基于多视觉惯导融合的自主泊车系统及方法
Wulf et al. Colored 2D maps for robot navigation with 3D sensor data
CN106406338A (zh) 一种基于激光测距仪的全向移动机器人的自主导航装置及其方法
CN113359710B (zh) 一种基于los理论的农机路径跟踪方法
Nehme et al. Lidar-based structure tracking for agricultural robots: Application to autonomous navigation in vineyards
CN114063615B (zh) 棚内垄间农药喷洒智能车的倒车导航控制方法及系统
Zhang et al. 3D perception for accurate row following: Methodology and results
CN112965481A (zh) 基于点云地图的果园作业机器人无人驾驶方法
CN110806585B (zh) 一种基于树干聚类跟踪的机器人定位方法及系统
Liu et al. Real-time 6d lidar slam in large scale natural terrains for ugv
CN109753075B (zh) 一种基于视觉的农林园区机器人导航方法
CN106556395A (zh) 一种基于四元数的单目视觉系统的导航方法
Nie et al. A forest 3-D LiDAR SLAM system for rubber-tapping robot based on trunk center atlas
CN112991435B (zh) 基于3D LiDAR的果园行末、行首识别方法
Kais et al. Towards outdoor localization using gis, vision system andstochastic error propagation
Ribeiro et al. 3D monitoring of woody crops using an unmanned ground vehicle
CN112445205A (zh) 地面植保设备的控制方法、装置、设备和存储介质
Yu et al. A camera/ultrasonic sensors based trunk localization system of semi-structured orchards
Gao et al. Canopy volume measurement of fruit trees using robotic platform loaded LiDAR data
Uzawa et al. Dataset Generation for Deep Visual Navigation in Unstructured Environments

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant