顾及无人机位姿的激光雷达测深航带严密平差方法及系统

摘要: 本发明属于机载lidar测深与测绘,涉及顾及无人机位姿的激光雷达测深航带严密平差方法及系统,尤其涉及一种引入b样条曲线对pos时变漂移进行建模、同时联合求解全局...
顾及无人机位姿的激光雷达测深航带严密平差方法及系统

本发明属于机载lidar测深与测绘,涉及顾及无人机位姿的激光雷达测深航带严密平差方法及系统,尤其涉及一种引入b样条曲线对pos时变漂移进行建模、同时联合求解全局boresight误差、并在严格3d距离约束下以匹配点空间距离均方根误差为动态监控指标的严密平差方法。

背景技术:

1、机载lidar测深(airborne lidar bathymetry,alb)系统通过在飞行平台上搭载蓝绿激光扫描仪,能够快速获取浅海水域及海岸带的大范围、高密度三维点云数据,已成为浅海地形测绘、海岸带监测等领域不可替代的技术手段。在机载lidar数据采集中,无论是测深还是测地模式,由于飞行航带宽度有限,大面积测绘任务通常需要布设多条平行或交叉的飞行航带以实现全覆盖。然而,各航带数据在重叠区域往往存在明显的空间位置偏差,尤其是高程方向的不一致性,直接影响后续地形数字高程模型生产的精度和可靠性。在测深场景中,由于激光需穿透水体到达海底,信号衰减严重、点云密度低、水体散射引入额外噪声,航带间的高程偏差问题较陆地测量更为突出。

2、目前机载lidar航带平差方法主要分为两类:一类是以迭代最近点(icp)及其变体为代表的点云层刚体配准方法,将整条航带视为刚体,通过最小化重叠区对应点间距离迭代求解航带间整体旋转与平移;另一类是有限参数误差补偿方法,将航带几何误差建模为整体平移、整体旋转或随时间线性变化的少量参数,借助重叠区匹配点解算。其中,与本技术最接近的是在点云层进行迭代配准的刚体/有限参数航带平差方案。

3、通过上述分析,现有航带平差方法存在的问题及缺陷为:

4、(1)刚体假设与忽略时变漂移误差:传统的icp(iterative closest point,迭代最近点)算法及其变体通常将整条航带点云视为刚体,仅通过全局的旋转与平移变换进行配准。然而,在机载lidar实际飞行过程中,gnss/ins系统受大气延迟、多路径效应以及惯性器件漂移等因素的影响,其定位定姿误差随时间呈现低频、非线性的漂移特征,即pos(position and orientation system)时变漂移误差。这种时变误差导致长航带数据出现弯曲、扭曲等非刚体变形,传统刚体配准方法无法有效建模和补偿。

5、(2)匹配点赋权机制单一,抗噪能力不足:航带重叠区域包含植被冠层、建筑物边缘、水体、海底礁石等复杂地物类型,其点云数据中含有大量几何特征不稳定或非地形表面的噪声点对。在机载lidar测深场景中,激光穿透水体的折射效应、水体悬浮颗粒的散射以及气泡、泡沫引起的异常回波,进一步加剧了匹配点对的噪声水平。部分现有方法主要采用统一权重或基于距离阈值的粗差剔除策略,对激光观测几何特性及残差统计特征的联合利用仍较为有限,未能充分利用激光测距的物理几何特性和残差统计分布特性来区分观测质量。这使得平差结果容易受到异常匹配点对的“拉偏”影响,降低了解的准确性和鲁棒性。

6、(3)视轴误差与pos漂移误差的耦合标定不充分:机载lidar系统中,激光扫描仪坐标系与imu(inertial measurement unit)坐标系之间存在的系统安装偏差称为视轴误差。视轴误差与pos时变漂移误差在点云坐标解算中相互耦合,若将两者分开独立标定,或仅标定其中之一而忽略另一误差源,则无法获得全局最优的校正结果,尤其是在多航带数据联合平差时,误差耦合效应更为显著。

7、上述缺陷的根本成因在于:其一,刚体或线性参数模型的误差自由度不足以表达gnss/ins误差随时间的低频非线性漂移,模型失配使长航带弯曲、扭曲变形无法被吸收;其二,统一权重或单一距离阈值剔除无法同时刻画激光观测几何质量差异与残差统计分布差异,测深场景的异常匹配拉偏解算结果;其三,视轴安装误差为全局常量、pos漂移为时变量,二者在观测方程中相互耦合,分开标定会存在残差且两类误差互相吸收。本技术所解决的正是上述三类根源性缺陷。

8、综上所述,研究一种能够同时建模视轴系统误差与pos时变漂移误差、以匹配点空间距离误差为独立监控指标、具备物理与统计双重鲁棒赋权机制的机载lidar多航带严密平差方法,是当前机载lidar测绘技术领域亟需解决的重要技术问题。

9、区别于上述现有技术,本技术至少在三个层面形成实质区别:(1)问题层面,针对pos时变漂移与boresight安装误差耦合这一被刚体配准方法忽略的非刚体误差源;(2)方案层面,将平差求解空间由点云坐标空间转换到pos位姿参数空间,以全局boresight参数与b样条时变漂移控制点构成低维联合状态向量,并以独立于目标误差函数的空间距离均方根误差作为过程监控量,形成由粗到精的动态收敛闭环;(3)效果层面,重叠区空间位置一致性由分米级提升至厘米级。本领域技术人员无法由上述刚体配准或有限参数补偿方案经简单组合得到本技术方案。

技术实现思路

1、为克服相关技术中存在的问题,本发明公开实施例提供了顾及无人机位姿的激光雷达测深航带严密平差方法及系统。

2、为实现上述目的,本发明采用如下技术方案:顾及无人机位姿的激光雷达测深航带严密平差方法,包括以下步骤:

3、s1、数据获取与预处理:获取机载lidar测深多航带原始点云数据及对应的pos轨迹数据,将所述点云数据与pos轨迹数据进行时间同步与坐标映射,建立初始局部坐标系;

4、s2、特征提取与状态向量构建:对所述点云数据进行降采样,基于特征值分解计算点云的几何特征,所述几何特征包括法向量与平面度;基准带点云几何在平差过程中严格保持不变;基于pos时间戳,构建包含全局boresight误差参数和从带时变pos漂移参数的联合状态向量,其中时变pos漂移参数由b样条曲线控制点表示;

5、s3、动态几何匹配:基于当前状态向量对从带点云进行位姿更新,以基准带点云为目标面,在动态搜索半径与3d欧氏距离评价阈值的双重距离约束下,利用平面度约束、法向量夹角约束及双向最近邻一致性验证筛选重叠区同名点对;针对满足3d距离约束的有效匹配点对计算空间距离均方根误差,作为独立于目标误差函数的监控指标,用于驱动搜索半径自适应更新及收敛判断;

6、s4、严密平差联合优化:基于所述同名点对、基准带目标平面的法向量及联合状态向量,以从带校正后点云到基准带目标平面的投影距离构建目标误差函数;利用联合赋权模型对所述目标误差函数进行加权,结合先验约束矩阵,使用levenberg-marquardt算法迭代求解最优的联合状态向量更新量;

7、s5、动态迭代与全分辨率校正:根据计算的空间距离均方根误差动态更新下一次迭代的搜索半径;当空间距离均方根误差满足收敛条件时停止迭代,获得最优联合状态向量,并将所述最优联合状态向量应用于全分辨率原始点云,输出平差优化后的高精度点云数据。

8、在步骤s2中,所述构建包含全局boresight误差参数和从带时变pos漂移参数的联合状态向量包括:对于每条从带,所述时变pos漂移参数采用三次b样条曲线建模,每个b样条控制点包含6个参数,依次为三轴平移漂移量和三轴姿态漂移量;所述联合状态向量包括全局boresight误差角向量以及各从带的b样条控制点参数向量;

9、在步骤s3中,所述基于当前状态向量对从带点云进行位姿更新包括:根据数据点的采集时间戳,通过b样条基函数插值得到该时刻的平移校正量和姿态校正量,并应用于该点;

10、在步骤s4中,所述目标误差函数的雅可比矩阵中对应于从带b样条控制点参数的偏导项由b样条基函数加权构建,其中平移分量的偏导为基函数值与法向量分量的乘积,姿态分量的偏导为基函数值与boresight偏导向量的乘积,且三次b样条的局部支撑特性使得所述雅可比矩阵具有稀疏结构。

11、在步骤s4中,所述联合赋权模型生成的总权重由物理权重和统计鲁棒权重相乘得到,其中,总权重、物理权重和统计鲁棒权重均为无量纲权重;

12、物理权重:根据lidar激光束在局部坐标系下的发射向量计算入射角的余弦值,结合激光测距长度,构建基于物理观测几何的权重:

13、;

14、;

15、式中,为激光束入射角,即激光发射方向与局部坐标系天顶轴之间的夹角;为激光测距长度,单位为;为激光发射向量在局部坐标系轴上的分量;为距离归一化因子单位为,用于保证物理权重无量纲;

16、统计鲁棒权重:基于当前次迭代中残差的中位数绝对偏差动态计算huber损失函数的截断阈值,对大于所述截断阈值的异常点对进行降权处理;其中,huber截断系数和截断阈值下限为预设参数:

17、;

18、;

19、式中,为huber截断阈值,单位为;为残差的中位数绝对偏差,单位为;为huber截断系数,无量纲,由残差中位数绝对偏差到标准差的一致性校正因子与huber函数渐近效率系数相乘得到;为当前迭代匹配点对的残差向量,即匹配点对的点到目标平面有符号投影距离,单位为;为截断阈值下限,单位为m,用于防止残差分布过于集中时截断阈值趋于零;

20、总权重为:

21、。

22、在步骤s4中,所述先验约束矩阵包括:

23、(1)boresight先验方差约束:

24、;

25、式中,为boresight先验方差,单位为;为boresight先验信息子块,为3×3对角矩阵,对角元素为boresight先验方差的倒数,单位为;为3阶单位矩阵,无量纲;

26、(2)pos平移与姿态漂移方差约束:

27、;

28、式中,为pos平移漂移先验方差,单位为;为pos姿态漂移先验方差,单位为;为b样条控制点数量;为从带b样条控制点参数的对角先验信息矩阵,对角元素为各参数先验方差的倒数,平移参数对应元素单位为、姿态参数对应元素单位为;为克罗内克积(kronecker product)运算符号,无量纲;为长度为的全1列向量,用于将单个控制点的6个先验权重复制到全部控制点,无量纲;

29、(3)用于控制b样条曲线平滑度的二阶差分平滑约束:

30、;

31、式中,为平滑系数,无量纲;为二阶离散差分矩阵,第行在控制点参数序号、、处依次取、、,其余元素为;为样条节点间距,单位为;为b样条二阶平滑约束的信息矩阵,为半正定稀疏矩阵,用于对相邻控制点之间的二阶差分施加惩罚、抑制修正曲线的高频振荡;为6阶单位矩阵,对应单个控制点的6个状态参数,无量纲;

32、(4)控制姿态漂移零均值的约束:

33、;

34、式中,为零均值约束方差,单位为;为元素仅在控制点参数序号处取值为,其余位置为0,下标分别对应roll、pitch、yaw三个姿态分量;为第个姿态分量的零均值软约束信息矩阵,为秩1矩阵,单位为,依次对应roll、pitch、yaw;上角标为向量或矩阵的转置符号,无量纲。

35、在步骤s3中,所述针对满足3d距离约束的有效匹配点对计算空间距离均方根误差,作为独立于目标误差函数的监控指标,用于驱动搜索半径自适应更新及收敛判断包括:

36、所述空间距离均方根误差仅针对3d空间距离小于设定的3d欧氏距离评价阈值的有效匹配点对计算,作为独立于所述目标误差函数的监控指标,不参与代价函数计算;

37、所述空间距离均方根误差用于驱动所述搜索半径的自适应更新,以及用于判定是否满足收敛条件而终止迭代。

38、在步骤s5中,所述动态更新下一次迭代的搜索半径包括:

39、将下一次迭代的目标搜索半径设定为当前空间距离均方根误差的设定倍数与预设最小半径之间的最大值,并引入指数衰减因子防止半径过快收缩:

40、;

41、;

42、式中,和分别为目标搜索半径和更新搜索半径,单位为;为目标半径放大系数,无量纲;为最小搜索半径,单位为;为半径保留因子,无量纲;为当前外循环迭代中有效匹配点对的空间距离均方根误差,单位为;为依据当前误差计算的目标搜索半径,单位为;为本轮外循环实际使用的搜索半径,单位为;

43、当迭代次数大于设定下限且时,终止迭代,其中为空间距离收敛阈值,用于判定重叠区空间位置一致性是否达到预设精度要求。

44、在步骤s4中,所述联合状态向量对应的雅可比矩阵由以下两部分拼接而成:

45、第一部分为相对视轴姿态参数对误差函数的偏导项:

46、;

47、式中,为匹配点对残差对全局boresight误差角向量的偏导矩阵(boresight雅可比块),其元素单位为;为各匹配点对的点到基准带目标平面的有符号投影距离构成的残差向量,单位为;为全局boresight误差角向量,为三维向量,单位为;为相对boresight误差角在横滚(roll)方向的分量,单位为;为相对boresight误差角在俯仰(pitch)方向的分量,单位为;为相对boresight误差角在航向(yaw)方向的分量,单位为;为列向量转置符号;为经过罗德里格斯旋转后的激光发射向量在体坐标系下的表示,无量纲单位方向向量;为法向量在体坐标系下的表示,无量纲单位向量;

48、第二部分为通过b样条基函数加权且从带独立pos平移与姿态漂移控制点对误差函数的偏导项:

49、;

50、式中,为匹配点对残差对从带b样条控制点参数向量的偏导矩阵(样条雅可比块),为稀疏矩阵,其中平移参数对应列无量纲、姿态参数对应列单位为;为数据点所处节点区间内四个非零b样条基函数的序号,,为无量纲整数;为由b样条控制点表示的时变pos漂移参数向量,平移分量单位为、姿态分量单位为;为第个三次b样条基函数在数据点采集时刻的取值,无量纲,取值范围为;为全局enu坐标系下目标平面的单位法向量,无量纲。

51、进一步,所述联合状态向量的参数维度由航带数量和b样条控制点数量决定,不随点云数据量增长而增长;

52、在步骤s2中,根据pos轨迹数据的时间范围确定b样条节点矢量和控制点数量,对降采样后的每个数据点,根据该数据点的采集时间戳,预计算并缓存该数据点所对应的b样条节点区间、控制点索引及非零基函数值;

53、在后续迭代中直接调用缓存结果构建状态改正量及雅可比矩阵。

54、在步骤s1中,所述时间同步与坐标映射包括:对pos轨迹数据进行时间域插值,将pos轨迹的位置和姿态角按每个数据点的精确采集时间进行插值;基于插值后的位置和姿态,构建旋转矩阵,将每个数据点的激光发射向量从体坐标系旋转至全局enu坐标系。

55、本发明的另一目的在于提供一种顾及无人机位姿的激光雷达测深航带严密平差系统,该系统用于实现所述顾及无人机位姿的激光雷达测深航带严密平差方法,该系统包括:

56、数据获取与预处理模块,用于获取机载lidar测深多航带原始点云数据及其对应的pos轨迹数据,将点云数据与pos轨迹数据进行时间同步与坐标映射,建立初始局部坐标系;

57、特征提取与状态向量构建模块,用于对点云数据进行降采样,计算点云的几何特征,所述几何特征包括法向量与平面度;基于pos时间戳,构建包含全局boresight误差参数和从带时变pos漂移参数的联合状态向量;

58、动态几何匹配模块,用于基于当前状态向量对从带点云进行位姿更新,在设定的3d欧氏距离阈值约束下,利用几何特征进行重叠区同名点对匹配,并计算匹配点对的空间距离均方根误差;

59、严密平差联合优化模块,用于基于同名点对、法向量以及联合状态向量构建目标误差函数,利用联合赋权模型进行加权,结合先验约束矩阵,使用levenberg-marquardt算法迭代求解最优的状态向量更新量;

60、动态迭代与全分辨率校正模块,用于根据空间距离均方根误差动态更新搜索半径,当空间距离均方根误差满足收敛条件时停止迭代,获得最优联合状态向量,并将其应用于全分辨率原始点云,输出平差优化后的高精度点云数据。

61、结合上述的所有技术方案,本发明所具备的有益效果为:

62、第一、联合参数化方面:本发明通过非刚体时变pos漂移的b样条参数化与一个全局boresight误差参数的联合估计,突破传统刚体配准假设,可逐时刻补偿平移与姿态漂移;联合状态向量维度仅由航带数量与b样条控制点数量决定,不随点云数据量增长。

63、双重赋权方面:物理权重依据激光入射角与激光测距长度刻画观测几何质量,huber统计鲁棒权重依据残差中位数绝对偏差自适应抑制粗差,二者相乘形成联合权重,双通道抑制低质量观测与异常匹配,提升强噪声测深场景下解的鲁棒性。

64、四层先验方面:boresight先验、pos平移与姿态漂移先验、b样条二阶差分平滑、姿态漂移零均值四层先验约束,保证了低维参数解算的可解性与平滑性,防止漂移发散,并使全局boresight参数与时变pos漂移参数可分离估计。

65、rmse闭环与低维稀疏方面:以独立于目标误差函数的空间距离均方根误差驱动搜索半径自适应更新与收敛判断,形成由粗到精、免人工调参的收敛闭环;三次b样条的局部支撑特性使雅可比矩阵天然稀疏,结合b样条基函数预计算缓存,迭代效率高、优化过程可监控。

66、第二、本发明依托pos位姿数据建立严密平差框架,引入boresight误差参数与b样条时变pos漂移参数的联合建模,并配合匹配点空间距离均方根误差动态监控机制,实现从带点云向基准带的高精度对齐。本发明在严格保持基准带地形几何不变的前提下,显著提升多航带重叠区的空间位置一致性,将重叠区匹配点空间距离均方根误差由分米级降低至厘米级。本发明的适配性强,可有效应用于浅海礁石、海岸带等复杂测深场景,以及常规机载激光陆地测量中多航带点云的高精度拼接与融合。

67、以同一数据为例:待估参数数量由非刚性icp逐点优化的17862个降至本发明的261个,下降约98.5%;单次参数解算耗时由0.89s降至0.22s,约为原耗时的四分之一;重叠区匹配点空间距离均方根误差/mae由非刚性icp平差后的8.1/5.9cm进一步降至6.7/5.1cm。

68、本发明显著降低了航带平差的参数求解维度和计算负担。本发明不直接对海量点云坐标设置独立优化变量,而是将航带几何误差参数化为相对boresight安装角参数以及由b样条控制点描述的时变pos位置、姿态改正参数,使待估参数数量主要由航带时间跨度和b样条节点间隔决定,而不随点云数量线性增长。由于三次b样条具有局部支撑特性,每个观测点仅与其所在节点区间对应的有限数量控制点相关,因此所构建的雅可比矩阵具有局部稀疏结构;结合点云网格降采样以及b样条基函数预计算,可进一步减少迭代过程中重复计算的开销。因此,在处理大规模、多航带测深点云时,本发明能够以较低的参数维度完成时变航带误差估计,具有较好的计算效率和工程应用价值。

69、本发明可封装为数据获取与预处理、特征提取与状态构建、动态几何匹配、联合优化、动态迭代与全分辨率校正等模块化软件功能,能够方便地嵌入机载lidar测深数据处理软件、测绘内业软件或无人机载激光雷达数据后处理平台,作为高精度航带拼接/平差的核心引擎对外提供服务。预期可应用于高精度海陆一体化地形测绘、浅水区资源调查、水下工程勘察等场景。

70、现有机载lidar航带平差方法通常主要针对整体刚体变换或有限参数模型进行误差补偿,对于pos时变漂移、boresight系统误差及航带间非刚性几何偏差的联合建模仍存在不足。本发明通过固定基准约束、b样条时变漂移建模和相对boresight联合估计,实现对航带时变几何误差的参数化补偿。

71、在机载lidar航带平差领域,长期存在只有在点云层(全局刚体或逐点)进行高维度的直接优化才能提升平差精度的技术偏见。本发明打破了这一技术偏见,创造性地航带几何误差的求解由直接依赖点云坐标的高维优化转化为基于pos误差参数的低维参数化优化,通过少量b样条控制点参数描述整条航带的时变位置和姿态变化,在保持较高平差精度的同时,有效降低待估参数维度和法方程求解规模,提高大规模点云数据处理的计算效率。

72、第三、本发明可封装为航带严密平差核心算法引擎,嵌入机载lidar测深数据处理软件、测绘内业软件或无人机载激光数据后处理平台,作为高精度航带拼接/平差模块对外提供服务,可显著减少人工编辑航带偏差的内业工时、提升多航带成果一致性与交付效率,并可推广至海陆一体化地形测绘、浅水区资源调查与水下工程勘察等场景。

73、目前能够同时实现全局boresight安装误差与pos时变漂移误差联合严密解算、并以独立的匹配点空间距离均方根误差进行过程闭环监控的机载lidar测深航带平差方案,在国内外公开技术资料中尚不多见。

74、长航带因pos低频非线性时变漂移产生的弯曲、扭曲等非刚性变形,以及boresight安装误差与pos漂移误差相互耦合、分开标定时残差互相吸收,是机载lidar多航带高精度一致性处理中人们一直渴望解决但始终未能有效解决的技术难题;本发明通过b样条时变参数化、全局boresight联合估计与姿态零均值可分离先验设计,对该难题给出了有效解决方案。

75、在机载lidar航带平差领域,长期存在“只有在点云层进行全局刚体或逐点高维直接优化才能提升平差精度”的技术偏见;本发明将航带几何误差的求解由点云坐标高维空间转换到低维pos位姿参数空间,以少量待估参数即达到并优于逐点高维优化的配准精度,克服了上述技术偏见。