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.根据权利要求5所述的方法,其特征在于,所述从所述初始优化轨迹中提取待匹配节点的步骤,包括:针对所述初始优化轨迹中的每个节点,判断该节点与前后预设间隔的节点构成的夹角是否大于预设夹角阈值;若大于,则将该节点确定为轨迹拐点;
针对所述初始优化轨迹中的每个节点,判断该节点与该节点的临近节点之间的时间戳间隔是否大于预设第二时间间隔;若大于,则将该节点确定为轨迹交叉点;
将所述轨迹拐点和所述轨迹交叉点确定为待匹配节点。
7.根据权利要求4所述的方法,其特征在于,所述基于所述初始优化轨迹中各节点的位姿数据、所述GPS有效节点的GPS数据以及各回环节点对对应的三维点云之间的刚体变换矩阵,对所述初始优化轨迹进行全局优化,得到绝对轨迹的步骤,包括:确定所述初始优化轨迹中的第二标准节点和第二GPS约束节点;所述第二标准节点为预先确定的所述初始优化轨迹中GPS有效的一个节点,所述第二GPS约束节点为所述初始优化轨迹中除所述第二标准节点之外的GPS有效的节点;
针对每一所述第二GPS约束节点,基于该第二GPS约束节点的GPS数据和所述第二标准节点的GPS数据,计算该第二GPS约束节点与所述第二标准节点的相对位置;
以所述初始优化轨迹中所有节点的位姿作为优化变量,构建第二最小优化误差函数;
所述第二最小优化误差函数包括第三优化误差、第四优化误差和第五优化误差;其中,所述第三优化误差包含:所述初始优化轨迹中各个相邻节点间的相对位姿误差,所述第四优化误差包含:每个所述第二GPS约束节点与所述第二标准节点间的相对位置误差,所述第五优化误差包含:所述回环节点对中两个节点之间的回环误差;
求解所述第二最小优化误差函数,确定所述初始优化轨迹中各节点的全局优化后位姿,并根据各节点的全局优化后位姿确定绝对轨迹。
8.根据权利要求7所述的方法,其特征在于,所述第二最小优化误差函数为:
其中,E2表示第二最小优化误差函数,ξ2表示所述初始优化轨迹中所有节点的位姿的李代数,i、j和k表示节点的序号,eij表示节点i和节点j之间的相对位姿误差, 表示预设的第一信息矩阵,Ξ表示相邻节点对和一阶跳边节点对的集合,ei2表示所述第二GPS约束节点与所述第二标准节点间的相对位置误差, 表示预设的第二信息矩阵,U2表示第二GPS约束节点的集合,ejk表示回环节点对中节点j和节点k之间的回环误差,G表示回环节点对的集合, 表示预设的第三信息矩阵,(·)T表示矩阵转置;
其中,节点i和节点j之间的相对位姿误差eij通过如下公式进行计算:
其中,Δ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通过如下公式进行计算:
其中 是节点j和节点k的三维点云之间的刚体变换矩阵,Tj表示节点j的待优化的位姿,Tk表示节点k的待优化的位姿;ξjk表示 的李代数,ξj表示Tj的李代数,ξk表示Tk的李代数。
9.一种轨迹优化装置,其特征在于,所述装置包括:
获取模块,用于获取目标轨迹的节点数据,所述节点数据包括:目标轨迹中各节点的时间戳、各节点的三维点云数据、各节点的GPS数据和各节点的位姿数据;
分段模块,用于按照各所述节点的时间戳的先后顺序将各所述节点划分到多条累计分段轨迹中,各累计分段轨迹中包含的第一个节点均为所述目标轨迹中的初始节点,各累计分段轨迹中包含上一累计分段轨迹中的所有节点和相对于上一累计分段轨迹新增的节点,且各累计分段轨迹相对于上一累计分段轨迹中新增的节点中包含至少一个GPS有效节点;
第一优化模块,用于基于各所述节点的位姿数据和各所述节点的GPS数据,依次对每个累计分段轨迹中的节点进行位姿优化,直到完成所述目标轨迹中各节点的位姿优化,将优化后的所述目标轨迹作为初始优化轨迹;
第二优化模块,用于基于所述初始优化轨迹中各节点的三维点云数据、位姿数据以及所述GPS有效节点的GPS数据,对所述初始优化轨迹进行全局回环优化,得到绝对轨迹。
10.一种电子设备,其特征在于,包括处理器、通信接口、存储器和通信总线,其中,处理器,通信接口,存储器通过通信总线完成相互间的通信;
存储器,用于存放计算机程序;
处理器,用于执行存储器上所存放的程序时,实现权利要求1-8任一所述的方法步骤。