CN112084289B - 一种轨迹融合方法及装置 - Google Patents

一种轨迹融合方法及装置 Download PDF

Info

Publication number
CN112084289B
CN112084289B CN202010988161.1A CN202010988161A CN112084289B CN 112084289 B CN112084289 B CN 112084289B CN 202010988161 A CN202010988161 A CN 202010988161A CN 112084289 B CN112084289 B CN 112084289B
Authority
CN
China
Prior art keywords
node
track
pose
fused
nodes
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202010988161.1A
Other languages
English (en)
Other versions
CN112084289A (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.)
Hubei Ecarx Technology Co Ltd
Original Assignee
Hubei Ecarx Technology 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 Hubei Ecarx Technology Co Ltd filed Critical Hubei Ecarx Technology Co Ltd
Priority to CN202010988161.1A priority Critical patent/CN112084289B/zh
Publication of CN112084289A publication Critical patent/CN112084289A/zh
Application granted granted Critical
Publication of CN112084289B publication Critical patent/CN112084289B/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/29—Geographical information databases
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/30—Determination of transform parameters for the alignment of images, i.e. image registration
    • G06T7/33—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods
    • G06T7/337—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods involving reference images or patches
    • 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/10028—Range image; Depth image; 3D point clouds
    • 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/30241—Trajectory

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Databases & Information Systems (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Data Mining & Analysis (AREA)
  • General Engineering & Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Processing Or Creating Images (AREA)

Abstract

本发明实施例提供了一种轨迹融合方法及装置，确定基础轨迹的第一节点的节点数据和待融合轨迹的第二节点的节点数据，确定第二节点的邻近第一节点，对该第二节点和邻近第一节点进行三维点云特征匹配，得到匹配节点对；对所有匹配节点对的第二节点和邻近第一节点进行三维点云配准，得到每一匹配节点对中第二节点和邻近第一节点的三维点云之间的刚体变换矩阵，作为待融合轨迹和基础轨迹之间的关联约束；对待融合轨迹进行位姿优化，并将位姿优化后的待融合轨迹和基础轨迹进行组合，得到融合轨迹。能够实现自动高精度的轨迹融合。

Description

一种轨迹融合方法及装置
技术领域
本发明涉及定位导航技术领域，特别是涉及一种轨迹融合方法及装置。
背景技术
高精度地图在实现高安全、高可靠的无人驾驶系统中具有举足轻重的地位。利用即时定位与建图(Simultaneous Localization and Mapping，简称SLAM)技术自动构建城市级别的高精度地图是当前的主流趋势。
在实际生产中，SLAM技术可以用来估计数据采集车每一时刻的姿态，再结合GPS信息进行绝对轨迹优化，从而得到可用于拼接高精度地图的轨迹。
对于城市级别的道路数据而言，单个采集车不可能一次性采集整个城市的道路数据，多车分次采集是目前常见的采集方式。这种采集方式自然会带来一个问题，如何将多条轨迹融合起来，使得拼接的高精度地图无缝衔接。
然而当前多轨迹融合大多依靠人工手动对齐，耗时较多且精度低。
发明内容
本发明实施例的目的在于提供一种轨迹融合方法及装置，以实现自动高精度的融合多轨迹。具体技术方案如下：
为实现上述目的，本发明实施例提供了一种轨迹融合方法，所述方法包括：
获取待融合轨迹和基础轨迹；
确定所述基础轨迹的第一节点的节点数据和所述待融合轨迹的第二节点的节点数据，所述节点数据包括：节点的三维点云数据、节点的GPS数据和节点的位姿数据；
针对每一第二节点，基于该第二节点的GPS数据和各所述第一节点的GPS数据，确定该第二节点的邻近第一节点，对该第二节点和所述邻近第一节点进行三维点云特征匹配，得到匹配节点对；
对所有匹配节点对的第二节点和邻近第一节点进行三维点云配准，得到每一匹配节点对中第二节点和邻近第一节点的三维点云之间的刚体变换矩阵，作为所述待融合轨迹和所述基础轨迹之间的关联约束；
基于每一所述匹配节点对中第二节点和邻近第一节点的三维点云之间的刚体变换矩阵、所述基础轨迹中各第一节点的位姿以及所述待融合轨迹中各第二节点的位姿，对所述待融合轨迹进行位姿优化，并将位姿优化后的待融合轨迹和所述基础轨迹进行组合，得到融合轨迹。
可选的，所述针对每一第二节点，基于该第二节点的GPS数据和各所述第一节点的GPS数据，确定该第二节点的邻近第一节点，包括：
针对每一第二节点，基于哈希编码规则将该第二节点的GPS数据转换为GPS哈希码；
基于预先构建的第一节点集合与GPS哈希码的对应关系，从各所述第一节点集合中确定目标第一节点集合，所述目标第一节点集合的GPS哈希码与所述第二节点的GPS哈希码相同；
基于所述第二节点的GPS数据和所述目标第一节点集合中各第一节点的GPS数据，从所述目标第一节点集合中查找与所述第二节点距离最近的第一节点，作为该第二节点的邻近第一节点。
可选的，所述对该第二节点和所述邻近第一节点进行三维点云特征匹配得到匹配节点对的步骤，包括：
确定所述第二节点的三维点云数据，作为源点云数据；
确定所述邻近第一节点的三维点云数据，作为目标点云数据；
分别对所述源点云数据和所述目标点云数据进行关键点提取，得到源点云关键点和目标点云关键点；
分别对所述源点云关键点和所述目标点云关键点进行局部特征描述，得到源点云关键点的局部特征信息和目标点云关键点的局部特征信息；所述局部特征信息表示关键点与邻近的其他数据点之间的相对位置关系；
基于所述源点云关键点的局部特征信息和所述目标点云关键点的局部特征信息进行同名点匹配，并确定同名点匹配对的数目；
判断所述同名点匹配对的数目是否大于预设阈值，若大于，则所述第二节点和所述邻近第一节点构成匹配节点对。
可选的，所述基于每一匹配节点对中第二节点和邻近第一节点的三维点云之间的刚体变换矩阵、所述基础轨迹中各第一节点的位姿以及所述待融合轨迹中各第二节点的位姿，对所述待融合轨迹进行位姿优化的步骤，包括：
从所述基础轨迹的第一节点中确定一个标准节点，并将该标准节点的位姿确定为标准位姿；
以所述标准位姿为基准，对所述基础轨迹中其他第一节点的位姿进行转换，得到各其他第一节点相对于所述标准节点的第一相对位姿；
以所述标准位姿为基准，对所述待融合轨迹中各第二节点的位姿进行转换，得到各第二节点相对于所述标准节点的第二相对位姿；
根据所述刚体变换矩阵、所述第一相对位姿和所述第二相对位姿，对所述待融合轨迹中各第二节点的所述第二相对位姿进行优化。
可选的，所述根据所述刚体变换矩阵、所述第一相对位姿和所述第二相对位姿，对所述待融合轨迹中各第二节点的所述第二相对位姿进行优化的步骤，包括：
以所述待融合轨迹中各第二节点的第二相对位姿作为优化变量，构建最小优化误差函数，所述最小优化误差函数包括第一优化误差、第二优化误差和第三优化误差，所述第一优化误差包含所述待融合轨迹中各个相邻节点间的相对位姿误差，所述第二优化误差包含所述待融合轨迹中预先确定的回环节点间的相对位姿误差，所述第三优化误差包含各匹配节点对中第二节点和邻近第一节点之间的相对位姿误差；
求解所述最小优化误差函数，确定所述待融合轨迹中各节点的优化后位姿。
可选的，所述最小优化误差函数为：
Figure GDA0003152320010000031
其中，E表示最小优化误差函数，ξ表示所述待融合轨迹中各节点的位姿的李代数，i、j和k表示所述待融合轨迹中节点的序号，ε表示相邻节点对、一阶跳边节点对和回环节点对的集合，eij表示第i个节点和第j个节点之间的相对位姿误差，
Figure GDA0003152320010000041
表示预设的针对eij的第一信息矩阵，ek表示所述匹配节点对中第k个第二节点和该第k个第二节点对应的邻近第一节点之间的相对位姿误差，Ω表示匹配节点对的集合，
Figure GDA0003152320010000042
表示预设的针对ek的第二信息矩阵，(·)T表示矩阵转置，(·)-1表示矩阵求逆；
其中，第i个节点和第j个节点之间的相对位姿误差eij通过如下公式进行计算：
Figure GDA0003152320010000043
其中，ΔTij表示根据第i个节点和第j个节点的优化前的位姿计算出的第i个节点和第j个节点之间的相对位姿，Ti表示第i个节点的待优化的位姿，Tj表示第j个节点的待优化的位姿；ξij表示ΔTij的李代数，ξi表示Ti的李代数，ξj表示Tj的李代数；
所述匹配节点对中第k个第二节点和该第k个第二节点对应的邻近第一节点之间的相对位姿误差ek通过如下公式进行计算：
Figure GDA0003152320010000044
其中
Figure GDA0003152320010000045
Tk表示匹配节点对中第k个第二节点的待优化的位姿，ξk表示Tk的李代数，
Figure GDA0003152320010000046
表示第k个第二节点和该第k个第二节点对应的邻近第一节点的三维点云之间的刚体变换矩阵，nbk表示第k个第二节点对应的邻近第一节点，
Figure GDA0003152320010000047
表示第k个第二节点对应的邻近第一节点的位姿，(·)-1表示矩阵求逆。
可选的，在所述将位姿优化后的待融合轨迹和所述基础轨迹进行组合，得到融合轨迹之后，还包括：
将所述融合轨迹确定为新的基础轨迹，执行获取待融合轨迹的步骤。
为实现上述目的，本发明实施例还提供了一种轨迹融合装置，所述装置包括：
获取模块，用于获取待融合轨迹和基础轨迹；
第一确定模块，用于确定所述基础轨迹的第一节点的节点数据和所述待融合轨迹的第二节点的节点数据，所述节点数据包括：节点的三维点云数据、节点的GPS数据和节点的位姿数据；
第二确定模块，用于针对每一第二节点，基于该第二节点的GPS数据和各所述第一节点的GPS数据，确定该第二节点的邻近第一节点，对该第二节点和所述邻近第一节点进行三维点云特征匹配，得到匹配节点对；
配准模块，用于对所有匹配节点对的第二节点和邻近第一节点进行三维点云配准，得到每一匹配节点对中第二节点和邻近第一节点的三维点云之间的刚体变换矩阵，作为所述待融合轨迹和所述基础轨迹之间的关联约束；
融合模块，用于基于每一所述匹配节点对中第二节点和邻近第一节点的三维点云之间的刚体变换矩阵、所述基础轨迹中各第一节点的位姿以及所述待融合轨迹中各第二节点的位姿，对所述待融合轨迹进行位姿优化，并将位姿优化后的待融合轨迹和所述基础轨迹进行组合，得到融合轨迹。
为实现上述目的，本发明实施例还提供了一种电子设备，包括处理器、通信接口、存储器和通信总线；其中，处理器、通信接口、存储器通过通信总线完成相互间的通信；
存储器，用于存放计算机程序；
处理器，用于执行存储器上所存放的程序时，实现上述任一方法步骤。
为实现上述目的，本发明实施例还提供了一种计算机可读存储介质，所述计算机可读存储介质内存储有计算机程序，所述计算机程序被处理器执行时实现上述任一方法步骤。
本发明实施例有益效果：
应用本发明实施例提供的轨迹融合方法及装置，可以根据节点的GPS数据确定待融合轨迹中各第二节点的邻近第一节点，其中邻近第一节点是基础轨迹中的节点，进而通过三维点云特征匹配确定多个匹配节点对。通过三维点云配准确定匹配节点对中第二节点和邻近第一节点的刚体变换矩阵，以确定的刚体变换矩阵作为待融合轨迹和基础轨迹之间的关联约束，对待融合轨迹的节点进行位姿优化，进而将位姿优化后的待融合轨迹和基础轨迹进行组合，即可得到融合轨迹。可见能够实现自动高精度的轨迹融合。
当然，实施本发明的任一产品或方法并不一定需要同时达到以上所述的所有优点。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的实施例。
图1为本发明实施例提供的轨迹融合方法的一种流程示意图；
图2为本发明实施例提供的对待融合轨迹进行位姿优化的一种流程示意图；
图3为本发明实施例提供的对待融合轨迹进行位姿优化的另一种流程示意图；
图4为本发明实施例提供的位姿图的一种示意图；
图5(a)为本发明实施例提供的两条采集路线的一种示意图，图5(b)为本发明实施例提供的采集得到的基础轨迹的一种示意图，图5(c)为本发明实施例提供的采集得到的待融合轨迹的一种示意图；图5(d)为本发明实施例提供的经过轨迹融合后得到的轨迹的一种示意图；
图6为本发明实施例提供的查找邻近第一节点的一种流程示意图；
图7为本发明实施例提供的存储节点数据的一种示意图；
图8为本发明实施例提供的三维点云特征匹配的一种流程示意图；
图9为本发明实施例提供的轨迹融合方法的另一种流程示意图；
图10为本发明实施例提供的轨迹融合装置的一种结构示意图；
图11为本发明实施例提供的电子设备的一种结构示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
为了解决现有的采用人工手动方式融合SLAM轨迹导致的耗时较多且精度低的技术问题，本发明实施例提供了一种轨迹融合方法及装置。参见图1，方法可以包括以下步骤：
S101：获取待融合轨迹和基础轨迹。
本发明实施例提供的轨迹融合方法可以应用于SLAM领域。通常SLAM系统包括采集设备和后端处理设备。其中，采集设备可以是装载有多线激光雷达的采集车。
具体的，在SLAM技术中，采集设备在待探测的道路上移动，并通过激光雷达对周边环境进行探测，每隔一段时间获取一帧三维点云数据。此外，通过GPS信号对采集设备进行定位。后端处理设备对采集设备探测到的数据进行优化处理，以生成用于拼接高精度地图的轨迹。
对于城市级别的道路数据而言，单个采集设备不可能一次性采集整个城市的道路数据，通常采用多个采集设备分次采集的方式。
每个采集设备可以采集得到一条轨迹的数据，轨迹的数据由轨迹节点的数据组成，每个轨迹节点的数据包括：该轨迹节点的时间戳、采集设备在该时间戳采集到的三维点云数据、采集设备在该时间戳的位姿数据和采集设备在该时间戳的GPS数据。其中，GPS数据是采用GPS信号对采集设备进行定位得到的，GPS数据包括经度、纬度和高度。
为了便于说明，本申请中，节点均表示轨迹节点，节点的三维点云数据表示采集设备在该节点的时间戳下采集到的三维点云数据，节点的GPS数据表示采集设备在该节点的时间戳的GPS数据，节点的位姿表示采集设备在该节点的时间戳的位姿。
本步骤中，可以获取待融合轨迹和基础轨迹。其中，基础轨迹是当前已经完成融合的轨迹，可以理解为，在轨迹融合过程中，基础轨迹作为待融合轨迹的基准，轨迹融合过程中基础轨迹不需要进行调整。待融合轨迹是需要与基础轨迹进行融合的轨迹，通过对待融合轨迹进行调整，使得待融合轨迹能够与基础轨迹衔接。
S102：确定基础轨迹的第一节点的节点数据和待融合轨迹的第二节点的节点数据，节点数据包括：节点的三维点云数据、节点的GPS数据和节点的位姿。
本发明实施例中，轨迹融合的实质是建立基础轨迹和待融合轨迹之间的关联约束，并根据关联约束调整待融合轨迹中各节点的位姿数据，以使得基础轨迹和待融合轨迹可以高精度的无缝衔接。在轨迹融合过程中，基础轨迹中各节点的位姿是固定不变的。
本发明实施例中，基础轨迹和待融合轨迹均可以是结合GPS数据进行优化过的绝对轨迹。其中，结合GPS数据进行优化的过程可以参见相关技术，本发明实施例对此不做限定。
本发明实施例中，为了便于说明，基础轨迹包含的各个节点均记为第一节点，待融合轨迹包含的各个节点均记为第二节点。
基础轨迹中的第一节点和待融合轨迹中的第二节点的节点数据均包括：节点的三维点云数据、节点的GPS数据和节点的位姿数据。关于节点数据的具体介绍，可以参见S101相关描述。
S103：针对每一第二节点，基于该第二节点的GPS数据和各第一节点的GPS数据，确定该第二节点的邻近第一节点，对该第二节点和邻近第一节点进行三维点云特征匹配，得到匹配节点对。
本发明实施例中，为了将待融合轨迹和基础轨迹进行融合，需要建立待融合轨迹和基础轨迹之间的关联约束。
本发明实施例中，预先规划的不同采集路线之间可以有一定的重合，以便于后续轨迹融合。相应的，通过SLAM技术采集得到的基础轨迹和待融合轨迹之间也存在一定的重合，因此在轨迹融合过程中，可以将重合的采集路线对应的基础轨迹中的第一节点和待融合轨迹中的第二节点之间的位姿约束，作为待融合轨迹和基础轨迹之间的关联约束。
具体的，在重合的采集路线对应的基础轨迹和待融合轨迹中，能够建立位姿约束的第一节点和第二节点必然距离很近，因此，针对每个第二节点，可以基于该第二节点的GPS数据和各第一节点的GPS数据，确定该第二节点的邻近第一节点。其中，第二节点的邻近第一节点表示基础轨迹中与该第二节点距离最近的第一节点。
本发明实施例中，可以根据如下公式计算第一节点和第二节点之间的距离：
Figure GDA0003152320010000091
dx＝(lon2-lon1)Rnh cos(lat1)
dy＝(lat2-lat1)Rmh
dz＝height2-height1
其中，n1表示第一节点，n2表示第二节点，d(n1,n2)表示第一节点和第二节点的距离，lon1,lat1和height1分别表示第一节点n1的经度、纬度和高度，lon2,lat2和height2分别表示第二节点n2的经度、纬度和高度，Rmh表示子午圈主曲率半径，Rnh表示卯酉圈主曲率半径。
在本发明的一种实施例中，在确定第二节点的邻近第一节点后，可以对该第二节点和邻近第一节点进行三维点云特征匹配。
在本发明的另一种实施例中，为了减少三维点云特征匹配的计算量，在确定第二节点的邻近第一节点后，判断该第二节点与邻近第一节点之间的距离是否小于预设阈值，若小于，则对该第二节点和邻近第一节点进行三维点云特征匹配。若不小于，则认为该第二节点和邻近第一节点之间不存在位姿约束，因此不必进行三维点云特征匹配。
其中，三维点云特征匹配是衡量三维点云之间相似度的一种算法。本发明实施例中，可以根据三维点云特征匹配的匹配结果判断第二节点的三维点云与邻近第一节点的三维点云是否足够相似，如果足够相似则表示该第二节点和邻近第一节点存在位姿约束，因此可以进行三维点云配准，在这种情况下，该第二节点和邻近第一节点共同构成一个匹配节点对。
S104：对所有匹配节点对的第二节点和邻近第一节点进行三维点云配准，得到每一匹配节点对中第二节点和邻近第一节点的三维点云之间的刚体变换矩阵，作为待融合轨迹和基础轨迹之间的关联约束。
本发明实施例中，构成匹配对的第二节点和邻近第一节点的三维点云相似度较高，可以进行三维点云配准。具体的，可以采用迭代最近邻算法或者广义迭代最近邻算法对第二节点的三维点云和邻近第一节点的三维点云进行配准，得到每一匹配节点对中第二节点和对应的邻近第一节点的三维点云之间的刚体变换矩阵。
上述刚体变换矩阵能够表示匹配节点对中第二节点和邻近第一节点之间的位姿约束，因此，可以将基于所有匹配节点对计算得到的刚体变换矩阵作为待融合轨迹和基础轨迹之间的关联约束。
S105：基于每一匹配节点对中第二节点和邻近第一节点的三维点云之间的刚体变换矩阵、基础轨迹中各第一节点的位姿以及待融合轨迹中各第二节点的位姿，对待融合轨迹进行位姿优化，并将位姿优化后的待融合轨迹和基础轨迹进行组合，得到融合轨迹。
本发明实施例中，每一匹配节点对中第二节点和邻近第一节点的三维点云之间的刚体变换矩阵作为轨迹融合过程中待融合轨迹和基础轨迹之间的关联约束。
其中，轨迹融合可以理解为对待融合轨迹中各第二节点的位姿进行优化，以使得位姿优化后的待融合轨迹能够与基础轨迹高精度无缝衔接。
本发明实施例中，可以采用位姿图优化方法对待融合轨迹进行位姿优化。其中，位姿图是一个专业术语，就是由节点对应的位姿及其约束形成的图结构。位姿图就是用来描述节点间位姿约束的数学工具，可以根据各节点的位姿及其约束关系构建出位姿图。
换言之，位姿图优化就是将节点的位姿作为待优化变量，节点的位姿对应位姿图中的顶点，与位姿有关的几何约束作为约束边添加到位姿图中，每条约束边对应一个优化误差，因此可以根据位姿图中的约束边构建优化误差函数，该优化误差函数包含各个约束边对应的优化误差，该优化误差函数的优化变量为节点的位姿，进而可以采用非线性优化的方法求解优化误差函数，使得总体的优化误差最小。
本发明实施例中，每一匹配节点对中第二节点和邻近第一节点的三维点云之间的刚体变换矩阵可以作为位姿优化中的约束，优化变量为待融合轨迹中各个节点的位姿。
在本发明的一种实施例中，参见图2，上述步骤S105中：基于每一匹配节点对中第二节点和邻近第一节点的三维点云之间的刚体变换矩阵、基础轨迹中各第一节点的位姿以及待融合轨迹中各第二节点的位姿，对待融合轨迹进行位姿优化的步骤，具体可以包括以下细化步骤：
S201：从基础轨迹的第一节点中确定一个标准节点，并将该标准节点的位姿确定为标准位姿。
本发明实施例中，由于基础轨迹中第一节点的位姿和待融合轨迹中第二节点的位姿所对应的空间直角坐标系通常是不同的，因此进行位姿优化之前，需要将基础轨迹中第一节点的位姿和待融合轨迹中第二节点的位姿均转换到相同的空间直角坐标系中。
由于轨迹融合中基础轨迹中的所有节点的位姿是固定不变的，因此可以从基础轨迹的第一节点中确定一个标准节点，并将该标准节点的位姿确定为标准位姿。
一种实施例中，可以选取基础轨迹中的任一第一节点作为标准节点。
另一种实施例中，为了进一步提高轨迹融合的精确度，可以根据所有匹配节点对中的第一节点的GPS数据计算平均值，得到平均位置，并选取基础轨迹中与该平均位置距离最近的第一节点，作为标准节点。
具体的，若有k个匹配节点对，则能够建立k个位姿约束，表示为：
Figure GDA0003152320010000111
其中nbk表示基础轨迹中的第k个第一节点，nk表示待融合轨迹中的第k个第二节点，
Figure GDA0003152320010000112
表示基础轨迹中的第k个第一节点的三维点云与待融合轨迹中的第k个第二节点的三维点云之间的刚体变换矩阵。
则可以计算k个匹配节点对包含的基础轨迹中的k个第一节点的平均位置，具体计算公式可以为：
Figure GDA0003152320010000121
Figure GDA0003152320010000122
其中，lonavg和latavg分别表示平均位置的经度和纬度，Ω表示匹配节点对中第一节点的集合。
进而从基础轨迹中的k个第一节点中选取与平均位置距离最近的第一节点，作为标准节点。
S202：以标准位姿为基准，对基础轨迹中其他第一节点的位姿进行转换，得到各其他第一节点相对于标准节点的第一相对位姿。
本发明实施例中，可以将基础轨迹中其他第一节点的位姿均转换到以标准节点为原点的空间直角坐标系中，得到各其他第一节点相对于标准节点的第一相对位姿。
具体的位姿转换过程可以参见如下公式：
Figure GDA0003152320010000123
Figure GDA0003152320010000124
Figure GDA0003152320010000125
其中，Tni为节点ni的相对于标准节点的第一相对位姿，αi，βi和γi分别为节点ni的转换前位姿的三个姿态角，loni和lati分别表示节点ni的经度和纬度，Rmh表示子午圈主曲率半径，Rnh表示卯酉圈主曲率半径，
Figure GDA0003152320010000131
和
Figure GDA0003152320010000132
分别表示标准节点
Figure GDA0003152320010000133
的经度和纬度。
S203：以标准位姿为基准，对待融合轨迹中各第二节点的位姿进行转换，得到各第二节点相对于标准节点的第二相对位姿。
相应的，将待融合轨迹中各第二节点的位姿转换到以标准节点为原点的空间直角坐标系中，得到各第二节点相对于标准节点的第二相对位姿。
本发明实施例中，不限定上述步骤S202和S203的执行顺序。
S204：根据刚体变换矩阵、第一相对位姿和第二相对位姿，对待融合轨迹中各第二节点的第二相对位姿进行优化。
在将第一节点和第二节点的位姿均转换到同一空间直角坐标系后，可以根据每一匹配节点对中第二节点和邻近第一节点的三维点云之间的刚体变换矩阵、各第一节点转换位姿后得到的第一相对位姿和各第二节点转换位姿后得到的第二相对位姿，对待融合轨迹中各第二节点的第二相对位姿进行优化。
在本发明的一种实施例中，参见图3，上述步骤S204，具体可以包括以下细化步骤：
S301：以待融合轨迹中各第二节点的第二相对位姿作为优化变量，构建最小优化误差函数，最小优化误差函数包括第一优化误差、第二优化误差和第三优化误差，第一优化误差包含待融合轨迹中各个相邻节点间的相对位姿误差，第二优化误差包含待融合轨迹中预先确定的回环节点间的相对位姿误差，第三优化误差包含各匹配节点对中第二节点和邻近第一节点之间的相对位姿误差。
如上文，本发明实施例中，可以采用位姿图优化方法对待融合轨迹中的各第二节点进行位姿优化。作为一个示例，图4为本发明实施例提供的位姿图的一种示意图，如图4所示，位姿图中的顶点表示各个节点的位姿，位姿图中的边表示约束关系，具体的，包括相邻边、一阶跳边、回环边和匹配约束边。图中用实线单向箭头表示相邻边、虚线单向箭头表示一阶跳边、实线双向箭头表示回环边、虚线双向箭头表示匹配约束边。其中，相邻边、一阶跳边、回环边的两端都是待融合轨迹中节点的位姿，都是待优化变量，因此相邻边、一阶跳边、回环边均属于二元边，而匹配约束边一端是基础轨迹中节点的固定不变的位姿，另一端是待融合轨迹中节点的位姿，只有待融合轨迹中节点的位姿是需要优化的，因此匹配约束边属于一元边。
本发明实施例中，上述回环边可以是预先确定的。具体的，针对待融合轨迹，可以通过三维点云配准或邻近搜索等方式确定待融合轨迹中的回环节点对，每个回环节点对包括互为回环关系的两个节点，则每个回环节点对包含的两个节点的位姿之间存在回环边。
本发明实施例中，位姿图中每条约束边都对应一个优化误差，因此在位姿优化中，可以构建最小优化误差函数，其中最小优化误差函数包括第一优化误差、第二优化误差和第三优化误差。
其中第一优化误差包含：待融合轨迹中各个相邻节点间的相对位姿误差。具体的，相邻节点间的相对位姿可以是前端里程计通过三维点云配准得到的，属于观测量，作为位姿优化时的相对位姿约束。
则优化得到的待融合轨迹中节点间相对位姿与前端里程计通过三维点云配准得到的相对位姿之间的差异，即可理解为上述相邻节点间的相对位姿误差。
第二优化误差包含：待融合轨迹中预先确定的回环节点间的相对位姿误差。具体的，在确定回环节点对之后，可以根据三维点云配准技术计算回环节点对中包含的两个节点的三维点云之间的刚体变换矩阵，刚体变换矩阵属于观测量，作为位姿优化时的相对位姿约束，则优化得到的待融合轨迹中回环节点对包含的两个节点间的相对位姿与该刚体变换矩阵之间的差异，即可理解为回环节点间的相对位姿误差。
第三优化误差包含：各匹配节点对中第二节点和对应的邻近第一节点之间的相对位姿误差。
本发明实施例中，针对各个匹配节点对，根据三维点云配准技术计算得到匹配节点对中第二节点和邻近第一节点的三维点云之间的刚体变换矩阵，该刚体变换矩阵属于观测量，作为位姿优化时的相对位姿约束，则优化得到的匹配节点对中第二节点和邻近第一节点之间的相对位姿与该刚体变换矩阵之间的差异，即可理解为匹配节点对中第二节点和邻近第一节点之间的相对位姿误差。
在本发明的一种实施例中，构建的最小优化误差函数可以为：
Figure GDA0003152320010000151
其中，E表示最小优化误差函数，ξ表示待融合轨迹中各节点的位姿的李代数，i、j和k表示待融合轨迹中节点的序号，ε表示相邻节点对、一阶跳边节点对和回环节点对的集合，eij表示第i个节点和第j个节点之间的相对位姿误差，
Figure GDA0003152320010000152
表示预设的针对eij的第一信息矩阵，
Figure GDA0003152320010000153
用于对不同eij进行加权，其具体值可以根据实际需求进行设定。
上述公式中
Figure GDA0003152320010000154
能够表示上述第一优化误差和第二优化误差之和。
ek表示匹配节点对中第k个第二节点和该第k个第二节点对应的邻近第一节点之间的相对位姿误差，Ω表示匹配节点对的集合，
Figure GDA0003152320010000155
表示预设的针对ek的第二信息矩阵，
Figure GDA0003152320010000156
用于对不同的ek进行加权，其具体值可以根据实际需求进行设定。
上述公式中
Figure GDA0003152320010000157
表示上述第三优化误差。
为了便于理解，下面对照图4所示的位姿图对最小优化误差函数进行举例说明。
图4所示实施例中，基础轨迹中第一节点包括nb1,nb2,nb3和nb4，对应的位姿分别为Tb1,Tb2,Tb3和Tb4,待融合轨迹中第二节点包括n1,n2,n3,n4,n5,n6,n7和n8，对应的位姿分别为T1,T2,T3,T4,T5,T6,T7和T8。如图4所示，约束边包括相邻边、一阶跳边、回环边和匹配约束边，每条约束边对应一个相对位姿误差，则图4所示实施例中，所构建的最小优化误差函数中相对位姿误差包括：待融合轨迹中各相邻节点之间的相对位姿误差、一阶跳边节点对中节点n1和n3之间的相对位姿误差、回环节点对中节点n1和n5之间的相对位姿误差、匹配节点对中节点n2和节点nb1之间的相对位姿误差以及匹配节点对中节点n4和节点nb3之间的相对位姿误差。
图4仅作为一个示例，在实际应用中，基础轨迹中第一节点的数目和待融合轨迹中第二节点的数目均较大，所确定的匹配节点对的数目也较大。
本发明实施例中，上述最小优化误差函数中，第i个节点和第j个节点之间的相对位姿误差eij可以通过如下公式进行计算：
Figure GDA0003152320010000161
其中，ΔTij表示根据第i个节点和第j个节点的优化前的位姿计算出的第i个节点和第j个节点之间的相对位姿，Ti表示第i个节点的待优化的位姿，Tj表示第j个节点的待优化的位姿；ξij表示ΔTij的李代数，ξi表示Ti的李代数，ξj表示Tj的李代数。
匹配节点对中第k个第二节点和该第k个第二节点对应的邻近第一节点之间的相对位姿误差ek通过如下公式进行计算：
Figure GDA0003152320010000162
其中
Figure GDA0003152320010000163
Tk表示匹配节点对中第k个第二节点的待优化的位姿，ξk表示Tk的李代数，
Figure GDA0003152320010000164
表示第k个第二节点和该第k个第二节点对应的邻近第一节点的三维点云之间的刚体变换矩阵，nbk表示第k个第二节点对应的邻近第一节点，
Figure GDA0003152320010000165
表示第k个第二节点对应的邻近第一节点的位姿，(·)-1表示矩阵求逆。
S302：求解最小优化误差函数，确定待融合轨迹中各节点的优化后位姿。
为了求解上述最小优化误差函数，可以分别确定相对位姿误差对待优化位姿的导数。
本发明实施例中，相邻边、一阶跳边和回环边均为二元边，其对应的相对位姿误差的求导方式是相同的。
具体的，设ni和nj是待融合轨迹中的两个节点，对应的转换坐标后的位姿分别为Ti和Tj，则二者间的相对位姿
Figure GDA0003152320010000171
对应的相对位姿误差可以表示为：
Figure GDA0003152320010000172
为了推导相对位姿误差对两端待优化的位姿的雅各比矩阵，对相对位姿误差eij施加左乘扰动，有
Figure GDA0003152320010000173
利用伴随性质，exp(ξ^)T＝Texp((Ad(T-1)ξ)^)，可将上式改写为
Figure GDA0003152320010000174
其中，
Figure GDA0003152320010000175
Figure GDA0003152320010000176
对于匹配节点对中第二节点和邻近第一节点之间的相对位姿误差，由于邻近第一节点属于基础轨迹中的节点，其位姿是固定不变的，因此匹配约束边是一元边，其求导方式与二元边不同。
具体的，匹配节点对中第k个第二节点和该第k个第二节点对应的邻近第一节点之间的相对位姿误差ek通过如下公式进行计算：
Figure GDA0003152320010000177
其中
Figure GDA0003152320010000178
Tk表示匹配节点对中第k个第二节点的待优化的位姿，ξk表示Tk的李代数，
Figure GDA0003152320010000179
表示第k个第二节点和该第k个第二节点对应的邻近第一节点的三维点云之间的刚体变换矩阵，nbk表示第k个第二节点对应的邻近第一节点，
Figure GDA00031523200100001710
表示第k个第二节点对应的邻近第一节点的位姿。(·)-1表示矩阵求逆。
上述相对位姿误差ek对待优化位姿的求导表示为：
Figure GDA0003152320010000181
进而可以求解上述构建的最小优化误差函数，得到待融合轨迹中各第二节点的优化后位姿。
具体的，可以采用非线性优化方法进行求解，例如采用非线性最小二乘法、一阶梯度和二阶梯度法等。
本发明实施例中，将位姿优化后的待融合轨迹和基础轨迹进行组合，即可得到融合轨迹。具体的，可以将待融合轨迹中位姿优化后的第二节点的数据和基础轨迹中第一节点的数据整合在一起，作为融合轨迹中节点的数据。
应用本发明实施例提供的轨迹融合方法，可以根据节点的GPS数据确定待融合轨迹中各第二节点的邻近第一节点，并通过三维点云特征匹配确定多个匹配节点对。通过三维点云配准确定匹配节点对中第二节点和邻近第一节点的刚体变换矩阵，以确定的刚体变换矩阵作为待融合轨迹和基础轨迹之间的关联约束，对待融合轨迹的节点进行位姿优化，进而将位姿优化后的待融合轨迹和基础轨迹进行组合，即可得到融合轨迹。可见能够实现自动高精度的轨迹融合。
本领域技术人员可以理解，两条绝对轨迹被融合之后，融合轨迹的数据可以共同用于拼接高精度的点云地图。
作为一个示例，参见图5(a)、图5(b)、图5(c)和图5(d)，图5(a)为本发明实施例提供的两条采集路线的一种示意图，图5(b)为本发明实施例提供的采集得到的基础轨迹的一种示意图，图5(c)为本发明实施例提供的采集得到的待融合轨迹的一种示意图；图5(d)为本发明实施例提供的经过轨迹融合后得到的轨迹的一种示意图。
在本发明的一种实施例中，为了便于快速查询到第二节点的邻近第一节点，上述步骤S103中：针对每一第二节点，基于该第二节点的GPS数据和各第一节点的GPS数据，确定该第二节点的邻近第一节点，可以包括以下细化步骤：
S601：针对每一第二节点，基于哈希编码规则将该第二节点的GPS数据转换为GPS哈希码。
本发明实施例中，预设的哈希编码规则用于将节点的GPS数据中的经度和纬度映射为GPS哈希码。
作为一个示例，若GPS数据的纬度值为39.9232，则根据下表(纬度二分示意表)，经过15次二分后得到对应的纬度编码为：101110001100011。相应的，地球经度的取值范围为[-180°，180°]，若GPS数据的经度值为116.3895，同样对其进行15次二分后得到对应的经度编码为：110100101100010。将经度编码和纬度编码相结合，奇数位放置经度编码值，偶数位放置纬度编码值，得到30位的编码：111001110100100011110000001101。然后每5位一组，将30位编码划分为：11100 11101 00100 01111 00000 01101，将每个组看做二进制数值，转换为十进制数，分别为28、29、4、15、0和13。根据如下数值字符映射表，得到映射后的GPS哈希码：wx4g0e。
Figure GDA0003152320010000191
Figure GDA0003152320010000201
纬度二分示意表
数值 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15
字符 0 1 2 3 4 5 6 7 8 9 b c d e f g
数值 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31
字符 h j k m n p q r s t u v w x y z
数值字符映射表
可见，基于预设的哈希编码规则可以将任意的GPS的经纬度信息转换为GPS哈希码，且GPS哈希码中前缀匹配越多，表示两个位置越接近。此外，二分的次数越多，则字符串长度越长，划分越精确。可以根据实际需求设定二分的次数。
S602：基于预先构建的第一节点集合与GPS哈希码的对应关系，从各第一节点集合中确定目标第一节点集合，目标第一节点集合的GPS哈希码与第二节点的GPS哈希码相同。
本发明实施例中，可以按照上述哈希编码规则，预先对基础轨迹中各第一节点的GPS数据进行编码，得到每个第一节点的GPS哈希码，由于距离较近的第一节点对应的GPS哈希码可能是相同的，因此一个GPS哈希码对应一个第一节点集合，进而可以建立不同第一节点集合与不同GPS哈希码的对应关系。
作为一个示例，参见图7，可以将GPS哈希码相同的第一节点存储至同一文件夹中，文件夹用对应的GPS哈希码来命名，从而建立第一节点集合和GPS哈希码的对应关系。如图7所示，“wtw37c”、“wtw37d”、“wtw37f”和“wtw379”均表示文件夹，“wtw37c”文件夹中存储有GPS哈希码为wtw37c的所有第一节点的数据，包括三维点云数据的访问地址、位姿数据和GPS数据，这些数据可以用poseInfo.txt格式进行存储。通过节点的三维点云数据的访问地址可以调取该节点的三维点云数据。
可见，基于上述方式，能够建立不同第一节点集合与不同GPS哈希码的对应关系。上述采用文件夹建立对应关系的方式仅作为一个示例，本发明实施例对此不做限定。
本发明实施例中，可以根据所确定的第二节点的GPS哈希码，以及预先构建的第一节点集合与GPS哈希码的对应关系，从第一节点集合中确定目标第一节点集合，目标第一节点集合的GPS哈希码与第二节点的GPS哈希码相同。
承接上例，若某个第二节点的GPS哈希码为“wtw37c”，则可以确定上述第一个文件夹对应的第一节点集合为该第二节点的目标第一节点集合。
S603：基于第二节点的GPS数据和目标第一节点集合中各第一节点的GPS数据，从目标第一节点集合中查找与第二节点距离最近的第一节点，作为该第二节点的邻近第一节点。
本发明实施例中，只需从目标第一节点集合中查找与第二节点距离最近的第一节点即可，查找到的第一节点即为该第二节点的邻近第一节点。
可见，本发明实施例设计了一种基于GPS哈希编码对基础轨迹的节点数据进行分区管理的策略，相比于遍历查找的方式，大幅降低了复杂度，能够在轨迹融合过程中，快速查询待融合轨迹与基础轨迹之间的重合轨迹，且便于增量式管理。
在本发明的一种实施例中，上述步骤S103中：对该第二节点和邻近第一节点进行三维点云特征匹配得到匹配节点对的步骤，具体可以包括以下细化步骤：
S801：确定第二节点的三维点云数据，作为源点云数据。
S802：确定邻近第一节点的三维点云数据，作为目标点云数据。
本发明实施例中，在进行三维点云特征匹配时，需要确定源点云数据和目标点云数据，可以将第二节点的三维点云数据作为源点云数据；将邻近第一节点的三维点云数据，作为目标点云数据。
S803：分别对源点云数据和目标点云数据进行关键点提取，得到源点云关键点和目标点云关键点。
本发明实施例中，可以基于三维点云算法库PCL提供的Harris3D或者ISS3D等算法进行关键点提取，分别得到源点云关键点和目标点云关键点。
S804：分别对源点云关键点和目标点云关键点进行局部特征描述，得到源点云关键点的局部特征信息和目标点云关键点的局部特征信息；局部特征信息表示关键点与邻近的其他数据点之间的相对位置关系。
本发明实施例中，可以基于SHOT、FPFH等特征描述算法，分别对源点云关键点和目标点云关键点进行局部特征描述，得到源点云关键点的局部特征信息和目标点云关键点的局部特征信息，其中局部特征信息表示关键点与邻近的其他数据点之间的相对位置关系。
S805：基于源点云关键点的局部特征信息和目标点云关键点的局部特征信息进行同名点匹配，并确定同名点匹配对的数目。
本发明实施例中，可以基于源点云关键点的局部特征信息和目标点云关键点的局部特征信息进行同名点匹配。其中同名点匹配对包含一个源点云关键点和一个目标点云关键点，且该源点云关键点的局部特征信息与目标点云关键点的局部特征信息相同。
容易理解的，同名点匹配对包含的源点云关键点和目标点云关键点的局特征信息是相同的，则该源点云关键点和目标点云关键点均对应物理世界中的同一个点。
本发明实施例中，同名点匹配对的数目越多，则表示源点云和目标点云越相似。
S806：判断同名点匹配对的数目是否大于预设阈值，若大于，则第二节点和邻近第一节点构成匹配节点对。
本发明实施例中，可以判断同名点匹配对的数目是否大于预设阈值，若大于，则表示第二节点和邻近第一节点能够构成匹配节点对；若不大于，则表示第二节点和邻近第一节点不能够构成匹配节点对。
可见，本发明实施例中，对第二节点和邻近第一节点进行三维点云特征匹配，如果匹配得到的同名点匹配对数目大于预设阈值，则表示第二节点和邻近第一节点构成匹配节点对，二者可以进行三维点云配准，以得到相应的刚体变换矩阵，作为位姿优化的约束。
在本发明的一种实施例中，在将位姿优化后的待融合轨迹和基础轨迹进行组合，得到融合轨迹之后，方法还可以包括：
将融合轨迹确定为新的基础轨迹，返回执行S101步骤。
可见，本发明实施例中，将待融合轨迹与基础轨迹融合之后，将融合得到的轨迹作为新的基础轨迹，并获取新的待融合轨迹，重新执行轨迹融合的步骤。能够实现自动增量式多轨迹融合。
为了便于理解，下面结合附图图9，对本发明实施例提供的轨迹融合方法进行进一步介绍。
如图9所示，可以先判断当前基础轨迹数据库是否为空，若为空，则将当前待融合轨迹确定为基础轨迹。若不为空，则根据当前待融合轨迹的节点数据和当前基础轨迹的节点数据，构建当前待融合轨迹与当前基础轨迹之间的关联约束。通过位姿图优化的方式，基于上述关联约束对待融合轨迹中节点进行位姿优化，得到融合后的轨迹。将融合后的轨迹确定为新的基础轨迹，即更新基础轨迹数据库，重新获取当前待融合轨迹，以进行下一个轨迹融合过程。
此外，在获取融合后的轨迹之后，可以判断融合后的轨迹是否满足制图需求，如果不满足则舍去，若满足，则将融合后的轨迹确定为新的基础轨迹。
本发明实施例还提供了一种轨迹融合装置，参见图10，装置包括以下模块：
获取模块1001，用于获取待融合轨迹和基础轨迹；
第一确定模块1002，用于确定基础轨迹的第一节点的节点数据和待融合轨迹的第二节点的节点数据，节点数据包括：节点的三维点云数据、节点的GPS数据和节点的位姿数据；
第二确定模块1003，用于针对每一第二节点，基于该第二节点的GPS数据和各第一节点的GPS数据，确定该第二节点的邻近第一节点，对该第二节点和邻近第一节点进行三维点云特征匹配，得到匹配节点对；
配准模块1004，用于对所有匹配节点对的第二节点和邻近第一节点进行三维点云配准，得到每一匹配节点对中第二节点和邻近第一节点的三维点云之间的刚体变换矩阵，作为待融合轨迹和基础轨迹之间的关联约束；
融合模块1005，用于基于每一匹配节点对中第二节点和邻近第一节点的三维点云之间的刚体变换矩阵、基础轨迹中各第一节点的位姿以及待融合轨迹中各第二节点的位姿，对待融合轨迹进行位姿优化，并将位姿优化后的待融合轨迹和基础轨迹进行组合，得到融合轨迹。
应用本发明实施例提供的轨迹融合装置，可以根据节点的GPS数据确定待融合轨迹中各第二节点的邻近第一节点，其中邻近第一节点是基础轨迹中的节点，进而通过三维点云特征匹配确定多个匹配节点对。通过三维点云配准确定匹配节点对中第二节点和邻近第一节点的刚体变换矩阵，以确定的刚体变换矩阵作为待融合轨迹和基础轨迹之间的关联约束，对待融合轨迹的节点进行位姿优化，进而将位姿优化后的待融合轨迹和基础轨迹进行组合，即可得到融合轨迹。可见能够实现自动高精度的轨迹融合。
基于相同的发明构思，根据上述轨迹融合方法实施例，本发明实施例还提供了一种电子设备，如图11所示，包括处理器1101、通信接口1102、存储器1103和通信总线1104，其中，处理器1101，通信接口1102，存储器1103通过通信总线1104完成相互间的通信，
存储器1103，用于存放计算机程序；
处理器1101，用于执行存储器1103上所存放的程序时，实现如下步骤：
获取待融合轨迹和基础轨迹；
确定基础轨迹的第一节点的节点数据和待融合轨迹的第二节点的节点数据，节点数据包括：节点的三维点云数据、节点的GPS数据和节点的位姿数据；
针对每一第二节点，基于该第二节点的GPS数据和各第一节点的GPS数据，确定该第二节点的邻近第一节点，对该第二节点和邻近第一节点进行三维点云特征匹配，得到匹配节点对；
对所有匹配节点对的第二节点和邻近第一节点进行三维点云配准，得到每一匹配节点对中第二节点和邻近第一节点的三维点云之间的刚体变换矩阵，作为待融合轨迹和基础轨迹之间的关联约束；
基于每一匹配节点对中第二节点和邻近第一节点的三维点云之间的刚体变换矩阵、基础轨迹中各第一节点的位姿以及待融合轨迹中各第二节点的位姿，对待融合轨迹进行位姿优化，并将位姿优化后的待融合轨迹和基础轨迹进行组合，得到融合轨迹。
上述电子设备提到的通信总线可以是外设部件互连标准(Peripheral ComponentInterconnect，PCI)总线或扩展工业标准结构(Extended Industry StandardArchitecture，EISA)总线等。该通信总线可以分为地址总线、数据总线、控制总线等。为便于表示，图中仅用一条粗线表示，但并不表示仅有一根总线或一种类型的总线。
通信接口用于上述电子设备与其他设备之间的通信。
存储器可以包括随机存取存储器(Random Access Memory，RAM)，也可以包括非易失性存储器(Non-Volatile Memory，NVM)，例如至少一个磁盘存储器。可选的，存储器还可以是至少一个位于远离前述处理器的存储装置。
上述的处理器可以是通用处理器，包括中央处理器(Central Processing Unit，CPU)、网络处理器(Network Processor，NP)等；还可以是数字信号处理器(Digital SignalProcessing，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现场可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。
应用本发明实施例提供的电子设备，可以根据节点的GPS数据确定待融合轨迹中各第二节点的邻近第一节点，其中邻近第一节点是基础轨迹中的节点，进而通过三维点云特征匹配确定多个匹配节点对。通过三维点云配准确定匹配节点对中第二节点和邻近第一节点的刚体变换矩阵，以确定的刚体变换矩阵作为待融合轨迹和基础轨迹之间的关联约束，对待融合轨迹的节点进行位姿优化，进而将位姿优化后的待融合轨迹和基础轨迹进行组合，即可得到融合轨迹。可见能够实现自动高精度的轨迹融合。
在本发明提供的又一实施例中，还提供了一种计算机可读存储介质，该计算机可读存储介质内存储有计算机程序，计算机程序被处理器执行时实现上述任一轨迹融合方法的步骤。
在本发明提供的又一实施例中，还提供了一种包含指令的计算机程序产品，当其在计算机上运行时，使得计算机执行上述实施例中任一轨迹融合方法的步骤。
在上述实施例中，可以全部或部分地通过软件、硬件、固件或者其任意组合来实现。当使用软件实现时，可以全部或部分地以计算机程序产品的形式实现。所述计算机程序产品包括一个或多个计算机指令。在计算机上加载和执行所述计算机程序指令时，全部或部分地产生按照本发明实施例所述的流程或功能。所述计算机可以是通用计算机、专用计算机、计算机网络、或者其他可编程装置。所述计算机指令可以存储在计算机可读存储介质中，或者从一个计算机可读存储介质向另一个计算机可读存储介质传输，例如，所述计算机指令可以从一个网站站点、计算机、服务器或数据中心通过有线(例如同轴电缆、光纤、数字用户线(DSL))或无线(例如红外、无线、微波等)方式向另一个网站站点、计算机、服务器或数据中心进行传输。所述计算机可读存储介质可以是计算机能够存取的任何可用介质或者是包含一个或多个可用介质集成的服务器、数据中心等数据存储设备。所述可用介质可以是磁性介质，(例如，软盘、硬盘、磁带)、光介质(例如，DVD)、或者半导体介质(例如固态硬盘Solid State Disk(SSD))等。
需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
本说明书中的各个实施例均采用相关的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于轨迹融合装置、电子设备、计算机可读存储介质及计算机程序产品实施例而言，由于其基本相似于轨迹融合方法实施例，所以描述的比较简单，相关之处参见轨迹融合方法实施例的部分说明即可。
以上所述仅为本发明的较佳实施例，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内所作的任何修改、等同替换、改进等，均包含在本发明的保护范围内。

Claims (10)

1.一种轨迹融合方法，其特征在于，所述方法包括：
获取待融合轨迹和基础轨迹；
确定所述基础轨迹的第一节点的节点数据和所述待融合轨迹的第二节点的节点数据，所述节点数据包括：节点的三维点云数据、节点的GPS数据和节点的位姿数据；
针对每一第二节点，基于该第二节点的GPS数据和各所述第一节点的GPS数据，确定该第二节点的邻近第一节点，对该第二节点和所述邻近第一节点进行三维点云特征匹配，得到匹配节点对；
对所有匹配节点对的第二节点和邻近第一节点进行三维点云配准，得到每一匹配节点对中第二节点和邻近第一节点的三维点云之间的刚体变换矩阵，作为所述待融合轨迹和所述基础轨迹之间的关联约束；
基于每一所述匹配节点对中第二节点和邻近第一节点的三维点云之间的刚体变换矩阵、所述基础轨迹中各第一节点的位姿以及所述待融合轨迹中各第二节点的位姿，对所述待融合轨迹进行位姿优化，并将位姿优化后的待融合轨迹和所述基础轨迹进行组合，得到融合轨迹。
2.根据权利要求1所述的方法，其特征在于，所述针对每一第二节点，基于该第二节点的GPS数据和各所述第一节点的GPS数据，确定该第二节点的邻近第一节点，包括：
针对每一第二节点，基于哈希编码规则将该第二节点的GPS数据转换为GPS哈希码；
基于预先构建的第一节点集合与GPS哈希码的对应关系，从各所述第一节点集合中确定目标第一节点集合，所述目标第一节点集合的GPS哈希码与所述第二节点的GPS哈希码相同；
基于所述第二节点的GPS数据和所述目标第一节点集合中各第一节点的GPS数据，从所述目标第一节点集合中查找与所述第二节点距离最近的第一节点，作为该第二节点的邻近第一节点。
3.根据权利要求1所述的方法，其特征在于，所述对该第二节点和所述邻近第一节点进行三维点云特征匹配得到匹配节点对的步骤，包括：
确定所述第二节点的三维点云数据，作为源点云数据；
确定所述邻近第一节点的三维点云数据，作为目标点云数据；
分别对所述源点云数据和所述目标点云数据进行关键点提取，得到源点云关键点和目标点云关键点；
分别对所述源点云关键点和所述目标点云关键点进行局部特征描述，得到源点云关键点的局部特征信息和目标点云关键点的局部特征信息；所述局部特征信息表示关键点与邻近的其他数据点之间的相对位置关系；
基于所述源点云关键点的局部特征信息和所述目标点云关键点的局部特征信息进行同名点匹配，并确定同名点匹配对的数目；
判断所述同名点匹配对的数目是否大于预设阈值，若大于，则所述第二节点和所述邻近第一节点构成匹配节点对。
4.根据权利要求1所述的方法，其特征在于，所述基于每一匹配节点对中第二节点和邻近第一节点的三维点云之间的刚体变换矩阵、所述基础轨迹中各第一节点的位姿以及所述待融合轨迹中各第二节点的位姿，对所述待融合轨迹进行位姿优化的步骤，包括：
从所述基础轨迹的第一节点中确定一个标准节点，并将该标准节点的位姿确定为标准位姿；
以所述标准位姿为基准，对所述基础轨迹中其他第一节点的位姿进行转换，得到各其他第一节点相对于所述标准节点的第一相对位姿；
以所述标准位姿为基准，对所述待融合轨迹中各第二节点的位姿进行转换，得到各第二节点相对于所述标准节点的第二相对位姿；
根据所述刚体变换矩阵、所述第一相对位姿和所述第二相对位姿，对所述待融合轨迹中各第二节点的所述第二相对位姿进行优化。
5.根据权利要求4所述的方法，其特征在于，所述根据所述刚体变换矩阵、所述第一相对位姿和所述第二相对位姿，对所述待融合轨迹中各第二节点的所述第二相对位姿进行优化的步骤，包括：
以所述待融合轨迹中各第二节点的第二相对位姿作为优化变量，构建最小优化误差函数，所述最小优化误差函数包括第一优化误差、第二优化误差和第三优化误差，所述第一优化误差包含所述待融合轨迹中各个相邻节点间的相对位姿误差，所述第二优化误差包含所述待融合轨迹中预先确定的回环节点间的相对位姿误差，所述第三优化误差包含各匹配节点对中第二节点和邻近第一节点之间的相对位姿误差；
求解所述最小优化误差函数，确定所述待融合轨迹中各节点的优化后位姿。
6.根据权利要求5所述的方法，其特征在于，所述最小优化误差函数为：
Figure FDA0003120987190000031
其中，E表示最小优化误差函数，ξ表示所述待融合轨迹中各节点的位姿的李代数，i、j和k表示所述待融合轨迹中节点的序号，ε表示相邻节点对、一阶跳边节点对和回环节点对的集合，eij表示第i个节点和第j个节点之间的相对位姿误差，
Figure FDA0003120987190000032
表示预设的针对eij的第一信息矩阵，ek表示所述匹配节点对中第k个第二节点和该第k个第二节点对应的邻近第一节点之间的相对位姿误差，Ω表示匹配节点对的集合，
Figure FDA0003120987190000033
表示预设的针对ek的第二信息矩阵，(·)T表示矩阵转置，(·)-1表示矩阵求逆；
其中，第i个节点和第j个节点之间的相对位姿误差eij通过如下公式进行计算：
Figure FDA0003120987190000034
其中，ΔTij表示根据第i个节点和第j个节点的优化前的位姿计算出的第i个节点和第j个节点之间的相对位姿，Ti表示第i个节点的待优化的位姿，Tj表示第j个节点的待优化的位姿；ξij表示ΔTij的李代数，ξi表示Ti的李代数，ξj表示Tj的李代数；
所述匹配节点对中第k个第二节点和该第k个第二节点对应的邻近第一节点之间的相对位姿误差ek通过如下公式进行计算：
Figure FDA0003120987190000041
Tk表示匹配节点对中第k个第二节点的待优化的位姿，ξk表示Tk的李代数，
Figure FDA0003120987190000042
表示第k个第二节点和该第k个第二节点对应的邻近第一节点的三维点云之间的刚体变换矩阵，nbk表示第k个第二节点对应的邻近第一节点，Tnbk表示第k个第二节点对应的邻近第一节点的位姿，(·)-1表示矩阵求逆。
7.根据权利要求1所述的方法，其特征在于，在所述将位姿优化后的待融合轨迹和所述基础轨迹进行组合，得到融合轨迹之后，还包括：
将所述融合轨迹确定为新的基础轨迹，执行获取待融合轨迹的步骤。
8.一种轨迹融合装置，其特征在于，所述装置包括：
获取模块，用于获取待融合轨迹和基础轨迹；
第一确定模块，用于确定所述基础轨迹的第一节点的节点数据和所述待融合轨迹的第二节点的节点数据，所述节点数据包括：节点的三维点云数据、节点的GPS数据和节点的位姿数据；
第二确定模块，用于针对每一第二节点，基于该第二节点的GPS数据和各所述第一节点的GPS数据，确定该第二节点的邻近第一节点，对该第二节点和所述邻近第一节点进行三维点云特征匹配，得到匹配节点对；
配准模块，用于对所有匹配节点对的第二节点和邻近第一节点进行三维点云配准，得到每一匹配节点对中第二节点和邻近第一节点的三维点云之间的刚体变换矩阵，作为所述待融合轨迹和所述基础轨迹之间的关联约束；
融合模块，用于基于每一所述匹配节点对中第二节点和邻近第一节点的三维点云之间的刚体变换矩阵、所述基础轨迹中各第一节点的位姿以及所述待融合轨迹中各第二节点的位姿，对所述待融合轨迹进行位姿优化，并将位姿优化后的待融合轨迹和所述基础轨迹进行组合，得到融合轨迹。
9.一种电子设备，其特征在于，包括处理器、通信接口、存储器和通信总线，其中，处理器，通信接口，存储器通过通信总线完成相互间的通信；
存储器，用于存放计算机程序；
处理器，用于执行存储器上所存放的程序时，实现权利要求1-7任一所述的方法步骤。
10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质内存储有计算机程序，所述计算机程序被处理器执行时实现权利要求1-7任一所述的方法步骤。
CN202010988161.1A 2020-09-18 2020-09-18 一种轨迹融合方法及装置 Active CN112084289B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010988161.1A CN112084289B (zh) 2020-09-18 2020-09-18 一种轨迹融合方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010988161.1A CN112084289B (zh) 2020-09-18 2020-09-18 一种轨迹融合方法及装置

Publications (2)

Publication Number Publication Date
CN112084289A CN112084289A (zh) 2020-12-15
CN112084289B true CN112084289B (zh) 2021-08-20

Family

ID=73738995

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010988161.1A Active CN112084289B (zh) 2020-09-18 2020-09-18 一种轨迹融合方法及装置

Country Status (1)

Country Link
CN (1) CN112084289B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112985404A (zh) * 2021-02-09 2021-06-18 广州小鹏自动驾驶科技有限公司 一种停车场众包地图生成方法、装置、设备和介质
CN112988931B (zh) * 2021-03-03 2023-02-03 广州小鹏自动驾驶科技有限公司 一种行驶轨迹的对齐方法、装置、设备和存储介质
CN112669250B (zh) * 2021-03-16 2021-09-17 湖北亿咖通科技有限公司 一种轨迹对齐方法及电子设备
CN113643346A (zh) * 2021-07-28 2021-11-12 杭州易现先进科技有限公司 场景重建方法和扫描设备
CN114490900B (zh) * 2021-12-28 2022-10-14 广州小鹏自动驾驶科技有限公司 一种多车次轨迹数据融合方法、装置、设备及存储介质

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105976353B (zh) * 2016-04-14 2020-01-24 南京理工大学 基于模型和点云全局匹配的空间非合作目标位姿估计方法
WO2018151672A1 (en) * 2017-02-17 2018-08-23 Dataspark Pte. Ltd. Trajectory analysis through fusion of multiple data sources
CN110634104B (zh) * 2019-09-05 2022-11-18 北京智行者科技股份有限公司 多地图拼接方法及装置
CN111552757B (zh) * 2020-04-30 2022-04-01 上海商汤临港智能科技有限公司 生成电子地图的方法、装置、设备及存储介质

Also Published As

Publication number Publication date
CN112084289A (zh) 2020-12-15

Similar Documents

Publication Publication Date Title
CN112084289B (zh) 一种轨迹融合方法及装置
CN111862162B (zh) 回环检测方法及系统、可读存储介质、电子设备
Walter et al. Matching spatial data sets: a statistical approach
US8532922B2 (en) Method for simplifying a description of a route of travel
CN109741209B (zh) 台风灾害下配电网多源数据融合方法、系统及存储介质
WO2019201135A1 (zh) 基于全局路网特征的数据匹配方法、装置及存储介质
CN111026978A (zh) 位置查询方法、装置、计算机设备及存储介质
CN112902970A (zh) 一种巡检路径规划方法和巡检机器人
CN109885635A (zh) 地图关联方法、装置、存储介质及计算机设备
CN115544088A (zh) 地址信息查询方法、装置、电子设备及存储介质
CN113704853B (zh) 一种基于道路元拓扑特征分类的多源道路网数据自动融合方法
Cho et al. A basis of spatial big data analysis with map-matching system
CN117824667A (zh) 一种基于二维码和激光的融合定位方法及介质
Lei Geospatial data conflation: A formal approach based on optimization and relational databases
CN111540202B (zh) 一种相似卡口确定方法、装置、电子设备及可读存储介质
CN111862163B (zh) 一种轨迹优化方法及装置
CN116012413A (zh) 图像特征点的跟踪方法、装置、电子设备及存储介质
CN113256804B (zh) 一种三维重建尺度恢复方法、装置、电子设备及存储介质
CN108763817B (zh) 一种基于最小二乘法建模的电力地下管网匹配方法
CN113988184A (zh) 一种矢量道路数据匹配与更新方法、装置和计算机设备
CN110619134B (zh) 解决路网数据飞点、点密度问题一体化检测及修复方法
CN113094462B (zh) 数据处理方法和装置及存储介质
Förstner Friedrich Ackermann’s scientific research program
Yumaganov et al. An improved map matching algorithm based on dynamic programming approach
Siddiquee et al. Map matching for error prone GPS data on a sparse road network and predicting travel time of a route

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