Hilly and mountain tractor pose resolving system based on laser radar global coordinate system

Abstract

The invention belongs to the technical field of hilly and mountainous region tractor pose resolving, and particularly relates to a hilly and mountainous region tractor pose resolving system based on a laser radar global coordinate system, which comprises an adjustable direct current stabilized power supply, an industrial personal computer, a display module, a GPS / INS combination module, a base station module and a C16 line laser radar module, the GPS / INS combined module is integrated in a moving station to measure position and attitude information of a carrier coordinate system in real time, and the base station module and the GPS / INS module are used in a matched manner to mutually calculate position and attitude information measured by satellite signals; according to the hilly and mountainous region tractor pose resolving system, a global coordinate system is taken as a reference, the position and the pose of a carrier coordinate system are estimated in real time by acquiring parameters of a laser radar and an IMU (Inertial Measurement Unit), and then the precision of the real-time pose of a tractor in a hilly and mountainous region environment is improved by fusing data of the IMU and the laser radar by using an unscented Kalman filtering algorithm.

Description

Technical Field

[0001] The invention belongs to the technical field of tractor posture calculation in hilly and mountainous areas, and in particular relates to a tractor posture calculation system in hilly and mountainous areas based on a laser radar global coordinate system. Background Art

[0002] When tractors are operating in hilly and mountainous environments, satellite positioning is incomplete. Complex land types and vegetation occlusions reduce the accuracy and timeliness of satellite positioning. The inertial navigation system will cause the position and attitude accuracy of the measured carrier to decrease due to time accumulation errors, and the real-time update of the system will be delayed. On a global scale, the existing tractor positioning and attitude measurement mainly adopts high-precision multi-source combined navigation technology, including redundant positioning and attitude measurement methods based on multi-sensor data fusion such as the global satellite positioning system (GNSS), inertial navigation system (INS), and lidar. However, in actual applications, the problem of the global satellite positioning system satellite signal occlusion cannot accurately locate, and the inertial navigation system error accumulation causes the real-time measurement accuracy of the position and attitude of hilly and mountainous tractors to decrease. This is very inconvenient for real-time positioning and attitude measurement of hilly and mountainous tractors. Summary of the invention

[0003] The purpose of the present invention is to provide a hilly and mountainous tractor posture solution system based on a laser radar global coordinate system. The hilly and mountainous tractor posture solution system takes the global coordinate system as a reference, obtains the parameters of the laser radar and IMU in real time to estimate the position and posture of the carrier coordinate system, and then uses the unscented Kalman filter algorithm to fuse the data of the IMU and the laser radar to improve the real-time posture and accuracy of the tractor in a hilly and mountainous environment.

[0004] In order to achieve the above object, the present invention adopts the following technical solution: A hilly mountain tractor posture solution system based on a laser radar global coordinate system comprises an adjustable DC regulated power supply, an industrial computer, a display module, a GPS / INS combination module, a base station module and a C16-line laser radar module, wherein the adjustable DC regulated power supply provides a 24V rated voltage required by the industrial computer; the C16-line laser radar module scans point cloud data of the global coordinate system in real time, and obtains carrier posture change data through an integral algorithm; the GPS / INS combination module is integrated in a mobile station to measure the position posture information of the carrier coordinate system in real time, and the base station module and the GPS / INS module are used in conjunction with each other to calculate the posture information measured by satellite signals; the industrial computer receives the posture information of the GPS / INS combination module and the C16-line laser radar module and uses an unscented Kalman filter algorithm to perform data fusion processing, which is then used to correct the GPS / INS combination module; the display module outputs the change and processing process of the position posture information of the hilly mountain tractor carrier coordinate system.

[0005] Furthermore, the C16 line laser radar module includes a laser radar body, a three-wire segment, a network cable segment and a power adapter; the power adapter is used to power the laser radar body, and the network cable segment is used to transmit the point cloud data obtained by the laser radar body during operation to the industrial computer for posture calculation; the global coordinate system point cloud data obtained by the laser radar body is expressed as: P = [p x p y p z ], Establish the direction vector of the carrier coordinate system relative to the global coordinate system: Therefore, the carrier coordinate system posture change equation obtained from the laser radar point cloud data is expressed as:

[0006] Furthermore, the GPS / INS combination module includes a base station, a base station GNSS antenna, a mobile station, a mobile station GNSS antenna, a USB data interface line segment and a triangular bracket; the GPS / INS combination module obtains the position and attitude information of the carrier coordinate system by collecting satellite signals and integrating the inertia of INS, and transmits the position and attitude change information in the carrier coordinate system to the industrial computer through the USB interface; if w=[w x w y w z ] represents the angular velocity of the carrier coordinate system in the GPS / INS combination module, and △t represents the time interval. The calculation formula for the carrier coordinate system attitude change is expressed as:

[0007] Furthermore, with the support of the computing power of the industrial computer, the unscented Kalman filter algorithm is used to perform data fusion processing on the posture change information obtained by the two sensors; Create the state vector: According to the observed posture information, the observation vector is established, which is expressed as: The state transfer equation is expressed as: k|k-1 =f(x k-1|k-1 ,u)+w; The observation equation is expressed as: k =h(x k )+v k , the initial state estimate is x0, the initial variance matrix is ​​P0, the process noise covariance is Q, and the observation noise covariance is R; then, in the prediction process, the calculation expression of the sigma point is: χ k-1 =x k-1|k-1 Construct the i-th state prediction equation: The equations for calculating the forecast mean and covariance are: In the update step, the calculation equation for generating the observed sigma point is expressed as: The calculation equations for the observed mean, observed covariance and cross covariance are expressed as: The Kalman gain equation is expressed as: The state estimation and covariance update equations are: The pose information obtained after data fusion using the UKF algorithm is used to correct and compensate for the reduced accuracy or no satellite signal problems in the GPS / INS module.

[0008] Furthermore, the display module is connected to the display module via the DP interface of the industrial computer; the display module is used to display the posture change state of the body coordinate system relative to the global coordinate system under data fusion, so as to facilitate the driver to judge the control of the operation.

[0009] Furthermore, the industrial computer adopts the RCX-1430FR-PEG model, and the industrial computer can establish linkage with each sensor.

[0010] The beneficial effects of the present invention are as follows: the hilly mountain tractor posture solution system based on the laser radar global coordinate system provided by the present invention has the following advantages during specific operation and use: 1. Compared with the traditional GPS positioning, the laser radar system of the present invention is not affected by factors such as weather and occlusion in hilly and mountainous environments, and can maintain stable positioning and attitude measurement performance in complex hilly and mountainous environments; at the same time, compared with the traditional inertial navigation system positioning and attitude measurement method, it will not produce time accumulation errors. The laser radar body has good anti-interference ability when working, and the working accuracy and real-time performance are also reliably guaranteed; 2. The system of the present invention uses the global coordinate system as a reference object, and obtains the change information of the carrier coordinate system relative to the global coordinate system through real-time scanning of the laser radar body. The laser radar body has a strong data fusion capability and can perform data fusion with other sensors (such as the above-mentioned GPS / INS combined navigation module) to further improve the accuracy and reliability of posture solution; the position and posture of the hilly mountain tractor in the carrier coordinate system are obtained from the laser radar point cloud data, and the angular velocity information is obtained from the GPS / INS combined navigation module and integrated to obtain the posture information of the hilly mountain tractor. After processing using the UKF algorithm in the industrial computer, the accuracy and reliability of the posture estimation are effectively improved. The obtained information is used to correct the error accumulation of the INS system and assist the GPS in positioning and attitude measurement, which solves the problem of inaccurate positioning and attitude measurement of the GPS / INS combined module when the satellite signal is weakened or disappeared and the conventional time accumulation error problem of the INS system in a low-cost and efficient manner. BRIEF DESCRIPTION OF THE DRAWINGS

[0011] Figure 1 The system block diagram of the hilly mountain tractor posture solving system based on the laser radar global coordinate system of the present invention; Figure 2 This is a physical composition diagram of the hilly mountain tractor posture solving system based on the laser radar global coordinate system of the present invention; Figure 3 The present invention is a posture solution effect diagram of the method of the present invention obtained by taking the posture data obtained in the global coordinate system of the laser radar and the posture data calculated by the GPS / INS combination module as an example for data fusion; The numbers in the figure are: 1-LiDAR network cable, 2-LiDAR power supply, 3-LiDAR body, 4-Industrial computer display DP interface, 5-LiDAR network cable interface, 6-GPS / INS combined navigation mobile station USB interface, 7-Adjustable DC regulated power supply, 8-Industrial computer body, 9-LiDAR power plug, 10-Regulated DC power plug, 11-Display module, 12-Mobile station module, 13-Mobile station antenna, 14-Mobile station USB interface, 15-Base station module, 16-Base station antenna, 17-Industrial computer display module connecting line DP interface, 18-Connecting line display module interface. DETAILED DESCRIPTION

[0012] Specific embodiment 1: The technical scheme of the present invention will be clearly and completely described below in conjunction with the embodiments. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments in the present invention, all other embodiments obtained by ordinary technicians in the field without creative work are within the scope of protection of the present invention. It should be noted that: in the present invention, if there is no special description, all the embodiments and preferred implementation methods mentioned herein can be combined with each other to form a new technical scheme. In the present invention, if there is no special description, all the technical features and preferred features mentioned herein can be combined with each other to form a new technical scheme. In the present invention, if there is no special description, the components involved or their preferred components can be combined with each other to form a new technical scheme. The "range" disclosed in the present invention is in the form of lower limit and upper limit, which can be one or more lower limits, and one or more upper limits. In the present invention, unless otherwise specified, each reaction or operation step can be carried out sequentially or in sequence. Preferably, the reaction method in this article is carried out sequentially. Unless otherwise specified, the professional and scientific terms used in this article have the same meaning as those familiar to those skilled in the art. In addition, any method or material similar or equal to the recorded content can also be applied to the present invention.

[0013] As the specification of the present invention Figure 1 , Instruction Manual Figure 2 As shown, the present invention provides a hilly mountain tractor posture solution system based on a laser radar global coordinate system. The hilly mountain tractor posture solution system can use the laser radar as an auxiliary means to scan the feature points of the global coordinate system in real time to obtain a data point cloud. After performing technical operations on the data point cloud, the accurate and real-time position and posture information of the carrier coordinate system relative to the global coordinate system is obtained, and then the unscented Kalman filter algorithm is used to perform data fusion processing on the data of the GPS / INS combination module and the laser radar module in the industrial computer, and the fused posture information is used to correct and compensate for the information error of the GPS / INS combination module. Specifically, to achieve the above purpose, the present invention provides a hilly mountain tractor posture solution system based on a laser radar global coordinate system, which mainly includes an adjustable DC regulated power supply, an industrial computer, a display module, a GPS / INS combination module, a base station module and a C16 line laser radar module. The physical connection diagram of the hilly mountain tractor posture solution system is shown in the attached manual Figure 2As shown; the adjustable DC regulated power supply provides the 24V rated voltage required by the industrial computer; the C16-line laser radar module scans the point cloud data of the global coordinate system in real time, and obtains the carrier's attitude change data through an integration algorithm; the GPS / INS combination module is integrated in the mobile station to measure the position and attitude information of the carrier coordinate system in real time, and the base station module and the GPS / INS combination module are used in conjunction with each other to calculate the position and attitude information measured by satellite signals; the industrial computer receives the attitude information of the GPS / INS combination module and the C16-line laser radar module and uses the unscented Kalman filter algorithm for data fusion processing, which is then used to correct the GPS / INS combination module; the display module can output the changes and processing process of the position and attitude information of the hilly mountain tractor carrier coordinate system.

[0014] The C16 laser radar module in the present invention is installed at the center of mass of the carrier. The laser radar module as a whole includes a laser radar body, a three-wire wiring segment, a power adapter and a network cable segment about 1.5 meters long. The laser radar body uses a frequency of 20HZ to scan the surrounding environment to obtain point cloud data. In the point cloud data P = [p x p y p z ] is used to calculate the direction vector of the carrier relative to the global coordinate system, and the following calculation formula is established: The unit vector representation of the direction vector is: Therefore, the attitude angle calculation formula can be established based on the point cloud data of the laser radar: in, Represents the component corresponding to the unit direction vector.

[0015] The GPS / INS combination module and the base station module in the present invention are a complete set of satellite inertial navigation modules. The installation method is consistent with the C16 laser radar module, and the installation position is at the center of mass of the tractor in the hilly mountain area. As the core of the positioning and attitude determination technology, the expression for calculating the attitude change of the carrier coordinate system in the time interval △t and the angular velocity w obtained is: The attitude information obtained by the above two sensors in the present invention is fused in the industrial computer using the unscented Kalman filter algorithm and then output to the GPS / INS combination module for correcting the problem of GPS no signal and the problem of INS time accumulation error; Let the state vector be expressed as: The observation vectors obtained by the above two groups of sensors are expressed as: The state transfer equation is expressed as: k|k-1 =f(x k-1|k-1 ,u)+w; The observation equation is expressed as: k =h(xk )+v k , the initial state estimate is x0, the initial variance matrix is ​​P0, the process noise covariance is Q, and the observation noise covariance is R; then, in the prediction process, the calculation expression of the sigma point is: χ k-1 =x k-1|k-1 Construct the i-th state prediction equation: The equations for calculating the forecast mean and covariance are: In the update step, the calculation equation for generating the observed sigma point is expressed as: The calculation equations for the observed mean, observed covariance and cross covariance are expressed as: The Kalman gain equation is constructed as: The state estimation and covariance update equations are:

[0016] In summary, when the hilly mountain tractor posture solution system based on the laser radar global coordinate system of the present invention is in use, the adjustable DC regulated power supply converts the AC power into about 24V DC power and supplies it to the industrial computer; after receiving the C16 laser radar module point cloud data and the GPS / INS combination module posture information, the industrial computer uses the unscented Kalman filter algorithm to perform data fusion, and then corrects the compensation combination module and outputs the fused posture change information to the display module at the same time; the above is the specific working principle of the present invention, using the C16 laser radar module as an auxiliary tool, the global coordinate system is scanned in real time to obtain the global coordinate system point cloud data, and the point cloud data is inverted The pose change of the carrier coordinate system is deduced, and the pose information in the GPS / INS combined navigation module is combined with the unscented Kalman filter algorithm to fuse the pose information obtained from the lidar and GPS / INS. Then, the problems in the GPS / INS combined module are corrected and compensated to achieve accurate measurement and tracking of the pose and position of the carrier coordinate system, providing a reliable and accurate solution for the pose calculation of tractors in hilly and mountainous areas, avoiding duplication or omission of operations. Even in the operating environment where the vegetation is blocked and stable satellite signals cannot be received, the position and pose information of the carrier can be measured in real time with the assistance of the C16 line lidar, so that the arable land operation can be completed smoothly and efficiently. The above description is only a preferred embodiment of the present invention and does not limit the present invention in any form. Although the present invention has been disclosed as a preferred embodiment as above, it is not used to limit the present invention. Any technician familiar with this profession can make some changes or modify the technical contents disclosed above into equivalent embodiments without departing from the scope of the technical solution of the present invention. However, any simple modification, equivalent change and modification made to the above embodiments according to the technical essence of the present invention without departing from the content of the technical solution of the present invention still fall within the scope of the technical solution of the present invention.

Claims

1. A hilly mountain tractor posture solution system based on a laser radar global coordinate system, characterized by: It includes an adjustable DC regulated power supply, an industrial computer, a display module, a GPS / INS combination module, a base station module and a C16-line laser radar module, wherein the adjustable DC regulated power supply provides a 24V rated voltage required by the industrial computer; the C16-line laser radar module scans point cloud data of a global coordinate system in real time, and obtains attitude change data of a carrier through an integral algorithm; the GPS / INS combination module is integrated in a mobile station to measure the position and attitude information of a carrier coordinate system in real time, and the base station module and the GPS / INS module are used in conjunction with each other to calculate the position and attitude information measured by satellite signals; the industrial computer receives the attitude information of the GPS / INS combination module and the C16-line laser radar module and uses an unscented Kalman filter algorithm to perform data fusion processing, which is then used to correct the GPS / INS combination module; the display module outputs the change and processing process of the position and attitude information of a carrier coordinate system of a hilly mountain tractor.

2. The hilly mountain tractor posture solving system based on the laser radar global coordinate system according to claim 1 is characterized by: The C16 line laser radar module includes a laser radar body, a three-wire segment, a network cable segment and a power adapter; the power adapter is used to power the laser radar body, and the network cable segment is used to transmit the point cloud data obtained by the laser radar body during operation to the industrial computer for posture calculation; the global coordinate system point cloud data obtained by the laser radar body is expressed as: P = [p x p y p z ], Establish the direction vector of the carrier coordinate system relative to the global coordinate system: Therefore, the carrier coordinate system posture change equation obtained from the laser radar point cloud data is expressed as:

3. The hilly mountain tractor posture solving system based on the laser radar global coordinate system according to claim 1 is characterized by: The GPS / INS combination module includes a base station, a base station GNSS antenna, a mobile station, a mobile station GNSS antenna, a USB data interface line segment and a triangular bracket; the GPS / INS combination module obtains the position and attitude information of the carrier coordinate system by collecting satellite signals and integrating the inertia of INS, and transmits the position and attitude change information in the carrier coordinate system to the industrial computer through the USB interface; if w=[w x w y w z ] represents the angular velocity of the carrier coordinate system in the GPS / INS combination module, and △t represents the time interval. The calculation formula for the carrier coordinate system attitude change is expressed as:

4. The hilly mountain tractor posture solving system based on the laser radar global coordinate system according to claim 1 is characterized in that: With the support of the computing power of the industrial computer, the unscented Kalman filter algorithm is used to perform data fusion processing on the posture change information obtained by the two sensors; Create the state vector: According to the pose information, the observation vector is established, which is expressed as: The state transfer equation is expressed as: k|k-1 =f(x k-1|k-1 ,u)+w; The observation equation is expressed as: k =h(x k )+v k , The initial state estimate is x0, the initial variance matrix is ​​P0, the process noise covariance is Q, and the observation noise covariance is R; Then, during the prediction process, the calculation expression of the sigma point is: x k-1 =x k-1|k-1 Construct the i-th state prediction equation: The equations for calculating the forecast mean and covariance are: In the update step, the calculation equation for generating the observed sigma point is expressed as: The calculation equations for the observed mean, observed covariance and cross covariance are expressed as: The Kalman gain equation is expressed as: The state estimation and covariance update equations are: The pose information obtained after data fusion using the UKF algorithm is used to correct and compensate for the reduced accuracy or no satellite signal problems in the GPS / INS module.

5. The hilly mountain tractor posture solving system based on the laser radar global coordinate system according to claim 1 is characterized in that: The display module is connected to the display module via the DP interface of the industrial computer; the display module is used to display the posture change state of the three-dimensional coordinate system relative to the global coordinate system under data fusion, which is convenient for the driver to judge and control the working status of the tractor in the hilly and mountainous areas.

6. The hilly mountain tractor posture solving system based on the laser radar global coordinate system according to claim 1 is characterized by: The industrial computer adopts the RCX-1430FR-PEG model, and the industrial computer can establish linkage with each sensor.

Images