CN115451948A - 一种基于多传感器融合的农业无人车定位里程计方法及系统 - Google Patents
一种基于多传感器融合的农业无人车定位里程计方法及系统 Download PDFInfo
- Publication number
- CN115451948A CN115451948A CN202210950380.XA CN202210950380A CN115451948A CN 115451948 A CN115451948 A CN 115451948A CN 202210950380 A CN202210950380 A CN 202210950380A CN 115451948 A CN115451948 A CN 115451948A
- Authority
- CN
- China
- Prior art keywords
- point cloud
- information
- point
- unmanned vehicle
- depth
- 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
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/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/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/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/183—Compensation of inertial measurements, e.g. for temperature effects
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
-
- 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/93—Lidar systems specially adapted for specific applications for anti-collision purposes
Abstract
本发明提出一种基于多传感器融合的农业无人车定位里程计方法和系统,包括:通过视觉传感器检测获取农业无人车所处环境的多个视觉特征点,利用深度传感器采集到的环境点云获取视觉特征点的深度信息,将获取到深度信息的视觉特征点作为稳定特征点,统计稳定特征点数量占视觉特征点总数的比例,作为复杂度;惯性传感器采集相对位姿信息,根据相对位姿信息生成惯性位姿信息;通过卡尔曼滤波器融合惯性位姿信息和视觉特征点,得到第一位姿信息,根据相对位姿信息为环境点云进行去畸形化处理,得到校正点云,以结合惯性位姿信息执行点云配准处理,得到第二位姿信息;根据复杂度,选择第一位姿信息或第二位姿信息,作为农业无人车的最终定位结果。
Description
技术领域
本发明涉及农业无人车、多传感器融合和定位里程计技术领域,并特别涉及一种基于多传感器融合的农业无人车定位里程计方法及系统。
背景技术
随着传感器硬件技术、计算机软件技术以及人工智能技术的发展,无人车在不同场景下的应用越来越广泛。在过去的几年里,无人车在仓储物流、探索救援等领域的应用已较为成熟,技术方案也趋向于多样化和实用化,但是无人车在农业领域(精准农业、智慧农业等)内还亟待发展,存在一系列的问题与限制,这与农业环境的多变性与复杂性有不可忽视的关系。其中农业环境下无人车的定位是当前无人车落地应用的一个技术挑战。里程计为整个定位软硬件系统。
农业无人车定位技术属于无人车定位技术领域的子领域,现有的无人车定位技术主要有以下几个:1)卫星差分定位:基于GNSS系统或北斗卫星定位系统,辅以差分信号基站,解算出高精度的无人车的经纬度坐标;但基于GPS获取到的姿态精度低。姿态的参数包括车辆翻滚角、俯仰角、航向角;2)视觉定位:基于单目相机或立体相机,利用环境中的视觉几何特征或光度特征,解算出无人车的相对运动位姿(位置和姿态);3)激光雷达定位:基于多线激光雷达,利用环境中的激光点云的形状分布特征或几何特征,解算出无人车的相对运动位姿;4)二维码定位:基于预先设立在环境中的二维码,利用相机扫描二维码的方式,获取无人车的位姿;5)UMB定位:基于预先安置在环境中的UWB无线基站,利用安装在无人车上的定位标签,通过无线通信技术的测算无人车位姿。
服务于农业的无人车类型包括无人驾驶拖拉机、收割机、农业自主巡检车辆等,农业对于这些无人车定位的需求主要体现在定位精度高(定位精度要求厘米级)、稳定性好(定位信号输出频率稳定在10Hz以上)以及可靠性强(定位系统不易宕机)等方面。上文中所述无人车的定位方法在农业场景下应用均存在一些问题。1)无人车在农田作业的部分场景无法使用卫星差分定位的方法获取精确位置,例如塑料大棚等室内作业场景以及卫星信号被高架桥梁遮挡的农田场景;2)待耕作或收割的农田包含的视觉几何特征和光度特征较少,不利于相机进行特征提取与匹配,因此很难通过视觉获取无人车精准度定位信息;3)农田场景常常包含较为广阔的平面,不利于激光雷达进行点云配准,会出现激光雷达里程计退化的现象;4)农田场景面积较大,不适合二维码定位或者UWB定位这类需要提前在环境中布置设备的定位方法。综上所述,现有的无人车定位方法无法满足农业无人车定位在精准性、稳定性以及可靠性方面的需求。
农业场景下进行无人车定位的难点在于农业场景中环境多变(光照、天气、地形、地块作业类型等)、路况颠簸、环境几何特征少、卫星差分信号不稳定,对于单一传感器来说,无法同时应对这些难点,多传感器融合可以综合利用各种传感器的优势覆盖大多数场景,因此多传感器融合在定位技术中的应用可有效解决农业场景中的定位问题。在农业场景中应用多传感器融合定位,需要注意多传感器的融合不是硬件设备的累加,需要选取适合的传感器最小集(sensor suite)来在定位系统性能和成本之间权衡,在达到系统精准性、稳定性和可靠性的基础上尽量缩减成本。
现有融合技术是大多基于卡尔曼滤波器或者图优化技术进行传感器的定位信息融合。这两种融合方法依赖于传感器测量的无人车位置的不确定性来进行融合,通常使用协方差矩阵来对位置测量的不确定性进行描述。农业环境下场景多变,在变化的场景下进行传感器定位信息融合,需要实时动态地修改系统参数来使定位系统适应环境,采用协方差矩阵对于不确定性进行描述方法较为单一,无法充分感知环境的复杂度来对融合系统及时调整。随着周围环境的变化,当融合里程计系统发生退化现象时,需要及时感知并对系统进行针对性优化,来满足系统的精准性、稳定性和可靠性。
发明内容
发现根据上述分析,本发明提出了一种实时感知周围农业环境并对系统进行动态调优的方法,提升了融合里程计系统对环境的适应性。该方法对融合系统的优化不仅仅依赖于传感器测量的不确定性协方差矩阵,而是通过视觉信息和激光点云信息对周围环境的感知来判断系统是否出现退化现象,进而通过针对性设计的优化策略对系统输出做出调整。
本发明的目的是提升农业场景下无人车定位的精准性、稳定性和可靠性,解决现有定位系统环境适应性差的问题,提出了一种基于多传感器融合的农业无人车定位里程计方法,其中包括:
步骤1、农业无人车设有视觉传感器、惯性传感器和深度传感器,通过该视觉传感器检测获取该农业无人车所处环境的多个视觉特征点,同时利用该深度传感器采集到的环境点云获取视觉特征点的深度信息,将获取到深度信息的视觉特征点作为稳定特征点,统计稳定特征点数量占视觉特征点总数的比例,作为当前环境的复杂度;
步骤2、该惯性传感器采集相对位姿信息,根据该相对位姿信息生成惯性位姿信息;通过卡尔曼滤波器融合该惯性位姿信息和该视觉特征点,得到第一位姿信息,根据该相对位姿信息为该环境点云进行去畸形化处理,得到校正点云,以结合该惯性位姿信息执行点云配准处理,得到第二位姿信息;
步骤3、根据该复杂度,选择该第一位姿信息或该第二位姿信息,作为该农业无人车的最终定位结果。
所述的基于多传感器融合的农业无人车定位里程计方法,其中该去畸形化处理包括:
其中Δφtk代表t时刻接收到的点云中的第k个点的姿态修正信息,Δptk代表t时刻接收到的点云中的第k个点的位置修正信息,t代表该点实际接收到的时刻,tk代表该帧点云接受的起始时间,Δt代表该帧点云接收经历的总时长,Δφk,k+1和Δpk,k+1分别代表该惯性传感器计算出的该帧点云相对于上一帧的姿态变换和位置变换;
融合该姿态修正信息和该位置修正信息,构成位姿变换矩阵,通过该姿变换矩阵与该环境点云的3D坐标相乘,得到该校正点云的3D坐标。
所述的基于多传感器融合的农业无人车定位里程计方法,其中该惯性位姿信息的生成过程包括:
该惯性位姿信息包括和分别代表姿态和位置的变换信息,tk和tk-1代表接收到两帧连续点云或者图像的时刻,下标k代表点云或者图像的序号,ωt和at代表t时该惯性传感器采集的角速度和加速度,vk-1代表接收到第k-1点云或者图像时测量得到的速度;
所述的基于多传感器融合的农业无人车定位里程计方法,其中该点云配准处理包括:
将该校正点云进行网格化处理,对每一个网格中所包含的激光点云采用主成分分析的方法进行正态分布计算;其中每一帧点云经过PCA后的点云分布表示如下:
待匹配的两帧点云分别为P={pi}和Q={qi},基于正态分布变换的迭代最近点的目标函数为:
式中R和τ分别代表两帧点云变化间的旋转信息和平移信息,取该目标函数值最小时对应的R和τ,作为变换关系Tkk-1,Tkk-1代表第k帧点云和第k-1点云之间的旋转信息R和平移信息t;
该第二位姿信息Tk为:Tk=∏k TkTk-1。
所述的基于多传感器融合的农业无人车定位里程计方法,其中该第一位姿信息生成过程包括:
根据激光雷达坐标系和相机坐标系间的变换信息TLC,将点云坐标投影从激光雷达坐标系投影到相机坐标系中,该视觉特征点通过搜索即可获取到深度;设激光点云坐标为p(x,y,z),投影后的坐标为p'(x',y',z'),跟据下列公式进行投影:
p’=TLCp
视觉特征点在进行深度获取前的像素坐标为x(u,v,1),经过激光点云投影之后,对每一个视觉特征点,在指定半径内搜索投影后的激光点云,统计所有搜索到的点云的深度平均值作为视觉特征点的深度信息,获取深度后的视觉特征点坐标通过对极几何的方法计算出视觉特征点的空间位置坐标P(X,Y,Z);
其中K代表相机的内参矩阵,R代表所要解算的位姿;
所述的基于多传感器融合的农业无人车定位里程计方法,其中该步骤3包括:
通过视觉特征点检测模块得出的视觉特征点个数为n,其中m个特征点可激光雷达点云通过获取到深度,则环境复杂度E:
该最终定位结果Tfinal为:
其中threshold为常数,取值范围在0到1之间。
本发明还提出了一种基于多传感器融合的农业无人车定位里程计系统,其中包括:
初始模块,用于通过农业无人车的视觉传感器检测获取该农业无人车所处环境的多个视觉特征点,同时利用农业无人车的深度传感器采集到的环境点云获取视觉特征点的深度信息,将获取到深度信息的视觉特征点作为稳定特征点,统计稳定特征点数量占视觉特征点总数的比例,作为当前环境的复杂度;
定位模块,用于通过农业无人车的惯性传感器采集相对位姿信息,根据该相对位姿信息生成惯性位姿信息;通过卡尔曼滤波器融合该惯性位姿信息和该视觉特征点,得到第一位姿信息,根据该相对位姿信息为该环境点云进行去畸形化处理,得到校正点云,以结合该惯性位姿信息执行点云配准处理,得到第二位姿信息;
决策模块,用于根据该复杂度,选择该第一位姿信息或该第二位姿信息,作为该农业无人车的最终定位结果。
所述的基于多传感器融合的农业无人车定位里程计系统,其中该去畸形化处理包括:
其中Δφtk代表t时刻接收到的点云中的第k个点的姿态修正信息,Δptk代表t时刻接收到的点云中的第k个点的位置修正信息,t代表该点实际接收到的时刻,tk代表该帧点云接受的起始时间,Δt代表该帧点云接收经历的总时长,Δφk,k+1和Δpk,k+1分别代表该惯性传感器计算出的该帧点云相对于上一帧的姿态变换和位置变换;
融合该姿态修正信息和该位置修正信息,构成位姿变换矩阵,通过该姿变换矩阵与该环境点云的3D坐标相乘,得到该校正点云的3D坐标;
该惯性位姿信息的生成过程包括:
该惯性位姿信息包括和分别代表姿态和位置的变换信息,tk和tk-1代表接收到两帧连续点云或者图像的时刻,下标k代表点云或者图像的序号,ωt和at代表t时该惯性传感器采集的角速度和加速度,vk-1代表接收到第k-1点云或者图像时测量得到的速度;
该点云配准处理包括:
将该校正点云进行网格化处理,对每一个网格中所包含的激光点云采用主成分分析的方法进行正态分布计算;其中每一帧点云经过PCA后的点云分布表示如下:
待匹配的两帧点云分别为P={pi}和Q={qi},基于正态分布变换的迭代最近点的目标函数为:
式中R和τ分别代表两帧点云变化间的旋转信息和平移信息,取该目标函数值最小时对应的R和τ,作为变换关系Tkk-1,Tkk-1代表第k帧点云和第k-1点云之间的旋转信息R和平移信息t;
该第二位姿信息Tk为:Tk=ΠkTkTk-1;
该第一位姿信息生成过程包括:
根据激光雷达坐标系和相机坐标系间的变换信息TLC,将点云坐标投影从激光雷达坐标系投影到相机坐标系中,该视觉特征点通过搜索即可获取到深度;设激光点云坐标为p(x,y,z),投影后的坐标为p'(x',y',z'),跟据下列公式进行投影:
p’=TLCp
视觉特征点在进行深度获取前的像素坐标为x(u,v,1),经过激光点云投影之后,对每一个视觉特征点,在指定半径内搜索投影后的激光点云,统计所有搜索到的点云的深度平均值作为视觉特征点的深度信息,获取深度后的视觉特征点坐标通过对极几何的方法计算出视觉特征点的空间位置坐标P(X,Y,Z);
其中K代表相机的内参矩阵,R代表所要解算的位姿;
该决策模块用于通过视觉特征点检测模块得出的视觉特征点个数为n,其中m个特征点可激光雷达点云通过获取到深度,则环境复杂度E:
该最终定位结果Tfinal为:
其中threshold为常数,取值范围在0到1之间。
本发明还提出了一种存储介质,用于存储执行所述任意一种基于多传感器融合的农业无人车定位里程计方法的程序。
本发明还提出了一种客户端,用于所述任意一种基于多传感器融合的农业无人车定位里程计系统。
由以上方案可知,本发明的优点在于:
该发明提出的多传感器融合里程计包含两个子里程计系统,即激光惯性里程计和视觉惯性里程计。两个子系统同时运行提供两个位置信息输出,通过视觉信息和激光点云信息来对周围环境进行动态感知,计算分析环境复杂度,进而判断哪个子系统的输出可作为最终的定位信息输出。该系统满足农业环境下的场景多变特性对无人车定位技术的精准性、稳定性和可靠性的需求。
附图说明
图1为本发明中提出的方法的硬件集成载体图;
图2为本发明中提出的方法的软件框架图;
图3为本发明中提出的方法的具体操作流程图。
具体实施方式
基于上述分析,本发明提出了一种应用于农业场景的多传感器融合里程计系统。该里程计系统基于三种定位传感器(单目相机、16线激光雷达、惯性测量单元IMU)和车辆信息(视觉信息、激光点云信息、关系信息),利用SLAM(同步建图与定位)等相关技术进行信息融合,结合农业场景的特点进行针对性优化,最终输出精准稳定的农业无人车定位信息,同时兼顾农业场景下低成本的需求。具体来说,本发明包括如下关键技术点:
关键点1,环境复杂度分析技术:由于视觉信息对于环境的细节描述更为丰富和具体,激光点云信息对于环境的几何特征描述较为详细,因此基于视觉信息和点云信息来对当前无人车所处的农业环境进行复杂度分析。通过视觉特征检测获取环境中的特征点,同时利用激光雷达采集到的环境点云信息获取视觉特征点的深度信息。由于激光点云是离散的,视觉特征点所在的位置不一定有对应的激光点,故本发明根据视觉特征点是否能获取到深度将特征点分为稳定特征点和不稳定特征点,统计稳定特征点的数目所占特征点总数的比例,以此作为当前环境的复杂度描述;稳定特征点的提取与复杂度的分析同时使用到了视觉信息和点云信息,能够较为准确地评价当前无人车所处环境包含的特征情况,使融合里程计系统的环境适应性更强。
关键点2,激光-视觉-IMU传感器融合技术:为了提升里程计系统输出的稳定性和运行可靠性,使用两个子里程计系统(视觉惯性里程计和激光惯性里程计)。视觉惯性里程计系统由单目相机和IMU驱动,通过卡尔曼滤波器来融合相机和IMU的信息,激光惯性里程计系统由激光雷达和IMU驱动,IMU输出的相对位置信息为激光雷达点云进行去畸形化处理和为ICP点云配准算法提供初始化位置估计,根据关键点1中所述的环境复杂度来决定系统的最终的位姿信息输出。两个子里程计同时运行,通过环境复杂度分析策略来兼顾无人车位置信息输出的准确和连续,减少里程计系统退化的现象。
为让本发明的上述特征和效果能阐述的更明确易懂,下文特举实施例,并配合说明书附图作详细说明如下。
具体实施方式如下:
1.将硬件进行系统化集成(如图1所示);
2.解析接收来自三种传感器(惯性单元IMU,激光雷达,相机)的原始数据;
3.惯性单元数据处理
3.1根据IMU输入信息进行点云去畸化
从IMU采集到的惯性数据首先用于激光雷达点云的去畸化处理,消除由于激光雷达旋转导致的同一帧扫描到的点云的位置误差。去畸化的方式基于匀速运动假设,如下所示:
其中Δφtk代表t时刻接收到的点云中的第k个点的姿态修正信息,Δptk代表t时刻接收到的点云中的第k个点的位置修正信息,t代表该点实际接收到的时刻,tk代表该帧点云接受的起始时间,Δt代表该帧点云接收经历的总时长,Δφk,k+1和Δpk,k+1代表IMU计算出的该帧点云相对于上一帧的位姿变换情况。
将(1)中计算得出的点云修正信息转化为位姿变换矩阵R,根据下列公式计算去畸化之后的点云坐标。Poriginal和Pcorrect分别代表去畸化前后的点云3D坐标。
Pcorrect=RPoriginal (2)
3.2 IMU预计算初始化信息
根据以下公式计算固定时间间隔的两帧激光点云或者两帧视觉图像之间的位置变换信息,其中和分别代表预计算出的姿态和位置的变换信息,tk和tk-1代表接收到两帧连续点云或者图像的时刻,下标k代表点云或者图像的序号,ωt和at代表t时刻IMU测量得到的角速度和加速度,vk-1代表接收到第k-1点云或者图像时测量得到的速度。
4.激光雷达点云数据处理:
4.1点云配准ICP(迭代最近点法)算法
从激光雷达获取到的点云信息在经历接收解析和去畸化等处理后,通过ICP(迭代最近点法)进行位姿测算。本发明采用基于NDT(正态分布变换)的ICP算法进行计算。首先,将点云进行网格化处理,每一个网格的尺寸是1m×1m×1m,对每一个网格中所包含的激光点云采用PCA(主成分分析)的方法进行正态分布计算,之后基于这些正态分布进行ICP配准。
设每一帧点云经过PCA后的点云分布表示如下:
其中代表该帧点云第i个网格中正态分布的中心点坐标,代表该帧点云第i个网格的协方差矩阵。A代表该帧点云中的所有网格经过PCA后的概率分布集合,ai代表第i个网格中的点云概率分布,与ai的意思相同,只是对ai的具体解释。
使用公式(5)中的表示方法,设需要进行匹配的两帧点云分别为P={pi}和Q={qi},则基于NDT的ICP算法的目标函数如下:
R和τ分别代表两帧点云变化间的旋转信息和平移信息,通过ICP算法不断优化修改R和τ使得目标函数值最小,即可最终求得两帧点云之间的变换关系Tkk-1,其中Tkk-1代表第k帧点云和第k-1点云之间的旋转信息R和平移信息τ。
最终激光惯性子系统输出的无人车定位信息Tk为:
Tk=ΠkTkTk-1 (7)
4.2特征点深度获取
激光雷达采集到的点云信息可以为视觉特征点提供深度信息,根据预先标定好的激光雷达坐标系和相机坐标系之间的变换信息TLC,将点云坐标投影从激光雷达坐标系投影到相机坐标系中,视觉特征点通过搜索即可获取到深度。设激光点云坐标为p(x,y,z),投影后的坐标为p'(x',y',z'),跟据下列公式进行投影:
p'=TLCp (8)
设视觉特征点在进行深度获取前的像素坐标为x(u,v,1),经过激光点云投影之后,对每一个视觉特征点,在半径为0.5m之内搜索投影后的激光点云,之后统计所有搜索到的点云,计算其深度的平均值作为视觉特征点的深度信息,获取深度后的视觉特征点坐标如下所示:
5.视觉信息数据处理:
5.1视觉特征点提取与匹配
采用角点检测算法提取视觉图像中的SURF、SIFT或ORB特征点描述子,将每个的特征点在相机坐标系下的像素坐标表示为:
x(u,v,1) (10)
之后搜索两帧视觉图像之间对应的匹配特征点,使用快速近似最邻近的方法来寻找匹配的特征点。通过4.2中的方法获取到特征点的深度信息,获取到深度的特征点可表示为公式(9)。最后通过对极几何的方法计算出特征点的空间位置坐标,表示如下。快速近似最邻近法和对极几何等方法已较为成熟,在此不再赘述。
P(X,Y,Z) (11)
5.2光束法平差
得到匹配特征点之后,使用光束法(Bundle Adjustment)平差来最小化匹配特征点间的重投影误差,进而解算出相机的位姿(转化之后即可获取到无人车的位置信息)。设有n个特征点的空间坐标P表示为公式(11),它们的像素坐标u表示为公式(9)光束法平差优化的目标函数如下所示。
其中K代表相机的内参矩阵,R代表所要解算的位姿。
5.3卡尔曼滤波
通过光束法平差得到的位姿,需要与IMU输入的惯性信息进一步融合得出视觉惯性里程计子系统的输出。融合的方法采用扩展卡尔曼滤波。首先使用IMU预计算得到的位姿公式(3)(4)来进行卡尔曼滤波器的预测步骤,得出预测的位姿之后在卡尔曼滤波的更新步骤中计算卡尔曼滤波增益K,最后融合公式(12)中优化得出的位姿z,作为最终的位置信息输出
其中C表示位姿测算的协方差矩阵。扩展卡尔曼滤波器的应用已较为成熟,详细流程不再赘述。
6.环境复杂度分析与系统决策
设通过视觉特征点检测模块得出的视觉特征点个数为n,其中m个特征点可激光雷达点云通过获取到深度,则定义环境复杂度E为:
最终得系统位置信息Tfinal输出为:
其中threshold为常数,取值范围在0到1之间。
以下为与上述方法实施例对应的系统实施例,本实施方式可与上述实施方式互相配合实施。上述实施方式中提到的相关技术细节在本实施方式中依然有效,为了减少重复,这里不再赘述。相应地,本实施方式中提到的相关技术细节也可应用在上述实施方式中。
本发明还提出了一种基于多传感器融合的农业无人车定位里程计系统,其中包括:
初始模块,用于通过农业无人车的视觉传感器检测获取该农业无人车所处环境的多个视觉特征点,同时利用农业无人车的深度传感器采集到的环境点云获取视觉特征点的深度信息,将获取到深度信息的视觉特征点作为稳定特征点,统计稳定特征点数量占视觉特征点总数的比例,作为当前环境的复杂度;
定位模块,用于通过农业无人车的惯性传感器采集相对位姿信息,根据该相对位姿信息生成惯性位姿信息;通过卡尔曼滤波器融合该惯性位姿信息和该视觉特征点,得到第一位姿信息,根据该相对位姿信息为该环境点云进行去畸形化处理,得到校正点云,以结合该惯性位姿信息执行点云配准处理,得到第二位姿信息;
决策模块,用于根据该复杂度,选择该第一位姿信息或该第二位姿信息,作为该农业无人车的最终定位结果。
所述的基于多传感器融合的农业无人车定位里程计系统,其中该去畸形化处理包括:
其中Δφtk代表t时刻接收到的点云中的第k个点的姿态修正信息,Δptk代表t时刻接收到的点云中的第k个点的位置修正信息,t代表该点实际接收到的时刻,tk代表该帧点云接受的起始时间,Δt代表该帧点云接收经历的总时长,Δφk,k+1和Δpk,k+1分别代表该惯性传感器计算出的该帧点云相对于上一帧的姿态变换和位置变换;
融合该姿态修正信息和该位置修正信息,构成位姿变换矩阵,通过该姿变换矩阵与该环境点云的3D坐标相乘,得到该校正点云的3D坐标;
该惯性位姿信息的生成过程包括:
该惯性位姿信息包括和分别代表姿态和位置的变换信息,tk和tk-1代表接收到两帧连续点云或者图像的时刻,下标k代表点云或者图像的序号,ωt和at代表t时该惯性传感器采集的角速度和加速度,vk-1代表接收到第k-1点云或者图像时测量得到的速度;
该点云配准处理包括:
将该校正点云进行网格化处理,对每一个网格中所包含的激光点云采用主成分分析的方法进行正态分布计算;其中每一帧点云经过PCA后的点云分布表示如下:
待匹配的两帧点云分别为P={pi}和Q={qi},基于正态分布变换的迭代最近点的目标函数为:
式中R和τ分别代表两帧点云变化间的旋转信息和平移信息,取该目标函数值最小时对应的R和τ,作为变换关系Tkk-1,Tkk-1代表第k帧点云和第k-1点云之间的旋转信息R和平移信息t;
该第二位姿信息Tk为:Tk=ΠkTkTk-1;
该第一位姿信息生成过程包括:
根据激光雷达坐标系和相机坐标系间的变换信息TLC,将点云坐标投影从激光雷达坐标系投影到相机坐标系中,该视觉特征点通过搜索即可获取到深度;设激光点云坐标为p(x,y,z),投影后的坐标为p'(x',y',z'),跟据下列公式进行投影:
p’=TLCp
视觉特征点在进行深度获取前的像素坐标为x(u,v,1),经过激光点云投影之后,对每一个视觉特征点,在指定半径内搜索投影后的激光点云,统计所有搜索到的点云的深度平均值作为视觉特征点的深度信息,获取深度后的视觉特征点坐标通过对极几何的方法计算出视觉特征点的空间位置坐标P(X,Y,Z);
其中K代表相机的内参矩阵,R代表所要解算的位姿;
该决策模块用于通过视觉特征点检测模块得出的视觉特征点个数为n,其中m个特征点可激光雷达点云通过获取到深度,则环境复杂度E:
该最终定位结果Tfinal为:
其中threshold为常数,取值范围在0到1之间。
本发明还提出了一种存储介质,用于存储执行所述任意一种基于多传感器融合的农业无人车定位里程计方法的程序。
本发明还提出了一种客户端,用于所述任意一种基于多传感器融合的农业无人车定位里程计系统。
Claims (10)
1.一种基于多传感器融合的农业无人车定位里程计方法,其特征在于,包括:
步骤1、农业无人车设有视觉传感器、惯性传感器和深度传感器,通过该视觉传感器检测获取该农业无人车所处环境的多个视觉特征点,同时利用该深度传感器采集到的环境点云获取视觉特征点的深度信息,将获取到深度信息的视觉特征点作为稳定特征点,统计稳定特征点数量占视觉特征点总数的比例,作为当前环境的复杂度;
步骤2、该惯性传感器采集相对位姿信息,根据该相对位姿信息生成惯性位姿信息;通过卡尔曼滤波器融合该惯性位姿信息和该视觉特征点,得到第一位姿信息,根据该相对位姿信息为该环境点云进行去畸形化处理,得到校正点云,以结合该惯性位姿信息执行点云配准处理,得到第二位姿信息;
步骤3、根据该复杂度,选择该第一位姿信息或该第二位姿信息,作为该农业无人车的最终定位结果。
4.如权利要求3所述的基于多传感器融合的农业无人车定位里程计方法,其特征在于,该点云配准处理包括:
将该校正点云进行网格化处理,对每一个网格中所包含的激光点云采用主成分分析的方法进行正态分布计算;其中每一帧点云经过PCA后的点云分布表示如下:
待匹配的两帧点云分别为P={pi}和Q={qi},基于正态分布变换的迭代最近点的目标函数为:
式中R和τ分别代表两帧点云变化间的旋转信息和平移信息,取该目标函数值最小时对应的R和τ,作为变换关系Tkk-1,Tkk-1代表第k帧点云和第k-1点云之间的旋转信息R和平移信息t;
该第二位姿信息Tk为:Tk=∏kTkTk-1。
5.如权利要求4所述的基于多传感器融合的农业无人车定位里程计方法,其特征在于,该第一位姿信息生成过程包括:
根据激光雷达坐标系和相机坐标系间的变换信息TLC,将点云坐标投影从激光雷达坐标系投影到相机坐标系中,该视觉特征点通过搜索即可获取到深度;设激光点云坐标为p(x,y,z),投影后的坐标为p'(x',y',z'),跟据下列公式进行投影:
p′=TLCp
视觉特征点在进行深度获取前的像素坐标为x(u,v,1),经过激光点云投影之后,对每一个视觉特征点,在指定半径内搜索投影后的激光点云,统计所有搜索到的点云的深度平均值作为视觉特征点的深度信息,获取深度后的视觉特征点坐标通过对极几何的方法计算出视觉特征点的空间位置坐标P(X,Y,Z);
其中K代表相机的内参矩阵,R代表所要解算的位姿;
7.一种基于多传感器融合的农业无人车定位里程计系统,其特征在于,包括:
初始模块,用于通过农业无人车的视觉传感器检测获取该农业无人车所处环境的多个视觉特征点,同时利用农业无人车的深度传感器采集到的环境点云获取视觉特征点的深度信息,将获取到深度信息的视觉特征点作为稳定特征点,统计稳定特征点数量占视觉特征点总数的比例,作为当前环境的复杂度;
定位模块,用于通过农业无人车的惯性传感器采集相对位姿信息,根据该相对位姿信息生成惯性位姿信息;通过卡尔曼滤波器融合该惯性位姿信息和该视觉特征点,得到第一位姿信息,根据该相对位姿信息为该环境点云进行去畸形化处理,得到校正点云,以结合该惯性位姿信息执行点云配准处理,得到第二位姿信息;
决策模块,用于根据该复杂度,选择该第一位姿信息或该第二位姿信息,作为该农业无人车的最终定位结果。
8.如权利要求7所述的基于多传感器融合的农业无人车定位里程计系统,其特征在于,该去畸形化处理包括:
其中Δφtk代表t时刻接收到的点云中的第k个点的姿态修正信息,Δptk代表t时刻接收到的点云中的第k个点的位置修正信息,t代表该点实际接收到的时刻,tk代表该帧点云接受的起始时间,Δt代表该帧点云接收经历的总时长,Δφk,k+1和Δpk,k+1分别代表该惯性传感器计算出的该帧点云相对于上一帧的姿态变换和位置变换;
融合该姿态修正信息和该位置修正信息,构成位姿变换矩阵,通过该姿变换矩阵与该环境点云的3D坐标相乘,得到该校正点云的3D坐标;
该惯性位姿信息的生成过程包括:
该惯性位姿信息包括和分别代表姿态和位置的变换信息,tk和tk-1代表接收到两帧连续点云或者图像的时刻,下标k代表点云或者图像的序号,ωt和at代表t时该惯性传感器采集的角速度和加速度,vk-1代表接收到第k-1点云或者图像时测量得到的速度;
该点云配准处理包括:
将该校正点云进行网格化处理,对每一个网格中所包含的激光点云采用主成分分析的方法进行正态分布计算;其中每一帧点云经过PCA后的点云分布表示如下:
待匹配的两帧点云分别为P={pi}和Q={qi},基于正态分布变换的迭代最近点的目标函数为:
式中R和τ分别代表两帧点云变化间的旋转信息和平移信息,取该目标函数值最小时对应的R和τ,作为变换关系Tkk-1,Tkk-1代表第k帧点云和第k-1点云之间的旋转信息R和平移信息t;
该第二位姿信息Tk为:Tk=ΠkTkTk-1;
该第一位姿信息生成过程包括:
根据激光雷达坐标系和相机坐标系间的变换信息TLC,将点云坐标投影从激光雷达坐标系投影到相机坐标系中,该视觉特征点通过搜索即可获取到深度;设激光点云坐标为p(x,y,z),投影后的坐标为p'(x',y',z'),跟据下列公式进行投影:
p′=TLCp
视觉特征点在进行深度获取前的像素坐标为x(u,v,1),经过激光点云投影之后,对每一个视觉特征点,在指定半径内搜索投影后的激光点云,统计所有搜索到的点云的深度平均值作为视觉特征点的深度信息,获取深度后的视觉特征点坐标通过对极几何的方法计算出视觉特征点的空间位置坐标P(X,Y,Z);
其中K代表相机的内参矩阵,R代表所要解算的位姿;
该决策模块用于通过视觉特征点检测模块得出的视觉特征点个数为n,其中m个特征点可激光雷达点云通过获取到深度,则环境复杂度E:
该最终定位结果Tfinal为:
其中threshold为常数,取值范围在0到1之间。
9.一种存储介质,用于存储执行如权利要求1到6所述任意一种基于多传感器融合的农业无人车定位里程计方法的程序。
10.一种客户端,用于权利要求7或8中任意一种基于多传感器融合的农业无人车定位里程计系统。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210950380.XA CN115451948A (zh) | 2022-08-09 | 2022-08-09 | 一种基于多传感器融合的农业无人车定位里程计方法及系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210950380.XA CN115451948A (zh) | 2022-08-09 | 2022-08-09 | 一种基于多传感器融合的农业无人车定位里程计方法及系统 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN115451948A true CN115451948A (zh) | 2022-12-09 |
Family
ID=84297048
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210950380.XA Pending CN115451948A (zh) | 2022-08-09 | 2022-08-09 | 一种基于多传感器融合的农业无人车定位里程计方法及系统 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115451948A (zh) |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116518992A (zh) * | 2023-04-14 | 2023-08-01 | 之江实验室 | 一种退化场景下的无人车定位方法和装置 |
CN116660923A (zh) * | 2023-08-01 | 2023-08-29 | 齐鲁空天信息研究院 | 一种融合视觉和激光雷达的无人农机机库定位方法和系统 |
CN116878504A (zh) * | 2023-09-07 | 2023-10-13 | 兰笺(苏州)科技有限公司 | 基于多传感器融合的建筑物外墙作业无人机精准定位方法 |
CN117113284A (zh) * | 2023-10-25 | 2023-11-24 | 南京舜云智慧城市科技有限公司 | 多传感器融合的数据处理方法、装置与多传感器融合方法 |
-
2022
- 2022-08-09 CN CN202210950380.XA patent/CN115451948A/zh active Pending
Cited By (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116518992A (zh) * | 2023-04-14 | 2023-08-01 | 之江实验室 | 一种退化场景下的无人车定位方法和装置 |
CN116518992B (zh) * | 2023-04-14 | 2023-09-08 | 之江实验室 | 一种退化场景下的无人车定位方法和装置 |
CN116660923A (zh) * | 2023-08-01 | 2023-08-29 | 齐鲁空天信息研究院 | 一种融合视觉和激光雷达的无人农机机库定位方法和系统 |
CN116660923B (zh) * | 2023-08-01 | 2023-09-29 | 齐鲁空天信息研究院 | 一种融合视觉和激光雷达的无人农机机库定位方法和系统 |
CN116878504A (zh) * | 2023-09-07 | 2023-10-13 | 兰笺(苏州)科技有限公司 | 基于多传感器融合的建筑物外墙作业无人机精准定位方法 |
CN116878504B (zh) * | 2023-09-07 | 2023-12-08 | 兰笺(苏州)科技有限公司 | 基于多传感器融合的建筑物外墙作业无人机精准定位方法 |
CN117113284A (zh) * | 2023-10-25 | 2023-11-24 | 南京舜云智慧城市科技有限公司 | 多传感器融合的数据处理方法、装置与多传感器融合方法 |
CN117113284B (zh) * | 2023-10-25 | 2024-01-26 | 南京舜云智慧城市科技有限公司 | 多传感器融合的数据处理方法、装置与多传感器融合方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110988912B (zh) | 自动驾驶车辆的道路目标与距离检测方法、系统、装置 | |
CN110146909B (zh) | 一种定位数据处理方法 | |
CN110859044B (zh) | 自然场景中的集成传感器校准 | |
CN115451948A (zh) | 一种基于多传感器融合的农业无人车定位里程计方法及系统 | |
CN112197770B (zh) | 一种机器人的定位方法及其定位装置 | |
Levinson | Automatic laser calibration, mapping, and localization for autonomous vehicles | |
CN112634451A (zh) | 一种融合多传感器的室外大场景三维建图方法 | |
JP4984659B2 (ja) | 自車両位置推定装置 | |
JP6456141B2 (ja) | 地図データの生成 | |
CN111366153B (zh) | 一种激光雷达与imu紧耦合的定位方法 | |
CN116625354B (zh) | 一种基于多源测绘数据的高精度地形图生成方法及系统 | |
CN115272596A (zh) | 一种面向单调无纹理大场景的多传感器融合slam方法 | |
CN114325634A (zh) | 一种基于激光雷达的高鲁棒性野外环境下可通行区域提取方法 | |
CN115183762A (zh) | 一种机场仓库内外建图方法、系统、电子设备及介质 | |
Ericson et al. | Analysis of two visual odometry systems for use in an agricultural field environment | |
CN113327296A (zh) | 基于深度加权的激光雷达与相机在线联合标定方法 | |
CN117029870A (zh) | 一种基于路面点云的激光里程计 | |
KR102130687B1 (ko) | 다중 센서 플랫폼 간 정보 융합을 위한 시스템 | |
CN116385997A (zh) | 一种车载障碍物精确感知方法、系统及存储介质 | |
CN114723920A (zh) | 一种基于点云地图的视觉定位方法 | |
CN115280960A (zh) | 一种基于田间视觉slam的联合收获机转向控制方法 | |
Zhou et al. | Localization for unmanned vehicle | |
CN117576218B (zh) | 一种自适应的视觉惯导里程计输出方法 | |
CN117433511B (zh) | 一种多传感器融合定位方法 | |
CN115994934B (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 |