A method and system for predicting the trajectory of a bucket of a backhoe dredger

Abstract

The present invention discloses a method and system for predicting the trajectory of a bucket of a backhoe dredger. The next position of the bucket is predicted according to an observation model, a translational motion model and an external disturbance model. The observation model, the translational motion model and the external disturbance model are introduced, the observation model, the translational motion model and the external disturbance model are considered as a whole, and multiple interactions of the observation model, the translational motion model and the external disturbance model are realized, so as to ensure the prediction accuracy of the next position of the bucket. Furthermore, the next position of the bucket is corrected, and the corresponding correction value is output; the translation transformation vector of the backhoe dredger is defined based on the real-time update of the correction value, the position prediction value of the bucket is defined according to the translation transformation vector of the backhoe dredger and the bucket, the corresponding trajectory is defined based on multiple position prediction values, and the association of multiple position prediction values ​​is realized, so as to ensure the prediction accuracy of the trajectory.

Description

Technical Field

[0001] The present invention relates to the technical field of backhoe dredgers, and in particular to a method and system for predicting the trajectory of a bucket of a backhoe dredger. Background Art

[0002] With the development of science and technology, backhoe dredgers are gradually used in people's lives. Backhoe dredgers can perform operations such as channel excavation and slope precision excavation. First, the excavation action of the backhoe dredger will cause the hull to move horizontally and vertically. Secondly, the backhoe dredger is more susceptible to the influence of wind, waves, and currents, causing its bucket to swing greatly with the hull. It is incompatible with the observation model, translational motion model and external disturbance model, which affects the accuracy of the trajectory prediction of the backhoe dredger's bucket. Summary of the invention

[0003] The purpose of the present invention is to overcome the shortcomings of the prior art. The present invention provides a method and system for predicting the trajectory of a bucket of a backhoe dredger. The observation model of the bucket of the backhoe dredger is defined according to the position of the bucket of the backhoe dredger; the translational motion model of the backhoe dredger is constructed based on multiple motion parameters of the backhoe dredger; the external disturbance model of the backhoe dredger is constructed based on the environmental parameters around the backhoe dredger; the next position of the bucket is predicted according to the observation model, the translational motion model and the external disturbance model, the observation model, the translational motion model and the external disturbance model are introduced, the observation model, the translational motion model and the external disturbance model are considered as a whole, and multiple interactions of the observation model, the translational motion model and the external disturbance model are realized, thereby ensuring the prediction accuracy of the next position of the bucket.

[0004] Furthermore, the next position of the bucket is corrected and the corresponding correction value is output; the translation transformation vector of the backhoe dredger is defined based on the real-time update of the correction value, the position prediction value of the bucket is defined according to the translation transformation vector of the backhoe dredger and the bucket, and the corresponding trajectory is defined based on multiple position prediction values, thereby realizing the association of multiple position prediction values ​​to ensure the prediction accuracy of the trajectory.

[0005] An embodiment of the present invention provides a method for predicting the trajectory of a bucket of a backhoe dredger, which is applied to a scenario of predicting the trajectory of a bucket of a backhoe dredger;

[0006] The method for predicting the trajectory of the bucket of the backhoe dredger comprises:

[0007] An observation model of the backhoe dredger bucket is defined according to the position of the backhoe dredger bucket;

[0008] A translational motion model of a backhoe dredger is constructed based on multiple motion parameters of the backhoe dredger;

[0009] An external disturbance model of the backhoe dredger is constructed based on the environmental parameters around the backhoe dredger;

[0010] Predict the next position of the bucket based on the observation model, translational motion model, and external disturbance model;

[0011] Correct the next step position of the bucket and output the corresponding correction value;

[0012] A translation transformation vector of the backhoe dredger is defined based on real-time updating of the correction value, a position prediction value of the bucket is defined according to the translation transformation vector of the backhoe dredger and the bucket, and a corresponding trajectory is defined based on a plurality of position prediction values.

[0013] Optionally, defining the observation model of the bucket of the backhoe dredger according to the position of the bucket of the backhoe dredger includes:

[0014] Define the action space based on the position of the backhoe dredger;

[0015] Construct the corresponding coordinate system for the action space;

[0016] Collect the backhoe dredger's bucket location;

[0017] Based on the coordinate system and the position of the bucket of the backhoe dredger, the coordinates corresponding to the bucket are defined;

[0018] An observation model of the bucket of the backhoe dredger is defined according to the coordinates corresponding to the bucket, the known rotation transformation term, and the predicted translation transformation term;

[0019] Using the 4x4 homogeneous coordinate transformation R, X b Convert to geodetic coordinates X g ;

[0020] in, ,in R 11 , R 12 ⋯ R 33 is a known rotation transformation term, T x ,T y ,T z is the translation transformation term to be predicted; Represents the geodetic coordinates The direction vector of the axis in the body coordinate system Quantity, Represents the geodetic coordinates The direction vector of the axis in the body coordinate system Quantity, Represents the geodetic coordinates The direction vector of the axis in the body coordinate system Quantity;

[0021] The calculation process is:

[0022] .

[0023] Optionally, constructing the translational motion model of the backhoe dredger based on multiple motion parameters of the backhoe dredger includes:

[0024] Real-time monitoring of backhoe dredgers;

[0025] Collect the motion state of the backhoe dredger and define multiple motion parameters of the backhoe dredger;

[0026] Perform multiple interactions on multiple motion parameters of the backhoe dredger, where the multiple motion parameters of the backhoe dredger include the translation transformation of the backhoe dredger at time K, the translation speed of the backhoe dredger at time K, the translation acceleration of the backhoe dredger at time K, and the acceleration offset of the acceleration sensor;

[0027] defining a plurality of model data according to multiple interactions of a plurality of motion parameters of the backhoe dredger;

[0028] Constructing a translational motion model of a backhoe dredger based on multiple model data;

[0029] Use T to represent the translation transformation, that is T =[ T x ,T y ,T z ];

[0030] use Indicates the sampling time interval;

[0031] Setting up the state transfer equation: Definition X =[ T,V,a b ], that is, the motion state of the backhoe dredger system; use X k Represents the value of X at time K, using T k represents the translation transformation of the backhoe dredger at time K, V k represents the translation speed of the backhoe dredger at time K, a k represents the translational acceleration of the backhoe dredger at time K, ab Represents the acceleration offset of the acceleration sensor, f e Denote the external disturbance, we get:

[0032] .

[0033] Optionally, constructing an external disturbance model of the backhoe dredger based on environmental parameters around the backhoe dredger includes:

[0034] freeze frame backhoe dredger;

[0035] Collect environmental parameters around the backhoe dredger;

[0036] Modeling of environmental parameters around a backhoe dredger;

[0037] In the process of modeling the environmental parameters around the backhoe dredger, the position disturbance variance, velocity disturbance variance, and acceleration bias disturbance variance are collected;

[0038] Multiple interactions are performed on the position disturbance variance, velocity disturbance variance, and acceleration bias disturbance variance;

[0039] The external disturbance model of the backhoe dredger is constructed based on the multiple interactions of position disturbance variance, velocity disturbance variance and acceleration bias disturbance variance.

[0040] right f e Modeling: Assumptions f e is a zero-mean Gaussian distribution, and its covariance matrix Q is designed to be

[0041] ;

[0042] in, σ 2 T is the position disturbance variance, σ 2 V is the velocity disturbance variance, σ 2 a is the acceleration bias disturbance variance, I 3 is the 3x3 identity matrix and O3 is the 3x3 zero matrix.

[0043] Optionally, predicting the next position of the bucket according to the observation model, the translational motion model, and the external disturbance model includes:

[0044] Freeze the external disturbance model;

[0045] Multiple interactions between observation model, translational motion model and external disturbance model;

[0046] An observation vector is defined based on multiple interactions of an observation model, a translational motion model, and an external disturbance model;

[0047] The observation vector fuses the acceleration sensor and RTK data, and predicts the translation transformation vector to obtain the corresponding prediction value, which is used as the next step position of the bucket;

[0048] Based on the optimal estimate of the covariance at time K-1, the value of the estimated error covariance at time K is predicted to obtain the predicted value of the estimated error covariance :

[0049] ;

[0050] Where A is the state transition matrix, ;in Indicates the sampling time interval.

[0051] Optionally, the step of correcting the next position of the bucket and outputting a corresponding correction value includes:

[0052] Freeze the next position of the bucket;

[0053] The Kalman gain is calculated based on the next step position of the bucket.

[0054] Optionally, the step of correcting the next position of the bucket and outputting a corresponding correction value also includes:

[0055] Correct the predicted value of the motion state of the backhoe dredger at time K based on the Kalman gain to output a corresponding correction value;

[0056] Correct the estimated error covariance at time K according to the correction value.

[0057] Optionally, defining a translation transformation vector of the backhoe dredger based on real-time updating of the correction value, defining a position prediction value of the bucket according to the translation transformation vector of the backhoe dredger and the bucket, and defining a corresponding trajectory based on a plurality of position prediction values ​​includes:

[0058] Collecting calibration values;

[0059] Update the correction value in real time;

[0060] The translation transformation vector of the backhoe is defined based on real-time updating of the correction values.

[0061] Optionally, the method of defining a translation transformation vector of the backhoe dredger based on real-time updating of the correction value, defining a position prediction value of the bucket according to the translation transformation vector of the backhoe dredger and the bucket, and defining a corresponding trajectory based on a plurality of position prediction values, further includes:

[0062] Associating the translation transformation vector of the backhoe and the bucket;

[0063] A predicted position value of the bucket is defined based on a translation transformation vector of the backhoe dredger and the bucket;

[0064] A corresponding trajectory is defined based on the multiple position prediction values.

[0065] In addition, an embodiment of the present invention further provides a trajectory prediction system for a bucket of a backhoe dredger, the trajectory prediction system for the bucket of a backhoe dredger comprising:

[0066] An observation module, used for defining an observation model of the bucket of the backhoe dredger according to the position of the bucket of the backhoe dredger;

[0067] A translational motion module, used for constructing a translational motion model of the backhoe dredger based on multiple motion parameters of the backhoe dredger;

[0068] An external disturbance module, used for constructing an external disturbance model of the backhoe dredger based on environmental parameters around the backhoe dredger;

[0069] A prediction module, used to predict the next position of the bucket based on the observation model, the translational motion model, and the external disturbance model;

[0070] A correction module, used to correct the next step position of the bucket and output the corresponding correction value;

[0071] The trajectory module is used to define a translation transformation vector of the backhoe dredger based on real-time updating of the correction value, define a position prediction value of the bucket according to the translation transformation vector of the backhoe dredger and the bucket, and define a corresponding trajectory based on multiple position prediction values.

[0072] In an embodiment of the present invention, through the method in the embodiment of the present invention, an observation model of the bucket of the backhoe dredger is defined according to the position of the bucket of the backhoe dredger; a translational motion model of the backhoe dredger is constructed based on multiple motion parameters of the backhoe dredger; an external disturbance model of the backhoe dredger is constructed based on environmental parameters around the backhoe dredger; the next position of the bucket is predicted according to the observation model, the translational motion model and the external disturbance model, and the observation model, the translational motion model and the external disturbance model are introduced, the observation model, the translational motion model and the external disturbance model are considered as a whole, and multiple interactions of the observation model, the translational motion model and the external disturbance model are realized, thereby ensuring the prediction accuracy of the next position of the bucket.

[0073] Furthermore, the next position of the bucket is corrected and the corresponding correction value is output; the translation transformation vector of the backhoe dredger is defined based on the real-time update of the correction value, the position prediction value of the bucket is defined according to the translation transformation vector of the backhoe dredger and the bucket, and the corresponding trajectory is defined based on multiple position prediction values, thereby realizing the association of multiple position prediction values ​​to ensure the prediction accuracy of the trajectory. BRIEF DESCRIPTION OF THE DRAWINGS

[0074] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0075] Figure 1 is a flow chart of a method for predicting the trajectory of a bucket of a backhoe dredger in an embodiment of the present invention;

[0076] Figure 2 is a flow chart of S11 in the method for predicting the trajectory of the bucket of a backhoe dredger in an embodiment of the present invention;

[0077] Figure 3 is a flow chart of S12 in the method for predicting the trajectory of the bucket of a backhoe dredger in an embodiment of the present invention;

[0078] Figure 4 is a flow chart of S13 in the method for predicting the trajectory of the bucket of a backhoe dredger in an embodiment of the present invention;

[0079] Figure 5 is a schematic flow chart of S14 in the method for predicting the trajectory of the bucket of a backhoe dredger in an embodiment of the present invention;

[0080] Figure 6 is a schematic flow chart of S15 in the method for predicting the trajectory of the bucket of a backhoe dredger in an embodiment of the present invention;

[0081] Figure 7 is a flow chart of S16 in the method for predicting the trajectory of the bucket of a backhoe dredger in an embodiment of the present invention;

[0082] Figure 8 Schematic diagram of the structure of the trajectory prediction system of the bucket of the backhoe dredger in the embodiment of the present invention;

[0083] Fig. 9 The figure is a hardware diagram of an electronic device according to an exemplary embodiment. DETAILED DESCRIPTION

[0084] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0085] See also Figures 1 to 9 A backhoe dredger bucket trajectory prediction method is applied to a backhoe dredger bucket trajectory prediction scenario; the backhoe dredger bucket trajectory prediction method includes:

[0086] Step S11: defining an observation model of the bucket of the backhoe dredger according to the position of the bucket of the backhoe dredger;

[0087] Step S12: defining an observation model of the bucket of the backhoe dredger according to the position of the bucket of the backhoe dredger;

[0088] Step S13: constructing an external disturbance model of the backhoe dredger based on the environmental parameters around the backhoe dredger;

[0089] Step S14: predicting the next position of the bucket according to the observation model, the translational motion model and the external disturbance model;

[0090] Step S15: Correct the next step position of the bucket and output the corresponding correction value;

[0091] Step S16: defining a translation transformation vector of the backhoe dredger based on real-time updating of the correction value, defining a position prediction value of the bucket according to the translation transformation vector of the backhoe dredger and the bucket, and defining a corresponding trajectory based on a plurality of position prediction values.

[0092] In an embodiment of the present invention, through the method in the embodiment of the present invention, an observation model of the bucket of the backhoe dredger is defined according to the position of the bucket of the backhoe dredger; a translational motion model of the backhoe dredger is constructed based on multiple motion parameters of the backhoe dredger; an external disturbance model of the backhoe dredger is constructed based on environmental parameters around the backhoe dredger; the next position of the bucket is predicted according to the observation model, the translational motion model and the external disturbance model, and the observation model, the translational motion model and the external disturbance model are introduced, the observation model, the translational motion model and the external disturbance model are considered as a whole, and multiple interactions of the observation model, the translational motion model and the external disturbance model are realized, thereby ensuring the prediction accuracy of the next position of the bucket.

[0093] Furthermore, the next position of the bucket is corrected and the corresponding correction value is output; the translation transformation vector of the backhoe dredger is defined based on the real-time update of the correction value, the position prediction value of the bucket is defined according to the translation transformation vector of the backhoe dredger and the bucket, and the corresponding trajectory is defined based on multiple position prediction values, thereby realizing the association of multiple position prediction values ​​to ensure the prediction accuracy of the trajectory.

[0094] refer to Figure 2 , in step S11, an observation model of the bucket of the backhoe dredger is defined according to the position of the bucket of the backhoe dredger;

[0095] In the specific implementation process of the present invention, the specific steps may be:

[0096] S111: defining an action space based on the position of the backhoe dredger;

[0097] S112: constructing a corresponding coordinate system for the action space;

[0098] S113: collecting the position of the bucket of the backhoe dredger;

[0099] S114: defining coordinates corresponding to the bucket based on the coordinate system and the position of the bucket of the backhoe dredger;

[0100] S115: defining an observation model of the bucket of the backhoe dredger according to the coordinates corresponding to the bucket, the known rotation transformation term, and the predicted translation transformation term.

[0101] In an embodiment of the present application, the action space is defined based on the position of the backhoe dredger, the position of the backhoe dredger is introduced, and the position of the backhoe dredger is controlled to define the action space, thereby ensuring the accuracy of the action space.

[0102] Furthermore, a corresponding coordinate system is constructed for the action space to realize coordinate control of the action space. At the same time, the position of the bucket of the backhoe dredger is collected; based on the coordinate system and the position of the bucket of the backhoe dredger, the coordinates corresponding to the bucket are defined, which is compatible with the overall consideration of the coordinate system and the position of the bucket of the backhoe dredger, and realizes multi-dimensional control of the coordinate system and the position of the bucket of the backhoe dredger, ensuring the accuracy of the coordinates corresponding to the bucket.

[0103] Therefore, the observation model of the bucket of the backhoe dredger is defined according to the coordinates corresponding to the bucket, the known rotation transformation terms and the predicted translation transformation terms, and the coordinates corresponding to the bucket, the known rotation transformation terms and the predicted translation transformation terms are introduced. The coordinates corresponding to the bucket, the known rotation transformation terms and the predicted translation transformation terms are considered as a whole, and multi-dimensional control of the coordinates corresponding to the bucket, the known rotation transformation terms and the predicted translation transformation terms is achieved, thereby ensuring the accuracy of the observation model of the bucket of the backhoe dredger.

[0104] Specifically, define the bucket geodetic coordinates as vector X g =( x,y,z ) T , define the measurement values ​​of the boom angle sensor, arm angle sensor, bucket angle sensor, base roll angle sensor, and base pitch angle sensor as θ a1 , θ a2 , θ b , θ r , θ p , the rotation angle of the base is θ c .in θ c It is defined in the geodetic coordinate system, and the remaining parameters are defined in the local coordinate system of each part.

[0105] The backhoe dredger is considered as a multi-link robotic arm system with 6 degrees of freedom. Through its design drawings, 6 sets of Modified DH parameters are obtained, each of which includes: the angle between adjacent rotation axes, the distance between adjacent rotation axes, the rotation angle of the current rotation axis, and the axial distance between adjacent rotation axes. Then the Modified DH transformation matrix is ​​obtained, and then a mapping is obtained, so that X b =f ( θ a1 , θ a2 , θ b , θ r , θ p, θ c ),in X b The coordinates of the bucket in the machine body coordinate system. The Modified DH parameter calculation mapping is a conventional calculation method and will not be described in detail.

[0106] Then, using the 4x4 homogeneous coordinate transformation R, X b Convert to geodetic coordinates X g ;

[0107] in, ,in R 11 , R 12 ⋯ R 33 is a known rotation transformation term, T x ,T y ,T z is the translation transformation term to be predicted; Represents the geodetic coordinates The direction vector of the axis in the body coordinate system Quantity, Represents the geodetic coordinates The direction vector of the axis in the body coordinate system Quantity, Represents the geodetic coordinates The direction vector of the axis in the body coordinate system Quantity.

[0108] The calculation process is: ;

[0109] refer to Figure 3 , in step S12, an observation model of the bucket of the backhoe dredger is defined according to the position of the bucket of the backhoe dredger;

[0110] In the specific implementation process of the present invention, the specific steps may be:

[0111] S121: Real-time monitoring of backhoe dredgers;

[0112] S122: collecting the motion state of the backhoe dredger and defining a plurality of motion parameters of the backhoe dredger;

[0113] S123: performing multiple interactions on multiple motion parameters of the backhoe dredger, where the multiple motion parameters of the backhoe dredger include the translation transformation of the backhoe dredger at time K, the translation speed of the backhoe dredger at time K, the translation acceleration of the backhoe dredger at time K, and the acceleration offset of the acceleration sensor;

[0114] S124: defining a plurality of model data according to multiple interactions of a plurality of motion parameters of the backhoe dredger;

[0115] S125: Constructing a translational motion model of the backhoe dredger based on the multiple model data.

[0116] In an embodiment of the present application, a backhoe dredger is monitored in real time, a backhoe dredger is introduced, and the backhoe dredger is monitored in real time to collect the motion state of the backhoe dredger, and multiple motion parameters of the backhoe dredger are defined, multiple motion parameters of the backhoe dredger are introduced, and multiple interactions of the multiple motion parameters of the backhoe dredger are realized.

[0117] The multiple motion parameters of the backhoe dredger are further subjected to multiple interactions. At this time, the multiple motion parameters of the backhoe dredger include the translation transformation of the backhoe dredger at time K, the translation speed of the backhoe dredger at time K, the translation acceleration of the backhoe dredger at time K, and the acceleration offset of the acceleration sensor.

[0118] Therefore, multiple model data are defined according to the multiple interactions of multiple motion parameters of the backhoe dredger; the translational motion model of the backhoe dredger is constructed based on the multiple model data, which realizes the multiple interactions of multiple motion parameters of the backhoe dredger, ensures the accuracy of the multiple model data, and further ensures the accuracy of the construction of the translational motion model of the backhoe dredger.

[0119] Specifically, in order to improve the measurement accuracy of the translation transformation, the measurement data of the hull acceleration sensor is introduced.

[0120] Use T to represent the translation transformation, that is T =[ T x ,T y ,T z ];

[0121] use Indicates the sampling time interval;

[0122] Setting up the state transfer equation: Definition X =[ T,V,a b ], that is, the motion state of the backhoe dredger system; use X k Represents the value of X at time K, using T k represents the translation transformation of the backhoe dredger at time K, V k represents the translation speed of the backhoe dredger at time K, a k represents the translational acceleration of the backhoe dredger at time K, a b Represents the acceleration offset of the acceleration sensor,f e Denote the external disturbance, we get:

[0123] .

[0124] refer to Figure 4 , in step S13, in the floor cleaning space, an external disturbance model of the backhoe dredger is constructed based on the environmental parameters around the backhoe dredger;

[0125] In the specific implementation process of the present invention, the specific steps may be:

[0126] S131: freeze frame backhoe dredger;

[0127] S132: Collect environmental parameters around the backhoe dredger;

[0128] S133: Modeling of environmental parameters around a backhoe dredger;

[0129] S134: in the process of modeling the environmental parameters around the backhoe dredger, collecting the position disturbance variance, the velocity disturbance variance, and the acceleration bias disturbance variance;

[0130] S135: multiple interactions are performed on the position disturbance variance, velocity disturbance variance, and acceleration bias disturbance variance;

[0131] S136: Constructing an external disturbance model of the backhoe dredger based on multiple interactions of position disturbance variance, velocity disturbance variance, and acceleration bias disturbance variance.

[0132] In an embodiment of the present application, a backhoe dredger is frozen, the backhoe dredger is introduced, the backhoe dredger is controlled, and at the same time, environmental parameters around the backhoe dredger are collected; the environmental parameters around the backhoe dredger are modeled, and further modeling of the environmental parameters around the backhoe dredger is achieved.

[0133] Furthermore, in the process of modeling the environmental parameters around the backhoe dredger, the position disturbance variance, velocity disturbance variance, and acceleration bias disturbance variance are collected; multiple interactions are performed on the position disturbance variance, velocity disturbance variance, and acceleration bias disturbance variance; based on the multiple interactions of the position disturbance variance, velocity disturbance variance, and acceleration bias disturbance variance, the external disturbance model of the backhoe dredger is constructed, which is compatible with the overall consideration of the position disturbance variance, velocity disturbance variance, and acceleration bias disturbance variance, realizes the multiple interactions of the position disturbance variance, velocity disturbance variance, and acceleration bias disturbance variance, and ensures the accuracy of the external disturbance model of the backhoe dredger.

[0134] right f e Modeling: Assumptions fe is a zero-mean Gaussian distribution, and its covariance matrix Q is designed to be

[0135] ;

[0136] in, σ 2 T is the position disturbance variance, σ 2 V is the velocity disturbance variance, σ 2 a is the acceleration bias disturbance variance, I 3 is the 3x3 identity matrix and O3 is the 3x3 zero matrix.

[0137] refer to Figure 5 , S14: predicting the next position of the bucket based on the observation model, the translational motion model, and the external disturbance model;

[0138] In the specific implementation process of the present invention, the specific steps may be:

[0139] S141: Fixed-frame external disturbance model;

[0140] S142: Multiple interactions between observation model, translational motion model and external disturbance model;

[0141] S143: defining an observation vector based on multiple interactions of the observation model, the translational motion model, and the external disturbance model;

[0142] S144: The observation vector is integrated with the acceleration sensor and the RTK data, and the translation transformation vector is predicted to obtain a corresponding prediction value, which is used as the next step position of the bucket.

[0143] In an embodiment of the present application, an observation model of the bucket of a backhoe dredger is defined according to the position of the bucket of the backhoe dredger; a translational motion model of the backhoe dredger is constructed based on multiple motion parameters of the backhoe dredger; an external disturbance model of the backhoe dredger is constructed based on environmental parameters around the backhoe dredger; the next position of the bucket is predicted according to the observation model, the translational motion model and the external disturbance model, and the observation model, the translational motion model and the external disturbance model are introduced, the observation model, the translational motion model and the external disturbance model are considered as a whole, and multiple interactions of the observation model, the translational motion model and the external disturbance model are realized, thereby ensuring the prediction accuracy of the next position of the bucket.

[0144] At this time, the external disturbance model is frozen, and the observation model, translational motion model and external disturbance model are introduced. Multiple interactions are performed on the observation model, translational motion model and external disturbance model, thus realizing multiple interactions among the observation model, translational motion model and external disturbance model.

[0145] Therefore, the observation vector is defined based on the multiple interactions of the observation model, the translation motion model and the external disturbance model to ensure the accuracy of the observation vector. At the same time, the observation vector fuses the acceleration sensor and RTK data, and predicts the translation transformation vector to obtain the corresponding prediction value, and the prediction value is used as the next position of the bucket to ensure the accuracy of the prediction value.

[0146] Specifically, define the observation vector z k :Use RTK measurement data to observe the position of the backhoe dredger; definition z k = CX k . Where C is the observation matrix, C =[ I 3,O3,O3], which means that only the position component in the system state is observed, so z k = T k ;

[0147] Fusion of acceleration sensor and RTK data to predict the translation transformation vector: Based on the optimal estimate of X at time K-1, predict the value of X at time K to obtain the predicted value :

[0148] ;

[0149] Based on the optimal estimate of the covariance at time K-1, the value of the estimated error covariance at time K is predicted to obtain the predicted value of the estimated error covariance :

[0150] ;

[0151] Where A is the state transition matrix, ,in Indicates the sampling time interval.

[0152] refer to Figure 6 , S15: correct the next step position of the bucket and output the corresponding correction value;

[0153] In the specific implementation process of the present invention, the specific steps may be:

[0154] S151: freeze the next step position of the bucket;

[0155] S152: Calculating Kalman gain based on the next position of the bucket;

[0156] S153: Correcting the predicted value of the motion state of the backhoe dredger at time K based on the Kalman gain to output a corresponding correction value;

[0157] S154: Correct the estimated error covariance at time K according to the correction value.

[0158] In an embodiment of the present application, the next position of the bucket is frozen; the Kalman gain is calculated based on the next position of the bucket, the calculation of the next position of the bucket is realized, and the Kalman gain is introduced, so as to correct the motion state prediction value of the backhoe dredger at the moment K based on the Kalman gain to output the corresponding correction value; the estimated error covariance at the moment K is corrected according to the correction value, so as to realize the dynamic correction of the estimated error covariance at the moment K.

[0159] Specifically, based on the prediction value of the previous step, the Kalman gain is calculated:

[0160] ;

[0161] Correction of the predicted value of the motion state of the backhoe dredger system at time K , get the correction value ;

[0162] ;

[0163] Correct the estimated error covariance at time K:

[0164] ;

[0165] refer to Figure 7 , S16: defining a translation transformation vector of the backhoe dredger based on real-time updating of the correction value, defining a position prediction value of the bucket according to the translation transformation vector of the backhoe dredger and the bucket, and defining a corresponding trajectory based on a plurality of position prediction values;

[0166] In the specific implementation process of the present invention, the specific steps may be:

[0167] S161: Collecting calibration values;

[0168] S162: updating the correction value in real time;

[0169] S163: defining a translation transformation vector of the backhoe dredger based on real-time updating of the correction value;

[0170] S164: Associating the translation transformation vector of the backhoe dredger and the bucket;

[0171] S165: defining a predicted position value of the bucket based on a translation transformation vector of the backhoe dredger and the bucket;

[0172] S166: Define corresponding trajectories according to the multiple position prediction values.

[0173] In the specific implementation process of the present invention, the next step position of the bucket is corrected and the corresponding correction value is output; the translation transformation vector of the backhoe dredger is defined based on the real-time update of the correction value, the position prediction value of the bucket is defined according to the translation transformation vector of the backhoe dredger and the bucket, and the corresponding trajectory is defined based on multiple position prediction values, thereby realizing the association of multiple position prediction values ​​to ensure the prediction accuracy of the trajectory.

[0174] At this time, the correction values ​​are collected and updated in real time; the translation transformation vector of the backhoe dredger is defined based on the real-time update of the correction values, so that the real-time update of the correction values ​​is realized and the control of the translation transformation vector of the backhoe dredger is ensured.

[0175] Therefore, the translation transformation vector of the backhoe dredger and the bucket are associated; the position prediction value of the bucket is defined based on the translation transformation vector of the backhoe dredger and the bucket; the corresponding trajectory is defined according to multiple position prediction values, and the association of multiple position prediction values ​​is achieved to ensure the prediction accuracy of the trajectory. Specifically, the translation transformation vector of the backhoe dredger is obtained. , the bucket geodetic coordinates can be calculated as vector X g The predicted value of .

[0176] In an embodiment of the present invention, through the method in the embodiment of the present invention, an observation model of the bucket of the backhoe dredger is defined according to the position of the bucket of the backhoe dredger; a translational motion model of the backhoe dredger is constructed based on multiple motion parameters of the backhoe dredger; an external disturbance model of the backhoe dredger is constructed based on environmental parameters around the backhoe dredger; the next position of the bucket is predicted according to the observation model, the translational motion model and the external disturbance model, and the observation model, the translational motion model and the external disturbance model are introduced, the observation model, the translational motion model and the external disturbance model are considered as a whole, and multiple interactions of the observation model, the translational motion model and the external disturbance model are realized, thereby ensuring the prediction accuracy of the next position of the bucket.

[0177] Furthermore, the next position of the bucket is corrected and the corresponding correction value is output; the translation transformation vector of the backhoe dredger is defined based on the real-time update of the correction value, the position prediction value of the bucket is defined according to the translation transformation vector of the backhoe dredger and the bucket, and the corresponding trajectory is defined based on multiple position prediction values, thereby realizing the association of multiple position prediction values ​​to ensure the prediction accuracy of the trajectory.

[0178] See also Figure 8 , Figure 8 It is a schematic diagram of the structural composition of the trajectory prediction system of the bucket of the backhoe dredger in an embodiment of the present invention.

[0179] like Figure 8 As shown, a trajectory prediction system for a bucket of a backhoe dredger, the trajectory prediction system for the bucket of a backhoe dredger comprising:

[0180] An observation module 21, for defining an observation model of the bucket of the backhoe dredger according to the position of the bucket of the backhoe dredger;

[0181] A translational motion module 22, for defining an observation model of the bucket of the backhoe dredger according to the position of the bucket of the backhoe dredger;

[0182] An external disturbance module 23, used to construct an external disturbance model of the backhoe dredger based on environmental parameters around the backhoe dredger;

[0183] A prediction module 24, for predicting the next position of the bucket based on the observation model, the translational motion model and the external disturbance model;

[0184] The correction module 25 is used to correct the next step position of the bucket and output the corresponding correction value;

[0185] The trajectory module 26 is used to define a translation transformation vector of the backhoe dredger based on real-time updating of the correction value, define a position prediction value of the bucket according to the translation transformation vector of the backhoe dredger and the bucket, and define a corresponding trajectory based on multiple position prediction values.

[0186] See also Fig. 9 , refer to the following Fig. 9 The electronic device 40 according to this embodiment of the present invention will be described. Fig. 9 The electronic device 40 shown is only an example and should not bring any limitation to the functions and scope of use of the embodiments of the present invention.

[0187] like Fig. 9 As shown, the electronic device 40 is in the form of a general computing device. The components of the electronic device 40 may include but are not limited to: at least one processing unit 41, at least one storage unit 42, and a bus 43 connecting different system components (including the storage unit 42 and the processing unit 41).

[0188] The storage unit stores program codes, which can be executed by the processing unit 41, so that the processing unit 41 executes the steps according to various exemplary embodiments of the present invention described in the above “Embodiment Method” section of this specification.

[0189] The storage unit 42 may include a readable medium in the form of a volatile storage unit, such as a random access memory unit (RAM) 421 and / or a cache memory unit 422 , and may further include a read-only memory unit (ROM) 423 .

[0190] The storage unit 42 may also include a program / utility 424 having a set (at least one) of program modules 425, such program modules 425 including but not limited to: an operating system, one or more application programs, other program modules, and program data, each of which or some combination may include an implementation of a network environment.

[0191] Bus 43 may represent one or more of several types of bus structures, including a memory unit bus or memory unit controller, a peripheral bus, an accelerated graphics port, a processing unit, or a local bus using any of a variety of bus architectures.

[0192] The electronic device 40 may also communicate with one or more external devices (e.g., keyboards, pointing devices, Bluetooth devices, etc.), one or more devices that enable a user to interact with the electronic device 40, and / or any device that enables the electronic device 40 to communicate with one or more other computing devices (e.g., routers, modems, etc.). Such communication may be performed via an input / output (I / O) interface 44. Furthermore, the electronic device 40 may also communicate with one or more networks (e.g., a local area network (LAN), a wide area network (WAN), and / or a public network, such as the Internet) via a network adapter 45. Fig. 9 As shown, the network adapter 45 communicates with other modules of the electronic device 40 via the bus 43. It should be understood that although Fig. 9 Not shown, other hardware and / or software modules may be used in conjunction with the electronic device 40, including but not limited to: microcode, device drivers, redundant processing units, external disk drive arrays, RAID systems, tape drives, and data backup planning systems.

[0193] Through the description of the above implementation, it is easy for those skilled in the art to understand that the example implementation described here can be implemented by software, or by software combined with necessary hardware. Therefore, the technical solution according to the implementation of the present disclosure can be embodied in the form of a software product, which can be stored in a non-volatile storage medium (which can be a CD-ROM, a USB flash drive, a mobile hard disk, etc.) or on a network, including several instructions to enable a computing device (which can be a personal computer, a server, a multi-parameter sensor device, or a network device, etc.) to execute the method according to the implementation of the present disclosure.

[0194] Those skilled in the art can understand that all or some of the steps in the various methods of the above embodiments can be completed by instructing related hardware through a program, and the program can be stored in a computer-readable storage medium, and the storage medium can include: a read-only memory (ROM), a random access memory (RAM), a disk or an optical disk, etc. In addition, it stores computer program instructions, and when the computer program instructions are executed by a computer, the computer executes the above method.

[0195] In addition, the above describes in detail the trajectory prediction method and system for the bucket of a backhoe dredger provided in the embodiments of the present invention. Specific examples are used herein to illustrate the principles and implementation methods of the present invention. The description of the above embodiments is only used to help understand the method of the present invention and its core idea. For those skilled in the art, according to the ideas of the present invention, there will be changes in the specific implementation methods and application scopes. In summary, the content of this specification should not be understood as a limitation on the present invention.

Claims

1. A method for predicting the trajectory of a bucket of a backhoe dredger, characterized in that: Trajectory prediction scenario for the bucket of a backhoe dredger; The method for predicting the trajectory of the bucket of the backhoe dredger comprises: An observation model of the backhoe dredger bucket is defined according to the position of the backhoe dredger bucket: an action space is defined based on the position of the backhoe dredger; a corresponding coordinate system is constructed for the action space; coordinates corresponding to the bucket are defined based on the coordinate system and the position of the backhoe dredger bucket; an observation model of the backhoe dredger bucket is defined according to the coordinates corresponding to the bucket, known rotation transformation items, and predicted translation transformation items; Constructing a translational motion model of the backhoe dredger based on multiple motion parameters of the backhoe dredger: defining multiple model data according to multiple interactions of the multiple motion parameters of the backhoe dredger; constructing a translational motion model of the backhoe dredger based on the multiple model data; An external disturbance model of a backhoe dredger is constructed based on the environmental parameters around the backhoe dredger: in the process of modeling the environmental parameters around the backhoe dredger, the position disturbance variance, the velocity disturbance variance, and the acceleration bias disturbance variance are collected; an external disturbance model of a backhoe dredger is constructed based on the multiple interactions of the position disturbance variance, the velocity disturbance variance, and the acceleration bias disturbance variance; Predict the next position of the bucket based on the observation model, translational motion model, and external disturbance model; Correct the next step position of the bucket and output the corresponding correction value; A translation transformation vector of the backhoe dredger is defined based on real-time updating of the correction value, a position prediction value of the bucket is defined according to the translation transformation vector of the backhoe dredger and the bucket, and a corresponding trajectory is defined based on a plurality of position prediction values.

2. The method for predicting the trajectory of a bucket of a backhoe dredger according to claim 1, characterized in that: The method of constructing a translational motion model of a backhoe dredger based on multiple motion parameters of the backhoe dredger comprises: Real-time monitoring of backhoe dredgers; Collect the motion state of the backhoe dredger and define multiple motion parameters of the backhoe dredger; Use T to represent the translation transformation, that is T =[ T x ,T y ,T z ]; T x ,T y ,T z is the translation transformation term to be predicted; Use Δt to represent the sampling time interval; Setting up the state transfer equation: Definition X =[ T,V,a b ], that is, the motion state of the backhoe dredger system; use X k Represents the value of X at time K, using T k represents the translation transformation of the backhoe dredger at time K, V k represents the translation speed of the backhoe dredger at time K, a k represents the translational acceleration of the backhoe dredger at time K, a b Represents the acceleration offset of the acceleration sensor, f e Denote the external disturbance, we get: 。 3. The method for predicting the trajectory of a bucket of a backhoe dredger according to claim 2, characterized in that: The external disturbance model of the backhoe dredger is constructed based on the environmental parameters around the backhoe dredger, including: freeze frame backhoe dredger; Collect environmental parameters around the backhoe dredger; Modeling of environmental parameters around a backhoe dredger; right f e Modeling: Assumptions f e is a zero-mean Gaussian distribution, and its covariance matrix Q is designed to be ； in, σ 2 T is the position disturbance variance, σ 2 V is the velocity disturbance variance, σ 2 a is the acceleration bias disturbance variance, I 3 is the 3x3 identity matrix and O3 is the 3x3 zero matrix.

4. The method for predicting the trajectory of a bucket of a backhoe dredger according to claim 3, characterized in that: The predicting of the next position of the bucket according to the observation model, the translational motion model and the external disturbance model includes: Freeze the external disturbance model; Multiple interactions between observation model, translational motion model and external disturbance model; An observation vector is defined based on multiple interactions of an observation model, a translational motion model, and an external disturbance model; The observation vector fuses the acceleration sensor and RTK data, and predicts the translation transformation vector to obtain the corresponding prediction value, which is used as the next step position of the bucket; Based on the optimal estimate of the covariance at time K-1, the value of the estimated error covariance at time K is predicted to obtain the predicted value of the estimated error covariance ; ； Where A is the state transition matrix, ; Where Δt represents the sampling time interval.

5. The method for predicting the trajectory of a bucket of a backhoe dredger according to claim 4, characterized in that: The step of correcting the next position of the bucket and outputting a corresponding correction value includes: Freeze the next position of the bucket; The Kalman gain is calculated based on the next step position of the bucket.

6. The method for predicting the trajectory of a bucket of a backhoe dredger according to claim 5, characterized in that: The step of correcting the next step position of the bucket and outputting the corresponding correction value also includes: Correct the predicted value of the motion state of the backhoe dredger at time K based on the Kalman gain to output a corresponding correction value; Correct the estimated error covariance at time K according to the correction value.

7. The method for predicting the trajectory of a bucket of a backhoe dredger according to claim 6, characterized in that: The method of defining a translation transformation vector of the backhoe dredger based on real-time updating of the correction value, defining a position prediction value of the bucket according to the translation transformation vector of the backhoe dredger and the bucket, and defining a corresponding trajectory based on a plurality of position prediction values ​​includes: Collecting calibration values; Update the correction value in real time; The translation transformation vector of the backhoe is defined based on real-time updating of the correction values.

8. The method for predicting the trajectory of a bucket of a backhoe dredger according to claim 7, characterized in that: The method further includes defining a translation transformation vector of the backhoe dredger based on real-time updating of the correction value, defining a position prediction value of the bucket according to the translation transformation vector of the backhoe dredger and the bucket, and defining a corresponding trajectory based on a plurality of position prediction values. Associating the translation transformation vector of the backhoe and the bucket; A predicted position value of the bucket is defined based on a translation transformation vector of the backhoe dredger and the bucket; A corresponding trajectory is defined based on the multiple position prediction values.

9. A backhoe dredger bucket trajectory prediction system, characterized in that: The backhoe dredger bucket trajectory prediction system is applied to the backhoe dredger bucket trajectory prediction method as claimed in any one of claims 1 to 8, and the backhoe dredger bucket trajectory prediction system comprises: An observation module is used to define an observation model of the bucket of the backhoe dredger according to the position of the bucket of the backhoe dredger: define an action space based on the position of the backhoe dredger; construct a corresponding coordinate system for the action space; define coordinates corresponding to the bucket based on the coordinate system and the position of the bucket of the backhoe dredger; define an observation model of the bucket of the backhoe dredger according to the coordinates corresponding to the bucket, known rotation transformation items, and predicted translation transformation items; A translational motion module, for constructing a translational motion model of a backhoe dredger based on a plurality of motion parameters of the backhoe dredger: defining a plurality of model data according to multiple interactions of the plurality of motion parameters of the backhoe dredger; and constructing a translational motion model of the backhoe dredger based on the plurality of model data; The external disturbance module is used to construct an external disturbance model of the backhoe dredger based on the environmental parameters around the backhoe dredger: in the process of modeling the environmental parameters around the backhoe dredger, the position disturbance variance, the velocity disturbance variance, and the acceleration bias disturbance variance are collected; the external disturbance model of the backhoe dredger is constructed based on the multiple interactions of the position disturbance variance, the velocity disturbance variance, and the acceleration bias disturbance variance; A prediction module, used to predict the next position of the bucket based on the observation model, the translational motion model, and the external disturbance model; A correction module, used to correct the next step position of the bucket and output the corresponding correction value; The trajectory module is used to define a translation transformation vector of the backhoe dredger based on real-time updating of the correction value, define a position prediction value of the bucket according to the translation transformation vector of the backhoe dredger and the bucket, and define a corresponding trajectory based on multiple position prediction values.

Images