Method, apparatus, and system for estimating the coordinates of a bucket tooth tip, excavator, and storage support

Abstract

This disclosure relates to a method, apparatus, and system for estimating the coordinates of the bucket tooth tip of an excavator and a storage rack. The method includes: establishing a kinematic model of an excavator based on the dimensions of a plurality of excavator components; obtaining measured values ​​of the excavator bucket tooth tip coordinates based on angles of the plurality of excavator components measured by excavator sensors with the kinematic model; obtaining system noise and measurement noise of the excavator bucket tooth tip coordinates; and determining estimated values ​​of the excavator bucket tooth tip coordinates based on the measured values ​​of the excavator bucket tooth tip coordinates, the system noise, and the measurement noise of the excavator bucket tooth tip coordinates.

Description

Title of the invention: Method, apparatus, and system for estimating coordinates of a bucket tooth tip, excavator, and storage medium REFERENCE TO RELATED APPLICATIONS

[0001] The present disclosure is based on, and claims priority from, Chinese application for invention No. CN202311169255.6, filed on September 12, 2023. Technical field

[0002] The present disclosure relates to the field of engineering machinery technology, in particular to a method, apparatus and system for estimating coordinates of a bucket tooth tip, an excavator and a storage medium. CONTEXT

[0003] The excavator is a multifunctional engineering machine widely used in work scenarios such as mining, water conservancy, transportation, and electrical engineering, etc. Unmanned excavators can replace excavator operators to operate and complete work in scenarios with risks of landslides, toxic and harmful gases, while alleviating labor shortages caused by the aging of modern society.In the work of an unmanned excavator, it is necessary to accurately estimate the three-dimensional coordinates of its bucket tooth tip in an excavator body coordinate system or other fixed coordinate systems, in order to determine parameters such as the distance and angle through which the bucket needs to move, and to provide input parameters and a basis for a decision-making module, planning module and other control functional modules of the unmanned excavator.With the increasing quality requirements of customers for excavation operations in solid waste treatment, trenching, slope grading, mining and other work operations, improving the measurement and calculation accuracy of the three-dimensional coordinates of the excavator bucket tooth tip can effectively improve the work accuracy and work quality of the excavator. ABSTRACT .

[0004] According to one aspect of the present disclosure, there is provided a method for estimating coordinates of a bucket tooth tip, comprising: establishing a kinematic model of an excavator according to dimensions of a plurality of components of the excavator, and obtaining measured values ​​of the coordinates of the bucket tooth tip of the excavator according to angles of the plurality of excavator components measured by excavator sensors with the kinematic model; obtaining system noise and measurement noise of the excavator bucket tooth tip coordinates; and determining estimated values ​​of the excavator bucket tooth tip coordinates based on the measured values ​​of the excavator bucket tooth tip coordinates, the system noise and the measurement noise of the excavator bucket tooth tip coordinates.

[0005] In some embodiments of the present disclosure, obtaining system noise and measurement noise in the excavator bucket tooth tip coordinates comprises: determining a measurement error covariance in the angle measurement process and determining a system noise covariance in the excavator coordinate calculation process.

[0006] In some embodiments of the present disclosure, determining the estimated values ​​of the excavator bucket tooth tip coordinates based on the measured values ​​of the excavator bucket tooth tip coordinates, system noise, and measurement noise of the excavator bucket tooth tip coordinates, comprises: determining the estimated values ​​of the excavator bucket tooth tip coordinates by using a predetermined filter to estimate the measured values ​​of the excavator bucket tooth tip coordinates, based on the measured values ​​of the excavator bucket tooth tip coordinates, system noise, and measurement noise of the excavator bucket tooth tip coordinates.

[0007] In some embodiments of the present disclosure, determining the estimated values ​​of the excavator bucket tooth tip coordinates by using a predetermined filter for estimating the measured values ​​of the excavator bucket tooth tip coordinates, based on the measured values ​​of the excavator bucket tooth tip coordinates, system noise, and measurement noise of the excavator bucket tooth tip coordinates, comprises: obtaining a predicted value of a state vector at time k+1 according to the state vector at time k, wherein the state vector is a state vector of a bucket tooth tip coordinate estimation system, and the state vector represents the excavator bucket tooth tip coordinates at time;obtaining the predicted value of an error covariance matrix at time k+1 according to an estimated value of the error covariance matrix at time k, wherein the error covariance matrix is ​​configured to represent an estimation accuracy of the state vector; and estimating the state vector and the error covariance matrix at time k+1 to obtain estimated values ​​of the state vector and the error covariance matrix at time k+1, according to the values; predicted values ​​of the state vector and the error covariance matrix at time k+1, and the measured value output by the system at time k+1.

[0008] In some embodiments of the present disclosure, determining the estimated values ​​of the excavator bucket tooth tip coordinates by using a predetermined filter to estimate the measured values ​​of the excavator bucket tooth tip coordinates, based on the measured values ​​of the excavator bucket tooth tip coordinates, the system noise, and the measurement noise of the excavator bucket tooth tip coordinates, further comprises: assigning the estimated values ​​of the state vector and the error covariance matrix at time k+1 to the step of obtaining a predicted value of the state vector at time k+1 according to the state vector at time k and to the step of obtaining the predicted value of the error covariance matrix at time k+1 according to an estimated value of the error covariance matrix at time k,to iteratively predict values ​​of the state vector and error covariance matrix at a subsequent time. ,

[0009] In some embodiments of the present disclosure, obtaining a predicted value of the state vector at time k+1 according to the state vector at time k, comprises: defining the state vector; establishing a dynamic model of the bucket tooth tip coordinate estimation system, wherein the dynamic model is a state transition matrix configured to represent a transformation relationship between the state vector at time k and the state vector at time k+1; and obtaining the predicted value of the state vector at time k+1 according to the state vector at time k and the state transition matrix.

[0010] In some embodiments of the present disclosure, obtaining the predicted value of the error covariance matrix at time k+1 according to the estimated value of the error covariance matrix at time k, comprises: defining a covariance matrix of a system process noise; and obtaining the predicted value of the error covariance matrix at time k+1 according to the estimated value of the error covariance matrix at time k, the state transition matrix, and the covariance matrix of the system process noise.

[0011] In some embodiments of the present disclosure, estimating the state vector and error covariance matrix at time k+1 to obtain estimated values ​​of the state vector and error covariance matrix at time k+1, based on predicted values ​​of the state vector and error covariance matrix at time k+1 and measured values ​​output by the system at time k+1, comprises: determining a filter gain value at time k+1 based on the predicted value of the error covariance matrix at time k+1, a system measurement matrix of the bucket tooth tip coordinates, and a matrix of measurement noise covariance; estimating the state vector at time k+1 to obtain the estimated value of the state vector at time k+1, based on the predicted value of the state vector at time k+1, the filter gain value at time k+1 and the measured values ​​of the system output at time k+1; and estimating the error covariance matrix at time k+1 to obtain the estimated value of the error covariance matrix at time k+1, based on the predicted value of the error covariance matrix at time k+1 and the filter gain value at time k+1.

[0012] In some embodiments of the present disclosure, determining the filter gain value at time k+1 based on the predicted value of the error covariance matrix at time k+1, the system measurement matrix of the bucket tooth tip coordinates, and the measurement noise covariance matrix, comprises: determining an estimate variance based on the predicted value of the error covariance matrix at time k+1 and the system measurement matrix of the bucket tooth tip coordinates; determining a total variance based on the estimate variance and the measurement noise covariance matrix; and determining the filter gain value at time k+1 based on a ratio of the estimate variance to the total variance.

[0013] In some embodiments of the present disclosure, estimating the state vector at time k+1 to obtain the estimated value of the state vector at time k+1, based on the predicted value of the state vector at time k+1, the filter gain value at time k+1, and the measured values ​​of the system output at time k+1, comprises: defining a measurement vector of the bucket tooth tip coordinates; establishing a measurement model of the bucket tooth tip coordinate estimation system, wherein the measurement model is used to map the state vector to the measurement vector; determining a measurement vector value at time k+1 based on the predicted value of the state vector at time k+1 and the system measurement matrix of the bucket tooth tip coordinates;and estimating the state vector at time k+1 to obtain the estimated value of the state vector at time k+1, based on the measurement vector value at time k+1, the predicted value of the state vector at time k+1, the filter gain value at time k+1, and the measured values ​​of the system output at time k+1. ;

[0014] In some embodiments of the present disclosure, estimating the error covariance matrix at time k+1 to obtain the estimated value of the error covariance matrix at time k+1, according to the predicted value of the error covariance matrix at time k+1 and the filter gain value at time k+1, comprises: estimating the error covariance matrix at time k+1 to obtain the estimated value of the error covariance matrix at time k+1, in function of the predicted value of the error covariance matrix at time k+1, the system measurement matrix of the bucket tooth tip coordinates, and the filter gain value at time k+1.

[0015] In some embodiments of the present disclosure, establishing the kinematic model of the excavator according to the dimensions of the plurality of components of the excavator, comprises: establishing five coordinate systems at different components of the excavator, wherein the origins of the five coordinate systems are respectively: an intersection of a vertical axis about which a rotating platform of the excavator rotates and the ground, a connecting joint between a boom and the rotating platform of the excavator, a connecting joint between the boom and an arm of the excavator, a joint between the arm and a bucket of the excavator, and the bucket tooth tip, wherein a coordinate system taking the intersection of the vertical axis about which the rotating platform of the excavator rotates and the ground as an origin is an excavator coordinate system.

[0016] According to another aspect of the present disclosure, there is provided a bucket tooth tip coordinate estimation apparatus, comprising: a coordinate measurement unit configured to establish a kinematic model of an excavator according to dimensions of a plurality of components of the excavator, and obtain measured values ​​of the coordinates of the bucket tooth tip of the excavator according to angles of the plurality of components of the excavator measured by excavator sensors with the kinematic model; a noise acquisition unit configured to obtain system noise and measurement noise of the coordinates of the bucket tooth tip of the excavator;and a coordinate estimation unit configured to determine estimated values ​​of the coordinates of the excavator bucket tooth tip based on the measured values ​​of the coordinates of the excavator bucket tooth tip, the system noise, and the measurement noise of the coordinates of the excavator bucket tooth tip.;

[0017] According to yet another aspect of the present disclosure, there is provided an apparatus for estimating bucket tooth tip coordinates, comprising: a memory configured to store computer instructions; and a processor configured to execute the instructions, such that the bucket tooth tip coordinate apparatus performs the method as described in any of the above embodiments.

[0018] According to another aspect of the present disclosure, there is provided a system for estimating coordinates of a bucket tooth tip, comprising dynamic excavator sensors and the apparatus as described in any of the above embodiments.

[0019] In some embodiments of the present disclosure, the excavator dynamic sensors include at least one of a rotary encoder for measuring a rotation angle of a rotating platform of the excavator, a boom inclinometer for measuring a boom angle, an arm inclinometer for measuring an arm angle, and a bucket inclinometer for measuring a bucket angle.

[0020] According to another aspect of the present disclosure, there is provided an excavator, comprising the system as described in any of the above embodiments.

[0021] According to yet another aspect of the present disclosure, there is provided a computer-readable storage medium having stored thereon computer instructions which, when executed by a processor, perform the method as described in any of the above embodiments.

[0022] According to yet another aspect of the present disclosure, there is provided a computer program, comprising: instructions which, when executed by a processor, cause the processor to perform the method as described in any one of the above embodiments. Brief description of the drawings

[0023] In order to more clearly explain the embodiments of the present invention or the technical solutions of the prior art, a brief introduction will be given below of the drawings to be used in describing the embodiments or the prior art. It is obvious that the drawings illustrated below are only some embodiments of the present disclosure. Those skilled in the art may also acquire other drawings based on these drawings, provided that no inventive effort is involved.

[0024] [Fig.l] is a schematic diagram of an excavator according to certain embodiments of the present disclosure.

[0025] [Fig.2] is a schematic diagram of an excavator according to other embodiments of the present disclosure.

[0026] [Fig. 3] is a schematic diagram of a method for estimating coordinates of a bucket tooth tip according to certain embodiments of the present disclosure.

[0027] [Fig.4] is a schematic diagram of a method for estimating bucket tooth tip coordinates according to other embodiments of the present disclosure.

[0028] [Fig.5] is a schematic diagram of a bucket tooth tip coordinate estimation apparatus according to certain embodiments of the present disclosure.

[0029] [Fig.6] is a structural diagram of a coordinate estimation unit according to certain embodiments of the present disclosure.

[0030] [Fig.7] is a schematic structural diagram of a bucket tooth tip coordinate estimation apparatus according to other embodiments of the present disclosure. DETAILED DESCRIPTION

[0031] Below, a clear and complete description will be made of the technical solution of embodiments of the present disclosure with reference to the figures of the embodiments. Obviously, only certain embodiments of the present disclosure, rather than all embodiments thereof, are given here. The following description of at least one exemplary embodiment is in fact merely illustrative and is in no way intended to limit the invention, its application or its use. All other embodiments acquired by those skilled in the art based on the embodiments of the present disclosure without creative efforts shall fall within the scope of protection of the present disclosure.

[0032] Unless otherwise indicated, the relative arrangement, numerical expressions and values ​​of the components and steps set forth in these examples do not limit the scope of the invention.

[0033] At the same time, it should be understood that, for ease of description, the dimensions of the various parts shown in the drawings are not drawn in actual proportions.

[0034] Techniques, methods and apparatus known to those skilled in the art of interest may not be described in detail, but if so, such techniques, methods and apparatus should be considered part of the disclosure.

[0035] In all examples presented and discussed herein, any specific value should be interpreted as merely illustrative and not as limiting. Thus, other exemplary embodiments may have different values.

[0036] Note that similar reference numerals and reference letters are designated analogously in the accompanying drawings and, therefore, once an element is defined in a drawing, it is not necessary to describe it further in the accompanying drawings.

[0037] The inventor discovered through research that various internal and external factors, such as fluctuations in the hydraulic cylinders of the boom and bucket, can cause variations in angles, such as the arm angle and the boom angle, during the automatic work process of an unmanned excavator. These fluctuations can cause system noise during the measurement process of the three-dimensional coordinates of the excavator bucket tooth tip. Inclinometers, rotary encoders, and other sensors are used to continuously measure angles such as the arm angle and boom angle at constant intervals, and the measurement noise is included in the angle measurement values ​​of the excavator components, resulting in measurement noise during the process of measuring the three-dimensional coordinates of the bucket tooth tip. These two types of noise can increase the detection error of the three-dimensional coordinates of the excavator bucket tooth tip, reduce the accuracy and quality of the excavator's work, and thus affect the work effect.

[0038] The inventor also found through research that the relevant technology cannot effectively deal with system noise and measurement noise during the process of measuring the three-dimensional coordinates of the bucket tooth tip of the excavator, resulting in the inability of the three-dimensional coordinates of the bucket tooth tip of the excavator to meet the requirements of digging, sloping and other high-precision work scenarios.

[0039] In view of at least one of the above technical problems, the present disclosure provides a bucket tooth tip coordinate estimation method, apparatus and system, an excavator and a storage medium, which can improve the measurement accuracy of the three-dimensional coordinates of the bucket tooth tip and improve the work quality of the excavator. The present disclosure will be described in detail below in association with specific embodiments.

[0040] [Fig. 1] is a schematic diagram of an excavator according to some embodiments of the present disclosure. [Fig. 2] is a schematic diagram of an excavator according to other embodiments of the present disclosure. As shown in Figs. 1 and 2, the excavator of the present disclosure is composed of four parts: a bucket tooth tip coordinate estimation system 1, a rotating platform 2, a traveling device 3, and a working device 4. The working device is mainly composed of three components: a boom, an arm, and a bucket, which are assembled together using other auxiliary connecting rods.The excavator includes a bucket, which is a component directly involved in excavation and development tasks; the body of the hydraulic excavator is mainly composed of the rotating platform, on which power devices, transmission systems and the operating cabin of the excavator are carried or mounted. The rotating platform is connected to the traveling device by a rotating device, and can rotate in a circular motion according to the needs of the excavation task, and the device . work platform rotates accordingly. The traveling device of the fully hydraulic excavator is driven by hydraulic motors to perform actions such as traveling, moving, transferring, etc. During the operation of the hydraulic excavator, the traveling device and the rotating platform are driven by a traveling motor and a rotating motor, respectively; the components of the working device are driven by corresponding hydraulic cylinders.

[0041] The bucket tooth tip coordinate estimation system 1 is used for establishing a kinematic model of an excavator based on dimensions of a plurality of components of the excavator, and obtaining measured values ​​of the bucket tooth tip coordinates of the excavator based on angles of the plurality of components of the excavator measured by excavator sensors with the kinematic model; obtaining system noise and measurement noise of the bucket tooth tip coordinates of the excavator; and determining estimated values ​​of the bucket tooth tip coordinates of the excavator based on the measured values ​​of the bucket tooth tip coordinates of the excavator, the system noise, and the measurement noise of the bucket tooth tip coordinates of the excavator.

[0042] The present disclosure can improve the measurement accuracy of the three-dimensional coordinates of the bucket tooth tip and improve the working quality of the excavator.

[0043] The bucket tooth tip coordinate estimation method and system of the present disclosure will be described below with specific embodiments.

[0044] [Fig. 3] is a schematic diagram of a method according to certain embodiments of the present disclosure. The embodiment of [Fig. 3] may be performed by the bucket tooth tip coordinate estimation apparatus or system of the present disclosure, or by the excavator of the present disclosure. As shown in [Fig. 3], the method of the embodiment of [Fig. 3] may comprise at least one of steps 100 to 300.

[0045] In step 100, a kinematic model of an excavator is established based on dimensions of a plurality of components of the excavator, and measured values ​​of the coordinates of the bucket tooth tip of the excavator are obtained based on angles of the plurality of components of the excavator measured by excavator sensors with the kinematic model.

[0046] In some embodiments of the present disclosure, the excavator comprises four dynamic sensors, namely a rotary encoder for measuring a rotation angle of a rotating platform of the excavator, a boom inclinometer to measure a boom angle, an arm inclinometer to measure a boom angle and a bucket inclinometer to measure a bucket angle.

[0047] In some embodiments of the present disclosure, step 100 may comprise: establishing five coordinate systems at different components of the excavator, the origins of the five coordinate systems being respectively: an intersection of a vertical axis about which a rotating platform of the excavator rotates and the ground, a connecting joint between a boom and the rotating platform of the excavator, a connecting joint between the boom and an arm of the excavator, a joint between the arm and a bucket of the excavator, and the bucket tooth tip, wherein a coordinate system taking the intersection of the vertical axis about which the rotating platform of the excavator rotates and the ground as an origin is an excavator coordinate system.

[0048] [Fig. 2] also shows a schematic diagram of an excavator model and its coordinate systems according to some embodiments of the present disclosure. As shown in [Fig. 2], step 100 may comprise: establishing five coordinate systems at different components of the excavator, the origins of the five coordinate systems respectively being: an intersection of a vertical axis Z0 around which a rotating platform of the excavator rotates and the ground, a connecting joint between a boom and the rotating platform of the excavator, a connecting joint between the boom and an arm of the excavator, a joint between the arm and a bucket of the excavator, and the bucket tooth tip, the intersection of the vertical axis Z0 around which the rotating platform of the excavator rotates and the ground being the origin of an excavator coordinate system;establishing a kinematic model of an excavator based on dimensions of a plurality of components of the excavator; and obtaining measured values ​​of the coordinates of the bucket tooth tip of the excavator based on angles of the plurality of components of the excavator measured by excavator sensors with the kinematic model, the measured values ​​of the coordinates of the bucket tooth tip of the excavator being the coordinates of the bucket tooth tip of the excavator calculated based on angles of the plurality of components of the excavator measured by excavator sensors with the kinematic model. ;

[0049] In some embodiments of the present disclosure, as shown in Figure 2, step 100 may comprise: defining five sets of generalized coordinates Qj ([ = 0,1, 2,3, 4) to describe the relative rotation between two adjacent rigid bodies, according to the excavator model and its corresponding coordinate systems Oi-xiyizi (i=0,1,2,3,4). In the present disclosure, a system of coordinates is fixed on each connecting rod of the excavator, then the spatial relationship between two adjacent connecting rods is described using a 4x4 homogeneous transformation matrix. By sequential transformation, the posture of an end effector with respect to the base coordinate system can be derived, thus establishing a kinematic equation of the excavator.

[0050] In some embodiments of the present disclosure, as shown in [Fig.2], the excavator coordinate system is OO-xOyOzO, the xOyO plane overlaps the ground, and the excavator rotating platform rotates about the Z0 axis. 0102 is an equivalent boom, 0203 is an equivalent arm, 0304 is an equivalent bucket building surface, and 04 is the bucket tooth tip.

[0051] In some embodiments of the present disclosure, step 100 may include: determining a relative displacement between the bucket tooth tip and the boom pivot in the vehicle body coordinate system according to a boom tilt angle, an arm tilt angle, and a bucket tilt angle of the excavator, as well as a boom length, an arm length, and a bucket length of the excavator; determining a real-time position of the bucket tooth tip in the vehicle body coordinate system according to the relative displacement between the bucket tooth tip and the boom pivot in the vehicle body coordinate system, as well as a real-time position of the boom pivot in the vehicle body coordinate system.

[0052] In some embodiments of the present disclosure, step 100 may include: determining a coordinate transformation matrix between the vehicle body coordinate system and the world coordinate system based on vehicle posture information of the excavator; determining a relative displacement between the bucket tooth tip and the boom pivot in the vehicle body coordinate system based on a boom tilt angle, an arm tilt angle, and a bucket tilt angle of the excavator, and a boom length, an arm length, and a bucket length of the excavator;determining a relative displacement between the bucket tooth tip and the boom pivot in the world coordinate system, based on the relative displacement between the bucket tooth tip and the boom pivot in the vehicle body coordinate system and the coordinate transformation matrix; determining a real-time position of the bucket tooth tip in the world coordinate system based on the relative displacement between the bucket tooth tip and the boom pivot in the world coordinate system, as well as a real-time position of the boom pivot in the world coordinate system. In the present disclosure, the vehicle body coordinate system of the excavator is located in the world coordinate system; to obtain the three-dimensional (3D) coordinate values ​​of the bucket tooth tip in the vehicle body coordinate system OO-xOyOzO of the excavator by measurement.

[0053] In step 200, a system noise and a measurement noise of the coordinates of the bucket tooth tip of the excavator are obtained.

[0054] In some embodiments of the present disclosure, step 200 may comprise: determining a measurement error covariance during the angle measurement process and determining a system noise covariance during the excavator coordinate calculation process.

[0055] In some embodiments of the present disclosure, step 200 may comprise: analyzing the characteristics of the excavator body and sensors to obtain the system noise and the measurement noise in the 3D coordinates of the excavator bucket tooth tip.

[0056] In step 300, estimated values ​​of the excavator bucket tooth tip coordinates are determined based on the measured values ​​of the excavator bucket tooth tip coordinates, the system noise, and the measurement noise of the excavator bucket tooth tip coordinates.

[0057] In some embodiments of the present disclosure, step 300 may comprise: determining the estimated values ​​of the excavator bucket tooth tip coordinates by using a predetermined filter to estimate the measured values ​​of the excavator bucket tooth tip coordinates, based on the measured values ​​of the excavator bucket tooth tip coordinates, the system noise, and the measurement noise of the excavator bucket tooth tip coordinates.

[0058] In some embodiments of the present disclosure, the predetermined filter may be a Kalman filter.

[0059] In some embodiments of the present disclosure, step 300 may comprise at least one of steps 310 to 340.

[0060] In step 310, a predicted value of a state vector at time k+1 is obtained based on the state vector at time k, wherein the state vector is a state vector of a bucket tooth tip coordinate estimation system, and the state vector represents the coordinates of the bucket tooth tip of the excavator.

[0061] In some embodiments of the present disclosure, step 310 may comprise at least one of steps 311 to 313.

[0062] In step 311, the state vector is defined.

[0063] In some embodiments of the present disclosure, step 311 may comprise: defining the state vector W(k)=[x(k),y(k),z(k)] of the system for estimate 3D coordinates of the bucket tooth tip, which include 3D coordinates of the excavator bucket tooth tip on the x-axis, y-axis, and z-axis, respectively. W(k) is a 3x1 vector.

[0064] In step 312, a dynamic model of the bucket tooth tip coordinate estimation system is established, wherein the dynamic model is a state transition matrix configured to represent a transformation relationship between the state vector at time k and the state vector at time k+1.

[0065] In some embodiments of the present disclosure, step 312 may comprise: establishing a dynamic model of the system to estimate the coordinates of the bucket tooth tip as shown in formula (1).

[0066] [Math.l] W(k + 1) = AW(k) (1)

[0067] The model of formula (1) describes the transformation relationship between the state vector at time k and the state vector at time k+1.

[0068] A in formula (1) represents a state transition matrix, which is the system dynamic model.

[0069] In some embodiments of the present disclosure, the state transition matrix A for a Kalman filter is as follows.

[0070] [Math.2] 10 0' 0 1.

[0071] In step 313, the predicted value of the state vector at time k+1 is obtained as a function of the state vector at time k and the state transition matrix.

[0072] In some embodiments of the present disclosure, step 313 may include: inputting the state vector and the state transition matrix at time k into the dynamic model of the bucket tooth tip coordinate estimation system (e.g., inputting into formula (1)), and obtaining predicted values ​​of the state vector at time k+1.

[0073] In the excavator coordinate estimation of the above embodiment, it is necessary to refer to the values ​​at a previous time point. Here, the use of the identity matrix is ​​a common way to introduce the values ​​at the previous time point into the Kalman filter. Formula (1) is a prediction formula for state variables, indicating that the values ​​at time k+1 depend on the values ​​at time k.

[0074] In some embodiments of the present disclosure, a difference between the predicted value at time k+1 obtained using formula (1) and the measured value actual value at time k+1 is calculated, then used in conjunction with a Kalman gain to obtain the estimated variable value at time k+1 using formula (4).

[0075] In step 320, the predicted value of an error covariance matrix at time k + 1 is obtained based on an estimated value of the error covariance matrix at time k, wherein the error covariance matrix is ​​configured to represent the estimation accuracy of the state vector.

[0076] In some embodiments of the present disclosure, step 320 may comprise at least one of steps 321 to 322.

[0077] In step 321, a covariance matrix Q of the system process noise is defined.

[0078] In some embodiments of the present disclosure, the equation for the state transition from the estimated values ​​W(k) of the 3D coordinates at time k to the estimated values ​​W(k+1) of the 3D coordinates at time k+1 is shown in formula (2-1).

[0079] [Math.3] W(k+1) = AW(k) + w (2-1)

[0080] In formula (2-1), w is the process noise.

[0081] In some embodiments of the present disclosure, Q represents a covariance matrix of system process noise, which is white Gaussian noise. Q is a 3x3 matrix.

[0082] In some embodiments of the present disclosure, the values ​​of Q are the following.

[0083] [Math.4] 5 0 0 ■ 0 10.

[0084] In some embodiments of the present disclosure, the covariance matrix Q of the system process noise has initial values ​​selected according to practice. The fluctuation ranges on the x, y, and z axes when adjusting the excavator are approximately 5 centimeters, 5 centimeters, and 10 centimeters, respectively. The covariance matrix Q of the system process noise will be adjusted in a subsequent adjustment process according to the filtering effect.

[0085] In some embodiments of the present disclosure, the covariance matrix Q of the system process noise is calculated according to formula (2-2).

[0086] [Math.5] Q = cov(w) = E{wwT] (2-2)

[0087] In formula (2-2), E{«] represents the expected value.

[0088] In step 322, the predicted value of the error covariance matrix at time k+1 is obtained based on the estimated value of the error covariance matrix at time k, the state transition matrix, and the system process noise covariance matrix.

[0089] In some embodiments of the present disclosure, step 322 may comprise: obtaining the predicted values ​​of the error covariance matrix at time k+1 according to formula (2).

[0090] [Math.6] P(k+1) = AP(k)Ar + Q (2)

[0091] In formula (2), P(k) is the error covariance at time k. p(k+ 1) is the predicted values ​​of the error covariance at time k+1; P(k+ 1) is the predicted values ​​of the error covariance at time k+1 calculated based on the error covariance at time k, which is a 3x3 matrix.

[0092] In some embodiments of the present disclosure, the state transition matrix A in formula (2) of the Kalman filter system is used to determine the predicted value of the system noise.

[0093] In step 330, the state vector and the error covariance matrix at time k+1 are estimated to obtain estimated values ​​of the state vector and the error covariance matrix at time k+1, based on the predicted values ​​of the state vector and the error covariance matrix at time k+1 and the measured values ​​output by the system at time k+1.

[0094] In some embodiments of the present disclosure, step 330 may comprise at least one of steps 331 to 333.

[0095] In step 331, a filter gain value at time k+1 is determined based on the predicted value of the error covariance matrix at time k+1, a system measurement matrix of the bucket tooth tip coordinates, and a measurement noise covariance matrix.

[0096] In some embodiments of the present disclosure, step 331 may comprise at least one of steps 3311 to 3315.

[0097] In step 3311, a measurement vector of the coordinates of the bucket tooth tip is defined.

[0098] In some embodiments of the present disclosure, step 3311 may comprise: defining a measurement vector S(k)=[x(k),y(k),z(k)] for the bucket tooth tip coordinate estimation system. As a state vector, the measurement vector in the present disclosure is also configured to represent the bucket tooth tip coordinates of the excavator. S(k) represents the coordinate values ​​of the excavator bucket tooth tip calculated based on the sensor measurement values.

[0099] In some embodiments of the present disclosure, S(k+l)=[x(k+l),Y(k + l),z(k+l)], indicating that the measured values ​​of the 3D coordinates of the excavator bucket tooth tip at time k+1 are represented by a 3x1 vector.

[0100] In step 3312, a measurement model of the bucket tooth tip coordinate estimation system is established, the measurement model being used to map the state vector to the measurement vector.

[0101] In some embodiments of the present disclosure, step 3312 may include: establishing a measurement model of the bucket tooth tip coordinate estimation system to determine how to map the system state vector to the measurement vector using, for example, formula (3-1). In the present disclosure, the state vector is the same as the measurement vector. In addition, measurement noise must also be considered in this equation. Y(k+1) represents the values ​​of the measurement vector calculated based on the predicted values ​​of the bucket tooth tip coordinates at time k+1.

[0102] [Math.7] Y(k + 1) = HW(k+l) (3-1)

[0103] In formula (3-1), H is the system measurement matrix of the 3D coordinates of the excavator bucket tooth tip, which is a 3x3 matrix.

[0104] In certain embodiments of the present disclosure, the values ​​of H are as follows.

[0105] [Math.8] H= 0 1 .0 0 0' 0 1.

[0106] In the present disclosure, H is an identity matrix, indicating that the values ​​to be estimated are the same as the values ​​of the state variables. This matrix H is mainly used for related calculations in formulas (3), (4) and (5) in a Kalman filtering system. The reserved identity matrix H is also here a preparation for future adjustment and optimization. If the variables to be measured are certain variables based on the state variables, an appropriate change or adjustment can be made to the matrix H accordingly.

[0107] In step 3313, an estimation variance is determined based on the predicted values ​​of the error covariance matrix at time k+1 and the system measurement matrix of the bucket tooth tip coordinates.

[0108] In some embodiments of the present disclosure, step 3313 may comprise: determining an estimation variance HP(k+ 1)HT based on predicted values ​​p(k+ 1) of the error covariance matrix at time k+1, the system measurement matrix H of the bucket tooth tip coordinates, and a transpose matrix HT of the system measurement matrix.

[0109] In step 3314, a total variance is determined based on the estimation variance and the measurement noise covariance matrix.

[0110] In some embodiments of the present disclosure, step 3314 may comprise: determining a total variance Hp^-i- ])HT+R according to the estimation variance and a covariance matrix R of the measurement noise.

[0111] In some embodiments of the present disclosure, R is a measurement noise covariance matrix, which is also white Gaussian noise. R is a 3x3 matrix, and the values ​​of R in this embodiment are as follows.

[0112] [Math.9] LO 0 5J

[0113] R has initial values ​​selected according to practice. The fluctuation ranges of the measurement noise on the x, y and z axes are 1 cm, 1 cm and 5 cm respectively. The covariance matrix R of the measurement noise can be adjusted in a subsequent fitting process.

[0114] In some embodiments of the present disclosure, the equations for a state transition from the estimated 3D coordinate values ​​at time k to the measured 3D coordinate values ​​at time k+1 are shown in formulas (2-1) and (3-2).

[0115] [Math. 10] W(k+l) = AW(k) + w (2-1)

[0116] [Math. 11] S(k+l) = HW(k+l) + v (3-2)

[0117] In formulas (2-1) and (3-2), w represents the process noise and v represents the measurement noise.

[0118] In some embodiments of the present disclosure, the covariance matrix R of the measurement noise may be determined according to formula (3-3), wherein E{«] represents the expected value.

[0119] [Math. 12] R “ cov(v) = E{vvT} (3-3)

[0120] In step 3315, the filter gain values ​​at time k+1 are determined based on a ratio of the estimation variance to the total variance.

[0121] In some embodiments of the present disclosure, K(k+1) is a Kalman gain matrix at time k+1, which is a 3x3 matrix.

[0122] In some embodiments of the present disclosure, step 3315 may comprise: determining the filter gain value K(k+1) at time k+1 according to formula (3).

[0123] [Math. 13] K(k+ 1)= H(k + l)HT(HP(k+ 1)Hr + R) ' (3)

[0124] In some embodiments of the present disclosure, the smaller the process noise value Q, the greater our confidence in the predicted value of the model and the faster the system converges. In contrast, the opposite is true. The larger the measurement noise value R, the lower the confidence in the measurement value, too large a value will cause the system to respond slowly, and too small a value may cause the system to oscillate. For the process noise value Q and the measurement noise value R, when adjusting the parameters, one of these parameters can be fixed to adjust the other, for example, the value of Q can be adjusted from a small value to a large value to ensure that the system convergence speed is normal, and the value of R can be adjusted from a large value to a small value to make the output result close to the actual value.The range of values ​​is the difference between the upper and lower limits of the 3D coordinate values ​​measured in a period of time.

[0125] In some embodiments of the present disclosure, for the initial values ​​of W and P, their values ​​determine the initial convergence rate and are generally set to the same order of magnitude or a smaller number than the ideal values ​​to achieve faster convergence. As the iteration proceeds, the value of P will converge to a minimum estimate covariance matrix

[0126] In step 332, the state vector at time k+1 is estimated to obtain the estimated value of the state vector at time k+1, based on the predicted value of the state vector at time k+1, the filter gain value at time k+1 and the measured values ​​of the system output at time k+1.

[0127] In some embodiments of the present disclosure, step 332 may comprise at least one of steps 3321 to 3323.

[0128] In step 3321, a measurement vector value Y(k+1) at time k+1 is determined based on the predicted value jp(£+1) of the state vector at time k+1 and the system measurement matrix of the bucket tooth tip coordinates.

[0129] In some embodiments of the present disclosure, step 3321 may comprise: determining the measurement vector value Y(k+1) at time k+1 according to formula (3-1).

[0130] In step 3322, the state vector at time k+1 is estimated to obtain the estimated value W(k+ 1 ) of the state vector at time k+1, as a function of the measurement vector value Y(k+1) at time k+1, the predicted value W(k+ 1) of the state vector at time k+1, the filter gain value K( k + 1) at time k+1 and the measured value S ( k + 1 ) of the system output at time k+1.

[0131] In some embodiments of the present disclosure, step 3321 may comprise: determining a predicted measurement noise value according to the difference between the measured value S(k+ 1) output by the system at time k+1 and the measurement vector values ​​Y(k+1) at time k+1; determining the estimated measurement noise value according to the product of the predicted measurement noise value and the filter gain values ​​K(k+ 1) at time k+1; obtaining the estimated value W(k + 1) of the state vector at time k+1 according to the estimated measurement noise value and the predicted values ​​W(k + 1) of the state vector at time k+1.

[0132] In some embodiments of the present disclosure, step 3322 may comprise: determining the measurement vector value Y(k+1) at time k+1 according to formula (4).

[0133] [Math. 14] W(k+1) =W(k+1)+K(k+1) (S(k+l)-HW(k +1) ) (4)

[0134] In step 333, the error covariance matrix at time k+1 is estimated to obtain the estimated value of the error covariance matrix at time k+1, based on the predicted value of the error covariance matrix at time k+1 and the filter gain value at time k+1.

[0135] In some embodiments of the present disclosure, step 333 may comprise: estimating the error covariance matrix at time k+1 to obtain the estimated value of the error covariance matrix at time k+1, based on the predicted value P( k + 1 ) of the error covariance matrix at time k + 1, of the system measurement matrix H of the bucket tooth tip coordinates, of an identity matrix I and of the filter gain value K( k + 1 ) at time k+1.

[0136] In some embodiments of the present disclosure, step 333 may comprise: determining a first matrix K(k+1)H according to the product of the system measurement matrix H of the bucket tooth tip coordinates and the filter gain value K(k+1) at time k+1; determining a difference matrix according to the difference between the identity matrix I and the first matrix; and determining the estimated value P(k+1) of the error covariance matrix at time k+1 according to the product of the difference matrix and the predicted value of the error covariance matrix at time k+1.

[0137] In some embodiments of the present disclosure, step 333 may comprise: determining the estimated value P(k + 1) of the error covariance matrix at time k+1 according to formula (5).

[0138] [Math. 15] P(k+l)=(l- K(k+l)H)P(k+l) (5)

[0139] In step 340, the estimated value W(k + 1 ) of the state vector and the estimated value P( k + 1 ) of the error covariance matrix at time k+1 are input to step 310 of obtaining a predicted value of the state vector at time k+1 according to the state vector at time k and to step 320 of obtaining the predicted value of the error covariance matrix at time k+1 according to an estimated value of the error covariance matrix at time k, to iteratively predict values ​​of the state vector and the error covariance matrix at a subsequent time.

[0140] The implementation of estimating the 3D coordinate system parameters of the excavator bucket tooth tip using a Kalman filter in the above embodiment mainly consists of two steps: a prediction step and an update step. The prediction step comprises at least one of formulas (1) and (2); and the update step comprises at least one of formulas (3), (4) and (5).

[0141] From formulas (1) to (5), it can be seen that the five formulas used in a Kalman filtering module for estimating 3D coordinates of the excavator bucket tooth tip can be divided into a prediction group (Formulas (1) and (2)) and an update group (Formulas (3), (4) and (5)). The prediction group is always used to predict a current state based on a previous state, and to calculate the predicted value of the 3D coordinates of the bucket tooth tip at the current time. The update group is used to modify the predicted information based on the observation information to obtain an optimal estimation. In the above equations, W(k+1) is the optimal estimated value of the excavator bucket tooth tip coordinates output by the Kalman filter.

[0142] The inventor also discovered through research that the fluctuation of hydraulic cylinders such as the boom cylinder and the arm cylinder mounted on the excavator will inevitably cause variations in angles such as the boom angle and the arm angle. Therefore, the 3D coordinates of the bucket tooth tip obtained based on data such as the angles and lengths of the boom and the arm will include system noise during the measurement process. Inclinometers, rotational encoders, and other sensors are used to continuously measure angles, such as the boom and arm angles, at constant intervals. The measurement errors of the inclinometers or rotational encoders may cause measurement noise in the measured angle values ​​of the excavator components, resulting in the measurement noise during the measurement process of the 3D coordinates of the bucket tooth tip.These factors cause noise in the 3D coordinate calculation result of the excavator bucket tooth tip in the excavator vehicle body coordinate system, resulting in large fluctuations and errors in the measurement and calculation of the 3D coordinate values ​​of the tooth tip.

[0143] In order to reduce the impact of system noise and measurement noise on the accuracy of excavator bucket tooth tip coordinate measurement, firstly, a state space model of the 3D coordinate measurement variables is established. By Kalman filtering, the estimation of the state variables is updated by using the optimal estimation value of a previous state and its error variance estimation, as well as the current measured value, to find the current optimal estimation value. In the present disclosure, the estimation of the current 3D coordinate value depends on the estimation error of the previous moment and the current observation value. In the present disclosure, the estimated values ​​are continuously corrected by continuous prediction and actual measurement to achieve a desired uniform state.

[0144] The present disclosure can achieve rapid optimal estimation of the coordinates of the bucket tooth tip of the excavator in the vehicle body coordinate system of the excavator, effectively reducing the impact of system noise and measurement noise on the 3D coordinate system of the excavator, thereby improving the detection accuracy of the 3D coordinates of the bucket tooth tip, effectively improving the work quality of the excavator, and achieving a good result at low cost.

[0145] [Fig.4] is a schematic diagram of a method for estimating bucket tooth tip coordinates according to other embodiments of the present disclosure. The embodiment of [Fig. 4] may be performed by the bucket tooth tip coordinate estimation apparatus or system of the present disclosure, or by the excavator of the present disclosure. As shown in [Fig. 4], the method of the embodiment of [Fig. 4] may comprise at least one of steps 41 to 48.

[0146] In step 41, a state vector W(k)=[x(k),y(k),z(k)] is defined for the bucket tooth tip coordinate estimation system, which includes the 3D coordinates of the excavator bucket tooth tip on the x-axis, y-axis, and z-axis, respectively.

[0147] In step 42, a dynamic model for the bucket tooth tip coordinate estimation system is established as follows: W ( k + 1 ) = AW ( k ) ■ This model describes the transformation relationship between the state vector at time k and the state vector at time k+1.

[0148] In some embodiments of the present disclosure, the state transition matrix A for a Kalman filter is as follows.

[0149] [Math. 16] 10 0' 0 1.

[0150] In step 43, a measurement vector S(k)=[x(k),y(k),z(k)] is defined for the bucket tooth tip coordinate estimation system. As a state vector, the measurement vector in the present disclosure is also configured to represent the coordinates of the bucket tooth tip of the excavator. S(k) represents the coordinate values ​​of the bucket tooth tip of the excavator calculated based on the sensor measurement values.

[0151] In step 44, a measurement model Y ( k + 1 ) = HW ( k + 1 ) is estimated for the bucket tooth tip coordinate estimation system, which is used to determine how to map the system state vector to the measurement vector. In the present disclosure, the state vector is the same as the measurement vector. In addition, measurement noise must also be considered in this equation. Y(k) represents values ​​of the measurement vector calculated based on the predicted values ​​of the bucket tooth tip coordinates at time k+1.

[0152] In step 45, a system state vector and a covariance matrix are initialized. The covariance matrix describes the estimation accuracy of the state vector, which is established based on prior knowledge of the coordinate system of the excavator bucket tooth tip in the following manner.

[0153] [Math. 17] LO 0 0 ■ 0 10.

[0154] In step 46, values ​​of the state vector and error covariance at time k + 1 are predicted based on values ​​of the state vector and the error covariance at the instant k.

[0155] In some embodiments of the present disclosure, step 46 may comprise: predicting values ​​of the state vector and the error covariance at time k+1 based on formulas (1) and (2).

[0156] In step 47, an optimal estimation of the state variable and the error covariance at time k+1 is performed to generate an optimal estimation of the state variable and the error covariance at time k+1, based on the predicted values ​​of the state vector and the error covariance at time k+1, as well as actual measured output variable values.

[0157] In some embodiments of the present disclosure, step 47 may comprise: generating an optimal estimate of the state variable and the error covariance at time k+1 according to formulas (3), (4) and (5).

[0158] In step 48, the optimal estimate of the state variable and the error covariance at time k+1 are input to step 46 to iteratively predict values ​​of the state vector and the error covariance a next time.

[0159] [Fig. 5] is a schematic diagram of a bucket tooth tip coordinate estimation apparatus according to some embodiments of the present disclosure. As shown in [Fig. 5], the bucket tooth tip coordinate estimation apparatus of the present disclosure may comprise a coordinate measurement unit 51, a noise acquisition unit 52, and a coordinate estimation unit 53.

[0160] The coordinate measurement unit 51 is configured to establish a kinematic model of an excavator based on dimensions of a plurality of components of the excavator, and to obtain measured values ​​of the coordinates of the bucket tooth tip of the excavator based on angles of the plurality of components of the excavator measured by excavator sensors with the kinematic model.

[0161] In some embodiments of the present disclosure, the coordinate measurement unit 51 is configured to establish five coordinate systems at different components of the excavator, wherein the origins of the five coordinate systems are respectively: an intersection of a vertical axis about which a rotating platform of the excavator rotates and the ground, a connecting joint between a boom and the rotating platform of the excavator, a connecting joint between the boom and an arm of the excavator, a joint between the arm and a bucket of the excavator, and the bucket tooth tip, in which a coordinate system taking the intersection of the vertical axis around which the rotating platform of the excavator rotates and the ground as the origin is an excavator coordinate system.

[0162] The noise acquisition unit 52 is configured to obtain system noise and measurement noise of the coordinates of the bucket tooth tip of the excavator.

[0163] In some embodiments of the present disclosure, the noise acquisition unit 52 may be configured to determine a measurement error covariance during the angle measurement process and determine a system noise covariance during the excavator coordinate calculation process.

[0164] The coordinate estimation unit 53 is configured to determine estimated values ​​of the coordinates of the bucket tooth tip of the excavator based on the measured values ​​of the coordinates of the bucket tooth tip of the excavator, the system noise, and the measurement noise of the coordinates of the bucket tooth tip of the excavator.

[0165] In some embodiments of the present disclosure, the coordinate estimation unit 53 is configured to determine the estimated values ​​of the coordinates of the bucket tooth tip of the excavator by using a predetermined filter to estimate the measured values ​​of the coordinates of the bucket tooth tip of the excavator, based on the measured values ​​of the coordinates of the bucket tooth tip of the excavator, the system noise, and the measurement noise of the coordinates of the bucket tooth tip of the excavator.

[0166] In some embodiments of the present disclosure, the bucket tooth tip coordinate estimation apparatus may be used to perform the bucket tooth tip coordinate estimation method as described in any of the embodiments above (such as any of the embodiments illustrated in Figures 3, 4 and 6).

[0167] [Fig.6] is a structural diagram of a coordinate estimation unit according to some embodiments of the present disclosure. As shown in [Fig. 6], the coordinate estimation unit of the present disclosure (such as the coordinate estimation unit 53 of the embodiment of [Fig. 5]) may comprise a 3D coordinate prediction module 531 and a 3D coordinate updating module 532.

[0168] The 3D coordinate prediction module 531 is configured to obtain predicted values ​​of a state vector and an error covariance matrix at time k+1 according to the estimated values ​​of the state vector and the error covariance matrix at time k, wherein the state vector is a state vector of a system bucket tooth tip coordinate estimation, the state vector represents the bucket tooth tip coordinates of the excavator at a time, and the error covariance matrix is ​​configured to represent the estimation accuracy of the state vector.

[0169] In some embodiments of the present disclosure, the 3D coordinate prediction module 531 is configured to obtain a predicted value of a state vector at time k+1 according to the state vector at time k; obtain the predicted value of an error covariance matrix at time k+1 according to an estimated value of the error covariance matrix at time k.

[0170] In some embodiments of the present disclosure, the 3D coordinate prediction module 531 is configured to set the state vector in the case where the predicted value of the state vector at time k+1 is obtained according to the state vector at time k; establish a dynamic model of the bucket tooth tip coordinate estimation system, wherein the dynamic model is a state transition matrix configured to represent a transformation relationship between the state vector at time k and the state vector at time k+1; and obtain the predicted value of the state vector at time k+1 according to the state vector at time k and the state transition matrix.

[0171] In some embodiments of the present disclosure, the 3D coordinate prediction module 531 is configured to define a system process noise covariance matrix in the case where the predicted value of the error covariance matrix at time k+1 is obtained according to the estimated value of the error covariance matrix at time k; and obtain the predicted value of the error covariance matrix at time k+1 according to the estimated value of the error covariance matrix at time k, the state transition matrix, and the system process noise covariance matrix.

[0172] The 3D coordinate update module 532 is configured to estimate the state vector and the error covariance matrix at time k+1 to obtain estimated values ​​of the state vector and the error covariance matrix at time k+1, based on the predicted values ​​of the state vector and the error covariance matrix at time k+1 and the measured values ​​output by the system at time k+1.

[0173] In some embodiments of the present disclosure, the 3D coordinate update module 532 may be configured to determine a filter gain value at time k+1 according to the predicted value of the error covariance matrix at time k+1, a system measurement matrix of the bucket tooth tip coordinates, and a measurement noise covariance matrix; estimate the state vector at time k+1 to obtain the estimated value of the state vector at time k+1, according to the predicted value of the state vector at time k+1, the filter gain value at time k+1 and the measured values ​​of the system output at time k+1; and estimating the error covariance matrix at time k+1 to obtain the estimated value of the error covariance matrix at time k+1, according to the predicted value of the error covariance matrix at time k+1 and the filter gain value at time k+1.

[0174] In some embodiments of the present disclosure, the 3D coordinate update module 532 may be configured to determine an estimation variance according to the predicted value of the error covariance matrix at time k+1 and the system measurement matrix of the bucket tooth tip coordinates, in the case where the filter gain value at time k+1 is determined according to the predicted value of the error covariance matrix at time k+1, the system measurement matrix of the bucket tooth tip coordinates and the measurement noise covariance matrix; determine a total variance according to the estimation variance and the measurement noise covariance matrix; and determine the filter gain value at time k+1 according to a ratio of the estimation variance to the total variance.

[0175] In some embodiments of the present disclosure, the 3D coordinate update module 532 may be configured to define a measurement vector of the bucket tooth tip coordinates in the case where the state vector at time k+1 is estimated to obtain the estimated value of the state vector at time k+1, based on the predicted value of the state vector at time k+1, the filter gain value at time k+1 and the measured values ​​of the system output at time k+1; establish a measurement model of the bucket tooth tip coordinate estimation system, wherein the measurement model is used to map the state vector to the measurement vector; determine a measurement vector value at time k+1 based on the predicted value of the state vector at time k+1 and the system measurement matrix of the bucket tooth tip coordinates;and estimating the state vector at time k+1 to obtain the estimated value of the state vector at time k+1, based on the measurement vector value at time k+1, the predicted value of the state vector at time k+1, the filter gain value at time k+1 and the measured value of the system output at time k+1. ;

[0176] In some embodiments of the present disclosure, the 3D coordinate update module 532 may be configured to estimate the error covariance matrix at time k+1 to obtain the estimated value of the error covariance matrix at time k+1, according to the predicted value of the error covariance matrix at time k+1, the system measurement matrix of the bucket tooth tip coordinates and the filter gain value at time k+1, in the case where the error covariance matrix at time k+1 is estimated to obtain the estimated value of the error covariance matrix at time k+1, depending on the predicted value of the error covariance matrix at time k+1 and the filter gain value at time k+1.

[0177] In some embodiments of the present disclosure, the 3D coordinate update module 532 is further configured to input the estimated values ​​of the state vector and the error covariance matrix at time k+1 into the 3D coordinate prediction module 531 to enable the 3D coordinate prediction module 531 to iteratively predict values ​​of the state vector and the error covariance a next time.

[0178] [Fig.6] also provides a schematic diagram of the architecture of the method for estimating bucket tooth tip coordinates according to a Kalman filter. As shown in [Fig.6], the architecture of the method for estimating 3D bucket tooth tip coordinates according to a Kalman filter is composed of formulas (1) to (5) provided in the present disclosure.

[0179] In some embodiments of the present disclosure, as shown in [Fig.6], the 3D coordinate prediction module 531 mainly contains formulas (1) and (2), which are mainly used to predict values ​​at time k+1 according to the optimal estimation of the state vector and the error covariance matrix at time k.

[0180] In some embodiments of the present disclosure, as shown in [Fig.6], the 3D coordinate update module 532 mainly contains formulas (3), (4) and (5), which are mainly used to perform the optimal estimation of the state vector and the error covariance matrix at time k+1, according to the predicted values ​​of the state vector and the error covariance matrix at time k+1, as well as the measured values ​​output by the system at time k+1.

[0181] In some embodiments of the present disclosure, as shown in [Fig.6], the formulas used in the Kalman filter to estimate the 3D coordinates of the excavator bucket tooth tip and their relationship are as follows.

[0182] Formula (1) is configured to calculate the predicted value of the system state at time k+1 based on the value of the state at time k, wherein W(k+1) is the predicted value of the state at time k+1 based on the values ​​of the state at time k; W(k) is the optimal result of the state at time k; in the equation, A is a state transition matrix; formula (1) transmits the predicted value w(k+ 1) of the prediction variables to formula (4) and receives the optimal estimation value W(k+1) of the state variables sent by formula (4).

[0183] Formula (2) is configured to calculate the predicted value of an error covariance corresponding to w(k+ 1), wherein p(k + 1) cst 'a predicted value of the error covariance at time k+1 calculated on the basis of the covariance at time k, and P(k) is the optimal covariance result at time k; Q is the system process noise covariance; formula (2) passes the predicted values ​​w(k + 1) of the coordinate error covariance to formulas (3) and (5) to calculate the gain value and covariance of the optimal error covariance estimate at time k+1, respectively.

[0184] Formula (3) is configured to calculate a gain value at time k+1, wherein K(k+1) is the gain of the Kalman filter at time k+1, and is a ratio of the estimation variance to the total variance (the sum of the estimation variance and the measurement variance); H is a system measurement matrix; R is the covariance of the measurement noise; and the Kalman filtering estimates the variable value at a time according to the variation trend of the current observation values ​​of the variables, and this gain matrix represents the magnitude of a variation level. Formula (3) passes the gain K(k+1) at time k+1 to formulas (4) and (5) for the optimal estimation of the 3D coordinates and the optimal estimation of covariance at time k+1, respectively.

[0185] Formula (4) is configured to calculate the optimal estimation values ​​of the 3D coordinates at time k+1, wherein W(k+1) is the optimal result of the system state at time k+1; S(k+1) represents the system measurement values ​​at time k+1; and formula (4) passes W(k+1) to formula (1) to predict the value of the 3D coordinates at time k+2.

[0186] Formula (5) is configured to calculate a covariance corresponding to the optimal system result at time k+1, wherein P(k+1) is a covariance corresponding to the optimal system estimation result at time k+1, which is used to correct the variance between the estimated values ​​and the actual values. Formula (5) passes P(k+1) to formula (2) to predict the value of the error covariance of the 3D coordinates at time k+2.

[0187] The present disclosure provides a method and apparatus for estimating the 3D coordinates of the bucket tooth tip of the excavator based on Kalman filtering, which is used for generating optimal estimation values ​​of the 3D coordinates of the bucket tooth tip in an engineering machine coordinate system during work of an excavator, a loader or other engineering machines, so as to solve the problem of reduced accuracy caused by measurement noise and system noise in the process of calculating the 3D coordinates of the bucket tooth tip in related technologies, thereby improving the measurement accuracy of the 3D coordinates of the bucket tooth tip and improving the work quality of the excavator.

[0188] [Fig.7] is a schematic structural diagram of a bucket tooth tip coordinate estimation apparatus according to other embodiments of the present disclosure. As shown in [Fig.7], the bucket tooth tip coordinate estimation apparatus of the present disclosure may comprise a memory 71 and a processor 72.

[0189] The memory 71 is used to store instructions. The processor 72 is coupled to the memory 71, and is configured to perform, based on instructions stored in the memory, the bucket tooth tip coordinate estimation method provided in the above embodiments (any of the embodiments illustrated in Figures 3, 4 and 6).

[0190] As shown in [Fig.7], the bucket tooth tip coordinate estimation apparatus further comprises an interface 73 for performing information interaction with other devices. Furthermore, the bucket tooth tip coordinate estimation apparatus further comprises a bus 74. The processor 72, the communication interface 73 and the memory 71 perform communication with each other via the bus 74.

[0191] The memory 71 may include high-speed RAM, and may also include non-volatile memory such as at least one disk storage device. The memory 71 may also be a memory array. The memory 71 may also be partitioned into blocks, which may be combined into virtual volumes according to a certain rule.

[0192] Additionally, the processor 72 may be a central processing unit (CPU), or may be an application-specific integrated circuit (ASIC) or one or more integrated circuits configured to implement embodiments of the present disclosure.

[0193] The present disclosure relates to the field of measuring coordinates of key components during engineering works, in particular to a method and apparatus for estimating the 3D coordinates of the excavator bucket tooth tip based on Kalman filtering.

[0194] According to another aspect of the present disclosure, there is provided a bucket tooth tip coordinate estimation system, comprising excavator dynamic sensors and the bucket tooth tip coordinate estimation apparatus as described in any of the above embodiments (e.g., the embodiment illustrated in [Fig.6] or 7).

[0195] In some embodiments of the present disclosure, the excavator dynamic sensors include at least one of a rotary encoder for measuring a rotation angle of the excavator rotary device, an inclinometer for boom to measure a boom angle, an arm inclinometer to measure an arm angle and a bucket inclinometer to measure a bucket angle.

[0196] In some embodiments of the present disclosure, the rotary encoder is a rotational encoder.

[0197] In some embodiments of the present disclosure, the bucket tooth tip 3D coordinate estimation system based on Kalman filtering adopts rotational encoders and inclinometers for angle measurement, and transmits the data to the bucket tooth tip coordinate estimation apparatus for processing.

[0198] In some embodiments of the present disclosure, the rotational encoder is a robust AR62 / 63 absolute encoder or the Heidelberg ERN 1387 2048 62S14-70 rotary encoder.

[0199] In some embodiments of the present disclosure, the inclinometer is a BW-VG525 ultra-high precision CAN dynamic inclinometer.

[0200] In some embodiments of the present disclosure, the bucket tooth tip estimation apparatus may be implemented by an industrial control computer.

[0201] In some embodiments of the present disclosure, the industrial control computer is a Nuvo-7531 industrial control computer, which includes a processor and a memory. When the processor is used to execute a computer program stored in the memory, all functions of the method for estimating 3D coordinates of the excavator bucket tooth tip based on Kalman filtering as described above are implemented.

[0202] In some embodiments of the present disclosure, in order to protect the sensor, the bucket inclinometer is mounted near a bucket rotation axis.

[0203] In some embodiments of the present disclosure, after the inclinometer is installed, its output voltages and the corresponding angle values ​​are actually measured, based on which a linear adjustment is performed, indicating that the inclinometer has good linearity. The adjustment relationship between the output voltage U of the inclinometer and the measured inclination T is U= -0.0553T +0.0254.

[0204] The present disclosure provides a method, apparatus and system for estimating 3D coordinates of the bucket tooth tip of the excavator based on Kalman filtering, which belong to the technical field of engineering machinery. The technical solution of the present disclosure comprises: establishing a kinematic model of the excavator according to the dimensions of the plurality of components of the excavator, and calculating rotation angles of the boom, arm and bucket of the excavator, as well as 3D coordinates of the bucket tooth tip of bucket of the excavator in an excavator coordinate system according to the angles of the plurality of components of the excavator measured by the four dynamic sensors of the excavator with the kinematic model. After measuring and calculating the 3D coordinates of the bucket tooth tip of the excavator, together with prior knowledge such as a measurement error covariance and a system noise covariance in the 3D coordinate measurement system of the excavator, an optimal estimation of the measured values ​​of the 3D coordinates is performed based on Kalman filtering, thereby significantly improving the measurement accuracy of the 3D coordinates of the bucket tooth tip of the excavator.

[0205] The present disclosure is for existing excavator bucket tooth tip 3D coordinate measurement systems, and can output the optimal estimation values ​​of the 3D coordinates in real time without hardware modification, thereby significantly improving the detection accuracy of the 3D coordinates, and effectively improving the working accuracy and quality of the excavator.

[0206] According to another aspect of the present disclosure, as shown in [Fig.2], there is provided an excavator, comprising the bucket tooth tip coordinate estimation system as described in any of the above embodiments.

[0207] According to yet another aspect of the present disclosure, there is provided a computer-readable storage medium having stored thereon computer instructions which, when executed by a processor, perform the method of estimating bucket tooth tip coordinates as described in any of the above embodiments.

[0208] The computer-readable storage medium of the present disclosure is a non-transitory computer-readable storage medium.

[0209] Those skilled in the art should understand that embodiments of the present disclosure may be provided as a method, apparatus, or computer program product. Accordingly, embodiments of the present disclosure may take the form of an all-hardware embodiment, an all-software embodiment, or an embodiment containing both hardware and software elements. Further, the present disclosure may take the form of a computer program product embodied on one or more non-transitory computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage device, etc.) having computer-usable program code embodied therein.

[0210] The present disclosure is described with reference to flowcharts and / or block diagrams of methods, apparatus (systems) and computer program products according to embodiments of the present disclosure. It is to be understood that each process and / or block in the flowcharts and / or block diagrams, and combinations of the processes and / or blocks in the flowcharts and / or block diagrams can be implemented by computer program instructions.The computer program instructions may be applied to a processor of a general purpose computer, a special purpose computer, an embedded processor, or other programmable data processing device to generate a machine such that the instructions executed by a processor of a computer or other programmable data processing device to generate means implementing the functions specified in one or more flows of the flowcharts and / or one or more blocks of the functional diagrams.

[0211] The computer program instructions may also be stored in a computer-readable storage device capable of directing a computer or other programmable data processing apparatus to operate in a specific manner such that the instructions stored in the computer-readable storage device produce an article of manufacture comprising instruction means implementing the functions specified in one or more flows of the flowcharts and / or one or more blocks of the functional diagrams.

[0212] These computer program instructions may also be loaded onto a computer or other programmable device to perform a series of operational steps on the computer or other programmable device to generate a computer-implemented process such that the instructions executed on the computer or other programmable device provide steps implementing the functions specified in one or more flows of the flowcharts and / or one or more blocks of the functional diagrams.

[0213] The bucket tooth tip coordinate estimation apparatus, coordinate measurement unit, noise acquisition unit, coordinate estimation unit, 3D coordinate prediction module, and 3D coordinate update module described above may be implemented as a general-purpose processor for performing the functions described in this application, programmable logic controller (PLC), digital signal processor (DSP), application-specific integrated circuit (ASIC), field-programmable gate array (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, or any suitable combination thereof.

[0214] As noted by those skilled in the art, some or all of the method steps of the disclosed embodiments may be completed with hardware, which may be implemented as a general-purpose processor, programmable logic controller, digital signal processor, specific integrated circuit, field-programmable gate array or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, or any suitable combination thereof.

[0215] Up to this point, the present disclosure has been described in detail. In order to avoid confusing the concepts of the present disclosure, certain details known in the art are not described. Based on the above description, those skilled in the art can understand how to implement the technical solutions disclosed herein.

[0216] A person skilled in the art may understand that some or all of the steps for carrying out the method in the above embodiments may be supplemented by hardware or a program instructing the associated hardware, the program being able to be stored in a non-transitory computer-readable storage medium. The storage medium may be a read-only memory (ROM), a magnetic disk or a compact disc (CD).

[0217] The above description of the present invention is given by way of illustration and description, but is not exhaustive and is not intended to limit the present invention to the form disclosed herein. Various modifications and variations will be apparent to one skilled in the art. Embodiments are selected and described for better illustration of the principle and practical application of the present invention, so that one skilled in the art may understand the present invention and envision various embodiments with various modifications suitable for specific uses.

Claims

Claims

1. A method for estimating coordinates of a bucket tooth tip, comprising: establishing a kinematic model of an excavator based on dimensions of a plurality of components of the excavator, and obtaining measured values ​​of the coordinates of the bucket tooth tip of the excavator based on angles of the plurality of components of the excavator measured by excavator sensors with the kinematic model; obtaining system noise and measurement noise of the coordinates of the bucket tooth tip of the excavator; and determining estimated values ​​of the coordinates of the bucket tooth tip of the excavator based on the measured values ​​of the coordinates of the bucket tooth tip of the excavator, the system noise, and the measurement noise of the coordinates of the bucket tooth tip of the excavator.

2. The method of claim 1, wherein obtaining the system noise and the measurement noise in the coordinates of the bucket tooth tip of the excavator comprises: determining a measurement error covariance when measuring the angles of the plurality of components of the excavator and determining a system noise covariance when calculating the coordinates of the bucket tooth tip of the excavator.

3. The method of claim 1 or 2, wherein determining the estimated values ​​of the excavator bucket tooth tip coordinates based on the measured values ​​of the excavator bucket tooth tip coordinates, the system noise, and the measurement noise of the excavator bucket tooth tip coordinates, comprises: determining the estimated values ​​of the excavator bucket tooth tip coordinates by using a predetermined filter to estimate the measured values ​​of the excavator bucket tooth tip coordinates, based on the measured values ​​of the excavator bucket tooth tip coordinates, the system noise, and the measurement noise of the excavator bucket tooth tip coordinates.

4. The method of claim 3, wherein determining the estimated values ​​of the coordinates of the bucket tooth tip of the excavator by using the predetermined filter to estimate the measured values ​​of the coordinates of the bucket tooth tip of the excavator, based on the measured values ​​of the coordinates of the bucket tooth tip of the excavator, the system noise, and the measurement noise of the coordinates of the bucket tooth tip of the excavator, comprises: obtaining a predicted value of a state vector at time k+1 according to the state vector at time k, wherein the state vector is a state vector of a bucket tooth tip coordinate estimation system, and the state vector represents the coordinates of the bucket tooth tip of the excavator at time;obtaining the predicted value of an error covariance matrix at time k+1 according to an estimated value of the error covariance matrix at time k, wherein the error covariance matrix is ​​configured to represent an estimation accuracy of the state vector; and estimating the state vector and the error covariance matrix at time k+1 to obtain estimated values ​​of the state vector and the error covariance matrix at time k+1, based on the predicted values ​​of the state vector and the error covariance matrix at time k+1, and the measured values ​​output by the bucket tooth tip coordinate estimation system at time k+1.;

5. The method of claim 4, wherein determining the estimated values ​​of the coordinates of the bucket tooth tip of the excavator by using the predetermined filter to estimate the measured values ​​of the coordinates of the bucket tooth tip of the excavator, based on the measured values ​​of the coordinates of the bucket tooth tip of the excavator, the system noise, and the measurement noise of the coordinates of the bucket tooth tip of the excavator, further comprises: assigning the estimated values ​​of the state vector and the error covariance matrix at time k+1 to obtaining the predicted value of the state vector at time k+1 based on the state vector at time k and obtaining the predicted value of the error covariance matrix error covariance at time k+1 as a function of the estimated value of the error covariance matrix at time k, to iteratively predict values ​​of the state vector and the error covariance matrix at a subsequent time.

6. The method of claim 4, wherein obtaining the predicted value of the state vector at time k+1 according to the state vector at time k, comprises: defining the state vector; establishing a dynamic model of the bucket tooth tip coordinate estimation system, wherein the dynamic model is a state transition matrix configured to represent a transformation relationship between the state vector at time k and the state vector at time k+1; and obtaining the predicted value of the state vector at time k+1 according to the state vector at time k and the state transition matrix.

7. The method of claim 4, wherein obtaining the predicted value of the error covariance matrix at time k+1 according to the estimated value of the error covariance matrix at time k, comprises: defining a system noise covariance matrix; and obtaining the predicted value of the error covariance matrix at time k+1 according to the estimated value of the error covariance matrix at time k, the state transition matrix and the system noise covariance matrix.

8. The method of claim 4, wherein estimating the state vector and the error covariance matrix at time k+1 to obtain estimated values ​​of the state vector and the error covariance matrix at time k+1, based on predicted values ​​of the state vector and the error covariance matrix at time k+1, and measured values ​​output by the bucket tooth tip coordinate estimation system at time k+1, comprises: determining a filter gain value at time k+1 based on the predicted value of the error covariance matrix at time k+1, a measurement matrix of the bucket tooth tip coordinate estimation system, and a measurement noise covariance matrix; estimating the state vector at time k+1 to obtain the estimated value of the state vector at time k+1, based on the predicted value of the state vector at time k+1, the filter gain value at time k+1, and the measured values ​​of the output of the bucket tooth tip coordinate estimation system at time k+1; and estimating the error covariance matrix at time k+1 to obtain the estimated value of the error covariance matrix at time k+1, based on the predicted value of the error covariance matrix at time k+1 and the filter gain value at time k+1.

9. The method of claim 8, wherein determining the filter gain value at time k+1 based on the predicted value of the error covariance matrix at time k+1, the measurement matrix of the bucket tooth tip coordinate estimation system, and the measurement noise covariance matrix comprises: determining an estimation variance based on the predicted value of the error covariance matrix at time k+1 and the measurement matrix of the bucket tooth tip coordinate estimation system; determining a total variance based on the estimation variance and the measurement noise covariance matrix; and determining the filter gain value at time k+1 based on a ratio of the estimation variance to the total variance.

10. The method of claim 8, wherein estimating the state vector at time k+1 to obtain the estimated value of the state vector at time k+1, based on the predicted value of the state vector at time k+1, the filter gain value at time k+1, and the measured values ​​of the output of the bucket tooth tip coordinate estimation system at time k+1, comprises: defining a measurement vector of the bucket tooth tip coordinates; establishing a measurement model of the bucket tooth tip coordinate estimation system, wherein the measurement model is used to map the state vector to the measurement vector;

11.

12. determining a measurement vector value at time k+1 based on the predicted value of the state vector at time k+1 and the measurement matrix of the bucket tooth tip coordinate estimation system; and estimating the state vector at time k+1 to obtain the estimated value of the state vector at time k+1, based on the measurement vector value at time k+1, the predicted value of the state vector at time k+1, the filter gain value at time k+1, and the measured value of the output of the bucket tooth tip coordinate estimation system at time k+1. The method of claim 8, wherein estimating the error covariance matrix at time k+1 to obtain the estimated value of the error covariance matrix at time k+1, based on the predicted value of the error covariance matrix at time k+1 and the filter gain value at time k+1, comprises: estimating the error covariance matrix at time k+1 to obtain the estimated value of the error covariance matrix at time k+1, based on the predicted value of the error covariance matrix at time k+1, the system measurement matrix of the bucket tooth tip coordinates and the filter gain value at time k+1. The method according to any one of claims 1 to 11, wherein establishing the kinematic model of the excavator based on the dimensions of the plurality of components of the excavator, comprises: establishing five coordinate systems at different components of the excavator, wherein the origins of the five coordinate systems are respectively: an intersection of a vertical axis about which a rotating platform of the excavator rotates and the ground, a connecting joint between a boom and the rotating platform of the excavator, a connecting joint between the boom and an arm of the excavator, a joint between the arm and a bucket of the excavator, and the bucket tooth tip, wherein a coordinate system taking the intersection of the vertical axis about which the rotating platform of the excavator rotates and the ground as an origin is an excavator coordinate system.

13. A bucket tooth tip coordinate estimation apparatus, comprising: a coordinate measurement unit configured to establish a kinematic model of an excavator based on dimensions of a plurality of components of the excavator, and to obtain measured values ​​of the coordinates of the bucket tooth tip of the excavator based on angles of the plurality of components of the excavator measured by excavator sensors with the kinematic model; a noise acquisition unit configured to obtain system noise and measurement noise of the coordinates of the bucket tooth tip of the excavator;and a coordinate estimation unit configured to determine estimated values ​​of the coordinates of the bucket tooth tip of the excavator based on the measured values ​​of the coordinates of the bucket tooth tip of the excavator, the system noise, and the measurement noise of the coordinates of the bucket tooth tip of the excavator.;

14. An apparatus for estimating coordinates of a bucket tooth tip, comprising: a memory configured to store computer instructions; and a processor configured to execute the instructions, such that the apparatus for estimating coordinates of the bucket tooth tip performs the method according to any one of claims 1 to 12.

15. A bucket tooth tip coordinate estimation system, comprising excavator dynamic sensors and the apparatus of claim 13 or 14.

16. The system of claim 15, wherein the excavator dynamic sensors comprise at least one of a rotary encoder for measuring a rotation angle of a rotating platform of the excavator, a boom inclinometer for measuring a boom angle, an arm inclinometer for measuring an arm angle, and a bucket inclinometer for measuring a bucket angle.

17.

18. An excavator, comprising the system of claim 15 or 16. A computer-readable storage medium having stored thereon computer instructions which, when executed by a

19. processor, implement the method according to any one of claims 1 to 12. Computer program, including: instructions which, when executed by a processor, cause the processor to perform the method of any one of claims 1 to 12.

Images