CN114739402B - 一种融合定位方法、介质和电子设备 - Google Patents

一种融合定位方法、介质和电子设备 Download PDF

Info

Publication number
CN114739402B
CN114739402B CN202210430709.XA CN202210430709A CN114739402B CN 114739402 B CN114739402 B CN 114739402B CN 202210430709 A CN202210430709 A CN 202210430709A CN 114739402 B CN114739402 B CN 114739402B
Authority
CN
China
Prior art keywords
pose
vehicle
matrix set
observation
determining
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.)
Active
Application number
CN202210430709.XA
Other languages
English (en)
Other versions
CN114739402A (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.)
Hozon New Energy Automobile Co Ltd
Original Assignee
Hozon New Energy Automobile 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 Hozon New Energy Automobile Co Ltd filed Critical Hozon New Energy Automobile Co Ltd
Priority to CN202210430709.XA priority Critical patent/CN114739402B/zh
Publication of CN114739402A publication Critical patent/CN114739402A/zh
Application granted granted Critical
Publication of CN114739402B publication Critical patent/CN114739402B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/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/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
    • 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/34—Route searching; Route guidance
    • G01C21/36—Input/output arrangements for on-board computers
    • 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

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)
  • Traffic Control Systems (AREA)

Abstract

本申请涉及自动驾驶领域，特别涉及一种融合定位方法、介质和电子设备，包括：获取由惯导***检测到的惯导数据，以及由观测传感器组采集到不同类型的观测数据，其中，观测传感器组为至少包括地面穿透雷达的传感器组合；根据惯导数据确定出车辆的预测位姿，并根据观测数据确定出车辆的各个观测位姿；将预测位姿与各个观测位姿进行融合定位计算，得到实际车辆位姿。如此，采用了地面穿透雷达采集地下物体数据，又由于地下物体不易受环境或者天气的影响，从而可以有效提高在极端环境下对车辆位姿定位的精确度。

Description

一种融合定位方法、介质和电子设备
技术领域
本申请涉及自动驾驶技术领域，尤其涉及一种融合定位方法、介质和电子设备。
背景技术
在自动驾驶领域，多传感器融合定位越来越受关注。目前普遍使用有对激光雷达、相机、惯性测量单元(Inertial Measurement Unit，IMU)以及全球导航卫星***(GlobalNavigation Satellite System，GNSS)进行融合定位的方法，可以获得比单传感器更高的定位精准度。
然而，由于车辆的驾驶环境复杂，现有的传感器融合无法使用于一些极端场景。比如，在极端天气下，可能出现激光雷达受到干扰、相机无法采集足够的特征等问题，从而导致定位不够准确。例如，大雪时路边或周边环境被积雪覆盖，激光雷达扫描的点云地图和之前的点云地图差异过大，进而无法基于点云地图实现定位。与此同时，在此种场景下，道路上的标识如车道线、箭头、斑马线等也被积雪覆盖，使得相机无法获得路面标识，无法完成定位。又如，在车辆周围的粉尘严重时，也会出现激光雷达干扰严重、相机无法检测到足够的特征的问题，从而导致定位失效。
发明内容
本申请实施例提供了一种融合定位方法、介质和电子设备，用以将基于地面穿透雷达采集数据计算得到的车辆位姿和基于其他传感器计算得到的车辆位姿进行融合定位计算，得到车辆的实际车辆位姿，提高了车辆在特殊环境下的位姿定位准确度。
第一方面，本申请实施例提供了一种融合定位方法，该方法包括：
获取由惯导***检测到的惯导数据，以及由观测传感器组采集到不同类型的观测数据，其中，观测传感器组为至少包括地面穿透雷达的传感器组合；
根据惯导数据确定出车辆的预测位姿，并根据观测数据确定出车辆的各个观测位姿；
将预测位姿与各个观测位姿进行融合定位计算，得到实际车辆位姿。
如此，可以实现将基于地面穿透雷达确定的观测车辆位姿和其他车辆位姿进行融合计算，降低了环境对车辆位姿的定位的影响，提高了车辆在特殊环境下对车辆位姿定位的准确性。
在上述第一方面的一种可能的实现中，并根据观测数据确定出车辆的各个观测位姿，包括：
根据激光雷达传感器在当前时刻采集到的点云数据和在当前时刻的前一时刻采集的点云数据，通过帧间匹配算法计算出当前时刻对应的观测车辆位姿；
根据图像采集设备在当前时刻采集到的图像数据和在当前时刻的前一时刻采集的图像数据，通过帧间匹配算法计算出当前时刻对应的观测车辆位姿；
根据地面穿透雷达传感器在当前时刻的前一时刻采集到的信号强度数据和在当前时刻采集的当前信号强度数据，使用帧间匹配算法计算出当前时刻对应的观测车辆位姿。
在上述第一方面的一种可能的实现中，将预测位姿与各个观测位姿进行融合定位计算，得到实际车辆位姿，包括：
确定观测传感器组中的每个传感器对应的卡尔曼增益参数K；
基于观测传感器组中的每个传感器对应的观测车辆位姿和每个传感器对应的卡尔曼增益参数K对预测车辆位姿进行迭代更新，得到实际车辆位姿。
如此，可以实现将基于地面穿透雷达确定的观测车辆位姿和其他车辆位姿进行融合计算，降低了环境对车辆位姿的定位的影响，提高了车辆在特殊环境下对车辆位姿定位的准确性。
在上述第一方面的一种可能的实现中，将预测位姿与各个观测位姿进行融合定位计算，得到实际车辆位姿，包括：
获取前m个时刻的全球定位***GNSS和惯性测量单元IMU融合计算后的估计车辆位姿和观测传感器组中的各传感器各自对应的观测车辆位姿；
将估计车辆位姿和观测车辆位姿输入预设算法，计算得到第m+1时刻的车辆实际位姿。
如此，可以基于前m个时刻的观测车辆位姿和前m个时刻的估计车辆位姿来推测车辆的在前m个时刻的实际位姿，进而根据该前m个时刻的实际位姿，更准确的确定出第m+1个时刻的实际车辆位姿。
在上述第一方面的一种可能的实现中，观测传感器组包括激光雷达、图像采集设备中的至少一种。
第二方面，本申请实施例提供了一种融合定位***，***包括：
获取模块，用于获取由惯导***检测到的惯导数据，以及由观测传感器组采集到不同类型的观测数据，其中，所述观测传感器组为至少包括地面穿透雷达的传感器组合；
处理模块，用于根据所述惯导数据确定出车辆的预测位姿，并根据所述观测数据确定出车辆的各个观测位姿；将所述预测位姿与所述各个观测位姿进行融合定位计算，得到实际车辆位姿。
在上述第二方面的一种可能的实现中，获取模块具体用于：
根据激光雷达传感器在当前时刻采集到的点云数据和在当前时刻的前一时刻采集的点云数据，通过帧间匹配算法计算出当前时刻对应的观测车辆位姿；
根据图像采集设备在当前时刻采集到的图像数据和在当前时刻的前一时刻采集的图像数据，通过帧间匹配算法计算出当前时刻对应的观测车辆位姿；
根据地面穿透雷达传感器在当前时刻的前一时刻采集到的信号强度数据和在当前时刻采集的当前信号强度数据，使用帧间匹配算法计算出当前时刻对应的观测车辆位姿。
在上述第二方面的一种可能的实现中，处理模块具体用于：
确定观测传感器组中的每个传感器对应的卡尔曼增益参数K；
基于观测传感器组中的每个传感器对应的观测车辆位姿和每个传感器对应的卡尔曼增益参数K对预测车辆位姿进行迭代更新，得到实际车辆位姿。
第三方面，本申请实施例提供了一种电子设备，包括：存储器，用于存储由电子设备的一个或多个处理器执行的指令，以及处理器，是电子设备的处理器之一，用于执行第一方面任一项所述的融合定位方法。
第四方面，本申请实施例提供了一种可读介质，所述可读介质上存储有指令，该指令在电子设备上执行时使电子设备执行第一方面任一项所述的融合定位方法。
上述第二方面至第四方面中的各个方面以及各个方面可能达到的技术效果请参照上述针对第一方面或第一方面中的各种可能方案可以达到的技术效果说明，这里不再重复赘述。
附图说明
图1根据本申请的一些实施例，示出了一种融合定位方法的流程示意图；
图2根据本申请的一些实施例，示出了一种车辆200的结构示意图；
图3根据本申请的一些实施例，示出了又一种融合定位方法的流程示意图；
图4根据本申请的一些实施例，示出了一种车辆200在0时刻至m+1时刻的场景示意图；
图5根据本申请的一些实施例，示出了一种融合定位***的结构示意图；
图6根据本申请的一些实施例，示出了一种适用于本申请实施例提供的融合定位方法的电子设备100的结构示意图。
具体实施方式
为了使本申请的目的、技术方案和优点更加清楚，下面将结合附图对本申请作进一步地详细描述。方法实施例中的具体操作方法也可以应用于装置实施例或***实施例中。需要说明的是，在本申请的描述中“多个”理解为“至少两个”。“和/或”，描述关联对象的关联关系，表示可以存在三种关系，例如，A和/或B，可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。A与B连接，可以表示：A与B直接连接和A与B通过C连接这两种情况。另外，在本申请的描述中，“第一”、“第二”等词汇，仅用于区分描述的目的，而不能理解为指示或暗示相对重要性，也不能理解为指示或暗示顺序。
首先，对本申请实施例中的部分术语进行解释说明，以便本领域技术人员理解。
激光雷达：由发射***、接收***、信息处理等部分组成。它利用激光对周围环境进行密集采样，以产生高精度的坐标和强度测量值。能生成带坐标的点云数据。
点云数据：经过后处理，从空间上进行过组织的激光雷达数据被称为点云数据。点云一般包括扫描的物体的点的xyz坐标值(或极坐标值)，反射强度值以及时间戳等属性。
约束：对于质点和质点系中的各质点位置和速度的限制成为约束。
约束方程：表示质点和质点系中的各质点位置和速度的限制条件的数学方程称为约束方程。
双面约束：如果约束在两个方向都起限制作用，称为双面约束。
单面约束：如果约束在一个方向起限制作用，称为单面约束。
下面结合附图，对本申请实施例进行详细描述。
现有的融合定位多基于激光雷达、视觉传感器等设备采集的信息进行融合定位，然而，在特殊情况如极端环境下，例如暴雪、隧道等环境下，由于受环境影响，激光雷达、视觉传感器无法采集到有用信息，进而无法较为准确的定位。
有鉴于此，本申请提供了一种融合定位的方法，通过将地面穿透雷达作为融合定位的目标之一，将根据地面穿透雷达采集的信息计算得到的车辆位姿和根据其他传感器设备采集的信息计算得到的车辆位姿进行融合计算，得到车辆位姿。该车辆位姿的融合计算过程，利用地面穿透雷达采集的信息为地面以下的土壤岩石等不易受极端环境影响的特性，得到的车辆位姿更为精准，从而实现了在极端环境下的高精度定位。
具体地，图1根据本申请的一些实施例，示出了一种基于滤波方式的融合定位方法的流程示意图，该流程包括如下步骤：
S101：获取由惯导***检测到的惯导数据，以及由观测传感器组采集到不同类型的观测数据。
在本申请的一些实施例中，惯导***例如可以为GNSS和IMU组合，GNSS可采集天线信号，IMU可采集加速度和角速度信号，基于GNSS和IMU采集的信号数据即为由惯导数据。
在本申请的实施例中，观测传感器组至少包括地面穿透雷达，地面穿透雷达通过采集不同地下物体反射的信号强度，可以实现对地下物体进行区分。由于地面穿透雷达探测的物体为地下物体，又由于地下物体不易受环境或者天气的影响，从而可以有效提高在极端环境下对车辆位姿定位的精确度。
观测传感器组除包含地面穿透雷达后，还可以包含其他传感器，例如，激光雷达、图像采集设备、事件传感器等，本申请对观测传感器组中除地面穿透雷达外的传感器的数量和类型不做限制。
示例性的，图2根据本申请的一些实施例，示出了一种车辆200的结构示意图。如图2所示，车辆200上安装有惯导***201、激光雷达202、图像采集设备203以及地面穿透雷达204，也即观测传感器组为激光雷达202、图像采集设备203以及地面穿透雷达204。在本实施例中，将激光雷达202采集的点云数据作为其对应的观测数据、将图像采集设备203采集的图像数据作为其对应的观测数据、以及将地面穿透雷达204采集的信号强度数据作为其对应的观测数据。
S102:根据惯导数据确定出车辆的预测位姿，并根据观测数据确定出车辆的各个观测位姿。
在惯导数据为GNSS采集的数据和IMU采集的数据情况下，可以通过融合计算得到车辆的预测位姿。
在一些实施例中，可以通过帧间匹配算法计算出观测传感器组中各传感器对应的车辆位姿，也就是通过前后两个时刻采集到的数据进行匹配计算，从而根据该计算结果就可以确定出观测车辆位姿。例如，对于图2所示的观测传感器组，可以基于激光雷达202获取点云数据，通过帧间匹配算法对当前时刻的点云数据和当前时刻的前一时刻的点云数据进行匹配，进而确定出当前时刻t激光雷达202对应的观测车辆位姿。并且，基于图像采集设备203采集的图像数据，通过帧间匹配算法对当前时刻的图像数据和当前时刻的前一时刻进行匹配，可以确定当前时刻t图像采集设备203对应的观测车辆位姿。以及，通过地面穿透雷达204接收到的当前时刻对应的信号强度数据和当前时刻的前一时刻的信号强度数据，确定当前时刻t地面穿透雷达204对应的观测车辆位姿。如此，得到了时刻t的各传感器对应的观测车辆位姿。
在本申请的一些实施例中，车辆位姿可以通过(x,y,z,yaw,roll,pitch)向量来表征。其中，(x,y,z)表示车辆的坐标，(yaw,roll,pitch)分别表示车辆的俯仰角、偏航角、翻滚角。
可以理解，前述帧间匹配算法可以是ICP、PL-ICP、NICP和IMLS-ICP等，本申请对此不做限制。
S103：将预测位姿与各个观测位姿进行融合定位计算，得到实际车辆位姿。
在本申请实施例中，在获取到预测位姿以及各个观测车辆位姿之后，可以通过如下两种方法来计算出车辆实际位姿：
方法一：
在一些实施例中，可以基于观测车辆位姿对预测车辆位姿进行修正，进而得到车辆实际位姿。
具体地，获取到观测传感器组中各传感器对应的观测车辆位姿后，可以基于观测传感器组中的不同传感器的卡尔曼增益参数K，对预测车辆位姿进行修正。卡尔曼增益参数可以通过如下公式(1)求得：
K＝P'HT(HP'HT+R)-1 (1)
其中，K为卡尔曼增益系数，H为测量矩阵，在本申请中H为1，P为协方差矩阵，可预先保存，R为常数。
求得各传感器对应的卡尔曼增益系数K后，结合各传感器的观测车辆位姿，对预测车辆位姿进行修正，即可以通过如下公式(2)来计算：
Xi＝Xi-1+KiYi (2)
其中，Ki表示观测传感器组中的第i个传感器对应的卡尔曼增益系数，Yi表示基于观测传感器组中的第i个传感器采集的数据得到的观测车辆位姿。当i＝1的时候，也即Xi-1为X0的时候，表示车辆的预测位姿，也即为经过GNSS和IMU融合计算后得到的位姿。Xi表示第i次修正后的车辆位姿。
具体地，遍历传感器组中的每个传感器，获取对应的Xi-1、Ki、Yi，直到遍历完成，得到的最后更新的车辆位姿即为车辆的实际位姿，也即，当i的值等于观测传感器组中传感器的个数时，Xi表示车辆的实际位姿。以观测传感器组为图2所示的车辆200中的观测传感器组为例，当i等于1时，获取激光雷达202对应的K1和Y1，以及预测车辆位姿X0，可以计算得到第一次修正后的车辆位姿X1。当i等于2时，获取图像采集设备203对应的K2和Y2，以及第一次修正后的车辆位姿X1，可以计算得到第二次修正后的车辆位姿X2。类似的，可以计算得到第三次修正后的车辆位姿X3。此时，观测传感器组中的传感器全部遍历完成，也即X3即为车辆的实际车辆位姿。
可以理解，上述修正顺序仅为示例性的，在另外一些实施例中，还可以基于其他顺序对预测车辆位姿进行修正，本申请对此不做限制。
可以理解，前述将GNSS和IMU融合后的位姿作为预测车辆位姿仅为示例性的，在另一些实施例中，还可以仅将IMU积分后得到的位姿作为预测车辆位姿，在本申请中不做限制。
可以理解，前述基于地面穿透雷达和激光雷达、图像采集设备接收的数据进行融合定位仅为示例性的，在另一些实施例中，还可以基于地面穿透雷达、事件传感器、车联网等进行融合定位，本申请对和地面穿透雷达进行融合定位的传感器类型和传感器数量不做限制。
可以理解，在另一些实施例中，还可以仅基于当前时刻接收到的传感器数据进行融合。例如，在某一时刻，在仅接收到地面穿透雷达、激光雷达检测的数据时，仅基于地面穿透雷达、激光雷达检测的数据进行融合。
可以理解，在接收到各传感器数据后，还可以基于各传感器的数据进行校验，舍弃偏差过大的数据。例如，在一些实施例中，可以使用卡方校验的方法抛弃偏差明显的观测车辆位姿。
基于地面穿透雷达采集的数据计算得到的观测车辆位姿对预测车辆位姿进行修正时，由于地面穿透雷达采集的信息为地下物件，不易天气和环境等影响，因此计算得到的观测车辆位姿也更为准确，可以有效解决在极端环境下，例如，处于雪天环境或者隧道等情况下对车辆位姿定位不准确的问题。
方法二：
具体地，图3示出了另外一种融合定位方法的流程示意图，步骤如下：
S301：获取前m个时刻的GNSS和IMU融合计算后的估计车辆位姿和前m个时刻的观测传感器组中的各传感器对应的观测车辆位姿。
例如，图4根据本申请的一些实施例，示出了一种车辆200在0时刻至m+1时刻的场景示意图。
如图4所示，根据车辆200中的惯导***201(例如GNSS和IMU融合计算)可以得到车辆在0时刻至m时刻的估计车辆位姿分别为H0、H1…Hm，也即图4中的滑动窗口中包含的车辆位姿有H0、H1…Hm。并且，在0时刻、1时刻…m时刻，都可以根据激光雷达202采集的数据，利用帧间匹配算法计算得到激光雷达202对应的观测车辆位姿。类似地，可以基于帧间匹配算法分别计算得到图像采集设备203(例如摄像机)对应的观测车辆位姿以及地面穿透雷达204对应的观测车辆位姿。
S302：根据估计车辆位姿和观测车辆位姿确定待优化位姿矩阵集合。
在本申请的实施例中，待优化位姿矩阵集合为滑动窗口需要进行位姿优化的车辆位姿集合。
例如，针对图4中的场景，待优化位姿矩阵集合即滑动窗口中的(0时刻至m时刻)的车辆位姿X0、X1…Xm。
S303：根据估计车辆位姿确定单边约束矩阵集合。
在本申请的一些实施例中，可以将GNSS和IMU计算得到的估计车辆位姿与滑动窗口中的待优化位姿矩阵组合，建立单面约束方程，从而确定出单边约束矩阵集合。
例如，针对图4中的场景，可以获取车辆在0时刻至m时刻的估计车辆位姿H0、H1…Hm(0时刻至m时刻，GNSS和IMU融合计算得到的位姿)，从而和滑动窗口中的(也即0时刻至m时刻)待优化车辆位姿矩阵集合X0、X1…Xm一一建立单面约束方程，从而确定出单边约束矩阵集合。
S304：根据待优化位姿矩阵集合和观测车辆位姿确定双边约束矩阵集合。
在本申请的一些实施例中，可以将基于观测传感器组中的传感器计算得到的观测车辆位姿与滑动窗口中的待优化位姿矩阵集合组合，建立双面约束方程，从而确定出双边约束矩阵集合。
例如，针对图4中的场景，可以由激光雷达202、图像采集设备203以及地面穿透雷达204与0时刻至m时刻中每一时刻的待优化位姿X0、X1…Xm分别建立双面约束方程，进而确定出双边约束矩阵集合。
S305：将待优化位姿矩阵集合、单边约束矩阵集合和双边约束矩阵集合输入预设的优化算法。
S306：采用预设的优化算法，以单边约束矩阵集合和双边约束矩阵集合为约束，对待优化位姿矩阵集合进行优化，获得优化后的位姿矩阵集合。
在一些实施例中，预设的优化算法可以为高斯牛顿算法(Gauss-Newton，GN)法和列文伯格-马夸尔特算法(Levenberg-Marquardt，LM)等，本申请对此不做限制。
基于预设的优化算法，即可确定优化后的位姿矩阵集合。
S307：根据优化后的位姿矩阵集合，确定第m+1时刻的实际车辆位姿。
例如，对于图4中的场景，通过优化后的位姿矩阵集合，可以确定第m+1时刻的实际车辆位姿。
可以理解，在本申请的一些实施例中，还可以将待优化位姿矩阵集合整体作为待优化变量，通过待优化位姿矩阵集合、单边约束矩阵集合以及双边约束矩阵集合构建位姿图，将待优化位姿矩阵集合中每个位姿点的位姿参数作为位姿图的每一个顶点，将约束关系作为位姿图的边，则可以通过图优化的形式，通过图模型来表达一个非线性最小二乘优化问题，从而确定出优化后的位姿矩阵集合。
例如，对于图4所示的场景，可以将滑动窗口中(也即0时刻至m时刻)的待优化位姿X0、X1…Xm作为位姿图的顶点，基于激光雷达202、图像采集设备203以及地面穿透雷达204计算出的各观测车辆位姿对待优化位姿的双面约束关系作为边，以及将GNSS和IMU融合计算得到的估计车辆位姿H0、H1…Hm对待优化位姿的单面约束关系作为边，从而对该位姿图进行优化，通过非线性最小二乘优化确定出优化后的0时刻到m时刻的车辆位姿。
可以理解，前述0时刻、m时刻等仅为示例性的，本申请对此不做限制。
本申请提供的融合定位方法，通过将地面穿透雷达作为融合定位的目标之一，融合计算过程利用了地面穿透雷达采集的信息为地面以下的土壤岩石等不易受极端环境影响的特性，得到的车辆位姿更为精准，从而实现了在极端环境下的高精度定位。
基于同一发明构思，本申请实施例还提供了一种融合定位***，如图5所示为本申请实施例提供的一种融合定位***的结构示意图，通过该融合定位***可以实现在极端环境下提高车辆位姿定位的准确度，该***包括：
获取模块501，用于获取由惯导***检测到的惯导数据，以及由观测传感器组采集到不同类型的观测数据，其中，所述观测传感器组为至少包括地面穿透雷达的传感器组合；
处理模块502，用于根据所述惯导数据确定出车辆的预测位姿，并根据所述观测数据确定出车辆的各个观测位姿；将所述预测位姿与所述各个观测位姿进行融合定位计算，得到实际车辆位姿。
在一些实施例中，所述处理模块502，具体用于根据激光雷达传感器在当前时刻采集到的点云数据和在当前时刻的前一时刻采集的点云数据，通过帧间匹配算法计算出当前时刻对应的观测车辆位姿；
根据图像采集设备在当前时刻采集到的图像数据和在当前时刻的前一时刻采集的图像数据，通过帧间匹配算法计算出当前时刻对应的观测车辆位姿；
根据地面穿透雷达传感器在当前时刻的前一时刻采集到的信号强度数据和在当前时刻采集的当前信号强度数据，使用帧间匹配算法计算出当前时刻对应的观测车辆位姿。
在一些实施例中，所述处理模块502，具体用于确定观测传感器组中的每个传感器对应的卡尔曼增益参数K；
基于观测传感器组中的每个传感器对应的观测车辆位姿和每个传感器对应的卡尔曼增益参数K对预测车辆位姿进行迭代更新，得到实际车辆位姿。
进一步，基于同一发明构思，本申请实施例中还提供了一种电子设备，图6示出了一种电子设备100的结构示意图。如图6所示，电子设备100包括一个或多个处理器101、***内存102、非易失性存储器(Non-Volatile Memory，NVM)103、通信接口104、输入/输出(I/O)设备105、以及用于耦接处理器101、***内存102、非易失性存储器103、通信接口104和输入/输出(I/O)设备105的***控制逻辑106。其中：
处理器101可以包括一个或多个处理单元，例如，可以包括中央处理器CPU(Central Processing Unit)、图像处理器GPU(Graphics Processing Unit)、数字信号处理器DSP(Digital Signal Processor)、微处理器MCU(Micro-programmed Control Unit)、AI(Artificial Intelligence，人工智能)处理器或可编程逻辑器件FPGA(FieldProgrammable Gate Array)、神经网络处理器(Neural-network Processing Unit，NPU)等的数据处理单元或处理电路可以包括一个或多个单核或多核处理器。在一些实施例中，处理器101可以用于执行指令实现上述将预测车辆位姿和观测车辆位姿融合计算的相关功能。
***内存102是易失性存储器，例如随机存取存储器(Random-Access Memory，RAM)，双倍数据率同步动态随机存取存储器(Double Data Rate Synchronous DynamicRandom Access Memory，DDR SDRAM)等。***内存用于临时存储数据和/或指令，例如，在本申请的一些实施例中，***内存102可以存储观测车辆位姿、估计车辆位姿等。
非易失性存储器103可以包括用于存储数据和/或指令的一个或多个有形的、非暂时性的计算机可读介质。在一些实施例中，非易失性存储器103可以包括闪存等任意合适的非易失性存储器和/或任意合适的非易失性存储设备，例如硬盘驱动器(Hard Disk Drive，HDD)、光盘(Compact Disc，CD)、数字通用光盘(Digital Versatile Disc，DVD)、固态硬盘(Solid-State Drive，SSD)等。在一些实施例中，非易失性存储器103也可以存储观测车辆位姿、估计车辆位姿等。
特别地，***内存102和非易失性存储器103可以分别包括：指令107的临时副本和永久副本。指令107可以包括：由处理器101中的至少一个执行时使电子设备100实现本申请各实施例提供的融合定位方法。
通信接口104可以包括收发器，用于为电子设备100提供有线或无线通信接口，进而通过一个或多个网络与任意其他合适的设备进行通信。在一些实施例中，通信接口104可以集成于电子设备100的其他组件，例如通信接口104可以集成于处理器101中。
输入/输出(I/O)设备105可以包括输入设备如键盘、鼠标等，输出设备如显示器等，用户可以通过输入/输出(I/O)设备105与电子设备100进行交互，例如业务人员可以通过输入/输出(I/O)设备105输入/选择进行数据变更的内容。
***控制逻辑106可以包括任意合适的接口控制器，以电子设备100的其他模块提供任意合适的接口。例如在一些实施例中，***控制逻辑106可以包括一个或多个存储器控制器，以提供连接到***内存102和非易失性存储器103的接口。
在一些实施例中，处理器101中的至少一个可以与用于***控制逻辑106的一个或多个控制器的逻辑封装在一起，以形成***封装(System in Package，SiP)。在另一些实施例中，处理器101中的至少一个还可以与用于***控制逻辑106的一个或多个控制器的逻辑集成在同一芯片上，以形成片上***(System-on-Chip，SoC)。
可以理解，图6所示的电子设备100的结构只是一种示例，在另一些实施例中，电子设备100可以包括比图示更多或更少的部件，或者组合某些部件，或者拆分某些部件，或者不同的部件布置。图示的部件可以以硬件，软件或软件和硬件的组合实现。
本申请实施例还提供了一种程序产品，用于实现上述各实施例提供的融合定位方法。
本申请公开的机制的各实施例可以被实现在硬件、软件、固件或这些实现方法的组合中。本申请的实施例可实现为在可编程***上执行的计算机模块或模块代码，该可编程***包括至少一个处理器、存储***(包括易失性和非易失性存储器和/或存储元件)、至少一个输入设备以及至少一个输出设备。
可将模块代码应用于输入指令，以执行本申请描述的各功能并生成输出信息。可以按已知方式将输出信息应用于一个或多个输出设备。为了本申请的目的，处理***包括具有诸如例如数字信号处理器(Digital Signal Processor，DSP)、微控制器、专用集成电路(Application Specific Integrated Circuit，ASIC)或微处理器之类的处理器的任何***。
模块代码可以用高级模块化语言或面向对象的编程语言来实现，以便与处理***通信。在需要时，也可用汇编语言或机器语言来实现模块代码。事实上，本申请中描述的机制不限于任何特定编程语言的范围。在任一情形下，该语言可以是编译语言或解释语言。
在一些情况下，所公开的实施例可以以硬件、固件、软件或其任何组合来实现。所公开的实施例还可以被实现为由一个或多个暂时或非暂时性机器可读(例如，计算机可读)存储介质承载或存储在其上的指令，其可以由一个或多个处理器读取和执行。例如，指令可以通过网络或通过其他计算机可读介质分发。因此，机器可读介质可以包括用于以机器(例如，计算机)可读的形式存储或传输信息的任何机制，包括但不限于，软盘、光盘、光碟、只读存储器(CD-ROMs)、磁光盘、只读存储器(Read Only Memory，ROM)、随机存取存储器(RandomAccess Memory，RAM)、可擦除可编程只读存储器(Erasable Programmable Read OnlyMemory，EPROM)、电可擦除可编程只读存储器(Electrically Erasable ProgrammableRead-Only Memory，EEPROM)、磁卡或光卡、闪存、或用于利用因特网以电、光、声或其他形式的传播信号来传输信息(例如，载波、红外信号数字信号等)的有形的机器可读存储器。因此，机器可读介质包括适合于以机器(例如计算机)可读的形式存储或传输电子指令或信息的任何类型的机器可读介质。
在附图中，可以以特定布置和/或顺序示出一些结构或方法特征。然而，应该理解，可能不需要这样的特定布置和/或排序。而是，在一些实施例中，这些特征可以以不同于说明性附图中所示的方式和/或顺序来布置。另外，在特定图中包括结构或方法特征并不意味着暗示在所有实施例中都需要这样的特征，并且在一些实施例中，可以不包括这些特征或者可以与其他特征组合。
虽然通过参照本申请的某些优选实施例，已经对本申请进行了图示和描述，但本领域的普通技术人员应该明白，可以在形式上和细节上对其作各种改变，而不偏离本申请的精神和范围。

Claims (9)

1.一种融合定位方法，其特征在于，包括：
获取由惯导***检测到的惯导数据，以及由观测传感器组采集到不同类型的观测数据，其中，所述观测传感器组为至少包括地面穿透雷达的传感器组合；
根据所述惯导数据确定出车辆的预测位姿，并根据所述观测数据确定出车辆的各个观测位姿；
获取前m个时刻的全球定位***GNSS和惯性测量单元IMU融合计算后的估计车辆位姿和观测传感器组中的各传感器各自对应的观测车辆位姿；
根据估计车辆位姿和观测车辆位姿确定待优化位姿矩阵集合；
根据估计车辆位姿确定单边约束矩阵集合；
根据所述待优化位姿矩阵集合和观测车辆位姿确定双边约束矩阵集合；
将所述待优化位姿矩阵集合、所述单边约束矩阵集合和所述双边约束矩阵集合输入预设的优化算法；
采用预设的优化算法，以所述单边约束矩阵集合和所述双边约束矩阵集合为约束，对所述待优化位姿矩阵集合进行优化，获得优化后的位姿矩阵集合；
根据优化后的位姿矩阵集合，确定第m+1时刻的实际车辆位姿。
2.如权利要求1所述的方法，其特征在于，所述根据所述观测数据确定出车辆的各个观测位姿，包括：
根据激光雷达传感器在当前时刻采集到的点云数据和在当前时刻的前一时刻采集的点云数据，通过帧间匹配算法计算出当前时刻对应的观测车辆位姿；
根据图像采集设备在当前时刻采集到的图像数据和在当前时刻的前一时刻采集的图像数据，通过帧间匹配算法计算出当前时刻对应的观测车辆位姿；
根据地面穿透雷达传感器在当前时刻的前一时刻采集到的信号强度数据和在当前时刻采集的当前信号强度数据，使用帧间匹配算法计算出当前时刻对应的观测车辆位姿。
3.如权利要求1所述的方法，其特征在于，将所述预测位姿与所述各个观测位姿进行融合定位计算，得到实际车辆位姿，包括：
确定观测传感器组中的每个传感器对应的卡尔曼增益参数K；
基于观测传感器组中的每个传感器对应的观测车辆位姿和每个传感器对应的卡尔曼增益参数K对预测车辆位姿进行迭代更新，得到实际车辆位姿。
4.如权利要求1所述的方法，其特征在于，所述观测传感器组包括激光雷达、图像采集设备中的至少一种。
5.一种融合定位***，其特征在于，所述***包括：
获取模块，用于获取由惯导***检测到的惯导数据，以及由观测传感器组采集到不同类型的观测数据，其中，所述观测传感器组为至少包括地面穿透雷达的传感器组合；
处理模块，用于根据所述惯导数据确定出车辆的预测位姿，并根据所述观测数据确定出车辆的各个观测位姿；获取前m个时刻的全球定位***GNSS和惯性测量单元IMU融合计算后的估计车辆位姿和观测传感器组中的各传感器各自对应的观测车辆位姿；
根据估计车辆位姿和观测车辆位姿确定待优化位姿矩阵集合；
根据估计车辆位姿确定单边约束矩阵集合；
根据所述待优化位姿矩阵集合和观测车辆位姿确定双边约束矩阵集合；
将所述待优化位姿矩阵集合、所述单边约束矩阵集合和所述双边约束矩阵集合输入预设的优化算法；
采用预设的优化算法，以所述单边约束矩阵集合和所述双边约束矩阵集合为约束，对所述待优化位姿矩阵集合进行优化，获得优化后的位姿矩阵集合；
根据优化后的位姿矩阵集合，确定第m+1时刻的实际车辆位姿。
6.如权利要求5所述的***，其特征在于，所述处理模块，具体用于:
根据激光雷达传感器在当前时刻采集到的点云数据和在当前时刻的前一时刻采集的点云数据，通过帧间匹配算法计算出当前时刻对应的观测车辆位姿；
根据图像采集设备在当前时刻采集到的图像数据和在当前时刻的前一时刻采集的图像数据，通过帧间匹配算法计算出当前时刻对应的观测车辆位姿；
根据地面穿透雷达传感器在当前时刻的前一时刻采集到的信号强度数据和在当前时刻采集的当前信号强度数据，使用帧间匹配算法计算出当前时刻对应的观测车辆位姿。
7.如权利要求5所述的***，其特征在于，所述处理模块，具体用于:
确定观测传感器组中的每个传感器对应的卡尔曼增益参数K；
基于观测传感器组中的每个传感器对应的观测车辆位姿和每个传感器对应的卡尔曼增益参数K对预测车辆位姿进行迭代更新，得到实际车辆位姿。
8.一种电子设备，其特征在于，包括：
存储器，用于存储由电子设备的一个或多个处理器执行的指令，以及处理器，是电子设备的处理器之一，用于执行权利要求1至4中任一项所述的融合定位方法。
9.一种可读介质，其特征在于，所述可读介质上存储有指令，该指令在电子设备上执行时使电子设备执行权利要求1至4中任一项所述的融合定位方法。
CN202210430709.XA 2022-04-22 2022-04-22 一种融合定位方法、介质和电子设备 Active CN114739402B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210430709.XA CN114739402B (zh) 2022-04-22 2022-04-22 一种融合定位方法、介质和电子设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210430709.XA CN114739402B (zh) 2022-04-22 2022-04-22 一种融合定位方法、介质和电子设备

Publications (2)

Publication Number Publication Date
CN114739402A CN114739402A (zh) 2022-07-12
CN114739402B true CN114739402B (zh) 2024-06-25

Family

ID=82284665

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210430709.XA Active CN114739402B (zh) 2022-04-22 2022-04-22 一种融合定位方法、介质和电子设备

Country Status (1)

Country Link
CN (1) CN114739402B (zh)

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110906923A (zh) * 2019-11-28 2020-03-24 重庆长安汽车股份有限公司 车载多传感器紧耦合融合定位方法、***、存储介质及车辆
CN111595333A (zh) * 2020-04-26 2020-08-28 武汉理工大学 视觉惯性激光数据融合的模块化无人车定位方法及***

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
GB2579415B (en) * 2018-11-30 2021-11-10 Thales Holdings Uk Plc Method and apparatus for determining a position of a vehicle
CN112154303B (zh) * 2019-07-29 2024-04-05 深圳市大疆创新科技有限公司 高精度地图定位方法、***、平台及计算机可读存储介质
CN113984044A (zh) * 2021-10-08 2022-01-28 杭州鸿泉物联网技术股份有限公司 一种基于车载多感知融合的车辆位姿获取方法及装置
CN113933818A (zh) * 2021-11-11 2022-01-14 阿波罗智能技术(北京)有限公司 激光雷达外参的标定的方法、设备、存储介质及程序产品
CN114264301B (zh) * 2021-12-13 2024-06-21 青岛慧拓智能机器有限公司 车载多传感器融合定位方法、装置、芯片及终端

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110906923A (zh) * 2019-11-28 2020-03-24 重庆长安汽车股份有限公司 车载多传感器紧耦合融合定位方法、***、存储介质及车辆
CN111595333A (zh) * 2020-04-26 2020-08-28 武汉理工大学 视觉惯性激光数据融合的模块化无人车定位方法及***

Also Published As

Publication number Publication date
CN114739402A (zh) 2022-07-12

Similar Documents

Publication Publication Date Title
JP6812404B2 (ja) 点群データを融合させるための方法、装置、コンピュータ読み取り可能な記憶媒体、及びコンピュータプログラム
JP6831414B2 (ja) 測位のための方法、測位のための装置、デバイス及びコンピュータ読み取り可能な記憶媒体
KR102525227B1 (ko) 도로 정보 데이터를 결정하는 방법, 장치, 전자 기기, 저장 매체 및 프로그램
CN109435955B (zh) 一种自动驾驶***性能评估方法、装置、设备及存储介质
WO2019092418A1 (en) Method of computer vision based localisation and navigation and system for performing the same
EP3669141A1 (en) Vehicle positioning system using lidar
Levinson Automatic laser calibration, mapping, and localization for autonomous vehicles
JP7245084B2 (ja) 自動運転システム
US11295521B2 (en) Ground map generation
US20210404814A1 (en) Map Generation Using Two Sources of Sensor Data
CN113933818A (zh) 激光雷达外参的标定的方法、设备、存储介质及程序产品
CN114111775B (zh) 一种多传感器融合定位方法、装置、存储介质及电子设备
US11796324B2 (en) Vehicle control device
CN112380312A (zh) 基于栅格检测的激光地图更新方法、终端及计算机设备
CN112652062B (zh) 一种点云地图构建方法、装置、设备和存储介质
CN114248778B (zh) 移动设备的定位方法和定位装置
CN115900712B (zh) 一种信源可信度评价组合定位方法
CN115183762A (zh) 一种机场仓库内外建图方法、***、电子设备及介质
Khoshelham et al. Vehicle positioning in the absence of GNSS signals: Potential of visual-inertial odometry
CN115077519A (zh) 基于模板匹配与激光惯导松耦合的定位建图方法和装置
CN112967393B (zh) 车辆移动轨迹的修正方法、装置、电子设备及存储介质
CN114739402B (zh) 一种融合定位方法、介质和电子设备
WO2023222671A1 (en) Position determination of a vehicle using image segmentations
CN113034538B (zh) 一种视觉惯导设备的位姿跟踪方法、装置及视觉惯导设备
CN114440864A (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
CB02 Change of applicant information

Address after: 314500 988 Tong Tong Road, Wu Tong Street, Tongxiang, Jiaxing, Zhejiang

Applicant after: United New Energy Automobile Co.,Ltd.

Address before: 314500 988 Tong Tong Road, Wu Tong Street, Tongxiang, Jiaxing, Zhejiang

Applicant before: Hozon New Energy Automobile Co., Ltd.

CB02 Change of applicant information
GR01 Patent grant