一种基于时空法向的动态感知激光雷达惯性里程计方法

摘要: 本发明涉及机器人自主定位与环境感知,具体涉及一种基于时空法向的动态感知激光雷达惯性里程计方法。背景技术:1、激光雷达惯性里程计技术通过融合激光雷达的三维几何信息...
一种基于时空法向的动态感知激光雷达惯性里程计方法

本发明涉及机器人自主定位与环境感知,具体涉及一种基于时空法向的动态感知激光雷达惯性里程计方法。

背景技术:

1、激光雷达惯性里程计技术通过融合激光雷达的三维几何信息和惯性测量单元的高频运动信息,实现机器人在无外部定位条件下的连续位姿估计,被广泛应用于自动驾驶、工业移动机器人等场景。现有激光雷达惯性里程计方法通常基于静态环境假设,利用连续帧之间的几何一致性完成点云匹配和状态估计。然而实际环境中存在行人、车辆等动态目标,其产生的非静态激光点一旦参与匹配,会导致错误数据关联、位姿漂移和地图动态残影。现有动态处理方法中,一类利用点云空间变化或连续帧差异检测动态区域,但依赖已有位姿估计结果,位姿已受污染时检测效果有限;另一类通过语义分割或几何一致性过滤动态观测,语义方法依赖标注数据且对未知动态目标适应能力差,几何方法依赖预设阈值,在复杂场景中容易误检。此外,现有方法将动态检测与状态估计相互独立,未能充分利用点云在空间和时间维度上的联合结构信息。

2、因此,现有技术中缺乏一种能突破静态环境假设、将动态感知直接融合至点云注册过程、并利用空间时间联合信息实现稳定点筛选与鲁棒状态估计的激光雷达惯性里程计方法,成为本领域亟待解决的技术问题。

技术实现思路

1、本发明所要解决的技术问题是:现有激光雷达惯性里程计方法依赖静态环境假设,在存在行人、车辆等动态目标的复杂环境中,动态点云参与扫描匹配导致错误数据关联、位姿估计漂移和地图污染的问题。

2、为解决上述技术问题,本发明提供一种基于时空法向的动态感知激光雷达惯性里程计方法,包括以下步骤:

3、步骤s100:获取激光雷达点云数据和惯性测量单元数据,对多传感器数据进行时间同步、坐标转换以及运动畸变补偿,得到去畸变点云;

4、步骤s200:将去畸变点云中的每个激光点扩展为四维时空点,建立四维时空点云模型,在时间滑动窗口地图内对局部邻域的四维时空点进行协方差矩阵特征分解,取最小特征值对应的特征向量作为时空法向量;

5、步骤s300:根据时空法向量中的时间方向分量,对激光点进行稳定性判定,将点云划分为稳定点集合和不稳定点集合;

6、步骤s400:利用稳定点集合与长期体素地图建立点到平面约束,结合惯性测量单元预积分约束进行联合状态优化,获得机器人位姿估计结果;

7、步骤s500:对不稳定点执行空间一致性检测,区分真实动态目标与静态误检测结构,更新长期体素地图,输出动态环境下优化后的定位结果。

8、进一步地,步骤s100包括:

9、步骤s110:获取机器人平台采集的激光雷达点云数据和惯性测量单元数据,对多源传感器数据进行时间同步,将激光雷达点云和惯性测量单元测量值对齐到同一时间基准;

10、步骤s120:利用惯性测量单元运动估计,对激光雷达扫描过程中因机器人运动产生的点云运动畸变进行补偿,使不同时间采样的激光点转换到统一参考坐标系,得到去畸变后的激光点云;

11、步骤s130:对去畸变后的激光点云进行体素降采样,降低后续处理的点云规模。

12、进一步地,步骤s200包括:

13、步骤s210:将去畸变后的每个激光点由三维空间坐标扩展为包含采样时间的四维时空点,形成四维时空点云模型,第i个激光点对应的四维时空点,满足

14、;

15、其中,、、表示激光点在统一参考坐标系下的三维空间坐标,表示该激光点的采样时间,表示转置操作;

16、步骤s220:在时间滑动窗口地图内,针对当前待计算时空法向量的激光点,利用三维空间距离进行近邻搜索,建立局部空间时间邻域,时间滑动窗口地图保存设定时间段内的激光雷达点云,仅时间窗口内的历史扫描点参与邻域构建;

17、步骤s230:对局部邻域内的四维时空点计算协方差矩阵,并对协方差矩阵进行特征分解,取最小特征值对应的特征向量作为该激光点的时空法向量,时空法向量满足

18、;

19、其中,、、为空间方向分量,为时间方向分量。

20、进一步地,步骤s300包括:

21、步骤s310:根据时空法向量中的时间方向分量和空间方向分量,计算动态判断指标,动态判断指标根据时空法向量与其空间投影之间的夹角确定,满足:

22、;

23、其中,为时间方向分量,、、为空间方向分量;

24、步骤s320:将动态判断指标与预设动态判断阈值进行比较,当时间方向分量绝对值小于或等于0.1时,判定对应激光点为稳定点,当大于0.1时,判定对应激光点为不稳定点。

25、进一步地,步骤s400包括:

26、步骤s410:利用稳定点集合,与长期体素地图建立点到平面匹配约束,对于稳定点集合中的每个激光点,在长期体素地图中查找对应的空间平面,建立点到平面残差,点到平面残差满足:

27、;

28、其中,为当前扫描中的稳定激光点坐标,为长期体素地图中对应空间平面的法向量,为对应空间平面上的参考点,为机器人当前位姿变换矩阵,表示转置操作;

29、步骤s420:构建惯性测量单元预积分残差与激光雷达点到平面残差的联合优化目标函数;

30、步骤s430:采用迭代扩展卡尔曼滤波方法对联合优化目标函数进行求解,获得机器人最优状态估计,系统状态变量包括机器人姿态、位置、速度、陀螺仪偏置和加速度计偏置。

31、进一步地,步骤s420中联合优化目标函数为:

32、;

33、其中,表示第j帧激光雷达扫描对应的最优状态估计结果,为惯性测量单元预积分残差,为惯性测量单元残差协方差矩阵,为激光雷达测量协方差矩阵。

34、进一步地,步骤s500包括:

35、步骤s510:维护时间滑动窗口地图和长期体素地图双地图结构,时间滑动窗口地图保存设定时间段内的连续激光雷达扫描点云,长期体素地图保存经过稳定性筛选的稳定环境结构;

36、步骤s520:完成当前扫描的位姿估计后,将稳定点集合中的静态结构以及不稳定点集合中通过空间一致性检测判定为静态误检测的结构加入长期体素地图,时间窗口外的数据从时间滑动窗口地图中删除。;

37、步骤s530:对不稳定点执行空间一致性检测,包括对不稳定点执行聚类、去除孤立点、对聚类建立包围盒并过滤体积超过预设阈值的聚类、建立短期空间一致性体素地图、计算候选动态点簇与短期空间一致性体素地图之间的空间体积重叠程度,根据重叠程度区分真实动态目标与静态误检测结构;

38、步骤s540:根据空间一致性检测结果更新长期体素地图,保留稳定环境结构,剔除动态目标点,输出动态环境下优化后的定位结果。

39、进一步地,步骤s530中体积重叠率满足:

40、;

41、其中,表示候选点簇包围区域,表示短期空间一致性体素地图,表示空间体积计算函数。

42、进一步地,一种基于时空法向的动态感知激光雷达惯性里程计系统,包括:

43、数据预处理模块,用于完成激光雷达数据和惯性测量单元数据的时间同步、运动畸变补偿和体素降采样;

44、时空法向分析与稳定性判定模块,用于维护时间滑动窗口地图,建立局部空间时间邻域,计算时空法向量,并根据时间方向分量判定稳定点与不稳定点;

45、动态感知点云注册与状态估计模块,用于利用稳定点集合与长期体素地图建立点到平面约束,融合惯性测量单元预积分约束,迭代更新机器人状态;

46、双地图管理模块,用于维护时间滑动窗口地图和长期体素地图;

47、空间一致性检测与静态地图构建模块,用于对不稳定点进行聚类和体积重叠检测,区分动态目标与误检测静态结构,更新长期体素地图。

48、本发明与现有技术相比,具有以下有益效果:(1)通过将激光点由三维空间坐标扩展为包含采样时间的四维时空点,在时间滑动窗口地图内对局部邻域进行协方差矩阵特征分解,取最小特征值对应特征向量作为时空法向量,实现了对激光点空间时间联合结构的显式建模,无需依赖语义模型即可根据时间分量判断观测点稳定性。(2)通过将动态稳定性判定直接融合在点云注册的每次迭代过程中,仅将稳定激光点用于与长期体素地图建立点到平面约束,从源头上避免了动态点对位姿优化的污染,解决了传统方法中动态检测与状态估计相互依赖、动态点已造成位姿误差后难以完全消除的问题。(3)通过采用时间滑动窗口地图与长期体素地图双地图结构,时间滑动窗口地图保存近期连续扫描点用于时空法向量计算,长期体素地图保存稳定环境结构用于点云配准,实现了短期动态信息与长期静态信息的分离管理,提高了地图的长期一致性。(4)通过对不稳定点执行上采样、聚类、体积过滤和与短期空间一致性体素地图的体积重叠检测,实现了真实动态目标与静态误检测结构的有效区分,避免了因遮挡或视角变化被误判的静态结构被错误删除。(5)通过构建仅包含惯性测量单元预积分残差和激光雷达点到平面残差的联合优化目标函数,并采用迭代扩展卡尔曼滤波方法求解,实现了动态环境下的鲁棒实时位姿估计。