CN114167858B - Navigation control method and device for vehicle, navigation controller and vehicle control system - Google Patents
Navigation control method and device for vehicle, navigation controller and vehicle control system Download PDFInfo
- Publication number
- CN114167858B CN114167858B CN202111339417.7A CN202111339417A CN114167858B CN 114167858 B CN114167858 B CN 114167858B CN 202111339417 A CN202111339417 A CN 202111339417A CN 114167858 B CN114167858 B CN 114167858B
- Authority
- CN
- China
- Prior art keywords
- data
- attitude
- vehicle
- navigation
- radar
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 63
- 230000033001 locomotion Effects 0.000 claims abstract description 50
- 238000007499 fusion processing Methods 0.000 claims abstract description 25
- 230000004927 fusion Effects 0.000 claims description 34
- 238000004891 communication Methods 0.000 claims description 28
- 238000012545 processing Methods 0.000 claims description 27
- 238000012937 correction Methods 0.000 claims description 13
- 238000005070 sampling Methods 0.000 claims description 13
- 230000001133 acceleration Effects 0.000 claims description 11
- 230000008569 process Effects 0.000 claims description 8
- 238000004364 calculation method Methods 0.000 claims description 2
- 238000010586 diagram Methods 0.000 description 11
- 230000005540 biological transmission Effects 0.000 description 6
- 238000004422 calculation algorithm Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 230000002123 temporal effect Effects 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000002457 bidirectional effect Effects 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000000802 evaporation-induced self-assembly Methods 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 238000009499 grossing Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000006855 networking Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 238000005295 random walk Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
- G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
- G05D1/0278—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle using satellite positioning signals, e.g. GPS
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Aviation & Aerospace Engineering (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Optics & Photonics (AREA)
- Electromagnetism (AREA)
- Navigation (AREA)
Abstract
The invention provides a navigation control method and device for a vehicle, a navigation controller and a vehicle control system, wherein the method comprises the following steps: acquiring first sensing data output by an inertial sensor, inputting the first sensing data into a preset motion model, outputting attitude prediction data, and updating model parameters of the motion model based on the attitude preset data; acquiring second sensing data, and generating attitude reference data based on the second sensing data; the data frequency of the second sensing data is lower than that of the first sensing data; and performing data fusion processing on the attitude prediction data and the attitude reference data to obtain navigation control parameters. According to the mode, the high-frequency sensing data and the low-frequency sensing data are processed separately, the attitude prediction data obtained through the high-frequency first sensing data update the parameters of the motion model, and the high control error caused by updating the operation model through the low-frequency data is avoided, so that the control precision of the vehicle track is improved.
Description
Technical Field
The invention relates to the technical field of automatic driving, in particular to a navigation control method and device of a vehicle, a navigation controller and a vehicle control system.
Background
In the automatic driving vehicle, a controller in the vehicle acquires sensing data such as the position and the running state of the vehicle through various sensors, and outputs a navigation control signal after the sensing data are fused and analyzed, so that the vehicle is controlled to run according to the navigation control signal. In the process, the frequencies of data collected by different sensors are different, and in order to realize the fusion of various sensing data, the down-sampling processing needs to be performed on the high-frequency sensing data, so that the frequency of the high-frequency sensing data is reduced, but the mode easily damages a motion model, so that a higher error is caused, and the control precision of a vehicle track is negatively influenced.
Disclosure of Invention
In view of this, the present invention provides a method and an apparatus for controlling vehicle navigation, a navigation controller, and a vehicle control system, so as to avoid a higher control error caused by updating an operation model with low-frequency data, and improve the control accuracy of a vehicle trajectory.
In a first aspect, embodiments of the present invention provide a navigation control method for a vehicle, which is applied to a navigation controller in the vehicle; the method comprises the following steps: acquiring first sensing data output by an inertial sensor, inputting the first sensing data into a preset motion model, outputting attitude prediction data, and updating model parameters of the motion model based on the attitude preset data; acquiring second sensing data, and generating attitude reference data based on the second sensing data; the second sensing data comprises positioning data, radar data and line control data; the data frequency of the second sensing data is lower than that of the first sensing data; and performing data fusion processing on the attitude prediction data and the attitude reference data to obtain navigation control parameters.
The step of performing data fusion processing on the attitude prediction data and the attitude reference data to obtain the navigation control parameters comprises the following steps: and inputting the attitude prediction data and the attitude reference data into a preset Kalman filter, and outputting navigation control parameters and Kalman gain values.
After the steps of inputting the attitude prediction data and the attitude reference data into a preset kalman filter and outputting navigation control parameters and a kalman gain value, the method further comprises the following steps: correcting the attitude prediction data based on the Kalman gain value to obtain attitude correction data; the error state in the motion model is updated by the pose correction data.
The step of performing data fusion processing on the attitude prediction data and the attitude reference data to obtain navigation control parameters includes: down-sampling the attitude prediction data to enable the data frequency of the attitude prediction data to be the same as that of the attitude reference data; and fusing the attitude prediction data subjected to the down-sampling processing and the attitude reference data to obtain navigation control parameters.
The navigation controller is respectively in communication connection with a radar sensor, a drive-by-wire sensor and a main controller in the vehicle; the method further comprises the following steps: and sending clock synchronization signals of a Media Access Control (MAC) layer to the radar sensor, the drive-by-wire sensor and the main controller based on a preset clock synchronization protocol.
The navigation controller is in communication connection with a main controller in the vehicle; the step of acquiring the second sensing data includes: acquiring radar data from a radar drive of a master controller; wherein the radar data includes a motion variation amount of a radar in the vehicle at a specified time; the radar data is obtained by analyzing and processing radar point cloud data acquired by a radar and calculating the position and pose through radar driving; acquiring drive-by-wire data from a drive-by-wire of a main controller; the drive-by-wire data comprises the running speed and the turning angle of the vehicle; and the drive-by-wire data is obtained by analyzing the chassis data of the vehicle through drive-by-wire.
The navigation controller is in communication connection with a main controller in the vehicle; after the step of performing data fusion processing on the attitude prediction data and the attitude reference data to obtain the navigation control parameters, the method further comprises the following steps: sending the navigation control parameters to an inertial navigation fusion drive of the main controller so as to control the running state of the vehicle through the inertial navigation fusion drive; wherein the navigation control parameters include: linear acceleration data, angular velocity data, position data, and rotational angle data in a plurality of specified dimensional directions.
In a second aspect, an embodiment of the present invention provides a navigation control apparatus for a vehicle, the apparatus being provided in a navigation controller in the vehicle; the device comprises: the first data processing module is used for acquiring first sensing data output by the inertial sensor, inputting the first sensing data into a preset motion model, outputting attitude prediction data and updating model parameters of the operation model based on the attitude preset data; the second data processing module is used for acquiring second sensing data and generating attitude reference data based on the second sensing data; the second sensing data comprises positioning data, radar data and line control data; the data frequency of the second sensing data is lower than that of the first sensing data; and the data fusion module is used for carrying out data fusion processing on the attitude prediction data and the attitude reference data to obtain the navigation control parameters.
In a third aspect, embodiments of the present invention provide a navigation controller, which includes a processor and a memory, where the memory stores machine executable instructions capable of being executed by the processor, and the processor executes the machine executable instructions to implement the above-mentioned navigation control method for a vehicle.
In a fourth aspect, an embodiment of the present invention provides a vehicle control system, including the above-mentioned navigation controller, main controller, inertial sensor, positioning device, radar sensor and drive-by-wire sensor; the navigation sensor is respectively in communication connection with the main controller, the inertial sensor, the positioning device, the radar sensor and the drive-by-wire sensor; the main controller is respectively connected with the radar sensor and the drive-by-wire sensor in a communication way.
In a fifth aspect, embodiments of the present invention provide a machine-readable storage medium storing machine-executable instructions that, when invoked and executed by a processor, cause the processor to implement the vehicle navigation control method described above.
The embodiment of the invention has the following beneficial effects:
according to the navigation control method and device, the navigation controller and the vehicle control system of the vehicle, the first sensing data output by the inertial sensor is obtained, the first sensing data is input into the preset motion model, the attitude prediction data is output, and the model parameters of the motion model are updated based on the attitude preset data; acquiring second sensing data, and generating attitude reference data based on the second sensing data; the second sensing data comprise positioning data, radar data and line control data; the data frequency of the second sensing data is lower than that of the first sensing data; and performing data fusion processing on the attitude prediction data and the attitude reference data to obtain navigation control parameters. According to the method, the high-frequency sensing data and the low-frequency sensing data are processed separately, the attitude prediction data obtained through the high-frequency first sensing data updates the parameters of the motion model, and the high control error caused by updating the operation model through the low-frequency data is avoided, so that the control precision of the vehicle track is improved.
Additional features and advantages of the invention will be set forth in the description which follows, and in part will be obvious from the description, or may be learned by the practice of the invention. The objectives and other advantages of the invention will be realized and attained by the structure particularly pointed out in the written description and claims hereof as well as the appended drawings.
In order to make the aforementioned and other objects, features and advantages of the present invention comprehensible, preferred embodiments accompanied with figures are described in detail below.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below, and it is obvious that the drawings in the following description are some embodiments of the present invention, and other drawings can be obtained by those skilled in the art without creative efforts.
Fig. 1 is a schematic scene diagram of a navigation control method for a vehicle according to an embodiment of the present invention;
FIG. 2 is a flowchart of a navigation control method for a vehicle according to an embodiment of the present invention;
FIG. 3 is a diagram illustrating a data fusion process according to an embodiment of the present invention;
FIG. 4 is a diagram illustrating communication data transmission of a navigation system according to an embodiment of the present invention;
fig. 5 is a schematic diagram of a time synchronization communication layer according to an embodiment of the present invention;
fig. 6 is a schematic diagram of a navigation control device of a vehicle according to an embodiment of the present invention;
fig. 7 is a schematic structural diagram of an electronic device according to an embodiment of the present invention;
fig. 8 is a schematic structural diagram of a vehicle control system according to an embodiment of the present invention.
Detailed Description
To make the objects, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
As shown in fig. 1, the navigation controller is respectively in communication connection with an Inertial sensor, a positioning device, a radar sensor and a drive-by-wire sensor in the vehicle, and obtains sensing data acquired by various types of sensors and performs fusion processing on the sensing data, wherein the Inertial sensor generally comprises a gyroscope, an accelerator, an algorithm processing Unit and the like, and is used for detecting and measuring acceleration, inclination, impact, vibration, rotation and multiple degree of freedom motion parameters, and vehicle position and attitude information and the like can be obtained through the Inertial sensor data. The positioning device comprises a satellite positioning device (GNSS), a map matching positioning device, a multi-sensor fusion positioning device and the like, and is used for positioning longitude, latitude and height and acquiring information including position, speed, time and the like. Radar sensors (LiDAR) can be classified into a millimeter wave radar, a decimetric wave radar, a centimeter wave radar, a millimeter wave radar, a laser/infrared radar and the like according to the working wavelength of the LiDAR, and the data of the LiDAR sensors are used for acquiring the distance, the speed and the like of obstacles to realize the identification, classification and tracking of the obstacles. The drive-by-wire sensor transmits data of the chassis of the vehicle body including the running state of the vehicle, the speed per hour, the gear position, the turning angle and the like through drive-by-wire (also called as a wheel speed meter). It should be noted that the inertial sensor data is high-frequency data, that is, the first sensing data, and the positioning data, the radar data, and the line control data have a frequency lower than that of the inertial sensor data and belong to second sensing data.
In order to facilitate understanding of the present embodiment, a method for controlling navigation of a vehicle disclosed in an embodiment of the present invention is first described in detail, and as shown in fig. 2, the method is applied to a navigation controller in a vehicle; the method comprises the following steps:
step S202, acquiring first sensing data output by an inertial sensor, inputting the first sensing data into a preset motion model, outputting attitude prediction data, and updating model parameters of the motion model based on the attitude prediction data;
the navigation controller acquires external environment information in real time through different types of sensors, establishes an external environment model, and predicts the possible running state of the vehicle in advance through a preset motion model so as to complete path planning. Therefore, real-time updating of the motion model parameters is required, and the inertial sensor components include: the system comprises a gyroscope, an accelerator and an algorithm processing unit, wherein the inertial navigation sensor is used for detecting inertial data, perceiving the pose of the vehicle through the detected inertial data, and measuring and outputting acceleration, inclination, impact, vibration, rotation and multi-degree-of-freedom motion information of the vehicle.
In the related art, the inertial sensor for a conventional vehicle or an autonomous vehicle is characterized by a high update frequency (up to 100Hz or more), and because of the characteristic of high-frequency output, downsampling is usually implemented by filtering and interpolating data of the inertial sensor to ensure that the frequencies of output data of a plurality of sensors are consistent, so as to implement data fusion.
Based on the above, in the step, after the first data output by the inertial sensor is processed by the preset motion model, the attitude prediction data is output, and the parameters in the motion model are independently updated based on the attitude prediction data.
The motion model may be implemented by a pre-integration model, and parameters of the model may include world coordinate system parameters, IMU body coordinate system parameters, gyroscope parameters, accelerometer parameters, and the like.
Step S204, acquiring second sensing data, and generating attitude reference data based on the second sensing data; the second sensing data comprises positioning data, radar data and line control data; the data frequency of the second sensing data is lower than that of the first sensing data;
the second sensing data comprises positioning data, radar data and line control data, wherein the positioning data relates to various sensor types and sensing modes, for example, the positioning data comprises global satellite positioning (GNSS), inertial navigation positioning, map matching positioning, multi-sensor fusion positioning and other modes, the GNSS positioning technology combines a plurality of sets of satellite navigation positioning technologies to send navigation signals, and required position, speed and time information can be provided for the automatic driving automobile.
And collecting point cloud data by the radar sensor. Various radar sensors, for example, a laser radar sensor and a millimeter wave radar sensor, may be installed in the vehicle. And after the point cloud data is collected by the radar sensor, the point cloud data is sent to a radar driver in the main unit. And the radar drive analyzes the point cloud data of the radar sensor to obtain radar data. If the radar sensors comprise multiple types, the point cloud data of each type of radar sensor is analyzed to obtain multiple initial radar data, and then the multiple initial radar data are averaged or otherwise processed to obtain final radar data. A navigation controller of the vehicle acquires radar data from radar drive of a main controller, wherein the radar data comprises movement variation of a radar in the vehicle at a specified moment; the radar data is obtained by analyzing and processing radar point cloud data acquired by a radar and calculating the pose through radar driving.
The drive-by-wire sensor acquires chassis data of a vehicle, wherein the chassis data is mainly used for indicating the running state of the vehicle, and the drive-by-wire sensor transmits the chassis data to the drive-by-wire in the main unit; the drive-by-wire is used for analyzing the chassis data of the vehicle body to obtain the running state data of the running speed, the turning angle, the acceleration, the accelerator state, the braking state, the gear position and the like of the vehicle. Then, a navigation controller of the vehicle acquires Drive-by-wire data from a Drive-by-wire of a main controller; the drive-by-wire data herein includes the running state data of the aforementioned vehicle, for example, running speed, turning angle, acceleration, accelerator state, brake state, gear position, etc. of the vehicle; the drive-by-wire data is obtained by analyzing the chassis data of the vehicle through drive-by-wire.
The data accuracy of the positioning data, the radar data, the line control data and the like is high, the accuracy can reach centimeter level, but the acquisition frequency is low, generally, the acquisition frequency is Hertz of unit level, and the data frequency of the sensing data is lower than that of the first sensing data output by the inertial sensor. Since the data frequency of the second sensing data is lower than that of the first sensing data, the data frequency of the attitude reference data is also lower than that of the attitude prediction data based on this.
Attitude reference data is generated based on the second sensing data, and the attitude reference data can comprise attitude information and driving information such as speed per hour, gear, corner, position, distance and the like.
And step S206, performing data fusion processing on the attitude prediction data and the attitude reference data to obtain navigation control parameters.
The attitude prediction data and the attitude reference data are subjected to data fusion processing, and the data fusion processing can be realized through various data fusion algorithms or models. And obtaining more accurate positioning by using the attitude prediction data and the attitude reference data through data association, correlation, combination and other modes to obtain convincing and implementable navigation control parameters.
The navigation control method of the vehicle obtains first sensing data output by an inertial sensor, inputs the first sensing data into a preset motion model, outputs attitude prediction data, and updates model parameters of the motion model based on the attitude preset data; acquiring second sensing data, and generating attitude reference data based on the second sensing data; the second sensing data comprises positioning data, radar data and line control data; the data frequency of the second sensing data is lower than that of the first sensing data; and performing data fusion processing on the attitude prediction data and the attitude reference data to obtain navigation control parameters. According to the method, the high-frequency sensing data and the low-frequency sensing data are processed separately, the attitude prediction data obtained through the high-frequency first sensing data update the parameters of the motion model, and the high control error caused by updating the operation model through the low-frequency data is avoided, so that the control precision of the vehicle track is improved.
The following embodiments provide specific implementations of the data fusion process in the navigation control method.
In the present embodiment, data fusion is achieved by a kalman filter. Specifically, the attitude prediction data and the attitude reference data are input into a preset Kalman filter, and navigation control parameters and Kalman gain values are output. Correcting the attitude prediction data based on the Kalman gain value to obtain attitude correction data; the error state in the motion model is updated with the pose correction data.
Errors in the motion model mainly include deterministic errors and random errors. The deterministic error can be calibrated in advance, and mainly comprises a bias error and a ratio between an actual value and a sensor output value. The random errors mainly include white gaussian noise errors and random walk errors. The attitude correction data is mainly used for updating the error value of the deterministic error and the parameters in the error formula of the random error.
Since the attitude prediction data is obtained from the first sensing data and the attitude reference data is obtained from the second sensing data, the data frequency of the attitude prediction data is higher than that of the attitude reference data, and the data frequency of the attitude prediction data needs to be adjusted in order to realize fusion. Specifically, downsampling processing is performed on the attitude prediction data so that the data frequency of the attitude prediction data is the same as the data frequency of the attitude reference data; and fusing the attitude prediction data subjected to the down-sampling processing and the attitude reference data to obtain navigation control parameters. The navigation control parameters include: linear acceleration data, angular velocity data, position data, and rotational angle data in a plurality of specified dimensional directions.
And inputting the attitude prediction data and the attitude reference data which are subjected to downsampling processing into a preset Kalman filter based on the attitude prediction data and the attitude reference data which are obtained by the first sensing data and the second sensing data, and outputting navigation control parameters and Kalman gain values. And the navigation controller sends the navigation control parameters to the inertial navigation fusion drive of the main controller so as to control the running state of the vehicle through the inertial navigation fusion drive. And the Kalman gain value is used for correcting the attitude prediction data to obtain attitude correction data and updating an error state in the motion model.
For ease of understanding, FIG. 3 shows a schematic diagram of the algorithmic steps of the data fusion process in one embodiment. High-frequency inertial sensing data sent by an inertial sensor are transmitted to a preset motion model, the running motion state of the automobile is predicted through the motion model, attitude prediction data such as acceleration, inclination, impact, vibration, rotation and multi-degree-of-freedom motion information of the automobile are output, and parameters in the motion model are updated independently based on the attitude prediction data.
Sensor data obtained from a positioning device (such as GPS global satellite positioning), a radar sensor and a wheel speed meter belong to low-frequency sensing data, provide accurate and reliable attitude and running information such as speed per hour, gear, corner, position, distance and the like, and are integrated into attitude reference information. In order to ensure that the output data frequencies of a plurality of sensors are consistent, the attitude prediction data are sampled and processed to update the amount to be estimated, data fusion is carried out on the attitude prediction data and the attitude reference data in a preset Kalman filter (EKF for short), navigation control parameters and Kalman gain values are output, the attitude prediction data are corrected based on the Kalman gain values to obtain attitude correction data, and the error state in the motion model is updated through the attitude correction data. And the navigation control parameters are sent to the inertial navigation fusion drive of the main controller, and the driving state of the vehicle is controlled through the inertial navigation fusion drive.
The advantages of using the above data fusion include: first, error independence, sensors used for data fusion where a single sensor measurement fails (possibly due to limited usage scenarios, electronic failures, etc.), does not result in other sensors failing at the same time for the same reason; secondly, the mutual complementarity of the sensors, such as inertial sensing data, can fill in the positioning output of the radar sensor during the multiple positioning intervals for smoothing the positioning result of the radar sensor; the radar sensor provides an initial value for the inertial sensing data, and the problem of integral deviation in the process of independent use of the inertial sensing data is solved; the radar sensor can make up the problem of positioning accuracy, and positioner can provide map range data for radar sensor map location matching.
The data fusion process distinguishes sensor data with high frequency and sensor data with low frequency, parameters in the motion model are updated based on the attitude prediction data, data are not subjected to down-sampling processing in the process, interference on the motion model after the down-sampling processing of the sensor data with high frequency in the prior art is relieved, and negative influence on vehicle track precision and map consistency is reduced.
The following embodiments provide another implementation of the data fusion process in the navigation control method.
The navigation controller is respectively in communication connection with a radar sensor, a drive-by-wire sensor and a main controller in the vehicle; acquiring radar data from a radar drive of a master controller; wherein the radar data includes a motion variation amount of a radar in the vehicle at a specified time; the radar data is obtained by analyzing and processing radar point cloud data acquired by a radar and calculating the pose of the radar point cloud data through radar driving; acquiring drive-by-wire data from a drive-by-wire of a main controller; the drive-by-wire data includes a running speed and a turning angle of the vehicle; the drive-by-wire data is obtained by analyzing the chassis data of the vehicle through drive-by-wire.
Performing data fusion processing on attitude prediction data and attitude reference data obtained based on sensor data to obtain navigation control parameters, sending the navigation control parameters to inertial navigation fusion drive of a main controller, and controlling the running state of a vehicle through the inertial navigation fusion drive; wherein the navigation control parameters include: linear acceleration data, angular velocity data, position data, and rotational angle data in a plurality of specified dimensional directions. In the data transmission process, the navigation controller sends clock synchronization signals of a Media Access Control (MAC) layer to the radar sensor, the drive-by-wire sensor and the main controller based on a preset clock synchronization protocol.
Fig. 4 shows a schematic diagram of communication data transmission of a navigation System according to an embodiment of the present invention, in which the Positioning System and the inertial navigation System include an Inertial Measurement Unit (IMU) inertial sensor, a Global Positioning System (GPS) Positioning device, and a navigation controller module, where an Antenna (Antenna) is in communication connection with the GPS Positioning device to assist Positioning, the IMU inertial sensor and the GPS Positioning device are in communication connection with the navigation controller respectively, and can send inertial navigation sensing data (frequency may be 200 hz) and Positioning data (frequency may be 1 hz) to the navigation controller, the navigation controller includes a clock by wire (PTP Grand) and an Extended Kalman Filter (EKF) software program for time calibration and data fusion, the navigation controller is in communication connection with a radar sensor, a drive-by-wire sensor, and a Master controller in a vehicle respectively, and the clock by-wire module in the navigation controller sends clock synchronization signals to the radar sensor, the drive-by-wire sensor, the Master controller, the IMU sensor, and the GPS Positioning device, and the extended kalman filter software program processes data fusion based on the inertial sensor, the drive-by-wire, the drive data in the Positioning device, and the GPS drive data.
The main controller, i.e. the main unit or the redundant unit high-energy computer group, is a vehicle computer control system, including: the system comprises a system clock, a radar drive, an inertial navigation fusion drive, a planning and control module, a drive-by-wire and a bus gateway module. The system clock is used for synchronizing the peripheral components; the radar drive is used for analyzing radar point cloud data transmitted by the radar sensor and sending the motion variation of the radar at each moment to the extended Kalman filter: the radar drive can be adapted to a driver of different products of a plurality of manufacturers to finish the analysis of radar data and the basic pose calculation. The inertial navigation driver is used for receiving navigation control parameter information and comprises: linear acceleration in three-dimensional directions (Ax, ay, az:); angular velocities in three dimensions (Wx, wy, wz); position in three dimensions (z, y, z); the euler angle rotation in three dimensions (Pitch, yaw, roll).
A planning and control module for unmanned route planning and control correlation; the drive-by-wire module receives information transmitted by the drive-by-wire sensor through the wheel encoder and the bus gateway, is used for analyzing data of the vehicle body chassis, and simultaneously sends the following data to the EKF extended Kalman filter, and comprises the following steps: the running speed (v) of the vehicle and the turning angle (w) of the vehicle are calculated by the drive-by-wire and the matching of the vehicle body chassis data. The drive-by-wire can be adapted to various different vehicle types, and data description of an upper layer is packaged according to the difference and the sameness of communication protocols of different vehicle types. The inertial navigation fusion drive module, the planning and control module, the drive-by-wire module and the bus gateway module sequentially transmit data in a bidirectional mode for data exchange among the controllers.
Time alignment is critical for data fusion of the individual sensors. Particularly, the high-frequency data and the low-frequency data can be accurately fused only by aligning according to the closest time. In the data transmission process, based on a preset clock synchronization protocol, a PTP Grand Master optimal clock module in the navigation controller sends clock synchronization signals of a medium access control MAC layer to a radar sensor, a drive-by-wire sensor and a main controller.
Fig. 5 is a schematic diagram of a time synchronization communication layer according to an embodiment of the present invention, where computers are connected to each other through a physical interface transceiver by using a network medium, and five-layer network communication of the computers includes a physical layer, a data link layer, a network layer, a transmission layer, and an application layer. The method comprises the following steps: the physical interface transceiver, the media access controller, the hardware interrupt, the network protocol stack, the application and other control modules are communicated with each other between the same layer, and the lower layer provides service for the upper layer. In order to pursue higher real-time performance, the clock of the MAC layer is used, and the accuracy of the synchronization time can reach nanosecond level, so that the clock synchronization of data can be completed on a link layer, and the real-time performance is greatly improved.
In data transmission of a communication module, the most important premise of sensor data fusion is time Calibration (Temporal Calibration), a traditional method generally uses a Pulse Per Second (PPS) to complete clock synchronization, and the method has the advantages of simple implementation and relatively stable precision, but has the disadvantages of being seriously dependent on the stability of a GPS satellite signal and being incapable of covering clock hopping, so that the embodiment proposes that a protocol of IEEE1588 is used to replace the PPS used traditionally to complete the time Calibration (Temporal Calibration) Calibration, and the method has the advantages of strong flexibility, a free networking function, and link stability capable of being compensated by errors and being not affected by satellite information quality (without using an atomic clock). Therefore, all scenes in the automatic driving can be covered stably and reliably.
In correspondence with the above method embodiment, referring to fig. 6, a schematic diagram of a navigation control device of a vehicle, the device comprising the steps of:
the first data processing module 602 is configured to obtain first sensing data output by the inertial sensor, input the first sensing data into a preset motion model, output attitude prediction data, and update a model parameter of the operation model based on the attitude preset data;
a second data processing module 604, configured to obtain second sensing data and generate attitude reference data based on the second sensing data; the second sensing data comprises positioning data, radar data and line control data; the data frequency of the second sensing data is lower than that of the first sensing data;
and the data fusion module 606 is configured to perform data fusion processing on the attitude prediction data and the attitude reference data to obtain a navigation control parameter.
The above-identified navigation control apparatus for a vehicle, responsive to a navigation controller in the vehicle; the system comprises a motion model, a posture prediction module, a model parameter updating module and a control module, wherein the motion model is used for acquiring first sensing data output by an inertial sensor, inputting the first sensing data into a preset motion model, outputting posture prediction data and updating the model parameter of the operation model based on the posture preset data; the attitude reference data acquisition unit is used for acquiring second sensing data and generating attitude reference data based on the second sensing data; the second sensing data comprise positioning data, radar data and line control data; the data frequency of the second sensing data is lower than that of the first sensing data; the method is used for carrying out data fusion processing on the attitude prediction data and the attitude reference data to obtain navigation control parameters.
The data fusion module is further configured to: and inputting the attitude prediction data and the attitude reference data into a preset Kalman filter, and outputting navigation control parameters and Kalman gain values.
The apparatus further comprises a correction module configured to: correcting the attitude prediction data based on the Kalman gain value to obtain attitude correction data; the error state in the motion model is updated by the pose correction data.
The data fusion module is further configured to: down-sampling the attitude prediction data to enable the data frequency of the attitude prediction data to be the same as that of the attitude reference data; and fusing the attitude prediction data subjected to the down-sampling processing and the attitude reference data to obtain navigation control parameters.
The navigation controller is respectively in communication connection with a radar sensor, a drive-by-wire sensor and a main controller in the vehicle; the apparatus further comprises a synchronization module configured to: and based on a preset clock synchronization protocol, sending a clock synchronization signal of a Media Access Control (MAC) layer to the radar sensor, the drive-by-wire sensor and the main controller.
The navigation controller is in communication connection with a main controller in the vehicle; the second data processing module is further configured to: acquiring radar data from a radar drive of a master controller; wherein the radar data includes a motion variation amount of a radar in the vehicle at a specified time; the radar data is obtained by analyzing and processing radar point cloud data acquired by a radar and calculating the position and pose through radar driving; acquiring drive-by-wire data from a drive-by-wire of a main controller; the drive-by-wire data comprises the running speed and the turning angle of the vehicle; and the drive-by-wire data is obtained by analyzing the chassis data of the vehicle through drive-by-wire.
The navigation controller is in communication connection with a main controller in the vehicle; the apparatus further comprises a control module configured to: sending the navigation control parameters to inertial navigation fusion drive of the main controller so as to control the running state of the vehicle through the inertial navigation fusion drive; wherein, the navigation control parameters include: linear acceleration data, angular velocity data, position data, and rotational angle data in a plurality of specified dimensional directions.
The embodiment of the invention provides a navigation controller, which comprises a processor and a memory, wherein the memory stores machine executable instructions capable of being executed by the processor, and the processor executes the machine executable instructions to realize the navigation control method of the vehicle.
Referring to fig. 7, the navigation controller includes a processor 100 and a memory 101, the memory 101 stores machine executable instructions capable of being executed by the processor 100, and the processor 100 executes the machine executable instructions to implement the navigation control method of the vehicle.
Further, the electronic device shown in fig. 7 further includes a bus 102 and a communication interface 103, and the processor 100, the communication interface 103, and the memory 101 are connected through the bus 102.
The Memory 101 may include a Random Access Memory (RAM) and a non-volatile Memory (non-volatile Memory), such as at least one disk Memory. The communication connection between the network element of the system and at least one other network element is realized through at least one communication interface 103 (which may be wired or wireless), and the internet, a wide area network, a local network, a metropolitan area network, and the like can be used. The bus 102 may be an ISA bus, a PCI bus, an EISA bus, or the like. The bus may be divided into an address bus, a data bus, a control bus, etc. For ease of illustration, only one double-headed arrow is shown in FIG. 7, but this does not indicate only one bus or one type of bus.
The embodiment also provides a vehicle control system, which comprises the navigation controller, the main controller, the inertial sensor, the positioning device, the radar sensor and the drive-by-wire sensor; referring to fig. 8, the navigation sensor is in communication connection with the main controller, the inertial sensor, the positioning device, the radar sensor and the drive-by-wire sensor, respectively; and the main controller is in communication connection with the radar sensor and the drive-by-wire sensor respectively.
The present embodiments also provide a machine-readable storage medium having stored thereon machine-executable instructions that, when invoked and executed by a processor, cause the processor to implement the vehicle navigation control method described above.
The navigation control method, the navigation control device, the navigation controller and the vehicle control system for the vehicle provided by the embodiment of the invention comprise a computer readable storage medium storing program codes, instructions included in the program codes can be used for executing the method in the previous method embodiment, and specific implementation can refer to the method embodiment, and is not repeated herein.
It is clear to those skilled in the art that, for convenience and brevity of description, the specific working processes of the system and the apparatus described above may refer to the corresponding processes in the foregoing method embodiments, and are not described herein again.
In addition, in the description of the embodiments of the present invention, unless otherwise explicitly specified or limited, the terms "mounted," "connected," and "connected" are to be construed broadly, e.g., as meaning either a fixed connection, a removable connection, or an integral connection; can be mechanically or electrically connected; they may be connected directly or indirectly through intervening media, or they may be interconnected between two elements. The specific meanings of the above terms in the present invention can be understood in a specific case to those skilled in the art.
The functions may be stored in a computer-readable storage medium if they are implemented in the form of software functional units and sold or used as separate products. Based on such understanding, the technical solution of the present invention may be embodied in the form of a software product, which is stored in a storage medium and includes instructions for causing a computer device (which may be a personal computer, a server, or a network device) to execute all or part of the steps of the method according to the embodiments of the present invention. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), a magnetic disk or an optical disk, and other various media capable of storing program codes.
In the description of the present invention, it should be noted that the terms "center", "upper", "lower", "left", "right", "vertical", "horizontal", "inner", "outer", etc., indicate orientations or positional relationships based on the orientations or positional relationships shown in the drawings, and are only for convenience of description and simplicity of description, but do not indicate or imply that the device or element being referred to must have a particular orientation, be constructed and operated in a particular orientation, and thus, should not be construed as limiting the present invention. Furthermore, the terms "first," "second," and "third" are used for descriptive purposes only and are not to be construed as indicating or implying relative importance.
Finally, it should be noted that: although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art will understand that the following embodiments are merely illustrative of the present invention, and not restrictive, and the scope of the present invention is not limited thereto: any person skilled in the art can modify or easily conceive the technical solutions described in the foregoing embodiments or equivalent substitutes for some technical features within the technical scope of the present disclosure; such modifications, changes or substitutions do not depart from the spirit and scope of the embodiments of the present invention, and they should be construed as being included therein. Therefore, the protection scope of the present invention shall be subject to the protection scope of the claims.
Claims (10)
1. A navigation control method of a vehicle, characterized in that the method is applied to a navigation controller in a vehicle; the method comprises the following steps:
acquiring first sensing data output by an inertial sensor, inputting the first sensing data into a preset motion model, outputting attitude prediction data, and updating model parameters of the motion model based on the attitude prediction data; the data is not processed by down sampling in the process; wherein the model parameters include: world coordinate system parameters, IMU body coordinate system parameters, gyroscope parameters and accelerometer parameters;
acquiring second sensing data, and generating attitude reference data based on the second sensing data; the second sensing data comprise positioning data, radar data and line control data; a data frequency of the second sensing data is lower than a data frequency of the first sensing data;
performing data fusion processing on the attitude prediction data and the attitude reference data to obtain navigation control parameters;
and performing data fusion processing on the attitude prediction data and the attitude reference data to obtain navigation control parameters, wherein the step comprises the following steps of:
down-sampling the attitude prediction data to enable the data frequency of the attitude prediction data to be the same as that of the attitude reference data;
and fusing the attitude prediction data subjected to down-sampling processing with the attitude reference data to obtain navigation control parameters.
2. The method according to claim 1, wherein the step of performing data fusion processing on the attitude prediction data and the attitude reference data to obtain the navigation control parameters comprises:
and inputting the attitude prediction data and the attitude reference data into a preset Kalman filter, and outputting navigation control parameters and Kalman gain values.
3. The method of claim 2, wherein after the steps of inputting the attitude prediction data and the attitude reference data into a predetermined kalman filter and outputting navigation control parameters and kalman gain values, the method further comprises:
correcting the attitude prediction data based on the Kalman gain value to obtain attitude correction data;
updating an error state in the motion model with the pose correction data.
4. The method of claim 1, wherein the navigation controller is communicatively coupled to a radar sensor, a drive-by-wire sensor, and a master controller, respectively, in the vehicle; the method further comprises the following steps:
and sending clock synchronization signals of a Media Access Control (MAC) layer to the radar sensor, the drive-by-wire sensor and the main controller based on a preset clock synchronization protocol.
5. The method of claim 1, wherein the navigation controller is communicatively coupled to a master controller in a vehicle; the step of acquiring second sensing data includes:
acquiring the radar data from a radar drive of the master controller; wherein the radar data includes a motion variation amount of a radar in the vehicle at a specified time; the radar data is obtained by analyzing and processing radar point cloud data acquired by the radar and performing pose calculation through the radar drive;
acquiring the drive-by-wire data from the drive-by-wire of the main controller; wherein the drive-by-wire data includes an operating speed and a turning angle of the vehicle; and the drive-by-wire data is obtained by analyzing the chassis data of the vehicle through the drive-by-wire.
6. The method of claim 1, wherein the navigation controller is communicatively coupled to a master controller in a vehicle; after the step of performing data fusion processing on the attitude prediction data and the attitude reference data to obtain the navigation control parameters, the method further comprises the following steps:
sending the navigation control parameters to inertial navigation fusion drive of the main controller so as to control the driving state of the vehicle through the inertial navigation fusion drive; wherein the navigation control parameters include: linear acceleration data, angular velocity data, position data, and rotational angle data in a plurality of specified dimensional directions.
7. A navigation control apparatus of a vehicle, characterized in that the apparatus is provided to a navigation controller in the vehicle; the device comprises:
the first data processing module is used for acquiring first sensing data output by an inertial sensor, inputting the first sensing data into a preset motion model, outputting attitude prediction data and updating model parameters of the motion model based on the attitude prediction data; the data is not processed by down sampling in the process; wherein the model parameters include: world coordinate system parameters, IMU body coordinate system parameters, gyroscope parameters and accelerometer parameters;
the second data processing module is used for acquiring second sensing data and generating attitude reference data based on the second sensing data; the second sensing data comprises positioning data, radar data and line control data; the data frequency of the second sensing data is lower than the data frequency of the first sensing data;
the data fusion module is used for carrying out data fusion processing on the attitude prediction data and the attitude reference data to obtain navigation control parameters;
the data fusion module is further used for performing downsampling processing on the attitude prediction data so that the data frequency of the attitude prediction data is the same as that of the attitude reference data; and fusing the attitude prediction data subjected to down-sampling processing with the attitude reference data to obtain navigation control parameters.
8. A navigation controller comprising a processor and a memory, the memory storing machine executable instructions executable by the processor, the processor executing the machine executable instructions to implement a navigation control method of a vehicle as claimed in any one of claims 1 to 6.
9. A vehicle control system, characterized in that the system comprises the navigation controller of claim 8, and further comprises a main controller, an inertial sensor, a positioning device, a radar sensor, and a drive-by-wire sensor;
the navigation controller is respectively in communication connection with the main controller, the inertial sensor, the positioning device, the radar sensor and the drive-by-wire sensor; and the main controller is in communication connection with the radar sensor and the drive-by-wire sensor respectively.
10. A machine-readable storage medium having stored thereon machine-executable instructions which, when invoked and executed by a processor, cause the processor to implement the method of navigation control of a vehicle of any of claims 1-6.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111339417.7A CN114167858B (en) | 2021-11-12 | 2021-11-12 | Navigation control method and device for vehicle, navigation controller and vehicle control system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111339417.7A CN114167858B (en) | 2021-11-12 | 2021-11-12 | Navigation control method and device for vehicle, navigation controller and vehicle control system |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114167858A CN114167858A (en) | 2022-03-11 |
| CN114167858B true CN114167858B (en) | 2022-11-11 |
Family
ID=80479234
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111339417.7A Active CN114167858B (en) | 2021-11-12 | 2021-11-12 | Navigation control method and device for vehicle, navigation controller and vehicle control system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114167858B (en) |
Family Cites Families (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2016187759A1 (en) * | 2015-05-23 | 2016-12-01 | SZ DJI Technology Co., Ltd. | Sensor fusion using inertial and image sensors |
| CN106017474B (en) * | 2016-06-17 | 2018-02-27 | 北京航空航天大学 | A kind of double frequency junction filter and filtering method for eliminating single-point GPS high order site errors |
| CN108106635A (en) * | 2017-12-15 | 2018-06-01 | 中国船舶重工集团公司第七0七研究所 | Inertia defends the anti-interference posture course calibration method of long endurance for leading integrated navigation system |
| CN108344415B (en) * | 2018-01-30 | 2021-06-04 | 北京大学 | Combined navigation information fusion method |
| CN110873888B (en) * | 2018-09-04 | 2022-05-06 | 腾讯大地通途（北京）科技有限公司 | Positioning method, positioning device, positioning apparatus, and computer storage medium |
| CN112923932A (en) * | 2019-12-06 | 2021-06-08 | 鸿富锦精密工业（深圳）有限公司 | High-precision map generation method based on multi-sensing fusion positioning |
| CN113310487B (en) * | 2021-05-25 | 2022-11-04 | 云南电网有限责任公司电力科学研究院 | Ground-oriented mobile robot-oriented integrated navigation method and device |
| CN113587934B (en) * | 2021-07-30 | 2024-03-19 | 深圳市普渡科技有限公司 | Robot, indoor positioning method and device and readable storage medium |
-
2021
- 2021-11-12 CN CN202111339417.7A patent/CN114167858B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN114167858A (en) | 2022-03-11 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US8775063B2 (en) | System and method of lane path estimation using sensor fusion | |
| US10422658B2 (en) | Method, fusion filter, and system for fusing sensor signals with different temporal signal output delays into a fusion data set | |
| CN113514806B (en) | Obstacle determination method and device in automatic driving process and electronic equipment | |
| EP3578919B1 (en) | Travel history storage method, method for producing travel path model, method for estimating local position, and travel history storage device | |
| KR102565482B1 (en) | Apparatus for determining position of vehicle and method thereof | |
| KR20220024791A (en) | Method and apparatus for determining the trajectory of a vehicle | |
| CN113405545A (en) | Positioning method, positioning device, electronic equipment and computer storage medium | |
| EP4020111B1 (en) | Vehicle localisation | |
| CN113196109B (en) | Method for determining an integrity range | |
| US11577736B2 (en) | Method and device for ascertaining a highly accurate estimated value of a yaw rate for controlling a vehicle | |
| CN113984044A (en) | Vehicle pose acquisition method and device based on vehicle-mounted multi-perception fusion | |
| CN113252052A (en) | Map creation device, map creation system, map creation method, and storage medium | |
| KR102336071B1 (en) | Apparatus for determining position of vehicle and method thereof | |
| CN114167858B (en) | Navigation control method and device for vehicle, navigation controller and vehicle control system | |
| CN113306559A (en) | Compensation for vertical road camber in road shape estimation | |
| JP7204612B2 (en) | POSITION AND POSTURE ESTIMATION DEVICE, POSITION AND POSTURE ESTIMATION METHOD, AND PROGRAM | |
| CN114074693A (en) | Train positioning method, device and system with multiple sensors integrated and train | |
| WO2022203026A1 (en) | Driving control device | |
| CN113917512B (en) | Positioning method and device for automatic driving vehicle, electronic equipment and storage medium | |
| CN115046546A (en) | Automatic driving automobile positioning system and method based on lane line identification | |
| CN113358123A (en) | Trusted motion unit | |
| US20210293976A1 (en) | Method for selecting a restricted or empty set of hypotheses of possible positions of a vehicle | |
| JP7409037B2 (en) | Estimation device, estimation method, estimation program | |
| CN115388914B (en) | Parameter calibration method and device for sensor, storage medium and electronic device | |
| CN116659487B (en) | Pose adjustment method, pose adjustment device, electronic equipment and readable storage medium |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |