Positioning method for engineering machinery

Abstract

The invention relates to a positioning method for engineering machinery, the engineering machinery is provided with a camera, a satellite navigation module and an IMU module, the method comprises the following steps: obtaining image data from the camera, and obtaining visual SLAM coordinate data of the engineering machinery according to the image data; obtaining IMU data from an IMU module, and fusing the visual SLAM coordinate data and the IMU data to obtain first fused positioning data; and obtaining RTK data from the satellite navigation module, and fusing the first fused positioning data with the RTK data to obtain second fused positioning data. According to the method and the device, the image data of the camera, the IMU data and the RTK data are fused, so that accurate positioning of the engineering machinery can be realized no matter in an open-air environment or a non-open-air environment, and unmanned driving of the engineering machinery is facilitated.

Description

TECHNICAL FIELD

[0001] The present disclosure relates to the field of engineering machinery and electronics, in particular, to a positioning method for engineering machinery. BACKGROUND

[0002] The wave of unmanned technology has spread to the engineering machinery industry, and the first engineering machinery favored by unmanned technology is the road roller.

[0003] Firstly, the working condition of the road roller is simple and the action is single, and the unmanned technology is relatively easy to realize. Secondly, the large amplitude vibration generated during the operation of the road roller will damage the driver's body. Therefore, more and more manufacturers apply unmanned technology to road rollers. The positioning methods of the current mainstream unmanned road rollers are GNSS (Global Navigation Satellite System) positioning, laser radar positioning and IMU (Inertial Measurement Unit) positioning. The road roller is provided with a camera, and the camera is only used for identifying objects, but not for positioning the road roller.

[0004] However, in a non-open environment (such as indoors or in a tunnel) where satellite signals are difficult to reach, GNSS positioning technology cannot achieve accurate positioning; laser radar positioning is high in cost and cannot identify texture information; IMU positioning cannot guarantee long-time positioning accuracy due to error accumulation. SUMMARY

[0005] The starting point of the present disclosure is to provide a positioning method for engineering machinery to solve the above problems existing in the prior art.

[0006] Embodiments of the present disclosure provide a positioning method for engineering machinery, the method comprising:

[0007] obtaining image data from the camera, and obtaining visual SLAM (Simultaneous Localization and Mapping) coordinate data of the engineering machinery according to the image data;

[0008] obtaining IMU data from the IMU module, and fusing the visual SLAM coordinate data and the IMU data to obtain first fused positioning data;

[0009] acquire RTK (Real-time kinematic) data from the satellite navigation module, and fuse the first fused positioning data with the RTK data to obtain second fused positioning data representing a position and a motion trend of the engineering machine.

[0010] Optionally, acquiring the visual SLAM coordinate data of the engineering machine from the image data comprises:

[0011] preprocessing the image data;

[0012] extracting feature points and initializing the preprocessed image data, thereby obtaining a fundamental matrix between images, key frame image data, and an initial pose of the engineering machine;

[0013] matching current frame image data with key frame image data by means of the fundamental matrix and the initial pose, and obtaining a current pose of the engineering machine by solving a problem of minimizing a projection error;

[0014] performing loop detection and repositioning on the current pose of the engineering machine, thereby obtaining a global three-dimensional map including the visual SLAM coordinate data of the engineering machine.

[0015] Optionally, preprocessing the image comprises performing de-distortion processing and grayscale processing on the image data; using an ORB (Oriented Fast and Rotated Brief) algorithm to extract feature points from the image data; and / or using a RANSAC (Random Sample Consensus) algorithm to initialize the image data.

[0016] Optionally, fusing the visual SLAM coordinate data and the IMU data comprises:

[0017] fusing the visual SLAM coordinate data and the IMU data through IMU pre-integration and IMU initialization.

[0018] Optionally, fusing the first fused positioning data with the RTK data comprises:

[0019] using a nonlinear optimization method to tightly fuse the first fused positioning data with the RTK data.

[0020] Optionally, the nonlinear optimization method comprises an extended Kalman filter method, in which a state equation and an observation equation are adopted, wherein the state equation describes the dynamic behavior of the construction machine, and the observation equation describes the values of the image data, the IMU data and the RTK data of the camera.

[0021] Optionally, the state equation and the observation equation are in the form of:

[0022] x(k+1)=f(x(k),u(k))+w(k),

[0023] z(k)=h(x(k))+v(k),

[0024] wherein x represents a state vector of the construction machine, including position, velocity, attitude and sensor error; u represents a control vector of the construction machine, including acceleration and angular velocity of the construction machine; w and v are process noise and observation noise, respectively.

[0025] Optionally, before fusing the first fused positioning data with the RTK data, the method further comprises:

[0026] adjusting the RTK data to a state capable of being fused with the first fused positioning data by using a coarse-to-fine initialization method, wherein the coarse-to-fine initialization method comprises coarse positioning point positioning, yaw angle offset calibration and anchor point optimization.

[0027] Optionally, after obtaining the second fused positioning data, the method further comprises:

[0028] multiplying the second fused positioning data by a weight matrix, wherein different coefficients in the weight matrix represent the importance of the visual SLAM coordinate data and the RTK data in different environments, and the different environments include an open-air environment and a non-open-air environment.

[0029] Optionally, the first fused positioning data and the second fused positioning data comprise coordinate and velocity data of the construction machine.

[0030] The positioning method for the construction machine of the present disclosure has at least the following advantages:

[0031] In the present disclosure, the image data, the IMU data and the RTK data of the camera are fused, so that accurate positioning of the construction machine can be achieved in both open-air environments and non-open-air environments, which helps unmanned driving of the construction machine.

[0032] In the present disclosure, the image data is preprocessed by distortion removal processing and grayscale processing, thereby reducing computational complexity and improving computational accuracy.

[0033] In this disclosure, visual SLAM coordinate data and IMU data are fused through IMU pre-integration and IMU initialization, thereby achieving data fusion and reducing computational burden.

[0034] In this disclosure, a coarse-to-fine initialization method is used to adjust the RTK data to a state that can be fused with the first fused positioning data, thereby enabling the RTK data to be used in the fusion process.

[0035] In this disclosure, a weight matrix is ​​set up so that the fusion weight of image data and RTK data can be adjusted according to the different environments in which the construction machinery is located (e.g., open-air environment or non-open-air environment), thereby obtaining more accurate positioning data of the construction machinery in different environments. Attached Figure Description

[0036] Other details and advantages of this disclosure will become apparent from the detailed description provided below. It should be understood that the following figures are merely schematic and not drawn to scale, and therefore should not be considered as a limitation of this disclosure. A detailed description will follow with reference to the figures, in which:

[0037] Figure 1 A flowchart of a positioning method for engineering machinery according to a first embodiment of the present disclosure is shown.

[0038] Figure 2 The image shows an engineering machine equipped with a camera, satellite navigation module, and IMU module.

[0039] Figure 3 A flowchart of a positioning method for engineering machinery according to a second specific embodiment of the present disclosure is shown. Detailed Implementation

[0040] Embodiments of this disclosure are described below with reference to the accompanying drawings. In the following description, numerous specific details are set forth to enable those skilled in the art to more fully understand and implement this disclosure. However, it will be apparent to those skilled in the art that implementations of this disclosure may not include some of these specific details. Furthermore, it should be understood that this disclosure is not limited to the specific embodiments described. Rather, this disclosure may be practiced with any combination of the features and elements described below, regardless of whether they relate to different embodiments. Therefore, the following aspects, features, embodiments, and advantages are for illustrative purposes only and should not be construed as elements or limitations of the claims unless expressly set forth in the claims.

[0041] This invention proposes a positioning method for construction machinery, fusing camera image data, RTK data, and IMU data to achieve positioning of construction machinery (e.g., road rollers) in all scenarios (open and closed environments). Visual SLAM positioning based on camera image data does not require satellite signals, thus enabling positioning in closed environments where satellite signals are difficult to reach. However, visual SLAM positioning based on camera image data suffers from poor positioning accuracy in some situations (e.g., in strong sunlight), and its stability as a standalone positioning method does not meet industrial application standards. This invention, based on visual SLAM technology using camera image data, fuses RTK data and IMU data with visual SLAM coordinate data, thereby overcoming the poor positioning accuracy of visual SLAM in some situations. Therefore, low-cost, accurate positioning can be achieved in both open and closed environments, contributing to the autonomous driving of construction machinery.

[0042] Now refer to Figure 1 The diagram shows a flowchart of a positioning method for construction machinery according to a first specific embodiment of this disclosure. The construction machinery is equipped with a camera, a satellite navigation module, and an IMU module. Figure 2 Such engineering machinery is illustrated. This method can be performed by any suitable processing device or system with data transmission and reception capabilities, and these variations do not exceed the scope of protection of this invention. For example... Figure 1 As shown, the method includes the following steps:

[0043] Step S101: Acquire image data from the camera, and obtain visual SLAM coordinate data of the construction machinery based on the image data.

[0044] Specifically, the visual SLAM coordinate data of the construction machinery can be obtained from the image data in the following way:

[0045] First, the image data is preprocessed. Those skilled in the art will understand that any suitable method can be used to preprocess the image data, and these variations do not exceed the scope of this disclosure. For example, distortion correction and grayscale conversion can be performed on the image data to reduce computational complexity and improve computational accuracy.

[0046] Then, feature point extraction and initialization are performed on the preprocessed image data to obtain the basic matrix between images, keyframe image data, and the initial pose of the engineering machinery. Those skilled in the art will understand that any suitable method can be used for feature point extraction and initialization of the image data, and these variations do not exceed the scope of this disclosure. Specifically, the ORB algorithm can be used for feature point extraction of the image data, and the RANSAC algorithm can be used for initialization.

[0047] Next, the current frame image data is matched with the key frame image data using the basic matrix and the initial pose, and the current pose of the engineering machinery is obtained by solving the problem of minimizing the projection error.

[0048] Finally, loop closure detection and repositioning are performed on the current pose of the construction machinery to obtain a global 3D map, which includes the visual SLAM coordinate data of the construction machinery in the global 3D map.

[0049] Step S102: Obtain IMU data from the IMU module, and fuse the visual SLAM coordinate data and the IMU data to obtain the first fused positioning data.

[0050] Those skilled in the art will understand that visual SLAM coordinate data and IMU data can be fused in any suitable manner, and these variations do not exceed the scope of this disclosure. Specifically, visual SLAM coordinate data and IMU data can be fused through IMU pre-integration and IMU initialization. The first fused positioning data may include the coordinates and velocity data of the engineering machinery.

[0051] Step S103: Acquire RTK data from the satellite navigation module, and fuse the first fused positioning data with the RTK data to obtain second fused positioning data. The second fused positioning data may include the coordinates and speed data of the construction machinery.

[0052] Those skilled in the art will understand that the first fused positioning data and RTK data can be fused in any suitable manner, and these variations do not exceed the scope of protection of this disclosure. Specifically, a nonlinear optimization method can be used to tightly fuse the first fused positioning data and RTK data. Nonlinear optimization methods include, but are not limited to, the extended Kalman filter method, which employs state equations and observation equations. The state equations describe the dynamic behavior of the engineering machinery, and the observation equations describe the values ​​of the camera's image data, IMU data, and RTK data.

[0053] The specific forms of the state equations and observation equations can be:

[0054] x(k+1)=f(x(k),u(k))+w(k),

[0055] z(k) = h(x(k)) + v(k),

[0056] Where x represents the state vector of the construction machinery, including position, velocity, attitude and sensor error; u represents the control vector of the construction machinery, including acceleration and angular velocity; w and v are the process noise and observation noise, respectively.

[0057] In addition, before fusing the first fused positioning data with the RTK data, a coarse-to-fine initialization method can be used to adjust the RTK data to a state that allows for fusion with the first fused positioning data. The coarse-to-fine initialization method includes coarse positioning point localization, yaw angle offset calibration, and anchor point optimization.

[0058] Furthermore, after obtaining the second fused positioning data, it can be multiplied by a weight matrix. Different coefficients in the weight matrix represent the importance of visual SLAM coordinate data and RTK data in different environments, including both open-air and non-open-air environments. Using this weight matrix, more accurate coordinates and speed values ​​of the construction machinery can be obtained in different environments.

[0059] Those skilled in the art will understand that the positioning method for construction machinery in this disclosure can be used for any suitable construction machinery, such as a road roller, and these variations do not exceed the scope of protection of this disclosure.

[0060] Based on the principle of the positioning method for engineering machinery according to the first specific embodiment of this disclosure, Figure 3 A positioning method for engineering machinery according to a second embodiment of this disclosure is schematically illustrated. For example... Figure 3 As shown, the method includes:

[0061] Step S301: Acquire image data from a camera (e.g., a front-facing binocular camera) on the construction machinery and preprocess the image data. Preprocessing includes distortion correction and grayscale conversion of the image data. Preprocessing the image data can reduce computational complexity and improve computational accuracy.

[0062] Step S302: Extract and initialize feature points from the image data to obtain the fundamental matrix between images, keyframe image data, and the initial pose of the engineering machinery. The ORB algorithm can be used for feature point extraction, and the RANSAC algorithm can be used for initialization.

[0063] Step S303: Match the current frame image data with the key frame image data using the basic matrix and initial pose, and obtain the current pose of the engineering machinery by solving the problem of minimizing the projection error.

[0064] Step S304: Perform loop closure detection and repositioning operations on the current pose of the construction machinery to obtain a global 3D map established by ORB-SLAM3. The global 3D map includes the visual SLAM coordinate data of the construction machinery in the global 3D map.

[0065] Step S305: The visual SLAM coordinate data and IMU data from the IMU module are fused to obtain the first fused positioning data. This process mainly employs IMU pre-integration and IMU initialization to achieve the fusion process. Since the IMU data contains velocity data, the first fused positioning data can include the coordinates and velocity data of the construction machinery.

[0066] The IMU pre-integration process integrates (accumulates) the IMU outputs (angular velocity, acceleration) between the current frame image data and the keyframe image data, outputting the relative state (relative rotation, relative velocity, relative position) between the two frames. In step 303, there are two adjacent keyframe image data, i and j. The pose of keyframe i is relative to the world frame. If the IMU accelerometer readings are directly integrated on the pose of keyframe i to obtain the pose transformation of j relative to the world coordinate system, then during iterative optimization, after the pose of i changes, integration needs to be performed again. However, IMU pre-integration calculates the relative pose between the two frames, so it is not necessary to re-integrate all frames, thus reducing the computational burden.

[0067] When ORB-SLAM3 first starts running, the IMU module has not yet been initialized, and the system operates in pure vision mode. Map initialization is also completed during this stage, but the map coordinate system (world coordinate system) lacks gravity information at this point. When the number of keyframes with IMU pre-integrated data exceeds a certain threshold, IMU initialization begins. Using the accumulated keyframes and IMU pre-integrated data, the direction of gravity in the world coordinate system can be determined, thus obtaining the initial values ​​from the gravity coordinate system to the world coordinate system. After obtaining the rotation matrix from the world coordinate system to the gravity coordinate system, the z-axis of the world coordinate system can be aligned with the gravity direction. Subsequently, the pose of each keyframe is corrected, along with the pose of map points and the results of IMU pre-integration. Finally, the two are fused to obtain the first fused localization data.

[0068] Step S306: The first fused positioning data is fused with the RTK data from the satellite navigation module to obtain second fused positioning data, which may include the coordinates and velocity data of the construction machinery. This second fused positioning data represents the final determined position and movement trend of the construction machinery. By fusing the three types of data, the positioning accuracy and versatility of the construction machinery are improved.

[0069] Specifically, a nonlinear optimization method can be used to tightly fuse the first set of coordinate and velocity information with the RTK data; this nonlinear fusion process is also called tight coupling. The nonlinear optimization method can be the EKF (Extended Kalman Filter) method. This process can include state equations and observation equations. The state equations describe the dynamic behavior of the engineering machinery, including position, velocity, attitude, and sensor errors; the observation equations describe the system output, i.e., the measurements from the three sensors (camera, satellite navigation module, and IMU module). The model of the state equations and observation equations can be expressed as follows:

[0070] x(k+1)=f(x(k),u(k))+w(k)

[0071] z(k)=h(x(k))+v(k)

[0072] Where x represents the state vector, including position, velocity, attitude and sensor error; u represents the control vector, including roller acceleration and angular velocity; w and v are the process noise and observation noise, respectively.

[0073] In addition, to suppress unstable satellite signals, only RTK data from satellites that have been continuously locked for a certain period of time are allowed to be fused. Since RTK data is acquired via a slow satellite receiver wireless link (50 bits / second), it is unavailable until the corresponding RTK data is fully transmitted. Therefore, an initialization phase is needed to correctly initialize the system state of the nonlinear estimation, that is, to adjust the RTK data to a state that can be fused with the first fused positioning data. For this purpose, a coarse-to-fine initialization method can be adopted, which mainly includes three steps: (1) coarse positioning point positioning; (2) yaw angle offset calibration; and (3) anchor point optimization. Through this initialization process, the RTK data can be used for the fusion process.

[0074] After step 306, the second fused positioning data is finally obtained by fusing the visual SLAM coordinate data, IMU data and RTK data, so as to obtain the positioning coordinates and speed values ​​of the construction machinery more accurately and robustly.

[0075] Furthermore, since satellite navigation modules are better suited for positioning in open-air environments, while cameras are better suited for positioning in in-situ environments (e.g., indoors), this invention preferably incorporates an additional weight matrix. Different coefficients in the weight matrix represent the relative importance of the two types of sensor data (RTK data and camera image data) in different environments. Specifically, the second fused positioning data obtained in step S306 can be multiplied by this weight matrix. When the data is below a set threshold, the image data from the camera has a greater weight; when the data is above the threshold, the RTK data from the satellite navigation module has a greater weight. Therefore, after multiplying the second fused positioning data by this matrix and then by the weight matrix, more accurate positioning data for engineering machinery can be obtained in different environments.

[0076] Industrial applicability

[0077] In this disclosure, image data from cameras, IMU data, and RTK data are fused together, enabling precise positioning of construction machinery in both open and closed environments, which is helpful for the unmanned driving of construction machinery.

[0078] In this disclosure, image data is preprocessed through distortion correction and grayscale conversion, thereby reducing computational complexity and improving computational accuracy.

[0079] In this disclosure, visual SLAM coordinate data and IMU data are fused through IMU pre-integration and IMU initialization, thereby achieving data fusion and reducing computational burden.

[0080] In this disclosure, a coarse-to-fine initialization method is used to adjust the RTK data to a state that can be fused with the first fused positioning data, thereby enabling the RTK data to be used in the fusion process.

[0081] In this disclosure, a weight matrix is ​​set up so that the fusion weight of image data and RTK data can be adjusted according to the different environments in which the construction machinery is located (e.g., open-air environment or non-open-air environment), thereby obtaining more accurate positioning data of the construction machinery in different environments.

[0082] While this disclosure has been described above with reference to preferred embodiments, it is not limited thereto. Any modifications and alterations made by those skilled in the art without departing from the spirit and scope of this disclosure should be included within the scope of protection of this disclosure. Therefore, the scope of protection of this disclosure should be determined by the scope defined in the claims.

Claims

1. A positioning method for engineering machinery, characterized in that, The engineering machinery is equipped with a camera, a satellite navigation module, and an IMU module. The method includes: Acquire image data from the camera, and obtain visual SLAM coordinate data of the construction machinery based on the image data; IMU data from the IMU module is acquired, and the visual SLAM coordinate data and the IMU data are fused to obtain first fused positioning data; RTK data from the satellite navigation module is acquired, and the first fused positioning data is fused with the RTK data to obtain second fused positioning data.

2. The method according to claim 1, wherein, Obtaining the visual SLAM coordinate data of the engineering machinery based on the image data includes: The image data is preprocessed; Feature points are extracted and initialized from the preprocessed image data to obtain the basic matrix between images, key frame image data, and the initial pose of the engineering machinery. The current frame image data is matched with the key frame image data using the basic matrix and the initial pose, and the current pose of the engineering machinery is obtained by solving the problem of minimizing the projection error. The current pose of the construction machinery is subjected to loop closure detection and repositioning to obtain a global 3D map, which includes the visual SLAM coordinate data of the construction machinery.

3. The method according to claim 2, wherein, Preprocessing the image data includes distortion correction and grayscale conversion. The ORB algorithm is used to extract feature points from the image data; and / or, The image data is initialized using the RANSAC algorithm.

4. The method according to claim 1, wherein, The fusion of the visual SLAM coordinate data and the IMU data includes: The visual SLAM coordinate data and the IMU data are fused through IMU pre-integration and IMU initialization.

5. The method according to claim 1, wherein, The fusion of the first fused positioning data and the RTK data includes: A nonlinear optimization method is used to tightly fuse the first fused positioning data with the RTK data.

6. The method according to claim 5, wherein, The nonlinear optimization method includes the extended Kalman filter method, which employs state equations and observation equations. The state equations describe the dynamic behavior of the engineering machinery, and the observation equations describe the values ​​of image data, IMU data, and RTK data from the camera.

7. The method according to claim 6, wherein, The state equation and observation equation are in the following forms: x(k+1)=f(x(k),u(k))+w(k), z(k) = h(x(k)) + v(k), Where x represents the state vector of the construction machinery, including position, velocity, attitude and sensor error; u represents the control vector of the construction machinery, including acceleration and angular velocity; w and v are the process noise and observation noise, respectively.

8. The method according to claim 1, wherein, Before fusing the first fused positioning data with the RTK data, the method further includes: The RTK data is adjusted to a state that can be fused with the first fused positioning data using a coarse-to-fine initialization method. The coarse-to-fine initialization method includes coarse positioning point positioning, yaw angle offset calibration, and anchor point optimization.

9. The method according to claim 1, wherein, After obtaining the second fused positioning data, the method further includes: The second fused positioning data is multiplied by a weight matrix, where different coefficients in the weight matrix represent the importance of visual SLAM coordinate data and RTK data in different environments, including open-air and non-open-air environments.

10. The method according to claim 1, wherein, The first fused positioning data and the second fused positioning data include the coordinates and speed data of the engineering machinery.

Images