CN113252033A - 基于多传感器融合的定位方法、定位系统及机器人 - Google Patents

基于多传感器融合的定位方法、定位系统及机器人 Download PDF

Info

Publication number
CN113252033A
CN113252033A CN202110722413.0A CN202110722413A CN113252033A CN 113252033 A CN113252033 A CN 113252033A CN 202110722413 A CN202110722413 A CN 202110722413A CN 113252033 A CN113252033 A CN 113252033A
Authority
CN
China
Prior art keywords
robot
information
positioning
time
sensor
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.)
Granted
Application number
CN202110722413.0A
Other languages
English (en)
Other versions
CN113252033B (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.)
CHANGSHA HAIGE BEIDOU INFORMATION TECHNOLOGY CO LTD
Original Assignee
CHANGSHA HAIGE BEIDOU INFORMATION 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 CHANGSHA HAIGE BEIDOU INFORMATION TECHNOLOGY CO LTD filed Critical CHANGSHA HAIGE BEIDOU INFORMATION TECHNOLOGY CO LTD
Priority to CN202110722413.0A priority Critical patent/CN113252033B/zh
Publication of CN113252033A publication Critical patent/CN113252033A/zh
Application granted granted Critical
Publication of CN113252033B publication Critical patent/CN113252033B/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
    • 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
    • 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/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • 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/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Abstract

本发明公开了一种基于多传感器融合的定位方法，包括采用传感器获取当前机器人所处环境的环境信息和运动信息；计算机器人的位姿变换信息；对传感器获取的信息进行实时融合得到机器人的当前位置信息；直接获取机器人当前的经纬度信息得到机器人当前的绝对位置；融合机器人的当前位置信息和当前的绝对位置得到机器人的精确位姿。本发明还公开了一种实现所述基于多传感器融合的定位方法的定位系统，和包括了所述基于多传感器融合的定位方法和定位系统的机器人。本发明通过对多种传感器的定位进行修正和融合，提高了机器人在复杂的室外环境下的定位精度和定位鲁棒性，能够有效避免机器人的定位丢失，而且可靠性高，实用性好。

Description

基于多传感器融合的定位方法、定位系统及机器人
技术领域
本发明属于定位技术领域，具体涉及一种基于多传感器融合的定位方法、定位系统及机器人。
背景技术
随着经济技术的发展和人们生活水平的提高，定位技术已经广泛应用于人们的生产和生活当中，给人们的生产和生活带来了无尽的便利。
定位技术是移动机器人的关键技术之一。机器人应用场景复杂，在室外移动机器人上有多种传感器同时用于测量机器人和外界环境的数据，不同场景需要使用不同的传感器。比如GNSS 定位使用GPS/北斗定位模块，航迹推算定位是使用的IMU测量姿态数据和里程计测量位移和速度数据，NDT（Normal Distributions Transform）配准则是使用激光雷达点云数据与高精点云地图进行匹配之后，输出在点云地图上的位置。基于点的匹配算法中常用最多的是ICP（Iterative Closest Point）算法，该算法需要给定一个较好的初值，同时由于该算法本身的缺陷，最终迭代计算的结果可能会陷入局部最优而不是全局最优。这些定位方法都有各自的优点和缺点，从而导致了不同的定位技术在不同的环境中，其定位结果具有一定的不确定性，进而影响定位的精度。另外，由于机器人应用场景复杂，所以在复杂应用场景下容易出现定位丢失问题。
此外，现有技术中，采用MEMS惯性传感器进行定位时，系统极易受到干扰；而采用NDT算法进行定位时，容易产生定位跳动的问题，因此算法整体的鲁棒性下降；同时，现有技术并未直接体现各类定位技术的优势，从而使得现有的定位技术，其定位精度较差，定位鲁棒性不高，而且防止定位丢失方面存在不足。
发明内容
本发明的目的之一在于提供一种定位精度高、定位鲁棒性好且能够有效防止定位丢失的基于多传感器融合的定位方法。
本发明的目的之二在于提供一种实现所述基于多传感器融合的定位方法的定位系统。
本发明的目的之三在于提供一种包括了所述基于多传感器融合的定位方法和定位系统的机器人。
本发明提供的这种基于多传感器融合的定位方法，包括如下步骤：
S1.采用若干种传感器获取当前机器人所处环境的环境信息和机器人当前的运动信息；
S2. 根据步骤S1获取的环境信息和运动信息，计算得到机器人的位姿变换信息；
S3. 对若干种传感器获取的信息进行实时不间断融合，从而得到机器人的当前位置信息；
S4. 直接获取机器人当前的经纬度信息，从而得到机器人当前的绝对位置；
S5. 将步骤S3得到的机器人的当前位置信息和步骤S4得到的机器人当前的绝对位置进行融合，从而得到机器人的精确位姿。
所述的基于多传感器融合的定位方法，还包括如下步骤：
S6. 对定位过程的连续性进行判断，并在满足设定条件时，进行重定位。
所述的重定位，具体包括局部重定位和全局重定位；局部重定位用于在系统的输出位置不符合连续性判断时；全局重定位用于在系统出现传感器失效的情况时。
步骤S1所述的采用若干种传感器获取当前机器人所处环境的环境信息和机器人当前的运动信息，具体为采用多线激光雷达采集三维点云信息，采用惯性传感器（IMU）采集三轴加速度信息，采用北斗系统采集经纬度信息，采用轮式里程计采集机器人的轮子转速信息。
步骤S2所述的根据步骤S1获取的环境信息和运动信息，计算得到机器人的位姿变换信息，具体为将多线激光雷达采集的三维点云信息通过NDT算法计算机器人的位姿变换信息；将惯性传感器采集的三轴加速度信息通过惯导算法计算机器人的位姿变换信息。
步骤S3所述的对若干种传感器获取的信息进行实时不间断融合，从而得到机器人的当前位置信息，具体为将惯性传感器对应的惯导算法得到的位姿变换信息、轮式里程计对应的航位推算算法得到的位置信息和多线激光雷达对应的NDT算法得到的位姿变换信息，不间断地采用无迹卡尔曼滤波（UKF）算法进行融合，从而得到odom→robot之间的坐标变换；同时，通过NDT算法粗估和ICP算法精细匹配，提高点云匹配的精度；其中odom为惯性传感器、轮式里程计和多线激光雷达融合后的坐标系，robot为机器人的坐标系。
所述的不间断地采用无迹卡尔曼滤波（UKF）算法进行融合，从而得到odom→robot之间的坐标变换，具体为采如下步骤进行融合并得到坐标变换：
根据轮式里程计系统计算的位姿变换
Figure DEST_PATH_IMAGE001
，IMU里程计系统计算的位姿变换
Figure 491963DEST_PATH_IMAGE002
，对无迹卡尔曼滤波（UKF）下一个状态进行预测，预测函数为：
Figure DEST_PATH_IMAGE003
其中g()为机器人非线性运动模型的状态变换函数，
Figure 235929DEST_PATH_IMAGE004
为机器人在k时刻的状态，
Figure DEST_PATH_IMAGE005
为机器人在k时刻预测k+1时刻的状态，
Figure 958028DEST_PATH_IMAGE006
为控制变量；
采用如下步骤进行计算：
（1）读取k时刻最优估计状态
Figure DEST_PATH_IMAGE007
以及最优估计状态协方差矩阵
Figure 886670DEST_PATH_IMAGE008
；
（2）计算上一时刻k与这一时刻k+1的时间间隔
Figure DEST_PATH_IMAGE009
；
（3）根据
Figure 818854DEST_PATH_IMAGE010
、
Figure DEST_PATH_IMAGE011
、
Figure 976297DEST_PATH_IMAGE012
、
Figure 67749DEST_PATH_IMAGE013
、
Figure DEST_PATH_IMAGE014
、
Figure 42659DEST_PATH_IMAGE015
以及采样参数
Figure DEST_PATH_IMAGE016
选取sigmapoint点集，其中
Figure 334575DEST_PATH_IMAGE017
为直线加速度，
Figure 951501DEST_PATH_IMAGE018
为偏航角加速度噪声，
Figure 897461DEST_PATH_IMAGE019
为直线加速度过程噪声的方差、
Figure DEST_PATH_IMAGE020
为偏航角加速度过程噪声的方差；
（4）将选取的sigma point点集代入机器人非线性运动模型g()中，计算得到预测的sigma point点集，利用预测得到的sigma point点集带权重地得到k+1时刻的预测状态
Figure 121900DEST_PATH_IMAGE021
和协方差矩阵
Figure DEST_PATH_IMAGE022
；
（5）读取k+1时刻传感器数值，判断传感器类型，选择对应的h()转换函数，对预测sigma point点集从预测空间下转换到测量空间下；
（6）使用测量空间下的sigma point点集和测量协方差矩阵R计算测量空间下的预测状态均值
Figure 28676DEST_PATH_IMAGE023
和预测状态协方差矩阵
Figure DEST_PATH_IMAGE024
；
（7）计算sigma point在测量空间转换到状态空间下的转换函数
Figure 262342DEST_PATH_IMAGE025
；
（8）计算卡尔曼增益
Figure DEST_PATH_IMAGE026
，更新k+1时刻的最佳估计状态
Figure 938174DEST_PATH_IMAGE027
以及最佳估计状态矩阵
Figure DEST_PATH_IMAGE028
；
上述步骤（1）~（8）为UKF算法迭代一次的步骤，读取激光雷达里程计的数据，下一时刻则读取其他传感器的数据，如轮式里程计，循环读取各传感器数据作融合处理，以上为基于UKF的数据融合定位算法计算流程；
Figure 113941DEST_PATH_IMAGE007
融合出odom->robot之间的坐标变换
Figure 773592DEST_PATH_IMAGE029
。
所述的通过NDT算法粗估和ICP算法精细匹配，提高点云匹配的精度，具体为采用如下步骤提高点云匹配的精度：
基于UKF的惯性导航系统，其结果作为NDT算法的每一轮迭代的迭代初值，提高NDT算法的迭代稳定性与准确度；配合ICP精细匹配模块可以进一步增加点云匹配的精度。
示例性地，假设t时刻与t+1时刻周围环境点云数据分别为
Figure 169256DEST_PATH_IMAGE030
和
Figure 230753DEST_PATH_IMAGE031
；
通过惯性里程计计算
Figure 249525DEST_PATH_IMAGE032
点云数据对应的第二相对位置与
Figure 255527DEST_PATH_IMAGE030
点云数据对应的第一相对位置。经过NDT算法对第一相对位置与第二相对位置计算后输出的点云分别为
Figure 752367DEST_PATH_IMAGE030
和
Figure 668371DEST_PATH_IMAGE033
（即计算后的第二相对位置），并计算
Figure 467831DEST_PATH_IMAGE030
和
Figure 102074DEST_PATH_IMAGE034
的变换参数
Figure DEST_PATH_IMAGE035
；ICP算法通过计算
Figure 261660DEST_PATH_IMAGE033
与
Figure 766591DEST_PATH_IMAGE030
对应点距离，计算出旋转平移矩阵
Figure 127165DEST_PATH_IMAGE036
，通过
Figure 124071DEST_PATH_IMAGE036
对
Figure 962714DEST_PATH_IMAGE033
变换，并计算变换之后的均方差。若均方差小于预设的误差阈值，则算法结束。否则继续重复迭代直至均方差小于预设的误差阈值或者迭代次数满足预设的迭代数阈值，则终止计算。需要说明的是，预设的误差阈值根据需要进行设置，在此不作限定。预设的迭代数阈值根据大量实验数据进行设置，在此不作限定。
步骤S4所述的直接获取机器人当前的经纬度信息，从而得到机器人当前的绝对位置，具体为通过北斗定位系统直接采集机器人当前的经纬度信息，并计算得到机器人当前在ENU坐标系下的绝对角度，以及计算机器人的绝对位置信息。
步骤S5所述的将步骤S3得到的机器人的当前位置信息和步骤S4得到的机器人当前的绝对位置进行融合，从而得到机器人的精确位姿，具体为将将步骤S3得到的机器人的当前位置信息和步骤S4得到的机器人当前的绝对位置，采用扩展卡尔曼滤波（EKF）算法进行融合，发布机器人的累计误差补偿值，并最终计算出机器人的准确位姿。
所述的采用扩展卡尔曼滤波（EKF）算法进行融合，具体为采用如下步骤进行融合：
1）根据k 时刻的最优估计状态
Figure DEST_PATH_IMAGE037
和状态转移矩阵F预测k+1 时刻的预测状态
Figure 446785DEST_PATH_IMAGE038
；
2）根据k时刻的最优估计协方差矩阵
Figure 978260DEST_PATH_IMAGE008
和过程噪声矩阵Q预测k+1时刻的预测协方差矩阵
Figure DEST_PATH_IMAGE039
；
3）根据k 时刻预测k+1 时刻的预测协方差矩阵
Figure 55938DEST_PATH_IMAGE039
和k+1时刻测量值协方差矩阵
Figure 839217DEST_PATH_IMAGE040
计算 k+1时刻的卡尔曼增益
Figure DEST_PATH_IMAGE041
；
4）根据k时刻预测k+1 时刻的预测状态
Figure 318740DEST_PATH_IMAGE042
、k+1时刻的测量状态
Figure DEST_PATH_IMAGE043
、卡尔曼增益
Figure 614592DEST_PATH_IMAGE041
以及观测矩阵H计算k+1时刻的最优估计状态
Figure 445145DEST_PATH_IMAGE044
；
5）根据k时刻预测k+1时刻的预测协方差矩阵
Figure DEST_PATH_IMAGE045
和卡尔曼增益
Figure 966869DEST_PATH_IMAGE041
预测k+1时刻的最优估计协方差矩阵
Figure 832056DEST_PATH_IMAGE046
；
上述步骤为EKF算法迭代一次的步骤，该时刻读取里程计的数据，下一时刻则读取GNSS数据，循环读取各传感器数据作融合处理；
Figure 705335DEST_PATH_IMAGE037
融合出机器人的准确位姿。
本发明还公开了一种实现所述基于多传感器融合的定位方法的定位系统，包括多线激光雷达模块、惯性传感器模块、北斗定位模块、轮式里程计模块和综合定位模块；多线激光雷达模块、惯性传感器模块、北斗定位模块和轮式里程计模块均与综合定位模块连接；多线激光雷达模块用于采集三维点云信息并上传综合定位模块；惯性传感器模块用于采集三轴加速度信息并上传综合定位模块；北斗定位模块用于采集机器人当前的经纬度信息并上传综合定位模块；轮式里程计模块用于采集机器人的轮子转速信息并上传综合定位模块；综合定位模块用于采用上述的基于多传感器融合的定位方法对机器人进行精确定位。
本发明还公开了一种机器人，该机器人包括了上述的基于多传感器融合的定位方法和定位系统。
本发明提供的这种基于多传感器融合的定位方法、定位系统及机器人，通过对多种传感器的定位进行进行修正和融合，提高了机器人在复杂的室外环境下的定位精度和定位鲁棒性，能够有效避免机器人的定位丢失，而且可靠性高，实用性好。
附图说明
图1为本发明的方法的方法流程示意图。
图2为本发明的系统的功能模块图。
具体实施方式
如图1所示为本发明的方法的方法流程示意图：本发明提供的这种基于多传感器融合的定位方法，包括如下步骤：
S1. 采用若干种传感器获取当前机器人所处环境的环境信息和机器人当前的运动信息；具体为采用多线激光雷达采集三维点云信息，采用惯性传感器（IMU）采集三轴加速度信息，采用北斗系统采集经纬度信息，采用轮式里程计采集机器人的轮子转速信息；
S2. 根据步骤S1获取的环境信息和运动信息，计算得到机器人的位姿变换信息；具体为将多线激光雷达采集的三维点云信息通过NDT算法计算机器人的位姿变换信息；将惯性传感器采集的三轴加速度信息通过惯导算法计算机器人的位姿变换信息；
S3. 对若干种传感器获取的信息进行实时不间断融合，从而得到机器人的当前位置信息；具体为将惯性传感器对应的惯导算法得到的位姿变换信息、轮式里程计对应的航位推算算法得到的位置信息和多线激光雷达对应的NDT算法得到的位姿变换信息，不间断地采用无迹卡尔曼滤波（UKF）算法进行融合，从而得到odom→robot之间的坐标变换；同时，通过NDT算法粗估和ICP精细匹配，提高点云匹配的精度；其中odom为惯性传感器、轮式里程计和多线激光雷达融合后的坐标系，robot为机器人的坐标系；
具体实施时，具体为采如下步骤进行融合并得到坐标变换：
根据轮式里程计系统计算的位姿变换
Figure DEST_PATH_IMAGE047
，IMU里程计系统计算的位姿变换
Figure 882238DEST_PATH_IMAGE048
，对无迹卡尔曼滤波（UKF）下一个状态进行预测，预测函数为：
Figure DEST_PATH_IMAGE049
其中g()为机器人非线性运动模型的状态变换函数，
Figure 600795DEST_PATH_IMAGE007
为机器人在k时刻的状态，
Figure 195856DEST_PATH_IMAGE050
为机器人在k时刻预测k+1时刻的状态，
Figure 240035DEST_PATH_IMAGE006
为控制变量；
采用如下步骤进行计算：
（1）读取k时刻最优估计状态
Figure 779601DEST_PATH_IMAGE007
以及最优估计状态协方差矩阵
Figure DEST_PATH_IMAGE051
；
（2）计算上一时刻k与这一时刻k+1的时间间隔
Figure 426483DEST_PATH_IMAGE052
；
（3）根据
Figure 684DEST_PATH_IMAGE007
、
Figure DEST_PATH_IMAGE053
、
Figure 559972DEST_PATH_IMAGE054
、
Figure 586834DEST_PATH_IMAGE055
、
Figure DEST_PATH_IMAGE056
、
Figure 37407DEST_PATH_IMAGE057
以及采样参数
Figure 466114DEST_PATH_IMAGE016
选取sigma point点集，其中
Figure 586517DEST_PATH_IMAGE058
为直线加速度，
Figure DEST_PATH_IMAGE059
为偏航角加速度噪声，
Figure 976041DEST_PATH_IMAGE019
为直线加速度过程噪声的方差、
Figure 105671DEST_PATH_IMAGE060
为偏航角加速度过程噪声的方差；
（4）将选取的sigma point点集代入机器人非线性运动模型g()中，计算得到预测的sigma point点集，利用预测得到的sigma point点集带权重地得到k+1时刻的预测状态
Figure DEST_PATH_IMAGE061
和协方差矩阵
Figure 247940DEST_PATH_IMAGE062
；
（5）读取k+1时刻传感器数值，判断传感器类型，选择对应的h()转换函数，对预测sigma point点集从预测空间下转换到测量空间下；
（6）使用测量空间下的sigma point点集和测量协方差矩阵R计算测量空间下的预测状态均值
Figure DEST_PATH_IMAGE063
和预测状态协方差矩阵
Figure 539244DEST_PATH_IMAGE064
；
（7）计算sigma point在测量空间转换到状态空间下的转换函数
Figure 418994DEST_PATH_IMAGE025
；
（8）计算卡尔曼增益
Figure DEST_PATH_IMAGE065
，更新k+1时刻的最佳估计状态
Figure 821156DEST_PATH_IMAGE066
以及最佳估计状态矩阵
Figure DEST_PATH_IMAGE067
；
上述步骤（1）~（8）为UKF算法迭代一次的步骤，读取激光雷达里程计的数据，下一时刻则读取其他传感器的数据，如轮式里程计，循环读取各传感器数据作融合处理，以上为基于UKF的数据融合定位算法计算流程；
Figure 83510DEST_PATH_IMAGE010
融合出odom->robot之间的坐标变换
Figure 811295DEST_PATH_IMAGE068
；
同时，采用如下步骤提高点云匹配的精度：
基于UKF的惯性导航系统，其结果作为NDT算法的每一轮迭代的迭代初值，提高NDT算法的迭代稳定性与准确度；配合ICP精细匹配模块可以进一步增加点云匹配的精度；
示例性地，假设t时刻与t+1时刻周围环境点云数据分别为
Figure 34466DEST_PATH_IMAGE030
和
Figure 646844DEST_PATH_IMAGE031
；
通过惯性里程计计算
Figure 904650DEST_PATH_IMAGE032
点云数据对应的第二相对位置与
Figure 803336DEST_PATH_IMAGE030
点云数据对应的第一相对位置。经过NDT算法对第一相对位置与第二相对位置计算后输出的点云分别为
Figure 638436DEST_PATH_IMAGE030
和
Figure DEST_PATH_IMAGE069
（即计算后的第二相对位置），并计算
Figure 647981DEST_PATH_IMAGE030
和
Figure 760293DEST_PATH_IMAGE069
的变换参数
Figure 970826DEST_PATH_IMAGE070
；ICP算法通过计算
Figure 168589DEST_PATH_IMAGE069
与
Figure 981824DEST_PATH_IMAGE030
对应点距离，计算出旋转平移矩阵
Figure 338856DEST_PATH_IMAGE036
，通过
Figure 579345DEST_PATH_IMAGE036
对
Figure 529983DEST_PATH_IMAGE069
变换，并计算变换之后的均方差。若均方差小于预设的误差阈值，则算法结束。否则继续重复迭代直至均方差小于预设的误差阈值或者迭代次数满足预设的迭代数阈值，则终止计算。需要说明的是，预设的误差阈值根据需要进行设置，在此不作限定。预设的迭代数阈值根据大量实验数据进行设置，在此不作限定；
S4. 直接获取机器人当前的经纬度信息，从而得到机器人当前的绝对位置；具体为通过北斗定位系统直接采集机器人当前的经纬度信息，并计算得到机器人当前在ENU坐标系下的绝对角度，以及计算机器人的绝对位置信息；
S5. 将步骤S3得到的机器人的当前位置信息和步骤S4得到的机器人当前的绝对位置进行融合，从而得到机器人的精确位姿；具体为将将步骤S3得到的机器人的当前位置信息和步骤S4得到的机器人当前的绝对位置，采用扩展卡尔曼滤波（EKF）算法进行融合，发布机器人的累计误差补偿值，并最终计算出机器人的准确位姿；
具体实施时，1）根据k 时刻的最优估计状态
Figure 756696DEST_PATH_IMAGE037
和状态转移矩阵F预测k+1 时刻的预测状态
Figure DEST_PATH_IMAGE071
；
2）根据k时刻的最优估计协方差矩阵
Figure 578022DEST_PATH_IMAGE072
和过程噪声矩阵Q预测k+1时刻的预测协方差矩阵
Figure 989411DEST_PATH_IMAGE073
；
3）根据k 时刻预测k+1 时刻的预测协方差矩阵
Figure DEST_PATH_IMAGE074
和k+1时刻测量值协方差矩阵
Figure 20821DEST_PATH_IMAGE075
计算 k+1时刻的卡尔曼增益
Figure DEST_PATH_IMAGE076
；
4）根据k时刻预测k+1 时刻的预测状态
Figure 175859DEST_PATH_IMAGE077
、k+1时刻的测量状态
Figure 724128DEST_PATH_IMAGE078
、卡尔曼增益
Figure DEST_PATH_IMAGE079
以及观测矩阵H计算k+1时刻的最优估计状态
Figure 306419DEST_PATH_IMAGE080
；
5）根据k时刻预测k+1时刻的预测协方差矩阵
Figure 90704DEST_PATH_IMAGE081
和卡尔曼增益
Figure 49433DEST_PATH_IMAGE082
预测k+1时刻的最优估计协方差矩阵
Figure 110930DEST_PATH_IMAGE083
；
上述步骤为EKF算法迭代一次的步骤，该时刻读取里程计的数据，下一时刻则读取GNSS数据，循环读取各传感器数据作融合处理；
Figure 739488DEST_PATH_IMAGE037
融合出机器人的准确位姿；
S6. 对定位过程的连续性进行判断，并在满足设定条件时，进行重定位；
重定位具体包括局部重定位和全局重定位；
局部重定位主要是依靠北斗导航数据的回溯以及大尺度的搜索，适用于NDT算法输出位置不符合连续性判断的情况；全局重定位用于在系统出现部分传感器失效的情况时。
如图2所示为本发明的系统的功能模块图：本发明提供的这种实现所述基于多传感器融合的定位方法的定位系统，包括多线激光雷达模块、惯性传感器模块、北斗定位模块、轮式里程计模块和综合定位模块；多线激光雷达模块、惯性传感器模块、北斗定位模块和轮式里程计模块均与综合定位模块连接；多线激光雷达模块用于采集三维点云信息并上传综合定位模块；惯性传感器模块用于采集三轴加速度信息并上传综合定位模块；北斗定位模块用于采集机器人当前的经纬度信息并上传综合定位模块；轮式里程计模块用于采集机器人的轮子转速信息并上传综合定位模块；综合定位模块用于采用上述的基于多传感器融合的定位方法对机器人进行精确定位。

Claims (9)

1.一种基于多传感器融合的定位方法，其特征在于包括如下步骤：
S1. 采用若干种传感器获取当前机器人所处环境的环境信息和机器人当前的运动信息；具体为采用多线激光雷达采集三维点云信息，采用惯性传感器采集三轴加速度信息，采用北斗系统采集经纬度信息，采用轮式里程计采集机器人的轮子转速信息；
S2. 根据步骤S1获取的环境信息和运动信息，计算得到机器人的位姿变换信息；
S3. 对若干种传感器获取的信息进行实时不间断融合，从而得到机器人的当前位置信息；
S4. 直接获取机器人当前的经纬度信息，从而得到机器人当前的绝对位置；
S5. 将步骤S3得到的机器人的当前位置信息和步骤S4得到的机器人当前的绝对位置进行融合，从而得到机器人的精确位姿。
2.根据权利要求1所述的基于多传感器融合的定位方法，其特征在于还包括如下步骤：
S6. 对定位过程的连续性进行判断，并在满足设定条件时，进行重定位；所述的重定位，具体包括局部重定位和全局重定位；局部重定位用于在系统的输出位置不符合连续性判断时；全局重定位用于在系统出现传感器失效的情况时。
3.根据权利要求2所述的基于多传感器融合的定位方法，其特征在于步骤S2所述的根据步骤S1获取的环境信息和运动信息，计算得到机器人的位姿变换信息，具体为将多线激光雷达采集的三维点云信息通过NDT算法计算机器人的位姿变换信息；将惯性传感器采集的三轴加速度信息通过惯导算法计算机器人的位姿变换信息。
4.根据权利要求3所述的基于多传感器融合的定位方法，其特征在于步骤S3所述的对若干种传感器获取的信息进行实时不间断融合，从而得到机器人的当前位置信息，具体为将惯性传感器对应的惯导算法得到的位姿变换信息、轮式里程计对应的航位推算算法得到的位置信息和多线激光雷达对应的NDT算法得到的位姿变换信息，不间断地采用无迹卡尔曼滤波算法进行融合，从而得到odom→robot之间的坐标变换；同时，通过NDT算法粗估和ICP精细匹配，提高点云匹配的精度；其中odom为惯性传感器、轮式里程计和多线激光雷达融合后的坐标系，robot为机器人的坐标系。
5.根据权利要求4所述的基于多传感器融合的定位方法，其特征在于所述的不间断地采用无迹卡尔曼滤波算法进行融合，从而得到odom→robot之间的坐标变换，具体为采如下步骤进行融合并得到坐标变换：
根据轮式里程计系统计算的位姿变换
Figure 811474DEST_PATH_IMAGE001
，IMU里程计系统计算的位姿变换
Figure 994194DEST_PATH_IMAGE002
，对无迹卡尔曼滤波下一个状态进行预测，预测函数为：
Figure 914876DEST_PATH_IMAGE003
其中g()为机器人非线性运动模型的状态变换函数，
Figure 575665DEST_PATH_IMAGE005
为机器人在k时刻的状态，
Figure 655616DEST_PATH_IMAGE007
为机器人在k时刻预测k+1时刻的状态，
Figure 743658DEST_PATH_IMAGE008
为控制变量；
采用如下步骤进行计算：
（1）读取k时刻最优估计状态
Figure 656031DEST_PATH_IMAGE009
以及最优估计状态协方差矩阵
Figure 120511DEST_PATH_IMAGE011
；
（2）计算上一时刻k与这一时刻k+1的时间间隔
Figure 54969DEST_PATH_IMAGE012
；
（3）根据
Figure 313912DEST_PATH_IMAGE013
、
Figure 209186DEST_PATH_IMAGE014
、
Figure 946198DEST_PATH_IMAGE015
、
Figure 735163DEST_PATH_IMAGE016
、
Figure 430586DEST_PATH_IMAGE017
、
Figure 937791DEST_PATH_IMAGE018
以及采样参数
Figure 353860DEST_PATH_IMAGE019
选取sigmapoint点集，其中
Figure 262910DEST_PATH_IMAGE020
为直线加速度，
Figure 863656DEST_PATH_IMAGE021
为偏航角加速度噪声，
Figure 858157DEST_PATH_IMAGE022
为直线加速度过程噪声的方差、
Figure 77916DEST_PATH_IMAGE023
为偏航角加速度过程噪声的方差；
（4）将选取的sigma point点集代入机器人非线性运动模型g()中，计算得到预测的sigma point点集，利用预测得到的sigma point点集带权重地得到k+1时刻的预测状态
Figure 841473DEST_PATH_IMAGE024
和协方差矩阵
Figure 878699DEST_PATH_IMAGE025
；
（5）读取k+1时刻传感器数值，判断传感器类型，选择对应的h()转换函数，对预测sigmapoint点集从预测空间下转换到测量空间下；
（6）使用测量空间下的sigma point点集和测量协方差矩阵R计算测量空间下的预测状态均值
Figure 94917DEST_PATH_IMAGE026
和预测状态协方差矩阵
Figure 616903DEST_PATH_IMAGE027
；
（7）计算sigma point在测量空间转换到状态空间下的转换函数
Figure 969387DEST_PATH_IMAGE028
；
（8）计算卡尔曼增益
Figure 177514DEST_PATH_IMAGE029
，更新k+1时刻的最佳估计状态
Figure 881028DEST_PATH_IMAGE030
以及最佳估计状态矩阵
Figure 567224DEST_PATH_IMAGE031
；
上述步骤（1）~（8）为UKF算法迭代一次的步骤，循环读取各传感器数据作融合处理；
Figure 180739DEST_PATH_IMAGE013
融合出odom->robot之间的坐标变换
Figure 559768DEST_PATH_IMAGE032
。
6.根据权利要求5所述的基于多传感器融合的定位方法，其特征在于步骤S4所述的直接获取机器人当前的经纬度信息，从而得到机器人当前的绝对位置，具体为通过北斗定位系统直接采集机器人当前的经纬度信息，并计算得到机器人当前在ENU坐标系下的绝对角度，以及计算机器人的绝对位置信息。
7.根据权利要求6所述的基于多传感器融合的定位方法，其特征在于步骤S5所述的将步骤S3得到的机器人的当前位置信息和步骤S4得到的机器人当前的绝对位置进行融合，从而得到机器人的精确位姿，具体为将将步骤S3得到的机器人的当前位置信息和步骤S4得到的机器人当前的绝对位置，采用扩展卡尔曼滤波算法进行融合，发布机器人的累计误差补偿值，并最终计算出机器人的准确位姿；所述的采用扩展卡尔曼滤波算法进行融合，具体为采用如下步骤进行融合：
1）根据k 时刻的最优估计状态
Figure 750578DEST_PATH_IMAGE033
和状态转移矩阵F预测k+1 时刻的预测状态
Figure 240465DEST_PATH_IMAGE034
；
2）根据k时刻的最优估计协方差矩阵
Figure 442908DEST_PATH_IMAGE035
和过程噪声矩阵Q预测k+1时刻的预测协方差矩阵
Figure 992838DEST_PATH_IMAGE036
；
3）根据k 时刻预测k+1 时刻的预测协方差矩阵
Figure 670944DEST_PATH_IMAGE037
和k+1时刻测量值协方差矩阵
Figure 698942DEST_PATH_IMAGE038
计算 k+1时刻的卡尔曼增益
Figure 146104DEST_PATH_IMAGE039
；
4）根据k时刻预测k+1 时刻的预测状态
Figure 476723DEST_PATH_IMAGE040
、k+1时刻的测量状态
Figure 642125DEST_PATH_IMAGE041
、卡尔曼增益
Figure 473814DEST_PATH_IMAGE042
以及观测矩阵H计算k+1时刻的最优估计状态
Figure 41062DEST_PATH_IMAGE043
；
5）根据k时刻预测k+1时刻的预测协方差矩阵
Figure 41117DEST_PATH_IMAGE044
和卡尔曼增益
Figure 693815DEST_PATH_IMAGE045
预测k+1时刻的最优估计协方差矩阵
Figure 329196DEST_PATH_IMAGE046
；
上述步骤为EKF算法迭代一次的步骤，循环读取各传感器数据作融合处理；
Figure 485371DEST_PATH_IMAGE047
融合出机器人的准确位姿。
8.一种实现权利要求1~7之一所述的基于多传感器融合的定位方法的定位系统，其特征在于包括多线激光雷达模块、惯性传感器模块、北斗定位模块、轮式里程计模块和综合定位模块；多线激光雷达模块、惯性传感器模块、北斗定位模块和轮式里程计模块均与综合定位模块连接；多线激光雷达模块用于采集三维点云信息并上传综合定位模块；惯性传感器模块用于采集三轴加速度信息并上传综合定位模块；北斗定位模块用于采集机器人当前的经纬度信息并上传综合定位模块；轮式里程计模块用于采集机器人的轮子转速信息并上传综合定位模块；综合定位模块用于采用上述的基于多传感器融合的定位方法对机器人进行精确定位。
9.一种机器人，其特征在于包括了权利要求1~7之一所述的基于多传感器融合的定位方法和权利要求8所述的定位系统。
CN202110722413.0A 2021-06-29 2021-06-29 基于多传感器融合的定位方法、定位系统及机器人 Active CN113252033B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110722413.0A CN113252033B (zh) 2021-06-29 2021-06-29 基于多传感器融合的定位方法、定位系统及机器人

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110722413.0A CN113252033B (zh) 2021-06-29 2021-06-29 基于多传感器融合的定位方法、定位系统及机器人

Publications (2)

Publication Number Publication Date
CN113252033A true CN113252033A (zh) 2021-08-13
CN113252033B CN113252033B (zh) 2021-10-15

Family

ID=77190065

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110722413.0A Active CN113252033B (zh) 2021-06-29 2021-06-29 基于多传感器融合的定位方法、定位系统及机器人

Country Status (1)

Country Link
CN (1) CN113252033B (zh)

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113791423A (zh) * 2021-09-24 2021-12-14 山东新一代信息产业技术研究院有限公司 一种基于多传感器融合的机器人重定位方法
CN114170320A (zh) * 2021-10-29 2022-03-11 广西大学 一种基于多传感器融合的打桩机自动定位和工况自适应方法
CN114348138A (zh) * 2022-01-22 2022-04-15 石家庄东方热电热力工程有限公司 一种水冷壁场景的爬壁机器人导航系统及其导航方法
CN114440881A (zh) * 2022-01-29 2022-05-06 海南大学 一种融合多源传感器信息的无人车定位方法
CN114562994A (zh) * 2022-03-09 2022-05-31 上海应用技术大学 移动机器人处于动态环境中的定位方法
CN114719843A (zh) * 2022-06-09 2022-07-08 长沙金维信息技术有限公司 复杂环境下的高精度定位方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109341706A (zh) * 2018-10-17 2019-02-15 张亮 一种面向无人驾驶汽车的多特征融合地图的制作方法
CN110243358A (zh) * 2019-04-29 2019-09-17 武汉理工大学 多源融合的无人车室内外定位方法及系统
CN111702763A (zh) * 2020-06-28 2020-09-25 国网四川省电力公司电力科学研究院 一种基于北斗系统的变电站巡检机器人重定位系统及方法
CN111780763A (zh) * 2020-06-30 2020-10-16 杭州海康机器人技术有限公司 一种基于视觉地图的视觉定位方法、装置
CN112612029A (zh) * 2020-12-24 2021-04-06 哈尔滨工业大学芜湖机器人产业技术研究院 融合ndt和icp的栅格地图定位方法
CN112985416A (zh) * 2021-04-19 2021-06-18 湖南大学 激光与视觉信息融合的鲁棒定位和建图方法及系统

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9439269B2 (en) * 2013-11-21 2016-09-06 General Electric Company Powerline luminaire communications
CN107561549B (zh) * 2017-08-17 2020-07-24 广州视源电子科技股份有限公司 终端位置的重定位方法、装置、终端及存储介质
CN110009739B (zh) * 2019-01-29 2023-03-24 浙江省北大信息技术高等研究院 移动摄像机的数字视网膜的运动特征的提取与编码方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109341706A (zh) * 2018-10-17 2019-02-15 张亮 一种面向无人驾驶汽车的多特征融合地图的制作方法
CN110243358A (zh) * 2019-04-29 2019-09-17 武汉理工大学 多源融合的无人车室内外定位方法及系统
CN111702763A (zh) * 2020-06-28 2020-09-25 国网四川省电力公司电力科学研究院 一种基于北斗系统的变电站巡检机器人重定位系统及方法
CN111780763A (zh) * 2020-06-30 2020-10-16 杭州海康机器人技术有限公司 一种基于视觉地图的视觉定位方法、装置
CN112612029A (zh) * 2020-12-24 2021-04-06 哈尔滨工业大学芜湖机器人产业技术研究院 融合ndt和icp的栅格地图定位方法
CN112985416A (zh) * 2021-04-19 2021-06-18 湖南大学 激光与视觉信息融合的鲁棒定位和建图方法及系统

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113791423A (zh) * 2021-09-24 2021-12-14 山东新一代信息产业技术研究院有限公司 一种基于多传感器融合的机器人重定位方法
CN114170320A (zh) * 2021-10-29 2022-03-11 广西大学 一种基于多传感器融合的打桩机自动定位和工况自适应方法
CN114170320B (zh) * 2021-10-29 2022-10-28 广西大学 一种基于多传感器融合的打桩机自动定位和工况自适应方法
CN114348138A (zh) * 2022-01-22 2022-04-15 石家庄东方热电热力工程有限公司 一种水冷壁场景的爬壁机器人导航系统及其导航方法
CN114440881A (zh) * 2022-01-29 2022-05-06 海南大学 一种融合多源传感器信息的无人车定位方法
CN114440881B (zh) * 2022-01-29 2023-05-30 海南大学 一种融合多源传感器信息的无人车定位方法
CN114562994A (zh) * 2022-03-09 2022-05-31 上海应用技术大学 移动机器人处于动态环境中的定位方法
CN114719843A (zh) * 2022-06-09 2022-07-08 长沙金维信息技术有限公司 复杂环境下的高精度定位方法
CN114719843B (zh) * 2022-06-09 2022-09-30 长沙金维信息技术有限公司 复杂环境下的高精度定位方法

Also Published As

Publication number Publication date
CN113252033B (zh) 2021-10-15

Similar Documents

Publication Publication Date Title
CN113252033B (zh) 基于多传感器融合的定位方法、定位系统及机器人
CN110243358B (zh) 多源融合的无人车室内外定位方法及系统
CN108731670B (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
CN106767752B (zh) 一种基于偏振信息的组合导航方法
CN109931926B (zh) 一种基于站心坐标系的小型无人机无缝自主式导航方法
CN112505737B (zh) 一种gnss/ins组合导航方法
CN112697138B (zh) 一种基于因子图优化的仿生偏振同步定位与构图的方法
CN109186597B (zh) 一种基于双mems-imu的室内轮式机器人的定位方法
CN108387236B (zh) 一种基于扩展卡尔曼滤波的偏振光slam方法
CN109507706B (zh) 一种gps信号丢失的预测定位方法
CN111025366B (zh) 基于ins及gnss的网格slam的导航系统及方法
CN111982106A (zh) 导航方法、装置、存储介质及电子装置
CN112004183B (zh) 一种基于卷积神经网络融合IMU和WiFi信息的机器人自主定位方法
CN110412596A (zh) 一种基于图像信息和激光点云的机器人定位方法
CN112923919A (zh) 基于图优化的行人定位方法及系统
CN115900708A (zh) 基于gps引导式粒子滤波的机器人多传感器融合定位方法
CN115183762A (zh) 一种机场仓库内外建图方法、系统、电子设备及介质
CN113008229A (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
CN114897942B (zh) 点云地图的生成方法、设备及相关存储介质
CN115930948A (zh) 一种果园机器人融合定位方法
Zarei et al. Performance improvement for mobile robot position determination using cubature Kalman filter
Aravind et al. Enhancing GPS Position Estimation Using Multi-Sensor Fusion and Error-State Extended Kalman Filter
Xu et al. The Fusion Of GPS And Gyroscope Based On Kalman Filter
CN117346768B (zh) 同时适用于室内外的多传感器融合感知定位方法
CN115574816B (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
GR01 Patent grant
GR01 Patent grant