一种基于北斗的桅杆吊作业端多源位姿融合解算方法与流程

本发明涉及工程机械位姿检测，尤其涉及一种基于北斗的桅杆吊作业端多源位姿融合解算方法。

背景技术：

1、桅杆吊作为港口、场站与工程施工现场的核心起重装备，其作业端吊具的实时位姿是实现精准吊装、防碰撞与作业自动化的关键参数。传统桅杆吊位姿检测多依赖卷扬编码器与限位开关，仅能获取粗略的绳长与臂架角度，无法给出作业端的三维绝对位姿，且精度低、实时性差。

2、现有部分方案引入北斗定位技术获取臂架位置，但单一北斗系统在遮挡、多路径场景下易出现失锁或精度下降，无法持续提供可靠位姿；辅以惯导的组合方案多采用简单卡尔曼滤波融合，未根据信号质量自适应调整权重，北斗失锁时惯导漂移误差会快速累积。此外，现有方案大多仅解算臂架顶端位姿，未结合卷扬绳长、臂架倾角构建作业端运动学约束，无法直接得到吊具端的精准位姿，也缺乏标定参数自优化机制，长期运行后因安装偏移、臂架形变导致解算精度持续衰减。因此，需要一种能够实现北斗-惯导自适应融合、结合运动学约束修正且具备闭环自优化能力的作业端位姿解算方法。

技术实现思路

1、针对上述存在的技术不足，本发明的目的是提出一种基于北斗的桅杆吊作业端多源位姿融合解算方法，旨在解决现有技术中单一北斗可靠性差、惯导漂移无抑制、缺乏作业端运动学约束修正且无闭环自优化机制，导致位姿解算精度低、场景适应性差且长期稳定性不足的技术问题。

2、为解决上述技术问题，本发明采用如下技术方案：本发明提供一种基于北斗的桅杆吊作业端多源位姿融合解算方法。

3、所述一种基于北斗的桅杆吊作业端多源位姿融合解算方法包括：

4、步骤s10：同步采集桅杆吊臂架北斗定位数据、imu惯导数据、卷扬编码器数据与臂架倾角传感数据，经野值剔除、时空对齐与坐标系统一预处理，输出标准化多源时序数据集、统一坐标系转换参数及传感器标定初值；

5、步骤s20：基于步骤s10得到的标准化多源时序数据集，执行北斗载波相位差分解算与捷联惯导递推算，输出北斗高精度位姿、惯导递推位姿及对应置信度指标；

6、步骤s30：基于步骤s10的传感器标定初值及步骤s20输出的北斗高精度位姿、惯导递推位姿与置信度指标，采用自适应联邦卡尔曼滤波执行多源位姿初融合，输出初融合位姿结果与融合残差序列；

7、步骤s40：基于步骤s30输出的初融合位姿结果及步骤s10的卷扬编码器数据、臂架倾角传感数据，引入作业端运动学约束模型执行误差修正，输出作业端最终位姿解算结果与位姿精度评估参数；

8、步骤s50：根据步骤s40输出的最终位姿解算结果及步骤s30输出的融合残差序列，执行传感器标定参数迭代优化与滤波增益自适应调整，输出位姿解算稳态标志并反馈更新解算基线。

9、优选地，所述步骤s10中，同步采集桅杆吊臂架北斗定位数据、imu惯导数据、卷扬编码器数据与臂架倾角传感数据，经野值剔除、时空对齐与坐标系统一预处理，得到标准化多源时序数据集、统一坐标系转换参数及传感器标定初值的步骤，具体包括：

10、步骤s101：以1hz采样率采集安装于臂架顶端的北斗双模接收机输出的原始观测数据与星历数据，同时以200hz采样率采集同位置安装的imu输出的三轴加速度、三轴角速度数据，以50hz采样率采集卷扬轴编码器输出的绳长数据与臂架倾角传感器输出的俯仰角数据；

11、步骤s102：对北斗原始观测数据进行多路径抑制与野值剔除，对imu数据进行四阶巴特沃斯低通滤波，截止频率20hz，对编码器与倾角数据进行滑动均值去噪，以北斗秒脉冲为时间基准对所有传感数据进行插值对齐，实现毫秒级时空同步，生成等间隔标准化多源时序数据集；

12、步骤s103：建立北斗坐标系、惯导坐标系与桅杆吊机体坐标系的转换关系，标定各传感器安装偏差与臂架几何参数，输出统一坐标系转换参数与传感器标定初值。

13、优选地，所述步骤s20中，基于步骤s10得到的标准化多源时序数据集，执行北斗载波相位差分解算与捷联惯导递推算，输出北斗高精度位姿、惯导递推位姿及对应置信度指标的步骤，具体包括：

14、步骤s201：基于北斗原始观测数据与基准站差分数据，采用载波相位差分算法进行解算，结合周跳检测与模糊度固定，得到臂架顶端的北斗高精度定位结果与姿态角，同时根据可见星数量、pdop值计算北斗定位置信度；

15、步骤s202：以步骤s10的传感器标定初值为初始对准参数，对滤波后的imu数据进行捷联惯导递推算，输出惯导系下的位置、速度与姿态角，同时根据递推时长与角速度噪声水平计算惯导置信度；

16、步骤s203：将北斗位姿与惯导位姿统一转换至机体坐标系下，输出北斗高精度位姿、惯导递推位姿及各自的置信度指标，置信度取值范围为0至1。

17、优选地，所述步骤s201中北斗定位置信度根据可见星数与pdop值分级，可见星数≥8且pdop≤2时置信度取0.95，可见星数5~7且pdop≤4时置信度取0.7，可见星数<5时置信度取0.3及以下，置信度>0。

18、优选地，所述步骤s30中，基于步骤s20输出的北斗高精度位姿、惯导递推位姿、置信度指标与传感器标定初值，采用自适应联邦卡尔曼滤波执行多源位姿初融合，得到初融合位姿结果、融合残差序列及运动学约束参数集的步骤，具体包括：

19、步骤s301：以传感器标定初值为坐标系基准，以北斗与惯导为两个独立子滤波器，分别以各自置信度作为观测噪声权重系数，自适应调整子滤波器的观测噪声协方差矩阵；

20、步骤s302：采用联邦卡尔曼滤波架构，两个子滤波器并行执行滤波更新，主滤波器按置信度加权进行信息融合，得到全局最优的臂架顶端初融合位姿结果；

21、步骤s303：记录每时刻滤波更新的新息序列作为融合残差，同步将运动学约束基础参数进行时间戳匹配后封装为运动学约束参数集，输出初融合位姿结果、融合残差序列及运动学约束参数集。

22、优选地，所述步骤s40中，基于步骤s30输出的初融合位姿结果与运动学约束参数集，引入作业端运动学约束模型执行误差修正，得到作业端最终位姿解算结果与位姿精度评估参数的步骤，具体包括：

23、步骤s401：从运动学约束参数集中提取臂架倾角数据与卷扬绳长数据，结合臂架几何参数，构建桅杆吊作业端运动学模型，由臂架顶端初融合位姿推算作业端吊具的理论位姿；

24、步骤s402：引入绳长约束与摆角约束，对初融合位姿进行运动学误差修正，补偿臂架形变、绳索摆动带来的位姿偏差，得到作业端吊具的三维位置与姿态角；

25、步骤s403：根据融合残差与约束偏差计算位姿精度评估参数，包括水平位置误差估计、高程误差估计，输出作业端最终位姿解算结果与位姿精度评估参数。

26、优选地，所述步骤s50中，根据步骤s40输出的最终位姿解算结果及步骤s30输出的融合残差序列，执行传感器标定参数迭代优化与滤波增益自适应调整，输出位姿解算稳态标志并反馈更新解算基线的步骤，具体包括：

27、步骤s501：当北斗置信度高于设定阈值且融合残差序列持续平稳时，以最终位姿解算结果为比对基准，结合高置信度北斗观测值迭代修正传感器安装偏差与臂架几何参数，输出优化后的标定参数并反馈至步骤s10更新基准；

28、步骤s502：基于融合残差序列的统计特性计算观测噪声偏差，同时结合最终位姿解算结果的时序波动量估计过程噪声水平，联合自适应调整联邦卡尔曼滤波的过程噪声协方差与观测噪声权重，优化后续帧的滤波增益；

29、步骤s503：当连续30帧融合残差均小于设定残差阈值，且最终位姿解算结果的连续帧位姿波动小于限值时，判定解算收敛并置位位姿解算稳态标志，输出稳态标志并反馈更新步骤s20的解算初始基线。

30、本发明还提供一种基于北斗的桅杆吊作业端多源位姿融合解算系统包括：

31、多源传感采集预处理模块：用于同步采集桅杆吊臂架北斗定位数据imu惯导数据、卷扬编码器数据与臂架倾角传感数据，经野值剔除、时空对齐与坐标系统一预处理，输出标准化多源时序数据集、统一坐标系转换参数及传感器标定初值；

32、双源位姿独立解算模块：用于基于多源传感采集预处理模块得到的标准化多源时序数据集与传感器标定初值，执行北斗载波相位差分解算与捷联惯导递推算，输出北斗高精度位姿、惯导递推位姿、对应置信度指标及运动学约束基础参数；

33、自适应滤波初融合模块：用于基于双源位姿独立解算模块输出的北斗高精度位姿、惯导递推位姿、置信度指标与传感器标定初值，采用自适应联邦卡尔曼滤波执行多源位姿初融合，输出初融合位姿结果、融合残差序列及运动学约束参数集；

34、运动学约束修正模块：用于基于自适应滤波初融合模块输出的初融合位姿结果与运动学约束参数集，引入作业端运动学约束模型执行误差修正，输出作业端最终位姿解算结果与位姿精度评估参数；

35、迭代优化闭环反馈模块：用于根据运动学约束修正模块输出的最终位姿解算结果及自适应滤波初融合模块输出的融合残差序列，执行传感器标定参数迭代优化与滤波增益自适应调整，输出位姿解算稳态标志并反馈更新解算基线。

36、本发明还提供一种基于北斗的桅杆吊作业端多源位姿融合解算设备，所述基于北斗的桅杆吊作业端多源位姿融合解算设备包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的基于北斗的桅杆吊作业端多源位姿融合解算程序，所述基于北斗的桅杆吊作业端多源位姿融合解算程序被所述处理器执行时实现上述方法。

37、本发明还提供一种计算机程序产品，所述计算机程序产品包括基于北斗的桅杆吊作业端多源位姿融合解算程序，所述基于北斗的桅杆吊作业端多源位姿融合解算程序被处理器执行时实现上述方法。

38、本发明的有益效果在于：

39、通过同步采集北斗、imu、编码器与倾角多源数据并完成时空对齐与坐标系统一，构建了多维度的位姿观测底座，突破了单一传感器的观测局限，为后续分层融合提供了多源冗余数据支撑，从数据源层面提升了解算的可靠性。

40、采用置信度自适应的联邦卡尔曼滤波架构，根据北斗信号质量与惯导漂移程度动态调整融合权重，北斗信号良好时以北斗为主修正惯导漂移，北斗遮挡时以惯导为主维持位姿输出，兼顾了高精度与高连续性，有效提升了复杂场景下的位姿解算鲁棒性。

41、引入卷扬绳长与臂架倾角的运动学约束模型，将臂架顶端位姿进一步修正为作业端吊具位姿，直接输出吊装作业所需的末端位姿，同时补偿臂架形变、绳索摆动带来的系统误差，使解算结果更贴合实际作业需求。

42、通过融合残差反向迭代优化标定参数与滤波增益，形成采集、解算、融合、修正和优化的完整闭环，可自动校准安装偏差与长期形变带来的参数漂移，使系统具备长期运行精度保持能力，无需频繁人工标定。