[发明专利]一种轨迹对齐方法及电子设备

权利要求书

1.一种轨迹对齐方法，其特征在于，包括：

获取第一轨迹和第二轨迹，所述第一轨迹和所述第二轨迹中均包括多个节点及每个节点的位姿和三维点云；

在所述第一轨迹的各个节点中，选择多个具有一定距离间隔的点确定为待关联节点；针对每个待关联节点，基于该待关联节点与所述第二轨迹中各节点的距离，在所述第二轨迹的各节点中，确定与该待关联节点具有关联关系的目标节点，其中，所述节点中包括时间戳，所述在所述第一轨迹的各个节点中，选择多个具有一定距离间隔的点确定为待关联节点，包括：根据所述时间戳的顺序，在所述第一轨迹的各个节点中，将时间最早的节点确定为当前待确定节点；判断是否存在与所述当前待确定节点具有关联关系的目标节点；若存在，则将所述当前待确定节点确定为待关联节点；在所述第一轨迹的各个节点中，确定与所确定的待关联节点之间的距离在第二预设阈值之外的节点，依次作为下一待确定节点；判断是否存在与所述下一待确定节点具有关联关系的目标节点；若存在，则将所述下一待确定节点确定为待关联节点；

将该待关联节点的三维点云与该待关联节点的邻近节点的三维点云进行融合，得到该待关联节点的三维点云地图；将所述目标节点的三维点云与所述目标节点的邻近节点的三维点云进行融合，得到所述目标节点的三维点云地图；将该待关联节点的三维点云地图与所述目标节点的三维点云地图确定为具有关联关系的三维点云地图对，其中，所述将该待关联节点的三维点云与该待关联节点的邻近节点的三维点云进行融合，得到该待关联节点的三维点云地图，包括：计算得到所述待关联节点与所述待关联节点的邻近节点之间的刚体变换矩阵，对所述待关联节点的邻近节点的三维点云进行该邻近节点对应的刚体变换，将刚体变换后的该邻近节点的三维点云与所述待关联节点的三维点云进行融合，得到所述待关联节点的三维点云地图；所述将所述目标节点的三维点云与所述目标节点的邻近节点的三维点云进行融合，得到所述目标节点的三维点云地图，包括：计算得到所述目标节点与所述目标节点的邻近节点之间的刚体变换矩阵，对所述目标节点的邻近节点的三维点云进行该邻近节点对应的刚体变换，将刚体变换后的该邻近节点的三维点云与所述目标节点的三维点云进行融合，得到所述目标节点的三维点云地图；

在可视化界面中展示所述三维点云地图对，接收用户对所展示的三维点云地图中的三维点的选择指令；根据所述选择指令，确定所述用户在三维点云地图中选择的预设数量个三维点对；其中，所述三维点对中包括该待关联节点的三维点云地图中的一个三维点与所述目标节点的三维点云地图中的一个三维点；

基于所述预设数量个三维点对，对该待关联节点的三维点云地图和所述目标节点的三维点云地图进行配准，得到该待关联节点的三维点云地图和所述目标节点的三维点云地图之间的约束关系；

基于所述约束关系，对所述第一轨迹中的各节点的位姿以及所述第二轨迹中各节点的位姿进行位姿优化，得到对齐轨迹。

2.根据权利要求1所述的方法，其特征在于，所述获取第一轨迹和第二轨迹，包括：

获取两条待处理轨迹，所述待处理轨迹中包括多个节点及每个节点的位姿，所述节点中包括以下任意一种或多种：经度、纬度、高度、姿态角；

分别计算各节点的经度的平均值、纬度的平均值以及高度的平均值，作为几何重心的经度、纬度以及高度，其中，几何重心为所有节点的中心点；

确定所述几何重心对应的子午圈主曲率半径和卯酉圈主曲率半径；

针对每个节点，基于所述几何重心的经度、纬度、高度、子午圈主曲率半径、卯酉圈主曲率半径、该节点的经度、纬度、高度以及该节点的姿态角，对该节点的位姿进行标准化，将任意一条标准化后的待处理轨迹确定为第一轨迹，将另一条标准化后的待处理轨迹确定为第二轨迹。