CN116045972A - Road gradient estimation method based on vehicle attitude angle - Google Patents
Road gradient estimation method based on vehicle attitude angle Download PDFInfo
- Publication number
- CN116045972A CN116045972A CN202211448013.6A CN202211448013A CN116045972A CN 116045972 A CN116045972 A CN 116045972A CN 202211448013 A CN202211448013 A CN 202211448013A CN 116045972 A CN116045972 A CN 116045972A
- Authority
- CN
- China
- Prior art keywords
- road gradient
- error
- vehicle
- ins
- frequency domain
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 50
- 238000001914 filtration Methods 0.000 claims abstract description 23
- 238000012545 processing Methods 0.000 claims abstract description 10
- 230000009466 transformation Effects 0.000 claims abstract description 6
- 238000004364 calculation method Methods 0.000 claims abstract description 5
- 238000005259 measurement Methods 0.000 claims description 52
- 238000005070 sampling Methods 0.000 claims description 34
- 239000011159 matrix material Substances 0.000 claims description 12
- 230000008569 process Effects 0.000 claims description 9
- 238000009499 grossing Methods 0.000 claims description 6
- 238000005516 engineering process Methods 0.000 claims description 5
- 230000009977 dual effect Effects 0.000 claims description 3
- 230000010354 integration Effects 0.000 claims description 3
- 238000001228 spectrum Methods 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims 1
- 238000004422 calculation algorithm Methods 0.000 abstract description 6
- 238000001514 detection method Methods 0.000 abstract description 2
- 230000006870 function Effects 0.000 description 15
- 230000015654 memory Effects 0.000 description 15
- 238000004590 computer program Methods 0.000 description 10
- 230000008859 change Effects 0.000 description 9
- 230000009286 beneficial effect Effects 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 230000004927 fusion Effects 0.000 description 3
- 206010063385 Intellectualisation Diseases 0.000 description 1
- 230000001133 acceleration Effects 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 238000003491 array Methods 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 239000000446 fuel Substances 0.000 description 1
- 230000035772 mutation Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000001105 regulatory effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000000926 separation method Methods 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Navigation (AREA)
Abstract
The invention relates to the technical field of road detection, and particularly discloses a road gradient estimation method based on a vehicle attitude angle, which comprises the steps of acquiring GNSS data and INS data, and processing the GNSS data and the INS data based on Kalman filtering to obtain a vehicle pitch angle; performing Fourier transform on the pitch angle of the vehicle, and converting the time domain signal into a frequency domain signal; filtering the high-frequency domain signals through a low-pass filter, and reserving the low-frequency domain signals; and carrying out Fourier inverse transformation on the low-frequency domain signals to obtain the real-time road gradient value. Compared with the conventional kinematic or dynamic method for estimating the road gradient, the method does not need to add more sensors, and has simple information sources; and a complex vehicle model does not need to be built, so that the calculation amount of the algorithm is small.
Description
Technical Field
The invention relates to the technical field of road detection, in particular to a road gradient estimation method based on a vehicle attitude angle.
Background
In the current society, the automobile intellectualization is one of the most important strategic directions of the global automobile industry development, and the road gradient is taken as an important parameter in a vehicle dynamics model, so that the road gradient can be estimated rapidly and accurately, the performances of the automobile automatic transmission such as gear shifting smoothness and fuel economy are improved to a great extent, and the aspects of stability control of a vehicle, selection of a driving decision, research on dynamic characteristics (such as tire force and the like) of the vehicle are served. The real-time gradient information of the road surface is an indispensable parameter for improving the performance of the intelligent vehicle longitudinal control system when the vehicle runs under complex and changeable road conditions. In addition, at multi-layer road intersections often occurring in cities, the horizontal distance of each lane is generally small, and the conventional planar positioning using only two-dimensional information will be likely to have a lane matching error. At this time, the positioning system is required to provide accurate gradient information so as to improve matching performance and complete vehicle navigation.
The kinematic and dynamic methods currently mainly used for road gradient estimation have respective limitations: the kinematic model does not consider the vehicle attitude information, and the gradient estimation result has deviation from the actual value; and the dynamic model has high requirements on model precision and poor algorithm stability.
Disclosure of Invention
The invention aims to provide a road gradient estimation method based on a vehicle attitude angle, so as to solve the problems in the background art.
In order to achieve the above purpose, the present invention provides the following technical solutions:
a vehicle attitude angle-based road gradient estimation method, the method comprising:
acquiring GNSS data and INS data, and processing the GNSS data and the INS data based on Kalman filtering to obtain a pitch angle of the vehicle;
performing Fourier transform on the pitch angle of the vehicle, and converting the time domain signal into a frequency domain signal;
filtering the high-frequency domain signals through a low-pass filter, and reserving the low-frequency domain signals; the high-frequency domain signals correspond to vehicle pitching angles, and the low-frequency domain signals correspond to road gradient information;
and carrying out Fourier inverse transformation on the low-frequency domain signals to obtain the real-time road gradient value.
As a further scheme of the invention: the step of obtaining GNSS data and INS data, processing the GNSS data and the INS data based on Kalman filtering to obtain a pitch angle of a vehicle comprises the following steps: the measurement obtained by the GNSS and INS sensors is fused by adopting a Kalman filtering technology, and the method specifically comprises the following steps:
system state vector:
position error measurement equation:
speed error measurement equation:
heading angle error measurement equation:
calculating one-step state prediction:
state one-step prediction mean square error:
kalman filter gain:
state estimation:
state estimation mean square error:
wherein ,
respectively representing the east, north and sky attitude error angles of the INS;
speed errors in the east and north directions of the INS; />
Latitude, longitude, and altitude errors for INS; />
Zero bias error of the gyroscope, subscript +.>
Respectively representing three directions of the roll axis, the pitch axis and the yaw axis of the gyroscope, and is marked with a superscript/>
Then the projection in the carrier coordinate system is represented; />
Zero offset error of the accelerometer, subscript +.>
Respectively representing the directions of a horizontal axis, a vertical axis and a vertical axis of the accelerometer, and the superscript +.>
Also representing the projection in the carrier coordinate system; in the position error measurement equation, < >>
For a measurement matrix of position errors, < >>
Difference between the position measurements of INS and GNSS, +.>
A measurement noise matrix that is a position error; in the speed error measurement equation, < >>
For a measurement matrix of speed errors, < >>
Difference between speed measurements of INS and GNSS, +.>
A measurement noise matrix that is a velocity error; in the course angle measurement equation, < >>
and />
Respectively representing the course angle obtained by IMU integration and the course angle obtained by double-antenna GNSS measurement, +.>
The difference between the resulting heading angle and the gaussian white noise of the dual antenna GNSS measured heading angle is integrated for the IMU.
As a further scheme of the invention: in the Kalman filtering formula
From the state estimate of the previous moment +.>
Predicting the state of time; (/>
) Measuring the prediction error, wherein the measurement error comprises measurement information; by predicting the mean square error in one step for the state +.>
Deriving to obtain the calculated gain factor +.>
To estimate the state of the previous moment and +.>
The time measurements are linearly combined (weighted estimation) to finally obtain +.>
State estimation of time of day->
Also known as status->
Is a posterior estimate of (1).
As a further scheme of the invention: the step of performing fourier transform on the pitch angle of the vehicle and converting the time domain signal into a frequency domain signal comprises the steps of:
converting the aperiodic pitch angle signal by using Fourier transform to obtain a frequency spectrum density function:
wherein ,
is a complex function and can be written as:
wherein ,
is->
Which represents the relative magnitudes of the frequency components in the signal.
As a further scheme of the invention: for the fourier transform-based calculation process, various parameters involved in the fourier transform function include:
the sampling frequency is not less than twice the maximum value of the road gradient signal frequency;
the sampling time is determined to be the minimum value by the sampling frequency;
the Fourier transform points are the same as the sampling points, and the sampling points are determined by the sampling time;
the low pass filter cut-off frequency is determined based on a sliding window switching and data smoothing method.
Compared with the prior art, the invention has the beneficial effects that: compared with the conventional kinematic or dynamic method for estimating the road gradient, the method does not need to add more sensors, and has simple information sources; and a complex vehicle model does not need to be built, so that the calculation amount of the algorithm is small. The algorithm used in the invention is not limited by the motion state of the vehicle, and can ensure the estimation accuracy under severe working conditions such as turning, rapid acceleration and deceleration of the vehicle. The gradient estimation method provided by the invention carries out corresponding self-adaptive adjustment on the Fourier transformation parameters (including sampling time, sampling points, cut-off frequency regulated when low-pass filtering is carried out, and the like) used in the estimation process. Under the condition of larger gradient change rate, the real-time performance is higher, and the accuracy is better; and under the condition of low gradient change rate, the output road gradient information is more stable and the shake is less.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the following description will briefly introduce the drawings that are needed in the embodiments or the description of the prior art, and it is obvious that the drawings in the following description are only some embodiments of the present invention.
Fig. 1 is a system configuration diagram of a road gradient estimation method based on a vehicle attitude angle.
FIG. 2 is a flow chart diagram of a method of road slope estimation based on vehicle attitude angle.
Detailed Description
In order to make the technical problems, technical schemes and beneficial effects to be solved more clear, the invention is further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are for purposes of illustration only and are not intended to limit the scope of the invention.
Example 1
Fig. 2 shows a flow chart of a road gradient estimation method based on a vehicle attitude angle, and in an embodiment of the invention, a road gradient estimation method based on a vehicle attitude angle includes steps S100 to S400:
step S100: acquiring GNSS data and INS data, and processing the GNSS data and the INS data based on Kalman filtering to obtain a pitch angle of the vehicle;
step S200: performing Fourier transform on the pitch angle of the vehicle, and converting the time domain signal into a frequency domain signal;
step S300: filtering the high-frequency domain signals through a low-pass filter, and reserving the low-frequency domain signals; the high-frequency domain signals correspond to vehicle pitching angles, and the low-frequency domain signals correspond to road gradient information;
step S400: and carrying out Fourier inverse transformation on the low-frequency domain signals to obtain the real-time road gradient value.
The invention provides a combined positioning technology based on fusion of information of a global navigation satellite system (Global Navigation Satellite System, GNSS), an inertial navigation system (Inertial Navigation System, INS) and a vehicle chassis sensor, and a road gradient estimation method of Fourier transform and filtering technology, which are beneficial to improving algorithm stability, gradient estimation accuracy and instantaneity and reducing calculated amount.
The aim of the invention can be achieved by the following technical scheme:
as shown in fig. 1, in an example of the technical solution of the present invention, a road gradient estimation method based on vehicle-mounted multi-source sensor fusion is provided, and a system for completing the method includes:
the GNSS/INS fusion model vehicle state estimation module is used for processing GNSS and INS measurement by adopting Kalman filtering, so that accurate vehicle pitch angle estimation is obtained and provided for the gradient estimation module;
the gradient estimation module is divided into three parts: the estimation system carries out Fourier transform on the estimation result of the pitch angle of the vehicle and converts the time domain signal into a frequency domain; the estimation system II uses low-pass filtering to filter out the real pitch angle of the high-frequency vehicle, and retains the low-frequency road gradient information; the estimation system three uses an inverse fourier transform to convert road slope information in the frequency domain into a real-time road slope value estimate.
The step of obtaining GNSS data and INS data, processing the GNSS data and the INS data based on Kalman filtering to obtain a pitch angle of a vehicle comprises the following steps: the measurement obtained by the GNSS and INS sensors is fused by adopting a Kalman filtering technology, and the method specifically comprises the following steps:
system state vector:
position error measurement equation:
speed error measurement equation:
heading angle error measurement equation:
calculating one-step state prediction:
state one-step prediction mean square error:
kalman filter gain:
state estimation:
state estimation mean square error:
wherein ,
respectively representing the east, north and sky attitude error angles of the INS;
speed errors in the east and north directions of the INS; />
Latitude, longitude, and altitude errors for INS; />
Zero bias error of the gyroscope, subscript +.>
Respectively representing the three directions of the roll axis, the pitch axis and the yaw axis of the gyroscope, and the superscript +.>
Then the projection in the carrier coordinate system is represented; />
Zero offset error of the accelerometer, subscript +.>
Respectively representing the directions of a horizontal axis, a vertical axis and a vertical axis of the accelerometer, and the superscript +.>
Also representing the projection in the carrier coordinate system; in the position error measurement equation, < >>
For a measurement matrix of position errors, < >>
Difference between the position measurements of INS and GNSS, +.>
A measurement noise matrix that is a position error; in the speed error measurement equation, < >>
For a measurement matrix of speed errors, < >>
Difference between speed measurements of INS and GNSS, +.>
A measurement noise matrix that is a velocity error; in the course angle measurement equation, < >>
and />
Respectively representing the course angle obtained by IMU integration and the course angle obtained by double-antenna GNSS measurement, +.>
The difference between the resulting heading angle and the gaussian white noise of the dual antenna GNSS measured heading angle is integrated for the IMU.
Specifically, in the Kalman filtering formula
From the state estimate of the previous moment +.>
Predicting the state of time; (/>
) Measuring the prediction error, wherein the measurement error comprises measurement information; by predicting the mean square error in one step for the state +.>
Deriving to obtain the calculated gain factor +.>
To estimate the state of the previous moment and +.>
The time measurements are linearly combined (weighted estimation) to finally obtain +.>
State estimation of time of day->
Also known as status->
Is a posterior estimate of (1).
As a preferred embodiment of the present invention, the step of performing fourier transform on the pitch angle of the vehicle to convert the time domain signal into the frequency domain signal includes:
converting the aperiodic pitch angle signal by using Fourier transform to obtain a frequency spectrum density function:
wherein ,
is a complex function and can be written as:
wherein ,
is->
Which represents the relative magnitudes of the frequency components in the signal.
The input of the estimation system in the road gradient estimation module is pitch angle frequency domain data obtained in the estimation system, wherein the road gradient is a slow variable, the frequency is low, the pitch angle change speed of the vehicle is high, and the pitch angle change speed is high frequency components in the estimation signal, so that the pitch angle change speed is filtered by using low-pass filtering, and low-frequency gradient data is reserved for output.
The input of the estimation system in the road gradient estimation module is the frequency domain information of the road gradient obtained in the estimation system, the frequency domain information is subjected to Fourier inverse transformation, and the final real-time road gradient estimation result is output.
As a preferred embodiment of the present invention, for the calculation process based on fourier transform, various parameters related to fourier transform function include:
the sampling frequency is not less than twice the maximum value of the road gradient signal frequency;
the sampling time is determined to be the minimum value by the sampling frequency;
the Fourier transform points are the same as the sampling points, and the sampling points are determined by the sampling time;
the low pass filter cut-off frequency is determined based on a sliding window switching and data smoothing method.
In order to better obtain a smooth road gradient estimation and reflect the situation that the road gradient generates a large mutation in real time, various parameters related to a fourier transform function used in an algorithm need to be adjusted, specifically:
sampling frequency: in combination with the nyquist sampling theorem, a limitation that the sampling frequency needs to be equal to twice the maximum value (10 Hz) of the road gradient signal frequency, and the output frequency (100 Hz) of the vehicle state estimation model, the sampling frequency of the fourier analysis and the frequency of the pitch angle estimation are set to 100Hz.
Sampling time: in the case of known sampling frequency, the more the number of sampling points, the longer the sampling time, so that it can be dividedThe smaller the minimum separation of the two frequencies is distinguished. To distinguish 1Hz frequency domain data, the waveform resolution formula is used
It is known that the sampling time is at least 1s. Then the number of required sampling points is at least 100 at a sampling frequency of 100Hz.
Fourier transform points: under the condition of not wasting the sampling data and guaranteeing the accuracy of the frequency domain graph, the number of the Fourier transform points is directly equal to the number of the sampling points, namely 100.
Low pass filter cut-off frequency: under the condition that different Fourier transform points are adopted, namely the wave resolution in the frequency domain diagram is different, the actual filtering cut-off frequency is affected, if the wave resolution is too large, judgment can not be carried out at accurate frequency value points, and data information can not be accurately filtered, so that the sliding window switching and data smoothing method is designed by combining the road gradient real condition
The sliding window switching method specifically comprises the following steps:
in general, we consider the frequency of the road slope retardation to be below 1 Hz. However, when the road gradient is suddenly changed, the change of the road gradient is accelerated and the frequency is increased; while at the same time the real pitch angle variation of the vehicle will correspondingly be faster and more frequent, i.e. the high frequency components in the frequency domain will be more amplitude compared to the case where the road is relatively gentle. In the event of abrupt changes in road gradient, the frequency domain distinction from the true pitch angle of the vehicle will therefore increase. In order to ensure the real-time performance of road gradient estimation, the number of sampling data and the number of Fourier transform data points can be changed by changing a data sliding window, so that the waveform resolution is changed, and finally the cut-off frequency of actual filtering is changed.
When specifically estimating the gradient: under the condition that the road is gentle, the obtained road gradient estimation result can be ensured to be more stable by using a larger data sliding window, and the shake is smaller; in the case of abrupt change of road gradient, the estimation result can more sensitively follow the change of road gradient by using a smaller data sliding window, so that the real-time performance is higher.
The data smoothing method specifically comprises the following steps:
if switching is performed between the estimation results obtained by the long and short data sliding windows, there may be a large difference between the two estimation results in the case of a large road gradient change rate, and therefore, a jump in the estimation results may occur (particularly in the case of switching the data sliding window from a small to a large one). At this time, a gentle transition between the two estimated data results can be performed by a weighted smoothing method, so as to remove gradient estimation abrupt peak values generated by data jump.
The functions that can be realized by the road gradient estimation method based on the vehicle attitude angle are all completed by computer equipment, and the computer equipment comprises one or more processors and one or more memories, wherein at least one program code is stored in the one or more memories, and the program code is loaded and executed by the one or more processors to realize the functions of the road gradient estimation method based on the vehicle attitude angle.
The processor takes out instructions from the memory one by one, analyzes the instructions, then completes corresponding operation according to the instruction requirement, generates a series of control commands, enables all parts of the computer to automatically, continuously and cooperatively act to form an organic whole, realizes the input of programs, the input of data, the operation and the output of results, and the arithmetic operation or the logic operation generated in the process is completed by the arithmetic unit; the Memory comprises a Read-Only Memory (ROM) for storing a computer program, and a protection device is arranged outside the Memory.
For example, a computer program may be split into one or more modules, one or more modules stored in memory and executed by a processor to perform the present invention. One or more of the modules may be a series of computer program instruction segments capable of performing specific functions for describing the execution of the computer program in the terminal device.
It will be appreciated by those skilled in the art that the foregoing description of the service device is merely an example and is not meant to be limiting, and may include more or fewer components than the foregoing description, or may combine certain components, or different components, such as may include input-output devices, network access devices, buses, etc.
The processor may be a central processing unit (Central Processing Unit, CPU), other general purpose processors, digital signal processors (Digital Signal Processor, DSP), application specific integrated circuits (Application Specific Integrated Circuit, ASIC), off-the-shelf programmable gate arrays (Field-Programmable Gate Array, FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, or the like. The general purpose processor may be a microprocessor or the processor may be any conventional processor or the like, which is the control center of the terminal device described above, and which connects the various parts of the entire user terminal using various interfaces and lines.
The memory may be used for storing computer programs and/or modules, and the processor may implement various functions of the terminal device by running or executing the computer programs and/or modules stored in the memory and invoking data stored in the memory. The memory may mainly include a storage program area and a storage data area, wherein the storage program area may store an operating system, an application program required for at least one function (such as an information acquisition template display function, a product information release function, etc.), and the like; the storage data area may store data created according to the use of the berth status display system (e.g., product information acquisition templates corresponding to different product types, product information required to be released by different product providers, etc.), and so on. In addition, the memory may include high-speed random access memory, and may also include non-volatile memory, such as a hard disk, memory, plug-in hard disk, smart Media Card (SMC), secure Digital (SD) Card, flash Card (Flash Card), at least one disk storage device, flash memory device, or other volatile solid-state storage device.
The modules/units integrated in the terminal device may be stored in a computer readable storage medium if implemented in the form of software functional units and sold or used as separate products. Based on this understanding, the present invention may implement all or part of the modules/units in the system of the above-described embodiments, or may be implemented by instructing the relevant hardware by a computer program, which may be stored in a computer-readable storage medium, and which, when executed by a processor, may implement the functions of the respective system embodiments described above. Wherein the computer program comprises computer program code, which may be in the form of source code, object code, executable files or in some intermediate form, etc. The computer readable medium may include: any entity or device capable of carrying computer program code, a recording medium, a U disk, a removable hard disk, a magnetic disk, an optical disk, a computer Memory, a Read-Only Memory (ROM), a random access Memory (RAM, random Access Memory), an electrical carrier signal, a telecommunications signal, a software distribution medium, and so forth.
It should be noted that, in this document, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising one … …" does not exclude the presence of other like elements in a process, method, article, or apparatus that comprises the element.
The foregoing description is only of the preferred embodiments of the present invention, and is not intended to limit the scope of the invention, but rather is intended to cover any equivalents of the structures or equivalent processes disclosed herein or in the alternative, which may be employed directly or indirectly in other related arts.
Claims (5)
1. A method of estimating a road gradient based on a vehicle attitude angle, the method comprising:
acquiring GNSS data and INS data, and processing the GNSS data and the INS data based on Kalman filtering to obtain a pitch angle of the vehicle;
performing Fourier transform on the pitch angle of the vehicle, and converting the time domain signal into a frequency domain signal;
filtering the high-frequency domain signals through a low-pass filter, and reserving the low-frequency domain signals; the high-frequency domain signals correspond to vehicle pitching angles, and the low-frequency domain signals correspond to road gradient information;
and carrying out Fourier inverse transformation on the low-frequency domain signals to obtain the real-time road gradient value.
2. The vehicle attitude angle-based road gradient estimation method according to claim 1, wherein the step of acquiring GNSS data and INS data, processing the GNSS data and INS data based on kalman filtering, and obtaining a vehicle pitch angle includes: the measurement obtained by the GNSS and INS sensors is fused by adopting a Kalman filtering technology, and the method specifically comprises the following steps:
system state vector:
position error measurement equation:
speed error measurement equation:
heading angle error measurement equation:
calculating one-step state prediction:
state one-step prediction mean square error:
kalman filter gain:
state estimation:
state estimation mean square error:
wherein ,
respectively representing the east, north and sky attitude error angles of the INS; />
Speed errors in the east and north directions of the INS; />
Latitude, longitude, and altitude errors for INS; />
Zero bias error of the gyroscope, subscript +.>
Respectively representing the three directions of the roll axis, the pitch axis and the yaw axis of the gyroscope, and the superscript +.>
Then the projection in the carrier coordinate system is represented; />
Zero offset error of the accelerometer, subscript +.>
Respectively representing the directions of a horizontal axis, a vertical axis and a vertical axis of the accelerometer, and the superscript +.>
Also representing the projection in the carrier coordinate system; in the position error measurement equation, < >>
For a measurement matrix of position errors, < >>
Difference between the position measurements of INS and GNSS, +.>
A measurement noise matrix that is a position error; in the speed error measurement equation, < >>
For a measurement matrix of speed errors, < >>
The difference between the speed measurements of the INS and GNSS,
a measurement noise matrix that is a velocity error; in the course angle measurement equation, < >>
and />
Respectively representing the course angle obtained by IMU integration and the course angle obtained by double-antenna GNSS measurement, +.>
The difference between the resulting heading angle and the gaussian white noise of the dual antenna GNSS measured heading angle is integrated for the IMU.
3. The vehicle attitude angle-based road gradient estimation method according to claim 2, wherein in the kalman filter formula
From the state estimate of the previous moment +.>
Predicting the state of time; (/>
) Measuring the prediction error, wherein the measurement error comprises measurement information; by predicting the mean square error in one step for the state +.>
Deriving to obtain the calculated gain factor +.>
To estimate the state of the previous moment and +.>
The time measurements are linearly combined (weighted estimation) to finally obtain +.>
State estimation of time of day->
Also known as status->
Is a posterior estimate of (1).
4. The vehicle attitude angle-based road gradient estimation method according to claim 1, characterized in that the step of fourier transforming the vehicle pitch angle to convert a time domain signal into a frequency domain signal includes:
converting the aperiodic pitch angle signal by using Fourier transform to obtain a frequency spectrum density function:
wherein ,
is a complex function and can be written as:
wherein ,
is->
Which represents the relative magnitudes of the frequency components in the signal.
5. The vehicle attitude angle-based road gradient estimation method according to any one of claims 1 to 4, wherein, for the fourier transform-based calculation process, various parameters involved in a fourier transform function include:
the sampling frequency is not less than twice the maximum value of the road gradient signal frequency;
the sampling time is determined to be the minimum value by the sampling frequency;
the Fourier transform points are the same as the sampling points, and the sampling points are determined by the sampling time;
the low pass filter cut-off frequency is determined based on a sliding window switching and data smoothing method.
Applications Claiming Priority (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN2022102017552 | 2022-03-02 | ||
| CN202210201755 | 2022-03-02 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116045972A true CN116045972A (en) | 2023-05-02 |
Family
ID=86112621
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211448013.6A Pending CN116045972A (en) | 2022-03-02 | 2022-11-18 | Road gradient estimation method based on vehicle attitude angle |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116045972A (en) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117056799A (en) * | 2023-08-03 | 2023-11-14 | 广东省机场管理集团有限公司工程建设指挥部 | Processing method, device, equipment and medium for vehicle sensor data |
| CN117128927A (en) * | 2023-10-26 | 2023-11-28 | 腾讯科技（深圳）有限公司 | Method and device for determining parking floor and storage medium |
| CN117909665A (en) * | 2024-03-18 | 2024-04-19 | 青岛哈尔滨工程大学创新发展中心 | Ship motion envelope forecast data processing method and system based on Fourier filtering |
-
2022
- 2022-11-18 CN CN202211448013.6A patent/CN116045972A/en active Pending
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117056799A (en) * | 2023-08-03 | 2023-11-14 | 广东省机场管理集团有限公司工程建设指挥部 | Processing method, device, equipment and medium for vehicle sensor data |
| CN117056799B (en) * | 2023-08-03 | 2024-03-26 | 广东省机场管理集团有限公司工程建设指挥部 | Processing method, device, equipment and medium for vehicle sensor data |
| CN117128927A (en) * | 2023-10-26 | 2023-11-28 | 腾讯科技（深圳）有限公司 | Method and device for determining parking floor and storage medium |
| CN117909665A (en) * | 2024-03-18 | 2024-04-19 | 青岛哈尔滨工程大学创新发展中心 | Ship motion envelope forecast data processing method and system based on Fourier filtering |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN116045972A (en) | Road gradient estimation method based on vehicle attitude angle | |
| WO2020154970A1 (en) | Deep learning–based feature extraction for lidar localization of autonomous driving vehicles | |
| US20190003839A1 (en) | Method and system for updating maps based on control feedbacks of autonomous driving vehicles | |
| CN109435955B (en) | Performance evaluation method, device and equipment for automatic driving system and storage medium | |
| US10613489B2 (en) | Method and system for determining optimal coefficients of controllers for autonomous driving vehicles | |
| US11704554B2 (en) | Automated training data extraction method for dynamic models for autonomous driving vehicles | |
| Nitsche et al. | Comparison of machine learning methods for evaluating pavement roughness based on vehicle response | |
| US10775790B2 (en) | Methods and systems for model predictive control of autonomous driving vehicle | |
| CN108345021B (en) | Doppler radar assisted GPS/INS vehicle speed measurement method | |
| US11332152B2 (en) | Method and apparatus for determining a velocity of a vehicle | |
| CN110341715A (en) | Unmanned vehicle ramp method for control speed and device | |
| CN114111775B (en) | Multi-sensor fusion positioning method and device, storage medium and electronic equipment | |
| CN112964291B (en) | Sensor calibration method, device, computer storage medium and terminal | |
| CN112835085A (en) | Method and device for determining vehicle position | |
| Bonnabel et al. | Design and prototyping of a low-cost vehicle localization system with guaranteed convergence properties | |
| Gao et al. | An integrated land vehicle navigation system based on context awareness | |
| Bifulco et al. | Real-time smoothing of car-following data through sensor-fusion techniques | |
| CN110779514B (en) | Hierarchical Kalman fusion method and device for auxiliary attitude determination of bionic polarization navigation | |
| CN111469781B (en) | For use in output of information processing system method and apparatus of (1) | |
| US20210188266A1 (en) | A camera-based low-cost lateral position calibration method for level-3 autonomous vehicles | |
| CN112304281A (en) | Road slope measuring method, terminal equipment and storage medium | |
| US11662454B2 (en) | Systems and methods for range-rate dealiasing using position consistency | |
| CN111060944B (en) | Vehicle dynamic tracking method based on current-epipse model | |
| WO2022198590A1 (en) | Calibration method and apparatus, intelligent driving system, and vehicle | |
| CN114252081B (en) | Positioning method, device, equipment and 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 |