可靠性评估与精确增量关联的激光雷达里程计方法及系统

本发明涉及移动机器人定位、三维点云处理及激光雷达感知，具体涉及一种基于可靠性评估与精确增量数据关联的激光雷达里程计方法及系统。所述方法适用于地面车辆、无人运输平台、巡检机器人和移动测绘设备，能够在不依赖预先构建地图的情况下，根据连续激光雷达扫描估计传感器六自由度位姿并增量构建局部地图。

背景技术：

1、激光雷达里程计利用连续点云估计传感器运动，是自主移动系统实现定位、建图和环境感知的基础。特征式激光雷达里程计通常从点云中提取边缘特征和平面特征，将当前扫描与历史局部地图建立点到线或点到面的对应关系，并通过非线性优化求取当前位姿。该类方法具有结构清晰和实时性较好的优点。

2、然而，旋转式激光雷达的一帧扫描由不同方位的回波在不同时间采集。当平台在一帧扫描期间发生平移或旋转时，将全部扫描点视为同时获得会造成线、面结构弯曲，使后续特征提取和扫描-地图配准产生系统性误差。连续时间轨迹模型能够处理该问题，但其状态量和计算量较大，不利于轻量化实时里程计。

3、现有扫描-地图配准还通常把所有通过距离阈值和几何拟合检验的对应关系赋予相近权重。实际上，小残差并不必然表示可靠观测：运动物体或瞬态结构可能在同一空间网格单元内形成聚集且方向一致的异常大残差；历史地图点可能累积相互冲突的观测；近邻点虽然近似共线或共面，却可能对部分运动自由度的几何约束较弱；远距离回波和掠入射条件下平面回波的量测不确定性也明显增大。单一残差阈值或固定鲁棒损失函数难以同时区分上述情况。

4、更完整的对应关系评价会增加近邻搜索、局部直线或平面拟合以及权重计算的开销。相邻非线性迭代中的查询位置通常只发生小幅变化，但若每次都在关联半径内执行全量精确近邻搜索并重新计算协方差矩阵的特征分解，会占用实时计算预算并降低实时处理裕度；若采用近似近邻或未经精确性验证即复用缓存，又可能改变实际进入优化器的因子集合，造成优化目标与设计目标不一致。

5、因此，需要一种能够联合评价当前残差、地图历史、局部几何、量测不确定性和约束方向，同时又能够利用可检验的充分条件验证缓存候选集合是否包含精确近邻，并在判据无法确定时自动在关联半径内执行全量精确近邻搜索的激光雷达里程计方案。

技术实现思路

1、本发明的目的在于提供一种基于可靠性评估与精确增量数据关联的激光雷达里程计方法及系统，以减小扫描内运动畸变、不可靠对应关系、局部几何退化和重复近邻计算对定位精度及实时性的影响。

2、为实现上述目的，本发明在每帧处理中先根据扫描点的相对采样相位和最近两个有效位姿之间的相对运动对点云进行运动补偿；对补偿后的边缘特征和平面特征执行首轮扫描-地图配准优化，得到粗位姿估计；在固定的粗位姿估计下重新计算各有效对应关系的几何残差，更新极坐标网格单元的跨帧残差历史，并确定残差一致性权重、方向均衡权重及鲁棒尺度；在最终轮每次重建对应关系时，依据当前精确近邻确定地图稳定性权重和局部几何质量权重，依据边缘特征点的量测距离确定边缘距离权重，依据当前旋转确定平面法向投影不确定性权重，并以加权鲁棒目标函数求得最终位姿。

3、本发明将不同跨帧状态的更新时间分别限定在其所依赖的位姿阶段。极坐标网格单元的残差历史在固定粗位姿下依据当前单元残差不一致度每帧更新一次，并用于生成最终轮的残差一致性权重，最终轮的中间迭代不再改写该历史。地图点支持计数增量和冲突计数增量在最终轮内暂存，仅在最终位姿求解完成后写入跨帧地图点状态；被判定为不一致的当前特征不插入地图。由此避免非线性优化的中间状态反复写入跨帧历史或地图状态。

4、本发明还为边缘特征地图和平面特征地图分别维护增量体素索引。每个当前特征的帧内缓存包括上一次查询位置、有限数量的最近候选地图点、候选集合外地图点距离下界以及有序近邻标识键。根据三角不等式更新新查询位置处的候选集合外地图点距离下界；通过所述距离下界与缓存候选的第小距离之间的比较，验证缓存候选集合是否包含当前个精确近邻，并判断关联半径内的近邻数量是否不足；不能据此完成判定时在关联半径内执行全量精确近邻搜索。

5、在局部地图于同一帧各优化轮次中保持固定的条件下，有序近邻标识键不变意味着由地图决定的质心、协方差矩阵的特征分解结果、几何有效性、几何质量和平面偏置均不变，因而允许复用这些局部几何量。在最终轮每次重建对应关系时，当前点变换、残差、雅可比及依赖当前位姿的权重均重新计算，鲁棒尺度按固定粗位姿下的统计结果确定，且缓存不跨越地图更新。

6、本发明的技术方案为可靠性评估与精确增量数据关联的激光雷达里程计方法，包括：

7、获取当前帧激光雷达扫描数据和基于相对采样时间的扫描运动补偿；

8、从运动补偿后的扫描中提取边缘特征和平面特征，并获取由历史地图点组成的局部地图；

9、执行首轮扫描-地图配准优化得到粗位姿估计，在粗位姿下计算几何残差并更新跨帧残差历史；在最终轮每次重建对应关系时，联合地图稳定性、局部几何质量、投影不确定性及方向均衡权重构造有界信息权重；

10、执行精确增量数据关联，根据查询位置变化更新候选集合外地图点距离下界，与缓存候选中预设数量近邻的最大距离比较，判定缓存是否包含精确近邻；无法判定时在关联半径内执行全量精确近邻搜索；

11、将精确近邻的地图点唯一标识排序形成有序近邻标识键，仅在同一帧局部地图固定且所述有序近邻标识键不变时复用地图相关局部几何模型；

12、在最终轮扫描-地图配准优化中利用所述有界信息权重求取最终位姿，并仅在最终位姿求得后将地图点支持记录和冲突记录的帧内增量写入跨帧地图点状态以及更新局部地图。

13、进一步的，记录扫描点在一帧扫描内的相对采样相位，依据最近两个有效位姿之间的相对运动对所述扫描点进行扫描内运动补偿，所述扫描内运动补偿包括：将扫描点相对于首个有效回波的方位角差按取模并映射至区间，再除以得到归一化采样相位；由最近两帧有效位姿的相对变换求取六维运动扭量，将所述运动扭量的旋转分量和平移分量分别乘以经预设上下限约束的旋转缩放系数和平移缩放系数；按照扫描点的归一化采样相位与统一参考相位之差，经三维特殊欧氏群指数映射将扫描点变换至所述统一参考相位。

14、进一步的，根据所述几何残差的帧内鲁棒统计量更新按极坐标网格单元聚合的跨帧残差历史并确定残差一致性权重，所述残差一致性权重的确定包括：在固定的粗位姿估计下计算当前帧各几何残差的中位数和中位绝对偏差，由此确定残差中心、鲁棒尺度、大残差判定阈值和huber损失函数的转折尺度；将有效对应关系划分到以激光雷达为中心、按方位角和径向距离离散的极坐标网格单元，联合每一极坐标网格单元中残差超过所述判定阈值的归一化平均超出量、归一化大残差占比和残差位移向量方向一致度，得到单元残差不一致度；对样本数不低于预设下限且当前单元残差不一致度不低于预设触发阈值的单元执行指数平滑更新，对未同时满足上述条件的单元执行衰减更新，并在跨帧历史达到确认阈值后降低该单元的残差一致性权重。

15、进一步的，联合地图稳定性、局部几何质量、投影不确定性及方向均衡权重构造有界信息权重包括：根据当前精确近邻地图点的跨帧支持记录和冲突记录确定地图稳定性权重，根据所述当前精确近邻的协方差特征值确定局部几何质量权重，根据边缘特征点的量测距离确定边缘距离权重，并根据平面特征点的量测距离以及平面法向与经当前旋转变换后的单位观测射线之间夹角的余弦绝对值，确定平面法向投影不确定性权重；根据边缘类型和平面类型统计方向均衡权重；所述地图稳定性权重的确定包括：为每个地图点保存支持计数和冲突计数，利用beta分布先验计算地图点的稳定性概率，将各精确近邻的稳定性概率经仿射变换后限幅至预设权重区间，并采用中位数对所得近邻稳定性权重进行鲁棒聚合；在最终轮扫描-地图配准优化中先形成帧内支持计数增量或冲突计数增量，在最终位姿求解完成后再将所述帧内计数增量写入跨帧地图点状态；根据所述残差一致性评价结果被判定为不一致的当前特征不插入局部地图。

16、进一步的，所述局部几何质量权重、投影不确定性权重和方向均衡权重的确定包括：根据近邻协方差矩阵的升序特征值计算线性度和平面度，并分别映射为边缘几何权重和平面几何权重；根据运动补偿后特征点的量测距离确定边缘距离权重，根据平面法向与经当前旋转变换后的单位观测射线之间夹角的余弦绝对值以及量测距离确定平面法向投影不确定性权重；将有效直线方向和平面法向分别划分至方向分箱，根据非空方向分箱的平均约束数与各方向分箱内约束数的比值确定有界逆频数方向均衡权重。

17、进一步的，在首轮和最终轮扫描-地图配准优化中，对边缘特征地图和平面特征地图分别维护增量空间索引；针对每一特征查询，设预设正整数目标近邻数为，缓存前一查询位置、候选地图点集合以及候选集合外地图点距离下界，根据查询位置变化更新所述距离下界；根据更新后的距离下界与缓存候选的第小距离之间的比较，验证缓存候选集合是否包含当前个精确近邻；判定其包含时从缓存候选集合中确定所述精确近邻，判定关联半径内近邻数量不足时拒绝该特征查询，不能据此完成判定时通过所述增量空间索引在关联半径内执行全量精确近邻搜索；具体的：

18、设为特征查询索引，和分别为特征查询的当前地图坐标系三维位置向量和缓存基准三维位置向量，二者单位均为米；设目标近邻数和缓存容量均为正整数，且不小于；设为正的关联半径，为正的距离比较裕量，二者单位均为米；所述精确增量数据关联包括：在处保存由不超过个最近候选地图点组成的集合，以及非负的候选集合外地图点有效距离下界，的单位为米；所述缓存候选和全量精确近邻搜索得到的地图点均按其至的欧氏距离升序排列，距离相同时按地图点唯一标识升序排列；以减去与之间的欧氏距离，得到候选集合外地图点在处的距离下界，为实数，单位为米；当中的候选数不小于、非负的第小候选距离与之和小于，且小于时确定性接受，的单位为米；当中的候选数小于且不小于，或者中的前个近邻已经确定为精确近邻且不小于时确定性拒绝；仅在所述确定性接受条件和确定性拒绝条件均不成立时，在以为球心、以为半径且不包含边界的球形邻域内执行全量精确近邻搜索并刷新缓存。

19、进一步的，对于满足关联接受条件的查询，所述有序近邻标识键由个精确近邻地图点的唯一标识按预设顺序排列形成；同一帧的各优化轮次中局部地图固定，有序近邻标识键相同时复用对应的地图相关量，包括质心、协方差矩阵的特征值及特征向量、几何有效性标志、几何质量、平面偏置以及拟合失败状态，有序近邻标识键不同时重新计算上述地图相关量；特征点变换、残差、雅可比及位姿相关权重不作为局部几何缓存内容，并随位姿更新重新计算；鲁棒尺度按固定粗位姿下的统计结果确定；所述缓存不跨越局部地图更新。

20、本发明还提供一种基于可靠性评估与精确增量数据关联的激光雷达里程计系统，包括：

21、数据获取与运动补偿模块，用于获取含扫描点相对采样相位的激光雷达扫描，并依据最近两个有效位姿之间的相对运动执行扫描内运动补偿；

22、特征与局部地图模块，用于提取边缘特征和平面特征，并维护具有地图点支持记录和冲突记录的边缘特征地图及平面特征地图；

23、分阶段优化与可靠性评估模块，用于执行首轮扫描-地图配准优化得到粗位姿估计，在粗位姿下计算几何残差并更新跨帧残差历史；在最终轮每次重建对应关系时，联合地图稳定性、局部几何质量、投影不确定性及方向均衡权重构造有界信息权重；

24、精确增量数据关联模块，用于执行精确增量数据关联，根据查询位置变化更新候选集合外地图点距离下界，与缓存候选中预设数量近邻的最大距离比较，判定缓存是否包含精确近邻；无法判定时在关联半径内执行全量精确近邻搜索；

25、局部几何复用模块，用于将精确近邻的地图点唯一标识排序形成有序近邻标识键，仅在同一帧局部地图固定且所述有序近邻标识键不变时复用地图相关局部几何模型；

26、最终优化与地图状态更新模块，用于在最终轮扫描-地图配准优化中利用所述有界信息权重求取最终位姿，并仅在最终位姿求得后将地图点支持记录和冲突记录的帧内增量写入跨帧地图点状态以及更新局部地图；

27、结果输出模块，用于输出最终位姿、激光雷达轨迹和局部地图。

28、本发明还提供一种电子设备，包括存储器、处理器以及存储在所述存储器中并能够在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述技术方案所述的方法。

29、本发明还提供一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述技术方案所述的方法。

30、与现有技术相比，本发明至少具有以下有益效果：第一，利用相对采样相位和由最近两个有效位姿确定的相对运动扭量，以较低计算量减小扫描内运动畸变；第二，在固定的粗位姿估计下联合多类可靠性证据，减少运动物体、瞬态结构、局部几何约束退化和高噪声观测对位姿估计的影响；第三，残差历史仅在固定粗位姿下每帧更新一次，地图点状态与局部地图仅在最终位姿求得后更新，避免中间迭代状态影响后续帧；第四，通过近邻精确性判据与关联半径内全量精确近邻搜索相结合，降低重复搜索和拟合开销，同时保持精确近邻集合和加权优化目标不变。

31、本发明还提供执行上述方法的激光雷达里程计系统、电子设备及计算机可读存储介质。各模块可以由同一处理器中的软件线程实现，也可以分配至多个处理器、图形处理器、可编程逻辑器件或专用硬件加速单元。