一种轨迹优化方法及装置制造方法及图纸

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

【技术实现步骤摘要】
一种轨迹优化方法及装置
本专利技术涉及定位导航
，特别是涉及一种轨迹优化方法及装置。
技术介绍
即时定位与建图(SimultaneousLocalizationandMapping,SLAM)技术作为一种前沿技术被广泛应用于自动构建大规模高精度地图。相比于视觉SLAM系统，利用多线激光雷达作为主要传感器的激光SLAM系统在测距精度和感知范围上都有明显的优势。激光SLAM系统分为前端里程计与后端优化模块，其中前端里程计主要通过三维点云配准技术估计相邻帧的相对位姿变换关系，后端则利用回环检测对整个轨迹进行优化。然而，通过现有的SLAM技术得到的轨迹只是一条相对轨迹，换言之，采用该轨迹构建出的地图与真实世界存在一个未知的刚体变换，因此不能直接用于构建高精度地图。
技术实现思路
本专利技术实施例的目的在于提供一种轨迹优化方法及装置，以实现生成可直接用于构建高精度地图的绝对轨迹。具体技术方案如下：为实现上述目的，本专利技术实施例提供了一种轨迹优化方法，所述方法包括：获取目标轨迹的节点数据，所述节点本文档来自技高网...

【技术保护点】
1.一种轨迹优化方法，其特征在于，所述方法包括：/n获取目标轨迹的节点数据，所述节点数据包括:目标轨迹中各节点的时间戳、各节点的三维点云数据、各节点的GPS数据和各节点的位姿数据；/n按照各所述节点的时间戳的先后顺序将各所述节点划分到多条累计分段轨迹中，各累计分段轨迹中包含的第一个节点均为所述目标轨迹中的初始节点，各累计分段轨迹中包含上一累计分段轨迹中的所有节点和相对于上一累计分段轨迹新增的节点，且各累计分段轨迹相对于上一累计分段轨迹中新增的节点中包含至少一个GPS有效节点；/n基于各所述节点的位姿数据和各所述节点的GPS数据，依次对每个累计分段轨迹中的节点进行位姿优化，直到完成所述目标轨迹中...

【技术特征摘要】
1.一种轨迹优化方法，其特征在于，所述方法包括：
获取目标轨迹的节点数据，所述节点数据包括:目标轨迹中各节点的时间戳、各节点的三维点云数据、各节点的GPS数据和各节点的位姿数据；
按照各所述节点的时间戳的先后顺序将各所述节点划分到多条累计分段轨迹中，各累计分段轨迹中包含的第一个节点均为所述目标轨迹中的初始节点，各累计分段轨迹中包含上一累计分段轨迹中的所有节点和相对于上一累计分段轨迹新增的节点，且各累计分段轨迹相对于上一累计分段轨迹中新增的节点中包含至少一个GPS有效节点；
基于各所述节点的位姿数据和各所述节点的GPS数据，依次对每个累计分段轨迹中的节点进行位姿优化，直到完成所述目标轨迹中各节点的位姿优化，将优化后的所述目标轨迹作为初始优化轨迹；
基于所述初始优化轨迹中各节点的三维点云数据、位姿数据以及所述GPS有效节点的GPS数据，对所述初始优化轨迹进行全局回环优化，得到绝对轨迹。


2.根据权利要求1所述的方法，其特征在于，所述基于各所述节点的位姿数据和各所述节点的GPS数据，依次对每个累计分段轨迹中的节点进行位姿优化的步骤，包括：
针对当前待优化的累计分段轨迹，确定第一GPS约束节点；所述第一GPS约束节点为当前待优化的累计分段轨迹相对于上一累计分段轨迹新增节点中GPS数据有效的节点；
针对每一所述第一GPS约束节点，基于该第一GPS约束节点的GPS数据和所述第一标准节点的GPS数据，计算该第一GPS约束节点与所述第一标准节点的相对位置；所述第一标准节点为预先确定的位于第一条累计分段轨迹且GPS数据有效的一个节点；
固定当前待优化的累计分段轨迹中包含的上一累计分段轨迹中所有节点的位姿，以当前待优化的累计分段轨迹相对于上一累计分段轨迹中新增的节点的位姿作为优化变量，构建第一最小优化误差函数；所述第一最小优化误差函数包括第一优化误差和第二优化误差；其中，所述第一优化误差包含：所述当前待优化的累计分段轨迹中各个相邻节点间的相对位姿误差，所述第二优化误差包含：每个所述第一GPS约束节点与所述第一标准节点间的相对位置误差；
求解所述第一最小优化误差函数，确定当前待优化的累计分段轨迹相对于上一累计分段轨迹中新增的节点的优化后位姿。


3.根据权利要求2所述的方法，其特征在于，所述第一最小优化误差函数为：



其中，E1表示所述第一最小优化误差函数，ξ1表示当前待优化的累计分段轨迹相对于上一累计分段轨迹中新增的节点的位姿的李代数，i和j表示节点的序号，eij表示节点i和节点j之间的相对位姿误差，表示预设的第一信息矩阵，Ξ表示相邻节点对和一阶跳边节点对的集合，ei1表示所述第一GPS约束节点与所述第一标准节点间的相对位置误差，U1表示第一GPS约束节点的集合，表示预设的第二信息矩阵，(·)T表示矩阵转置；
其中，节点i和节点j之间的相对位姿误差eij通过如下公式进行计算：



其中，Δ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与所述第一标准节点间的相对位置的空间坐标。


4.根据权利要求1所述的方法，其特征在于，所述基于所述初始优化轨迹中各节点的三维点云数据、位姿数据以及所述GPS有效节点的GPS数据，对所述初始优化轨迹进行全局回环优化，得到绝对轨迹的步骤，包括：
对所述初始优化轨迹进行回环检测，确定回环节点对；每一所述回环节点对包含两个互为回环关系的节点；
对每个回环节点对包含的两个节点进行三维点云配准，得到各回环节点对中两个节点的三维点云之间的刚体变换矩阵；
基于所述初始优化轨迹中各节点的位姿数据、所述GPS有效节点的GPS数据以及各回环节点对对应的三维点云之间的刚体变换矩阵，对所述初始优化轨迹进行全局优化，得到绝对轨迹。


5.根据权利要求4所述的方法，其特征在于，所述对所述初始优化轨迹进行回环检测，确定回环节点对的步骤，包括：
从所述初始优化轨迹中提取待匹配节点，所述待匹配节点包括轨迹拐点和轨迹交叉点；
针对每一待匹配节点，从所述初始优化轨迹中确定与所述待匹配节点距离不超过预设距离阈值的候选匹配节点，并从所述候选匹配节点中找到与所述待匹配节点的时间戳间隔大于预设第一时间间隔，且与所述待匹配节点距离最近的节点，作为所述待匹配节点的回环匹配节点；
将每一完成匹配的待匹配节点与其匹配到的回环匹配节点确定为一回环节点对。


6...

【专利技术属性】
技术研发人员：鲁荣荣，杨再甫，林宝尉，王彦哲，
申请(专利权)人：湖北亿咖通科技有限公司，
类型：发明
国别省市：湖北;42