CN116893425A - 一种果园采摘机器人超高精度定位方法 - Google Patents

一种果园采摘机器人超高精度定位方法 Download PDF

Info

Publication number
CN116893425A
CN116893425A CN202310877578.4A CN202310877578A CN116893425A CN 116893425 A CN116893425 A CN 116893425A CN 202310877578 A CN202310877578 A CN 202310877578A CN 116893425 A CN116893425 A CN 116893425A
Authority
CN
China
Prior art keywords
pose
imu
laser
pose estimation
data
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
CN202310877578.4A
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.)
Nanjing University of Science and Technology
Original Assignee
Nanjing University of Science and Technology
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 Nanjing University of Science and Technology filed Critical Nanjing University of Science and Technology
Priority to CN202310877578.4A priority Critical patent/CN116893425A/zh
Publication of CN116893425A publication Critical patent/CN116893425A/zh
Pending legal-status Critical Current

Links

Classifications

    • 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
    • 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
    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/43—Determining position using carrier phase measurements, e.g. kinematic positioning; using long or short baseline interferometry
    • 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
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/497—Means for monitoring or calibrating
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/77—Determining position or orientation of objects or cameras using statistical methods
    • 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/30—Subject of image; Context of image processing
    • G06T2207/30181—Earth observation
    • G06T2207/30188—Vegetation; Agriculture
    • 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/30—Subject of image; Context of image processing
    • G06T2207/30248—Vehicle exterior or interior
    • G06T2207/30252—Vehicle exterior; Vicinity of vehicle

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Multimedia (AREA)
  • Automation & Control Theory (AREA)
  • Electromagnetism (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Evolutionary Biology (AREA)
  • Probability & Statistics with Applications (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种果园采摘机器人超高精度定位方法，首先分别收集将激光定位信息，视觉定位信息，IMU位姿估计信息和差分RTK位姿估计数据，并分别基于不同的赋权对估计的定位位姿进行分配和融合，输出最终的机器人位姿信息，本发明融合激光、视觉、IMU和差分RTK信号，弥补了激光、视觉、IMU精度不够，适用场景不一致，以及当树冠遮挡时，卫星信号差，差分RTK定位精度不可靠的问题，融合客观赋权和主观赋权，同时结合各个传感器的特点，提高定位精度，进行自适应动态权值评估，使得融合结果更准确，鲁棒性更高。

Description

一种果园采摘机器人超高精度定位方法
技术领域
本发明属于机器人定位领域，具体涉及一种果园采摘机器人超高精度定位方法。
背景技术
随着社会发展水平不断提升、人们生活水平提高，市场需求旺盛，果树大规模种植。国家近年来对于农业机器人的发展十分重视，积极推广使用自动化农机器械。在机器人定位方面大多使用的是卫星导航定位系统，但是例如果园此类的作业环境复杂，有大量的树冠导致卫星信号受到遮挡，使得定位精度大大降低且定位效果很不稳定。而室内广泛使用的基于三维激光雷达的定位方法，由于在室外，地面不平坦导致的激光扫描不均匀，以及由墙面、地面、桌面等形状固定的物体提供的角/面特征点信息很少，室外定位精度很受影响。视觉传感器，在室外又容易受到光照影响，定位精度不稳定。惯性传感器因为误差累计问题，时间长了之后误差会漂移数十米，只能作为辅助定位工具。不同的传感器有各自的特点，单一的传感器难以解决果园此类复杂环境下的定位问题。因此，研究一种多传感器融合的果园机器人高精度实时定位方法迫在眉睫。
不同的传感器定位精度和稳定性不一样。常用的激光、视觉、惯性传感器定位精度在“分米”级别。此类传感器的各种融合定位算法，可以将定位精度降低到10cm左右，可满足高精度定位要求。目前依靠全球卫星导航定位系统的差分RTK(实时动态)定位的精度可达1.5cm，缺点是信号受遮挡情况下，定位精度会漂移到10cm以上，无法全程依赖差分RTK定位。
发明内容
针对上述问题，本发明的目的在于提供一种果园采摘机器人超高精度定位方法，使得在室外果园激光雷达点云匹配效果差，视觉传感器受光照影响大，卫星信号遮挡情况下定位精度低的各种复杂情况下，依然可以利用多传感器互补融合的方法实现超高精度实时定位。
实现本发明目的的具体技术方案为：
一种果园采摘机器人超高精度定位方法，包括以下步骤：
步骤1、基于激光雷达扫描周围环境，获得点云信息，基于IMU信息对由于运动产生畸变的激光点云进行纠正，对准到全局点云中；
步骤2、双目视觉相机采集环境信息，提取特征点信息，结合激光雷达和IMU信息，进行特征点深度关联，进行帧间运动估计；
步骤3、采集卫星定位数据，根据卫星数判断数据是否可用，若可用，则基于RTK位姿估计数据作为初值，若不可用，则基于激光-视觉融合位姿，用分段拟合法融合IMU信息，进行当前时刻的IMU位姿估计数据；
步骤4、将激光定位信息，视觉定位信息，IMU位姿估计信息和差分RTKRTK位姿估计数据输入数据紧耦合优化模型，输出优化位姿，对优化位姿进行平滑处理，输出为激光-视觉紧耦合定位位姿；
步骤5、判断搜索到的卫星数，根据卫星数量确定输出的机器人初步位姿；
步骤6、判断卫星信号是否丢失，根据卫星信号丢失时间进行机器人位姿优化；
步骤7、为差分RTK位姿估计数据、IMU位姿估计数据、激光-视觉紧耦合定位位姿进行权重重分配，并输出最终位姿为机器人位姿。
与现有技术相比，本发明的有益效果在于：
(1)本发明融合激光、视觉、IMU和差分RTK信号，弥补了激光、视觉、IMU精度不够，适用场景不一致的问题，以及当树冠遮挡时，卫星信号差，差分RTK定位精度不可靠的问题；
(2)本发明融合客观赋权和主观赋权，同时结合各个传感器的特点，提高定位精度，进行自适应动态权值评估，使得融合结果更准确，鲁棒性更高。
下面结合附图和具体实施方式对本发明做进一步的说明。
附图说明
图1为本发明的果园采摘机器人超高精度定位场景示意图。
图2为本发明的果园采摘机器人超高精度定位方法流程示意图。
具体实施方式
结合图1，机器人在果园中行走时，激光雷达、视觉传感器和惯性传感器会连续的采集环境信息和机器人运动状态，而全球卫星导航系统的信号和差分RTK信号会受到果树树冠的遮挡，且卫星在太空中相对机器人的位姿不断变化，不同时刻搜到的卫星数是不一样的，无法保证数据的连续性和精度的稳定性
因此，本发明提出一种果园采摘机器人超高精度定位方法，包括以下步骤：
步骤1、基于激光雷达扫描周围环境，获得点云信息，基于IMU信息对由于运动产生畸变的激光点云进行纠正，对准到全局点云中：
步骤1-1、确定IMU和激光雷达的内外参转换矩阵，将IMU和激光雷达固定在一起，先进行IMU内参标定求得IMU的静止误差即内参矩阵，再进行IMU-激光雷达外参联合标定，求出IMU和激光雷达的外参转换矩阵，包括平移矩阵和旋转矩阵；
步骤1-2、同步激光雷达和IMU时间戳，根据激光雷达和IMU自带的时间戳，获取距离某一帧点云时间戳最近的IMU数据，时间戳相减求出差值，如果时间差小于同步阈值则作为同步数据输出；如果时间差大于同步阈值则丢弃此数据，拿下一帧激光雷达采样数据循环上述过程；
步骤1-3、激光雷达的一帧数据输出是点云形式，根据激光雷达参数，将激光雷达输出的点云数据进行分线束处理，记录每个点所属线束和每个点在此帧点云内的相对扫描时间；
步骤1-4、获取IMU坐标系下三个轴相对于世界坐标系的欧拉角和三个轴上的加速度，去除重力影响获得IMU在世界坐标系下的加速度，得到每一帧IMU数据在世界坐标系下对应的位移和速度，使用得到的运动信息进行插值，计算每一个激光点相对于起始点扫描时刻激光点的畸变补偿变换矩阵；
步骤1-5、针对每一个激光点，用补偿变换矩阵乘以原始的雷达激光点坐标，获取矫正后的激光点坐标，即激光定位信息。
步骤2、双目视觉相机采集环境信息，提取特征点信息，结合激光雷达和IMU信息，进行特征点深度关联，进行帧间运动估计：
步骤2-1、将双目视觉相机、激光雷达和IMU固定，进行双目视觉相机-激光雷达-IMU外参联合标定，确定相机、激光雷达和IMU的外参转换矩阵，包括平移矩阵和旋转矩阵，并对双目视觉相机进行内参标定，确定内参标定矩阵；
步骤2-2、结合IMU数据以及步骤2-1标定的内外参转换矩阵，对采集的图像进行去畸变，对图像进行增强滤波处理，根据RGB信息提取视觉特征点；
步骤2-3、采集相邻多帧激光雷达的点云深度信息；
步骤2-4、将视觉特征点和激光点云点投影到同一个坐标系中，搜索每个视觉特征点附近的n个最近深度点云，由该n个激光点确定视觉特征点的深度值，确定特征点的三维坐标；
步骤2-5、对带由深度标签的视觉特征点进行帧间的特征点匹配，根据匹配结果，通过非线性最小二乘法构建目标函数，进行迭代优化，求出位姿估计，即视觉定位信息。
步骤3、采集卫星定位数据，根据卫星数判断数据是否可用，若可用，则基于RTK位姿估计数据作为初值，若不可用，则基于激光-视觉融合位姿，用分段拟合法融合IMU信息，进行当前时刻的IMU位姿估计数据：
步骤3-1、采集卫星定位数据，若搜索到的卫星数≥M颗，则认为卫星信号可用，则固定在机器人上的信号接收机，同时接收卫星信号和通过基准站发出的载波相位差分改正信号，在这两个信号基础上，实现差分计算，输出为差分RTK位姿，同时将上一时刻收到的差分RTK位姿作为初值，转入步骤3-2；
若搜索到的卫星数＜M颗，则认为卫星信号不可用，忽略该数据，接收上一时刻受到的激光-视觉融合位姿作为初值，转入步骤3-3；
步骤3-2、将差分RTK位姿进行坐标转换，从世界坐标系转换到机器人坐标系中：
其中，(XL,YL,ZL)为机器人坐标系下坐标，(XW,YW,ZW)为世界坐标系下坐标，即全球卫星导航系统提供的RTK坐标，RX(α)，RY(β)，Rz(θ)为三轴坐标转换矩阵，对应α，β，θ为全球卫星导航系统提供的俯仰角，横滚角和航向角；
转换后得到当前时刻的RTK位姿估计。
步骤3-3、根据IMU数据，计算当前帧和上一帧的姿态角的变化量，并加到上一时刻的位姿上，得到当前时刻的IMU位姿估计。
步骤4、将激光定位信息，视觉定位信息，IMU位姿估计信息和差分RTK位姿估计数据输入数据紧耦合优化模型，用贝叶斯树映射进行图优化，输出优化位姿，对优化位姿进行平滑处理，输出为激光-视觉紧耦合定位位姿；
GTSAM是基于因子图的c++库，其中提供了iSAM和iSAM2算法，借助GTSAM工具构建数据紧耦合优化模型，我们将激光定位信息、视觉定位信息、IMU位姿估计信息和RTK位姿估计信息作为自定义因子输入进该模型，该模型采用贝叶斯树映射进行图优化，对优化位姿进行平滑处理，输出为激光-视觉紧耦合定位位姿。
步骤5、判断搜索到的卫星数，根据卫星数量确定输出的机器人初步位姿：
若搜索到的卫星数≥N颗，即信号非常好的情况下，此时差分RTK的定位精度可达到1.5cm，则直接输出差分RTK位姿估计数据作为机器人位姿，并且记录下当前定位信息，作为下个时刻的初值；
若搜索到的卫星数为：M颗<卫星数<N颗，则根据向量相似度，将差分RTK位姿估计信息、IMU位姿估计信息、激光-视觉紧耦合定位位姿赋权值，并基于权值对三个位姿进行融合，得到融合后的位姿估计结果：
分别确定差分RTK位姿估计数据、IMU位姿估计数据、激光-视觉紧耦合定位位姿中两两向量之间的长度相似度α：
分别确定差分RTK位姿估计数据、IMU位姿估计数据、激光-视觉紧耦合定位位姿中两两向量之间的方向相似度β：
其中，θ表示两个向量之间的夹角；
依据长度相似度和方向相似度分别确定差分RTK位姿估计数据、IMU位姿估计数据、激光-视觉紧耦合定位位姿中两两向量之间的相似度γ：
γ(X,Y)＝α·β
则任一向量与其他向量的总相似度为：
γ＝(γ1,γ2,...,γn)
根据上式的总相似度为向量差分RTK位姿估计数据、IMU位姿估计数据、激光-视觉紧耦合定位位姿赋权值，得到融合后的位姿估计结果W：
W客观＝α11·W1+α12·W2+α13·W3
其中，W1、W2、W3分别表示差分RTK位姿估计数据、IMU位姿估计数据、激光-视觉紧耦合定位位姿，α1k，k＝1,2,3，分别表示差分RTK位姿估计数据、IMU位姿估计数据、激光-视觉紧耦合定位位姿权值。
步骤6、判断卫星信号是否丢失，根据卫星信号丢失时间进行机器人位姿优化：
对于IMU来说，存在累计漂移误差问题，当时间太久，误差将累计到无法忍受的地步，因此必须不断的输入高可靠性的初值，来进行位姿估计，而低频输出的差分RTK信号作为高频输出的IMU数据的初值最为合适，因此需要判断卫星信号丢失时间：
当卫星信号未发生丢失，即丢失时间为0秒时，转入步骤7；
当卫星信号丢失时间<L秒时，IMU漂移量不大，可以根据历史数据进行准确的位姿估计，剔除差分RTK位姿估计数据，即将差分RTK位姿估计数据的权值置0，并返回执行步骤5；
当卫星信号丢失时间≥L秒时，IMU漂移量太大，剔除差分RTK位姿估计数据和IMU位姿估计数据，即将差分RTK位姿估计数据和IMU位姿估计数据权值置0，并返回执行步骤5。
步骤7、为差分RTK位姿估计数据、IMU位姿估计数据、激光-视觉紧耦合定位位姿进行权重重分配，并输出最终位姿为机器人位姿，具体为：
步骤7-1、为差分RTK位姿估计数据、IMU位姿估计数据、激光-视觉紧耦合定位位姿赋予主观权值，得到估计位姿W主观：
W主观＝α21·W1+α22·W2+α23·W3
α21+α22+α23＝1
其中，α21,α22,α23是主观权值；
步骤7-2、确定权值集组合，权重向量αk＝[αk1,αk2,αk3]，k＝1,2，则记权重向量的任意组合为：
αk＝[αk1,αk2,αk3]，k＝1,2
式中，δk为线性组合系数，α为一种可能的权重组合；
步骤7-3、构建优化模型：
得到最优线性组合系数δk集合{δ1,δ2}；
步骤7-4、对权值δk进行归一化处理，得到最后的权重集合确定最后输出的机器人优化位姿W*，初始位姿集合W＝[W1,W2,W3]：
W*＝∑α*·WT
实施例
一种果园采摘机器人超高精度定位方法，包括以下步骤：
步骤1、基于激光雷达扫描周围环境，获得点云信息，基于IMU信息对由于运动产生畸变的激光点云进行纠正，对准到全局点云中：
步骤1-1、确定IMU和激光雷达的内外参转换矩阵，将IMU和激光雷达固定在一起，先进行IMU内参标定求得IMU的静止误差即内参矩阵，再进行IMU-激光雷达外参联合标定，求出IMU和激光雷达的外参转换矩阵，包括平移矩阵和旋转矩阵；
步骤1-2、同步激光雷达和IMU时间戳，根据激光雷达和IMU自带的时间戳，获取距离某一帧点云时间戳最近的IMU数据，时间戳相减求出差值，如果时间差小于同步阈值则作为同步数据输出；如果时间差大于同步阈值则丢弃此数据，拿下一帧激光雷达采样数据循环上述过程；
步骤1-3、激光雷达的一帧数据输出是点云形式，根据激光雷达参数，将激光雷达输出的点云数据进行分线束处理，记录每个点所属线束和每个点在此帧点云内的相对扫描时间；
步骤1-4、获取IMU坐标系下三个轴相对于世界坐标系的欧拉角和三个轴上的加速度，去除重力影响获得IMU在世界坐标系下的加速度，得到每一帧IMU数据在世界坐标系下对应的位移和速度，使用得到的运动信息进行插值，计算每一个激光点相对于起始点扫描时刻激光点的畸变补偿变换矩阵；
步骤1-5、针对每一个激光点，用补偿变换矩阵乘以原始的雷达激光点坐标，获取矫正后的激光点坐标，即激光定位信息。
步骤2、双目视觉相机采集环境信息，提取特征点信息，结合激光雷达和IMU信息，进行特征点深度关联，进行帧间运动估计：
步骤2-1、将双目视觉相机、激光雷达和IMU固定，进行双目视觉相机-激光雷达-IMU外参联合标定，确定相机、激光雷达和IMU的外参转换矩阵，包括平移矩阵和旋转矩阵，并对双目视觉相机进行内参标定，确定内参标定矩阵；
步骤2-2、结合IMU数据以及步骤2-1标定的内外参转换矩阵，对采集的图像进行去畸变，对图像进行增强滤波处理，根据RGB信息提取视觉特征点；
步骤2-3、采集相邻多帧激光雷达的点云深度信息；
步骤2-4、将视觉特征点和激光点云点投影到同一个坐标系中，搜索每个视觉特征点附近的n个最近深度点云，由该n个激光点确定视觉特征点的深度值，确定特征点的三维坐标；
步骤2-5、对带由深度标签的视觉特征点进行帧间的特征点匹配，根据匹配结果，通过非线性最小二乘法构建目标函数，进行迭代优化，求出位姿估计，即视觉定位信息。
步骤3、采集卫星定位数据，根据卫星数判断数据是否可用，若可用，则基于RTK位姿估计数据作为初值，若不可用，则基于激光-视觉融合位姿，用分段拟合法融合IMU信息，进行当前时刻的IMU位姿估计数据：
步骤3-1、采集卫星定位数据，若搜索到的卫星数≥M颗，则认为卫星信号可用，则固定在机器人上的信号接收机，同时接收卫星信号和通过基准站发出的载波相位差分改正信号，在这两个信号基础上，实现差分计算，输出为差分RTK位姿，同时将上一时刻收到的差分RTK位姿作为初值，转入步骤3-2；
若搜索到的卫星数＜M颗，则认为卫星信号不可用，忽略该数据，接收上一时刻受到的激光-视觉融合位姿作为初值，转入步骤3-3；
步骤3-2、将差分RTK位姿进行坐标转换，从世界坐标系转换到机器人坐标系中：
其中，(XL,YL,ZL)为机器人坐标系下坐标，(XW,YW,ZW)为世界坐标系下坐标，即全球卫星导航系统提供的RTK坐标，RX(α)，RY(β)，RZ(θ)为三轴坐标转换矩阵，对应α，β，θ为全球卫星导航系统提供的俯仰角，横滚角和航向角；
转换后得到当前时刻的RTK位姿估计。
步骤3-3、根据IMU数据，计算当前帧和上一帧的姿态角的变化量，并加到上一时刻的位姿上，得到当前时刻的IMU位姿估计。
步骤4、将激光定位信息，视觉定位信息，IMU位姿估计信息和差分RTKRTK位姿估计数据输入数据紧耦合优化模型，用贝叶斯树映射进行图优化，输出优化位姿，对优化位姿进行平滑处理，输出为激光-视觉紧耦合定位位姿
GTSAM是基于因子图的c++库，其中提供了iSAM和iSAM2算法，借助GTSAM工具构建数据紧耦合优化模型，我们将激光定位信息、视觉定位信息、IMU位姿估计信息和RTK位姿估计信息作为自定义因子输入进该模型，该模型采用贝叶斯树映射进行图优化，对优化位姿进行平滑处理，输出为激光-视觉紧耦合定位位姿。
步骤5、判断搜索到的卫星数，根据卫星数量确定输出的机器人初步位姿：
若搜索到的卫星数≥N颗，即信号非常好的情况下，此时差分RTK的定位精度可达到1.5cm，则直接输出差分RTK位姿估计数据作为机器人位姿，并且记录下当前定位信息，作为下个时刻的初值；
若搜索到的卫星数为：M颗<卫星数<N颗，则根据向量相似度，将差分RTK位姿估计信息、IMU位姿估计信息、激光-视觉紧耦合定位位姿赋权值，并基于权值对三个位姿进行融合，得到融合后的位姿估计结果：
分别确定差分RTK位姿估计数据、IMU位姿估计数据、激光-视觉紧耦合定位位姿中两两向量之间的长度相似度α：
分别确定差分RTK位姿估计数据、IMU位姿估计数据、激光-视觉紧耦合定位位姿中两两向量之间的方向相似度β：
其中，θ表示两个向量之间的夹角；
依据长度相似度和方向相似度分别确定差分RTK位姿估计数据、IMU位姿估计数据、激光-视觉紧耦合定位位姿中两两向量之间的相似度γ：
γ(X,Y)＝α·β
则任一向量与其他向量的总相似度为：
γ＝(γ1,γ2,...,γn)
根据上式的总相似度为向量差分RTK位姿估计数据、IMU位姿估计数据、激光-视觉紧耦合定位位姿赋权值，得到融合后的位姿估计结果W：
W客观＝α11·W1+α12·W2+α13·W3
其中，W1、W2、W3分别表示差分RTK位姿估计数据、IMU位姿估计数据、激光-视觉紧耦合定位位姿，α1k，k＝1,2,3，分别表示差分RTK位姿估计数据、IMU位姿估计数据、激光-视觉紧耦合定位位姿权值。
步骤6、判断卫星信号是否丢失，根据卫星信号丢失时间进行机器人位姿优化：
对于IMU来说，存在累计漂移误差问题，当时间太久，误差将累计到无法忍受的地步，因此必须不断的输入高可靠性的初值，来进行位姿估计，而低频输出的差分RTK信号作为高频输出的IMU数据的初值最为合适，因此需要判断卫星信号丢失时间：
当卫星信号未发生丢失，即丢失时间为0秒时，转入步骤7；
当卫星信号丢失时间<L秒时，IMU漂移量不大，可以根据历史数据进行准确的位姿估计，剔除差分RTK位姿估计数据，即将差分RTK位姿估计数据的权值置0，并返回执行步骤5；
当卫星信号丢失时间≥L秒时，IMU漂移量太大，剔除差分RTK位姿估计数据和IMU位姿估计数据，即将差分RTK位姿估计数据和IMU位姿估计数据权值置0，并返回执行步骤5。
步骤7、为差分RTK位姿估计数据、IMU位姿估计数据、激光-视觉紧耦合定位位姿进行权重重分配，并输出最终位姿为机器人位姿，具体为：
步骤7-1、为差分RTK位姿估计数据、IMU位姿估计数据、激光-视觉紧耦合定位位姿赋予主观权值，得到估计位姿W主观：
W主观＝α21·W1+α22·W2+α23·W3
α21+α22+α23＝1
其中，α21,α22,α23是主观权值；
步骤7-2、确定权值集组合，权重向量αk＝[αk1,αk2,αk3]，k＝1,2，则记权重向量的任意组合为：
αk＝[αk1,αk2,αk3]，k＝1,2
式中，δk为线性组合系数，α为一种可能的权重组合；
步骤7-3、构建优化模型：
得到最优线性组合系数δk集合{δ1,δ2}；
步骤7-4、对权值δk进行归一化处理，得到最后的权重集合确定最后输出的机器人优化位姿W*，初始位姿集合W＝[W1,W2,W3]：
W*＝∑α*·WT
本专利所述方法在搭载Ouster 32线激光雷达、MicroStrain 3dm-gx5-25惯性传感器、Realsense D455双目相机和华测CGI-610组合导航系统的机器车上进行了实验。实验车分别在茶园(开阔无遮挡)和果园(存在树木遮挡)进行了定位精度测量，实验结果表明，在开阔处，卫星信号差分RTK定位精度可达1.66cm，在树木遮挡情况下，精度会有所漂移，遮挡严重时，差分RTK信号丢失，卫星信号定位精度漂移至13.86cm，远低于未遮挡情况下的定位精度。IMU的一小时静态零偏误差在0.066cm。激光-视觉紧耦合定位位姿(未使用RTK进行实时纠正)的绝对姿态误差为12.13cm，激光-视觉紧耦合定位位姿(使用RTK进行实时纠正)的绝对姿态误差降低为5.06cm。
通过本专利方法，将差分RTK信号、IMU数据和激光-视觉紧耦合定位位姿，多源数据智能融合处理，使平均定位精度降低至1.98cm。
以上实施例显示和描述了本发明的基本原理、主要特征。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。

Claims (8)

1.一种果园采摘机器人超高精度定位方法，其特征在于，包括以下步骤：
步骤1、基于激光雷达扫描周围环境，获得点云信息，基于IMU信息对由于运动产生畸变的激光点云进行纠正，对准到全局点云中；
步骤2、双目视觉相机采集环境信息，提取特征点信息，结合激光雷达和IMU信息，进行特征点深度关联，进行帧间运动估计；
步骤3、采集卫星定位数据，根据卫星数判断数据是否可用，若可用，则基于RTK位姿估计数据作为初值，若不可用，则基于激光-视觉融合位姿，用分段拟合法融合IMU信息，进行当前时刻的IMU位姿估计数据；
步骤4、将激光定位信息，视觉定位信息，IMU位姿估计信息和差分RTKRTK位姿估计数据输入数据紧耦合优化模型，输出优化位姿，对优化位姿进行平滑处理，输出为激光-视觉紧耦合定位位姿；
步骤5、判断搜索到的卫星数，根据卫星数量确定输出的机器人初步位姿；
步骤6、判断卫星信号是否丢失，根据卫星信号丢失时间进行机器人位姿优化；
步骤7、为差分RTK位姿估计数据、IMU位姿估计数据、激光-视觉紧耦合定位位姿进行权重重分配，并输出最终位姿为机器人位姿。
2.根据权利要求1所述的果园采摘机器人超高精度定位方法，其特征在于，所述步骤1中的基于IMU信息对激光点云信息进行纠正，具体为：
步骤1-1、确定IMU和激光雷达的内外参转换矩阵；
步骤1-2、同步激光雷达和IMU时间戳；
步骤1-3、根据激光雷达参数，将激光雷达输出的点云数据进行分线束处理，记录每个点所属线束和每个点在此帧点云内的相对扫描时间；
步骤1-4、获取IMU坐标系下三个轴相对于世界坐标系的欧拉角和三个轴上的加速度，去除重力影响获得IMU在世界坐标系下的加速度，得到每一帧IMU数据在世界坐标系下对应的位移和速度，使用得到的运动信息进行插值，计算每一个激光点相对于起始点扫描时刻激光点的畸变补偿变换矩阵；
步骤1-5、针对每一个激光点，用补偿变换矩阵乘以原始的雷达激光点坐标，获取矫正后的激光点坐标。
3.根据权利要求1所述的果园采摘机器人超高精度定位方法，其特征在于，所述步骤2中的帧间运动估计，具体为：
步骤2-1、将双目视觉相机、激光雷达和IMU固定，进行双目视觉相机-激光雷达-IMU外参联合标定，确定相机、激光雷达和IMU的外参转换矩阵，包括平移矩阵和旋转矩阵，并对双目视觉相机进行内参标定，确定内参标定矩阵；
步骤2-2、结合IMU数据以及步骤2-1标定的内外参转换矩阵，对采集的图像进行去畸变，对图像进行增强滤波处理，根据RGB信息提取视觉特征点；
步骤2-3、采集相邻多帧激光雷达的点云深度信息；
步骤2-4、将视觉特征点和激光点云点投影到同一个坐标系中，搜索每个视觉特征点附近的n个最近深度点云，由该n个激光点确定视觉特征点的深度值，确定特征点的三维坐标；
步骤2-5、对带由深度标签的视觉特征点进行帧间的特征点匹配，根据匹配结果，通过非线性最小二乘法构建目标函数，进行迭代优化，求出位姿估计。
4.根据权利要求1所述的果园采摘机器人超高精度定位方法，其特征在于，所述步骤3中的当前时刻的RTK位姿估计或IMU位姿估计，具体为：
步骤3-1、采集卫星定位数据，若搜索到的卫星数≥M颗，则认为卫星信号可用，则固定在机器人上的信号接收机，同时接收卫星信号和通过基准站发出的载波相位差分改正信号，在这两个信号基础上，实现差分计算，输出为差分RTK位姿，同时将上一时刻收到的差分RTK位姿作为初值，转入步骤3-2；
若搜索到的卫星数＜M颗，则认为卫星信号不可用，忽略该数据，接收上一时刻受到的激光-视觉融合位姿作为初值，转入步骤3-3；
步骤3-2、将差分RTK位姿进行坐标转换，从世界坐标系转换到机器人坐标系中：
其中，(XL,YL,ZL)为机器人坐标系下坐标，(XW,YW,ZW)为世界坐标系下坐标，即全球卫星导航系统提供的RTK坐标，RX(α)，RY(β)，RZ(θ)为三轴坐标转换矩阵，对应α，β，θ为全球卫星导航系统提供的俯仰角，横滚角和航向角；
转换后得到当前时刻的RTK位姿估计。
步骤3-3、根据IMU数据，计算当前帧和上一帧的姿态角的变化量，并加到上一时刻的位姿上，得到当前时刻的IMU位姿估计。
5.根据权利要求1所述的果园采摘机器人超高精度定位方法，其特征在于，所述步骤5中的根据搜索的卫星数量确定输出的机器人初步位姿，具体为：
若搜索到的卫星数≥N颗，则直接输出差分RTK位姿估计数据作为机器人位姿，并且记录下当前定位信息，作为下个时刻的初值；
若搜索到的卫星数为：M颗<卫星数<N颗，则根据向量相似度，将差分RTK位姿估计信息、IMU位姿估计信息、激光-视觉紧耦合定位位姿赋权值，并基于权值对三个位姿进行融合，得到融合后的位姿估计结果。
6.根据权利要求5所述的果园采摘机器人超高精度定位方法，其特征在于，所述根据向量相似度，将差分RTK位姿估计数据、IMU位姿估计数据、激光-视觉紧耦合定位位姿赋权值，具体为：
分别确定差分RTK位姿估计数据、IMU位姿估计数据、激光-视觉紧耦合定位位姿中两两向量之间的长度相似度α：
分别确定差分RTK位姿估计数据、IMU位姿估计数据、激光-视觉紧耦合定位位姿中两两向量之间的方向相似度β：
其中，θ表示两个向量之间的夹角；
依据长度相似度和方向相似度分别确定差分RTK位姿估计数据、IMU位姿估计数据、激光-视觉紧耦合定位位姿中两两向量之间的相似度γ：
γ(X,Y)＝α·β
则任一向量与其他向量的总相似度为：
γ＝(γ1,γ2,...,γn)
根据上式的总相似度为向量差分RTK位姿估计数据、IMU位姿估计数据、激光-视觉紧耦合定位位姿赋权值，得到融合后的位姿估计结果W：
W客观＝α11·W1+α12·W2+α13·W3
其中，W1、W2、W3分别表示差分RTK位姿估计数据、IMU位姿估计数据、激光-视觉紧耦合定位位姿，α1k，k＝1,2,3，分别表示差分RTK位姿估计数据、IMU位姿估计数据、激光-视觉紧耦合定位位姿权值。
7.根据权利要求6所述的果园采摘机器人超高精度定位方法，其特征在于，所述步骤6中的根据卫星信号丢失时间进行位姿优化，具体为：
判断卫星信号丢失时间：
当卫星信号未发生丢失，即丢失时间为0秒时，转入步骤7；
当卫星信号丢失时间<L秒时，剔除差分RTK位姿估计数据，即将差分RTK位姿估计数据的权值置0，并返回执行步骤5；
当卫星信号丢失时间>L秒时，除差分RTK位姿估计数据和IMU位姿估计数据，即将差分RTK位姿估计数据和IMU位姿估计数据权值置0，并返回执行步骤5。
8.根据权利要求6所述的果园采摘机器人超高精度定位方法，其特征在于，所述步骤7中的为差分RTK定位数据、IMU优化位姿、激光-视觉紧耦合定位位姿进行权重重分配，具体为：
步骤7-1、为差分RTK位姿估计数据、IMU位姿估计数据、激光-视觉紧耦合定位位姿赋予主观权值，得到估计位姿W主观：
W主观＝α21·W1+α22·W2+α23·W3
α21+α22+α23＝1
其中，α21,α22,α23是主观权值；
步骤7-2、确定权值集组合，权重向量αk＝[αk1,αk2,αk3]，k＝1,2，则记权重向量的任意组合为：
αk＝[αk1,αk2,αk3]，k＝1,2
式中，δk为线性组合系数，α为一种可能的权重组合；
步骤7-3、构建优化模型：
得到最优线性组合系数δk集合{δ1,δ2}；
步骤7-4、对权值δk进行归一化处理，得到最后的权重集合确定最后输出的机器人优化位姿W*，初始位姿集合W＝[W1,W2,W3]：
W*＝∑α*·WT。
CN202310877578.4A 2023-07-18 2023-07-18 一种果园采摘机器人超高精度定位方法 Pending CN116893425A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310877578.4A CN116893425A (zh) 2023-07-18 2023-07-18 一种果园采摘机器人超高精度定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310877578.4A CN116893425A (zh) 2023-07-18 2023-07-18 一种果园采摘机器人超高精度定位方法

Publications (1)

Publication Number Publication Date
CN116893425A true CN116893425A (zh) 2023-10-17

Family

ID=88313311

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310877578.4A Pending CN116893425A (zh) 2023-07-18 2023-07-18 一种果园采摘机器人超高精度定位方法

Country Status (1)

Country Link
CN (1) CN116893425A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117433511A (zh) * 2023-12-20 2024-01-23 绘见科技(深圳)有限公司 一种多传感器融合定位方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117433511A (zh) * 2023-12-20 2024-01-23 绘见科技(深圳)有限公司 一种多传感器融合定位方法
CN117433511B (zh) * 2023-12-20 2024-03-12 绘见科技(深圳)有限公司 一种多传感器融合定位方法

Similar Documents

Publication Publication Date Title
Zhu et al. Camvox: A low-cost and accurate lidar-assisted visual slam system
CN112902953B (zh) 一种基于slam技术的自主位姿测量方法
CN113436260B (zh) 基于多传感器紧耦合的移动机器人位姿估计方法和系统
CN107590827A (zh) 一种基于Kinect的室内移动机器人视觉SLAM方法
CN109633664B (zh) 基于rgb-d与激光里程计的联合定位方法
CN112987065B (zh) 一种融合多传感器的手持式slam装置及其控制方法
CN111795686A (zh) 一种移动机器人定位与建图的方法
CN111521195B (zh) 一种智能机器人
CN113658337B (zh) 一种基于车辙线的多模态里程计方法
CN116893425A (zh) 一种果园采摘机器人超高精度定位方法
CN115272596A (zh) 一种面向单调无纹理大场景的多传感器融合slam方法
CN115468567A (zh) 一种面向越野环境的激光视觉融合slam方法
CN115451948A (zh) 一种基于多传感器融合的农业无人车定位里程计方法及系统
CN111538029A (zh) 一种视觉与雷达融合的测量方法及终端
CN111998862A (zh) 一种基于bnn的稠密双目slam方法
CN115639547A (zh) 多线激光雷达与gnss_ins联合标定方法、系统及介质
CN115218906A (zh) 面向室内slam的视觉惯性融合定位方法及系统
CN102175227B (zh) 一种探测车在卫星图像上的快速定位方法
CN117367427A (zh) 一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法
CN116878542A (zh) 一种抑制里程计高度漂移的激光slam方法
CN115930948A (zh) 一种果园机器人融合定位方法
CN115112115A (zh) 一种机器人果园高精度实时定位方法
CN112985388B (zh) 基于大位移光流法的组合导航方法及系统
CN114897967A (zh) 一种面向挖掘装备自主作业的物料形态识别方法
CN112767481B (zh) 一种基于视觉边缘特征的高精度定位及建图方法

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