CN111862163B - 一种轨迹优化方法及装置 - Google Patents

一种轨迹优化方法及装置 Download PDF

Info

Publication number
CN111862163B
CN111862163B CN202010766551.4A CN202010766551A CN111862163B CN 111862163 B CN111862163 B CN 111862163B CN 202010766551 A CN202010766551 A CN 202010766551A CN 111862163 B CN111862163 B CN 111862163B
Authority
CN
China
Prior art keywords
node
track
optimization
gps
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
CN202010766551.4A
Other languages
English (en)
Other versions
CN111862163A (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 CN202010766551.4A priority Critical patent/CN111862163B/zh
Publication of CN111862163A publication Critical patent/CN111862163A/zh
Application granted granted Critical
Publication of CN111862163B publication Critical patent/CN111862163B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • G06T7/248—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments involving reference images or patches
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • 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
    • 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/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
    • 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
    • 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)
  • Computer Vision & Pattern Recognition (AREA)
  • Databases & Information Systems (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Multimedia (AREA)
  • Data Mining & Analysis (AREA)
  • General Engineering & Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明实施例提供了一种轨迹优化方法及装置，获取目标轨迹的节点数据，节点数据包括:目标轨迹中各节点的时间戳、各节点的三维点云数据、各节点的GPS数据和各节点的位姿数据；按照各节点的时间戳的先后顺序将各节点划分到多条累计分段轨迹中；基于各节点的位姿数据和各节点的GPS数据，依次对每个累计分段轨迹中的节点进行位姿优化；基于初始优化轨迹中各节点的三维点云数据、位姿数据以及GPS有效节点的GPS数据，对初始优化轨迹进行全局回环优化，得到绝对轨迹。可见，以节点的GPS数据为约束，依次优化各个累计分段中节点的位姿，可逐步优化整条轨迹，再进行全局回环优化，能够得到可直接用于构建高精度地图的绝对轨迹。

Description

一种轨迹优化方法及装置
技术领域
本发明涉及定位导航技术领域，特别是涉及一种轨迹优化方法及装置。
背景技术
即时定位与建图(Simultaneous Localization and Mapping,SLAM)技术作为一种前沿技术被广泛应用于自动构建大规模高精度地图。相比于视觉SLAM系统，利用多线激光雷达作为主要传感器的激光SLAM系统在测距精度和感知范围上都有明显的优势。
激光SLAM系统分为前端里程计与后端优化模块，其中前端里程计主要通过三维点云配准技术估计相邻帧的相对位姿变换关系，后端则利用回环检测对整个轨迹进行优化。
然而，通过现有的SLAM技术得到的轨迹只是一条相对轨迹，换言之，采用该轨迹构建出的地图与真实世界存在一个未知的刚体变换，因此不能直接用于构建高精度地图。
发明内容
本发明实施例的目的在于提供一种轨迹优化方法及装置，以实现生成可直接用于构建高精度地图的绝对轨迹。具体技术方案如下：
为实现上述目的，本发明实施例提供了一种轨迹优化方法，所述方法包括：
获取目标轨迹的节点数据，所述节点数据包括:目标轨迹中各节点的时间戳、各节点的三维点云数据、各节点的GPS数据和各节点的位姿数据；
按照各所述节点的时间戳的先后顺序将各所述节点划分到多条累计分段轨迹中，各累计分段轨迹中包含的第一个节点均为所述目标轨迹中的初始节点，各累计分段轨迹中包含上一累计分段轨迹中的所有节点和相对于上一累计分段轨迹新增的节点，且各累计分段轨迹相对于上一累计分段轨迹中新增的节点中包含至少一个GPS有效节点；
基于各所述节点的位姿数据和各所述节点的GPS数据，依次对每个累计分段轨迹中的节点进行位姿优化，直到完成所述目标轨迹中各节点的位姿优化，将优化后的所述目标轨迹作为初始优化轨迹；
基于所述初始优化轨迹中各节点的三维点云数据、位姿数据以及所述GPS有效节点的GPS数据，对所述初始优化轨迹进行全局回环优化，得到绝对轨迹。
可选的，所述基于各所述节点的位姿数据和各所述节点的GPS数据，依次对每个累计分段轨迹中的节点进行位姿优化的步骤，包括：
针对当前待优化的累计分段轨迹，确定第一GPS约束节点；所述第一GPS约束节点为当前待优化的累计分段轨迹相对于上一累计分段轨迹新增节点中GPS数据有效的节点；
针对每一所述第一GPS约束节点，基于该第一GPS约束节点的GPS数据和所述第一标准节点的GPS数据，计算该第一GPS约束节点与所述第一标准节点的相对位置；所述第一标准节点为预先确定的位于第一条累计分段轨迹且GPS数据有效的一个节点；
固定当前待优化的累计分段轨迹中包含的上一累计分段轨迹中所有节点的位姿，以当前待优化的累计分段轨迹相对于上一累计分段轨迹中新增的节点的位姿作为优化变量，构建第一最小优化误差函数；所述第一最小优化误差函数包括第一优化误差和第二优化误差；其中，所述第一优化误差包含：所述当前待优化的累计分段轨迹中各个相邻节点间的相对位姿误差，所述第二优化误差包含：每个所述第一GPS约束节点与所述第一标准节点间的相对位置误差；
求解所述第一最小优化误差函数，确定当前待优化的累计分段轨迹相对于上一累计分段轨迹中新增的节点的优化后位姿。
可选的，所述第一最小优化误差函数为：
Figure BDA0002614886420000021
其中，E1表示所述第一最小优化误差函数，ξ1表示当前待优化的累计分段轨迹相对于上一累计分段轨迹中新增的节点的位姿的李代数，i和j表示节点的序号，eij表示节点i和节点j之间的相对位姿误差，
Figure BDA0002614886420000031
表示预设的第一信息矩阵，Ξ表示相邻节点对和一阶跳边节点对的集合，ei1表示所述第一GPS约束节点与所述第一标准节点间的相对位置误差，U1表示第一GPS约束节点的集合，
Figure BDA0002614886420000032
表示预设的第二信息矩阵，(·)T表示矩阵转置；
其中，节点i和节点j之间的相对位姿误差eij通过如下公式进行计算：
Figure BDA0002614886420000033
其中，ΔTij表示根据节点i和节点j的优化前的位姿计算出的节点i和节点j之间的相对位姿，Ti表示节点i的待优化的位姿，Tj表示节点j的待优化的位姿；ξij表示ΔTij的李代数，ξi表示Ti的李代数，ξj表示Tj的李代数；
所述第一GPS约束节点i与所述第一标准节点间的相对位置误差ei1通过如下公式进行计算：
ei1＝T′i1p-[xi1,yi1,zi1,1]T，p＝[0,0,0,1]T；
其中，T′i1表示所述第一GPS约束节点i与所述第一标准节点间的相对位姿，(xi1,yi1,zi1)表示所述第一GPS约束节点i与所述第一标准节点间的相对位置的空间坐标。
可选的，所述基于所述初始优化轨迹中各节点的三维点云数据、位姿数据以及所述GPS有效节点的GPS数据，对所述初始优化轨迹进行全局回环优化，得到绝对轨迹的步骤，包括：
对所述初始优化轨迹进行回环检测，确定回环节点对；每一所述回环节点对包含两个互为回环关系的节点；
对每个回环节点对包含的两个节点进行三维点云配准，得到各回环节点对中两个节点的三维点云之间的刚体变换矩阵；
基于所述初始优化轨迹中各节点的位姿数据、所述GPS有效节点的GPS数据以及各回环节点对对应的三维点云之间的刚体变换矩阵，对所述初始优化轨迹进行全局优化，得到绝对轨迹。
可选的，所述对所述初始优化轨迹进行回环检测，确定回环节点对的步骤，包括：
从所述初始优化轨迹中提取待匹配节点，所述待匹配节点包括轨迹拐点和轨迹交叉点；
针对每一待匹配节点，从所述初始优化轨迹中确定与所述待匹配节点距离不超过预设距离阈值的候选匹配节点，并从所述候选匹配节点中找到与所述待匹配节点的时间戳间隔大于预设第一时间间隔，且与所述待匹配节点距离最近的节点，作为所述待匹配节点的回环匹配节点；
将每一完成匹配的待匹配节点与其匹配到的回环匹配节点确定为一回环节点对。
可选的，所述从所述初始优化轨迹中提取待匹配节点的步骤，包括：
针对所述初始优化轨迹中的每个节点，判断该节点与前后预设间隔的节点构成的夹角是否大于预设夹角阈值；若大于，则将该节点确定为轨迹拐点；
针对所述初始优化轨迹中的每个节点，判断该节点与该节点的临近节点之间的时间戳间隔是否大于预设第二时间间隔；若大于，则将该节点确定为轨迹交叉点；
将所述轨迹拐点和所述轨迹交叉点确定为待匹配节点。
可选的，所述基于所述初始优化轨迹中各节点的位姿数据、所述GPS有效节点的GPS数据以及各回环节点对对应的三维点云之间的刚体变换矩阵，对所述初始优化轨迹进行全局优化，得到绝对轨迹的步骤，包括：
确定所述初始优化轨迹中的第二标准节点和第二GPS约束节点；所述第二标准节点为预先确定的所述初始优化轨迹中GPS有效的一个节点，所述第二GPS约束节点为所述初始优化轨迹中除所述第二标准节点之外的GPS有效的节点；
针对每一所述第二GPS约束节点，基于该第二GPS约束节点的GPS数据和所述第二标准节点的GPS数据，计算该第二GPS约束节点与所述第二标准节点的相对位置；
以所述初始优化轨迹中所有节点的位姿作为优化变量，构建第二最小优化误差函数；所述第二最小优化误差函数包括第三优化误差、第四优化误差和第五优化误差；其中，所述第三优化误差包含：所述初始优化轨迹中各个相邻节点间的相对位姿误差，所述第四优化误差包含：每个所述第二GPS约束节点与所述第二标准节点间的相对位置误差，所述第五优化误差包含：所述回环节点对中两个节点之间的回环误差；
求解所述第二最小优化误差函数，确定所述初始优化轨迹中各节点的全局优化后位姿，并根据各节点的全局优化后位姿确定绝对轨迹。
可选的，所述第二最小优化误差函数为：
Figure BDA0002614886420000051
其中，E2表示第二最小优化误差函数，ξ2表示所述初始优化轨迹中所有节点的位姿的李代数，i、j和k表示节点的序号，eij表示节点i和节点j之间的相对位姿误差，
Figure BDA0002614886420000052
表示预设的第一信息矩阵，Ξ表示相邻节点对和一阶跳边节点对的集合，ei2表示所述第二GPS约束节点与所述第二标准节点间的相对位置误差，
Figure BDA0002614886420000053
表示预设的第二信息矩阵，U2表示第二GPS约束节点的集合，ejk表示回环节点对中节点j和节点k之间的回环误差，G表示回环节点对的集合，
Figure BDA0002614886420000054
表示预设的第三信息矩阵，(·)T表示矩阵转置；
其中，节点i和节点j之间的相对位姿误差eij通过如下公式进行计算：
Figure BDA0002614886420000055
其中，ΔTij表示根据节点i和节点j的优化前的位姿计算出的节点i和节点j之间的相对位姿，Ti表示节点i的待优化的位姿，Tj表示节点j的待优化的位姿；ξij表示ΔTij的李代数，ξi表示Ti的李代数，ξj表示Tj的李代数；
所述第二GPS约束节点i与所述第二标准节点间的相对位置误差ei2通过如下公式进行计算：
ei2＝T′i2p-[xi2,yi2,zi2,1]T，p＝[0,0,0,1]T；
其中，T′i2表示所述第二GPS约束节点i与所述第二标准节点间的相对位姿，(xi2,yi2,zi2)表示所述第二GPS约束节点i与所述第二标准节点间的相对位置的空间坐标；
所述回环节点对中节点j和节点k之间的回环误差ejk通过如下公式进行计算：
Figure BDA0002614886420000061
其中
Figure BDA0002614886420000062
是节点j和节点k的三维点云之间的刚体变换矩阵，Tj表示节点j的待优化的位姿，Tk表示节点k的待优化的位姿；ξjk表示
Figure BDA0002614886420000063
的李代数，ξj表示Tj的李代数，ξk表示Tk的李代数。
为实现上述目的，本发明实施例还提供了一种轨迹优化装置，所述装置包括：
获取模块，用于获取目标轨迹的节点数据，所述节点数据包括:目标轨迹中各节点的时间戳、各节点的三维点云数据、各节点的GPS数据和各节点的位姿数据；
分段模块，用于按照各所述节点的时间戳的先后顺序将各所述节点划分到多条累计分段轨迹中，各累计分段轨迹中包含的第一个节点均为所述目标轨迹中的初始节点，各累计分段轨迹中包含上一累计分段轨迹中的所有节点和相对于上一累计分段轨迹新增的节点，且各累计分段轨迹相对于上一累计分段轨迹中新增的节点中包含至少一个GPS有效节点；
第一优化模块，用于基于各所述节点的位姿数据和各所述节点的GPS数据，依次对每个累计分段轨迹中的节点进行位姿优化，直到完成所述目标轨迹中各节点的位姿优化，将优化后的所述目标轨迹作为初始优化轨迹；
第二优化模块，用于基于所述初始优化轨迹中各节点的三维点云数据、位姿数据以及所述GPS有效节点的GPS数据，对所述初始优化轨迹进行全局回环优化，得到绝对轨迹。
为实现上述目的，本发明实施例还提供了一种电子设备，包括处理器、通信接口、存储器和通信总线；其中，处理器、通信接口、存储器通过通信总线完成相互间的通信；
存储器，用于存放计算机程序；
处理器，用于执行存储器上所存放的程序时，实现上述任一方法步骤。
为实现上述目的，本发明实施例还提供了一种计算机可读存储介质，所述计算机可读存储介质内存储有计算机程序，所述计算机程序被处理器执行时实现上述任一方法步骤。
本发明实施例有益效果：
应用本发明实施例提供的轨迹优化方法及装置，以节点的GPS数据为约束，依次优化各个累计分段轨迹中节点的位姿，可逐步优化整条轨迹，得到接近全局最优的初始优化轨迹。在初始优化轨迹的基础上，结合节点的三维点云数据、位姿数据和GPS数据，进行全局回环优化，由于在轨迹优化的过程中结合了轨迹节点的GPS数据，能够得到可直接用于构建高精度地图的绝对轨迹。
当然，实施本发明的任一产品或方法并不一定需要同时达到以上所述的所有优点。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的实施例。
图1为本发明实施例提供的轨迹优化方法的一种流程示意图；
图2为本发明实施例提供的累计分段轨迹的一种示意图；
图3为本发明实施例提供的累计分段优化的一种流程示意图；
图4为本发明实施例提供的轨迹节点的位姿图的一种示意图；
图5(a)是本发明实施例提供的预先设定的SLAM采集路线的一种示意图；图5(b)为本发明实施例提供的累计分段优化前的轨迹示意图；图5(c)为本发明实施例提供的累计分段优化后的轨迹示意图；
图6为本发明实施例提供的全局回环约束的一种流程示意图；
图7为本发明实施例提供的确定回环节点对的一种流程示意图；
图8(a)是本发明示例提供的初始优化轨迹中轨迹拐点的一种示意图；图8(b)是本发明实施例提供的初始优化轨迹中轨迹交叉点的一种示意图；图8(c)为本发明实施例提供的初始优化轨迹中回环节点对的一种示意图；
图9为本发明实施例提供的对初始优化轨迹进行全局优化的一种流程示意图；
图10(a)为本发明实施例提供的初始优化轨迹中高程错位的一种示意图；
图10(b)为本发明实施例提供的全局优化后修复高程错位的一种示意图；
图11为本发明实施例提供的轨迹优化方法的另一种流程示意图；
图12为本发明实施例提供的轨迹优化装置的一种结构示意图；
图13为本发明实施例提供的电子设备的一种结构示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
为了解决现有的激光SLAM系统生成的相对轨迹无法直接应用于构建高精地图的技术问题，本发明实施例提供了一种轨迹优化方法，参见图1，方法可以包括以下步骤：
S101：获取目标轨迹的节点数据，节点数据包括:目标轨迹中各节点的时间戳、各节点的三维点云数据、各节点的GPS数据和各节点的位姿数据。
本发明实施例提供的轨迹优化方法可以应用于激光SLAM技术领域。
具体的，激光SLAM技术中，采集设备在待探测的道路上运动，并通过激光雷达对周边环境进行探测，每隔一段时间获取一帧三维点云数据。本发明实施例中，在采集设备运动过程中，额外通过GPS信号对采集设备进行定位。
本发明实施例中，目标轨迹中包含多个轨迹节点，因此目标轨迹的数据包括多个轨迹节点的数据，每个轨迹节点的数据包括：该轨迹节点的时间戳、采集设备在该时间戳采集到的三维点云数据、采集设备在该时间戳的位姿数据和采集设备在该时间戳的GPS数据，其中，GPS数据是采用GPS信号对采集设备进行定位得到的，GPS数据包括经度、纬度、高度和GPS有效标识。
为了便于说明，本申请中，节点均表示轨迹节点，节点的三维点云数据表示该节点时间戳下采集设备采集到的三维点云数据，节点的位姿数据表示该节点时间戳下采集设备的位姿数据，节点的GPS数据表示该节点时间戳下采集设备的GPS数据。
本发明实施例中，采集设备可以是装载有多线激光雷达的采集小车。采集设备可以将探测到的三维点云数据发送至前端里程计，前端里程计可以是SLAM系统的前端设备，通常具有较强的数据处理能力。前端里程计可以对相邻两个第二时间戳对应的三维点云数据进行配准，以计算相邻两个时间戳下采集设备的相对位姿。
举例来讲，对第二时间戳采集到的三维点云数据和第一时间戳采集到的三维点云数据进行配准，能够得到第二时间戳下采集设备与第一时间戳下采集设备的相对位姿。设第一时间戳下采集设备的位姿是已知的，则可以根据相对位姿，计算第二时间戳下采集设备的位姿。
其中，前端里程计根据三维点云配准技术，计算采集设备的相对位姿过程可以参见相关技术，在此不赘述。
按照上述方式，前端里程计可以根据采集到的三维点云数据，计算各个时间戳下采集设备的位姿数据。
进而，前端里程计可以确定各个轨迹节点的节点数据。其中，轨迹节点的节点数据包括时间戳、采集设备在该时间戳采集到的三维点云数据、采集设备在该时间戳的位姿数据和采集设备在该时间戳的GPS数据，其中，各轨迹节点的位姿数据可以是相对于初始轨迹节点的相对位姿，GPS数据包括经度、纬度、高度和GPS有效标识。
其中，GPS有效标识分为有效和无效两种，只有GPS有效的轨迹节点的GPS信息才是可用的。具体的，由于前端里程计的频率和GPS的频率不同，且前端里程计的频率大于GPS的频率，因此只有部分轨迹节点的GPS数据是有效的。举例来讲，当前端里程计的频率是10Hz，而GPS的频率是1Hz时，只有轨迹节点的时间戳与GPS的时间戳间隔小于0.1/2＝0.05秒，相应的轨迹节点的GPS数据才是有效的。
本领域技术人员可以理解，在激光SLAM技术领域，前端里程计计算得到的轨迹节点的位姿数据是存在误差的，因此SLAM的后端设备需要对各轨迹节点的位姿进行优化，以使得各轨迹节点的位姿更接近真实值。
本发明实施例提供的轨迹优化方法可以应用于激光SLAM系统的后端设备。即后端设备根据本发明实施例提供的轨迹优化方法对前端里程计输出的轨迹节点的位姿进行优化，以生成可以直接用于拼接高精地图的绝对轨迹。
本步骤中，后端设备可以获取前端里程计输出的目标轨迹的节点数据。
S102：按照各节点的时间戳的先后顺序将各节点划分到多条累计分段轨迹中，各累计分段轨迹中包含的第一个节点均为目标轨迹中的初始节点，各累计分段轨迹中包含上一累计分段轨迹中的所有节点和相对于上一累计分段轨迹新增的节点，且各累计分段轨迹相对于上一累计分段轨迹中新增的节点中包含至少一个GPS有效节点。
由于激光SLAM的前端里程计随着时间推移会产生累计的位姿误差，直接优化整个轨迹可能有陷入局部最优甚至收敛到错误的结果。为了解决这一问题，本发明实施例提供了一种累计分段优化的策略，累计分段优化中主要以GPS有效节点的GPS数据为约束。
具体的，可以按照节点的时间戳的先后顺序，将节点划分到多条累计分段轨迹中，各累计分段轨迹中包含的第一个节点均为目标轨迹中的初始节点，各累计分段轨迹中包含上一累计分段轨迹中的所有节点和相对于上一累计分段轨迹新增的节点，且各累计分段轨迹相对于上一累计分段轨迹中新增的节点中包含至少一个GPS有效节点。
作为一个示例，参见图2，图2示出了10个节点，其中用虚线圆圈表示的节点为GPS有效节点，用实线圆圈表示的节点不是GPS有效节点，即图2中的节点n2,n4,n7和n9是GPS有效节点。则可以按照上述累计分段策略，对n1-n10这10个节点进行分段。
作为一个示例，如图2所示，第一个累计分段轨迹包含节点n1-n4，第二个累计分段轨迹包含节点n1-n8，第三个累计分段轨迹包含节点n1-n10。即每个累计分段轨迹都包含上一累计分段轨迹中的所有节点，此外还包括相对于上一累计分段轨迹中新增的节点。
S103：基于各节点的位姿数据和各节点的GPS数据，依次对每个累计分段轨迹中的节点进行位姿优化，直到完成目标轨迹中各节点的位姿优化，将优化后的目标轨迹作为初始优化轨迹。
承接上述图2示例，本发明实施例中，可以先对第一个累计分段轨迹n1-n4进行位姿优化，接着对第二个累计分段轨迹n1-n8进行位姿优化，再对第三个累计分段轨迹n1-n10进行位姿优化，进而完成目标轨迹中各节点的位姿优化。
在本发明的一种实施例中，参见图3，基于各节点的位姿数据和各节点的GPS数据，依次对每个累计分段轨迹中的节点进行位姿优化的步骤，可以包括以下步骤：
S301：针对当前待优化的累计分段轨迹，确定第一GPS约束节点；第一GPS约束节点为当前待优化的累计分段轨迹相对于上一累计分段轨迹新增节点中GPS数据有效的节点。
本发明实施例中，在对各累计分段轨迹的节点进行位姿优化时，是以GPS有效节点的GPS数据作为约束的。
其中，针对当前待优化的累计分段轨迹，所确定的第一GPS约束节点可能是一个，也可能是多个。若当前待优化的累计分段轨迹相对于上一累计分段轨迹新增节点中GPS数据有效的节点有一个，则所确定的第一GPS约束节点即为一个；若当前待优化的累计分段轨迹相对于上一累计分段轨迹新增节点中GPS数据有效的节点有多个，则确定的第一GPS约束节点即为多个。
承接上述图2示例，针对第二个累计分段轨迹n1-n8，相比于第一个累计分段轨迹n1-n4新增的节点中GPS数据有效的节点只有n7，则针对第二个累计分段轨迹，所确定的第一GPS约束节点即为节点n7。
S302：针对每一第一GPS约束节点，基于该第一GPS约束节点的GPS数据和第一标准节点的GPS数据，计算该第一GPS约束节点与第一标准节点的相对位置；第一标准节点为预先确定的位于第一条累计分段轨迹且GPS数据有效的一个节点。
本发明实施例中，第一标准节点只有一个，且是位于第一条累计分段轨迹的GPS数据有效的节点，可以根据实际需求进行设定。承接上述图2示例，可以将第一条累计分段轨迹n1-n4中GPS数据有效的节点n2选定为第一标准节点。
本发明实施例中，可以基于第一GPS约束节点的GPS数据和第一标准节点的GPS数据，计算第一GPS约束节点与第一标准节点间的相对位置。具体的，可以采用如下公式计算相对位置的空间坐标：
xi＝(loni-lonb1)Rnhcos(latb1)
yi＝(lati-latb1)Rmh
zi＝heighti-heightb1
其中xi，yi和zi表示第一GPS约束节点ni与第一标准节点间的相对位置的三维空间坐标，loni,lati和heighti分别是第一GPS约束节点ni对应的经度、纬度以及高度，lonb1,latb1和heightb1分别是第一标准节点nb1对应的经度、纬度以及高度，Rmh和Rnh分别是地球的子午圈主曲率半径以及卯酉圈主曲率半径。
S303：固定当前待优化的累计分段轨迹中包含的上一累计分段轨迹中所有节点的位姿，以当前待优化的累计分段轨迹相对于上一累计分段轨迹中新增的节点的位姿作为优化变量，构建第一最小优化误差函数；第一最小优化误差函数包括第一优化误差和第二优化误差；其中，第一优化误差包含：当前待优化的累计分段轨迹中各个相邻节点间的相对位姿误差，第二优化误差包含：每个第一GPS约束节点与第一标准节点间的相对位置误差。
本发明实施例中，针对当前待优化的累计分段轨迹，可以固定当前待优化的累计分段轨迹中包含的上一累计分段轨迹中所有节点的位姿，仅对当前待优化的累计分段轨迹中相对于上一累计分段轨迹中新增的节点的位姿进行优化。
承接上述图2示例，本发明实施例中，针对第二个累计分段轨迹n1-n8，由于上一累计分段轨迹包含的节点为n1-n4，则在对第二个累计分段轨迹进行位姿优化时，可以固定节点n1-n4的位姿，仅对节点n5-n8的位姿进行优化。
本发明实施例中，可以采用位姿图优化方法对各累计分段轨迹进行位姿优化。其中，位姿图是一个专业术语，就是由节点对应的位姿及其约束形成的图结构。位姿图就是用来描述节点间位姿约束的数学工具，可以根据各节点的位姿及其约束关系构建出位姿图，通过对位姿图进行优化，就可以调节节点的位姿，使得节点的位姿更接近真实值。
换言之，位姿图优化是将节点的位姿作为待优化变量，节点的位姿对应位姿图中的顶点，与位姿有关的几何约束作为约束边添加到位姿图中，每条约束边对应一个优化误差，因此可以根据位姿图中的约束边构建优化误差函数，该优化误差函数包含各个约束边对应的优化误差，该优化误差函数的优化变量是节点的位姿，进而可以采用非线性优化的方法求解优化误差函数，使得总体的优化误差最小。
作为一个示例，图4为本发明实施例提供的位姿图的一种示意图，如图4所示，位姿图中顶点的表示各个节点的位姿，位姿图中的边表示约束关系，具体的，包括相邻边、一阶跳边和GPS约束边。图中用实线箭头表示相邻边，单向虚线箭头表示一阶跳边，一阶跳边指表示两个节点之间间隔一个节点，例如图4的位姿图中T1和T3之间的虚线边；双向虚线箭头表示GPS约束边。其中，相邻边和一阶跳边属于二元边，即约束边的两端都是待优化变量。GPS约束边属于一元边，只有一端对应优化变量。
本发明实施例中，位姿图中每条约束边都对应一个优化误差，因此在位姿优化中，可以构建第一最小优化误差函数，其中第一最小优化误差函数包括第一优化误差和第二优化误差。
其中第一优化误差包含：当前待优化的累计分段轨迹中各个相邻节点间的相对位姿误差。
具体的，相邻节点间的相对位姿是前端里程计通过三维点云配准得到的，因此属于观测量，作为位姿优化时的相对位姿约束。
则优化得到的节点间相对位姿与前端里程计通过三维点云配准得到的相对位姿之间的差异，即可理解为相对位姿误差。
第二优化误差包含：每个第一GPS约束节点与第一标准节点间的相对位置误差。
具体的，由于节点间的相对位姿包含旋转量和平移量，而在建立GPS位置约束时，仅考虑平移量，不需要考虑旋转量。因此可以通过第一GPS约束节点与第一标准节点的相对位姿确定二者之间的平移量，作为二者的相对位置。此外，还可以通过第一GPS约束节点与第一标准节点的GPS数据确定二者的相对位置。
则根据GPS数据计算到的第一GPS约束节点与第一标准节点的相对位置，以及根据相对位姿计算到的相对位置之间的差异，即可理解为上述第二优化误差。其中，根据GPS数据计算到的第一GPS约束节点与第一标准节点的相对位置是观测量，作为位姿优化时的GPS位置约束。
举例来讲，假设节点n2是第一标准节点，ni是第一GPS约束节点，T2是第一标准节点n2相对于初始节点的相对位姿，Ti是第一GPS约束节点ni相对于初始节点的相对位姿。则ni相对于n2的相对位姿表示为：T′i＝T2 -1Ti。则根据第一GPS约束节点ni与第一标准节点n2的相对位姿T′i计算的二者的相对位置可以表示为T′ip，其中p＝[0,0,0,1]T。此外，根据二者的GPS数据计算得到的相对位置的三维空间坐标为(xi,yi,zi)，则可以根据如下公式计算第一GPS约束节点ni与第一标准节点n2的相对位置的误差：ei＝T′ip-[xi,yi,zi,1]T。
在本发明的一种实施例中，构建的第一最小优化误差函数可以为：
Figure BDA0002614886420000141
其中，E1表示第一最小优化误差函数，ξ1表示当前待优化的累计分段轨迹相对于上一累计分段轨迹中新增的节点的位姿的李代数，i和j表示节点的序号，eij表示节点i和节点j之间的相对位姿误差，
Figure BDA0002614886420000142
表示预设的第一信息矩阵，Ξ表示相邻节点对和一阶跳边节点对的集合，ei1表示第一GPS约束节点与第一标准节点间的相对位置误差，U1表示第一GPS约束节点的集合，
Figure BDA0002614886420000143
表示预设的第二信息矩阵，(·)T表示矩阵转置。
其中，第一信息矩阵
Figure BDA0002614886420000144
和第二信息矩阵
Figure BDA0002614886420000145
可以根据实际需求进行设定。作为一个示例，第一信息矩阵
Figure BDA0002614886420000146
可以比第二信息矩阵
Figure BDA0002614886420000147
大4-5个数量级，以尽可能的保证局部轨迹的平滑。
本发明实施例中，位姿优化的目标是确定当前待优化位姿的值，使得第一优化误差和第二优化误差的总和最小。
S304：求解第一最小优化误差函数，确定当前待优化的累计分段相对于上一累计分段中新增的轨迹节点的优化后位姿。
为了求解上述第一最小优化误差函数，可以分别确定第一优化误差和第二优化误差对待优化变量的导数。
针对第一优化误差，假设轨迹节点ni相对于初始轨迹节点的相对位姿为Ti，轨迹节点nj相对于初始轨迹节点的相对位姿为Tj，则轨迹节点ni相对于轨迹节点nj的相对位姿为ΔTij＝Ti -1Tj，构建相对位姿误差为：
Figure BDA0002614886420000151
其中ξij，ξi，ξj分别是ΔTij，Ti和Tj对应的李代数；
Figure BDA0002614886420000152
是预先设置的第一信息矩阵，可以设置为6×6信息矩阵，通常为单位阵。
为了推导相对位姿误差对两端待优化的位姿的雅各比矩阵，对相对位姿误差eij施加左乘扰动，有
Figure BDA0002614886420000153
利用伴随性质，exp(ξ∧)T＝Texp((Ad(T-1)ξ)∧)，可将上式改写为
Figure BDA0002614886420000154
其中，
Figure BDA0002614886420000155
Figure BDA0002614886420000156
如上文，第二优化误差，即第一GPS约束节点ni与第一标准节点的相对位置的误差可以表示为ei＝T′ip-[xi,yi,zi,1]T。第二优化误差对待优化位姿的求导表示为：
Figure BDA0002614886420000161
其中I3×3是单位阵，t是第一GPS约束节点ni与第一标准节点的相对位姿T′i对应的平移向量，ξ′i是T′i对应的李代数。
进而可以求解上述构建的第一最小优化误差函数，得到当前待优化的累计分段相对于上一累计分段中新增的节点的优化后位姿。
具体的，可以采用非线性优化方法进行求解，例如采用非线性最小二乘法、一阶梯度和二阶梯度法等。
S104：基于初始优化轨迹中各节点的三维点云数据、位姿数据以及GPS有效节点的GPS数据，对初始优化轨迹进行全局回环优化，得到绝对轨迹。
本发明实施例中，在经过累计分段优化后，得到初始优化轨迹。
作为一个示例，参见图5(a)、图5(b)和图5(c)，其中图5(a)是本发明实施例提供的预先设定的SLAM采集路线的一种示意图，图5(b)为本发明实施例提供的累计分段优化前的轨迹示意图，图5(c)为本发明实施例提供的累计分段优化后的轨迹示意图。可见，累计分段优化后得到的初始优化轨迹已经大体接近真实轨迹。
本发明实施例中，为了进一步提高轨迹精度，可以在初始优化轨迹的基础上，基于初始优化轨迹中各节点的三维点云数据、位姿数据和GPS有效节点的GPS数据，对初始化优化轨迹进行全局回环优化，进而得到能够直接用于拼接高精地图的绝对轨迹。
可见，应用本发明实施例提供的轨迹优化方法，以节点的GPS数据为约束，依次优化各个累计分段轨迹中节点的位姿，可逐步优化整条轨迹，得到接近全局最优的初始优化轨迹。在初始优化轨迹的基础上，结合节点的三维点云数据、位姿数据和GPS数据，进行全局回环优化，由于在轨迹优化的过程中结合了轨迹节点的GPS数据，能够得到可直接用于构建高精度地图的绝对轨迹。
参见图6，在本发明的一种实施例中，图1所示的步骤S104:基于初始优化轨迹中各节点的三维点云数据、位姿数据以及GPS有效节点的GPS数据，对初始优化轨迹进行全局回环优化，得到绝对轨迹，具体可以包括以下细化步骤：
S601：对初始优化轨迹进行回环检测，确定回环节点对，每一回环节点对包含两个互为回环关系的节点。
在本发明的一种实施例中，可以采用三维点云特征匹配的方式对初始优化轨迹进行回环检测，确定回环节点对。
在本发明的另一种实施例中，由于经过累计分段优化得到的初始优化轨迹已经大体接近真实轨迹，为了提高检测回环节点对的效率，可以直接在初始优化轨迹的基础上，通过近邻搜索的方式，快速查找回环节点对。
具体的，参见图7，步骤S601可以包括以下细化步骤：
S701：从初始优化轨迹中提取待匹配节点，待匹配节点包括轨迹拐点和轨迹交叉点。
本发明实施例中，为了构建有效的回环约束，可以从初始优化轨迹中提取轨迹拐点和轨迹交叉点，其中轨迹拐点是轨迹中曲率较大的点，轨迹交叉点也可以理解为轨迹上的十字交叉点。
S702：针对每一待匹配节点，从初始优化轨迹中确定与待匹配节点距离不超过预设距离阈值的候选匹配节点，并从候选匹配节点中找到与待匹配节点的时间戳间隔大于预设第一时间间隔，且与待匹配节点距离最近的节点，作为待匹配节点的回环匹配节点。
作为一个示例，预设距离阈值为δd，预设第一时间间隔为δt。待匹配节点组成的集合为：Ω＝{ni1,ni2,...,nik}。对于Ω中的每一个元素nij，j∈{1,2,...,k}，可以从初始优化轨迹中找出与之距离不超过δd的节点，作为候选匹配节点。
本发明实施例中，可以从初始优化轨迹中两个节点之间的相对位姿中提取相对平移量，进而根据相对平移量计算该两个节点之间的距离。
记nij的候选匹配节点构成的集合为Θj＝{nj1，nj2，...，njl}。遍历Θj中的每个元素，从中找出与nij所对应的时间间隔大于δt的节点，记这些节点组成的集合为
Figure BDA0002614886420000183
然后从Φj中找出与nij距离最近的元素，记为nj*，nj*为待匹配节点nij的回环匹配节点。
S703：将每一完成匹配的待匹配节点与其匹配到的回环匹配节点确定为一回环节点对。
结合上例，njx为待匹配节点nij的回环匹配节点，则nij和nj*为一个回环节点对。
下面分别介绍提取轨迹拐点和轨迹交叉点的过程。
在本发明的一种实施例中，从初始优化轨迹中提取待匹配节点的步骤，可以包括：
步骤a1：针对初始优化轨迹中的每个节点，判断该节点与前后预设间隔的节点构成的夹角是否大于预设夹角阈值；若大于，则将该节点确定为轨迹拐点。
具体的，对于任意一节点ni，可以提取该节点前后间隔g个时刻的节点ni-g与ni+g，然后计算向量
Figure BDA0002614886420000181
与向量
Figure BDA0002614886420000182
的夹角θ，如果夹角θ大于预设夹角阈值，则ni是一个拐点。例如设置夹角阈值为20度。
此外，通常ni临近的点都会被检测为拐点，可以通过聚类取中间值，剔除冗余的拐点。
步骤a2：针对初始优化轨迹中的每个节点，判断该节点与该节点的临近节点之间的时间戳间隔是否大于预设第二时间间隔；若大于，则将该节点确定为轨迹交叉点。
其中，临近轨迹节点表示距离最近的轨迹节点。对于任意一节点ni，判断与ni距离最近的点对应时间戳与ni对应时间戳的时间戳间隔是否大于预设的第二时间间隔，若大于，则将该节点ni确定为轨迹交叉点。
作为一个示例，参见图8(a)、图8(b)和图8(c)，图8(a)、图8(b)和图8(c)中，白色线条表示轨迹，白色方块表示轨迹中的节点。图8(a)是本发明示例提供的初始优化轨迹中轨迹拐点的一种示意图，图8(b)是本发明实施例提供的初始优化轨迹中轨迹交叉点的一种示意图，图8(c)为本发明实施例提供的初始优化轨迹中回环节点对的一种示意图，如图8(c)所示，互相连接的两个白色方块表示一个回环节点对。
可见，本发明实施例中，由于经过累计分段优化得到的初始优化轨迹已经大体接近真实轨迹，因此可以在初始优化轨迹基础上，通过对每个轨迹节点进行近邻搜索，即可快速确定回环节点对，相比于传统的依赖三维局部特征匹配的后端回环检测，显著提高了回环检测的效率。
S602：对每个回环节点对包含的两个节点进行三维点云配准，得到各回环节点对中两个节点的三维点云之间的刚体变换矩阵。
本发明实施例中，经过步骤S601之后，可以确定多个回环节点对。
作为一个示例，设节点nij与节点nj*组成一个回环节点对。则可以分别读取节点nij与节点nj*的三维点云，记为源三维点云Cij与目标三维点云
Figure BDA0002614886420000197
利用迭代最近邻算法或者广义迭代最近邻算法对源三维点云Cij与目标三维点云
Figure BDA0002614886420000198
进行配准，得到源三维点云Cij与目标三维点云
Figure BDA0002614886420000199
的刚体变换矩阵
Figure BDA0002614886420000191
由于迭代最近邻算法需要较好的初值才能收敛到全局最优，这里以初值
Figure BDA0002614886420000192
作为配准算法的输入，可以得到精确的配准结果，其中
Figure BDA0002614886420000193
与
Figure BDA0002614886420000194
分别是累积分段优化后的节点nij与节点nj*相对于初始节点的相对位姿。
将所有有效回环组成的集合记为G，G中每个元素是一个三元组，形如
Figure BDA0002614886420000195
其中j和k是轨迹节点的序号，
Figure BDA0002614886420000196
是节点nj对应的三维点云到节点nk的刚体变换矩阵。
S603：基于初始优化轨迹中的各节点的位姿数据、GPS有效节点的GPS数据以及各回环节点对对应的三维点云之间的刚体变换矩阵，对初始优化轨迹进行全局优化，得到绝对轨迹。
本发明实施例中，在确定回环节点对之后，可以将回环节点对对应三维点云之间的刚体变换矩阵作为回环约束，结合GPS有效节点的GPS位置约束，对初始优化轨迹进行全局优化。
在本发明的一种实施例中，参见图9，步骤S603可以包括以下细化步骤：
S901：确定初始优化轨迹中的第二标准节点和第二GPS约束节点；第二标准节点为预先确定的初始优化轨迹中GPS有效的一个节点，第二GPS约束节点为初始优化轨迹中除第二标准节点之外的GPS有效的节点。
本发明实施例中，第二标准节点是初始优化轨迹中GPS有效的一个节点，可以为预先设定的。为了尽可能减少误差，可以将GPS有效的节点中GPS误差最小的节点选定为第二标准节点。
承接图2所示例子，若GPS有效节点n2,n4,n7和n9中n4的GPS误差最小，则可以将n4确定为第二标准节点。
S902：针对每一第二GPS约束节点，基于该第二GPS约束节点的GPS数据和第二标准节点的GPS数据，计算该第二GPS约束节点与第二标准节点的相对位置。
为了计算第二GPS约束节点与第二标准节点之间的相对位置，可以将各个第二GPS约束节点的GPS数据转换到与第二标准节点相同的坐标系。
本发明实施例中，可以基于第二GPS约束节点的GPS数据和第二标准节点的GPS数据，计算第二GPS约束节点与第二标准节点间的相对位置。
具体的计算过程可以参见步骤S302相关描述，不再赘述。
S903：以初始优化轨迹中所有节点的位姿作为优化变量，构建第二最小优化误差函数；第二最小优化误差函数包括第三优化误差、第四优化误差和第五优化误差；其中，第三优化误差包含：初始优化轨迹中各个相邻节点间的相对位姿误差，第四优化误差包含：每个第二GPS约束节点与第二标准节点间的相对位置误差，第五优化误差包含：回环节点对中两个节点之间的回环误差。
本发明实施例中，在累计分段优化过程中，根据各第二GPS约束节点的GPS数据建立GPS位置约束，并基于回环节点对对应的三维点云之间刚体变换矩阵建立回环约束。
具体的，构建第二最小优化误差函数，第二最小优化误差函数包括第三优化误差、第四优化误差和第五优化误差。其中第三优化误差包含：初始优化轨迹中各个相邻节点间的相对位姿误差，第四优化误差包含：每个第二GPS约束节点与第二标准节点间的相对位置误差，第五优化误差包含：回环节点对中两个节点之间的回环误差。
上述第三优化误差和第四优化误差可以参见第一最小优化误差函数的相关描述，不再赘述。
对于第五优化误差，即回环节点对中两个节点之间的回环误差，可以表示为：
Figure BDA0002614886420000211
在本发明的一种实施例中，回环误差对待优化的位姿的求导过程也可以参见步骤S304中求解第一最小优化误差函数的相关描述，不再赘述。
在本发明的一种实施例中，所构建的第二最小优化误差函数可以为：
Figure BDA0002614886420000212
其中，E2表示第二最小优化误差函数，ξ2表示初始优化轨迹中所有节点的位姿的李代数，i、j和k表示节点的序号，eij表示节点i和节点j之间的相对位姿误差，
Figure BDA0002614886420000213
表示预设的第一信息矩阵，Ξ表示相邻节点对和一阶跳边节点对的集合，ei2表示第二GPS约束节点与第二标准节点间的相对位置误差，
Figure BDA0002614886420000214
表示预设的第二信息矩阵，U2表示第二GPS约束节点的集合，ejk表示回环节点对中节点j和节点k之间的回环误差，G表示回环节点对的集合，
Figure BDA0002614886420000215
表示预设的第三信息矩阵，(·)T表示矩阵转置。
第一信息矩阵
Figure BDA0002614886420000216
第二信息矩阵
Figure BDA0002614886420000217
和第三信息矩阵
Figure BDA0002614886420000218
可以根据实际需求进行设定。作为一个示例，第一信息矩阵
Figure BDA0002614886420000219
可以比第二信息矩阵
Figure BDA00026148864200002110
大4-5个数量级，以尽可能的保证局部轨迹的平滑。此外，第三信息矩阵
Figure BDA00026148864200002111
可以比第一信息矩阵
Figure BDA00026148864200002112
大1-2个数量级，从而尽可能提高轨迹在回环点处的精度。
节点i和节点j之间的相对位姿误差eij通过如下公式进行计算：
Figure BDA0002614886420000221
其中，ΔTij表示根据节点i和节点j的优化前的位姿计算出的节点i和节点j之间的相对位姿，Ti表示节点i的待优化的位姿，Tj表示节点j的待优化的位姿；ξij表示ΔTij的李代数，ξi表示Ti的李代数，ξj表示Tj的李代数；
第二GPS约束节点i与第二标准节点间的相对位置误差ei2通过如下公式进行计算：
ei2＝T′i2p-[xi2,yi2,zi2,1]T，p＝[0,0,0,1]T；
其中，T′i2表示第二GPS约束节点i与第二标准节点间的相对位姿，(xi2,yi2,zi2)表示第二GPS约束节点i与第二标准节点间的相对位置的空间坐标；
回环节点对中节点j和节点k之间的回环误差ejk通过如下公式进行计算：
Figure BDA0002614886420000222
其中，
Figure BDA0002614886420000223
是节点j和节点k的三维点云之间的刚体变换矩阵，Tj表示节点j的待优化的位姿，Tk表示节点k的待优化的位姿；ξjk表示
Figure BDA0002614886420000224
的李代数，ξj表示Tj的李代数，ξk表示Tk的李代数。
S904：求解第二最小优化误差函数，确定初始优化轨迹中各节点的全局优化后位姿，并根据各节点的全局优化后位姿确定绝对轨迹。
本发明实施例中，求解第二最小优化误差函数，即可确定初始优化轨迹中各轨迹节点的全局优化后位姿。
具体的，可以采用非线性优化方法进行求解，例如采用非线性最小二乘法、一阶梯度和二阶梯度法等。
本发明实施例中，初始优化轨迹中未加入回环约束，因此在初始优化轨迹的回环点处存在一定的位姿偏差，经过全局优化后能够有效修正位姿偏差，得到更为精确的绝对轨迹。
举例来讲，参见图10(a)和图10(b)，图10(a)是经过累计分段优化后的初始优化轨迹，其轨迹的十字路口存在高程错位，即原本在同一水平面的十字路口出现了高度上的错位；图10(b)是经过全局优化后的绝对轨迹，十字路口的高程错位得到了有效的修正，说明轨迹精度进一步提升。
本领域技术人员可以理解，轨迹节点间的相对位姿能够表示采集小车在一定时间内的运动，因此，通过优化后的各个节点的位姿，可以还原出采集小车的运动轨迹，由于在轨迹优化的过程中添加了GPS位置约束，使得优化后的轨迹节点能够满足GPS位置约束，换言之，全局优化后得到的轨迹是可以直接用于构建高精度地图的绝对轨迹。
为了便于理解，下面结合附图图11，对本发明实施例提供的SLAM绝对轨迹生成方法进行进一步介绍。
如图11所示，后端设备接收前端里程计输出的目标轨迹的节点数据，其中节点数据包括：节点的时间戳、各节点的三维点云、各节点的GPS数据和各节点的位姿数据，GPS数据包括经度、纬度、高度和GPS有效标识；基于节点数据，结合GPS数据进行累计分段优化，得到初始优化轨迹；从初始优化轨迹中提取轨迹拐点和轨迹交叉点；再针对轨迹拐点和轨迹交叉点进行邻近搜索，得到回环节点对，构建回环约束。综合考虑节点间位姿约束、GPS位置约束和回环约束，进行全局优化，得到绝对轨迹。
本发明实施例还提供了一种轨迹优化装置，参见图12，装置包括以下模块：
获取模块1201，用于获取目标轨迹的节点数据，节点数据包括:目标轨迹中各节点的时间戳、各节点的三维点云数据、各节点的GPS数据和各节点的位姿数据；
分段模块1202，用于按照各节点的时间戳的先后顺序将各节点划分到多条累计分段轨迹中，各累计分段轨迹中包含的第一个节点均为目标轨迹中的初始节点，各累计分段轨迹中包含上一累计分段轨迹中的所有节点和相对于上一累计分段轨迹新增的节点，且各累计分段轨迹相对于上一累计分段轨迹中新增的节点中包含至少一个GPS有效节点；
第一优化模块1203，用于基于各节点的位姿数据和各节点的GPS数据，依次对每个累计分段轨迹中的节点进行位姿优化，直到完成目标轨迹中各节点的位姿优化，将优化后的目标轨迹作为初始优化轨迹；
第二优化模块1204，用于基于初始优化轨迹中各节点的三维点云数据、位姿数据以及GPS有效节点的GPS数据，对初始优化轨迹进行全局回环优化，得到绝对轨迹。
应用本发明实施例提供的轨迹优化方法，以节点的GPS数据为约束，依次优化各个累计分段轨迹中节点的位姿，可逐步优化整条轨迹，得到接近全局最优的初始优化轨迹。在初始优化轨迹的基础上，结合节点的三维点云数据、位姿数据和GPS数据，进行全局回环优化，由于在轨迹优化的过程中结合了轨迹节点的GPS数据，能够得到可直接用于构建高精度地图的绝对轨迹。
基于相同的发明构思，根据上述轨迹优化方法实施例，本发明实施例还提供了一种电子设备，如图13所示，包括处理器1301、通信接口1302、存储器1303和通信总线1304，其中，处理器1301，通信接口1302，存储器1303通过通信总线1304完成相互间的通信，
存储器1303，用于存放计算机程序；
处理器1301，用于执行存储器1303上所存放的程序时，实现如下步骤：
获取目标轨迹的节点数据，节点数据包括:目标轨迹中各节点的时间戳、各节点的三维点云数据、各节点的GPS数据和各节点的位姿数据；
按照各节点的时间戳的先后顺序将各节点划分到多条累计分段轨迹中，各累计分段轨迹中包含的第一个节点均为目标轨迹中的初始节点，各累计分段轨迹中包含上一累计分段轨迹中的所有节点和相对于上一累计分段轨迹新增的节点，且各累计分段轨迹相对于上一累计分段轨迹中新增的节点中包含至少一个GPS有效节点；
基于各节点的位姿数据和各节点的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数据为约束，依次优化各个累计分段轨迹中节点的位姿，可逐步优化整条轨迹，得到接近全局最优的初始优化轨迹。在初始优化轨迹的基础上，结合节点的三维点云数据、位姿数据和GPS数据，进行全局回环优化，由于在轨迹优化的过程中结合了轨迹节点的GPS数据，能够得到可直接用于构建高精度地图的绝对轨迹。
在本发明提供的又一实施例中，还提供了一种计算机可读存储介质，该计算机可读存储介质内存储有计算机程序，计算机程序被处理器执行时实现上述任一轨迹优化方法的步骤。
在本发明提供的又一实施例中，还提供了一种包含指令的计算机程序产品，当其在计算机上运行时，使得计算机执行上述实施例中任一轨迹优化方法的步骤。
在上述实施例中，可以全部或部分地通过软件、硬件、固件或者其任意组合来实现。当使用软件实现时，可以全部或部分地以计算机程序产品的形式实现。所述计算机程序产品包括一个或多个计算机指令。在计算机上加载和执行所述计算机程序指令时，全部或部分地产生按照本发明实施例所述的流程或功能。所述计算机可以是通用计算机、专用计算机、计算机网络、或者其他可编程装置。所述计算机指令可以存储在计算机可读存储介质中，或者从一个计算机可读存储介质向另一个计算机可读存储介质传输，例如，所述计算机指令可以从一个网站站点、计算机、服务器或数据中心通过有线(例如同轴电缆、光纤、数字用户线(DSL))或无线(例如红外、无线、微波等)方式向另一个网站站点、计算机、服务器或数据中心进行传输。所述计算机可读存储介质可以是计算机能够存取的任何可用介质或者是包含一个或多个可用介质集成的服务器、数据中心等数据存储设备。所述可用介质可以是磁性介质，(例如，软盘、硬盘、磁带)、光介质(例如，DVD)、或者半导体介质(例如固态硬盘Solid State Disk(SSD))等。
需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
本说明书中的各个实施例均采用相关的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于轨迹优化装置、电子设备、计算机可读存储介质及计算机程序产品实施例而言，由于其基本相似于轨迹优化方法实施例，所以描述的比较简单，相关之处参见轨迹优化方法实施例的部分说明即可。
以上所述仅为本发明的较佳实施例，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内所作的任何修改、等同替换、改进等，均包含在本发明的保护范围内。

Claims (9)

1.一种轨迹优化方法，其特征在于，所述方法包括：
获取目标轨迹的节点数据，所述节点数据包括:目标轨迹中各节点的时间戳、各节点的三维点云数据、各节点的GPS数据和各节点的位姿数据；
按照各所述节点的时间戳的先后顺序将各所述节点划分到多条累计分段轨迹中，各累计分段轨迹中包含的第一个节点均为所述目标轨迹中的初始节点，各累计分段轨迹中包含上一累计分段轨迹中的所有节点和相对于上一累计分段轨迹新增的节点，且各累计分段轨迹相对于上一累计分段轨迹中新增的节点中包含至少一个GPS有效节点；
基于各所述节点的位姿数据和各所述节点的GPS数据，依次对每个累计分段轨迹中的节点进行位姿优化，直到完成所述目标轨迹中各节点的位姿优化，将优化后的所述目标轨迹作为初始优化轨迹；
基于所述初始优化轨迹中各节点的三维点云数据、位姿数据以及所述GPS有效节点的GPS数据，对所述初始优化轨迹进行全局回环优化，得到绝对轨迹；
所述基于各所述节点的位姿数据和各所述节点的GPS数据，依次对每个累计分段轨迹中的节点进行位姿优化的步骤，包括：
针对当前待优化的累计分段轨迹，确定第一GPS约束节点；所述第一GPS约束节点为当前待优化的累计分段轨迹相对于上一累计分段轨迹新增节点中GPS数据有效的节点；
针对每一所述第一GPS约束节点，基于该第一GPS约束节点的GPS数据和第一标准节点的GPS数据，计算该第一GPS约束节点与所述第一标准节点的相对位置；所述第一标准节点为预先确定的位于第一条累计分段轨迹且GPS数据有效的一个节点；
固定当前待优化的累计分段轨迹中包含的上一累计分段轨迹中所有节点的位姿，以当前待优化的累计分段轨迹相对于上一累计分段轨迹中新增的节点的位姿作为优化变量，构建第一最小优化误差函数；所述第一最小优化误差函数包括第一优化误差和第二优化误差；其中，所述第一优化误差包含：所述当前待优化的累计分段轨迹中各个相邻节点间的相对位姿误差，所述第二优化误差包含：每个所述第一GPS约束节点与所述第一标准节点间的相对位置误差；
求解所述第一最小优化误差函数，确定当前待优化的累计分段轨迹相对于上一累计分段轨迹中新增的节点的优化后位姿。
2.根据权利要求1所述的方法，其特征在于，所述第一最小优化误差函数为：
Figure FDA0003109761740000021
其中，E1表示所述第一最小优化误差函数，ξ1表示当前待优化的累计分段轨迹相对于上一累计分段轨迹中新增的节点的位姿的李代数，i和j表示节点的序号，eij表示节点i和节点j之间的相对位姿误差，
Figure FDA0003109761740000022
表示预设的第一信息矩阵，Ξ表示相邻节点对和一阶跳边节点对的集合，ei1表示所述第一GPS约束节点与所述第一标准节点间的相对位置误差，U1表示第一GPS约束节点的集合，
Figure FDA0003109761740000023
表示预设的第二信息矩阵，(·)T表示矩阵转置；
其中，节点i和节点j之间的相对位姿误差eij通过如下公式进行计算：
Figure FDA0003109761740000024
其中，ΔTij表示根据节点i和节点j的优化前的位姿计算出的节点i和节点j之间的相对位姿，Ti表示节点i的待优化的位姿，Tj表示节点j的待优化的位姿；ξij表示ΔTij的李代数，ξi表示Ti的李代数，ξj表示Tj的李代数；(·)∧表示李代数运算中向量转矩阵运算，(·)∨表示李代数运算中矩阵转向量运算；
所述第一GPS约束节点i与所述第一标准节点间的相对位置误差ei1通过如下公式进行计算：
ei1＝T′i1p-[xi1,yi1,zi1,1]T，p＝[0,0,0,1]T；
其中，T′i1表示所述第一GPS约束节点i与所述第一标准节点间的相对位姿，(xi1,yi1,zi1)表示所述第一GPS约束节点i与所述第一标准节点间的相对位置的空间坐标。
3.根据权利要求1所述的方法，其特征在于，所述基于所述初始优化轨迹中各节点的三维点云数据、位姿数据以及所述GPS有效节点的GPS数据，对所述初始优化轨迹进行全局回环优化，得到绝对轨迹的步骤，包括：
对所述初始优化轨迹进行回环检测，确定回环节点对；每一所述回环节点对包含两个互为回环关系的节点；
对每个回环节点对包含的两个节点进行三维点云配准，得到各回环节点对中两个节点的三维点云之间的刚体变换矩阵；
基于所述初始优化轨迹中各节点的位姿数据、所述GPS有效节点的GPS数据以及各回环节点对对应的三维点云之间的刚体变换矩阵，对所述初始优化轨迹进行全局优化，得到绝对轨迹。
4.根据权利要求3所述的方法，其特征在于，所述对所述初始优化轨迹进行回环检测，确定回环节点对的步骤，包括：
从所述初始优化轨迹中提取待匹配节点，所述待匹配节点包括轨迹拐点和轨迹交叉点；
针对每一待匹配节点，从所述初始优化轨迹中确定与所述待匹配节点距离不超过预设距离阈值的候选匹配节点，并从所述候选匹配节点中找到与所述待匹配节点的时间戳间隔大于预设第一时间间隔，且与所述待匹配节点距离最近的节点，作为所述待匹配节点的回环匹配节点；
将每一完成匹配的待匹配节点与其匹配到的回环匹配节点确定为一回环节点对。
5.根据权利要求4所述的方法，其特征在于，所述从所述初始优化轨迹中提取待匹配节点的步骤，包括：
针对所述初始优化轨迹中的每个节点，判断该节点与前后预设间隔的节点构成的夹角是否大于预设夹角阈值；若大于，则将该节点确定为轨迹拐点；
针对所述初始优化轨迹中的每个节点，判断该节点与该节点的临近节点之间的时间戳间隔是否大于预设第二时间间隔；若大于，则将该节点确定为轨迹交叉点；
将所述轨迹拐点和所述轨迹交叉点确定为待匹配节点。
6.根据权利要求3所述的方法，其特征在于，所述基于所述初始优化轨迹中各节点的位姿数据、所述GPS有效节点的GPS数据以及各回环节点对对应的三维点云之间的刚体变换矩阵，对所述初始优化轨迹进行全局优化，得到绝对轨迹的步骤，包括：
确定所述初始优化轨迹中的第二标准节点和第二GPS约束节点；所述第二标准节点为预先确定的所述初始优化轨迹中GPS有效的一个节点，所述第二GPS约束节点为所述初始优化轨迹中除所述第二标准节点之外的GPS有效的节点；
针对每一所述第二GPS约束节点，基于该第二GPS约束节点的GPS数据和所述第二标准节点的GPS数据，计算该第二GPS约束节点与所述第二标准节点的相对位置；
以所述初始优化轨迹中所有节点的位姿作为优化变量，构建第二最小优化误差函数；所述第二最小优化误差函数包括第三优化误差、第四优化误差和第五优化误差；其中，所述第三优化误差包含：所述初始优化轨迹中各个相邻节点间的相对位姿误差，所述第四优化误差包含：每个所述第二GPS约束节点与所述第二标准节点间的相对位置误差，所述第五优化误差包含：所述回环节点对中两个节点之间的回环误差；
求解所述第二最小优化误差函数，确定所述初始优化轨迹中各节点的全局优化后位姿，并根据各节点的全局优化后位姿确定绝对轨迹。
7.根据权利要求6所述的方法，其特征在于，所述第二最小优化误差函数为：
Figure FDA0003109761740000041
其中，E2表示第二最小优化误差函数，ξ2表示所述初始优化轨迹中所有节点的位姿的李代数，i、j和k表示节点的序号，eij表示节点i和节点j之间的相对位姿误差，
Figure FDA0003109761740000042
表示预设的第一信息矩阵，Ξ表示相邻节点对和一阶跳边节点对的集合，ei2表示所述第二GPS约束节点与所述第二标准节点间的相对位置误差，
Figure FDA0003109761740000043
表示预设的第二信息矩阵，U2表示第二GPS约束节点的集合，ejk表示回环节点对中节点j和节点k之间的回环误差，G表示回环节点对的集合，
Figure FDA0003109761740000051
表示预设的第三信息矩阵，(·)T表示矩阵转置；
其中，节点i和节点j之间的相对位姿误差eij通过如下公式进行计算：
Figure FDA0003109761740000052
其中，ΔTij表示根据节点i和节点j的优化前的位姿计算出的节点i和节点j之间的相对位姿，Ti表示节点i的待优化的位姿，Tj表示节点j的待优化的位姿；ξij表示ΔTij的李代数，ξi表示Ti的李代数，ξj表示Tj的李代数；(·)∧表示李代数运算中向量转矩阵运算，(·)∨表示李代数运算中矩阵转向量运算；
所述第二GPS约束节点i与所述第二标准节点间的相对位置误差ei2通过如下公式进行计算：
ei2＝T′i2p-[xi2,yi2,zi2,1]T，p＝[0,0,0,1]T；
其中，T′i2表示所述第二GPS约束节点i与所述第二标准节点间的相对位姿，(xi2,yi2,zi2)表示所述第二GPS约束节点i与所述第二标准节点间的相对位置的空间坐标；
所述回环节点对中节点j和节点k之间的回环误差ejk通过如下公式进行计算：
Figure FDA0003109761740000053
其中
Figure FDA0003109761740000054
是节点j和节点k的三维点云之间的刚体变换矩阵，Tj表示节点j的待优化的位姿，Tk表示节点k的待优化的位姿；ξjk表示
Figure FDA0003109761740000055
的李代数，ξj表示Tj的李代数，ξk表示Tk的李代数。
8.一种轨迹优化装置，其特征在于，所述装置包括：
获取模块，用于获取目标轨迹的节点数据，所述节点数据包括:目标轨迹中各节点的时间戳、各节点的三维点云数据、各节点的GPS数据和各节点的位姿数据；
分段模块，用于按照各所述节点的时间戳的先后顺序将各所述节点划分到多条累计分段轨迹中，各累计分段轨迹中包含的第一个节点均为所述目标轨迹中的初始节点，各累计分段轨迹中包含上一累计分段轨迹中的所有节点和相对于上一累计分段轨迹新增的节点，且各累计分段轨迹相对于上一累计分段轨迹中新增的节点中包含至少一个GPS有效节点；
第一优化模块，用于基于各所述节点的位姿数据和各所述节点的GPS数据，依次对每个累计分段轨迹中的节点进行位姿优化，直到完成所述目标轨迹中各节点的位姿优化，将优化后的所述目标轨迹作为初始优化轨迹；
第二优化模块，用于基于所述初始优化轨迹中各节点的三维点云数据、位姿数据以及所述GPS有效节点的GPS数据，对所述初始优化轨迹进行全局回环优化，得到绝对轨迹；
所述第一优化模块，具体用于：
针对当前待优化的累计分段轨迹，确定第一GPS约束节点；所述第一GPS约束节点为当前待优化的累计分段轨迹相对于上一累计分段轨迹新增节点中GPS数据有效的节点；
针对每一所述第一GPS约束节点，基于该第一GPS约束节点的GPS数据和第一标准节点的GPS数据，计算该第一GPS约束节点与所述第一标准节点的相对位置；所述第一标准节点为预先确定的位于第一条累计分段轨迹且GPS数据有效的一个节点；
固定当前待优化的累计分段轨迹中包含的上一累计分段轨迹中所有节点的位姿，以当前待优化的累计分段轨迹相对于上一累计分段轨迹中新增的节点的位姿作为优化变量，构建第一最小优化误差函数；所述第一最小优化误差函数包括第一优化误差和第二优化误差；其中，所述第一优化误差包含：所述当前待优化的累计分段轨迹中各个相邻节点间的相对位姿误差，所述第二优化误差包含：每个所述第一GPS约束节点与所述第一标准节点间的相对位置误差；
求解所述第一最小优化误差函数，确定当前待优化的累计分段轨迹相对于上一累计分段轨迹中新增的节点的优化后位姿。
9.一种电子设备，其特征在于，包括处理器、通信接口、存储器和通信总线，其中，处理器，通信接口，存储器通过通信总线完成相互间的通信；
存储器，用于存放计算机程序；
处理器，用于执行存储器上所存放的程序时，实现权利要求1-7任一所述的方法步骤。
CN202010766551.4A 2020-08-03 2020-08-03 一种轨迹优化方法及装置 Active CN111862163B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010766551.4A CN111862163B (zh) 2020-08-03 2020-08-03 一种轨迹优化方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010766551.4A CN111862163B (zh) 2020-08-03 2020-08-03 一种轨迹优化方法及装置

Publications (2)

Publication Number Publication Date
CN111862163A CN111862163A (zh) 2020-10-30
CN111862163B true CN111862163B (zh) 2021-07-23

Family

ID=72952746

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010766551.4A Active CN111862163B (zh) 2020-08-03 2020-08-03 一种轨迹优化方法及装置

Country Status (1)

Country Link
CN (1) CN111862163B (zh)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107272673A (zh) * 2017-05-18 2017-10-20 中山大学 基于位姿链模型的slam后端轨迹优化方法
CN107356252A (zh) * 2017-06-02 2017-11-17 青岛克路德机器人有限公司 一种融合视觉里程计与物理里程计的室内机器人定位方法
CN109341694A (zh) * 2018-11-12 2019-02-15 哈尔滨理工大学 一种移动探测机器人的自主定位导航方法
CN109343540A (zh) * 2018-11-30 2019-02-15 广东工业大学 一种基于回环检测的slam后端轨迹优化方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10380762B2 (en) * 2016-10-07 2019-08-13 Vangogh Imaging, Inc. Real-time remote collaboration and virtual presence using simultaneous localization and mapping to construct a 3D model and update a scene based on sparse data

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107272673A (zh) * 2017-05-18 2017-10-20 中山大学 基于位姿链模型的slam后端轨迹优化方法
CN107356252A (zh) * 2017-06-02 2017-11-17 青岛克路德机器人有限公司 一种融合视觉里程计与物理里程计的室内机器人定位方法
CN109341694A (zh) * 2018-11-12 2019-02-15 哈尔滨理工大学 一种移动探测机器人的自主定位导航方法
CN109343540A (zh) * 2018-11-30 2019-02-15 广东工业大学 一种基于回环检测的slam后端轨迹优化方法

Also Published As

Publication number Publication date
CN111862163A (zh) 2020-10-30

Similar Documents

Publication Publication Date Title
CN111028277B (zh) 基于伪孪生卷积神经网络的sar和光学遥感图像配准方法
WO2021232470A1 (zh) 基于多传感器融合的slam制图方法、系统
CN113168717B (zh) 一种点云匹配方法及装置、导航方法及设备、定位方法、激光雷达
CN112268559B (zh) 复杂环境下融合slam技术的移动测量方法
CN112084289B (zh) 一种轨迹融合方法及装置
Cui et al. Efficient large-scale structure from motion by fusing auxiliary imaging information
CN113989451B (zh) 高精地图构建方法、装置及电子设备
CN112183171A (zh) 一种基于视觉信标建立信标地图方法、装置
CN114964212B (zh) 面向未知空间探索的多机协同融合定位与建图方法
CN114120149B (zh) 一种倾斜摄影测量建筑物特征点提取方法、装置、电子设备及介质
CN112861341A (zh) 一种结合形状和环境特征的多尺度面要素匹配方法及系统
Fakhfakh et al. Bayesian curved lane estimation for autonomous driving
Lu et al. A lightweight real-time 3D LiDAR SLAM for autonomous vehicles in large-scale urban environment
CN111862163B (zh) 一种轨迹优化方法及装置
CN115239776B (zh) 点云的配准方法、装置、设备和介质
Liu et al. An iterative approach based on contextual information for matching multi‐scale polygonal object datasets
WO2022252482A1 (zh) 机器人及其环境地图构建方法和装置
Wang et al. Feature-based visual navigation integrity monitoring for urban autonomous platforms
CN108763817B (zh) 一种基于最小二乘法建模的电力地下管网匹配方法
Xue et al. Visual-Marker Based Localization for Flat-Variation Scene
CN113899357B (zh) 视觉slam的增量建图方法、装置、机器人及可读存储介质
Li et al. Geometric analysis of reality-based indoor 3D mapping
CN112365541B (zh) 一种基于相似变换的大场景相机姿态注册方法
Liu et al. A building polygonal object matching method based on minimum bounding rectangle combinatorial optimization and relaxation labeling
Chehreghan et al. Estimation of empirical parameters in matching of linear vector datasets: an optimization approach

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