CN114248778A - 移动设备的定位方法和定位装置 - Google Patents

移动设备的定位方法和定位装置 Download PDF

Info

Publication number
CN114248778A
CN114248778A CN202011002221.4A CN202011002221A CN114248778A CN 114248778 A CN114248778 A CN 114248778A CN 202011002221 A CN202011002221 A CN 202011002221A CN 114248778 A CN114248778 A CN 114248778A
Authority
CN
China
Prior art keywords
vector
projection
vector elements
elements
vector element
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
CN202011002221.4A
Other languages
English (en)
Other versions
CN114248778B (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.)
Huawei Technologies Co Ltd
Original Assignee
Huawei Technologies 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 Huawei Technologies Co Ltd filed Critical Huawei Technologies Co Ltd
Priority to CN202011002221.4A priority Critical patent/CN114248778B/zh
Priority to EP21870863.4A priority patent/EP4209755A4/en
Priority to PCT/CN2021/099232 priority patent/WO2022062480A1/zh
Publication of CN114248778A publication Critical patent/CN114248778A/zh
Priority to US18/124,354 priority patent/US20230222688A1/en
Application granted granted Critical
Publication of CN114248778B publication Critical patent/CN114248778B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
    • B60W40/02—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models related to ambient conditions
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
    • 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/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/98—Detection or correction of errors, e.g. by rescanning the pattern or by human intervention; Evaluation of the quality of the acquired patterns
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
    • B60W2050/0001—Details of the control system
    • B60W2050/0043—Signal treatments, identification of variables or parameters, parameter estimation or state estimation
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10016—Video; Image sequence
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/20—Special algorithmic details
    • G06T2207/20084—Artificial neural networks [ANN]
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30244—Camera pose

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Theoretical Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Transportation (AREA)
  • Mechanical Engineering (AREA)
  • Quality & Reliability (AREA)
  • Multimedia (AREA)
  • Mathematical Physics (AREA)
  • Human Computer Interaction (AREA)
  • Image Analysis (AREA)

Abstract

本申请提供一种移动设备的定位方法和定位装置。本申请提供的技术方案中，基于图像中提取得到的感知矢量元素的横向位置和地图中的矢量元素投影得到的投影矢量元素的横向位置对感知矢量元素和投影矢量元素进行关联，使得被关联的感知矢量元素的横向位置和被关联的投影矢量元素分别按相同的顺序排列时，排列后的每个感知矢量元素与排列后的投影矢量元素中对应的投影矢量元素的横向位置分布具有一致性，并基于上述关联结果对移动设备进行位姿估计。本申请提供的技术方案，可以提高数据关联的准确度，以及提高移动设备的定位精度。

Description

移动设备的定位方法和定位装置
技术领域
本申请涉及智能驾驶领域，并且更具体地，涉及移动设备的定位方法和定位装置。
背景技术
目前，自动驾驶汽车、无人机和机器人等智能移动设备已经广泛应用于生活中的各个领域。智能移动设备的关键技术主要通过感知定位、决策规划和执行控制来实现。其中，感知定位是智能移动设备运行的基础。
矢量定位是智能移动设备进行感知定位的一种有效方法。矢量定位是指：智能移动设备利用传感器对周围环境进行图像采集，并从采集的图像中提取环境中的语义信息，以及通过矢量形式表达提取到的语义信息以得到矢量元素；然后将语义信息对应的矢量元素与从地图中提取的矢量元素进行关联，并基于语义信息对应的矢量元素和地图中与之关联的矢量元素对智能移动设备的系统状态向量进行估计，从而实现智能移动设备的感知定位。
现有技术中，对语义信息对应的矢量元素和从地图中提取的矢量元素进行关联时，通过语义信息对应的矢量元素与从地图中提取的矢量元素之间的距离大小来判断是否将语义信息对应的矢量元素与从地图中提取的矢量元素关联。具体地，将从地图中提取得到的矢量元素中离语义信息对应的矢量元素距离最近的矢量元素与该语义信息对应的矢量元素相关联。这种关联方式容易造成误关联现象，从而导致智能移动设备的定位误差较大。
发明内容
本申请提供了移动设备的定位方法和定位装置，可以提高定位精度。
第一方面，本申请提供一种移动设备的定位方法。所述方法包括：从所述移动设备的图像采集装置采集的第一图像中提取N个感知矢量元素，N为正整数；基于所述移动设备的第一粒子位姿，将地图中的M′个地图矢量元素投影到所述图像采集装置的图像坐标系，得到与所述M′个地图矢量元素中M个地图矢量元素一一对应的M个第一投影矢量元素，M′为正整数，M为小于或等于M′的正整数；获取所述N个感知矢量元素和所述M个第一投影矢量元素的关联结果，所述关联结果包括：所述N个感知矢量元素中的L个感知矢量元素与所述M个第一投影矢量元素中的L个第一投影矢量元素一一关联，所述L个感知矢量元素中第t个感知矢量元素在所述图像坐标系中的横向坐标小于所述L个感知矢量元素中第t+1个感知矢量元素在所述图像坐标系中的横向坐标时，所述第t个感知矢量元素对应的第一投影矢量元素的横向坐标小于所述第t+1个感知矢量元素对应的第一投影矢量元素的横向坐标，L为大于1的整数，且L小于或等于M和N的最小值，t为小于L的正整数，所述L个感知矢量元素中每个感知矢量元素与对应的第一投影矢量元素为同种类型的矢量元素；根据所述L个感知矢量元素中每个感知矢量元素的坐标和对应的第一投影矢量元素的坐标以及所述第一粒子位姿，对所述移动设备进行位姿估计。
本申请提出的方法中，在对投影矢量元素和感知矢量元素进行关联时，使得关联后的感知矢量元素的横向坐标按照从小到大的顺序排列，关联后的投影矢量元素按照从小到大的顺序排列时，每个感知矢量元素的顺序与其关联的投影矢量元素的顺序相同，即感知矢量元素的横向位置分布于所关联的投影矢量元素的横向位置分布保持一致性。这样可以提高感知矢量元素与投影矢量元素的关联正确性，本质上提高感知矢量元素与地图矢量元素的关联准确性从而可以提高所估计的位置的准确性。
结合第一方面，在第一种可能的实现方式中，所述根据所述L个感知矢量元素中每个感知矢量元素的坐标和对应的第一投影矢量元素的坐标以及所述第一粒子位姿，对所述移动设备进行位姿估计，包括：根据所述L个感知矢量元素中每个感知矢量元素的横向坐标和对应的第一投影矢量元素的横向坐标之间的差值，确定所述L个感知矢量元素与对应的L个第一投影矢量元素之间的位置误差；根据所述位置误差确定所述第一粒子位姿的权重；根据所述第一粒子位姿的权重和所述第一粒子位姿，对所述移动设备进行位姿估计。
该实现方式中，基于感知矢量元素的横向坐标和所关联的投影矢量元素的横向坐标之间的差值来对移动设备进行位姿估计，可以避免平面假设引入的额外误差所导致的位姿估计不准的问题。换句话说，该实现方式可以提高位姿估计的准确性。
结合第一种可能的实现方式，在第二种可能的实现方式中，所述L个感知矢量元素中每个感知矢量元素的横向坐标与对应的第一投影矢量元素的横向坐标之间的差值，以及所述位置误差之间满足如下关系式：
Figure BDA0002694727090000026
其中，ut表示所述L个感知矢量元素中第t个感知矢量元素的横向坐标，ut'表示所述第t个感知矢量元素对应的第一投影矢量元素的横向坐标，error表示所述位置误差。
结合第一方面或上述任意一种可能的实现方式，在第三种可能的实现方式中，所述获取所述N个感知矢量元素和所述M个第一投影矢量元素的关联结果，包括：基于所述移动设备的第二粒子位姿，将所述M′个地图矢量元素投影到所述图像坐标系中，得到与所述M′个地图矢量元素中的M个地图矢量元素一一对应的M个第二投影矢量元素；从所述M个第二投影矢量元素和所述N个感知矢量元素中选出使得如下关系式得到满足L个第二投影矢量元素和L个感知矢量元素，其中，所述L个第二投影矢量元素与所述L个感知矢量元素一一关联：
Figure BDA0002694727090000021
Figure BDA0002694727090000022
Figure BDA0002694727090000023
xij＝0 or 1
Figure BDA0002694727090000024
Figure BDA0002694727090000025
其中，ω为预设的值，eij表示所述N个感知矢量元素中第i个感知矢量元素与所述M个第二投影矢量元素中第j个第二投影矢量元素的横向坐标之间的差值，xij表示所述第i个感知矢量元素与所述第j个第二投影矢量元素是否关联，Ni表示与所述N个感知矢量元素中第i个感知矢量元素相邻的感知矢量元素的个数，
Figure BDA0002694727090000032
表示与所述N个感知矢量元素中第i个感知矢量元素相邻的感知矢量元素的集合，xk表示所述N个感知矢量元素中的第k个感知矢量元素，pk表示所述第k个感知矢量元素是否已经找到关联的第二投影矢量元素，nk表示所述M个第二投影矢量元素中与所述第k个感知矢量元素匹配的第二投影矢量元素，
Figure BDA0002694727090000035
表示所述第i个感知矢量元素和所述第k个感知矢量元素之间的相邻关系与所述第j个第二投影矢量元素和与nk表示的第二投影矢量元素之间的相邻关系是否相同，
Figure BDA0002694727090000034
表示所述M个第二投影矢量元素中与所述第j个第二投影矢量元素相邻的第二投影矢量元素的集合，yk表示所述M个第二投影矢量元素中的第k个第二投影矢量元素，mk表示所述N个感知矢量元素中与所述第k个第二投影矢量元素关联的感知矢量元素，
Figure BDA0002694727090000033
表示所述第j个第二投影矢量元素和所述第k个第二投影矢量元素之间的相邻关系与所述第i个感知矢量元素和与mk表示的感知矢量元素之间的相邻关系是否相同；将所述L个感知矢量元素中每个感知矢量元素与所关联的第二投影矢量元素所对应的地图矢量元素关联；将所述L感知矢量元素中每个感知矢量元素与所关联的地图矢量元素所对应的第一投影矢量元素关联。
该实现方式中，将感知矢量元素与另一种粒子位姿对应的地图矢量元素的近似最优关联结果，作为感知矢量元素与第一粒子位姿对应的地图矢量元素的关联结果，并根据该关联结果对感知矢量元素和第一粒子位姿对应的投影矢量元素进行关联，可以提高关联准确性，从而提高位姿估计的准确性。
结合第三种可能的实现方式，在第四种可能的实现方式中，所述基于所述移动设备的第二粒子位姿，将所述M′个地图矢量元素投影到所述图像坐标系中，得到与所述M′个地图矢量元素中的M个地图矢量元素一一对应的M个第二投影矢量元素之前，所述方法还包括：将所述M′个地图矢量元素基于所述移动设备的多个粒子位姿中的每个粒子位姿投影到所述图像坐标系中，得到所述每个粒子位姿对应的M个投影地图矢量元素；将所述N个感知矢量元素与所述多个粒子位姿中的每个粒子位姿对应的M个投影地图矢量元素进行关联，得到所述每个粒子位姿对应的关联结果，所述多个粒子位姿中的第s个粒子位姿对应的关联结果包括：所述第s个粒子位姿对应的M个投影地图矢量元素中的Ls个投影矢量元素与所述N个感知矢量元素中的Ls个感知矢量元素一一关联，并且，所述Ls个感知矢量元素中第h个感知矢量元素在所述图像坐标系中的横向坐标小于所述Ls个感知矢量元素中第h+1个感知矢量元素在所述图像坐标系中的横向坐标时，所述第h个感知矢量元素关联的投影矢量元素的横向坐标小于所述第h+1个感知矢量元素关联的投影矢量元素的横向坐标，所述Ls个感知矢量元素中每个感知矢量元素的横向坐标与关联的投影矢量元素的横向坐标之间的差值小于预设的阈值，Ls为小于或等于M，且小于或等于N的正整数，h为小于Ls的正整数；根据所述多个粒子位姿中每个粒子位姿对应的关联结果，计算所述每个粒子位姿对应的Ls个投影矢量元素与对应的Ls个感知矢量元素之间的投影误差，得到与所述多个粒子位姿一一对应的多个投影误差；将所述多个投影误差中的最小投影误差对应的粒子位姿确定为所述第二粒子位姿。
该实现方式中，针对每个粒子位姿，按照一般的关联方式将感知矢量元素与该粒子位姿对应的投影矢量元素关联，并根据每个粒子位姿的关联结果计算投影误差，最终将投影误差最小的粒子位姿选为第二粒子位姿。这样可以使得基于第二粒子位姿的近似最优匹配结果对第一粒子位姿对应的投影矢量元素进行关联时，可以提高关联的准确性，进而可以提高位姿估计的准确性。
结合第四种可能的实现方式，在第五种可能的实现方式中，所述每个粒子位姿对应的投影误差通过如下关系式中C的值来度量：
Figure BDA0002694727090000031
其中，ω为预设的参数，ei表示所述Ls个投影矢量元素中的第i个投影矢量元素的横向坐标与所述Ls个感知矢量元素中的第i个感知矢量元素的横向坐标之间的差值，C的值越大表示所述每个粒子位姿对应的投影误差越小。
结合第一方面或上述任意一种可能的实现方式，在第六种可能的实现方式中，所述方法还包括：从所述图像采集装置采集的第二图像中提取X个感知矢量元素，X为正整数；若所述第二图像的时间戳位于上一次进行滤波观测更新使用的感知矢量元素所对应的图像的时间戳之后，则根据所述X个感知矢量元素使用滤波观测更新方法对所述移动设备进行位姿估计，否则根据所述X个感知矢量元素使用局部位姿图优化方法对所述移动设备进行位姿估计。
该实现方式，可以解决同一张图像中不同类型的感知矢量元素的异步输出问题和时间戳在前的图像中的感知矢量元素的输出时间晚于时间戳在后的图像中的感知矢量元素的输出时间的乱序问题，从而可以提高位姿估计的准确性。
结合第一方面或第一种或第二种可能的实现方式，在第七种可能的实现方式中，所述获取所述N个感知矢量元素和所述M个第一投影矢量元素的关联结果，包括：将所述N个感知矢量元素按照横向坐标从小到大的顺序排列，得到第一序列；从所述第一序列中的第一个感知矢量元素开始至最后一个感知矢量元素，针对所述第一序列中的每个感知矢量元素执行如下操作：从所述M个第一投影矢量元素中查找与所述每个感知矢量元素类型相同、与所述每个感知矢量元素的横向坐标之差最小、与所述每个感知矢量元素的横向坐标小于设定的阈值，且横向坐标大于所述每个感知矢量元素之前的感知矢量元素所关联的第一投影矢量元素的横向坐标的第一投影矢量元素，并将查找到的第一投影矢量元素与所述每个感知矢量元素关联。
第二方面，本申请提供一种移动设备的定位装置，所述装置可以包括用于实现第一方面中的方法的各个模块，这些模块可以通过软件和/或硬件的方式实现。
第三方面，本申请提供一种移动设备的定位装置，该装置包括：处理器，用于执行存储器存储的程序，当所述存储器存储的程序被执行时，所述处理器用于实现第一方面中的任意一种实现方式中的方法。
可选地，该装置还可以包括所述存储器，所述存储器用于存储程序，进一步地，可以用于存储定位过程中的相关数据信息。
作为一种示例，该装置可以是芯片，该芯片还可以包括通信接口。例如，该装置可以包括现场可编程门阵列(field－programmable gate array，FPGA)或者专用集成电路(application-specific integrated circuit，ASIC)。
第四方面，本申请提供一种计算机可读介质，该计算机可读介质存储用于设备执行的程序代码，该程序代码包括用于执行第一方面中的任意一种实现方式中的方法的指令。
第五方面，本申请提供一种包含指令的计算机程序产品，当该计算机程序产品在计算机上运行时，使得计算机执行上述第一方面中的任意一种实现方式中的方法。
第六方面，本申请提供一种移动设备，该移动设备可以包括第二方面至第五方面中任意一个所述的装置。
附图说明
图1为本申请的实施例的一种应用场景的示意图；
图2为智能驾驶车辆采用粒子滤波系统实现感知定位的一种方法的流程示意图；
图3为本申请一个实施例提供的数据关联的示意图；
图4为本申请一个实施例提供的数据关联方法的流程示意图；
图5为本申请另一个实施例提供的数据关联的方法的流程示意图；
图6为本申请一个实施例提供的移动设备的定位方法的流程示意图；
图7为本申请一个实施例的矢量元素的投影误差的示意图；
图8为本申请一个实施例提供的融合滤波观测更新与局部位姿图优化的定位方法的流程示意图；
图9为本申请一个实施例的装置的示意性结构图；
图10为本申请另一个实施例的装置的示意性结构图。
具体实施方式
为了便于理解，首先对本申请实施例所涉及的概念进行说明。
移动设备：是指任意能够通过传感器提供的数据进行感知定位的设备。例如，移动设备可以包括智能驾驶车辆、机器人或飞机等。智能驾驶车辆可以包括无人驾驶车辆或智能辅助驾驶车辆等；飞机可以包括无人机或智能辅助驾驶飞机等。
本申请的实施例中的传感器可以包括：卫星定位系统(global positioningsystem，GPS)、全球卫星导航系统(global navigation satellite system，GNSS)、惯性测量单元(inertial measurement unit，IMU)、视觉惯性导航系统(visual-inertialnavigation system，VINS)、惯性导航系统(inertial navigation system，INS)、图形处理单元(graphics processing unit，GPU)、单目相机和轮速计等。
传感器提供的数据可以包括移动设备的航向角、轮速、加速度、角速度和图像信息等。
矢量元素：是指移动设备的行驶环境中使用矢量形式表示的语义信息。例如，矢量元素可以为道路边缘、车道线、路面标识、红绿灯、标牌和灯杆等。
矢量定位：是指移动设备利用传感器将从采集的图像中提取的矢量元素与从地图中提取的矢量元素进行关联，对移动设备的系统状态向量进行估计，从而实现移动设备的感知定位的一种定位方法。
图1为本申请的实施例的一种应用场景的示意图。图1所示的场景是对智能驾驶车辆100进行感知定位的场景。传感器101用于获取自动驾驶车辆在行驶过程中的数据信息。作为示例，上述传感器可以包括GPS、GNSS、IMU、VINS、INS、GPU、单目相机和轮速计等。
可以理解的是，图1的场景仅是一种示例，本申请的技术方案还可以应用于其它场景，只要该场景涉及对移动设备的感知定位即可。例如，本申请的技术方案还可以应用于智能机器人定位、无人机定位等场景。
在图1所示的场景中，对智能驾驶车辆进行感知定位时，作为一种示例，可以构建里程计运动模型来更新粒子位姿，并采用粒子滤波系统中的观测更新操作计算每个粒子位姿的权重，以及基于每个粒子位姿的权重，对所有粒子位姿进行加权平均运算，计算出智能驾驶车辆的位姿和方差，或者说对智能驾驶车辆的位姿进行估计。
智能驾驶车辆采用粒子滤波系统实现感知定位的一种示例性方法如图2所示。下面对图2中的各个步骤进行介绍。为了便于描述，后续内容均将智能驾驶车辆简称为车辆。该方法可以由智能驾驶车辆的智能辅助驾驶系统执行。
S201，获取上一时刻对车辆进行感知定位所得的位姿信息。为了便于描述，将该上一时刻称为k时刻。
车辆的位姿信息可以包括车辆的位置信息和姿态信息，姿态信息可以包括航向角。
当上一时刻为车辆的初始运动时刻时，可以使用车辆上的GNSS和/或IMU获取车辆的初始位置信息。
S202，获取传感器采集的信息，并构建里程计运动模型。
传感器采集的信息可以包括车辆的轮速、加速度、角速度和包含周围环境信息的图像等。为了便于描述，将该图像称为第一图像。构建的里程计运动模型的一种示例如下：
Figure BDA0002694727090000061
Figure BDA0002694727090000062
Figure BDA0002694727090000063
Figure BDA0002694727090000064
Figure BDA0002694727090000065
Figure BDA0002694727090000066
其中，Δtk为k时刻与k+1时刻之间的时间间隔，k+1时刻为k时刻之后第一次对车辆进行感知定位的时刻，k+1时刻的预测位姿记为
Figure BDA0002694727090000067
为预测的k+1时刻车辆在世界坐标系下的二维位置，
Figure BDA0002694727090000068
为预测的k+1时刻车辆在世界坐标系下的航向角，k时刻的预测位姿记为Pk{xk,yk,θk}，xk,yk为预测的k时刻车辆在世界坐标系下的二维位置，θk为预测的k时刻车辆在世界坐标系下的航向角，ωk为车辆在k时刻的角速度，
Figure BDA0002694727090000069
为k时刻车辆左轮的轮速，
Figure BDA00026947270900000610
为k时刻车辆右轮的轮速，
Figure BDA00026947270900000611
为k时刻车辆的平均轮速，
Figure BDA00026947270900000612
为k时刻车辆在横向方向的轮速，
Figure BDA00026947270900000613
为k时刻车辆在纵向方向的轮速，δ表示噪声误差。
针对车辆的多个粒子位姿中的每个粒子位姿，可以通过上述里程计运动模型来获得k+1时刻的预测位姿。
k时刻为车辆的初始时刻时，这多个粒子位姿可以是基于车辆的初始位姿在车辆周围采集得到的粒子位姿。
S203，提取第一图像中的矢量元素。
为了描述方便，本实施例将从图像中提取的矢量元素称为感知矢量元素，将从第一图像中提取的感知矢量元素称为第一感知矢量元素。本实施例中，将第一图像中提取得到的感知矢量元素的数量记为N，N为正整数。
本实施例中，从第一图像中提取感知矢量元素可以包括：确定每个感知矢量元素的像素位置信息和元素类型。感知矢量元素的像素位置信息可以包括感知矢量元素的视觉边框的中心点在图像坐标系中的像素坐标和该视觉边框的长和宽。感知矢量元素的元素类型可以包括：道路边缘、车道线、路面标识、红绿灯、标牌或灯杆等。
S204，提取地图中的矢量元素。
本实施例中的地图可以是车辆所在区域的电子地图，例如高精度地图。例如，该地图可以是车辆所在市或所在区等区域的电子地图。为了便于描述，本实施例将从地图中提取的矢量元素称为地图矢量元素。
从地图中提取矢量元素时，作为一种示例，可以从地图中提取位于车辆指定位姿前方预设范围内的矢量元素。本实施例中将从地图中提取的地图矢量元素的数量记为M′，M′为正整数。该指定位姿可以是对S203中基于里程计运动模型处理得到的多个位置进行加权平均计算，得到的位姿。
从地图中提取地图矢量元素可以包括：从地图中确定每个地图矢量元素的位置信息和元素类型。地图矢量元素的位置信息可以是地图矢量元素在世界坐标系下的位置。地图矢量元素的元素类型可以为道路边缘、车道线、路面标识、红绿灯、标牌和灯杆等。
一种可能的实现方式，预设范围可以根据经验进行设置，例如，将预设范围设置为100米，则可以在地图中提取车辆前方100米以内的地图矢量元素。
另一种可能的实现方式，预设范围可以为图像采集装置的感知距离，即图像采集装置所采集的第一图像中的物体与车辆之间的距离。例如，图像采集装置的感知距离为80米，则可以在地图中提取车辆前方80米以内的地图矢量元素。
S205，基于每个粒子位姿，将地图矢量元素投影到像素坐标系下，得到投影后的矢量元素。本实施例中将地图矢量元素投影得到的矢量元素称为投影矢量元素。
基于每个粒子位姿对地图矢量元素进行投影，可以理解为基于车辆在k+1时刻的粒子位姿中的每个粒子位姿对地图矢量元素进行投影。
例如，针对k+1时刻的每个粒子位姿，将M′个地图矢量元素中的M个地图矢量元素投影到像素坐标系中，得到该粒子位姿对应的M个投影矢量元素。其中，M为小于或等于M′的正整数，且这M个投影矢量元素与这M个地图矢量元素一一对应。
可以理解的是，针对不同的粒子位姿，这M′个地图矢量元素中在像素坐标系中有对应投影矢量元素的地图矢量元素的数量可以相同，可以不同。也就是说，不同的粒子位姿所对应的M的值可能相同，有可能不相同。
将地图矢量元素投影到像素坐标系中得到对应的投影矢量元素，可以理解为：对地图矢量元素对应的物体在世界坐标中的位置进行变换，得到该物体在像素坐标系中的位置，即该物体在像素坐标系中的像素坐标。该物体在像素坐标系中的位置和该地图矢量元素的元素类型可以构成该地图矢量元素对应的投影矢量元素。
投影矢量元素在像素坐标系中的位置与地图矢量元素在世界坐标系中的位置之间满足如下关系式：
Figure BDA0002694727090000071
其中，Pi为第i个投影矢量元素在像素坐标系中的位置，
Figure BDA0002694727090000072
为第i个地图矢量元素在世界坐标系下的位置；Tv表示第一粒子位姿的矩阵表达形式，右上角的T表示矩阵转置操作；
Figure BDA0002694727090000073
表示自车坐标系和相机坐标系之间的变换关系，即相机的外参数；K表示相机的内参数；
Figure BDA0002694727090000074
表示第i个矢量元素转换到相机坐标系z轴上的坐标。相机的内外参数皆可通过相机标定获得。
若将车辆的多个粒子位姿中的一个粒子位姿称为第一粒子位姿，则基于第一粒子位姿，将M′个地图矢量元素中的M个地图矢量元素投影到像素坐标系中，可以得到M个投影矢量元素，这M个投影矢量元素与这M个地图矢量元素一一对应。本实施例中，将第一粒子位姿对应的M个投影矢量元素称为M个第一投影矢量元素。
S206，针对每个粒子位姿，对该粒子位姿对应的M个投影矢量元素与N个感知投影矢量元素关联。
投影矢量元素与感知投影矢量元素之间的一种关联方式包括：根据感知矢量元素与投影矢量元素之间的距离进行关联。具体地，针对每个感知矢量元素，计算该感知矢量元素的像素坐标与具有相同元素类型的每个投影矢量元素的像素坐标之间的距离，并将最小距离对应的投影矢量元素与该感知矢量元素关联。
S207，针对每个粒子位姿，根据该粒子位姿对应的投影矢量元素的位置与感知矢量元素的位置，计算该粒子位姿的投影误差。
例如，针对每个粒子位姿，假设该粒子位姿对应的M个投影矢量元素中有L个投影矢量元素与N个感知矢量元素中的L个感知矢量元素关联，则根据该L个投影矢量元素中每个投影矢量元素与这L个感知矢量元素中对应感知矢量元素之间的距离计算该粒子位姿的投影误差。
作为一种示例，每个粒子位姿对应的L个投影矢量元素中每个投影矢量元素与这L个感知矢量元素中对应感知矢量元素之间的距离与该粒子位姿的投影误差满足如下关系式：
Figure BDA0002694727090000081
其中，Δut表示上述L个感知矢量元素中第t个感知矢量元素与对应的投影矢量元素的距离，error表示投影误差，“||”表示求绝对值。
S208，根据每个粒子位姿的投影误差，对车辆的位姿进行滤波观测更新。示例性的，采用归一化的高斯模型计算k+1时刻第i个粒子位姿的权重：
Figure BDA0002694727090000082
Figure BDA0002694727090000083
其中，
Figure BDA0002694727090000084
表示k+1时刻第i个粒子位姿的权重，
Figure BDA0002694727090000085
表示k时刻第i个粒子位姿的权重，μ表示感知误差的均值，一般设为0；σ2表示感知误差的方差；α表示粒子上一时刻和当前时刻权重的置信参数，即k时刻和k+1时刻的权重的置信参数，errori表示第i个粒子位姿的投影误差。μ和σ2可以是根据历史感知矢量元素和对应真实地图矢量元素的投影矢量数据之间的投影误差统计得到的值。
获得每个粒子位姿的权重之后，对所有粒子位姿进行加权平均，得到车辆在k+1时刻的位姿估计{xk+1,yk+1,θk+1}和方差：
Figure BDA0002694727090000086
Figure BDA0002694727090000087
Figure BDA0002694727090000088
或者
Figure BDA0002694727090000089
Figure BDA00026947270900000810
θk+1＝tan-1(sinθk+1/cosθk+1)
其中，
Figure BDA00026947270900000811
表示n个粒子位姿中第i个粒子位姿，即世界坐标系下的二维位置和航向角；
Figure BDA00026947270900000812
表示k+1时刻第i个粒子位姿的权重，n为粒子位姿的数量。
估计位姿{xk+1,yk+1,θk+1}对应的方差包括：
Figure BDA0002694727090000091
Figure BDA0002694727090000092
Figure BDA0002694727090000093
其中，
Figure BDA0002694727090000094
表示k+1时刻第i个粒子位姿的权重，n为粒子位姿的数量。
在获得k+1时刻的观测更新位姿和方差后，可以判断是否执行重采样。是否重采样的判断条件如下：
Figure BDA0002694727090000095
其中，
Figure BDA0002694727090000096
表示k+1时刻第i个粒子位姿的权重，n为粒子位姿的数量。若
Figure BDA0002694727090000097
则重采样，否则可以不执行重采样。
进行重采样时，一种示例性方法为自适应重采样，即保留权重较大的10％粒子位姿，在k+1时刻位姿附近生成10％的新粒子位姿，剩余的80％粒子位姿从原始粒子集合随机选择，这样可以得到k+1时刻更新后的n个粒子位姿。
图2所示的方法中，对感知矢量元素和投影矢量元素进行关联时，将与感知矢量元素距离最近的投影矢量元素与该感知矢量元素关联，这样容易造成误关联现象。例如，如图3所示，使用图2所示的方法，会将两个信号灯一一对应的两个感知矢量元素301和302与两个投影矢量元素303和304中左边的投影矢量元素303关联，但实际上右边的感知矢量元素302应该关联右边的投影矢量元素304。
针对上述因为将距离最近的感知矢量元素和投影矢量元素关联而容易导致关联错误，进而导致位姿估计错误的问题，本申请提供了一种数据关联的方法，该方法中在对感知矢量元素和投影矢量元素进行关联时，基于感知矢量元素对投影矢量元素的横向位置的一致性进行关联。横向位置一致性是指对于投影矢量元素和所关联的感知矢量元素具有相同的元素类型，且被关联的感知矢量元素的横向位置分布和对应的投影矢量元素的横向位置分布是一致的。
例如，将N个感知矢量元素与M个投影矢量元素进行关联时，若N个感知矢量元素中的L个感知矢量元素与M个投影矢量元素中的L个投影矢量元素一一关联，则L个感知矢量元素中第t个感知矢量元素在图像坐标系中的横向坐标小于L个感知矢量元素中第t+1个感知矢量元素在图像坐标系中的横向坐标，且第t个感知矢量元素对应的投影矢量元素的横向坐标小于第t+1个感知矢量元素对应的投影矢量元素的横向坐标，以及第t个感知矢量元素与对应的投影矢量元素的元素类型相同时，可以认为这L个感知矢量元素与这L个感知矢量元素的横向位置分布具有一致性。其中，L为大于1的整数，且L小于或等于M和N的最小值，t为小于L的正整数。
本申请的方法可以提高感知矢量元素和投影矢量元素关联的准确度，减少误关联现象。在一些可能的实现方式中，L个感知矢量元素中每个感知矢量元素的横向坐标与对应的投影矢量元素的横向坐标之间的差值应小于预设的阈值。这样可以进一步保证关联的准确度，从而可以提高定位精度。
使用本申请提出的矢量元素的关联方法，可以实现图3中所示的感知矢量元素301与投影矢量元素303的关联，可以实现感知矢量元素302与投影矢量元素304的关联，从而可以提高关联准确度，进而可以提高定位精度。
下面结合附图，先介绍本申请实施例提供的数据关联方法。图4为本申请实施例提供的一种数据关联方法的流程示意图。如图4所示，该方法包括以下步骤：
S401，将N个感知矢量元素按照一定顺序排列为第一序列。
例如，将N个感知矢量元素按照横向坐标从小到大的顺序排列，得到第一序列；
又如，将N个感知矢量元素按照横向坐标从大到小的顺序排列，得到第一序列；
S402，从M个投影矢量元素中查找能够与第一序列中的感知矢量元素关联的投影矢量元素。
在一些可能的实现方式中，可以从第一序列中的第一个感知矢量元素开始至最后一个感知矢量元素，针对第一序列中的每个感知矢量元素执行如下操作：从M个投影矢量元素中查找与该感知矢量元素类型相同、与该感知矢量元素的横向坐标之差最小、与该感知矢量元素的横向坐标小于设定的阈值、与该感知矢量元素的纵向坐标小于设定的阈值，且横向坐标大于该感知矢量元素之前的感知矢量元素所关联的投影矢量元素的横向坐标的投影矢量元素。
作为一种示例，可以将M个投影矢量元素按照一定顺序排列为第二序列，第二序列中的投影矢量元素的排列顺序与第一序列中的感知矢量元素的排列顺序可以相同，例如，第二序列中的投影矢量元素按照横向坐标从小到大的顺序排列；然后，从第一序列中的第一个感知矢量元素开始至最后一个感知矢量元素，针对第一序列中的每个感知矢量元素执行如下操作：从第二序列中还未查找过的第一个投影矢量元素开始，查找与该感知矢量元素类型相同、与该感知矢量元素的横向坐标之差最小、与该感知矢量元素的横向坐标小于设定的阈值，与该感知矢量元素的纵向坐标小于设定的阈值，且横向坐标大于该感知矢量元素之前的感知矢量元素所关联的投影矢量元素的横向坐标的投影矢量元素。
示例性的，从M个投影矢量元素中查找与第一序列中的第二个感知矢量元素类型相同、与第二个感知矢量元素的横向坐标之差最小且小于50个像素、与第二个感知矢量元素的纵向坐标之差小于100个像素、且横向坐标大于第一个感知矢量元素关联的投影矢量元素的横向坐标的投影矢量元素。
S403，将感知矢量元素与查找的投影矢量元素进行关联。
在本申请实施例中，N个感知矢量元素和M个投影矢量元素的关联结果满足横向位置一致性，可以提高感知矢量元素与投影矢量元素关联的准确性，减少误关联。
进一步地，每个感知矢量元素的横向坐标与关联的投影矢量元素的横向坐标之间的差值小于预设的阈值，可以进一步提高感知矢量元素与投影矢量元素关联的准确性，进一步减少误关联。
进一步第，每个感知矢量元素的纵向坐标与关联的投影矢量元素的纵向坐标之间的差值小于预设的阈值，可以进一步提高感知矢量元素与投影矢量元素关联的准确性，进一步减少误关联。
图5为本申请实施例提供的另一种数据关联的方法的流程示意图。如图5所示，该方法可以包括：
S501，将M′个地图矢量元素基于移动设备的多个粒子位姿中的每个粒子位姿投影到图像坐标系中，得到每个粒子位姿对应的M个投影矢量元素，这M个投影矢量元素与这M′个地图矢量元素中的M个地图矢量元素一一对应，M为小于或等于M′的正整数。
可以理解的是，针对不同的粒子位姿，这M′个地图矢量元素中在像素坐标系中有对应投影矢量元素的地图矢量元素的数量可以相同，可以不同。也就是说，不同的粒子位姿所对应的M的值可能相同，有可能不相同。
S502，将N个感知矢量元素与多个粒子位姿中的每个粒子位姿对应的M个投影地图矢量元素进行关联，得到每个粒子位姿对应的关联结果。
将N个感知矢量元素与每个粒子位姿对应的M个投影地图矢量元素进行关联的关联方法，可以参考图4所示的方法。
示例性的，第5个粒子位姿对应的关联结果可以包括：第5个粒子位姿对应的10个投影矢量元素中的8个投影矢量元素与12个感知矢量元素中的8个感知矢量元素一一关联，并且，8个感知矢量元素中第3个感知矢量元素在图像坐标系中的横向坐标小于8个感知矢量元素中第4个感知矢量元素在图像坐标系中的横向坐标时，第3个感知矢量元素关联的投影矢量元素的横向坐标小于第4个感知矢量元素关联的投影矢量元素的横向坐标，8个感知矢量元素中每个感知矢量元素的横向坐标与关联的投影矢量元素的横向坐标之间的差值小于50个像素，8个感知矢量元素中每个感知矢量元素的纵向坐标与关联的投影矢量元素的纵向坐标之间的差值小于100个像素。
S503，根据多个粒子位姿中每个粒子位姿对应的关联结果，计算每个粒子位姿对应的Ls个投影矢量元素与对应的Ls个感知矢量元素之间的投影误差，得到与多个粒子位姿一一对应的多个投影误差。
在一种可能的实现方式中，针对每个粒子位姿对应的Ls个投影矢量元素和对应的Ls个感知矢量元素，可以通过如下关系式计算得到每个粒子位姿对应的C：
Figure BDA0002694727090000111
其中，ω为预设的参数，ei表示Ls个投影矢量元素中的第i个投影矢量元素的横向坐标与Ls个感知矢量元素中的第i个感知矢量元素的横向坐标之间的差值，C表示每个粒子位姿对应的平均位置误差和匹配数量的加权评估值。
然后可以通过C来度量每个例子位姿对应的投影误差，例如C的值越大，则认为对应的粒子位姿的投影误差越大。
可以理解的是，上述关系式仅是一种示例，对上述关系式进行任意变换所得的关系式都落入本申请实施例的保护范围。
S504，将多个投影误差中的最小投影误差对应的粒子位姿确定为第二粒子位姿。第二粒子位姿也可以称为近似最优匹配粒子位姿。
例如，可以将最大的C值对应的粒子位姿选为第二粒子位姿，就将最大的C值对应的粒子位姿选为近似最优匹配粒子位姿。
S505，对第二粒子位姿对应的M个投影矢量元素和N个感知矢量元素进行关联，并将其中满足预设关系式的关联结果作为该第二粒子位姿的最优关联结果。
在一种可能的实现方式中，预设关系式如下：
Figure BDA0002694727090000112
Figure BDA0002694727090000113
Figure BDA0002694727090000121
xij＝0 or 1
Figure BDA0002694727090000122
Figure BDA0002694727090000123
其中，ω为预设的值，eij表示N个感知矢量元素中第i个感知矢量元素与第二粒子位姿对应的M个投影矢量元素中第j个投影矢量元素的横向坐标之间的差值，xij表示第i个感知矢量元素与第j个投影矢量元素是否关联，Ni表示与N个感知矢量元素中第i个感知矢量元素相邻的感知矢量元素的个数，
Figure BDA0002694727090000125
表示与N个感知矢量元素中第i个感知矢量元素相邻的感知矢量元素的集合，xk表示N个感知矢量元素中的第k个感知矢量元素，pk表示所述第k个感知矢量元素是否已经找到关联的投影矢量元素，nk表示M个投影矢量元素中与所述第k个感知矢量元素匹配的投影矢量元素，
Figure BDA0002694727090000127
表示第i个感知矢量元素和第k个感知矢量元素之间的相邻关系与第j个投影矢量元素和与nk表示的投影矢量元素之间的相邻关系是否相同，
Figure BDA0002694727090000124
表示M个投影矢量元素中与第j个投影矢量元素相邻的投影矢量元素的集合，yk表示M个投影矢量元素中的第k个投影矢量元素，mk表示N个感知矢量元素中与第k个投影矢量元素关联的感知矢量元素，
Figure BDA0002694727090000126
表示第j个投影矢量元素和第k个投影矢量元素之间的相邻关系与第i个感知矢量元素和与mk表示的感知矢量元素之间的相邻关系是否相同。
S506，针对移动设备的多个粒子位姿中的第一粒子位姿，基于第二粒子位姿的最优关联结果，对第一粒子位姿对应的M个投影矢量元素和N个感知矢量元素进行关联。其中，第一粒子位姿可以是这个粒子位姿中除第二粒子位姿外的任意粒子位姿。
例如，将第二粒子位姿对应的投影矢量元素称为第二投影矢量元素，将第一粒子位姿对应的投影矢量元素称为第一投影矢量元素，则根据第二粒子位姿的最优关联结果，将该最优关联结果中的每个第二投影矢量元素对应的地图矢量元素与该第二投影矢量元素关联，并将第一粒子位姿对应的M个第一投影矢量元素中与该地图矢量元素对应的第一投影矢量元素与该第二投影矢量元素关联的感知矢量元素关联，最终可以得到第一粒子位姿对应的关联结果。
本申请实施例提供的数据关联方法，首先确定最优第二粒子位姿，再确定第二粒子位姿的最优关联结果，最后再基于第二粒子位姿的最优关联结果获取其他粒子位姿的关联结果，可以提高所有粒子位姿的关联结果的准确性，进而提高移动设备定位的精度。
本申请实施例提供的确定第二粒子位姿的方法，对每个粒子位姿得到的关联结果进行加权平均等计算，得到每个粒子位姿对应的位置误差。将位置误差最小时对应的粒子位姿确定为第二粒子位姿，提高了确定第二粒子位姿的准确度。
可以理解的是，上述实施例中从多个粒子位姿中选出最优粒子位置作为第二粒子位姿仅是一种示例，本申请的其他实施例中，可以通过其他方式从多个粒子位姿中选取第二粒子位姿，例如，可以从多个粒子位姿中随机选取第二粒子位姿，或者可以将权重最大的粒子位姿作为第二粒子位姿等。
进一步，本申请还提出了基于上述任意数据关联方法的定位方法。本申请提出的定位方法，因为误关联现象较少，所以可以提高位姿估计的准确度。
图6为本申请实施例提供的一种移动设备的定位方法的流程示意图。如图6所示，该方法可以包括：
S601，从移动设备的图像采集装置采集的第一图像中提取N个感知矢量元素，N为正整数。
其中，第一图像中可以包含不同类型的感知矢量元素，不同类型的感知矢量元素可以通过不同的神经网络进行提取。
在一些可能的实现方式中，移动设备的图像采集装置可以为单目相机、双目相机或深度相机。这三种图像采集装置都能够采集移动设备前方一定距离内的图像，双目相机和深度相机还能够提供图像的距离信息。
示例性的，单目相机结构简单，成本较低，在同一时间只能获取到一张图片，提取这张图片中的矢量元素，以二维的形式反映三维的场景。
示例性的，双目相机由两个单目相机组成，在同一时间可以获取两张图片。这两个单目相机之间的距离(也可以称为基线)是已知的，通过这个基线来估计每个像素的空间位置，基线距离越大，能够测量的范围就越大。双目相机使用GPU和现场可编程门阵列(fieldprogrammable gate array，FPGA)设备，实时输出图像的距离信息。
示例性的，深度相机可以通过红外结构光或激光时间测距原理(time of flight，ToF)，测量出物体离相机的距离。
S602，基于移动设备的第一粒子位姿，将地图中的M′个地图矢量元素投影到图像采集装置的图像坐标系，得到与M′个地图矢量元素中M个地图矢量元素一一对应的M个第一投影矢量元素，M′为正整数，M为小于或等于M′的正整数。
其中，移动设备可以具有多个粒子位姿，第一粒子位姿为这多个粒子位姿中的任意粒子位姿。这M个地图矢量元素可以是从高精度电子地图中位于移动设备预设范围内提取的所有地图矢量元素中全部或部分地图矢量元素。
针对这M个地图矢量元素中的同一个地图矢量元素，每一个第一粒子位姿都有一个投影矢量元素与该地图矢量元素对应。本实施例中，将第一粒子位姿对应的投影矢量元素称为第一投影矢量元素。
S603，获取N个感知矢量元素和M个第一投影矢量元素的关联结果，所述关联结果包括：N个感知矢量元素中的L个感知矢量元素与M个第一投影矢量元素中的L个第一投影矢量元素一一关联，L个感知矢量元素中第t个感知矢量元素在图像坐标系中的横向坐标小于L个感知矢量元素中第t+1个感知矢量元素在图像坐标系中的横向坐标时，第t个感知矢量元素对应的第一投影矢量元素的横向坐标小于第t+1个感知矢量元素对应的第一投影矢量元素的横向坐标，L为大于1的整数，且L小于或等于M和N的最小值，t为小于L的正整数，L个感知矢量元素中每个感知矢量元素与对应的第一投影矢量元素为同种类型的矢量元素。
本实施例中，获取到的N个感知矢量元素中并不是所有的感知矢量元素都能够找到与之关联的第一投影矢量元素。同理，获取到的M个第一投影矢量元素中也不是所有的第一投影矢量元素都能够找到与之关联的感知矢量元素。至多有L个感知矢量元素可以关联到L个第一投影矢量元素，L为大于1且小于或等于M和N的最小值的正整数。
例如，移动设备从第一图像和地图中获取到8个感知矢量元素和10个第一投影矢量元素，至多有8个感知矢量元素与8个第一投影矢量元素进行关联。
又如，移动设备从个第一图像和地图中获取到6个感知矢量元素和5个第一投影矢量元素，至多有5个感知矢量元素与5个第一投影矢量元素进行关联。
S604，根据L个感知矢量元素中每个感知矢量元素的坐标和对应的第一投影矢量元素的坐标以及第一粒子位姿，对移动设备进行位姿估计。
例如，可以采用滤波更新系统，根据L个感知矢量元素中每个感知矢量元素的坐标和对应的第一投影矢量元素的坐标，计算第一粒子位姿的权重，在基于该权重和第一粒子位姿对移动设备进行位姿估计。
作为一种可能的实现方式，根据L个感知矢量元素中每个感知矢量元素的横向坐标和对应的第一投影矢量元素的横向坐标之间的差值以及第一粒子位姿，对移动设备进行位姿估计。
作为又一种可能的实现方式，根据L个感知矢量元素中每个感知矢量元素的坐标和对应的第一投影矢量元素的坐标之间的距离以及第一粒子位姿，对移动设备进行位姿估计。
作为另一种可能的实现方式，根据L个感知矢量元素中每个感知矢量元素的纵向坐标和对应的第一投影矢量元素的纵向坐标之间的差值以及第一粒子位姿，对移动设备进行位姿估计。
在该实现方式中，根据L个感知矢量元素中每个感知矢量元素的坐标和对应的第一投影矢量元素的坐标以及第一粒子位姿，对移动设备进行位姿估计时，可以将每个感知矢量元素的横向坐标与对应的第一投影矢量元素的横向坐标之间的差值为这两个矢量元素之间的投影误差，并根据该投影误差构建移动设备的重投影横向误差观测模型，然后根据该观测模型计算第一粒子位姿的权重，再基于第一粒子位姿和其权重对移动设备进行位姿估计。
图7为矢量元素的投影误差的一种示意图。如图7所示，701为一个感知矢量元素，其中心点的像素坐标为(ui,vi)，702为701对应的第一投影矢量元素，其中心点的像素坐标为
Figure BDA0002694727090000141
则重投影横向误差观测模型的一种示例为：
Figure BDA0002694727090000142
其中，ui表示感知矢量元素的横向坐标，vi表示感知矢量元素的纵向坐标，
Figure BDA0002694727090000144
表示第一投影矢量元素的横向坐标，
Figure BDA0002694727090000143
表示第一投影矢量元素的纵向坐标，L为感知矢量元素的个数，error表示感知矢量元素与第一投影矢量元素的横向误差。
本实施例的一种实现方式中，根据该观测模型计算第一粒子位姿的权重，再基于第一粒子位姿和其权重对移动设备进行位姿估计的实现方式可以参考S207中的相关内容，此处不再赘述。
在本申请实施例中，N个感知矢量元素和M个第一投影矢量元素的关联结果满足横向位置一致性，可以提高感知矢量元素与第一投影矢量元素关联的准确性，减少误关联。
本实施例的一种可能的实现方式中，L个感知矢量元素中每个感知矢量元素的横向坐标与对应的第一投影矢量元素的横向坐标之间的差值小于预设的阈值，每个感知矢量元素的纵向坐标与对应的第一投影矢量元素的纵向坐标之间的差值小于预设的阈值。每个感知矢量元素的横向坐标与对应的第一投影矢量元素的横向坐标之间的差值小于的预设阈值，以及每个感知矢量元素的纵向坐标与对应的第一投影矢量元素的纵向坐标之间的差值小于的预设阈值可以是根据经验进行设置的。
例如，移动设备从第一图像和地图中获取到8个感知矢量元素和10个第一投影矢量元素，有6个感知矢量元素与6个第一投影矢量元素进行关联。6个感知矢量元素中第2个感知矢量元素在图像坐标系中的横向坐标小于6个感知矢量元素中第3个感知矢量元素在图像坐标系中的横向坐标时，第2个感知矢量元素对应的第一投影矢量元素的横向坐标小于第3个感知矢量元素对应的第一投影矢量元素的横向坐标，6个感知矢量元素中每个感知矢量元素的横向坐标与对应的第一投影矢量元素的横向坐标之间的差值小于50个像素，6个感知矢量元素中每个感知矢量元素的纵向坐标与对应的第一投影矢量元素的纵向坐标之间的差值小于100个像素，且6个感知矢量元素中每个感知矢量元素与对应的第一投影矢量元素为同种类型的矢量元素。
该实现方式中，因为每个感知矢量元素的横向坐标与关联的第一投影矢量元素的横向坐标之间的差值应足够小，因此可以进一步提高感知矢量元素与第一投影矢量元素关联的准确性，减少误关联。
在采用粒子滤波系统通过滤波观测更新方法实现移动设备的定位的方法中，没有考虑多矢量异步输出问题，因此会导致定位误差较大。例如，在k+1时刻进行位姿估计时，由于神经网络提取图像中某些类型的矢量元素的时间比较长，导致神经网络输出该类型的矢量元素的时间位于k+1时刻之后，则进行位姿估计所使用的感知矢量元素中就不会包含该类型的矢量元素。这会导致位姿估计所参考的矢量元素数量较少，从而影响位姿估计的准确度。
针对上述问题，本申请实施例提供了一种融合滤波观测更新与局部位姿图优化的定位方法，通过对每个采集到的图像的时间戳进行判断，来决定执行滤波观测更新过程还是执行局部位姿图优化过程。本实施例提供的定位方法，可以解决多矢量元素异步输出所导致的定位精度较低的问题。也就是说，本实施例提供的定位方法，可以提高定位精度。
下面结合附图，介绍本申请实施例提供的融合滤波观测更新与局部位姿图优化的定位方法。图8为本申请实施例提供的融合滤波观测更新与局部位姿图优化的定位方法的流程示意图。如图8所示，该方法可以包括以下步骤：
S801，获取感知矢量元素的时间戳。
作为一种示例，若该感知矢量元素是从第一图像中提取的，则可以将第一图像的时间戳作为该感知矢量元素的时间戳。若该感知矢量元素是从第二图像中提取的，则可以将第二图像的时间戳作为该感知矢量元素的时间戳。
可选地，第一图像和第二图像的时间戳可以分别为第一图像和第二图像的采集时间。例如，第一图像的时间戳为2020年9月1日10点30分05秒，第二图像的时间戳为2020年9月1日10点32分21秒。
S802，判断感知矢量元素的时间戳是否位于上一次进行滤波观测更新使用的感知矢量元素所对应的图像的时间戳之后，若是，则执行S803，否则执行S804。
例如，假设上一次进行滤波观测更新使用的感知矢量元素的时间戳为2020年9月1日10点32分18秒，而当前获取的感知矢量元素的时间戳位于2020年9月1日10点32分18秒之后，则执行S803，否则执行S804。
S803，执行滤波观测更新过程。
滤波观测更新的具体实现方式可以参考图2或图6所示的方法中的相关步骤，此处不再赘述。
S804，执行局部位姿图优化过程。
局部位姿图优化过程是指对当前滑动窗口中的矢量因子执行位姿优化，通过优化滑动窗口内的位姿估计，来更新当前里程计运动估计结果。
在k时刻进行局部位姿图优化过程的一种实现方式中，首先构建k时刻的矢量因子
Figure BDA0002694727090000151
和里程计因子
Figure BDA0002694727090000152
如下：
Figure BDA0002694727090000153
Figure BDA0002694727090000161
其中，
Figure BDA0002694727090000162
表示k时刻路标重投影到图像像素坐标系下的横向位置；
Figure BDA0002694727090000163
表示k时刻感知矢量元素横向像素坐标位置；
Figure BDA0002694727090000164
表示k到k+1时刻里程计的位姿变化估计；Tk和Tk+1分别表示k时刻和k+1时刻自车位姿的矩阵形式表达：
Figure BDA0002694727090000165
Figure BDA0002694727090000166
然后，将构建的矢量因子和里程计因子插入到滑动窗口中，并构建如下的目标函数：
Figure BDA0002694727090000167
其中，x表示滑动窗口内待优化的所有位姿；
Figure BDA0002694727090000168
表示里程计因子的协方差矩阵；
Figure BDA0002694727090000169
表示矢量因子的协方差矩阵。然后使用拉文伯格-马奎尔特方法有效地优化非线性最小二乘目标函数，得到优化的位姿。并使用该优化的位姿更新里程计位姿递推估计结果。
本申请实施例提供的融合滤波观测更新与局部位姿图优化方法，根据图像的时间戳大小来判断执行滤波观测更新过程还是局部位姿图优化过程，减小了计算量，降低了累计误差，解决了多矢量异步输出问题，保证了实时输出自动驾驶车辆的位姿，提高了定位精度。
本申请还提出了前述各个实施例的方法所对应的装置。例如，本申请提出的装置可以包括用于实现图4、图5或图6所示方法的各个功能模块，以实现对应的方法。
图9是本申请一个实施例提供的定位装置900的示意性框图。应理解，图9示出的定位装置900仅是示例，本申请实施例的装置900还可包括其他模块或单元。
例如，装置900可以包括提取模块910、投影模块920、关联模块930和估计模块940。其中，提取模块910用于执行S601，投影模块920用于执行S602，关联模块930用于执行S603，估计模块940用于执行S604。
应理解，这里的装置900以功能模块的形式体现。这里的术语“模块”可以通过软件和/或硬件形式实现，对此不作具体限定。例如，“模块”可以是实现上述功能的软件程序、硬件电路或二者结合。硬件电路可能包括应用特有集成电路(application specificintegrated circuit，ASIC)、电子电路、用于执行一个或多个软件或固件程序的处理器(例如共享处理器、专有处理器或组处理器等)和存储器、合并逻辑电路和/或其它支持所描述的功能的合适组件。
以提取模块910通过软件形式实现为例，提取模块910可以包括用于从图像中提取各种类型的矢量元素的神经网络模型；以提取模块910通过软件形式和硬件形式实现为例，提取模块910可以包括用于从图像中提取各种类型的矢量元素的神经网络模型和神经网络处理器。
图10是本申请一个实施例的装置1000的示意性框图。图10所示的装置1000包括存储器1001、处理器1002、通信接口1003以及总线1004。其中，存储器1001、处理器1002、通信接口1003通过总线1004实现彼此之间的通信连接。
存储器1001可以是只读存储器(read only memory，ROM)，静态存储设备，动态存储设备或者随机存取存储器(random access memory，RAM)。存储器1001可以存储程序，当存储器1001中存储的程序被处理器1002执行时，处理器1002用于执行图4、图5或图6所示的方法的各个步骤。
处理器1002可以采用通用的中央处理器(central processing unit，CPU)，微处理器，应用专用集成电路(application specific integrated circuit，ASIC)，或者一个或多个集成电路，用于执行相关程序，以实现本申请各个实施例中的方法。
处理器1002还可以是一种集成电路芯片，具有信号的处理能力。在实现过程中，本申请各个实施例的方法的各个步骤可以通过处理器1002中的硬件的集成逻辑电路或者软件形式的指令完成。
上述处理器1002还可以是通用处理器、数字信号处理器(digital signalprocessing，DSP)、专用集成电路(ASIC)、现成可编程门阵列(field programmable gatearray，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。可以实现或者执行本申请实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
结合本申请实施例所公开的方法的步骤可以直接体现为硬件译码处理器执行完成，或者用译码处理器中的硬件及软件模块组合执行完成。软件模块可以位于随机存储器，闪存、只读存储器，可编程只读存储器或者电可擦写可编程存储器、寄存器等本领域成熟的存储介质中。该存储介质位于存储器1001，处理器1002读取存储器1001中的信息，结合其硬件完成本申请各个装置包括的单元所需执行的功能，例如，可以执行图4、图5或图6所示实施例的各个步骤/功能。
通信接口1003可以使用但不限于收发器一类的收发装置，来实现装置1000与其他设备或通信网络之间的通信。
总线1004可以包括在装置1000各个部件(例如，存储器1001、处理器1002、通信接口1003)之间传送信息的通路。
应理解，本申请实施例所示的装置1000可以是电子设备，或者，也可以是配置于电子设备中的芯片。
应理解，本申请实施例中的处理器可以为中央处理单元(central processingunit，CPU)，该处理器还可以是其他通用处理器、数字信号处理器(digital signalprocessor，DSP)、专用集成电路(application specific integrated circuit，ASIC)、现成可编程门阵列(field programmable gate array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
还应理解，本申请实施例中的存储器可以是易失性存储器或非易失性存储器，或可包括易失性和非易失性存储器两者。其中，非易失性存储器可以是只读存储器(read-only memory，ROM)、可编程只读存储器(programmable ROM，PROM)、可擦除可编程只读存储器(erasable PROM，EPROM)、电可擦除可编程只读存储器(electrically EPROM，EEPROM)或闪存。易失性存储器可以是随机存取存储器(random access memory，RAM)，其用作外部高速缓存。通过示例性但不是限制性说明，许多形式的随机存取存储器(random accessmemory，RAM)可用，例如静态随机存取存储器(static RAM，SRAM)、动态随机存取存储器(DRAM)、同步动态随机存取存储器(synchronous DRAM，SDRAM)、双倍数据速率同步动态随机存取存储器(double data rate SDRAM，DDR SDRAM)、增强型同步动态随机存取存储器(enhanced SDRAM，ESDRAM)、同步连接动态随机存取存储器(synchlink DRAM，SLDRAM)和直接内存总线随机存取存储器(direct rambus RAM，DR RAM)。
上述实施例，可以全部或部分地通过软件、硬件、固件或其他任意组合来实现。当使用软件实现时，上述实施例可以全部或部分地以计算机程序产品的形式实现。计算机程序产品包括一个或多个计算机指令或计算机程序。在计算机上加载或执行计算机指令或计算机程序时，全部或部分地产生按照本申请实施例的流程或功能。计算机可以为通用计算机、专用计算机、计算机网络、或者其他可编程装置。计算机指令可以存储在计算机可读存储介质中，或者从一个计算机可读存储介质向另一个计算机可读存储介质传输，例如，计算机指令可以从一个网站站点、计算机、服务器或数据中心通过有线(例如红外、无线、微波等)方式向另一个网站站点、计算机、服务器或数据中心进行传输。计算机可读存储介质可以是计算机能够存取的任何可用介质或者是包含一个或多个可用介质集合的服务器、数据中心等数据存储设备。可用介质可以是磁性介质(例如，软盘、硬盘、磁带)、光介质(例如，DVD)、或者半导体介质。半导体介质可以是固态硬盘。
应理解，本文中术语“和/或”，仅仅是一种描述关联对象的关联关系，表示可以存在三种关系，例如，A和/或B，可以表示：单独存在A，同时存在A和B，单独存在B这三种情况，其中A,B可以是单数或者复数。另外，本文中字符“/”，一般表示前后关联对象是一种“或”的关系，但也可能表示的是一种“和/或”的关系，具体可参考前后文进行理解。
本申请中，“至少一个”是指一个或者多个，“多个”是指两个或两个以上。“以下至少一项(个)”或其类似表达，是指的这些项中的任意组合，包括单项(个)或复数项(个)的任意组合。例如，a,b,或c中的至少一项(个)，可以表示：a,b,c,a-b,a-c,b-c,或a-b-c，其中a,b,c可以是单个，也可以是多个。
应理解，在本申请的各种实施例中，上述各过程的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本申请实施例的实施过程构成任何限定。
本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。
所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统、装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在本申请所提供的几个实施例中，应该理解到，所揭露的系统、装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。
作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。
功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本申请各个实施例方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器、随机存取存储器、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述，仅为本申请的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应以权利要求的保护范围为准。

Claims (19)

1.一种移动设备的定位方法，其特征在于，包括：
从所述移动设备的图像采集装置采集的第一图像中提取N个感知矢量元素，N为正整数；
基于所述移动设备的第一粒子位姿，将地图中的M′个地图矢量元素投影到所述图像采集装置的图像坐标系，得到与所述M′个地图矢量元素中M个地图矢量元素一一对应的M个第一投影矢量元素，M′为正整数，M为小于或等于M′的正整数；
获取所述N个感知矢量元素和所述M个第一投影矢量元素的关联结果，所述关联结果包括：所述N个感知矢量元素中的L个感知矢量元素与所述M个第一投影矢量元素中的L个第一投影矢量元素一一关联，所述L个感知矢量元素中第t个感知矢量元素在所述图像坐标系中的横向坐标小于所述L个感知矢量元素中第t+1个感知矢量元素在所述图像坐标系中的横向坐标时，所述第t个感知矢量元素对应的第一投影矢量元素的横向坐标小于所述第t+1个感知矢量元素对应的第一投影矢量元素的横向坐标，L为大于1的整数，且L小于或等于M和N的最小值，t为小于L的正整数，所述L个感知矢量元素中每个感知矢量元素与对应的第一投影矢量元素为同种类型的矢量元素；
根据所述L个感知矢量元素中每个感知矢量元素的坐标和对应的第一投影矢量元素的坐标以及所述第一粒子位姿，对所述移动设备进行位姿估计。
2.根据权利要求1所述的方法，其特征在于，所述根据所述L个感知矢量元素中每个感知矢量元素的坐标和对应的第一投影矢量元素的坐标以及所述第一粒子位姿，对所述移动设备进行位姿估计，包括：
根据所述L个感知矢量元素中每个感知矢量元素的横向坐标和对应的第一投影矢量元素的横向坐标之间的差值，确定所述L个感知矢量元素与对应的L个第一投影矢量元素之间的位置误差；
根据所述位置误差确定所述第一粒子位姿的权重；
根据所述第一粒子位姿的权重和所述第一粒子位姿，对所述移动设备进行位姿估计。
3.根据权利要求2所述的方法，其特征在于，所述L个感知矢量元素中每个感知矢量元素的横向坐标与对应的第一投影矢量元素的横向坐标之间的差值，以及所述位置误差之间满足如下关系式：
Figure FDA0002694727080000011
其中，ut表示所述L个感知矢量元素中第t个感知矢量元素的横向坐标，ut'表示所述第t个感知矢量元素对应的第一投影矢量元素的横向坐标，error表示所述位置误差。
4.根据权利要求1至3中任一项所述的方法，其特征在于，所述获取所述N个感知矢量元素和所述M个第一投影矢量元素的关联结果，包括：
基于所述移动设备的第二粒子位姿，将所述M′个地图矢量元素投影到所述图像坐标系中，得到与所述M′个地图矢量元素中的M个地图矢量元素一一对应的M个第二投影矢量元素；
从所述M个第二投影矢量元素和所述N个感知矢量元素中选出使得如下关系式得到满足L个第二投影矢量元素和L个感知矢量元素，其中，所述L个第二投影矢量元素与所述L个感知矢量元素一一关联：
Figure FDA0002694727080000021
Figure FDA0002694727080000022
Figure FDA0002694727080000023
xij＝0 or 1
Figure FDA0002694727080000024
Figure FDA0002694727080000025
其中，ω为预设的值，eij表示所述N个感知矢量元素中第i个感知矢量元素与所述M个第二投影矢量元素中第j个第二投影矢量元素的横向坐标之间的差值，xij表示所述第i个感知矢量元素与所述第j个第二投影矢量元素是否关联，Ni表示与所述N个感知矢量元素中第i个感知矢量元素相邻的感知矢量元素的个数，
Figure FDA0002694727080000026
表示与所述N个感知矢量元素中第i个感知矢量元素相邻的感知矢量元素的集合，xk表示所述N个感知矢量元素中的第k个感知矢量元素，pk表示所述第k个感知矢量元素是否已经找到关联的第二投影矢量元素，nk表示所述M个第二投影矢量元素中与所述第k个感知矢量元素匹配的第二投影矢量元素，
Figure FDA0002694727080000027
表示所述第i个感知矢量元素和所述第k个感知矢量元素之间的相邻关系与所述第j个第二投影矢量元素和与nk表示的第二投影矢量元素之间的相邻关系是否相同，
Figure FDA0002694727080000028
表示所述M个第二投影矢量元素中与所述第j个第二投影矢量元素相邻的第二投影矢量元素的集合，yk表示所述M个第二投影矢量元素中的第k个第二投影矢量元素，mk表示所述N个感知矢量元素中与所述第k个第二投影矢量元素关联的感知矢量元素，
Figure FDA0002694727080000029
表示所述第j个第二投影矢量元素和所述第k个第二投影矢量元素之间的相邻关系与所述第i个感知矢量元素和与mk表示的感知矢量元素之间的相邻关系是否相同；
将所述L个感知矢量元素中每个感知矢量元素与所关联的第二投影矢量元素所对应的地图矢量元素关联；
将所述L感知矢量元素中每个感知矢量元素与所关联的地图矢量元素所对应的第一投影矢量元素关联。
5.根据权利要求4所述的方法，其特征在于，所述基于所述移动设备的第二粒子位姿，将所述M′个地图矢量元素投影到所述图像坐标系中，得到与所述M′个地图矢量元素中的M个地图矢量元素一一对应的M个第二投影矢量元素之前，所述方法还包括：
将所述M′个地图矢量元素基于所述移动设备的多个粒子位姿中的每个粒子位姿投影到所述图像坐标系中，得到所述每个粒子位姿对应的M个投影地图矢量元素；
将所述N个感知矢量元素与所述多个粒子位姿中的每个粒子位姿对应的M个投影地图矢量元素进行关联，得到所述每个粒子位姿对应的关联结果，所述多个粒子位姿中的第s个粒子位姿对应的关联结果包括：所述第s个粒子位姿对应的M个投影地图矢量元素中的Ls个投影矢量元素与所述N个感知矢量元素中的Ls个感知矢量元素一一关联，并且，所述Ls个感知矢量元素中第h个感知矢量元素在所述图像坐标系中的横向坐标小于所述Ls个感知矢量元素中第h+1个感知矢量元素在所述图像坐标系中的横向坐标时，所述第h个感知矢量元素关联的投影矢量元素的横向坐标小于所述第h+1个感知矢量元素关联的投影矢量元素的横向坐标，所述Ls个感知矢量元素中每个感知矢量元素的横向坐标与关联的投影矢量元素的横向坐标之间的差值小于预设的阈值，Ls为小于或等于M，且小于或等于N的正整数，h为小于Ls的正整数；
根据所述多个粒子位姿中每个粒子位姿对应的关联结果，计算所述每个粒子位姿对应的Ls个投影矢量元素与对应的Ls个感知矢量元素之间的投影误差，得到与所述多个粒子位姿一一对应的多个投影误差；
将所述多个投影误差中的最小投影误差对应的粒子位姿确定为所述第二粒子位姿。
6.根据权利要求5所述的方法，其特征在于，所述每个粒子位姿对应的投影误差通过如下关系式中C的值来度量：
Figure FDA0002694727080000031
其中，ω为预设的参数，ei表示所述Ls个投影矢量元素中的第i个投影矢量元素的横向坐标与所述Ls个感知矢量元素中的第i个感知矢量元素的横向坐标之间的差值，C的值越大表示所述每个粒子位姿对应的投影误差越小。
7.根据权利要求1至6中任一项所述的方法，其特征在于，还包括：
从所述图像采集装置采集的第二图像中提取X个感知矢量元素，X为正整数；
若所述第二图像的时间戳位于上一次进行滤波观测更新所使用的感知矢量元素所对应的图像的时间戳之后，则根据所述X个感知矢量元素使用滤波观测更新方法对所述移动设备进行位姿估计，否则根据所述X个感知矢量元素使用局部位姿图优化方法对所述移动设备进行位姿估计。
8.根据权利要求1至3中任一项所述的方法，其特征在于，所述获取所述N个感知矢量元素和所述M个第一投影矢量元素的关联结果，包括：
将所述N个感知矢量元素按照横向坐标从小到大的顺序排列，得到第一序列；
从所述第一序列中的第一个感知矢量元素开始至最后一个感知矢量元素，针对所述第一序列中的每个感知矢量元素执行如下操作：从所述M个第一投影矢量元素中查找与所述每个感知矢量元素类型相同、与所述每个感知矢量元素的横向坐标之差最小、与所述每个感知矢量元素的横向坐标小于设定的阈值，且横向坐标大于所述每个感知矢量元素之前的感知矢量元素所关联的第一投影矢量元素的横向坐标的第一投影矢量元素，并将查找到的第一投影矢量元素与所述每个感知矢量元素关联。
9.一种移动设备的定位装置，其特征在于，包括：
提取模块，用于从所述移动设备的图像采集装置采集的第一图像中提取N个感知矢量元素，N为正整数；
投影模块，用于基于所述移动设备的第一粒子位姿，将地图中的M′个地图矢量元素投影到所述图像采集装置的图像坐标系，得到与所述M′个地图矢量元素中M个地图矢量元素一一对应的M个第一投影矢量元素，M′为正整数，M为小于或等于M′的正整数；
关联模块，用于获取所述N个感知矢量元素和所述M个第一投影矢量元素的关联结果，所述关联结果包括：所述N个感知矢量元素中的L个感知矢量元素与所述M个第一投影矢量元素中的L个第一投影矢量元素一一关联，所述L个感知矢量元素中第t个感知矢量元素在所述图像坐标系中的横向坐标小于所述L个感知矢量元素中第t+1个感知矢量元素在所述图像坐标系中的横向坐标时，所述第t个感知矢量元素对应的第一投影矢量元素的横向坐标小于所述第t+1个感知矢量元素对应的第一投影矢量元素的横向坐标，L为大于1的整数，且L小于或等于M和N的最小值，t为小于L的正整数，所述L个感知矢量元素中每个感知矢量元素与对应的第一投影矢量元素为同种类型的矢量元素；
估计模块，用于根据所述L个感知矢量元素中每个感知矢量元素的坐标和对应的第一投影矢量元素的坐标以及所述第一粒子位姿，对所述移动设备进行位姿估计。
10.根据权利要求9所述的装置，其特征在于，所述估计模块具体用于：
根据所述L个感知矢量元素中每个感知矢量元素的横向坐标和对应的第一投影矢量元素的横向坐标之间的差值，确定所述L个感知矢量元素与对应的L个第一投影矢量元素之间的位置误差；
根据所述位置误差确定所述第一粒子位姿的权重；
根据所述第一粒子位姿的权重和所述第一粒子位姿，对所述移动设备进行位姿估计。
11.根据权利要求10所述的装置，其特征在于，所述L个感知矢量元素中每个感知矢量元素的横向坐标与对应的第一投影矢量元素的横向坐标之间的差值，以及所述位置误差之间满足如下关系式：
Figure FDA0002694727080000041
其中，ut表示所述L个感知矢量元素中第t个感知矢量元素的横向坐标，ut'表示所述第t个感知矢量元素对应的第一投影矢量元素的横向坐标，error表示所述位置误差。
12.根据权利要求9至11中任一项所述的装置，其特征在于，所述关联模块具体用于：
基于所述移动设备的第二粒子位姿，将所述M′个地图矢量元素投影到所述图像坐标系中，得到与所述M′个地图矢量元素中的M个地图矢量元素一一对应的M个第二投影矢量元素；
从所述M个第二投影矢量元素和所述N个感知矢量元素中选出使得如下关系式得到满足L个第二投影矢量元素和L个感知矢量元素，其中，所述L个第二投影矢量元素与所述L个感知矢量元素一一关联：
Figure FDA0002694727080000042
Figure FDA0002694727080000043
Figure FDA0002694727080000044
xij＝0 or 1
Figure FDA0002694727080000045
Figure FDA0002694727080000046
其中，ω为预设的值，eij表示所述N个感知矢量元素中第i个感知矢量元素与所述M个第二投影矢量元素中第j个第二投影矢量元素的横向坐标之间的差值，xij表示所述第i个感知矢量元素与所述第j个第二投影矢量元素是否关联，Ni表示与所述N个感知矢量元素中第i个感知矢量元素相邻的感知矢量元素的个数，
Figure FDA0002694727080000047
表示与所述N个感知矢量元素中第i个感知矢量元素相邻的感知矢量元素的集合，xk表示所述N个感知矢量元素中的第k个感知矢量元素，pk表示所述第k个感知矢量元素是否已经找到关联的第二投影矢量元素，nk表示所述M个第二投影矢量元素中与所述第k个感知矢量元素匹配的第二投影矢量元素，
Figure FDA0002694727080000048
表示所述第i个感知矢量元素和所述第k个感知矢量元素之间的相邻关系与所述第j个第二投影矢量元素和与nk表示的第二投影矢量元素之间的相邻关系是否相同，
Figure FDA0002694727080000051
表示所述M个第二投影矢量元素中与所述第j个第二投影矢量元素相邻的第二投影矢量元素的集合，yk表示所述M个第二投影矢量元素中的第k个第二投影矢量元素，mk表示所述N个感知矢量元素中与所述第k个第二投影矢量元素关联的感知矢量元素，
Figure FDA0002694727080000052
表示所述第j个第二投影矢量元素和所述第k个第二投影矢量元素之间的相邻关系与所述第i个感知矢量元素和与mk表示的感知矢量元素之间的相邻关系是否相同；
将所述L个感知矢量元素中每个感知矢量元素与所关联的第二投影矢量元素所对应的地图矢量元素关联；
将所述L感知矢量元素中每个感知矢量元素与所关联的地图矢量元素所对应的第一投影矢量元素关联。
13.根据权利要求12所述的装置，其特征在于，所述投影模块基于所述移动设备的第二粒子位姿，将所述M′个地图矢量元素投影到所述图像坐标系中，得到与所述M′个地图矢量元素中的M个地图矢量元素一一对应的M个第二投影矢量元素之前，所述关联模块还用于：
将所述M′个地图矢量元素基于所述移动设备的多个粒子位姿中的每个粒子位姿投影到所述图像坐标系中，得到所述每个粒子位姿对应的M个投影地图矢量元素；
将所述N个感知矢量元素与所述多个粒子位姿中的每个粒子位姿对应的M个投影地图矢量元素进行关联，得到所述每个粒子位姿对应的关联结果，所述多个粒子位姿中的第s个粒子位姿对应的关联结果包括：所述第s个粒子位姿对应的M个投影地图矢量元素中的Ls个投影矢量元素与所述N个感知矢量元素中的Ls个感知矢量元素一一关联，并且，所述Ls个感知矢量元素中第h个感知矢量元素在所述图像坐标系中的横向坐标小于所述Ls个感知矢量元素中第h+1个感知矢量元素在所述图像坐标系中的横向坐标时，所述第h个感知矢量元素关联的投影矢量元素的横向坐标小于所述第h+1个感知矢量元素关联的投影矢量元素的横向坐标，所述Ls个感知矢量元素中每个感知矢量元素的横向坐标与关联的投影矢量元素的横向坐标之间的差值小于预设的阈值，Ls为小于或等于M，且小于或等于N的正整数，h为小于Ls的正整数；
根据所述多个粒子位姿中每个粒子位姿对应的关联结果，计算所述每个粒子位姿对应的Ls个投影矢量元素与对应的Ls个感知矢量元素之间的投影误差，得到与所述多个粒子位姿一一对应的多个投影误差；
将所述多个投影误差中的最小投影误差对应的粒子位姿确定为所述第二粒子位姿。
14.根据权利要求13所述的装置，其特征在于，所述每个粒子位姿对应的投影误差通过如下关系式中C的值来度量：
Figure FDA0002694727080000053
其中，ω为预设的参数，ei表示所述Ls个投影矢量元素中的第i个投影矢量元素的横向坐标与所述Ls个感知矢量元素中的第i个感知矢量元素的横向坐标之间的差值，C的值越大表示所述每个粒子位姿对应的投影误差越小。
15.根据权利要求9至14中任一项所述的装置，其特征在于，所述提取模块还用于：从所述图像采集装置采集的第二图像中提取X个感知矢量元素，X为正整数；
若所述第二图像的时间戳位于上一次进行滤波观测更新使用的感知矢量元素所对应的图像的时间戳之后，则所述估计模块还用于根据所述X个感知矢量元素使用滤波观测更新方法对所述移动设备进行位姿估计，否则还用于根据所述X个感知矢量元素使用局部位姿图优化方法对所述移动设备进行位姿估计。
16.根据权利要求9至11中任一项所述的装置，其特征在于，所述关联模块具体用于：
将所述N个感知矢量元素按照横向坐标从小到大的顺序排列，得到第一序列；
从所述第一序列中的第一个感知矢量元素开始至最后一个感知矢量元素，针对所述第一序列中的每个感知矢量元素执行如下操作：从所述M个第一投影矢量元素中查找与所述每个感知矢量元素类型相同、与所述每个感知矢量元素的横向坐标之差最小、与所述每个感知矢量元素的横向坐标小于设定的阈值，且横向坐标大于所述每个感知矢量元素之前的感知矢量元素所关联的第一投影矢量元素的横向坐标的第一投影矢量元素，并将查找到的第一投影矢量元素与所述每个感知矢量元素关联。
17.一种移动设备的定位装置，其特征在于，所述装置包括与存储器耦合的处理器，所述存储器用于存储程序代码，所述处理器用于执行所述存储器中的所述程序代码，以实现如权利要求1至8中任一项所述的方法。
18.一种计算机可读介质，其特征在于，所述计算机可读介质存储用于计算机执行的程序代码，该程序代码包括用于执行如权利要求1至8中任一项所述的方法的指令。
19.一种芯片，其特征在于，所述芯片包括处理器与数据接口，所述处理器通过所述数据接口读取存储器上存储的指令，以执行如权利要求1至8中任一项所述的方法。
CN202011002221.4A 2020-09-22 2020-09-22 移动设备的定位方法和定位装置 Active CN114248778B (zh)

Priority Applications (4)

Application Number Priority Date Filing Date Title
CN202011002221.4A CN114248778B (zh) 2020-09-22 2020-09-22 移动设备的定位方法和定位装置
EP21870863.4A EP4209755A4 (en) 2020-09-22 2021-06-09 POSITIONING METHOD AND POSITIONING DEVICE OF A MOBILE DEVICE
PCT/CN2021/099232 WO2022062480A1 (zh) 2020-09-22 2021-06-09 移动设备的定位方法和定位装置
US18/124,354 US20230222688A1 (en) 2020-09-22 2023-03-21 Mobile device positioning method and positioning apparatus

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011002221.4A CN114248778B (zh) 2020-09-22 2020-09-22 移动设备的定位方法和定位装置

Publications (2)

Publication Number Publication Date
CN114248778A true CN114248778A (zh) 2022-03-29
CN114248778B CN114248778B (zh) 2024-04-12

Family

ID=80789611

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011002221.4A Active CN114248778B (zh) 2020-09-22 2020-09-22 移动设备的定位方法和定位装置

Country Status (4)

Country Link
US (1) US20230222688A1 (zh)
EP (1) EP4209755A4 (zh)
CN (1) CN114248778B (zh)
WO (1) WO2022062480A1 (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2024041447A1 (zh) * 2022-08-24 2024-02-29 上海安亭地平线智能交通技术有限公司 位姿的确定方法、装置、电子设备和存储介质

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20230400306A1 (en) * 2022-06-14 2023-12-14 Volvo Car Corporation Localization for autonomous movement using vehicle sensors
CN115143985B (zh) * 2022-09-05 2022-12-09 小米汽车科技有限公司 车辆定位方法、装置、车辆及可读存储介质

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20050182518A1 (en) * 2004-02-13 2005-08-18 Evolution Robotics, Inc. Robust sensor fusion for mapping and localization in a simultaneous localization and mapping (SLAM) system
CN101509781A (zh) * 2009-03-20 2009-08-19 同济大学 基于单目摄像头的步行机器人定位系统
US20160327653A1 (en) * 2014-02-03 2016-11-10 Board Of Regents, The University Of Texas System System and method for fusion of camera and global navigation satellite system (gnss) carrier-phase measurements for globally-referenced mobile device pose determination
CN110954113A (zh) * 2019-05-30 2020-04-03 北京初速度科技有限公司 一种车辆位姿的修正方法和装置
CN111220164A (zh) * 2020-01-21 2020-06-02 北京百度网讯科技有限公司 定位方法、装置、设备及存储介质
CN111220154A (zh) * 2020-01-22 2020-06-02 北京百度网讯科技有限公司 车辆定位方法、装置、设备和介质

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9727793B2 (en) * 2015-12-15 2017-08-08 Honda Motor Co., Ltd. System and method for image based vehicle localization
CN109061703B (zh) * 2018-06-11 2021-12-28 阿波罗智能技术(北京)有限公司 用于定位的方法、装置、设备和计算机可读存储介质

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20050182518A1 (en) * 2004-02-13 2005-08-18 Evolution Robotics, Inc. Robust sensor fusion for mapping and localization in a simultaneous localization and mapping (SLAM) system
CN101509781A (zh) * 2009-03-20 2009-08-19 同济大学 基于单目摄像头的步行机器人定位系统
US20160327653A1 (en) * 2014-02-03 2016-11-10 Board Of Regents, The University Of Texas System System and method for fusion of camera and global navigation satellite system (gnss) carrier-phase measurements for globally-referenced mobile device pose determination
CN110954113A (zh) * 2019-05-30 2020-04-03 北京初速度科技有限公司 一种车辆位姿的修正方法和装置
CN111220164A (zh) * 2020-01-21 2020-06-02 北京百度网讯科技有限公司 定位方法、装置、设备及存储介质
CN111220154A (zh) * 2020-01-22 2020-06-02 北京百度网讯科技有限公司 车辆定位方法、装置、设备和介质

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2024041447A1 (zh) * 2022-08-24 2024-02-29 上海安亭地平线智能交通技术有限公司 位姿的确定方法、装置、电子设备和存储介质

Also Published As

Publication number Publication date
EP4209755A1 (en) 2023-07-12
US20230222688A1 (en) 2023-07-13
CN114248778B (zh) 2024-04-12
WO2022062480A1 (zh) 2022-03-31
EP4209755A4 (en) 2024-02-28

Similar Documents

Publication Publication Date Title
EP3732657B1 (en) Vehicle localization
JP7210165B2 (ja) 仮想経路を表示する方法、装置及びディスプレイ装置
US11113544B2 (en) Method and apparatus providing information for driving vehicle
KR102483649B1 (ko) 차량 위치 결정 방법 및 차량 위치 결정 장치
US10604156B2 (en) System and method for adjusting a road boundary
CN107328410B (zh) 用于定位自动驾驶车辆的方法和汽车电脑
US9495602B2 (en) Image and map-based detection of vehicles at intersections
US11670087B2 (en) Training data generating method for image processing, image processing method, and devices thereof
KR20200044420A (ko) 위치 추정 방법 및 장치
CN114248778B (zh) 移动设备的定位方法和定位装置
KR20200023865A (ko) 차선 결정 방법 및 장치
CN111351502B (zh) 用于从透视图生成环境的俯视图的方法，装置和计算机程序产品
JP6950832B2 (ja) 位置座標推定装置、位置座標推定方法およびプログラム
JP6520740B2 (ja) 物体検出方法、物体検出装置、およびプログラム
US20200125860A1 (en) Method and apparatus for determining driving information
Shunsuke et al. GNSS/INS/on-board camera integration for vehicle self-localization in urban canyon
CN110136058A (zh) 一种基于俯视拼接图的建图方法及车载终端
CN114636414A (zh) 高清晰度城市地图绘制
KR20210034253A (ko) 위치 추정 장치 및 방법
Badino et al. Stereo-based free space computation in complex traffic scenarios
CN112528719A (zh) 推定装置、推定方法以及存储介质
CN116762094A (zh) 数据处理方法和设备
Choi et al. Point Cloud-Based Lane Detection for Optimal Local Path Planning
US11461944B2 (en) Region clipping method and recording medium storing region clipping program
US20240119740A1 (en) Method and apparatus with lane line determination

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