CN113310488A - 一种基于slam的果园机器人导航方法 - Google Patents

一种基于slam的果园机器人导航方法 Download PDF

Info

Publication number
CN113310488A
CN113310488A CN202110472508.1A CN202110472508A CN113310488A CN 113310488 A CN113310488 A CN 113310488A CN 202110472508 A CN202110472508 A CN 202110472508A CN 113310488 A CN113310488 A CN 113310488A
Authority
CN
China
Prior art keywords
orchard
robot
trunk
map
slam
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.)
Pending
Application number
CN202110472508.1A
Other languages
English (en)
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.)
Xian Jiaotong University
Original Assignee
Xian Jiaotong University
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 Xian Jiaotong University filed Critical Xian Jiaotong University
Priority to CN202110472508.1A priority Critical patent/CN113310488A/zh
Publication of CN113310488A publication Critical patent/CN113310488A/zh
Pending legal-status Critical Current

Links

Images

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/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/18—Stabilised platforms, e.g. by gyroscope
    • 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
    • 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
    • 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/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
    • Y02A40/00—Adaptation technologies in agriculture, forestry, livestock or agroalimentary production
    • Y02A40/10—Adaptation technologies in agriculture, forestry, livestock or agroalimentary production in agriculture

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种基于SLAM的果园机器人导航方法，果园机器人采用树干方位测量模块、里程计测量模块感知果园环境，并使用基于非线性优化的SLAM方案对感知数据进行融合，构建以二维树干坐标表征的果园地图，从而实现专用于果园环境下的移动机器人高精度建图与全局定位功能；所述果园地图的特征在于包含果园中所有树干的中心二维坐标，及每棵树干的轮廓拟合圆形半径，上述果园地图构建完毕后，还用于实现机器人在果园环境中的路径规划。本发明提出的导航方法能够实现机器人在果园环境中的高精度全局定位与“点到点”式地路径规划，从而达成对果园中各植株及其周边土壤环境进行差异化管理的目标，促进机器人技术在精准农业领域的应用。

Description

一种基于SLAM的果园机器人导航方法
技术领域
本发明属于农业机器人技术领域，具体涉及一种果园机器人基于SLAM(Simultaneous Localization and Mapping)的果园机器人导航方法。
背景技术
精准农业被视为未来农业的发展方向，同时机器人技术的兴起也大大提升了农业的生产效率。果园环境中，精准农业理念下的机器人需对每一植株个体及其周边土壤环境执行高分辨率地信息收集任务，并根据每一植株生长状况定制不同的植保方案，以此显著地节约肥、水、药等资源，并提高优质果率。此外机器人还需自动执行果实采摘、运输等任务。
现有的果园导航方法多是使用传感器提取机器人两侧树行的中心线，并将其作为机器人的目标运动轨迹，这类方法仅能够满足机器人在果园中自主巡航的功能，而在精准农业领域，需要实现厘米级地高精度全局定位与“点到点”式地路径规划，才能实现对各植株及其周边土壤环境的差异化管理。因此，果园环境下机器人的高精度定位与路径规划是必须要实现的技术。
发明内容
本发明提供了一种基于SLAM的果园机器人导航方法，用以解决精准农业理念下机器人在果园环境下的高精度全局定位与路径规划问题。
本发明采用如下技术方案来实现的：
一种基于SLAM的果园机器人导航方法，包括：
果园机器人采用树干方位测量模块、里程计测量模块感知果园环境，并使用基于非线性优化的SLAM方案对感知数据进行融合，构建以二维树干坐标表征的果园地图，从而实现专用于果园环境下的移动机器人高精度建图与全局定位功能；所述果园地图的特征在于包含果园中所有树干的中心二维坐标，及每棵树干的轮廓拟合圆形半径，上述果园地图构建完毕后，还用于实现机器人在果园环境中的路径规划。
本发明进一步的改进在于，树干方位测量模块使用的传感器包括一个单目相机与一个单线激光雷达，其中由单目相机获取环境图像，并基于由深度学习方法训练得到的树干检测模型，实时地检测机器人周围树干在图像中的位置，并标定相机与激光雷达的外参矩阵，将雷达感知的点云投影到图像中，获取图像中树干像素的深度信息，从而得到树干相对机器人的方位。
本发明进一步的改进在于，里程计测量模块使用的传感器包括机器人底盘的轮速计与陀螺仪，这两种传感器的数据通过航迹推算方法计算机器人位姿变换的一种测量。
本发明进一步的改进在于，SLAM方案采用基于非线性优化的SLAM技术将果园建图问题建模成非线性最小二乘问题，求解变量为机器人建图过程中的全运动轨迹与果园地图，步骤为：
I：建图开始，设置机器人初始位姿，初始化非线性最小二乘问题；
II：获取当前时刻树干方位测量模块观测到的树干方位信息；
III：若里程计模块观测到此时机器人位姿相对上一时刻的变换超过设定阈值，则进入步骤IV，否则重复步骤II；
IV：将当前时刻机器人轨迹点加入最小二乘问题待求解变量中，并将当前时刻由里程计测量得到的误差项加入到最小二乘问题的损失函数中；
V：使用极大似然估计方法判断被观测树干的编号，若该树干从未被观测过，则在地图中加入该树干，并初始化其中心二维坐标与半径，将由当前时刻树干方位测量得到的误差项加入到最小二乘问题的损失函数中；
VI：使用非线性优化方法迭代求解该最小二乘问题，最终得到机器人全运动轨迹与果园地图的解，求解结果会作为下一次循环时求解器使用的迭代初值，最后重新进入步骤II；
VII：建图结束，输出果园地图，果园地图构建结束后，定位系统依靠该地图进行重定位，地图在定位过程中不会更新。
本发明进一步的改进在于，用于机器人路径规划时，将上述方法构建的果园地图转化成二值栅格地图，每个栅格的值表示其内部是否包含障碍物，即是否包括树干。
本发明进一步的改进在于，将果园移动机器人的路径规划分为两部分：全局路径规划器与局部轨迹规划器，全局路径规划器用于在果园栅格地图中搜索一条从机器人当前位姿到给定任务位姿的无碰撞最短路径，该路径使用一系列离散的轨迹点表达，局部轨迹规划器用于根据全局路径规划器输出的全局路径与激光雷达感知的障碍物信息，将全局路径优化成一条符合机器人运动学约束，且能躲避果园中动态障碍物的局部轨迹，并实现对机器人底盘的速度控制。
与现有技术相比，本发明至少具有以下有益的技术效果：
本发明提出了一种适用于精准农业的果园机器人高精度建图与全局定位方法，该方法能够从多种传感器数据中提取树干方位信息、机器人底盘里程信息，并通过SLAM技术融合这些信息，最终创建了以二维树干坐标与半径表征的果园地图。另外，本发明还将此地图应用于机器人在果园中的重定位任务。这种建图定位方法能够对果园中的植株与机器人进行厘米级高精度定位，助力精准农业在果园环境下的落地。
进一步的，本发明根据所提出的果园地图形式，配套地提出了一种果园机器人路径规划方法。该路径规划方法不仅能找到从机器人当前位姿到目标任务位姿的最短路径，还能够满足机器人底盘的运动学约束，并且可以躲避动态的障碍物。本发明能够顺利地完成“点到点”式地路径规划，助力达成精准农业机器人自动对果园中植株进行差异化管理的目标。
附图说明
为了更清楚地说明本发明的技术方案，下面将对实施例中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为使用本发明提出果园导航方法的具体实施例的系统框架图；
图2为本发明实施例中机器人建图定位过程的示意图；
图3为本发明实施例使用的果园环境下高精度建图方法的流程框图；
图4为本发明实施例使用极大似然方法判断树干编号的流程框图；
图5为本发明实施例使用的果园环境下高精度全局定位方法的流程框图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
请参阅图1，使用本发明提出果园导航方法的具体实施例的系统框架图。
系统中包含两个计算设备，其中工控机107需要执行三个复杂功能模块的计算任务：树干方位测量模块1073、建图与定位模块1072、路径规划模块1071；而STM32单片机106需要执行两个简单功能模块的计算任务：里程计测量模块1062、底盘速度控制模块1061。工控机107与STM32单片机106通过串口进行数据通讯。
树干方位测量模块1073通过单目相机105获取机器人周围环境图像，并基于由深度学习方法训练得到的树干检测模型10731，实时地检测机器人周围树干在图像中的位置，在本实施例中采用的深度学习算法为Faster R-CNN。经过基于Faster R-CNN的树干检测10732后，可得到树干在图像中的BoundingBox，即树干在图像中的位置。树干方位测量模块1073通过单线激光雷达104获取树干的深度信息，单线激光雷达的点云数据表达形式为一系列(li,θi)，分别表示每一激光束的测量距离与测量角度，其中i表示激光束编号，这些激光束可以转换为在雷达坐标系下的一系列点云(xl,i,yl,i)，即一系列二维坐标，其中l表示激光雷达坐标系。点云投影10734在获取到树干在图像中的BoundingBox后，会读取激光雷达与相机的外参矩阵10733(需提前标定相机与激光雷达的外参矩阵)，从而将点云投影到图片中，设一框住树干的BoundingBox的横向坐标范围为(u1,u2)，选取投影后像素横坐标最靠近
Figure BDA0003045892260000051
的激光束，其对应的点云坐标为(xl,yl)。由于激光雷达坐标系与机器人坐标系的外参可人工测量得到，因此可得此时被观测树干外缘在机器人坐标系中的二维坐标为(xr，，yr)，其中r表示机器人坐标系。因此，当前时刻t树干相对机器人方位的测量值可表达为zt＝(xr，，yr)，(zt的示意图见图2，在后文会进一步描述)，该观测将会传递到建图与定位模块1072作为输入数据。
里程计测量模块1062收集电机编码器102与陀螺仪103的感知数据，并通过航迹推算方法计算机器人位姿变换的一种测量，可以设相邻时刻由里程计获得的机器人二维位姿变换测量结果为ut，(ut的示意图见图2，在后文会进一步描述)，这是一个三维向量，即两个标量表示的二维位移及一个标量表示的二维旋转。里程计测量ut会传递到建图与定位模块1072以及路径规划模块1071作为输入数据。
建图与定位模块1072基于SLAM技术实现果园机器人的高精度建图与全局定位功能。首先在果园地图未知的情况下，首先运行建图步骤10721，构建果园地图10722，并存储在工控机中；然后建图结束后，即可以使用定位步骤10723实现机器人在果园环境中的高精度全局定位功能。同时，机器人在地图坐标系中位姿是路径规划模块1071的输入数据。建图步骤10721的流程框图在图3，定位步骤10723的流程框图在图5，后文会有进一步描述。
路径规划模块1071分为两部分：全局路径规划器10711与局部轨迹规划器10712。在本实施例中使用的全局路径规划算法是A*算法，使用该算法，需将果园地图10722栅格化。全局路径规划器在接收到目标任务位姿后，会在果园地图中搜索一条从机器人当前位姿到给定任务位姿的无碰撞最短路径，即一系列离散的轨迹点。但需要注意的是，全局路径无法预知机器人在行驶过程中是否会碰到地图中所没有的障碍物，即动态障碍物；此外，A*算法将机器人建模成一个质点，其规划的路径不一定满足机器人运动学约束(如差速式底盘无法做到横向移动，而A*规划出的路径却可能是横移的)。因此需要使用局部轨迹规划器10712对全局路径进行优化。而在本发明实例中使用的局部轨迹规划算法是Timed-Elastic-Band(TEB)算法。TEB算法仅优化从当前位姿开始的一段全局路径，并重复执行，直到机器人到达目标位姿。TEB算法将路径优化成能够躲避动态障碍物的轨迹，最终输出速度指令给底盘速度控制模块1061执行。另外，局部轨迹规划器10712需要获取激光雷达104感知的障碍物信息以及里程计测量模块1062的里程计测量。
底盘速度控制模块1061接收到局部轨迹规划器10712发送的速度控制指令后，会将其分解为底盘电机的转速控制指令，并通过电机驱动器101执行。
请参阅图2，为本发明实施例中机器人建图定位过程的示意图。
其中矩形表示机器人，而圆形则是果园中各树干的轮廓拟合圆形，本发明实施例将机器人建图起点时的位姿作为地图坐标系XmOmYm的原点(即201)。图中实线矩形203表示当前时刻t机器人在地图坐标系中的状态，其位姿使用一个三维向量xt表示，即两个标量表示的二维位移及一个标量表示的二维旋转。
图中虚线矩形202表示上一时刻t-1机器人在地图坐标系中的状态，其位姿为xt-1。由里程计获得的机器人从t-1时刻到t时刻的二维位姿变换一种测量，其结果为ut。若里程计测量没有噪声，则应满足等式xt＝g(xt-1，ut)，其中g(·)表示机器人底盘的运动方程，但实际情况中噪声是一定存在的。
图中204的圆形表示果园中一棵树干，该树干中心在地图坐标系中坐标为pj，是一个二维向量，该树干轮廓拟合圆形半径为rj。本发明提出的果园地图即由果园中所有树干中心的二维坐标，以及每棵树干轮廓拟合圆形的半径组成，可表示为(p1：J，r1：J)，其中J表示果园中共有J棵树干，树干标号从1到J；p表示树干中心坐标，是二维向量；r表示树干半径，是一维标量。设t时刻编号为j的树干被机器人观测到，该树干外缘到在机器人坐标系中的坐标由树干方位测量模块1073测量的结果可表示为zt,j，是一个二维向量，同理，若编号为j的树干在t-1时刻被观测到，由树干方位测量模块1073测量的结果为zt-1,j。
要注意，果园中的树干半径大小不一，因此需要分别求解，而本发明实施例使用的优化求解方法在迭代求非线性最小二乘问题时需要初值，因此，需对果园中一部分树干的半径进行人工测量，并描述为一个高斯分布。在建图过程中，每观测到一棵果园地图中还没有的树干时，需设置该树干半径的初值，可从高斯分布中随机采样获得。
若树干方位测量模块没有噪声且能获取准确无误的机器人位姿，则应满足等式zt,j＝h(xt,pj,rj)，或者有pj＝h-1(xt,zt,j,rj)，其中h(·)表示机器人的观测方程，h-1(·)是h(·)的逆方程。毫无疑问，实际情况中误差是一定存在的。
请参阅图3，为本发明实施例使用的果园环境下高精度建图方法的流程框图，即图1中建图与定位模块1072中基于非线性优化的SLAM系统10721的具体算法流程。
301、建图开始，在本发明实施例中，首先设置机器人的初始位姿
Figure BDA0003045892260000071
并初始化非线性最小二乘问题的损失函数为：
Figure BDA0003045892260000072
此时最小二乘问题的待求解变量仅有机器人的初始位姿x0，其中Ωi表示初始误差项的权重，为使初始位姿保持不变，即
Figure BDA0003045892260000073
设
Figure BDA0003045892260000074
302、获取树干方位测量zt。
303、判断里程计测量ut是否超过阈值，若超过则进入步骤304，否则回到步骤302。
304、设当前时刻t的机器人位姿为xt，作为待求解变量加入最小二乘问题中，最小二乘问题的损失函数中添加当前时刻t由里程计测量提供的误差项
Figure BDA0003045892260000081
其中，eg,t＝xt-g(xt-1，ut)，Ωg表示该误差项的权重。
305、极大似然法判断树干编号，(极大似然方法的具体流程见图4，后文会有更进一步说明)极大似然法判断树干编号。若是已观测树干j，损失函数加入误差项
Figure BDA0003045892260000082
若是新观测树干，则令其编号为J+1，将pJ+1、rJ+1加入待求解变量，损失函数加入误差项
Figure BDA0003045892260000083
其中，eh，t，j＝zt-h(xt，pj，rj)为树干方位测量提供的误差项，Ωh表示该误差项的权重。
306、本发明实施例采用高斯牛顿方法作为非线性优化求解器，求解使损失函数f(x0：t，p1：J，r1：J)最小的x0：t，p1：J，r1：J，此时问题的数学形式写为：
Figure BDA0003045892260000084
最小二乘问题的求解结果
Figure BDA0003045892260000085
还将其作为下次迭代求解的初值。
307、由用户决定建图过程是否结束，若结束则进入步骤308，否则回到步骤302。
308、输出果园地图，也就是果园中所有树干中心的二维坐标，以及每棵树干轮廓拟合圆形的半径，即
Figure BDA0003045892260000086
请参阅图4，为本发明实施例使用极大似然方法判断树干编号的流程框图。
401、输入上一时刻机器人位姿
Figure BDA0003045892260000087
及t时刻树干方位观测值zt。
402、使用运动方程估计t时刻机器人在地图坐标系中位姿：
Figure BDA0003045892260000088
403、计算地图已有的J棵树干是当前观测树干的误差，即
Figure BDA0003045892260000089
其中能够得到误差最小值的树干编号为：
Figure BDA00030458922600000810
404、判断
Figure BDA00030458922600000811
是否满足。其中，由于ek是二维向量，因此选择5.99作为卡方检测的阈值。若不满足，则进入步骤405，否则进入步骤406。
405、卡方检测不满足，即观测到了地图中还未有的树干，设置其编号为J+1，从统计半径高斯分布中随机采样作为该树半径初值
Figure BDA0003045892260000091
并根据观测方程的逆方程得到新树干在地图坐标系中的坐标初值
Figure BDA0003045892260000092
随后进入步骤407。
406、卡方检测满足，即认为当前观测树干的编号为k。随后进入步骤407。
407、返回t时刻被观测树干的编号。
请参阅图5，为本发明实施例使用的果园环境下高精度全局定位方法的流程框图，即图1中建图与定位模块1072中基于非线性优化的定位系统10723的具体算法流程。
501、定位开始，在本发明实施例中，首先设置机器人的初始位姿
Figure BDA0003045892260000093
并加载果园地图，即加载
Figure BDA0003045892260000094
再初始化非线性最小二乘问题的损失函数为：
Figure BDA0003045892260000095
此时最小二乘问题的待求解变量仅有机器人的初始位姿x0，其中Ωi表示初始误差项的权重，为使初始位姿保持不变，即
Figure BDA0003045892260000096
设
Figure BDA0003045892260000097
502、获取树干方位测量zt。
503、判断里程计测量ut是否超过阈值，若超过则进入步骤504，则进入步骤508。
504、设当前时刻t的机器人位姿为xt，作为待求解变量加入最小二乘问题中，最小二乘问题的损失函数中添加当前时刻t由里程计测量提供的误差项
Figure BDA0003045892260000098
其中eg,t＝xt-g(xt-1,ut)，Ωg表示该误差项的权重。
505、极大似然法判断树干编号为j，并在损失函数中添加由树干方位测量模块提供的误差项
Figure BDA0003045892260000099
其中，eh,t,j＝zt-h(xt,pj,rj)为树干方位测量提供的误差项,Ωh表示该误差项的权重。
506、本发明实施例采用高斯牛顿方法作为非线性优化求解器，求解使损失函数f(xt-4:t)最小的xt-4:t，此时问题的数学形式写为：
Figure BDA0003045892260000101
本发明实施例在定位系统中使用的非线性最小二乘问题只求解最近五个时刻机器人轨迹点位姿，因此损失函数中与最近五个时刻机器人轨迹点变量不相关的误差项需被删除。求解结果
Figure BDA0003045892260000102
还将其作为下次迭代求解的初值。
507、将
Figure BDA0003045892260000103
作为当前时刻机器人在地图坐标系中的位姿输出，并返回步骤502。
508、由于ut并不大，本发明实施例认为此时ut的误差较小，为加快定位算法的运行速度，直接使用运动方程求解机器人当前时刻的位姿
Figure BDA0003045892260000104
输出位姿后返回步骤502。
对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内。

Claims (6)

1.一种基于SLAM的果园机器人导航方法，其特征在于，包括：
果园机器人采用树干方位测量模块、里程计测量模块感知果园环境，并使用基于非线性优化的SLAM方案对感知数据进行融合，构建以二维树干坐标表征的果园地图，从而实现专用于果园环境下的移动机器人高精度建图与全局定位功能；所述果园地图的特征在于包含果园中所有树干的中心二维坐标，及每棵树干的轮廓拟合圆形半径，上述果园地图构建完毕后，还用于实现机器人在果园环境中的路径规划。
2.根据权利要求1所述的一种基于SLAM的果园机器人导航方法，其特征在于，树干方位测量模块使用的传感器包括一个单目相机与一个单线激光雷达，其中由单目相机获取环境图像，并基于由深度学习方法训练得到的树干检测模型，实时地检测机器人周围树干在图像中的位置，并标定相机与激光雷达的外参矩阵，将雷达感知的点云投影到图像中，获取图像中树干像素的深度信息，从而得到树干相对机器人的方位。
3.根据权利要求1所述的一种基于SLAM的果园机器人导航方法，其特征在于，里程计测量模块使用的传感器包括机器人底盘的轮速计与陀螺仪，这两种传感器的数据通过航迹推算方法计算机器人位姿变换的一种测量。
4.根据权利要求1所述的一种基于SLAM的果园机器人导航方法，其特征在于，SLAM方案采用基于非线性优化的SLAM技术将果园建图问题建模成非线性最小二乘问题，求解变量为机器人建图过程中的全运动轨迹与果园地图，步骤为：
I：建图开始，设置机器人初始位姿，初始化非线性最小二乘问题；
II：获取当前时刻树干方位测量模块观测到的树干方位信息；
III：若里程计模块观测到此时机器人位姿相对上一时刻的变换超过设定阈值，则进入步骤IV，否则重复步骤II；
IV：将当前时刻机器人轨迹点加入最小二乘问题待求解变量中，并将当前时刻由里程计测量得到的误差项加入到最小二乘问题的损失函数中；
V：使用极大似然估计方法判断被观测树干的编号，若该树干从未被观测过，则在地图中加入该树干，并初始化其中心二维坐标与半径，将由当前时刻树干方位测量得到的误差项加入到最小二乘问题的损失函数中；
VI：使用非线性优化方法迭代求解该最小二乘问题，最终得到机器人全运动轨迹与果园地图的解，求解结果会作为下一次循环时求解器使用的迭代初值，最后重新进入步骤II；
VII：建图结束，输出果园地图，果园地图构建结束后，定位系统依靠该地图进行重定位，地图在定位过程中不会更新。
5.根据权利要求1所述的一种基于SLAM的果园机器人导航方法，其特征在于，用于机器人路径规划时，将上述方法构建的果园地图转化成二值栅格地图，每个栅格的值表示其内部是否包含障碍物，即是否包括树干。
6.根据权利要求1所述的一种基于SLAM的果园机器人导航方法，其特征在于，将果园移动机器人的路径规划分为两部分：全局路径规划器与局部轨迹规划器，全局路径规划器用于在果园栅格地图中搜索一条从机器人当前位姿到给定任务位姿的无碰撞最短路径，该路径使用一系列离散的轨迹点表达，局部轨迹规划器用于根据全局路径规划器输出的全局路径与激光雷达感知的障碍物信息，将全局路径优化成一条符合机器人运动学约束，且能躲避果园中动态障碍物的局部轨迹，并实现对机器人底盘的速度控制。
CN202110472508.1A 2021-04-29 2021-04-29 一种基于slam的果园机器人导航方法 Pending CN113310488A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110472508.1A CN113310488A (zh) 2021-04-29 2021-04-29 一种基于slam的果园机器人导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110472508.1A CN113310488A (zh) 2021-04-29 2021-04-29 一种基于slam的果园机器人导航方法

Publications (1)

Publication Number Publication Date
CN113310488A true CN113310488A (zh) 2021-08-27

Family

ID=77371351

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110472508.1A Pending CN113310488A (zh) 2021-04-29 2021-04-29 一种基于slam的果园机器人导航方法

Country Status (1)

Country Link
CN (1) CN113310488A (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114413892A (zh) * 2022-01-19 2022-04-29 东南大学 一种新型果园机器人组合导航方法
CN114690769A (zh) * 2022-03-07 2022-07-01 美的集团(上海)有限公司 路径规划方法、电子设备及存储介质、计算机程序产品
CN115294562A (zh) * 2022-07-19 2022-11-04 广西大学 一种植保机器人作业环境智能感知方法
CN116660916A (zh) * 2023-05-26 2023-08-29 广东省农业科学院设施农业研究所 一种用于果园移动机器人的定位方法、建图方法及电子设备

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2019185213A (ja) * 2018-04-04 2019-10-24 国立大学法人九州工業大学 自律移動ロボット及びその制御方法
US20200029489A1 (en) * 2018-07-26 2020-01-30 Bear Flag Robotics, Inc. Vehicle Controllers For Agricultural And Industrial Applications
CN211207169U (zh) * 2019-11-12 2020-08-07 西北农林科技大学 一种温室采摘机器人底盘控制系统
CN112113565A (zh) * 2020-09-22 2020-12-22 温州科技职业学院 一种农业温室环境的机器人定位系统
CN112363503A (zh) * 2020-11-06 2021-02-12 南京林业大学 一种基于激光雷达的果园车辆自动导航控制系统
CN112525202A (zh) * 2020-12-21 2021-03-19 北京工商大学 一种基于多传感器融合的slam定位导航方法及系统

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2019185213A (ja) * 2018-04-04 2019-10-24 国立大学法人九州工業大学 自律移動ロボット及びその制御方法
US20200029489A1 (en) * 2018-07-26 2020-01-30 Bear Flag Robotics, Inc. Vehicle Controllers For Agricultural And Industrial Applications
CN211207169U (zh) * 2019-11-12 2020-08-07 西北农林科技大学 一种温室采摘机器人底盘控制系统
CN112113565A (zh) * 2020-09-22 2020-12-22 温州科技职业学院 一种农业温室环境的机器人定位系统
CN112363503A (zh) * 2020-11-06 2021-02-12 南京林业大学 一种基于激光雷达的果园车辆自动导航控制系统
CN112525202A (zh) * 2020-12-21 2021-03-19 北京工商大学 一种基于多传感器融合的slam定位导航方法及系统

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
胡晨: "果园作业机器人定位与地图构建研究", 《中国优秀博硕士学位论文全文数据库(硕士)信息科技辑》 *

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114413892A (zh) * 2022-01-19 2022-04-29 东南大学 一种新型果园机器人组合导航方法
CN114413892B (zh) * 2022-01-19 2024-01-02 东南大学 一种新型果园机器人组合导航方法
CN114690769A (zh) * 2022-03-07 2022-07-01 美的集团(上海)有限公司 路径规划方法、电子设备及存储介质、计算机程序产品
CN114690769B (zh) * 2022-03-07 2024-05-10 美的集团(上海)有限公司 路径规划方法、电子设备及存储介质、计算机程序产品
CN115294562A (zh) * 2022-07-19 2022-11-04 广西大学 一种植保机器人作业环境智能感知方法
CN115294562B (zh) * 2022-07-19 2023-05-09 广西大学 一种植保机器人作业环境智能感知方法
CN116660916A (zh) * 2023-05-26 2023-08-29 广东省农业科学院设施农业研究所 一种用于果园移动机器人的定位方法、建图方法及电子设备
CN116660916B (zh) * 2023-05-26 2024-02-02 广东省农业科学院设施农业研究所 一种用于果园移动机器人的定位方法、建图方法及电子设备

Similar Documents

Publication Publication Date Title
CN113310488A (zh) 一种基于slam的果园机器人导航方法
Gao et al. Review of wheeled mobile robots’ navigation problems and application prospects in agriculture
Vázquez-Arellano et al. 3-D reconstruction of maize plants using a time-of-flight camera
CN110866927B (zh) 一种基于垂足点线特征结合的ekf-slam算法的机器人定位与构图方法
CN106123890A (zh) 一种多传感器数据融合的机器人定位方法
CN114526745B (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及系统
CN102831646A (zh) 一种基于扫描激光的大尺度三维地形建模方法
CN111338383B (zh) 基于gaas的自主飞行方法及系统、存储介质
CN111427370A (zh) 一种基于稀疏位姿调整的移动机器人的Gmapping建图方法
Libby et al. Deployment of a point and line feature localization system for an outdoor agriculture vehicle
CN111707272A (zh) 一种地下车库自动驾驶激光定位系统
CN104992074A (zh) 机载激光扫描系统航带拼接方法与装置
CN103901891A (zh) 一种基于层次结构的动态粒子树slam算法
CN113763549B (zh) 融合激光雷达和imu的同时定位建图方法、装置和存储介质
CN114119920A (zh) 三维点云地图构建方法、系统
CN113110455A (zh) 一种未知初始状态的多机器人协同探索方法、装置及系统
Meng et al. Intelligent robotic system for autonomous exploration and active slam in unknown environments
CN114047766B (zh) 面向室内外场景长期应用的移动机器人数据采集系统及方法
Chen et al. Design and implementation of an artificial intelligence of things-based autonomous mobile robot system for pitaya harvesting
CN113379915B (zh) 一种基于点云融合的行车场景构建方法
CN113838203B (zh) 基于三维点云地图和二维栅格地图的导航系统及应用方法
CN115655311A (zh) 一种基于扫描匹配的阿克曼型机器人里程计标定方法
CN113850915A (zh) 一种基于Autoware的车辆循迹方法
Qiu et al. LiDAR point-cloud odometer based mobile robot routine tracking in orchards
Yang et al. SLAM self-cruise vehicle based on ROS platform

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
RJ01 Rejection of invention patent application after publication

Application publication date: 20210827