CN113029137A - 一种多源信息自适应融合定位方法及*** - Google Patents

一种多源信息自适应融合定位方法及*** Download PDF

Info

Publication number
CN113029137A
CN113029137A CN202110355182.4A CN202110355182A CN113029137A CN 113029137 A CN113029137 A CN 113029137A CN 202110355182 A CN202110355182 A CN 202110355182A CN 113029137 A CN113029137 A CN 113029137A
Authority
CN
China
Prior art keywords
positioning
point cloud
vehicle
laser point
gnss
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
CN202110355182.4A
Other languages
English (en)
Other versions
CN113029137B (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.)
Tsinghua University
Dongfeng Motor Corp
Original Assignee
Tsinghua University
Dongfeng Motor Corp
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 Tsinghua University, Dongfeng Motor Corp filed Critical Tsinghua University
Priority to CN202110355182.4A priority Critical patent/CN113029137B/zh
Publication of CN113029137A publication Critical patent/CN113029137A/zh
Application granted granted Critical
Publication of CN113029137B publication Critical patent/CN113029137B/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/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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Theoretical Computer Science (AREA)
  • Mathematical Physics (AREA)
  • Software Systems (AREA)
  • Databases & Information Systems (AREA)
  • Pure & Applied Mathematics (AREA)
  • General Engineering & Computer Science (AREA)
  • Mathematical Optimization (AREA)
  • Computational Mathematics (AREA)
  • Algebra (AREA)
  • Automation & Control Theory (AREA)
  • Mathematical Analysis (AREA)
  • Navigation (AREA)
  • Traffic Control Systems (AREA)

Abstract

本发明涉及一种多源信息自适应融合定位方法及***，其特征在于，包括以下内容：1)得到每帧激光点云数据优化后的匹配残差以及每帧激光点云数据的所有特征点距离激光雷达的平均距离；2)确定每帧激光点云数据的调节因子；3)确定GNSS定位数据的调节因子；4)根据待测车辆的惯性导航定位***获取的惯性导航定位数据和确定的调节因子，融合待测车辆的GNSS接收机获取的GNSS定位数据、激光雷达***获取的激光点云数据以及轮速计获取的定位数据，得到待测车辆当前时刻的姿态、速度和位置，本发明可以广泛应用于自动驾驶车辆导航定位领域中。

Description

一种多源信息自适应融合定位方法及***
技术领域
本发明是关于一种多源信息自适应融合定位方法及***，属于自动驾驶车辆导航定位领域。
背景技术
近年来，无人驾驶备受关注，其在提升交通效率及减少安全事故方面有着很大的潜力。高级别自动驾驶一般包括感知定位、决策规划和运动控制三大模块，其中，定位模块是最基础也是最关键的环节，其负责实时估计车辆的位置、速度和姿态并输入到感知决策等模块，以保证无人驾驶的正常运行。随着无人驾驶级别的不断提高，对于定位模块的精度、抗干扰性以及多场景适应性也提出更高的要求。目前，常见的独立定位方法包括卫星导航定位(GNSS)、惯性导航定位(INS)、基于视觉的匹配定位、基于激光点云的匹配定位、基于先验地图的匹配定位以及轮速计等其他辅助定位，不同的定位方法均存在各自的优劣势，例如：卫星导航定位在开阔地带定位信号质量较好，加上实时动态载波相位差分(RTK)的辅助，定位精度可达到厘米级，但是在城市环境或隧道，由于多路径效应和非视距效应，卫星导航定位会出现跳动或失锁现象；惯性导航定位是一种相对定位方法，由于惯性测量单元的测量值中存在零偏及噪声，在进行积分定位时会出现累积误差；在光照、阴影等情况下，由于图像质量受到严重影响，基于视觉的匹配定位也会发生不同程度的失效；在特征结构缺乏或单一的场景中，基于激光点云的匹配定位也会发生退化现象。可见，单一定位方法很难满足多场景下的高精度定位。因此，基于多传感器的组合导航定位成为目前研究与应用的重点，其中，GNSS/INS组合导航定位应用最为广泛。
在相对开阔区域，GNSS/INS组合导航定位***可以提供高精度、高频率的6自由度位姿，但是当GNSS信号受到严重干扰或长时间失锁情况下，其定位精度会大大降低。为保证持续可靠的高精度定位，融合其他异构传感器信息是一种解决思路。近些年，同时定位与建图方法(SLAM)迅速发展，为无人驾驶定位提供新的可用位姿源。在实际应用及鲁棒性方面，激光SLAM整体要优于视觉SLAM，在GNSS信号受到干扰的环境中往往具有丰富的特征，此时，基于特征匹配的激光SLAM定位会更加可靠，而在GNSS定位良好的空旷区域中，由于结构特征缺少或单一激光SLAM会出现匹配失效，这正是GNSS与激光SLAM在定位方面的互补特性。
如何针对不同场景，根据相关参数自适应地调整GNSS和激光SLAM的量测噪声协方差，在线调节两种定位信息在融合算法中的权重，是实现多源信息自适应融合定位的关键问题，也是组合定位***实现场景自适应性的关键问题。然而，传统的GNSS/INS组合导航定位难以满足自动驾驶在多场景下的定位要求。
发明内容
针对上述问题，本发明的目的是提供一种能够满足自动驾驶在多场景下定位要求的多源信息自适应融合定位方法及***。
为实现上述目的，本发明采取以下技术方案：一种多源信息自适应融合定位方法，包括以下内容：
1)采用基于激光点云的匹配定位方法，根据待测车辆的激光雷达***获取的待测车辆周围环境的若干帧激光点云数据，得到每帧激光点云数据优化后的匹配残差以及每帧激光点云数据的所有特征点距离激光雷达的平均距离；
2)采用模糊逻辑推理方法，根据得到的匹配残差和平均距离，确定每帧激光点云数据的调节因子；
3)采用模糊逻辑推理方法，根据待测车辆的GNSS接收机获取的GNSS定位数据，确定GNSS定位数据的调节因子；
4)根据待测车辆的惯性导航定位***获取的惯性导航定位数据和确定的调节因子，融合待测车辆的GNSS接收机获取的GNSS定位数据、激光雷达***获取的激光点云数据以及轮速计获取的定位数据，得到待测车辆当前时刻的姿态、速度和位置。
进一步地，所述步骤1)的具体过程为：
1.1)待测车辆的激光雷达***发送激光信号并实时接收由待测车辆周围物体反射的信号，以点云形式表达待测车辆周围的环境，得到若干帧激光点云数据；
1.2)基于点的曲率大小，提取一帧激光点云数据的特征数据；
1.3)根据提取的特征数据，构建点到线或面的距离残差方程，并以当前帧激光点云数据与上一帧激光点云数据之间的相对位姿为优化变量，以残差距离最小为优化目标，采用列文伯格-马夸尔特算法对距离残差方程进行最优化求解，得到当前帧激光点云数据与上一帧激光点云数据之间的相对位姿，并输出优化后的匹配残差以及当前帧激光点云数据的所有特征点坐标；
1.4)根据当前帧激光点云数据的特征点坐标，计算出当前帧激光点云数据的所有特征点距离激光雷达的平均距离；
1.5)进入步骤1.2)提取下一帧激光点云数据的特征数据，直至计算得到每帧激光点云数据的所有特征点距离激光雷达的平均距离。
进一步地，所述步骤2)的具体过程为：
2.1)将匹配残差分为S、M和L三个等级，并分别确定三个等级匹配残差的隶属度函数；
2.2)将特征点平均距离分为S、M和L三个等级，并分别确定三个等级特征点平均距离的隶属度函数；
2.3)将输出的调节因子分为S、M和L三个等级，并分别确定三个等级调节因子的隶属度函数；
2.4)根据确定的隶属度函数，对根据每帧激光点云数据优化后的匹配残差以及所有特征点距离激光雷达的平均距离进行模糊化，并按照预先设定的模糊推理规则进行推理，得到每帧激光点云数据的调节因子隶属度；
2.5)采用加权平均判决法，对推理得到的调节因子隶属度进行去模糊化，得到调节因子确定值α1，此时，激光点云数据的量测协方差为
Figure BDA0003003365230000031
其中，
Figure BDA0003003365230000032
为激光点云数据的量测协方差初始值。
进一步地，所述步骤3)的具体过程为：
3.1)将可见卫星数分为S、M和L三个等级，并分别确定三个等级可见卫星数的隶属度函数；
3.2)将水平精度因子分为S、M和L三个等级，并分别确定三个等级水平精度因子的隶属度函数；
3.3)将输出的调节因子分为S、M和L三个等级，并分别确定三个等级调节因子的隶属度函数；
3.4)根据确定的隶属度函数，对获取的可见卫星数和水平精度因子进行模糊化，并按照预先设定的模糊推理规则进行推理，得到GNSS定位数据的调节因子隶属度；
3.5)采用加权平均判决法，对推理得到的调节因子隶属度进行去模糊化，得到调节因子确定值α2，此时，GNSS定位数据的量测协方差为
Figure BDA0003003365230000033
其中，
Figure BDA0003003365230000034
为GNSS定位的量测协方差初始值。
进一步地，所述步骤4)的具体过程为：
4.1)根据待测车辆的加速度计和陀螺仪获取的三轴加速度和三轴角速度，确定惯性导航定位***的惯性导航定位数据；
4.2)设定扩展卡尔曼滤波的状态量X为：
X＝[δp δv φ ba bg]T
其中，δp为位置误差，且δp＝[δL δλ δh]，分别为纬度误差、经度误差和高度误差；δv为速度误差，且δv＝[δve δvn δvu]，分别为东向、北向和天向速度误差；φ为失准角，即理想导航系与实际导航系之间的旋转矢量；bg为陀螺仪的零偏；
4.3)建立状态方程
Figure BDA0003003365230000035
Figure BDA0003003365230000041
其中，X为状态向量；F(X)为状态转移方程；G(X)为噪声驱动方程；Wb为***噪声；
4.4)根据待测车辆的GNSS接收机获取的GNSS定位数据、激光雷达***获取的激光点云数据和轮速计获取的定位数据，以及确定的调节因子，分别建立对应的观测模型；
4.5)根据建立的状态方程和观测模型，进行扩展卡尔曼滤波的时间更新和量测更新，得到误差状态；
4.6)根据待测车辆的惯性导航定位***获取的惯性导航定位数据和得到的误差状态，确定待测车辆当前时刻的姿态、速度和位置。
进一步地，所述步骤4.1)中的惯性导航定位数据包括待测车辆的姿态更新
Figure BDA0003003365230000042
速度更新
Figure BDA0003003365230000043
和位置更新
Figure BDA0003003365230000044
Figure BDA0003003365230000045
Figure BDA0003003365230000046
Figure BDA0003003365230000047
其中，
Figure BDA0003003365230000048
为四元数姿态更新矩阵；
Figure BDA0003003365230000049
为姿态四元数；
Figure BDA00030033652300000410
为待测车辆坐标系相对东-北-天导航坐标系的方向余弦矩阵；fb为待测车辆的加速度计输出的三轴加速度；ba为加速度计的零偏；
Figure BDA00030033652300000411
为地球坐标系相对于惯性坐标系的角速度矢量；
Figure BDA00030033652300000412
为导航坐标系相对于地球坐标系的角速度矢量；v为速度，且v＝[ve vn vu]分别为东向、北向和天向的速度分量；gn为重力场矢量；
Figure BDA00030033652300000413
为变换矩阵；secL为纬度的正割值，L为纬度；RN为东西圈主曲率半径；h为高程；RM为子午圈主曲率半径；且：
Figure BDA00030033652300000414
Figure BDA00030033652300000415
其中，
Figure BDA00030033652300000416
为车辆坐标系相对于导航坐标系下的角速度向量；
Figure BDA00030033652300000417
为
Figure BDA00030033652300000418
构成的反对称矩阵；
Figure BDA00030033652300000419
为待测车辆的陀螺仪输出的三轴角速度。
进一步地，所述步骤4.4)的具体过程为：
4.4.1)根据GNSS接收机获取的GNSS定位数据和GNSS定位数据的调节因子，建立GNSS定位观测模型Zgnss：
Figure BDA0003003365230000051
其中，pins和vins分别为惯导推算定位的位置和速度；pgnss和vgnss分别为GNSS接收机输出的位置和速度；Vgnss为GNSS接收机的量测噪声，其服从均值为0、协方差为
Figure BDA0003003365230000052
的高斯分布；Hgnss为GNSS定位观测矩阵，且：
Figure BDA0003003365230000053
其中，I为单位矩阵；0为零矩阵；
4.4.2)根据获取的激光点云数据和每帧激光点云数据的调节因子，建立激光点云定位观测模型Zlidar：
Figure BDA0003003365230000054
其中，Plidar为激光点云定位计算出的车辆位置；
Figure BDA0003003365230000055
为激光点云定位得到的待测车辆的姿态与惯导推算定位得到的待测车辆的姿态之间的失准角；Vlidar为激光点云定位的误差噪声，其服从均值为0、协方差为
Figure BDA0003003365230000056
的高斯分布；Hlidar为激光点云定位观测矩阵，且：
Figure BDA0003003365230000057
4.4.3)根据轮速计获取的定位数据，建立轮速计定位观测模型Zv：
Figure BDA0003003365230000058
其中，vb为车辆速度在车辆坐标系下的三维向量，且
Figure BDA0003003365230000059
Figure BDA00030033652300000510
为车辆横向速度，
Figure BDA00030033652300000511
为轮速计所获取的待测车辆的前向速度值，
Figure BDA00030033652300000512
为车辆纵向速度；Hv为轮速计观测矩阵，且Hv＝[03×3 I3×3 03×9]；Vv为轮速计的量测噪声，其服从均值为0、协方差为
Figure BDA00030033652300000513
的高斯分布。
进一步地，所述步骤4.5)的具体过程为：
4.5.1)假设上一时刻k-1的误差状态为Xk-1/k-1、协方差为pk-1/k-1，通过下述方程进行时间更新：
Xk/k-1＝Γk-1/k-1Xk-1/k-1
pk/k-1＝Γk-1/k-1pk-1/k-1Γk-1/k-1+Qk-1
其中，Xk/k-1为一步预测误差状态；pk/k-1为一步预测误差状态的协方差；Γk-1/k-1为一步状态转移矩阵，且Γk-1/k-1≈I+F(Xk-1/k-1)ΔT，I为单位矩阵，F(Xk-1/k-1)为状态转移方程，ΔT为更新时间间隔；Qk-1为***噪声协方差；
4.5.2)时间更新后，若接收到GNSS定位数据、激光点云数据或轮速计定位数据三者中的任意定位数据，则通过下述方程进行量测更新，得到当前时刻的误差状态Xk/k和协方差pk/k；否则，只进行时间更新：
Figure BDA0003003365230000061
Xk/k＝Xk/k-1+Kk(Zk-HkXk/k-1)
pk/k＝(I-KkHk)pk/k-1
其中，Kk为卡尔曼增益；Hk为相应定位信息的观测矩阵；
Figure BDA0003003365230000062
为观测矩阵的转置；Rk为量测噪声协方差；Zk为量测向量；pk为状态协方差。
进一步地，所述步骤4.6)的具体过程为：
4.6.1)根据待测车辆的惯性导航定位***获取的惯性导航定位数据，得到待测车辆当前时刻的姿态qins，k、速度vins，k和位置pins，k；
4.6.2)根据得到的误差状态Xk/k和协方差pk/k，对当前时刻k的姿态qins，k、速度vins，k和位置pins，k进行修正，得到待测车辆最终的当前时刻的姿态qk、速度vk和位置pk：
Figure BDA0003003365230000063
vk＝vins，k-δv
pk＝pins，k-δp
其中，φ为失准角。
一种多源信息自适应融合定位***，包括：
激光点云匹配定位模块，用于采用基于激光点云的匹配定位方法，根据待测车辆的激光雷达***获取的待测车辆周围环境的若干帧激光点云数据，得到每帧激光点云数据优化后的匹配残差以及每帧激光点云数据的所有特征点距离激光雷达的平均距离；
第一模糊逻辑推理模块，用于采用模糊逻辑推理方法，根据得到的匹配残差和平均距离，确定每帧激光点云数据的调节因子；
第二模糊逻辑推理模块，用于采用模糊逻辑推理方法，根据待测车辆的GNSS接收机获取的GNSS定位数据，确定GNSS定位数据的调节因子；
融合定位模块，用于根据待测车辆的惯性导航定位***获取的惯性导航定位数据和确定的调节因子，融合待测车辆的GNSS接收机获取的GNSS定位数据、激光雷达***获取的激光点云数据以及轮速计获取的定位数据，得到待测车辆当前时刻的姿态、速度和位置。
本发明由于采取以上技术方案，其具有以下优点：
1、本发明针对GNSS定位性能易受到环境影响的问题，在传统GNSS/惯性组合定位***基础上，基于误差状态卡尔曼滤波融合激光点云定位数据，充分利用GNSS定位和激光点云定位的互补特性，使得即使在GNSS信号受到干扰的区域中也能保证较高的定位精度。
2、本发明针对GNSS定位数据及激光点云定位数据的噪声特性具有时变特性的问题，在信息融合的基础上，基于模糊逻辑规则，对GNSS定位数据和激光点云定位数据的噪声参数进行自适应调整，从而可以根据场景的变化动态调整两种定位数据的权重，具有较好的场景自适应性，可以广泛应用于自动驾驶车辆导航定位领域中。
附图说明
图1是本发明一实施例提供的方法流程示意图，其中，IMU为惯性测量单元，是车辆惯性导航定位***内的传感器；
图2是本发明一实施例提供的方法中激光点云定位的模糊逻辑隶属函数示意图；
图3是本发明一实施例提供的方法中GNSS定位的模糊逻辑隶属函数示意图。
具体实施方式
下面将参照附图更详细地描述本发明的示例性实施方式。虽然附图中显示了本发明的示例性实施方式，然而应当理解，可以以各种形式实现本发明而不应被这里阐述的实施方式所限制。相反，提供这些实施方式是为了能够更透彻地理解本发明，并且能够将本发明的范围完整的传达给本领域的技术人员。
应理解的是，文中使用的术语仅出于描述特定示例实施方式的目的，而无意于进行限制。除非上下文另外明确地指出，否则如文中使用的单数形式“一”、“一个”以及“所述”也可以表示包括复数形式。术语“包括”、“包含”、“含有”以及“具有”是包含性的，并且因此指明所陈述的特征、步骤、操作、元件和/或部件的存在，但并不排除存在或者添加一个或多个其它特征、步骤、操作、元件、部件、和/或它们的组合。文中描述的方法步骤、过程、以及操作不解释为必须要求它们以所描述或说明的特定顺序执行，除非明确指出执行顺序。还应当理解，可以使用另外或者替代的步骤。
尽管可以在文中使用术语第一、第二、第三等来描述多个元件、部件、区域、层和/或部段，但是，这些元件、部件、区域、层和/或部段不应被这些术语所限制。这些术语可以仅用来将一个元件、部件、区域、层或部段与另一区域、层或部段区分开。除非上下文明确地指出，否则诸如“第一”、“第二”之类的术语以及其它数字术语在文中使用时并不暗示顺序或者次序。因此，以下讨论的第一元件、部件、区域、层或部段在不脱离示例实施方式的教导的情况下可以被称作第二元件、部件、区域、层或部段。
为了便于描述，可以在文中使用空间相对关系术语来描述如图中示出的一个元件或者特征相对于另一元件或者特征的关系，这些相对关系术语例如为“内部”、“外部”、“内侧”、“外侧”、“下面”、“上面”等。这种空间相对关系术语意于包括除图中描绘的方位之外的在使用或者操作中装置的不同方位。
本发明实施例提供的多源信息自适应融合定位方法及***，针对多场景下的无人驾驶车辆定位，在传统的GNSS/INS组合导航定位中，加入激光雷达、轮速计等其他信息，并基于模糊逻辑对GNSS及激光雷达定位信息的噪声协方差进行自适应调整，在线调整两者在融合定位算法中的权重，实现组合定位***的多场景自适应性。
实施例1
如图1所示，本实施例提供一种多源信息自适应融合定位方法，包括以下步骤：
1)待测车辆的激光雷达***发送激光信号并实时接收由待测车辆周围物体反射的信号，以点云形式表达待测车辆周围的环境，得到若干帧激光点云数据，每帧激光点云数据均为对待测车辆周围环境的完整表达。
2)采用基于激光点云的匹配定位方法，根据获取的若干帧激光点云数据，匹配得到相邻帧激光点云数据的相对位姿，并输出每帧激光点云数据优化后的匹配残差以及每帧激光点云数据的所有特征点距离激光雷达的平均距离，具体为：
2.1)基于点的曲率大小，提取一帧激光点云数据的特征数据，其中，提取的特征数据包括面特征点和线特征点，对于当前帧激光点云数据中的所有特征点，在上一帧激光点云数据中找出对应边和对应面，第一帧激光点云数据为基准，不需要进行估计定位。
2.2)根据提取的特征数据，构建点到线或面的距离残差方程，并以当前帧激光点云数据与上一帧激光点云数据之间的相对位姿为优化变量，以残差距离最小为优化目标，采用列文伯格-马夸尔特算法对距离残差方程进行最优化求解，得到当前帧激光点云数据与上一帧激光点云数据之间的相对位姿，并输出优化后的匹配残差以及当前帧激光点云数据的所有特征点坐标。其中，具体的计算过程可以采用现有技术中公开的LOAM(LidarOdometry and Mapping，实时激光雷达里程计和建图)算法，在此不多做赘述。
2.3)根据当前帧激光点云数据的特征点坐标，计算出当前帧激光点云数据的所有特征点距离激光雷达的平均距离，以用于下述步骤中的模糊逻辑推理。
2.4)进入步骤2.1)提取下一帧激光点云数据的特征数据，直至计算得到每帧激光点云数据的所有特征点距离激光雷达的平均距离。
3)采用模糊逻辑推理方法，根据每帧激光点云数据优化后的匹配残差以及所有特征点距离激光雷达的平均距离，确定每帧激光点云数据的调节因子，以在线调节激光雷达***定位误差的协方差矩阵，具体为：
3.1)根据实际需求，将匹配残差分为S、M和L三个等级，并分别确定三个等级匹配残差的隶属度函数，如图2(a)所示。
3.2)根据实际需求，将特征点平均距离分为S、M和L三个等级，并分别确定三个等级特征点平均距离的隶属度函数，如图2(b)所示。
3.3)根据实际需求，将输出的调节因子分为S、M和L三个等级，并分别确定三个等级调节因子的隶属度函数，如图2(c)所示。
3.4)根据确定的隶属度函数，对根据每帧激光点云数据优化后的匹配残差以及所有特征点距离激光雷达的平均距离进行模糊化，并按照预先设定的模糊推理规则进行推理，得到每帧激光点云数据的调节因子隶属度，其中，模糊推理规则为：
a)当匹配残差为S且特征点平均距离为S时，调节因子为S；
b)当匹配残差为S且特征点平均距离为M时，调节因子为S；
c)当匹配残差为S且特征点平均距离为L时，调节因子为M；
d)当匹配残差为M且特征点平均距离为S时，调节因子为S；
e)当匹配残差为M且特征点平均距离为M时，调节因子为M；
f)当匹配残差为M且特征点平均距离为L时，调节因子为L；
g)当匹配残差为L且特征点平均距离为S时，调节因子为M；
h)当匹配残差为L且特征点平均距离为M时，调节因子为L；
i)当匹配残差为L且特征点平均距离为L时，调节因子为L；
上述推理规则均采用最小法则，即取隶属度最小值。
3.5)采用加权平均判决法，对推理得到的调节因子隶属度进行去模糊化，得到调节因子确定值α1，此时，激光点云数据的量测协方差为
Figure BDA0003003365230000091
其中，
Figure BDA0003003365230000092
为激光点云数据的量测协方差初始值。
4)采用模糊逻辑推理方法，根据待测车辆的GNSS接收机获取的GNSS定位数据，确定GNSS定位数据的调节因子，用于在线调节GNSS定位误差的协方差矩阵，其中，GNSS定位数据包括可见卫星数和水平精度因子，具体为：
4.1)根据实际需求，将可见卫星数分为S、M和L三个等级，并分别确定三个等级可见卫星数的隶属度函数，如图3(a)所示。
4.2)根据实际需求，将水平精度因子分为S、M和L三个等级，并分别确定三个等级水平精度因子的隶属度函数，如图3(b)所示。
4.3)根据实际需求，将输出的调节因子分为S、M和L三个等级，并分别确定三个等级调节因子的隶属度函数，如图3(c)所示。
4.4)根据确定的隶属度函数，对获取的可见卫星数和水平精度因子进行模糊化，并按照预先设定的模糊推理规则进行推理，得到GNSS定位数据的调节因子隶属度，其中，模糊推理规则为：
A)当可见卫星数为S且水平精度因子为S时，调节因子为M；
B)当可见卫星数为S且水平精度因子为M时，调节因子为L；
C)当可见卫星数为S且水平精度因子为L时，调节因子为L；
D)当可见卫星数为M且水平精度因子为S时，调节因子为S；
E)当可见卫星数为M且水平精度因子为M时，调节因子为M；
F)当可见卫星数为M且水平精度因子为L时，调节因子为L；
G)当可见卫星数为L且水平精度因子为S时，调节因子为S；
H)当可见卫星数为L且水平精度因子为M时，调节因子为M；
I)当可见卫星数为L且水平精度因子为L时，调节因子为L；
上述推理规则均采用最小法则，即取隶属度最小值。
4.5)采用加权平均判决法，对推理得到的调节因子隶属度进行去模糊化，得到调节因子确定值α2，此时，GNSS定位数据的量测协方差为
Figure BDA0003003365230000101
其中，
Figure BDA0003003365230000102
为GNSS定位的量测协方差初始值。
5)基于序贯式的误差状态扩展卡尔曼滤波方法，根据待测车辆的惯性导航定位***获取的惯性导航定位数据和确定的调节因子，融合待测车辆的GNSS接收机获取的GNSS定位数据、激光雷达***获取的激光点云数据以及轮速计获取的定位数据等量测数据，对待测车辆的惯导推算定位误差进行实时估计并对其反馈修正，得到待测车辆最终的当前时刻k的姿态qk、速度vk和位置pk，具体为：
5.1)根据待测车辆的加速度计和陀螺仪获取的三轴加速度和三轴角速度，确定惯性导航定位***的惯性导航定位数据，包括待测车辆的姿态更新
Figure BDA0003003365230000103
速度更新
Figure BDA0003003365230000104
和位置更新
Figure BDA0003003365230000105
Figure BDA0003003365230000106
Figure BDA0003003365230000107
Figure BDA0003003365230000108
其中，
Figure BDA0003003365230000109
为四元数姿态更新矩阵；
Figure BDA00030033652300001010
为姿态四元数；
Figure BDA00030033652300001011
为待测车辆坐标系相对东-北-天导航坐标系的方向余弦矩阵；fb为待测车辆的加速度计输出的三轴加速度；ba为加速度计的零偏；
Figure BDA0003003365230000111
为地球坐标系相对于惯性坐标系的角速度矢量；
Figure BDA0003003365230000112
为导航坐标系相对于地球坐标系的角速度矢量；v为速度，且v＝[ve vn vu]分别为东向、北向和天向的速度分量；gn为重力场矢量；
Figure BDA0003003365230000113
为变换矩阵；secL为纬度的正割值，L为纬度；RN为东西圈主曲率半径；h为高程；RM为子午圈主曲率半径。且：
Figure BDA0003003365230000114
Figure BDA0003003365230000115
其中，
Figure BDA0003003365230000116
为车辆坐标系相对于导航坐标系下的角速度向量；
Figure BDA0003003365230000117
为
Figure BDA0003003365230000118
构成的反对称矩阵；
Figure BDA0003003365230000119
为待测车辆的陀螺仪输出的三轴角速度。
5.2)设定扩展卡尔曼滤波的状态量X为：
X＝[δp δv φ ba bg]T (6)
其中，δp为位置误差，且δp＝[δL δλ δh]，分别为纬度误差、经度误差和高度误差；δv为速度误差，且δv＝[δve δvn δvu]，分别为东向、北向和天向速度误差；φ为失准角，即理想导航系与实际导航系之间的旋转矢量；bg为陀螺仪的零偏。
5.3)建立状态方程
Figure BDA00030033652300001110
Figure BDA00030033652300001111
其中，X为状态向量；F(X)为状态转移方程；G(X)为噪声驱动方程；Wb为***噪声。且：
Figure BDA00030033652300001112
Figure BDA00030033652300001113
Figure BDA00030033652300001114
Figure BDA00030033652300001115
Figure BDA00030033652300001116
Figure BDA0003003365230000121
Figure BDA0003003365230000122
其中，
Figure BDA0003003365230000123
和
Figure BDA0003003365230000124
分别为待测车辆的加速度计和陀螺仪输出值的噪声；ωie为地球坐标系相对于惯性坐标系的角速度矢量；
Figure BDA0003003365230000125
为导航坐标系相对于惯性坐标系的角速度矢量。
5.4)根据待测车辆的GNSS接收机获取的GNSS定位数据、激光雷达***获取的激光点云数据和轮速计获取的定位数据等量测数据，以及确定的调节因子，分别建立对应的观测模型：
5.4.1)根据GNSS接收机获取的GNSS定位数据，建立GNSS定位观测模型Zgnss：
Figure BDA0003003365230000126
其中，pins和vins分别为惯导推算定位的位置和速度；pgnss和vgnss分别为GNSS接收机输出的位置和速度；Vgnss为GNSS接收机的量测噪声，其服从均值为0、协方差为
Figure BDA0003003365230000127
的高斯分布，此处
Figure BDA0003003365230000128
为上述步骤5.5)中所得的值；Hgnss为GNSS定位观测矩阵，且：
Figure BDA0003003365230000129
其中，I为单位矩阵；0为零矩阵。
5.4.2)根据激光雷达***获取的激光点云数据和每帧激光点云数据的调节因子，建立激光点云定位观测模型Zlidar：
Figure BDA00030033652300001210
其中，plidar为激光点云定位计算出的车辆位置(即每帧激光点云数据的所有特征点距离激光雷达的平均距离)；
Figure BDA00030033652300001211
为步骤2.2)中得到的待测车辆的姿态与惯导推算定位得到的待测车辆的姿态之间的失准角；Vlidar为激光点云定位的误差噪声，其服从均值为0、协方差为
Figure BDA00030033652300001212
的高斯分布，此处
Figure BDA00030033652300001213
为上述步骤3.5)中所得的值；Hlidar为激光点云定位观测矩阵，且：
Figure BDA00030033652300001214
5.4.3)根据轮速计获取的定位数据，建立轮速计定位观测模型Zv：
Figure BDA00030033652300001215
其中，vb为车辆速度在车辆坐标系下的三维向量，且
Figure BDA00030033652300001216
Figure BDA00030033652300001217
为车辆横向速度，
Figure BDA00030033652300001218
为轮速计所获取的待测车辆的前向速度值，
Figure BDA00030033652300001219
为车辆纵向速度，基于地面车辆的运动特性，可以假设
Figure BDA0003003365230000131
Hv为轮速计观测矩阵，且Hv＝[03×3 I3×3 03×9]；Vv为轮速计的量测噪声，其服从均值为0、协方差为
Figure BDA0003003365230000132
的高斯分布，此处
Figure BDA0003003365230000133
5.5)根据建立的状态方程和观测模型，进行扩展卡尔曼滤波的时间更新和量测更新：
5.5.1)假设上一时刻k-1的误差状态为Xk-1/k-1、协方差为pk-1/k-1，通过下述方程(20)进行预测，即时间更新：
Xk/k-1＝Γk-1/k-1Xk-1/k-1 (21)
pk/k-1＝Γk-1/k-1pk-1/k-1Γk-1/k-1+Qk-1 (22)
其中，Xk/k-1为一步预测误差状态；pk/k-1为一步预测误差状态的协方差；Γk-1/k-1为一步状态转移矩阵，且Γk-1/k-1≈I+F(Xk-1/k-1)ΔT，I为单位矩阵，F(Xk-1/k-1)为状态转移方程，可由上述公式(8)求得，ΔT为更新时间间隔；Qk-1为***噪声协方差。
5.5.2)时间更新后，若接收到GNSS定位数据、激光点云数据或轮速计定位数据三者中的任意定位数据，则通过下述方程(23)、(24)和(25)进行量测更新；否则，只进行时间更新：
Figure BDA0003003365230000134
Xk/k＝Xk/k-1+Kk(Zk-HkXk/k-1) (24)
pk/k＝(I-KkHk)Pk/k-1 (25)
其中，Kk为卡尔曼增益；Hk为相应定位信息的观测矩阵；
Figure BDA0003003365230000135
为观测矩阵的转置；Rk为量测噪声协方差；Zk为量测向量；pk为状态协方差。至此，可以得到当前时刻的误差状态Xk/k和协方差pk/k。
5.6)根据待测车辆的惯性导航定位***获取的惯性导航定位数据，得到待测车辆当前时刻的姿态qins，k、速度vins，k和位置pins，k。
5.7)根据步骤5.5)中得到的误差状态Xk/k和协方差pk/k，对当前时刻的姿态qins，k、速度vins，k和位置pins，k进行修正，确定待测车辆最终的当前时刻的姿态qk、速度vk和位置pk：
Figure BDA0003003365230000136
vk＝vins，k-δv (27)
pk＝pins，k-δp (28)
实施例2
本实施例提供一种多源信息自适应融合定位***，包括：
激光点云匹配定位模块，用于采用基于激光点云的匹配定位方法，根据待测车辆的激光雷达***获取的待测车辆周围环境的若干帧激光点云数据，得到每帧激光点云数据优化后的匹配残差以及每帧激光点云数据的所有特征点距离激光雷达的平均距离。
第一模糊逻辑推理模块，用于采用模糊逻辑推理方法，根据得到的匹配残差和平均距离，确定每帧激光点云数据的调节因子。
第二模糊逻辑推理模块，用于采用模糊逻辑推理方法，根据待测车辆的GNSS接收机获取的GNSS定位数据，确定GNSS定位数据的调节因子。
融合定位模块，用于基于序贯式的误差状态扩展卡尔曼滤波方法，根据待测车辆的惯性导航定位***获取的惯性导航定位数据和确定的调节因子，融合待测车辆的GNSS接收机获取的GNSS定位数据、激光雷达***获取的激光点云数据以及轮速计获取的定位数据，得到待测车辆当前时刻的姿态、速度和位置。
在一个优选的实施例中，第一模糊逻辑推理模块包括第一模糊化模块、第一模糊推理模块和第一去模糊化模块。
第一模糊化模块用于根据预先确定的隶属度函数，对根据每帧激光点云数据优化后的匹配残差以及所有特征点距离激光雷达的平均距离进行模糊化。
第一模糊推理模块用于根据模糊化后的匹配残差和特征点平均距离，按照预先设定的模糊推理规则进行推理，得到每帧激光点云数据的调节因子隶属度。
第一去模糊化模块用于采用加权平均判决法，对推理得到的调节因子隶属度进行去模糊化，得到调节因子确定值。
在一个优选的实施例中，第二模糊逻辑推理模块包括第二模糊化模块、第二模糊推理模块和第二去模糊化模块。
第二模糊化模块用于根据预先确定的隶属度函数，对获取的可见卫星数和水平精度因子进行模糊化。
第二模糊推理模块用于根据模糊化后的可见卫星数和水平精度因子，按照预先设定的模糊推理规则进行推理，得到GNSS定位数据的调节因子隶属度。
第二去模糊化模块用于采用加权平均判决法，对推理得到的调节因子隶属度进行去模糊化，得到调节因子确定值。
上述各实施例仅用于说明本发明，其中各部件的结构、连接方式和制作工艺等都是可以有所变化的，凡是在本发明技术方案的基础上进行的等同变换和改进，均不应排除在本发明的保护范围之外。

Claims (10)

1.一种多源信息自适应融合定位方法，其特征在于，包括以下内容：
1)采用基于激光点云的匹配定位方法，根据待测车辆的激光雷达***获取的待测车辆周围环境的若干帧激光点云数据，得到每帧激光点云数据优化后的匹配残差以及每帧激光点云数据的所有特征点距离激光雷达的平均距离；
2)采用模糊逻辑推理方法，根据得到的匹配残差和平均距离，确定每帧激光点云数据的调节因子；
3)采用模糊逻辑推理方法，根据待测车辆的GNSS接收机获取的GNSS定位数据，确定GNSS定位数据的调节因子；
4)根据待测车辆的惯性导航定位***获取的惯性导航定位数据和确定的调节因子，融合待测车辆的GNSS接收机获取的GNSS定位数据、激光雷达***获取的激光点云数据以及轮速计获取的定位数据，得到待测车辆当前时刻的姿态、速度和位置。
2.如权利要求1所述的一种多源信息自适应融合定位方法，其特征在于，所述步骤1)的具体过程为：
1.1)待测车辆的激光雷达***发送激光信号并实时接收由待测车辆周围物体反射的信号，以点云形式表达待测车辆周围的环境，得到若干帧激光点云数据；
1.2)基于点的曲率大小，提取一帧激光点云数据的特征数据；
1.3)根据提取的特征数据，构建点到线或面的距离残差方程，并以当前帧激光点云数据与上一帧激光点云数据之间的相对位姿为优化变量，以残差距离最小为优化目标，采用列文伯格-马夸尔特算法对距离残差方程进行最优化求解，得到当前帧激光点云数据与上一帧激光点云数据之间的相对位姿，并输出优化后的匹配残差以及当前帧激光点云数据的所有特征点坐标；
1.4)根据当前帧激光点云数据的特征点坐标，计算出当前帧激光点云数据的所有特征点距离激光雷达的平均距离；
1.5)进入步骤1.2)提取下一帧激光点云数据的特征数据，直至计算得到每帧激光点云数据的所有特征点距离激光雷达的平均距离。
3.如权利要求1所述的一种多源信息自适应融合定位方法，其特征在于，所述步骤2)的具体过程为：
2.1)将匹配残差分为S、M和L三个等级，并分别确定三个等级匹配残差的隶属度函数；
2.2)将特征点平均距离分为S、M和L三个等级，并分别确定三个等级特征点平均距离的隶属度函数；
2.3)将输出的调节因子分为S、M和L三个等级，并分别确定三个等级调节因子的隶属度函数；
2.4)根据确定的隶属度函数，对根据每帧激光点云数据优化后的匹配残差以及所有特征点距离激光雷达的平均距离进行模糊化，并按照预先设定的模糊推理规则进行推理，得到每帧激光点云数据的调节因子隶属度；
2.5)采用加权平均判决法，对推理得到的调节因子隶属度进行去模糊化，得到调节因子确定值α1，此时，激光点云数据的量测协方差为
Figure FDA0003003365220000021
其中，
Figure FDA0003003365220000022
为激光点云数据的量测协方差初始值。
4.如权利要求3所述的一种多源信息自适应融合定位方法，其特征在于，所述步骤3)的具体过程为：
3.1)将可见卫星数分为S、M和L三个等级，并分别确定三个等级可见卫星数的隶属度函数；
3.2)将水平精度因子分为S、M和L三个等级，并分别确定三个等级水平精度因子的隶属度函数；
3.3)将输出的调节因子分为S、M和L三个等级，并分别确定三个等级调节因子的隶属度函数；
3.4)根据确定的隶属度函数，对获取的可见卫星数和水平精度因子进行模糊化，并按照预先设定的模糊推理规则进行推理，得到GNSS定位数据的调节因子隶属度；
3.5)采用加权平均判决法，对推理得到的调节因子隶属度进行去模糊化，得到调节因子确定值α2，此时，GNSS定位数据的量测协方差为
Figure FDA0003003365220000023
其中，
Figure FDA0003003365220000024
为GNSS定位的量测协方差初始值。
5.如权利要求4所述的一种多源信息自适应融合定位方法，其特征在于，所述步骤4)的具体过程为：
4.1)根据待测车辆的加速度计和陀螺仪获取的三轴加速度和三轴角速度，确定惯性导航定位***的惯性导航定位数据；
4.2)设定扩展卡尔曼滤波的状态量X为：
X＝[δp δv φ ba bg]T
其中，δp为位置误差，且δp＝[δL δλ δh]，分别为纬度误差、经度误差和高度误差；δv为速度误差，且δv＝[δve δvn δvu]，分别为东向、北向和天向速度误差；φ为失准角，即理想导航系与实际导航系之间的旋转矢量；bg为陀螺仪的零偏；
4.3)建立状态方程
Figure FDA0003003365220000025
Figure FDA0003003365220000026
其中，X为状态向量；F(X)为状态转移方程；G(X)为噪声驱动方程；Wb为***噪声；
4.4)根据待测车辆的GNSS接收机获取的GNSS定位数据、激光雷达***获取的激光点云数据和轮速计获取的定位数据，以及确定的调节因子，分别建立对应的观测模型；
4.5)根据建立的状态方程和观测模型，进行扩展卡尔曼滤波的时间更新和量测更新，得到误差状态；
4.6)根据待测车辆的惯性导航定位***获取的惯性导航定位数据和得到的误差状态，确定待测车辆当前时刻的姿态、速度和位置。
6.如权利要求5所述的一种多源信息自适应融合定位方法，其特征在于，所述步骤4.1)中的惯性导航定位数据包括待测车辆的姿态更新
Figure FDA0003003365220000031
速度更新
Figure FDA0003003365220000032
和位置更新
Figure FDA0003003365220000033
Figure FDA0003003365220000034
Figure FDA0003003365220000035
Figure FDA0003003365220000036
其中，
Figure FDA0003003365220000037
为四元数姿态更新矩阵；
Figure FDA0003003365220000038
为姿态四元数；
Figure FDA0003003365220000039
为待测车辆坐标系相对东-北-天导航坐标系的方向余弦矩阵；fb为待测车辆的加速度计输出的三轴加速度；ba为加速度计的零偏；
Figure FDA00030033652200000310
为地球坐标系相对于惯性坐标系的角速度矢量；
Figure FDA00030033652200000311
为导航坐标系相对于地球坐标系的角速度矢量；v为速度，且v＝[ve vn vu]分别为东向、北向和天向的速度分量；gn为重力场矢量；
Figure FDA00030033652200000312
为变换矩阵；secL为纬度的正割值，L为纬度；RN为东西圈主曲率半径；h为高程；RM为子午圈主曲率半径；且：
Figure FDA00030033652200000313
Figure FDA00030033652200000314
其中，
Figure FDA00030033652200000315
为车辆坐标系相对于导航坐标系下的角速度向量；
Figure FDA00030033652200000316
为
Figure FDA00030033652200000317
构成的反对称矩阵；
Figure FDA00030033652200000318
为待测车辆的陀螺仪输出的三轴角速度。
7.如权利要求6所述的一种多源信息自适应融合定位方法，其特征在于，所述步骤4.4)的具体过程为：
4.4.1)根据GNSS接收机获取的GNSS定位数据和GNSS定位数据的调节因子，建立GNSS定位观测模型Zgnss：
Figure FDA00030033652200000319
其中，pins和vins分别为惯导推算定位的位置和速度；pgnss和vgnss分别为GNSS接收机输出的位置和速度；Vgnss为GNSS接收机的量测噪声，其服从均值为0、协方差为
Figure FDA0003003365220000041
的高斯分布；Hgnss为GNSS定位观测矩阵，且：
Figure FDA0003003365220000042
其中，I为单位矩阵；0为零矩阵；
4.4.2)根据获取的激光点云数据和每帧激光点云数据的调节因子，建立激光点云定位观测模型Zlidar：
Figure FDA0003003365220000043
其中，plidar为激光点云定位计算出的车辆位置；
Figure FDA0003003365220000044
为激光点云定位得到的待测车辆的姿态与惯导推算定位得到的待测车辆的姿态之间的失准角；Vlidar为激光点云定位的误差噪声，其服从均值为0、协方差为
Figure FDA0003003365220000045
的高斯分布；Hlidar为激光点云定位观测矩阵，且：
Figure FDA0003003365220000046
4.4.3)根据轮速计获取的定位数据，建立轮速计定位观测模型Zv：
Figure FDA0003003365220000047
其中，vb为车辆速度在车辆坐标系下的三维向量，且
Figure FDA0003003365220000048
Figure FDA0003003365220000049
为车辆横向速度，
Figure FDA00030033652200000410
为轮速计所获取的待测车辆的前向速度值，
Figure FDA00030033652200000411
为车辆纵向速度；Hv为轮速计观测矩阵，且Hv＝[03×3 I3×3 03×9]；Vv为轮速计的量测噪声，其服从均值为0、协方差为
Figure FDA00030033652200000412
的高斯分布。
8.如权利要求7所述的一种多源信息自适应融合定位方法，其特征在于，所述步骤4.5)的具体过程为：
4.5.1)假设上一时刻k-1的误差状态为Xk-1/k-1、协方差为Pk-1/k-1，通过下述方程进行时间更新：
Xk/k-1＝Γk-1/k-1Xk-1/k-1
Pk/k-1＝Γk-1/k-1Pk-1/k-1Γk-1/k-1+Qk-1
其中，Xk/k-1为一步预测误差状态；Pk/k-1为一步预测误差状态的协方差；Γk-1/k-1为一步状态转移矩阵，且Γk-1/k-1≈I+F(Xk-1/k-1)ΔT，I为单位矩阵，F(Xk-1/k-1)为状态转移方程，ΔT为更新时间间隔；Qk-1为***噪声协方差；
4.5.2)时间更新后，若接收到GNSS定位数据、激光点云数据或轮速计定位数据三者中的任意定位数据，则通过下述方程进行量测更新，得到当前时刻的误差状态Xk/k和协方差Pk/k；否则，只进行时间更新：
Figure FDA00030033652200000413
Xk/k＝Xk/k-1+Kk(Zk-HkXk/k-1)
Pk/k＝(I-KkHk)Pk/k-1
其中，Kk为卡尔曼增益；Hk为相应定位信息的观测矩阵；
Figure FDA0003003365220000052
为观测矩阵的转置；Rk为量测噪声协方差；Zk为量测向量；Pk为状态协方差。
9.如权利要求8所述的一种多源信息自适应融合定位方法，其特征在于，所述步骤4.6)的具体过程为：
4.6.1)根据待测车辆的惯性导航定位***获取的惯性导航定位数据，得到待测车辆当前时刻的姿态qins,k、速度vins,k和位置pins,k；
4.6.2)根据得到的误差状态Xk/k和协方差Pk/k，对当前时刻k的姿态qins,k、速度vins,k和位置pins,k进行修正，得到待测车辆最终的当前时刻的姿态qk、速度vk和位置pk：
Figure FDA0003003365220000051
vk＝vins,k-δv
pk＝pins,k-δp
其中，φ为失准角。
10.一种多源信息自适应融合定位***，其特征在于，包括：
激光点云匹配定位模块，用于采用基于激光点云的匹配定位方法，根据待测车辆的激光雷达***获取的待测车辆周围环境的若干帧激光点云数据，得到每帧激光点云数据优化后的匹配残差以及每帧激光点云数据的所有特征点距离激光雷达的平均距离；
第一模糊逻辑推理模块，用于采用模糊逻辑推理方法，根据得到的匹配残差和平均距离，确定每帧激光点云数据的调节因子；
第二模糊逻辑推理模块，用于采用模糊逻辑推理方法，根据待测车辆的GNSS接收机获取的GNSS定位数据，确定GNSS定位数据的调节因子；
融合定位模块，用于根据待测车辆的惯性导航定位***获取的惯性导航定位数据和确定的调节因子，融合待测车辆的GNSS接收机获取的GNSS定位数据、激光雷达***获取的激光点云数据以及轮速计获取的定位数据，得到待测车辆当前时刻的姿态、速度和位置。
CN202110355182.4A 2021-04-01 2021-04-01 一种多源信息自适应融合定位方法及*** Active CN113029137B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110355182.4A CN113029137B (zh) 2021-04-01 2021-04-01 一种多源信息自适应融合定位方法及***

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110355182.4A CN113029137B (zh) 2021-04-01 2021-04-01 一种多源信息自适应融合定位方法及***

Publications (2)

Publication Number Publication Date
CN113029137A true CN113029137A (zh) 2021-06-25
CN113029137B CN113029137B (zh) 2023-02-28

Family

ID=76453745

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110355182.4A Active CN113029137B (zh) 2021-04-01 2021-04-01 一种多源信息自适应融合定位方法及***

Country Status (1)

Country Link
CN (1) CN113029137B (zh)

Cited By (19)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113804202A (zh) * 2021-11-19 2021-12-17 智道网联科技(北京)有限公司 组合导航方法、电子设备及存储介质
CN113984044A (zh) * 2021-10-08 2022-01-28 杭州鸿泉物联网技术股份有限公司 一种基于车载多感知融合的车辆位姿获取方法及装置
CN114061596A (zh) * 2021-11-19 2022-02-18 北京国家新能源汽车技术创新中心有限公司 自动驾驶定位方法、***、测试方法、设备及存储介质
CN114296090A (zh) * 2021-12-22 2022-04-08 华人运通(上海)自动驾驶科技有限公司 一种车辆定位方法、装置、设备及介质
CN114323003A (zh) * 2021-12-27 2022-04-12 青岛慧拓智能机器有限公司 一种基于umb、imu及激光雷达的井工矿融合定位方法
CN114527487A (zh) * 2022-02-24 2022-05-24 东南大学 点云稀疏场景下主动特征辅助的自适应融合定位方法
CN114527481A (zh) * 2022-02-24 2022-05-24 东南大学 一种面向遮挡测评环境的多传感高精度融合测量方法
CN114842150A (zh) * 2022-05-20 2022-08-02 北京理工大学 一种融合图案信息的数字化车辆点云模型构建方法及***
CN114877883A (zh) * 2022-03-22 2022-08-09 武汉大学 车路协同环境下考虑通信延迟的车辆定位方法及***
CN115574818A (zh) * 2022-12-09 2023-01-06 北京理工大学 基于改进联邦滤波的结构化道路车辆导航定位方法及***
CN115586511A (zh) * 2022-11-25 2023-01-10 唐山百川工业服务有限公司 一种基于阵列立柱的激光雷达二维定位方法
CN115856979A (zh) * 2023-02-16 2023-03-28 智道网联科技(北京)有限公司 自动驾驶车辆的定位方法、装置及电子设备、存储介质
WO2023060631A1 (zh) * 2021-10-14 2023-04-20 重庆数字城市科技有限公司 一种激光点云数据实时处理***和方法
CN116642501A (zh) * 2023-07-25 2023-08-25 齐鲁空天信息研究院 一种惯性为核心的车道线辅助定位的多源融合方法
CN117034191A (zh) * 2023-08-04 2023-11-10 广东省机场管理集团有限公司工程建设指挥部 基于5g云平台的车辆多源信息融合方法、装置及介质
CN117128951A (zh) * 2023-10-27 2023-11-28 中国科学院国家授时中心 适用于自动驾驶农机的多传感器融合导航定位***及方法
CN117760417A (zh) * 2023-12-19 2024-03-26 苏州尚同墨方智能科技有限公司 一种基于4d毫米波雷达与imu的融合定位方法及***
CN117972637A (zh) * 2024-03-28 2024-05-03 天津大学 角振动台的角速度数据融合方法和角速度数据融合装置
CN114842150B (zh) * 2022-05-20 2024-06-28 北京理工大学 一种融合图案信息的数字化车辆点云模型构建方法及***

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20180299273A1 (en) * 2017-04-17 2018-10-18 Baidu Online Network Technology (Beijing) Co., Ltd. Method and apparatus for positioning vehicle
CN109946731A (zh) * 2019-03-06 2019-06-28 东南大学 一种基于模糊自适应无迹卡尔曼滤波的车辆高可靠融合定位方法
CN110243358A (zh) * 2019-04-29 2019-09-17 武汉理工大学 多源融合的无人车室内外定位方法及***
CN110969649A (zh) * 2019-11-29 2020-04-07 上海有个机器人有限公司 一种激光点云与地图的匹配评价方法、介质、终端和装置
WO2020104423A1 (en) * 2018-11-20 2020-05-28 Volkswagen Aktiengesellschaft Method and apparatus for data fusion of lidar data and image data

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20180299273A1 (en) * 2017-04-17 2018-10-18 Baidu Online Network Technology (Beijing) Co., Ltd. Method and apparatus for positioning vehicle
WO2020104423A1 (en) * 2018-11-20 2020-05-28 Volkswagen Aktiengesellschaft Method and apparatus for data fusion of lidar data and image data
CN109946731A (zh) * 2019-03-06 2019-06-28 东南大学 一种基于模糊自适应无迹卡尔曼滤波的车辆高可靠融合定位方法
CN110243358A (zh) * 2019-04-29 2019-09-17 武汉理工大学 多源融合的无人车室内外定位方法及***
CN110969649A (zh) * 2019-11-29 2020-04-07 上海有个机器人有限公司 一种激光点云与地图的匹配评价方法、介质、终端和装置

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
LINNA ZHOU,ET AL.: "Localization of coal mine rescue robots based on multi-sensor fusion", 《IEEE:2021 CHINA AUTOMATION CONGRESS (CAC)》 *
WENRUI JIN，ET AL.: "A Modified Kalman Filtering via Fuzzy Logic System for", 《PROCEEDINGS OF THE 2007 IEEE》 *
彭文正等: "多传感器信息融合的自动驾驶车辆定位与速度估计", 《传感技术学报》 *

Cited By (27)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113984044A (zh) * 2021-10-08 2022-01-28 杭州鸿泉物联网技术股份有限公司 一种基于车载多感知融合的车辆位姿获取方法及装置
WO2023060631A1 (zh) * 2021-10-14 2023-04-20 重庆数字城市科技有限公司 一种激光点云数据实时处理***和方法
CN113804202A (zh) * 2021-11-19 2021-12-17 智道网联科技(北京)有限公司 组合导航方法、电子设备及存储介质
CN114061596A (zh) * 2021-11-19 2022-02-18 北京国家新能源汽车技术创新中心有限公司 自动驾驶定位方法、***、测试方法、设备及存储介质
CN113804202B (zh) * 2021-11-19 2022-02-22 智道网联科技(北京)有限公司 组合导航方法、电子设备及存储介质
CN114061596B (zh) * 2021-11-19 2024-03-22 北京国家新能源汽车技术创新中心有限公司 自动驾驶定位方法、***、测试方法、设备及存储介质
WO2023087802A1 (zh) * 2021-11-19 2023-05-25 北京国家新能源汽车技术创新中心有限公司 自动驾驶定位方法、***、测试方法、设备及存储介质
CN114296090A (zh) * 2021-12-22 2022-04-08 华人运通(上海)自动驾驶科技有限公司 一种车辆定位方法、装置、设备及介质
CN114323003A (zh) * 2021-12-27 2022-04-12 青岛慧拓智能机器有限公司 一种基于umb、imu及激光雷达的井工矿融合定位方法
CN114527481A (zh) * 2022-02-24 2022-05-24 东南大学 一种面向遮挡测评环境的多传感高精度融合测量方法
CN114527487A (zh) * 2022-02-24 2022-05-24 东南大学 点云稀疏场景下主动特征辅助的自适应融合定位方法
CN114877883A (zh) * 2022-03-22 2022-08-09 武汉大学 车路协同环境下考虑通信延迟的车辆定位方法及***
CN114877883B (zh) * 2022-03-22 2024-04-26 武汉大学 车路协同环境下考虑通信延迟的车辆定位方法及***
CN114842150A (zh) * 2022-05-20 2022-08-02 北京理工大学 一种融合图案信息的数字化车辆点云模型构建方法及***
CN114842150B (zh) * 2022-05-20 2024-06-28 北京理工大学 一种融合图案信息的数字化车辆点云模型构建方法及***
CN115586511A (zh) * 2022-11-25 2023-01-10 唐山百川工业服务有限公司 一种基于阵列立柱的激光雷达二维定位方法
CN115586511B (zh) * 2022-11-25 2023-03-03 唐山百川工业服务有限公司 一种基于阵列立柱的激光雷达二维定位方法
CN115574818B (zh) * 2022-12-09 2023-02-28 北京理工大学 基于改进联邦滤波的结构化道路车辆导航定位方法及***
CN115574818A (zh) * 2022-12-09 2023-01-06 北京理工大学 基于改进联邦滤波的结构化道路车辆导航定位方法及***
CN115856979A (zh) * 2023-02-16 2023-03-28 智道网联科技(北京)有限公司 自动驾驶车辆的定位方法、装置及电子设备、存储介质
CN116642501B (zh) * 2023-07-25 2023-09-29 齐鲁空天信息研究院 一种惯性为核心的车道线辅助定位的多源融合方法
CN116642501A (zh) * 2023-07-25 2023-08-25 齐鲁空天信息研究院 一种惯性为核心的车道线辅助定位的多源融合方法
CN117034191A (zh) * 2023-08-04 2023-11-10 广东省机场管理集团有限公司工程建设指挥部 基于5g云平台的车辆多源信息融合方法、装置及介质
CN117128951A (zh) * 2023-10-27 2023-11-28 中国科学院国家授时中心 适用于自动驾驶农机的多传感器融合导航定位***及方法
CN117128951B (zh) * 2023-10-27 2024-02-02 中国科学院国家授时中心 适用于自动驾驶农机的多传感器融合导航定位***及方法
CN117760417A (zh) * 2023-12-19 2024-03-26 苏州尚同墨方智能科技有限公司 一种基于4d毫米波雷达与imu的融合定位方法及***
CN117972637A (zh) * 2024-03-28 2024-05-03 天津大学 角振动台的角速度数据融合方法和角速度数据融合装置

Also Published As

Publication number Publication date
CN113029137B (zh) 2023-02-28

Similar Documents

Publication Publication Date Title
CN113029137B (zh) 一种多源信息自适应融合定位方法及***
CN111045068B (zh) 一种基于非导航卫星信号的低轨卫星自主定轨定姿方法
CN104729506B (zh) 一种视觉信息辅助的无人机自主导航定位方法
CN111238467B (zh) 一种仿生偏振光辅助的无人作战飞行器自主导航方法
CN112505737B (zh) 一种gnss/ins组合导航方法
CN110411462A (zh) 一种gnss/惯性/车道线约束/里程计多源融合方法
CN109752725A (zh) 一种低速商用机器人、定位导航方法及定位导航***
CN108871336A (zh) 一种车辆位置估算***及方法
Zhang et al. A hybrid intelligent algorithm DGP-MLP for GNSS/INS integration during GNSS outages
Gu et al. Sensor fusion in mobile robot: some perspectives
CN115574816A (zh) 仿生视觉多源信息智能感知无人平台
CN116719037A (zh) 一种用于智能割草机器人的环境感知方法及***
CN115265532A (zh) 一种用于船用组合导航中的辅助滤波方法
JP7275553B2 (ja) 移動体、移動体の制御方法及びプログラム
Zuo et al. A GNSS/IMU/vision ultra-tightly integrated navigation system for low altitude aircraft
CN113237482B (zh) 一种基于因子图的城市峡谷环境下车辆鲁棒定位方法
Sharaf et al. Merits and limitations of using fuzzy inference system for temporal integration of INS/GPS in vehicular navigation
CN116625394A (zh) 一种未知路况下机器人环境感知与路径寻优***
CN116105729A (zh) 一种针对野外洞穴森林环境侦察的多传感器融合定位方法
Selkäinaho Adaptive autonomous navigation of mobile robots in unknown environments
Hu et al. Fusion of vision, 3D gyro and GPS for camera dynamic registration
Xu et al. Adaptive kalman filter with linear equality road constraints for autonomous vehicle localization
CN114543793A (zh) 车辆导航***的多传感器融合定位方法
Murthy et al. Application of fuzzy logic based Kalman filter and vehicle rate sensor in optimizing differential global position system
Zhou et al. Encoder/INS/GNSS Integrated Navigation for Autonomous Land Vehicles with Mecanum Wheels

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