CN114018248B - 一种融合码盘和激光雷达的里程计方法与建图方法 - Google Patents

Abstract

本发明涉及一种融合码盘和激光雷达的里程计方法与建图方法，包括以下步骤：通过车轮编码器和方向盘转角传感器获取车轮里程计信息；通过激光雷达获得点云数据，对该点云数据进行格式转换，获得激光扫描序列；对所述激光扫描序列进行特征点提取，所述特征点包括平面点和边缘点；以所述车轮里程计信息作为优化初值，基于所述特征点实现位姿优化，获得激光里程计结果，作为最终的里程计结果，基于因子图，融合回环因子和可选的GPS因子对姿态进行优化，通过增量平滑和建图算法在线生成全局地图。与现有技术相比，本发明具有低漂移、低延迟等优点。

Description

一种融合码盘和激光雷达的里程计方法与建图方法

技术领域

本发明涉及机器人定位技术领域，尤其是涉及一种融合码盘和激光雷达的里程计方法与建图方法。

背景技术

同步定位与建图(SLAM)是机器人领域的热门话题，近年来，自动驾驶汽车的发展给SLAM带来了新的场景和新的挑战。最重要的挑战之一是自我定位，这也是SLAM的基本问题，但它在更大范围内面临来自现实世界的各种挑战，例如光照、天气、GPS信号质量等。在这一挑战下，利用了不同类型传感器的融合方法脱颖而出，如视觉惯性法和激光雷达惯性法的组合等。

LiDAR Odometer and Mapping(LOAM)方法使用两层优化来实现高频LiDAR里程计和低频LiDAR建图和里程计校正，但是，里程表的频率受到激光雷达帧率的限制。LiDAR惯性里程计方法融合惯性测量产生更高的里程计输出频率，有助于在不假设线性运动的情况下校正点云，其性能优于LOAM，尤其是当传感器剧烈移动或旋转时。但它在一个紧密耦合的框架中工作，惯性模块和LiDAR里程计模块之间的反馈使它在点云的良好特征很少时容易失效。

特征提取是基于特征的激光雷达里程计方法的核心，它决定了精度、鲁棒性甚至计算效率。特征提取的一般过程是：将点分类为边缘点或平坦点，在对应的特征图中搜索相邻点，以最小化点对线或点对点的方式将点合并入特征图中。这种方法适用于大多数平面物体，如墙壁或地面，产生平滑的平面特征图，但一些粗糙的物体，如灌木或草地，会产生有噪声的边缘特征图，大多数方法基于相邻点的空间分布过滤边缘特征。但是，实际上，一些被遗弃的特征，例如植被的特征点，在经过体素网格过滤后可以看作是平坦的特征，因为它们在较大的邻域中看起来是平坦的，过多丢弃特征会对降低系统的鲁棒性。

SLAM系统的另一个重要问题是漂移的校正。现有技术中提出一种基于迭代最近点(ICP)的闭环方法，但是当漂移太大时很容易失败。

综上所述，有必要为自动驾驶汽车设计一个准确、鲁棒和实时的SLAM系统。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种低漂移、低延迟的融合码盘和激光雷达的里程计与建图方法。

本发明的目的可以通过以下技术方案来实现：

一种融合码盘和激光雷达的里程计方法，包括以下步骤：

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

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

对所述激光扫描序列进行特征点提取；

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

进一步地，所述车轮里程计信息基于阿克曼转向几何学获取。

进一步地，获得点云数据后，对点云数据进行去畸变处理。

进一步地，所述采用基于角度的几何方法对所述激光扫描序列进行特征点提取，所述特征点包括平面点和边缘点。

进一步地，所述基于角度的几何方法具体为：

通过以下公式计算激光数据点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为平面点，若corner angle大于第二预设阈值angle_corner，则判定对应的点M为边缘点。

进一步地，进行所述位姿优化时，以三元组对所提取的特征进行参数化，其中，所述三元组采用PCA算法基于特征点的最近邻点计算获得。

进一步地，采用Levenberg-Marquardt方法实现所述位姿优化。

进一步地，所述位姿优化采用的优化公式为：

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

进一步地，所述点线因子和退化因子根据待优化点云帧的边缘点和边缘点特征地图构建，点面因子根据待优化点云帧的平面点和平面特征地图构建。

边缘点特征地图和平面特征地图为局部特征地图，基于平面点、边缘点和当前位姿构建。

本发明还提供一种融合码盘和激光雷达的建图方法，包括以下步骤：

采用如上所述的里程计方法获得激光里程计结果；

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

进一步地，在GPS可用时，至少采用三个GPS因子对姿态进行优化。

与现有技术相比，本发明具有以下有益效果：

1、本发明融合了多种传感器的测量结果，选择了车轮编码器、方向盘转角编码器和激光雷达进行量程计数据处理，可实时准确地估计低漂移位姿，车轮编码器和方向盘转角编码器高频且实时地估计位姿增量，用于点云去畸变和为优化位姿提供初值，激光雷达以较低的频率估计车辆的精确位姿变化以补偿编码器累计的误差。

2、本发明融合回环因子和可选的GPS因子，在实时准确地估计低漂移位姿的基础上，构建高精度点云地图。

3、本发明使用简单而高效的车轮里程计代替惯性测量单元(IMU)，适用于车载平台。

4、本发明方法在从自动驾驶汽车平台收集的数据集上进行了广泛的评估，并和已开源的部分相关工作进行了对比，结果表明它具有更低的漂移率，在最大规模的测试中达到了0.53％。

附图说明

图1为本发明的框架原理示意图；

图2为坐标变换树示意图；

图3为阿克曼转向几何示意图；

图4为从激光测量数据中提取corner angle的示意图；

图5为局部地图的滑动窗口更新策略；

图6为位姿估计的因子图，其中，因子被可视化为带有不同标记的线，估计变量被可视化为带有变量名称的圆圈；

图7为测试案例中全局点云地图与卫星地图对齐示意图；

图8为不同方法的估计轨迹与RTK测量轨迹的对比，其中，(8a)为Single Loop数据集，(8b)为未使用回环修正的Single Loop数据集，(8c)为West Campus数据集，(8d)为South Campus数据集；

图9为不同轨迹长度的漂移率比较示意图，其中，(9a)为Single Loop数据集的漂移率，(9b)为West Campus数据集的漂移率，(9c)为South Campus数据集的漂移率。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。

实施例1

本实施例提供一种融合码盘和激光雷达的里程计方法，参考图1所示，该方法包括以下步骤：采集车轮编码器和方向盘转角传感器的数据，基于阿克曼转向几何获取车轮里程计信息；通过激光雷达获得点云数据，对该点云数据进行格式转换，获得激光扫描序列；对所述激光扫描序列进行去畸变和特征点提取；基于所提取的特征点进行位姿优化，获得最终的高频位姿输出。

实施例2

如图1所示，本实施例提供一种在线建图方法，基于一个三层SLAM框架实现。在该框架中，第一层，引入了基于阿克曼转向几何的车轮里程计方法，实时输出高频运动，用于点云去畸变，并作为位姿优化的初值；第二层，采用基于角度度量的改进的基于特征的两阶段方法从点云中提取边缘特征、平面特征和退化特征，分别在局部扫描尺度和局部地图尺度上处理特征以获得更多稳定的特征提取结果，然后对特征进行解析，以帧-局部地图的方式形成对传感器姿态的约束，用于LiDAR里程计优化；第三层，应用基于图的方法来构建具有LiDAR里程计因子、回环因子和可选GPS因子的因子图优化问题，该问题通过iSAM方法求解，在该层中，用于融合GPS测量结果而无需显式初始化的Auto-Aligned-GPS因子，可用于大规模建图任务。在三层框架中，车轮里程计在第一层以非常低的延迟运行，累积误差通过第二层的LiDAR里程计优化进行补偿。而建图运行在对实时处理要求较低的第三层。

实施例3

本实施例提供一种SLAM(simultaneous localizationandmapping，同步定位与建图)系统WLAOM，它利用不同的传感器进行实时里程计和在线地图绘制；改进的基于特征的LiDAR里程计方法通过基于运动学模型的车轮里程计方法得到增强，以产生低延迟的低漂移姿态估计，采用基于因子图的方法来融合回环检测和GPS测量，其中对自动对齐GPS因子进行建模以校正姿态并估计GPS和本地坐标系之间的变换关系。这使得系统可以绘制覆盖数公里的广阔区域。

系统由五个模块组成，直接从LiDAR和编码器数据接收点云数据，并通过CAN接收可选的GPS测量数据，系统输出优化的全局轨迹和全局点云图，如果GPS测量可用，它们都与GPS坐标系对齐。通过优化方法将被视为一组路径点的姿势图与GPS测量的对应路径点集对齐，其中每个姿势的漂移同时最小化。这一思想被实现为自动对准GPS因子，它估计从局部坐标到GPS坐标的转换，同时纠正姿态漂移。里程计的输出采用ROS提供的坐标变换树的形式，如图2所示。图2中，earth表示GPS的本地坐标系，可以是ENU或UTM。地图是指原点位于起点的全局地图和全局轨迹的坐标系。localmap是指LiDAR里程计的坐标系，随着系统长距离移动，它会缓慢漂移。odom表示车轮里程计的坐标系，漂移快但延迟低。lidar_link和gps_link对应相应传感器的自体坐标系。圆之间的箭头定义为坐标系之间的变换，其中变换矩阵_eT_m、_mT_lm、_lmT_o和_oT_bl由它们对应的模块估计，而_blT_ll和_blT_gl直接校准为常数。图2中未显示的其他变换可以从像_oT_ll＝_oT_bl·_blT_ll这样的组合中导出。

(1)车轮里程计

基于如图3所示的阿克曼转向几何学构建车轮里程计的运动学模型。给定转弯半径R和任何单个车轮的速度v_wheel，后轴中心的线速度v与v_wheel成正比，角速度w＝v·c，其中c表示曲率，c·R＝1。车轮编码器直接输出与车轮滚动距离有关的计数。因此不需要估计车轮的速度和计算距离，这会引入额外的计时问题。取而代之的是，将v替换为ds，表示距离的小增量，w替换为dθ，表示偏航角的小增量，这样ds和dθ就可以用式(1)-(5)来计算，其中fl表示左前轮，fr表示右前轮，rl表示左后轮，rr表示右后轮。

dθ＝ds·c (5)

假设车轮没有纵向滑动，ds_x，其中x＝fl，fr，rl，rr，可以使用任何车轮编码器的计数增量计算，平均值用作最终ds。假设车轮没有侧向滑移，曲率c取决于转向角，可以使用公式(6)计算。

其中，η表示转向系统的传动比。

最后，车辆的3-DOF位姿可以简单地估计为：

其中，x和y表示后轴中心的位置，θ表示车辆的偏航角。将3-DOF姿态向6-DOF变换_oT_bl时，仅假设其余三个自由度为零。

大多数LiDAR传感器执行光束转向来扫描环境，这会不断改变光束的方向以获得一系列距离测量值。利用这种机制来统一不同LiDAR产生的点云的格式。点按测量时间排序，这需要点数据为每个点提供时间戳字段。此外，许多LiDAR还提供称为ring的数据字段，指示哪个扫描仪产生该点。因此，所提出的算法需要时间戳和环来恢复每个激光扫描仪的扫描序列。序列可称为扫描。例如，RS-LiDAR-16有16次扫描，而DJI生产的Livox Avia有6次扫描。只要时间戳和ring可用，这种转换允许算法适应不同类型的LiDAR。

(2)点云去畸变

激光雷达的运动会导致点云失真，需要进行处理。本实施例中，点云使用以下公式逐点去畸变：

其中，表示时间戳为t_k的一帧点云中的第k个点。/>是从车轮里程计估计的变换中插值获得的，假设在车轮里程计更新之间存在线性运动模型。/>表示。/>表示。与LOAM相比，LOAM在一个扫描周期内假设恒定的角速度和线速度，本方法采用的车轮编码器具有更高的更新率，进而使得去畸变方式更准确。

(3)特征提取

本方法在局部扫描尺度上提取特征，采用基于角度的几何方法对所述激光扫描序列进行特征点提取，包括平面点和边缘点(边缘点)的提取。

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

采用如图4所示的狭窄邻域内的点计算原理，根据相邻点之间的角度或距离的差异将扫描划分为段，点太少的段被忽略以滤除噪声点，然后将剩余线段的第一个和最后一个点标记为边缘点，移除被遮挡的点。最后，线段内的点根据通过式(9)计算激光数据点M的角度corner angle，若corner angle小于第一预设阈值angle_planar，则判定对应的点M为平面点，若corner angle大于第二预设阈值angle_corner，则判定对应的点M为边缘点。angle_planar和angle_corner这两个参数可以根据实验选择，如可将两个参数均设置有56°。

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

上述提取过程可使用非去畸变点云完成，因为corner angle是使用在很短的时间内获得的相邻点计算的，其中传感器可以被视为静止，而去畸变的过程可能会给这些局部点引入额外的噪声，可能影响特征提取的质量。

(4)激光雷达里程计

LiDAR里程计以扫描到地图的方式执行，如图6所示，传入的激光帧被配准到局部地图，然后合并。为了限制局部地图的数据规模，它被划分为在局部地图片段，它是通过将沿固定长度轨迹聚和的所有经过配准的特征点合并成两个体素地图，M_edge和M_planar，分别对应两种类型的特征点的p_edge和p_planer描述。然后使用滑动窗口方法构建局部地图，只保留几个最新的局部地图片段。

对于局部地图，点云配准是通过使用式(10)优化当前帧的位姿_lmT_ll来最小化配准误差，可以通过Levenberg-Marquardt算法求解：

||·||_∑表示由协方差矩阵∑参数化的马氏距离，并且

d_edge＝(_lmT_ll·p_edge-c)×n (11)

d_planar＝(_lmT_ll·p_planer-c)·n (12)

其中，d_edge表示点到线的距离，d_planar表示点到平面的距离，c表示特征的中心，n表示特征的方向向量，表示边缘线的方向或与平面正交的方向。

对于非线性优化，_lmT_ll的初始估计值由下式给出：

_lmT_ll＝_lmT_o·_oT_ll (13)

其中，_oT_ll由车轮里程计给出，_lmT_o是估计车轮里程计的累积姿态误差。_lmT_ll使LiDAR的位姿更新如下：

综上所述，LiDAR里程计算法的迭代部分包括式(10)和式(14)的迭代以及图5所示的局部地图的更新。

局部地图利用了采集的平面点和角点以及优化后的位姿构建，包括角点特征地图Map_edge和平面特征地图Map_planar，其各自对应一个特征地图参数Δ_edge和Δ_planar，用于体素滤波以平滑地图。同时，为了控制局部特征地图的规模，只有在距离当前位姿一定半径范围内的得到的激光数据被用于局部特征地图构建。

特征提取的第二阶段是特征参数化。

生成的特征由三元组{c，n，∑}参数化，该三元组是使用PCA算法与几个最近邻点计算的。使用PCA从K个邻近点中提取特征值{λ₁，λ₂，λ₃}和特征向量{n₁，n₂，n₃}，λ₁＜λ₂＜λ₃，c是这些点的平均位置。具体地，平面特征参数化为：

{c，n₁，∑_planar} (15)

其中，σ_prior表示LiDAR测量的噪声，σ_planar表示平面特征阈值，Δ_planar表示局部体素图M_planar的单个单元格大小，用于体素滤波以平滑地图。为了控制局部特征地图的规模，只有在距离当前位姿一定半径范围内的得到的激光数据被用于局部特征地图构建。式(17)和(18)设置了过滤平面特征的标准，对相邻点的分布有要求，式(16)直接从点云数据估计特征的不确定性。

同样，边缘特征参数化为：

{c，n₃，Σ_edge} (19)

σ_edge表示边缘特征阈值。

由于真实数据中的噪声点，可能是植被点，边缘特征不如平面特征稳定。因此，在分类为边缘特征之前，该特征必须不是退化特征。退化特征参数化为：

{c，n₁，∑_de}

其中，Δ_edge表示局部体素地图M_edge的单个单元格大小。

退化特征的形式与平面特征非常相似，但在等式中增加了不确定性惩罚。式(22)用于减少其在优化中的权重。大多数时候丢弃这种特征不会带来影响，但是当环境太嘈杂而无法提取其他类型的特征时它会有所帮助。如果退化特征被利用，则式(10)改写为：

其中为d_de是类似于式(12)的点到平面距离。为最终得到的优化位姿，优化初值由外部轮速里程计提供。

式(12)中的点线因子退化因子/>根据待优化点云帧的角点和角点特征地图构建，点面因子/>根据待优化点云帧的平面点和平面特征地图构建。

(5)基于图的建图模块

虽然激光雷达里程计比车轮里程计准确得多，但由于里程计的性质，它在长距离行驶后会出现漂移，会累积误差。需要额外的测量来消除漂移以创建具有内部一致性的全局地图。回环是带环轨迹的一种解决方案。当GPS信号可用时，融合GPS测量是另一种实用的方法。如图6所示的因子图集成了这两种方法以校正漂移。它通过iSAM算法的开源实现GTSAM求解，这使得建图能够有效地在线运行。全局地图被表示为一系列与从经过配准的点云帧生成的子地图相关联的位姿节点，这是一种拓扑地图。为了缩小因子图的规模，假设LiDAR里程计的漂移在短距离内很小，每5米添加一个新位姿节点。引入了四种类型的因子来优化位姿，如图6所示。先验因子只是将坐标原点设置为轨迹的起点。LiDAR里程计因子和回环因子的细节可参考文献“LIO-SAM：Tightly-coupled Lidar Inertial Odometry viaSmoothing and Mapping”(SHAN T，ENGLOT B，MEYERS D，et a1.2020IEEE/RSJInternational Conference on Intelligent Robots and Systems(IROS).IEEE，2020-2021：5135-5142)，为简洁起见，此处省略了它们的细节。对于融合GPS测量，假设未知坐标变换关系_gpsT_map，它将点从本地坐标系转换为GPS坐标。GPS因子使用_gpsT_map和子图的位姿来评估GPS测量与其估计值之间的差异，如式(25)：

其中，T_k是子图的位姿，t_gps是像ENU(earth-north-up)系统那样以笛卡尔坐标表示的GPS测量值，Σ_gps表示GPS测量值不确定性的协方差矩阵。这种带有GPS因子的图结构类似于文献“LiDAR Inertial Odometry Aided Robust LiDAR Localization System inChanging City Scenes”(DING W，HOU S，GAO H，et a1.2020IEEE InternationalConference on Robotics and Automation(ICRA).IEEE，2020-2020：4322-4328)中的图结构。

GPS因子不需要额外的过程将GPS测量值转换到本地坐标系，而是在统一优化过程中自动估计坐标变换关系以及校正漂移，这简化了GPS融合过程，并且它可以增量融合GPS测量，能够与iSAM兼容。这使得系统对GPS信号质量具有鲁棒性，因为它只需要相距一定距离的几次GPS测量来校正姿势，而不是连续的GPS测量。本方法中，需要至少三个GPS因子才能形成一个封闭的约束。

实验

本实施例采用搭建的自动驾驶汽车平台上收集的数据进行验证。自动驾驶汽车平台中，转向编码器和车轮编码器，是车辆的原始部件，通过控制器局域网(CAN)提供100Hz输出。安装的中央LiDAR提供10Hz 16通道数据，视野(FOV)为360°×30°，其中水平分辨率为0.2°，垂直分辨率为2°，可安装在车辆顶部。惯性和GPS测量系统RT3002提供可选的GPS测量，以便在实时运动学(RTK)可用时进行验证，可安装在后轴的中心，为验证提供精确的地面实况数据。

本实施例在ubuntu18.04下i5-8265U CPU的笔记本电脑上对上述方法进行了测试。各模块的工作频率见表1。由于车轮里程计由车轮编码器触发，其频率与车速成正比，并受编码器更新率的限制。特征提取、点云去畸变和激光雷达里程计在一个管道中处理10Hz点云数据，并在系统繁忙时丢弃数据，而不会影响性能。建图模块大部分时间以低频率记录地图数据，仅在检测到回环或GPS可用时优化全局地图，工作负载均衡，满足里程计的实时性能和全局映射的在线处理。当GPS信号可用时，局部地图就很好地位于GPS坐标系中。图7显示了使用本发明的点云图与卫星地图对齐示意。

表1系统更新频率

为了进一步评估准确性，本实施例将本发明方法与使用开源实现的几个现有方法进行了比较，如表2所示，并从自动驾驶汽车平台收集了三个数据集作为基准，如表3所示。

表2不同SLAM算法间传感器配置及回环功能的比较

表3数据细节

通过与从RT3002获得的地面真实轨迹进行比较来评估轨迹的准确性，预计在RTK模式下具有2cm位置协方差的高精度。但由于信号质量的原因，有时精度会下降，设备会显示没有可用的RTK模式。因此，仅利用了RTK模式下的数据，地面实况轨迹如图8所示。注意到GPS测量在高度上的精度较差，精度也通过将轨迹投影到地平面并计算水平位置误差来评估，如表4中的APE2D所示。绝对位姿误差(APE)和相对位姿误差(RPE)被广泛用于评估SLAM算法的精度。对于APE，首先使用Umeyama算法将轨迹与地面真实轨迹对齐，然后使用等式(26)计算APE，式(27)计算APE2D。

其中，Q_i表示真实姿势，P_i表示估计姿势，S表示对齐轨迹的变换，trans(·)表示提取姿势的平移部分，<·>表示通过投影从3D姿势中提取2D姿势。为了计算RPE，分别从真实轨迹和估计轨迹中选择一对沿轨迹相距Δ米的姿态，使用如下公式计算：

相对于轨迹长度Δ的漂移率定义为：

选择了一系列轨迹长度Δ_i，采用式(30)获得整体RPE，并采用式(31)获得总漂移率：

图8显示了与地面实况相比的估计轨迹，大多数轨迹与地面实况很好地匹配，除了A-LOAM显着偏离图(8a)和图(8c)中的地面实况轨迹，另外，因为没有配备回环，并且LeGO-LOAM未能检测到图8中的回环闭合。这表明如果成功执行回环，则可以大大提高准确性，否则精度会被漂移破坏。图(8b)比较了回环校正前的高度漂移，其中本发明方法优于LIO-SAM。LIO-SAM需要更大的阈值来搜索潜在的闭环，否则，它无法纠正数据集SingleLoop的z漂移。对于图(8d)所示的数据集SouthCampus，只有使用GPS的方法成功并绘制在图上，而其他方法积累了太多误差，无法找到回环。这表明里程计和回环的组合只有在回环不太长的情况下才能很好地发挥作用，因为本发明中的回环方法是基于位置的，位置误差太大会导致失败，但是融合GPS通过限制漂移可以来解决这个问题。

图9显示了不同轨迹长度的漂移率，其中本发明方法优于其他方法，并且如果去除GPS，则具有与LIO-SAM相似的性能。表4通过APE或APE2D表征绝对精度，通过整体漂移率表征相对精度，其中最好和次好的以粗体显示。

表4轨迹精度

符号说明

——表示源帧在时间t相对于目标帧的6-DOF位姿

——表示源帧中带有时间戳t的点p，可通过/>将点转化为对应的目标帧，表示为/>

以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术人员无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。

Claims (6)

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因子对姿态进行优化，通过增量平滑和建图算法在线生成全局地图。