Real-time safety warning method and device for scalable mechanical arm based on UWB communication ranging

Abstract

The application discloses a scalable mechanical arm real-time safety warning method and device based on UWB communication ranging, aiming at the technical defects of low reliability of contact measurement, high cost of optical / radar ranging, poor environmental adaptability, no system error compensation and single measurement dimension existing in the existing crane arm measurement scheme, the application adopts UWB double-side bidirectional DS-TWR ranging to obtain the linear original distance of both ends of the arm, combines with the double-antenna GNSS carrier phase difference directional real-time collection of the azimuth angle and the pitch angle attitude information, constructs a four-dimensional error compensation model containing antenna delay, multipath effect, temperature drift and bending deflection, performs global correction on the original ranging data, and finally outputs high-precision real-time arm length parameters. The application also discloses a matching special measurement device, realizes non-contact, low-cost, high-refresh-rate and full-working-condition stable measurement, and effectively solves the problems that the traditional scheme is easily interfered by dust, rain, snow and deformation.

Description

Technical Field

[0001] This invention relates to a real-time safety early warning method and device for a telescopic robotic arm based on UWB communication ranging. Background Technology

[0002] The boom extension length and attitude angle of telescopic construction machinery such as truck cranes and crawler cranes are core parameters for calculating lifting torque, determining operational safety boundaries, and supervising compliance with special equipment regulations. The real-time performance, accuracy, and stability of the measurement data directly determine the safety of lifting operations.

[0003] Existing solutions for boom length measurement already have numerous publicly available patents and mature technologies. The mainstream technologies can be broadly categorized into four types: mechanical contact, optical ranging, radar ranging, and sensor-based indirect measurement. Each type corresponds to multiple existing authorized and publicly available patents. However, all these technologies have inherent technical limitations and cannot meet the high-precision, high-reliability measurement requirements of cranes under all operating conditions. Specific existing patented technologies and their corresponding limitations are as follows:

[0004] 1. Mechanical Wire / Code Disc Contact Measurement Patents: Existing patents such as CN113063381B disclose arm length measurement schemes using a wire sensor, Hall effect code disc, and gear reduction potentiometer. The core principle relies on a mechanical transmission structure to collect displacement data as the arm extends and retracts, thereby calculating the arm length. The advantages of this type of scheme are low static cost, simple structure, and reasonable basic stability. However, long-term engineering applications and patent practical verification have shown that it has fatal flaws: relying entirely on physical mechanical transmission, it is prone to failures such as wire jamming, wear, breakage, gear backlash misalignment, and poor potentiometer contact under conditions of construction site dust, oil, high-frequency vibration, and repeated extension and retraction. After 1-2 years of service, the measurement accuracy decreases significantly and the data fluctuates frequently. At the same time, the built-in wiring is complex to install and difficult to disassemble and maintain, making it unsuitable for long-term dynamic monitoring in harsh field conditions, resulting in a high failure rate and high maintenance costs.

[0005] 2. Laser ranging measurement patents: Patents such as CN110654995B disclose a crane boom length measurement scheme that uses a built-in laser sensor in conjunction with a limiting structure, utilizing the laser linear ranging principle to collect the boom extension length. This scheme has high static measurement accuracy and no mechanical contact wear. However, its practical application has significant drawbacks: laser ranging is highly dependent on a clean line of sight optical path, and is prone to signal obstruction, ranging jumps, and data failures under conditions of dust, fog, rain, snow, and strong light obstruction, resulting in extremely poor environmental adaptability; the equipment hardware and calibration costs are high, leading to low cost-effectiveness for mass installation; at the same time, it can only achieve single length measurement, and cannot simultaneously collect boom pitch angle and azimuth attitude parameters, and it does not have a boom bending deformation compensation mechanism under load. When the boom is bent under load, the measurement error increases significantly, failing to meet the requirements for high-precision torque calculation.

[0006] 3. Radar ranging and indirect sensing measurement schemes: Among existing publicly available technologies, some schemes use millimeter-wave radar ranging and hydraulic cylinder stroke sensors to indirectly calculate the boom length, and then calculate the total boom length by accumulating the displacement of the hydraulic cylinder extension and retraction. This type of scheme can achieve non-contact measurement and no mechanical loss, but it has significant shortcomings: radar devices are expensive and have a large beam angle; the narrow cavity of the crane boom is prone to multipath reflection interference, resulting in poor ranging stability; the indirect measurement method using hydraulic cylinder stroke is affected by hydraulic cylinder leakage, extension and retraction clearance, and boom deformation, and the cumulative error continues to accumulate with the number of extensions and retractions, without a real-time calibration mechanism, resulting in low measurement accuracy; moreover, it only outputs a single length parameter, without synchronous attitude acquisition and a multi-dimensional error compensation system, which limits both the adaptability to working conditions and the measurement accuracy.

[0007] In summary, existing publicly available patents and general technical solutions cannot simultaneously meet the core requirements of low cost, adaptability to all working conditions, multi-dimensional attitude synchronous measurement, systematic error self-compensation, and high-precision dynamic monitoring. They generally suffer from technical shortcomings such as weak environmental adaptability, single measurement dimension, large dynamic error, and poor long-term reliability, making it difficult to meet the safety monitoring and precise torque control requirements of special equipment in complex dynamic operation scenarios of engineering machinery. Summary of the Invention

[0008] Purpose of the invention: In order to overcome the shortcomings of the existing technology, the present invention provides a real-time safety early warning method and device for a telescopic mechanical arm based on UWB communication ranging, which is based on a UWB radio ranging chip (the unit price of consumer-grade chips can be as low as 1-2 US dollars), which greatly reduces the cost of the ranging module.

[0009] Technical Solution: To solve the above-mentioned technical problems, the present invention provides a real-time safety early warning method for a retractable mechanical boom based on UWB communication ranging, comprising the following steps:

[0010] S1. Deploy ranging and orientation equipment: Install a UWB base station module at the root of the telescopic mechanical boom and a UWB tag module at the top of the boom. The antennas of the UWB base station module and the UWB tag module are deployed opposite each other along the axis of the boom. Deploy a dual-antenna GNSS orientation module on the boom for collecting the boom attitude parameters.

[0011] S2, UWB bilateral bidirectional ranging calculation: Employing the DS-TWR bilateral bidirectional ranging mechanism, it collects six timestamps and calculates the signal time-of-flight (ToF) through the interaction of three sets of data packets (Poll, Response, and Final) between the base station and the tag, combining this with the speed of light to obtain the original straight-line ranging value d at both ends of the boom. raw ;

[0012] S3, GNSS attitude parameter acquisition: The horizontal azimuth and vertical pitch angles of the boom are calculated in real time using the dual-antenna GNSS carrier phase differential RTK algorithm, and the baseline distance between the two antennas is used as an auxiliary calibration reference.

[0013] S4. Multi-dimensional error compensation correction: Based on a preset compensation model, the original ranging value d is corrected. raw By superimposing antenna delay compensation, multipath effect compensation, temperature drift compensation, and boom bending deformation compensation, the corrected precise boom length d is obtained. corrected ;

[0014] S5. Data Linkage Output: The corrected boom length, azimuth angle, and pitch angle are uploaded to the main controller and torque limiter of the construction machinery in real time to realize real-time calculation of working torque and safety limit protection.

[0015] As a preferred option, the specific process in step S2 is as follows:

[0016] 1) The base station sends a Poll data packet to the tag at time t1 and records the sending timestamp;

[0017] 2) The tag receives the Poll data packet at time t2 and records the receiving timestamp;

[0018] 3) After a fixed delay of TreplyB, the tag sends a Response data packet to the base station at time t3 and records the sending timestamp;

[0019] 4) The base station receives the Response data packet at time t4 and records the receiving timestamp;

[0020] 5) After a fixed delay of TreplyA, the base station sends a Final data packet to the tag at time t5 and records the sending timestamp;

[0021] 6) The tag receives the Final data packet at time t6 and records the receiving timestamp;

[0022] The round-trip time of flight (ToF) of the signal is calculated using the six timestamps recorded in the three message exchanges mentioned above.

[0023] Tround1=t4-t1

[0024] Tround2=t6-t3

[0025] Treply1=t3-t2

[0026] Treply2 = t5 - t4;

[0027] The Time of Flight (ToF) is then calculated as follows:

[0028] ToF=(Tround1×Tround2-TreplyA×TreplyB) / (Tround1+Tround2+TreplyA+TreplyB);

[0029] The length of the upper arm is: d raw =c×ToF, where c is the speed of light.

[0030] Preferably, step S3 includes the following steps:

[0031] S301. Equipment calibration and attitude alignment: The master and slave antennas of the dual-antenna GNSS directional module are arranged parallel to the axis of the mechanical arm, keeping the antenna baseline horizontal and without tilt, and the baseline plane parallel to the working plane of the arm. Before leaving the factory, the antenna phase center deviation calibration and installation zero-bias calibration are completed, the baseline standard distance is solidified, and installation errors and inherent hardware deviations are eliminated.

[0032] S302. Satellite signal acquisition and filtering: Dual antennas synchronously receive observation data from multi-system GNSS satellites, including carrier phase, pseudorange, and satellite ephemeris information. Set the satellite cutoff elevation angle, remove invalid satellite signals with low elevation angles and severe multipath interference, and retain high-quality observation data through signal-to-noise ratio filtering.

[0033] S303, Construction of double-difference observation equations: Based on the synchronous observation data of the master and slave antennas, construct double-difference carrier phase observation equations between satellites and between antennas to eliminate common errors of satellite clock error, receiver clock error, ionospheric and tropospheric delay, and retain effective observation parameters that are only related to the baseline vector;

[0034] S304. Integer Ambiguity Fixation: The LAMBDA algorithm is used to iteratively solve the double-difference observations to quickly fix the integer ambiguity of the carrier phase. The validity of the ambiguity solution is verified by the baseline length constraint, and a high-precision antenna baseline relative position vector is output.

[0035] S305, Attitude Angle Calculation Output: The calculated baseline vector is converted to the Northeast Atmosphere coordinate system, and the horizontal azimuth and vertical pitch angles of the boom are calculated in real time through vector trigonometric function relationships to complete the attitude data normalization process.

[0036] S306, Baseline Calibration Benchmark Update: Extract the baseline distance calculated by dual antennas in real time, compare it with the preset calibration standard baseline distance, use the deviation value as an auxiliary calibration parameter, and synchronously participate in the benchmark correction of subsequent UWB ranging raw data to achieve the same source calibration of attitude parameters and ranging parameters.

[0037] As a preferred option, the specific process in step S4 is as follows:

[0038] S401. Inherent Hardware Delay Calibration Compensation: Retrieve the antenna delay compensation parameters preset at the factory, combine them with the current UWB module operating frequency band, read the inherent transmission delay deviation between the chip timing node and the antenna radiating end, and calculate the real-time antenna delay compensation amount Δd. antennaEliminate fixed system errors caused by hardware assembly and chip timing;

[0039] S402. Dynamic Compensation for Multipath Interference: Combining the boom pitch angle, real-time original rangefinder boom length, and preset geometric parameters of the crane boom collected in real time in step S3, the pre-calibrated boom length-pitch angle-multipath deviation mapping table is retrieved, and the multipath effect compensation amount Δd is obtained by looking up the table through real-time parameter matching. multipath Correcting the ranging offset error caused by the reflection and refraction of UWB signals by the metal shell of the boom;

[0040] S403, Ambient Temperature Drift Adaptive Compensation: The ambient temperature T is collected in real time by an onboard high-precision temperature sensor. The reference temperature T0 calibrated by the equipment and the quadratic polynomial calibration coefficients a0, a1, and a2 are retrieved and substituted into the temperature compensation model to calculate the temperature drift compensation amount Δd. temperature This offsets the ranging error caused by temperature changes in the UWB signal propagation rate and the offset of module circuit operating parameters.

[0041] S404. Mechanical Bending Deformation Error Compensation: Real-time crane load data and boom pitch angle are collected. Combined with the boom's preset cross-sectional moment of inertia and material elastic modulus structural parameters, the real-time boom end deflection δ is calculated based on cantilever beam deformation theory. The bending deformation compensation amount Δd is then solved using the arc length correction formula. deflection To compensate for the deviation between the straight-line distance measurement value and the actual arm length arc length caused by the bending of the boom under load;

[0042] S405, Multi-parameter fusion precise correction: The antenna delay compensation, multipath effect compensation, temperature drift compensation, and boom bending deformation compensation obtained above are synchronously superimposed onto the original ranging value d. raw By using a preset fusion formula, multi-dimensional errors are uniformly corrected, resulting in a high-precision, unbiased output of the boom length d. corrected .

[0043] In this invention, in step S405, d corrected =d raw +Δd antenna +Δd multipath +Δd temperature +Δd deflection In the formula, Δd antenna Δd is the antenna delay compensation amount. multipath Δd is the multipath effect compensation amount. temperature Δd is the temperature drift compensation amount. deflection This is the compensation amount for the bending deformation of the boom. Δd temperature =a0+a1•(T-T0)+a2•(T-T0)², where T is the current temperature, T0 is the calibration reference temperature, and a0, a1, and a2 are calibration coefficients. Δddeflection ≈8δ² / (3L), where L is the length of the boom and δ is the estimated value of the boom end deflection. The deflection δ can be calculated from the moment of inertia I, elastic modulus E, and current bending moment M of the boom section: δ=ML² / (2EI).

[0044] Preferably, the specific steps of step S6 are as follows:

[0045] Step S61 System Initialization: Preset vehicle geometry model and safety threshold parameters

[0046] During the equipment power-on and system initialization phases, basic parameters are entered into the database and configured to provide fixed benchmark parameters for subsequent real-time calculations, supporting adaptation to different construction machinery models.

[0047] 1. Vehicle model parameter matching: Based on the current operating equipment model, the system's built-in vehicle geometric parameter library is called to load the fixed parameters of the corresponding vehicle model, including: vehicle chassis length, width and height, three-dimensional offset of the boom hinge point relative to the chassis positioning reference point, maximum / minimum extension length of the boom extension limit, and original geometric offset vectors of the four chassis corner points relative to the positioning reference point.

[0048] 2. Safety parameter configuration: The adjacent boundary data of the construction site is manually entered or automatically loaded by the system to convert the safety boundaries of railways, foundation pits and fences into a sequence of calibration points in the site coordinate system to form the outline of the adjacent boundary; at the same time, safety warning thresholds are configured, including the first-level warning distance, the second-level prohibition distance and the prediction duration T;

[0049] 3. Algorithm parameter initialization: Initialize the extended Kalman filter (EKF) parameters, set the initial values ​​of the state equation, observation equation, process noise, and observation noise, and clear the historical location time series data buffer queue (by default, the most recent 10 frames and 1 second of real-time data are cached).

[0050] Step S62: Real-time synchronous acquisition and time alignment of multi-source heterogeneous data

[0051] The system synchronously collects data from multiple core sensors at a fixed frequency of 10-20Hz, unifies timestamps, and eliminates timing misalignment issues caused by data transmission delays from multiple devices.

[0052] 1. Chassis positioning data acquisition: The real-time three-dimensional coordinates Pref(x0,y0,z0) of the vehicle positioning reference point and the vehicle heading angle θ are obtained through the chassis URTK positioning module. heading The data refresh rate is 20Hz.

[0053] 2. Upper arm posture and length data acquisition: Retrieve the corrected and accurate upper arm length d from the output of step S4. corrected Simultaneously, the azimuth angle θ of the boom output from the dual-antenna GNSS directional module is acquired. az Pitch angle θel The data is synchronized with the RTK positioning data in time.

[0054] Step S63 Real-time calculation of the three-dimensional spatial coordinates of key points in the site coordinate system

[0055] Based on the calibrated positioning, attitude, and length data, combined with the vehicle's geometric model, and through coordinate rotation transformation and offset compensation, the real-time three-dimensional spatial coordinates of the vehicle's four corner points, the boom hinge point, and the boom tip are accurately calculated.

[0056] 1. Calculation of vehicle chassis corner coordinates: based on the vehicle heading angle θ heading Construct the horizontal plane rotation matrix R(θ) heading The original geometric offset vectors of the four corner points of the chassis are rotated and transformed, and then superimposed with the chassis reference point coordinates Pref to obtain the real-time three-dimensional coordinates P of the four corner points in the site coordinate system. corneri i = 1, 2, 3, 4, corresponding to the front left, front right, back left, and back right corner points;

[0057] 2. Calculation of boom hinge point coordinates: Combining the chassis reference point coordinates with the preset 3D offset of the hinge point, calculate the real-time spatial coordinates P of the boom root hinge point. armjoint , serving as the reference fulcrum for calculating the upper arm's attitude;

[0058] 3. Calculation of the coordinates of the boom tip: Construct a three-dimensional rotation matrix using the boom azimuth and pitch angles.

[0059] R(θ az ,θ el ), combined with the corrected arm length d corrected The hinge point offset is used to calculate the real-time spatial coordinate P of the boom tip at the end of the boom. boomtip Completely restores the real-time spatial posture of the upper arm;

[0060] Step S64: Real-time motion state estimation based on EKF filtering

[0061] The system caches spatial coordinate data of key points across multiple consecutive frames, smooths the original position data using an extended Kalman filter algorithm, suppresses UWB and GNSS data jump errors, and accurately calculates the real-time motion velocity and acceleration vectors of each key point.

[0062] 1. Time-series data caching: The spatial coordinates of the boom tip and chassis corner points calculated for each frame are stored in a cache queue in chronological order, always retaining the latest 10 frames of valid data, covering 0.5-1s of operation time sequence, forming a continuous position trajectory sequence;

[0063] 2. Motion State Modeling: Using the spatial position, velocity, and acceleration of key points as state variables, a uniformly accelerated motion model is constructed. Combined with sensor-measured position data as observations, an EKF filtering equation is established.

[0064] 3. Filtering, Smoothing, and State Update: Through a two-step EKF prediction and correction process, single-point anomalous jump data is eliminated, and the smoothed real-time motion parameters of each key point are obtained through iterative updates, including the three-dimensional velocity vector V. current 3D acceleration vector A current ;

[0065] Step S65: Short-time motion trajectory and adjacent distance prediction

[0066] Based on the filtered real-time motion state parameters, and using the uniform acceleration motion formula, the spatial position trajectory of each core key point within the next T seconds is predicted, and the adjacent safety distance at the predicted time is calculated simultaneously.

[0067] 1. Position trajectory prediction: Using the short-time motion prediction formula: P predicted =P current +V current ×T+0.5×A current ×T², calculate the predicted three-dimensional coordinates of the boom tip and the four corner points of the vehicle at preset times of 1-3 seconds in the future, and generate a continuous motion trajectory.

[0068] 2. Real-time neighbor distance calculation: The real-time coordinates of each key point in the current frame and the predicted coordinates in the next T seconds are traversed and calculated with the sequence of adjacent boundary calibration points of the site. The shortest distance algorithm from point to line segment is used to solve the real-time shortest distance and the predicted shortest distance between each key point and the safety boundary.

[0069] Step S66: Predictive security alarm logic judgment and hierarchical output

[0070] By combining real-time distance, predicted distance, and preset safety thresholds, a tiered early warning system is implemented, completing a closed loop of safety protection from real-time monitoring to advance prediction, thus meeting the safety management needs of construction sites adjacent to the site.

[0071] 1. Safety threshold determination: The system compares the predicted shortest distance with the preset warning threshold: when the distance from the key point to the adjacent boundary is less than the first-level warning threshold after T seconds, a pre-judgment warning is triggered; when the predicted distance is less than the second-level prohibition threshold, a high-risk alarm is triggered and the equipment safety restrictions are linked.

[0072] 2. Alarm content output: Accurately outputs predicted alarm information, including risk type, remaining time, intrusion distance, and key risk points (arm tip / vehicle corner), such as "the end of the boom will intrude into the adjacent 3-meter warning zone within 2 seconds".

[0073] 3. Data linkage output: Real-time spatial coordinates, motion trajectory, real-time / predicted safety distance, and alarm status are synchronously output to the torque limiter and construction safety supervision platform via CAN bus and MQTT protocol, supporting equipment torque protection, visual monitoring, collision avoidance control and other services.

[0074] Step S67 Abnormal Redundancy Protection

[0075] To ensure stable system operation under complex construction site conditions, a data anomaly tolerance mechanism has been added: when UWB ranging, GNSS orientation, and RTK positioning data experience packet loss, jumps, or failures, the system automatically activates historical valid data cache or backup calibration parameters to maintain the normal operation of spatial calculation and prediction functions, while simultaneously uploading equipment anomaly alarms to prevent safety monitoring failures caused by system downtime.

[0076] A real-time measurement device for the length of a retractable robotic arm based on UWB communication ranging, comprising:

[0077] The UWB base station module is installed at the base of the boom and is used to initiate ranging timing, send and receive UWB data packets, and perform preliminary ranging data calculations.

[0078] UWB tag module, installed at the top of the boom, is equipped with lithium battery and ultra-low power sleep circuit for passive response ranging interaction;

[0079] The dual-antenna GNSS orientation module includes two sets of GNSS antennas and an RTK receiver, used to output the azimuth, elevation and baseline length of the boom in real time;

[0080] Dedicated mounting bracket assemblies, including UWB module brackets and GNSS antenna brackets;

[0081] The main controller unit is electrically connected to the UWB base station module and the dual-antenna GNSS directional module. It has a built-in error compensation algorithm to complete data correction, logical operations, external communication and torque limiter linkage.

[0082] Preferably, the UWB module bracket is equipped with a U-shaped clamp and a universal angle adjustment mechanism to ensure that the antenna is arranged in opposite directions along the axis of the boom; the GNSS antenna bracket is equipped with a horizontal leveling structure and a height adjustment rod to ensure that the baseline direction is parallel to the axis of the boom.

[0083] Beneficial effects: The real-time safety early warning method and device for retractable mechanical boom based on UWB communication ranging of the present invention has the following advantages:

[0084] 1. This invention integrates chassis RTK positioning, dual-antenna GNSS orientation, and error-compensated UWB precise boom length, combined with a standardized vehicle geometric model and a unified global coordinate system for the site. It can simultaneously calculate the three-dimensional spatial coordinates of the four chassis corners, boom hinge points, and boom tip, completely restoring the real-time spatial attitude of the entire crane. It eliminates the need for separate calibration of multiple independent positioning devices, and the multi-source sensor data naturally matches the time sequence, providing complete spatial foundation data for the overall safety management of smart construction sites.

[0085] 2. This invention uses extended Kalman filtering to smooth multi-frame continuous position time-series data, accurately estimates the real-time three-dimensional velocity and acceleration of each key point, predicts the equipment's trajectory for the next 1-3 seconds based on a uniform acceleration motion model, and calculates in advance the shortest safe distance between the predicted time and the adjacent boundary. It can output risk warnings in advance before the machinery touches the warning zone, giving operators sufficient buffer time to operate, reducing the incidence of construction safety accidents in restricted areas such as railways, foundation pits, and pipelines from the source, and significantly improving the safety redundancy of special engineering machinery operations.

[0086] 3. This invention uses the EKF filtering algorithm to iteratively correct the position data of 10 consecutive frames, effectively suppressing coordinate jitter caused by random sensor errors and outputting a continuous and smooth motion trajectory. At the same time, it sets up multiple data redundancy fault tolerance mechanisms. When any sensor signal of UWB ranging, GNSS orientation, or chassis RTK is lost or abnormal for a short time, it relies on historical cached valid data to continuously complete spatial coordinate calculation and motion prediction, and synchronously reports sensor fault alarms to avoid interruption of safety monitoring functions. It is suitable for complex field construction conditions such as dust, obstruction, and multi-metal reflection. Attached Figure Description

[0087] Figure 1 This is a schematic diagram of the installation of the present invention.

[0088] Figure 2 This is the timing diagram for the UWBDS-TWR bilateral bidirectional ranging of the present invention.

[0089] Figure 3 This is a flowchart of the error compensation model.

[0090] In the diagram: 101: UWB base station module; 102: UWB tag module; 103: UWB base station bracket; 104: UWB tag bracket; 105: Dual-antenna GNSS directional module; 105a: GNSS first antenna; 105b: GNSS second antenna; 106a: First GNSS antenna bracket; 106b: Second GNSS antenna bracket; 107: Main controller; 108: URTK positioning module; 200: Telescopic mechanical arm; 201: Base of the arm (root); 202: Top of the arm (tip). Detailed Implementation

[0091] The invention will now be further described with reference to the accompanying drawings.

[0092] like Figures 1 to 3 As shown, the present invention provides a real-time safety early warning method for a retractable mechanical boom based on UWB communication ranging, comprising the following steps:

[0093] S1. Deploy ranging and orientation equipment: Install a UWB base station module at the root of the telescopic mechanical boom and a UWB tag module at the top of the boom. The antennas of the UWB base station module and the UWB tag module are deployed opposite each other along the axis of the boom. Deploy a dual-antenna GNSS orientation module on the boom for collecting the boom attitude parameters.

[0094] S2, UWB bilateral bidirectional ranging calculation: Employing the DS-TWR bilateral bidirectional ranging mechanism, it collects six timestamps and calculates the signal time-of-flight (ToF) through the interaction of three sets of data packets (Poll, Response, and Final) between the base station and the tag, combining this with the speed of light to obtain the original straight-line ranging value d at both ends of the boom. raw ;

[0095] S3, GNSS attitude parameter acquisition: The horizontal azimuth and vertical pitch angles of the boom are calculated in real time using the dual-antenna GNSS carrier phase differential RTK algorithm, and the baseline distance between the two antennas is used as an auxiliary calibration reference.

[0096] S4. Multi-dimensional error compensation correction: Based on a preset compensation model, the original ranging value d is corrected. raw By superimposing antenna delay compensation, multipath effect compensation, temperature drift compensation, and boom bending deformation compensation, the corrected precise boom length d is obtained. corrected ;

[0097] S5. Data Linkage Output: The corrected boom length, azimuth angle, and pitch angle are uploaded to the main controller and torque limiter of the construction machinery in real time to realize real-time calculation of working torque and safety limit protection.

[0098] S6. Based on RTK positioning and vehicle model, real-time spatial position calculation and motion prediction are performed. The distance between the spatial coordinates of each key point and the adjacent edge line calibration point sequence is calculated to obtain the real-time shortest distance between each key point and the adjacent edge line. The system predicts the distance between the arm tip and the adjacent edge line after T seconds. When the predicted shortest distance is lower than the set threshold, the system issues a predictive alarm 1-3 seconds in advance.

[0099] In this invention, such as Figure 2 As shown, the specific process in step S2 is as follows:

[0100] 1) The base station sends a Poll data packet to the tag at time t1 and records the sending timestamp;

[0101] 2) The tag receives the Poll data packet at time t2 and records the receiving timestamp;

[0102] 3) After a fixed delay of TreplyB, the tag sends a Response data packet to the base station at time t3 and records the sending timestamp;

[0103] 4) The base station receives the Response data packet at time t4 and records the receiving timestamp;

[0104] 5) After a fixed delay of TreplyA, the base station sends a Final data packet to the tag at time t5 and records the sending timestamp;

[0105] 6) The tag receives the Final data packet at time t6 and records the receiving timestamp;

[0106] The round-trip time of flight (ToF) of the signal is calculated using the six timestamps recorded in the three message exchanges mentioned above.

[0107] Tround1=t4-t1

[0108] Tround2=t6-t3

[0109] Treply1=t3-t2

[0110] Treply2 = t5 - t4;

[0111] The Time of Flight (ToF) is then calculated as follows:

[0112] ToF=(Tround1×Tround2-TreplyA×TreplyB) / (Tround1+Tround2+TreplyA+TreplyB);

[0113] The length of the upper arm is: d raw =c×ToF, where c is the speed of light.

[0114] In this invention, step S3 includes the following steps:

[0115] S301. Equipment calibration and attitude alignment: The master and slave antennas of the dual-antenna GNSS directional module are arranged parallel to the axis of the mechanical arm, keeping the antenna baseline horizontal and without tilt, and the baseline plane parallel to the working plane of the arm. Before leaving the factory, the antenna phase center deviation calibration and installation zero-bias calibration are completed, the baseline standard distance is solidified, and installation errors and inherent hardware deviations are eliminated.

[0116] S302. Satellite signal acquisition and filtering: Dual antennas synchronously receive observation data from multi-system GNSS satellites, including carrier phase, pseudorange, and satellite ephemeris information. Set the satellite cutoff elevation angle, remove invalid satellite signals with low elevation angles and severe multipath interference, and retain high-quality observation data through signal-to-noise ratio filtering.

[0117] S303, Construction of double-difference observation equations: Based on the synchronous observation data of the master and slave antennas, construct double-difference carrier phase observation equations between satellites and between antennas to eliminate common errors of satellite clock error, receiver clock error, ionospheric and tropospheric delay, and retain effective observation parameters that are only related to the baseline vector;

[0118] S304. Integer Ambiguity Fixation: The LAMBDA algorithm is used to iteratively solve the double-difference observations to quickly fix the integer ambiguity of the carrier phase. The validity of the ambiguity solution is verified by the baseline length constraint, and a high-precision antenna baseline relative position vector is output.

[0119] S305, Attitude Angle Calculation Output: The calculated baseline vector is converted to the Northeast Atmosphere coordinate system, and the horizontal azimuth and vertical pitch angles of the boom are calculated in real time through vector trigonometric function relationships to complete the attitude data normalization process.

[0120] S306, Baseline Calibration Benchmark Update: Extract the baseline distance calculated by dual antennas in real time, compare it with the preset calibration standard baseline distance, use the deviation value as an auxiliary calibration parameter, and synchronously participate in the benchmark correction of subsequent UWB ranging raw data to achieve the same source calibration of attitude parameters and ranging parameters.

[0121] In this invention, step S4 is specifically performed as follows:

[0122] S401, Inherent Hardware Delay Calibration Compensation: Retrieve the antenna delay compensation parameters that are fixed at the factory of the device, combine them with the current UWB module operating frequency band, read the inherent transmission delay deviation between the chip timing node and the antenna radiating end, calculate the real-time antenna delay compensation amount Δdantenna, and eliminate the fixed system errors caused by hardware assembly and chip timing.

[0123] S402, Multipath Interference Dynamic Compensation: Combining the boom pitch angle, real-time original ranging boom length, and preset geometric parameters of the crane boom collected in real time in step S3, the pre-calibrated boom length-pitch angle-multipath deviation mapping table is retrieved, and the multipath effect compensation amount Δdmultipath is obtained by real-time parameter matching and table lookup, correcting the ranging offset error caused by the reflection and refraction of UWB signals by the boom metal shell.

[0124] S403, Ambient Temperature Drift Adaptive Compensation: The ambient temperature T is collected in real time by an onboard high-precision temperature sensor. The reference temperature T0 calibrated by the equipment and the quadratic polynomial calibration coefficients a0, a1, a2 are retrieved and substituted into the temperature compensation model to calculate the temperature drift compensation amount Δdtemperature, which offsets the ranging error caused by the UWB signal propagation rate and the deviation of the module circuit working parameters due to temperature changes.

[0125] S404, Mechanical Bending Deformation Error Compensation: Collect real-time lifting data of the crane and real-time pitch angle of the boom. Combine the pre-set cross-sectional moment of inertia and material elastic modulus structural parameters of the boom. Calculate the real-time deflection δ at the boom end based on the cantilever beam deformation theory. Solve for the bending deformation compensation amount Δddeflection through the arc length correction formula to compensate for the deviation between the straight distance measurement value and the actual boom arc length caused by the bending of the boom under load.

[0126] S405, Multi-parameter fusion and precise correction: The antenna delay compensation, multipath effect compensation, temperature drift compensation, and boom bending deformation compensation obtained above are simultaneously superimposed on the original ranging value draw. The multi-dimensional error is uniformly corrected through a preset fusion formula, and the high-precision, unbiased boom length dcorrected is output.

[0127] In step S405, d corrected =d raw The formula is: +Δdantenna+Δdmultipath+Δdtemperature+Δddeflection, where Δdantenna is the antenna delay compensation, Δdmultipath is the multipath effect compensation, Δdtemperature is the temperature drift compensation, and Δddeflection is the boom bending deformation compensation. Δdtemperature = a0 + a1•(T-T0) + a2•(T-T0)², where T is the current temperature, T0 is the calibration reference temperature, and a0, a1, and a2 are calibration coefficients. Δddeflection ≈ 8δ² / (3L), where L is the boom length, δ is the estimated boom end deflection, and the deflection δ can be calculated from the boom section moment of inertia I, elastic modulus E, and current bending moment M: δ = ML² / (2EI).

[0128] Taking a 50-ton truck crane construction scenario adjacent to a railway as an example, the specific steps of step S6 are as follows:

[0129] Step S61 System Initialization: Preset vehicle geometry model and safety threshold parameters

[0130] During the equipment power-on and system initialization phases, basic parameters are entered into the database and configured to provide fixed benchmark parameters for subsequent real-time calculations, supporting adaptation to different construction machinery models.

[0131] 1. Vehicle model parameter matching: Based on the current operating equipment model, the system's built-in vehicle geometric parameter library is called to load the fixed parameters of the corresponding vehicle model, including: vehicle chassis length, width and height, three-dimensional offset of the boom hinge point relative to the chassis positioning reference point, maximum / minimum extension length of the boom extension limit, and original geometric offset vectors of the four chassis corner points relative to the positioning reference point.

[0132] Vehicle geometric model parameters

[0133] The chassis positioning reference point is the center of the chassis.

[0134] Chassis dimensions: Length 12.0m, Width 2.8m, Height 3.5m;

[0135] Offset of boom hinge point (relative to reference point): X=-2.5m, Y=0m, Z=+1.8m;

[0136] Four corner point offset vectors:

[0137] Front left: (-6.0, +1.4, +1.75), Front right: (-6.0, -1.4, +1.75)

[0138] Back left: (+6.0, +1.4, +1.75), Back right: (+6.0, -1.4, +1.75)

[0139] Boom extension range: 10m~40m.

[0140] 2. Safety parameter configuration: The adjacent boundary data of the construction site is manually entered or automatically loaded by the system to convert the safety boundaries of railways, foundation pits and fences into a sequence of calibration points in the site coordinate system to form the outline of the adjacent boundary; at the same time, safety warning thresholds are configured, including the first-level warning distance, the second-level prohibition distance and the prediction duration T;

[0141] Level 1 warning distance: 5m; Level 2 prohibition distance: 3m; Predicted duration T=2s;

[0142] Adjacent boundary: Railway safety protection line, site coordinate system calibration point sequence: [(100,50,0),(100,100,0)], vertical boundary line segment

[0143] 3. Algorithm parameter initialization: Initialize the extended Kalman filter (EKF) parameters, set the initial values ​​of the state equation, observation equation, process noise, and observation noise, and clear the historical location time series data cache queue (the default cache is the real-time data within the last 10 frames and 1 second); EKF cache configuration: save the coordinate data of the last 10 frames (1 second time series).

[0144] Step S62: Real-time synchronous acquisition and time alignment of multi-source heterogeneous data

[0145] The system synchronously collects data from multiple core sensors at a fixed frequency of 10-20Hz, unifies timestamps, and eliminates timing misalignment issues caused by data transmission delays from multiple devices.

[0146] 1. Chassis positioning data acquisition: The real-time three-dimensional coordinates Pref(x0,y0,z0) of the vehicle positioning reference point and the vehicle heading angle θ are obtained through the chassis URTK positioning module. heading The data refresh rate is 20Hz.

[0147] 2. Upper arm posture and length data acquisition: Retrieve the corrected and accurate upper arm length d from the output of step S4. corrected Simultaneously, the azimuth angle θ of the boom output from the dual-antenna GNSS directional module is acquired. az Pitch angle θ el The data is synchronized with the RTK positioning data in time.

[0148] URTK chassis positioning reference point coordinates: P ref (92,70,1.2), vehicle heading angle (θ) heading =0°);

[0149] Arm length d after error compensation corrected =32m;

[0150] upper arm azimuth angle θ az} = 90° (towards the positive Y-axis), pitch angle θel = 45°;

[0151] Data synchronization frequency: 10Hz.

[0152] Step S63 Real-time calculation of the three-dimensional spatial coordinates of key points in the site coordinate system

[0153] Based on the calibrated positioning, attitude, and length data, combined with the vehicle's geometric model, and through coordinate rotation transformation and offset compensation, the real-time three-dimensional spatial coordinates of the vehicle's four corner points, the boom hinge point, and the boom tip are accurately calculated.

[0154] 1. Calculation of vehicle chassis corner coordinates: based on the vehicle heading angle θ heading Construct the horizontal plane rotation matrix R(θ) heading The original geometric offset vectors of the four corner points of the chassis are rotated and transformed, and then superimposed with the chassis reference point coordinates Pref to obtain the real-time three-dimensional coordinates P of the four corner points in the site coordinate system. corneri i = 1, 2, 3, 4, corresponding to the front left, front right, back left, and back right corner points;

[0155] 2. Calculation of boom hinge point coordinates: Combining the chassis reference point coordinates with the preset 3D offset of the hinge point, calculate the real-time spatial coordinates P of the boom root hinge point. armjoint, serving as the reference fulcrum for calculating the upper arm's attitude;

[0156] 3. Calculation of the coordinates of the boom tip: Construct a three-dimensional rotation matrix using the boom azimuth and pitch angles.

[0157] R(θ az ,θ el ), combined with the corrected arm length d corrected The hinge point offset is used to calculate the real-time spatial coordinate P of the boom tip at the end of the boom. boomtip Completely restores the real-time spatial posture of the upper arm;

[0158] Chassis corner point (heading 0°, rotation matrix unchanged, offset directly superimposed on reference point)

[0159] Front left corner point: (92-6.0,70+1.4,1.2+1.75)=(86,71.4,2.95)

[0160] Front right corner point: (86, 68.6, 2.95)

[0161] Left corner point: (98, 71.4, 2.95)

[0162] Right corner point: (98, 68.6, 2.95)

[0163] Hinge point coordinates P arm_joint (X=92-2.5=89.5, Y=70, Z=1.2+1.8=3.0), that is (89.5,70,3.0);

[0164] arm tip (P) boom_tip )

[0165] Spatial components of 90° azimuth, 45° elevation, and 32m arm length:

[0166] Horizontal Y-axis: 32 × cos45° ≈ 22.627m; Vertical Z-axis: 32 × sin45° ≈ 22.627m

[0167] Arm tip coordinates:

[0168] X=89.5, Y=70+22.627=92.627, Z=3.0+22.627=25.627

[0169] P boom_tip =(89.5,92.627,25.627);

[0170] Step S64: Real-time motion state estimation based on EKF filtering

[0171] The system caches spatial coordinate data of key points across multiple consecutive frames, smooths the original position data using an extended Kalman filter algorithm, suppresses UWB and GNSS data jump errors, and accurately calculates the real-time motion velocity and acceleration vectors of each key point.

[0172] 1. Time-series data caching: The spatial coordinates of the boom tip and chassis corner points calculated for each frame are stored in a cache queue in chronological order, always retaining the latest 10 frames of valid data, covering 0.5-1s of operation time sequence, forming a continuous position trajectory sequence;

[0173] 2. Motion State Modeling: Using the spatial position, velocity, and acceleration of key points as state variables, a uniformly accelerated motion model is constructed. Combined with sensor-measured position data as observations, an EKF filtering equation is established.

[0174] 3. Filtering, Smoothing, and State Update: Through a two-step EKF prediction and correction process, single-point anomalous jump data is eliminated, and the smoothed real-time motion parameters of each key point are obtained through iterative updates, including the three-dimensional velocity vector V. current 3D acceleration vector A current ;

[0175] Three-dimensional velocity V current =(0,0.6,0)m / s (the arm rotates at a constant speed only in the positive Y-axis direction);

[0176] Three-dimensional acceleration A current =(0,0.05,0)m / s 2 .

[0177] Step S65: Short-time motion trajectory and adjacent distance prediction

[0178] Based on the filtered real-time motion state parameters, and using the uniform acceleration motion formula, the spatial position trajectory of each core key point within the next T seconds is predicted, and the adjacent safety distance at the predicted time is calculated simultaneously.

[0179] Position trajectory prediction: Using the short-time motion prediction formula: P predicted =P current +V current ×T+0.5×A current ×T², calculate the predicted three-dimensional coordinates of the boom tip and the four corner points of the vehicle at preset times of 1-3 seconds in the future, and generate a continuous motion trajectory.

[0180] P predicted =P current +V current ×T+0.5×A current ×T 2 Y pred =92.627 + 0.6 × 2 + 0.5 × 0.05 × 2 2=92.627 + 1.2 + 0.1 = 93.927;

[0181] Predicted arm tip coordinates: P predicted =(89.5,93.927,25.627);

[0182] 2. Real-time neighbor distance calculation: The real-time coordinates of each key point in the current frame and the predicted coordinates in the next T seconds are traversed and calculated with the sequence of adjacent boundary calibration points of the site. The shortest distance algorithm from point to line segment is used to solve the real-time shortest distance and the predicted shortest distance between each key point and the safety boundary.

[0183] Distance calculation

[0184] The railway boundary segment X=100, Y∈[50,100];

[0185] The shortest distance from point (89.5, 93.927, 25.627) to the line segment is: 100 - 89.5 = 10.5m (current real-time distance).

[0186] Predict the shortest distance after 2 seconds: 100-89.5=10.5m (X remains unchanged, only Y increases, and the horizontal distance from the X=100 boundary remains unchanged).

[0187] Adjust vehicle URTK positioning: (P ref (96,70,1.2)), with the remaining parameters unchanged.

[0188] The real-time coordinates of the arm tip are X=96-2.5=93.5, Y=92.627;

[0189] Current distance from railway boundary: 100-93.5=6.5m (greater than 3m is the prohibition line, less than 5m is the first level of warning).

[0190] The predicted value for Y in 2 seconds is 93.927, and the predicted distance is 100 - 93.5 = 6.5 m.

[0191] If the chassis continues to move in the positive X direction, its speed in X is 0.4 m / s, and after 2 seconds, X increases by 0.8 m.

[0192] The predicted value X = 93.5 + 0.8 = 94.3, and the predicted shortest distance = 5.7m, still triggers a Level 1 warning;

[0193] If the speed of X is 0.8 m / s, and X increases by 1.6 m after 2 s, the predicted X = 95.1, and the predicted distance = 4.9 m < 5 m warning threshold.

[0194] Step S66: Predictive security alarm logic judgment and hierarchical output

[0195] By combining real-time distance, predicted distance, and preset safety thresholds, a tiered early warning system is implemented, completing a closed loop of safety protection from real-time monitoring to advance prediction, thus meeting the safety management needs of construction sites adjacent to the site.

[0196] 1. Safety threshold determination: The system compares the predicted shortest distance with the preset warning threshold: when the distance from the key point to the adjacent boundary is less than the first-level warning threshold after T seconds, a pre-judgment warning is triggered; when the predicted distance is less than the second-level prohibition threshold, a high-risk alarm is triggered and the equipment safety restrictions are linked.

[0197] 2. Alarm content output: Accurately outputs predicted alarm information, including risk type, remaining time, intrusion distance, and key risk points (arm tip / vehicle corner), such as "the end of the boom will intrude into the adjacent 3-meter warning zone within 2 seconds".

[0198] 3. Data linkage output: Real-time spatial coordinates, motion trajectory, real-time / predicted safety distance, and alarm status are synchronously output to the torque limiter and construction safety supervision platform via CAN bus and MQTT protocol, supporting equipment torque protection, visual monitoring, collision avoidance control and other services.

[0199] The above high-risk working conditions: the predicted shortest distance is 4.9m, which is less than the first-level warning distance of 5m;

[0200] Judgment logic: If the pre-judgment warning is triggered but the 3m level 2 prohibition threshold is not reached, the device will not be locked.

[0201] Warning text output: The end of the boom will intrude into the adjacent 5-meter warning zone within 2 seconds. The current predicted safe distance is 4.9m.

[0202] Data output: Real-time and predicted coordinates of the boom tip, and a predicted safety distance of 4.9m are synchronized to the torque limiter via CAN bus; at the same time, the data is uploaded to the remote construction monitoring platform via MQTT to display an early warning pop-up.

[0203] Step S67 Abnormal Redundancy Protection

[0204] To ensure stable system operation under complex construction site conditions, a data anomaly tolerance mechanism has been added: when UWB ranging, GNSS orientation, and RTK positioning data experience packet loss, jumps, or failures, the system automatically activates historical valid data cache or backup calibration parameters to maintain the normal operation of spatial calculation and prediction functions, while simultaneously uploading equipment anomaly alarms to prevent safety monitoring failures caused by system downtime.

[0205] A real-time measurement device for the length of a retractable robotic arm based on UWB communication ranging, comprising:

[0206] 1) UWB base station module 101: Installed at the bottom of the boom, it includes a UWB radio frequency chip, antenna, microcontroller, power management unit and serial communication interface, and is fixed to the outer surface of the boom by the UWB base station bracket 103, with the antenna pointing along the axis of the boom.

[0207] 2) UWB tag module 102: Installed at the top of the boom, i.e. the tip of the boom, it includes a UWB radio frequency chip, antenna, microcontroller, lithium battery and ultra-low power management circuit. It is fixed to the outer surface of the boom by the UWB tag bracket 104, and the antenna is pointed along the axis of the boom. The tag module is in intermittent working mode and enters deep sleep during non-ranging periods to reduce power consumption.

[0208] 3) Dual-antenna GNSS directional module 105: It includes two GNSS antenna brackets 105a and 105b and a GNSS receiver host. The two antennas are installed at different positions on the boom through the third brackets 106a and 106b respectively. The receiver host is installed at the bottom of the boom or in the control room. It outputs azimuth and elevation angles through carrier phase differential algorithm.

[0209] 4) Main controller 107: Installed in the control room of the truck crane, it includes a microprocessor, display and communication interface, receives UWB ranging data and GNSS orientation data, runs the error compensation model, outputs the corrected boom length, azimuth angle and pitch angle, and is linked with the torque limiter.

[0210] 5) UWB base station bracket 103 and UWB tag bracket 104: specially designed mechanical brackets that fix the UWB module to the outer surface of the arm. The brackets include U-shaped clamps (to adapt to different cross-section arm bodies), angle adjustment mechanisms (to ensure that the antenna points along the axial direction) and protective shells (waterproof and dustproof rating of IP65 or above).

[0211] 6) GNSS antenna brackets 106a and 106b: Fix the GNSS antenna to the upper surface of the boom. The bracket includes a magnetic base or bolt fixing mechanism, a height adjustment rod and an antenna mounting platform to ensure that the baseline direction of the two antennas is parallel to the axis of the boom.

[0212] The above description is only a preferred embodiment of the present invention. It should be noted that for those skilled in the art, several improvements and modifications can be made without departing from the principle of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention.

Claims

1. A real-time safety early warning method for a retractable robotic arm based on UWB communication ranging, characterized in that, Includes the following steps: S1. Deploy ranging and orientation equipment: Install a UWB base station module at the root of the telescopic mechanical boom and a UWB tag module at the top of the boom. The antennas of the UWB base station module and the UWB tag module are deployed opposite each other along the axis of the boom. Deploy a dual-antenna GNSS orientation module on the boom for collecting the boom attitude parameters. S2, UWB bilateral bidirectional ranging calculation: Employing the DS-TWR bilateral bidirectional ranging mechanism, it collects six timestamps and calculates the signal time-of-flight (ToF) through the interaction of three sets of data packets (Poll, Response, and Final) between the base station and the tag, combining this with the speed of light to obtain the original straight-line ranging value d at both ends of the boom. raw ; S3, GNSS attitude parameter acquisition: The horizontal azimuth and vertical pitch angles of the boom are calculated in real time using the dual-antenna GNSS carrier phase differential RTK algorithm, and the baseline distance between the two antennas is used as an auxiliary calibration reference. S4. Multi-dimensional error compensation correction: Based on a preset compensation model, the original ranging value d is corrected. raw By superimposing antenna delay compensation, multipath effect compensation, temperature drift compensation, and boom bending deformation compensation, the corrected precise boom length d is obtained. corrected ; S5. Data Linkage Output: The corrected boom length, azimuth angle, and pitch angle are uploaded to the main controller and torque limiter of the construction machinery in real time to realize real-time calculation of working torque and safety limit protection. S6. Based on RTK positioning and vehicle model, real-time spatial position calculation and motion prediction are performed. The distance between the spatial coordinates of each key point and the adjacent edge line calibration point sequence is calculated to obtain the real-time shortest distance between each key point and the adjacent edge line. The system predicts the distance between the arm tip and the adjacent edge line after T seconds. When the predicted shortest distance is lower than the set threshold, the system issues a predictive alarm 1-3 seconds in advance.

2. The real-time safety early warning method for a telescopic mechanical boom based on UWB communication ranging according to claim 1, characterized in that: In step S2, the specific process is as follows: 1) The base station sends a Poll data packet to the tag at time t1 and records the sending timestamp; 2) The tag receives the Poll data packet at time t2 and records the receiving timestamp; 3) After a fixed delay of TreplyB, the tag sends a Response data packet to the base station at time t3 and records the sending timestamp; 4) The base station receives the Response data packet at time t4 and records the receiving timestamp; 5) After a fixed delay of TreplyA, the base station sends a Final data packet to the tag at time t5 and records the sending timestamp; 6) The tag receives the Final data packet at time t6 and records the receiving timestamp; The round-trip time of flight (ToF) of the signal is calculated using the six timestamps recorded in the three message exchanges mentioned above. Tround1=t4-t1 Tround2=t6-t3 Treply1=t3-t2 Treply2 = t5 - t4; The Time of Flight (ToF) is then calculated as follows: ToF=(Tround1×Tround2-TreplyA×TreplyB) / (Tround1+Tround2+TreplyA+TreplyB); The length of the upper arm is: d raw =c×ToF, where c is the speed of light.

3. The real-time safety early warning method for a telescopic mechanical boom based on UWB communication ranging according to claim 1, characterized in that, Step S3 includes the following steps: S301. Equipment calibration and attitude alignment: The master and slave antennas of the dual-antenna GNSS directional module are arranged parallel to the axis of the mechanical arm, keeping the antenna baseline horizontal and without tilt, and the baseline plane parallel to the working plane of the arm. Before leaving the factory, the antenna phase center deviation calibration and installation zero-bias calibration are completed, the baseline standard distance is solidified, and installation errors and inherent hardware deviations are eliminated. S302. Satellite signal acquisition and filtering: Dual antennas synchronously receive observation data from multi-system GNSS satellites, including carrier phase, pseudorange, and satellite ephemeris information. Set the satellite cutoff elevation angle, remove invalid satellite signals with low elevation angles and severe multipath interference, and retain high-quality observation data through signal-to-noise ratio filtering. S303, Construction of double-difference observation equations: Based on the synchronous observation data of the master and slave antennas, construct double-difference carrier phase observation equations between satellites and between antennas to eliminate common errors of satellite clock error, receiver clock error, ionospheric and tropospheric delay, and retain effective observation parameters that are only related to the baseline vector; S304. Integer Ambiguity Fixation: The LAMBDA algorithm is used to iteratively solve the double-difference observations to quickly fix the integer ambiguity of the carrier phase. The validity of the ambiguity solution is verified by the baseline length constraint, and a high-precision antenna baseline relative position vector is output. S305, Attitude Angle Calculation Output: The calculated baseline vector is converted to the Northeast Atmosphere coordinate system, and the horizontal azimuth and vertical pitch angles of the boom are calculated in real time through vector trigonometric function relationships to complete the attitude data normalization process. S306, Baseline Calibration Benchmark Update: Extract the baseline distance calculated by dual antennas in real time, compare it with the preset calibration standard baseline distance, use the deviation value as an auxiliary calibration parameter, and synchronously participate in the benchmark correction of subsequent UWB ranging raw data to achieve the same source calibration of attitude parameters and ranging parameters.

4. The real-time safety early warning method for a telescopic mechanical arm based on UWB communication ranging according to claim 1, characterized in that: In step S4, the specific process is as follows: S401. Inherent Hardware Delay Calibration Compensation: Retrieve the antenna delay compensation parameters preset at the factory, combine them with the current UWB module operating frequency band, read the inherent transmission delay deviation between the chip timing node and the antenna radiating end, and calculate the real-time antenna delay compensation amount Δd. antenna ; S402. Dynamic Compensation for Multipath Interference: Combining the boom pitch angle, real-time original rangefinder boom length, and preset geometric parameters of the crane boom collected in real time in step S3, the pre-calibrated boom length-pitch angle-multipath deviation mapping table is retrieved, and the multipath effect compensation amount Δd is obtained by looking up the table through real-time parameter matching. multipath Correcting the ranging offset error caused by the reflection and refraction of UWB signals by the metal shell of the boom; S403, Ambient Temperature Drift Adaptive Compensation: The ambient temperature T is collected in real time by an onboard high-precision temperature sensor. The reference temperature T0 calibrated by the equipment and the quadratic polynomial calibration coefficients a0, a1, and a2 are retrieved and substituted into the temperature compensation model to calculate the temperature drift compensation amount Δd. temperature ; S404. Mechanical Bending Deformation Error Compensation: Real-time crane load data and boom pitch angle are collected. Combined with the boom's preset cross-sectional moment of inertia and material elastic modulus structural parameters, the real-time boom end deflection δ is calculated based on cantilever beam deformation theory. The bending deformation compensation amount Δd is then solved using the arc length correction formula. deflection ; S405, Multi-parameter fusion precise correction: The antenna delay compensation, multipath effect compensation, temperature drift compensation, and boom bending deformation compensation obtained above are synchronously superimposed onto the original ranging value d. raw By using a preset fusion formula, multi-dimensional errors are uniformly corrected, resulting in a high-precision, unbiased output of the boom length d. corrected .

5. The real-time safety early warning method for a telescopic mechanical boom based on UWB communication ranging according to claim 4, characterized in that: In step S405, d corrected =d raw +Δd antenna +Δd multipath +Δd temperature +Δd deflection In the formula, Δd antenna Δd is the antenna delay compensation amount. multipath Δd is the multipath effect compensation amount. temperature Δd is the temperature drift compensation amount. deflection This is the compensation amount for the bending deformation of the boom.

6. The real-time safety early warning method for a telescopic mechanical boom based on UWB communication ranging according to claim 5, characterized in that: Δd temperature =a0+a1•(T-T0)+a2•(T-T0)², where T is the current temperature, T0 is the calibration reference temperature, and a0, a1, and a2 are calibration coefficients.

7. The real-time safety early warning method for a telescopic mechanical boom based on UWB communication ranging according to claim 5, characterized in that: Δddeflection≈8δ² / (3L), where L is the length of the boom and δ is the estimated value of the boom end deflection. The deflection δ can be calculated from the moment of inertia I, elastic modulus E, and current bending moment M of the boom section: δ=ML² / (2EI).

8. The real-time safety early warning method for a telescopic mechanical boom based on UWB communication ranging according to claim 1, characterized in that: The specific steps of step S6 are as follows: Step S61 System Initialization: Preset vehicle geometry model and safety threshold parameters During the equipment power-on and system initialization phases, basic parameters are entered into the database and configured to provide fixed benchmark parameters for subsequent real-time calculations, supporting adaptation to different construction machinery models. Vehicle model parameter matching: Based on the current operating equipment model, the system's built-in vehicle geometric parameter library is called to load the fixed parameters of the corresponding vehicle model, including: vehicle chassis length, width and height, three-dimensional offset of the boom hinge point relative to the chassis positioning reference point, maximum / minimum extension length of the boom extension limit, and original geometric offset vectors of the four chassis corner points relative to the positioning reference point. Safety parameter configuration: Manually input or automatically load the adjacent boundary data of the construction site, convert the safety boundaries of railways, foundation pits, and fences into a sequence of calibration points in the site coordinate system to form the outline of the adjacent boundaries; at the same time, configure safety warning thresholds, including the first-level warning distance, the second-level prohibition distance, and the prediction duration T; Algorithm parameter initialization: Initialize the extended Kalman filter (EKF) parameters, set the initial values ​​of the state equation, observation equation, process noise, and observation noise, and clear the historical location time series data buffer queue; Step S62: Real-time synchronous acquisition and time alignment of multi-source heterogeneous data The system synchronously collects data from multiple core sensors at a fixed frequency of 10-20Hz, using a unified timestamp. Chassis positioning data acquisition: The real-time three-dimensional coordinates Pref(x0,y0,z0) of the vehicle positioning reference point and the vehicle heading angle θ are obtained through the chassis URTK positioning module. heading The data refresh rate is 20Hz. Big arm posture and length data acquisition: Retrieve the corrected and accurate big arm length d from the output of step S4. corrected Simultaneously, the azimuth angle θ of the boom output from the dual-antenna GNSS directional module is acquired. az Pitch angle θ el The data is synchronized with the RTK positioning data in time. Step S63 Real-time calculation of the three-dimensional spatial coordinates of key points in the site coordinate system Based on the calibrated positioning, attitude, and length data, combined with the vehicle's geometric model, and through coordinate rotation transformation and offset compensation, the real-time three-dimensional spatial coordinates of the vehicle's four corner points, the boom hinge point, and the boom tip are accurately calculated. Vehicle chassis corner coordinate calculation: based on the vehicle heading angle θ heading Construct the horizontal plane rotation matrix R(θ) heading The original geometric offset vectors of the four corner points of the chassis are rotated and transformed, and then superimposed with the chassis reference point coordinates Pref to obtain the real-time three-dimensional coordinates P of the four corner points in the site coordinate system. corneri i = 1, 2, 3, 4, corresponding to the front left, front right, back left, and back right corner points; Calculation of boom hinge point coordinates: Combining the chassis reference point coordinates with the preset 3D offset of the hinge point, calculate the real-time spatial coordinates P of the boom root hinge point. armjoint , serving as the reference fulcrum for calculating the upper arm's attitude; Calculation of the coordinates of the boom tip: Constructing a three-dimensional rotation matrix using the boom azimuth and pitch angles. R(θ az ,θ el ), combined with the corrected arm length d corrected The hinge point offset is used to calculate the real-time spatial coordinate P of the boom tip at the end of the boom. boomtip Completely restores the real-time spatial posture of the upper arm; Step S64: Real-time motion state estimation based on EKF filtering The system caches spatial coordinate data of key points across multiple consecutive frames, smooths the original position data using an extended Kalman filter algorithm, suppresses UWB and GNSS data jump errors, and accurately calculates the real-time motion velocity and acceleration vectors of each key point. Time-series data caching: The spatial coordinates of the boom tip and chassis corner points calculated for each frame are stored in a cache queue in chronological order, always retaining the latest 10 frames of valid data, covering 0.5-1s of operation time sequence, forming a continuous position trajectory sequence; Motion state modeling: Using the spatial position, velocity, and acceleration of key points as state variables, a uniformly accelerated motion model is constructed. Combined with sensor-measured position data as observations, an EKF filtering equation is established. Filtering, smoothing, and state update: Through a two-step EKF prediction and correction process, single-point anomalous jump data is eliminated, and the smoothed real-time motion parameters of each key point are iteratively updated, including the three-dimensional velocity vector V. current 3D acceleration vector A current ; Step S65: Short-time motion trajectory and adjacent distance prediction Based on the filtered real-time motion state parameters, and using the uniform acceleration motion formula, the spatial position trajectory of each core key point within the next T seconds is predicted, and the adjacent safety distance at the predicted time is calculated simultaneously. Position trajectory prediction: Using the short-time motion prediction formula: P predicted =P current +V current ×T+0.5×A current ×T², calculate the predicted three-dimensional coordinates of the boom tip and the four corner points of the vehicle at preset times of 1-3 seconds in the future, and generate a continuous motion trajectory. Real-time adjacent distance calculation: The real-time coordinates of each key point in the current frame and the predicted coordinates in the next T seconds are traversed and calculated with the sequence of adjacent boundary calibration points of the site. The shortest distance algorithm from point to line segment is used to solve the real-time shortest distance and the predicted shortest distance between each key point and the safety boundary. Step S66: Predictive security alarm logic judgment and hierarchical output By combining real-time distance, predicted distance, and preset safety thresholds, a tiered early warning system is implemented, completing a closed loop of safety protection from real-time monitoring to advance prediction, thus meeting the safety management needs of adjacent construction sites. Safety threshold determination: The system compares the predicted shortest distance with the preset warning threshold: when the distance from the key point to the adjacent boundary is less than the first-level warning threshold after T seconds, a pre-judgment warning is triggered; when the predicted distance is less than the second-level prohibition threshold, a high-risk alarm is triggered and the equipment safety restrictions are linked. Alarm content output: Accurately outputs predicted alarm information, including risk type, remaining time, intrusion distance, and key risk points such as arm tip / vehicle corner.

9. A real-time measurement device for the length of a telescopic mechanical boom based on UWB communication ranging, used to implement the measurement method according to any one of claims 1-8, characterized in that, include: The UWB base station module is installed at the base of the boom and is used to initiate ranging timing, send and receive UWB data packets, and perform preliminary ranging data calculations. UWB tag module, installed at the top of the boom, is equipped with lithium battery and ultra-low power sleep circuit for passive response ranging interaction; The dual-antenna GNSS orientation module includes two sets of GNSS antennas and an RTK receiver, used to output the azimuth, elevation and baseline length of the boom in real time; Dedicated mounting bracket assemblies, including UWB module brackets and GNSS antenna brackets; The main controller unit is electrically connected to the UWB base station module and the dual-antenna GNSS directional module. It has a built-in error compensation algorithm to complete data correction, logical operations, external communication and torque limiter linkage.

Images