CN113391338A - 紧耦合轨迹修复方法、装置、设备、介质及产品 - Google Patents

Abstract

本发明实施例提供一种紧耦合轨迹修复方法、装置、设备、介质及产品，该方法包括：获取由紧耦合位姿点组成的紧耦合轨迹和由松耦合位姿点组成的松耦合轨迹；确定所述紧耦合轨迹中的异常轨迹段和所述松耦合轨迹中与异常轨迹段匹配的补偿轨迹段；所述异常轨迹段包括多个异常位姿点；根据所述紧耦合轨迹、所述异常轨迹段和所述补偿轨迹段修复所述异常轨迹段。本发明实施例的紧耦合轨迹修复方法，由于松耦合轨迹GNSS定位的解算相对独立，与紧耦合轨迹之间的精确度相差也较小，从而结合松耦合轨迹的补偿轨迹段修复后的紧耦合轨迹不会出现轨迹信息错误，同时，精确度较高。

Description

紧耦合轨迹修复方法、装置、设备、介质及产品

技术领域

本发明实施例涉及自动驾驶技术领域，尤其涉及一种紧耦合轨迹修复方法、装置、设备、介质及产品。

背景技术

高精度地图 (英文简称为：HD Map，英文全称为：High Definition Map)是服务于自动驾驶的地图，在辅助定位、增强感知能力、路径规划等方面都有着重要作用，而获取车体精准运动轨迹是建立高质量的高精度地图的必要前提。利用高精度惯导以及GNSS（英文全称为：Global Navigation Satellite System，中文为：全球导航卫星系统）定位信息的组合导航系统可以对车体运动轨迹进行解算。

组合导航系统一般有两种输出模式：松耦合和紧耦合模式，一般情况下紧耦合可以解算出更高精度的轨迹，但是对于高楼林立的城市场景，由于建筑物的遮挡，卫星信号容易丢失，紧耦合解算的轨迹容易出现一些异常跳点，从而导致紧耦合输出的轨迹信息出现错误。

发明内容

本发明提供一种紧耦合轨迹修复方法、装置、设备、介质及产品，用以解决目前紧耦合解算的轨迹容易出现一些异常跳点，从而导致紧耦合输出的轨迹信息出现错误的问题。

本发明实施例第一方面提供一种紧耦合轨迹修复方法，包括：

获取由紧耦合位姿点组成的紧耦合轨迹和由松耦合位姿点组成的松耦合轨迹；

确定所述紧耦合轨迹中的异常轨迹段和所述松耦合轨迹中与异常轨迹段匹配的补偿轨迹段；所述异常轨迹段包括多个异常位姿点；

根据所述紧耦合轨迹、所述异常轨迹段和所述补偿轨迹段修复所述异常轨迹段。

可选的，如上所述的方法，所述确定所述紧耦合轨迹中的异常轨迹段和所述松耦合轨迹中与异常轨迹段匹配的补偿轨迹段，包括：

根据轨迹平滑性检测算法确定所述紧耦合轨迹中的异常位姿点；

确定各所述异常位姿点之间的相对距离；

将所述异常位姿点和以该异常位姿点为中心间隔预设距离内的其他异常位姿点确定为异常轨迹段；其中，在异常轨迹段中该异常位姿点和其他异常位姿点之间的相对距离小于或等于预设距离；

从所述松耦合轨迹中确定与所述异常轨迹段匹配的补偿轨迹段。

可选的，如上所述的方法，所述异常位姿点包括位姿参数；

所述确定各所述异常位姿点之间的相对距离，包括：

以所述紧耦合位姿点中处于中心位置的中心位姿点为坐标原点建立局部坐标系；

将各异常位姿点的坐标转换到局部坐标系下得到各个异常位姿点在局部坐标系的相对坐标；

根据各所述异常位姿点的相对坐标确定所述相对距离。

可选的，如上所述的方法，所述根据所述紧耦合轨迹、所述异常轨迹段和所述补偿轨迹段修复所述异常轨迹段，包括：

根据所述紧耦合位姿点和所述异常轨迹段确定所述异常轨迹段的两端对应的两个延伸轨迹段；其中一个延伸轨迹段的起点为所述异常轨迹段的两端中第一端的异常位姿点，终点为所述第一端的异常位姿点沿紧耦合轨迹延伸设定距离后的一紧耦合位姿点；另一个延伸轨迹段的起点为所述异常轨迹段的两端中第二端的异常位姿点，终点为所述第二端的异常位姿点沿着紧耦合轨迹延伸设定距离后的一紧耦合位姿点；

根据所述异常轨迹段、两个延伸轨迹段、所述补偿轨迹段以及预设的非线性优化算法修复所述异常轨迹段。

可选的，如上所述的方法，所述异常轨迹段包括异常位姿点的位姿参数；所述延伸轨迹段包括紧耦合位姿点的位姿参数；所述补偿轨迹段包括松耦合位姿点的位姿参数；

所述根据所述异常轨迹段、两个延伸轨迹段、所述补偿轨迹段以及预设的非线性优化算法修复所述异常轨迹段，包括：

根据所述异常轨迹段的各异常位姿点的位姿参数构建待优化位姿矩阵集合；

根据所述两个延伸轨迹段的各紧耦合位姿点的位姿参数构建单边约束矩阵集合；

根据所述待优化位姿矩阵集合和所述补偿轨迹段的各松耦合位姿点的位姿参数构建双边约束矩阵集合；

将所述待优化位姿矩阵集合、单边约束矩阵集合和双边约束矩阵集合输入预设的非线性优化算法；

采用所述预设的非线性优化算法，以单边约束矩阵集合和双边约束矩阵集合为约束，对所述待优化位姿矩阵集合进行优化，以获得优化后的位姿矩阵集合；

根据所述优化后的位姿矩阵集合，确定并输出修复后的异常轨迹段。

可选的，如上所述的方法，所述紧耦合位姿点和松耦合位姿点都包括以下位姿参数：

纬度、经度、高度、横滚角、俯仰角和航向角。

本发明实施例第二方面提供一种紧耦合轨迹修复装置，包括：

获取模块，用于获取由紧耦合位姿点组成的紧耦合轨迹和由松耦合位姿点组成的松耦合轨迹；

确定模块，用于确定所述紧耦合轨迹中的异常轨迹段和所述松耦合轨迹中与异常轨迹段匹配的补偿轨迹段；所述异常轨迹段包括多个异常位姿点；

修复模块，用于根据所述紧耦合轨迹、所述异常轨迹段和所述补偿轨迹段修复所述异常轨迹段。

可选的，如上所述的装置，所述确定模块具体用于：

根据轨迹平滑性检测算法确定所述紧耦合轨迹中的异常位姿点；确定各所述异常位姿点之间的相对距离；将所述异常位姿点和以该异常位姿点为中心间隔预设距离内的其他异常位姿点确定为异常轨迹段；其中，在异常轨迹段中该异常位姿点和其他异常位姿点之间的相对距离小于或等于预设距离；从所述松耦合轨迹中确定与所述异常轨迹段匹配的补偿轨迹段。

可选的，如上所述的装置，所述异常位姿点包括位姿参数；

所述确定模块在确定各所述异常位姿点之间的相对距离时，具体用于：

以所述紧耦合位姿点中处于中心位置的中心位姿点为坐标原点建立局部坐标系；将各异常位姿点的坐标转换到局部坐标系下得到各个异常位姿点在局部坐标系的相对坐标；根据各所述异常位姿点的相对坐标确定所述相对距离。

可选的，如上所述的装置，所述修复模块具体用于：

根据所述紧耦合位姿点和所述异常轨迹段确定所述异常轨迹段的两端对应的两个延伸轨迹段；其中一个延伸轨迹段的起点为所述异常轨迹段的两端中第一端的异常位姿点，终点为所述第一端的异常位姿点沿紧耦合轨迹延伸设定距离后的一紧耦合位姿点；另一个延伸轨迹段的起点为所述异常轨迹段的两端中第二端的异常位姿点，终点为所述第二端的异常位姿点沿着紧耦合轨迹延伸设定距离后的一紧耦合位姿点。

可选的，如上所述的装置，所述异常轨迹段包括异常位姿点的位姿参数；所述延伸轨迹段包括紧耦合位姿点的位姿参数；所述补偿轨迹段包括松耦合位姿点的位姿参数；

所述修复模块在根据所述异常轨迹段、两个延伸轨迹段、所述补偿轨迹段以及预设的非线性优化算法修复所述异常轨迹段时，具体用于：

根据所述异常轨迹段的各异常位姿点的位姿参数构建待优化位姿矩阵集合；根据所述两个延伸轨迹段的各紧耦合位姿点的位姿参数构建单边约束矩阵集合；根据所述待优化位姿矩阵集合和所述补偿轨迹段的各松耦合位姿点的位姿参数构建双边约束矩阵集合；将所述待优化位姿矩阵集合、单边约束矩阵集合和双边约束矩阵集合输入预设的非线性优化算法；采用所述预设的非线性优化算法，以单边约束矩阵集合和双边约束矩阵集合为约束，对所述待优化位姿矩阵集合进行优化，以获得优化后的位姿矩阵集合；根据所述优化后的位姿矩阵集合，确定并输出修复后的异常轨迹段。

可选的，如上所述的装置，所述紧耦合位姿点和松耦合位姿点都包括以下位姿参数：

纬度、经度、高度、横滚角、俯仰角和航向角。

本发明实施例第三方面提供一种电子设备，包括：存储器，处理器；

存储器；用于存储所述处理器可执行指令的存储器；

其中，所述处理器被配置为由所述处理器执行第一方面任一项所述的紧耦合轨迹修复方法。

本发明实施例第四方面提供一种计算机可读存储介质，所述计算机可读存储介质中存储有计算机执行指令，所述计算机执行指令被处理器执行时用于实现第一方面任一项所述的紧耦合轨迹修复方法。

本发明实施例第五方面提供一种计算机程序产品，包括计算机程序，该计算机程序被处理器执行时实现第一方面任一项所述的紧耦合轨迹修复方法。

本发明实施例提供的一种紧耦合轨迹修复方法、装置、设备、介质及产品，该方法包括：获取由紧耦合位姿点组成的紧耦合轨迹和由松耦合位姿点组成的松耦合轨迹；确定所述紧耦合轨迹中的异常轨迹段和所述松耦合轨迹中与异常轨迹段匹配的补偿轨迹段；所述异常轨迹段包括多个异常位姿点；根据所述紧耦合轨迹、所述异常轨迹段和所述补偿轨迹段修复所述异常轨迹段。本发明实施例的紧耦合轨迹修复方法，首先获取紧耦合轨迹和松耦合轨迹，并从紧耦合轨迹中确定异常轨迹段，从松耦合轨迹中确定与异常轨迹段匹配的补偿轨迹段。最后根据所述紧耦合轨迹、所述异常轨迹段和所述补偿轨迹段修复所述异常轨迹段。由于松耦合轨迹GNSS定位的解算相对独立，与紧耦合轨迹之间的精确度相差也较小，从而结合松耦合轨迹的补偿轨迹段修复后的紧耦合轨迹不会出现轨迹信息错误，同时，精确度较高。

附图说明

此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本发明的实施例，并与说明书一起用于解释本发明的原理。

图1为可以实现本发明实施例的紧耦合轨迹修复方法的场景图；

图2为本发明第一实施例提供的紧耦合轨迹修复方法的流程示意图；

图3为本发明第二实施例提供的紧耦合轨迹修复方法的流程示意图；

图4为本发明第二实施例提供的紧耦合轨迹修复方法中轨迹修复示意图；

图5为本发明第二实施例提供的紧耦合轨迹修复方法的步骤207的流程示意图；

图6为本发明第三实施例提供的紧耦合轨迹修复装置的结构示意图；

图7为本发明第四实施例提供的电子设备的结构示意图。

通过上述附图，已示出本发明明确的实施例，后文中将有更详细的描述。这些附图和文字描述并不是为了通过任何方式限制本发明构思的范围，而是通过参考特定实施例为本领域技术人员说明本发明的概念。

具体实施方式

这里将详细地对示例性实施例进行说明，其示例表示在附图中。下面的描述涉及附图时，除非另有表示，不同附图中的相同数字表示相同或相似的要素。以下示例性实施例中所描述的实施方式并不代表与本发明相一致的所有实施方式。相反，它们仅是与如所附权利要求书中所详述的、本发明的一些方面相一致的装置和方法的例子。

下面以具体地实施例对本发明的技术方案进行详细说明。下面这几个具体的实施例可以相互结合，对于相同或相似的概念或过程可能在某些实施例中不再赘述。下面将结合附图，对本发明的实施例进行描述。

为了清楚理解本申请的技术方案，首先对现有技术的方案进行详细介绍。目前自动驾驶领域中，高精度地图是服务于自动驾驶的地图，高精度地图一般需要通过组合导航系统获取车体精准运动轨迹。组合导航系统可以根据车辆本身的运动轨迹进行进一步的结算，从而输出解算后的运动轨迹。一般情况下，组合导航系统存在两种输出模式：松耦合和紧耦合模式，紧耦合可以解算出更高精度的轨迹，但是对于高楼林立的城市场景，由于建筑物的遮挡，卫星信号容易丢失，紧耦合解算的轨迹容易出现一些异常跳点，而松耦合由于高精度惯导与GNSS定位的解算相对独立，对应阶段可能输出相对平滑的轨迹。因而，目前紧耦合解算的轨迹容易出现一些异常跳点，从而导致紧耦合输出的轨迹信息出现错误。

所以针对现有技术中的紧耦合模式解算的轨迹容易出现一些异常跳点，从而导致紧耦合输出的轨迹信息出现错误的问题，发明人在研究中发现，为了解决该问题，可以通过松耦合轨迹中与紧耦合轨迹异常区域对应的轨迹段对紧耦合轨迹做补偿修复，从而修复紧耦合轨迹出现的轨迹信息错误，提高精确度。具体的，首先获取由紧耦合位姿点组成的紧耦合轨迹和由松耦合位姿点组成的松耦合轨迹，然后确定紧耦合轨迹中的异常轨迹段和松耦合轨迹中与异常轨迹段匹配的补偿轨迹段。其中，异常轨迹段包括多个异常位姿点。最后根据紧耦合轨迹、异常轨迹段和补偿轨迹段修复异常轨迹段。本发明实施例的紧耦合轨迹修复方法，首先获取紧耦合轨迹和松耦合轨迹，并从紧耦合轨迹中确定异常轨迹段，从松耦合轨迹中确定与异常轨迹段匹配的补偿轨迹段。最后根据紧耦合轨迹、异常轨迹段和补偿轨迹段修复异常轨迹段。由于松耦合轨迹GNSS定位的解算相对独立，与紧耦合轨迹之间的精确度相差也较小，从而结合松耦合轨迹的补偿轨迹段修复后的紧耦合轨迹不会出现轨迹信息错误，同时，精确度较高。

发明人基于上述的创造性发现，提出了本申请的技术方案。

下面对本发明实施例提供的紧耦合轨迹修复方法的应用场景进行介绍。如图1所示，其中，1为第一电子设备，2为第二电子设备。第一电子设备1和第二电子设备2可以设置在车辆上。本发明实施例提供的紧耦合轨迹修复方法对应的应用场景的网络架构中包括：第一电子设备1和第二电子设备2。第一电子设备1可以是车辆的车载终端。第二电子设备2作为高精地图数据库进行使用。第二电子设备2存储有车辆的紧耦合轨迹和松耦合轨迹。同时，在其他的应用场景中，第二电子设备2也可以与第一电子设备1合并，使第一电子设备1中具有高精地图数据库功能。在进行紧耦合轨迹修复时，第一电子设备1从第二电子设备2中获取紧耦合轨迹和松耦合轨迹，该预设条件可以按照实际需求进行设置，比如预设空间范围或预设时间范围等。然后第一电子设备1从紧耦合轨迹中确定异常轨迹段，从松耦合轨迹中确定与异常轨迹段匹配的补偿轨迹段。第一电子设备1根据紧耦合轨迹、异常轨迹段和补偿轨迹段修复异常轨迹段。在修复完成后，可以输出紧耦合轨迹至第一电子设备1的显示屏幕上进行显示，或者输出至其他的电子设备。由于松耦合轨迹GNSS定位的解算相对独立，与紧耦合轨迹之间的精确度相差也较小，从而结合松耦合轨迹的补偿轨迹段修复后的紧耦合轨迹不会出现轨迹信息错误，同时，精确度较高。

下面结合说明书附图对本发明实施例进行介绍。

图2为本发明第一实施例提供的紧耦合轨迹修复方法的流程示意图，如图2所示，本实施例中，本发明实施例的执行主体为紧耦合轨迹修复装置，该紧耦合轨迹修复装置可以集成在电子设备中。则本实施例提供的紧耦合轨迹修复方法包括以下几个步骤：

步骤S101，获取由紧耦合位姿点组成的紧耦合轨迹和由松耦合位姿点组成的松耦合轨迹。

本实施例中，获取紧耦合轨迹和松耦合轨迹可以按照空间范围、时间范围来获取。空间范围可以是从某坐标至另一个坐标之间的紧耦合轨迹和松耦合轨迹，时间范围可以是从某时刻至另一个时刻之间的紧耦合轨迹和松耦合轨迹，对此本实施例不作限定。

紧耦合位姿点和松耦合位姿点都属于位姿点，位姿点是具有位姿参数的点，位姿参数包括位置信息和姿态信息，位置信息可以包括经纬度信息、高度信息等，姿态信息可以包括方位角、航向角等与方向有关的信息。

紧耦合轨迹由多个连续的紧耦合位姿点构成，同理，松耦合轨迹也由多个连续的松耦合位姿点组成。

步骤S102，确定紧耦合轨迹中的异常轨迹段和松耦合轨迹中与异常轨迹段匹配的补偿轨迹段。异常轨迹段包括多个异常位姿点。

本实施例中，异常轨迹段可以由紧耦合轨迹中多个连续异常位姿点组成，该连续异常位姿点指的是每两个异常位姿点之间不会间隔有新的异常位姿点，可以间隔正常的紧耦合位姿点。

与异常轨迹段匹配是指与异常轨迹段在位姿参数上是匹配的，比如异常轨迹段为车辆在时间上从上午10点至上午10点半的所有紧耦合位姿点集合，则补偿轨迹段也为时间上从上午10点至上午10点半的所有松耦合位姿点集合。

在进行修复时，松耦合位姿点也可能出现异常情况，当松耦合位姿点出现异常时，可以直接输出车辆的原始轨迹，而不对异常轨迹段进行修复处理。

步骤S103，根据紧耦合轨迹、异常轨迹段和补偿轨迹段修复异常轨迹段。

本实施例中，可以通过紧耦合轨迹、异常轨迹段和补偿轨迹段直接修复异常轨迹段，也可以通过预设的算法修复异常轨迹段，本实施例对此不作限定。

本发明实施例提供的一种紧耦合轨迹修复方法，该方法包括：获取由紧耦合位姿点组成的紧耦合轨迹和由松耦合位姿点组成的松耦合轨迹。确定紧耦合轨迹中的异常轨迹段和松耦合轨迹中与异常轨迹段匹配的补偿轨迹段。异常轨迹段包括多个异常位姿点。根据紧耦合轨迹、异常轨迹段和补偿轨迹段修复异常轨迹段。

本发明实施例的紧耦合轨迹修复方法，首先获取紧耦合轨迹和松耦合轨迹，并从紧耦合轨迹中确定异常轨迹段，从松耦合轨迹中确定与异常轨迹段匹配的补偿轨迹段。最后根据紧耦合轨迹、异常轨迹段和补偿轨迹段修复异常轨迹段。由于松耦合轨迹GNSS定位的解算相对独立，与紧耦合轨迹之间的精确度相差也较小，从而结合松耦合轨迹的补偿轨迹段修复后的紧耦合轨迹不会出现轨迹信息错误，同时，精确度较高。

图3为本发明第二实施例提供的紧耦合轨迹修复方法的流程示意图，如图3所示，本实施例提供的紧耦合轨迹修复方法，是在本发明上一实施例提供的紧耦合轨迹修复方法的基础上，对各个步骤进行了进一步的细化。则本实施例提供的紧耦合轨迹修复方法包括以下步骤。

步骤S201，获取由紧耦合位姿点组成的紧耦合轨迹和由松耦合位姿点组成的松耦合轨迹。

本实施例中，步骤201的实现方式与本发明上一实施例中的步骤101的实现方式类似，在此不再一一赘述。

步骤S202，根据轨迹平滑性检测算法确定紧耦合轨迹中的异常位姿点。

本实施例中，轨迹平滑性检测算法可以采用常用的轨迹平滑性检测算法，比如多项式平滑检测算法、卷积平滑算法等，本实施例对此不作限定。具体的，可以取出第 i个紧耦合位姿点为中心点，建立中心点与其左右临近k个点的集合，针对该集合可以使用一些轨迹平滑性检测方法判断中心点是否出现异常，比如跳变、断连等，从而将跳变、断连的紧耦合位姿点确定为异常位姿点。

步骤S203，确定各异常位姿点之间的相对距离。

本实施例中，异常位姿点只有自身的坐标，为了方便后续的修复处理，需要确定异常轨迹段，因而，可以首先确定各异常位姿点之间的相对距离，从而更效率的划分出对应的异常轨迹段。

可选的，本实施例中，异常位姿点包括位姿参数。

确定各异常位姿点之间的相对距离，包括：

以紧耦合位姿点中处于中心位置的中心位姿点为坐标原点建立局部坐标系。

将各异常位姿点的坐标转换到局部坐标系下得到各个异常位姿点在局部坐标系的相对坐标。

根据各异常位姿点的相对坐标确定相对距离。

本实施例中，确定各异常位姿点之间的相对距离时，可以首先以紧耦合位姿点中处于中心位置的中心位姿点为坐标原点建立局部坐标系，每个紧耦合位姿点包括纬度、经度、高度、横滚角、俯仰角、航向角等位姿参数。

假设输入的异常轨迹段为

，轨迹 中的位置姿态信息按照时间戳顺序依次存储，其中

分别表示第i个异 常轨迹段的位姿点对应的时间戳、横滚角、俯仰角、航向角、经度、纬度以及高度，n表示该轨 迹包含的位姿点总数。位置姿态信息包括位置信息和姿态信息，位置信息包括纬度、经度、 高度，姿态信息包括横滚角、俯仰角、航向角。紧耦合位姿点的原始坐标为WGS84坐标，该坐 标不利于相对位移的计算，从而需要将各位姿点的坐标从WGS84坐标转到局部坐标系，坐标 通常使用纬度、经度和高度来表示。

上述输入轨迹也可以转换为：

。其中，R_i，t_i分 别表示第i个点对应的旋转矩阵和位移向量，R_i的表达式与欧拉角旋转顺序的定义有关，常 见的一种表达式为

，

，

表示绕z轴旋转

的旋 转矩阵，

表示绕x轴旋转

的旋转矩阵，

表示绕y轴旋转

的旋转矩阵。由于 需要对轨迹中的每个位姿点进行异常检测与修复，不失一般性，我们以第i个点为例进行方 案说明，其他位姿点则同理类推。

异常位姿点A在以中心位姿点B为原点的局部坐标系下的坐标可通过预设的坐标转换算法计算，坐标通常使用纬度、经度和高度来表示。预设的坐标转换算法如下所示：

其中，

，

分别是参考点坐标

（分别代表纬度、经度和高 度）处对应的子午圈主曲率半径以及卯酉圈主曲率半径。

可选的，本实施例中，紧耦合位姿点和松耦合位姿点都包括以下位姿参数：

纬度、经度、高度、横滚角、俯仰角和航向角。

步骤S204，将异常位姿点和以该异常位姿点为中心间隔预设距离内的其他异常位姿点确定为异常轨迹段。其中，在异常轨迹段中该异常位姿点和其他异常位姿点之间的相对距离小于或等于预设距离。

本实施例中，由于GNSS失效、受遮挡的情况具有连续性，轨迹异常点一般是非孤立的，经常成段出现，因此需要对检测到的异常点的临近点同样进行异常点检测，将相邻距离小于一定阈值的异常点聚合为异常段进行统一处理。同时为了保证与临近的正常轨迹的连续性，可以根据异常段的长度自适应地将异常段往两端扩展一定的距离。确定出的异常轨迹段可以是多个，优选的是确定出的异常轨迹段相互之间不存在重叠部分，从而降低修复所占的资源。

步骤S205，从松耦合轨迹中确定与异常轨迹段匹配的补偿轨迹段。

本实施例中，步骤205的实现方式与本发明上一实施例中的步骤102的实现方式类似，在此不再一一赘述。

步骤S206，根据紧耦合位姿点和异常轨迹段确定异常轨迹段的两端对应的两个延伸轨迹段。其中一个延伸轨迹段的起点为异常轨迹段的两端中第一端的异常位姿点，终点为第一端的异常位姿点沿紧耦合轨迹延伸设定距离后的一紧耦合位姿点；另一个延伸轨迹段的起点为异常轨迹段的两端中第二端的异常位姿点，终点为第二端的异常位姿点沿着紧耦合轨迹延伸设定距离后的一紧耦合位姿点。

本实施例中，假设异常轨迹段的两端为a位姿点和b位姿点，则其中一个延伸轨迹段为从a位姿点向非异常轨迹段的方向延伸一定距离所形成的轨迹段，而另一个延伸轨迹段为b位姿点向非异常轨迹段的方向延伸一定距离所形成的轨迹段。

构建延伸轨迹段主要是为了在修复时，满足修复后的异常轨迹段与紧耦合轨迹的连接关系，使修复后的异常轨迹段与延伸轨迹段能够完全匹配。

步骤S207，根据异常轨迹段、两个延伸轨迹段、补偿轨迹段以及预设的非线性优化算法修复异常轨迹段。

本实施例中，可以通过两个延伸轨迹段、补偿轨迹段作为约束条件，再根据预设的非线性优化算法对异常轨迹段进行修复。预设的非线性优化算法可以是常用的高斯牛顿法等。

修复后的紧耦合轨迹如图4所示，从图4中可看出，紧耦合轨迹中异常区域为图中虚线所示的向松耦合轨迹方向突出的区域，在该区域中紧耦合轨迹出现了明显的跳点，由多个跳点即异常位姿点组成了异常轨迹段，而修复后的轨迹如图中所示，通过松耦合轨迹的相对位姿约束关系恢复了平滑性，提高了该段轨迹的精度。

图5为本发明第二实施例提供的紧耦合轨迹修复方法的步骤207的流程示意图，如图5所示，本实施例提供的紧耦合轨迹修复方法，是在本发明上一实施例提供的紧耦合轨迹修复方法的基础上，对步骤207进行了进一步的细化。则本实施例提供的紧耦合轨迹修复方法包括以下步骤。

需要说明的是，异常轨迹段包括异常位姿点的位姿参数。延伸轨迹段包括紧耦合位姿点的位姿参数。补偿轨迹段包括松耦合位姿点的位姿参数。

步骤S2071，根据异常轨迹段的各异常位姿点的位姿参数构建待优化位姿矩阵集合。

步骤S2072，根据两个延伸轨迹段的各紧耦合位姿点的位姿参数构建单边约束矩阵集合。

步骤S2073，根据待优化位姿矩阵集合和补偿轨迹段的各松耦合位姿点的位姿参数构建双边约束矩阵集合。

步骤S2074，将待优化位姿矩阵集合、单边约束矩阵集合和双边约束矩阵集合输入预设的非线性优化算法。

步骤S2075，采用预设的非线性优化算法，以单边约束矩阵集合和双边约束矩阵集合为约束，对待优化位姿矩阵集合进行优化，以获得优化后的位姿矩阵集合。

步骤S2076，根据优化后的位姿矩阵集合，确定并输出修复后的异常轨迹段。

本实施例中，可以将待优化位姿矩阵集合整体作为变量，待优化位姿矩阵集合中每个位姿点的位姿参数都是待优化变量。如果将待优化位姿矩阵集合、单边约束矩阵集合以及双边约束矩阵集合统称为位姿图，则可以通过图优化的形式，通过图模型来表达一个非线性最小二乘优化问题。此时，待优化位姿矩阵集合中每个位姿点的位姿参数构成位姿图的每一个顶点，记待优化位姿矩阵集合为：

其中，T_i表示第i个点对应的待优化位姿矩阵，

，m表示该待优 化的异常轨迹段中的位姿点总数。

添加单边约束。为了保证异常段修复后与临近段的平滑性，对异常段进行了扩展，得到两个延伸轨迹段。其中一个延伸轨迹段的起点为异常轨迹段的两端中其中一端的异常位姿点，终点为其中一端的异常位姿点沿紧耦合轨迹延伸后的某个紧耦合位姿点；另一个延伸轨迹段的起点为异常轨迹段的两端中另一端的异常位姿点，终点为另一端的异常位姿点沿着紧耦合轨迹延伸后的某个紧耦合位姿点。如图4所示，其中一个延伸轨迹段起点为紧耦合异常区域中异常轨迹段中左下端的异常位姿点-标线处，并沿着紧耦合轨迹往左下方向延伸一段距离。另一个延伸轨迹段起点为异常轨迹段中右上端的异常位姿点-标线处，并沿着紧耦合轨迹往右上方向延伸一段距离。对两段异常段中的点利用紧耦合数据添加单边约束，记单边约束矩阵集合为：

其中，

表示紧耦合轨迹中的第i个点的位姿矩阵的逆矩阵，p表示两个延伸轨 迹段中的位姿点数量和。

添加双边约束。双边约束使用无异常的松耦合轨迹中对应的补偿轨迹段为待优化变量构建相邻位姿关系，记双边约束矩阵集合为：

其中，

表示松耦合轨迹中补偿轨迹段的第i个点的位姿矩阵的逆矩阵，

表示松耦合轨迹中补偿轨迹段的第i+1个点的位姿矩阵，

表示第i个点对应的 待优化位姿矩阵的逆矩阵，

表示第i+1个点对应的待优化位姿矩阵，q表示异常轨迹段 中的位姿点数量。

当构建完成后，可以选择高斯牛顿法等非线性优化算法对位姿图进行求解，从而输出修复后的异常轨迹段的位姿点，如图4所示，修复后的轨迹呈一条直线，与紧耦合轨迹中非异常部分匹配度较高，也不会包含方向和位置都严重脱离紧耦合轨迹的异常位姿点。而异常轨迹段呈向松耦合轨迹突出的凸面形状，包含多个方向和位置都严重脱离紧耦合轨迹的异常位姿点。图4中为了更形象的展示异常轨迹段和修复后轨迹的对比，并没有呈现修复后的实际效果，在修复后会将修复后的异常轨迹段的位姿点替换原来的异常轨迹段的位姿点，如图中修复后的异常区域所示。因而，不会保留异常轨迹段和修复前的紧耦合轨迹。

图6为本发明第三实施例提供的紧耦合轨迹修复装置的结构示意图，如图6所示，本实施例中，紧耦合轨迹修复装置300的执行主体可以为车载装置。车载装置可以为一种驾驶脑，或车机、或DHU（娱乐主机和仪表的集成机器）、或IHU（Infotainment Head Unit，信息娱乐主机）、或IVI（In-Vehicle Infotainment，车载信息娱乐系统）、或任意一种车载信息交互终端。该紧耦合轨迹修复装置300包括：

获取模块301，用于获取由紧耦合位姿点组成的紧耦合轨迹和由松耦合位姿点组成的松耦合轨迹。

确定模块302，用于确定紧耦合轨迹中的异常轨迹段和松耦合轨迹中与异常轨迹段匹配的补偿轨迹段。异常轨迹段包括多个异常位姿点。

修复模块303，用于根据紧耦合轨迹、异常轨迹段和补偿轨迹段修复异常轨迹段。

本实施例提供的紧耦合轨迹修复装置可以执行图2所示方法实施例的技术方案，其实现原理和技术效果与图2所示方法实施例类似，在此不再一一赘述。

本发明提供的紧耦合轨迹修复装置在上一实施例提供的紧耦合轨迹修复装置的基础上，对紧耦合轨迹修复装置300进行了进一步的细化。

可选的，本实施例中，确定模块302具体用于：

根据轨迹平滑性检测算法确定紧耦合轨迹中的异常位姿点。确定各异常位姿点之间的相对距离。将异常位姿点和以该异常位姿点为中心间隔预设距离内的其他异常位姿点确定为异常轨迹段。其中，在异常轨迹段中该异常位姿点和其他异常位姿点之间的相对距离小于或等于预设距离。从松耦合轨迹中确定与异常轨迹段匹配的补偿轨迹段。

可选的，本实施例中，异常位姿点包括位姿参数。

确定模块302在确定各异常位姿点之间的相对距离时，具体用于：

以紧耦合位姿点中处于中心位置的中心位姿点为坐标原点建立局部坐标系。将各异常位姿点的坐标转换到局部坐标系下得到各个异常位姿点在局部坐标系的相对坐标。根据各异常位姿点的相对坐标确定相对距离。

可选的，本实施例中，修复模块303具体用于：

根据紧耦合位姿点和异常轨迹段确定异常轨迹段的两端对应的两个延伸轨迹段。其中一个延伸轨迹段的起点为异常轨迹段的两端中第一端的异常位姿点，终点为第一端的异常位姿点沿紧耦合轨迹延伸设定距离后的一紧耦合位姿点；另一个延伸轨迹段的起点为异常轨迹段的两端中第二端的异常位姿点，终点为第二端的异常位姿点沿着紧耦合轨迹延伸设定距离后的一紧耦合位姿点。根据异常轨迹段、两个延伸轨迹段、补偿轨迹段以及预设的非线性优化算法修复异常轨迹段。

可选的，本实施例中，异常轨迹段包括异常位姿点的位姿参数。延伸轨迹段包括紧耦合位姿点的位姿参数。补偿轨迹段包括松耦合位姿点的位姿参数。

修复模块303在根据异常轨迹段、两个延伸轨迹段、补偿轨迹段以及预设的非线性优化算法修复异常轨迹段时，具体用于：

根据异常轨迹段的各异常位姿点的位姿参数构建待优化位姿矩阵集合。根据两个延伸轨迹段的各紧耦合位姿点的位姿参数构建单边约束矩阵集合。根据待优化位姿矩阵集合和补偿轨迹段的各松耦合位姿点的位姿参数构建双边约束矩阵集合。将待优化位姿矩阵集合、单边约束矩阵集合和双边约束矩阵集合输入预设的非线性优化算法。采用预设的非线性优化算法，以单边约束矩阵集合和双边约束矩阵集合为约束，对待优化位姿矩阵集合进行优化，以获得优化后的位姿矩阵集合。根据优化后的位姿矩阵集合，确定并输出修复后的异常轨迹段。

可选的，本实施例中，紧耦合位姿点和松耦合位姿点都包括以下位姿参数：

纬度、经度、高度、横滚角、俯仰角和航向角。

本实施例提供的紧耦合轨迹修复装置可以执行图2-图5所示方法实施例的技术方案，其实现原理和技术效果与图2-图5所示方法实施例类似，在此不再一一赘述。

根据本发明的实施例，本发明还提供了一种电子设备、一种计算机可读存储介质和一种计算机程序产品。

如图7所示，图7是本发明第四实施例提供的电子设备的结构示意图。电子设备旨在各种形式适用于车辆的数字计算机，诸如，驾驶脑，或车机、或DHU（娱乐主机和仪表的集成机器）、或IHU（Infotainment Head Unit，信息娱乐主机）、或IVI（In-VehicleInfotainment，车载信息娱乐系统）、或任意一种车载信息交互终端。电子设备还可以表示各种形式的移动装置，诸如，个人数字处理和其它类似的计算装置。本文所示的部件、它们的连接和关系、以及它们的功能仅仅作为示例，并且不意在限制本文中描述的和/或者要求的本发明的实现。

如图7所示，该电子设备包括：处理器401、存储器402。各个部件利用不同的总线互相连接，并且可以被安装在公共主板上或者根据需要以其它方式安装。处理器可以对在电子设备内执行的指令进行处理。

存储器402即为本发明所提供的非瞬时计算机可读存储介质。其中，存储器存储有可由至少一个处理器执行的指令，以使至少一个处理器执行本发明所提供的紧耦合轨迹修复方法。本发明的非瞬时计算机可读存储介质存储计算机指令，该计算机指令用于使计算机执行本发明所提供的紧耦合轨迹修复方法。

存储器402作为一种非瞬时计算机可读存储介质，可用于存储非瞬时软件程序、非瞬时计算机可执行程序以及模块，如本发明实施例中的紧耦合轨迹修复方法对应的程序指令/模块（例如，附图6所示的获取模块301、确定模块302和修复模块303）。处理器401通过运行存储在存储器402中的非瞬时软件程序、指令以及模块，从而执行服务器的各种功能应用以及数据处理，即实现上述方法实施例中的紧耦合轨迹修复方法。

同时，本实施例还提供一种计算机产品，当该计算机产品中的指令由电子设备的处理器执行时，使得电子设备能够执行上述实施例一和二的紧耦合轨迹修复方法。

本领域技术人员在考虑说明书及实践这里公开的发明后，将容易想到本发明实施例的其它实施方案。本发明旨在涵盖本发明实施例的任何变型、用途或者适应性变化，这些变型、用途或者适应性变化遵循本发明实施例的一般性原理并包括本发明实施例未公开的本技术领域中的公知常识或惯用技术手段。说明书和实施例仅被视为示例性的，本发明实施例的真正范围和精神由下面的权利要求书指出。

应当理解的是，本发明实施例并不局限于上面已经描述并在附图中示出的精确结构，并且可以在不脱离其范围进行各种修改和改变。本发明实施例的范围仅由所附的权利要求书来限制。

Claims (10)

1.一种紧耦合轨迹修复方法，其特征在于，包括：

获取由紧耦合位姿点组成的紧耦合轨迹和由松耦合位姿点组成的松耦合轨迹；

确定所述紧耦合轨迹中的异常轨迹段和所述松耦合轨迹中与异常轨迹段匹配的补偿轨迹段；所述异常轨迹段包括多个异常位姿点；

根据所述紧耦合轨迹、所述异常轨迹段和所述补偿轨迹段修复所述异常轨迹段。

2.根据权利要求1所述的方法，其特征在于，所述确定所述紧耦合轨迹中的异常轨迹段和所述松耦合轨迹中与异常轨迹段匹配的补偿轨迹段，包括：

根据轨迹平滑性检测算法确定所述紧耦合轨迹中的异常位姿点；

确定各所述异常位姿点之间的相对距离；

将所述异常位姿点和以该异常位姿点为中心间隔预设距离内的其他异常位姿点确定为异常轨迹段；其中，在异常轨迹段中该异常位姿点和其他异常位姿点之间的相对距离小于或等于预设距离；

从所述松耦合轨迹中确定与所述异常轨迹段匹配的补偿轨迹段。

3.根据权利要求2所述的方法，其特征在于，所述异常位姿点包括位姿参数；

所述确定各所述异常位姿点之间的相对距离，包括：

以所述紧耦合位姿点中处于中心位置的中心位姿点为坐标原点建立局部坐标系；

将各异常位姿点的坐标转换到局部坐标系下得到各个异常位姿点在局部坐标系的相对坐标；

根据各所述异常位姿点的相对坐标确定所述相对距离。

4.根据权利要求3所述的方法，其特征在于，所述根据所述紧耦合轨迹、所述异常轨迹段和所述补偿轨迹段修复所述异常轨迹段，包括：

根据所述紧耦合位姿点和所述异常轨迹段确定所述异常轨迹段的两端对应的两个延伸轨迹段；其中一个延伸轨迹段的起点为所述异常轨迹段的两端中第一端的异常位姿点，终点为所述第一端的异常位姿点沿紧耦合轨迹延伸设定距离后的一紧耦合位姿点；另一个延伸轨迹段的起点为所述异常轨迹段的两端中第二端的异常位姿点，终点为所述第二端的异常位姿点沿着紧耦合轨迹延伸设定距离后的一紧耦合位姿点；

根据所述异常轨迹段、两个延伸轨迹段、所述补偿轨迹段以及预设的非线性优化算法修复所述异常轨迹段。

5.根据权利要求4所述的方法，其特征在于，所述异常轨迹段包括异常位姿点的位姿参数；所述延伸轨迹段包括紧耦合位姿点的位姿参数；所述补偿轨迹段包括松耦合位姿点的位姿参数；

所述根据所述异常轨迹段、两个延伸轨迹段、所述补偿轨迹段以及预设的非线性优化算法修复所述异常轨迹段，包括：

根据所述异常轨迹段的各异常位姿点的位姿参数构建待优化位姿矩阵集合；

根据所述两个延伸轨迹段的各紧耦合位姿点的位姿参数构建单边约束矩阵集合；

根据所述待优化位姿矩阵集合和所述补偿轨迹段的各松耦合位姿点的位姿参数构建双边约束矩阵集合；

将所述待优化位姿矩阵集合、单边约束矩阵集合和双边约束矩阵集合输入预设的非线性优化算法；

采用所述预设的非线性优化算法，以单边约束矩阵集合和双边约束矩阵集合为约束，对所述待优化位姿矩阵集合进行优化，以获得优化后的位姿矩阵集合；

根据所述优化后的位姿矩阵集合，确定并输出修复后的异常轨迹段。

6.根据权利要求1至5任一所述的方法，其特征在于，所述紧耦合位姿点和松耦合位姿点都包括以下位姿参数：

纬度、经度、高度、横滚角、俯仰角和航向角。

7.一种紧耦合轨迹修复装置，其特征在于，包括：

获取模块，用于获取由紧耦合位姿点组成的紧耦合轨迹和由松耦合位姿点组成的松耦合轨迹；

确定模块，用于确定所述紧耦合轨迹中的异常轨迹段和所述松耦合轨迹中与异常轨迹段匹配的补偿轨迹段；所述异常轨迹段包括多个异常位姿点；

修复模块，用于根据所述紧耦合轨迹、所述异常轨迹段和所述补偿轨迹段修复所述异常轨迹段。

8.一种电子设备，其特征在于，包括：存储器，处理器；

存储器；用于存储所述处理器可执行指令的存储器；

其中，所述处理器被配置为由所述处理器执行如权利要求1至6任一项所述的紧耦合轨迹修复方法。

9.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质中存储有计算机执行指令，所述计算机执行指令被处理器执行时用于实现如权利要求1至6任一项所述的紧耦合轨迹修复方法。

10.一种计算机程序产品，包括计算机程序，其特征在于，该计算机程序被处理器执行时实现权利要求1至6任一项所述的紧耦合轨迹修复方法。

Images