[发明专利]一种基于激光雷达和先验地图的实时定位系统及方法

权利要求书

1.一种基于激光雷达和先验地图的实时定位方法，其特征在于，该方法包括以下步骤：

步骤1：进行点云注册，具体为在第t个时间戳将激光雷达的实时点云帧注册，提取为特征点云数据；第t个时间戳时的点云注册过程具体包括以下处理：

步骤1.1：去除无效点，即去除激光雷达传感器产生的点云帧中的无效点，记录原始数据时间戳t，以此时间戳作为航迹推算结果和最终车辆定位结果的时间戳；

步骤1.2：划分点云层ID，即根据点云中每个点的俯仰角α，将点云中具有相同俯仰角α的点划分到同一个层ID中；设激光雷达的垂直视角为±θ，垂直分辨率为β，线数为n，它们满足关系对其中任意一点对应的坐标值(x,y,z)，计算其俯仰角度α以及对应的层ID，表达式如下：

点云层ID不为正整数的视其为无效点，予以丢弃；

步骤1.3：计算曲率，以曲率作为提取特征点云的依据降低计算复杂度，只利用当前点p_i(x_i,y_i,z_i)及在同一层ID中其前后各5个点(p_i-5,…,p_i-1,p_i+1,…,p_i+5)的坐标差的平方和来近似估计曲率k，表达式如下：

k_x＝x_i-5+…+x_i-1+x_i+1+…+x_i+5-10x_i

k_y＝y_i-5+…+y_i-1+y_i+1+…+y_i+5-10y_i

k_z＝z_i-5+…+z_i-1+z_i+1+…+z_i+5-10z_i

其中，k_x,k_y,k_z分别表示x,y,z方向的曲率，x_i-5,…,x_i-1,x_i+1,…,x_i+5分别表示当前点前后各5个点的x坐标，y_i-5,…,y_i-1,y_i+1,…,y_i+5分别表示当前点前后各5个点的y坐标，z_i-5,…,z_i-1,z_i+1,…,z_i+5分别表示当前点前后各5个点的z坐标；

步骤1.4：提取特征点云，即根据每个点对应曲率k的大小，提取四类点云包括轮廓点云次级轮廓点云平面点云和次级平面点云之后，将所有层ID分组中的特征点分别合并，形成对应的特征点云，发布给激光雷达里程计节点和地图配准定位节点；

步骤2：进行第t个时间戳激光雷达里程计，具体为在第t个时间戳激光雷达里程计模块接收到特征点云，之后，计算连续两帧特征点的变换关系进而计算得到相对于初始位置的航迹推算

步骤3：进行地图配准定位，具体为以连续帧周期进行地图配准定位，在第k周期，获取地图片段关键帧特征点云和里程计的航迹推算结果后，通过NDT配准得出车辆相对准确的定位

步骤4：进行定位融合，即在第t个时间戳将定位频率为10Hz的航迹推算结果与1Hz的地图定位结果融合计算，得到定位频率为10Hz的车辆定位