[发明专利]一种检测激光建图里程计与组合导航定位偏差的方法及系统

权利要求书

1.一种检测激光建图里程计与组合导航定位偏差的方法，其特征在于，包括如下步骤：

(1)轨迹数据的预处理；

(2)建立偏差方程；

(3)计算轨迹点偏差值；

(4)判断并输出差值较大点坐标。

2.根据权利要求1所述的一种检测激光建图里程计与组合导航定位偏差的方法，其特征在于，

在步骤(1)中，首先建立激光点云地图，对点云进行合理的抽析，通过设定关键帧的距离来控制点云的密度，在初步建立点云地图后，通过激光匹配的方法推算地图采集平台的轨迹数据，存储的数据中包含了位置和航向信息；对组合导航进行有效性进行判断，保存其轨迹数据，其中包含了位置和航向信息。

3.根据权利要求2所述的一种检测激光建图里程计与组合导航定位偏差的方法，其特征在于，

在步骤(2)中，采用最小二乘法对激光匹配轨迹进行局部拟合，给定数据点p_i(x_i,y_i,z_i)，其中i＝1,2,…,m，求近似曲线并且使得近似曲线与y＝f(x)的偏差最小，近似曲线在点p_i处的偏差i＝1,2,...,m，曲线拟合公式如下：

建立一个激光匹配轨迹点数组队列，将其按顺序填入a个轨迹点的位置坐标，通过最小二乘公式建立直线方程，然后，在a个轨迹点头部去除1个数据，最后在尾部循环填入一个数据，以此类推会按顺序建立N个直线方程；同理在时间对齐的情况下，组合导航轨迹线也采用相同的方法按顺序建立N个直线方程。

4.根据权利要求3所述的一种检测激光建图里程计与组合导航定位偏差的方法，其特征在于，

在步骤(3)中，选择激光匹配轨迹线和组合导航轨迹线两条轨迹中一条轨迹的N个直线方程，再使用其相对轨迹线的轨迹点，通过点到直线的距离公式，即可得到两条轨迹偏差。

5.根据权利要求4所述的一种检测激光建图里程计与组合导航定位偏差的方法，其特征在于，

在步骤(4)中，将轨迹点的坐标组成一个队列，在队列中建立一个滑动窗口，将位置与航向作差后的数据按顺序取b个值放入滑动窗口中，计算滑动窗口中数据的方差并记录，窗口中第一个数据会实时弹出，并在窗口尾部按顺序加入新的航位插值，以此类推，能够得到整条轨迹线上每个点对应航位变化的方差，通过航位方差与预设阈值比对，能够判断出两条轨迹出现偏差的较大点的位置，并根据轨迹点的ID输出对应轨迹点的坐标值。

6.一种检测激光建图里程计与组合导航定位偏差的系统，其特征在于，包括：

(1)轨迹数据的预处理单元；

(2)建立偏差方程单元；

(3)计算轨迹点偏差值单元；

(4)判断并输出差值较大点坐标单元。

7.根据权利要求6所述的一种检测激光建图里程计与组合导航定位偏差的系统，其特征在于，

所述轨迹数据的预处理单元用于，首先建立激光点云地图，对点云进行合理的抽析，通过设定关键帧的距离来控制点云的密度，在初步建立点云地图后，通过激光匹配的方法推算地图采集平台的轨迹数据，存储的数据中包含了位置和航向信息；对组合导航进行有效性进行判断，保存其轨迹数据，其中包含了位置和航向信息。