[发明专利]一种基于点云融合的行车场景构建方法

权利要求书

1.一种基于点云融合的行车场景构建方法，其特征在于，包括以下步骤：

步骤1，对车载激光雷达在汽车运动过程中扫描收集的点云数据进行畸变补偿；

步骤2，将畸变补偿后的点云数据进行传感器时间对齐，使点云数据在进行畸变补偿的同时，对同一时间下的里程计数据进行线性插值计算，使里程计数据向点云数据对齐时间，得到实现传感器对齐的机器人的位置姿态作为前端里程估计的初始预测位置姿态；

步骤3，创建一个任务队列用来存放点云数据，并生成一个局部地图；任务队列中每插入一帧点云数据时，输入该点云数据对应的机器人的初始预测位置姿态，与局部地图进行点云匹配，对点云数据进行正态分布变换，得到目标点云的所有单位体素，代入高斯概率模型，寻找到局部地图上与初始预测位置姿态最近似的点，得到前端里程估计的位姿预估值；

步骤4，采用基于点云的场景识别方式，将前端里程估计创建的任务队列中的点云数据用scan Context特征图进行描述；定义scan Context特征图之间的距离计算公式；从scanContext特征图中提取描述符环，将其表示为一个多维向量以进行KD树的构建，获得当前场景的环的最相似区域，从中选取和当前场景的环最相似的历史环对应的scan context作为候选场景的scan context；计算任务队列中的点云数据的scan Context和候选场景的scancontext之间的距离，满足阈值的候选场景则被认为是闭环，则将所述位姿预估值进行后端优化；

步骤5，将机器人不同时刻通过前端里程估计得到的位姿预估值抽象定义为节点，机器人在不同位置上的观测所产生的约束被抽象为节点之间的边，节点的位姿信息和路标构成图的顶点集，用边来表示观测数据的约束；

定义误差函数为两个节点之间的虚拟测量值的均值与期望的虚拟测量值的差值；通过高斯牛顿法对误差函数进行迭代求最优解，将求得的最优解叠加到初始预测位置姿态得到新的位姿预估值，将新的位姿预估值作为下一轮迭代的初始预测位置姿态，继续迭代直至位姿预估值误差最小；

步骤1-步骤5在边缘设备中进行，构建出局部地图后再上传到云服务器进行全局地图拼接，最后再将拼接好的地图回传到边缘设备供车辆进行定位操作。

2.根据权利要求1所述的基于点云融合的行车场景构建方法，其特征在于，所述点云数据进行畸变补偿，包括：

根据读取点云帧的首尾数据计算点云帧夹角，得到点云帧采样的实际时间；

利用读取进来的惯性测量单元信息获取车辆角速度和线速度，用角速度乘以时间得到车辆旋转角度；用线速度乘以时间得到车辆偏移位移，生成旋转偏移矩阵；

将原始点云数据矩阵乘上旋转偏移矩阵，得到畸变补偿后的点云数据。

3.根据权利要求1所述的基于点云融合的行车场景构建方法，其特征在于，所述实现传感器对齐的机器人的位置姿态的计算公式为：

其中，X(t_n)代表t_n点的机器人的位置姿态，X(t_o1)、X(t_o2)分别是t_o1、t_o2时刻的里程计数据。

4.根据权利要求1所述的基于点云融合的行车场景构建方法，其特征在于，点云数据的任何区域可通过环和扇形唯一索引，将点云数据用二维形式表示即得到scan Context特征图；

记两个点云的scan context特征图为I^q和I^c，定义scan Context特征图之间的距离计算公式如下：

其中，N_s代表点云中的扇形数量，分别为利用误差函数对I^q和I^c的求解结果；

对于存在同一位置但是不同时间访问的两个scan context特征图的情况，所有可能出现列平移的scan contexts特征图的列向量之和并找到最小值作为最终的距离，列平移的最佳匹配关系n^*定义为：

最小距离定义为：

其中，N_s代表点云中的扇形数量，n代表的就是其中的一个扇形。