CN111707275A - 一种定位方法、装置、电子设备及计算机可读存储介质 - Google Patents

Abstract

本申请披露了一种定位方法，包括：基于初始位置，获取局部语义地图；基于局部语义地图，确定局部平面；确定全局坐标系O₁和局部平面的坐标系O₂的转换矩阵T_p；基于所述转换矩阵T_p，确定车辆定位。本申请通过将平面假设作为一种约束，应用到定位过程中，提高了定位精度以及定位鲁棒性和成功率。

Description

一种定位方法、装置、电子设备及计算机可读存储介质

技术领域

本申请涉及无人驾驶领域，具体涉及一种定位方法、装置、电子 设备及存储介质。

背景技术

随着智能驾驶的发展和普及，各种应用场景也对其关键技术尤其 是定位技术提出了更高的要求。目前主流的定位技术包括GPS定位 技术，基于外部设施如基站的定位技术，包括车载传感器的定位技术 如lslam、vlsam等。上述技术各有优缺点以及应用限制，为了适应越 来越广泛且复杂的应用场景，行业目前越来越重视对环境高级语义信 息的利用以提升定位效果，然而，高级语义具有的稀疏性，只能提供 有限的定位约束，容易出现定位失败的情况。

发明内容

针对现有技术中的定位精度低、失败率高的问题，本申请实施例 提出了一种定位方法、装置、电子设备及计算机可读存储介质。

本申请实施例的第一方面提供了一种定位方法，包括：基于当前 初始定位估计值，获取局部语义地图；基于局部语义地图，确定局部 平面；确定全局坐标系O₁和局部平面的坐标系O₂的转换矩阵T_p；基于 所述转换矩阵T_p，确定车辆定位。

在一些实施例中，所述基于初始位置，获取局部语义地图包括： 若当前初始定位估计值与上一次更新局部语义地图时输入的定位估 计值的差值不大于预设距离阈值时，获取上一次的局部语义地图；若 当前初始定位估计值与上一次更新局部地图时输入的定位估计值的 差值大于预设距离阈值时，更新所述局部语义地图。

在一些实施例中，所述基于局部语义地图，确定局部平面，包括： 基于语义标签获取位于道路平面上的地图元素；根据地图元素的三维 坐标信息，进行平面拟合，获得局部平面。

在一些实施例中，所述局部平面的坐标系r₂的建立过程包括：获 取任意两个参与平面拟合的地图元素在局部平面上的两个投影点坐 标，并基于两个投影点坐标确定基向量l₁；确定局部平面的法向量， 并将法向量确定为基向量l₃；叉乘所述基向量l₁和所述基向量l₃，确 定基向量l₂；将基向量{l₁ l₂ l₃}单位化获得{n₁ n₂ n₃}。

在一些实施例中，所述确定全局坐标系O₁和所述局部平面的坐标 系O₂的转换矩阵T_p，包括：确定{n₁ n₂ n₃}在O₂坐标系下的观测 {n₁′ n₂′ n₃′}；基于公式T_p ^*＝argmin∑||T_pn_i-n_i′||，确定转换矩 阵T_p，其中，i＝1,2,3；其中，

在一些实施例中，所述基于所述转换矩阵T_p，确定车辆定位，包 括：当观测约束的个数不小于预设阈值时，基于所述转换矩阵T_p，将 平面约束加入到观测约束中，进行联合优化，确定车辆定位。

在一些实施例中，所述基于所述转换矩阵T_p，将平面约束加入到 观测约束中，进行联合优化，确定车辆定位，包括：基于方程 T^*＝argmin[∑e(m_i,M_i)+f(T,T_p)]，并结合平面约束

确定车辆定位，其中T^*为车辆定位 结果的最优解，e为与T相关的观测误差函数，m_i与M_i为匹配上的观 测元素，f为平面约束误差函数；T_pv为将T转换到O₂坐标系下得到，E_pv为将T_pv分解为六个自由度的欧拉角得到。

在一些实施例中，所述基于所述转换矩阵T_p，确定车辆定位，包 括：当观测约束的个数小于预设阈值时，将所述观测约束投影到所述 局部平面上，进而确定车辆定位。

在一些实施例中，所述将所述观测约束投影到所述局部平面上， 进而确定车辆定位，包括：基于方程

确定车辆精确定位，其中，T_pv为将T转换到O₂坐标系下得到，e为与T相关的观测误差函数，m_i与M_i为匹配上的观测元素，T^*为车辆定位结果的最优解。

本申请实施例的第二方面提供了一种定位装置，包括：语义地图 获取单元，用于基于初始位置，获取局部语义地图；局部平面确定单 元，用于基于局部语义地图，确定局部平面；转换矩阵确定单元，用 于确定全局坐标系O₁和局部平面的坐标系O₂的转换矩阵T_p；定位确定 单元，用于基于所述转换矩阵T_p，将平面约束加入到观测约束中，进 行联合优化，确定车辆定位。

在一些实施例中，所述语义地图获取单元具体用于：若当前初始 定位估计值与上一次更新局部语义地图时输入的定位估计值的差值 不大于预设距离阈值时，获取上一次的局部语义地图；若当前初始定 位估计值与上一次更新局部地图时输入的定位估计值的差值大于预 设距离阈值时，更新所述局部语义地图。

在一些实施例中，所述局部平面确定单元具体用于：基于语义标 签获取位于道路平面上的地图元素；根据地图元素的三维坐标信息， 进行平面拟合，获得局部平面。

在一些实施例中，所述定位装置还包括坐标系O₂建立单元，所述 坐标系O₂建立单元具体用于：获取任意两个参与平面拟合的地图元素 在局部平面上的两个投影点坐标，并基于两个投影点坐标确定基向量 l₁；确定局部平面的法向量，并将法向量确定为基向量l₃；叉乘所述 基向量l₁和所述基向量l₃，确定基向量l₂；将基向量{l₁ l₂ l₃}单位 化获得{n₁ n₂n₃}。

在一些实施例中，所述转换矩阵确定单元具体用于：确定 {n₁ n₂ n₃}在O₂坐标系下的观测{n₁′ n₂′ n₃′}；基于公式 T_p ^*＝argmin∑||T_pn_i-n_i′||，确定转换矩阵T_p，其中，i＝1,2,3；其 中，

在一些实施例中，所述车辆定位单元具体用于：当观测约束的个 数不小于预设阈值时，基于所述转换矩阵T_p，将平面约束加入到观测 约束中，进行联合优化，确定车辆定位。

在一些实施例中，所述车辆定位单元具体用于：当观测约束的个 数不小于预设阈值时，基于方程T^*＝argmin[∑e(m_i,M_i)+f(T,T_p)]， 并结合平面约束条件

确定车辆定位， 其中T^*为车辆定位结果的最优解，e为与T相关的观测误差函数，m_i与 M_i为匹配上的观测元素，f为平面约束误差函数；T_pv为将T转换到O₂坐标系下得到，E_pv为将T_pv分解为六个自由度的欧拉角得到。

在一些实施例中，所述车辆定位单元具体用于：当观测约束的个 数小于预设阈值时，将所述观测约束投影到所述局部平面上，进而确 定车辆定位。

在一些实施例中，所述车辆定位单元具体用于：当观测约束的个 数小于预设阈值时，基于方程

确定车辆精确定位，其中，T_pv为将T转换到O₂坐标系下得到，为与 T相关的观测误差函数，m_i与M_i为匹配上的观测元素，T^*为车辆定位 结果的最优解。

本申请实施例的第三方面提供了一种电子设备，包括：存储器以 及一个或多个处理器；其中，所述存储器与所述一个或多个处理器通 信连接，所述存储器中存储有可被所述一个或多个处理器执行的指令， 所述指令被所述一个或多个处理器执行时，所述电子设备用于实现如 前述各实施例所述的定位方法。

本申请实施例的第四方面提供了一种计算机可读存储介质，其上 存储有计算机可执行指令，当所述计算机可执行指令被计算装置执行 时，可用来实现如前述各实施例所述的定位方法。

本申请相比于现有技术，具有如下有益效果：

一、充分地利用地图语义，进一步提升定位精度以及定位平顺性 和鲁棒性；

二、在三维定位无法实现的情况下，采用对约束量需求更少的二 维定位方式，提高定位成功率。

附图说明

为了更清楚地说明本申请实施例的技术方案，下面将对实施例描 述中所需要使用的附图作简单的介绍。显而易见地，下面描述中的附 图仅仅是本申请的一些实施例，对于本领域的普通技术人员来讲，在 不付出创造性劳动的前提下，还可以根据这些附图将本申请应用于其 它类似情景。除非从语言环境中显而易见或另做说明，图中相同标号 代表相同结构和操作。

图1是根据本申请的一些实施例所示的定位方法示意图；

图2是根据本申请的一些实施例所示的局部平面确定方法示意 图；

图3是根据本申请的一些实施例所示的一种全局坐标系和局部 平面坐标系示意图；

图4是根据本申请的实施例所示的一种局部平面的坐标系O₂的 建立过程流程图；

图5是根据本申请的一些实施例所示的一种转换矩阵确定方法 流程图；

图6是根据本申请的一些实施例所示的一种定位装置示意图；以 及

图7是根据本申请的一些实施例所示的电子设备示意图。

具体实施方式

在下面的详细描述中，通过示例阐述了本申请的许多具体细节， 以便提供对相关披露的透彻理解。然而，对于本领域的普通技术人员 来讲，本申请显而易见的可以在没有这些细节的情况下实施。应当理 解的是，本申请中使用“系统”、“装置”、“单元”和/或“模块”术 语，是用于区分在顺序排列中不同级别的不同部件、元件、部分或组 件的一种方法。然而，如果其他表达式可以实现相同的目的，这些术 语可以被其他表达式替换。

应当理解的是，当设备、单元或模块被称为“在……上”、“连接 到”或“耦合到”另一设备、单元或模块时，其可以直接在另一设备、 单元或模块上，连接或耦合到或与其他设备、单元或模块通信，或者 可以存在中间设备、单元或模块，除非上下文明确提示例外情形。例 如，本申请所使用的术语“和/或”包括一个或多个相关所列条目的 任何一个和所有组合。

本申请所用术语仅为了描述特定实施例，而非限制本申请范围。 如本申请说明书和权利要求书中所示，除非上下文明确提示例外情形， “一”、“一个”、“一种”和/或“该”等词并非特指单数，也可包括 复数。一般说来，术语“包括”与“包含”仅提示包括已明确标识的 特征、整体、步骤、操作、元素和/或组件，而该类表述并不构成一 个排它性的罗列，其他特征、整体、步骤、操作、元素和/或组件也 可以包含在内。

参看下面的说明以及附图，本申请的这些或其他特征和特点、操 作方法、结构的相关元素的功能、部分的结合以及制造的经济性可以 被更好地理解，其中说明和附图形成了说明书的一部分。然而，可以 清楚地理解，附图仅用作说明和描述的目的，并不意在限定本申请的 保护范围。可以理解的是，附图并非按比例绘制。

本申请中使用了多种结构图用来说明根据本申请的实施例的各 种变形。应当理解的是，前面或下面的结构并不是用来限定本申请。 本申请的保护范围以权利要求为准。

鉴于现有技术中，同时考虑到智能驾驶车辆在大部分场景下都行 驶于平面道路上，而地图元素的平面性也可以看作一种隐藏的语义信 息，因此本申请提出了一种基于局部平面假设的语义定位方法，以更 加充分地利用地图语义，进一步提升定位精度以及定位平顺性和鲁棒 性。

同时，由于平面假设的存在，本申请同时提出了降维定位策略， 在三维定位无法实现的情况下尝试对约束量需求更少的二维定位方 式，提高定位成功率。

图1根据本申请的一些实施例所示的定位方法示意图。所述定位 方法为基于局部语义平面假设的定位方法。

在102中，基于初始位置，获取局部语义地图。语义地图是对处 于“概念/语义空间”中的功能的几何体呈现，它由连接着的线段系 联，形成一个网络。在一些实施例中，无人驾驶车辆中存储的通常是 全局语义地图。例如，所述无人驾驶车辆存储的是一个园区、一个城 市的所有道路的语义地图，而局部语义地图仅是全局语义地图的一部 分，甚至，所述局部语义地图为能够实现车辆定位的最小的局部语义 地图。

在一些实施例中，获取局部语义地图的获取首先需要一个初始定 位估计值，以此为基础更新其附近的局部地图，该初始值可以来自外 部定位源，也可以来自内部的估计算法。具体地，若当前初始定位估 计值与上一次更新局部语义地图时输入的定位估计值的差值不大于 预设距离阈值时，获取上一次的局部语义地图；若当前初始定位估计 值与上一次更新局部地图时输入的定位估计值的差值大于预设距离 阈值时，更新所述局部语义地图。判断是否更新的原因是：如果局部 地图更新频率太高则会降低程序运行效率，因此需要判断是否需要进 行更新。逻辑关系如下式表示：

d(T_estimate-T_last)>T_thresh (1)

其中T_estimate为当前初始定位计值，T_last为上一次更新局部地图时输 入的定位估计值，d为距离度量函数，T_thresh为设定的距离阈值，即 本次是否更新局部地图取决于当前定位与上次更新时定位之间的距 离，如果距离过小，则直接获取上次的局部语义地图。更新局部语义 地图的过程即是从全局语义地图提取位于局部区域范围内的地图信 息，可以使用最近邻相关算法。

在104中，基于局部语义地图，确定局部平面。与102相对应地， 所述基于局部语义地图，确定局部平面也包括两种情形。当不需要更 新时，直接获取上次更新后计算的局部平面方程，以提高运行效率； 当需要更新局部语义地图时，基于重新获取的局部语义地图计算新的 局部平面方程。

在一些实施例中，所述局部平面的计算过程包括：1.选取点集； 2.拟合选取的点集。图2是根据本申请的一些实施例所示的局部平面 确定方法示意图。

在202中，基于语义标签获取位于道路平面上的地图元素。由于 地图具有语义信息，每一个地图元素都具有一个语义标签，由此可以 很方便地获取位于道路平面上的地图元素，例如车道线，人行道等。 地图元素的定义为：定义区域或形状的多边形、定义路线或路径的线 条，以及定义标记或图钉的点。

在一些实施例中，还可以对获取的地图元素进行筛选。例如，可 以使用类似于RANSAC等随机一致性算法对参与平面拟合但与拟合 出的平面距离过大的元素(外点)进行剔除。

在204中，根据地图元素的三维坐标信息，进行平面拟合，获得 局部平面。在一些实施例中，所述局部平面可以基于多个(至少3个) 地图元素对应的三维坐标信息，进行拟合获得，如公式(2)所示：

其中V表示三维空间平面参数矢量，p为平面地图元素三维坐标，上 式求得的最优解即为拟合平面参数。如前所述，获取局部平面后，可 以将平面性作为一种约束，用于精确定位。

在106中，确定全局坐标系O₁和局部平面的坐标系O₂的转换矩阵 T_p。

如图3所示，T_p的作用是通过运算将三维空间坐标转换到二维平 面坐标，以利用平面约束条件。计算T_p的关键是找到位于局部平面P上 的一个坐标系O₂，并计算O₁，O₂之间的变换矩阵，即得到T_p。局部 平面的坐标系O₂的建立过程见图4及其描述。

在108中，基于所述转换矩阵T_p，确定车辆定位。本申请设计了 一种基于平面约束进行定位的方法。值得说明的是，基于转换矩阵T_p， 确定车辆定位的方法可以有多种，本申请中采用的是最优的方式，但 并不用于限定本申请的保护范围，本申请的保护范围以权利要求为准。

图4给出了一种局部平面的坐标系O₂的建立过程流程图。

在402中，获取任意两个参与平面拟合的地图元素在局部平面上 的两个投影点坐标，并基于两个投影点坐标确定基向量l₁。如202中 所述，可以对获取的地图元素进行筛选。所述任意两个参与平面拟合 的地图元素是经过筛选后的地图元素中的任意两个。所述基向量可由 公式(3)确定：

其中k为计算中间标量，V为平面参数矢量，p为平面地图元素三 维坐标，p_proj为p在平面上的投影。

在404中，确定局部平面的法向量，并将法向量确定为基向量l₃。 l₃为平面P的法相量，可直接由矢量V的前三位得到。

在406中，叉乘所述基向量l₁和所述基向量l₃，确定基向量l₂。 由于l₂同时于l₁、l₃正交，因此可由l₁、l₃的叉乘运算得出，如公式(4) 所示：

l₂＝l₁×l₃ (4)

在408中，将基向量{l₁ l₂ l₃}单位化获得{n₁ n₂ n₃}。

值得说明的是，图4仅是给出了一种确定局部平面的坐标系O₂的 方法，其并不用于限定本申请，本领域的技术人员可以在不付出创造 性劳动的情况下，对其进行的任意变形，均在本申请的保护范围内， 本申请的保护范围以权利要求为准。

图5是根据本申请的一些实施例所示的一种转换矩阵确定方法 流程图。

在502中，确定{n₁ n₂ n₃}在O₂坐标系下的观测 {n₁′ n₂′ n₃′}。由于其为一组基向量，则可以简单地如公式(5) 表示出来：

在504中，基于公式T_p ^*＝argmin∑||T_pn_i-n_i′||，确定转换矩 阵T_p，其中，i＝1,2,3。

如108中所述，基于所述转换矩阵T_p，确定车辆定位。为了提高 定位成功率，本申请将车辆定位方式基于观测约束的个数，划分为两 种情形。

当观测约束的个数不小于预设阈值时，进行优化定位。所述优化 定位过程为：基于所述转换矩阵T_p，将平面约束加入到观测约束中， 进行联合优化，确定车辆定位。

当观测约束的个数不小于预设阈值时，进行降维定位。所述降维 定位过程为：将所述观测约束投影到所述局部平面上，进而确定车辆 定位。

具体地，所述优化定位过程为：

将平面约束加入观测约束进行联合优化，如公式(6)所示：

T^*＝argmin[∑e(m_i,M_i)+f(T,T_p)] (6)

其中T^*为车辆定位结果的最优解，e为与T相关的观测误差函数，m_i与 M_i为匹配上的观测元素，f为平面约束误差函数。

为了实现平面约束，首先将T转换到O₂坐标系下得到T_pv，再将其 分解为六个自由度的欧拉角表示E_pv，则E_pv位于局部平面P上的条件 为其中三个自由度为零，如公式(7)所示：

由上述过程可以实现车辆定位的优化计算，如果定位成功，则直 接输出定位结果，流程结束。同样，从理论上说，也可能存在定位失 败的情况，即有效观测约束的数量小于预设阈值时(针对公式7，预 设阈值为4，当采用不同的定位方法时，可以设定不同的预设阈值)， 此时则采用降维定位的策略。

降维定位即利用局部平面信息将三维定位问题转化为二维定位 问题。具体地，所述降维定位过程如公式(8)所示：

其中，T_pv为将T转换到O₂坐标系下得到，e为与T相关的观测误差函 数，m_i与M_i为匹配上的观测元素，T^*为车辆定位结果的最优解。

首先将优化O₁坐标系下T的问题转化为优化O₂坐标系下T_pv的问 题，由于坐标系发生了转换，观测地图元素M_i也需要转换为T_pM_i， 同时由于二维平面假设，还需要将之通过proj函数投影到局部平面P 上，构成二维误差函数。由此计算出的T_pv ^*同样位于p平面上，存在 于O₂坐标系下，最后通过

逆变换到O₁坐标系下即可获得最终的三 维定位结果T^*。由于计算T_pv ^*时使用的是二维误差函数，因此最少只 需要2个有效平面约束即可实现求解过程，在三维定位无法直接实现 的情况下也可以获得有效输出结果。

图6是根据本申请的一些实施例所示的一种定位装置示意图。图 6所示的定位装置用于执行如图1-5所示的定位方法。

如图6所示，所述定位装置600包括语义地图获取单元601、局 部平面确定单元602、转换矩阵确定模块603和定位确定单元604。

其中：

语义地图获取单元601用于基于初始位置，获取局部语义地图；

局部平面确定单元602用于基于局部语义地图，确定局部平面；

转换矩阵确定单元603用于确定全局坐标系O₁和局部平面的坐 标系O₂的转换矩阵T_p；

定位确定单元604用于基于所述转换矩阵T_p，将平面约束加入到 观测约束中，进行联合优化，确定车辆定位。

具体地，所述语义地图获取单元601用于：若当前初始定位估计 值与上一次更新局部语义地图时输入的定位估计值的差值不大于预 设距离阈值时，获取上一次的局部语义地图；若当前初始定位估计值 与上一次更新局部地图时输入的定位估计值的差值大于预设距离阈 值时，更新所述局部语义地图。

具体地，所述局部平面确定单元602用于基于语义标签获取位于 道路平面上的地图元素；根据地图元素的三维坐标信息，进行平面拟 合，获得局部平面。

具体地，所述转换矩阵确定单元603用于确定{n₁ n₂ n₃}在O₂坐标系下的观测{n₁′n₂′ n₃′}；基于公式T_p ^*＝argmin∑||T_pn_i- n_i′||，确定转换矩阵T_p，其中i＝1,2,3，

所述{n₁ n₂ n₃}的确定过程如图4中所述，在此不做展开。

具体地，定位确定单元604用于：当观测约束的个数不小于预设 阈值时，基于所述转换矩阵T_p，将平面约束加入到观测约束中，进行 联合优化，确定车辆定位；当观测约束的个数小于预设阈值时，将所 述观测约束投影到所述局部平面上，进而确定车辆定位。所述优化定 位和降维定位的方法如前文所述，在此不做展开。

图7是适于用来实现根据本申请实施方式的电子设备的结构示 意图。

如图7所示，电子设备700包括中央处理单元(CPU)701，其 可以根据存储在只读存储器(ROM)702中的程序或者从存储部分 708加载到随机访问存储器(RAM)703中的程序而执行上述图1-5 所示的实施方式中的各种处理。在RAM703中，还存储有电子设备 700操作所需的各种程序和数据。CPU701、ROM702以及RAM703 通过总线704彼此相连。输入/输出(I/O)接口705也连接至总线704。

以下部件连接至I/O接口705：包括键盘、鼠标等的输入部分706； 包括诸如阴极射线管(CRT)、液晶显示器(LCD)等以及扬声器等 的输出部分707；包括硬盘等的存储部分708；以及包括诸如LAN卡、 调制解调器等的网络接口卡的通信部分709。通信部分709经由诸如 因特网的网络执行通信处理。驱动器710也根据需要连接至I/O接口 705。可拆卸介质711，诸如磁盘、光盘、磁光盘、半导体存储器等 等，根据需要安装在驱动器710上，以便于从其上读出的计算机程序 根据需要被安装入存储部分708。

特别地，根据本申请的实施方式，上文参考图1-5描述的方法可 以被实现为计算机软件程序。例如，本申请的实施方式包括一种计算 机程序产品，其包括有形地包含在及其可读介质上的计算机程序，所 述计算机程序包含用于执行图1-5的方法的程序代码。在这样的实施 方式中，该计算机程序可以通过通信部分709从网络上被下载和安装， 和/或从可拆卸介质711被安装。

附图中的流程图和框图，图示了按照本申请各种实施方式的装置、 方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点 上，路程图或框图中的每个方框可以代表一个模块、程序段或代码的 一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现 规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现 中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。 例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时 也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是， 框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的系统来实现， 或者可以用专用硬件与计算机指令的组合来实现。

描述于本申请实施方式中所涉及到的单元或模块可以通过软件 的方式实现，也可以通过硬件的方式来实现。所描述的单元或模块也 可以设置在处理器中，这些单元或模块的名称在某种情况下并不构成 对该单元或模块本身的限定。

作为另一方面，本申请还提供了一种计算机可读存储介质，该计 算机可读存储介质可以是上述实施方式中所述装置中所包含的计算 机可读存储介质；也可以是单独存在，未装配入设备中的计算机可读 存储介质。计算机可读存储介质存储有一个或者一个以上程序，所述 程序被一个或者一个以上的处理器用来执行描述于本申请的方法。

综上所述，本申请提出了一种定位方法、装置、电子设备及其计 算机可读存储介质。本申请实施例通过将平面约束与观测约束结合， 提升定位精度以及定位平顺性和鲁棒性；并在观测约束个数小于预设 阈值时，将三维定位问题转化为二位定位问题，进行降维定位。

应当理解的是，本申请的上述具体实施方式仅仅用于示例性说明 或解释本申请的原理，而不构成对本申请的限制。因此，在不偏离本 申请的精神和范围的情况下所做的任何修改、等同替换、改进等，均 应包含在本申请的保护范围之内。此外，本申请所附权利要求旨在涵 盖落入所附权利要求范围和边界、或者这种范围和边界的等同形式内 的全部变化和修改例。

Claims (10)

1.一种定位方法，其特征在于，包括：

基于当前初始定位估计值，获取局部语义地图；

基于局部语义地图，确定局部平面；

确定全局坐标系O₁和局部平面的坐标系O₂的转换矩阵T_p；

基于所述转换矩阵T_p，确定车辆定位。

2.如权利要求1所述的方法，其特征在于，所述基于初始位置，获取局部语义地图包括：

若当前初始定位估计值与上一次更新局部语义地图时输入的定位估计值的差值不大于预设距离阈值时，获取上一次的局部语义地图；

若当前初始定位估计值与上一次更新局部地图时输入的定位估计值的差值大于预设距离阈值时，更新所述局部语义地图。

3.如权利要求1所述的方法，其特征在于，所述基于局部语义地图，确定局部平面，包括：

基于语义标签获取位于道路平面上的地图元素；

根据地图元素的三维坐标信息，进行平面拟合，获得局部平面。

4.如权利要求1所述的方法，其特征在于，所述局部平面的坐标系O₂的建立过程包括：

获取任意两个参与平面拟合的地图元素在局部平面上的两个投影点坐标，并基于两个投影点坐标确定基向量l₁；

确定局部平面的法向量，并将法向量确定为基向量l₃；

叉乘所述基向量l₁和所述基向量l₃，确定基向量l₂；

将基向量{l₁ l₂ l₃}单位化获得{n₁ n₂ n₃}。

5.如权利要求4所述的方法，其特征在于，所述确定全局坐标系O₁和所述局部平面的坐标系O₂的转换矩阵T_p，包括：

确定{n₁ n₂ n₃}在O₂坐标系下的观测{n₁′ n₂′ n₃′}；

基于公式T_p ^*＝argmin∑||T_pn_i-n_i′||，确定转换矩阵T_p，其中，i＝1，2，3；

其中，

6.如权利要求1所述的方法，其特征在于，所述基于所述转换矩阵T_p，确定车辆定位，包括：

当观测约束的个数不小于预设阈值时，基于所述转换矩阵T_p，将平面约束加入到观测约束中，进行联合优化，确定车辆定位。

7.如权利要求6所述的方法，其特征在于，所述基于所述转换矩阵T_p，将平面约束加入到观测约束中，进行联合优化，确定车辆定位，包括：

基于方程T^*＝argmin[∑e(m_i，M_i)+f(T，T_p)]，并结合平面约束

确定车辆定位，其中T^*为车辆定位结果的最优解，e为与T相关的观测误差函数，m_i与M_i为匹配上的观测元素，f为平面约束误差函数；T_pv为将T转换到O₂坐标系下得到，E_pv为将T_pv分解为六个自由度的欧拉角得到。

8.如权利要求1所述的方法，其特征在于，所述基于所述转换矩阵T_p，确定车辆定位，包括：

当观测约束的个数小于预设阈值时，将所述观测约束投影到所述局部平面上，进而确定车辆定位。

9.如权利要求8所述的方法，其特征在于，所述将所述观测约束投影到所述局部平面上，进而确定车辆定位，包括：

基于方程

确定车辆精确定位，其中，T_pv为将T转换到O₂坐标系下得到，e为与T相关的观测误差函数，m_i与M_i为匹配上的观测元素，T^*为车辆定位结果的最优解。

10.一种定位装置，其特征在于，包括：

语义地图获取单元，用于基于初始位置，获取局部语义地图；

局部平面确定单元，用于基于局部语义地图，确定局部平面；

转换矩阵确定单元，用于确定全局坐标系O₁和局部平面的坐标系O₂的转换矩阵T_p；

定位确定单元，用于基于所述转换矩阵T_p，将平面约束加入到观测约束中，进行联合优化，确定车辆定位。

Images