CN112433211B - 一种位姿确定方法及装置、电子设备和存储介质 - Google Patents

一种位姿确定方法及装置、电子设备和存储介质 Download PDF

Info

Publication number
CN112433211B
CN112433211B CN202011362399.XA CN202011362399A CN112433211B CN 112433211 B CN112433211 B CN 112433211B CN 202011362399 A CN202011362399 A CN 202011362399A CN 112433211 B CN112433211 B CN 112433211B
Authority
CN
China
Prior art keywords
line segment
pose
radar
map
segment map
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
CN202011362399.XA
Other languages
English (en)
Other versions
CN112433211A (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.)
Zhejiang Shangtang Technology Development Co Ltd
Original Assignee
Zhejiang Shangtang Technology Development 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 Zhejiang Shangtang Technology Development Co Ltd filed Critical Zhejiang Shangtang Technology Development Co Ltd
Priority to CN202011362399.XA priority Critical patent/CN112433211B/zh
Publication of CN112433211A publication Critical patent/CN112433211A/zh
Priority to PCT/CN2021/092487 priority patent/WO2022110653A1/zh
Application granted granted Critical
Publication of CN112433211B publication Critical patent/CN112433211B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/88—Radar or analogous systems specially adapted for specific applications
    • 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/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/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
    • G01C21/30—Map- or contour-matching
    • 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
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/86—Combinations of radar systems with non-radar systems, e.g. sonar, direction finder
    • 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
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/88—Radar or analogous systems specially adapted for specific applications
    • G01S13/881—Radar or analogous systems specially adapted for specific applications for robotics

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

Abstract

本公开涉及一种位姿确定方法及装置、电子设备和存储介质，所述方法包括：根据行驶设备在线段地图中的预估位姿，确定所述行驶设备的雷达在所述线段地图中的预估位姿，所述线段地图中的事物通过线段表示；根据雷达的预估位姿，对所述雷达采集的点云与所述线段地图中的线段进行匹配，并根据匹配结果确定匹配后所述雷达在所述线段地图中的位姿；根据匹配后所述雷达的位姿，确定所述行驶设备在所述线段地图中的目标位姿。本公开实施例可减少位姿确定过程的计算量，提高了行驶设备位姿确定的效率。

Description

一种位姿确定方法及装置、电子设备和存储介质
技术领域
本公开涉及计算机技术领域，尤其涉及一种位姿确定方法及装置、电子设备和存储介质。
背景技术
目前，基于雷达的位姿估计技术已经取得了广泛应用，例如应用于智能车的自动驾驶或辅助驾驶中，通过雷达来估计智能车的位置和朝向，能够帮助智能车合理规划下一步的驾驶路线。
在相关技术中，智能车往往需要及时地得到当前位姿，然而，由于智能车等行驶设备的处理性能往往有限，因此，急需减少位姿估计过程的计算量，以提高位姿估计的速度。
发明内容
本公开提出了一种位姿确定的技术方案。
根据本公开的一方面，提供了一种位姿确定方法，包括：
根据行驶设备在线段地图中的预估位姿，确定所述行驶设备的雷达在所述线段地图中的预估位姿，所述线段地图中的事物通过线段表示；
根据雷达的预估位姿，对所述雷达采集的点云与所述线段地图中的线段进行匹配，并根据匹配结果确定匹配后所述雷达在所述线段地图中的位姿；
根据匹配后所述雷达的位姿，确定所述行驶设备在所述线段地图中的目标位姿。
在一种可能的实现方式中，所述根据行驶设备在线段地图中的预估位姿，确定所述行驶设备的雷达在所述线段地图中的预估位姿，包括：
利用所述行驶设备在多个第一时刻的预估位姿，通过插值处理得到所述行驶设备在第二时刻的预估位姿，所述第二时刻包括所述雷达采集所述点云的时刻；
根据所述行驶设备在所述第二时刻的预估位姿，得到所述雷达在所述第二时刻的预估位姿。
在一种可能的实现方式中，在所述对所述雷达采集的点云与所述线段地图中的线段进行匹配前，还包括：
根据所述行驶设备的预估位姿中的朝向，确定所述线段地图中满足第一预设条件的目标线段；
对所述雷达采集的点云与所述线段地图中的线段进行匹配，包括：
对所述雷达采集的点云与所述目标线段进行匹配。
在一种可能的实现方式中，所述第一预设条件，包括：
线段与所述朝向的夹角的绝对值小于第一阈值；
线段与第一线段的距离小于距离阈值，所述第一线段为与所述朝向的夹角的绝对值小于第二阈值的线段。
在一种可能的实现方式中，根据雷达的预估位姿，对所述雷达采集的点云与所述线段地图中的线段进行匹配，并根据匹配结果确定匹配后所述雷达在所述线段地图中的位姿，包括：
根据所述雷达的预估位姿，确定所述雷达扫描的点云在所述线段地图中的坐标；
根据所述点云在所述线段地图中的坐标和所述线段地图中的线段，确定目标转换矩阵，使得所述坐标处的点云经所述目标转换矩阵平移旋转后与所述线段地图中的线段匹配；
通过所述目标转换矩阵，对所述雷达的预估位姿进行平移旋转，得到所述雷达在所述线段地图中的位姿。
在一种可能的实现方式中，根据匹配后所述雷达的位姿，确定所述行驶设备在所述线段地图中的目标位姿，包括：
根据匹配后所述雷达的位姿，确定匹配后所述行驶设备的位姿；
将匹配后所述行驶设备的位姿与所述行驶设备的预估位姿进行融合，得到所述行驶设备在所述线段地图中的目标位姿。
在一种可能的实现方式中，响应于所述位姿确定方法周期性执行的情况下，在所述确定所述行驶设备的雷达在所述线段地图中的预估位姿前，还包括：
根据上一周期确定的所述行驶设备的目标位姿，以及当前周期所述行驶设备采集的传感数据，对所述行驶设备在线段地图中的位姿进行预估，得到当前周期所述行驶设备的预估位姿，所述传感数据为基于所述行驶设备的运动信息采集的。
在一种可能的实现方式中，所述线段地图是通过对初始地图进行直线拟合得到的，所述初始地图包括：占据栅格地图和/或场地设计图。
在一种可能的实现方式中，所述行驶设备包括基于嵌入式平台的行驶设备，所述行驶设备运行于沙盘中，所述线段地图为所述沙盘的地图。
根据本公开的一方面，提供了一种位姿确定装置，包括：
预估位姿确定单元，用于根据行驶设备在线段地图中的预估位姿，确定所述行驶设备的雷达在所述线段地图中的预估位姿，所述线段地图中的事物通过线段表示；
雷达位姿确定单元，用于根据雷达的预估位姿，对所述雷达采集的点云与所述线段地图中的线段进行匹配，并根据匹配结果确定匹配后所述雷达在所述线段地图中的位姿；
目标位姿确定单元，用于根据匹配后所述雷达的位姿，确定所述行驶设备在所述线段地图中的目标位姿。
在一种可能的实现方式中，所述预估位姿确定单元，包括：
利用所述行驶设备在多个第一时刻的预估位姿，通过插值处理得到所述行驶设备在第二时刻的预估位姿，所述第二时刻包括所述雷达采集所述点云的时刻；
根据所述行驶设备在所述第二时刻的预估位姿，得到所述雷达在所述第二时刻的预估位姿。
在一种可能的实现方式中，所述装置还包括：
目标线段确定单元，用于根据所述行驶设备的预估位姿中的朝向，确定所述线段地图中满足第一预设条件的目标线段；
所述雷达位姿确定单元，用于对所述雷达采集的点云与所述目标线段进行匹配。
在一种可能的实现方式中，所述第一预设条件，包括：
线段与所述朝向的夹角的绝对值小于第一阈值；
线段与第一线段的距离小于距离阈值，所述第一线段为与所述朝向的夹角的绝对值小于第二阈值的线段。
在一种可能的实现方式中，所述雷达位姿确定单元，用于根据所述雷达的预估位姿，确定所述雷达扫描的点云在所述线段地图中的坐标；根据所述点云在所述线段地图中的坐标和所述线段地图中的线段，确定目标转换矩阵，使得所述坐标处的点云经所述目标转换矩阵平移旋转后与所述线段地图中的线段匹配；通过所述目标转换矩阵，对所述雷达的预估位姿进行平移旋转，得到所述雷达在所述线段地图中的位姿。
在一种可能的实现方式中，所述目标位姿确定单元，用于根据匹配后所述雷达的位姿，确定匹配后所述行驶设备的位姿；将匹配后所述行驶设备的位姿与所述行驶设备的预估位姿进行融合，得到所述行驶设备在所述线段地图中的目标位姿。
在一种可能的实现方式中，所述装置还包括：
当前预估位姿确定单元，用于根据上一周期确定的所述行驶设备的目标位姿，以及当前周期所述行驶设备采集的传感数据，对所述行驶设备在线段地图中的位姿进行预估，得到当前周期所述行驶设备的预估位姿，所述传感数据为基于所述行驶设备的运动信息采集的。
在一种可能的实现方式中，所述线段地图是通过对初始地图进行直线拟合得到的，所述初始地图包括：占据栅格地图和/或场地设计图。
在一种可能的实现方式中，所述行驶设备包括基于嵌入式平台的行驶设备，所述行驶设备运行于沙盘中，所述线段地图为所述沙盘的地图。
根据本公开的一方面，提供了一种电子设备，包括：处理器；用于存储处理器可执行指令的存储器；其中，所述处理器被配置为调用所述存储器存储的指令，以执行上述方法。
根据本公开的一方面，提供了一种计算机可读存储介质，其上存储有计算机程序指令，所述计算机程序指令被处理器执行时实现上述方法。
在本公开实施例中，能够根据行驶设备在线段地图中的预估位姿来确定行驶设备的雷达在线段地图中的预估位姿，通过根据预先估计的预估位姿，来对雷达采集的点云与线段地图中的线段进行匹配，缩小了线段地图中进行匹配的区域的范围，减少了位姿确定过程的计算量。另外，通过用线段简略地表示地图中的事物，在匹配时，是利用点云与线段地图中的线段进行匹配，减少了位姿确定过程的计算量，提高了行驶设备位姿确定的效率。
应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，而非限制本公开。根据下面参考附图对示例性实施例的详细说明，本公开的其它特征及方面将变得清楚。
附图说明
此处的附图被并入说明书中并构成本说明书的一部分，这些附图示出了符合本公开的实施例，并与说明书一起用于说明本公开的技术方案。
图1示出根据本公开实施例的位姿确定方法的流程图；
图2示出根据本公开实施例的一种线段地图中目标线段的示意图；
图3示出根据本公开实施例的另一种线段地图中目标线段的示意图；
图4示出根据本公开实施例的一种位姿确定装置的框图；
图5示出根据本公开实施例的一种电子设备的框图；
图6示出根据本公开实施例的一种电子设备的框图。
具体实施方式
以下将参考附图详细说明本公开的各种示例性实施例、特征和方面。附图中相同的附图标记表示功能相同或相似的元件。尽管在附图中示出了实施例的各种方面，但是除非特别指出，不必按比例绘制附图。
在这里专用的词“示例性”意为“用作例子、实施例或说明性”。这里作为“示例性”所说明的任何实施例不必解释为优于或好于其它实施例。
本文中术语“和/或”，仅仅是一种描述关联对象的关联关系，表示可以存在三种关系，例如，A和/或B，可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。另外，本文中术语“至少一种”表示多种中的任意一种或多种中的至少两种的任意组合，例如，包括A、B、C中的至少一种，可以表示包括从A、B和C构成的集合中选择的任意一个或多个元素。
另外，为了更好地说明本公开，在下文的具体实施方式中给出了众多的具体细节。本领域技术人员应当理解，没有某些具体细节，本公开同样可以实施。在一些实例中，对于本领域技术人员熟知的方法、手段、元件和电路未作详细描述，以便于凸显本公开的主旨。
图1示出根据本公开实施例的位姿确定方法的流程图，如图1所示，所述位姿确定方法包括：
在步骤S11中，根据行驶设备在线段地图中的预估位姿，确定所述行驶设备的雷达在所述线段地图中的预估位姿。
线段地图中的事物通过线段进行表示，例如通过线段来表示道路和建筑物等等，线段地图的具体实现方式可以有很多种，后文会结合本公开可能的实现方式，对线段地图做详细描述，此处不作赘述。
这里的行驶设备可以是地理位置可变更的设备，地理位置的变更可以是行驶设备自主变更，也可以是由外力导致的变更。行驶设备例如可以是车辆、船舶、飞行器等设备，或者也可以是机器人，例如扫地机器人、搬运机器人等等。
位姿可以包括位置和姿态，位置可以是设备在地图中的坐标，姿态可以是设备在地图中的方向。
行驶设备的预估位姿可以是在步骤S13中的匹配进行前预先估计的位姿，具体可以是根据行驶设备目标时刻确定的位姿进行估计得到的，后文会结合本公开可能的实现方式对行驶设备的预估位姿做详细描述。
行驶设备的雷达在所述线段地图中的预估位姿，可以是根据行驶设备与雷达的位姿的相对关系，由行驶设备的预估位姿来确定的。在雷达底座在行驶设备上固定的情况下，该相对关系往往是固定不变的，因此，该相对关系可以预先由人工确定，即该相对关系为预设值。
在步骤S12中，根据所述雷达的预估位姿，对所述雷达采集的点云与所述线段地图中的线段进行匹配，并根据匹配结果确定匹配后所述雷达在所述线段地图中的位姿。
由于雷达采集的点云是位于雷达的极坐标系中的，该坐标系例如是以雷达为中心点的极坐标系，雷达对周围事物进行扫描得到的点云位于极坐标系中的中心点周围。
在得到了雷达在线段地图中的预估位姿后，即确定了雷达在世界坐标系中的估计位置和姿态，因此，可以根据雷达在世界坐标系中的预估位置和姿态，将极坐标系中的点云映射到世界坐标系中，得到雷达扫描的点云在世界坐标系中的位置和姿态。即根据雷达在线段地图中的预估位姿，和雷达扫描的点云在雷达极坐标系中的位姿，得到雷达扫描的点云在线段地图中的位姿。
在得到雷达扫描的点云在线段地图中的位姿后，由于该位姿为预先估计的位姿，即预先估计的雷达扫描的点云在线段地图中的大体位置和方向，因此，可以根据雷达扫描的点云在线段地图中的大体位置和方向，对雷达采集的点云与线段地图中的线段进行匹配，而不用对整个线段地图中的所有线段进行匹配，可以减少匹配时线段的数量，提高效率。
这里所说的“匹配”，可理解为在线段地图中，寻找与点云所表示的事物相对应的线段，在一个示例中，可以通过对点云进行平移旋转，去寻找尽可能与点云重合的线段，一旦找到与点云的重合程度达到预期的线段，则视为匹配完成。
在点云与所述线段地图中的线段进行匹配的过程中，会对点云进行移动和旋转，在线段地图中寻找与点云尽可能地重合的线段。由于点云是雷达对周围事物扫描得到的点的相对位置信息，而线段地图中的线段表征的也是事物的位置信息，因此，在点云与线段匹配的情况下，可以根据点云在线段地图中的位置反推雷达的位姿，反推出来的位姿即可认为是扫描点云时雷达的位姿。
后文会结合本公开可能的实现方式对匹配的过程进行详细描述，此处不作赘述。
在步骤S13中，根据匹配后所述雷达的位姿，确定所述行驶设备在所述线段地图中的目标位姿。
在雷达的位姿确定后，可以根据行驶设备与雷达的位姿的相对关系，来确定行驶设备在线段地图中的位姿，为便于后续描述，这里将该位姿称为目标位姿。另外，在确定目标位姿时，还可以根据其它因素来确定，后文会结合本公开可能的实现方式进行详细描述，此处不作赘述。
在本公开实施例中，能够根据行驶设备在线段地图中的预估位姿来确定行驶设备的雷达在线段地图中的预估位姿，通过根据预先估计的预估位姿，来对雷达采集的点云与线段地图中的线段进行匹配，缩小了线段地图中进行匹配的区域的范围，减少了位姿确定过程的计算量。另外，通过用线段简略地表示地图中的事物，在匹配时，是利用点云与线段地图中的线段进行匹配，减少了位姿确定过程的计算量，提高了行驶设备位姿确定的效率。
本公开提供的位姿确定方法还可以有多种实现方式，如前文所述，本公开实施例中，通过线段地图进行点线匹配来确定行驶设备的位姿，在一种可能的实现方式中，所述线段地图是通过对初始地图进行直线拟合得到的，所述初始地图包括：占据栅格地图和/或场地设计图。
占据栅格地图的构建方式可以有多种，例如，可以通过即时定位与建图算法(GMapping)对行驶设备的行驶场地进行建图，得到行驶场地的占据栅格地图。
对初始地图进行直线拟合的方式可以有多种，例如，基于最小二乘法的直线拟合、基于梯度下降法的直线拟合。
本公开实施例中，通过直接对初始图像进行直线拟合，即可得到线段地图，提高了绘制线段地图的效率。另外，直接基于场地设计图来确定线段地图，由于场地设计图往往是较为简单的线条图，因此，更加适用于直线拟合算法，得到的线段地图的准确性更高，且无需通过其它算法对初始地图进行构建，便利性较高。
另外，得到的线段地图相对于其它地图，是非常轻量级的地图，能够提高位姿估计的速度。
如前文所述，行驶设备的预估位姿可以是根据行驶设备目标时刻确定的位姿进行估计得到的，在一种可能的实现方式中，在本公开的位姿确定方法周期性执行的情况下，目标时刻的位姿可以是上一周期确定的目标位姿，另外，目标时刻的位姿也可以是通过其它方式确定的位姿，例如通过全球定位系统等位置传感器确定的位姿。
在一种可能的实现方式中，响应于所述位姿确定方法周期性执行的情况下，在所述确定所述行驶设备的雷达在所述线段地图中的预估位姿前，还包括：根据上一周期确定的所述行驶设备的目标位姿，以及当前周期所述行驶设备采集的传感数据，对所述行驶设备在线段地图中的位姿进行预估，得到当前周期所述行驶设备的预估位姿，所述传感数据为基于所述行驶设备的运动信息采集的。
行驶设备的运动信息可包括姿态角、角速度、加速度、轮速等信息中的至少一种，传感数据可以是惯性测量单元(Inertial measurement unit，IMU)或轮速计等传感器采集的数据。
在对行驶设备在线段地图中的位姿进行预估的过程中，可以基于上一周期确定的行驶设备的目标位姿，对当前周期前端IMU采集的数据进行积分融合，并对轮速计采集的轮速进行积分融合，得到当前周期行驶设备的预估位姿；或者，可以基于上一周期确定的行驶设备的目标位姿，对传感器采集的传感数据进行卡尔曼滤波融合，得到当前周期行驶设备的预估位姿。
在通过积分融合的方式得到行驶设备的预估位姿的过程中，会对IMU采集到的角速度在时间上进行积分，既能够得到行驶设备的角度的变化量；通过对轮速计采集的轮速进行积分，既能够得到行驶设备位移的变化量。那么，基于上一周期确定的行驶设备的目标位姿，再加上得到的角度的变化量和位移的变化量，既能够得到当前周期行驶设备的预估位姿。
在通过卡尔曼滤波融合得到行驶设备的预估位姿的过程中，会通过卡尔曼预测方程，基于上一周期确定的行驶设备的目标位姿，将不同传感数据得到的角度和位移分别按比例进行融合，得到当前周期行驶设备的预估位姿，具体融合的比例由卡尔曼增益Kg来确定，Kg由卡尔曼更新方程根据卡尔曼预测方程的预测结果来更新，使得更新后的卡尔曼预测方程的预测结果更准确。
对于起始周期确定预估位姿过程中所使用的目标位姿，可以是由人工设定的，或者是根据人工设定的其它参数进行计算得到的。
由于行驶设备可能在不停地运动，因此，可以在当前周期内按预设频率确定预估位姿，得到当前周期内多个第一时刻的预估位姿，例如，可以按1秒输出100次的频率来输出预估位姿。
本公开实施例中，通过上一周期确定的目标位姿，以及当前周期行驶设备采集的传感数据，对行驶设备在线段地图中的位姿进行预估，能够准确地预估行驶设备的位姿，方便后续进行点线匹配，提高了确定的位姿的准确性。
在确定行驶设备的预估位姿后，即可对雷达的预估位姿进行确定。在一种可能的实现方式中，所述根据行驶设备在线段地图中的预估位姿，确定所述行驶设备的雷达在所述线段地图中的预估位姿，包括：利用所述行驶设备在多个第一时刻的预估位姿，通过插值处理得到所述行驶设备在第二时刻的预估位姿，所述第二时刻包括所述雷达采集所述点云的时刻；根据所述行驶设备在所述第二时刻的预估位姿，得到所述雷达在所述第二时刻的预估位姿。
雷达是按一定频率扫描周围事物得到点云，为方便描述，这里将雷达采集点云的时刻称为第二时刻，第二时刻可以是有多个的。由于前端确定行驶设备预估位姿的第一时刻可能与第二时刻不同，因此，可以利用行驶设备在多个第一时刻的预估位姿，通过插值处理得到行驶设备在第二时刻的预估位姿。
在插值的过程中，会通过内插的方式，对多个第一时刻的数据范围内的预估位姿进行插值处理，得到行驶设备在第二时刻的预估位姿；此外，还可以通过外插的方式，对多个第一时刻的数据范围以外的预估位姿进行插值处理，得到行驶设备在第二时刻的预估位姿。
插值得到的第二时刻的预估位姿是行驶设备的预估位姿，因此，可以根据雷达和行驶设备位姿的相对关系，由插值得到的第二时刻的预估位姿，得到雷达在第二时刻的预估位姿。
在本公开实施例中，通过对行驶设备的预估位姿进行插值的方式，准确地得到了雷达采集点云的时刻的预估位姿，方便后续进行点线匹配，提高了确定的位姿的准确性。
在一种可能的实现方式中，在所述对所述雷达采集的点云与所述线段地图中的线段进行匹配前，还包括：根据所述行驶设备的预估位姿中的朝向，确定所述线段地图中满足第一预设条件的目标线段；对所述雷达采集的点云与所述线段地图中的线段进行匹配，包括：对所述雷达采集的点云与所述目标线段进行匹配。
行驶设备在轨道中行驶的过程中，往往是沿着轨道行驶，因此行驶设备的朝向和所处的轨道两侧往往是平行或接近于平行的状态。而线段地图中的事物也是通过线段来表示的，因此，可以根据行驶设备的朝向来筛选进行匹配的线段，具体可以通过设置条件来筛选线段，选择与行驶设备的朝向相符合的线段。
在一种可能的实现方式中，所述第一预设条件，包括：
线段与所述朝向的夹角的绝对值小于第一阈值；
线段与第一线段的距离小于距离阈值，所述第一线段为与所述朝向的夹角的绝对值小于第二阈值的线段。
第一阈值例如可以是30°，这样可以选取行驶设备朝向±30°以内的线段，如图2所示，对于环形轨道的线段地图，行驶设备朝向如图所示，与行驶设备朝向的夹角的绝对值小于±30°以内的线段集合a如图所示。
在确定与所示朝向的夹角的绝对值小于第一阈值的线段时，对于环形的小车轨道，按逆时针或顺时针的顺序，依次确定构成线段地图中的每一条线段在坐标系中的角度；然后再确定坐标系中小车朝向的角度，根据小车朝向的角度，确定与小车朝向的夹角的绝对值小于第一阈值的线段。
第二阈值小于第一阈值，例如可以是3°或5°等值，这样第一线段即为与行驶设备的朝向平行或近似平行的线段。在确定出第一线段后，再选取出与第一线段的距离小于距离阈值的线段，作为后续匹配的目标线段。距离阈值可以是根据经验设置的经验值，本公开对具体取值不作限定。
如图3所示，对于环形轨道的线段地图，行驶设备朝向如图所示，图中示出了与行驶设备的朝向近乎平行的第一线段，以及与第一线段的距离小于距离阈值的线段集合b。
本公开实现方式中，确定线段之间的距离的方式可以有很多种，例如，线段的距离可以是两个线段的中点之间的距离，或者可以是两个线段上所有点的距离集合中的最小值。对具体确定线段之间的距离的方式，本公开对此不作限定。
本公开实施例中，通过根据所述行驶设备的预估位姿中的朝向，确定所述线段地图中满足第一预设条件的目标线段，可以筛选掉不必要的局外匹配，可以减少后续进行匹配的线段的数量，减少了匹配过程中的计算量，提高了位姿确定的效率。
在一种可能的实现方式中，根据所述雷达的预估位姿，对所述雷达采集的点云与所述线段地图中的线段进行匹配，并根据匹配结果确定匹配后所述雷达在所述线段地图中的位姿，包括：根据所述雷达的预估位姿，确定所述雷达扫描的点云在所述线段地图中的坐标；根据所述点云在所述线段地图中的坐标和所述线段地图中的线段，确定目标转换矩阵，使得所述坐标处的点云经所述目标转换矩阵平移旋转后与所述线段地图中的线段匹配；通过所述目标转换矩阵，对所述雷达的预估位姿进行平移旋转，得到所述雷达在所述线段地图中的位姿。
点云与所述线段地图中的线段进行匹配的过程中，可在线段地图中寻找可以与点云尽可能地重合或者完全重合的线段，当找到与点云尽可能地重合或完全重合的线段后，即实现了点云与线段的匹配。
在匹配的过程中，可根据雷达在线段地图(世界坐标系)中的预估位姿，和雷达扫描的点云在雷达极坐标系中的位姿，得到雷达扫描的点云在线段地图中的坐标。
然后，可在所述线段地图中，通过转换矩阵对所述点云进行平移旋转，确定平移旋转过程中，使得所述点云与所述线段地图中的线段匹配的目标转换矩阵。在旋转的过程中，可计算点云中的点与最近的线段的距离，在点云中的点与最近的线段的距离之和最小的情况下，即视为点云与地图中的线段匹配。
由于雷达扫描的点云表征雷达周围事物的相对位置关系，因此，在点云与地图中的线段匹配后，表明雷达扫描点云时的实际位姿，是通过目标旋转矩阵对预估位姿旋转后的位姿。因此，雷达更准确的位姿应当是通过目标转换矩阵对预估位姿进行旋转平移后的位姿，那么，可以通过目标转换矩阵，对所述雷达的预估位姿进行平移旋转，得到雷达在线段地图中的位姿。
本公开实施例中的点云与线段匹配的过程可通过算法来实现，例如，可以基于高斯牛顿法、列文伯格-马尔夸特法或信赖域狗腿(dogleg)法等方法来实现，对具体算法的过程此处不做赘述。
本公开实施例中，通过根据预先估计的预估位姿，来对雷达采集的点云与线段地图中的线段进行匹配，由于通过线段简略地表示地图中的事物，因此在匹配时，利用点云与线段地图中的线段进行匹配，减少了位姿确定过程的计算量，提高了行驶设备位姿确定的效率。
在一种可能的实现方式中，根据匹配后所述雷达的位姿，确定所述行驶设备在所述线段地图中的目标位姿，包括：根据匹配后所述雷达的位姿，确定匹配后所述行驶设备的位姿；将匹配后所述行驶设备的位姿与所述行驶设备的预估位姿进行融合，得到所述行驶设备在所述线段地图中的目标位姿。
具体可以根据雷达和行驶设备的位姿的相对关系，来确定匹配后行驶设备的位姿。然后将匹配后行驶设备的位姿与行驶设备的预估位姿进行融合，将融合后的位姿作为行驶设备在线段地图中的目标位姿。例如，可以对匹配后行驶设备的位姿与行驶设备的预估位姿进行加权平均，得到融合后的目标位姿；或者也可以通过卡尔曼滤波算法对二者进行融合，得到融合后的目标位姿，卡尔曼滤波算法会通过卡尔曼预测方程，将匹配后行驶设备的位姿与行驶设备的预估位姿按比例进行融合，得到融合后的目标位姿，具体融合的比例由卡尔曼增益来确定，卡尔曼增益由卡尔曼更新方程根据卡尔曼预测方程的预测结果来更新，使得更新后的卡尔曼预测方程的预测结果更准确。
在融合的过程中，可以利用插值到第二时刻后的行驶设备的预估位姿与匹配后行驶设备的位姿进行融合，也可以直接利用第一时刻的行驶设备的预估位姿与匹配后行驶设备的位姿进行融合。
本公开实施例中，通过将匹配后行驶设备的位姿与行驶设备的预估位姿进行融合，使得不同位姿确定方式的缺点互相弥补，降低各种不确定性因素带来的误差，可以提高得到的目标位姿的准确性。
在一种可能的实现方式中，所述行驶设备包括基于嵌入式平台的行驶设备，例如，运行于沙盘中用于人工智能教学的小车、室内扫地机器人等等。基于嵌入式平台的行驶设备其计算性能通常较低，而行驶设备对位姿确定的实时性要求较高，因此，通过本公开提供的位姿确定方法，能够减少行驶设备的计算量，提高位姿确定的速度。
在一种可能的实现方式中，所述行驶设备运行于沙盘中，所述线段地图为所述沙盘的地图。由于沙盘的环境较为单调，在很多路段，雷达扫描的前后两帧没有足够的差异性，通过相邻帧的点云之间进行匹配，或者相邻帧之间进行点线匹配，无法准确匹配出行驶设备的位姿。而本公开实施例中，通过雷达的点云与地图的线段进行匹配，可以能显著提高定位精度。
在一个应用示例中，人工智能教学的小车在沙盘中自动行驶，可预先通过沙盘的场地设计图建立沙盘的线段地图，沙盘中的道路、建筑、障碍物等以线段表示在线段地图中。
在初始周期中，可人为设定小车在线段地图中的第一个预估位姿S1，并根据初始周期每一个第一时刻采集的传感数据，结合预估位姿S1，确定在后续的每个第一时刻小车的预估位姿S2，S3……Sn(n为第一时刻数量)，通过对S1……Sn进行插值，并根据雷达与小车的相对位姿进行变换，得到雷达在每一个第二时刻的预估位姿P1……Pn。
根据小车的预估位姿S2，S3……Sn从线段地图中筛选出符合第一预设条件的目标线段，根据雷达的预估位姿P1……Pn确定雷达扫描的点云在所述线段地图中的坐标，基于转换矩阵对点云的坐标进行平移旋转，直到在目标线段中找到与点云基本重合的线段，确定此时的转换矩阵为目标转换矩阵，利用该目标转换矩阵对雷达的预估位姿P1……Pn进行平移旋转，得到匹配后雷达的位姿P’1……P’n，进而根据雷达与小车之间的相对位姿关系，得到匹配后小车的位姿S1”,S”2，S”3……S”n，将该位姿与小车的预估位姿S1,S2，S3……Sn进行融合，即得到小车在初始周期的目标位姿S1’,S’2，S’3……S’n。
以S’n取代上文中的S1，即可开始下一周期的小车目标位姿的确定。
在一种可能的实现方式中，所述位姿确定方法可以由行驶设备上的位姿确定模块来执行，或者，也可以是由行驶设备将采集的数据发送给其它设备，由其它设备来执行，例如可以由终端设备或服务器等电子设备执行，终端设备可以是用户设备(User Equipment，UE)、移动设备、用户终端、终端、蜂窝电话、无绳电话、个人数字处理(Personal DigitalAssistant，PDA)、手持设备、计算设备、车载设备、可穿戴设备等，所述方法可以通过处理器调用存储器中存储的计算机可读指令的方式来实现。或者，可通过服务器执行所述方法。
可以理解，本公开提及的上述各个方法实施例，在不违背原理逻辑的情况下，均可以彼此相互结合形成结合后的实施例，限于篇幅，本公开不再赘述。本领域技术人员可以理解，在具体实施方式的上述方法中，各步骤的具体执行顺序应当以其功能和可能的内在逻辑确定。
此外，本公开还提供了位姿确定装置、电子设备、计算机可读存储介质、程序，上述均可用来实现本公开提供的任一种位姿确定方法，相应技术方案和描述和参见方法部分的相应记载，不再赘述。
图4示出根据本公开实施例的位置确定装置的框图，如图4所示，所述装置40包括：
预估位姿确定单元401，用于根据行驶设备在线段地图中的预估位姿，确定所述行驶设备的雷达在所述线段地图中的预估位姿，所述线段地图中的事物通过线段表示；
雷达位姿确定单元402，用于根据雷达的预估位姿，对所述雷达采集的点云与所述线段地图中的线段进行匹配，并根据匹配结果确定匹配后所述雷达在所述线段地图中的位姿；
目标位姿确定单元403，用于根据匹配后所述雷达的位姿，确定所述行驶设备在所述线段地图中的目标位姿。
在一种可能的实现方式中，所述预估位姿确定单元401，包括：
利用所述行驶设备在多个第一时刻的预估位姿，通过插值处理得到所述行驶设备在第二时刻的预估位姿，所述第二时刻包括所述雷达采集所述点云的时刻；
根据所述行驶设备在所述第二时刻的预估位姿，得到所述雷达在所述第二时刻的预估位姿。
在一种可能的实现方式中，所述装置还包括：
目标线段确定单元，用于根据所述行驶设备的预估位姿中的朝向，确定所述线段地图中满足第一预设条件的目标线段；
所述雷达位姿确定单元，用于对所述雷达采集的点云与所述目标线段进行匹配。
在一种可能的实现方式中，所述第一预设条件，包括：
线段与所述朝向的夹角的绝对值小于第一阈值；
线段与第一线段的距离小于距离阈值，所述第一线段为与所述朝向的夹角的绝对值小于第二阈值的线段。
在一种可能的实现方式中，所述雷达位姿确定单元402，用于根据所述雷达的预估位姿，确定所述雷达扫描的点云在所述线段地图中的坐标；根据所述点云在所述线段地图中的坐标和所述线段地图中的线段，确定目标转换矩阵，使得所述坐标处的点云经所述目标转换矩阵平移旋转后与所述线段地图中的线段匹配；通过所述目标转换矩阵，对所述雷达的预估位姿进行平移旋转，得到所述雷达在所述线段地图中的位姿。
在一种可能的实现方式中，所述目标位姿确定单元403，用于根据匹配后所述雷达的位姿，确定匹配后所述行驶设备的位姿；将匹配后所述行驶设备的位姿与所述行驶设备的预估位姿进行融合，得到所述行驶设备在所述线段地图中的目标位姿。
在一种可能的实现方式中，所述装置还包括：
当前预估位姿确定单元，用于根据上一周期确定的所述行驶设备的目标位姿，以及当前周期所述行驶设备采集的传感数据，对所述行驶设备在线段地图中的位姿进行预估，得到当前周期所述行驶设备的预估位姿，所述传感数据为基于所述行驶设备的运动信息采集的。
在一种可能的实现方式中，所述线段地图是通过对初始地图进行直线拟合得到的，所述初始地图包括：占据栅格地图和/或场地设计图。
在一种可能的实现方式中，所述行驶设备包括基于嵌入式平台的行驶设备，所述行驶设备运行于沙盘中，所述线段地图为所述沙盘的地图。
在一些实施例中，本公开实施例提供的装置具有的功能或包含的模块可以用于执行上文方法实施例描述的方法，其具体实现可以参照上文方法实施例的描述，为了简洁，这里不再赘述。
本公开实施例还提出一种计算机可读存储介质，其上存储有计算机程序指令，所述计算机程序指令被处理器执行时实现上述方法。计算机可读存储介质可以是非易失性计算机可读存储介质。
本公开实施例还提出一种电子设备，包括：处理器；用于存储处理器可执行指令的存储器；其中，所述处理器被配置为调用所述存储器存储的指令，以执行上述方法。
本公开实施例还提供了一种计算机程序产品，包括计算机可读代码，当计算机可读代码在设备上运行时，设备中的处理器执行用于实现如上任一实施例提供的位姿确定方法的指令。
本公开实施例还提供了另一种计算机程序产品，用于存储计算机可读指令，指令被执行时使得计算机执行上述任一实施例提供的位姿确定方法的操作。
电子设备可以被提供为终端、服务器或其它形态的设备。
图5示出根据本公开实施例的一种电子设备800的框图。例如，电子设备800可以是移动电话，计算机，数字广播终端，消息收发设备，游戏控制台，平板设备，医疗设备，健身设备，个人数字助理等终端。
参照图5，电子设备800可以包括以下一个或多个组件：处理组件802，存储器804，电源组件806，多媒体组件808，音频组件810，输入/输出(I/O)的接口812，传感器组件814，以及通信组件816。
处理组件802通常控制电子设备800的整体操作，诸如与显示，电话呼叫，数据通信，相机操作和记录操作相关联的操作。处理组件802可以包括一个或多个处理器820来执行指令，以完成上述的方法的全部或部分步骤。此外，处理组件802可以包括一个或多个模块，便于处理组件802和其他组件之间的交互。例如，处理组件802可以包括多媒体模块，以方便多媒体组件808和处理组件802之间的交互。
存储器804被配置为存储各种类型的数据以支持在电子设备800的操作。这些数据的示例包括用于在电子设备800上操作的任何应用程序或方法的指令，联系人数据，电话簿数据，消息，图片，视频等。存储器804可以由任何类型的易失性或非易失性存储设备或者它们的组合实现，如静态随机存取存储器(SRAM)，电可擦除可编程只读存储器(EEPROM)，可擦除可编程只读存储器(EPROM)，可编程只读存储器(PROM)，只读存储器(ROM)，磁存储器，快闪存储器，磁盘或光盘。
电源组件806为电子设备800的各种组件提供电力。电源组件806可以包括电源管理系统，一个或多个电源，及其他与为电子设备800生成、管理和分配电力相关联的组件。
多媒体组件808包括在所述电子设备800和用户之间的提供一个输出接口的屏幕。在一些实施例中，屏幕可以包括液晶显示器(LCD)和触摸面板(TP)。如果屏幕包括触摸面板，屏幕可以被实现为触摸屏，以接收来自用户的输入信号。触摸面板包括一个或多个触摸传感器以感测触摸、滑动和触摸面板上的手势。所述触摸传感器可以不仅感测触摸或滑动动作的边界，而且还检测与所述触摸或滑动操作相关的持续时间和压力。在一些实施例中，多媒体组件808包括一个前置摄像头和/或后置摄像头。当电子设备800处于操作模式，如拍摄模式或视频模式时，前置摄像头和/或后置摄像头可以接收外部的多媒体数据。每个前置摄像头和后置摄像头可以是一个固定的光学透镜系统或具有焦距和光学变焦能力。
音频组件810被配置为输出和/或输入音频信号。例如，音频组件810包括一个麦克风(MIC)，当电子设备800处于操作模式，如呼叫模式、记录模式和语音识别模式时，麦克风被配置为接收外部音频信号。所接收的音频信号可以被进一步存储在存储器804或经由通信组件816发送。在一些实施例中，音频组件810还包括一个扬声器，用于输出音频信号。
I/O接口812为处理组件802和外围接口模块之间提供接口，上述外围接口模块可以是键盘，点击轮，按钮等。这些按钮可包括但不限于：主页按钮、音量按钮、启动按钮和锁定按钮。
传感器组件814包括一个或多个传感器，用于为电子设备800提供各个方面的状态评估。例如，传感器组件814可以检测到电子设备800的打开/关闭状态，组件的相对定位，例如所述组件为电子设备800的显示器和小键盘，传感器组件814还可以检测电子设备800或电子设备800一个组件的位置改变，用户与电子设备800接触的存在或不存在，电子设备800方位或加速/减速和电子设备800的温度变化。传感器组件814可以包括接近传感器，被配置用来在没有任何的物理接触时检测附近物体的存在。传感器组件814还可以包括光传感器，如CMOS或CCD图像传感器，用于在成像应用中使用。在一些实施例中，该传感器组件814还可以包括加速度传感器，陀螺仪传感器，磁传感器，压力传感器或温度传感器。
通信组件816被配置为便于电子设备800和其他设备之间有线或无线方式的通信。电子设备800可以接入基于通信标准的无线网络，如WiFi，2G或3G，或它们的组合。在一个示例性实施例中，通信组件816经由广播信道接收来自外部广播管理系统的广播信号或广播相关信息。在一个示例性实施例中，所述通信组件816还包括近场通信(NFC)模块，以促进短程通信。例如，在NFC模块可基于射频识别(RFID)技术，红外数据协会(IrDA)技术，超宽带(UWB)技术，蓝牙(BT)技术和其他技术来实现。
在示例性实施例中，电子设备800可以被一个或多个应用专用集成电路(ASIC)、数字信号处理器(DSP)、数字信号处理设备(DSPD)、可编程逻辑器件(PLD)、现场可编程门阵列(FPGA)、控制器、微控制器、微处理器或其他电子元件实现，用于执行上述方法。
在示例性实施例中，还提供了一种非易失性计算机可读存储介质，例如包括计算机程序指令的存储器804，上述计算机程序指令可由电子设备800的处理器820执行以完成上述方法。
图6示出根据本公开实施例的一种电子设备1900的框图。例如，电子设备1900可以被提供为一服务器。参照图6，电子设备1900包括处理组件1922，其进一步包括一个或多个处理器，以及由存储器1932所代表的存储器资源，用于存储可由处理组件1922的执行的指令，例如应用程序。存储器1932中存储的应用程序可以包括一个或一个以上的每一个对应于一组指令的模块。此外，处理组件1922被配置为执行指令，以执行上述方法。
电子设备1900还可以包括一个电源组件1926被配置为执行电子设备1900的电源管理，一个有线或无线网络接口1950被配置为将电子设备1900连接到网络，和一个输入输出(I/O)接口1958。电子设备1900可以操作基于存储在存储器1932的操作系统，例如Windows ServerTM，Mac OS XTM，UnixTM,LinuxTM，FreeBSDTM或类似。
在示例性实施例中，还提供了一种非易失性计算机可读存储介质，例如包括计算机程序指令的存储器1932，上述计算机程序指令可由电子设备1900的处理组件1922执行以完成上述方法。
本公开可以是系统、方法和/或计算机程序产品。计算机程序产品可以包括计算机可读存储介质，其上载有用于使处理器实现本公开的各个方面的计算机可读程序指令。
计算机可读存储介质可以是可以保持和存储由指令执行设备使用的指令的有形设备。计算机可读存储介质例如可以是――但不限于――电存储设备、磁存储设备、光存储设备、电磁存储设备、半导体存储设备或者上述的任意合适的组合。计算机可读存储介质的更具体的例子(非穷举的列表)包括：便携式计算机盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、静态随机存取存储器(SRAM)、便携式压缩盘只读存储器(CD-ROM)、数字多功能盘(DVD)、记忆棒、软盘、机械编码设备、例如其上存储有指令的打孔卡或凹槽内凸起结构、以及上述的任意合适的组合。这里所使用的计算机可读存储介质不被解释为瞬时信号本身，诸如无线电波或者其他自由传播的电磁波、通过波导或其他传输媒介传播的电磁波(例如，通过光纤电缆的光脉冲)、或者通过电线传输的电信号。
这里所描述的计算机可读程序指令可以从计算机可读存储介质下载到各个计算/处理设备，或者通过网络、例如因特网、局域网、广域网和/或无线网下载到外部计算机或外部存储设备。网络可以包括铜传输电缆、光纤传输、无线传输、路由器、防火墙、交换机、网关计算机和/或边缘服务器。每个计算/处理设备中的网络适配卡或者网络接口从网络接收计算机可读程序指令，并转发该计算机可读程序指令，以供存储在各个计算/处理设备中的计算机可读存储介质中。
用于执行本公开操作的计算机程序指令可以是汇编指令、指令集架构(ISA)指令、机器指令、机器相关指令、微代码、固件指令、状态设置数据、或者以一种或多种编程语言的任意组合编写的源代码或目标代码，所述编程语言包括面向对象的编程语言—诸如Smalltalk、C++等，以及常规的过程式编程语言—诸如“C”语言或类似的编程语言。计算机可读程序指令可以完全地在用户计算机上执行、部分地在用户计算机上执行、作为一个独立的软件包执行、部分在用户计算机上部分在远程计算机上执行、或者完全在远程计算机或服务器上执行。在涉及远程计算机的情形中，远程计算机可以通过任意种类的网络—包括局域网(LAN)或广域网(WAN)—连接到用户计算机，或者，可以连接到外部计算机(例如利用因特网服务提供商来通过因特网连接)。在一些实施例中，通过利用计算机可读程序指令的状态信息来个性化定制电子电路，例如可编程逻辑电路、现场可编程门阵列(FPGA)或可编程逻辑阵列(PLA)，该电子电路可以执行计算机可读程序指令，从而实现本公开的各个方面。
这里参照根据本公开实施例的方法、装置(系统)和计算机程序产品的流程图和/或框图描述了本公开的各个方面。应当理解，流程图和/或框图的每个方框以及流程图和/或框图中各方框的组合，都可以由计算机可读程序指令实现。
这些计算机可读程序指令可以提供给通用计算机、专用计算机或其它可编程数据处理装置的处理器，从而生产出一种机器，使得这些指令在通过计算机或其它可编程数据处理装置的处理器执行时，产生了实现流程图和/或框图中的一个或多个方框中规定的功能/动作的装置。也可以把这些计算机可读程序指令存储在计算机可读存储介质中，这些指令使得计算机、可编程数据处理装置和/或其他设备以特定方式工作，从而，存储有指令的计算机可读介质则包括一个制造品，其包括实现流程图和/或框图中的一个或多个方框中规定的功能/动作的各个方面的指令。
也可以把计算机可读程序指令加载到计算机、其它可编程数据处理装置、或其它设备上，使得在计算机、其它可编程数据处理装置或其它设备上执行一系列操作步骤，以产生计算机实现的过程，从而使得在计算机、其它可编程数据处理装置、或其它设备上执行的指令实现流程图和/或框图中的一个或多个方框中规定的功能/动作。
附图中的流程图和框图显示了根据本公开的多个实施例的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或指令的一部分，所述模块、程序段或指令的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
该计算机程序产品可以具体通过硬件、软件或其结合的方式实现。在一个可选实施例中，所述计算机程序产品具体体现为计算机存储介质，在另一个可选实施例中，计算机程序产品具体体现为软件产品，例如软件开发包(Software Development Kit，SDK)等等。
以上已经描述了本公开的各实施例，上述说明是示例性的，并非穷尽性的，并且也不限于所披露的各实施例。在不偏离所说明的各实施例的范围和精神的情况下，对于本技术领域的普通技术人员来说许多修改和变更都是显而易见的。本文中所用术语的选择，旨在最好地解释各实施例的原理、实际应用或对市场中的技术的改进，或者使本技术领域的其它普通技术人员能理解本文披露的各实施例。

Claims (10)

1.一种位姿确定方法，其特征在于，包括：
根据行驶设备在线段地图中的预估位姿，确定所述行驶设备的雷达在所述线段地图中的预估位姿，所述线段地图中的事物通过线段表示；
根据雷达的预估位姿，对所述雷达采集的点云与所述线段地图中的线段进行匹配，并根据匹配结果确定匹配后所述雷达在所述线段地图中的位姿，所述匹配包括：在所述线段地图中，使所述雷达采集的点云中的点与最近的线段的距离之和最小；
根据匹配后所述雷达的位姿，确定所述行驶设备在所述线段地图中的目标位姿；
在所述对所述雷达采集的点云与所述线段地图中的线段进行匹配前，还包括：
根据所述行驶设备的预估位姿中的朝向，确定所述线段地图中满足第一预设条件的目标线段；
对所述雷达采集的点云与所述线段地图中的线段进行匹配，包括：
对所述雷达采集的点云与所述目标线段进行匹配；
所述第一预设条件，包括：
线段与所述朝向的夹角的绝对值小于第一阈值；
线段与第一线段的距离小于距离阈值，所述第一线段为与所述朝向的夹角的绝对值小于第二阈值的线段，所述第二阈值小于所述第一阈值，所述第一线段为与行驶设备的朝向平行或近似平行的线段。
2.根据权利要求1所述方法，其特征在于，所述根据行驶设备在线段地图中的预估位姿，确定所述行驶设备的雷达在所述线段地图中的预估位姿，包括：
利用所述行驶设备在多个第一时刻的预估位姿，通过插值处理得到所述行驶设备在第二时刻的预估位姿，所述第二时刻包括所述雷达采集所述点云的时刻；
根据所述行驶设备在所述第二时刻的预估位姿，得到所述雷达在所述第二时刻的预估位姿。
3.根据权利要求1-2任一所述方法，其特征在于，根据雷达的预估位姿，对所述雷达采集的点云与所述线段地图中的线段进行匹配，并根据匹配结果确定匹配后所述雷达在所述线段地图中的位姿，包括：
根据所述雷达的预估位姿，确定所述雷达扫描的点云在所述线段地图中的坐标；
根据所述点云在所述线段地图中的坐标和所述线段地图中的线段，确定目标转换矩阵，使得所述坐标处的点云经所述目标转换矩阵平移旋转后与所述线段地图中的线段匹配；
通过所述目标转换矩阵，对所述雷达的预估位姿进行平移旋转，得到所述雷达在所述线段地图中的位姿。
4.根据权利要求1-3任一所述方法，其特征在于，根据匹配后所述雷达的位姿，确定所述行驶设备在所述线段地图中的目标位姿，包括：
根据匹配后所述雷达的位姿，确定匹配后所述行驶设备的位姿；
将匹配后所述行驶设备的位姿与所述行驶设备的预估位姿进行融合，得到所述行驶设备在所述线段地图中的目标位姿。
5.根据权利要求1-4任一所述方法，其特征在于，响应于所述位姿确定方法周期性执行的情况下，在所述确定所述行驶设备的雷达在所述线段地图中的预估位姿前，还包括：
根据上一周期确定的所述行驶设备的目标位姿，以及当前周期所述行驶设备采集的传感数据，对所述行驶设备在线段地图中的位姿进行预估，得到当前周期所述行驶设备的预估位姿，所述传感数据为基于所述行驶设备的运动信息采集的。
6.根据权利要求1-5任一所述方法，其特征在于，所述线段地图是通过对初始地图进行直线拟合得到的，所述初始地图包括：占据栅格地图和/或场地设计图。
7.根据权利要求1-6任一所述方法，其特征在于，所述行驶设备包括基于嵌入式平台的行驶设备，所述行驶设备运行于沙盘中，所述线段地图为所述沙盘的地图。
8.一种位姿确定装置，其特征在于，包括：
预估位姿确定单元，用于根据行驶设备在线段地图中的预估位姿，确定所述行驶设备的雷达在所述线段地图中的预估位姿，所述线段地图中的事物通过线段表示；
雷达位姿确定单元，用于根据雷达的预估位姿，对所述雷达采集的点云与所述线段地图中的线段进行匹配，并根据匹配结果确定匹配后所述雷达在所述线段地图中的位姿，所述匹配包括：在所述线段地图中，使所述雷达采集的点云中的点与最近的线段的距离之和最小；
目标位姿确定单元，用于根据匹配后所述雷达的位姿，确定所述行驶设备在所述线段地图中的目标位姿；
所述装置还包括：
目标线段确定单元，用于根据所述行驶设备的预估位姿中的朝向，确定所述线段地图中满足第一预设条件的目标线段；
所述雷达位姿确定单元，用于对所述雷达采集的点云与所述目标线段进行匹配；
所述第一预设条件，包括：
线段与所述朝向的夹角的绝对值小于第一阈值；
线段与第一线段的距离小于距离阈值，所述第一线段为与所述朝向的夹角的绝对值小于第二阈值的线段，所述第二阈值小于所述第一阈值，所述第一线段为与行驶设备的朝向平行或近似平行的线段。
9.一种电子设备，其特征在于，包括：
处理器；
用于存储处理器可执行指令的存储器；
其中，所述处理器被配置为调用所述存储器存储的指令，以执行权利要求1至7中任意一项所述的方法。
10.一种计算机可读存储介质，其上存储有计算机程序指令，其特征在于，所述计算机程序指令被处理器执行时实现权利要求1至7中任意一项所述的方法。
CN202011362399.XA 2020-11-27 2020-11-27 一种位姿确定方法及装置、电子设备和存储介质 Active CN112433211B (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN202011362399.XA CN112433211B (zh) 2020-11-27 2020-11-27 一种位姿确定方法及装置、电子设备和存储介质
PCT/CN2021/092487 WO2022110653A1 (zh) 2020-11-27 2021-05-08 一种位姿确定方法及装置、电子设备和计算机可读存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011362399.XA CN112433211B (zh) 2020-11-27 2020-11-27 一种位姿确定方法及装置、电子设备和存储介质

Publications (2)

Publication Number Publication Date
CN112433211A CN112433211A (zh) 2021-03-02
CN112433211B true CN112433211B (zh) 2022-11-29

Family

ID=74698665

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011362399.XA Active CN112433211B (zh) 2020-11-27 2020-11-27 一种位姿确定方法及装置、电子设备和存储介质

Country Status (2)

Country Link
CN (1) CN112433211B (zh)
WO (1) WO2022110653A1 (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112433211B (zh) * 2020-11-27 2022-11-29 浙江商汤科技开发有限公司 一种位姿确定方法及装置、电子设备和存储介质
CN113406659A (zh) * 2021-05-28 2021-09-17 浙江大学 一种基于激光雷达信息的移动机器人位置重识别方法
CN113776533A (zh) * 2021-07-29 2021-12-10 北京旷视科技有限公司 可移动设备的重定位方法及装置

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
RU2249232C2 (ru) * 2003-04-15 2005-03-27 Открытое акционерное общество "Корпорация "Фазотрон-Научно-исследовательский институт радиостроения" (ОАО "Корпорация "Фазотрон-НИИР") Способ сопровождения радиоконтрастного объекта по направлению и устройство сопровождения радиоконтрастного объекта по направлению
CN104575075A (zh) * 2015-01-14 2015-04-29 合肥革绿信息科技有限公司 一种基于北斗的城市路网车辆坐标校正方法及装置
CN107704821A (zh) * 2017-09-29 2018-02-16 河北工业大学 一种弯道的车辆位姿计算方法
CN109839112A (zh) * 2019-03-11 2019-06-04 中南大学 地下作业设备定位方法、装置、系统及存储介质
CN111223145A (zh) * 2020-01-03 2020-06-02 上海有个机器人有限公司 数据处理方法、系统、服务装置及其存储介质
WO2020164010A1 (zh) * 2019-02-13 2020-08-20 深圳市大疆创新科技有限公司 车道线检测方法、装置、系统与车辆、存储介质
CN111929699A (zh) * 2020-07-21 2020-11-13 北京建筑大学 一种顾及动态障碍物的激光雷达惯导里程计与建图方法及系统

Family Cites Families (18)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN100449444C (zh) * 2006-09-29 2009-01-07 浙江大学 移动机器人在未知环境中同时定位与地图构建的方法
US8369606B2 (en) * 2010-07-21 2013-02-05 Palo Alto Research Center Incorporated System and method for aligning maps using polyline matching
CN103842244B (zh) * 2011-10-03 2016-12-14 古野电气株式会社 显示装置及显示方法
CN103941264B (zh) * 2014-03-26 2017-04-19 南京航空航天大学 一种室内未知环境下使用激光雷达定位方法
US9773183B2 (en) * 2015-05-07 2017-09-26 The United States Of America As Represented By The Secretary Of The Air Force Morphological automatic Landolt C orientation detection
US10082797B2 (en) * 2015-09-16 2018-09-25 Ford Global Technologies, Llc Vehicle radar perception and localization
CN106123890A (zh) * 2016-06-14 2016-11-16 中国科学院合肥物质科学研究院 一种多传感器数据融合的机器人定位方法
US10192098B2 (en) * 2016-09-09 2019-01-29 MorphoTrak, LLC Palm print image matching techniques
CN110631554B (zh) * 2018-06-22 2021-11-30 北京京东乾石科技有限公司 机器人位姿的确定方法、装置、机器人和可读存储介质
CN111033299B (zh) * 2018-07-02 2023-06-16 北京嘀嘀无限科技发展有限公司 基于点云利用位姿预估的车辆导航系统
CN109341688A (zh) * 2018-09-05 2019-02-15 南京理工大学 一种基于构造顺序的地图调用定位算法
CN109655805B (zh) * 2019-01-25 2021-12-10 南京理工大学 一种基于扫描线段重合长度估计的激光雷达定位方法
CN110926485B (zh) * 2019-11-11 2021-10-08 华中科技大学 一种基于直线特征的移动机器人定位方法及系统
CN111325796B (zh) * 2020-02-28 2023-08-18 北京百度网讯科技有限公司 用于确定视觉设备的位姿的方法和装置
CN111522022B (zh) * 2020-04-20 2023-03-28 西安电子科技大学 基于激光雷达的机器人进行动态目标检测方法
CN111590595B (zh) * 2020-06-30 2021-09-28 深圳市银星智能科技股份有限公司 一种定位方法、装置、移动机器人及存储介质
CN114814872A (zh) * 2020-08-17 2022-07-29 浙江商汤科技开发有限公司 位姿确定方法及装置、电子设备和存储介质
CN112433211B (zh) * 2020-11-27 2022-11-29 浙江商汤科技开发有限公司 一种位姿确定方法及装置、电子设备和存储介质

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
RU2249232C2 (ru) * 2003-04-15 2005-03-27 Открытое акционерное общество "Корпорация "Фазотрон-Научно-исследовательский институт радиостроения" (ОАО "Корпорация "Фазотрон-НИИР") Способ сопровождения радиоконтрастного объекта по направлению и устройство сопровождения радиоконтрастного объекта по направлению
CN104575075A (zh) * 2015-01-14 2015-04-29 合肥革绿信息科技有限公司 一种基于北斗的城市路网车辆坐标校正方法及装置
CN107704821A (zh) * 2017-09-29 2018-02-16 河北工业大学 一种弯道的车辆位姿计算方法
WO2020164010A1 (zh) * 2019-02-13 2020-08-20 深圳市大疆创新科技有限公司 车道线检测方法、装置、系统与车辆、存储介质
CN109839112A (zh) * 2019-03-11 2019-06-04 中南大学 地下作业设备定位方法、装置、系统及存储介质
CN111223145A (zh) * 2020-01-03 2020-06-02 上海有个机器人有限公司 数据处理方法、系统、服务装置及其存储介质
CN111929699A (zh) * 2020-07-21 2020-11-13 北京建筑大学 一种顾及动态障碍物的激光雷达惯导里程计与建图方法及系统

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
动态环境下无人地面车辆点云地图快速重定位方法;郑壮壮等;《兵工学报》;20200831(第08期);全文 *
基于Kinect的改进移动机器人视觉SLAM;蔡军等;《智能系统学报》;20180424(第05期);全文 *
基于NDT与ICP结合的点云配准算法;王庆闪等;《计算机工程与应用》;20200731(第07期);全文 *
基于点线相合的机器人增量式地图构建;熊蓉等;《控制理论与应用》;20070415(第02期);全文 *

Also Published As

Publication number Publication date
WO2022110653A1 (zh) 2022-06-02
CN112433211A (zh) 2021-03-02

Similar Documents

Publication Publication Date Title
CN112433211B (zh) 一种位姿确定方法及装置、电子设备和存储介质
CN111983635B (zh) 位姿确定方法及装置、电子设备和存储介质
US10959049B2 (en) Scene sharing-based navigation assistance method and terminal
CN111524185A (zh) 定位方法及装置、电子设备和存储介质
CN111881827B (zh) 目标检测方法及装置、电子设备和存储介质
CN112146645B (zh) 一种坐标系对齐的方法及装置、电子设备和存储介质
US20220076469A1 (en) Information display device and information display program
CN112432637B (zh) 定位方法及装置、电子设备和存储介质
CN111666917A (zh) 姿态检测及视频处理方法、装置、电子设备和存储介质
US12020463B2 (en) Positioning method, electronic device and storage medium
KR102277503B1 (ko) 객체 인식 기반 실내 측위를 위한 단말장치, 서비스 서버 및 그 방법
CN113074726A (zh) 位姿确定方法及装置、电子设备和存储介质
CN111860373B (zh) 目标检测方法及装置、电子设备和存储介质
CN110865405A (zh) 融合定位方法及装置、移动设备控制方法及电子设备
KR20220130707A (ko) 포인트 클라우드 맵 구축 방법 및 장치, 전자 기기, 저장 매체 및 프로그램
CN113066135A (zh) 图像采集设备的标定方法及装置、电子设备和存储介质
CN106533907B (zh) 信息发送方法及装置
CN112432636B (zh) 定位方法及装置、电子设备和存储介质
CN112837372A (zh) 数据生成方法及装置、电子设备和存储介质
WO2022237071A1 (zh) 定位方法及装置、电子设备、存储介质和计算机程序
CN111832338A (zh) 对象检测方法及装置、电子设备和存储介质
WO2022110777A1 (zh) 定位方法及装置、电子设备、存储介质、计算机程序产品、计算机程序
CN112461245A (zh) 数据处理方法及装置、电子设备和存储介质
CN112308878A (zh) 一种信息处理方法、装置、电子设备和存储介质
JP7543745B2 (ja) 粒子フィルタを使用した位置特定方法、プログラム、及びシステム

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
REG Reference to a national code

Ref country code: HK

Ref legal event code: DE

Ref document number: 40039153

Country of ref document: HK

GR01 Patent grant
GR01 Patent grant