基于UWB通信测距的可伸缩机械大臂实时安全预警方法及装置与流程

本发明涉及基于uwb通信测距的可伸缩机械大臂实时安全预警方法及装置

背景技术：

1、汽车起重机、履带起重机等伸缩式工程机械的大臂伸出长度、姿态角度是起重力矩计算、作业安全边界判定、特种设备合规监管的核心参数，测量数据的实时性、准确性与稳定性直接决定起重作业安全性。

2、现有大臂长度测量方案已存在大量公开专利及成熟技术方案，主流技术主要分为机械接触式、光学测距式、雷达测距式、传感器间接测算式四大类，对应多项现有授权及公开专利，各类技术均存在固有技术缺陷，无法适配起重机全工况高精度、高可靠测量需求，具体现有专利技术及对应缺陷如下：

3、1、机械拉线/码盘接触式测量专利：如cn113063381b等现有专利，公开了采用拉绳传感器、霍尔码盘、齿轮减速电位计配合的臂长测量方案，核心原理依靠机械传动结构跟随大臂伸缩采集位移数据，实现臂长测算。该类方案的优势为静态低成本、结构简单、基础稳定性尚可。但长期工程应用及专利实操验证表明，其存在致命缺陷：完全依赖物理机械传动，工地粉尘、油污、高频振动、反复伸缩工况下，极易出现拉线卡滞、磨损、断裂、齿轮间隙偏移、电位计接触不良等故障，设备服役1-2年后测量精度大幅衰减、数据频繁跳变；同时内置式布线安装复杂、拆装维护难度大，无法适配野外恶劣工况长期动态监测，故障率高、运维成本居高不下。

4、2、激光测距测量专利：如cn110654995b等专利，公开了采用内置激光传感器配合限位结构的起重机臂长测量方案，利用激光直线测距原理采集大臂伸缩长度。该方案静态测量精度较高、无机械接触磨损。但实际应用缺陷极为突出：激光测距高度依赖洁净通视光路，粉尘、雾霭、雨雪、强光遮挡工况下极易出现信号遮挡、测距跳变、数据失效，环境适应性极差；设备硬件及标定成本高昂，批量装机性价比低；同时仅能实现单一长度测量，无法同步采集大臂俯仰角、方位角姿态参数且未设置大臂承重弯曲形变补偿机制，大臂受载弯曲时测量误差大幅增大，无法满足高精度力矩计算需求。

5、3、雷达测距及间接传感测量方案：现有公开技术中，部分方案采用毫米波雷达测距、油缸行程传感器间接测算臂长，通过油缸伸缩位移累加换算大臂总长。该类方案可实现无接触测量、无机械损耗，但存在显著短板：雷达器件成本高、波束角偏大，起重机大臂狭窄腔体易产生多径反射干扰，测距稳定性差；油缸行程间接测量方式，受油缸泄漏、伸缩间隙、臂体形变影响，累计误差随伸缩次数持续叠加，无实时校准机制，测量精度低；且仅输出单一长度参数，无姿态同步采集与多维度误差补偿体系，工况适配性与测量精度双重受限。

6、综上，现有已公开专利及通用技术方案，均无法同时兼顾低成本、全工况适配、多维度姿态同步测量、系统化误差自补偿与高精度动态监测的核心需求，普遍存在环境适应性弱、测量维度单一、动态误差大、长期可靠性差的技术短板，难以满足工程机械复杂动态作业场景下的特种设备安全监测与精准力矩控制需求。

技术实现思路

1、发明目的：为了克服现有技术中存在的不足，本发明提供一种基于uwb通信测距的可伸缩机械大臂实时安全预警方法及装置，基于uwb无线电测距芯片（消费级芯片单价可低至1-2美元），大幅降低测距模块成本。

2、技术方案：为解决上述技术问题，本发明的一种基于uwb通信测距的可伸缩机械大臂实时安全预警方法，包括以下步骤：

3、s1、布设测距与定向设备：在可伸缩机械大臂根部安装uwb基站模块，在大臂顶端安装uwb标签模块，所述uwb基站模块与uwb标签模块的天线沿大臂轴线对向布设；在大臂上布设用于采集大臂姿态参数的双天线gnss定向模块；

4、s2、uwb双边双向测距运算：采用ds-twr双边双向测距机制，通过基站与标签之间poll、response、final三组数据包交互，采集六组时间戳并计算信号飞行时间tof，结合光速得到大臂两端直线原始测距值draw；

5、s3、gnss姿态参数采集：通过双天线gnss载波相位差分rtk算法，实时解算大臂的水平方位角与垂直俯仰角，并将双天线基线距离作为辅助校准基准；

6、s4、多维度误差补偿修正：基于预设补偿模型，对原始测距值draw叠加天线延迟补偿量、多径效应补偿量、温度漂移补偿量、大臂弯曲形变补偿量，得到修正后的精准大臂长度dcorrected；

7、s5、数据联动输出：将修正后的大臂长度、方位角、俯仰角实时上传至工程机械主控制器与力矩限制器，实现作业力矩实时计算与安全限位保护。

8、作为优选，步骤s2中，具体过程如下：

9、1）基站端在时刻t1向标签端发送poll数据包，并记录发送时间戳；

10、2）标签端在时刻t2接收到poll数据包，记录接收时间戳；

11、3）标签端经过固定延迟treplyb后，在时刻t3向基站端发送response数据包，并记录发送时间戳；

12、4）基站端在时刻t4接收到response数据包，记录接收时间戳；

13、5）基站端经过固定延迟treplya后，在时刻t5向标签端发送final数据包，并记录发送时间戳；

14、6）标签端在时刻t6接收到final数据包，记录接收时间戳；

15、通过上述三次消息交互中记录的6个时间戳，计算信号的往返飞行时间tof：

16、tround1=t4-t1

17、tround2=t6-t3

18、treply1=t3-t2

19、treply2=t5-t4；

20、则飞行时间tof计算为：

21、tof=(tround1×tround2-treplya×treplyb)/(tround1+tround2+treplya+treplyb)；

22、大臂长度为：draw=c×tof，其中c为光速。

23、作为优选，步骤s3包含以下步骤：

24、s301、设备标定与姿态对齐：将双天线gnss定向模块的主、从天线沿机械大臂轴向平行布设，保持天线基线水平无倾斜、基线平面与大臂作业平面平行，出厂前完成天线相位中心偏差标定、安装零偏校准，固化基线标准距离，消除安装误差与硬件固有偏差；

25、s302、卫星信号采集与滤波筛选：双天线同步接收多系统gnss卫星观测数据，包含载波相位、伪距、卫星星历信息，设置卫星截止高度角，剔除低高度角、多径干扰严重的无效卫星信号，通过信噪比筛选保留高质量观测数据；

26、s303、双差观测方程构建：基于主、从天线的同步观测数据，构建卫星间、天线间的双差载波相位观测方程，消除卫星钟差、接收机钟差、电离层与对流层延时共性误差，保留仅与基线向量相关的有效观测参数；

27、s304、整周模糊度固定解算：采用lambda算法对双差观测值进行迭代解算，快速固定载波相位整周模糊度，通过基线长度约束校验模糊度解算有效性，输出高精度天线基线相对位置向量；

28、s305、姿态角解算输出：将解算得到的基线向量转换至东北天地理坐标系，通过向量三角函数关系实时解算大臂水平方位角与垂直俯仰角，完成姿态数据归一化处理；

29、s306、基线校准基准更新：实时提取双天线解算基线距离，与预设标定标准基线距离比对，将偏差值作为辅助校准参数，同步参与后续uwb测距原始数据的基准修正，实现姿态参数与测距参数的同源校准。

30、作为优选，步骤s4中，具体过程如下：

31、s401、固有硬件延迟标定补偿：调取设备出厂固化的天线延迟补偿参数，结合当前uwb模块工作频段，读取芯片计时节点与天线辐射端的固有传输延迟偏差，计算得到实时天线延迟补偿量δdantenna，消除硬件装配、芯片时序带来的固定系统误差；

32、s402、多径干扰动态补偿：结合s3步骤实时采集的大臂俯仰角、实时原始测距臂长以及起重机大臂预设几何结构参数，调取预标定的臂长-俯仰角-多径偏差映射表，通过实时参数匹配查表获取多径效应补偿量δdmultipath，修正大臂金属壳体对uwb信号反射、折射产生的测距偏移误差；

33、s403、环境温度漂移自适应补偿：通过板载高精度温度传感器实时采集作业环境温度t，调取设备标定的参考温度t0及二次多项式标定系数a0、a1、a2，代入温度补偿模型计算得到温度漂移补偿量δdtemperature，抵消温度变化导致的uwb信号传播速率、模块电路工作参数偏移带来的测距误差；

34、s404、机械弯曲形变误差补偿：采集起重机实时吊重数据、大臂实时俯仰角，结合大臂预设的截面惯性矩、材料弹性模量结构参数，基于悬臂梁形变理论计算大臂臂端实时挠度δ，通过弧长修正公式求解弯曲形变补偿量δddeflection，补偿大臂承重弯曲导致的直线测距值与实际臂长弧长的偏差；

35、s405、多参数融合精准修正：将上述获取的天线延迟补偿量、多径效应补偿量、温度漂移补偿量、大臂弯曲形变补偿量同步叠加至原始测距值draw，通过预设融合公式完成多维度误差统一修正，输出高精度、无系统偏差的精准大臂长度dcorrected。

36、在本发明中，步骤s405中，dcorrected＝draw+δdantenna+δdmultipath+δdtemperature+δddeflection，式中，δdantenna为天线延迟补偿量，δdmultipath为多径效应补偿量，δdtemperature为温度漂移补偿量，δddeflection为大臂弯曲形变补偿量。δdtemperature=a0+a1•(t-t0)+a2•(t-t0)²，其中t为当前温度，t0为标定参考温度，a0、a1、a2为标定系数。δddeflection≈8δ²/(3l)，其中l为大臂长度，δ为臂端挠度估算值，挠度δ可由大臂截面惯性矩i、弹性模量e、当前弯矩m计算：δ=ml²/(2ei)。

37、作为优选，所述步骤s6的具体步骤为：

38、步骤s61系统初始化：预置车辆几何模型与安全阈值参数

39、在设备上电、系统初始化阶段，完成基础参数入库与配置，为后续实时计算提供固定基准参数，支持不同工程机械车型适配：

40、1.车型参数匹配：根据当前作业设备型号，调用系统内置车辆几何参数库，加载对应车型的固定参数，包含：车辆底盘长宽高、大臂铰接点相对底盘定位参考点的三维偏移量、大臂伸缩极限长度最大/最小伸出长度、车辆四个底盘角点相对定位参考点的原始几何偏移向量；

41、2.安全参数配置：人工录入或系统自动加载施工场地邻边边界数据，将铁路、基坑、围挡安全边界转换为场地坐标系下的标定点序列，形成邻边边界轮廓线；同时配置安全预警阈值，包含一级警戒距离、二级禁止距离、预测时长t；

42、3.算法参数初始化：初始化扩展卡尔曼滤波ekf参数，设置状态方程、观测方程、过程噪声、观测噪声初始值，清空历史位置时序数据缓存队列（默认缓存最近10帧、1s内的实时数据）；

43、步骤s62多源异构数据实时同步采集与时间对齐

44、系统以10-20hz固定频率，同步采集多路核心传感数据，统一时间戳，消除多设备数据传输延迟导致的时序错位问题：

45、1.底盘定位数据采集：通过底盘urtk定位模块获取车辆定位参考点的实时三维坐标pref(x0,y0,z0)及车辆航向角θheading，数据刷新频率20hz；

46、2.大臂姿态与长度数据采集：调取步骤s4输出的修正后精准大臂长度dcorrected，同时采集双天线gnss定向模块输出的大臂方位角θaz、俯仰角θel，数据与rtk定位数据时序同步；

47、步骤s63场地坐标系下关键点三维空间坐标实时解算

48、基于校准后的定位、姿态、长度数据，结合车辆几何模型，通过坐标旋转变换、偏移补偿，精准解算车辆四大角点、大臂铰接点、大臂末端臂尖的实时三维空间坐标:

49、1.车辆底盘角点坐标解算：根据车辆航向角θheading构建水平面旋转矩阵r(θheading)，将底盘四个角点的原始几何偏移向量进行旋转变换，叠加底盘参考点坐标pref，得到场地坐标系下四个角点的实时三维坐标pcorneri,i=1、2、3、4，对应前左、前右、后左、后右角点；

50、2.大臂铰接点坐标解算：结合底盘参考点坐标与预置的铰接点三维偏移量，计算大臂根部铰接点的实时空间坐标parmjoint，作为大臂姿态解算的基准支点;

51、3.大臂末端臂尖坐标解算：通过大臂方位角、俯仰角构建空间三维旋转矩阵

52、r(θaz,θel)，结合修正后臂长dcorrected、铰接点偏移量，解算大臂末端臂尖的实时空间坐标pboomtip，完整还原大臂实时空间姿态;

53、步骤s64基于ekf滤波的运动状态实时估计

54、缓存连续多帧关键点空间坐标数据，通过扩展卡尔曼滤波算法平滑原始位置数据，抑制uwb、gnss数据跳变误差，精准求解各关键点的实时运动速度、加速度向量:

55、1.时序数据缓存：将每帧解算的臂尖、底盘角点空间坐标按时间顺序存入缓存队列，始终保留最新10帧有效数据,覆盖0.5-1s作业时序，形成连续位置轨迹序列;

56、2.运动状态建模：以关键点空间位置、运动速度、加速度为状态量，构建匀加速运动模型，结合传感器实测位置数据作为观测值，搭建ekf滤波方程;

57、3.滤波平滑与状态更新：通过ekf预测、校正双步骤，剔除单点异常跳变数据，迭代更新得到各关键点平滑后的实时运动参数，包括三维速度向量vcurrent、三维加速度向量acurrent;

58、步骤s65短时运动轨迹与邻边距离预测

59、基于滤波后的实时运动状态参数，采用匀加速运动公式，预测未来t秒内各核心关键点的空间位置轨迹，同步计算预测时刻的邻边安全距离:

60、1.位置轨迹预测：采用短时运动预测公式：ppredicted=pcurrent+vcurrent×t+0.5×acurrent×t²，分别计算大臂臂尖、车辆四大角点在未来1-3s预设时刻的预测三维坐标，生成连续运动轨迹；

61、2.实时邻边距离计算：将当前帧各关键点实时坐标、未来t秒预测坐标，分别与场地邻边边界标定点序列进行遍历运算，采用点到线段最短距离算法，求解各关键点与安全边界的实时最短距离和预测最短距离；

62、步骤s66预判式安全告警逻辑判定与分级输出

63、结合实时距离、预测距离与预设安全阈值，实现分级预警，完成从实时监测到提前预判的安全防护闭环，适配工地邻边施工安全管控需求。

64、1.安全阈值判定：系统对比预测最短距离与预设警戒阈值：当预测t秒后关键点距离邻边边界小于一级警戒阈值时，触发预判预警；当预测距离小于二级禁止阈值时，触发高危告警并联动设备安全限制；

65、2.告警内容输出：精准输出预判告警信息，包含风险类型、剩余时间、侵入距离、风险关键点（臂尖/车辆角点），例如“大臂末端将在2秒内侵入邻边3米警戒区”。

66、3.数据联动输出：将实时空间坐标、运动轨迹、实时/预测安全距离、告警状态通过can总线、mqtt协议同步输出至力矩限制器、施工安全监管平台，支撑设备力矩保护、可视化监控、防碰撞管控等业务。

67、步骤s67异常冗余防护

68、为保障复杂工地工况下系统稳定运行，增设数据异常容错机制：当uwb测距、gnss定向、rtk定位数据出现丢包、跳变、失效时，系统自动启用历史有效数据缓存或备用校准参数，维持空间计算与预测功能正常运行，同时上传设备异常告警，避免系统宕机导致的安全监测失效。

69、一种基于uwb通信测距的可伸缩机械大臂长度实时测量装置，包括：

70、uwb基站模块，安装于大臂根部，用于发起测距时序、收发uwb数据包、初步运算测距数据；

71、uwb标签模块，安装于大臂顶端，配置锂电池与超低功耗休眠电路，用于被动响应测距交互；

72、双天线gnss定向模块，包括两组gnss天线与rtk接收机，用于实时输出大臂方位角、俯仰角与基线长度；

73、专用安装支架组件，包括uwb模块支架与gnss天线支架；

74、主控制器单元，电连接所述uwb基站模块与双天线gnss定向模块，内置误差补偿算法，用于完成数据修正、逻辑运算、对外通信与力矩限制器联动。

75、作为优选，所述uwb模块支架设有u型卡箍与万向角度调节机构，用于保证天线沿大臂轴线对向布设；所述gnss天线支架设有水平调平结构与高度调节杆，保证基线方向与大臂轴线平行。

76、有益效果：本发明的基于uwb通信测距的可伸缩机械大臂实时安全预警方法及装置，具有以下优点：

77、1.本发明融合底盘rtk定位、双天线gnss定向、经误差补偿的uwb精准臂长，结合标准化车辆几何模型，统一场地全局坐标系，可同步解算车辆四个底盘角点、大臂铰接点、臂尖末端全部三维空间坐标，完整还原吊车整机实时空间姿态；无需多套独立定位设备分别校准，多源传感数据天然时序匹配，为智慧工地全局安全管控提供完整空间基础数据。

78、2.本发明通过扩展卡尔曼滤波对多帧连续位置时序数据平滑处理，精准估算各关键点实时三维速度、加速度，基于匀加速运动模型预测未来1～3秒设备运动轨迹，并提前计算预测时刻与邻边边界的最短安全距离；可在机械尚未触碰警戒区时提前输出风险提示，给予操作人员充足操作缓冲时间，从源头降低铁路、基坑、管线等受限区域施工安全事故发生率，显著提升特种工程机械作业安全冗余。

79、3.本发明采用ekf滤波算法对连续10帧位置数据迭代校正，有效抑制传感器随机误差造成的坐标抖动，输出连续平滑的运动轨迹；同时设置多重数据冗余容错机制，当uwb测距、gnss定向、底盘rtk任一传感信号短时丢失或异常时，依靠历史缓存有效数据持续完成空间坐标解算与运动预测，同步上报传感器故障告警，避免安全监测功能中断，适配粉尘、遮挡、多金属反射等复杂野外施工工况。