CN116295345A - 存在光学干扰环境下的定位方法、装置、设备、介质 - Google Patents

存在光学干扰环境下的定位方法、装置、设备、介质 Download PDF

Info

Publication number
CN116295345A
CN116295345A CN202310258499.5A CN202310258499A CN116295345A CN 116295345 A CN116295345 A CN 116295345A CN 202310258499 A CN202310258499 A CN 202310258499A CN 116295345 A CN116295345 A CN 116295345A
Authority
CN
China
Prior art keywords
data
laser
dynamic data
pose
time stamp
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
CN202310258499.5A
Other languages
English (en)
Other versions
CN116295345B (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 Zhierxing Technology Co ltd
Original Assignee
Shanghai Zhierxing 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 Zhierxing Technology Co ltd filed Critical Shanghai Zhierxing Technology Co ltd
Priority to CN202310258499.5A priority Critical patent/CN116295345B/zh
Publication of CN116295345A publication Critical patent/CN116295345A/zh
Application granted granted Critical
Publication of CN116295345B publication Critical patent/CN116295345B/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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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/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/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • 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
    • 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/53—Determining attitude
    • 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
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/497—Means for monitoring or calibrating
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
    • Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
    • Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]

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)
  • Navigation (AREA)

Abstract

本申请提供一种存在光学干扰环境下的定位方法、装置、设备、介质，应用于自动化机械的智能融合定位技术领域。方法，包括：建立目标区域的离线环境地图和干扰识别模型；使用全局位姿数据和惯性动态数据初始化系统滤波器，将GNSS的时间戳设置为第一时间戳；对于实时获取的激光数据帧使用干扰识别模型进行过滤，得到激光有效数据帧；基于全局位姿数据，将激光有效数据帧与离线环境地图进行匹配，得到第一时间戳时刻下的激光位姿数据和激光动态数据；根据测量滤波器的测量位姿数据、测量动态数据和第二时间戳，更新系统滤波器，得到自动化机械的自身位姿数据和自身动态数据，可以得到稳定准确的定位。

Description

存在光学干扰环境下的定位方法、装置、设备、介质
技术领域
本申请涉及自动化机械的智能融合定位技术领域，尤其涉及一种用于室外作业或室内作业的自动化机械的智能融合定位领域，具体涉及一种存在光学干扰环境下的定位方法、装置、设备、介质。
背景技术
自动化机械经常需要在多种环境下进行室内或室外的自主作业，通过配备光学传感器和惯性传感器后，可借助光学定位和惯性动态反馈，计算自身的实时位姿。
但是，基于激光点云的定位易受作业环境影响，例如，气候或其他因素常会对自动化机械的环境感知产生光学干扰，使激光匹配定位的误差增大。并且，惯性动态可用于短时间的位姿计算，无法单独提供长时间的准确位姿。
因此，需要一种新的在存在光学干扰环境下的实现稳定定位的技术方案。
发明内容
有鉴于此，本说明书实施例提供一种存在光学干扰环境下的定位方法、装置、设备、介质，以解决现有技术中的定位方法在存在光学干扰时定位不准确的技术问题。
本说明书实施例提供以下技术方案：
本说明书实施例提供一种存在光学干扰环境下的定位方法，适用于自动化机械，包括：
建立目标区域的离线环境地图和干扰识别模型，干扰识别模型用于对目标区域中由于光学干扰产生的干扰数据进行过滤；
在进行定位时，使用全球导航卫星系统提供的全局位姿数据和IMU传感器的惯性动态数据初始化系统滤波器，得到系统滤波器内的初始系统位姿数据和初始系统动态数据，同时将全球导航卫星系统的时间戳设置为第一时间戳；
对于实时获取的激光数据帧使用干扰识别模型进行干扰数据过滤，得到激光有效数据帧；
基于全局位姿数据，将激光有效数据帧与离线环境地图进行匹配，得到第一时间戳时刻下的激光位姿数据和激光动态数据；
根据测量滤波器的测量位姿数据、测量动态数据和第二时间戳，与激光位姿数据、激光动态数据和第一时间戳，更新系统滤波器，得到更新系统位姿数据和更新系统动态数据和第三时间戳，测量滤波器预先设置在自动化机械上，第三时间戳等于第一时间戳或者第二时间戳；
根据更新系统位姿数据、系统动态数据和第三时间戳，得到自动化机械的自身位姿数据和自身动态数据；
根据自身位姿数据和自身动态数据和第三时间戳，得到当前时刻的预测系统位姿数据。
在一种优选的技术方案中，建立目标区域的离线环境地图和干扰识别模型，包括：
通过获取的目标区域的激光点云数据和惯性数据，建立目标区域的离线环境地图，惯性数据是通过自动化机械上配备的惯性传感器采集的；
通过采集的目标区域内发生光学干扰时的场景数据训练学习模型，得到干扰识别模型，场景数据包括干扰数据和非干扰数据。
在一种优选的技术方案中，根据测量滤波器的测量位姿数据、测量动态数据和第二时间戳，与激光位姿数据、激光动态数据和第一时间戳，更新系统滤波器，得到更新系统位姿数据和更新系统动态数据和第三时间戳，包括：
根据测量滤波器的测量位姿数据、测量动态数据和第二时间戳，确定第一时间戳时刻下的是激光位姿数据和激光动态数据是否有效；
若有效，则将第一时间戳、激光位姿数据和激光动态数据输入到系统滤波器内，更新系统滤波器，得到更新系统位姿数据和更新系统动态数据，并将第一时间戳设置为第三时间戳；
若无效，则将测量滤波器的测量位姿数据和测量动态数据输入到系统滤波器内，更新系统滤波器，得到更新系统位姿数据和更新系统动态数据，并将第二时间戳设置为第三时间戳。
在一种优选的技术方案中，根据测量滤波器的测量位姿数据、测量动态数据和第二时间戳，确定第一时间戳时刻下的是激光位姿数据和激光动态数据是否有效，包括：
根据第一时间戳与第二时间戳的时间间隔，以及测量位姿数据和测量动态数据，得到预测测量位姿数据和预测测量动态数据；
将预测测量位姿数据和预测测量动态数据分别与激光位姿数据和激光动态数据进行比较，得到比较结果；
根据比较结果和预设阈值，确定是否有效。
在一种优选的技术方案中，若有效，则将第一时间戳、激光位姿数据和激光动态数据输入到系统滤波器内，更新系统滤波器，得到更新系统位姿数据和更新系统动态数据，并将第一时间戳设置为第三时间戳，包括：
将第一时间戳、激光位姿数据和激光动态数据，分别赋值给第三时间戳、测量位姿数据和测量动态数据；
将第三时间戳、激光位姿数据和激光动态数据输入到系统滤波器内，得到系统滤波器在第三时间戳时刻的更新系统位姿数据、更新系统动态数据和不确定度；
以第三时间戳、激光位姿数据和激光动态数据为基础，累加惯性动态数据对时间的积分，直到获取到下一帧激光数据帧时，作为激光有效数据帧，与离线环境地图进行匹配。
在一种优选的技术方案中，若无效，则将测量滤波器的测量位姿数据和测量动态数据输入到系统滤波器内，更新系统滤波器，得到更新系统位姿数据和更新系统动态数据，并将第二时间戳设置为第三时间戳，包括：
将第二时间戳、预测测量位姿数据和预测测量动态数据输入到系统滤波器内，得到系统滤波器在第二时间戳时刻的更新系统位姿数据、更新系统动态数据，其中，第二时间戳表示第三时间戳；
以第三时间戳、更新系统位姿数据和更新系统动态数据为基础，累加惯性动态数据对时间的积分，直到获取到下一帧激光数据帧时，作为有效数据帧，与离线环境地图进行匹配。
在一种优选的技术方案中，根据更新系统位姿数据、系统动态数据和第三时间戳，得到自动化机械的自身位姿数据和自身动态数据，包括：
基于更新系统位姿数据、系统动态数据和第三时间戳，获取从第三时间戳开始，到IMU传感器的惯性动态数据的最新数据帧的第四时间戳期间的所有的惯性动态数据，进行积分并累加到更新系统位姿数据和系统动态数据上，得到第四时间戳下的自身位姿数据和自身动态数据。
本说明书实施例还提供一种存在光学干扰环境下的定位装置，适用于自动化机械，包括：
建立模块：建立目标区域的离线环境地图和干扰识别模型，干扰识别模型用于对目标区域中由于光学干扰产生的干扰数据进行过滤；
定位模块：在进行定位时，使用全球导航卫星系统提供的全局位姿数据和IMU传感器的惯性动态数据初始化系统滤波器，得到系统滤波器内的初始系统位姿数据和初始系统动态数据，同时将全球导航卫星系统的时间戳设置为第一时间戳；
过滤模块：对于实时获取的激光数据帧使用干扰识别模型进行干扰数据过滤，得到激光有效数据帧；
匹配模块：基于全局位姿数据，将激光有效数据帧与离线环境地图进行匹配，得到第一时间戳时刻下的激光位姿数据和激光动态数据；
更新模块：根据测量滤波器的测量位姿数据、测量动态数据和第二时间戳，与激光位姿数据、激光动态数据和第一时间戳，更新系统滤波器，得到更新系统位姿数据和更新系统动态数据和第三时间戳，测量滤波器预先设置在自动化机械上，第三时间戳等于第一时间戳或者第二时间戳；
处理模块：根据更新系统位姿数据、系统动态数据和第三时间戳，得到自动化机械的自身位姿数据和自身动态数据；
预测模块：根据自身位姿数据和自身动态数据和第三时间戳，得到当前时刻的预测系统位姿数据。
在一种优选的实施方式中，建立模块，包括：
获取子模块：通过获取的目标区域的激光点云数据和惯性数据，建立目标区域的离线环境地图，惯性数据是通过自动化机械上配备的惯性传感器采集的；
采集子模块：通过采集的目标区域内发生光学干扰时的场景数据训练学习模型，得到干扰识别模型，场景数据包括干扰数据和非干扰数据。
在一种优选的实施方式中，更新模块，包括：
判断子模块：根据测量滤波器的测量位姿数据、测量动态数据和第二时间戳，确定第一时间戳时刻下的是激光位姿数据和激光动态数据是否有效；
有效子模块：若有效，则将第一时间戳、激光位姿数据和激光动态数据输入到系统滤波器内，更新系统滤波器，得到更新系统位姿数据和更新系统动态数据，并将第一时间戳设置为第三时间戳；
无效子模块：若无效，则将测量滤波器的测量位姿数据和测量动态数据输入到系统滤波器内，更新系统滤波器，得到更新系统位姿数据和更新系统动态数据，并将第二时间戳设置为第三时间戳。
在一种优选的实施方式中，判断子模块，包括：
预测单元：根据第一时间戳与第二时间戳的时间间隔，以及测量位姿数据和测量动态数据，得到预测测量位姿数据和预测测量动态数据；
比较单元：将预测测量位姿数据和预测测量动态数据分别与激光位姿数据和激光动态数据进行比较，得到比较结果；
确定单元：根据比较结果和预设阈值，确定是否有效。
在一种优选的实施方式中，有效子模块，包括：
赋值单元：将第一时间戳、激光位姿数据和激光动态数据，分别赋值给第三时间戳、测量位姿数据和测量动态数据；
第一更新单元：将第三时间戳、激光位姿数据和激光动态数据输入到系统滤波器内，得到系统滤波器在第三时间戳时刻的更新系统位姿数据、更新系统动态数据和不确定度；
第一累加单元：以第三时间戳、激光位姿数据和激光动态数据为基础，累加惯性动态数据对时间的积分，直到获取到下一帧激光数据帧时，作为激光有效数据帧，与离线环境地图进行匹配。
在一种优选的实施方式中，无效子模块，包括：
第二更新单元：将第二时间戳、预测测量位姿数据和预测测量动态数据输入到系统滤波器内，得到系统滤波器在第二时间戳时刻的更新系统位姿数据、更新系统动态数据，其中，第二时间戳表示第三时间戳；
第二累加单元：以第三时间戳、更新系统位姿数据和更新系统动态数据为基础，累加惯性动态数据对时间的积分，直到获取到下一帧激光数据帧时，作为有效数据帧，与离线环境地图进行匹配。
在一种优选的实施方式中，处理模块，包括：
处理子模块：基于更新系统位姿数据、系统动态数据和第三时间戳，获取从第三时间戳开始，到IMU传感器的惯性动态数据的最新数据帧的第四时间戳期间的所有的惯性动态数据，进行积分并累加到更新系统位姿数据和系统动态数据上，得到第四时间戳下的自身位姿数据和自身动态数据。
本说明书实施例还提供一种电子设备，包括：至少一个处理器；以及，与至少一个处理器通信连接的存储器；其中，存储器存储有可被至少一个处理器执行的指令，指令被至少一个处理器执行，以使至少一个处理器能够执行上述的存在光学干扰环境下的定位方法。
本说明书实施例还提供一种计算机存储介质，计算机存储介质存储有计算机可执行指令，计算机可执行指令被处理器执行时执行上述的存在光学干扰环境下的定位方法。
与现有技术相比，本说明书实施例采用的上述至少一个技术方案能够达到的有益效果至少包括：
1.使用存在光学干扰时的干扰场景的离线数据进行训练，可以对干扰场景进行检测，对干扰数据进行滤除，从数据源头降低干扰数据对定位的干扰。
2.对匹配的结果进行有效性检查，识别出错误的匹配结果，可以避免错误传递到系统状态中。
3.在匹配错误时，仍然为下一帧的匹配提供初始值(基于系统位姿数据和系统动态数据的IMU积分结果)，且该初始值具有相当的准确性，使后续的匹配可以基于合理的初始位置运行，以极大提高获得匹配正确的可能性。
4.系统滤波器根据光学传感器的数据(光学测量或光学预测)更新系统位姿数据，之后到当前时刻的状态预测，即结合了IMU的积分预测又结合了滤波器内的系统动态数据，确保了高频输出的可能性。
附图说明
为了更清楚地说明本申请实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。
图1是本申请实施例提供的一种存在光学干扰环境下的定位方法的流程示意图；
图2是本申请实施例提供的一种传统定位方法的错误定位输出结果的示意图；
图3是本申请实施例提供的一种使用本说明书提供的存在光学干扰环境下的定位方法的输出结果的示意图；
图4是本申请实施例提供的另一种存在光学干扰环境下的定位方法的流程示意图。
具体实施方式
下面结合附图对本申请实施例进行详细描述。
以下通过特定的具体实例说明本申请的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本申请的其他优点与功效。显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。本申请还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本申请的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
要说明的是，下文描述在所附权利要求书的范围内的实施例的各种方面。应显而易见，本文中所描述的方面可体现于广泛多种形式中，且本文中所描述的任何特定结构及/或功能仅为说明性的。基于本申请，所属领域的技术人员应了解，本文中所描述的一个方面可与任何其它方面独立地实施，且可以各种方式组合这些方面中的两者或两者以上。举例来说，可使用本文中所阐述的任何数目和方面来实施设备及/或实践方法。另外，可使用除了本文中所阐述的方面中的一或多者之外的其它结构及/或功能性实施此设备及/或实践此方法。
还需要说明的是，以下实施例中所提供的图示仅以示意方式说明本申请的基本构想，图式中仅显示与本申请中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也可能更为复杂。
另外，在以下描述中，提供具体细节是为了便于透彻理解实例。然而，所属领域的技术人员将理解，可在没有这些特定细节的情况下实践。
自动化机械经常需要在多种环境下进行室内或室外的自主作业，需要依靠对环境的感知以进行自身定位，并针对作业项目进行决策规划与运动控制。自动化机械常配备光学传感器(如激光雷达)和惯性传感器(如惯性导航模块中的惯性运动单元(InertialMotion Unit，IMU)，通过一定频率发射激光获取环境反射信息，与离线点云地图信息进行匹配以计算自身在背景环境的位姿，结合惯性传感器提供的动态信息，计算自身的实时位姿。
但是，当环境中存在影响光线传播的干扰因素，如雨雪、灰尘、水雾等，这些干扰因素会在激光雷达点云中产生对定位无帮助的干扰点云，使系统接收到的可用于自身定位的有效信息量减少，无法用于自身定位的无效信息增多，引起激光匹配的准确性下降或者短时的失效，进而影响自动化机械的定位性能。
可知的是，惯性传感器不受光学环境影响，可以直接测量自身的加速度和角速度信息，对其积分后可得到轨迹。但缺点是轨迹的误差随时间的积累而增大，无法单独提供长时间的准确位姿。
而差分定位系统(如全球导航卫星系统(Global Navigation Satellite System，GNSS)等)受使用环境的限制较大，在室外开阔处可提供稳定定位，在室内环境和室外遮挡或多径环境下定位性能较差,难以在室内、金属顶棚/茂密树木/近楼宇处等环境下提供稳定的定位信息。
基于此，本说明书实施例提出了一种处理方案：通过融合激光雷达和惯性传感器提供稳定定位，通过传感器和预先通过激光雷达建好的离线点云地图，获取固定频率的定位输出。
以下结合附图，说明本申请各实施例提供的技术方案。
图1是本申请实施例提供的一种存在光学干扰环境下的定位方法的流程示意图，如图1所示，本说明书实施例提供一种存在光学干扰环境下的定位方法，适用于自动化机械，包括：
步骤S1：建立目标区域的离线环境地图和干扰识别模型。
其中，干扰识别模型用于对目标区域中由于光学干扰产生的干扰数据进行过滤。目标区域表示自动化机械需要进行自主作业的区域，并且依靠目标区域中的环境中进行自身定位，针对作业项目进行决策规划和运动控制。
具体地，建立目标区域的离线环境地图和干扰识别模型，包括：通过获取的目标区域的激光点云数据和惯性数据，建立目标区域的离线环境地图；通过采集的目标区域内发生光学干扰时的场景数据训练学习模型，得到干扰识别模型，场景数据包括干扰数据和非干扰数据。
其中，惯性数据是通过自动化机械上配备的惯性传感器采集的。激光点云数据可以通过激光雷达获取。
在本说明书实施例中对于建立离线环境地图的方式不做限制，示例性的，可以通过实时定位建图(Simultaneous Localization And Mapping，SLAM)的方法建立离线环境地图。
在本说明书实施例中预先采集目标区域中的光学干扰发生时，干扰数据由无到有、由少到多的场景数据并进行干扰点标注，例如，环境中存在大量扬尘时激光雷达的点云数据存在大量干扰点，可分别采集在环境地图范围内无扬尘时(如单帧激光雷达扬尘点云数<50)、少量扬尘时(如扬尘点云数<500)、大量扬尘时(如扬尘点云数>500)的激光数据帧，并将扬尘点云标注为干扰点，非扬尘点标注为非干扰点，使用数据训练学习模型(如卷积神经网络)，以对干扰数据获取较高的识别率，对非干扰数据获取较低的误识别率，以用于干扰点过滤。
步骤S2：在进行定位时，使用全球导航卫星系统(Global Navigation SatelliteSystem，GNSS)提供的全局位姿数据和IMU传感器的惯性动态数据初始化系统滤波器，得到系统滤波器内的初始系统位姿数据和初始系统动态数据，同时将全球导航卫星系统的时间戳设置为第一时间戳。
其中，系统滤波器内含自身的位置/方向角(统称为系统位姿)，速度/加速度/角速度(统称为系统动态)，与时间戳(即第一时间戳)。在定位功能激活时，系统滤波器获取初始系统位姿数据和初始系统动态数据。
具体地，在定位功能激活时，使用GNSS提供的全局位姿数据作为步骤S4的初始值，并使用GNSS和IMU传感器的惯性动态数据初始化系统滤波器。
示例性的，使用GNSS提供的全局位姿数据作为实时激光数据帧与离线点云图的初始值，使用GNSS提供的全局位姿数据和IMU传感器提供的加速度数据和角速度数据(惯性动态数据)作为系统滤波器内系统位姿和系统动态的初始值，即初始系统位姿数据和初始系统动态数据，同时将时间戳设置为GNSS数据帧的第一时间戳。
步骤S3：对于实时获取的激光数据帧使用干扰识别模型进行干扰数据过滤，得到激光有效数据帧。
具体地，对于定位功能激活后，实时获取的激光数据帧进行干扰场景检测，如检测到场景存在干扰，则去除干扰数据点、保留非干扰数据点，获得包含有效数据的传感器测量(激光有效数据帧)。示例性的，使用干扰识别模型识别激光点云实时数据中的灰尘点并去除，保留非灰尘的点云数据。
步骤S4：基于全局位姿数据，将激光有效数据帧与离线环境地图进行匹配，得到第一时间戳时刻下的激光位姿数据和激光动态数据。
具体地，在步骤S2中提及使用GNSS提供的全局位姿数据作为实时激光数据帧与离线点云图的初始值。
进一步地，基于初始值(即全局位姿数据)，使用激光有效数据帧与离线环境地图进行匹配，形成激光有效数据帧时间戳(第一时间戳)时刻下的位置数据和方向角数据(即激光位姿数据)；通过不同帧间的差分计算，可获取第一时间戳时刻下的速度数据、加速度数据和角速度数据(即激光动态数据)。
步骤S5：根据测量滤波器的测量位姿数据、测量动态数据和第二时间戳，与激光位姿数据、激光动态数据和第一时间戳，更新系统滤波器，得到更新系统位姿数据和更新系统动态数据和第三时间戳。
其中，测量滤波器预先设置在自动化机械上，第三时间戳等于第一时间戳或者第二时间戳。
具体地，设置测量滤波器，内含测量位姿数据、测量动态数据和第二时间戳。
在一种可选的实施方式中，根据测量滤波器的测量位姿数据、测量动态数据和第二时间戳，与激光位姿数据、激光动态数据和第一时间戳，更新系统滤波器，得到更新系统位姿数据和更新系统动态数据和第三时间戳，包括：根据测量滤波器的测量位姿数据、测量动态数据和第二时间戳，确定第一时间戳时刻下的是激光位姿数据和激光动态数据是否有效；若有效，则将第一时间戳、激光位姿数据和激光动态数据输入到系统滤波器内，更新系统滤波器，得到更新系统位姿数据和更新系统动态数据，并将第一时间戳设置为第三时间戳；若无效，则将测量滤波器的测量位姿数据和测量动态数据输入到系统滤波器内，更新系统滤波器，得到更新系统位姿数据和更新系统动态数据，并将第二时间戳设置为第三时间戳。
其中，根据测量滤波器的测量位姿数据、测量动态数据和第二时间戳，确定第一时间戳时刻下的是激光位姿数据和激光动态数据是否有效，包括：根据第一时间戳与第二时间戳的时间间隔，以及测量位姿数据和测量动态数据，得到预测测量位姿数据和预测测量动态数据；将预测测量位姿数据和预测测量动态数据分别与激光位姿数据和激光动态数据进行比较，得到比较结果；根据比较结果和预设阈值，确定是否有效。
具体地，当新的激光位姿数据、激光动态数据和第一时间戳到来时，计算新的第一时间戳与测量滤波器内第二时间戳的时间间隔(dt)，使用测量滤波器内的测量动态数据将测量位姿数据与测量动态数据基于匀加速运动和匀转速运动预测到新进激光数据帧的时间戳时刻，得到预测测量位姿数据和预测测量动态数据，将预测测量位姿数据和预测测量动态数据与激光位姿数据与激光动态数据进行比较，检查差值(比较结果)是否在预设阈值之内，以确定是否有效。
进一步地，若有效，则将第一时间戳、激光位姿数据和激光动态数据输入到系统滤波器内，更新系统滤波器，得到更新系统位姿数据和更新系统动态数据，并将第一时间戳设置为第三时间戳，包括：将第一时间戳、激光位姿数据和激光动态数据，分别赋值给第三时间戳、测量位姿数据和测量动态数据；将第三时间戳、激光位姿数据和激光动态数据输入到系统滤波器内，得到系统滤波器在第三时间戳时刻的更新系统位姿数据、更新系统动态数据和不确定度；以第三时间戳、激光位姿数据和激光动态数据为基础，累加惯性动态数据对时间的积分，直到获取到下一帧激光数据帧时，作为激光有效数据帧，与离线环境地图进行匹配。
在一种可选的实施方式中，若比较结果在预设阈值之内，则确定第一时间戳时刻下的是激光位姿数据和激光动态数据有效。
具体地，若有效，将激光位姿数据、激光动态数据及激光数据帧的第一时间戳赋值给测量滤波器内的测量位姿数据、测量动态数据与第二时间戳，并将该激光位姿数据、激光动态数据及激光数据帧的第一时间戳输入到系统滤波器内。更新滤波器内在激光数据帧第一时间戳时刻的系统位姿数据、系统动态数据和不确定度；并将激光位姿数据、激光动态数据和第一时间戳作为基础，累加IMU的惯性动态数据对时间的积分，直至下一帧激光数据帧到来，用作与离线环境地图做匹配的初始值。
示例性的，计算激光数据帧的第一时间戳与系统滤波器内时间戳的时间间隔(dt)，将使用系统动态数据(速度、加速度、角速度)将系统位姿数据与系统动态数据基于匀加速运动和匀转速运动预测到激光数据帧时间戳时刻，再与激光位姿数据与激光动态数据进行非线性卡尔曼滤波，融合滤波得到的位姿数据和动态数据作为系统滤波器的更新系统位姿数据和更新系统动态数据，激光数据帧的第一时间戳作为系统滤波器的第三时间戳；同时以该激光数据帧的第一时间戳下的激光位姿数据、激光动态数据为基础值，获取第一时间戳与新IMU数据帧时间戳的时间间隔，使用新IMU数据帧的惯性动态数据将对dt积分后获得位姿(位置、朝向角)变化量和动态(速度)变化量，将该位姿变化量和动态变化量累加到基础值的位姿数据和动态数据，以更新基础值的位姿数据和动态数据，并将基础值的时间戳更新为IMU数据帧时间戳；随着周期性收到新的IMU惯性动态数据，该基础值的位姿数据和动态数据会得到周期性累加更新。有新的激光数据帧到来后，读取最新的基础值，作为对新的激光数据帧进行与离线环境地图匹配的初始值。
进一步地，若无效，则将测量滤波器的测量位姿数据和测量动态数据输入到系统滤波器内，更新系统滤波器，得到更新系统位姿数据和更新系统动态数据，并将第二时间戳设置为第三时间戳，包括：将第二时间戳、预测测量位姿数据和预测测量动态数据输入到系统滤波器内，得到系统滤波器在第二时间戳时刻的更新系统位姿数据、更新系统动态数据，其中，第二时间戳表示第三时间戳；以第三时间戳、更新系统位姿数据和更新系统动态数据为基础，累加惯性动态数据对时间的积分，直到获取到下一帧激光数据帧时，作为激光有效数据帧，与离线环境地图进行匹配。
在一种可选的实施方式中，若比较结果在预设阈值之外，则确定第一时间戳时刻下的是激光位姿数据和激光动态数据无效。
具体地，若无效，使用测量滤波器给出的预测测量位姿数据和预测测量动态数据，输入到系统滤波器内，更新系统滤波器内在激光数据帧时间戳时刻的系统位姿数据、系统动态数据；并将更新系统位姿数据、更新系统动态数据和第二时间戳做基础，累加IMU测量的惯性动态数据对时间的积分，直至下一帧激光数据帧到来，用作与离线环境地图做匹配的初始值。
示例性的，计算激光数据帧的第一时间戳与系统滤波器的第二时间戳的时间间隔(dt)，使用系统动态数据(速度、加速度、角速度)，基于匀加速运动和匀转速运动，将系统位姿数据与系统动态数据预测到激光数据帧时间戳时刻后，与预测测量位姿数据与预测测量动态数据进行非线性卡尔曼融合滤波，融合滤波得到的位姿数据和动态数据作为系统滤波器的更新系统位姿数据和更新系统动态数据，并使用激光数据帧的第二时间戳更新系统滤波器的第三时间戳。同时以该激光数据帧的第二时间戳下的预测测量位姿数据、预测测量动态数据为基础值，获取该第三时间戳与新IMU数据帧时间戳的时间间隔(dt)，使用新IMU数据帧的惯性动态数据将对dt积分后获得位姿(位置、朝向角)变化量和动态(速度)变化量，将位姿变化量和动态变化量累加到基础值的位姿数据和动态数据，以更新基础值的位姿数据和动态数据，并将基础值的时间戳更新为IMU数据帧时间戳；随着周期性收到新的IMU惯性动态数据，该基础值的位姿数据和动态数据会得到周期性累加更新。有新的激光数据帧到来后，读取最新的基础值，作为对新的激光数据帧进行与离线环境地图匹配的初始值。
步骤S6：根据更新系统位姿数据、系统动态数据和第三时间戳，得到自动化机械的自身位姿数据和自身动态数据。
具体地，基于更新系统位姿数据、更新系统动态数据和第三时间戳，获取从第三时间戳开始、到最新的IMU数据帧时刻结束，期间所有的IMU惯性动态数据，对其做积分后并累加更新已有的系统位姿数据和系统动态数据上，得到最新的IMU数据帧时刻下的自身位姿数据和自身动态数据。
在一种可选的实施方式中，根据更新系统位姿数据、系统动态数据和第三时间戳，得到自动化机械的自身位姿数据和自身动态数据，包括：基于更新系统位姿数据、系统动态数据和第三时间戳，获取从第三时间戳开始，到IMU传感器的惯性动态数据的最新数据帧的第四时间戳期间的所有的惯性动态数据，进行积分并累加到更新系统位姿数据和系统动态数据上，得到第四时间戳下的自身位姿数据和自身动态数据。
具体的，保存获取系统滤波器的第三时间戳之后所有的IMU数据帧。计算系统滤波器的第三时间戳与第三时间戳之后一帧IMU惯性动态数据的时间戳的时间间隔(dt)，将IMU惯性动态数据对dt进行积分获得获得位姿(位置、朝向角)变化量和动态(速度)变化量，将位姿变化量和动态变化量与更新系统位姿数据和更新系统动态数据相加，得到该IMU数据帧时间戳时刻的自身位姿数据和自身动态数据；之后选取后续一帧IMU惯性动态数据，使用惯性动态数据对dt积分后得到的位姿变化量和动态变化量，更新自身位姿数据、自身动态数据和对应的时间戳，直到保存的IMU惯性动态数据全部使用。
步骤S7：根据自身位姿数据和自身动态数据和第三时间戳，得到当前时刻的预测系统位姿数据。
具体地，基于步骤S6计算得到的自身位姿数据、自身动态数据和第三时间戳，计算第三时间戳到当前时刻的时间差，预测获取当前时刻下系统的位姿数据。具体的，计算系统滤波器的第三时间戳与当前软件运行时刻时间戳的时间间隔(dt)，基于系统动态数据(速度、加速度、角速度)和dt，使用匀加速运动和匀转速运动模型对系统位姿数据(位置、朝向角)做预测，获取当前软件运行时刻的预测系统位姿数据(位置、朝向角)。
本说明书实施例中使用存在光学干扰时的干扰场景的离线数据进行训练，可以对干扰场景进行检测，对干扰数据进行滤除，从数据源头降低干扰数据对定位的干扰；对匹配的结果进行有效性检查，识别出错误的匹配结果，可以避免错误传递到系统状态中；在匹配错误时，仍然为下一帧的匹配提供初始值(基于系统位姿数据和系统动态数据的IMU积分结果)，且该初始值具有相当的准确性，使后续的匹配可以基于合理的初始位置运行，以极大提高获得匹配正确的可能性；系统滤波器根据光学传感器的数据(光学测量或光学预测)更新系统位姿数据，之后到当前时刻的状态预测，即结合了IMU的积分预测又结合了滤波器内的系统动态数据，确保了高频输出的可能性。
图2是本申请实施例提供的一种传统定位方法的错误定位输出结果的示意图，如图2所示，其中，白色轨迹表示在大量扬尘下进行人工行驶，得到的定位轨迹，黑色方框框内轨迹表示匹配失效造成轨迹出现跳变的情况，可以发现通过现有方法可以出现定位失效的情况发生。
图3是本申请实施例提供的一种使用本说明书提供的存在光学干扰环境下的定位方法的输出结果的示意图，如图3所示，通过白色轨迹可以看出可以得到正确的定位轨迹。
可知的是，图2和图3中的细网格元素表示软件可视化的网格背景。
下面以一个具体的实施例对本申请提供的存在光学干扰环境下的定位方法进行说明。
图4是本申请实施例提供的另一种存在光学干扰环境下的定位方法的流程示意图，如图4所示，本申请实施例提供的一种存在光学干扰环境下的定位方法，包括：
步骤S101：通过获取的目标区域的激光点云数据和惯性数据，建立目标区域的离线环境地图，通过采集的目标区域内发生光学干扰时的场景数据训练学习模型，得到干扰识别模型。
具体地，预先采集目标区域的光学数据并建立离线环境地图，可采集激光雷达点云地图、相机点云地图和三维重建地图等等。
进一步地，预先采集各种干扰场景、干扰数据由无到有、由少到多的场景数据，训练学习模型，可使用基于点云或图像的深度学习模型，或各类特征学习与分类器模型，如神经网络、基于特征提取的集成学习模型、支持向量机等等，获取对干扰场景的识别与干扰数据点的分割。
步骤S102：在进行定位时，使用全球导航卫星系统(Global NavigationSatellite System，GNSS)提供的全局位姿数据和IMU传感器的惯性动态数据初始化系统滤波器，得到系统滤波器内的初始系统位姿数据和初始系统动态数据，同时将全球导航卫星系统的时间戳设置为第一时间戳。
其中，系统滤波器可以使用基于卡尔曼滤波器、信息滤波器等传统贝叶斯滤波器，也可使用基于因子图等图模型。
步骤S103：对于实时获取的激光数据帧使用干扰识别模型进行干扰数据过滤，得到激光有效数据帧。
具体地，对光学传感器测量的数据帧进行干扰场景检测，如检测到场景存在干扰，则去除干扰数据点、保留非干扰数据点，获得包含有效数据的传感器测量(激光有效数据帧)，干扰场景检测和干扰点识别，则通过离线训练模型的在线推理获取。
步骤S104：基于全局位姿数据，将激光有效数据帧与离线环境地图进行匹配，得到第一时间戳时刻下的激光位姿数据和激光动态数据。
具体地，基于初始值(全局位姿数据)，使用激光有效数据帧与离线环境地图匹配，形成激光数据帧时间戳时刻下的位置、方向角(激光位姿数据)。通过不同帧间的差分计算，可获取时间戳时刻下的速度、加速度、角速度(光激光动态数据)。其中，匹配方法可采用基于点/面/体特征的匹配如ICP/NDT类的方法，或基于深度学习语义特征的匹配。其中，ICP表示迭代最近点算法(Iterative Closest Point)；NDT表示正态分布变换(NormalDistribution Transform)。
步骤S105：根据测量滤波器的测量位姿数据、测量动态数据和第二时间戳，确定第一时间戳时刻下的是激光位姿数据和激光动态数据是否有效，若有效，则执行步骤S106-步骤S107，若无效，则执行步骤S108-步骤S109。
具体地，可通过设置静态或动态阈值的方法，也可使用额外的学习模型，对是否有异常、无效的激光位姿数据和激光动态数据进行识别。
步骤S106：若有效，则将第一时间戳、激光位姿数据和激光动态数据输入到系统滤波器内，更新系统滤波器，得到更新系统位姿数据和更新系统动态数据，并将第一时间戳设置为第三时间戳。
步骤S107：以第三时间戳、激光位姿数据和激光动态数据为基础，累加惯性动态数据对时间的积分，直到获取到下一帧激光数据帧时，作为激光有效数据帧，与离线环境地图进行匹配。
步骤S108：若无效，则将测量滤波器的测量位姿数据和测量动态数据输入到系统滤波器内，更新系统滤波器，得到更新系统位姿数据和更新系统动态数据，并将第二时间戳设置为第三时间戳。
步骤S109：将第二时间戳、预测测量位姿数据和预测测量动态数据输入到系统滤波器内，得到系统滤波器在第二时间戳时刻的更新系统位姿数据、更新系统动态数据，其中，第二时间戳表示第三时间戳；以第三时间戳、更新系统位姿数据和更新系统动态数据为基础，累加惯性动态数据对时间的积分，直到获取到下一帧激光数据帧时，作为激光有效数据帧，与离线环境地图进行匹配。
在步骤S106和步骤S108之后，执行步骤S110-步骤S111。
步骤S110：根据更新系统位姿数据、系统动态数据和第三时间戳，得到自动化机械的自身位姿数据和自身动态数据。
步骤S111：根据自身位姿数据和自身动态数据和第三时间戳，得到当前时刻的预测系统位姿数据。
本说明书实施例中使用存在光学干扰时的干扰场景的离线数据进行训练，可以对干扰场景进行检测，对干扰数据进行滤除，从数据源头降低干扰数据对定位的干扰；对匹配的结果进行有效性检查，识别出错误的匹配结果，可以避免错误传递到系统状态中；在匹配错误时，仍然为下一帧的匹配提供初始值(基于系统位姿数据和系统动态数据的IMU积分结果)，且该初始值具有相当的准确性，使后续的匹配可以基于合理的初始位置运行，以极大提高获得匹配正确的可能性；系统滤波器根据光学传感器的数据(光学测量或光学预测)更新系统位姿数据，之后到当前时刻的状态预测，即结合了IMU的积分预测又结合了滤波器内的系统动态数据，确保了高频输出的可能性。
本说明书实施例还提供一种存在光学干扰环境下的定位装置，适用于自动化机械，包括：
建立模块：建立目标区域的离线环境地图和干扰识别模型，干扰识别模型用于对目标区域中由于光学干扰产生的干扰数据进行过滤。
在一种优选的实施方式中，建立模块，包括：获取子模块：通过获取的目标区域的激光点云数据和惯性数据，建立目标区域的离线环境地图，惯性数据是通过自动化机械上配备的惯性传感器采集的。采集子模块：通过采集的目标区域内发生光学干扰时的场景数据训练学习模型，得到干扰识别模型，场景数据包括干扰数据和非干扰数据。
定位模块：在进行定位时，使用全球导航卫星系统提供的全局位姿数据和IMU传感器的惯性动态数据初始化系统滤波器，得到系统滤波器内的初始系统位姿数据和初始系统动态数据，同时将全球导航卫星系统的时间戳设置为第一时间戳。
过滤模块：对于实时获取的激光数据帧使用干扰识别模型进行干扰数据过滤，得到激光有效数据帧。
匹配模块：基于全局位姿数据，将激光有效数据帧与离线环境地图进行匹配，得到第一时间戳时刻下的激光位姿数据和激光动态数据。
更新模块：根据测量滤波器的测量位姿数据、测量动态数据和第二时间戳，与激光位姿数据、激光动态数据和第一时间戳，更新系统滤波器，得到更新系统位姿数据和更新系统动态数据和第三时间戳，测量滤波器预先设置在自动化机械上，第三时间戳等于第一时间戳或者第二时间戳。
在一种优选的实施方式中，更新模块，包括：判断子模块：根据测量滤波器的测量位姿数据、测量动态数据和第二时间戳，确定第一时间戳时刻下的是激光位姿数据和激光动态数据是否有效。有效子模块：若有效，则将第一时间戳、激光位姿数据和激光动态数据输入到系统滤波器内，更新系统滤波器，得到更新系统位姿数据和更新系统动态数据，并将第一时间戳设置为第三时间戳。无效子模块：若无效，则将测量滤波器的测量位姿数据和测量动态数据输入到系统滤波器内，更新系统滤波器，得到更新系统位姿数据和更新系统动态数据，并将第二时间戳设置为第三时间戳。
在一种优选的实施方式中，判断子模块，包括：预测单元：根据第一时间戳与第二时间戳的时间间隔，以及测量位姿数据和测量动态数据，得到预测测量位姿数据和预测测量动态数据；比较单元：将预测测量位姿数据和预测测量动态数据分别与激光位姿数据和激光动态数据进行比较，得到比较结果；确定单元：根据比较结果和预设阈值，确定是否有效。
在一种优选的实施方式中，有效子模块，包括：赋值单元：将第一时间戳、激光位姿数据和激光动态数据，分别赋值给第三时间戳、测量位姿数据和测量动态数据；第一更新单元：将第三时间戳、激光位姿数据和激光动态数据输入到系统滤波器内，得到系统滤波器在第三时间戳时刻的更新系统位姿数据、更新系统动态数据和不确定度；第一累加单元：以第三时间戳、激光位姿数据和激光动态数据为基础，累加惯性动态数据对时间的积分，直到获取到下一帧激光数据帧时，作为激光有效数据帧，与离线环境地图进行匹配。
在一种优选的实施方式中，无效子模块，包括：第二更新单元：将第二时间戳、预测测量位姿数据和预测测量动态数据输入到系统滤波器内，得到系统滤波器在第二时间戳时刻的更新系统位姿数据、更新系统动态数据，其中，第二时间戳表示第三时间戳；第二累加单元：以第三时间戳、更新系统位姿数据和更新系统动态数据为基础，累加惯性动态数据对时间的积分，直到获取到下一帧激光数据帧时，作为有效数据帧，与离线环境地图进行匹配。
处理模块：根据更新系统位姿数据、系统动态数据和第三时间戳，得到自动化机械的自身位姿数据和自身动态数据。
在一种优选的实施方式中，处理模块，包括：处理子模块：基于更新系统位姿数据、系统动态数据和第三时间戳，获取从第三时间戳开始，到IMU传感器的惯性动态数据的最新数据帧的第四时间戳期间的所有的惯性动态数据，进行积分并累加到更新系统位姿数据和系统动态数据上，得到第四时间戳下的自身位姿数据和自身动态数据。
预测模块：根据自身位姿数据和自身动态数据和第三时间戳，得到当前时刻的预测系统位姿数据。
本说明书实施例还提供一种电子设备，包括：至少一个处理器；以及，与至少一个处理器通信连接的存储器；其中，存储器存储有可被至少一个处理器执行的指令，指令被至少一个处理器执行，以使至少一个处理器能够执行上述的存在光学干扰环境下的定位方法。
本说明书实施例还提供一种计算机存储介质，计算机存储介质存储有计算机可执行指令，计算机可执行指令被处理器执行时执行上述的存在光学干扰环境下的定位方法。
本说明书中，各个实施例之间相同相似的部分互相参见即可，每个实施例侧重说明的都是与其他实施例的不同之处。尤其，对于后面说明的产品实施例而言，由于其与方法是对应的，描述比较简单，相关之处参见系统实施例的部分说明即可。
以上所述，仅为本申请的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应以权利要求的保护范围为准。

Claims (10)

1.一种存在光学干扰环境下的定位方法，适用于自动化机械，其特征在于，包括：
建立目标区域的离线环境地图和干扰识别模型，所述干扰识别模型用于对所述目标区域中由于光学干扰产生的干扰数据进行过滤；
在进行定位时，使用全球导航卫星系统提供的全局位姿数据和IMU传感器的惯性动态数据初始化系统滤波器，得到系统滤波器内的初始系统位姿数据和初始系统动态数据，同时将所述全球导航卫星系统的时间戳设置为第一时间戳；
对于实时获取的激光数据帧使用所述干扰识别模型进行干扰数据过滤，得到激光有效数据帧；
基于所述全局位姿数据，将所述激光有效数据帧与所述离线环境地图进行匹配，得到所述第一时间戳时刻下的激光位姿数据和激光动态数据；
根据测量滤波器的测量位姿数据、测量动态数据和第二时间戳，与所述激光位姿数据、所述激光动态数据和所述第一时间戳，更新所述系统滤波器，得到更新系统位姿数据和更新系统动态数据和第三时间戳，所述测量滤波器预先设置在自动化机械上，所述第三时间戳等于所述第一时间戳或者所述第二时间戳；
根据所述更新系统位姿数据、所述系统动态数据和所述第三时间戳，得到所述自动化机械的自身位姿数据和自身动态数据；
根据所述自身位姿数据和所述自身动态数据和第三时间戳，得到当前时刻的预测系统位姿数据。
2.根据权利要求1所述的存在光学干扰环境下的定位方法，其特征在于，所述建立目标区域的离线环境地图和干扰识别模型，包括：
通过获取的所述目标区域的激光点云数据和惯性数据，建立所述目标区域的离线环境地图，所述惯性数据是通过所述自动化机械上配备的惯性传感器采集的；
通过采集的所述目标区域内发生光学干扰时的场景数据训练学习模型，得到干扰识别模型，所述场景数据包括所述干扰数据和非干扰数据。
3.根据权利要求1所述的存在光学干扰环境下的定位方法，其特征在于，所述根据测量滤波器的测量位姿数据、测量动态数据和第二时间戳，与所述激光位姿数据、所述激光动态数据和所述第一时间戳，更新所述系统滤波器，得到更新系统位姿数据和更新系统动态数据和第三时间戳，包括：
根据所述测量滤波器的所述测量位姿数据、所述测量动态数据和所述第二时间戳，确定所述第一时间戳时刻下的是所述激光位姿数据和所述激光动态数据是否有效；
若有效，则将所述第一时间戳、所述激光位姿数据和所述激光动态数据输入到系统滤波器内，更新所述系统滤波器，得到所述更新系统位姿数据和所述更新系统动态数据，并将所述第一时间戳设置为所述第三时间戳；
若无效，则将所述测量滤波器的所述测量位姿数据和所述测量动态数据输入到系统滤波器内，更新所述系统滤波器，得到所述更新系统位姿数据和所述更新系统动态数据，并将所述第二时间戳设置为所述第三时间戳。
4.根据权利要求3所述的存在光学干扰环境下的定位方法，其特征在于，所述根据所述测量滤波器的所述测量位姿数据、所述测量动态数据和所述第二时间戳，确定所述第一时间戳时刻下的是所述激光位姿数据和所述激光动态数据是否有效，包括：
根据所述第一时间戳与所述第二时间戳的时间间隔，以及所述测量位姿数据和测量动态数据，得到预测测量位姿数据和预测测量动态数据；
将所述预测测量位姿数据和所述预测测量动态数据分别与所述激光位姿数据和激光动态数据进行比较，得到比较结果；
根据所述比较结果和预设阈值，确定是否有效。
5.根据权利要求3所述的存在光学干扰环境下的定位方法，其特征在于，所述若有效，则将所述第一时间戳、所述激光位姿数据和所述激光动态数据输入到系统滤波器内，更新所述系统滤波器，得到所述更新系统位姿数据和所述更新系统动态数据，并将所述第一时间戳设置为所述第三时间戳，包括：
将所述第一时间戳、所述激光位姿数据和所述激光动态数据，分别赋值给所述第三时间戳、所述测量位姿数据和所述测量动态数据；
将所述第三时间戳、所述激光位姿数据和所述激光动态数据输入到所述系统滤波器内，得到所述系统滤波器在所述第三时间戳时刻的所述更新系统位姿数据、所述更新系统动态数据和不确定度；
以所述第三时间戳、所述激光位姿数据和所述激光动态数据为基础，累加所述惯性动态数据对时间的积分，直到获取到下一帧所述激光数据帧时，作为所述激光有效数据帧，与所述离线环境地图进行匹配。
6.根据权利要求3-4任一项所述的存在光学干扰环境下的定位方法，其特征在于，所述若无效，则将所述测量滤波器的所述测量位姿数据和所述测量动态数据输入到系统滤波器内，更新所述系统滤波器，得到所述更新系统位姿数据和所述更新系统动态数据，并将所述第二时间戳设置为所述第三时间戳，包括：
将所述第二时间戳、所述预测测量位姿数据和所述预测测量动态数据输入到所述系统滤波器内，得到所述系统滤波器在所述第二时间戳时刻的所述更新系统位姿数据、所述更新系统动态数据，其中，所述第二时间戳表示所述第三时间戳；
以所述第三时间戳、所述更新系统位姿数据和所述更新系统动态数据为基础，累加所述惯性动态数据对时间的积分，直到获取到下一帧所述激光数据帧时，作为所述有效数据帧，与所述离线环境地图进行匹配。
7.根据权利要求1所述的存在光学干扰环境下的定位方法，其特征在于，所述根据所述更新系统位姿数据、所述系统动态数据和所述第三时间戳，得到所述自动化机械的自身位姿数据和自身动态数据，包括：
基于所述更新系统位姿数据、所述系统动态数据和所述第三时间戳，获取从所述第三时间戳开始，到所述IMU传感器的所述惯性动态数据的最新数据帧的第四时间戳期间的所有的所述惯性动态数据，进行积分并累加到所述更新系统位姿数据和所述系统动态数据上，得到所述第四时间戳下的所述自身位姿数据和所述自身动态数据。
8.一种存在光学干扰环境下的定位装置，适用于自动化机械，其特征在于，包括：
建立模块：建立目标区域的离线环境地图和干扰识别模型，所述干扰识别模型用于对所述目标区域中由于光学干扰产生的干扰数据进行过滤；
定位模块：在进行定位时，使用全球导航卫星系统提供的全局位姿数据和IMU传感器的惯性动态数据初始化系统滤波器，得到系统滤波器内的初始系统位姿数据和初始系统动态数据，同时将所述全球导航卫星系统的时间戳设置为第一时间戳；
过滤模块：对于实时获取的激光数据帧使用所述干扰识别模型进行干扰数据过滤，得到激光有效数据帧；
匹配模块：基于所述全局位姿数据，将所述激光有效数据帧与所述离线环境地图进行匹配，得到所述第一时间戳时刻下的激光位姿数据和激光动态数据；
更新模块：根据测量滤波器的测量位姿数据、测量动态数据和第二时间戳，与所述激光位姿数据、所述激光动态数据和所述第一时间戳，更新所述系统滤波器，得到更新系统位姿数据和更新系统动态数据和第三时间戳，所述测量滤波器预先设置在自动化机械上，所述第三时间戳等于所述第一时间戳或者所述第二时间戳；
处理模块：根据所述更新系统位姿数据、所述系统动态数据和所述第三时间戳，得到所述自动化机械的自身位姿数据和自身动态数据；
预测模块：根据所述自身位姿数据和所述自身动态数据和第三时间戳，得到当前时刻的预测系统位姿数据。
9.一种电子设备，其特征在于，包括：至少一个处理器；以及，与所述至少一个处理器通信连接的存储器；其中，所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行权利要求1-7中任一项所述的存在光学干扰环境下的定位方法。
10.一种计算机存储介质，其特征在于，所述计算机存储介质存储有计算机可执行指令，所述计算机可执行指令被处理器执行时执行权利要求1-7中任一项所述的存在光学干扰环境下的定位方法。
CN202310258499.5A 2023-03-16 2023-03-16 存在光学干扰环境下的定位方法、装置、设备、介质 Active CN116295345B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310258499.5A CN116295345B (zh) 2023-03-16 2023-03-16 存在光学干扰环境下的定位方法、装置、设备、介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310258499.5A CN116295345B (zh) 2023-03-16 2023-03-16 存在光学干扰环境下的定位方法、装置、设备、介质

Publications (2)

Publication Number Publication Date
CN116295345A true CN116295345A (zh) 2023-06-23
CN116295345B CN116295345B (zh) 2024-01-19

Family

ID=86790090

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310258499.5A Active CN116295345B (zh) 2023-03-16 2023-03-16 存在光学干扰环境下的定位方法、装置、设备、介质

Country Status (1)

Country Link
CN (1) CN116295345B (zh)

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107683420A (zh) * 2014-11-10 2018-02-09 威尔乌集团 位置跟踪系统和方法
US11069082B1 (en) * 2015-08-23 2021-07-20 AI Incorporated Remote distance estimation system and method
CN113902860A (zh) * 2021-10-10 2022-01-07 北京工业大学 一种基于多线激光雷达点云的多尺度静态地图构建方法
CN113960622A (zh) * 2021-10-26 2022-01-21 北京理工大学 融合激光雷达及imu传感器信息的实时定位方法及装置
CN114140760A (zh) * 2020-08-13 2022-03-04 长沙智能驾驶研究院有限公司 智能驾驶中障碍物检测去干扰方法、装置和计算机设备
CN114739415A (zh) * 2022-03-24 2022-07-12 中国人民解放军国防科技大学 基于多传感器融合的多车定位方法、装置及计算机设备
CN115372947A (zh) * 2022-08-25 2022-11-22 德清斯年智驾科技有限公司 针对自动驾驶车辆惯导及激光雷达的标定方法及标定系统
CN115597602A (zh) * 2022-10-20 2023-01-13 Oppo广东移动通信有限公司（Cn） 机器人导航定位方法及装置、可读介质和电子设备

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107683420A (zh) * 2014-11-10 2018-02-09 威尔乌集团 位置跟踪系统和方法
US11069082B1 (en) * 2015-08-23 2021-07-20 AI Incorporated Remote distance estimation system and method
CN114140760A (zh) * 2020-08-13 2022-03-04 长沙智能驾驶研究院有限公司 智能驾驶中障碍物检测去干扰方法、装置和计算机设备
CN113902860A (zh) * 2021-10-10 2022-01-07 北京工业大学 一种基于多线激光雷达点云的多尺度静态地图构建方法
CN113960622A (zh) * 2021-10-26 2022-01-21 北京理工大学 融合激光雷达及imu传感器信息的实时定位方法及装置
CN114739415A (zh) * 2022-03-24 2022-07-12 中国人民解放军国防科技大学 基于多传感器融合的多车定位方法、装置及计算机设备
CN115372947A (zh) * 2022-08-25 2022-11-22 德清斯年智驾科技有限公司 针对自动驾驶车辆惯导及激光雷达的标定方法及标定系统
CN115597602A (zh) * 2022-10-20 2023-01-13 Oppo广东移动通信有限公司（Cn） 机器人导航定位方法及装置、可读介质和电子设备

Also Published As

Publication number Publication date
CN116295345B (zh) 2024-01-19

Similar Documents

Publication Publication Date Title
CN109084732B (zh) 定位与导航方法、装置及处理设备
CN112639502B (zh) 机器人位姿估计
US11092444B2 (en) Method and system for recording landmarks in a traffic environment of a mobile unit
US9071829B2 (en) Method and system for fusing data arising from image sensors and from motion or position sensors
CN110146909A (zh) 一种定位数据处理方法
CN109074085A (zh) 一种自主定位和地图建立方法、装置和机器人
CN104811683A (zh) 用于估计位置的方法和设备
JP4984659B2 (ja) 自車両位置推定装置
CN111856499B (zh) 基于激光雷达的地图构建方法和装置
KR101888295B1 (ko) 레이저 거리 센서의 측정 거리에 대해 추정된 거리 유형의 신뢰성을 평가하는 방법 및 이를 이용한 이동 로봇의 위치 추정 방법
CN108446710A (zh) 室内平面图快速重建方法及重建系统
CN113066127B (zh) 一种在线标定设备参数的视觉惯性里程计方法和系统
CN115200572B (zh) 三维点云地图构建方法、装置、电子设备及存储介质
Coelho et al. Ekf and computer vision for mobile robot localization
CN110989619B (zh) 用于定位对象的方法、装置、设备和存储介质
CN117685953A (zh) 面向多无人机协同定位的uwb与视觉融合定位方法及系统
CN112762929B (zh) 一种智能导航方法、装置和设备
CN116295345B (zh) 存在光学干扰环境下的定位方法、装置、设备、介质
CN115797490B (zh) 基于激光视觉融合的建图方法及系统
CN116929334A (zh) 一种多传感器融合的建图方法及系统
KR101502071B1 (ko) 랜드마크 기반 비전항법시스템을 위한 카메라 데이터 생성기와 그것을 실행시키기 위한 프로그램을 기록한 컴퓨터로 읽을 수 있는 매체
CN115950414A (zh) 一种不同传感器数据的自适应多重融合slam方法
Lee et al. Development of advanced grid map building model based on sonar geometric reliability for indoor mobile robot localization
CN113495281A (zh) 可移动平台的实时定位方法及装置
CN110458887B (zh) 一种基于pca的加权融合室内定位方法

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