[发明专利]一种融合码盘和激光雷达的里程计方法与建图方法

权利要求书

1.一种融合码盘和激光雷达的里程计方法，其特征在于，包括以下步骤：

通过车轮编码器和方向盘转角传感器获取车轮里程计信息；

通过激光雷达获得点云数据，对该点云数据进行格式转换，获得激光扫描序列；

对所述激光扫描序列进行特征点提取，所述特征点包括平面点和边缘点；

以所述车轮里程计信息作为优化初值，基于所述特征点实现位姿优化，获得激光里程计结果，作为最终的里程计结果；

采用基于角度的几何方法对所述激光扫描序列进行特征点提取；

所述基于角度的几何方法具体为：

通过以下公式计算激光数据点M的角度corner angle：

其中，θ_l和θ_r为由激光雷达光心到点M的射线与其到点L和点R的夹角，点L和点R为点M的临近点，m为点L和点R的组数，下标k表示第k组，r_m、r_l、r_r分别表示激光雷达光心到点M的距离、激光雷达光心到点L的距离和激光雷达光心到点R的距离；

若corner angle小于第一预设阈值angle_planar，则判定对应的点M为平面点，若cornerangle大于第二预设阈值angle_corner，则判定对应的点M为边缘点；

采用Levenberg-Marquardt方法实现所述位姿优化；

所述位姿优化采用的优化公式为：

其中，为点线因子，为点面因子，||d_de||∑_den为退化因子，||·||_∑表示由协方差矩阵∑参数化的马氏距离，为优化后的位姿。

2.根据权利要求1所述的融合码盘和激光雷达的里程计方法，其特征在于，所述车轮里程计信息基于阿克曼转向几何学获取。

3.根据权利要求1所述的融合码盘和激光雷达的里程计方法，其特征在于，获得点云数据后，对点云数据进行去畸变处理。

4.根据权利要求1所述的融合码盘和激光雷达的里程计方法，其特征在于，进行所述位姿优化时，以三元组对所提取的特征进行参数化，其中，所述三元组采用PCA算法基于特征点的最近邻点计算获得。

5.根据权利要求1所述的融合码盘和激光雷达的里程计方法，其特征在于，所述点线因子和退化因子根据待优化点云帧的边缘点和边缘点特征地图构建，点面因子根据待优化点云帧的平面点和平面特征地图构建。

6.一种融合码盘和激光雷达的建图方法，其特征在于，包括以下步骤：

采用如权利要求1-5任一所述的里程计方法获得激光里程计结果；

基于因子图，以激光里程计结果作为激光里程计因子，融合回环因子和可选的GPS因子对姿态进行优化，通过增量平滑和建图算法在线生成全局地图。