基于激光雷达与组合导航的运动受限车辆标定方法及系统

摘要: 本发明涉及自动驾驶与机器人环境感知,具体涉及一种基于激光雷达与组合导航的运动受限车辆标定方法及系统。背景技术:1、在当今全球经济一体化与信息技术飞速发展的时代背...
基于激光雷达与组合导航的运动受限车辆标定方法及系统

本发明涉及自动驾驶与机器人环境感知,具体涉及一种基于激光雷达与组合导航的运动受限车辆标定方法及系统。

背景技术:

1、在当今全球经济一体化与信息技术飞速发展的时代背景下,物流业作为连接生产、消费、供给与需求的纽带,其运作效率与智能化水平已成为衡量现代化程度与经济竞争力的重要指标。港口码头作为国际贸易的核心枢纽,其承载的货物吞吐量逐年攀升,运营效率直接关系到国家供应链的安全与稳定。具备自主导航能力的自动导引车(automatedguided vehicle, agv)、无人清扫车、无人配送车等移动设备,正逐渐成为连接智慧港口、智能工厂各个环节的“毛细血管”,在提升周转效率、降低能耗成本方面发挥着不可替代的作用。

2、为了实现上述无人驾驶设备在复杂开阔场景(如自动化码头、大型物流园区)下的全天候、全天时环境感知与高精度定位,车辆通常需要搭载多种异构传感器,其中最典型的配置为激光雷达(lidar)与组合导航系统(gnss/ins)。充分利用组合导航系统提供的低频全局绝对定位信息来消除激光里程计的长时间累积误差,同时利用激光里程计的高频高精度局部相对位姿来平滑组合导航受干扰时的轨迹,即实现激光雷达与组合导航系统的多源数据紧耦合融合,已成为当前l4级高级别自动驾驶系统的主流和必然的解决方案。

3、实现多源异构传感器数据有效融合的首要前提和基础,是精确获取两者在物理空间中的相对安装位置与姿态关系,即传感器之间的外部参数(简称“外参标定”)。该外部参数包含了三维旋转矩阵与三维平移向量,共计六个自由度。外参标定的精度直接决定了多源异构数据能否在统一的时空坐标基准下严密对齐。传统的传感器外参标定方法主要分为基于人工标定板的方法和基于无目标的自然场景标定方法。基于标定板的方法需要在受控环境中严格放置具有特定反射率或几何特征(如大型棋盘格、高亮反光球、镂空多边形等)的标定物。虽然这类方法能够在理想的实验室或专用标定车间环境下获得极高的标定精度,但其操作流程极其繁琐,极度依赖专业标定人员的介入,耗时耗力,且完全无法在车辆实际运营的工作场景中随时进行。更为严重的是,对于大型重载agv等工程机械而言,由于其实际作业环境极其恶劣、车辆自身在载重运行时振动剧烈,传感器支架在长期的机械应力和热胀冷缩作用下,极易发生微观的机械形变,导致车辆出厂时精心校准的初始外参很快失效。因此,学术界和工业界亟需一种能够在自然行驶场景下,利用周围普通环境特征实现“在线自我标定”的方法,以适应工程化的大规模部署和设备的“终身免维护”需求。

4、在无目标的自然场景标定方法中,基于运动轨迹匹配的方法(如手眼标定原理,ax=xb模型)近年来成为了研究的核心热点与主流选择,该算法存在一个致命的局限性:这些方法多是针对无人机、手持测绘设备或灵巧的小型机器人设计开发的。对于重载agv、无人环卫车、园区无人接驳车等大型地面车辆,受到其机械结构和工作场景的严格限制,其运动往往被死死地限定在二维平坦的路面或结构化的平缓道路上。这种典型且固有的“平面运动”特性,导致大型车辆在z轴(垂直高程方向)上的平移位移量趋近于零,同时车体的横滚角(roll)和俯仰角(pitch)也几乎保持水平恒定,严重缺乏多维度的运动激励。

5、在数学机理层面,平面运动的缺失直接导致手眼标定的线性方程组发生严重的秩亏(rank deficiency),系统雅可比矩阵在z轴平移、roll角和pitch角方向上的特征值极小甚至为零。这就意味着,这三个维度的标定参数陷入了弱可观测乃至完全不可观测的状态。如果盲目地将现有的基于轨迹对齐的方法直接套用在这种平面运动受限的大型车辆上,由于关键维度的信息缺失,标定优化过程往往会陷入局部最优解,或者在传感器轻微观测噪声的扰动下发生严重的发散。

6、不仅如此,在实际的大型物流园区或自动化港口作业环境中,现实情况远比理论模型更为复杂。环境中存在大量频繁穿梭的动态物体(如重卡集卡、作业机械、行人),且严重环境多径效应会使得组合导航数据中掺杂大量的异常野值(outliers)。现有的标定方法大多建立在理想的高斯白噪声假设基础上,缺乏对这些非高斯大尺度噪声的鲁棒处理机制。一旦算法不加甄别地接收到了受严重干扰的错误gnss定位数据或退化的雷达匹配位姿,这些存在巨大偏差的残差项会迅速主导并污染整个优化框架,进一步彻底恶化原本就因平面受限运动而显得极为脆弱的标定系统。

7、近年来,为了克服平面运动带来的维度退化问题,部分现有研究(如基于联合优化的无目标标定框架)提出引入传感器已知的高度先验信息,并结合运动一致性约束构建庞大的目标函数,利用列文伯格-马夸尔特(lm)等优化算法进行系统轨迹与外参的协同精修。然而,这些方法在实际工程落地,尤其是面对港口、矿区等高动态复杂工况时,依然面临两项严峻挑战。首先,是针对复杂环境多径干扰的鲁棒性极差。传统的lm优化器对巨大的非高斯野值极其敏感,未经过严密甄别的污染数据会迅速主导并破坏联合优化框架,导致外参标定结果严重发散。其次,是系统架构难以满足实时在线标定的需求。现有的多约束联合优化方法多采用“离线”或“批处理(batch optimization)”架构,计算复杂度极高,无法适应自动驾驶车辆在日常运行中因传感器支架形变而产生的“在线、高频、实时”外参自适应更新需求。

技术实现思路

1、为了达到上述发明目的,本发明提供了一种基于激光雷达与组合导航的运动受限车辆标定方法及系统,其解决了现有标定方法在复杂环境多径干扰下难以实现高鲁棒性的外参实时在线标定的问题。

2、为了达到上述发明目的,本发明采用的技术方案为:

3、第一方面,提供一种基于激光雷达与组合导航的运动受限车辆标定方法,其包括步骤:

4、s1、获取待标定车辆的激光雷达点云与组合导航数据,并进行时间同步;

5、s2、根据激光雷达点云,提取激光雷达坐标系下的地平面法向量,并采用ieskf前端解算连续时刻的激光雷达里程计位姿;

6、s3、对组合导航数据进行多层级联验证处理,剔除受多径效应干扰的异常观测,筛选出导航数据;

7、s4、根据地平面法向量及组合导航系统与激光雷达间的已知高度先验,构建地平面几何残差;基于激光雷达里程计与导航数据的轨迹对齐关系,构建运动学残差;

8、s5、基于运动学残差与地平面几何残差构建混合优化目标函数,并采用非线性优化算法对混合优化目标函数进行迭代优化,得到激光雷达与组合导航系统间的最优外参。

9、在一种可能的实现方式中,步骤s3进一步包括:

10、s31、检测组合导航系统的定位状态字,并保留组合导航数据处于固定解模式的数据作为初始导航数据;

11、s32、监控初始导航数据在垂直方向的高程协方差,当高程协方差超过设定阈值时,则判定为多径效应干扰并剔除,得到过渡导航数据;

12、s33、对过渡导航数据进行滑窗,在滑窗内计算过渡导航数据与激光雷达里程计数据的位移偏差,删除位移偏差变化率超过动力学阈值的数据,得到导航数据。

13、在一种可能的实现方式中,所述地平面几何残差包括姿态残差与z轴平移残差,姿态残差的表达式为:

14、

15、其中,为姿态残差;为组合导航姿态;为待标定的旋转外参;为激光雷达系下的法向量;为全局坐标系法向;为对数映射算子;

16、所述z轴平移残差的表达式为:

17、

18、其中,为z轴平移残差;为组合导航系统与激光雷达间的已知高度先验;为待求解的平移外参在z轴方向上的分量。

19、在一种可能的实现方式中,所述运动学残差包括旋转增量残差和杆臂效应速度残差,所述旋转增量残差的表达式为:

20、

21、其中,为旋转增量残差;为组合导航系统在相邻时刻测得的相对旋转;为待标定的旋转外参;为激光雷达里程计测得的相对旋转;

22、所述杆臂效应速度残差的表达式为:

23、

24、其中,为速度残差;为待求解的平移外参;为组合导航系统观测速度在自身坐标系下的投影;分别为激光雷达坐标系下的线速度与角速度。

25、在一种可能的实现方式中,所述混合优化目标函数的表达式为:

26、

27、其中,为待求解的最优外参;为步骤s3得到的导航数据;、、和分别为中第k帧导航数据对应的旋转增量残差、杆臂效应速度残差、姿态残差和z轴平移残差;、、分别为对应残差项的缩放柯西核函数;、、和分别为对应残差项(即旋转增量残差、杆臂效应速度残差、姿态残差和z轴平移残差)的协方差矩阵;、、和分别为对应残差项基于协方差矩阵加权的二范数平方。

28、在一种可能的实现方式中,采用非线性优化算法对混合优化目标函数进行迭代优化,得到最优外参的方法包括:

29、s51、设定待求解的初始外参状态],在每次迭代中引入李代数误差状态增量,为旋转增量;为平移增量;为转置;

30、s52、在第次迭代的外参状态处,采用、、和分别对误差状态增量求偏导,得到对应的观测雅可比矩阵、、和;

31、s53、基于缩放柯西核函数的导数,计算、和对应项的等效迭代重加权权重、和;

32、s54、根据残差、、和,观测雅可比矩阵、、和及及等效迭代重加权权重、和,构建信息矩阵与梯度向量:

33、,

34、;

35、s55、构建lm法正则化正规方程,为阻尼因子,为单位矩阵;通过求解lm法正则化正规方程获取当前迭代步的误差状态最优增量;

36、s56、通过李代数空间到李群空间的指数映射操作,对第次迭代的外参状态进行广义加法更新,获取第次迭代的状态:

37、,

38、其中,和分别为第m+1次迭代时的旋转外参和平移外参;和分别为最优的旋转增量和平移增量;和分别为第m次迭代时的旋转外参和平移外参;为指数函数;为,其中、 和  分别为最优旋转增量  在三个坐标轴(如x轴、y轴、z轴)上的分量。

39、s57、计算更新状态下的混合优化目标函数的整体代价,若整体代价下降且增量模长 小于预设收敛阈值,则判定优化收敛,终止迭代并输出最优外参标定结果;若未收敛,则调整阻尼因子并返回步骤s52。

40、在一种可能的实现方式中,步骤s2进一步包括:

41、s21、采用点云分割算法从激光雷达点云中分离出地面点集;

42、s22、计算地面点集的协方差矩阵,并提取协方差矩阵最小特征值对应的特征向量作为地平面在激光雷达坐标系下的法向量;

43、s23、利用迭代误差状态卡尔曼滤波算法构建点到平面的观测模型,并推算激光雷达在连续时刻的相对运动状态,输出激光雷达的相对旋转增量、线速度及角速度。

44、在一种可能的实现方式中,步骤s23进一步包括:

45、s231、利用待标定车辆的运动学模型或惯性测量单元的高频测量数据,对上一时刻的后验状态进行前向积分,预测当前时刻的名义位姿状态,并根据激光雷达里程计位姿推算系统的误差状态转移矩阵及过程噪声协方差矩阵,推算当前时刻的先验协方差矩阵;

46、s232、将当前帧激光雷达点云通过名义位姿状态投影至局部地图坐标系,并采用提取的地平面法向量作为强几何先验,构建点到目标平面的隐式观测残差方程:

47、

48、其中,为点到目标平面的几何残差;为地平面法向量;和分别为构成名义位姿状态的名义旋转矩阵和平移向量;为当前帧激光雷达点云中的第个地面点在雷达局部坐标系下的坐标;为目标平面上的参考质心坐标;为转置;

49、s233、定义状态向量的误差状态,为李代数参数化下的旋转误差,为平移误差;

50、s234、对所述隐式观测残差方程在名义位姿状态处进行一阶泰勒展开,获取对应于误差状态的观测雅可比矩阵:

51、其中,为第个点的观测雅可比矩阵;为点的反对称矩阵形式;

52、s235、在当前名义位姿状态处获取卡尔曼增益,基于误差状态对名义位姿状态进行迭代修正,之后持续更新观测矩阵与卡尔曼增益,直至误差状态的模长小于预设收敛阈值,或达到最大迭代次数,终止迭代并输出最优的名义位姿状态;

53、s236、将当前帧的最优名义位姿状态与上一帧的名义位姿状态进行差分求导,解算出当前连续时刻内激光雷达的相对旋转增量、线速度及角速度。

54、在一种可能的实现方式中,对名义位姿状态进行迭代修正的表达式为:

55、,

56、其中,和分别为第和次迭代时的名义旋转矩阵;为指数函数;为;和分别为第和次迭代时的平移向量。

57、第二方面,提供一种基于激光雷达与组合导航的运动受限车辆标定系统,其包括步骤:

58、数据采集模块,用于获取待标定车辆的激光雷达点云与组合导航数据,并进行时间同步;

59、激光雷达里程计位姿计算模块,用户根据激光雷达点云,提取激光雷达坐标系下的地平面法向量,并采用ieskf前端解算连续时刻的激光雷达里程计位姿;

60、导航数据筛选模块,用户对组合导航数据进行多层级联验证处理,剔除受多径效应干扰的异常观测,筛选出导航数据;

61、残差生成模块,用于根据地平面法向量及组合导航系统与激光雷达间的已知高度先验,构建地平面几何残差;基于激光雷达里程计与导航数据的轨迹对齐关系,构建运动学残差;

62、最优外参计算模块,用于基于运动学残差与地平面几何残差构建混合优化目标函数,并采用非线性优化算法对混合优化目标函数进行迭代优化,得到激光雷达与组合导航系统间的最优外参。

63、与现有技术相比,本发明具有如下有益效果:

64、(1)极强的非高斯多径野值抗干扰能力:针对现有联合优化框架在真实港口等高动态场景下面对gnss多径跳变容易崩溃的痛点,本发明提出了多层级联验证机制。通过在物理、统计与几何多个层面进行严密甄别,成功将巨大的非高斯野值隔离在优化器之外,彻底阻断了污染数据对外参标定结果的破坏,极大地提升了系统在恶劣工况下的鲁棒性。

65、(2)支持在线自适应的高效实时标定架构:不同于现有技术中计算冗余、耗时极长的离线批量精修(batch optimization)方法,本方案采用ieskf前端解算激光雷达里程计位姿,不仅保证了局部高频位姿推算的平滑度,同时利用稀疏非线性优化保障了外参全局解算的实时性,能够完美适应车辆运行中因机械微调而产生的在线、动态自我标定需求。

66、(3)突破平面退化与可观测性瓶颈:本方案基于深度挖掘“地平面”这一强环境几何特征,提取地平面法向量并将其与运动学轨迹约束紧密结合,在李代数流形空间上构建的统一残差方程,高效地补全了垂直方向平移和水平姿态角(roll, pitch)的可观测性,避免了传统方法的矩阵秩亏与退化。