Three-dimensional terrain compensation method and system for special vehicle applications

Abstract

This application proposes a three-dimensional terrain compensation system for special vehicle applications, relating to the field of special vehicle application technology. The method includes: a vehicle attitude sensor for detecting the vehicle's attitude; a GNSS dual antenna for receiving satellite positioning information from the special vehicle; a steering control motor for controlling the vehicle's steering; a touchscreen display for displaying the vehicle's status information; and a core controller for controlling the vehicle's movement based on its attitude and satellite positioning information, controlling the steering via the steering control motor, and displaying the vehicle's status information on the touchscreen display. This invention, employing the above solution, can meet the automated control requirements of special vehicles in complex terrain.

Description

Technical Field

[0001] This application relates to the field of special vehicle application technology, and in particular to a three-dimensional terrain compensation method and system for special vehicle applications. Background Technology

[0002] Special vehicles are widely used in engineering construction, agricultural operations, fire safety, and national defense. Their working environments are often complex, requiring high work efficiency and necessitating automation and intelligent systems. Because special vehicles often operate on uneven terrain or with significant slopes, their automated control systems face considerable challenges. Traditional automated control systems are prone to instability or failure in complex terrain, leading to significant operational deviations and even threatening driving safety. Summary of the Invention

[0003] This application aims to at least partially address one of the technical problems in the related art.

[0004] Therefore, the first objective of this application is to propose a three-dimensional terrain compensation system for special vehicle applications, which solves the technical problem that existing methods are prone to instability or failure in complex terrain, leading to large operational deviations and even threatening driving safety, and meets the automated control requirements of special vehicles in complex terrain.

[0005] The second objective of this application is to propose a three-dimensional terrain compensation system for special vehicle applications.

[0006] To achieve the above objectives, the first aspect of this application proposes a three-dimensional terrain compensation system for special vehicle applications, comprising: a vehicle attitude sensor for detecting the vehicle attitude of the special vehicle; GNSS dual antennas for receiving satellite positioning information of the special vehicle; a steering control motor for controlling the steering of the special vehicle; a touch screen for displaying the status information of the special vehicle; and a core controller for controlling the movement of the special vehicle based on the vehicle attitude and satellite positioning information, controlling the steering of the special vehicle through the steering control motor, and displaying the status information of the special vehicle on the touch screen.

[0007] The three-dimensional terrain compensation system for special vehicle applications in this application estimates the vehicle pose by fusing vehicle attitude sensors and GNSS dual antennas, models and predicts the three-dimensional terrain, and compensates the control variables according to the terrain to meet the automated control requirements of special vehicles in complex terrain.

[0008] Optionally, in one embodiment of this application, the vehicle attitude sensor is horizontally mounted in the cab of a special vehicle by welding, with the vehicle attitude sensor aligned with the vehicle's orientation.

[0009] Optionally, in one embodiment of this application, the GNSS dual antenna includes a main antenna and a secondary antenna. The GNSS dual antenna is installed on the outer top of the special vehicle by welding. The connection line of the GNSS dual antenna is perpendicular to the central axis of the vehicle. The height of the main antenna and the secondary antenna are consistent, and the distance between the main antenna and the secondary antenna is greater than a preset distance.

[0010] Optionally, in one embodiment of this application, the core controller is installed in the cab of a special vehicle. The core controller includes: an RTK positioning module, a pose estimation module, a terrain modeling and prediction module, a three-dimensional terrain compensation calculation module, a control module, and a communication module.

[0011] The RTK positioning module is used to receive satellite positioning information from GNSS dual antennas and RTK differential data, and simultaneously obtain the elevation and heading angles of the dual antennas.

[0012] The pose estimation module is used to fuse satellite positioning information and vehicle attitude sensor data using an extended Kalman filter to estimate the vehicle's position and attitude, and obtain the vehicle's six-axis pose.

[0013] The terrain modeling and prediction module is used to model the terrain at the vehicle's location using the vehicle's six-axis pose and vehicle model to obtain the current three-dimensional terrain model, and to predict the terrain ahead using historical three-dimensional terrain models to obtain the predicted three-dimensional terrain model.

[0014] The 3D terrain compensation calculation module is used to solve the vehicle compensation amount based on the current and predicted 3D terrain model, as well as the vehicle's speed, angular velocity, and six-axis pose. The vehicle compensation amount is calculated through the vehicle model and motion model. The vehicle compensation amount includes lateral control compensation and position compensation. Lateral control compensation is used to compensate for the torque generated by the vehicle's roll angle on the vehicle's steering wheels, and position compensation is used to compensate for the deviation between the vehicle's center position projection and trajectory caused by uneven terrain.

[0015] The control module is used to compensate the control output based on the vehicle compensation amount, calculate the control quantity by combining the deviation between the vehicle position and the expected trajectory, and output it to the controller through the communication module.

[0016] Optionally, in one embodiment of this application, the pose estimation module includes a system modeling unit, an initialization unit, a prediction unit, an update unit, and a pose output unit, wherein...

[0017] The system modeling unit is used to establish the state space model and state vector of the special vehicle. The state vector includes the vehicle's position, velocity, and attitude, and the attitude includes pitch angle, roll angle, and yaw angle.

[0018] An initialization unit is used to initialize the state vector and the state covariance matrix corresponding to the state vector to obtain initial values. The initial values ​​of the state vector are the dual-antenna satellite position, heading angle information and the first measurement values ​​of the vehicle attitude sensor. The initial values ​​of the covariance matrix are randomly generated values.

[0019] The prediction unit is used at each time step to predict the next value of the state vector using the state transition equation and control input.

[0020] The update unit is used to calculate the measurement residual using the observation equation, adjust the predicted state vector using Kalman gain, and update the state covariance matrix when new dual-antenna satellite positioning information and vehicle attitude sensor data are received.

[0021] The pose output unit continuously receives positioning information from dual antennas and data from vehicle attitude sensors, calls the prediction unit and update unit to obtain real-time six-axis pose estimates and speeds of the vehicle body, and outputs them.

[0022] Optionally, in one embodiment of this application, the terrain at the vehicle's location is modeled to obtain a current three-dimensional terrain model, including:

[0023] The trajectory points of special vehicles are acquired, and the trajectory points are sampled at a preset frequency to obtain sparse terrain point clouds;

[0024] By combining the vehicle model and the vehicle posture corresponding to the trajectory points, the sparse terrain point cloud is expanded to obtain a dense terrain point cloud as the current 3D terrain model.

[0025] By using historical 3D terrain models, the terrain ahead is predicted to obtain a predicted 3D terrain model, including:

[0026] Based on the already modeled terrain point cloud, the terrain in front of the vehicle is predicted through 3D surface fitting.

[0027] Optionally, in one embodiment of this application, the control quantity includes a lateral control quantity and a longitudinal control quantity, and the control module is further configured to:

[0028] The lateral control signal is sent to the steering control motor so that the front wheels of the vehicle steer at the desired steering angle / steering angular velocity.

[0029] The longitudinal control input is sent to the throttle actuator and brake actuator to control the engine power output and braking force, so that the special vehicle can move according to the target acceleration / deceleration / target speed.

[0030] Optionally, in one embodiment of this application, a cable is also included for connecting the vehicle attitude sensor, GNSS dual antenna, steering control motor, and touch display to the core controller.

[0031] To achieve the above objectives, a second aspect of the present invention proposes a three-dimensional terrain compensation method for special vehicle applications, comprising: receiving satellite positioning information and RTK differential data from GNSS dual antennas, and simultaneously obtaining the elevation angle and heading angle of the dual antennas;

[0032] By using an extended Kalman filter to fuse satellite positioning information and vehicle attitude sensor data, the vehicle's position and attitude can be estimated, and the vehicle's six-axis pose can be obtained.

[0033] By using the vehicle's six-axis pose and vehicle model, the terrain at the vehicle's location is modeled to obtain the current three-dimensional terrain model. By using the historical three-dimensional terrain model, the terrain ahead is predicted to obtain the predicted three-dimensional terrain model.

[0034] The vehicle compensation is calculated based on the current and predicted 3D terrain model, as well as the vehicle's speed, angular velocity, and six-axis pose. The vehicle compensation is calculated using the vehicle model and motion model. The vehicle compensation includes lateral control compensation and position compensation. Lateral control compensation is used to compensate for the torque generated by the vehicle's roll angle on the vehicle's steering wheels, while position compensation is used to compensate for the deviation between the vehicle's center position projection and trajectory caused by uneven terrain.

[0035] The control output is compensated based on the vehicle compensation amount. The control quantity is calculated by combining the deviation between the vehicle position and the expected trajectory, and then output to the controller through the communication module.

[0036] Additional aspects and advantages of this application will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of this application. Attached Figure Description

[0037] The above and / or additional aspects and advantages of this application will become apparent and readily understood from the following description of the embodiments taken in conjunction with the accompanying drawings, wherein:

[0038] Figure 1 This is a schematic diagram of the structure of a three-dimensional terrain compensation system for special vehicle applications provided in Embodiment 1 of this application. Detailed Implementation

[0039] The embodiments of this application are described in detail below. Examples of these embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout. The embodiments described below with reference to the accompanying drawings are exemplary and intended to explain this application, and should not be construed as limiting this application.

[0040] The following description, with reference to the accompanying drawings, describes a three-dimensional terrain compensation method and system for special vehicle applications according to embodiments of this application.

[0041] Figure 1 This is a schematic diagram of the structure of a three-dimensional terrain compensation system for special vehicle applications provided in Embodiment 1 of this application.

[0042] like Figure 1 As shown, this is a three-dimensional terrain compensation system for special vehicle applications:

[0043] Vehicle attitude sensor 2 is used to detect the vehicle attitude of special vehicles;

[0044] GNSS dual antenna 1, used to receive satellite positioning information from special vehicles;

[0045] Steering control motor 3 is used to control the steering of special vehicles;

[0046] Touchscreen display 4 is used to display the status information of special vehicles;

[0047] The core controller 12 is used to control the movement of the special vehicle based on the vehicle's body posture and satellite positioning information, control the steering of the special vehicle through the steering control motor 3, and display the status information of the special vehicle on the touch screen 4.

[0048] The three-dimensional terrain compensation system for special vehicle applications in this application uses the vehicle attitude sensor 2 and GNSS dual antenna 1 to estimate vehicle pose, model and predict three-dimensional terrain, and compensate control quantities according to terrain to meet the automatic control requirements of special vehicles in complex terrain.

[0049] Optionally, in one embodiment of this application, the vehicle attitude sensor 2 is horizontally mounted in the cab of a special vehicle by welding, and the vehicle attitude sensor 2 is aligned with the vehicle body direction.

[0050] Optionally, in one embodiment of this application, the GNSS dual antenna 1 includes a main antenna and a secondary antenna. The GNSS dual antenna 1 is installed on the outer top of the special vehicle by welding. The connection line of the GNSS dual antennas is perpendicular to the central axis of the vehicle. The height of the main antenna and the secondary antenna are consistent, and the distance between the main antenna and the secondary antenna is more than 1m.

[0051] Optionally, in one embodiment of this application, the core controller 12 is installed in the cab of a special vehicle and powered by the vehicle's power supply, with a voltage range of 9-36V.

[0052] The core controller 12 includes: an RTK positioning module 8, a pose estimation module 9, a terrain modeling and prediction module 10, a 3D terrain compensation calculation module 11, a control module 6, and a communication module 7.

[0053] The RTK positioning module 8 is used to obtain centimeter-level positioning accuracy by receiving satellite positioning information from the GNSS dual antenna 1 and RTK differential data, and can also obtain the elevation angle and heading angle of the dual antennas.

[0054] The pose estimation module 9 is used to fuse dual-antenna positioning information and vehicle attitude sensor data to estimate the vehicle position and attitude. The fusion algorithm used is an extended Kalman filter to obtain the vehicle's six-axis pose, including three coordinates (northeast, high) and three angles (pitch, roll, and yaw).

[0055] The fusion localization process includes system modeling, initialization, prediction, and updating. First, a state-space model needs to be established. The state vector includes the vehicle's position (x, y, z coordinates) and velocity (V). x V y V z The initialization process involves several steps: 1) initializing the state vector and 2) initializing the attitude (pitch, roll, and yaw). Initialization aims to provide an initial estimate of the state vector and covariance matrix. For the state vector, the dual-antenna satellite position and yaw information, along with the first measurements from the vehicle attitude sensors, are used. For the covariance matrix, a larger initial value can be chosen to represent the initial uncertainty. 2) Prediction involves using the state transition equation and control inputs (such as acceleration) at each time step to predict the next value of the state vector, while simultaneously updating the state covariance matrix to represent the prediction uncertainty. 3) Update involves calculating the measurement residuals using the observation equations when new dual-antenna satellite positioning information and vehicle attitude sensor data are received. Then, Kalman gain is used to adjust the predicted state vector to be closer to the actual value, while simultaneously updating the state covariance matrix to represent the estimation uncertainty. After successful initialization, the system continuously receives dual-antenna positioning information and vehicle attitude sensor data, repeating the prediction and update steps to output real-time six-axis vehicle attitude estimates and velocity.

[0056] The terrain modeling and prediction module 10 is used to model the terrain at the vehicle's location using the vehicle's six-axis pose and vehicle model to obtain the current three-dimensional terrain model, and to predict the terrain ahead using the historical three-dimensional terrain model to obtain the predicted three-dimensional terrain model.

[0057] Terrain modeling requires storing the 3D positions and vehicle poses of historical trajectory points. Trajectory points are sampled at a certain frequency to obtain a sparse terrain point cloud. This point cloud is then expanded by combining the vehicle model and the corresponding vehicle poses of the trajectory points to obtain a denser terrain. Based on the modeled terrain point cloud, the terrain ahead of the vehicle can be predicted through 3D surface fitting.

[0058] The three-dimensional terrain compensation calculation module 11 is used to calculate the speed, angular velocity and attitude information of the vehicle based on the current and predicted three-dimensional terrain. On the one hand, there is a lateral slip force in the roll direction, which requires the calculation of the lateral control compensation term. On the other hand, the slope in the roll and pitch directions will cause the vehicle center projection point to be not on the rear wheel centerline, which will cause a non-negligible error for high-precision trajectory control, and position compensation is required.

[0059] Among them, lateral control compensation compensates for the torque generated by the vehicle's roll angle on the steering wheels, while position compensation compensates for the deviation between the vehicle's center position projection and the trajectory caused by uneven terrain. The compensation amount is calculated through the vehicle model and motion model. The compensation term is a proportional-integral-derivative (PID) controller, which will be used to adjust the vehicle's steering angle and improve the vehicle control accuracy under complex terrain.

[0060] Control module 6 is used to receive the compensation amount output by the three-dimensional terrain compensation calculation module, compensate the control output amount, calculate the control amount by combining the vehicle position and the deviation of the expected trajectory, and output it to the control module.

[0061] The control inputs are divided into lateral and longitudinal control inputs. The goal of lateral control is to make the vehicle travel along a predetermined trajectory. The output of the lateral control input is the desired steering angle or steering angular velocity. The system sends this control input to the steering control motor so that the front wheels of the vehicle steer at the desired steering angle. The goal of longitudinal control is to control the vehicle's speed and acceleration. The output of the longitudinal control input can be the desired acceleration, deceleration, or target speed. The system sends this control input to the throttle actuator and brake actuator to control the engine power output and braking force.

[0062] Optionally, in one embodiment of this application, a cable 5 is also included for connecting the vehicle attitude sensor 2, the GNSS dual antenna 1, the steering control motor 3, and the touch display screen 4 to the core controller 12.

[0063] After all modules of the three-dimensional terrain compensation system for special vehicle applications proposed in this application are installed, parameter calibration is first required to obtain vehicle model parameters and to compensate for sensor installation errors.

[0064] When the system starts working, the GNSS dual antennas acquire satellite positioning information, and the RTK positioning module calculates and obtains high-precision positioning data as well as dual antenna heading angle and roll angle data; the vehicle attitude sensor acquires the vehicle's three-axis attitude angle and angular velocity data; the data is transmitted through the communication module.

[0065] The attitude estimation module acquires data from the RTK positioning module and the vehicle attitude sensor, and fuses the sensor data using the extended Kalman filter algorithm to obtain high-precision vehicle pose results. The terrain modeling and prediction module receives the pose estimation results, models the terrain at the current position of the special vehicle, saves the current terrain data, and also predicts the terrain ahead, outputting it to the three-dimensional terrain compensation calculation module.

[0066] The 3D terrain compensation calculation module calculates two compensation items based on the terrain modeling results and pose estimation results. One item is the sideslip force caused by the roll angle, which needs to be compensated for by lateral control; the other item is the projection position error caused by the roll angle and pitch angle, which needs to be compensated for by position.

[0067] The control module receives data from the above modules, calculates the compensated control quantity, and outputs it to the steering control motor, thereby realizing automatic control of special vehicles with three-dimensional terrain compensation.

[0068] This system uses vehicle attitude sensors and GNSS dual antennas to obtain more accurate vehicle attitude estimation results, performs real-time terrain modeling and prediction, saves historical trajectory terrain data, and calculates vehicle control quantity compensation terms by combining three-dimensional terrain data. It also considers the sideslip force and position error caused by roll and pitch angles, thereby improving the stability and accuracy of automatic control of special vehicles in complex terrain, and enhancing their working efficiency and safety.

[0069] To achieve the above embodiments, this application also proposes a three-dimensional terrain compensation method for special vehicle applications.

[0070] The three-dimensional terrain compensation method for special vehicle applications includes: receiving satellite positioning information and RTK differential data from GNSS dual antennas, and simultaneously obtaining the elevation and heading angles of the dual antennas;

[0071] By using an extended Kalman filter to fuse satellite positioning information and vehicle attitude sensor data, the vehicle's position and attitude can be estimated, and the vehicle's six-axis pose can be obtained.

[0072] By using the vehicle's six-axis pose and vehicle model, the terrain at the vehicle's location is modeled to obtain the current three-dimensional terrain model. By using the historical three-dimensional terrain model, the terrain ahead is predicted to obtain the predicted three-dimensional terrain model.

[0073] The vehicle compensation is calculated based on the current and predicted 3D terrain model, as well as the vehicle's speed, angular velocity, and six-axis pose. The calculation includes lateral control compensation and position compensation.

[0074] The control output is compensated based on the vehicle compensation amount. The control quantity is calculated by combining the deviation between the vehicle position and the expected trajectory, and then output to the controller through the communication module.

[0075] It should be noted that the foregoing explanation of the embodiment of the three-dimensional terrain compensation system for special vehicle applications also applies to the three-dimensional terrain compensation method for special vehicle applications in this embodiment, and will not be repeated here.

[0076] In the description of this specification, the references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of this application. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples. Moreover, without contradiction, those skilled in the art can combine and integrate the different embodiments or examples described in this specification, as well as the features of different embodiments or examples.

[0077] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one of that feature. In the description of this application, "multiple" means at least two, such as two, three, etc., unless otherwise explicitly specified.

[0078] Any process or method description in the flowchart or otherwise herein can be understood as representing a module, segment, or portion of code comprising one or more executable instructions for implementing custom logic functions or processes, and the scope of the preferred embodiments of this application includes additional implementations in which functions may be performed not in the order shown or discussed, including substantially simultaneously or in reverse order depending on the functions involved, as should be understood by those skilled in the art to which embodiments of this application pertain.

[0079] The logic and / or steps represented in the flowchart or otherwise described herein, for example, can be considered as a sequenced list of executable instructions for implementing logical functions, and can be embodied in any computer-readable medium for use by, or in conjunction with, an instruction execution system, apparatus, or device (such as a computer-based system, a processor-included system, or other system that can fetch and execute instructions from, an instruction execution system, apparatus, or device). For the purposes of this specification, "computer-readable medium" can be any means that can contain, store, communicate, propagate, or transmit programs for use by, or in conjunction with, an instruction execution system, apparatus, or device. More specific examples (a non-exhaustive list) of computer-readable media include: an electrical connection having one or more wires (electronic device), a portable computer disk drive (magnetic device), random access memory (RAM), read-only memory (ROM), erasable and editable read-only memory (EPROM or flash memory), fiber optic devices, and portable optical disc read-only memory (CDROM). Alternatively, the computer-readable medium may be paper or other suitable media on which the program can be printed, since the program can be obtained electronically, for example, by optically scanning the paper or other medium, followed by editing, interpreting, or otherwise processing as necessary, and then stored in a computer memory.

[0080] It should be understood that various parts of this application can be implemented using hardware, software, firmware, or a combination thereof. In the above embodiments, multiple steps or methods can be implemented using software or firmware stored in memory and executed by a suitable instruction execution system. For example, if implemented in hardware as in another embodiment, it can be implemented using any one or a combination of the following techniques known in the art: discrete logic circuits having logic gates for implementing logical functions on data signals, application-specific integrated circuits (ASICs) having suitable combinational logic gates, programmable gate arrays (PGAs), field-programmable gate arrays (FPGAs), etc.

[0081] Those skilled in the art will understand that all or part of the steps of the methods in the above embodiments can be implemented by a program instructing related hardware. The program can be stored in a computer-readable storage medium, and when executed, the program includes one or a combination of the steps of the method embodiments.

[0082] Furthermore, the functional units in the various embodiments of this application can be integrated into a processing module, or each unit can exist physically separately, or two or more units can be integrated into a module. The integrated module can be implemented in hardware or as a software functional module. If the integrated module is implemented as a software functional module and sold or used as an independent product, it can also be stored in a computer-readable storage medium.

[0083] The storage medium mentioned above can be a read-only memory, a disk, or an optical disk, etc. Although embodiments of this application have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting this application. Those skilled in the art can make changes, modifications, substitutions, and variations to the above embodiments within the scope of this application.

Claims

1. A three-dimensional terrain compensation system for special vehicle applications, characterized in that, The application relates to a special vehicle control system, which comprises the following parts: a vehicle body posture sensor for detecting the posture of the special vehicle; a GNSS double-antenna for receiving satellite positioning information of the special vehicle; a steering control motor for controlling the steering of the special vehicle; a touch display screen for displaying the state information of the special vehicle; a core controller for controlling the movement of the special vehicle according to the vehicle body posture and the satellite positioning information, controlling the steering of the special vehicle through the steering control motor, and displaying the state information of the special vehicle on the touch display screen; wherein the core controller is installed in the cab of the special vehicle, and the core controller comprises an RTK positioning module, a posture estimation module, a terrain modeling and prediction module, a three-dimensional terrain compensation calculation module, a control module and a communication module, wherein the RTK positioning module is used for receiving satellite positioning information and RTK differential data of the GNSS double-antenna, and simultaneously obtaining the pitch angle and the heading angle of the double-antenna; the posture estimation module is used for fusing the satellite positioning information and the vehicle body posture sensor data by using an extended Kalman filter, realizing the estimation of the position and posture of the vehicle, and obtaining the six-axis posture of the vehicle; the terrain modeling and prediction module is used for modeling the terrain at the position of the vehicle by using the six-axis posture of the vehicle and a vehicle model, obtaining a current three-dimensional terrain model, and predicting the terrain in front by using a historical three-dimensional terrain model, obtaining a predicted three-dimensional terrain model; the three-dimensional terrain compensation calculation module is used for calculating the vehicle compensation amount based on the current and predicted three-dimensional terrain models and the speed, angular velocity and six-axis posture of the vehicle, wherein the vehicle compensation amount is calculated by using a vehicle model and a motion model, the vehicle compensation amount comprises a lateral control compensation and a position compensation, the lateral control compensation is used for compensating the torque generated by the roll angle of the vehicle on the steering wheel of the vehicle, and the position compensation is used for compensating the deviation of the projection of the center position of the vehicle from the track caused by the uneven terrain; the control module is used for compensating the control output amount according to the vehicle compensation amount, combining the deviation of the vehicle position from the expected track, calculating the lateral control amount and the longitudinal control amount, and sending the lateral control amount to the steering control motor through the communication module, so that the front wheel of the vehicle is steered according to the expected steering angle / steering angular velocity, sending the longitudinal control amount to the throttle actuator and the brake actuator through the communication module, so as to control the power output of the engine and the braking force, and realizing the movement of the special vehicle according to the target acceleration / deceleration / target speed; the modeling of the terrain at the position of the vehicle to obtain the current three-dimensional terrain model comprises the following steps: acquiring the track points of the special vehicle, sampling the track points according to a preset frequency, and obtaining sparse terrain point clouds; combining the vehicle model and the vehicle posture corresponding to the track points, expanding the sparse terrain point clouds, and obtaining dense terrain point clouds as the current three-dimensional terrain model; the prediction of the terrain in front by using the historical three-dimensional terrain model to obtain the predicted three-dimensional terrain model comprises the following steps: According to the terrain point cloud that has been modeled, the terrain in front of the vehicle is predicted through three-dimensional surface fitting.

2. The system of claim 1, wherein, The vehicle body posture sensor is horizontally installed in the cab of the special vehicle by welding, and the vehicle body posture sensor is aligned with the vehicle body direction.

3. The system of claim 1, wherein, The GNSS double antenna includes a main antenna and a sub antenna, and is installed on the outer top of the special vehicle by welding.

4. The system of claim 1, wherein, The pose estimation module includes a system modeling unit, an initialization unit, a prediction unit, an update unit, and a pose output unit. The system modeling unit is configured to establish a state space model and a state vector of the special vehicle. The initialization unit is configured to initialize the state vector and a state covariance matrix corresponding to the state vector to obtain initialization values. The prediction unit is configured to predict a next value of the state vector using a state transition equation and a control input at each time step. The update unit is configured to calculate a measurement residual using an observation equation when receiving new double-antenna satellite positioning information and vehicle body posture sensor data, adjust the predicted state vector using Kalman gain, and update the state covariance matrix. The pose output unit is configured to continuously receive double-antenna positioning information and vehicle body posture sensor data, call the prediction unit and the update unit, obtain real-time vehicle six-axis pose estimation values and speed, and output.

5. The system of claim 1, wherein, The method is implemented by the core controller of claim 1, and the method includes:

6. A three-dimensional terrain compensation method for special vehicle applications, characterized in that, receiving satellite positioning information and RTK differential data of the GNSS double antenna, and obtaining a double-antenna pitch angle and a heading angle at the same time; fusing the satellite positioning information and the vehicle body posture sensor data using an extended Kalman filter to estimate the vehicle position and attitude, and obtaining a six-axis pose of the vehicle; modeling the terrain at the position of the vehicle through the six-axis pose of the vehicle and a vehicle model, obtaining a current three-dimensional terrain model, and predicting the terrain in front through a historical three-dimensional terrain model to obtain a predicted three-dimensional terrain model; and ​ Based on the current and predicted three-dimensional terrain model and the speed, angular velocity and six-axis position of the vehicle, a vehicle compensation quantity is calculated, wherein the vehicle compensation quantity is calculated by a vehicle model and a motion model, and the vehicle compensation quantity includes lateral control compensation and position compensation, the lateral control compensation is used to compensate the torque generated by the vehicle roll angle on the vehicle steering wheel, and the position compensation is used to compensate the deviation of the vehicle center position projection from the track caused by the uneven terrain; According to the vehicle compensation quantity, the control output quantity is compensated, the lateral control quantity and the longitudinal control quantity are calculated by combining the vehicle position with the expected trajectory deviation, and the lateral control quantity is sent to the steering control motor through the communication module, so that the front wheels of the vehicle are steered according to the expected steering angle / steering angular velocity, the longitudinal control quantity is sent to the throttle actuator and the brake actuator through the communication module, so as to control the power output of the engine and the braking force, and the special vehicle is made to move according to the target acceleration / deceleration / target speed.

Images