Visual guide system for water operation of excavator and control method
By using multi-source sensor adaptive fusion and dynamic working face generation technology, the problems of unstable positioning and insufficient depth measurement in excavator water operations have been solved, achieving high-precision underwater construction guidance and improving the overall performance of the system.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- CHINA RAILWAY GUANGZHOU ENG GRP CO LTD
- Filing Date
- 2025-12-16
- Publication Date
- 2026-04-28
AI Technical Summary
Existing excavator water operation systems suffer from unstable positioning accuracy, insufficient underwater depth measurement accuracy, and poor design adaptability in aquatic environments, failing to meet the requirements for high-precision underwater construction.
Employing multi-source heterogeneous sensor adaptive fusion, pressure-depth joint state estimation, and dynamic working face real-time reconstruction technology, high-precision positioning and visual guidance are achieved through deep coupling of the above-water environment perception fusion module, the bucket underwater pose calculation module, the dynamic working face generation module, and the visual guidance and control module.
It improves the positioning stability and depth measurement accuracy in water-based operating environments, enhances the system's construction adaptability and flexibility, and achieves a nonlinear gain effect of 1+1>2.
Smart Images
Figure CN121934444A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of intelligent control technology for construction machinery, specifically to a visual guidance system and control method for excavator water operations. Background Technology
[0002] With the rapid development of infrastructure construction such as water conservancy projects, port dredging, and river regulation, the application scenarios of excavators in water operations are increasing. The water operation environment has unique characteristics such as obstructed visibility, invisible underwater terrain, and interference from water surface fluctuations, which places higher demands on the accuracy and safety of excavator operations.
[0003] In existing technologies, the Beidou UES series excavator intelligent system employs high-precision real-time dynamic positioning technology. By reading various sensors installed on the excavator and calculating the calibrated main pivot dimensions, it obtains real-time and accurate three-dimensional position information of the bucket. The system supports both separate and integrated intelligent receiver installation schemes, achieving an accuracy of 3cm RMS. It can indicate the relative position of the actual bucket to the design surface in various ways, including graphical and numerical representations, and provides 3D visualization guidance from multiple perspectives, including side, front, and top views.
[0004] However, existing technologies have the following shortcomings: First, in the water-based operating environment, satellite signals are affected by factors such as water surface reflection and multipath effects, resulting in a significant decrease in GNSS positioning accuracy. Existing systems lack adaptive signal quality assessment and compensation mechanisms for the water environment. Second, existing systems employ fixed-parameter sensor data fusion strategies, which cannot adaptively adjust the confidence weights of each sensor according to the dynamic environmental characteristics of water operations, leading to unstable positioning accuracy under complex conditions such as water surface fluctuations and signal obstruction. Third, during underwater operations, bucket depth information relies solely on the geometric calculations of the robotic arm, failing to fully utilize water pressure information for cross-validation and error compensation, making it difficult to meet the requirements of high-precision underwater construction. Fourth, the design surface of existing systems is a static preset mode, which cannot dynamically adjust the target surface based on real-time collected underwater terrain data, limiting the adaptability to complex underwater environments. Summary of the Invention
[0005] To address the shortcomings of existing technologies, this invention provides a visual guidance system and control method for excavator operations in water. Through innovative technologies such as adaptive fusion of multi-source heterogeneous sensors, pressure-depth joint state estimation, and real-time reconstruction of dynamic working surfaces, it achieves high-precision positioning and visual guidance in water operation environments, solving technical problems such as unstable positioning accuracy, insufficient underwater depth measurement accuracy, and poor adaptability of design surfaces in existing systems in complex water environments.
[0006] The present invention adopts the following technical solution: The excavator water operation visualization guidance system includes a water environment perception fusion module, a bucket underwater pose calculation module, a dynamic work surface generation module, and a visualization guidance control module; The above-water environment perception fusion module is used to collect GNSS positioning data, inertial measurement data and water pressure data, dynamically calculate the adaptive confidence weight of each sensor according to the characteristics of the above-water operation environment, and output the weighted fusion of multi-source sensor data to the bucket underwater pose calculation module. The underwater pose calculation module of the bucket is used to receive the fusion data output by the above-water environment perception fusion module, combine the angle information of each joint of the excavator, and calculate the three-dimensional position and attitude information of the bucket underwater through the pressure-depth joint state estimation algorithm. The pose calculation result is then output to the dynamic working face generation module and the visualization guidance control module. The dynamic working surface generation module is used to receive the pose information and historical working trajectory data output by the bucket underwater pose calculation module, generate an adaptive working target surface in real time through a dynamic surface fitting algorithm, and output the working target surface parameters to the visualization guidance and control module. The visualization guidance and control module is used to receive the real-time bucket pose output by the bucket underwater pose calculation module and the target surface output by the dynamic working surface generation module, calculate the deviation between the bucket and the target surface, generate a three-dimensional visualization guidance interface and control commands, and simultaneously feed the deviation information back to the bucket underwater pose calculation module for closed-loop correction.
[0007] Furthermore, the aquatic environment perception fusion module includes a GNSS receiving unit, an inertial measurement unit, a water pressure sensing unit, and an adaptive fusion unit. The GNSS receiving unit is used to receive multi-constellation satellite positioning signals and output position and velocity information. The inertial measurement unit is used to measure the three-axis acceleration and three-axis angular velocity of the excavator body. The water pressure sensing unit is installed on the bucket and is used to measure the water pressure value at the bucket's location. The adaptive fusion unit dynamically calculates the confidence weights of each sensor and performs weighted fusion based on GNSS signal quality indicators, inertial measurement drift characteristics, and water pressure measurement noise characteristics.
[0008] Furthermore, the adaptive fusion unit adopts an adaptive weight calculation strategy based on signal-to-noise ratio and geometric accuracy factor. The adaptive weight calculation strategy includes: calculating GNSS confidence based on the carrier-to-noise ratio and geometric accuracy factor output by the GNSS receiving unit; calculating inertial measurement confidence based on the cumulative drift of the inertial measurement unit; calculating water pressure measurement confidence based on the measurement stability of the water pressure sensing unit; and normalizing the three types of confidence as fusion weights.
[0009] Furthermore, the underwater pose calculation module for the bucket includes a kinematics calculation unit and a joint state estimation unit. The kinematics calculation unit calculates the position of the bucket in the machine coordinate system using forward kinematic equations based on the joint angles of the excavator boom, arm, and bucket, as well as the geometric dimensions of each boom. The joint state estimation unit uses an extended Kalman filter to take the kinematically calculated position, GNSS fused position, and water pressure-estimated depth as observations, and achieves optimal estimation of the bucket's three-dimensional pose through state prediction and measurement updates.
[0010] Furthermore, the joint state estimation unit weighted and fused the kinematic solution value of the bucket depth with the water pressure estimation value. The water pressure estimation depth was calculated based on the water pressure value measured by the water pressure sensing unit, the atmospheric pressure reference value, and the water density. The joint state estimation unit also includes a depth consistency detection mechanism. When the deviation between the kinematic solution depth and the water pressure estimation depth exceeds a preset threshold, the weight of the one with lower confidence is reduced.
[0011] Furthermore, the dynamic working surface generation module includes a trajectory storage unit and a surface fitting unit; the trajectory storage unit is used to store the three-dimensional coordinate data of the historical working trajectory points of the bucket; the surface fitting unit uses the moving least squares method to dynamically fit the historical trajectory points to generate a smooth and continuous working target surface, and updates the target surface parameters in real time according to the newly collected trajectory points.
[0012] Furthermore, the visualization guidance control module includes a deviation calculation unit, a graphics rendering unit, and a feedback correction unit; the deviation calculation unit is used to calculate the vertical and horizontal deviations between the current position of the bucket and the target surface; the graphics rendering unit is used to generate a visualization guidance interface that includes a 3D model of the excavator, the real-time position of the bucket, the target surface, and deviation indicators; the feedback correction unit is used to feed back the deviation information to the underwater pose calculation module of the bucket, and trigger pose correction when the deviations in multiple consecutive frames show a systematic offset trend.
[0013] Furthermore, the graphics rendering unit provides side view, front view, top view, and three-dimensional stereo view. Figure 4 The system provides a visual display from multiple perspectives and uses color coding to indicate the relative position of the bucket to the target surface. Green indicates that the bucket is within the allowable deviation range of the target surface, yellow indicates that the bucket is close to the boundary of the target surface, and red indicates that the bucket deviates from the target surface beyond the allowable range.
[0014] Furthermore, the system also includes an audible and visual warning module, which is connected to the visual guidance and control module and is used to issue sound and light warning signals when the bucket deviates from the working target surface or approaches a dangerous area.
[0015] A visual guidance and control method for excavator operations on water includes the following steps: Perform multi-source sensor data acquisition and adaptive fusion, collect GNSS positioning data, inertial measurement data and water pressure data, dynamically calculate the adaptive confidence weight of each sensor according to the characteristics of the water operation environment, and perform weighted fusion; Perform underwater pose joint state estimation of the bucket. Combine the fused sensor data, excavator joint angle and water pressure to estimate the depth, and use an extended Kalman filter to perform pressure-depth joint state estimation to obtain the three-dimensional position and attitude of the bucket. Dynamic operation target surface generation is performed. Based on the historical operation trajectory data of the bucket, the moving least squares method is used to perform dynamic surface fitting to generate an adaptive operation target surface. The system performs visual guidance and closed-loop control, calculates the deviation between the bucket and the target surface, generates a three-dimensional visual guidance interface, and feeds back the deviation information for closed-loop correction of pose calculation.
[0016] The beneficial effects of this invention are as follows: First, by using a multi-source sensor adaptive fusion algorithm based on signal-to-noise ratio and geometric accuracy factor, the confidence weights of each sensor are adjusted in real time according to the dynamic changes of the water operation environment. This effectively overcomes the impact of interference factors such as water surface reflection and multipath effect on positioning accuracy. Compared with the existing fixed weight fusion scheme, the positioning stability is improved by more than 40%.
[0017] Second, by using the pressure-depth joint state estimation algorithm, the water pressure sensing information and kinematic calculation results are cross-validated and fused for estimation, which realizes high-precision measurement of the underwater bucket depth. The depth measurement accuracy is improved from 3cm in the existing technology to 1.5cm, meeting the requirements of high-precision underwater construction.
[0018] Third, by using a dynamic working face generation algorithm based on the moving least squares method, the target face parameters are adaptively updated according to the real-time collected working trajectory data, which overcomes the shortcomings of existing static design faces that cannot adapt to complex underwater terrain and improves the flexibility and adaptability of construction.
[0019] Fourth, through a closed-loop feedback mechanism of visualization guidance and pose calculation, real-time correction of deviation information is achieved, forming a complete closed-loop system of perception-calculation-generation-control-feedback. The modules are deeply coupled and coordinated, and the overall system performance achieves a nonlinear gain effect of 1+1>2. Attached Figure Description
[0020] Figure 1 This is an overall architecture diagram of the excavator water operation visualization guidance system of the present invention; Figure 2 This is a schematic diagram of the structure of the aquatic environment perception and fusion module of the present invention; Figure 3 This is a schematic diagram of the underwater pose calculation module for the bucket of the present invention. Figure 4 This is a schematic diagram of the dynamic work surface generation module of the present invention; Figure 5 This is a schematic diagram of the structure of the visual guidance control module of the present invention; Figure 6 This is a flowchart of the visual guidance and control method for excavator water operations according to the present invention. Detailed Implementation
[0021] Please refer to the attached document. Figures 1-6 The present invention will now be described in further detail with reference to the accompanying drawings and embodiments.
[0022] like Figure 1 As shown, the excavator underwater operation visualization guidance system provided by this invention includes an underwater environment perception and fusion module 1, a bucket underwater pose calculation module 2, a dynamic work surface generation module 3, and a visualization guidance and control module 4. These four modules form a deeply coupled closed-loop collaborative system, with the specific data flow relationships as follows: The aquatic environment perception fusion module 1 serves as the system's data entry point, responsible for collecting GNSS positioning data, inertial measurement data, and water pressure data. It calculates the adaptive confidence weights of each sensor based on the dynamic characteristics of the aquatic operating environment and performs weighted fusion of multi-source heterogeneous sensor data. The fused data is then output to the bucket underwater pose calculation module 2.
[0023] The underwater bucket pose calculation module 2 receives fused data output from the surface environment perception fusion module 1, and combines it with angle sensor information from various joints of the excavator. Through a joint state estimation algorithm using forward kinematics calculation and extended Kalman filtering, it calculates the three-dimensional position and attitude information of the bucket in the geodetic coordinate system. The pose calculation results are simultaneously output to the dynamic work surface generation module 3 and the visualization guidance and control module 4.
[0024] The dynamic working surface generation module 3 receives the real-time bucket pose information output by the bucket underwater pose calculation module 2, stores the historical working trajectory points and performs dynamic surface fitting to generate adaptive working target surface parameters, and outputs them to the visualization guidance and control module 4.
[0025] The visualization guidance and control module 4 receives the real-time bucket pose output from the bucket underwater pose calculation module 2 and the target surface parameters output from the dynamic working surface generation module 3. It calculates the positional deviation between the bucket and the target surface and generates an intuitive 3D visualization guidance interface for the operator's reference. Simultaneously, the visualization guidance and control module 4 feeds back the deviation information to the bucket underwater pose calculation module 2, forming a closed-loop correction mechanism. When a systematic offset trend is detected, pose recalibration is triggered to ensure the accuracy and stability of the system during long-term operation.
[0026] The coupling relationship between the above four modules is reflected in the following aspects: Parameter-level coupling: The fused position output by the surface environment perception fusion module 1 is directly used as the observation input parameter of the bucket underwater pose calculation module 2, and the pose result output by the bucket underwater pose calculation module 2 is directly used as the fitting data source of the dynamic working surface generation module 3 and the display data source of the visualization guidance control module 4.
[0027] State-level coupling: The extended Kalman filter state vector in the underwater pose calculation module 2 of the bucket is related to the sensor confidence state of the surface environment perception fusion module 1. Changes in sensor confidence will affect the setting of the filter's measurement noise covariance matrix.
[0028] Logical coupling: The deviation detection logic of the visualization guidance control module 4 and the correction trigger logic of the bucket underwater pose calculation module 2 are interconnected to form a closed-loop control logic.
[0029] Synergistic Effect: Through the aforementioned multi-level coupling relationships, the system achieves synergistic effects in functions such as adaptive fusion of sensing data, optimization of pose estimation, dynamic adaptation of the target surface, and real-time correction of control deviations. Technical improvements in a single module can be passed on to other modules through coupling relationships, resulting in a non-linear improvement in overall performance. For example, the adaptive weight adjustment of the aquatic environment sensing fusion module 1 not only improves the fusion positioning accuracy but also enhances the state estimation accuracy of the bucket underwater pose calculation module 2 through parameter transfer. This, in turn, improves the surface fitting quality of the dynamic working surface generation module 3 and the deviation calculation accuracy of the visual guidance control module 4, ultimately achieving a 1+1>2 gain effect in the overall system performance.
[0030] like Figure 2 As shown, the water environment perception fusion module 1 includes a GNSS receiving unit 11, an inertial measurement unit 12, a water pressure sensing unit 13, and an adaptive fusion unit 14.
[0031] GNSS receiver unit 11 employs a multi-constellation, multi-frequency RTK receiver, supporting joint positioning with four major satellite navigation systems: GPS, GLONASS, BeiDou, and Galileo. In maritime operating environments, GNSS signals are susceptible to multipath effects caused by water surface reflection, leading to unstable positioning accuracy for single constellations. Multi-constellation joint positioning increases the number of visible satellites and improves geometric configuration strength, thereby enhancing positioning accuracy and reliability. The data output by GNSS receiver unit 11 includes: three-dimensional position coordinates (latitude, longitude, elevation), three-dimensional velocity, carrier-to-noise ratio (C / N0), geometrical factor of precision (GDOP), differential age, and positioning status indicator.
[0032] The inertial measurement unit 12 employs a MEMS inertial sensor, including a three-axis accelerometer and a three-axis gyroscope, outputting the three-axis specific force and three-axis angular velocity of the excavator body. The inertial measurement unit 12 is mounted on the excavator's slewing platform, and the measurement frequency is 100Hz. MEMS inertial sensors inherently possess zero-bias drift and scaling factor errors; long-term integration can lead to cumulative and divergent positioning errors. Therefore, the inertial measurement data needs to be fused with GNSS data, utilizing the absolute positioning capability of GNSS to correct the inertial integration error.
[0033] The water pressure sensing unit 13 is installed at the root of the bucket teeth, employing a pressure-resistant and waterproof encapsulation design. Its measuring range is 0-50m water depth, with a measurement accuracy of ±0.1%FS. The water pressure sensing unit 13 measures the absolute water pressure at the bucket's location. In still water, water pressure and depth have a linear relationship; the water pressure value can be converted to a water depth value using the hydrostatic pressure formula. In dynamic water environments, water flow velocity affects pressure measurement, requiring correction using a velocity compensation model.
[0034] The adaptive fusion unit 14 is the core processing unit of the aquatic environment perception fusion module 1. It is responsible for calculating the adaptive confidence weight based on the real-time quality indicators of each sensor and performing weighted fusion of multi-source data.
[0035] The adaptive fusion unit 14 employs an adaptive weight calculation algorithm based on signal-to-noise ratio and geometric accuracy factor. The core idea of this algorithm is that the reliability of sensor data is closely related to its measurement quality; the higher the measurement quality, the higher the fusion weight is assigned, and vice versa, the weight is reduced to minimize the negative impact of low-quality data on the fusion result.
[0036] For GNSS receiver unit 11, its confidence calculation considers two indicators: carrier-to-noise ratio (CNR) and geometrical accuracy factor (GEF). The CNR reflects the satellite signal strength, while the GEF reflects the amplification effect of the satellite's spatial configuration on positioning accuracy. The GNSS confidence calculation employs the following innovative algorithm: , in, This represents the GNSS confidence level, with a value ranging from 0 to 1. This is the average carrier-to-noise ratio of all visible satellites at the current epoch, expressed in dB-Hz. The carrier-to-noise ratio threshold is set to 35dB-Hz in this embodiment. When the average carrier-to-noise ratio is lower than this threshold, it indicates that the signal quality is poor. The carrier-to-noise ratio sensitivity coefficient controls the response speed of confidence as the carrier-to-noise ratio changes, and its value is 0.3. The geometric precision factor (GDOP) for the current epoch is dimensionless. The geometric precision factor threshold is set to 4.0. When GDOP is higher than this threshold, it indicates that the satellite geometry is poor. The GDOP sensitivity coefficient has a value of 0.5. is the base of the natural logarithm.
[0037] The algorithm adopts a double Sigmoid function form, which has the following technical advantages: First, the output range of the Sigmoid function is naturally normalized to the (0,1) interval, without the need for additional normalization processing; Second, the Sigmoid function has a smooth transition characteristic near the threshold, avoiding abrupt jumps in confidence level; Third, the sensitivity of confidence level to changes in indicators can be flexibly adjusted through the sensitivity coefficient.
[0038] For inertial measurement unit 12, its confidence level calculation mainly considers the cumulative drift. The positioning error of inertial navigation accumulates over time; the longer the time since the last GNSS correction, the greater the cumulative drift and the lower the confidence level. The formula for calculating the confidence level of inertial measurement is: , in, This represents the confidence level for inertial measurements, with a value ranging from 0 to 1. The drift attenuation coefficient reflects the drift characteristics of the inertial sensor and is determined based on the calibration of the actual MEMS sensor used. In this embodiment, the value is 0.02. The time interval since the last effective GNSS correction, in seconds; is the base of the natural logarithm.
[0039] This algorithm employs an exponential decay mechanism, consistent with the physical characteristic that inertial navigation errors increase over time. Once the GNSS signal is recovered... Reset to zero, and the inertial confidence level is restored to its maximum value of 1.
[0040] For the water pressure sensing unit 13, the confidence level calculation mainly considers measurement stability. During dynamic operation, water pressure measurement is affected by factors such as bucket movement and water flow disturbance, leading to decreased measurement stability. The water pressure measurement confidence level is calculated using the sliding window variance evaluation method: , in, Confidence level for water pressure measurement; To measure the variance of the water pressure measurements within the sliding window, the sliding window length is set to 20 sampling points, and the sampling period is 50ms. This is the variance sensitivity coefficient, with a value of 100.
[0041] When the water pressure measurement is stable, the variance is small and the confidence level is close to 1; when the water pressure measurement fluctuates drastically, the variance increases and the confidence level decreases.
[0042] After calculating the confidence scores of the three types of sensors, the adaptive fusion unit 14 normalizes the confidence scores into fusion weights: , in, , , These are the normalized fusion weights for GNSS, inertial measurement, and hydraulic pressure measurement, respectively, and the sum of the three is equal to 1.
[0043] Finally, weighted fusion is performed to obtain the fusion position: , in, This is the fused 3D position vector; Calculate the position for GNSS; The position is the inertial integral. This is a combined vector of depth information calculated by water pressure and horizontal position estimation.
[0044] like Figure 3 As shown, the underwater pose calculation module 2 for the bucket includes a kinematics calculation unit 21 and a joint state estimation unit 22.
[0045] The kinematics calculation unit 21 is responsible for calculating the position and attitude of the bucket in the machine coordinate system using forward kinematic equations based on the joint angles and geometric parameters of the excavator's robotic arm. The excavator's robotic arm consists of three links: the boom, the stick, and the bucket, forming a three-degree-of-freedom serial mechanism.
[0046] Let the angle of rotation of the boom relative to the slewing platform be... The angle of rotation of the stick relative to the boom is The angle of rotation of the bucket relative to the stick is The boom length is The length of the boom is The bucket length (the distance from the boom hinge to the bucket teeth) is The joint angles are measured by tilt sensors installed at each hinge point.
[0047] The kinematic model was established using the DH parameter method, and the position of the bucket teeth in the fuselage coordinate system was obtained through the following forward kinematics solution: , , , in, , , These represent the X, Y, and Z coordinates of the bucket teeth in the machine coordinate system, with the X-axis pointing forward, the Y-axis pointing to the left side of the machine, and the Z-axis pointing vertically upward. , , These are the lengths of the boom, stick, and bucket, respectively, in meters. , , These are the joint angles of the boom, stick, and bucket, respectively, in radians.
[0048] Transforming the excavator's position from the machine's coordinate system to the geodetic coordinate system requires considering the excavator's position and attitude. Let the excavator's position in the geodetic coordinate system be... The fuselage heading angle is The rotation angle of the slewing platform relative to the fuselage is Then the position of the bucket teeth in the geodetic coordinate system is: , , , in, , , The coordinates of the bucket teeth in the geodetic coordinate system; , , The coordinates of the excavator body in the geodetic coordinate system are provided by the fused position output by the water environment perception fusion module 1; The fuselage heading angle is provided by GNSS dual-antenna direction finding or inertial navigation. The angle of the slewing platform is measured by a slewing angle sensor.
[0049] The joint state estimation unit 22 uses an extended Kalman filter (EKF) for optimal estimation of the bucket pose. The state vector of the extended Kalman filter is defined as: , in, , , The three-dimensional position coordinates of the bucket in the geodetic coordinate system are given in meters. , , The three-dimensional velocity components of the bucket are expressed in meters per second. This is the depth estimation bias state, used to characterize the systematic deviation between the water pressure-calculated depth and the kinematically calculated depth, with the unit being meters.
[0050] The system state transition equation adopts a uniform motion model: , in, For the first The state vector at any given time; For the first The state vector at any given time; This is the state transition matrix; Let be the process noise vector, which follows a zero-mean Gaussian distribution, and its covariance matrix is... .
[0051] State transition matrix for: , in, The filter period is in seconds.
[0052] The measurement vector comprises three observation sources: GNSS fused position, kinematically calculated position, and pressure-estimated depth. The measurement equation is: , in, For the first The measurement vector at time; For measurement matrix; The measurement noise vector follows a zero-mean Gaussian distribution, and its covariance matrix is... .
[0053] The measurement vector is specifically defined as follows: , in, , , The three-dimensional coordinates of the GNSS fusion location; , , The bucket position is calculated using kinematics. The depth value is calculated based on water pressure.
[0054] Depth calculated by water pressure The following values are calculated from the measurements taken by the water pressure sensing unit 13: , in, Depth is calculated based on water pressure, in meters; The absolute pressure value measured by the water pressure sensing unit 13, in Pascals; This is a reference value for atmospheric pressure, in Pascals, which can be calibrated and obtained before entering the water. The density of water is expressed in kilograms per cubic meter; 1000 is used for freshwater and 1025 for seawater. The acceleration due to gravity is taken as 9.8 m / s².
[0055] Measurement noise covariance matrix in joint state estimation unit 22 An adaptive adjustment strategy is adopted, with its diagonal elements correlated with the sensor confidence scores calculated by the aquatic environment perception fusion module 1. When the confidence score of a certain sensor decreases, the corresponding measurement noise variance increases, reducing the weight of that observation source in the filter update. This adaptive mechanism enables the joint state estimation unit 22 to dynamically adjust the contribution ratio of each observation source according to real-time environmental changes, improving estimation accuracy and robustness.
[0056] The joint state estimation unit 22 also includes a depth consistency detection mechanism. Within each filtering cycle, the depth calculated from the kinematic solutions is compared. Depth calculated with water pressure Differences: , in, This represents the depth deviation, expressed in meters.
[0057] when When the depth exceeds a preset threshold (0.1 meters in this embodiment), an anomaly is determined in both depth observation sources, and a confidence assessment procedure is initiated. The confidence assessment is based on the consistency evaluation of historical data: the sum of squared residuals between the two depth sources and the filtered state estimate is calculated over the most recent N filtering cycles (N = 20). The source with the smaller sum of squared residuals has higher historical consistency and retains its original weight, while the source with the larger sum of squared residuals has its weight reduced to 50% of its original weight.
[0058] like Figure 4 As shown, the dynamic work surface generation module 3 includes a trajectory storage unit 31 and a surface fitting unit 32.
[0059] The trajectory storage unit 31 adopts a ring-shaped buffer structure to store the three-dimensional coordinate data of the bucket's historical operation trajectory points. The buffer capacity is set to 1000 points; when the number of stored points exceeds the capacity, the oldest data point is automatically overwritten. Each trajectory point contains the following information: its three-dimensional position in the geodetic coordinate system. Collection timestamp Operation status indicators (excavation / rotation / travel).
[0060] The trajectory storage unit 31 also includes an anomaly removal mechanism. When the rate of change of the position of a newly acquired trajectory point from the previous point exceeds the physically reasonable range (e.g., single-cycle displacement exceeds 0.5 meters), it is determined to be an anomaly and is not included in the storage.
[0061] The surface fitting unit 32 uses Moving Least Squares (MLS) to perform dynamic surface fitting, generating a smooth and continuous target surface. The advantage of Moving Least Squares over traditional Least Squares is that it can generate surface values at any position rather than being limited to node positions, and the fitting results are smooth and continuous with good filtering effect on data noise.
[0062] Let the target surface to be fitted be at any point. The elevation value at that location is Using quadratic polynomial basis functions: , in, , , , , , denoted as the coefficients of the polynomial to be determined.
[0063] The core of moving least squares is the introduction of a distance-based weighting function, which assigns greater weight to data points closer to the fitted point during the fitting process. The weighting function uses a Gaussian kernel function. , in, For the first Each trajectory point is relative to the fitted point The weights; For the first trajectory points to the fitting point Horizontal distance; To influence the radius parameter and control the spatial range of weight decay, this embodiment sets it to 2 meters.
[0064] The objective function for weighted least squares fitting is: , in, The objective function value; Let be the vector of coefficients to be determined; This represents the total number of trajectory points involved in the fitting process. For the first Elevation values of each trajectory point.
[0065] To find the extremum of the objective function, let The normal equation is obtained as follows: , in, Let be the basis function matrix, the first... Behavior ; This is a diagonal weight matrix, with diagonal elements as follows: ; Elevation vector .
[0066] Solving the normal equation yields the coefficient vector. Then calculate the fitting point. The elevation value of the target surface at the work location.
[0067] The surface fitting unit 32 employs an incremental update strategy. When a new trajectory point is added, it is not necessary to refit the entire surface; instead, only the affected region (the radius of influence of the new point) is updated. The surface parameters are within a certain range. This incremental update strategy significantly reduces the computational load and meets the requirements for real-time updates.
[0068] The parameters output by the dynamic work surface generation module 3 include: the polynomial coefficient matrix of the work target surface, the effective boundary of the target surface, and the target surface update timestamp. These parameters are output to the visualization guidance and control module 4 for deviation calculation and graphics rendering.
[0069] like Figure 5 As shown, the visualization guidance control module 4 includes a deviation calculation unit 41, a graphics rendering unit 42, and a feedback correction unit 43.
[0070] Deviation calculation unit 41 is responsible for calculating the positional deviation between the current position of the bucket and the target working surface. Let the current position of the bucket be... The target surface of the operation is The elevation value at that location is Then the vertical deviation is: , in, The value represents the vertical deviation. A positive value indicates that the bucket is above the target surface, while a negative value indicates that the bucket is below the target surface. The unit is meters.
[0071] The deviation calculation unit 41 also calculates the horizontal deviation of the bucket relative to the preset working trajectory. The preset working trajectory is assumed to be a space curve. ,parameter The horizontal deviation is the shortest distance from the current position of the bucket to the curve, where the arc length is the curve length. .
[0072] The deviation calculation unit 41 divides the deviation value into three levels: allowable range (absolute deviation value less than 3cm), warning range (absolute deviation value between 3-10cm), and out-of-limit range (absolute deviation value greater than 10cm). Different levels correspond to different color codes and audible and visual warning levels.
[0073] The graphics rendering unit 42 is responsible for generating the 3D visualization guide interface. The interface layout includes a main view area and an auxiliary information area. The main view area provides four viewing options: side view (viewed from the side of the excavator, mainly displaying bucket depth information), front view (viewed from the front of the excavator, mainly displaying bucket horizontal offset), top view (viewed from above, mainly displaying planar operation trajectory), and 3D stereo view (a freely rotatable and zoomable stereo display).
[0074] The graphics rendering unit 42 uses color coding to intuitively indicate the relative position of the bucket and the target surface. The colors are defined as follows: green indicates that the bucket is within the allowable deviation range of the target surface (absolute deviation less than 3cm), and the operator can continue the current operation; yellow indicates that the bucket is close to the boundary of the target surface (absolute deviation between 3-10cm), reminding the operator to pay attention to adjustment; red indicates that the bucket deviates from the target surface beyond the allowable range (absolute deviation greater than 10cm), and the operator needs to correct it immediately.
[0075] The graphics elements rendered by the graphics rendering unit 42 include: the three-dimensional model of the excavator (including the accurate geometry of the body, boom, stick, and bucket), the real-time position markers of the bucket teeth, the work target surface grid (displayed using a semi-transparent material), the deviation indicator line (connecting the current position of the bucket to the corresponding point of the target surface), and the numerical display panel (displaying precise deviation values, current depth, positioning status, and other information).
[0076] The refresh rate of the graphics rendering unit 42 is set to 20Hz to ensure smooth display and enable the operator to adjust the operation in a timely manner based on the real-time display information.
[0077] Feedback correction unit 43 is responsible for feeding back deviation information to bucket underwater pose calculation module 2, forming a closed-loop correction mechanism. The trigger condition for feedback correction is: the deviation value of M consecutive frames (M is 10) shows a monotonically increasing or decreasing trend, and the cumulative deviation change exceeds the threshold (set to 5cm). When the trigger condition is met, it is determined that there is a systematic offset, and the pose recorrection procedure is started.
[0078] The execution process of the pose recalibration procedure is as follows: the feedback calibration unit 43 sends a recalibration command to the bucket underwater pose calculation module 2, and the joint state estimation unit 22 of the bucket underwater pose calculation module 2 expands the current state estimation covariance matrix to twice the original size, accelerates the filter's response to new observation data, and eliminates accumulated systematic biases.
[0079] The system also includes an audible and visual warning module 5, which is connected to the visual guidance and control module 4. The audible and visual warning module 5 includes a buzzer and LED warning lights. When the deviation calculation unit 41 detects that the bucket deviates from the target working surface or approaches the electronic fence boundary, the audible and visual warning module 5 emits audible and visual warning signals of different frequencies and colors according to the warning level. A level one warning (yellow warning) uses intermittent buzzing and flashing yellow lights, while a level two warning (red warning) uses continuous buzzing and a constantly lit red light.
[0080] like Figure 6 As shown, the present invention also provides a visual guidance and control method for excavator water operations, comprising the following steps: Step S1: Perform multi-source sensor data acquisition and adaptive fusion. Specifically, this includes: acquiring position, velocity, carrier-to-noise ratio (CNR), and geometrical accuracy factor (GEF) data output from the GNSS receiver unit; acquiring triaxial acceleration and triaxial angular velocity data output from the inertial measurement unit (IMU); and acquiring water pressure data output from the water pressure sensor unit. Calculate the GNSS confidence level based on the CNR and GEF, the IMU confidence level based on the cumulative drift time, and the water pressure measurement confidence level based on the water pressure measurement variance. Normalize the three confidence levels into fusion weights, and perform weighted fusion to obtain the fused position.
[0081] Step S2: Perform underwater attitude joint state estimation of the bucket. This includes: reading joint angle sensor data from the boom, stick, and bucket; calculating the bucket's position in the fuselage coordinate system based on the forward kinematics equations; transforming the bucket's position to the geodetic coordinate system based on the fuselage position and attitude; converting water pressure measurements to water pressure-estimated depth; using GNSS fused position, kinematically calculated position, and water pressure-estimated depth as observations, employing an extended Kalman filter for state prediction and measurement updates to obtain the optimal estimate of the bucket's three-dimensional position and velocity; and performing depth consistency detection, adjusting the weights of the corresponding observation sources when the deviation between the kinematically calculated depth and the water pressure-estimated depth exceeds a threshold.
[0082] Step S3: Generate the dynamic target surface for the operation. This includes: storing the current position of the bucket in the trajectory buffer; performing outlier detection and removing outliers with abrupt position changes; when the number of trajectory points meets the fitting conditions, using the moving least squares method to dynamically fit the trajectory points to a surface; and outputting the polynomial coefficients and effective range parameters of the target surface for the operation.
[0083] Step S4: Execute visual guidance and closed-loop control. This includes: calculating the vertical and horizontal deviations between the current position of the bucket and the target surface; determining the warning level and color code based on the deviation values; rendering a 3D visualization interface containing the excavator model, bucket position, target surface, and deviation indication; detecting deviation change trends, and sending a recalibration command to the extended Kalman filter in step S2 when the deviation shows a systematic shift over multiple consecutive frames; and triggering audible and visual warnings based on the warning level.
[0084] The above steps S1 to S4 are executed cyclically with a period of 50ms, which means the system operating frequency is 20Hz.
[0085] To verify the technical effectiveness of this invention, an actual test was conducted at a river improvement project site. The test conditions were as follows: the excavator was a Caterpillar 320D, the operating water depth range was 2-5 meters, the water flow velocity was approximately 0.5 meters per second, and the test duration was 8 hours.
[0086] Positioning accuracy test: A high-precision RTK reference station was used to provide a true reference, and the deviation between the bucket position output by the system of this invention and the true value was compared. The test results show that the horizontal positioning accuracy is 2.1 cm (RMS) and the vertical positioning accuracy is 1.8 cm (RMS), which is a significant improvement compared to the 3 cm accuracy of the existing UES series system.
[0087] Positioning stability test: The change in positioning accuracy during periods of GNSS signal quality degradation (carrier-to-noise ratio below 35dB-Hz) was statistically analyzed. The positioning accuracy of the system of this invention decreased by only 15% during periods of signal quality degradation, while the positioning accuracy of the comparison scheme using fixed-weight fusion decreased by more than 50%, verifying the effectiveness of the adaptive fusion algorithm.
[0088] Depth measurement accuracy test: Bucket depth measurements were compared at calibration points with known depths. The depth measurement accuracy of the system of this invention is 1.5 cm (RMS), which is 50% higher than the scheme that only uses kinematic calculations, verifying the effectiveness of the pressure-depth joint estimation algorithm.
[0089] Work efficiency test: The time required to complete the same work task was statistically analyzed. The work efficiency of the system of this invention is 40% higher than that of the traditional manual layout method and 15% higher than that of the existing excavator guidance system, which verifies the combined effect of dynamic work surface generation and visual guidance.
Claims
1. A visual guidance system for excavator operations on water, characterized in that, It includes a water environment perception and fusion module, a bucket underwater pose calculation module, a dynamic work surface generation module, and a visual guidance and control module; The above-water environment perception fusion module is used to collect GNSS positioning data, inertial measurement data and water pressure data, dynamically calculate the adaptive confidence weight of each sensor according to the characteristics of the above-water operation environment, and output the weighted fusion of multi-source sensor data to the bucket underwater pose calculation module. The underwater pose calculation module of the bucket is used to receive the fusion data output by the above-water environment perception fusion module, combine the angle information of each joint of the excavator, and calculate the three-dimensional position and attitude information of the bucket underwater through the pressure-depth joint state estimation algorithm. The pose calculation result is then output to the dynamic working face generation module and the visualization guidance control module. The dynamic working surface generation module is used to receive the pose information and historical working trajectory data output by the bucket underwater pose calculation module, generate an adaptive working target surface in real time through a dynamic surface fitting algorithm, and output the working target surface parameters to the visualization guidance and control module. The visualization guidance and control module is used to receive the real-time bucket pose output by the bucket underwater pose calculation module and the target surface output by the dynamic working surface generation module, calculate the deviation between the bucket and the target surface, generate a three-dimensional visualization guidance interface and control commands, and simultaneously feed the deviation information back to the bucket underwater pose calculation module for closed-loop correction.
2. The visual guidance system for excavator water operations according to claim 1, characterized in that, The aquatic environment perception and fusion module includes a GNSS receiving unit, an inertial measurement unit, a water pressure sensing unit, and an adaptive fusion unit; the GNSS receiving unit is used to receive multi-constellation satellite positioning signals and output position and velocity information; The inertial measurement unit is used to measure the three-axis acceleration and three-axis angular velocity of the excavator body; the water pressure sensing unit is installed on the bucket and is used to measure the water pressure value at the bucket location; the adaptive fusion unit dynamically calculates the confidence weight of each sensor and performs weighted fusion based on GNSS signal quality indicators, inertial measurement drift characteristics and water pressure measurement noise characteristics.
3. The visual guidance system for excavator water operations according to claim 2, characterized in that, The adaptive fusion unit adopts an adaptive weight calculation strategy based on signal-to-noise ratio and geometric accuracy factor. The adaptive weight calculation strategy includes: calculating GNSS confidence based on the carrier-to-noise ratio and geometric accuracy factor output by the GNSS receiver unit; calculating inertial measurement confidence based on the cumulative drift of the inertial measurement unit; calculating water pressure measurement confidence based on the measurement stability of the water pressure sensor unit; and normalizing the three types of confidence as the fusion weight.
4. The visual guidance system for excavator water operations according to claim 1, characterized in that, The underwater pose calculation module for the bucket includes a kinematics calculation unit and a joint state estimation unit. The kinematics calculation unit calculates the position of the bucket in the machine coordinate system using forward kinematic equations based on the joint angles of the excavator's boom, arm, and bucket, as well as the geometric dimensions of each boom. The joint state estimation unit uses an extended Kalman filter to take the kinematically calculated position, GNSS fused position, and water pressure-estimated depth as observations, and achieves optimal estimation of the bucket's three-dimensional pose through state prediction and measurement updates.
5. The visual guidance system for excavator water operations according to claim 4, characterized in that, The joint state estimation unit weighted and fused the kinematic solution value of the bucket depth with the water pressure estimation value. The water pressure estimation depth was calculated based on the water pressure value measured by the water pressure sensing unit, the atmospheric pressure reference value, and the water density. The joint state estimation unit also includes a depth consistency detection mechanism. When the deviation between the kinematic solution depth and the water pressure estimation depth exceeds a preset threshold, the weight of the one with lower confidence is reduced.
6. The visual guidance system for excavator water operations according to claim 1, characterized in that, The dynamic working surface generation module includes a trajectory storage unit and a surface fitting unit; the trajectory storage unit is used to store the three-dimensional coordinate data of the historical working trajectory points of the bucket; the surface fitting unit uses the moving least squares method to dynamically fit the historical trajectory points to generate a smooth and continuous working target surface, and updates the target surface parameters in real time according to the newly collected trajectory points.
7. The visual guidance system for excavator water operations according to claim 1, characterized in that, The visualization guidance and control module includes a deviation calculation unit, a graphics rendering unit, and a feedback correction unit. The deviation calculation unit is used to calculate the vertical and horizontal deviations between the current position of the bucket and the target surface. The graphics rendering unit is used to generate a visualization guidance interface that includes a 3D model of the excavator, the real-time position of the bucket, the target surface, and deviation indicators. The feedback correction unit is used to feed back the deviation information to the underwater pose calculation module of the bucket, and trigger pose correction when the deviations in multiple consecutive frames show a systematic offset trend.
8. The visual guidance system for excavator water operations according to claim 7, characterized in that, The graphics rendering unit provides visualization display from four perspectives: side view, front view, top view, and three-dimensional stereo view. It also uses color coding to indicate the relative position of the bucket to the target surface. Green indicates that the bucket is within the allowable deviation range of the target surface, yellow indicates that the bucket is close to the boundary of the target surface, and red indicates that the bucket deviates from the target surface beyond the allowable range.
9. A visual guidance system for excavator waterborne operations according to any one of claims 1 to 8, characterized in that, The system also includes an audible and visual warning module, which is connected to the visual guidance and control module and is used to issue sound and light warning signals when the bucket deviates from the working target surface or approaches a dangerous area.
10. A visual guidance and control method for excavator water operations, employing the visual guidance system for excavator water operations as described in claim 1, characterized in that... Includes the following steps: Perform multi-source sensor data acquisition and adaptive fusion, collect GNSS positioning data, inertial measurement data and water pressure data, dynamically calculate the adaptive confidence weight of each sensor according to the characteristics of the water operation environment, and perform weighted fusion; Perform underwater pose joint state estimation of the bucket. Combine the fused sensor data, excavator joint angle and water pressure to estimate the depth, and use an extended Kalman filter to perform pressure-depth joint state estimation to obtain the three-dimensional position and attitude of the bucket. Dynamic operation target surface generation is performed. Based on the historical operation trajectory data of the bucket, the moving least squares method is used to perform dynamic surface fitting to generate an adaptive operation target surface. The system performs visual guidance and closed-loop control, calculates the deviation between the bucket and the target surface, generates a three-dimensional visual guidance interface, and feeds back the deviation information for closed-loop correction of pose calculation.