CN113239072A - 一种终端设备定位方法及其相关设备 - Google Patents

一种终端设备定位方法及其相关设备 Download PDF

Info

Publication number
CN113239072A
CN113239072A CN202110460636.4A CN202110460636A CN113239072A CN 113239072 A CN113239072 A CN 113239072A CN 202110460636 A CN202110460636 A CN 202110460636A CN 113239072 A CN113239072 A CN 113239072A
Authority
CN
China
Prior art keywords
pose
image frame
map
current image
point
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
CN202110460636.4A
Other languages
English (en)
Other versions
CN113239072B (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
Institute of Automation of Chinese Academy of Science
Original Assignee
Huawei Technologies Co Ltd
Institute of Automation of Chinese Academy of Science
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, Institute of Automation of Chinese Academy of Science filed Critical Huawei Technologies Co Ltd
Priority to CN202110460636.4A priority Critical patent/CN113239072B/zh
Publication of CN113239072A publication Critical patent/CN113239072A/zh
Priority to PCT/CN2022/089007 priority patent/WO2022228391A1/zh
Priority to EP22794861.9A priority patent/EP4322020A4/en
Priority to US18/494,547 priority patent/US20240062415A1/en
Application granted granted Critical
Publication of CN113239072B publication Critical patent/CN113239072B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
    • G06F16/20—Information retrieval; Database structures therefor; File system structures therefor of structured data, e.g. relational data
    • G06F16/24—Querying
    • G06F16/245—Query processing
    • 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
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
    • G06F16/20—Information retrieval; Database structures therefor; File system structures therefor of structured data, e.g. relational data
    • G06F16/29—Geographical information databases
    • 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
    • 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
    • G06T7/74—Determining position or orientation of objects or cameras using feature-based methods involving reference images or patches
    • 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/40—Extraction of image or video features
    • G06V10/44—Local feature extraction by analysis of parts of the pattern, e.g. by detecting edges, contours, loops, corners, strokes or intersections; Connectivity analysis, e.g. of connected components
    • G06V10/443—Local feature extraction by analysis of parts of the pattern, e.g. by detecting edges, contours, loops, corners, strokes or intersections; Connectivity analysis, e.g. of connected components by matching or filtering
    • 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)
  • Theoretical Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • Databases & Information Systems (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Data Mining & Analysis (AREA)
  • General Engineering & Computer Science (AREA)
  • Multimedia (AREA)
  • Automation & Control Theory (AREA)
  • Computational Linguistics (AREA)
  • Image Analysis (AREA)

Abstract

本申请提供一种终端设备定位方法及其相关设备，可提高终端设备的定位结果的准确性。本申请的方法包括：从矢量地图中，获取与终端设备拍摄的当前图像帧的第一特征点匹配的第一地图点；从矢量地图中，获取与当前图像帧之前的其它图像帧的第二特征点匹配的第二地图点；根据目标函数对终端设备拍摄当前图像帧时的位姿进行调整，得到当前次调整后的终端设备拍摄当前图像帧时的位姿，作为终端设备的定位结果，目标函数包括第一特征点与第一地图点之间的第一匹配误差，以及第二特征点与第二地图点之间的第二匹配误差。

Description

一种终端设备定位方法及其相关设备
技术领域
本申请涉及人工智能技术领域，尤其涉及一种终端设备定位方法及其相关设备。
背景技术
目前，自动驾驶汽车、无人机和机器人等智能终端设备已广泛应用于日常生活中。对于这部分可移动的终端设备而言，为了准确获取终端设备的实时位置，高精定位技术应运而生。
在对终端设备进行定位的过程中，可获取终端设备拍摄的当前图像帧，并从预置的矢量地图(该地图中，交通环境中的物体可通过地图点进行表示，例如，灯杆通过地图点汇聚成的直线进行表示，指示牌通过地图点汇聚成的矩形框进行表示等等)中获取与当前图像帧中用于呈现交通环境中的物体的特征点匹配的地图点，最后根据特征点与地图点之间的匹配结果确定终端设备在矢量地图中的定位结果。
然而，上述终端设备的定位过程所考虑的因素较为单一，导致终端设备的定位结果的准确性较低。
发明内容
本申请实施例提供了一种终端设备定位方法及其相关设备，可提高终端设备的定位结果的准确性。
本申请实施例的第一方面提供了一种终端设备定位方法，该方法包括：
终端设备在移动过程中，在当前时刻可通过摄像头对交通环境进行拍摄，得到当前图像帧。进一步地，终端设备还可获取当前图像帧之前的其它图像帧。那么，终端设备可根据当前图像帧以及其它图像帧，对自身进行定位。
具体地，终端设备先从矢量地图中，获取与当前图像帧的第一特征点匹配的第一地图点。例如，当前图像帧中用于呈现红绿灯的特征点以及矢量地图中用于表示红绿灯的地图点为相匹配的点，当前图像帧中用于呈现车道线的特征点以及矢量地图中用于表示车道线的地图点为相匹配的点等等。同样地，终端设备还可从矢量地图中，获取与当前图像帧之前的其它图像帧的第二特征点匹配的第二地图点。
由于第一特征点和第一地图点之间存在一定的匹配误差，第二特征点和第二地图点之间也存在一定的匹配误差，因此，需要令这两个匹配误差尽可能地小，才能提高终端设备的定位结果的准确性。
基于此，终端设备可根据第一特征点与第一地图点之间的第一匹配误差，以及第二特征点与第二地图点之间的第二匹配误差构建目标函数，并根据目标函数对终端设备拍摄当前图像帧时的位姿进行调整，即根据目标函数对终端设备拍摄当前图像帧时的位姿进行优化，直至目标函数收敛，从而得到当前次调整(优化)后的终端设备拍摄当前图像帧时的位姿，将其作为终端设备在矢量地图中的定位结果。其中，终端设备拍摄当前图像帧时的位姿通常指在拍摄当前图像帧时，终端设备在矢量地图对应的三维坐标系中所处的位姿。
从上述方法可以看出：在得到当前图像帧和当前图像帧之前的其它图像帧后，可从矢量地图中，获取与当前图像帧的第一特征点匹配的第一地图点，以及与当前图像帧之前的其它图像帧的第二特征点匹配的第二地图点。然后，可根据第一特征点与第一地图点之间的第一匹配误差，以及第二特征点与第二地图点之间的第二匹配误差构建的目标函数，对终端设备拍摄当前图像帧时的位姿进行调整，得到当前次调整后的终端设备拍摄当前图像帧时的位姿。前述过程中，由于目标函数既包含当前图像帧的特征点与矢量地图的地图点之间的匹配误差，以及其它图像帧的特征点与矢量地图的地图点之间的匹配误差，故通过该目标函数对终端设备拍摄当前图像帧时的位姿进行调整，不仅考虑了当前图像帧对终端设备拍摄当前图像帧时的位姿的优化过程造成的影响，还考虑了其它图像帧对终端设备拍摄当前图像帧时的位姿的优化过程造成的影响，即考虑了当前图像帧以及其它图像帧之间的关联性，所考虑的因素更全面，故基于此种方式得到的终端设备的定位结果，具备更高的准确性。
在一种可能的实现方式中，该方法还包括：获取终端设备拍摄当前图像帧时的位姿和上一次调整后的终端设备拍摄其它图像帧时的位姿，并对当前图像帧以及当前图像帧之前的其它图像帧进行语义检测，从而得到当前图像帧的第一特征点和当前图像帧之前的其它图像帧的第二特征点。然后，可根据终端设备拍摄当前图像帧时的位姿，从矢量地图中获取与第一特征点匹配的第一地图点，并根据上一次调整后的终端设备拍摄其它图像帧时的位姿，从矢量地图中获取与第二特征点匹配的第二地图点。如此一来，可完成特征点与地图点之间的关联匹配。
在一种可能的实现方式中，根据目标函数对终端设备拍摄当前图像帧时的位姿进行调整，得到当前次调整后的终端设备拍摄当前图像帧时的位姿包括：在获取第一特征点在第一坐标系中的位置以及第一地图点在第一坐标系中的位置后，根据第一特征点在第一坐标系中的位置与第一地图点在第一坐标系中的位置之间的距离进行计算，可得到第一匹配误差的初始值。接着，在获取第二特征点在第二坐标系中的位置以及第二地图点在第二坐标系中的位置后，根据第二特征点在第二坐标系中的位置与第二地图点在第二坐标系中的位置之间的距离进行计算，可得到第二匹配误差的初始值。最后，根据第一匹配误差的初始值以及第二匹配误差的初始值，对目标函数进行迭代求解，直至满足预置的迭代条件，得到当前次调整后的终端设备拍摄当前图像帧时的位姿。前述实现方式中，在完成第一特征点与第一地图点之间的匹配，第二特征点与第二地图点之间的匹配后，可计算出第一特征点与第一地图点之间的第一匹配误差的初始值，以及第二特征点与第二地图点之间的第二匹配误差的初始值，从而结合这两个初始值对对目标函数进行迭代求解，相当于根据当前图像帧和其它图像帧对终端设备拍摄当前图像帧时的位姿进行调整，所考虑的因素更全面，从而准确地得到终端设备的定位结果。
在一种可能的实现方式中，第一特征点在第一坐标系中的位置与第一地图点在第一坐标系中的位置之间的距离包括以下至少一种：(1)第一特征点在当前图像帧中的位置，与第一地图点在当前图像帧中的位置之间的距离，其中，第一地图点在当前图像帧中的位置根据第一地图点在矢量地图对应的三维坐标系中的位置以及终端设备拍摄当前图像帧时的位姿得到。(2)第一特征点在矢量地图对应的三维坐标系中的位置，与第一地图点在矢量地图对应的三维坐标系中的位置之间的距离，其中，第一特征点在矢量地图对应的三维坐标系中的位置根据第一特征点在当前图像帧中的位置以及终端设备拍摄当前图像帧时的位姿得到。(3)第一特征点在终端设备对应的三维坐标系中的位置，与第一地图点在终端设备对应的三维坐标系中的位置之间的距离，其中，第一特征点在终端设备对应的三维坐标系中的位置根据第一特征点在当前图像帧中的位置以及终端设备拍摄当前图像帧时的位姿得到，第一地图点在终端设备对应的三维坐标系中的位置根据第一地图点在矢量地图对应的三维坐标系中的位置以及终端设备拍摄当前图像帧时的位姿得到。
在一种可能的实现方式中，第二特征点在第二坐标系中的位置与第二地图点在第二坐标系中的位置之间的距离包括以下至少一种：(1)第二特征点在其它图像帧中的位置，与第二地图点在其它图像帧中的位置之间的距离，其中，第二地图点在其它图像帧中的位置根据第二地图点在矢量地图对应的三维坐标系中的位置以及上一次调整后的终端设备拍摄其它图像帧时的位姿得到。(2)第二特征点在矢量地图对应的三维坐标系中的位置，与第二地图点在矢量地图对应的三维坐标系中的位置之间的距离，其中，第二特征点在矢量地图对应的三维坐标系中的位置根据第二特征点在其它图像帧中的位置以及上一次调整后的终端设备拍摄其它图像帧时的位姿得到。(3)第二特征点在终端设备对应的三维坐标系中的位置，与第二地图点在终端设备对应的三维坐标系中的位置之间的距离，其中，第二特征点在终端设备对应的三维坐标系中的位置根据第二特征点在其它图像帧中的位置以及上一次调整后的终端设备拍摄其它图像帧时的位姿得到，第二地图点在终端设备对应的三维坐标系中的位置根据第二地图点在矢量地图对应的三维坐标系中的位置以及上一次调整后的终端设备拍摄其它图像帧时的位姿得到。
在一种可能的实现方式中，迭代条件为：对于任意一次迭代，若该次迭代所得到的帧间位姿差与终端设备计算的帧间位姿差之间的差值小于预置的阈值，则停止迭代，该次迭代得到的帧间位姿差根据该次迭代得到的终端设备拍摄当前图像帧时的位姿以及该次迭代得到的终端设备拍摄其它图像帧时的位姿确定，帧间位姿差为在当前图像帧和其它图像帧中，终端设备拍摄相邻两个图像帧之间的位姿差；若差值大于或等于阈值，则执行下一次迭代，直至迭代次数等于预置的次数。
在一种可能的实现方式中，其它图像帧的数量可随着终端设备的运动状态的变化而变化。具体地，其它图像帧的数量可根据终端设备的速度确定。
在一种可能的实现方式中，获取终端设备拍摄当前图像帧时的位姿包括：根据上一次调整后的终端设备拍摄其它图像帧时的位姿和终端设备计算的帧间位姿差，计算终端设备拍摄当前图像帧时的预测位姿；对终端设备拍摄当前图像帧时的预测位姿进行分层采样，得到终端设备拍摄当前图像帧时的位姿。前述实现方式中，通过分层采样所得到的终端设备拍摄当前图像帧时的位姿，可作为当前次调整的初始位姿，从而提高当前次调整的收敛速度和鲁棒性。
在一种可能的实现方式中，若终端设备拍摄当前图像帧时的位姿包含横坐标、纵坐标和航向角，则对终端设备拍摄当前图像帧时的预测位姿进行分层采样，得到终端设备拍摄当前图像帧时的位姿包括：获取第三地图点在矢量地图对应的三维坐标系中的位置和第一特征点在当前图像帧中的位置；保持终端设备拍摄当前图像帧时的预测位姿的航向角不变，改变终端设备拍摄当前图像帧时的预测位姿的横坐标和纵坐标，得到第一候选位姿；根据第一候选位姿对第三地图点在矢量地图对应的三维坐标系中的位置进行变换，得到第三地图点在预置的图像坐标系中的位置；保持终端设备拍摄当前图像帧时的预测位姿的横坐标和纵坐标不变，改变终端设备拍摄当前图像帧时的预测位姿的航向角，得到第二候选位姿；根据第二候选位姿对第一特征点在当前图像帧中的位置进行变换，得到第一特征点在图像坐标系中的位置；根据第三地图点在图像坐标系中的位置和第一特征点在图像坐标系中的位置之间的距离的大小，从第一候选位姿和第二候选位姿的组合中确定终端设备拍摄当前图像帧时的位姿。通过前述的位姿采样方式，可有效减少位姿采样过程中所需的计算量。
在一种可能的实现方式中，若终端设备拍摄当前图像帧时的位姿包含横坐标、纵坐标、竖坐标、航向角、滚动角和俯仰角，则对终端设备拍摄当前图像帧时的预测位姿进行分层采样，得到终端设备拍摄当前图像帧时的位姿包括：获取第三地图点在矢量地图对应的三维坐标系中的位置和第一特征点在当前图像帧中的位置；保持终端设备拍摄当前图像帧时的预测位姿的航向角、滚动角、俯仰角和竖坐标不变，改变终端设备拍摄当前图像帧时的预测位姿的横坐标和纵坐标，得到第一候选位姿；根据第一候选位姿对第三地图点在矢量地图对应的三维坐标系中的位置进行变换，得到第三地图点在预置的图像坐标系中的位置；保持终端设备拍摄当前图像帧时的预测位姿的横坐标、纵坐标、竖坐标、滚动角和俯仰角不变，改变终端设备拍摄当前图像帧时的预测位姿的航向角，得到第二候选位姿；根据第二候选位姿对第一特征点在当前图像帧中的位置进行变换，得到第一特征点在图像坐标系中的位置；根据第三地图点在图像坐标系中的位置和第一特征点在图像坐标系中的位置之间的距离的大小，从第一候选位姿和第二候选位姿的组合中确定第三候选位姿；保持第三候选位姿的预测位姿的横坐标、纵坐标、航向角和滚动角不变，改变第三候选位姿的俯仰角和竖坐标，得到第四候选位姿；根据第四候选位姿对第三地图点在矢量地图对应的三维坐标系中的位置进行变换，得到第三地图点在当前图像帧中的位置；根据第一特征点在当前图像帧中的位置和第三地图点在当前图像帧中的位置之间的距离的大小，从第四候选位姿确定终端设备拍摄当前图像帧时的位姿。通过前述的位姿采样方式，可有效减少位姿采样过程中所需的计算量。
本申请实施例的第二方面提供了一种终端设备定位装置，该装置包括：第一匹配模块，用于从矢量地图中，获取与当前图像帧的第一特征点匹配的第一地图点；第二匹配模块，用于从矢量地图中，获取与当前图像帧之前的其它图像帧的第二特征点匹配的第二地图点；优化模块，用于根据目标函数对终端设备拍摄当前图像帧时的位姿进行调整，得到当前次调整后的终端设备拍摄当前图像帧时的位姿，作为终端设备的定位结果，目标函数包括第一特征点与第一地图点之间的第一匹配误差，以及第二特征点与第二地图点之间的第二匹配误差。
从上述装置可以看出：在得到当前图像帧和当前图像帧之前的其它图像帧后，可从矢量地图中，获取与当前图像帧的第一特征点匹配的第一地图点，以及与当前图像帧之前的其它图像帧的第二特征点匹配的第二地图点。然后，可根据第一特征点与第一地图点之间的第一匹配误差，以及第二特征点与第二地图点之间的第二匹配误差构建的目标函数，对终端设备拍摄当前图像帧时的位姿进行调整，得到当前次调整后的终端设备拍摄当前图像帧时的位姿。前述过程中，由于目标函数既包含当前图像帧的特征点与矢量地图的地图点之间的匹配误差，以及其它图像帧的特征点与矢量地图的地图点之间的匹配误差，故通过该目标函数对终端设备拍摄当前图像帧时的位姿进行调整，不仅考虑了当前图像帧对终端设备拍摄当前图像帧时的位姿的优化过程造成的影响，还考虑了其它图像帧对终端设备拍摄当前图像帧时的位姿的优化过程造成的影响，即考虑了当前图像帧以及其它图像帧之间的关联性，所考虑的因素更全面，故基于此种方式得到的终端设备的定位结果，具备更高的准确性。
在一种可能的实现方式中，该装置还包括：获取模块，用于获取当前图像帧的第一特征点、当前图像帧之前的其它图像帧的第二特征点、终端设备拍摄当前图像帧时的位姿和上一次调整后的终端设备拍摄其它图像帧时的位姿；第一匹配模块，用于根据终端设备拍摄当前图像帧时的位姿，从矢量地图中，获取与第一特征点匹配的第一地图点；第二匹配模块，用于根据上一次调整后的终端设备拍摄其它图像帧时的位姿，从矢量地图中，获取与第二特征点匹配的第二地图点。
在一种可能的实现方式中，优化模块，用于：根据第一特征点在第一坐标系中的位置与第一地图点在第一坐标系中的位置之间的距离进行计算，得到第一匹配误差的初始值；根据第二特征点在第二坐标系中的位置与第二地图点在第二坐标系中的位置之间的距离进行计算，得到第二匹配误差的初始值；根据第一匹配误差的初始值以及第二匹配误差的初始值，对目标函数进行迭代求解，直至满足预置的迭代条件，得到当前次调整后的终端设备拍摄当前图像帧时的位姿。
在一种可能的实现方式中，第一特征点在第一坐标系中的位置与第一地图点在第一坐标系中的位置之间的距离包括以下至少一种：第一特征点在当前图像帧中的位置，与第一地图点在当前图像帧中的位置之间的距离；第一特征点在矢量地图对应的三维坐标系中的位置，与第一地图点在矢量地图对应的三维坐标系中的位置之间的距离；第一特征点在终端设备对应的三维坐标系中的位置，与第一地图点在终端设备对应的三维坐标系中的位置之间的距离。
在一种可能的实现方式中，第二特征点在第二坐标系中的位置与第二地图点在第二坐标系中的位置之间的距离包括以下至少一种：第二特征点在其它图像帧中的位置，与第二地图点在其它图像帧中的位置之间的距离；第二特征点在矢量地图对应的三维坐标系中的位置，与第二地图点在矢量地图对应的三维坐标系中的位置之间的距离；第二特征点在终端设备对应的三维坐标系中的位置，与第二地图点在终端设备对应的三维坐标系中的位置之间的距离。
在一种可能的实现方式中，迭代条件为：对于任意一次迭代，若该次迭代所得到的帧间位姿差与终端设备计算的帧间位姿差之间的差值小于预置的阈值，则停止迭代，该次迭代得到的帧间位姿差根据该次迭代得到的终端设备拍摄当前图像帧时的位姿以及该次迭代得到的终端设备拍摄其它图像帧时的位姿确定，帧间位姿差为在当前图像帧和其它图像帧中，终端设备拍摄相邻两个图像帧之间的位姿差；若差值大于或等于阈值，则执行下一次迭代，直至迭代次数等于预置的次数。
在一种可能的实现方式中，其它图像帧的数量根据终端设备的速度确定。
在一种可能的实现方式中，获取模块，用于：根据上一次调整后的终端设备拍摄其它图像帧时的位姿和终端设备计算的帧间位姿差，计算终端设备拍摄当前图像帧时的预测位姿；对终端设备拍摄当前图像帧时的预测位姿进行分层采样，得到终端设备拍摄当前图像帧时的位姿。
在一种可能的实现方式中，若终端设备拍摄当前图像帧时的位姿包含横坐标、纵坐标和航向角，则获取模块，用于：获取第三地图点在矢量地图对应的三维坐标系中的位置和第一特征点在当前图像帧中的位置；保持终端设备拍摄当前图像帧时的预测位姿的航向角不变，改变终端设备拍摄当前图像帧时的预测位姿的横坐标和纵坐标，得到第一候选位姿；根据第一候选位姿对第三地图点在矢量地图对应的三维坐标系中的位置进行变换，得到第三地图点在预置的图像坐标系中的位置；保持终端设备拍摄当前图像帧时的预测位姿的横坐标和纵坐标不变，改变终端设备拍摄当前图像帧时的预测位姿的航向角，得到第二候选位姿；根据第二候选位姿对第一特征点在当前图像帧中的位置进行变换，得到第一特征点在图像坐标系中的位置；根据第三地图点在图像坐标系中的位置和第一特征点在图像坐标系中的位置之间的距离的大小，从第一候选位姿和第二候选位姿的组合中确定终端设备拍摄当前图像帧时的位姿。通过前述的位姿采样方式，可有效减少位姿采样过程中所需的计算量。
在一种可能的实现方式中，若终端设备拍摄当前图像帧时的位姿包含横坐标、纵坐标、竖坐标、航向角、滚动角和俯仰角，则获取模块，用于：获取第三地图点在矢量地图对应的三维坐标系中的位置和第一特征点在当前图像帧中的位置；保持终端设备拍摄当前图像帧时的预测位姿的航向角、滚动角、俯仰角和竖坐标不变，改变终端设备拍摄当前图像帧时的预测位姿的横坐标和纵坐标，得到第一候选位姿；根据第一候选位姿对第三地图点在矢量地图对应的三维坐标系中的位置进行变换，得到第三地图点在预置的图像坐标系中的位置；保持终端设备拍摄当前图像帧时的预测位姿的横坐标、纵坐标、竖坐标、滚动角和俯仰角不变，改变终端设备拍摄当前图像帧时的预测位姿的航向角，得到第二候选位姿；根据第二候选位姿对第一特征点在当前图像帧中的位置进行变换，得到第一特征点在图像坐标系中的位置；根据第三地图点在图像坐标系中的位置和第一特征点在图像坐标系中的位置之间的距离的大小，从第一候选位姿和第二候选位姿的组合中确定第三候选位姿；保持第三候选位姿的预测位姿的横坐标、纵坐标、航向角和滚动角不变，改变第三候选位姿的俯仰角和竖坐标，得到第四候选位姿；根据第四候选位姿对第三地图点在矢量地图对应的三维坐标系中的位置进行变换，得到第三地图点在当前图像帧中的位置；根据第一特征点在当前图像帧中的位置和第三地图点在当前图像帧中的位置之间的距离的大小，从第四候选位姿确定终端设备拍摄当前图像帧时的位姿。通过前述的位姿采样方式，可有效减少位姿采样过程中所需的计算量。
本申请实施例的第三方面提供了一种终端设备定位装置，该装置包括存储器和处理器；存储器存储有代码，处理器被配置为执行代码，当代码被执行时，终端设备定位装置执行如第一方面或第一方面的任意一种可能的实现方式所述的方法。
本申请实施例的第四方面提供了一种车辆，该车辆包含如第三方面所述的终端设备定位装置。
本申请实施例的第五方面提供了一种计算机存储介质，计算机存储介质存储有计算机程序，该程序由计算机执行时，使得计算机实施如第一方面或第一方面的任意一种可能的实现方式所述的方法。
本申请实施例的第六方面提供了一种计算机程序产品，计算机程序产品存储有指令，指令在由计算机执行时，使得计算机实施如第一方面或第一方面的任意一种可能的实现方式所述的方法。
本申请实施例中，在得到当前图像帧和当前图像帧之前的其它图像帧后，可从矢量地图中，获取与当前图像帧的第一特征点匹配的第一地图点，以及与当前图像帧之前的其它图像帧的第二特征点匹配的第二地图点。然后，可根据第一特征点与第一地图点之间的第一匹配误差，以及第二特征点与第二地图点之间的第二匹配误差构建的目标函数，对终端设备拍摄当前图像帧时的位姿进行调整，得到当前次调整后的终端设备拍摄当前图像帧时的位姿。前述过程中，由于目标函数既包含当前图像帧的特征点与矢量地图的地图点之间的匹配误差，以及其它图像帧的特征点与矢量地图的地图点之间的匹配误差，故通过该目标函数对终端设备拍摄当前图像帧时的位姿进行调整，不仅考虑了当前图像帧对终端设备拍摄当前图像帧时的位姿的优化过程造成的影响，还考虑了其它图像帧对终端设备拍摄当前图像帧时的位姿的优化过程造成的影响(即考虑了当前图像帧以及其它图像帧之间的关联性)，所考虑的因素更全面，故基于此种方式得到的终端设备的定位结果，具备更高的准确性。
附图说明
图1为矢量地图的一个示意图；
图2为本申请实施例提供的终端设备的定位方法的一个流程示意图；
图3为本申请实施例提供的终端设备对应的三维坐标系的一个示意图；
图4为本申请实施例提供的帧间位姿差的一个示意图；
图5为本申请实施例提供的当前图像帧的第一特征点的一个示意图；
图6为本申请实施例提供的计算重叠程度的一个示意图；
图7为本申请实施例提供的终端设备定位装置的一个结构示意图；
图8为本申请实施例提供的终端设备定位装置的另一结构示意图。
具体实施方式
本申请实施例提供了一种终端设备定位方法及其相关设备，可提高终端设备的定位结果的准确性。
本申请的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的术语在适当情况下可以互换，这仅仅是描述本申请的实施例中对相同属性的对象在描述时所采用的区分方式。此外，术语“包括”和“具有”并他们的任何变形，意图在于覆盖不排他的包含，以便包含一系列单元的过程、方法、系统、产品或设备不必限于那些单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它单元。
本申请实施例可通过终端设备实现，例如，汽车上的车载设备、无人机、机器人等等。为了便于说明，下文将汽车上的车载设备简称为汽车，并以行驶中的汽车为例进行介绍。
当汽车在行驶时，若用户欲确定汽车所在的位置，则需对汽车进行高精定位。在相关技术中，汽车内部通常预置有完整的矢量地图，图1为矢量地图的一个示意图，如图1所示，矢量地图可显示出汽车当前所处的虚拟交通环境，该交通环境包含汽车周围的各个物体，例如，红绿灯、灯杆、指示牌、车道线等等。这些物体可通过矢量地图上的像素点进行表示，即通过矢量地图中的地图点进行表示。例如，灯杆可以通过多个地图点汇集而成的直线表示，指示牌可以通过多个地图点汇集而成的矩形框表示等等。值得注意的是，矢量地图所显示的虚拟交通环境是根据真实世界中的交通环境进行绘制的，而矢量地图所显示的汽车的位姿一般是汽车通过计算得到的，可能与真实世界中汽车的真实位姿存在差异，因此，需要对矢量地图中汽车的位姿进行修正和优化，从而提高汽车的定位结果的准确性。可以理解的是，汽车的位姿通常包含汽车的位置和汽车的朝向，后续不再赘述。
具体地，在行驶过程中，汽车可拍摄当前图像帧，用于呈现当前时刻汽车所处的真实交通环境。接着，汽车可将当前图像帧的特征点与矢量地图的地图点进行匹配，相当于将汽车所处的真实交通环境和汽车所处的虚拟交通环境进行匹配。最后，根据当前图像帧的特征点与矢量地图的地图点之间的匹配结果，例如，二者之间的匹配误差等等，对矢量地图中汽车的位姿进行调整，并以优化后的汽车的位姿作为汽车的定位结果。
然而，仅通过当前图像帧确定汽车的定位结果，所考虑的因素较为单一，导致汽车的定位结果的准确性较低。
基于此，为了提高终端设备的定位结果的准确性，本申请实施例提供了一种终端设备的定位方法。为了便于说明，下文将终端设备拍摄任意一个图像帧时的位姿，简称为该图像帧的位姿，例如，终端设备拍摄当前图像帧时的位姿，可简称为当前图像帧的位姿，又如，终端设备拍摄位于当前图像帧之前的其它图像帧时的位姿，可简称为其它图像帧的位姿等等，再如，对终端设备拍摄当前图像帧时的位姿进行当前次优化(调整)后，可得到当前次优化(调整)后的终端设备拍摄当前图像帧时的位姿，即当前次优化后的当前次图像帧的位姿等等，后续不再赘述。图2为本申请实施例提供的终端设备的定位方法的一个流程示意图，如图2所示，该方法包括：
201、获取当前图像帧的第一特征点、当前图像帧之前的其它图像帧的第二特征点、当前图像帧的位姿和上一次优化后的其它图像帧的位姿。
本实施例中，终端设备具有摄像头。在移动过程中，终端设备可通过摄像头对当前的交通环境进行拍摄，得到当前图像帧。进一步地，终端设备还可获取当前图像帧之前的其它图像帧，其它图像帧的数量可根据终端设备的速度确定，如公式(1)所示：
Figure BDA0003042103930000081
上式中，t为当前图像帧和其它图像帧的数量，t-1为其它图像帧的数量，t0为预设的阈值，α为预设的调整系数，v为当前时刻的终端设备的速度。如此一来，得到当前图像帧以及其它图像帧后，终端设备可根据当前图像帧以及其它图像帧，对自身进行定位。
终端设备得到当前图像帧和其它图像帧后，可获取当前图像帧的位姿以及上一次优化后的其它图像帧的位姿。需要说明的是，对于当前图像帧的位姿而言，可根据当前图像帧以及其它图像帧，对当前图像帧的位姿进行当前次的优化，以得到当前次优化后的当前图像帧的位姿以及当前次优化后的其它图像帧的位姿。由此可见，上一次优化后的其它图像帧的位姿为：根据其它图像帧对其它图像帧的位姿进行上一次的优化后，所得到的结果。
当前图像帧的位姿可通过如下方式获取：首先，根据上一次优化后的其它图像帧的位姿和终端设备计算的帧间位姿差，计算当前图像帧的预测位姿。然后，对当前图像帧的预测位姿进行分层采样，得到当前图像帧的位姿。
具体地，终端设备还可具有里程计，里程计可构建终端设备所对应的三维坐标系，例如，车体坐标系等等。图3为本申请实施例提供的终端设备对应的三维坐标系的一个示意图，如图3所示，在该三维坐标系中，原点为终端设备的运动起始点，X轴指向处于运动起始点时的终端设备的正前方，Y轴指向处于运动起始点时的终端设备的左侧，Z轴可默认为零。那么，当终端设备从原点处开始运动后，其位置和朝向在不断地发生变化(即发生旋转和平移)，里程计可在终端设备的运动过程中，可计算终端设备在拍摄相邻两个图像帧时所对应的位姿差，该位姿差即相邻两个图像帧之间的位姿差，也可称为帧间位姿差。帧间位姿差可通过公式(2)表示：ΔT＝{ΔR，Δt} (2)
上式中，ΔT为帧间位姿差，ΔR为相邻两个图像帧之间的旋转，Δt为相邻两个图像帧之间的平移。
为了进一步理解帧间位姿差，下文结合图4对其做进一步的介绍。图4为本申请实施例提供的帧间位姿差的一个示意图，如图4所示，设当前图像帧和当前图像帧之前的其它图像帧一共t帧。其中，F1表示其它图像帧中的第一个图像帧，F2表示其它图像帧中的第二个图像帧，…,Ft-1表示其它图像帧中的最后一个图像帧(即当前图像帧的前一个图像帧)，Ft表示当前图像帧。里程计可计算得到F1和F2之间的位姿差为ΔTt-1,…,得到Ft-1和Ft之间的位姿差为ΔTt-1。那么，当前图像帧的预测位姿可通过公式(3)计算得到：
Pt＝Pt-1*ΔTt-1 (3)
上式中，Pt为当前图像帧的预测位姿，Pt-1为上一次优化后的前一个图像帧的位姿。基于公式(3)，当前图像帧的预测位姿也可通过公式(4)计算得到：
Pt＝(ΔTt-0*ΔTt-2*…*ΔTt-m)*Pt-m (4)
上式中，Pt-m为上一次优化后的其它图像帧中的第t-m个图像帧的位姿。
得到当前图像帧的预测位姿后，可对预测位姿进行分层采样，从而得到当前图像帧的位姿，即用于进行当前次优化的位姿初始值。具体地，可通过多种方式对当前图像帧的预测位姿进行分层采样，下文将分别进行介绍：
在一种可能的实现方式中，若当前图像帧的位姿为三自由度的量，即包含横坐标、纵坐标和航向角，分层采样的过程包括：(一)可在矢量地图中提前划定的范围内任意选取部分地图点，作为第三地图点，并获取第三地图点在矢量地图对应的三维坐标系中的位置和第一特征点在当前图像帧中的位置。可以理解的是，第三地图点在矢量地图对应的三维坐标系中的位置为三维坐标，第一特征点在当前图像帧中的位置为二维坐标。(二)保持当前图像帧的预测位姿的航向角不变，改变当前图像帧的预测位姿的横坐标和纵坐标，得到第一候选位姿。(三)根据第一候选位姿对第三地图点在矢量地图对应的三维坐标系中的位置进行变换，得到第三地图点在预置的图像坐标系中的位置，该过程相当于将第三地图点投影至图像坐标系中。(四)保持当前图像帧的预测位姿的横坐标和纵坐标不变，改变当前图像帧的预测位姿的航向角，得到第二候选位姿。(五)根据第二候选位姿对第一特征点在当前图像帧中的位置进行变换，得到第一特征点在图像坐标系中的位置，该过程相当于将第一特征点投影至图像坐标系中。(六)根据第三地图点在图像坐标系中的位置和第一特征点在图像坐标系中的位置之间的距离的大小，从第一候选位姿和第二候选位姿的组合中确定当前图像帧的位姿。通过前述的位姿采样方式，可有效减少位姿采样过程中所需的计算量。
为了进一步理解上述采样过程，下文结合一个例子进行介绍，该例子包括：(一)确定当前图像帧的第一特征点，以及在矢量地图中用于分层采样的第三地图点。(二)保持当前图像帧的航向角不变，在横坐标的原始值的基础上采样N1次，并在纵坐标的原始值的基础上采样N2次，得到N1×N2个第一候选位姿。(三)根据每一个第一候选位姿，将矢量地图中的第三地图点投影至预置的图像坐标系，得到N1×N2组新的第三地图点。(四)保持当前图像帧的预测位姿的横坐标和纵坐标不变，在航向角的原始值的基础上采样N3次，得到N3个第二候选位姿。(五)根据每一个第二候选位姿，将当前图像帧的第一特征点投影至预置的图像坐标系，得到N3组新的第一特征点。(六)在预置的图像坐标系中，根据N1×N2组新的第三地图点和N3组新的第一特征点，构成N1×N2×N3个新的位姿组合，计算每一个组合中第三地图点与第一特征点之间的距离，即可得到N1×N2×N3个距离，从中选择最小的距离，并以该距离对应的第一候选位姿的横坐标和纵坐标，以及该距离对应的第二候选位姿的航向角，组成当前图像帧的位姿。
在另一种可能的实现方式中，若当前图像帧的位姿为六自由度的量，即包含横坐标、纵坐标、竖坐标、航向角、滚动角和俯仰角，则对分层采样的过程包括：(一)获取第三地图点在矢量地图对应的三维坐标系中的位置和第一特征点在当前图像帧中的位置。(二)保持当前图像帧的预测位姿的航向角、滚动角、俯仰角和竖坐标不变，改变当前图像帧的预测位姿的横坐标和纵坐标，得到第一候选位姿。(三)根据第一候选位姿对第三地图点在矢量地图对应的三维坐标系中的位置进行变换，得到第三地图点在预置的图像坐标系中的位置。(四)保持当前图像帧的预测位姿的横坐标、纵坐标、竖坐标、滚动角和俯仰角不变，改变当前图像帧的预测位姿的航向角，得到第二候选位姿。(五)根据第二候选位姿对第一特征点在当前图像帧中的位置进行变换，得到第一特征点在图像坐标系中的位置。(六)根据第三地图点在图像坐标系中的位置和第一特征点在图像坐标系中的位置之间的距离的大小，从第一候选位姿和第二候选位姿的组合中确定第三候选位姿。(七)保持第三候选位姿的预测位姿的横坐标、纵坐标、航向角和滚动角不变，改变第三候选位姿的俯仰角和竖坐标，得到第四候选位姿。(八)根据第四候选位姿对第三地图点在矢量地图对应的三维坐标系中的位置进行变换，得到第三地图点在当前图像帧中的位置。(九)根据第一特征点在当前图像帧中的位置和第三地图点在当前图像帧中的位置之间的距离的大小，从第四候选位姿确定当前图像帧的位姿。通过前述的位姿采样方式，可有效减少位姿采样过程中所需的计算量。
为了进一步理解上述采样过程，下文结合一个例子进行介绍，该例子包括步骤(一)至(九)，其中，步骤(一)至(五)可参考前述例子中的步骤(一)至(五)，此处不再赘述。(六)在预置的图像坐标系中，根据N1×N2组新的第三地图点和N3组新的第一特征点，构成N1×N2×N3个新的组合，计算每一个组合中第三地图点与第一特征点之间的距离，可得到N1×N2×N3个距离，从中选择最小的距离，并以该距离对应的第一候选位姿的横坐标和纵坐标，以及该距离对应的第二候选位姿的航向角，组成第三候选位姿。(七)保持第三候选位姿的预测位姿的横坐标、纵坐标、航向角和滚动角不变，在俯仰角的原始值的基础上采样N4次，并在竖坐标的原始值的基础上采样N5次，得到N4×N5个第四候选位姿。(八)根据每一个第四候选位姿，将矢量地图中的第三地图点投影至当前图像帧，得到N4×N5组新的第三地图点。(九)在当前图像帧中，根据N4×N5组新的第三地图点和当前图像帧中的第一特征点，构建N4×N5新的组合，计算每一个组合中第三地图点与第一特征点之间的距离，可得到N4×N5个距离，从中选择最小的距离，并以该距离对应的第四候选位姿的俯仰角和竖坐标，以及第三候选位姿的横坐标、纵坐标、航向角和滚动角，组成当前图像帧的位姿。
得到当前图像帧的位姿和上一次优化后的其它图像帧的位姿后，还可对当前图像帧和其它图像帧进行语义检测，从而得到当前图像帧的第一特征点和其它图像帧的第二特征点。具体地，可通过神经网络对当前图像帧和其它图像帧分别进行语义检测处理，即进行特征提取，从而得到当前图像帧的第一特征点和其它图像帧的第二特征点，第一特征点和第二特征点可以理解为图像上的语义标识。需要说明的是，当前图像帧的第一特征点包括交通环境中各类物体的特征点，图5为本申请实施例提供的当前图像帧的第一特征点的一个示意图，如图5所示，灯杆的特征点和车道线的特征点可以为两个端部的像素点，红绿灯的特征点和指示牌的特征点可以为多个像素点汇集成的矩形框(即外包围盒)等等。同理，其它图像帧的第二特征点也是如此，此处不再赘述。
应理解，前述的神经网络为经过训练后的神经网络模型。下文将对该神经网络的训练过程进行简单的介绍：
在进行模型训练前，获取某一批待训练图像帧，并提前确定每个待训练图像帧中的真实特征点。开始训练后，可向待训练模型输入每个待训练图像帧。然后，通过待训练模型获取每个待训练图像帧的特征点，这部分特征点为预测特征点。最后，通过目标损失函数计算每个待训练图像帧的特征点和相应图像帧的真实特征点之间的差距，若某个待训练图像帧对应的两部分特征点之间的差距在合格范围内，则视为合格的待训练图像帧，若在合格范围外，则视为不合格的待训练图像帧。若该批待训练图像帧中，仅有少量合格的待训练图像帧，则调整待训练模型的参数，并重新用另一批待训练图像帧进行训练，直至存在大量合格的待训练图像帧，以得到用于进行语义检测的神经网络。
还应理解，本实施例中，当前图像帧的位姿通常指在拍摄当前图像帧时，终端设备在矢量地图对应的三维坐标系中所处的位姿，同理，其它图像帧的位姿指在拍摄其它图像帧时，终端设备在矢量地图对应的三维坐标系中所处的位姿等等。
还应理解，上一次优化的过程可参考当前次优化的过程，同理，下一次优化的过程也可参考当前次优化的过程等等。
还应理解，在终端设备拍摄的所有图像帧中，第一个图像帧的位姿可通过终端设备的全球定位系统(global positioning system，GPS)获取，以作为第一次优化的对象。
202、根据当前图像帧的位姿，从矢量地图中，获取与第一特征点匹配的第一地图点。
经过步骤201得到当前图像帧的位姿后，即得到用于当前次优化的当前图像帧的位姿初始值，可基于该位姿从终端设备内部预置的矢量地图中，获取与第一特征点匹配的第一地图点。具体地，可通过多种方式获取与第一特征点匹配的第一地图点，下文将分别进行介绍：
在一种可能的实现方式中，可在矢量地图中划定一个包含终端设备的区域，例如，一个150m×150m的范围，并根据当前图像帧的位姿对该区域中的多个地图点在矢量地图对应的三维坐标系中的位置，进行坐标变换计算，从而得到这部分地图点在当前图像帧中的位置，该过程相当于根据当前图像帧的位姿将该区域内的多个地图点投影至当前图像帧中。由于当前图像帧的第一特征点包括各类物体的特征点，该区域内的多个地图点也包含各类物体的地图点，因此，可通过最近邻算法，对第一特征点以及这部分地图点在当前图像帧中的位置进行计算，以将第一特征点与这部分地图点进行同类物体之间的匹配，从而在这部分地图点中，确定与第一特征点匹配的第一地图点。例如，在矢量地图中，灯杆这一类物体可通过多个地图点汇集而成的直线进行表示，该直线投影至当前图像帧中依旧为一根直线，后续称为投影直线。而灯杆这一类物体在当前图像帧中则通过两个端部的特征点进行表示，后续将端部的特征点称为端点。那么，当矢量地图中有灯杆A、灯杆B、灯杆C投影至当前图像帧时，为了确定哪一个灯杆与当前图像帧中的灯杆D匹配，可计算灯杆D的两个端点到灯杆A的投影直线的距离的平均值，灯杆D的两个端点到灯杆B的投影直线的距离的平均值，以及灯杆D的两个端点到灯杆C的投影直线的距离的平均值，并将平均值最小的灯杆确定为与灯杆D匹配的灯杆，该灯杆的地图点则与灯杆D的特征点相匹配。又如，在矢量地图中，指示牌(或红绿灯)这一类物体可通过多个地图点汇集而成的矩形框进行表示，该矩形框投影至当前图像帧中依旧为一个矩形框，而指示牌这一类物体在当前图像帧中同样也通过多个特征点汇集而成的矩形框进行表示。那么，当矢量地图中有指示牌X、指示牌Y投影至当前图像帧时，为了确定哪一个指示牌与当前图像帧中的指示牌Z匹配，可计算指示牌Z的矩形框上四个顶点到指示牌X的矩形框中两条平行边的投影直线的距离的平均值，以及指示牌Z的矩形框上四个顶点到指示牌Y的矩形框中两条平行边的投影直线的距离的平均值，并将平均值最小的指示牌确定为与指示牌Z匹配的指示牌，该指示牌的地图点则与指示牌Z的特征点相匹配。再如，在矢量地图中，车道线这一类物体可通过多个地图点汇集而成的直线进行表示，该直线投影至当前图像帧中依旧为一根直线，而车道线这一类物体在当前图像帧中则通过两个端部的特征点进行表示。那么，当矢量地图中有车道线E、车道线F投影至当前图像帧时，为了确定哪一个车道线与当前图像帧中的车道线G匹配，可计算车道线G的两个端点到车道线E的投影直线的距离的平均值，以及车道线G与车道线E的投影直线之间的重叠程度，并计算车道线G的两个端点到车道线F的投影直线的距离的平均值，以及车道线G与车道线F的投影直线之间的重叠程度，并以距离和重叠程度为综合距离，将综合距离最小(例如，若车道线E的重叠程度和车道线F的重叠程度相同，则距离较小的车道线为综合距离小的车道线等等)的车道线确定为与车道线G匹配的车道线，该车道线的地图点则与车道线G的特征点相匹配。
具体地，重叠程度的计算过程如图6所示，图6为本申请实施例提供的计算重叠程度的一个示意图，设有当前图像帧的车道线JK，矢量地图的车道线的投影直线PQ，端点J在投影直线PQ上的垂足为U，端点K在在投影直线PQ上的垂足为V，故车道线JK和投影直线PQ之间的重叠程度如公式(5)所示：
Figure BDA0003042103930000131
上式中，loverlap为重叠程度，dUV为线段UV的长度，dUV∩PQ为线段UV与线段PQ之间重叠部分的长度。基于公式(5)可知，图6中从左往右的重叠程度依次为1、dPV/dUV、dPQ/dUV和0。
在另一种可能的实现方式中，可在矢量地图中划定一个包含终端设备的区域。然后，根据当前图像帧的位姿，对当前图像帧的第一特征点在当前图像帧中的位置，进行坐标变换计算，从而得到第一特征点在矢量地图对应的三维坐标系中的位置，该过程相当于根据当前图像帧的位姿将当前图像帧的第一特征点投影至矢量地图对应的三维坐标中。由于当前图像帧的第一特征点包括各类物体的特征点，矢量地图的该区域内的多个地图点也包含各类物体的地图点，因此，可通过最近邻算法，对第一特征点以及这部分地图点在矢量地图对应的三维坐标中的位置进行计算，以将第一特征点与这部分地图点进行同类物体之间的匹配，从而在这部分地图点中，确定与第一特征点匹配的第一地图点。
在另一种可能的实现方式中，可在矢量地图中划定一个包含终端设备的区域，并根据当前图像帧的位姿对该区域中的多个地图点在矢量地图对应的三维坐标系中的位置，进行坐标变换计算，从而得到这部分地图点在终端设备对应的三维坐标系中的位置。与此同时，还可根据当前图像帧的位姿，对当前图像帧的第一特征点在当前图像帧中的位置，进行坐标变换计算，从而得到第一特征点在终端设备对应的三维坐标系中的位置。由于当前图像帧的第一特征点包括各类物体的特征点，矢量地图的该区域内的多个地图点也包含各类物体的地图点，因此，可通过最近邻算法，对第一特征点以及这部分地图点在终端设备对应的三维坐标中的位置进行计算，以将第一特征点与这部分地图点进行同类物体之间的匹配，从而在这部分地图点中，确定与第一特征点匹配的第一地图点。
以上三种实现方式中，均是将当前图像帧中所有物体的特征点以及矢量地图的划分区域中所有物体的地图点设置在某一个坐标系中，从而完成特征点与地图点之间的匹配。此外，还可将某些类别的物体(例如，红绿灯、灯杆、指示牌等等)的特征点和地图点设置于某一个坐标系中(例如，当前图像帧)中进行匹配，并将其它类别的物体(例如，车道线)的特征点和地图点设置于另一坐标系中(例如，终端设备对应的三维坐标系)进行匹配。
值得注意的是，在得到与第一特征点匹配的第一地图点后，相当于得到了第一特征点在第一坐标系中的位置与第一地图点在第一坐标系中的位置之间的距离，该距离包括以下至少一种：1、第一特征点在当前图像帧中的位置，与第一地图点在当前图像帧中的位置之间的距离。2、第一特征点在矢量地图对应的三维坐标系中的位置，与第一地图点在矢量地图对应的三维坐标系中的位置之间的距离。3、第一特征点在终端设备对应的三维坐标系中的位置，与第一地图点在终端设备对应的三维坐标系中的位置之间的距离。
为了进一步理解上述说明，下文结合一个例子进行说明。设当前图像帧中包含灯杆W1、指示牌W2、车道线W3和车道线W4，矢量地图中的灯杆W5与灯杆W1匹配，矢量地图中的指示牌W6与指示牌W2匹配，矢量地图中的车道线W7与车道线W3匹配，矢量地图中的车道线W8与车道线W4匹配。当第一坐标系的含义不同时，将存在以下多种情况：
情况一：当第一坐标系为当前图像帧时，则第一特征点在第一坐标系中的位置与第一地图点在第一坐标系中的位置之间的距离即为第一特征点在当前图像帧中的位置与第一地图点在当前图像帧中的位置之间的距离，包括：投影至当前图像帧后，灯杆W1的两个端点到灯杆W5的投影直线的距离的平均值，指示牌W2的矩形框上四个顶点到指示牌W6的矩形框中两条平行边的投影直线的距离的平均值，车道线W3与车道线W7之间的综合距离，以及车道线W4与车道线W8之间的综合距离。
情况二：当第一坐标系包括当前图像帧和终端设备对应的三维坐标系时，则第一特征点在第一坐标系中的位置与第一地图点在第一坐标系中的位置之间的距离包括：第一特征点在当前图像帧中的位置与第一地图点在当前图像帧中的位置之间的距离，以及第一特征点在终端设备对应的三维坐标系中的位置，与第一地图点在终端设备对应的三维坐标系中的位置之间的距离。其中，第一特征点在当前图像帧中的位置与第一地图点在当前图像帧中的位置之间的距离包括：投影至当前图像帧后，灯杆W1的两个端点到灯杆W5的投影直线的距离的平均值，以及指示牌W2的矩形框上四个顶点到指示牌W6的矩形框中两条平行边的投影直线的距离的平均值。第一特征点在终端设备对应的三维坐标系中的位置，与第一地图点在终端设备对应的三维坐标系中的位置之间的距离包括：投影至终端设备对应的三维坐标系后，车道线W3与车道线W7之间的综合距离，以及车道线W4与车道线W8之间的综合距离。
同样地，还存在情况三(当第一坐标系为矢量地图对应的三维坐标系时)、情况四(当第一坐标系为终端设备对应的三维坐标系时)、情况五(当第一坐标系包括当前图像帧和矢量地图对应的三维坐标系时)、情况六(当第一坐标系包括终端设备对应的三维坐标系和矢量地图对应的三维坐标系)、以及情况七(当第一坐标系包括当前图像帧、终端设备对应的三维坐标系和矢量地图对应的三维坐标系时)，也可参照情况一和情况二的相关说明，此处不再赘述。
203、根据上一次优化后的其它图像帧的位姿，从矢量地图中，获取与第二特征点匹配的第二地图点。
经过步骤201得到上一次优化后的其它图像帧的位姿后，即得到用于当前次优化的其它图像帧的位姿初始值，可基于该位姿从终端设备内部矢量地图中，获取与第二特征点匹配的第二地图点。
在得到与第二特征点匹配的第二地图点后，相当于得到了第二特征点在第二坐标系中的位置与第二地图点在第二坐标系中的位置之间的距离，该距离包括以下至少一种：1、第二特征点在其它图像帧中的位置，与第二地图点在其它图像帧中的位置之间的距离。2、第二特征点在矢量地图对应的三维坐标系中的位置，与第二地图点在矢量地图对应的三维坐标系中的位置之间的距离。3、第二特征点在终端设备对应的三维坐标系中的位置，与第二地图点在终端设备对应的三维坐标系中的位置之间的距离。
需要说明的是，获取第二地图点的过程的介绍可参考步骤202中获取第一地图点的过程的相关说明部分，此处不再赘述。进一步地，第二特征点在第二坐标系中的位置与第二地图点在第二坐标系中的位置之间的距离，也可参考步骤202中，第一特征点在第一坐标系中的位置与第一地图点在第一坐标系中的位置之间的距离的相关说明部分，此处不再赘述。
204、根据目标函数对当前图像帧的位姿进行调整，得到当前次优化后的当前图像帧的位姿，作为终端设备的定位结果，目标函数包括第一特征点与第一地图点之间的第一匹配误差，以及第二特征点与第二地图点之间的第二匹配误差。
在得到与第一特征点匹配的第一地图点和与第二特征点匹配的第二地图点后，可根据第一特征点与第一地图点之间的第一匹配误差，以及第二特征点与第二地图点之间的第二匹配误差构建的目标函数，对当前图像帧的位姿进行调整，即对当前图像帧的位姿进行优化，得到当前次优化后的当前图像帧的位姿，作为终端设备的定位结果。
具体地，可先根据第一特征点在第一坐标系中的位置与第一地图点在第一坐标系中的位置之间的距离，得到第一匹配误差的初始值。依旧如上述例子进行说明，第一匹配误差的初始值可通过公式(6)得到：
Huberε1(dpp+dpl)+β*Huberε2(dpH)
Figure BDA0003042103930000151
Figure BDA0003042103930000152
Figure BDA0003042103930000153
Figure BDA0003042103930000154
Figure BDA0003042103930000155
Figure BDA0003042103930000156
Figure BDA0003042103930000157
上式中，第一匹配误差通过Huberε1和Huberε2确定，Huberε1为参数是ε1的Huber损失函数，Huberε2为参数是ε2的Huber损失函数，β为预置的参数，dpp为当前图像帧中灯杆这类物体对应的距离，
Figure BDA0003042103930000161
为当前图像帧中第i个灯杆与相匹配的灯杆之间的距离，
Figure BDA0003042103930000162
为第i个灯杆的两个端点到相匹配的灯杆的投影直线的距离，dpl为当前图像帧中红绿灯(或指示牌)这两类物体对应的距离，
Figure BDA0003042103930000163
为当前图像帧中第i个红绿灯(或指示牌)与相匹配的红绿灯(或指示牌)之间的距离，
Figure BDA0003042103930000164
和
Figure BDA0003042103930000165
为第i个红绿灯(或指示牌)的四个顶点到相匹配的红绿灯的矩形框中两条平行边的投影直线的距离，dpH为当前图像帧中车道线这类物体对应的综合距离，
Figure BDA0003042103930000166
为当前图像帧中第i个车道线与相匹配的车道线之间的综合距离，
Figure BDA0003042103930000167
为第i个车道线与相匹配的车道线之间的距离，
Figure BDA0003042103930000168
为第i个车道线与相匹配的车道线之间的重合程度，
Figure BDA0003042103930000169
为第i个车道线的两个端点到相匹配的车道线的投影直线的距离。
进一步地，还可根据第二特征点在第二坐标系中的位置与第二地图点在第二坐标系中的位置之间的距离进行计算，得到第二匹配误差的初始值。依旧如上述例子，第二匹配误差的初始值也可通过公式(6)得到，此处不再赘述。
得到第一匹配误差的初始值以及第二匹配误差的初始值后，可将这部分初始值输入目标函数，并对目标函数进行迭代求解，直至满足预置的迭代条件，得到当前次优化后的当前图像帧的位姿。基于公式(6)，目标函数可通过公式(7)表示：
Figure BDA00030421039300001610
上式中，在当前图像帧和其它图像帧中，
Figure BDA00030421039300001611
为第i个图像帧中灯杆这类物体对应的距离，
Figure BDA00030421039300001612
为第i个图像帧中红绿灯(或指示牌)这两类物体对应的距离，
Figure BDA00030421039300001613
为第i个图像帧中车道线这类物体对应的距离。
应理解，本实施例仅以公式(6)和公式(7)进行示意性说明，并不对匹配误差的计算方式和目标函数的表达方式构成限制。
在对目标函数进行迭代求解的过程中，完成第一次迭代后，即将第一匹配误差的初始值以及第二匹配误差的初始值输入目标函数进行求解后，可得到第一次迭代得到的当前图像帧的位姿以及第一次迭代得到的其它图像帧的位姿。然后，根据第一次迭代得到的当前图像帧的位姿以及第一次迭代得到的其它图像帧的位姿进行计算，得到第一次迭代得到的帧间位姿差，若该帧间位姿差与终端设备的里程计计算的帧间位姿差之间的差值小于预置的阈值，相当于目标函数收敛，则停止迭代，并以第一次迭代得到的当前图像帧的位姿作为当前次优化后的当前图像帧的位姿，若该帧间位姿差与终端设备的里程计计算的帧间位姿差之间的差值大于或等于预置的阈值，则进行第二次迭代。
在进行第二次迭代时，可根据第一次迭代得到的当前图像帧的位姿重新确定与第一特征点匹配的第一地图点(即重新执行步骤202)，并根据第一次迭代得到的其它图像帧的位姿重新确定与第二特征点匹配的第二地图点(即重新执行步骤203)。然后，计算第一特征点与第一地图点之间的第一匹配误差的第一次迭代值，第二特征点与第二地图点之间的第二匹配误差的第一次迭代值。随后，将第一匹配误差的第一次迭代值和第二匹配误差的第一次迭代值输出目标函数进行求解，可得到第二次迭代得到的当前图像帧的位姿以及第二次迭代得到的其它图像帧的位姿。接着，根据第二次迭代得到的当前图像帧的位姿以及第二次迭代得到的其它图像帧的位姿进行计算，得到第二次迭代得到的帧间位姿差，若该帧间位姿差与终端设备的里程计计算的帧间位姿差之间的差值小于预置的阈值，则停止迭代，并以第二次迭代得到的当前图像帧的位姿作为当前次优化后的当前图像帧的位姿，若该帧间位姿差与终端设备的里程计计算的帧间位姿差之间的差值大于或等于预置的阈值，则进行第三次迭代，直至迭代次数等于预置的次数，此时也认为目标函数收敛，则将最后一次迭代得到的当前图像帧的位姿作为当前次优化后的当前图像帧的位姿。
本实施例中，在得到当前图像帧和当前图像帧之前的其它图像帧后，可从矢量地图中，获取与当前图像帧的第一特征点匹配的第一地图点，以及与当前图像帧之前的其它图像帧的第二特征点匹配的第二地图点。然后，可根据第一特征点与第一地图点之间的第一匹配误差，以及第二特征点与第二地图点之间的第二匹配误差构建的目标函数，对当前图像帧的位姿进行调整，得到当前次优化后的当前图像帧的位姿。前述过程中，由于目标函数既包含当前图像帧的特征点与矢量地图的地图点之间的匹配误差，以及其它图像帧的特征点与矢量地图的地图点之间的匹配误差，故通过该目标函数对当前图像帧的位姿进行调整，不仅考虑了当前图像帧对当前图像帧的位姿的优化过程造成的影响，还考虑了其它图像帧对当前图像帧的位姿的优化过程造成的影响，即考虑了当前图像帧以及其它图像帧之间的关联性，所考虑的因素更全面，故基于此种方式得到的终端设备的定位结果，具备更高的准确性。
更进一步地，在相关技术中，目标函数仅通过当前图像帧的特征点以及矢量地图的地图点之间的匹配误差构建，由于当前图像帧所能呈现的内容有限，在挑选与当前图像帧的特征点匹配的地图点时，往往会出现地图点较为稀疏且存在重叠的情况，故在对目标函数进行迭代求解时，无法令特征点与地图点之间的匹配误差足够小，进而影响定位结果的准确性。本实施例中，目标函数通过当前图像帧的第一特征点与矢量地图的第一地图点之间的第一匹配误差，以及其它图像帧的第二特征点与矢量地图的第二地图点之间的第二匹配误差构建，由于多个图像帧所呈现的内容通常存在较大的区别度，可避免地图点较为稀疏且存在重叠的情况出现，故在对目标函数进行迭代求解时(对多个图像帧的位姿进行联合优化)，可令第一匹配误差和第二匹配误差足够小，进而提高定位结果的准确性。
更进一步地，通过分层采样所得到的当前图像帧的位姿，可作为当前次优化的当前图像帧的位姿初始值，从而提高当前次优化的收敛速度和鲁棒性。
以上是对本申请实施例提供的终端设备定位方法进行的详细说明，以下将对本申请实施例提供的终端设备定位装置进行介绍。图7为本申请实施例提供的终端设备定位装置的一个结构示意图，如图7所示，该装置包括：
第一匹配模块701，用于从矢量地图中，获取与终端设备拍摄的当前图像帧的第一特征点匹配的第一地图点；
第二匹配模块702，用于从矢量地图中，获取与当前图像帧之前的其它图像帧的第二特征点匹配的第二地图点；
调整模块703，用于根据目标函数对终端设备拍摄当前图像帧时的位姿进行调整，得到当前次调整后的终端设备拍摄当前图像帧时的位姿，作为终端设备的定位结果，目标函数包括第一特征点与第一地图点之间的第一匹配误差，以及第二特征点与第二地图点之间的第二匹配误差。
在一种可能的实现方式中，该装置还包括：获取模块700，用于获取当前图像帧的第一特征点、当前图像帧之前的其它图像帧的第二特征点、终端设备拍摄当前图像帧时的位姿和上一次调整后的终端设备拍摄其它图像帧时的位姿；第一匹配模块701，用于根据终端设备拍摄当前图像帧时的位姿，从矢量地图中，获取与第一特征点匹配的第一地图点；第二匹配模块702，用于根据上一次调整后的终端设备拍摄其它图像帧时的位姿，从矢量地图中，获取与第二特征点匹配的第二地图点。
在一种可能的实现方式中，调整模块703，用于：根据第一特征点在第一坐标系中的位置与第一地图点在第一坐标系中的位置之间的距离进行计算，得到第一匹配误差的初始值；根据第二特征点在第二坐标系中的位置与第二地图点在第二坐标系中的位置之间的距离进行计算，得到第二匹配误差的初始值；根据第一匹配误差的初始值以及第二匹配误差的初始值，对目标函数进行迭代求解，直至满足预置的迭代条件，得到当前次调整后的终端设备拍摄当前图像帧时的位姿。
在一种可能的实现方式中，第一特征点在第一坐标系中的位置与第一地图点在第一坐标系中的位置之间的距离包括以下至少一种：第一特征点在当前图像帧中的位置，与第一地图点在当前图像帧中的位置之间的距离；第一特征点在矢量地图对应的三维坐标系中的位置，与第一地图点在矢量地图对应的三维坐标系中的位置之间的距离；或者第一特征点在终端设备对应的三维坐标系中的位置，与第一地图点在终端设备对应的三维坐标系中的位置之间的距离。
在一种可能的实现方式中，第二特征点在第二坐标系中的位置与第二地图点在第二坐标系中的位置之间的距离包括以下至少一种：第二特征点在其它图像帧中的位置，与第二地图点在其它图像帧中的位置之间的距离；第二特征点在矢量地图对应的三维坐标系中的位置，与第二地图点在矢量地图对应的三维坐标系中的位置之间的距离；或者第二特征点在终端设备对应的三维坐标系中的位置，与第二地图点在终端设备对应的三维坐标系中的位置之间的距离。
在一种可能的实现方式中，迭代条件为：对于任意一次迭代，若该次迭代所得到的帧间位姿差与终端设备计算的帧间位姿差之间的差值小于预置的阈值，则停止迭代，该次迭代得到的帧间位姿差根据该次迭代得到的终端设备拍摄当前图像帧时的位姿以及该次迭代得到的终端设备拍摄其它图像帧时的位姿确定；若差值大于或等于阈值，则执行下一次迭代，直至迭代次数等于预置的次数。
在一种可能的实现方式中，其它图像帧的数量根据终端设备的速度确定。
在一种可能的实现方式中，获取模块700，用于：根据上一次调整后的终端设备拍摄其它图像帧时的位姿和终端设备计算的帧间位姿差，计算终端设备拍摄当前图像帧时的预测位姿；对终端设备拍摄当前图像帧时的预测位姿进行分层采样，得到终端设备拍摄当前图像帧时的位姿。
在一种可能的实现方式中，若终端设备拍摄当前图像帧时的位姿包含横坐标、纵坐标和航向角，则获取模块700，用于：获取第三地图点在矢量地图对应的三维坐标系中的位置和第一特征点在当前图像帧中的位置；保持终端设备拍摄当前图像帧时的预测位姿的航向角不变，改变终端设备拍摄当前图像帧时的预测位姿的横坐标和纵坐标，得到第一候选位姿；根据第一候选位姿对第三地图点在矢量地图对应的三维坐标系中的位置进行变换，得到第三地图点在预置的图像坐标系中的位置；保持终端设备拍摄当前图像帧时的预测位姿的横坐标和纵坐标不变，改变终端设备拍摄当前图像帧时的预测位姿的航向角，得到第二候选位姿；根据第二候选位姿对第一特征点在当前图像帧中的位置进行变换，得到第一特征点在图像坐标系中的位置；根据第三地图点在图像坐标系中的位置和第一特征点在图像坐标系中的位置之间的距离的大小，从第一候选位姿和第二候选位姿的组合中确定终端设备拍摄当前图像帧时的位姿。通过前述的位姿采样方式，可有效减少位姿采样过程中所需的计算量。
在一种可能的实现方式中，若终端设备拍摄当前图像帧时的位姿包含横坐标、纵坐标、竖坐标、航向角、滚动角和俯仰角，则获取模块700，用于：获取第三地图点在矢量地图对应的三维坐标系中的位置和第一特征点在当前图像帧中的位置；保持终端设备拍摄当前图像帧时的预测位姿的航向角、滚动角、俯仰角和竖坐标不变，改变终端设备拍摄当前图像帧时的预测位姿的横坐标和纵坐标，得到第一候选位姿；根据第一候选位姿对第三地图点在矢量地图对应的三维坐标系中的位置进行变换，得到第三地图点在预置的图像坐标系中的位置；保持终端设备拍摄当前图像帧时的预测位姿的横坐标、纵坐标、竖坐标、滚动角和俯仰角不变，改变终端设备拍摄当前图像帧时的预测位姿的航向角，得到第二候选位姿；根据第二候选位姿对第一特征点在当前图像帧中的位置进行变换，得到第一特征点在图像坐标系中的位置；根据第三地图点在图像坐标系中的位置和第一特征点在图像坐标系中的位置之间的距离的大小，从第一候选位姿和第二候选位姿的组合中确定第三候选位姿；保持第三候选位姿的预测位姿的横坐标、纵坐标、航向角和滚动角不变，改变第三候选位姿的俯仰角和竖坐标，得到第四候选位姿；根据第四候选位姿对第三地图点在矢量地图对应的三维坐标系中的位置进行变换，得到第三地图点在当前图像帧中的位置；根据第一特征点在当前图像帧中的位置和第三地图点在当前图像帧中的位置之间的距离的大小，从第四候选位姿确定终端设备拍摄当前图像帧时的位姿。通过前述的位姿采样方式，可有效减少位姿采样过程中所需的计算量。
需要说明的是，上述装置各模块/单元之间的信息交互、执行过程等内容，由于与本申请方法实施例基于同一构思，其带来的技术效果与本申请方法实施例相同，具体内容可参考本申请实施例前述所示的方法实施例中的叙述，此处不再赘述。
图8为本申请实施例提供的终端设备定位装置的另一结构示意图。如图8所示，本申请实施例中计算机一个实施例可以包括一个或一个以上中央处理器801，存储器802，输入输出接口803，有线或无线网络接口804，电源805。
存储器802可以是短暂存储或持久存储。更进一步地，中央处理器801可以配置为与存储器802通信，在计算机上执行存储器802中的一系列指令操作。
本实施例中，中央处理器801可以执行前述图2所示实施例中的方法步骤，具体此处不再赘述。
本实施例中，中央处理器801中的具体功能模块划分可以与前述图7中所描述的获取模块、第一匹配模块、第二匹配模块和优化模块等模块的划分方式类似，此处不再赘述。
本申请实施例还涉及一种计算机存储介质，包括计算机可读指令，当所述计算机可读指令被执行时，实现如图2所述的方法。
本申请实施例还涉及一种包含指令的计算机程序产品，当其在计算机上运行时，使得计算机执行如图2所述的方法。
所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统，装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在本申请所提供的几个实施例中，应该理解到，所揭露的系统，装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

Claims (19)

1.一种终端设备定位方法，其特征在于，所述方法包括：
从矢量地图中，获取与终端设备拍摄的当前图像帧的第一特征点匹配的第一地图点；
从所述矢量地图中，获取与所述当前图像帧之前的其它图像帧的第二特征点匹配的第二地图点；
根据目标函数对所述终端设备拍摄所述当前图像帧时的位姿进行调整，得到当前次调整后的终端设备拍摄所述当前图像帧时的位姿，作为所述终端设备的定位结果，所述目标函数包括所述第一特征点与所述第一地图点之间的第一匹配误差，以及所述第二特征点与所述第二地图点之间的第二匹配误差。
2.根据权利要求1所述的方法，其特征在于，所述方法还包括：
获取所述当前图像帧的第一特征点、所述当前图像帧之前的其它图像帧的第二特征点、所述终端设备拍摄所述当前图像帧时的位姿和上一次调整后的终端设备拍摄所述其它图像帧时的位姿；
所述从矢量地图中，获取与所述第一特征点匹配的第一地图点包括：
根据所述终端设备拍摄所述当前图像帧时的位姿，从矢量地图中，获取与所述第一特征点匹配的第一地图点；
所述从所述矢量地图中，获取与所述第二特征点匹配的第二地图点包括：
根据所述上一次调整后的终端设备拍摄所述其它图像帧时的位姿，从所述矢量地图中，获取与所述第二特征点匹配的第二地图点。
3.根据权利要求1或2所述的方法，其特征在于，所述根据目标函数对所述终端设备拍摄所述当前图像帧时的位姿进行调整，得到当前次调整后的终端设备拍摄所述当前图像帧时的位姿包括：
根据所述第一特征点在第一坐标系中的位置与所述第一地图点在所述第一坐标系中的位置之间的距离进行计算，得到所述第一匹配误差的初始值；
根据所述第二特征点在第二坐标系中的位置与所述第二地图点在所述第二坐标系中的位置之间的距离进行计算，得到所述第二匹配误差的初始值；
根据所述第一匹配误差的初始值以及所述第二匹配误差的初始值，对所述目标函数进行迭代求解，直至满足预置的迭代条件，得到当前次调整后的终端设备拍摄所述当前图像帧时的位姿。
4.根据权利要求3所述的方法，其特征在于，所述第一特征点在第一坐标系中的位置与所述第一地图点在所述第一坐标系中的位置之间的距离包括以下至少一种：
所述第一特征点在当前图像帧中的位置，与所述第一地图点在所述当前图像帧中的位置之间的距离；
所述第一特征点在矢量地图对应的三维坐标系中的位置，与所述第一地图点在所述矢量地图对应的三维坐标系中的位置之间的距离；或者
所述第一特征点在终端设备对应的三维坐标系中的位置，与所述第一地图点在所述终端设备对应的三维坐标系中的位置之间的距离。
5.根据权利要求3所述的方法，其特征在于，所述第二特征点在第二坐标系中的位置与所述第二地图点在所述第二坐标系中的位置之间的距离包括以下至少一种：
所述第二特征点在所述其它图像帧中的位置，与所述第二地图点在所述其它图像帧中的位置之间的距离；
所述第二特征点在矢量地图对应的三维坐标系中的位置，与所述第二地图点在所述矢量地图对应的三维坐标系中的位置之间的距离；或者
所述第二特征点在终端设备对应的三维坐标系中的位置，与所述第二地图点在所述终端设备对应的三维坐标系中的位置之间的距离。
6.根据权利要求3至5任意一项所述的方法，其特征在于，所述迭代条件为：对于任意一次迭代，若该次迭代所得到的帧间位姿差与所述终端设备计算的帧间位姿差之间的差值小于预置的阈值，则停止迭代，该次迭代得到的帧间位姿差根据该次迭代得到的终端设备拍摄所述当前图像帧时的位姿以及该次迭代得到的终端设备拍摄所述其它图像帧时的位姿确定，所述帧间位姿差为在所述当前图像帧和所述其它图像帧中，所述终端设备拍摄相邻两个图像帧之间的位姿差；若所述差值大于或等于所述阈值，则执行下一次迭代，直至迭代次数等于预置的次数。
7.根据权利要求1至6任意一项所述的方法，其特征在于，所述其它图像帧的数量根据所述终端设备的速度确定。
8.根据权利要求2至7任意一项所述的方法，其特征在于，所述获取所述终端设备拍摄所述当前图像帧时的位姿包括：
根据所述上一次调整后的终端设备拍摄所述其它图像帧时的位姿和所述终端设备计算的帧间位姿差，计算所述终端设备拍摄所述当前图像帧时的预测位姿；
对所述终端设备拍摄所述当前图像帧时的预测位姿进行分层采样，得到所述终端设备拍摄所述当前图像帧时的位姿。
9.一种终端设备定位装置，其特征在于，所述装置包括：
第一匹配模块，用于从矢量地图中，获取与终端设备拍摄的当前图像帧的第一特征点匹配的第一地图点；
第二匹配模块，用于从所述矢量地图中，获取与所述当前图像帧之前的其它图像帧的第二特征点匹配的第二地图点；
调整模块，用于根据目标函数对所述终端设备拍摄所述当前图像帧时的位姿进行调整，得到当前次调整后的终端设备拍摄所述当前图像帧时的位姿，作为所述终端设备的定位结果，所述目标函数包括所述第一特征点与所述第一地图点之间的第一匹配误差，以及所述第二特征点与所述第二地图点之间的第二匹配误差。
10.根据权利要求9所述的装置，其特征在于，所述装置还包括：
获取模块，用于获取所述当前图像帧的第一特征点、所述当前图像帧之前的其它图像帧的第二特征点、所述终端设备拍摄所述当前图像帧时的位姿和上一次调整后的终端设备拍摄所述其它图像帧时的位姿；
所述第一匹配模块，用于根据所述终端设备拍摄所述当前图像帧时的位姿，从矢量地图中，获取与所述第一特征点匹配的第一地图点；
所述第二匹配模块，用于根据所述上一次调整后的终端设备拍摄所述其它图像帧时的位姿，从所述矢量地图中，获取与所述第二特征点匹配的第二地图点。
11.根据权利要求9或10所述的装置，其特征在于，所述调整模块，用于：
根据所述第一特征点在第一坐标系中的位置与所述第一地图点在所述第一坐标系中的位置之间的距离进行计算，得到所述第一匹配误差的初始值；
根据所述第二特征点在第二坐标系中的位置与所述第二地图点在所述第二坐标系中的位置之间的距离进行计算，得到所述第二匹配误差的初始值；
根据所述第一匹配误差的初始值以及所述第二匹配误差的初始值，对所述目标函数进行迭代求解，直至满足预置的迭代条件，得到当前次调整后的终端设备拍摄所述当前图像帧时的位姿。
12.根据权利要求11所述的装置，其特征在于，所述第一特征点在第一坐标系中的位置与所述第一地图点在所述第一坐标系中的位置之间的距离包括以下至少一种：
所述第一特征点在当前图像帧中的位置，与所述第一地图点在所述当前图像帧中的位置之间的距离；
所述第一特征点在矢量地图对应的三维坐标系中的位置，与所述第一地图点在所述矢量地图对应的三维坐标系中的位置之间的距离；或者
所述第一特征点在终端设备对应的三维坐标系中的位置，与所述第一地图点在所述终端设备对应的三维坐标系中的位置之间的距离。
13.根据权利要求11所述的装置，其特征在于，所述第二特征点在第二坐标系中的位置与所述第二地图点在所述第二坐标系中的位置之间的距离包括以下至少一种：
所述第二特征点在其它图像帧中的位置，与所述第二地图点在所述其它图像帧中的位置之间的距离；
所述第二特征点在矢量地图对应的三维坐标系中的位置，与所述第二地图点在所述矢量地图对应的三维坐标系中的位置之间的距离；或者
所述第二特征点在终端设备对应的三维坐标系中的位置，与所述第二地图点在所述终端设备对应的三维坐标系中的位置之间的距离。
14.根据权利要求11至13任意一项所述的装置，其特征在于，所述迭代条件为：对于任意一次迭代，若该次迭代所得到的帧间位姿差与所述终端设备计算的帧间位姿差之间的差值小于预置的阈值，则停止迭代，该次迭代得到的帧间位姿差根据该次迭代得到的终端设备拍摄所述当前图像帧时的位姿以及该次迭代得到的终端设备拍摄所述其它图像帧时的位姿确定，所述帧间位姿差为在所述当前图像帧和所述其它图像帧中，所述终端设备拍摄相邻两个图像帧之间的位姿差；若所述差值大于或等于所述阈值，则执行下一次迭代，直至迭代次数等于预置的次数。
15.根据权利要求9至14任意一项所述的装置，其特征在于，所述其它图像帧的数量根据所述终端设备的速度确定。
16.根据权利要求10至15任意一项所述的装置，其特征在于，所述获取模块，用于：
根据所述上一次调整后的终端设备拍摄所述其它图像帧时的位姿和所述终端设备计算的帧间位姿差，计算所述终端设备拍摄所述当前图像帧时的预测位姿；
对所述终端设备拍摄所述当前图像帧时的预测位姿进行分层采样，得到所述终端设备拍摄所述当前图像帧时的位姿。
17.一种终端设备定位装置，其特征在于，包括存储器和处理器；所述存储器存储有代码，所述处理器被配置为执行所述代码，当所述代码被执行时，所述终端设备定位装置执行如权利要求1至8任意一项所述的方法。
18.一种计算机存储介质，其特征在于，所述计算机存储介质存储有计算机程序，该程序由计算机执行时，使得所述计算机实施权利要求1至8任意一项所述的方法。
19.一种计算机程序产品，其特征在于，所述计算机程序产品存储有指令，所述指令在由计算机执行时，使得所述计算机实施权利要求1至8任意一项所述的方法。
CN202110460636.4A 2021-04-27 2021-04-27 一种终端设备定位方法及其相关设备 Active CN113239072B (zh)

Priority Applications (4)

Application Number Priority Date Filing Date Title
CN202110460636.4A CN113239072B (zh) 2021-04-27 2021-04-27 一种终端设备定位方法及其相关设备
PCT/CN2022/089007 WO2022228391A1 (zh) 2021-04-27 2022-04-25 一种终端设备定位方法及其相关设备
EP22794861.9A EP4322020A4 (en) 2021-04-27 2022-04-25 METHOD FOR POSITIONING TERMINAL DEVICE AND ASSOCIATED DEVICE
US18/494,547 US20240062415A1 (en) 2021-04-27 2023-10-25 Terminal device localization method and related device therefor

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110460636.4A CN113239072B (zh) 2021-04-27 2021-04-27 一种终端设备定位方法及其相关设备

Publications (2)

Publication Number Publication Date
CN113239072A true CN113239072A (zh) 2021-08-10
CN113239072B CN113239072B (zh) 2024-09-06

Family

ID=77129462

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110460636.4A Active CN113239072B (zh) 2021-04-27 2021-04-27 一种终端设备定位方法及其相关设备

Country Status (4)

Country Link
US (1) US20240062415A1 (zh)
EP (1) EP4322020A4 (zh)
CN (1) CN113239072B (zh)
WO (1) WO2022228391A1 (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113838129A (zh) * 2021-08-12 2021-12-24 高德软件有限公司 一种获得位姿信息的方法、装置以及系统
CN114549633A (zh) * 2021-09-16 2022-05-27 北京小米移动软件有限公司 位姿检测方法、装置、电子设备和存储介质
WO2022228391A1 (zh) * 2021-04-27 2022-11-03 华为技术有限公司 一种终端设备定位方法及其相关设备

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR950005030A (ko) * 1993-07-16 1995-02-18 배순훈 블럭정합기를 이용한 대칭블럭 움직임 추정방법 및 장치
WO2017022033A1 (ja) * 2015-07-31 2017-02-09 富士通株式会社 画像処理装置、画像処理方法および画像処理プログラム
CN107610175A (zh) * 2017-08-04 2018-01-19 华南理工大学 基于半直接法和滑动窗口优化的单目视觉slam算法
CN109345588A (zh) * 2018-09-20 2019-02-15 浙江工业大学 一种基于Tag的六自由度姿态估计方法
CN111750864A (zh) * 2020-06-30 2020-10-09 杭州海康机器人技术有限公司 一种基于视觉地图的重定位方法和装置
CN111780763A (zh) * 2020-06-30 2020-10-16 杭州海康机器人技术有限公司 一种基于视觉地图的视觉定位方法、装置
CN111780764A (zh) * 2020-06-30 2020-10-16 杭州海康机器人技术有限公司 一种基于视觉地图的视觉定位方法、装置
CN112444242A (zh) * 2019-08-31 2021-03-05 北京地平线机器人技术研发有限公司 一种位姿优化方法及装置
CN112640417A (zh) * 2019-08-09 2021-04-09 华为技术有限公司 匹配关系确定方法、重投影误差计算方法及相关装置

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112444251B (zh) * 2019-08-29 2023-06-13 长沙智能驾驶研究院有限公司 车辆行车位置确定方法、装置、存储介质及计算机设备
CN112393723B (zh) * 2020-11-27 2023-10-24 北京三快在线科技有限公司 一种定位方法、设备、介质及无人设备
CN113239072B (zh) * 2021-04-27 2024-09-06 华为技术有限公司 一种终端设备定位方法及其相关设备

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR950005030A (ko) * 1993-07-16 1995-02-18 배순훈 블럭정합기를 이용한 대칭블럭 움직임 추정방법 및 장치
WO2017022033A1 (ja) * 2015-07-31 2017-02-09 富士通株式会社 画像処理装置、画像処理方法および画像処理プログラム
CN107610175A (zh) * 2017-08-04 2018-01-19 华南理工大学 基于半直接法和滑动窗口优化的单目视觉slam算法
CN109345588A (zh) * 2018-09-20 2019-02-15 浙江工业大学 一种基于Tag的六自由度姿态估计方法
CN112640417A (zh) * 2019-08-09 2021-04-09 华为技术有限公司 匹配关系确定方法、重投影误差计算方法及相关装置
CN112444242A (zh) * 2019-08-31 2021-03-05 北京地平线机器人技术研发有限公司 一种位姿优化方法及装置
CN111750864A (zh) * 2020-06-30 2020-10-09 杭州海康机器人技术有限公司 一种基于视觉地图的重定位方法和装置
CN111780763A (zh) * 2020-06-30 2020-10-16 杭州海康机器人技术有限公司 一种基于视觉地图的视觉定位方法、装置
CN111780764A (zh) * 2020-06-30 2020-10-16 杭州海康机器人技术有限公司 一种基于视觉地图的视觉定位方法、装置

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
王化友;代波;何玉庆;: "CFD-SLAM:融合特征法与直接法的快速鲁棒SLAM系统", 高技术通讯, no. 12 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2022228391A1 (zh) * 2021-04-27 2022-11-03 华为技术有限公司 一种终端设备定位方法及其相关设备
CN113838129A (zh) * 2021-08-12 2021-12-24 高德软件有限公司 一种获得位姿信息的方法、装置以及系统
CN113838129B (zh) * 2021-08-12 2024-03-15 高德软件有限公司 一种获得位姿信息的方法、装置以及系统
CN114549633A (zh) * 2021-09-16 2022-05-27 北京小米移动软件有限公司 位姿检测方法、装置、电子设备和存储介质

Also Published As

Publication number Publication date
WO2022228391A1 (zh) 2022-11-03
EP4322020A4 (en) 2024-10-09
US20240062415A1 (en) 2024-02-22
EP4322020A1 (en) 2024-02-14
CN113239072B (zh) 2024-09-06

Similar Documents

Publication Publication Date Title
CN109345574B (zh) 基于语义点云配准的激光雷达三维建图方法
CN108242079B (zh) 一种基于多特征视觉里程计和图优化模型的vslam方法
CN113239072B (zh) 一种终端设备定位方法及其相关设备
CN109186606B (zh) 一种基于slam和图像信息的机器人构图及导航方法
CN109509230A (zh) 一种应用于多镜头组合式全景相机的slam方法
CN111830953A (zh) 车辆自定位方法、装置及系统
CN111862213A (zh) 定位方法及装置、电子设备、计算机可读存储介质
CN112183171A (zh) 一种基于视觉信标建立信标地图方法、装置
CN110827353B (zh) 一种基于单目摄像头辅助的机器人定位方法
CN114088081B (zh) 一种基于多段联合优化的用于精确定位的地图构建方法
CN112734765A (zh) 基于实例分割与多传感器融合的移动机器人定位方法、系统及介质
CN110260866A (zh) 一种基于视觉传感器的机器人定位与避障方法
CN111998862A (zh) 一种基于bnn的稠密双目slam方法
CN111862200B (zh) 一种煤棚内无人机定位方法
CN111986261A (zh) 一种车辆定位方法、装置、电子设备及存储介质
CN111681172A (zh) 协同构建点云地图的方法、设备和系统
CN115471748A (zh) 一种面向动态环境的单目视觉slam方法
CN114494150A (zh) 一种基于半直接法的单目视觉里程计的设计方法
CN117367427A (zh) 一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法
CN111815684B (zh) 基于统一残差模型的空间多元特征配准优化方法及装置
CN111198563B (zh) 一种用于足式机器人动态运动的地形识别方法及系统
Jo et al. Mixture density-PoseNet and its application to monocular camera-based global localization
CN116125980A (zh) 无人货车驾驶方法、装置、电子设备及存储介质
CN114462545A (zh) 一种基于语义slam的地图构建方法及装置
CN114708321A (zh) 一种基于语义的相机位姿估计方法及系统

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant