Engineering machinery pose determination method, device, engineering machinery and storage medium

Abstract

An embodiment of the present invention provides a method and device for determining the position and pose of a construction machine, the construction machine, and a storage medium, belonging to the technical field of construction machines. The construction machine includes a turntable, and the method includes: establishing a turntable three-dimensional coordinate system and a target three-dimensional coordinate system, where the origin of the turntable three-dimensional coordinate system is the intersection point of the joint surface of the turntable and the traveling device of the construction machine and the rotation axis of the turntable; detecting the heading angle, roll angle, and pitch angle of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system; obtaining the attitude of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system according to the heading angle, roll angle, and pitch angle; detecting the coordinates of a reference point located on the turntable in the target three-dimensional coordinate system; and obtaining the position and pose of the construction machine according to the coordinates of the reference point and the attitude of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system. The present invention can accurately obtain the position and pose with low cost.

Description

Technical Field

[0001] The present invention relates to the technical field of construction machinery, and particularly to a method and device for determining the position and pose of construction machinery, a construction machinery, and a storage medium. Background Art

[0002] The observation technology of the position and pose (the position and pose refer to the position and attitude) of construction machinery, such as excavators, rollers, etc., is one of the key technologies for realizing unmanned construction. At present, in the method for observing the position and pose of construction machinery based on differential GPS, most of them adopt observation schemes based on dual-differential GPS and inertial navigation, or three sets of differential GPS systems, etc., with relatively high costs. Summary of the Invention

[0003] The purpose of the embodiments of the present invention is to provide a method and device for determining the position and pose of construction machinery, a construction machinery, and a storage medium, which can accurately obtain the position and pose with low costs.

[0004] To achieve the above purpose, the embodiments of the present invention provide a method for determining the position and pose of a construction machinery. The construction machinery includes a turntable. The method includes: establishing a three-dimensional coordinate system of the turntable and a target three-dimensional coordinate system, where the origin of the three-dimensional coordinate system of the turntable is the intersection point of the joint surface of the turntable and the traveling device of the construction machinery and the rotation axis of the turntable; detecting the heading angle, roll angle, and pitch angle of the three-dimensional coordinate system of the turntable relative to the target three-dimensional coordinate system; obtaining the attitude of the three-dimensional coordinate system of the turntable relative to the target three-dimensional coordinate system according to the heading angle, roll angle, and pitch angle; detecting the coordinates of a reference point located on the turntable in the target three-dimensional coordinate system; and obtaining the position and pose of the construction machinery according to the coordinates of the reference point and the attitude of the three-dimensional coordinate system of the turntable relative to the target three-dimensional coordinate system.

[0005] Further, the method is based on a positioning antenna and a directional antenna installed on the turntable, and a base station communicating with the positioning antenna and the directional antenna. The heading angle is obtained through a differential GPS system composed of the positioning antenna, the directional antenna, and the base station.

[0006] Further, the position of the reference point is the installation position of the positioning antenna.

[0007] Further, obtaining the pose of the construction machinery according to the coordinates of the reference point and the pose of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system includes: establishing a reference three-dimensional coordinate system with the coordinates of the reference point as the origin; determining the pose of the reference three-dimensional coordinate system relative to the target three-dimensional coordinate system according to the coordinates of the reference point and the pose of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system; determining the coordinates of the origin of the turntable three-dimensional coordinate system in the reference three-dimensional coordinate system; determining the pose of the turntable three-dimensional coordinate system relative to the reference three-dimensional coordinate system according to the coordinates of the origin of the turntable three-dimensional coordinate system; and obtaining the pose of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system according to the pose of the reference three-dimensional coordinate system relative to the target three-dimensional coordinate system and the pose of the turntable three-dimensional coordinate system relative to the reference three-dimensional coordinate system.

[0008] Further, the construction machinery has a bucket. After determining the pose of the reference three-dimensional coordinate system relative to the target three-dimensional coordinate system, the method further includes: determining the pose equation of the turntable three-dimensional coordinate system relative to the reference three-dimensional coordinate system; obtaining the pose equation of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system according to the pose of the reference three-dimensional coordinate system relative to the target three-dimensional coordinate system and the pose equation of the turntable three-dimensional coordinate system relative to the reference three-dimensional coordinate system; determining the coordinates of the tip of the bucket in the turntable three-dimensional coordinate system; detecting the coordinates of the tip of the bucket in the target three-dimensional coordinate system; and obtaining the pose of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system according to the coordinates of the tip of the bucket in the turntable three-dimensional coordinate system, the detected coordinates of the tip of the bucket, and the pose equation of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system.

[0009] Further, obtaining the pose of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system according to the coordinates of the tip of the bucket in the turntable three-dimensional coordinate system, the detected coordinates of the tip of the bucket, and the pose equation of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system includes: determining the coordinate equation of the tip of the bucket in the target three-dimensional coordinate system according to the pose equation of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system and the coordinates of the tip of the bucket in the turntable three-dimensional coordinate system; determining the coordinates of the reference point in the turntable three-dimensional coordinate system according to the coordinate equation of the tip of the bucket and the detected coordinates of the tip of the bucket; and substituting the coordinates of the reference point in the turntable three-dimensional coordinate system into the pose equation of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system to obtain the pose of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system.

[0010] Further, the method is also based on a biaxial inclinometer installed on the turntable, and the two measurement axes are respectively parallel to the x-axis and the y-axis of the turntable coordinate system, and is used to measure the roll angle and the pitch angle.

[0011] An embodiment of the present invention further provides a pose determination device for a construction machine. The construction machine includes a turntable. The device includes: a coordinate system establishment unit, a detection unit, and a processing unit. Among them, the coordinate system establishment unit is used to establish a turntable three-dimensional coordinate system and a target three-dimensional coordinate system. The origin of the turntable three-dimensional coordinate system is the intersection point of the joint surface of the turntable and the traveling device of the construction machine and the rotation axis of the turntable; the detection unit is used to detect the heading angle, roll angle, and pitch angle of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system; the processing unit is used to obtain the pose of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system according to the heading angle, roll angle, and pitch angle; the detection unit is further used to detect the coordinates of a reference point located on the turntable in the target three-dimensional coordinate system; the processing unit is further used to obtain the pose of the construction machine according to the coordinates of the reference point and the pose of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system.

[0012] Further, the detection unit includes a positioning antenna and a directional antenna installed on the turntable, and a base station in communication with the positioning antenna and the directional antenna. The heading angle is obtained through a differential GPS system composed of the positioning antenna, the directional antenna, and the base station.

[0013] Further, the position of the reference point is the installation position of the positioning antenna.

[0014] Further, the coordinate system establishment unit is further used to establish a reference three-dimensional coordinate system with the coordinates of the reference point as the origin; the processing unit is further used to: determine the pose of the reference three-dimensional coordinate system relative to the target three-dimensional coordinate system according to the coordinates of the reference point and the pose of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system; determine the coordinates of the origin of the turntable three-dimensional coordinate system in the reference three-dimensional coordinate system; determine the pose of the turntable three-dimensional coordinate system relative to the reference three-dimensional coordinate system according to the coordinates of the origin of the turntable three-dimensional coordinate system; obtain the pose of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system according to the pose of the reference three-dimensional coordinate system relative to the target three-dimensional coordinate system and the pose of the turntable three-dimensional coordinate system relative to the reference three-dimensional coordinate system.

[0015] Further, the construction machinery has a bucket. After determining the pose of the reference three-dimensional coordinate system relative to the target three-dimensional coordinate system, the processing unit is further configured to: determine the pose equation of the turntable three-dimensional coordinate system relative to the reference three-dimensional coordinate system; obtain the pose equation of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system according to the pose of the reference three-dimensional coordinate system relative to the target three-dimensional coordinate system and the pose equation of the turntable three-dimensional coordinate system relative to the reference three-dimensional coordinate system; determine the coordinates of the tip of the bucket in the turntable three-dimensional coordinate system; the detection unit is further configured to detect the coordinates of the tip of the bucket in the target three-dimensional coordinate system; the processing unit is further configured to obtain the pose of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system according to the coordinates of the tip of the bucket in the turntable three-dimensional coordinate system, the detected coordinates of the tip of the bucket, and the pose equation of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system.

[0016] Further, the processing unit is further configured to: determine the coordinate equation of the tip of the bucket in the target three-dimensional coordinate system according to the pose equation of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system and the coordinates of the tip of the bucket in the turntable three-dimensional coordinate system; determine the coordinates of the reference point in the turntable three-dimensional coordinate system according to the coordinate equation of the tip of the bucket and the detected coordinates of the tip of the bucket; substitute the coordinates of the reference point in the turntable three-dimensional coordinate system into the pose equation of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system to obtain the pose of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system.

[0017] Further, the detection unit further includes a biaxial inclinometer installed on the turntable, and the two measurement axes are respectively parallel to the x-axis and y-axis of the turntable coordinate system, and are used to measure the roll angle and pitch angle.

[0018] An embodiment of the present invention further provides a construction machinery, which includes the construction machinery pose determination device described above.

[0019] An embodiment of the present invention further provides a machine-readable storage medium, on which instructions are stored, and the instructions are used to cause the machine to execute the construction machinery pose determination method described above.

[0020] Through the above technical solution, a three-dimensional coordinate system of the turntable and a three-dimensional coordinate system of the target are first established, then the heading angle, roll angle, and pitch angle of the three-dimensional coordinate system of the turntable relative to the three-dimensional coordinate system of the target are detected, and then according to the heading angle, roll angle, and pitch angle, the attitude of the three-dimensional coordinate system of the turntable relative to the three-dimensional coordinate system of the target is obtained. Then, the coordinates of a reference point located on the turntable in the three-dimensional coordinate system of the target are detected, and finally, according to the coordinates of the reference point and the attitude of the three-dimensional coordinate system of the turntable relative to the three-dimensional coordinate system of the target, the pose of the construction machinery is obtained. Using the above method to obtain the pose does not require two or even multiple differential GPS systems, and the cost is relatively low.

[0021] Other features and advantages of the embodiments of the present invention will be described in detail in the subsequent specific implementation part. Brief Description of the Drawings

[0022] The drawings are used to provide a further understanding of the embodiments of the present invention, and constitute a part of the specification. Together with the following specific implementation, they are used to explain the embodiments of the present invention, but do not constitute a limitation to the embodiments of the present invention. In the drawings:

[0023] Figure 1 is a flowchart of a method for determining the pose of construction machinery provided by an embodiment of the present invention;

[0024] Figure 2 is a structural diagram of a differential GPS system provided by an embodiment of the present invention;

[0025] Figure 3 is a schematic diagram of a coordinate system provided by an embodiment of the present invention;

[0026] Figure 4 is a flowchart of a method for determining the pose of construction machinery provided by another embodiment of the present invention;

[0027] Figure 5 is a flowchart of a method for determining the pose of construction machinery provided by another embodiment of the present invention;

[0028] Figure 6 is a flowchart of a method for determining the pose of construction machinery provided by another embodiment of the present invention;

[0029] Figure 7 is a structural block diagram of a device for determining the pose of construction machinery provided by an embodiment of the present invention.

[0030] Description of the Reference Numerals

[0031] 1 Coordinate system establishment unit 2 Detection unit

[0032] 3 Processing unit. Detailed Description of the Invention

[0033] The following will describe in detail the specific implementation manners of the embodiments of the present invention with reference to the accompanying drawings. It should be understood that the specific implementation manners described herein are only used to illustrate and explain the embodiments of the present invention, and are not used to limit the embodiments of the present invention.

[0034] Figure 1 is a flowchart of a method for determining the pose of a construction machine provided by an embodiment of the present invention. As Figure 1 shown, the construction machine includes a turntable, and the method includes:

[0035] Step S11, establish a turntable three-dimensional coordinate system and a target three-dimensional coordinate system, where the origin of the turntable three-dimensional coordinate system is the intersection point of the joint surface of the turntable and the traveling device of the construction machine and the rotation axis of the turntable;

[0036] Specifically, the turntable three-dimensional coordinate system can be expressed as G-xyz, and the origin is the intersection point of the joint surface of the turntable and the traveling device of the construction machine (i.e., a device that enables the construction machine to travel, such as a crawler) and the rotation axis of the turntable. Preferably, the z-axis is upward along the rotation axis, the y-axis is perpendicular to the z-axis and parallel to the plane where the working device (such as a boom, an arm, and a bucket) is located, and points in the direction of the bucket, and the x-axis is determined according to the right-hand rule. The target three-dimensional coordinate system can be expressed as M-XYZ. Preferably, the X-axis points to the due east direction of geography, the Y-axis points to the due north direction of geography, and the Z-axis is determined according to the right-hand rule. Of course, the coordinate axes of the turntable three-dimensional coordinate system G-xyz and the target three-dimensional coordinate system M-XYZ can also be set otherwise, and the present invention is not limited thereto.

[0037] Step S12, detect the heading angle, roll angle, and pitch angle of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system;

[0038] Specifically, the present invention preferably uses a biaxial inclinometer installed on the turntable, and the two measurement axes of which are respectively parallel to the x-axis and the y-axis of the turntable coordinate system, to measure the roll angle α and the pitch angle β. Of course, other methods such as an encoder can also be used for measurement. The roll angle α is the angle by which the turntable three-dimensional coordinate system G-xyz rotates around the y-axis of the turntable three-dimensional coordinate system G-xyz relative to the target three-dimensional coordinate system M-XYZ, and the pitch angle β is the angle by which the turntable three-dimensional coordinate system G-xyz rotates around the x-axis of the turntable three-dimensional coordinate system G-xyz relative to the target three-dimensional coordinate system M-XYZ. Since the terms of the roll angle α and the pitch angle β are relatively common, they will not be elaborated here.

[0039] The present invention also preferably uses a positioning antenna, a directional antenna installed on the turntable, and a base station to obtain the heading angle θ. The positioning antenna, the directional antenna, and the base station, etc. constitute a differential GPS system, and the system structure is as Figure 2As shown in the figure. The base station part of the differential GPS system is installed at any suitable location around the construction area to be constructed. It is required that there are no obstacles within 10 meters around the base station antenna and the radio transmitting antenna, and there is no object covering above. The mobile station part is installed on the main body of the construction machinery vehicle. The radio receiving antenna can be installed at a suitable position such as above the cab, and the receiver is placed at any position in the cab that does not affect other operations. The base station transmits the data received by the base station antenna through the radio and the radio transmitting antenna; through the radio receiving antenna, the receiver can receive the observation data of the base station antenna. Based on the observation data of the base station antenna and the observation data of the received positioning antenna and orientation antenna, the receiver can calculate and output the coordinates of the installation point N of the positioning antenna in the three-dimensional coordinate system of the base station M N( M x N , M y N , M z N ), and the construction machinery, such as the heading angle θ of the rotary table of the excavator.

[0040] Specifically, as Figure 3 shown in the figure, the position of the base station can directly be the position of the origin of the target three-dimensional coordinate system M-XYZ (it can also not be the position of the origin of the target three-dimensional coordinate system M-XYZ, but the coordinates of the origin of the target three-dimensional coordinate system M-XYZ need to be known). The positioning antenna is installed at point N, and the orientation antenna is installed at point P. The connection line between the two can be parallel to the xGz or yGz plane of the rotary table three-dimensional coordinate system G-xyz (parallel to xGz in the figure). Since the position at the time of installing the positioning antenna is known, that is, the coordinates of point N in the rotary table three-dimensional coordinate system G-xyz G N( G x N , G y N , G z N)It is known that by using the algorithms of a relatively mature differential GPS system, the coordinates of point P in the three-dimensional coordinate system G-xyz of the turntable can be obtained, as well as the angle between the projection N'P' of the vector NP in the XMY plane and the X-axis of the target three-dimensional coordinate system M-XYZ (if the connection line between the positioning antenna and the directional antenna is parallel to the yGz plane, the angle between N'P' and the Y-axis of the target three-dimensional coordinate system M-XYZ is obtained, and then it is converted into the angle with the X-axis in the due north direction), which is both the angle of rotation of the three-dimensional coordinate system G-xyz of the turntable relative to the target three-dimensional coordinate system M-XYZ around the Z-axis of the target three-dimensional coordinate system M-XYZ, that is, the heading angle θ. The above provides a method for obtaining the heading angle and the coordinates of the positioning antenna using the base station, the positioning antenna, and the directional antenna. However, it can be understood that other positioning methods can also be used to obtain the heading angle and the coordinates of the positioning antenna. Even in the case where there is no positioning antenna, the coordinates of other positioning devices installed on the turntable can be obtained through other positioning methods.

[0041] Step S13: Obtain the attitude of the three-dimensional coordinate system of the turntable relative to the target three-dimensional coordinate system according to the heading angle, roll angle, and pitch angle.

[0042] Specifically, based on Euler angles, a 3×3 matrix composed of the direction cosines of the three coordinate axes x, y, and z of the three-dimensional coordinate system G-xyz of the turntable relative to the target three-dimensional coordinate system M-XYZ is established to represent the attitude of the three-dimensional coordinate system G-xyz of the turntable relative to the target three-dimensional coordinate system M-XYZ, that is:

[0043] First, the attitudes of the three-dimensional coordinate system G-xyz of the turntable and the target three-dimensional coordinate system M-XYZ are the same. Rotate the three-dimensional coordinate system G-xyz of the turntable by an angle θ around the z-axis, then by an angle β around the y-axis, and finally by an angle α around the x-axis. According to the principle of "from left to right", arrange the matrices corresponding to each rotation, so as to obtain the attitude of the three-dimensional coordinate system G-xyz of the turntable relative to the target three-dimensional coordinate system M-XYZ as:

[0044]

[0045] where c is cos and s is sin, the same below, R(z G ,θ) is the rotation matrix of the three-dimensional coordinate system G-xyz of the turntable around the z-axis, R(y G ,β) is the rotation matrix of the three-dimensional coordinate system G-xyz of the turntable around the y-axis, and R(x G ,α) is the rotation matrix of the three-dimensional coordinate system G-xyz of the turntable around the x-axis.

[0046] Step S14: Detect the coordinates of the reference point located on the turntable in the target three-dimensional coordinate system.

[0047] Specifically, the reference point should be a point on the turntable with known coordinates, such as the position of a certain positioning device. Or in the case of using a positioning antenna and a base station, the reference point can be directly selected as the position N of the positioning antenna. If it is not at point N or the position of the positioning device, it can be at any point with a known distance from point N or the positioning device. Here, taking the reference point at point N as an example, the usage method for other points is the same.

[0048] Step S15: Obtain the pose of the construction machinery according to the coordinates of the reference point and the attitude of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system.

[0049] Specifically, as Figure 4 shown, it includes:

[0050] Step S41: Establish a reference three-dimensional coordinate system with the coordinates of the reference point as the origin.

[0051] Specifically, the attitude of the reference three-dimensional coordinate system is the same as that of the turntable three-dimensional coordinate system G-xyz.

[0052] Step S42: Determine the pose of the reference three-dimensional coordinate system relative to the target three-dimensional coordinate system according to the coordinates of the reference point and the attitude of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system.

[0053] Specifically, using the same principle of Euler angles as above, the pose of the reference three-dimensional coordinate system relative to the target three-dimensional coordinate system M-XYZ is:

[0054]

[0055] Step S43: Determine the coordinates of the origin of the turntable three-dimensional coordinate system in the reference three-dimensional coordinate system.

[0056] Specifically, the attitude of the reference three-dimensional coordinate system is the same as that of the turntable three-dimensional coordinate system G-xyz, and the coordinates of point N in the turntable three-dimensional coordinate system G-xyz are G N( G x N , G y N , G z N ). Then, in the reference three-dimensional coordinate system, the coordinates of the origin of the turntable three-dimensional coordinate system G-xyz are expressed as:

[0057] N G(- G x N ,- G y N ,- G z N ).

[0058] Step S44: Determine the pose of the turntable three-dimensional coordinate system relative to the reference three-dimensional coordinate system according to the coordinates of the origin of the turntable three-dimensional coordinate system.

[0059] Specifically, using the same Euler angle principle as above, the pose of the turntable three-dimensional coordinate system G-xyz relative to the reference three-dimensional coordinate system is:

[0060]

[0061] Step S45: Obtain the pose of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system according to the pose of the reference three-dimensional coordinate system relative to the target three-dimensional coordinate system and the pose of the turntable three-dimensional coordinate system relative to the reference three-dimensional coordinate system.

[0062] Specifically, multiply the pose of the reference three-dimensional coordinate system relative to the target three-dimensional coordinate system M-XYZ by the pose of the turntable three-dimensional coordinate system G-xyz relative to the reference three-dimensional coordinate system to obtain the pose of the turntable three-dimensional coordinate system G-xyz relative to the target three-dimensional coordinate system M-XYZ as:

[0063]

[0064] However, due to the manufacturing and installation errors of the excavator, the coordinates of point N in the turntable coordinate system G-xyz G N( G x N , G y N , G z N ) have certain errors, which sometimes seriously affect the accuracy of the excavator pose observation. The present invention provides a method for accurately calibrating the coordinates of point N. Specifically as follows:

[0065] Figure 5 is a flowchart of a method for determining the pose of a construction machine provided by another embodiment of the present invention. As Figure 5 shown, the construction machine has a bucket. After determining the pose of the reference three-dimensional coordinate system relative to the target three-dimensional coordinate system, the method further includes:

[0066] Step S51: Determine the pose equation of the turntable three-dimensional coordinate system relative to the reference three-dimensional coordinate system.

[0067] Specifically, as shown above, the pose of the turntable three-dimensional coordinate system G-xyz relative to the reference three-dimensional coordinate system is:

[0068]

[0069] Wherein, G xN , G y N , G z N all have errors. Therefore, G x N , G y N , G z N are all regarded as unknowns to form a pose equation.

[0070] Step S52: Obtain the pose equation of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system according to the pose of the reference three-dimensional coordinate system relative to the target three-dimensional coordinate system and the pose equation of the turntable three-dimensional coordinate system relative to the reference three-dimensional coordinate system;

[0071] Specifically, as described above, the pose of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system is:

[0072]

[0073] wherein, G x N , G y N , G z N are all regarded as unknowns to form a pose equation.

[0074] Step S53: Determine the coordinates of the tip of the bucket in the turntable three-dimensional coordinate system;

[0075] Specifically, using existing mature technologies, such as the technology for solving the forward kinematic equation of a 4-degree-of-freedom serial mechanism composed of a turntable, a boom, an arm, and a bucket, and the technology for detecting the joint angles of the boom, the arm, and the bucket using a single-axis inclinometer. Based on the joint angles of the boom, the arm, and the bucket detected by the single-axis inclinometer and the forward kinematic equation of the 4-degree-of-freedom serial mechanism composed of the turntable, the boom, the arm, and the bucket, the coordinates of the tip A of the bucket in the turntable three-dimensional coordinate system G-xyz can be obtained G A( G x A , G y A , G z A ).

[0076] Step S54: Detect the coordinates of the tip of the bucket in the target three-dimensional coordinate system;

[0077] Specifically, use an accurate detection method. For example, install a positioning antenna at the tip of the bucket and detect through a differential GPS system to obtain the coordinates of the tip of the bucket in the target three-dimensional coordinate system ).

[0078] Step S55: Obtain the pose of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system based on the coordinates of the tip of the bucket in the turntable three-dimensional coordinate system, the detected coordinates of the tip of the bucket, and the pose equation of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system.

[0079] Specifically, the detailed steps in step S55 are as Figure 6 shown, and the method includes:

[0080] Step S61: Determine the coordinate equation of the tip of the bucket in the target three-dimensional coordinate system based on the pose equation of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system and the coordinates of the tip of the bucket in the turntable three-dimensional coordinate system.

[0081] Specifically, expand the coordinates of the tip of the bucket A in the turntable three-dimensional coordinate system G-xyz G A( G x A , G y A , G z A ) into homogeneous coordinates, that is G A( G x A , G y A , G z A ,1), and then multiply by the pose equation of the turntable three-dimensional coordinate system G-xyz relative to the target three-dimensional coordinate system M-XYZ to obtain the coordinate equation of the tip of the bucket A in the target three-dimensional coordinate system M-XYZ M A. This coordinate equation M A is G x N , G y N , G z N 's function, that is:

[0082] M A= M x A ( G x N , G y N , G z N ), M y A ( G x N , G y N , G zN ), M z A ( G x N , G y N , G z N )]。

[0083] Step S62: Determine the coordinates of the reference point in the turntable three-dimensional coordinate system according to the coordinate equation of the bucket tip and the detected coordinates of the bucket tip;

[0084] Specifically, making the coordinates of the accurate bucket tip correspond equally to the coordinate equation M A, a system of equations can be obtained:

[0085]

[0086]

[0087]

[0088] Solving the above system of equations can obtain the accurate G x N , G y N , G z N 。

[0089] Step S63: Substitute the coordinates of the reference point in the turntable three-dimensional coordinate system into the pose equation of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system to obtain the pose of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system.

[0090] Specifically, substituting the accurate G x N , G y N , G z N into the pose equation of the turntable three-dimensional coordinate system G-xyz relative to the target three-dimensional coordinate system M-XYZ, the accurate pose of the turntable three-dimensional coordinate system G-xyz relative to the target three-dimensional coordinate system M-XYZ can be obtained.

[0091] Figure 7 is the structural block diagram of the pose determination device of the construction machinery provided by an embodiment of the present invention. As Figure 7As shown in the figure, the construction machinery includes a turntable. The device includes: a coordinate system establishment unit 1, a detection unit 2, and a processing unit 3. Among them, the coordinate system establishment unit 1 is used to establish a three-dimensional coordinate system of the turntable and a target three-dimensional coordinate system. The origin of the three-dimensional coordinate system of the turntable is the intersection point of the joint surface of the turntable and the traveling device of the construction machinery and the rotation axis of the turntable; the detection unit 2 is used to detect the heading angle, roll angle, and pitch angle of the three-dimensional coordinate system of the turntable relative to the target three-dimensional coordinate system; the processing unit 3 is used to obtain the attitude of the three-dimensional coordinate system of the turntable relative to the target three-dimensional coordinate system according to the heading angle, roll angle, and pitch angle; the detection unit 2 is also used to detect the coordinates of a reference point located on the turntable in the target three-dimensional coordinate system; the processing unit 3 is also used to obtain the pose of the construction machinery according to the coordinates of the reference point and the attitude of the three-dimensional coordinate system of the turntable relative to the target three-dimensional coordinate system.

[0092] Further, the detection unit 2 includes a positioning antenna and a directional antenna installed on the turntable, and a base station communicating with the positioning antenna and the directional antenna. The heading angle is obtained through a differential GPS system composed of the positioning antenna, the directional antenna, and the base station.

[0093] Further, the position of the reference point is the installation position of the positioning antenna.

[0094] Further, the coordinate system establishment unit 1 is also used to establish a reference three-dimensional coordinate system with the coordinates of the reference point as the origin; the processing unit 3 is also used to: determine the pose of the reference three-dimensional coordinate system relative to the target three-dimensional coordinate system according to the coordinates of the reference point and the attitude of the three-dimensional coordinate system of the turntable relative to the target three-dimensional coordinate system; determine the coordinates of the origin of the three-dimensional coordinate system of the turntable in the reference three-dimensional coordinate system; determine the pose of the three-dimensional coordinate system of the turntable relative to the reference three-dimensional coordinate system according to the coordinates of the origin of the three-dimensional coordinate system of the turntable; obtain the pose of the three-dimensional coordinate system of the turntable relative to the target three-dimensional coordinate system according to the pose of the reference three-dimensional coordinate system relative to the target three-dimensional coordinate system and the pose of the three-dimensional coordinate system of the turntable relative to the reference three-dimensional coordinate system.

[0095] Further, the construction machinery has a bucket. After determining the pose of the reference three-dimensional coordinate system relative to the target three-dimensional coordinate system, the processing unit 3 is further configured to: determine the pose equation of the turntable three-dimensional coordinate system relative to the reference three-dimensional coordinate system; obtain the pose equation of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system according to the pose of the reference three-dimensional coordinate system relative to the target three-dimensional coordinate system and the pose equation of the turntable three-dimensional coordinate system relative to the reference three-dimensional coordinate system; determine the coordinates of the tip of the bucket in the turntable three-dimensional coordinate system; the detection unit 2 is further configured to detect the coordinates of the tip of the bucket in the target three-dimensional coordinate system; the processing unit 3 is further configured to obtain the pose of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system according to the coordinates of the tip of the bucket in the turntable three-dimensional coordinate system, the detected coordinates of the tip of the bucket, and the pose equation of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system.

[0096] Further, the processing unit 3 is further configured to: determine the coordinate equation of the tip of the bucket in the target three-dimensional coordinate system according to the pose equation of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system and the coordinates of the tip of the bucket in the turntable three-dimensional coordinate system; determine the coordinates of the reference point in the turntable three-dimensional coordinate system according to the coordinate equation of the tip of the bucket and the detected coordinates of the tip of the bucket; substitute the coordinates of the reference point in the turntable three-dimensional coordinate system into the pose equation of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system to obtain the pose of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system.

[0097] Further, the detection unit 2 further includes a biaxial inclinometer installed on the turntable, and the two measurement axes are respectively parallel to the x-axis and the y-axis of the turntable coordinate system, and are used for measuring the roll angle and the pitch angle.

[0098] The embodiments of the pose determination device of the above construction machinery are similar to the embodiments of the pose determination method of the construction machinery described above, and will not be elaborated here.

[0099] The pose determination device of the construction machinery includes a processor and a memory. The above coordinate system establishment unit, detection unit, and processing unit, etc. are all stored in the memory as program units, and the corresponding functions are realized by the processor executing the above program units stored in the memory.

[0100] The processor contains a kernel, and the kernel retrieves the corresponding program units from the memory. One or more kernels can be set, and the pose of the construction machinery can be obtained by adjusting the kernel parameters.

[0101] The memory may include non-permanent memory in the form of computer-readable media, such as random access memory (RAM) and / or non-volatile memory, such as read-only memory (ROM) or flash RAM, and the memory includes at least one memory chip.

[0102] An embodiment of the present invention provides a storage medium on which a program is stored, and when the program is executed by a processor, the pose determination method of the construction machinery is implemented.

[0103] An embodiment of the present invention provides a processor for running a program, wherein when the program runs, the pose determination method of the construction machinery is executed.

[0104] An embodiment of the present invention provides a device, which includes a processor, a memory, and a program stored on the memory and executable on the processor. When the processor executes the program, the following steps are implemented:

[0105] Establish a turntable three-dimensional coordinate system and a target three-dimensional coordinate system. The origin of the turntable three-dimensional coordinate system is the intersection point of the joint surface of the turntable and the traveling device of the construction machinery and the rotation axis of the turntable; detect the heading angle, roll angle, and pitch angle of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system; obtain the pose of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system according to the heading angle, roll angle, and pitch angle; detect the coordinates of a reference point located on the turntable in the target three-dimensional coordinate system; obtain the pose of the construction machinery according to the coordinates of the reference point and the pose of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system.

[0106] Further, the method is based on a positioning antenna and a directional antenna installed on the turntable, and a base station communicating with the positioning antenna and the directional antenna. The heading angle is obtained through a differential GPS system composed of the positioning antenna, the directional antenna, and the base station.

[0107] Further, the position of the reference point is the installation position of the positioning antenna.

[0108] Further, obtaining the pose of the construction machinery according to the coordinates of the reference point and the attitude of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system includes: establishing a reference three-dimensional coordinate system with the coordinates of the reference point as the origin; determining the pose of the reference three-dimensional coordinate system relative to the target three-dimensional coordinate system according to the coordinates of the reference point and the attitude of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system; determining the coordinates of the origin of the turntable three-dimensional coordinate system in the reference three-dimensional coordinate system; determining the pose of the turntable three-dimensional coordinate system relative to the reference three-dimensional coordinate system according to the coordinates of the origin of the turntable three-dimensional coordinate system; and obtaining the pose of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system according to the pose of the reference three-dimensional coordinate system relative to the target three-dimensional coordinate system and the pose of the turntable three-dimensional coordinate system relative to the reference three-dimensional coordinate system.

[0109] Further, the construction machinery has a digging bucket. After determining the pose of the reference three-dimensional coordinate system relative to the target three-dimensional coordinate system, the method further includes: determining the pose equation of the turntable three-dimensional coordinate system relative to the reference three-dimensional coordinate system; obtaining the pose equation of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system according to the pose of the reference three-dimensional coordinate system relative to the target three-dimensional coordinate system and the pose equation of the turntable three-dimensional coordinate system relative to the reference three-dimensional coordinate system; determining the coordinates of the tip of the digging bucket in the turntable three-dimensional coordinate system; detecting the coordinates of the tip of the bucket in the target three-dimensional coordinate system; and obtaining the pose of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system according to the coordinates of the tip of the digging bucket in the turntable three-dimensional coordinate system, the detected coordinates of the tip of the bucket, and the pose equation of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system.

[0110] Further, obtaining the pose of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system according to the coordinates of the tip of the digging bucket in the turntable three-dimensional coordinate system, the detected coordinates of the tip of the bucket, and the pose equation of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system includes: determining the coordinate equation of the tip of the bucket in the target three-dimensional coordinate system according to the pose equation of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system and the coordinates of the tip of the digging bucket in the turntable three-dimensional coordinate system; determining the coordinates of the reference point in the turntable three-dimensional coordinate system according to the coordinate equation of the tip of the bucket and the detected coordinates of the tip of the bucket; and substituting the coordinates of the reference point in the turntable three-dimensional coordinate system into the pose equation of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system to obtain the pose of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system.

[0111] Further, the method is also based on a biaxial inclinometer installed on the turntable, with two measurement axes parallel to the x-axis and y-axis of the turntable coordinate system respectively, for measuring the roll angle and pitch angle.

[0112] The devices in this article can be servers, PCs, PADs, mobile phones, etc.

[0113] The present application also provides a computer program product, which when executed on a data processing device, is adapted to execute a program initialized with the following method steps:

[0114] Establish a turntable three-dimensional coordinate system and a target three-dimensional coordinate system, where the origin of the turntable three-dimensional coordinate system is the intersection point of the joint surface of the turntable and the traveling device of the construction machinery and the rotation axis of the turntable; detect the heading angle, roll angle, and pitch angle of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system; obtain the attitude of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system according to the heading angle, roll angle, and pitch angle; detect the coordinates of a reference point located on the turntable in the target three-dimensional coordinate system; obtain the pose of the construction machinery according to the coordinates of the reference point and the attitude of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system.

[0115] Further, the method is based on a positioning antenna and a directional antenna installed on the turntable, and a base station communicating with the positioning antenna and the directional antenna, and the heading angle is obtained through a differential GPS system composed of the positioning antenna, the directional antenna, and the base station.

[0116] Further, the position of the reference point is the installation position of the positioning antenna.

[0117] Further, obtaining the pose of the construction machinery according to the coordinates of the reference point and the attitude of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system includes: establishing a reference three-dimensional coordinate system with the coordinates of the reference point as the origin; determining the pose of the reference three-dimensional coordinate system relative to the target three-dimensional coordinate system according to the coordinates of the reference point and the attitude of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system; determining the coordinates of the origin of the turntable three-dimensional coordinate system in the reference three-dimensional coordinate system; determining the pose of the turntable three-dimensional coordinate system relative to the reference three-dimensional coordinate system according to the coordinates of the origin of the turntable three-dimensional coordinate system; obtaining the pose of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system according to the pose of the reference three-dimensional coordinate system relative to the target three-dimensional coordinate system and the pose of the turntable three-dimensional coordinate system relative to the reference three-dimensional coordinate system.

[0118] Further, the construction machinery has a bucket. After determining the pose of the reference three-dimensional coordinate system relative to the target three-dimensional coordinate system, the method further includes: determining the pose equation of the turntable three-dimensional coordinate system relative to the reference three-dimensional coordinate system; obtaining the pose equation of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system according to the pose of the reference three-dimensional coordinate system relative to the target three-dimensional coordinate system and the pose equation of the turntable three-dimensional coordinate system relative to the reference three-dimensional coordinate system; determining the coordinates of the tip of the bucket in the turntable three-dimensional coordinate system; detecting the coordinates of the tip of the bucket in the target three-dimensional coordinate system; obtaining the pose of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system according to the coordinates of the tip of the bucket in the turntable three-dimensional coordinate system, the detected coordinates of the tip of the bucket, and the pose equation of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system.

[0119] Further, obtaining the pose of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system according to the coordinates of the tip of the bucket in the turntable three-dimensional coordinate system, the detected coordinates of the tip of the bucket, and the pose equation of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system includes: determining the coordinate equation of the tip of the bucket in the target three-dimensional coordinate system according to the pose equation of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system and the coordinates of the tip of the bucket in the turntable three-dimensional coordinate system; determining the coordinates of the reference point in the turntable three-dimensional coordinate system according to the coordinate equation of the tip of the bucket and the detected coordinates of the tip of the bucket; substituting the coordinates of the reference point in the turntable three-dimensional coordinate system into the pose equation of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system to obtain the pose of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system.

[0120] Further, the method is also based on a biaxial inclinometer installed on the turntable, and the two measurement axes are respectively parallel to the x-axis and y-axis of the turntable coordinate system, and is used to measure the roll angle and pitch angle.

[0121] Those skilled in the art should understand that the embodiments of the present application can be provided as a method, a system, or a computer program product. Therefore, the present application can take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present application can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0122] This application is described with reference to the flowcharts and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the application. It should be understood that each flow and / or block in the flowchart and / or block diagram, and combinations of flows and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to the processors of general purpose computers, special purpose computers, embedded processors, or other programmable data processing devices to produce a machine, such that the instructions executed by the processors of the computer or other programmable data processing devices produce means for implementing the functions specified in the Figure 1 one or more flows and / or blocks Figure 1 one or more blocks.

[0123] These computer program instructions can also be stored in a computer-readable memory that can direct a computer or other programmable data processing device to work in a particular manner, such that the instructions stored in the computer-readable memory produce a manufacture including instruction means that implement the functions specified in the Figure 1 one or more flows and / or blocks Figure 1 one or more blocks.

[0124] These computer program instructions can also be loaded onto a computer or other programmable data processing device, such that a series of operation steps are executed on the computer or other programmable device to produce a computer-implemented process, so that the instructions executed on the computer or other programmable device provide steps for implementing the functions specified in the Figure 1 one or more flows and / or blocks Figure 1 one or more blocks.

[0125] In a typical configuration, a computing device includes one or more processors (CPUs), an input / output interface, a network interface, and memory.

[0126] The memory may include non-permanent memory in the form of computer-readable media, random access memory (RAM), and / or non-volatile memory such as read-only memory (ROM) or flash memory. The memory is an example of computer-readable media.

[0127] Computer readable media include permanent and non-permanent, removable and non-removable media that can be implemented by any method or technology to store information. Information can be computer readable instructions, data structures, program modules or other data. Examples of computer storage media include, but are not limited to, phase change memory (PRAM), static random access memory (SRAM), dynamic random access memory (DRAM), other types of random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other memory technology, compact disk read-only memory (CD-ROM), digital versatile disk (DVD) or other optical storage, magnetic cassettes, magnetic tape magnetic disk storage or other magnetic storage devices or any other non-transmission media that can be used to store information that can be accessed by a computing device. As defined herein, computer readable media does not include temporary computer readable media (transitory media), such as modulated data signals and carrier waves.

[0128] It should also be noted that the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, commodity or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, commodity or device. In the absence of more restrictions, the elements defined by the sentence "comprises a ..." do not exclude the existence of other identical elements in the process, method, commodity or device including the elements.

[0129] The above are only embodiments of the present application and are not intended to limit the present application. For those skilled in the art, the present application may have various changes and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application should be included within the scope of the claims of the present application.

Claims

1. A method for determining the pose of a construction machinery, the construction machinery including a slewing platform, characterized in that, The method includes: Establishing a three-dimensional coordinate system of the turntable and a three-dimensional coordinate system of the target. The origin of the three-dimensional coordinate system of the turntable is the intersection point of the joint surface of the turntable and the traveling device of the construction machinery and the rotation axis of the turntable; Detecting the heading angle, roll angle, and pitch angle of the three-dimensional coordinate system of the turntable relative to the three-dimensional coordinate system of the target; Obtaining the attitude of the three-dimensional coordinate system of the turntable relative to the three-dimensional coordinate system of the target according to the heading angle, roll angle, and pitch angle; Detecting the coordinates of a reference point located on the turntable in the three-dimensional coordinate system of the target; Obtaining the pose of the construction machinery according to the coordinates of the reference point and the attitude of the three-dimensional coordinate system of the turntable relative to the three-dimensional coordinate system of the target, wherein, obtaining the pose of the construction machinery according to the coordinates of the reference point and the attitude of the three-dimensional coordinate system of the turntable relative to the three-dimensional coordinate system of the target includes: Establishing a reference three-dimensional coordinate system with the coordinates of the reference point as the origin; Determining the pose of the reference three-dimensional coordinate system relative to the three-dimensional coordinate system of the target according to the coordinates of the reference point and the attitude of the three-dimensional coordinate system of the turntable relative to the three-dimensional coordinate system of the target; Determining the coordinates of the origin of the three-dimensional coordinate system of the turntable in the reference three-dimensional coordinate system; Determining the pose of the three-dimensional coordinate system of the turntable relative to the reference three-dimensional coordinate system according to the coordinates of the origin of the three-dimensional coordinate system of the turntable; Obtaining the pose of the three-dimensional coordinate system of the turntable relative to the three-dimensional coordinate system of the target according to the pose of the reference three-dimensional coordinate system relative to the three-dimensional coordinate system of the target and the pose of the three-dimensional coordinate system of the turntable relative to the reference three-dimensional coordinate system.

2. The method for determining the pose of the construction machinery according to claim 1, characterized in that, This method is based on a positioning antenna and a directional antenna installed on the turntable, and a base station communicating with the positioning antenna and the directional antenna. The heading angle is obtained through a differential GPS system composed of the positioning antenna, the directional antenna, and the base station.

3. The method for determining the pose of the construction machinery according to claim 2, characterized in that, The position of the reference point is the installation position of the positioning antenna.

4. The method for determining the pose of a construction machine according to claim 1, wherein the construction machine has a bucket, characterized in that After determining the pose of the reference three-dimensional coordinate system relative to the three-dimensional coordinate system of the target, the method further includes: Determining the pose equation of the three-dimensional coordinate system of the turntable relative to the reference three-dimensional coordinate system; Obtaining the pose equation of the three-dimensional coordinate system of the turntable relative to the three-dimensional coordinate system of the target according to the pose of the reference three-dimensional coordinate system relative to the three-dimensional coordinate system of the target and the pose equation of the three-dimensional coordinate system of the turntable relative to the reference three-dimensional coordinate system; Determining the coordinates of the tip of the bucket in the three-dimensional coordinate system of the turntable; Detecting the coordinates of the tip of the bucket in the three-dimensional coordinate system of the target; Obtaining the pose of the three-dimensional coordinate system of the turntable relative to the three-dimensional coordinate system of the target according to the coordinates of the tip of the bucket in the three-dimensional coordinate system of the turntable, the detected coordinates of the tip of the bucket, and the pose equation of the three-dimensional coordinate system of the turntable relative to the three-dimensional coordinate system of the target.

5. The method for determining the pose of a construction machine according to claim 4, characterized in that, Obtaining the pose of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system based on the coordinates of the tip of the bucket in the turntable three-dimensional coordinate system, the detected coordinates of the tip of the bucket, and the pose equation of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system includes: Determining the coordinate equation of the tip of the bucket in the target three-dimensional coordinate system according to the pose equation of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system and the coordinates of the tip of the bucket in the turntable three-dimensional coordinate system; Determining the coordinates of the reference point in the turntable three-dimensional coordinate system according to the coordinate equation of the tip of the bucket and the detected coordinates of the tip of the bucket; Substituting the coordinates of the reference point in the turntable three-dimensional coordinate system into the pose equation of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system to obtain the pose of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system.

6. The method for determining the pose of the construction machinery according to claim 1, wherein This method is also based on a biaxial inclinometer installed on the turntable, and the two measurement axes are respectively parallel to the x-axis and y-axis of the turntable coordinate system, and is used to measure the roll angle and pitch angle.

7. A pose determination device for a construction machine, the construction machine including a turntable, characterized in that This device includes: A coordinate system establishment unit, a detection unit, and a processing unit, where The coordinate system establishment unit is used to establish a turntable three-dimensional coordinate system and a target three-dimensional coordinate system, and the origin of the turntable three-dimensional coordinate system is the intersection of the joint surface of the turntable and the traveling device of the construction machinery and the rotation axis of the turntable; The detection unit is used to detect the heading angle, roll angle, and pitch angle of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system; The processing unit is used to obtain the pose of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system according to the heading angle, roll angle, and pitch angle; The detection unit is also used to detect the coordinates of the reference point located on the turntable in the target three-dimensional coordinate system; The processing unit is also used to obtain the pose of the construction machinery according to the coordinates of the reference point and the pose of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system, where the coordinate system establishment unit is also used to establish a reference three-dimensional coordinate system with the coordinates of the reference point as the origin; The processing unit is also used to: Determine the pose of the reference three-dimensional coordinate system relative to the target three-dimensional coordinate system according to the coordinates of the reference point and the pose of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system; Determine the coordinates of the origin of the turntable three-dimensional coordinate system in the reference three-dimensional coordinate system; Determine the pose of the turntable three-dimensional coordinate system relative to the reference three-dimensional coordinate system according to the coordinates of the origin of the turntable three-dimensional coordinate system; Obtain the pose of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system according to the pose of the reference three-dimensional coordinate system relative to the target three-dimensional coordinate system and the pose of the turntable three-dimensional coordinate system relative to the reference three-dimensional coordinate system.

8. The pose determination device for construction machinery according to claim 7, wherein The detection unit includes a positioning antenna and a directional antenna installed on the turntable, and a base station communicating with the positioning antenna and the directional antenna, and the heading angle is obtained through a differential GPS system composed of the positioning antenna, the directional antenna, and the base station.

9. The pose determination device for construction machinery according to claim 8, wherein The position of the reference point is the installation position of the positioning antenna.

10. The pose determination device of the construction machinery according to claim 7, wherein the construction machinery has a bucket, characterized in that After determining the pose of the reference three-dimensional coordinate system relative to the target three-dimensional coordinate system, The processing unit is further configured to: Determine the pose equation of the turntable three-dimensional coordinate system relative to the reference three-dimensional coordinate system; Based on the pose of the reference three-dimensional coordinate system relative to the target three-dimensional coordinate system and the pose equation of the turntable three-dimensional coordinate system relative to the reference three-dimensional coordinate system, obtain the pose equation of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system; Determine the coordinates of the tip of the bucket in the turntable three-dimensional coordinate system; The detection unit is further configured to detect the coordinates of the tip of the bucket in the target three-dimensional coordinate system; The processing unit is further configured to obtain the pose of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system according to the coordinates of the tip of the bucket in the turntable three-dimensional coordinate system, the detected coordinates of the tip of the bucket, and the pose equation of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system.

11. The pose determination device for construction machinery according to claim 10, characterized in that, The processing unit is further configured to: Determine the coordinate equation of the tip of the bucket in the target three-dimensional coordinate system according to the pose equation of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system and the coordinates of the tip of the bucket in the turntable three-dimensional coordinate system; Determine the coordinates of the reference point in the turntable three-dimensional coordinate system according to the coordinate equation of the tip of the bucket and the detected coordinates of the tip of the bucket; Substitute the coordinates of the reference point in the turntable three-dimensional coordinate system into the pose equation of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system to obtain the pose of the turntable three-dimensional coordinate system relative to the target three-dimensional coordinate system.

12. The pose determination device for construction machinery according to claim 7, characterized in that, The detection unit further includes a biaxial inclinometer installed on the turntable, and the two measurement axes are respectively parallel to the x-axis and y-axis of the turntable coordinate system, and are used to measure the roll angle and pitch angle.

13. An engineering machinery, characterized in that, This construction machinery includes the pose determination device of the construction machinery according to any one of claims 7-12.

14. A machine-readable storage medium, characterized in that, Instructions are stored on this machine-readable storage medium, and the instructions are used to cause the machine to execute the pose determination method of the construction machinery according to any one of claims 1-6.

Images