CN113970332A - 基于无人扫地车融合导航输出结果平滑处理方法及系统 - Google Patents

基于无人扫地车融合导航输出结果平滑处理方法及系统 Download PDF

Info

Publication number
CN113970332A
CN113970332A CN202111449629.0A CN202111449629A CN113970332A CN 113970332 A CN113970332 A CN 113970332A CN 202111449629 A CN202111449629 A CN 202111449629A CN 113970332 A CN113970332 A CN 113970332A
Authority
CN
China
Prior art keywords
data
positioning data
fusion positioning
fusion
value
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
CN202111449629.0A
Other languages
English (en)
Other versions
CN113970332B (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.)
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 CN202111449629.0A priority Critical patent/CN113970332B/zh
Publication of CN113970332A publication Critical patent/CN113970332A/zh
Application granted granted Critical
Publication of CN113970332B publication Critical patent/CN113970332B/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/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
    • 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational 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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
    • G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)
  • Navigation (AREA)

Abstract

本发明涉及一种基于无人扫地车融合导航输出结果平滑处理方法及系统，方法包括：采用卡尔曼滤波算法将GPS原始数据、里程计数据和IMU惯导数据进行融合获得第二融合定位数据；采用卡尔曼滤波算法将激光雷达数据、里程计数据和IMU惯导数据进行融合获得第一融合定位数据；基于数据队列中相邻的第一融合定位数据和第二融合定位数据之间的差计算均值；利用滑动窗将切换点前n/2个融合定位数据减去均值，并替换数据队列中切换点前n/2个融合定位数据，将切换点后n/2个融合定位数据加上均值，并替换数据队列中切换点后n/2个融合定位数据，就保证了数据队列中融合定位数据组成的曲线是平滑的，防止车辆出现晃动。

Description

基于无人扫地车融合导航输出结果平滑处理方法及系统
技术领域
本发明涉及数据平滑处理技术领域，特别是涉及一种基于无人扫地车融合导航输出结果平滑处理方法及系统。
背景技术
在低速无人扫地车多传感器融合导航中，由于不同传感器在融合过程中存在偏差，如果偏差较大，且符合定位结果，则在直接输出中就会出现车辆的严重晃动。比如gps和激光雷达同时发布位置信息坐标x、y、z和航向信息，当以gps为主时，在路过高楼之间时，gps信号丢失，需要切换到以激光雷达定位，但是由于激光雷达定位技术的限制，其与gps坐标系会稍微有点偏差，这样在切换的过程中车定位坐标会有一个跳变，导致车辆出现晃动。另外，现有技术方案并未公开如何解决上述晃动，进而实现平滑处理，因此本领域亟需解决因切换传感器而导致的车辆晃动问题。
发明内容
本发明的目的是提供一种基于无人扫地车融合导航输出结果平滑处理方法及系统，以实现导航输出结果平滑处理。
为实现上述目的，本发明提供了一种基于无人扫地车融合导航输出结果平滑处理方法，所述方法包括：
步骤S1：获取激光雷达数据、里程计数据、IMU惯导数据和GPS原始数据；
步骤S2：以GPS原始数据为观测数据，采用卡尔曼滤波算法将所述GPS原始数据、所述里程计数据和所述IMU惯导数据进行融合，获得第二融合定位数据；
步骤S3：以激光雷达数据为观测数据，采用卡尔曼滤波算法将激光雷达数据、所述里程计数据和所述IMU惯导数据进行融合，获得第一融合定位数据；
步骤S4：将所述第一融合定位数据或所述第二融合定位数据存入数据队列中；
步骤S5：基于所述数据队列中相邻的所述第一融合定位数据和所述第二融合定位数据之间的差计算均值；
步骤S6：利用滑动窗将切换点前n/2个融合定位数据减去均值，并将减去均值的融合定位数据替换数据队列中切换点前n/2个融合定位数据，将切换点后n/2个融合定位数据加上均值，并将加上均值的融合定位数据替换数据队列中切换点后n/2个融合定位数据；n为大于0的偶数；相邻的两个融合定位数据包括第一融合定位数据和第二融合定位数据，则说明此时存在切换点。
可选地，所述方法还包括：
步骤S7：取出数据队列中前m个融合定位数据进行发布，并补充m个融合定位数据；其中，m为大于等于1的正整数。
可选地，所述基于所述数据队列中相邻的所述第一融合定位数据和所述第二融合定位数据之间的差计算均值，具体包括：
步骤S51：计算所述数据队列中相邻的所述第一融合定位数据和所述第二融合定位数据之间的差，获得偏差值；
步骤S52：判断所述偏差值是否满足第一设定条件；如果满足第一设定条件，则令n＝10，将所述偏差值分成n份获得的均值；如果不满足第一设定条件，则判断所述偏差值是否满足第二设定条件；如果满足第二设定条件，则令n＝30，将所述偏差值分成n份获得的均值；如果不满足第二设定条件，则判断所述偏差值是否满足第三设定条件；如果满足第三设定条件，则令n＝50，将所述偏差值分成n份获得的均值。
可选地，所述将所述第一融合定位数据或所述第二融合定位数据存入数据队列中，具体包括：
根据所述第二融合定位数据内的标志位判断是否为正常可用值；如果所述第二融合定位数据内的标志位为正常可用值，则将所述第二融合定位数据存入数据队列中；如果所述第二融合定位数据内的标志位为不可用值，则根据所述第一融合定位数据内的标志位判断是否为正常可用值；如果所述第一融合定位数据内的标志位判断为正常可用值，则将所述第一融合定位数据存入数据队列中；如果所述第一融合定位数据内的标志位判断为不可用值，则结束。
本发明还公开一种基于无人扫地车融合导航输出结果平滑处理系统，所述系统包括：
获取模块，用于获取激光雷达数据、里程计数据、IMU惯导数据和GPS原始数据；
第二融合定位数据确定模块，用于以GPS原始数据为观测数据，采用卡尔曼滤波算法将所述GPS原始数据、所述里程计数据和所述IMU惯导数据进行融合，获得第二融合定位数据；
第一融合定位数据确定模块，用于以激光雷达数据为观测数据，采用卡尔曼滤波算法将激光雷达数据、所述里程计数据和所述IMU惯导数据进行融合，获得第一融合定位数据；
数据存入模块，用于将所述第一融合定位数据或所述第二融合定位数据存入数据队列中；
均值计算模块，用于基于所述数据队列中相邻的所述第一融合定位数据和所述第二融合定位数据之间的差计算均值；
平滑处理模块，用于利用滑动窗将切换点前n/2个融合定位数据减去均值，并将减去均值的融合定位数据替换数据队列中切换点前n/2个融合定位数据，将切换点后n/2个融合定位数据加上均值，并将加上均值的融合定位数据替换数据队列中切换点后n/2个融合定位数据；n为大于0的偶数；相邻的两个融合定位数据包括第一融合定位数据和第二融合定位数据，则说明此时存在切换点。
可选地，所述系统还包括：
数据更新模块，用于取出数据队列中前m个融合定位数据进行发布，并补充m个融合定位数据；其中，m为大于等于1的正整数。
可选地，所述均值计算模块，具体包括：
偏差值计算单元，用于计算所述数据队列中相邻的所述第一融合定位数据和所述第二融合定位数据之间的差，获得偏差值；
判断单元，用于判断所述偏差值是否满足第一设定条件；如果满足第一设定条件，则令n＝10，将所述偏差值分成n份获得的均值；如果不满足第一设定条件，则判断所述偏差值是否满足第二设定条件；如果满足第二设定条件，则令n＝30，将所述偏差值分成n份获得的均值；如果不满足第二设定条件，则判断所述偏差值是否满足第三设定条件；如果满足第三设定条件，则令n＝50，将所述偏差值分成n份获得的均值。
可选地，所述数据存入模块，具体包括：
根据所述第二融合定位数据内的标志位判断是否为正常可用值；如果所述第二融合定位数据内的标志位为正常可用值，则将所述第二融合定位数据存入数据队列中；如果所述第二融合定位数据内的标志位为不可用值，则根据所述第一融合定位数据内的标志位判断是否为正常可用值；如果所述第一融合定位数据内的标志位判断为正常可用值，则将所述第一融合定位数据存入数据队列中；如果所述第一融合定位数据内的标志位判断为不可用值，则结束。
根据本发明提供的具体实施例，本发明公开了以下技术效果：
本发明涉及一种基于无人扫地车融合导航输出结果平滑处理方法及系统，方法包括：采用卡尔曼滤波算法将GPS原始数据、里程计数据和IMU惯导数据进行融合获得第二融合定位数据；采用卡尔曼滤波算法将激光雷达数据、里程计数据和IMU惯导数据进行融合获得第一融合定位数据；基于数据队列中相邻的第一融合定位数据和第二融合定位数据之间的差计算均值；利用滑动窗将切换点前n/2个融合定位数据减去均值，并替换数据队列中切换点前n/2个融合定位数据，将切换点后n/2个融合定位数据加上均值，并替换数据队列中切换点后n/2个融合定位数据，就保证了数据队列中融合定位数据组成的曲线是平滑的，防止车辆出现晃动。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明基于无人扫地车融合导航输出结果平滑处理方法流程图；
图2为本发明数据队列中存入的融合定位数据构成的曲线示意图；
图3为本发明求取均值示意图；
图4为本发明基于无人扫地车融合导航输出结果平滑处理系统结构图；
图5为原始没有进行平滑的定位曲线示意图；
图6为本发明进行平滑处理的定位曲线示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明的目的是提供一种基于无人扫地车融合导航输出结果平滑处理方法及系统，以实现导航输出结果平滑处理。
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
实施例1
如图1所示，本发明公开一种基于无人扫地车融合导航输出结果平滑处理方法，所述方法包括：
步骤S1：获取激光雷达数据、里程计数据、IMU惯导数据和GPS原始数据。
步骤S2：以GPS原始数据为观测数据，采用卡尔曼滤波算法将所述GPS原始数据、所述里程计数据和所述IMU惯导数据进行融合，获得第二融合定位数据。
步骤S3：以激光雷达数据为观测数据，采用卡尔曼滤波算法将激光雷达数据、所述里程计数据和所述IMU惯导数据进行融合，获得第一融合定位数据。
步骤S4：将所述第一融合定位数据或所述第二融合定位数据存入数据队列中。
步骤S5：基于所述数据队列中相邻的所述第一融合定位数据和所述第二融合定位数据之间的差计算均值。
步骤S6：利用滑动窗将切换点前n/2个融合定位数据减去均值，并将减去均值的融合定位数据替换数据队列中切换点前n/2个融合定位数据，将切换点后n/2个融合定位数据加上均值，并将加上均值的融合定位数据替换数据队列中切换点后n/2个融合定位数据；n为大于0的偶数；相邻的两个融合定位数据包括第一融合定位数据和第二融合定位数据，则说明此时存在切换点。
下面对各个步骤进行详细论述：
步骤S1：获取激光雷达数据、里程计数据、IMU惯导数据和GPS原始数据；所述IMU惯导数据包括车辆加速度和角加速度信息，所述里程计数据为车辆的速度信息。
步骤S2：以GPS原始数据为观测数据，采用卡尔曼滤波算法将所述GPS原始数据、所述里程计数据和所述IMU惯导数据进行融合，获得第二融合定位数据；初始时刻的位姿数据必然为GPS原始数据；因为本发明设置GPS优先级高于激光雷达。具体生成第二融合定位数据的方式与生成第一融合定位数据的方式相同，在此不再一一赘述。
步骤S3：以激光雷达数据为观测数据，采用卡尔曼滤波算法将激光雷达数据、所述里程计数据和所述IMU惯导数据进行融合，获得第一融合定位数据，具体包括：
步骤S31：采用ndt算法将所述激光雷达数据转换到84坐标系下，获得第一位姿数据；所述第一位姿数据包括航向角和位置信息。
步骤S32：根据当前时刻的位姿数据、所述里程计数据和所述IMU惯导数据进行推算，获得预测位姿数据L。
步骤S33：根据L′＝FL+u确定下一时刻的位姿数据，其中，L′表示下一时刻的位姿数据，F为系统状态方程，u为系统噪声。
步骤S34：根据W＝N-HL′确定测量余量，其中，W为测量余量，N为激光雷达数据通过ndt算法转换到84坐标系下获得第一位姿数据，H为测量矩阵。
步骤S35：根据
Figure BDA0003384949380000061
计算卡尔曼增益，其中，P′为预测协方差矩阵，H为测量矩阵，R为观测噪声，K为卡尔曼增益，F为系统状态方程，Q为过程噪音，P为状态协方差矩阵。
步骤S36：根据L″＝L′+KW计算第一融合定位数据，其中，L″为第一融合定位数据。
步骤S4：将所述第一融合定位数据或所述第二融合定位数据存入数据队列中，具体包括：
根据所述第二融合定位数据内的标志位判断是否为正常可用值；如果所述第二融合定位数据内的标志位为正常可用值，则将所述第二融合定位数据存入数据队列中；如果所述第二融合定位数据内的标志位为不可用值，则根据所述第一融合定位数据内的标志位判断是否为正常可用值；如果所述第一融合定位数据内的标志位判断为正常可用值，则将所述第一融合定位数据存入数据队列中；如果所述第一融合定位数据内的标志位判断为不可用值，则结束。数据队列中存入的融合定位数据构成的曲线如图2所示。本发明公开设定的正常可用值与不可用值是在实验之前进行设定的。
步骤S5：基于所述数据队列中相邻的所述第一融合定位数据和所述第二融合定位数据之间的差计算均值，具体包括：
步骤S51：计算所述数据队列中相邻的所述第一融合定位数据和所述第二融合定位数据之间的差，获得偏差值。在两组数据切换的过程中，由于两者坐标系存在偏差，所以同一个切换的位置点会产生一个偏差值delt_A，偏差值delt_A＝(delt_x,del_y,delt_z,delt_yaw)，其中，delt_x、del_y、delt_z分别表示三个方向上的位置偏差值，单位为米，delt_yaw表示航向角偏差，单位度。
步骤S52：判断所述偏差值是否满足第一设定条件；如果满足第一设定条件，则令n＝10，将所述偏差值分成n份获得的均值；如果不满足第一设定条件，则判断所述偏差值是否满足第二设定条件；如果满足第二设定条件，则令n＝30，将所述偏差值分成n份获得的均值；如果不满足第二设定条件，则判断所述偏差值是否满足第三设定条件；如果满足第三设定条件，则令n＝50，将所述偏差值分成n份获得的均值；如果不满足第三设定条件，则定位失败，结束，具体如图3所示。第一设定条件为delt_x<0.1m，delt_y<0.1m，delt_z<0.1m且delt_yaw<5°；第二设定条件为0.1m≤delt_x<0.3m，0.1m≤delt_y<0.3m，0.1m≤delt_z<0.3m且5°≤delt_yaw<10°；第三设定条件为0.3m≤delt_x<0.5m，0.3m≤delt_y<0.5m，0.3m≤delt_z<0.5m且10°≤delt_yaw<15°。
将所述偏差值分成n份获得均值，具体公式为：avg_data(avg_x,avg_y,avg_z,avg_yaw)＝delt_A(delt_x,delt_y,delt_z，delt_yaw)/n；其中，avg_data为将所述偏差值delt_A分成n份后获得的均值，avg_x、avg_y、avg_z分别为三个方向上的位置均值，delt_x、delt_y、delt_z分别为三个不同方向上的位置偏差值，avg_yaw、delt_yaw分别为航向角均值和航向角偏差值，avg_x、avg_y、avg_z、delt_x、delt_y、delt_z单位均为米，avg_yaw、delt_yaw的单位均为度。
步骤S6：利用滑动窗将切换点前n/2个融合定位数据减去均值，并将减去均值的融合定位数据替换数据队列中切换点前n/2个融合定位数据，将切换点后n/2个融合定位数据加上均值，并将加上均值的融合定位数据替换数据队列中切换点后n/2个融合定位数据；当切换点前融合定位数据为第一融合定位数据时，则切换点后的融合定位数据为第二融合定位数据；或者当切换点前融合定位数据为第二融合定位数据时，则切换点后的融合定位数据为第一融合定位数据；相邻的两个融合定位数据包括第一融合定位数据和第二融合定位数据，则说明此时存在切换点。本发明利用滑动窗将切换点前n/2个融合定位数据减去均值，将切换点后n/2个融合定位数据加上均值，这样就保证了在组合导航不同定位方式切换时产生的误差平均分到了n组融合结果中，也就保证了数据队列output_data中定位数据组成的曲线是平滑的，没有跳变。
步骤S7：取出数据队列中前m个融合定位数据进行发布，并补充m个融合定位数据，保证数据队列中包括200个融合定位数据，这样往复循环；由于队列中定位数据的曲线是经过本专利算法进行平滑的，所以就保证了发布定位结果的曲线也是平滑的，从而保证了车辆行驶的平稳性。
本发明将公开的平滑处理方法应用到无人机驾驶扫地车上，就可以实现无人驾驶扫地车在高楼、隧道等无gps信号的区域到空旷广场等有gps信号的区域实现平稳的运行。
实施例2
如图4所示，本发明还公开一种基于无人扫地车融合导航输出结果平滑处理系统，所述系统包括：
获取模块401，用于获取激光雷达数据、里程计数据、IMU惯导数据和GPS原始数据。
第二融合定位数据确定模块402，用于以GPS原始数据为观测数据，采用卡尔曼滤波算法将所述GPS原始数据、所述里程计数据和所述IMU惯导数据进行融合，获得第二融合定位数据。
第一融合定位数据确定模块403，用于以激光雷达数据为观测数据，采用卡尔曼滤波算法将激光雷达数据、所述里程计数据和所述IMU惯导数据进行融合，获得第一融合定位数据。
数据存入模块404，用于将所述第一融合定位数据或所述第二融合定位数据存入数据队列中。
均值计算模块405，用于基于所述数据队列中相邻的所述第一融合定位数据和所述第二融合定位数据之间的差计算均值。
平滑处理模块406，用于利用滑动窗将切换点前n/2个融合定位数据减去均值，并将减去均值的融合定位数据替换数据队列中切换点前n/2个融合定位数据，将切换点后n/2个融合定位数据加上均值，并将加上均值的融合定位数据替换数据队列中切换点后n/2个融合定位数据；n为大于0的偶数；相邻的两个融合定位数据包括第一融合定位数据和第二融合定位数据，则说明此时存在切换点。
数据更新模块407，用于取出数据队列中前m个融合定位数据进行发布，并补充m个融合定位数据；其中，m为大于等于1的正整数。
作为一种可选的实施方式，本发明所述均值计算模块405，具体包括：
偏差值计算单元，用于计算所述数据队列中相邻的所述第一融合定位数据和所述第二融合定位数据之间的差，获得偏差值。
判断单元，用于判断所述偏差值是否满足第一设定条件；如果满足第一设定条件，则令n＝10，将所述偏差值分成n份获得的均值；如果不满足第一设定条件，则判断所述偏差值是否满足第二设定条件；如果满足第二设定条件，则令n＝30，将所述偏差值分成n份获得的均值；如果不满足第二设定条件，则判断所述偏差值是否满足第三设定条件；如果满足第三设定条件，则令n＝50，将所述偏差值分成n份获得的均值。
作为一种可选的实施方式，本发明所述数据存入模块404，具体包括：
根据所述第二融合定位数据内的标志位判断是否为正常可用值；如果所述第二融合定位数据内的标志位为正常可用值，则将所述第二融合定位数据存入数据队列中；如果所述第二融合定位数据内的标志位为不可用值，则根据所述第一融合定位数据内的标志位判断是否为正常可用值；如果所述第一融合定位数据内的标志位判断为正常可用值，则将所述第一融合定位数据存入数据队列中；如果所述第一融合定位数据内的标志位判断为不可用值，则结束。
与实施例1相同的部分在此不再一一赘述，具体参考实施例1。
实施例3
采用本发明公开的方法进行验证，将不加平滑算法直接将定位结果输出，则车辆在gnss、lidar、里程计辅助路用惯导融合推算的部分就会出现左右摆动的情况，如果融合偏差继续扩大，可能导致车辆定位丢失，更严重的情况可能导致撞车。如图5为原始没有进行平滑的定位曲线，图6为进行平滑处理后的曲线，转弯处有gnss和lidar导航方式的切换，这证明本发明对曲线进行平滑处理效果明显，且是成功的。
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (8)

1.一种基于无人扫地车融合导航输出结果平滑处理方法，其特征在于，所述方法包括：
步骤S1：获取激光雷达数据、里程计数据、IMU惯导数据和GPS原始数据；
步骤S2：以GPS原始数据为观测数据，采用卡尔曼滤波算法将所述GPS原始数据、所述里程计数据和所述IMU惯导数据进行融合，获得第二融合定位数据；
步骤S3：以激光雷达数据为观测数据，采用卡尔曼滤波算法将激光雷达数据、所述里程计数据和所述IMU惯导数据进行融合，获得第一融合定位数据；
步骤S4：将所述第一融合定位数据或所述第二融合定位数据存入数据队列中；
步骤S5：基于所述数据队列中相邻的所述第一融合定位数据和所述第二融合定位数据之间的差计算均值；
步骤S6：利用滑动窗将切换点前n/2个融合定位数据减去均值，并将减去均值的融合定位数据替换数据队列中切换点前n/2个融合定位数据，将切换点后n/2个融合定位数据加上均值，并将加上均值的融合定位数据替换数据队列中切换点后n/2个融合定位数据；n为大于0的偶数；相邻的两个融合定位数据包括第一融合定位数据和第二融合定位数据，则说明此时存在切换点。
2.根据权利要求1所述的基于无人扫地车融合导航输出结果平滑处理方法，其特征在于，所述方法还包括：
步骤S7：取出数据队列中前m个融合定位数据进行发布，并补充m个融合定位数据；其中，m为大于等于1的正整数。
3.根据权利要求1所述的基于无人扫地车融合导航输出结果平滑处理方法，其特征在于，所述基于所述数据队列中相邻的所述第一融合定位数据和所述第二融合定位数据之间的差计算均值，具体包括：
步骤S51：计算所述数据队列中相邻的所述第一融合定位数据和所述第二融合定位数据之间的差，获得偏差值；
步骤S52：判断所述偏差值是否满足第一设定条件；如果满足第一设定条件，则令n＝10，将所述偏差值分成n份获得的均值；如果不满足第一设定条件，则判断所述偏差值是否满足第二设定条件；如果满足第二设定条件，则令n＝30，将所述偏差值分成n份获得的均值；如果不满足第二设定条件，则判断所述偏差值是否满足第三设定条件；如果满足第三设定条件，则令n＝50，将所述偏差值分成n份获得的均值。
4.根据权利要求1所述的基于无人扫地车融合导航输出结果平滑处理方法，其特征在于，所述将所述第一融合定位数据或所述第二融合定位数据存入数据队列中，具体包括：
根据所述第二融合定位数据内的标志位判断是否为正常可用值；如果所述第二融合定位数据内的标志位为正常可用值，则将所述第二融合定位数据存入数据队列中；如果所述第二融合定位数据内的标志位为不可用值，则根据所述第一融合定位数据内的标志位判断是否为正常可用值；如果所述第一融合定位数据内的标志位判断为正常可用值，则将所述第一融合定位数据存入数据队列中；如果所述第一融合定位数据内的标志位判断为不可用值，则结束。
5.一种基于无人扫地车融合导航输出结果平滑处理系统，其特征在于，所述系统包括：
获取模块，用于获取激光雷达数据、里程计数据、IMU惯导数据和GPS原始数据；
第二融合定位数据确定模块，用于以GPS原始数据为观测数据，采用卡尔曼滤波算法将所述GPS原始数据、所述里程计数据和所述IMU惯导数据进行融合，获得第二融合定位数据；
第一融合定位数据确定模块，用于以激光雷达数据为观测数据，采用卡尔曼滤波算法将激光雷达数据、所述里程计数据和所述IMU惯导数据进行融合，获得第一融合定位数据；
数据存入模块，用于将所述第一融合定位数据或所述第二融合定位数据存入数据队列中；
均值计算模块，用于基于所述数据队列中相邻的所述第一融合定位数据和所述第二融合定位数据之间的差计算均值；
平滑处理模块，用于利用滑动窗将切换点前n/2个融合定位数据减去均值，并将减去均值的融合定位数据替换数据队列中切换点前n/2个融合定位数据，将切换点后n/2个融合定位数据加上均值，并将加上均值的融合定位数据替换数据队列中切换点后n/2个融合定位数据；n为大于0的偶数；相邻的两个融合定位数据包括第一融合定位数据和第二融合定位数据，则说明此时存在切换点。
6.根据权利要求5所述的基于无人扫地车融合导航输出结果平滑处理系统，其特征在于，所述系统还包括：
数据更新模块，用于取出数据队列中前m个融合定位数据进行发布，并补充m个融合定位数据；其中，m为大于等于1的正整数。
7.根据权利要求5所述的基于无人扫地车融合导航输出结果平滑处理系统，其特征在于，所述均值计算模块，具体包括：
偏差值计算单元，用于计算所述数据队列中相邻的所述第一融合定位数据和所述第二融合定位数据之间的差，获得偏差值；
判断单元，用于判断所述偏差值是否满足第一设定条件；如果满足第一设定条件，则令n＝10，将所述偏差值分成n份获得的均值；如果不满足第一设定条件，则判断所述偏差值是否满足第二设定条件；如果满足第二设定条件，则令n＝30，将所述偏差值分成n份获得的均值；如果不满足第二设定条件，则判断所述偏差值是否满足第三设定条件；如果满足第三设定条件，则令n＝50，将所述偏差值分成n份获得的均值。
8.根据权利要求5所述的基于无人扫地车融合导航输出结果平滑处理系统，其特征在于，所述数据存入模块，具体包括：
根据所述第二融合定位数据内的标志位判断是否为正常可用值；如果所述第二融合定位数据内的标志位为正常可用值，则将所述第二融合定位数据存入数据队列中；如果所述第二融合定位数据内的标志位为不可用值，则根据所述第一融合定位数据内的标志位判断是否为正常可用值；如果所述第一融合定位数据内的标志位判断为正常可用值，则将所述第一融合定位数据存入数据队列中；如果所述第一融合定位数据内的标志位判断为不可用值，则结束。
CN202111449629.0A 2021-11-30 2021-11-30 基于无人扫地车融合导航输出结果平滑处理方法及系统 Active CN113970332B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111449629.0A CN113970332B (zh) 2021-11-30 2021-11-30 基于无人扫地车融合导航输出结果平滑处理方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111449629.0A CN113970332B (zh) 2021-11-30 2021-11-30 基于无人扫地车融合导航输出结果平滑处理方法及系统

Publications (2)

Publication Number Publication Date
CN113970332A true CN113970332A (zh) 2022-01-25
CN113970332B CN113970332B (zh) 2024-04-19

Family

ID=79590446

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111449629.0A Active CN113970332B (zh) 2021-11-30 2021-11-30 基于无人扫地车融合导航输出结果平滑处理方法及系统

Country Status (1)

Country Link
CN (1) CN113970332B (zh)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110006421A (zh) * 2019-03-27 2019-07-12 湖北三江航天万峰科技发展有限公司 一种基于mems和gps的车载导航方法及系统
CN110906923A (zh) * 2019-11-28 2020-03-24 重庆长安汽车股份有限公司 车载多传感器紧耦合融合定位方法、系统、存储介质及车辆
US20200200920A1 (en) * 2018-12-19 2020-06-25 Uber Technologies, Inc. Inferring Vehicle Location and Movement Using Sensor Data Fusion
US20210156713A1 (en) * 2018-08-06 2021-05-27 Tencent Technology (Shenzhen) Company Ltd Method and apparatus for reconstructing motion track, storage medium, and electronic device
CN112946681A (zh) * 2021-05-17 2021-06-11 知行汽车科技（苏州）有限公司 融合组合导航信息的激光雷达定位方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20210156713A1 (en) * 2018-08-06 2021-05-27 Tencent Technology (Shenzhen) Company Ltd Method and apparatus for reconstructing motion track, storage medium, and electronic device
US20200200920A1 (en) * 2018-12-19 2020-06-25 Uber Technologies, Inc. Inferring Vehicle Location and Movement Using Sensor Data Fusion
CN110006421A (zh) * 2019-03-27 2019-07-12 湖北三江航天万峰科技发展有限公司 一种基于mems和gps的车载导航方法及系统
CN110906923A (zh) * 2019-11-28 2020-03-24 重庆长安汽车股份有限公司 车载多传感器紧耦合融合定位方法、系统、存储介质及车辆
CN112946681A (zh) * 2021-05-17 2021-06-11 知行汽车科技（苏州）有限公司 融合组合导航信息的激光雷达定位方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
刘钢等: "一种新型的GPS/DR组合定位系统数据融合算法", 计算机仿真, vol. 24, no. 01, 20 January 2007 (2007-01-20), pages 117 - 120 *

Also Published As

Publication number Publication date
CN113970332B (zh) 2024-04-19

Similar Documents

Publication Publication Date Title
CN113945206A (zh) 一种基于多传感器融合的定位方法及装置
CN108180921B (zh) 利用gps数据的ar-hud导航系统及其导航方法
CN107328411A (zh) 车载定位系统和自动驾驶车辆
CN107328410A (zh) 用于定位自动驾驶车辆的方法和汽车电脑
CN108981687B (zh) 一种视觉与惯性融合的室内定位方法
CN111324848B (zh) 移动激光雷达测量系统车载轨迹数据优化方法
CN114636993A (zh) 一种激光雷达与imu的外参标定方法、装置及设备
CN113470089B (zh) 一种基于三维点云的跨域协同定位和建图方法及系统
CN113899363B (zh) 车辆的定位方法、装置及自动驾驶车辆
CN111915675B (zh) 基于粒子漂移的粒子滤波点云定位方法及其装置和系统
CN112415558B (zh) 行进轨迹的处理方法及相关设备
CN103344260A (zh) 基于rbckf的捷联惯导系统大方位失准角初始对准方法
CN112284376A (zh) 一种基于多传感器融合的移动机器人室内定位建图方法
CN112835064B (zh) 一种建图定位方法、系统、终端及介质
Chen et al. A novel fusion methodology to bridge GPS outages for land vehicle positioning
CN104697520A (zh) 一体化无陀螺捷联惯导系统与gps系统组合导航方法
CN114323050A (zh) 车辆定位方法、装置和电子设备
CN113984044A (zh) 一种基于车载多感知融合的车辆位姿获取方法及装置
CN109490931A (zh) 飞行定位方法、装置及无人机
CN112154303B (zh) 高精度地图定位方法、系统、平台及计算机可读存储介质
CN111882494B (zh) 位姿图处理方法、装置、计算机设备和存储介质
CN112154355B (zh) 高精度地图定位方法、系统、平台及计算机可读存储介质
CN113970332A (zh) 基于无人扫地车融合导航输出结果平滑处理方法及系统
CN110873577A (zh) 一种水下快速动基座对准方法及装置
CN113155156A (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