CN114137592A - 一种多源传感器融合定位的切换方法及系统 - Google Patents

一种多源传感器融合定位的切换方法及系统 Download PDF

Info

Publication number
CN114137592A
CN114137592A CN202111447901.1A CN202111447901A CN114137592A CN 114137592 A CN114137592 A CN 114137592A CN 202111447901 A CN202111447901 A CN 202111447901A CN 114137592 A CN114137592 A CN 114137592A
Authority
CN
China
Prior art keywords
data
laser radar
positioning
speed
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.)
Pending
Application number
CN202111447901.1A
Other languages
English (en)
Inventor
赵元丁
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shanghai Yuwan Technology Co ltd
Original Assignee
Shanghai Yuwan 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 Shanghai Yuwan Technology Co ltd filed Critical Shanghai Yuwan Technology Co ltd
Priority to CN202111447901.1A priority Critical patent/CN114137592A/zh
Publication of CN114137592A publication Critical patent/CN114137592A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • 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
    • 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/46—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being of a radio-wave signal type

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

本发明涉及一种多源传感器融合定位的切换方法及系统，方法包括：对第k时刻的所述卫星导航定位原始数据、所述速度原始数据、所述姿态原始数据和所述激光雷达定位原始数据分别进行预处理，获得卫星导航定位处理数据、速度处理数据、姿态处理数据和激光雷达定位处理数据；将第k时刻的卫星导航定位处理数据、激光雷达定位处理数据、姿态处理数据和速度处理数据输入至误差滤波模型中进行修正，并确定GNSS和激光雷达的切换策略。本发明公开的切换策略能够应用于无GPS区域以及多植被等复杂区域，有地解决了目前定位不稳定的问题。

Description

一种多源传感器融合定位的切换方法及系统
技术领域
本发明涉及传感器切换技术领域，特别是涉及一种多源传感器融合定位的切换方法及系统。
背景技术
现有多源传感器组合方案有如下几种:①全球导航卫星系统GNSS+惯性测量单元imu；②全球导航卫星系统GNSS+惯性测量单元imu+激光雷达lidar；但上述方案所应用的场景比较单一，受场景限制，当遇到复杂场景时，定位会丢失，不稳定。
发明内容
本发明的目的是提供一种多源传感器融合定位的切换方法及系统，以提高定位稳定性。
为实现上述目的，本发明提供了一种多源传感器融合定位的切换方法，所述方法包括：
步骤S1：获取第k时刻的卫星导航定位原始数据、速度原始数据、姿态原始数据和激光雷达定位原始数据；
步骤S2：对第k时刻的所述卫星导航定位原始数据、所述速度原始数据、所述姿态原始数据和所述激光雷达定位原始数据分别进行预处理，获得卫星导航定位处理数据、速度处理数据、姿态处理数据和激光雷达定位处理数据；
步骤S3：将第k时刻的卫星导航定位处理数据、激光雷达定位处理数据、姿态处理数据和速度处理数据输入至误差滤波模型中进行修正，并确定全球导航卫星系统GNSS和激光雷达是否正常；
步骤S4：如果GNSS正常，则切换GNSS，令k＝k+1，并返回“步骤S1”；如果GNSS异常，且激光雷达正常，则切换激光雷达，令k＝k+1，并返回“步骤S1”；如果GNSS和激光雷达均异常，则结束。
可选地，步骤S2具体包括：
步骤S21：对第k时刻所述卫星导航原始数据进行过滤处理，获得卫星导航定位处理数据；
步骤S22：筛选第k时刻所述激光雷达定位原始数据中匹配得分大于3.0的值作为激光雷达定位处理数据；
步骤S23：取静止状态下的加速度均值和角速度均值，然后将第k时刻所述姿态原始数据减去加速度均值和角速度均值获得姿态处理数据；
步骤S24：将第k时刻所述速度原始数据加入噪声，获得速度处理数据；所述速度原始数据包括GNSS对应的速度数据和激光雷达对应的速度数据；所述速度处理数据包括激光雷达对应的速度处理数据和GNSS对应的速度处理数据。
可选地，步骤S3具体包括：
步骤S31：将第k时刻的卫星导航定位处理数据、姿态处理数据和GNSS对应的速度处理数据输入至误差滤波模型中进行修正，获得第k+1时刻的卫星导航定位修正数据、GNSS对应的姿态修正数据和GNSS对应的速度修正数据；将第k时刻的激光雷达定位处理数据、姿态处理数据和激光雷达对应的速度处理数据输入至误差滤波模型中进行修正，获得第k+1时刻的激光雷达定位修正数据、激光雷达对应的姿态修正数据和激光雷达对应的速度修正数据；
步骤S32：根据第k+1时刻的卫星导航定位修正数据、GNSS对应的姿态修正数据和GNSS对应的速度修正数据计算第一卡尔曼检测值；根据第k+1时刻的激光雷达定位修正数据、激光雷达对应的姿态修正数据和激光雷达对应的速度修正数据计算第二卡尔曼检测值；
步骤S33：根据第一卡尔曼检测值判断GNSS是否正常；根据第二卡尔曼检测值判断激光雷达是否正常。
可选地，步骤S1具体包括：
步骤S11：利用全球导航卫星系统GNSS采集第k时刻的卫星导航定位原始数据；
步骤S12：利用里程计采集第k时刻的速度原始数据；
步骤S13：利用惯性测量单元采集第k时刻的姿态原始数据；
步骤S14：利用激光雷达采集第k时刻的激光雷达定位原始数据。
可选地，所述卫星导航定位原始数据包括星数、HDOP值、基线长和GNSS定位位置数据。
本发明还提供一种多源传感器融合定位的切换系统，所述系统包括：
获取模块，用于获取第k时刻的卫星导航定位原始数据、速度原始数据、姿态原始数据和激光雷达定位原始数据；
预处理模块，用于对第k时刻的所述卫星导航定位原始数据、所述速度原始数据、所述姿态原始数据和所述激光雷达定位原始数据分别进行预处理，获得卫星导航定位处理数据、速度处理数据、姿态处理数据和激光雷达定位处理数据；
修正模块，用于将第k时刻的卫星导航定位处理数据、激光雷达定位处理数据、姿态处理数据和速度处理数据输入至误差滤波模型中进行修正，并确定全球导航卫星系统GNSS和激光雷达是否正常；
切换模块，用于如果GNSS正常，则切换GNSS，令k＝k+1，并返回“获取模块”；如果GNSS异常，且激光雷达正常，则切换激光雷达，令k＝k+1，并返回“获取模块”；如果GNSS和激光雷达均异常，则结束。
可选地，所述预处理模块具体包括：
过滤单元，用于对第k时刻所述卫星导航原始数据进行过滤处理，获得卫星导航定位处理数据；
筛选单元，用于筛选第k时刻所述激光雷达定位原始数据中匹配得分大于3.0的值作为激光雷达定位处理数据；
姿态处理数据确定单元，用于取静止状态下的加速度均值和角速度均值，然后将第k时刻所述姿态原始数据减去加速度均值和角速度均值获得姿态处理数据；
噪声加入单元，用于将第k时刻所述速度原始数据加入噪声，获得速度处理数据；所述速度原始数据包括GNSS对应的速度数据和激光雷达对应的速度数据；所述速度处理数据包括激光雷达对应的速度处理数据和GNSS对应的速度处理数据。
可选地，所述修正模块具体包括：
修正单元，用于将第k时刻的卫星导航定位处理数据、姿态处理数据和GNSS对应的速度处理数据输入至误差滤波模型中进行修正，获得第k+1时刻的卫星导航定位修正数据、GNSS对应的姿态修正数据和GNSS对应的速度修正数据；将第k时刻的激光雷达定位处理数据、姿态处理数据和激光雷达对应的速度处理数据输入至误差滤波模型中进行修正，获得第k+1时刻的激光雷达定位修正数据、激光雷达对应的姿态修正数据和激光雷达对应的速度修正数据；
卡尔曼检测值计算单元，用于根据第k+1时刻的卫星导航定位修正数据、GNSS对应的姿态修正数据和GNSS对应的速度修正数据计算第一卡尔曼检测值；根据第k+1时刻的激光雷达定位修正数据、激光雷达对应的姿态修正数据和激光雷达对应的速度修正数据计算第二卡尔曼检测值；
传感器工作状态判断单元，用于根据第一卡尔曼检测值判断GNSS是否正常；根据第二卡尔曼检测值判断激光雷达是否正常。
可选地，所述获取模块具体包括：
第一获取单元，用于利用全球导航卫星系统GNSS采集第k时刻的卫星导航定位原始数据；
第二获取单元，用于利用里程计采集第k时刻的速度原始数据；
第三获取单元，用于利用惯性测量单元采集第k时刻的姿态原始数据；
第四获取单元，用于利用激光雷达采集第k时刻的激光雷达定位原始数据。
可选地，所述卫星导航定位原始数据包括星数、HDOP值、基线长和GNSS定位位置数据。
根据本发明提供的具体实施例，本发明公开了以下技术效果：
本发明涉及一种多源传感器融合定位的切换方法及系统，方法包括：对第k时刻的所述卫星导航定位原始数据、所述速度原始数据、所述姿态原始数据和所述激光雷达定位原始数据分别进行预处理，获得卫星导航定位处理数据、速度处理数据、姿态处理数据和激光雷达定位处理数据；将第k时刻的卫星导航定位处理数据、激光雷达定位处理数据、姿态处理数据和速度处理数据输入至误差滤波模型中进行修正，并确定GNSS和激光雷达的切换策略。本发明公开的切换策略能够应用于无GPS区域以及多植被等复杂区域，有地解决了目前定位不稳定的问题。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明多源传感器融合定位的切换方法流程图；
图2为本发明多源传感器融合定位的切换系统结构图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明的目的是提供一种多源传感器融合定位的切换方法及系统，以提高定位稳定性。
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
先解释一些跟技术方案有关的专业术语：
GNSS(GlobalNavigation Satellite System)：全球导航卫星系统，包括GDS、GPS、GLONASS、GALILEO四大导航系统。
imu(Inertial MeasurementUnit):惯性测量单元，内含加速度计、重力计、磁力计、三轴陀螺仪等高精度元件。
odom(odometry):里程计.
lidar(lightdetection andranging):激光雷达。
ESKF(error-state KalmanFilter):误差估计卡尔曼滤波。
EKF_FUSION_GNSS:GNSS融合卡尔曼。
EKF_FUSION_LIDAR:激光雷达融合卡尔曼。
HDOP(Horizontal DilutionPrecision)：包括经度和纬度等因子，称为水平(平面)位置精度因子。
MEMS(Micro Electromechanical System)：微机电系统。是指尺寸在几毫米乃至更小的高科技装置，其内部结构一般在微米甚至纳米量级，是一个独立的智能系统。主要由传感器、执行器和微能源三大部分组成。
实施例1
如图1所示，本发明公开一种多源传感器融合定位的切换方法，所述方法包括：
步骤S1：获取第k时刻的卫星导航定位原始数据、速度原始数据、姿态原始数据和激光雷达定位原始数据。
步骤S2：对第k时刻的所述卫星导航定位原始数据、所述速度原始数据、所述姿态原始数据和所述激光雷达定位原始数据分别进行预处理，获得卫星导航定位处理数据、速度处理数据、姿态处理数据和激光雷达定位处理数据。
步骤S3：将第k时刻的卫星导航定位处理数据、激光雷达定位处理数据、姿态处理数据和速度处理数据输入至误差滤波模型中进行修正，并确定全球导航卫星系统GNSS和激光雷达是否正常。
步骤S4：如果GNSS正常，则切换GNSS，令k＝k+1，并返回“步骤S1”；如果GNSS异常，且激光雷达正常，则切换激光雷达，令k＝k+1，并返回“步骤S1”；如果GNSS和激光雷达均异常，则结束。
下面对各个步骤进行详细论述：
步骤S1：获取第k时刻的卫星导航定位原始数据、速度原始数据、姿态原始数据和激光雷达定位原始数据，具体包括：
步骤S11：利用全球导航卫星系统GNSS采集第k时刻的卫星导航定位原始数据；所述卫星导航定位原始数据包括星数、HDOP值、基线长和GNSS定位位置数据。
步骤S12：利用里程计odom采集第k时刻的速度原始数据；所述速度原始数据包括GNSS对应的速度数据和激光雷达对应的速度数据。
步骤S13：利用惯性测量单元imu采集第k时刻的姿态原始数据；所述姿态原始数据包括角速度。
步骤S14：利用激光雷达lidar采集第k时刻的激光雷达定位原始数据。
步骤S2：对第k时刻的所述卫星导航定位原始数据、所述姿态原始数据、所述速度原始数据和所述激光雷达定位原始数据分别进行预处理，获得卫星导航定位处理数据、姿态处理数据、速度处理数据和激光雷达定位处理数据；所述速度处理数据包括激光雷达对应的速度处理数据和GNSS对应的速度处理数据；
步骤S2具体包括：
步骤S21：对第k时刻所述卫星导航原始数据进行过滤处理，获得卫星导航定位处理数据。
步骤S22：筛选第k时刻所述激光雷达定位原始数据中匹配得分大于3.0的值作为激光雷达定位处理数据。
步骤S23：取静止状态下的加速度均值和角速度均值，然后将所述姿态原始数据减去加速度均值和角速度均值获得姿态处理数据。
步骤S24：将第k时刻的速度原始数据加入噪声，获得速度处理数据；所述速度处理数据包括激光雷达对应的速度处理数据和GNSS对应的速度处理数据；具体地，将GNSS对应的速度数据和激光雷达对应的速度数据分别加入不同的噪声，获得激光雷达对应的速度处理数据和GNSS对应的速度处理数据。
步骤S3：将第k时刻的卫星导航定位处理数据、激光雷达定位处理数据、姿态处理数据和速度处理数据输入至误差滤波模型中进行修正，并确定GNSS和激光雷达是否正常，具体包括：
步骤S31：将第k时刻的卫星导航定位处理数据、姿态处理数据和GNSS对应的速度处理数据输入至误差滤波模型中进行修正，获得第k+1时刻的卫星导航定位修正数据、GNSS对应的姿态修正数据和GNSS对应的速度修正数据；将第k时刻的激光雷达定位处理数据、姿态处理数据和激光雷达对应的速度处理数据输入至误差滤波模型中进行修正，获得第k+1时刻的激光雷达定位修正数据、激光雷达对应的姿态修正数据和激光雷达对应的速度修正数据。
步骤S32：根据第k+1时刻的卫星导航定位修正数据、GNSS对应的姿态修正数据和GNSS对应的速度修正数据计算第一卡尔曼检测值；根据第k+1时刻的激光雷达定位修正数据、激光雷达对应的姿态修正数据和激光雷达对应的速度修正数据计算第二卡尔曼检测值。
步骤S33：根据第一卡尔曼检测值判断GNSS是否正常；根据第二卡尔曼检测值判断激光雷达是否正常。
姿态修正:
姿态解算采用旋转矢量的方式进行更新，设三轴陀螺仪的输出为ω＝[ωχωyωz]T，可以得到在一个解算周期T内的角度增量为：
Δθ＝【θxθyθz】＝ω·T(1)；
其中，ω为陀螺仪输出的角速度,ωχ,ωy,ωz分别表示三个轴向上的角速度，Δθ表示角度增量，θx,θy,θz分别表示三个轴向上的角度增量，T表示周期。
角度增量Δθ可通过公式(2)转换为第k时刻的姿态变化四元数，具体公式为：
Figure BDA0003384563540000081
其中，
Figure BDA0003384563540000082
基于第k时刻的姿态变化四元数qk和第k时刻的系统四元数pk计算t+1时刻的系统四元数，具体公式为：
Figure BDA0003384563540000083
上式(3)表述的是两个四元数相乘的运算，四元数相乘的法则见下式(4)：
Figure BDA0003384563540000084
其中，
Figure BDA0003384563540000091
Figure BDA0003384563540000092
速度修正：
根据公式(5)对第k时刻的速度进行修正获得第k+1时刻速度，具体公式为：
Figure BDA0003384563540000093
其中，
Figure BDA0003384563540000094
表示姿态矩阵，G表示重力矩阵，G＝[00g]T，g表示加速度，fn表示导航系下的加速度，fb表示载体系下加速度计的输出，b表示载体坐标系；n表示导航坐标系，vk表示当前k时刻导航系下的北东地速度，
Figure BDA0003384563540000095
位置修正：
位置修正时主要是根据当前k时刻更新的速度信息，并结合前一时刻的位置(纬度、经度、高度)信息获取当前k时刻的位置信息，可以将地球等效看成一个球形体，认为其半径为Re＝6378137m,可采用下式完成位置的修正。
根据公式(6)对第k时刻的速度进行修正获得第k+1时刻速度，具体公式为：
Figure BDA0003384563540000101
其中，L表示纬度，λ表示经度，h表示高度，T表示时间周期，v表示速度值
滤波模型:
所述滤波模型是在现有模型基础上增加了失准角、加速度计零偏以及陀螺仪零偏状态量，下面详细介绍该模型：
由于MEMS惯性传感器精度较低，可以忽略地球自转等因素的影响，可以建立卡尔曼滤波器的误差方程如下。状态量选取
Figure BDA0003384563540000106
,其中δρ＝[δL δλ δh]T为位置误差，δν＝[δνn δνe δνd]T为速度误差，Φ＝[Φn Φe Φd]T为失准角，
Figure BDA0003384563540000105
为加速计零偏，ε＝[εx εy εz]T为陀螺仪零偏。
建立误差方程如下：
Figure BDA0003384563540000102
Figure BDA0003384563540000103
Figure BDA0003384563540000104
Figure BDA0003384563540000111
Figure BDA0003384563540000112
可以建立系统的状态方程如下：
Figure BDA0003384563540000113
其中：
Figure BDA0003384563540000114
Figure BDA0003384563540000115
其中：W是系统(包括陀螺仪和加速度计)的噪声向量，为零均值的高斯白噪声序列(服从正态分布)，同样V是系统量测噪声向量，为零均值的高斯白噪声序列(服从正态分布)并且系统噪声向量V和系统量测噪声向量W为互不相关的两组向量，满足下式
Figure BDA0003384563540000116
上式(13)中，若系统的量测信息为外部其他传感器测量的位置和速度信息，可以得到系统的量测矩阵H单位阵，见下式(16)。
Figure BDA0003384563540000121
当K时刻真实的量测ZK到来时，它与量测一步预测
Figure BDA0003384563540000125
之间存在误差，记为：
Figure BDA0003384563540000122
滤波计算：
将卡尔曼滤波计算分为两大步进行描述，即时间预测和滤波更新两个步骤。时间预测阶段完成状态转移矩阵的计算和一步预测均方误差，如下式(18)描述：
Figure BDA0003384563540000123
当外部参考信息(如GNSS位置和速度)更新时即进行一次滤波更新，滤波更新公式见下式(19)描述。
Figure BDA0003384563540000124
其中，K表示卡尔曼增益，R表示传感器的噪声协方差。
经过式(18)和式(19)的计算，完成的状态量X的估计计算和协方差矩阵。
卡尔曼初始化是将误差状态量设置为0，然后利用公式(19)预测更新是更新每帧的误差值，如果当前帧没有观测值即(GNSS/LIDAR值)，则可以根据解算值和误差值计算当前位姿，如果当前帧有观测值，则利用公式(18)进行卡尔曼预测更新获得预测值，然后根据当前观测值和预测值求出融合后的误差值，也可以输出位姿，然后将状态量赋值为0，进行下一轮融合。
三种修正是给出误差状态方程，滤波模型是列出误差状态方程与观测方程，滤波计算是利用卡尔曼将上述两个方程求解得到最优位姿。
实施例2
如图2所示，本发明公开一种多源传感器融合定位的切换系统，所述系统包括：
获取模块201，用于获取第k时刻的卫星导航定位原始数据、速度原始数据、姿态原始数据和激光雷达定位原始数据。
预处理模块202，用于对第k时刻的所述卫星导航定位原始数据、所述速度原始数据、所述姿态原始数据和所述激光雷达定位原始数据分别进行预处理，获得卫星导航定位处理数据、速度处理数据、姿态处理数据和激光雷达定位处理数据。
修正模块203，用于将第k时刻的卫星导航定位处理数据、激光雷达定位处理数据、姿态处理数据和速度处理数据输入至误差滤波模型中进行修正，并确定全球导航卫星系统GNSS和激光雷达是否正常。
切换模块204，用于如果GNSS正常，则切换GNSS，令k＝k+1，并返回“获取模块201”；如果GNSS异常，且激光雷达正常，则切换激光雷达，令k＝k+1，并返回“获取模块201”；如果GNSS和激光雷达均异常，则结束。
下面对各个模块进行详细论述：
作为一种可选的实施方式，本发明所述预处理模块202具体包括：
过滤单元，用于对第k时刻所述卫星导航原始数据进行过滤处理，获得卫星导航定位处理数据。
筛选单元，用于筛选第k时刻所述激光雷达定位原始数据中匹配得分大于3.0的值作为激光雷达定位处理数据。
姿态处理数据确定单元，用于取静止状态下的加速度均值和角速度均值，然后将第k时刻所述姿态原始数据减去加速度均值和角速度均值获得姿态处理数据。
噪声加入单元，用于将第k时刻所述速度原始数据加入噪声，获得速度处理数据；所述速度原始数据包括GNSS对应的速度数据和激光雷达对应的速度数据；所述速度处理数据包括激光雷达对应的速度处理数据和GNSS对应的速度处理数据。
作为一种可选的实施方式，本发明所述修正模块203具体包括：
修正单元，用于将第k时刻的卫星导航定位处理数据、姿态处理数据和GNSS对应的速度处理数据输入至误差滤波模型中进行修正，获得第k+1时刻的卫星导航定位修正数据、GNSS对应的姿态修正数据和GNSS对应的速度修正数据；将第k时刻的激光雷达定位处理数据、姿态处理数据和激光雷达对应的速度处理数据输入至误差滤波模型中进行修正，获得第k+1时刻的激光雷达定位修正数据、激光雷达对应的姿态修正数据和激光雷达对应的速度修正数据。
卡尔曼检测值计算单元，用于根据第k+1时刻的卫星导航定位修正数据、GNSS对应的姿态修正数据和GNSS对应的速度修正数据计算第一卡尔曼检测值；根据第k+1时刻的激光雷达定位修正数据、激光雷达对应的姿态修正数据和激光雷达对应的速度修正数据计算第二卡尔曼检测值。
传感器工作状态判断单元，用于根据第一卡尔曼检测值判断GNSS是否正常；根据第二卡尔曼检测值判断激光雷达是否正常。
作为一种可选的实施方式，本发明所述获取模块201具体包括：
第一获取单元，用于利用全球导航卫星系统GNSS采集第k时刻的卫星导航定位原始数据。
第二获取单元，用于利用里程计采集第k时刻的速度原始数据。
第三获取单元，用于利用惯性测量单元采集第k时刻的姿态原始数据。
第四获取单元，用于利用激光雷达采集第k时刻的激光雷达定位原始数据。
作为一种可选的实施方式，本发明所述卫星导航定位原始数据包括星数、HDOP值、基线长和GNSS定位位置数据。
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (10)

1.一种多源传感器融合定位的切换方法，其特征在于，所述方法包括：
步骤S1：获取第k时刻的卫星导航定位原始数据、速度原始数据、姿态原始数据和激光雷达定位原始数据；
步骤S2：对第k时刻的所述卫星导航定位原始数据、所述速度原始数据、所述姿态原始数据和所述激光雷达定位原始数据分别进行预处理，获得卫星导航定位处理数据、速度处理数据、姿态处理数据和激光雷达定位处理数据；
步骤S3：将第k时刻的卫星导航定位处理数据、激光雷达定位处理数据、姿态处理数据和速度处理数据输入至误差滤波模型中进行修正，并确定全球导航卫星系统GNSS和激光雷达是否正常；
步骤S4：如果GNSS正常，则切换GNSS，令k＝k+1，并返回“步骤S1”；如果GNSS异常，且激光雷达正常，则切换激光雷达，令k＝k+1，并返回“步骤S1”；如果GNSS和激光雷达均异常，则结束。
2.根据权利要求1所述的多源传感器融合定位的切换方法，其特征在于，步骤S2具体包括：
步骤S21：对第k时刻所述卫星导航原始数据进行过滤处理，获得卫星导航定位处理数据；
步骤S22：筛选第k时刻所述激光雷达定位原始数据中匹配得分大于3.0的值作为激光雷达定位处理数据；
步骤S23：取静止状态下的加速度均值和角速度均值，然后将第k时刻所述姿态原始数据减去加速度均值和角速度均值获得姿态处理数据；
步骤S24：将第k时刻所述速度原始数据加入噪声，获得速度处理数据；所述速度原始数据包括GNSS对应的速度数据和激光雷达对应的速度数据；所述速度处理数据包括激光雷达对应的速度处理数据和GNSS对应的速度处理数据。
3.根据权利要求2所述的多源传感器融合定位的切换方法，其特征在于，步骤S3具体包括：
步骤S31：将第k时刻的卫星导航定位处理数据、姿态处理数据和GNSS对应的速度处理数据输入至误差滤波模型中进行修正，获得第k+1时刻的卫星导航定位修正数据、GNSS对应的姿态修正数据和GNSS对应的速度修正数据；将第k时刻的激光雷达定位处理数据、姿态处理数据和激光雷达对应的速度处理数据输入至误差滤波模型中进行修正，获得第k+1时刻的激光雷达定位修正数据、激光雷达对应的姿态修正数据和激光雷达对应的速度修正数据；
步骤S32：根据第k+1时刻的卫星导航定位修正数据、GNSS对应的姿态修正数据和GNSS对应的速度修正数据计算第一卡尔曼检测值；根据第k+1时刻的激光雷达定位修正数据、激光雷达对应的姿态修正数据和激光雷达对应的速度修正数据计算第二卡尔曼检测值；
步骤S33：根据第一卡尔曼检测值判断GNSS是否正常；根据第二卡尔曼检测值判断激光雷达是否正常。
4.根据权利要求1所述的多源传感器融合定位的切换方法，其特征在于，步骤S1具体包括：
步骤S11：利用全球导航卫星系统GNSS采集第k时刻的卫星导航定位原始数据；
步骤S12：利用里程计采集第k时刻的速度原始数据；
步骤S13：利用惯性测量单元采集第k时刻的姿态原始数据；
步骤S14：利用激光雷达采集第k时刻的激光雷达定位原始数据。
5.根据权利要求4所述的多源传感器融合定位的切换方法，其特征在于，所述卫星导航定位原始数据包括星数、HDOP值、基线长和GNSS定位位置数据。
6.一种多源传感器融合定位的切换系统，其特征在于，所述系统包括：
获取模块，用于获取第k时刻的卫星导航定位原始数据、速度原始数据、姿态原始数据和激光雷达定位原始数据；
预处理模块，用于对第k时刻的所述卫星导航定位原始数据、所述速度原始数据、所述姿态原始数据和所述激光雷达定位原始数据分别进行预处理，获得卫星导航定位处理数据、速度处理数据、姿态处理数据和激光雷达定位处理数据；
修正模块，用于将第k时刻的卫星导航定位处理数据、激光雷达定位处理数据、姿态处理数据和速度处理数据输入至误差滤波模型中进行修正，并确定全球导航卫星系统GNSS和激光雷达是否正常；
切换模块，用于如果GNSS正常，则切换GNSS，令k＝k+1，并返回“获取模块”；如果GNSS异常，且激光雷达正常，则切换激光雷达，令k＝k+1，并返回“获取模块”；如果GNSS和激光雷达均异常，则结束。
7.根据权利要求6所述的多源传感器融合定位的切换系统，其特征在于，所述预处理模块具体包括：
过滤单元，用于对第k时刻所述卫星导航原始数据进行过滤处理，获得卫星导航定位处理数据；
筛选单元，用于筛选第k时刻所述激光雷达定位原始数据中匹配得分大于3.0的值作为激光雷达定位处理数据；
姿态处理数据确定单元，用于取静止状态下的加速度均值和角速度均值，然后将第k时刻所述姿态原始数据减去加速度均值和角速度均值获得姿态处理数据；
噪声加入单元，用于将第k时刻所述速度原始数据加入噪声，获得速度处理数据；所述速度原始数据包括GNSS对应的速度数据和激光雷达对应的速度数据；所述速度处理数据包括激光雷达对应的速度处理数据和GNSS对应的速度处理数据。
8.根据权利要求7所述的多源传感器融合定位的切换系统，其特征在于，所述修正模块具体包括：
修正单元，用于将第k时刻的卫星导航定位处理数据、姿态处理数据和GNSS对应的速度处理数据输入至误差滤波模型中进行修正，获得第k+1时刻的卫星导航定位修正数据、GNSS对应的姿态修正数据和GNSS对应的速度修正数据；将第k时刻的激光雷达定位处理数据、姿态处理数据和激光雷达对应的速度处理数据输入至误差滤波模型中进行修正，获得第k+1时刻的激光雷达定位修正数据、激光雷达对应的姿态修正数据和激光雷达对应的速度修正数据；
卡尔曼检测值计算单元，用于根据第k+1时刻的卫星导航定位修正数据、GNSS对应的姿态修正数据和GNSS对应的速度修正数据计算第一卡尔曼检测值；根据第k+1时刻的激光雷达定位修正数据、激光雷达对应的姿态修正数据和激光雷达对应的速度修正数据计算第二卡尔曼检测值；
传感器工作状态判断单元，用于根据第一卡尔曼检测值判断GNSS是否正常；根据第二卡尔曼检测值判断激光雷达是否正常。
9.根据权利要求6所述的多源传感器融合定位的切换系统，其特征在于，所述获取模块具体包括：
第一获取单元，用于利用全球导航卫星系统GNSS采集第k时刻的卫星导航定位原始数据；
第二获取单元，用于利用里程计采集第k时刻的速度原始数据；
第三获取单元，用于利用惯性测量单元采集第k时刻的姿态原始数据；
第四获取单元，用于利用激光雷达采集第k时刻的激光雷达定位原始数据。
10.根据权利要求9所述的多源传感器融合定位的切换系统，其特征在于，所述卫星导航定位原始数据包括星数、HDOP值、基线长和GNSS定位位置数据。
CN202111447901.1A 2021-11-30 2021-11-30 一种多源传感器融合定位的切换方法及系统 Pending CN114137592A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111447901.1A CN114137592A (zh) 2021-11-30 2021-11-30 一种多源传感器融合定位的切换方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111447901.1A CN114137592A (zh) 2021-11-30 2021-11-30 一种多源传感器融合定位的切换方法及系统

Publications (1)

Publication Number Publication Date
CN114137592A true CN114137592A (zh) 2022-03-04

Family

ID=80386361

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111447901.1A Pending CN114137592A (zh) 2021-11-30 2021-11-30 一种多源传感器融合定位的切换方法及系统

Country Status (1)

Country Link
CN (1) CN114137592A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115856979A (zh) * 2023-02-16 2023-03-28 智道网联科技(北京)有限公司 自动驾驶车辆的定位方法、装置及电子设备、存储介质

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115856979A (zh) * 2023-02-16 2023-03-28 智道网联科技(北京)有限公司 自动驾驶车辆的定位方法、装置及电子设备、存储介质

Similar Documents

Publication Publication Date Title
CN111947652B (zh) 一种适用于月球着陆器的惯性/视觉/天文/激光测距组合导航方法
CN111678538B (zh) 一种基于速度匹配的动态水平仪误差补偿方法
Deilamsalehy et al. Sensor fused three-dimensional localization using IMU, camera and LiDAR
CN101413800B (zh) 导航/稳瞄一体化系统的导航、稳瞄方法
Parsa et al. Design and implementation of a mechatronic, all-accelerometer inertial measurement unit
CN106767787A (zh) 一种紧耦合gnss/ins组合导航装置
CN111024070A (zh) 一种基于航向自观测的惯性足绑式行人定位方法
CN107764261B (zh) 一种分布式pos传递对准用模拟数据生成方法和系统
CN109916394A (zh) 一种融合光流位置和速度信息的组合导航算法
Rad et al. Optimal attitude and position determination by integration of INS, star tracker, and horizon sensor
GB2378765A (en) Error compensation in an inertial navigation system
Wang et al. Improving the navigation performance of the MEMS IMU array by precise calibration
Amirsadri et al. Practical considerations in precise calibration of a low-cost MEMS IMU for road-mapping applications
CN108375383A (zh) 多相机辅助的机载分布式pos柔性基线测量方法和装置
Colomina et al. Redundant IMUs for precise trajectory determination
CN114877915B (zh) 一种激光陀螺惯性测量组件g敏感性误差标定装置及方法
Tomaszewski et al. Concept of AHRS algorithm designed for platform independent IMU attitude alignment
CN109916399B (zh) 一种阴影下的载体姿态估计方法
CN102607563B (zh) 利用背景天文信息对于航天器进行相对导航的系统
CN114137592A (zh) 一种多源传感器融合定位的切换方法及系统
RU2683144C1 (ru) Способ определения ошибок ориентации измерительных осей лазерных гироскопов и маятниковых акселерометров в бесплатформенной инерциальной навигационной системе
Spielvogel et al. A stable adaptive attitude estimator on SO (3) for true-North seeking gyrocompass systems: Theory and preliminary simulation evaluation
CN113008229A (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
US20210116247A1 (en) Method for harmonising two inertial measurement units with one another and navigation system implementing this method
Tang et al. An attitude estimate method for fixed-wing UAV s using MEMS/GPS data fusion

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