Excavator bucket tip positioning method based on triangular chain solution

Abstract

The application relates to a method for positioning a shovel tip of an excavator based on triangular chain calculation, and particularly relates to the technical field of spatial positioning. The method comprises the following steps: acquiring fixed geometric side length parameters of each connecting rod and a reference pitch angle at a calibration moment; synchronously collecting stroke lengths of each oil cylinder, a base attitude, a heading angle and geographical coordinates in real time; decomposing the multi-connecting rod mechanism into multiple triangles sharing edges, performing chain geometric calculation by using the cosine theorem to obtain an end distance and a geometric structure angle and generating end two-dimensional coordinates; performing pitch increment correction on the geometric structure angle by using a difference between the base pitch angle and the reference pitch angle, performing three-dimensional space reconstruction, and superimposing the base geographical coordinates to obtain end three-dimensional geographical coordinates. The application decomposes the multi-connecting rod mechanism into a triangular topological network and performs chain geometric calculation by using the cosine theorem, and the stroke lengths of the oil cylinders are used as direct input edges to participate in the calculation, so that the jump and dynamic response lag of the inclination sensor under strong vibration working conditions are avoided.

Description

Technical Field

[0001] This application relates to the field of spatial positioning, and in particular to a method for positioning the tip of an excavator shovel based on a triangular chain solution. Background Technology

[0002] Existing excavator shovel tip positioning solutions are mainly based on a forward kinematics model that installs tilt sensors at each joint. This solution measures the tilt angle of each joint relative to the horizontal direction in real time by installing tilt sensors at the boom, stick, and bucket joints, and then substituting each joint angle and the link length into the forward kinematics formula to calculate the end position.

[0003] Tilt sensors are essentially accelerometers based on the principle of gravitational acceleration, and their output value is calculated from the projection of gravitational acceleration onto the sensor's sensitive axis. When an excavator performs high-frequency, high-vibration operations such as hammering, rapid shoveling, and sudden stops and turns, the total acceleration experienced by the sensor includes both gravitational acceleration and acceleration generated by mechanical motion. These two components are superimposed in the time domain, and the sensor itself cannot distinguish between them, leading to drastic jumps in the tilt angle output. To suppress these jumps, existing solutions must introduce low-pass filtering algorithms with large time constants, typically resulting in filtering delays of hundreds of milliseconds or more.

[0004] Therefore, the existing solution has a technical problem of dynamic response lag under strong vibration operation conditions. The positioning results displayed in the cab are significantly lagging behind the actual movements of the robotic arm, making it difficult to meet the real-time guidance requirements of dynamic operation scenarios. Summary of the Invention

[0005] To overcome the technical problem of lag in dynamic response under strong vibration conditions in existing tilt sensor-based positive kinematic positioning schemes, this application provides an excavator shovel tip positioning method based on triangular chain calculation.

[0006] This application provides a method for positioning the excavator shovel tip based on a triangular chain solution, which adopts the following technical solution: A method for positioning the excavator blade tip based on triangular chain-like solution, wherein the excavator's multi-link mechanism includes multiple links connected sequentially through hinge points, includes the following steps: S1. Obtain the fixed geometric side length parameters of each link in the multi-link mechanism and the reference pitch angle, wherein the reference pitch angle is the pitch angle of the base at the calibration time; S2. Real-time synchronous acquisition of the stroke length of the boom cylinder, the stroke length of the stick cylinder, and the stroke length of the bucket cylinder, as well as the pitch angle, roll angle, yaw angle, and geographical coordinates of the base; S3. Based on the fixed geometric side length parameters, the stroke length of the boom cylinder, the stroke length of the stick cylinder, and the stroke length of the bucket cylinder, the multi-link mechanism is decomposed into multiple triangles with shared sides. The cosine theorem is used to perform chain-like geometric calculations on each triangle with shared sides to determine the end distance and geometric structure angle. The end distance is the straight-line distance of the end relative to the root of the boom. Based on the end distance and the geometric structure angle, the two-dimensional coordinates of the end in the longitudinal plane of the machine body are generated. S4. Based on the difference between the pitch angle of the base and the reference pitch angle, the pitch increment correction is performed on the geometric structure angle. The end-point two-dimensional coordinates are regenerated based on the corrected geometric structure angle and the end-point distance. The regenerated end-point two-dimensional coordinates are reconstructed in three-dimensional space by combining the roll angle of the base and the heading angle. The end-point three-dimensional geographic coordinates are obtained by superimposing the geographic coordinates of the base.

[0007] Optionally, S1 includes the following sub-steps: S11. With the excavator stationary, use external measuring equipment to measure the three-dimensional spatial coordinates of each hinge point in the multi-link mechanism; S12. Based on the three-dimensional spatial coordinates of each hinge point, calculate the fixed geometric side length parameters of each link in reverse using the distance formula between two points in space; S13. Record the pitch angle of the base at the measurement moment as the reference pitch angle, and store the fixed geometric side length parameter and the reference pitch angle in the system configuration file.

[0008] Optionally, step S3 includes the following sub-steps: S31. Map the stroke length of the boom cylinder, the stroke length of the stick cylinder, and the stroke length of the bucket cylinder to the corresponding geometric dynamic side lengths; S32. Using the dynamic geometric side lengths and the fixed geometric side length parameters as known quantities, the cosine theorem is applied sequentially to each triangle sharing a side to calculate the cosine value of the target angle in each triangle. The inverse cosine function is taken for each cosine value to determine the target angle in each triangle. At the hinge points shared by multiple triangles, the angle loop closure constraint where the sum of the angles at each hinge point equals the full angle is used to determine the remaining unknown angles. The determined angles or side lengths in the current triangle are used as known quantities input for the adjacent triangles. S33. After chain-like geometric calculation of all triangles, the end distance and the geometric structure angle are synthesized, and the end two-dimensional coordinates are generated based on the end distance and the geometric structure angle.

[0009] Optionally, in S32, after the cosine value is calculated using the cosine theorem at each step and before the inverse cosine function is taken, the cosine value is clamped to the closed interval [-1, 1]. In S33, the geometric structure angle is obtained by adding or subtracting the target angle, the remaining unknown angle, and the preset fixed reference angle from multiple hinge points in the chain geometry solution.

[0010] Optionally, step S4, which involves incrementally correcting the pitch angle of the geometric structure and regenerating the end-point two-dimensional coordinates, includes the following sub-steps: S41. Calculate the difference between the pitch angle of the base and the reference pitch angle as the pitch increment, and subtract the pitch increment from the geometric structure angle to obtain the corrected geometric structure angle; S42. Based on the end-point distance and the corrected geometric structure angle, the product of the end-point distance and the cosine of the corrected geometric structure angle is used as the first coordinate component in the longitudinal plane of the body, and the product of the end-point distance and the sine of the corrected geometric structure angle is used as the second coordinate component in the longitudinal plane of the body, and the two-dimensional coordinates of the end-point are regenerated.

[0011] Optionally, step S4 further includes the following sub-steps: S43. The roll angle of the base is used to perform roll compensation on the regenerated end-body two-dimensional coordinates, expanding the end-body two-dimensional coordinates in the longitudinal plane of the body into three-dimensional body coordinates; S44. The three-dimensional body coordinates are offset and compensated according to the preset installation offset vector to obtain the relative coordinates of the end relative to the geographic coordinate reference point. The geographic coordinate reference point is the measurement reference point corresponding to the geographic coordinates of the base. The installation offset vector is the three-dimensional displacement of the geographic coordinate reference point relative to the boom root in the body coordinate system. S45. Using the heading angle, rotate the relative coordinates from the body coordinate system to the northeast-sky coordinate system to obtain the northeast-sky coordinate increment; S46. The incremental northeast celestial coordinates are superimposed onto the geographic coordinates of the base to obtain the absolute coordinates of the end in the projected coordinate system. The absolute coordinates are then converted into latitude, longitude and elevation in the geodetic coordinate system using projection back calculation to obtain the three-dimensional geographic coordinates of the end.

[0012] Optionally, in S2, The stroke length of each cylinder is measured in real time using displacement sensors installed on the boom cylinder, stick cylinder, and bucket cylinder respectively. The pitch angle and roll angle of the base are measured in real time using attitude sensors mounted on the base. The heading angle and the geographic coordinates of the base are measured using a dual-antenna positioning and orientation device mounted on the base.

[0013] Optionally, after the chain geometry solution in S3 is completed, the following fault diagnosis and data reconstruction operations are performed on the results of the chain geometry solution: The theoretical stroke lengths of the boom cylinder, stick cylinder, and bucket cylinder are calculated in reverse using the results of the chain geometric solution. The differences between each theoretical stroke length and the stroke lengths of the boom cylinder, stick cylinder, and bucket cylinder are used to form a consistent residual vector. In response to the fact that the magnitude of the consistency residual vector exceeds a preset consistency threshold, the cylinder corresponding to the component with the largest absolute value is determined as the faulty cylinder based on the distribution of the absolute values ​​of each component in the consistency residual vector. Based on the stroke lengths of the boom cylinder, the stick cylinder, and the bucket cylinder (excluding the stroke length of the faulty cylinder), and combined with the geometric constraints of the fixed geometric side length parameters, the theoretical stroke length of the faulty cylinder in the current posture is solved in reverse. The theoretical stroke length is then substituted into the chain-like geometric solution as a replacement value.

[0014] Optionally, the chain geometry solution of S3 uses an angle loop closure constraint at the hinge point shared by multiple triangles. The angle loop closure constraint is a constraint that the sum of the angles at each hinge point is equal to the full angle. During the execution of the chain geometry solution in S3, the hinge points shared by multiple triangles are defined as redundant hinge points, where the circumferential angles of each hinge point are calculated by the cosine theorem of the triangle or by the fixed structural angle determined by the cosine theorem from the fixed geometric side length parameters, and the unknown angles do not need to be solved by the angle loop closure constraint. Calculate the difference between the sum of the circumferential angles at each redundant hinge point and the circumferential angle, and use it as the closure residual of each redundant hinge point; The error injection section is determined based on the distribution pattern of the closed residuals of each redundant hinge point along the chain geometric solution sequence, and the cylinder corresponding to the error injection section is cross-verified with the faulty cylinder.

[0015] Optionally, during the construction of the consistent residual vector in multiple consecutive frames, a sliding statistical window is maintained for the modulus of the consistent residual vector in multiple consecutive frames, and the mean and standard deviation of the modulus within the sliding statistical window are calculated. In response to the mean of the modulus continuously increasing at a preset rate and the standard deviation of the modulus remaining stable, it is determined to be a slow drift of the fixed geometric side length parameter, and an online micro-correction is performed on the fixed geometric side length parameter. The magnitude of the online micro-correction is limited by a preset upper limit for a single change. If the magnitude of the consistency residual vector of a single frame exceeds a preset mutation threshold, it is determined to be a sensor mutation fault, triggering the determination of the faulty cylinder and the inverse solution of the theoretical stroke length.

[0016] Optionally, using the result of the chain-like geometric solution of S3 as the initial value, an overdetermined set of equations is constructed, which includes the cosine theorem equations of all triangles and the closure constraints of the angle loops at all hinge points. The stroke lengths of the boom cylinder, the stick cylinder, the bucket cylinder, and the fixed geometric side length parameters are used as constraints. The target angles and the remaining unknown angles in the chain-like geometric solution are used as optimization variables. Weighted least squares iterative solution is performed to update the values ​​of the target angles and the remaining unknown angles. The weight matrix is ​​set according to the side length ratio of each triangle, and the closer the side length ratio is to that of an isosceles triangle, the higher the weight.

[0017] Optionally, when the excavator is in a stationary standby state, the zero-bias drift of the attitude sensor is estimated online by using the difference between the output of the attitude sensor during the stationary period and the theoretical value of the gravity vector, and the zero-bias drift is accumulated on the reference pitch angle for dynamic compensation.

[0018] Optionally, during operation, in response to the detection that the combination of the stroke length of the boom cylinder, the stroke length of the stick cylinder, and the stroke length of the bucket cylinder covers multiple preset sets of differentiated attitude conditions, the residual between the collected values ​​of the stroke length of each cylinder under each differentiated attitude condition and the theoretical prediction value of the current geometric model is used to perform online micro-correction on the fixed geometric side length parameter using the least squares method. The magnitude of the online micro-correction is limited by a preset single change upper limit.

[0019] Optionally, in response to detecting that the collected value of the roll angle of the base is invalid or exceeds a preset reasonable range, the roll angle of the base is set to zero in the three-dimensional spatial reconstruction of S4, and only the positioning result based on pitch increment correction is output. The display terminal is marked as the current degraded positioning mode and the corresponding accuracy degraded estimate.

[0020] Optionally, in response to the solution state of the dual-antenna positioning and orientation device being downgraded from a fixed solution to a floating-point solution or losing lock, the geographic coordinates and velocity of the base corresponding to the last valid fixed solution are used as initial values, and the position update of the end-point three-dimensional geographic coordinates is maintained by inertial calculation using the acceleration and angular velocity output by the attitude sensor; in response to the duration of the inertial calculation exceeding a preset time threshold, the current end-point three-dimensional geographic coordinates are frozen and an accuracy degradation warning is output on the display terminal.

[0021] Optionally, before each step of the cosine theorem calculation in S32, it is checked whether the lengths of the three sides of the triangle involved in the current step satisfy the condition for the existence of a triangle. In response to the fact that the sum of any two sides is not greater than the third side, it is determined that the data collected in the current frame is abnormal, the calculation of this frame is skipped and the calculation result of the previous valid frame is used.

[0022] Optionally, a sliding window midpoint filter is applied to the end two-dimensional coordinates generated by S3, and the median value among the solution results of the most recent multiple frames is taken as the output of the current frame; in response to detecting that the rate of change of the stroke length of the boom cylinder, the stroke length of the stick cylinder, or the stroke length of the bucket cylinder exceeds a preset speed threshold, the window length of the sliding window midpoint filter is reduced to maintain dynamic tracking performance.

[0023] Optionally, based on the terminal three-dimensional geographic coordinates, the nearest design section is automatically matched in the pre-stored design section database; in response to the terminal three-dimensional geographic coordinates being located between two adjacent design sections, linear interpolation is performed on the design contours of the two adjacent design sections to generate an interpolated section, and the terminal three-dimensional geographic coordinates are projected onto the interpolated section to calculate the vertical deviation from the design surface.

[0024] Optionally, based on the relationship between the vertical deviation of the end three-dimensional geographic coordinates and the design surface and a preset threshold, the operation guidance information is displayed on the display terminal in graded colors: in response to the vertical deviation being greater than zero, the first color is displayed to indicate that the design surface has not been reached; in response to the vertical deviation being between zero and the over-excavation threshold, the second color is displayed to indicate that the design surface has been reached; in response to the vertical deviation exceeding the over-excavation threshold, the third color is displayed to indicate over-excavation.

[0025] Optionally, temperature data of the environment where the displacement sensor is located is obtained, and temperature compensation correction is performed on the stroke length of the boom cylinder, the stick cylinder, and the bucket cylinder collected in S2 according to the thermal expansion coefficient of the rope material in the displacement sensor.

[0026] In summary, this application includes at least one of the following beneficial technical effects: 1. This application decomposes the multi-link mechanism into multiple triangles with shared sides and uses the cosine theorem to perform chain-like geometric calculations. The stroke length of the hydraulic cylinder is used as the dynamic input side of the triangle to directly participate in the calculation, avoiding dependence on tilt sensors. The stroke length of the hydraulic cylinder is measured by a displacement sensor in a linear displacement manner, which is not affected by angular acceleration. Under strong vibration operation conditions, it can be output in real time without the need to introduce a large time constant filter, thus eliminating the dynamic response lag problem of existing solutions.

[0027] 2. This application records the pitch angle of the base at the calibration time as the reference pitch angle. In real-time calculation, the difference between the pitch angle of the base and the reference pitch angle is used as the pitch increment to participate in the geometric structure angle correction, so that the calibration result is completely decoupled from the site tilt. The excavator can complete effective calibration in any tilted site. At the same time, this mechanism deducts the pitch attitude change of the base in real time, so that the end position does not produce false displacement with the shaking of the base in dynamic base scenarios such as ship-mounted.

[0028] 3. This application uses the shared side triangle topology to generate implicit geometric coupling constraints on the stroke length of the three cylinders. It uses the consistent residual vector to identify the faulty cylinder and uses the collected values ​​of the other two normal cylinders to solve the theoretical stroke length of the faulty cylinder and substitutes it into the chain geometric solution. This achieves self-diagnosis and data reconstruction of single cylinder faults without adding redundant sensors. Attached Figure Description

[0029] Figure 1 This is a diagram showing the hinge points of a multi-link mechanism for an excavator according to an embodiment of the present invention.

[0030] Figure 2 This is a flowchart of the excavator shovel tip positioning method based on triangular chain solution in one embodiment of the present invention. Detailed Implementation

[0031] The present application will be further described in detail below with reference to the accompanying drawings. It should be understood that the specific embodiments described herein are merely illustrative of the application and are not intended to limit the scope of the application.

[0032] This invention discloses a method for excavator tip positioning based on triangular chain-like calculation. This method obtains the fixed geometric side length parameters and reference pitch angle of each link in the excavator's multi-link mechanism, synchronously collects the stroke length of each cylinder, base attitude, heading angle, and geographic coordinates in real time, decomposes the multi-link mechanism into multiple triangles with shared sides, and uses the law of cosines to perform chain-like geometric calculations on each triangle with shared sides to obtain the end-point distance and geometric structure angle. Finally, it obtains the end-point three-dimensional geographic coordinates through pitch increment correction and three-dimensional spatial reconstruction.

[0033] The multi-link mechanism of an excavator consists of three interconnected links: the boom, stick, and bucket. These links are driven by boom cylinders, stick cylinders, and bucket cylinders, respectively. Each link is connected via hinge points to form an extendable robotic arm structure. The end effector refers to the working point of the excavator's robotic arm, typically the tip of a breaker hammer or the tip of a bucket tooth. The base refers to the slewing platform located at the bottom of the robotic arm, supporting the entire arm, and the vehicle or hull support structure beneath it. The base can rotate around a vertical axis to change the direction of the robotic arm, and its pitch and roll angles change in real time depending on the working site or the condition of the supporting hull. Hinge points are the rotational connection points between the links in the multi-link mechanism, and between each cylinder and its connected link. These include the boom root hinge point, the hinge points at both ends of each cylinder, the boom-stick connection hinge point, the stick-bucket connection hinge point, and the connection points of the bucket's four-bar linkage.

[0034] This embodiment uses a typical scenario of a shipborne excavator performing underwater crushing and slag removal operations in a waterway as a concrete background. In this scenario, the excavator is mounted on the deck of a construction vessel, and the hull is temporarily fixed to the waterway construction area using piling devices such as legs. The hull experiences continuous pitching and rolling motions with the waves, and the robotic arm performs crushing operations on the rock strata underwater before replacing the bucket to remove slag. Since the tip of the breaker hammer or the tip of the bucket teeth is always located in an invisible underwater area, the operator needs to rely on the real-time three-dimensional geographic coordinates displayed on the cab screen to determine the deviation of the current working position from the design cross-section. The three-dimensional geographic coordinates output by this method for this scenario include longitude, latitude, and elevation in the geodetic coordinate system, which can be directly used for comparison with the design cross-section and construction guidance.

[0035] To achieve the aforementioned end-point 3D geolocation, it is first necessary to establish a geometric model and attitude reference that fits the physical entity of each excavator through external measurements, providing reliable fixed parameter inputs for subsequent real-time calculations. The overall process of this method is as follows: Figure 2 As shown, steps S1-S4 are executed sequentially.

[0036] S1. Obtain the fixed geometric side length parameters of each link in the multi-link mechanism and the reference pitch angle, wherein the reference pitch angle is the pitch angle of the base at the calibration time.

[0037] Fixed geometric side length parameters refer to the lengths of each link in a multi-link mechanism and the relative distances between each hinge point. Typical parameters include the main boom length, stick length, bucket length, triangular link side length, and cylinder hinge point spacing. These side lengths reflect the unique physical dimensions of each excavator due to welding, machining, and assembly tolerances, and are used as known quantities in the chain geometry solution of S3 for the calculation of the cosine theorem of all triangles.

[0038] The fixation of geometric side length parameters and reference pitch angle is to ensure that subsequent real-time calculations have a geometric reference that fits the physical entity and to eliminate the influence of the calibration site tilt.

[0039] After S1 is completed, the system obtains a geometric model that fits the physical entity and an attitude reference that is decoupled from the calibration site. These two sets of parameters remain stable in subsequent operation cycles and are repeatedly used as fixed inputs for real-time calculations of S2-S4.

[0040] Specifically, S1 includes sub-steps S11-S13.

[0041] S11. With the excavator stationary, use external measuring equipment to measure the three-dimensional spatial coordinates of each hinge point in the multi-link mechanism.

[0042] External measuring equipment refers to high-precision three-dimensional measuring instruments that are independent of the excavator itself. A typical implementation is a laser total station, which can measure the three-dimensional spatial coordinates of a target point relative to the measuring instrument's own coordinate system with an accuracy within centimeters.

[0043] Reference Figure 1 As shown, the multi-link mechanism uses various line types to distinguish the different geometric elements participating in the chain-like geometric solution, clearly illustrating the overall geometric topology upon which the triangle chain-like solution method of this application relies. For ease of correspondence, Figure 1 All hinge points and end-of-operation points involved in the calculation are marked with circles and labeled with letters. Specifically, these include the boom root hinge point A, the boom cylinder cylinder end hinge point B, the boom cylinder piston rod end hinge point C, the stick cylinder cylinder end hinge point D, the boom and stick connection hinge point E, the stick cylinder piston rod end hinge point F, the bucket cylinder cylinder end hinge point P, the rocker arm and stick connection hinge point Q, the common hinge point R of the bucket cylinder piston rod end, rocker arm, and connecting rod, the stick and bucket connection hinge point M, the connecting rod and bucket connection hinge point N, and the end-of-operation point W.

[0044] The component assignments of each hinge point and end working point in the multi-link mechanism are as follows: The base has a boom root hinge point A and a boom cylinder end hinge point B, with the distance AB between them remaining constant on the base. The boom is a primary link with hinge point A as its root and hinge point E as its apex. Hinge points A, C, D, and E are all located on the boom, therefore the distances AC, AD, DC, CE, and DE remain constant on the boom, where AC is the main boom length. The stick is a secondary link with hinge point E as its root, extending through hinge point Q to hinge point M. Hinge points E, F, P, Q, and M are all located on the stick, therefore the distances EF, PE, EQ, PQ, PM, and QM remain constant on the stick. The bucket is a tertiary link with hinge point M as its root, extending to the end working point W. Hinge points M and N, and the end working point W, are all located on the bucket, therefore the distances MN, MW, and NW remain constant on the bucket. Hinge point E belongs to both the boom and the stick, serving as their rotational connection point; hinge point M belongs to both the stick and the bucket, serving as their rotational connection point. The piston rod end hinge point R of the bucket cylinder is connected to hinge point Q on the stick via rocker arm QR, and to hinge point N on the bucket via connecting rod RN. The lengths of the rocker arm QR and the connecting rod RN are both fixed geometric side length parameters. Hinge points Q, R, N, and M constitute a four-bar linkage mechanism that drives the bucket's rotation.

[0045] Figure 1 The first line type shows the structural links, including the sides AC, AD, DC, CE, DE on the boom, the sides EF, PE, EQ, PQ, PM, QM on the stick, the sides MN, MW, NW on the bucket, the rocker arm QR and the connecting rod RN in the four-bar linkage, and the side AB on the base. The structural links shown in the first line type correspond to the fixed geometric side length parameters in this method. Figure 1 The second line type shows each cylinder, namely the boom cylinder connecting hinge point B and hinge point C, the stick cylinder connecting hinge point D and hinge point F, and the bucket cylinder connecting hinge point P and hinge point R. The three sides BC, DF, and PR shown in the second line type correspond to the geometric dynamic side lengths in this method. Figure 1 The third line shows the shared edges involved in the chain-like geometric solution, which are RM, QW, EW, CW, and AW in the chain derivation order. This shared edge chain starts from the end working point W, passes through the shared hinge points M, Q, E, and C, and connects to the boom root hinge point A in sequence, fully presenting the chain-like geometric topology structure from the end to the boom root of this application. The aforementioned structural connecting rods, cylinders, and shared edges form a series of triangles with shared edges in pairs. Each triangle is connected end-to-end through the shared edges to form the complete topology of the S3 chain-like geometric solution.

[0046] Figure 1The horizontal forward reference of the machine body is indicated by a dashed line, and two angle markers are marked at hinge point A. These two markers have different meanings and should not be confused. Angle marker α is the angle between the horizontal forward reference of the machine body and the line connecting hinge point A to hinge point B. This angle is the preset fixed reference angle described in S33 later. Its value is determined by the relative installation position of hinge point A and hinge point B on the base and remains constant throughout the entire operation of the excavator. Angle marker θ is the angle between the horizontal forward reference of the machine body and the line connecting hinge point A to the end working point W, i.e., the shared side AW. This angle is the geometric structure angle output in S33, and its value changes in real time with the configuration of the robotic arm.

[0047] The specific criteria for determining the stationary state of the excavator are that the fluctuation of the stroke length of each cylinder is less than a preset threshold within a number of consecutive sampling periods, and that the pitch angle and roll angle of the base output by the attitude sensor do not fluctuate significantly within the same time period.

[0048] S12. Based on the three-dimensional spatial coordinates of each hinge point, calculate the fixed geometric side length parameters of each link in reverse using the distance formula between two points in space.

[0049] Let the hinge points at both ends of a certain connecting rod be respectively and The fixed geometric side length parameter of the link is calculated using the following formula for the distance between two points in space:

[0050] For example, suppose the coordinates of the boom root hinge point A measured by S11 are... The coordinates of the hinge point C at the end of the boom cylinder piston rod are: Meters, then the main boom length Meters (rounded to three decimal places). By iterating through all the links that need calibration, and substituting the coordinates of the hinge points at both ends of each link into the above formula, the complete set of fixed geometric side length parameters can be obtained in reverse. The side lengths to be calibrated include AB on the base, AC, AD, DC, CE, and DE on the boom, EF, PE, EQ, PQ, PM, and QM on the stick, MN, MW, and NW on the bucket, and QR and RN in the four-bar linkage, totaling seventeen sides. In cases where side NW on the bucket is difficult to measure directly due to structural obstruction, the fixed structural angle at hinge point M can be directly configured instead. Both are equivalent for subsequent solutions.

[0051] S13. Record the elevation angle of the base at the measurement moment as the reference elevation angle, and save the fixed geometric side length parameters and the reference elevation angle into the system configuration file.

[0052] At the same moment as performing the external measurement of S11, the pitch angle of the base, output by the attitude sensor mounted on the base, is read and fixed as the reference pitch angle in the system configuration file. It should be noted that the pitch angle of the base at this measurement moment is not required to be zero. If the calibration site is a slope with a forward inclination of 5 degrees, the recorded reference pitch angle is 5 degrees; if the calibration site is a barge, the reference pitch angle is the instantaneous pitch angle of the hull at the time of recording.

[0053] Besides the calibration path based on external measuring equipment, S1 can also employ other equivalent methods to obtain fixed geometric side length parameters and reference pitch angles. In some embodiments, a 3D laser scanner or multi-view photogrammetry is used to obtain the 3D spatial coordinates of each hinge point in the multi-link mechanism, replacing the measurement of key hinge points by external measuring equipment. Substituting the 3D coordinates of each hinge point obtained by any of the above methods into the spatial distance formula shown in S12, the fixed geometric side length parameters equivalent to those of the external measuring equipment can be calculated in reverse. This alternative method replaces the contact-based single-point marking operation of laser total station point-by-point measurement with non-contact overall scanning or multi-view visual reconstruction, achieving the same goal of obtaining a geometric model that fits the physical entity.

[0054] In other embodiments, before completing S1, the excavator is controlled to sequentially execute multiple preset differentiated postures, collecting the stroke length of each cylinder and the geographical coordinates of the base under each differentiated posture. The fixed geometric side length parameters and installation offset vectors of each connecting rod are then jointly inverted using the least squares method to replace the measurement of key hinge points by external measuring equipment. This alternative method uses statistical fitting to replace geometric measurement, enabling rapid calibration on-site when no external measuring equipment is available, while still achieving the goal of obtaining a geometric model that fits the physical entity.

[0055] In some embodiments, when the excavator is in a static standby state, the zero-bias drift of the attitude sensor is estimated online by using the difference between the output of the attitude sensor during the static period and the theoretical value of the gravity vector. This zero-bias drift is then accumulated onto the reference pitch angle for dynamic compensation. The criterion for the static standby state is similar to that of the stationary state in S11, i.e., the fluctuations in the stroke length of each cylinder and the base attitude are all less than a preset threshold within several consecutive sampling periods. After confirming entry into the static standby state, the acceleration measured by the attitude sensor should only include the projection of the Earth's gravity vector. The difference between the measured output and this theoretical value is the current zero-bias drift. Accumulating this drift onto the reference pitch angle recorded in S1 completes the dynamic compensation. Taking a typical microelectromechanical system attitude sensor as an example, its zero-bias drift can reach 0.1 to 0.3 degrees when the operating temperature changes by 20 degrees Celsius, and can further accumulate after hundreds of hours of continuous operation. This online compensation method also achieves the goal of maintaining a long-term stable reference pitch angle.

[0056] In other embodiments, during operation, in response to the detection that the combination of the stroke lengths of the boom cylinder, stick cylinder, and bucket cylinder covers multiple preset sets of differentiated attitude conditions, the residuals between the collected values ​​of each cylinder stroke length under each differentiated attitude condition and the theoretical predictions of the current geometric model are used to perform online micro-corrections on the fixed geometric side length parameters using the least squares method. The magnitude of the online micro-correction is limited by a preset upper limit for a single change. This mechanism uses similar mathematical methods to the aforementioned alternative calibration path based on multi-attitude inversion, but the timing and purpose of its application differ. The former is used to replace external measuring equipment during the initial calibration, while this mechanism is used to continuously maintain the accuracy of the calibrated parameters during operation. The specific action details and magnitude constraints of the online micro-correction involved in this mechanism, as well as the response linkage with the subsequent residual time-series statistical identification mechanism, are detailed in the complete expansion of the residual time-series statistical block.

[0057] S2. Real-time synchronous acquisition of the stroke length of the boom cylinder, the stick cylinder, and the bucket cylinder, as well as the pitch angle, roll angle, yaw angle, and geographical coordinates of the base.

[0058] Real-time synchronous acquisition refers to the synchronization of data acquisition from various sensors in time. Data collected at the same sampling moment form a complete data frame, which serves as the input vector for S3 and S4 calculations. A typical sampling frequency range is 10 Hz to 50 Hz. The choice of sampling frequency depends on the dynamic characteristics of the excavator's operation and the computing power constraints of the industrial control computer. In fast-moving scenarios such as crushing and dumping, a sampling frequency close to 50 Hz should be selected to ensure dynamic tracking accuracy. In slow-moving scenarios such as slag removal and leveling, a frequency close to 10 Hz can be selected to reduce the processing load on the industrial control computer.

[0059] The composition of the input vector and its mapping relationship with subsequent steps are as follows: The stroke lengths of the boom cylinder, stick cylinder, and bucket cylinder will be mapped to the corresponding dynamic geometric side lengths in S3 as dynamic inputs for chain-like geometric calculation; the pitch angle of the base will be subtracted from the reference pitch angle in S41 of S4 to obtain the pitch increment, which will be used to correct the geometric structure angle; the roll angle of the base will be used in S43 for roll compensation from the end-point two-dimensional coordinates to the three-dimensional body coordinates; the heading angle will be used in S45 for the rotation of relative coordinates from the body coordinate system to the northeast-sky coordinate system; and the geographic coordinates of the base will be used in S46 as the superposition reference for absolute coordinates.

[0060] The synchronous acquisition mechanism imposes explicit requirements on the time consistency of all sensors. If the sampling time of the boom cylinder stroke length is earlier than the sampling time of the base pitch angle, the time difference between the two will introduce an error due to inconsistency in the geometric calculation reference under dynamic conditions. For example, when the base pitch angle changes at a rate of 5 degrees per second and the time difference is 20 milliseconds, the angle error introduced by the base pitch angle sampling lag is approximately 0.1 degrees. This error, after geometric angle correction, will directly manifest as an elevation deviation in the end-positioning. A typical synchronous acquisition implementation method uses the unified clock of the industrial control computer as a reference, with all sensors simultaneously returning data via a serial bus under the same trigger signal, aligning the timestamps of all data to the trigger time.

[0061] The execution of S2 provides S3 and S4 with real-time updated input data frames. Each data frame independently participates in a complete chain of geometric solutions and 3D spatial reconstruction, and finally outputs the terminal 3D geographic coordinates corresponding to that data frame.

[0062] In S2, displacement sensors installed on the boom cylinder, stick cylinder, and bucket cylinder are used to measure the stroke length of each cylinder in real time; attitude sensors installed on the base are used to measure the pitch angle and roll angle of the base in real time; and dual-antenna positioning and orientation equipment installed on the base is used to measure the heading angle and the geographical coordinates of the base.

[0063] A typical implementation of a displacement sensor is the draw-wire type. A draw-wire type displacement sensor consists of a sensor body, a draw-wire, and a draw-wire retrieval mechanism. The sensor body is rigidly fixed to the outside of the hydraulic cylinder via a base. One end of the draw-wire is connected to a trunnion at the top of the piston rod, and the other end is kept taut by a spring-driven retrieval mechanism. When the piston rod extends, the draw-wire is pulled outwards. The encoder inside the sensor body measures the outward extension length of the draw-wire by the rotation angle of the draw-wire reel; this length corresponds to the stroke length of the hydraulic cylinder. During installation, the direction of the draw-wire's exit should be collinear with the cylinder axis, and guide pulleys and anti-slip mechanisms should be installed along the exit path to prevent the draw-wire from deviating from its designated path under strong vibration conditions.

[0064] This method uses a displacement sensor to measure the cylinder stroke length instead of directly installing tilt sensors at each joint, primarily due to the physical differences in their measurement principles. Tilt sensors based on gravitational acceleration are essentially accelerometers; their output is calculated from the projection of gravitational acceleration onto the sensor's sensitive axis. When an excavator performs high-frequency, high-vibration actions such as hammering or rapid shoveling, the total acceleration experienced by the sensor includes both gravitational acceleration and acceleration generated by mechanical motion. These two components are superimposed in the time domain, and the sensor itself cannot distinguish between them, leading to drastic jumps in the output value. To suppress this jump, traditional methods must introduce low-pass filtering algorithms with large time constants, typically resulting in filtering delays of 500 to 1000 milliseconds, causing the guidance displayed in the cab to lag significantly behind the actual movements of the robotic arm. The displacement sensor measures the linear displacement of the pull rope, a value whose change is solely caused by the relative position change of the piston rod and is physically unaffected by angular acceleration. Therefore, even under strong vibration conditions, it can directly output the true cylinder stroke length data without introducing significant filtering delays.

[0065] A typical implementation of an attitude sensor is an inertial measurement unit (IMU), which integrates a three-axis accelerometer and a three-axis gyroscope. The accelerometer measures the direction of the gravity vector to obtain the pitch and roll angles of the base, while the gyroscope provides a high-bandwidth reference for short-term attitude changes. The attitude sensor is mounted on the rotating platform of the base, with the mounting orientation conforming to the standard convention that the X-axis of the sensor's own coordinate system points forward of the aircraft, the Y-axis points to the right side of the aircraft, and the Z-axis points upward of the aircraft, to avoid introducing additional installation angle transformations during data processing.

[0066] The dual-antenna positioning and orientation device consists of a positioning host and two antennas, both mounted on a base. Typical installation positions are the rear and top of the base, or the front and rear. The line connecting the centers of the two antennas is the baseline, which is typically no less than 2 meters long to ensure the accuracy of the heading angle calculation. The positioning host simultaneously receives satellite signals from both antennas, calculates the high-precision geographic coordinates of each antenna using real-time dynamic differential technology, and calculates the azimuth angle of the baseline based on the baseline vector formed by the coordinate difference between the two antennas. The angle between the baseline and the forward direction of the aircraft is the antenna installation offset angle, which is measured and pre-set in the positioning host during system installation. The positioning host outputs the heading angle by subtracting the antenna installation offset angle from the azimuth angle of the baseline. This offset angle is zero when the baseline is parallel to the forward direction of the aircraft, and the azimuth angle of the baseline is the heading angle. The geographic coordinates of the base are usually taken from the geographic coordinates of the main antenna. In the subsequent S4 3D spatial reconstruction, the origin of the coordinates is transformed from the center of the main antenna to the location of the boom root by installing an offset vector.

[0067] In some embodiments, temperature data of the environment where the displacement sensor is located is acquired, and temperature compensation corrections are performed on the stroke lengths of the boom cylinder, stick cylinder, and bucket cylinder collected in S2 based on the thermal expansion coefficient of the drawstring material in the displacement sensor. The temperature compensation mechanism is designed to address the thermal expansion and contraction effect of the drawstring material. A temperature sensor is installed near the displacement sensor body to collect the ambient temperature in real time. The difference between the current collected temperature and the temperature at the calibration time is used as the temperature difference input, and the stroke length of each cylinder is corrected according to the following formula:

[0068] in, This is the stroke length after temperature compensation. The original travel length collected by S2. The coefficient of thermal expansion of the rope material is given. The current temperature being collected. To determine the temperature at the calibration time. For example, let the coefficient of thermal expansion of the rope material be 1 per degree Celsius. Given that the calibration temperature is 25 degrees Celsius, the current acquisition temperature is 45 degrees Celsius, and the original travel length is 3.000 meters, the temperature-compensated travel length is: Meters (retain four decimal places). In excavator operation scenarios, the temperature difference between day and night can reach more than 20 degrees Celsius. After the thermal deformation of the three ropes accumulates to the geometric dynamic side length, the multi-level amplification effect of the chain geometric solution can degrade the end-point positioning accuracy to the centimeter level. The corrected stroke length is used as the actual input of the S3 chain geometric solution, which also achieves the purpose of suppressing the impact of temperature drift on the end-point positioning accuracy.

[0069] In some embodiments, a combination of a single-antenna positioning and orientation device and an electronic compass is used to obtain the heading angle and the geographic coordinates of the base, replacing the dual-antenna positioning and orientation device. The single-antenna positioning and orientation device calculates the high-precision geographic coordinates of the antenna using real-time dynamic differential technology as the geographic coordinates of the base, while an independent electronic compass provides the heading angle. Typical implementations of the electronic compass include magnetic compasses, fiber optic gyroscopes, and microelectromechanical gyroscope integrators. When using a magnetic compass, the ferromagnetic structural components and motors of the excavator introduce hard iron and soft iron interference. After installation, the machine body needs to be rotated one full rotation in place to collect omnidirectional magnetic field data to complete hard iron and soft iron calibration. The magnetic north reference is then corrected to a true north reference according to the magnetic declination of the work site, ensuring that the output heading angle matches the true north reference required by S45. This alternative method also achieves the purpose of obtaining the geographic coordinates of the base and the heading angle in scenarios where only a single satellite antenna is available.

[0070] Regardless of whether a dual-antenna positioning and orientation device or a combination of a single-antenna positioning and orientation device and an electronic compass is used, the core output of S2 is the two physical quantities: the geographic coordinates of the base and the heading angle. The subsequent chain-like geometric calculation of S3 and the three-dimensional spatial reconstruction process of S4 have unified compatibility with the outputs of the two implementation schemes.

[0071] S3. Based on the fixed geometric side length parameters, the stroke length of the boom cylinder, the stroke length of the stick cylinder, and the stroke length of the bucket cylinder, the multi-link mechanism is decomposed into multiple triangles with shared sides. The cosine theorem is used to perform chain-like geometric calculations on each triangle with shared sides to determine the end distance and geometric structure angle. The end distance is the straight-line distance of the end relative to the root of the boom. Based on the end distance and the geometric structure angle, the two-dimensional coordinates of the end in the longitudinal plane of the machine body are generated.

[0072] A triangle with a shared edge is a pair of adjacent triangles that share a common edge. This common edge serves both as a known edge in the solution of the first triangle and as an input edge in the solution of the next triangle. Along the direction from the end to the root of the boom, a multi-link mechanism can be decomposed into a series of triangles with shared edges, such as... Figure 1 As directly presented, each triangle has its vertices at the hinge points and end work points defined by S11. The shared edges RM, QW, EW, CW, and AW shown in the aforementioned third line type are the common edges connecting the beginning and end of each triangle.

[0073] The decomposition of the chain-like geometric topology, divided by the drive segment, is as follows: The bucket cylinder drive segment, with hinge points P, Q, R, M, N and end-point working point W as vertices, includes triangles PQM, PQR, RQM, RMN, MNW, and QMW. Triangle PQR uses the geometric dynamic side length PR corresponding to the bucket cylinder as its input edge, triangle RQM outputs the shared edge RM, and triangle QMW outputs the shared edge QW. The stick cylinder drive segment, with hinge points C, D, E, F, P, Q and end-point working point W as vertices, includes triangles PQE, EQW, DEC, DEF, FEP, and CEW. Triangle DEF uses the geometric dynamic side length DF corresponding to the stick cylinder as its input edge, triangle EQW outputs the shared edge EW, and triangle CEW outputs the shared edge CW. The boom cylinder drive section uses hinge points A, B, C, D, E and the end-of-line working point W as vertices, and includes triangles ACD, ACW, and CAB. Triangle CAB uses the geometric dynamic side length BC corresponding to the boom cylinder as its input edge, and triangle ACW outputs the shared edge AW, which is the end-of-line distance. These three drive sections involve a total of fifteen distinct triangles, which are connected end-to-end through shared edges to form a complete chain topology. Triangles RQM, QMW, EQW, CEW, and ACW are each used twice in the chain geometry solution. The first time, the shared edge is calculated from two sides and their included angle. The second time, the other angle of the triangle is calculated from the three already calculated edges for use in closing the angle loop between adjacent hinge points. Triangles PQE and DEC are also each used twice, with the angles at two different vertices of the triangle calculated respectively. The same triangle can be used twice in two adjacent driving segments, and its vertices are written in the order of the vertices of the target angle in the following steps. Therefore, the same triangle may appear in different vertex orders. For example, triangle RQM and triangle RMQ refer to the same triangle.

[0074] The internal data flow of S3 is as follows: S31 maps the stroke length of the three cylinders to the corresponding dynamic geometric side length; S32 uses the dynamic geometric side length and fixed geometric side length parameters as known quantities, sequentially performs the law of cosines on the triangles sharing each side to calculate the cosine value of the target angle in each triangle, and takes the inverse cosine to obtain the target angle. At the same time, at the hinge points shared by multiple triangles, the remaining unknown angles are determined using angle loop closure constraints; S33 synthesizes the end distance and geometric structure angle after chain geometric calculation of all triangles, and then generates the end two-dimensional coordinates in the longitudinal plane of the body. The end two-dimensional coordinates output by S33 are located in the longitudinal plane of the body, which is the longitudinal symmetry plane of the base. The two-dimensional coordinates do not yet include the lateral offset caused by the roll of the base. This offset will be compensated by the roll angle of the base in the three-dimensional space reconstruction of S4.

[0075] Specifically, S3 includes sub-steps S31-S33.

[0076] S31. Map the stroke lengths of the boom cylinder, stick cylinder, and bucket cylinder to their corresponding geometric dynamic side lengths.

[0077] The dynamic geometric side length refers to the dynamic input side of each triangle in the chain-like geometric solution. The stroke length of the hydraulic cylinder only reflects the extension distance of the piston rod relative to the cylinder barrel. This distance, plus the distance between the fixed end hinge point of the hydraulic cylinder body and the cylinder barrel hinge point, constitutes a complete side of the triangle. The correspondence between the three hydraulic cylinders and the dynamic geometric side lengths is as follows: the stroke length of the boom cylinder is mapped to the distance BC between hinge point B and hinge point C; the stroke length of the stick cylinder is mapped to the distance DF between hinge point D and hinge point F; and the stroke length of the bucket cylinder is mapped to the distance PR between hinge point P and hinge point R. Taking the boom cylinder as an example, the distance from the piston rod end hinge point C to the cylinder barrel end hinge point B is equal to the stroke length of the boom cylinder plus the minimum installation length of the cylinder, constituting the dynamic geometric side length of triangle CAB; BC, together with the fixed geometric side length parameters AC on the boom and AB on the base, constitute the three sides of triangle CAB. The stroke lengths of the boom cylinder and bucket cylinder are mapped to the sides DF and PR of triangle DEF and PQR, respectively, in a similar manner.

[0078] S32. Using the dynamic and fixed geometric side lengths as known quantities, the cosine theorem is applied sequentially to each triangle sharing a side to calculate the cosine value of the target angle in each triangle. The inverse cosine function is taken for each cosine value to determine the target angle in each triangle. At the hinge points shared by multiple triangles, the angle loop closure constraint, where the sum of the angles at each hinge point equals the full angle, is used to determine the remaining unknown angles. The determined angles or side lengths in the current triangle are used as known quantities input for the adjacent triangles.

[0079] The two basic operations that S32 relies on are as follows. The first type is finding the included angle from the three sides. Let the lengths of the three sides of a triangle be... , , Target angle for and The included angle between the two sides,

[0080] Taking the inverse cosine of the cosine value yields the target angle. The second type involves finding the third side from two sides and the included angle, i.e.

[0081] When multiple triangles share the same hinge point, the sum of all angles converged at that hinge point equals a full circle. This constraint is called an angle loop closure constraint. When a hinge point has been determined through previous steps... There are several angles, among which To determine the total number of angles converged at this hinge point, the remaining unknown angle can be directly obtained through the angle loop closure constraint:

[0082] in, For the remaining unknown angles, For each determined angle, the angle loop closure constraint allows certain angles to be obtained without using the cosine theorem, reducing the strict requirement that all input edges for the cosine theorem at each step are known. In the chain topology of this method, the angle loop closure constraint is used once at each of the four shared hinge points: hinge point M, hinge point Q, hinge point E, and hinge point C. All angles are uniformly expressed in radians during the calculation process. The fixed reference angles and fixed structural angles given in degrees in the configuration file are converted to radians before being used in the calculation.

[0083] The following steps outline the twenty-seven steps of S32 in a chain-like derivation. The chain-like derivation begins with the bucket cylinder drive section. The first step involves calculating the solution using the three sides PQ, QM, and PM of triangle PQM according to the law of cosines. Taking the inverse cosine, we get This angle is the fixed structural angle on the pole.

[0084] The second step involves calculating the values ​​of the three sides PQ, QR, and PR of triangle PQR using the Law of Cosines. Taking the inverse cosine, we get Where PR is the geometric dynamic side length corresponding to the bucket cylinder, therefore It changes in real time as the bucket cylinder extends and retracts.

[0085] The third step is to determine the rotation angle of the rocker arm relative to the stick from the angle relationship at hinge point Q. .

[0086] The fourth step involves using the two sides QR and QM of triangle RQM and their included angle. Find the third side RM stands for the first shared edge in a chain topology.

[0087] Fifth step: Calculate the value using the Law of Cosines from the three sides RM, MN, and RN of triangle RMN. Taking the inverse cosine, we get .

[0088] Step 6: Calculate the value of triangle RMQ using the Law of Cosines from its three sides RM, QM, and QR. Taking the inverse cosine, we get .

[0089] Step 7: First determine the fixed structural angle on the bucket. In the case where side NW has been given during the calibration phase, triangle MNW is used according to... In reverse calculation, when edge NW is not given, the fixed structural angle in the configuration file is directly taken. Then convert it to radians; subsequently apply an angle loop closure constraint at hinge point M to obtain... .

[0090] Step 8: Using the two sides QM and MW of triangle QMW and their included angle... Find the third side QW is the second shared edge.

[0091] Step 9: Calculate the values ​​of the three sides PQ, EQ, and PE of triangle PQE using the Law of Cosines. Taking the inverse cosine, we get This angle is the fixed structural angle on the pole.

[0092] Step 10: Calculate the value of triangle MQW using the Law of Cosines from its three sides QM, QW, and MW. Taking the inverse cosine, we get .

[0093] Step 11: Apply an angle loop closure constraint at hinge point Q to obtain... .

[0094] Step 12: Using the two sides EQ and QW of triangle EQW and their included angle... Find the third side EW is the third shared edge. The chain derivation leads to the boom cylinder drive section.

[0095] Step 13: Calculate using the Law of Cosines from the three sides DE, CE, and DC of triangle DEC. Taking the inverse cosine, we get This angle is the fixed structure angle on the boom.

[0096] Step fourteen: Calculate using the Law of Cosines from the three sides DE, EF, and DF of triangle DEF. Taking the inverse cosine, we get Where DF is the geometric dynamic side length corresponding to the boom cylinder, therefore The angle changes in real time as the boom cylinder extends and retracts, and physically reflects the slewing posture of the boom relative to the arm.

[0097] Step 15: Calculate using the Law of Cosines from the three sides EF, PE, and FP of triangle FEP. Taking the inverse cosine, we get .

[0098] Step 16: Calculate using the Law of Cosines from the three sides PE, EQ, and PQ of triangle PEQ. Taking the inverse cosine, we get .

[0099] Step 17: Calculate the value of triangle QEW using the Law of Cosines from the three sides EQ, EW, and QW. Taking the inverse cosine, we get .

[0100] Step 18: Apply an angle loop closure constraint at hinge point E to obtain... .

[0101] Step 19: Using the two sides CE and EW of triangle CEW and their included angle... Find the third side CW is the fourth shared edge. The chain derivation leads to the boom cylinder drive section.

[0102] Step 20: Calculate using the Law of Cosines from the three sides AC, DC, and AD of triangle ACD. Taking the inverse cosine, we get This angle is the fixed structure angle on the boom.

[0103] Step 21: Calculate using the Law of Cosines from the three sides DC, CE, and DE of triangle DCE. Taking the inverse cosine, we get This angle is also a fixed structural angle on the boom.

[0104] Step 22: Calculate using the Law of Cosines from the three sides CE, CW, and EW of triangle ECW. Taking the inverse cosine, we get .

[0105] Step 23: Apply an angle loop closure constraint at hinge point C to obtain... .

[0106] Step 24: From the two sides AC and CW of triangle ACW and the included angle... Find the third side AW, or the straight-line distance between the end point of operation and the boom root hinge point, is the end distance described in this method and is the first key output of the chain geometry solution.

[0107] Step 25: Calculate using the Law of Cosines from the three sides AC, AB, and BC of triangle CAB. Taking the inverse cosine, we get Where BC is the geometric dynamic side length corresponding to the boom cylinder, therefore The angle changes in real time as the boom cylinder extends and retracts, and physically reflects the lifting posture of the boom relative to the base.

[0108] Step 26: Calculate using the Law of Cosines from the three sides AC, AW, and CW of triangle CAW. Taking the inverse cosine, we get .

[0109] Step 27, , With the preset fixed reference angle By performing addition and subtraction, the geometric structure angles are obtained. The specific rules for this synthesis process are detailed in S33.

[0110] Of the twenty-seven steps mentioned above, the angles calculated in steps one, nine, thirteen, fifteen, sixteen, twentieth, and twenty-first use only fixed geometric side length parameters as input. These values ​​remain constant after calibration and are called fixed structural angles. In engineering implementation, these angles can be calculated and cached once during system initialization and directly read in real-time loops without needing to be calculated repeatedly frame by frame. In the remaining steps, steps two, fourteen, and twenty-fifth use the corresponding dynamic geometric side lengths of the three cylinders as input, serving as the sole entry point for the chain-like geometric calculation of the cylinder stroke lengths. Steps three, seven, eleven, eighteen, and twenty-third are angle loop closure or angle addition / subtraction stages. Steps four, eight, twelve, nineteen, and twenty-four are shared edge generation stages, sequentially outputting RM, QW, EW, CW, and AW.

[0111] In some embodiments, before each step of the cosine theorem calculation in S32, it is checked whether the lengths of the three sides of the triangle involved in the current step satisfy the triangle existence condition. If the sum of any two sides is not greater than the third side, the data acquisition in the current frame is determined to be abnormal, the calculation of this frame is skipped, and the calculation result of the previous valid frame is used. The mathematical expression of the triangle existence condition is that the sum of any two sides is greater than the third side. For any triangle... , , It must meet the following conditions simultaneously. , , Three constraints exist; if any constraint is violated, the three sides cannot form a valid triangle, and the corresponding geometric topology does not exist physically. When the combination of the dynamic geometric side length output by S31 and the fixed geometric side length parameters causes a triangle to fail to meet the triangle existence condition, it indicates that the stroke length acquisition value of at least one cylinder in the current data frame is abnormal. Typical causes include displacement sensor failure, data cable detachment, and reading jumps caused by electromagnetic interference. Taking triangle RMN in step five as an example, its side RM is output in step four and is governed by the stroke length of the bucket cylinder. If the bucket cylinder reading jumps, causing RM to exceed the range... If the four-bar linkage cannot be closed geometrically, this anomaly is detected before taking the inverse cosine.

[0112] S33. After chain-like geometric calculations of all triangles, synthesize the end distance and geometric structure angle, and generate the end two-dimensional coordinates based on the end distance and geometric structure angle.

[0113] The final distance is obtained directly from the final side length of the triangle involved when the chain-like geometric solution is advanced to the end, that is, from the two sides AC and CW of triangle ACW in step 24 and the included angle. The obtained third side AW connects the boom root hinge point A and the end-effector working point W, and its length is the end-effector distance. The geometric structure angle is synthesized by adding and subtracting the two angles output at hinge point A during the chain geometry calculation process and a preset fixed reference angle. The specific synthesis rules involve the definition of the direction of each angle relative to the forward direction of the machine body, which will be elaborated below. The end-effector distance and geometric structure angle together describe the polar coordinate position of the end-effector relative to the boom root in the longitudinal plane of the machine body. This position is independent of the base roll, heading, and absolute geographical location, and is purely the geometric configuration information of the robotic arm.

[0114] In S32, after each step of the cosine theorem calculation to obtain the cosine value and before inverting the cosine function, the cosine value is clamped to... Within the closed interval. The numerical clamping mechanism inserts a preprocessing step after the output of the cosine theorem and before the inverse cosine function, forcibly constraining values ​​exceeding the upper bound to 1, and forcibly constraining values ​​below the lower bound to -1, while keeping values ​​within the interval unchanged. For example, when the output of the cosine theorem is... When clamped, the output is 1, and the inverse cosine function returns 0; when the output of the cosine theorem is... When clamped, the output is -1, and the inverse cosine function returns. The clamping operation only corrects minor out-of-bounds values ​​near the boundary and does not affect the normal calculation results within the interval, thus not introducing additional geometric solution errors.

[0115] In S33, the geometric structure angle is obtained by adding or subtracting the target angle, the remaining unknown angle output at the hinge point in the chain geometry solution, and a preset fixed reference angle. The fixed reference angle refers to the installation reference angle in the robotic arm's geometry, determined during the design phase by the excavator's structural installation angle or coordinate system definition, and is fixedly stored in the system configuration file along with the fixed geometric side length parameters during the calibration phase of S1. In the chain topology of this embodiment, the fixed reference angle... Right now Figure 1 The angle between the horizontal forward reference of the selected body and the line connecting hinge point A to hinge point B is uniquely determined by the relative installation position of hinge point A and hinge point B on the base, and does not change with the real-time movement of the robotic arm.

[0116] The specific rules for addition and subtraction are determined by the topological structure of the chain-like geometric solution. Taking the horizontal forward direction of the body as the zero direction and the counterclockwise direction as the positive direction, the direction of the line connecting hinge point A and hinge point B is... The output of step twenty-five Let A be the angle of rotation from line AB to line AC at hinge point A, therefore the direction of line AC is... The output of step twenty-six Let A be the angle at hinge point A from line AC to the shared edge AW, with the direction being clockwise. Therefore, the direction of the shared edge AW is... The direction of the shared edge AW is the geometric angle. Therefore, the geometric structure angles are synthesized according to the following formula:

[0117] in, The angle obtained from triangle CAB in step 25 changes in real time with the stroke length of the boom cylinder; The angle obtained from triangle CAW in the twenty-sixth step has its value shared by the stick cylinder and bucket cylinder, which carry the stroke length information. This is a preset fixed reference angle. The stroke length of the three cylinders is the geometric structure angle resulting from the combined action of these two angular components. When the end point is below the root of the boom, the synthesized result is usually negative, and its absolute value reflects the deflection of the end-point connection line relative to the horizontal forward direction of the machine body. In cases where the excavator's structural installation angle or machine coordinate system definition differs from this embodiment, the sign of each angular component needs to be re-determined based on the actual rotation direction of each angle using the same method. The form of the synthesized formula is adjusted accordingly, but the principle of synthesizing the geometric structure angle from the two rotation angles at hinge point A and the fixed reference angle remains unchanged.

[0118] Under the specific numerical example of the same working posture, let the calibrated main boom length be... Meter, fixed geometric side length parameters on the base Meters, the geometric dynamic side length corresponding to the stroke length of the boom cylinder in the current frame. The rice is obtained from step twenty-five.

[0119] Taking the inverse cosine function yields Degree (rounded to two decimal places). Let the shared edge obtained from the first twenty-four steps of chain derivation be... meters, end distance The rice is obtained from step twenty-six. Degrees. Assume a fixed reference angle. Substituting degrees into the above composite formula, we obtain the geometric structure angle as follows: The geometric structure angle of -87.50 degrees physically means that the end effector is located below the boom root and slightly off-center from the fuselage. The negative sign reflects the angle at which the end effector deviates from the horizontal forward direction along the negative Z-axis of the fuselage, i.e., below the fuselage. The geometric structure angle and the end effector distance together serve as inputs for the S4 pitch increment correction, and are then incorporated into the subsequent attitude decoupling and 3D space reconstruction process.

[0120] In some embodiments, using the results of the chain-like geometric solution in S3 as initial values, an overdetermined system of equations is constructed, including all triangle cosine theorem equations and closure constraints of angle loops at all hinge points. Constraints are set on the stroke lengths of the boom cylinder, stick cylinder, and bucket cylinder, along with fixed geometric side lengths. Target angles and remaining unknown angles from the chain-like geometric solution are used as optimization variables. Weighted least squares iteration is performed to update the values ​​of each target angle and remaining unknown angle. The weight matrix is ​​set according to the side length ratio of each triangle; the closer the side length ratio is to an isosceles triangle, the higher the weight. The updated angle values ​​replace the original output of the chain-like geometric solution in the synthesis of end-point distances and geometric angles in S33. This optimization also achieves the goal of high-precision synthesis of the end-point two-dimensional coordinates.

[0121] In other embodiments, a sliding window mid-range filter is applied to the end-effector 2D coordinates generated by S3, and the median of the solution results from the most recent multiple frames is taken as the output of the current frame. In response to the detection that the rate of change of the boom cylinder stroke length, stick cylinder stroke length, or bucket cylinder stroke length exceeds a preset speed threshold, the window length of the sliding window mid-range filter is reduced to maintain dynamic tracking performance. The typical window length is adjusted between 5 and 20 frames according to the sampling frequency; for example, at a 50 Hz sampling frequency, a 10-frame window corresponds to a filtering delay of 200 milliseconds. The preset speed threshold is set based on the maximum extension / retraction speed of a typical large hydraulic excavator boom cylinder of approximately 0.5 meters per second, stick cylinder of approximately 0.3 meters per second, and bucket cylinder of approximately 0.4 meters per second. In response to the rate of change exceeding the threshold, the filter window length switches from the default 10 frames to 3 frames; this alternative method also achieves the goal of balancing smooth end-effector 2D coordinate output with dynamic tracking performance.

[0122] After the chain-like geometric solution in S3 is completed, fault diagnosis and data reconstruction operations are performed on the results of the chain-like geometric solution. These operations include: reversing the calculation of the theoretical stroke length of the three cylinders and constructing a consistent residual vector; determining the faulty cylinder based on the component distribution of the residual vector; and using the collected values ​​of the two normal cylinders to reverse-calculate the theoretical stroke length of the faulty cylinder in the current posture as a substitute value for the chain-like geometric solution. The above operations are explained in detail below.

[0123] First, the theoretical stroke lengths of the boom cylinder, stick cylinder, and bucket cylinder are calculated in reverse using the results of chain-like geometric calculations. The differences between each theoretical stroke length and the stroke lengths of the boom cylinder, stick cylinder, and bucket cylinder are then used to form a consistent residual vector.

[0124] This method incorporates a fault diagnosis and data reconstruction mechanism to address the stability issues in chain-like geometric calculations. Chain-like geometric calculations rely on the simultaneous and normal input of the stroke lengths of all three hydraulic cylinders. A malfunction or data jump in the displacement sensor of any one cylinder will cause the calculation result to deviate from the true value. Traditional positioning schemes typically implement fault diagnosis using redundant sensor hardware, such as installing two independent displacement sensors on the same cylinder and identifying faults by comparing the data from the two sources. This method achieves the same functionality without adding any extra sensors. Its geometric basis stems from the coupling relationship between the end position of the triangle with shared sides and the lengths of the three hydraulic cylinders.

[0125] In the traditional three-joint forward kinematics scheme, three hydraulic cylinders independently drive the boom, stick, and bucket joints. Each joint angle is independently measured by an angle sensor at that joint, and any measurement anomaly does not produce an observable internal contradiction within a single frame. In the shared-side triangle topology of our method, the end position is jointly determined by the lengths of the three hydraulic cylinders through a single chain of derivations. The end distance AW is related to the geometric angle. Since both quantities simultaneously carry the stroke length information of the three hydraulic cylinders, under the condition that the end effector posture is independently constrained, the stroke length of the three hydraulic cylinders is no longer three free variables that can be arbitrarily combined. The independent constraint here is provided by the temporal continuity of the end effector motion: between two adjacent frames, the displacement of the robotic arm's end effector is physically limited by the maximum action rate of the hydraulic system, and the end effector position in the current frame can only fall within a neighborhood centered on the end effector position of the previous valid frame and with the maximum displacement as the radius.

[0126] The reverse calculation here utilizes this coupling relationship and continuity constraint. After the chain geometry solution is completed, the two-dimensional coordinates of the end point, as well as the angles and middle side lengths in the entire chain network, are determined. At this point, using the two-dimensional coordinates of the end point of the previous valid frame as a reference, the stroke length that the three cylinders should have under this reference posture can be derived in reverse along the opposite topological direction, i.e., the theoretical stroke length. The differences between each theoretical stroke length and the actual acquired value constitute a consistency residual vector. ,in , , , , , These are the theoretical stroke lengths of the boom cylinder, stick cylinder, and bucket cylinder, respectively. , , These are the actual collected values ​​from the three hydraulic cylinders. Consistency residual vector. modulus Essentially, it reflects the degree of deviation of the acquired data in this frame from the geometric attitude established by the previous valid frame. Under normal operating conditions where the actual motion at the end is much smaller than the sensor fault jump variable, this modulus value remains at the noise level.

[0127] In addition, in response to the magnitude of the consistency residual vector exceeding the preset consistency threshold, the cylinder corresponding to the component with the largest absolute value is identified as the faulty cylinder based on the distribution of the absolute values ​​of each component in the consistency residual vector.

[0128] The design of the preset consistency threshold is based on the residual noise level when the sensor is operating normally. The sensor's measurement noise, accumulated floating-point error, residual deviation of calibration parameters, and normal displacement at the ends of adjacent frames together constitute the normal fluctuation range of the residual. Typically, the magnitude of the consistency residual vector remains at the millimeter level under fault-free conditions. The consistency threshold should be set to a value several times or more of the normal fluctuation range, for example, three times the normal standard deviation, to avoid false alarms under normal operating conditions while ensuring sensitive detection of significant anomalies.

[0129] When the magnitude of the consistency residual vector exceeds a threshold, the location of the faulty cylinder is triggered. The location of the faulty cylinder is based on the absolute value distribution of the three components of the consistency residual vector. In a typical scenario of a single cylinder failure, the measured value of the faulty cylinder deviates significantly from the true value, resulting in a significant residual. The residuals of the other two normal cylinders are contributed only by normal noise and normal inter-frame displacement. Therefore, the absolute value of the residual component corresponding to the faulty cylinder will be significantly larger than the other two components. The specific discrimination rule is as follows: when the ratio of the absolute value of the largest component to the absolute value of the second largest component exceeds a preset discrimination threshold (e.g., greater than 2), the cylinder corresponding to the largest component is identified as the faulty cylinder. When this ratio does not exceed the discrimination threshold, it indicates that the residuals are significantly distributed across multiple components, and the fault may not be caused by a single cylinder. In this case, a system-level warning is triggered instead of proceeding to the subsequent data reconstruction process.

[0130] The fault location process is illustrated using a numerical example of a typical operating posture of a shipborne excavator. Assume the collected values ​​for the three hydraulic cylinders under the aforementioned operating posture are 1.200 meters for the boom cylinder, 0.800 meters for the stick cylinder, and 0.500 meters for the bucket cylinder. Under normal operating conditions, the residual components between the theoretical stroke length obtained through back-calculation using chain-like geometric solution and the collected values ​​are all within the millimeter range. For example… The value is approximately 0.004 meters, below the consistency threshold. When the displacement sensor of the boom cylinder malfunctions and the acquired value drifts to 0.830 meters, the theoretical stroke length calculated in reverse will deviate significantly from the acquired value, and the consistency residual vector will become... The modulus is approximately 0.031 meters, exceeding the assumed consistency threshold of 0.010 meters. The absolute values ​​of the three components are 0.008, 0.030, and 0.005, respectively. The ratio of the largest component (0.030) to the second largest component (0.008) is 3.75 times, exceeding the 2-times discrimination threshold. Therefore, the boom cylinder is identified as the faulty cylinder.

[0131] In addition, based on the stroke lengths of the boom cylinder, stick cylinder, and bucket cylinder (excluding the stroke length of the faulty cylinder), and combined with the geometric constraints of the fixed geometric side length parameters, the theoretical stroke length of the faulty cylinder in the current posture is solved in reverse. The theoretical stroke length is then substituted into the chain-like geometric solution as a replacement value.

[0132] After the faulty cylinder is located, the system cannot continue to rely on the acquired values ​​of that cylinder for chain geometric calculations under fault conditions. However, directly stopping the calculations would completely interrupt the positioning output, which does not meet the engineering requirements for continuous excavator operation. The aforementioned inverse calculation operation utilizes the geometric coupling of the shared-side triangle topology. Using the acquired values ​​of the two normal cylinders and the end pose of the previous valid frame as input, it inversely calculates the theoretical stroke length that the faulty cylinder should have in the current pose. This theoretical value is then used as a substitute value for the chain geometric calculations, allowing the positioning output to continue under fault conditions.

[0133] Examining this inverse kinematics process from a geometric perspective: the shared-side triangle topology establishes a constraint relationship between the two degrees of freedom of the end position and the stroke lengths of the three cylinders. After the faulty cylinder is removed, the stroke lengths of the two normal cylinders serve as two independent inputs, perfectly matching the two degrees of freedom of the end position. Given that the end position attitude has been initially determined by the solution results of the previous valid frame, the theoretical stroke length of the faulty cylinder can be uniquely determined by substituting the values ​​of the two normal cylinders into the geometric constraints. It should be noted that the validity of this inverse kinematics result depends on the assumption of continuity of the end position attitude between adjacent frames; therefore, its accuracy decreases as the duration of the fault increases.

[0134] Following the previous numerical example, once the stick cylinder is identified as the faulty cylinder, using the acquired values ​​of 1.200 meters for the boom cylinder and 0.500 meters for the bucket cylinder as known inputs, and combining fixed geometric side length parameters and the end position calculated from the previous valid frame, the theoretical stroke length of the stick cylinder in the current posture is obtained through geometric constraint inverse kinematics, which is approximately 0.800 meters, i.e., the true value before the stick cylinder malfunction. This 0.800 meters is used as a substitute value for the stick cylinder stroke length in the chained geometric calculation, continuing to maintain the normal output of the end-position 2D coordinates. Simultaneously, the system indicates on the display terminal that it is currently in data reconstruction mode, and the accuracy level of the positioning result is downgraded by one level, so that the operator is aware that there is a cylinder driven by reconstructed data in the current positioning link. Data reconstruction mode should be considered an emergency transitional measure; if the duration exceeds the preset maintenance time threshold, the system should prompt the operator to return to base to perform repair and replacement of the faulty cylinder.

[0135] During the chain-like geometric solution process in S3, hinge points shared by multiple triangles are identified as redundant hinge points. These hinge points are defined as those where all circumferential angles are calculated using the cosine theorem of the triangle or as fixed structural angles determined by the cosine theorem from fixed geometric side length parameters, and where solving for unknown angles does not require closure constraints of angle loops. The difference between the sum of all circumferential angles at each redundant hinge point and the circumferential angle is calculated as the closure residual for each redundant hinge point. Error injection sections are determined based on the distribution pattern of the closure residuals along the chain-like geometric solution sequence. The cylinders corresponding to these error injection sections are then cross-validated with the previously identified faulty cylinders.

[0136] Building upon the aforementioned external consistency verification mechanism for the cylinder layer, this mechanism provides supplementary diagnostic capabilities from the perspective of the chain hinge points, forming a dual-layer diagnostic structure of external and internal checks. The external consistency verification mechanism for the cylinder layer, after the entire chain geometry solution is completed, calculates the theoretical stroke length of the three cylinders in reverse based on the end-effector attitude parameters. This effectively detects overall data inconsistencies but cannot precisely locate concentrated sections of errors within the chain. When the magnitude of the consistency residual vector exceeds a threshold, external verification can only indicate an overall deviation between the collected data and the geometric model, but cannot pinpoint the specific solution segment in the chain derivation from which the deviation originates. This mechanism's internal chain monitoring is plug-and-play during the chain geometry solution execution, capable of acquiring closed residual signals at redundant hinge points, diagnosing the spatial distribution characteristics of errors from the internal perspective of the chain topology.

[0137] Whether a shared hinge point constitutes a redundant hinge point depends on the set of fixed geometric side length parameters configured in the S1 calibration stage. When only the minimum set of parameters necessary to maintain the chain derivation is configured, each of the four shared hinge points M, Q, E, and C has an angle not determined by previous steps. This angle can only be solved using the closed constraint that the sum of the angles equals the full angle. The angle information of such hinge points is not redundant; the difference between the sum of the determined circumferential angles and the full angle is numerically equal to the unsolved angle itself, carrying no error information, and therefore does not constitute a redundant hinge point. Since the three-dimensional spatial coordinates of all hinge points are measured by external measuring equipment in S11, the distance between any two hinge points on the same component can be calculated using the spatial distance formula in S12. Therefore, in the calibration stage, fixed geometric side length parameters exceeding the minimum necessary set can be configured at low cost, turning the corresponding shared hinge points into redundant hinge points.

[0138] Taking hinge point C as an example, after configuring the distance AE between hinge points A and E on the boom in addition to the minimum parameter set, the circumferential angle of hinge point C... The angle can be independently solved using the three sides AC, CE, and AE of triangle ACE by the law of cosines. This angle was already determined in the chain derivation by... and The sum is given, and the two independent paths give the same angle. Hinge point C then becomes a redundant hinge point, and its closure residual is... Taking hinge point Q as an example, after additionally configuring the distance EM between hinge point E and hinge point M on the stick, It can be solved independently from the three sides EQ, QM, and EM of triangle EQM, as shown in the chain derivation. and The sum of these constitutes the closed residual. Taking hinge point E as an example, after additionally configuring the distance FQ between hinge point F and hinge point Q on the stick, It can be solved independently from the three sides EF, EQ, and FQ of triangle FEQ, and... and The sum of these constitutes a closed residual. Taking hinge point M as an example, with side NW and fixed structural angle simultaneously configured... In this case, There are two independent sources: one is calculated from the triangle MNW, and the other is read directly from the configuration value. The difference between the two is the closed residual at hinge point M. The closed residuals of the above redundant hinge points are arranged in the execution order of the chain derivation to form the closed residual distribution vector.

[0139] The spatial distribution pattern of the closed residual distribution vector indicates the location of geometric model distortion in the chain topology. The closed residual of each redundant hinge point is jointly determined by the fixed geometric side length parameters of the component where the hinge point is located, and should be strictly zero under the ideal condition of accurate calibration. If the fixed geometric side length parameters of a component deviate from the actual physical dimensions due to long-term wear, thermal expansion, or loosening of assembly clearance, the closed residual of the hinge point corresponding to that component will deviate significantly from zero, while the closed residuals of the hinge points corresponding to other components remain within the normal noise level. Since the input data of each drive segment in the chain derivation comes from a specific hydraulic cylinder, there is a one-to-one correspondence between the error injection segment and the hydraulic cylinder: the segment where hinge point M and hinge point Q are located corresponds to the input of the bucket hydraulic cylinder, the segment where hinge point E is located corresponds to the input of the stick hydraulic cylinder, and the segment where hinge point C is located corresponds to the input of the boom hydraulic cylinder. For example, if it is observed that the closed residuals at hinge point C and hinge point E are both kept within the corresponding angular residuals at the millimeter level, while the closed residual at hinge point Q suddenly increases to 0.05 radians and the closed residual at hinge point M further increases to 0.08 radians, then the error injection section is located in the bucket cylinder drive section, and the input data of this section corresponds to the stroke length of the bucket cylinder.

[0140] Cross-validation of the in-chain monitoring results with the aforementioned external verification results of the cylinder layer further improves diagnostic reliability. The two types of verification independently judge the same dataset from different perspectives, and their results should remain consistent under the condition of a single anomaly source. When the external verification of the cylinder layer indicates that the residual component of a certain cylinder significantly exceeds the standard, and the closed residuals of all redundant hinge points increase synchronously and are concentrated in the drive segment corresponding to that cylinder, it indicates that the anomaly originates from the distortion of the geometric model parameters of that drive segment, and should trigger the correction of fixed geometric side length parameters rather than data reconstruction. When the external verification of the cylinder layer indicates that the residual component of a certain cylinder significantly exceeds the standard, and the closed residuals of all redundant hinge points remain within the normal noise level, it indicates that the geometric model parameters are still accurate, and the anomaly originates from the measurement output of the cylinder displacement sensor itself, and can directly proceed to the subsequent data reconstruction process. When the two types of verification results do not point to the same drive segment and cannot be classified into the two typical forms mentioned above, it indicates that the fault mode does not belong to the typical form of a single abnormal source. There may be more complex fault situations such as multiple sensors malfunctioning at the same time or numerical stability problems of certain solution segments in the chain topology. At this time, a system-level warning should be triggered and automatic data reconstruction should be paused, prompting the operator to return to the base to perform diagnosis and calibration.

[0141] During the construction of a consistent residual vector across multiple consecutive frames, a sliding statistical window is maintained for the magnitude of the consistent residual vector across these frames. The mean and standard deviation of the magnitude within the sliding statistical window are calculated. If the mean magnitude continuously increases at a preset rate while the standard deviation remains stable, it is determined to be a slow drift of the fixed geometric side length parameter. An online micro-correction is then performed on the fixed geometric side length parameter, with the magnitude of the online micro-correction limited by a preset upper limit for a single change. If the magnitude of the consistent residual vector in a single frame exceeds a preset abrupt change threshold, it is determined to be a sensor abrupt failure, triggering the determination of the aforementioned faulty cylinder and the inverse solution of the aforementioned theoretical stroke length.

[0142] The aforementioned external consistency verification and intra-chain monitoring mechanisms for the cylinder layer both determine whether anomalies exist in the current frame from a single-frame perspective. However, single-frame judgment cannot distinguish the temporal characteristics of anomalies. Three distinct anomalies—instantaneous sensor noise, slow drift of fixed geometric side-length parameters, and abrupt sensor failures—may exhibit similar numerical characteristics in the single-frame residual, but require entirely different response strategies: instantaneous noise should be ignored and not interfere with normal calculations; slow drift should trigger minor adjustments to geometric parameters to maintain the model baseline; and abrupt failures should immediately initiate fault location and data reconstruction. This mechanism, through temporal statistical analysis, imbues single-frame diagnosis with a temporal dimension, thereby achieving graded identification and differentiated responses for different anomaly types.

[0143] A sliding statistical window is a data structure that maintains residual data from the most recent few frames. The window length is determined by the sampling frequency and the response speed requirements for anomaly detection. At a typical 20Hz sampling frequency, the window length can be set to 100 frames, corresponding to a 5-second time span. The window stores a sequence of consistent residual vector magnitudes from the most recent 100 frames. Each time a new frame of data is acquired and the consistent residual calculation is completed, the new magnitude is added to the tail of the window while the oldest magnitude at the head of the window is removed, maintaining a constant window size. The mean magnitude within the window is calculated in real-time after each window update. and standard deviation of modulus These two statistics serve as input features for anomaly type identification.

[0144] The temporal characteristics of slow drift conditions are that the mean modulus increases monotonically at a stable rate over time, while the standard deviation of the modulus remains small. When the welded structural components of the excavator deviate slightly from the actual physical dimensions due to long-term wear, thermal expansion, or loosening of assembly gaps, the consistency residual of each frame will contain a common systematic component caused by geometric model distortion. This systematic component increases slowly with the accumulation of geometric distortion, while the random component in the residual of each frame, i.e., sensor noise, remains stable. Therefore, the mean modulus within the window shows a monotonically increasing trend, while the standard deviation remains stable. The principle for setting the preset rate is to slightly exceed the typical drift rate caused by normal wear and thermal expansion of the mechanical structure. A typical preset rate is an increase of 0.5 mm to 1 mm in the mean modulus per hour.

[0145] In response to the identification result of slow drift, the system initiates online micro-correction of the fixed geometric side length parameters. The specific correction mechanism follows the aforementioned multi-attitude inversion mechanism, utilizing the accumulated multi-attitude cylinder stroke length data over the past few minutes and the corresponding chain-like geometric solution residuals. The system then uses the least squares method to solve for the micro-deviation vector of the fixed geometric side length parameters and applies this deviation vector to the current fixed geometric side length parameters. In embodiments with redundant hinge points, the closure residuals of each redundant hinge point can serve as additional observations for the least squares solution, indicating which component's fixed geometric side length parameters should be corrected first. The magnitude of the online micro-correction is limited by a preset single-change upper limit, typically no more than 0.5 mm per adjustment. The engineering significance of this upper limit is to prevent occasional data anomalies from being misidentified as slow drift and contaminating the overall geometric model. When the abnormal data actually originates from a sudden sensor failure but is misidentified as slow drift by time-series statistics, the hard constraint on the single-correction magnitude can control the contamination within a recoverable range, preventing irreversible large-scale shifts in the geometric model.

[0146] The temporal characteristic of abrupt failure conditions is a single-frame magnitude jump exceeding a preset abrupt threshold. When a sensor experiences a sudden hardware failure, a data cable momentarily disconnects, or encounters strong electromagnetic interference, the magnitude of the consistency residual vector of the current frame can jump several times or even tens of times between adjacent frames, far exceeding normal noise levels. The preset abrupt threshold is set based on statistical principles, typically implemented as the mean of the magnitude within a window plus a certain multiple of the standard deviation of the magnitude, for example... The probability of a single frame modulus value exceeding the range is extremely low under fault-free conditions, corresponding to a normal data confidence interval of approximately 99.7%. Therefore, exceeding this range can be reasonably identified as a sudden fault.

[0147] In response to the identification result of a sudden fault, the system immediately triggers the aforementioned operation to determine the faulty cylinder, identifying the faulty cylinder through component analysis of the consistency residual vector. Subsequently, it automatically enters the data reconstruction operation, using the collected values ​​of the two normal cylinders as input to inversely solve the theoretical stroke length of the faulty cylinder as a substitute value into the chain geometric solution. The sudden fault response has the highest priority and is executed immediately upon triggering, without waiting for subsequent updates from the sliding statistical window, to avoid the continuous contamination of the chain geometric solution output by fault data.

[0148] Under the combined effect of the aforementioned multi-pose inversion mechanism, cylinder layer external verification mechanism, chain monitoring mechanism, and the time-series statistical identification mechanism of this mechanism, the three working conditions of chain geometric solution—normal operation, slow degradation, and sudden failure—are fully covered, forming a complete reliability assurance system that is self-monitoring, self-maintaining, and self-recovering.

[0149] S4. Based on the difference between the pitch angle of the base and the reference pitch angle, the pitch increment correction is performed on the geometric structure angle. The end-point two-dimensional coordinates are regenerated based on the corrected geometric structure angle and the end-point distance. The regenerated end-point two-dimensional coordinates are reconstructed in three-dimensional space by combining the roll angle and heading angle of the base. The end-point three-dimensional geographic coordinates are obtained by superimposing the geographic coordinates of the base.

[0150] After S3 is completed, the two-dimensional coordinates of the end point output by the chained geometric solution are located in the longitudinal plane of the machine body, with the boom root as the origin, describing the geometric position of the end point relative to the boom root. The core challenge that S4 needs to address is mapping this planar geometric solution result to latitude, longitude, and elevation in the geodetic coordinate system. The result of S3 only reflects the geometric configuration of the robotic arm itself, while the excavator as a whole also has real-time changing base attitude, orientation, and absolute position in geographical space. Without attitude decoupling and three-dimensional reconstruction, a correspondence cannot be established between the geometric configuration of the robotic arm and its geographical location.

[0151] The internal data flow of S4 consists of two stages. The first stage is pitch increment decoupling: the difference between the pitch angle of the base and the reference pitch angle recorded in stage S1 is used as the pitch increment to correct the geometric structure angle output by S3, and then the corrected geometric structure angle is obtained. The end-effector two-dimensional coordinates are regenerated using the corrected geometric structure angle and the end-effector distance as inputs. The purpose of this stage is to suppress the influence of the base pitch attitude on the expression of the manipulator's geometry in the longitudinal plane of the body. The second stage is three-dimensional space reconstruction: the two-dimensional coordinates are unfolded into three-dimensional body coordinates using the roll angle of the base. After installation offset compensation, the coordinate origin is transformed from the root of the boom to the center of the positioning antenna. After yaw angle rotation, the relative coordinates in the body coordinate system are transformed into the increment in the northeast-northeast coordinate system. Finally, the geographic coordinates of the base are superimposed and projection back calculation is performed to obtain the latitude, longitude and elevation in the geodetic coordinate system.

[0152] The specific compensation mechanism for base sway under shipboard conditions and the attitude stripping principle for non-horizontal field markings will be developed in subsequent pitch increment correction sub-steps; the geometric details of coordinate transformation at each level in 3D space reconstruction will be developed in subsequent 3D space reconstruction sub-steps.

[0153] Specifically, S4 includes sub-steps S41-S42, which involve performing pitch increment correction on the geometric structure angle and regenerating the end-point two-dimensional coordinates.

[0154] S41. Calculate the difference between the pitch angle of the base and the reference pitch angle as the pitch increment, and subtract the pitch increment from the geometric structure angle to obtain the corrected geometric structure angle.

[0155] S42. Based on the end-effector distance and the corrected geometric structure angle, the product of the end-effector distance and the cosine of the corrected geometric structure angle is used as the first coordinate component in the longitudinal plane of the body, and the product of the end-effector distance and the sine of the corrected geometric structure angle is used as the second coordinate component in the longitudinal plane of the body, and the two-dimensional coordinates of the end-effector are regenerated.

[0156] The execution process of S41 and S42 is illustrated using numerical examples of the aforementioned working postures of a shipborne excavator. Assume the reference pitch angle recorded at the calibration time in stage S1 is 5.00 degrees, corresponding to a 5-degree forward tilt of the hull during calibration; the current pitch angle collected at the base is 8.00 degrees, corresponding to a further 3-degree forward tilt caused by waves; the geometric structure angle output by S3 is -87.50 degrees, and the end-effector distance is 7.200 meters. The pitch increment obtained from executing S41 is... degrees, the corrected geometric structure angle is Degree. Executing S42 yields the first coordinate component of the end-point two-dimensional coordinates. meters, second coordinate component is Meters (rounded to three decimal places). Corrected end-point two-dimensional coordinates. The meter indicates the position of the end point 0.063 meters behind and 7.200 meters below the boom root. This result has been stripped of the combined effects of the hull calibration angle and real-time wave pitch, and only reflects the geometry of the robotic arm itself.

[0157] Furthermore, S4 also includes sub-steps S43-S46.

[0158] S43. Use the roll angle of the base to perform roll compensation on the regenerated end-body two-dimensional coordinates, and expand the end-body two-dimensional coordinates in the longitudinal plane of the body into three-dimensional body coordinates.

[0159] The two-dimensional coordinates output by S41 and S42 lie only in the longitudinal plane of the fuselage, which is the longitudinal symmetry plane of the base. When the base rolls (i.e., tilts left or right), this longitudinal symmetry plane rotates around the longitudinal axis of the base. The two-dimensional coordinates in the longitudinal plane need to be expanded into three-dimensional fuselage coordinates using the roll angle to reflect this rotation. The three-dimensional fuselage coordinate system has the boom root as its origin, with the X-axis pointing forward, the Y-axis pointing to the right side of the fuselage, and the Z-axis pointing upward. The X-axis component in the three-dimensional fuselage coordinates remains consistent with the first coordinate component of the two-dimensional coordinates. The Y-axis and Z-axis components are obtained by expanding the second coordinate component of the two-dimensional coordinates using the roll angle of the base.

[0160]

[0161]

[0162] in, and These are the first and second coordinate components of the two-dimensional coordinates output by S42, respectively. The roll angle of the base is positive when tilting to the right; , , This refers to the three-dimensional coordinates of the machine body. When the roll angle of the base is zero, Degenerate to zero and When the three-dimensional body coordinates are equal, they degenerate into a direct extension of the two-dimensional coordinates. When the roll angle of the base is non-zero, the displacement in the longitudinal plane of the robotic arm is simultaneously projected onto the Y-axis and Z-axis in the body coordinate system, which manifests as changes in lateral offset and vertical components.

[0163] S44. The three-dimensional body coordinates are offset and compensated according to the preset installation offset vector to obtain the relative coordinates of the end cap relative to the geographic coordinate reference point. The geographic coordinate reference point is the measurement reference point corresponding to the geographic coordinates of the base, and the installation offset vector is the three-dimensional displacement of the geographic coordinate reference point relative to the boom root in the body coordinate system. In this embodiment, the geographic coordinates of the base are measured by the main antenna of the dual-antenna positioning and orientation device; therefore, the geographic coordinate reference point is the center of the positioning antenna, and the positioning antenna center mentioned below refers to this geographic coordinate reference point.

[0164] The three-dimensional body coordinates originate from the boom root, while the base geographic coordinates involved in subsequent steps S45 and S46 are derived from the center position of the main antenna measured by the dual-antenna positioning and orientation equipment. Since their origins do not coincide, direct superposition will introduce positioning errors related to the installation offset. Installation offset vector. This describes the three-dimensional displacement of the positioning antenna center relative to the boom root in the machine coordinate system. This vector was determined through actual measurement and stored in the system configuration file during the installation of the excavator positioning system. The relative coordinates of the end effector relative to the positioning antenna center... The installation bias vector is obtained by subtracting it from the three-dimensional body coordinates:

[0165]

[0166]

[0167] Taking a typical dual-antenna installation as an example, the main antenna is installed at the top of the rear end of the base, with a displacement relative to the boom root of [missing information]. Meters, meaning 1.5 meters behind. rice, Meters, that is, 2.2 meters above. Taking the example of the operating attitude of a shipborne excavator, assuming the current roll angle of the base is 2 degrees (i.e., the hull is tilted 2 degrees to the right), the three-dimensional body coordinates output by S43 are: Meters (rounded to three decimal places). The relative coordinates after S44 offset compensation are: Meters. This relative coordinate indicates that the end is located 1.437 meters in front of the center of the main antenna, 0.251 meters to the right, and 9.396 meters below.

[0168] S45. Use the heading angle to rotate the relative coordinates from the body coordinate system to the northeast-sky coordinate system to obtain the northeast-sky coordinate increment.

[0169] relative coordinates Located in the aircraft coordinate system, its orientation is defined by three directions: forward, rightward, and upward. This orientation changes with the base's heading. To superimpose the relative coordinates onto the base's geographic coordinates to obtain the terminal's absolute geographic location, the relative coordinates must first be transformed from the aircraft coordinate system to a ground-fixed Northeast-Sky coordinate system. The Northeast-Sky coordinate system has its origin at the center of the positioning antenna, with the E-axis pointing due east, the N-axis due north, and the U-axis zenith. The orientation of this coordinate system is fixed to the ground and does not change with the base's attitude. The directional relationship between the aircraft coordinate system and the Northeast-Sky coordinate system is determined by the heading angle. The heading angle is defined as the angle between the aircraft's forward direction and true north. The rotation matrix converts the relative coordinates to northeast-sky coordinate increments. , , :

[0170]

[0171]

[0172] in, With true north as zero and clockwise as positive, continuing from the previous numerical example, assuming a heading angle of 90 degrees (i.e., the aircraft is pointing due east), the increment of the northeast celestial coordinates is... rice, rice, Meters. This increment indicates that the end is located 1.437 meters due east of the center of the main antenna, 0.251 meters due south, and the altitude has decreased by 9.396 meters.

[0173] S46. Overlay the northeast celestial coordinate increment onto the geographic coordinates of the base to obtain the absolute coordinates of the endpoint in the projected coordinate system. Use projection back calculation to convert the absolute coordinates into latitude, longitude and elevation in the geodetic coordinate system to obtain the three-dimensional geographic coordinates of the endpoint.

[0174] The geographic coordinates of the base are given in projection plane form by a dual-antenna positioning and orientation device. Typical projection systems include the Gauss-Kruger projection or the universal transverse Mercator projection. Let the geographic coordinates of the base be represented in the projection plane as follows: , , These represent the eastward coordinates, northward coordinates, and orthogonal elevation, respectively. The absolute coordinates of the endpoint in the projected coordinate system are obtained by superimposing the northeast celestial coordinate increments onto the geographic coordinates of the base:

[0175]

[0176]

[0177] in, , , These represent the eastward coordinates, northward coordinates, and elevation of the end point in the projected coordinate system. Continuing with the previous numerical example, let the geographic coordinates of the base be... rice, rice, Meters, which is the altitude corresponding to the ship's sailing height, then the absolute coordinates of the projected plane at the end are: rice, rice, rice. A negative value indicates that the end point is 4.396 meters below sea level, which is consistent with the physical scenario of the end point being underwater during underwater crushing and slag removal operations by ship-borne excavators in waterways.

[0178] Inverse projection calculation will calculate the absolute coordinates of the projection plane. , Conversion to latitude and longitude in geodetic coordinate system , Its mathematical form depends on the projection system used. Taking the Gauss-Kruger projection as an example, projection inverse calculation requires knowing the longitude of the central meridian of the projection and the expansion coefficients of each level of the Gauss projection. It is obtained through the inverse operation of the forward transformation formula from latitude and longitude to projected coordinates. Elevation remains unchanged. This is the final elevation at the endpoint. Endpoint 3D geographic coordinates. , , This is the final output of S4, which can be directly used for comparison with the design section and for construction guidance.

[0179] In some embodiments, in response to the detection that the collected roll angle value of the base is invalid or exceeds a preset reasonable range, the roll angle of the base is set to zero in the three-dimensional spatial reconstruction of S4, and only the positioning result based on pitch increment correction is output. The display terminal is marked as the current degraded positioning mode and the corresponding accuracy degradation estimate. The judgment conditions for roll angle abnormality include: the attitude sensor reports an error code or communication timeout, the collected value exceeds the physical reasonable range, and the collected value remains unchanged for several consecutive frames. The accuracy degradation estimate is estimated based on the position deviation caused by the lack of roll compensation: the horizontal lateral deviation is approximately the product of the second component of the end two-dimensional coordinate and the sine of the actual roll angle, and the vertical deviation is approximately the product of the second component of the end two-dimensional coordinate and the difference between 1 and the cosine of the actual roll angle. This degradation method also achieves the purpose of continuous output of the end three-dimensional geographic coordinate when the roll sensor fails.

[0180] In other embodiments, in response to the solution state of the dual-antenna positioning and orientation device degrading from a fixed solution to a floating-point solution or losing lock, the geographic coordinates and velocity of the base corresponding to the last valid fixed solution are used as initial values. The position update of the end-point three-dimensional geographic coordinates is maintained by inertial calculation using the acceleration and angular velocity output by the attitude sensor. In response to the duration of the inertial calculation exceeding a preset time threshold, the current end-point three-dimensional geographic coordinates are frozen and an accuracy degradation warning is output on the display terminal. The specific calculation process is as follows: starting from the moment of the last valid fixed solution, the pitch angle, roll angle and heading angle of the base are integrated frame by frame using the angular velocity output by the three-axis gyroscope in the attitude sensor to recursively deduce the real-time attitude of the base. The heading angle can no longer be given by the dual-antenna positioning and orientation device during the satellite signal interruption and can only be maintained by the angular velocity integration. Then, the specific force vector output by the three-axis accelerometer is projected from the sensor coordinate system to the northeast-northeast coordinate system using the recursively obtained attitude, and the components of local gravitational acceleration on each axis are subtracted to obtain the motion acceleration of the base in the northeast-northeast coordinate system. Let the geographic coordinates of the base at the time of the last valid fixed solution be... , , The velocity of the base at that moment is , , The three components of the above-mentioned acceleration in the northeast-northeast coordinate system are: , , The geographical coordinates of the current base obtained by inertial calculation are:

[0181]

[0182]

[0183] in, This represents the elapsed time since the last valid fixed solution. The preset time threshold is typically between 5 and 10 seconds. This bridging method also achieves the goal of continuous output of the terminal's three-dimensional geographic coordinates during short-term satellite signal interruptions.

[0184] In some embodiments, based on the terminal's three-dimensional geographic coordinates, the nearest design section is automatically matched from a pre-stored design section database. In response to the terminal's three-dimensional geographic coordinates being located between two adjacent design sections, linear interpolation is performed on the design profiles of the two adjacent design sections to generate an interpolated section. The terminal's three-dimensional geographic coordinates are then projected onto the interpolated section to calculate the vertical deviation from the design surface. The work guidance information is displayed on the display terminal using graded colors: in response to a vertical deviation greater than zero, the first color indicates that the design surface has not been reached; in response to a vertical deviation between zero and the over-excavation threshold, the second color indicates that the design surface has been reached; and in response to a vertical deviation exceeding the over-excavation threshold, the third color indicates over-excavation. The elevation of the interpolated section is obtained by linearly combining the elevations of corresponding points on the preceding and following sections.

[0185] in, To design the elevation of the interpolation section, It is the ratio of the distance from the horizontal position of the end point to the front section to the total distance between the front and rear sections. and These are the elevations of corresponding points on the front and rear sections, respectively. The vertical deviation is:

[0186] in, This is the elevation of the endpoint under the engineering elevation datum. Continuing from the previous example of the shipborne excavator's operating posture, the elevation component in the endpoint's three-dimensional geographic coordinates has already been calculated to be -4.396 meters. Assuming the conversion relationship between the engineering elevation datum and the geographic elevation datum is that the engineering elevation equals the geographic elevation minus 6.000 meters, then the value of the endpoint under the engineering elevation datum is... Meters. Assume the horizontal position of the end point lies between two adjacent cross-sections at chainage K0+100 meters and chainage K0+150 meters, with a distance of 30 meters from the preceding cross-section. Then... Let the design elevation at the corresponding horizontal position at the front section be -11.000 meters and the design elevation at the corresponding horizontal position at the rear section be -11.520 meters. These two negative values ​​correspond to the design elevation of the surface below sea level during underwater dredging operations in the waterway. Then, the interpolated section design elevation is... Meters (rounded to three decimal places), vertical deviation is The value in meters indicates that the end point is still 0.916 meters above the design surface, and the operator needs to continue digging down approximately 0.916 meters to reach the design surface. This display method also achieves the purpose of intuitively presenting the deviation of the end point relative to the design surface.

[0187] It should be understood that the sequence number of each step in the above embodiments does not imply the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of this application.

[0188] The above embodiments are only used to illustrate the technical solutions of this application, and are not intended to limit them. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of this application, and should all be included within the protection scope of this application.

Claims

1. A method for positioning the tip of an excavator based on a triangular chain-like solution, wherein the multi-link mechanism of the excavator includes multiple links connected sequentially through hinge points, characterized in that... Includes the following steps: S1. Obtain the fixed geometric side length parameters of each link in the multi-link mechanism and the reference pitch angle, wherein the reference pitch angle is the pitch angle of the base at the calibration time; S2. Real-time synchronous acquisition of the stroke length of the boom cylinder, the stroke length of the stick cylinder, and the stroke length of the bucket cylinder, as well as the pitch angle, roll angle, yaw angle, and geographical coordinates of the base; S3. Based on the fixed geometric side length parameters, the stroke length of the boom cylinder, the stroke length of the stick cylinder, and the stroke length of the bucket cylinder, the multi-link mechanism is decomposed into multiple triangles with shared sides. The cosine theorem is used to perform chain-like geometric calculations on each triangle with shared sides to determine the end distance and geometric structure angle. The end distance is the straight-line distance of the end relative to the root of the boom. Based on the end distance and the geometric structure angle, the two-dimensional coordinates of the end in the longitudinal plane of the machine body are generated. S4. Based on the difference between the pitch angle of the base and the reference pitch angle, the pitch increment correction is performed on the geometric structure angle. The end-point two-dimensional coordinates are regenerated based on the corrected geometric structure angle and the end-point distance. The regenerated end-point two-dimensional coordinates are reconstructed in three-dimensional space by combining the roll angle of the base and the heading angle. The end-point three-dimensional geographic coordinates are obtained by superimposing the geographic coordinates of the base.

2. The excavator shovel tip positioning method based on triangular chain solution according to claim 1, characterized in that, S1 includes the following sub-steps: S11. With the excavator stationary, use external measuring equipment to measure the three-dimensional spatial coordinates of each hinge point in the multi-link mechanism; S12. Based on the three-dimensional spatial coordinates of each hinge point, calculate the fixed geometric side length parameters of each link in reverse using the distance formula between two points in space; S13. Record the pitch angle of the base at the measurement moment as the reference pitch angle, and store the fixed geometric side length parameter and the reference pitch angle in the system configuration file.

3. The excavator shovel tip positioning method based on triangular chain solution according to claim 1, characterized in that, S3 includes the following sub-steps: S31. Map the stroke length of the boom cylinder, the stroke length of the stick cylinder, and the stroke length of the bucket cylinder to the corresponding geometric dynamic side lengths; S32. Using the dynamic geometric side lengths and the fixed geometric side length parameters as known quantities, the cosine theorem is applied sequentially to each triangle sharing a side to calculate the cosine value of the target angle in each triangle. The inverse cosine function is taken for each cosine value to determine the target angle in each triangle. At the hinge points shared by multiple triangles, the angle loop closure constraint where the sum of the angles at each hinge point equals the full angle is used to determine the remaining unknown angles. The determined angles or side lengths in the current triangle are used as known quantities input for the adjacent triangles. S33. After chain-like geometric calculation of all triangles, the end distance and the geometric structure angle are synthesized, and the end two-dimensional coordinates are generated based on the end distance and the geometric structure angle.

4. The excavator shovel tip positioning method based on triangular chain solution according to claim 3, characterized in that, In S32, after the cosine value is calculated using the cosine theorem at each step and before the inverse cosine function is taken, the cosine value is clamped to the closed interval [-1, 1]. In S33, the geometric structure angle is obtained by adding or subtracting the target angle, the remaining unknown angle, and the preset fixed reference angle from multiple hinge points in the chain geometry solution.

5. The excavator shovel tip positioning method based on triangular chain solution according to claim 1, characterized in that, The step S4, which involves incremental pitch correction of the geometric structure angle and regenerating the end-point two-dimensional coordinates, includes the following sub-steps: S41. Calculate the difference between the pitch angle of the base and the reference pitch angle as the pitch increment, and subtract the pitch increment from the geometric structure angle to obtain the corrected geometric structure angle; S42. Based on the end-point distance and the corrected geometric structure angle, the product of the end-point distance and the cosine of the corrected geometric structure angle is used as the first coordinate component in the longitudinal plane of the body, and the product of the end-point distance and the sine of the corrected geometric structure angle is used as the second coordinate component in the longitudinal plane of the body, and the two-dimensional coordinates of the end-point are regenerated.

6. The excavator shovel tip positioning method based on triangular chain solution according to claim 1, characterized in that, S4 further includes the following sub-steps: S43. The roll angle of the base is used to perform roll compensation on the regenerated end-body two-dimensional coordinates, expanding the end-body two-dimensional coordinates in the longitudinal plane of the body into three-dimensional body coordinates; S44. The three-dimensional body coordinates are offset and compensated according to the preset installation offset vector to obtain the relative coordinates of the end relative to the geographic coordinate reference point. The geographic coordinate reference point is the measurement reference point corresponding to the geographic coordinates of the base. The installation offset vector is the three-dimensional displacement of the geographic coordinate reference point relative to the boom root in the body coordinate system. S45. Using the heading angle, rotate the relative coordinates from the body coordinate system to the northeast-sky coordinate system to obtain the northeast-sky coordinate increment; S46. The incremental northeast celestial coordinates are superimposed onto the geographic coordinates of the base to obtain the absolute coordinates of the end in the projected coordinate system. The absolute coordinates are then converted into latitude, longitude and elevation in the geodetic coordinate system using projection back calculation to obtain the three-dimensional geographic coordinates of the end.

7. The excavator shovel tip positioning method based on triangular chain solution according to claim 1, characterized in that, In S2, The stroke length of each cylinder is measured in real time using displacement sensors installed on the boom cylinder, stick cylinder, and bucket cylinder respectively. The pitch angle and roll angle of the base are measured in real time using attitude sensors mounted on the base. The heading angle and the geographic coordinates of the base are measured using a dual-antenna positioning and orientation device mounted on the base.

8. The excavator shovel tip positioning method based on triangular chain solution according to claim 1, characterized in that, After the chain geometry solution in S3 is completed, the following fault diagnosis and data reconstruction operations are performed on the results of the chain geometry solution: The theoretical stroke lengths of the boom cylinder, stick cylinder, and bucket cylinder are calculated in reverse using the results of the chain geometric solution. The differences between each theoretical stroke length and the stroke lengths of the boom cylinder, stick cylinder, and bucket cylinder are used to form a consistent residual vector. In response to the fact that the magnitude of the consistency residual vector exceeds a preset consistency threshold, the cylinder corresponding to the component with the largest absolute value is determined as the faulty cylinder based on the distribution of the absolute values ​​of each component in the consistency residual vector. Based on the stroke lengths of the boom cylinder, the stick cylinder, and the bucket cylinder (excluding the stroke length of the faulty cylinder), and combined with the geometric constraints of the fixed geometric side length parameters, the theoretical stroke length of the faulty cylinder in the current posture is solved in reverse. The theoretical stroke length is then substituted into the chain-like geometric solution as a replacement value.

9. The excavator shovel tip positioning method based on triangular chain solution according to claim 8, characterized in that, The chain-like geometric solution of S3 uses angular loop closure constraints at the hinge points shared by multiple triangles. The angular loop closure constraint is a constraint that the sum of the angles at each hinge point is equal to the full circle. During the execution of the chain geometry solution in S3, the hinge points shared by multiple triangles are defined as redundant hinge points, where the circumferential angles of each hinge point are calculated by the cosine theorem of the triangle or by the fixed structural angle determined by the cosine theorem from the fixed geometric side length parameters, and the unknown angles do not need to be solved by the angle loop closure constraint. Calculate the difference between the sum of the circumferential angles at each redundant hinge point and the circumferential angle, and use it as the closure residual of each redundant hinge point; The error injection section is determined based on the distribution pattern of the closed residuals of each redundant hinge point along the chain geometric solution sequence, and the cylinder corresponding to the error injection section is cross-verified with the faulty cylinder.

10. The excavator shovel tip positioning method based on triangular chain solution according to claim 8, characterized in that, During the construction of the consistent residual vector across multiple consecutive frames, a sliding statistical window is maintained for the modulus of the consistent residual vector across multiple consecutive frames, and the mean and standard deviation of the modulus within the sliding statistical window are calculated. In response to the mean of the modulus continuously increasing at a preset rate and the standard deviation of the modulus remaining stable, it is determined to be a slow drift of the fixed geometric side length parameter, and an online micro-correction is performed on the fixed geometric side length parameter. The magnitude of the online micro-correction is limited by a preset upper limit for a single change. If the magnitude of the consistency residual vector of a single frame exceeds a preset mutation threshold, it is determined to be a sensor mutation fault, triggering the determination of the faulty cylinder and the inverse solution of the theoretical stroke length.

Images