CN112362051B - 一种基于信息融合的移动机器人导航定位系统 - Google Patents

一种基于信息融合的移动机器人导航定位系统 Download PDF

Info

Publication number
CN112362051B
CN112362051B CN202011106671.8A CN202011106671A CN112362051B CN 112362051 B CN112362051 B CN 112362051B CN 202011106671 A CN202011106671 A CN 202011106671A CN 112362051 B CN112362051 B CN 112362051B
Authority
CN
China
Prior art keywords
data
gnss
positioning
imu
time
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
CN202011106671.8A
Other languages
English (en)
Other versions
CN112362051A (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.)
Wuxi Kalman Navigation Technology Co ltd
Original Assignee
Wuxi Kalman Navigation Technology Co ltd
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 Wuxi Kalman Navigation Technology Co ltd filed Critical Wuxi Kalman Navigation Technology Co ltd
Priority to CN202011106671.8A priority Critical patent/CN112362051B/zh
Publication of CN112362051A publication Critical patent/CN112362051A/zh
Application granted granted Critical
Publication of CN112362051B publication Critical patent/CN112362051B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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
    • 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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • 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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled
    • G—PHYSICS
    • G08—SIGNALLING
    • G08C—TRANSMISSION SYSTEMS FOR MEASURED VALUES, CONTROL OR SIMILAR SIGNALS
    • G08C17/00—Arrangements for transmitting signals characterised by the use of a wireless electrical link
    • G08C17/02—Arrangements for transmitting signals characterised by the use of a wireless electrical link using a radio link

Landscapes

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

Abstract

本发明公开了一种基于GNSS/INS/LIDAR‑SLAM信息融合的移动机器人导航定位系统，属于机器人导航领域。本发明系统采用扩展卡尔曼滤波器融合GNSS定位结果、IMU数据以及激光雷达建图定位的相对位姿，获得移动机器人的定位结果。其中，对于计算性能需求大的SLAM算法，该系统采用基于特征提取的3D激光雷达SLAM技术，通过特征预提取降低点云匹配的计算量，从而使得该导航定位系统能够运行在嵌入式平台上。

Description

一种基于信息融合的移动机器人导航定位系统
技术领域
本发明涉及一种基于GNSS/INS/LIDAR-SLAM信息融合的移动机器人导航定位系统，属于机器人导航领域。
背景技术
随着无人驾驶技术和智能机器人的快速发展，GNSS、IMU、LIDAR这三种定位系统在无人驾驶领域被广泛研究及应用。
基于全球导航卫星系统(GNSS)的定位容易受到遮挡、多径的影响，导致定位精度的下降，甚至无法定位；惯性测量器件(IMU)可以在短时间内持续提供相对定位，但定位误差会随时间快速累积变大；对于陌生环境，融合定位与建图技术(SLAM)被广泛应用，SLAM系统能够在运动中逐步建立地图及定位，但是，SLAM系统的定位误差主要依靠回环检测来降低，对于大型户外环境，形成回环的可能性很小；另外，SLAM不能提供绝对定位，除非预制高精度地图，这通常在无人驾驶中使用，但对于使用嵌入式平台的移动机器人来说，这样做的成本很高。
用于SLAM的传感器通常有两种：摄像头和激光雷达(LIDAR)。相对于摄像头，激光雷达能够直接获取环境的结构信息，不易受光照和天气的影响。为了从LIDAR的测量数据中获得位姿信息，通常有3类方式：基于特征的匹配、基于点的匹配和基于概率分布的匹配。基于点的匹配直接搜索和匹配激光雷达数据中相应的点，精度高，不依赖环境特征，但是计算量很大，很难应用到嵌入式的计算平台。基于特征的匹配预先提取环境特征，比如角点、平面等，大大降低了匹配的数据，计算快，精度高，但是依赖环境特征，在特征稀少的环境下无法正常运行。基于概率分布的匹配通常采用三维占据栅格地图，通过三维栅格被占据的概率表示环境中物体的分布，避免了点的直接匹配，降低了计算量，对于一般的嵌入式计算平台，计算量仍然过大，如果要进一步降低计算量，需要增大栅格大小，这又会导致精度降低。
另外，现有的移动机器人导航定位系统多针对复杂的城市高楼环境，而较为空旷、移动物体较少、GNSS定位绝大多数时间可靠的陌生户外环境，或者，复杂环境与空旷环境交接处，也需要特定的定位方法来满足实际需求。
发明内容
[技术问题]
本发明要解决的技术问题是提供针对较为空旷、移动物体较少、GNSS定位绝大多数时间可靠的陌生户外环境下的、或者与城市高楼环境靠近的空旷环境下的、或者复杂城市高楼环境下的，使用嵌入式计算平台的移动机器人导航定位系统。
[技术方案]
本发明提出了一种基于GNSS/INS/LIDAR-SLAM信息融合的移动机器人导航定位系统，该系统的硬件组成为：GNSS天线，GNSS定位模块，无线模块，嵌入式计算平台，IMU，多线激光雷达，单片机。
所述GNSS天线连接GNSS定位模块，用于接收卫星导航信号。
所述无线模块选自包括但不限于WIFI模块、蓝牙模块、4G模块、2.4G数传模块中的一种，无线模块连接GNSS定位模块和嵌入式计算平台，能够通过无线接收差分电文数据(RTCM)，并发送给GNSS定位模块及嵌入式计算平台。
所述GNSS定位模块通过GNSS天线接收卫星导航信号，通过无线模块或嵌入式计算平台接收高精度差分定位所需的RTCM数据，负责解算得到高精度绝对定位数据和时间，并将定位数据和时间发送给单片机；其次，GNSS定位模块能够产生一秒一次的脉冲信号(1PPS)，该信号同时给单片机及多线激光雷达，用于同步传感器数据。
所述单片机与IMU连接，包括IMU数据到来中断接口及IMU数据接口，IMU发送三轴角速度及三轴加速度数据给单片机；单片机与GNSS定位模块连接，包括1PPS接口及数据接口；单片机与嵌入式计算平台连接，用于发送带时标的IMU数据及GNSS定位数据；单片机与多线激光雷达连接，用于发送时间数据。单片机负责接收GNSS板卡数据及IMU数据，并根据1PPS信号及GNSS数据中的时间数据，为IMU数据创建时标，同时将时间数据发送给多线激光雷达。
所述多线激光雷达根据单片机发送的时间数据及GNSS模块给出的1PPS信号，同步雷达扫描，得到点云数据，从而与IMU数据及GNSS定位数据时间同步。多线激光雷达将带有时标的点云数据发送给嵌入式计算平台。
所述嵌入式计算平台接收单片机发送的IMU数据及GNSS板卡定位数据，接收激光雷达发送的点云数据，融合三种数据，得到持续定位数据。嵌入式计算平台接收无线模块发送的 RTCM数据，将RTCM数据发送给GNSS定位模块。
本发明还提供一种应用上述系统来进行组合导航定位的方法，主要包括以下过程：
在1PPS信号到来的时刻，根据GNSS定位模块提供的时间数据，同步单片机内部的计数单元；在IMU数据到达中断触发时，使用内部计数单元的时间，作为本次的IMU数据时标，接收本次的IMU数据，将IMU数据与时标打包发送给嵌入式计算平台；在GNSS定位模块数据到达时，根据GNSS定位模块数据中的时间数据，构建符合NMEA标准的GPRMC 数据包并发送给激光雷达，将GNSS定位模块的数据发送给嵌入式计算平台；
等待IMU数据、GNSS定位数据、LIDAR点云数据就绪，嵌入式计算平台接收三个传感器的数据，扩展卡尔曼滤波器选取位置误差、速度误差、姿态误差、陀螺仪零偏误差、加速度计零偏误差作为状态量；
嵌入式计算平台接收GNSS定位数据，根据GNSS定位数据中的速度矢量判断是否进行组合导航初始化，当前速度矢量的大小超过设定阈值时，以此速度矢量相对于北东地坐标系的姿态作为移动机器人的初始姿态，此时的GNSS位置作为初始位置，此时的GNSS速度作为初始速度，开始组合导航；
当移动机器人从较开阔的环境逐渐进入GNSS无法正常定位的环境时，GNSS定位模块常常不能立刻定位失效，反而产生错误的定位数据，导致融合后的定位结果产生较大的漂移；对此，本发明可以采取两种措施剔除错误的GNSS定位数据：如果此时LIDAR-SLAM算法有足够多的特征点计算出相对位姿，当GNSS数据构建的位置误差观测向量超过设定的阈值时，判定GNSS定位错误，直到下一次GNSS重新定位前都不再使用GNSS的定位数据；通过计算一小段时间内移动机器人里程计增量与GNSS定位增量的误差，当误差超过设定的阈值时，判定GNSS定位错误，由于里程计在短时间内能够维持较高的精度，对于进入非定位环境时产生的较大偏移，该方法可以有效地检测出来；
嵌入式计算平台接收IMU数据，通过惯导解算算法，更新当前时刻的位置、速度、姿态；
嵌入式计算平台接收GNSS数据，构建位置误差、速度误差的观测向量，根据扩展卡尔曼滤波方程，更新当前时刻的位置、速度、姿态；
嵌入式计算平台接收LIDAR数据，首先对点云数据进行配准，经过配准可以去除具有相同下标的点云数据，以后，每个点数据都能通过唯一的两个下标去获取；根据历史导航结果，对点云进行畸变校正；
特征提取：通过计算点云的曲率，将曲率较大的点作为角点，曲率较小的点作为平面点，通过提取特征，只有角点和平面点保留下来，其余的点全部舍弃，获取了一组点云中的角点及平面点，如果这时点云地图为空，那么将这组角点及平面点转换到地图坐标系下保存，如果此时点云地图不空，那么可以构建一个非线性优化函数，采用高斯-牛顿或Levenberg-Marquardt等非线性优化算法进行迭代计算，得到该组点云到地图的位姿变换；
嵌入式平台的内存一般不大，该定位系统长时间工作时，形成的点云地图会占据大量的内存，对于这个问题，本发明在新的点云加入地图时，找出距离当前位姿超过设定阈值的点云，从点云地图中剔除，从而防止点云地图无限增大，耗尽系统的所有内存；
将得到的位姿变换，构建位置误差观测向量，根据扩展卡尔曼滤波方程，更新当前时刻的位置、速度、姿态。
[有益效果]
灵活的RTCM数据获取。比如，GNSS定位模块可以通过数传模块直接获取GNSS基准站发送的RTCM数据，也可以让嵌入式计算平台通过4G模块获得封装过的RTCM数据，解码得到原始的RTCM数据再发送给GNSS定位模块。这使得该系统可以应用于各种通信场合。
精准的传感器数据时间同步。该系统运用GNSS定位模块提供的1PPS信号及时间数据，用实时性较高的单片机同步IMU数据及GNSS定位数据，并将时间数据发送给激光雷达，实现了三个传感器的时间同步。精准的传感器数据时间同步能够提高数据融合定位算法的精度。
GNSS能够在开阔环境下获得高精度绝对定位，基于特征的LIDAR-SLAM算法能够在复杂环境下获得高精度相对定位，INS能够在短时间内持续相对定位，三者优势互补，且计算量较小，能够使配备嵌入式计算平台的移动机器人在室外陌生环境中获得持续的定位。
附图说明
图1实施例1所述定位系统的连接关系示意图。
图2实施例1所述定位系统的数据处理流程示意图。
图3是实施例2中的特定路线下的定位轨迹与点云地图。
图4是实施例2中的实验路线。
具体实施方式
实施例1
如图1所示，一种基于GNSS/INS/LIDAR-SLAM信息融合的移动机器人导航定位系统，其硬件组成为：GNSS天线，GNSS定位模块，无线模块，嵌入式计算平台，IMU，多线激光雷达，单片机。
GNSS天线连接GNSS定位模块，GNSS天线接收卫星导航信号并传递给GNSS定位模块。
无线模块选自包括但不限于WIFI模块、蓝牙模块、4G模块、2.4G数传模块中的一种，无线模块连接GNSS定位模块和嵌入式计算平台，能够通过无线接收差分电文数据(RTCM)，并发送给GNSS定位模块及嵌入式计算平台。
GNSS定位模块通过GNSS天线接收卫星导航信号，通过无线模块或嵌入式计算平台接收RTCM数据，负责解算得到绝对定位数据和时间，并将定位数据和时间发送给单片机；其次，GNSS定位模块能够产生一秒一次的脉冲信号(1PPS)，该信号同时给单片机及多线激光雷达，用于同步传感器数据。
单片机与IMU连接，包括IMU数据到来中断接口及IMU数据接口，IMU发送三轴角速度及三轴加速度数据给单片机；单片机与GNSS定位模块连接，包括1PPS接口及数据接口；单片机与嵌入式计算平台连接，用于发送带时标的IMU数据及GNSS定位数据；单片机与多线激光雷达连接，用于发送时间数据。单片机负责接收GNSS板卡数据及IMU数据，并根据 1PPS信号及GNSS数据中的时间数据，为IMU数据创建时标，同时将时间数据发送给多线激光雷达。
多线激光雷达根据单片机发送的时间数据及GNSS模块给出的1PPS信号，同步雷达扫描，得到点云数据，从而与IMU数据及GNSS定位数据时间同步。多线激光雷达将带有时标的点云数据发送给嵌入式计算平台。
嵌入式计算平台接收单片机发送的IMU数据及GNSS板卡定位数据，接收激光雷达发送的点云数据，融合三种数据，得到持续定位数据。嵌入式计算平台接收无线模块发送的RTCM 数据，将RTCM数据发送给GNSS定位模块。
具体地，
单片机工作流程：
在1PPS信号到来的时刻，根据GNSS定位模块提供的时间数据，同步单片机内部的计数单元。
在IMU数据到达中断触发时，使用内部计数单元的时间，作为本次的IMU数据时标。接收本次的IMU数据，将IMU数据与时标打包发送给嵌入式计算平台。
在GNSS定位模块数据到达时，根据GNSS定位模块数据中的时间数据，构建符合NMEA 标准的GPRMC数据包并发送给激光雷达。将GNSS定位模块的数据发送给嵌入式计算平台。
如图2所示，嵌入式计算平台工作流程：
等待IMU数据、GNSS定位数据、LIDAR点云数据就绪，接收三个传感器的数据。扩展卡尔曼滤波器选取位置误差、速度误差、姿态误差、陀螺仪零偏误差、加速度计零偏误差作为状态量，由于扩展卡尔曼滤波器用于IMU/GNSS融合定位的算法比较成熟，在许多书籍及论文中有详细描述，例如《捷联惯导算法与组合导航原理》(严恭敏，翁浚)，下面不再累述。
根据GNSS定位数据中的速度矢量判断是否进行组合导航初始化，当前速度矢量的大小超过设定阈值时，以此速度矢量相对于北东地坐标系的姿态作为移动机器人的初始姿态，此时的GNSS位置作为初始位置，此时的GNSS速度作为初始速度，开始组合导航。
接收IMU数据，通过惯导解算算法(《捷联惯导算法与组合导航原理》4.1节)，更新当前时刻的位置、速度、姿态。
接收GNSS数据，构建位置误差、速度误差的观测向量(《捷联惯导算法与组合导航原理》 6.3.3节)，根据扩展卡尔曼滤波方程，更新当前时刻的位置、速度、姿态。
接收LIDAR数据，首先对点云数据进行配准：将点云数据映射到一个二维数组中，以便于后期计算。对于点云数据中的每个点(每个点有x，y，z坐标，该坐标基于LIDAR自身坐标系)可以采用以下公式进行配准，但不限于以下公式：
Figure BDA0002727138430000061
Figure BDA0002727138430000062
其中，x，y，z表示点的坐标，anglebottom表示激光雷达的下探角度(为负值)，anglehorizon表示激光雷达的水平角度分辨率，hscan表示激光雷达的水平扫描线数，row、col表示二维数据的下标，resvertical表示激光雷达的垂直角度分辨率。
经过配准可以去除具有相同下标的点云数据，以后，每个点数据都能通过唯一的两个下标去获取。
由于激光雷达的扫描速度较慢，在机器人移动的过程中，会因为移动形成点云的畸变，影响后续的使用。可以根据历史导航结果，对点云进行畸变校正。对于配准后的每个点，按照以下公式修正畸变：
Figure BDA0002727138430000063
r2＝1-r1 式(4)
R＝Rlast·slerp(r1，Rnext) 式(5)
pos＝poslast*r1+posnext*r2 式(6)
p＝Rbegin T*(R*p+pos-posbegin) 式(7)
其中，t为该点的时标，timelast为该点时标前的历史导航结果的时标，timenext为该点时标后的历史导航结果的时标，r1，r2为比例系数，Rlast、poslast分别是timelast时刻的姿态矩阵与位置向量，Rnext、posnext分别是timenext时刻的姿态矩阵与位置向量，slerp为姿态插值算法，R是t时刻姿态的插值结果，pos是t时刻位置的插值结果，p为点云的三维坐标，Rbegin、posbegin分别是激光雷达扫描开始时刻的姿态矩阵与位置向量。
特征提取，该系统通过计算点云的曲率，将曲率较大的点作为角点，曲率较小的点作为平面点，特征提取不限于这种方式。曲率计算可以采用以下公式：
curve＝(drow,col-3+drow,col-2+drow,col-1+drow,col+1+drow,col+2+drow,col+3-6*drow,col)/drow,col
式(8)
其中，di，j表示配准后下标为i，j的点到激光雷达的距离，i象征式(8)中的下标row， j象征式(8)中的下标col-3、col-2、col-1、col+1、col+2、col+3、col。
通过提取特征，只有角点和平面点保留下来，其余的点全部舍弃。至此，该系统获取了一组点云中的角点及平面点，如果这时点云地图为空，那么将这组角点及平面点转换到地图坐标系下保存。如果此时点云地图不空，那么可以构建一个非线性优化函数，采用高斯-牛顿或Levenberg-Marquardt等非线性优化算法进行迭代计算，得到该组点云到地图的位姿变换。该非线性优化过程可以按以下方法进行，但不限于该方法：
步骤一，从最近的导航结果中获取初始的位姿变换。
步骤二，对每个特征点运用该位姿变换，计算地图坐标系下的坐标。
步骤三，对地图坐标系下每个特征点，运用kdtree寻找点云地图内最近的5个点，将这 5个点运用最小二乘法拟合三维空间内的平面。
步骤四，优化目标可以表示为在当前的位姿变换上叠加一个位姿变换增量使得每个特征点到对应平面的距离的平方和最小。
步骤五，采用非线性优化算法进行一次迭代计算，得到位姿变换增量，叠加到当前位姿上。
步骤六，迭代次数是否到达阈值，如果不满足，重复步骤2-5，否则，输出位姿变换，并应用该位姿变换，将该组点云的特征点变换到地图坐标系下，加入地图点云。
将得到的位姿变换，构建位置误差观测向量，根据扩展卡尔曼滤波方程，更新当前时刻的位置、速度、姿态。
通过融合GNSS/IMU/LIDAR的数据，很好地结合了各个传感器的优点，无论是开阔的环境还是复杂的环境，移动机器人都能够在陌生环境中获得持续的导航定位。
实施例2
我们采用无锡卡尔曼导航技术有限公司生产的智能平台控制器进行了实验，该控制器包含Nvidia的Jetson Nano嵌入式计算模块(4核A57，4GB LPDDR4内存)、STM32F107单片机、Openimu330B(IMU)、Unicore 4B0(GNSS模块)、EC20(4G通信模块)、LS LIDAR C16 (16线激光雷达)等。Jetson Nano运行了Linux操作系统，通过4G通信模块获取RTCM数据，并通过串口将数据发送给GNSS模块。STM32通过SPI接口获取IMU数据，通过串口获取GNSS模块数据，增加时标后又通过串口发送给Jetson Nano，此外通过另一个串口将时间数据发送给16线激光雷达。Jetson Nano通过STM32的串口获取IMU、GNSS数据，通过以太网接口获取激光雷达数据。在Jetson Nano的Linux系统上，部署了ROS(机器人操作系统)，ROS是一个便于机器人软件开发的中间系统，借助ROS的可视化工具进行融合定位结果的验证及展示。
如图4所示，选取一条经过空旷区域(图片左侧U型路线)及GNSS不可定位区域(楼内)的实验路线，控制实验平台行走完整个路线。图3为该路线下的定位轨迹与点云地图，可以发现，在空旷区域点云地图稀疏，此时主要依赖GNSS进行定位；在楼内点云地图密集，此时依赖LIDAR-SLAM进行定位；在室内外的交接处，可以发现定位轨迹略有跳变，这是因为单纯依赖LIDAR-SLAM进行定位时，定位产生了累计误差，当GNSS定位恢复时，累计误差得以消除，定位跳变到正确的位置上。实验证明，通过融合GNSS/IMU/LIDAR的数据，能够在GNSS失效的情况下，通过LIDAR-SLAM保持一定精度的连续定位，并在GNSS定位恢复后重新回到正确的位置上，实现较为空旷、移动物体较少、GNSS定位绝大多数时间可靠的陌生户外环境下的持续定位。
虽然本发明已以较佳实施例公开如上，但其并非用以限定本发明，任何熟悉此技术的人，在不脱离本发明的精神和范围内，都可做各种的改动与修饰，因此本发明的保护范围应该以权利要求书所界定的为准。

Claims (7)

1.一种利用移动机器人导航定位系统组合导航定位的方法，其特征在于，
所述移动机器人导航定位系统，是基于GNSS/INS/LIDAR-SLAM信息融合的移动机器人导航定位系统，包括：GNSS天线、GNSS定位模块、无线模块、嵌入式计算平台、IMU、多线激光雷达、单片机；
无线模块连接GNSS定位模块和嵌入式计算平台，GNSS天线连接GNSS定位模块，GNSS定位模块通过GNSS天线接收卫星导航信号；
单片机与IMU、GNSS定位模块、嵌入式计算平台、多线激光雷达连接；
所述方法主要包括以下过程：
单片机同步IMU数据及GNSS定位数据，并将时间数据发送给激光雷达，实现了三个传感器的时间同步；单片机在1PPS信号到来的时刻，根据GNSS定位模块提供的时间数据，同步单片机内部的计数单元；在IMU数据到达中断触发时，使用内部计数单元的时间，作为本次的IMU数据时标，接收本次的IMU数据，将IMU数据与时标打包发送给嵌入式计算平台；在GNSS定位模块数据到达时，根据GNSS定位模块数据中的时间数据，构建符合NMEA标准的GPRMC数据包并发送给激光雷达，将GNSS定位模块的数据发送给嵌入式计算平台；
等待IMU数据、GNSS定位数据、LIDAR点云数据就绪，嵌入式计算平台接收三个传感器的数据，扩展卡尔曼滤波器选取位置误差、速度误差、姿态误差、陀螺仪零偏误差、加速度计零偏误差作为状态量；
嵌入式计算平台接收GNSS定位数据，根据GNSS定位数据中的速度矢量判断是否进行组合导航初始化，当前速度矢量的大小超过设定阈值时，以此速度矢量相对于北东地坐标系的姿态作为移动机器人的初始姿态，此时的GNSS位置作为初始位置，此时的GNSS速度作为初始速度，开始组合导航；
嵌入式计算平台接收IMU数据，更新当前时刻的位置、速度、姿态；嵌入式计算平台接收GNSS数据，更新当前时刻的位置、速度、姿态；嵌入式计算平台接收LIDAR数据，首先对点云数据进行配准，根据历史导航结果，对点云进行畸变校正；
特征提取：通过计算点云的曲率，将曲率较大的点作为角点，曲率较小的点作为平面点，通过提取特征，获取了一组点云中的角点及平面点，将角点及平面点处理得到位姿变换，将得到的位姿变换，构建位置误差观测向量，根据扩展卡尔曼滤波方程，更新当前时刻的位置、速度、姿态；
当移动机器人从开阔的环境逐渐进入GNSS无法正常定位的环境时，采取两种措施剔除错误的GNSS定位数据：如果此时LIDAR-SLAM算法有足够多的特征点计算出相对位姿，当GNSS数据构建的位置误差观测向量超过设定的阈值时，判定GNSS定位错误，直到下一次GNSS重新定位前都不再使用GNSS的定位数据；或者，通过计算一小段时间内移动机器人里程计增量与GNSS定位增量的误差，当误差超过设定的阈值时，判定GNSS定位错误。
2.根据权利要求1所述的方法，其特征在于，在新的点云加入地图时，找出距离当前位姿超过设定阈值的点云，从点云地图中剔除，从而防止点云地图无限增大，耗尽系统的所有内存。
3.根据权利要求1所述的方法，其特征在于，所述无线模块选自WIFI模块、蓝牙模块、4G模块、2.4G数传模块。
4.根据权利要求1所述的方法，其特征在于，GNSS定位模块通过无线模块或嵌入式计算平台接收RTCM数据，负责解算得到绝对定位数据和时间，并将定位数据和时间发送给单片机；GNSS定位模块能够产生一秒一次的脉冲信号，该信号同时给单片机及多线激光雷达，用于同步传感器数据。
5.根据权利要求1所述的方法，其特征在于，所述单片机与IMU连接，包括IMU数据到来中断接口及IMU数据接口，IMU发送三轴角速度及三轴加速度数据给单片机；
单片机与GNSS定位模块连接，包括1PPS接口及数据接口；
单片机与嵌入式计算平台连接，用于发送带时标的IMU数据及GNSS定位数据；
单片机与多线激光雷达连接，用于发送时间数据。
6.根据权利要求5所述的方法，其特征在于，单片机负责接收GNSS板卡数据及IMU数据，并根据1PPS信号及GNSS数据中的时间数据，为IMU数据创建时标，同时将时间数据发送给多线激光雷达；
所述多线激光雷达根据单片机发送的时间数据及GNSS模块给出的1PPS信号，同步雷达扫描，得到点云数据，从而与IMU数据及GNSS定位数据时间同步。
7.根据权利要求5所述的方法，其特征在于，所述嵌入式计算平台接收单片机发送的IMU数据及GNSS板卡定位数据，接收激光雷达发送的点云数据，融合三种数据，得到持续定位数据；嵌入式计算平台接收无线模块发送的RTCM数据，将RTCM数据发送给GNSS定位模块。
CN202011106671.8A 2020-10-16 2020-10-16 一种基于信息融合的移动机器人导航定位系统 Active CN112362051B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011106671.8A CN112362051B (zh) 2020-10-16 2020-10-16 一种基于信息融合的移动机器人导航定位系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011106671.8A CN112362051B (zh) 2020-10-16 2020-10-16 一种基于信息融合的移动机器人导航定位系统

Publications (2)

Publication Number Publication Date
CN112362051A CN112362051A (zh) 2021-02-12
CN112362051B true CN112362051B (zh) 2021-11-02

Family

ID=74507344

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011106671.8A Active CN112362051B (zh) 2020-10-16 2020-10-16 一种基于信息融合的移动机器人导航定位系统

Country Status (1)

Country Link
CN (1) CN112362051B (zh)

Families Citing this family (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113253716A (zh) * 2021-03-10 2021-08-13 广州星际海洋工程设计有限公司 一种无人艇控制系统
WO2022193106A1 (zh) * 2021-03-16 2022-09-22 电子科技大学 一种通过惯性测量参数将gps与激光雷达融合定位的方法
CN112946681B (zh) * 2021-05-17 2021-08-17 知行汽车科技（苏州）有限公司 融合组合导航信息的激光雷达定位方法
CN113310487B (zh) * 2021-05-25 2022-11-04 云南电网有限责任公司电力科学研究院 一种面向地面移动机器人的组合导航方法和装置
CN113276084A (zh) * 2021-06-10 2021-08-20 广西大学 一种ros智能小车
CN113534227B (zh) * 2021-07-26 2022-07-01 中国电子科技集团公司第五十四研究所 一种适用于复杂非合作场景的多传感器融合绝对定位方法
CN113959437A (zh) * 2021-10-14 2022-01-21 重庆数字城市科技有限公司 一种用于移动测量设备的测量方法及系统
CN114061596B (zh) * 2021-11-19 2024-03-22 北京国家新能源汽车技术创新中心有限公司 自动驾驶定位方法、系统、测试方法、设备及存储介质
CN114499733A (zh) * 2022-02-16 2022-05-13 东南大学 一种四足机器人载slam装置及传感器时间同步方法
CN114993285B (zh) * 2022-04-27 2024-04-05 大连理工大学 一种基于四轮全向全驱移动机器人的二维激光雷达建图方法
US20230386350A1 (en) * 2022-05-25 2023-11-30 Verizon Patent And Licensing Inc. Systems and methods for indoor positioning of unmanned aerial vehicles
CN114839658B (zh) * 2022-06-28 2022-09-20 江苏德一佳安防科技有限公司 定位消防员建筑物进入点的方法及系统
CN115267725B (zh) * 2022-09-27 2023-01-31 上海仙工智能科技有限公司 一种基于单线激光雷达的建图方法及装置、存储介质
CN115856979B (zh) * 2023-02-16 2023-06-02 智道网联科技(北京)有限公司 自动驾驶车辆的定位方法、装置及电子设备、存储介质

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106153017A (zh) * 2015-04-13 2016-11-23 张谦 一种可移动式的三维建模成像系统
CN206563962U (zh) * 2017-03-08 2017-10-17 华南理工大学 基于双处理器结构的点云数据采集与处理装置
CN111025366A (zh) * 2019-12-31 2020-04-17 芜湖哈特机器人产业技术研究院有限公司 基于ins及gnss的网格slam的导航系统及方法
CN111457902A (zh) * 2020-04-10 2020-07-28 东南大学 基于激光slam定位的水域测量方法及系统

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9568611B2 (en) * 2014-08-20 2017-02-14 Nec Corporation Detecting objects obstructing a driver's view of a road

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106153017A (zh) * 2015-04-13 2016-11-23 张谦 一种可移动式的三维建模成像系统
CN206563962U (zh) * 2017-03-08 2017-10-17 华南理工大学 基于双处理器结构的点云数据采集与处理装置
CN111025366A (zh) * 2019-12-31 2020-04-17 芜湖哈特机器人产业技术研究院有限公司 基于ins及gnss的网格slam的导航系统及方法
CN111457902A (zh) * 2020-04-10 2020-07-28 东南大学 基于激光slam定位的水域测量方法及系统

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
弱GNSS信号区域的GNSS/INS/LiDAR高精度定位方法及其应用研究;钱闯;《中国博士学位论文全文数据库信息科技辑》;20190115;正文第47-137页 *
车载测量系统数据处理若干关键技术研究;邹晓亮;《中国博士学位论文全文数据库基础科学辑》;20120715;正文第47-49页 *

Also Published As

Publication number Publication date
CN112362051A (zh) 2021-02-12

Similar Documents

Publication Publication Date Title
CN112362051B (zh) 一种基于信息融合的移动机器人导航定位系统
US11802769B2 (en) Lane line positioning method and apparatus, and storage medium thereof
CN110361010B (zh) 一种基于占据栅格地图且结合imu的移动机器人定位方法
EP2133662B1 (en) Methods and system of navigation using terrain features
EP2749842B1 (en) System and method for collaborative navigation
CN107478214A (zh) 一种基于多传感器融合的室内定位方法及系统
US20080319664A1 (en) Navigation aid
JP2018028489A (ja) 位置推定装置、位置推定方法
US20150204983A1 (en) Method and apparatus for real-time positioning and navigation of a moving platform
CN111426320B (zh) 一种基于图像匹配/惯导/里程计的车辆自主导航方法
CN111025366B (zh) 基于ins及gnss的网格slam的导航系统及方法
CN102436004A (zh) 定位系统及定位方法
JP2016188806A (ja) 移動体及びシステム
CN113447949B (zh) 一种基于激光雷达和先验地图的实时定位系统及方法
CN111221020A (zh) 一种室内外定位方法、装置及系统
CN115272596A (zh) 一种面向单调无纹理大场景的多传感器融合slam方法
CN116047565A (zh) 一种多传感器数据融合定位系统
CN113763549A (zh) 融合激光雷达和imu的同时定位建图方法、装置和存储介质
CN113093759A (zh) 基于多传感器信息融合的机器人编队构造方法及系统
CN114812554A (zh) 基于滤波的多源融合机器人室内绝对定位方法
WO2022018964A1 (ja) 情報処理装置、情報処理方法およびプログラム
JP6613961B2 (ja) 状態推定装置、情報端末、情報処理システム、及びプログラム
CN114964221A (zh) 基于rtk/uwb/imu多单元协同室内外定位方法
CN113218380A (zh) 一种电子罗盘的校正方法、装置、电子设备及存储介质
Wei Multi-sources fusion based vehicle localization in urban environments under a loosely coupled probabilistic framework

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
PE01 Entry into force of the registration of the contract for pledge of patent right
PE01 Entry into force of the registration of the contract for pledge of patent right

Denomination of invention: A mobile robot navigation and positioning system based on information fusion

Effective date of registration: 20220506

Granted publication date: 20211102

Pledgee: Jiangsu SINOSURE technology microfinance Co.,Ltd.

Pledgor: WUXI KALMAN NAVIGATION TECHNOLOGY CO.,LTD.

Registration number: Y2022320000200