CN111665530B - GPS diagnosis method based on vehicle state - Google Patents
GPS diagnosis method based on vehicle state Download PDFInfo
- Publication number
- CN111665530B CN111665530B CN202010422088.1A CN202010422088A CN111665530B CN 111665530 B CN111665530 B CN 111665530B CN 202010422088 A CN202010422088 A CN 202010422088A CN 111665530 B CN111665530 B CN 111665530B
- Authority
- CN
- China
- Prior art keywords
- vehicle
- time
- data
- front wheel
- moment
- 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
Images
Classifications
-
- 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/396—Determining accuracy or reliability of position or pseudorange measurements
-
- 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
-
- 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
Abstract
The invention discloses a GPS diagnosis method based on vehicle state, comprising the following steps: basic information acquisition; time alignment of sensor feedback data; spatial alignment of sensor feedback data; caching data at the historical moment and maintaining the scale unchanged; estimating the current position of the vehicle according to the information of the historical moment; comparing with the positioning result of the GPS data and outputting a judging result. The diagnosis method provided by the invention is suitable for the unmanned environment of the mining area, and can infer the pose change condition of the vehicle in the time period after a series of data processing and calculation according to the self parameters of the vehicle in the time period from a certain moment to the current moment in the motion process of the vehicle; and then comparing the GPS data with GPS positioning information to judge whether the GPS data is abnormal or not, so as to avoid decision errors caused by positioning the GPS data which is shifted in the follow-up reference.
Description
Technical Field
The invention relates to the field of active safety of automobiles, in particular to a GPS diagnosis method based on a vehicle state.
Background
Currently, in the field of active safety of automobiles, the motion state of a vehicle is mainly measured or estimated by three methods. One is to use low cost vehicle sensors to make simple mathematical calculations on the measured signals to obtain relevant vehicle operating conditions. And secondly, the high-precision sensor is utilized to directly measure the running state of the related vehicle (such as a Global Navigation Satellite System (GNSS), in particular a high-precision Global Positioning System (GPS) and the like). The third is a model method, namely, the running process of the automobile is subjected to kinematic or dynamic modeling, and meanwhile, low-cost vehicle-mounted sensor information is used as observation information, and a proper filtering estimation algorithm is carried out to realize the estimation of the running state of the automobile. In mining areas, vehicles are generally positioned and track planned by using GPS information, and the vehicles cannot normally run or have serious consequences due to the fact that the road conditions of the mining areas in the open mining areas are poor and the GPS signals of roads under the mining pits are sometimes poor and easy to drift.
If the abnormal behavior of the vehicle is detected, the vehicle behavior can be detected, prompted and early-warned in the abnormal behavior change process of the vehicle, the vehicle state can be immediately judged when the vehicle reaches the final state, and the final early-warning is provided, so that the vehicle detection time is greatly shortened, the management center can be timely warned and reported, the event is rapidly processed, and the occurrence of secondary accidents is prevented. Therefore, the detection of abnormal behavior of the vehicle is performed in real time, so that accidents are prevented, the running efficiency of the vehicle is improved, and the low-carbon safe trip target is realized. So, research on automatic detection algorithms of abnormal data of vehicles and dynamic feedback of the abnormal data of the vehicles have become an important research direction of researchers.
Most of the prior technical schemes directly use GPS data to observe the state of the vehicle, do not evaluate the reliability and stability of the GPS data, and in fact, the GPS data is not continuously stable and is not wrong. If the wrong positioning information is used for controlling the vehicle, obstacle avoidance and other treatments, great system faults and potential safety hazards are necessarily brought. Patent CN108399743 a discloses a method and a flow for detecting abnormal behavior of a highway vehicle based on GPS data; patent CN102556075 a discloses a vehicle running state estimation method based on improved extended kalman filtering; patent CN108136867 a) discloses a vehicle location point forwarding method of an autonomous vehicle, and a flow chart of the technical scheme is shown in fig. 1; in the description process of the technical scheme, the reliability of the GPS data is not detected, a method related to the diagnosis of the GPS data is not involved, and the accuracy of all default GPS data is high. It is only mentioned in patent CN108136867 a that the second position related to the front wheels is calculated based on the moving direction and the first position related to the rear wheels, but the accuracy problem of the front wheel angle is not considered, the difference between the front wheel drive and the rear wheel drive is not considered, and the associated calculation is not made on the history position information.
Disclosure of Invention
The invention aims to solve the problems and the shortcomings, and provides an unmanned vehicle state estimation and GPS diagnosis method for an open-air mining area, which is suitable for the unmanned environment of the mining area, and can infer the pose change condition of the vehicle in the time period according to the self parameters (speed and front wheel rotation angle) from a certain moment to the current moment of the history of the movement of the vehicle after a series of data processing and calculation. And then, by comparing the GPS information with the GPS positioning information, whether the GPS data is abnormal or not can be judged, and decision errors caused by positioning the GPS data which is shifted in the follow-up reference process are avoided, which is particularly important for comparing the mining area automatic driving display depending on the GPS information.
In order to achieve the above purpose, the present invention adopts the following technical scheme:
a vehicle state-based GPS diagnostic method, comprising the steps of:
step one: basic information acquisition;
step two: time alignment of sensor feedback data;
step three: spatial alignment of sensor feedback data;
step four: caching data at the historical moment and maintaining the scale unchanged;
step five: estimating the current position of the vehicle according to the information of the historical moment;
step six: comparing with the positioning result of the GPS data and outputting a judging result.
Further, the first step, the front wheel steering angle information of the vehicle and the speed information of the vehicle are obtained through a vehicle drive-by-wire interface, and the longitude, latitude and heading information of the vehicle are obtained through a combined inertial navigation module; the input data of the front wheel corner needs to be subjected to stability judgment and processing.
Further, the stability judging and processing process of the input data of the front wheel corner is as follows:
the front wheel angle is phi, the difference value is judged from the first to the last, if phi i+2 -φ i+1 >Delta phi, fitting a unitary quadratic equation of the front wheel rotation angle and delta t over 20 time intervals using a least squares method, wherein phi i+2 Is the front wheel rotation angle value phi at the current moment i+1 The front wheel rotation angle value at the previous moment is delta phi, which is the maximum change value of the front wheel rotation angle in an interval;
the value y of the current front wheel rotation angle is calculated through the time interval x of the two front wheel rotation angle data, the process is as follows,
wherein y (x) i ) Front wheel rotation angle value estimated for i time, y i For the front wheel rotation angle value measured at the moment i, Q is the difference value between the estimated and measured front wheel rotation angles, and a, b and c are coefficients in a front wheel rotation angle estimation formula;
the values of a, b, c when Q is minimum are determined, first the partial derivatives of Q to a, b, c are determined:
the method comprises the following steps of:
the change curve fitting formula of the front wheel rotation angle in the last period of time is obtained through the process, namely the front wheel rotation angle value at the current moment can be predicted, and if the currently received front wheel rotation angle observed value is found to exceed the threshold value, the front wheel rotation angle value at the current moment is replaced in the mode, so that the normal operation of the algorithm is ensured.
Further, in the second step, the time alignment process is as follows:
system Time stamp Time_s to receive GPS header frame data i And the corresponding GPS self-contained Time interval (Time_g) i+1 -Time_g i ) Accumulating and delivering the Time stamp Time_s of the subsequent GPS data under the system Time reference i+1 The calculation formula is as follows:
Time_s i+1 ＝Time_s i +(Time_g i+1 -Time_g i )
the Time of receipt is time_s i GPS data and time of Ts i And Ts i+1 If a uniform change assumption is made on the speed in the short term, time_s i The speed at time should be:
wherein the method comprises the steps of
For Time_s i Vehicle speed at time instant>
For time Ts i The vehicle speed at the moment in time,
for Ts i+1 Vehicle speed at time.
Further, the spatial alignment process in the third step is as follows: firstly, a coordinate system of a vehicle is defined uniformly, the center of mass position of the vehicle at the current moment is taken as a center of a circle, the direction of the mass center of the vehicle pointing to the vehicle head is taken as a y axis, the direction of the mass center of the vehicle pointing to the right side of the vehicle body perpendicular to the y axis is taken as an x axis, the direction of the mass center of the vehicle pointing to the vehicle roof perpendicular to the x axis and the y axis is taken as a z axis, and the coordinate system of the mass center of the vehicle is established. Wherein, the vector rotating along the positive direction of the coordinate axis is positive;
the data obtained from the sensor is based on the current sensor coordinate system, and is spatially aligned into the vehicle centroid coordinate system, the spatial alignment including rotation and translation of the coordinate system, defining the rotation matrix of the coordinate system as
The translation matrix of the coordinate system is +.>
There is->
Wherein->
Indicated at t k+Δ1 At the moment, the mass center coordinates of the vehicle in an s coordinate system; />
Indicated at t k+Δ1 At time, the vehicle centroid coordinates in the v coordinate system.
Further, the step four, the vehicle information of the latest moment is received, including the front wheel rotation angle, the vehicle speed, the longitude, the latitude and the course angle, the calculation of the vehicle state relative to the previous moment is carried out after the information is received,
firstly, calculating the turning radius of the vehicle at the current moment according to the front wheel rotation angle and the front-rear wheel base of the vehicle body:
where radii is the turning radius at the center of the vehicle body, L is the vehicle wheelbase, and φ is the front wheel corner;
the turning radius at the center of the rear wheel axle distance of the vehicle is as follows:
1) Calculating vehicle cornering angular velocity
Wherein omega is the turning angular speed of the vehicle body, v vehicle Feeding back the vehicle speed for the vehicle;
2) Integral of angle
Δθ＝ω×Δt
Wherein delta theta is the angle through which the vehicle body rotates at the current angular speed within a delta t time; Δt is t i And t i+1 Is a time interval of (2);
3) Position estimation
Δy＝radii×sin(|ω×Δt|)
Δx＝radii×(1-cos(ω×Δt))
Wherein, deltax and Deltay are respectively under the vehicle coordinate system at the current moment, and after a Deltat time, the position coordinates of the vehicle are obtained;
4) Cached updates
In the cache, there are the latest N times (t 0～ t n ) Every time there is a new data update, the new data is stored in t n In the corresponding cache, t is removed 0 Corresponding historical data, and sequentially shifting the rest cache data; maintaining the cached data scale, namely, the cached data at the historical moment cannot be too much, adding extra calculated amount and generating overlarge accumulated errors; but also can not make the cache data too little, and increase the influence of random errors on the final result.
Further, in the fifth step, at t n The vehicle coordinate system at the moment is taken as a benchmark, a reference coordinate system is established, t i Time-of-day vehicle coordinate system to t n The transformation matrix of the vehicle coordinate system at the moment is
Wherein θ i ＝Δθ i+1 +…+Δθ n ,θ i At t n Relative vehicle orientation at time t i Angle difference, Δθ, of the vehicle orientation at time i+1 At t i To t i+1 The angle of the car body changes at any time;
t i the estimated position of the time is set to (x) i ,y i ) From t i ～t n The information of the moment of time is deduced,
wherein Deltax is i And Deltay i At t i From time to t i+1 The moving distance of the vehicle body at any time;
by the above solution, the state change of the vehicle at the ith moment relative to the current moment is obtained, and the obtained change amounts are displacement in the x and y directionsRelative rotation angle with the vehicle body; after the relative state change is obtained, t is used as n The vehicle coordinate system at the moment is taken as a benchmark, a reference coordinate system is established, t i Time sum t n The longitude and latitude and course information of the moment can be directly obtained by the combined inertial navigation module, and then t can be obtained by calculation i The vehicle position at time t n Coordinate position in vehicle coordinates at a time
Further, in the step six, the calculation result
And->
Make a determination if
Setting a correlation threshold c threshold For the calculation result, if the matching result deltas is smaller than the threshold value c threshold The GPS data is considered not to drift, if the matching result deltas is greater than the threshold value c threshold The GPS data is considered to drift and processing measures are required.
Compared with the prior art, the invention has the following beneficial effects:
(1) The invention provides a simple and efficient GPS data diagnosis solution, which can predict the accuracy of GPS information by utilizing the vehicle body state information at the historical moment, does not increase other measurement sensor data, and achieves simplification and high efficiency.
(2) The invention obtains the relative movement distance and angle of the vehicle by integrating the movement state of the vehicle at the historical moment
(3) The invention uses the principle of least square to judge and process the stability of the front wheel steering angle data in the calculation process, and time alignment is carried out on various data.
(4) The invention obtains the relative change quantity of the posture of the vehicle body for a period of time under the condition that the vehicle displacement and the rotation calculated by the method are related to a unified reference coordinate system, and then compares the relative change quantity with the result obtained by GPS positioning information to judge whether the GPS data is accurate or not
Drawings
Fig. 1 is a flow chart of an autonomous vehicle location point forwarding method provided in the prior art.
Fig. 2 is a flowchart of a GPS diagnostic method based on a vehicle state according to the present invention.
Fig. 3 is a time alignment schematic diagram of a GPS diagnostic method based on a vehicle state according to the present invention.
Fig. 4 is a vehicle relative angle diagram of a GPS diagnostic method based on a vehicle state according to the present invention.
Fig. 5 is a vehicle relative movement distance deviation of a GPS diagnostic method based on a vehicle state according to the present invention.
Detailed Description
Embodiments of the present invention will be described in detail and explicitly with reference to the examples and the accompanying drawings.
A vehicle state-based GPS diagnosis method, as shown in fig. 2, comprises the following specific steps:
step one: and obtaining basic information.
The front wheel rotation angle information of the vehicle and the speed information of the vehicle are obtained through a vehicle drive-by-wire interface, and the longitude and latitude information and the course information of the vehicle are obtained through a combined inertial navigation module. The data transmitted by the two methods have errors in space and time, and the original data is firstly required to be processed.
Abnormal jitter may occur in the front wheel corner data, but since the subsequent calculation is a correlation and accumulation process, if frame loss occurs in the middle, the final determination result is greatly affected, so that stability determination and processing are required for the input data, and the following process is used for processing the front wheel corner.
The front wheel angle is phi, the difference value is judged from the first to the last, if phi i+2 -φ i+1 >Delta phi, fitting the front wheel rotation angle sum in 20 time intervals by using a least square methodUnitary quadratic equation for Δt, where φ i+2 Is the front wheel rotation angle value phi at the current moment i+1 The front wheel rotation angle value at the previous moment is delta phi, which is the maximum change value of the front wheel rotation angle in one interval.
The time interval x of the two front wheel steering angle data in the following formula, the value y of the current front wheel steering angle is calculated,
wherein y (x) i ) Front wheel rotation angle value estimated for i time, y i For the front wheel rotation angle value measured at the moment i, Q is the difference between the estimated and measured front wheel rotation angles, and a, b and c are coefficients in the front wheel rotation angle estimation formula.
The values of a, b, c when Q is minimum are determined, first the partial derivatives of Q to a, b, c are determined:
the method comprises the following steps of:
the change curve fitting formula of the front wheel rotation angle in the last period of time can be obtained through the process, namely the front wheel rotation angle value at the current moment can be predicted, and if the currently received front wheel rotation angle observed value is found to exceed the threshold value, the front wheel rotation angle value at the current moment is replaced in the mode, so that the normal operation of the algorithm is ensured.
Step two: the sensor feeds back time alignment of the data.
Since the GPS data and the feedback-by-wire data may not be sent out in the same state of the vehicle body, calculation using data in different states obviously presents a lot of problems, so that the two parts of data are aligned to the same time reference first. The specific implementation process is that firstly, the system Time stamp Time_s of the GPS first frame data is received i And the corresponding GPS self-contained Time interval (Time_g) i+1 -Time_g i ) Accumulating and delivering the Time stamp Time_s of the subsequent GPS data under the system Time reference i+1 The calculation formula is as follows:
Time_s i+1 ＝Time_s i +(Time_g i+1 -Time_g i )
similarly, as shown in fig. 3, the data fed back by the control needs to be time-synchronized through the above operation, that is, the system time stamp of the first frame of data is obtained, and then accumulated with the time interval of the subsequently received data to deliver the time stamp under the system time reference. The time stamps of the two types of data are converted into the system time reference, and each data is provided with the time stamp with the same time reference after the operation, so that the sequence of the types of the received data can be easily judged. Thereafter Time-aligning the different types of data to the same type, e.g. Time of receipt of time_s i GPS data and time of Ts i And Ts i+1 If a uniform change assumption is made on the speed in the short term, time_s i The speed at time should be:
wherein the method comprises the steps of
For Time_s i Vehicle speed at time instant>
For time Ts i The vehicle speed at the moment in time,
is->
Vehicle speed at time.
And replacing the speed data with front wheel steering angle data to perform the same operation, so as to realize time alignment of the front wheel steering angle data.
Step three: the sensor feeds back the spatial alignment of the data.
Firstly, a coordinate system of a vehicle is defined uniformly, the center of mass position of the vehicle at the current moment is taken as a center of a circle, the direction of the mass center of the vehicle pointing to the vehicle head is taken as a y axis, the direction of the mass center of the vehicle pointing to the right side of the vehicle body perpendicular to the y axis is taken as an x axis, the direction of the mass center of the vehicle pointing to the vehicle roof perpendicular to the x axis and the y axis is taken as a z axis, and the coordinate system of the mass center of the vehicle is established. Wherein the vector rotated in the forward direction along the coordinate axis is positive.
The data obtained from the sensor is based on the current sensor coordinate system and is spatially aligned to the vehicle centroid coordinate system by a spatial alignment including rotation and translation of the coordinate system, defining a rotation matrix of the coordinate system as
The translation matrix of the coordinate system is +.>
There is->
Wherein->
Indicated at t k+Δ1 At the moment, the mass center coordinates of the vehicle in an s coordinate system; />
Indicated at t k+Δ1 At time, the vehicle centroid coordinates in the v coordinate system.
In particular, when calculating, GPS data, speed, and other data at different times, when referring to the vehicle body centroid coordinate system at different times, it is necessary to calculate motion information of the vehicle at the current time relative to motion information of the vehicle at other historical times according to motion information (including longitude, latitude, speed, and the like) of the vehicle itself and motion information of other vehicles (including longitude, latitude, speed, and the like).
Step four: data at the historical moment is cached and the scale is maintained unchanged.
And receiving vehicle information at the latest moment, wherein the vehicle information comprises front wheel rotation angles, vehicle speeds, longitudes, latitudes and course angles. After receiving the information, the vehicle state calculation is first performed at the previous time. The displacement and rotation of the vehicle at the current moment relative to the previous moment are mainly calculated.
Firstly, calculating the turning radius of the vehicle at the current moment according to the front wheel rotation angle and the front-rear wheel base of the vehicle body:
where radii is the turning radius at the center of the vehicle body, L is the vehicle wheelbase and φ is the front wheel turn angle.
The turning radius at the center of the rear wheel axle distance of the vehicle is as follows:
5) Calculating vehicle cornering angular velocity
Wherein omega is the turning angular speed of the vehicle body, v vehicle The vehicle speed is fed back for the vehicle.
6) Integral of angle
Δθ＝ω×Δt
Wherein delta theta is the angle through which the vehicle body rotates at the current angular speed within a delta t time; Δt is t i And t i+1 Is a time interval of (a) for a time period of (b).
7) Position estimation
Δy＝radii×sin(|ω×Δt|)
Δx＝radii×(1-cos(ω×Δt))
And the delta x and delta y are respectively in the vehicle coordinate system at the current moment, and after a delta t time, the position coordinates of the vehicle are obtained.
8) Cached updates
In the cache, there are the latest N times (t 0～ t n ) Every time there is a new data update, the new data is stored in t n In the corresponding cache, t is removed 0 Corresponding historical data and residual cache data are sequentially shifted. And maintaining the cached data scale, namely, the cached data at the historical moment cannot be too much, so that extra calculation amount is increased and overlarge accumulated errors are generated. But also can not make the cache data too little, and increase the influence of random errors on the final result.
Step five: and estimating the current position of the vehicle according to the information of the historical moment.
At t n The vehicle coordinate system at the moment is taken as a benchmark, a reference coordinate system is established, t i Time-of-day vehicle coordinate system to t n The transformation matrix of the vehicle coordinate system at the moment is
Wherein θ i ＝Δθ i+1 +…+Δθ n ,θ i At t n Relative vehicle orientation at time t i Angle difference, Δθ, of the vehicle orientation at time i+1 At t i To t i+1 The angle of the vehicle body changes at the moment.
t i The estimated position of the time is set to (x) i ,y i ) From t i ～t n The information of the moment of time is deduced,
wherein Deltax is i And Deltay i At t i From time to t i+1 The moving distance of the vehicle body at the moment.
Through the solution, the state change of the vehicle at the ith moment relative to the state change at the current moment can be obtained, and the obtained change amounts are displacement in the x and y directions and relative rotation angle of the vehicle body.
After the calculated state relative change amount is obtained, t is used as n The vehicle coordinate system at the moment is taken as a benchmark, a reference coordinate system is established, t i Time sum t n The longitude and latitude and course information of the moment can be directly obtained by the combined inertial navigation module, and then t can be obtained by calculation i The vehicle position at time t n Coordinate position in vehicle coordinates at a time
Step six: comparing with the positioning result of the GPS data and outputting a judging result.
Respective calculation results
And->
Judging if->
Setting a correlation threshold c threshold For the calculation result, if the matching result deltas is smaller than the threshold value c threshold The GPS data is considered to have not drifted, if matchedThe result deltas is greater than the threshold value c threshold The GPS data is considered to drift and some processing measures are required. The actual effect of testing the data by this method is shown in fig. 4 and 5. The dashed line in fig. 4 is the calculated time t of each vehicle using this method i And angular change for the previous n time intervals, the solid line is t of GPS data i And the heading angle change for the first n time intervals, it can be seen that the two are very close. Fig. 5 is the offset distance calculated by the sixth step, and it can be seen that the calculated position is less than 1 m as a whole compared to the GPS positioning result when the vehicle is traveling at a speed of 5 m/s.
It will be apparent to those skilled in the art that several modifications and improvements can be made to the embodiments of the present invention without departing from the inventive concept thereof, which fall within the scope of the invention.
Claims (4)
1. A vehicle state-based GPS diagnostic method, comprising the steps of:
step one: basic information acquisition;
step two: time alignment of sensor feedback data;
step three: spatial alignment of sensor feedback data;
step four: caching data at the historical moment and maintaining the scale unchanged;
step five: estimating the current position of the vehicle according to the information of the historical moment;
step six: comparing the GPS data with the positioning result of the GPS data and outputting a judging result;
the method comprises the steps that firstly, front wheel rotation angle information of a vehicle and speed information of the vehicle are obtained through a vehicle drive-by-wire interface, and longitude and latitude information of the vehicle are obtained through a combined inertial navigation module; the input data of the front wheel corner needs to be subjected to stability judgment and processing; the stability judging and processing process of the input data of the front wheel corner is as follows: the front wheel angle is phi, the difference value is judged from the first to the last, if phi i+2 -φ i+1 >Delta phi, fitting the front wheel rotation angle and delta t to one element two in 20 time intervals by using least square methodA secondary equation, wherein phi i+2 Is the front wheel rotation angle value phi at the current moment i+1 The front wheel rotation angle value at the previous moment is delta phi, which is the maximum change value of the front wheel rotation angle in an interval;
the value y of the current front wheel rotation angle is calculated through the time interval x of the two front wheel rotation angle data, the process is as follows,
wherein y (x) i ) Front wheel rotation angle value estimated for i time, y i For the front wheel rotation angle value measured at the moment i, Q is the difference value between the estimated and measured front wheel rotation angles, and a, b and c are coefficients in a front wheel rotation angle estimation formula;
the values of a, b, c when Q is minimum are determined, first the partial derivatives of Q to a, b, c are determined:
the method comprises the following steps of:
the change curve fitting formula of the front wheel rotation angle in the last period of time is obtained through the process, namely the front wheel rotation angle value at the current moment can be predicted, and if the currently received front wheel rotation angle observed value is found to exceed the threshold value, the front wheel rotation angle value at the current moment is replaced in the mode, so that the normal operation of an algorithm is ensured;
step four, receiving the vehicle information at the latest moment, including the front wheel rotation angle, the vehicle speed, the longitude, the latitude and the course angle, calculating the vehicle state at the last moment after receiving the information,
firstly, calculating the turning radius of the vehicle at the current moment according to the front wheel rotation angle and the front-rear wheel base of the vehicle body:
where radii is the turning radius at the center of the vehicle body, L is the vehicle wheelbase, and φ is the front wheel corner;
the turning radius at the center of the rear wheel axle distance of the vehicle is as follows:
1) Calculating vehicle cornering angular velocity
Wherein omega is the turning angular speed of the vehicle body, v vehicle Feeding back the vehicle speed for the vehicle;
2) Integral of angle
Δθ＝ω×Δt
Wherein delta theta is the angle through which the vehicle body rotates at the current angular speed within a delta t time; Δt is t i And t i+1 Time of (1)An interval;
3) Position estimation
Δy＝radii×sin(|ω×Δt|)
Δx＝radii×(1-cos(ω×Δt))
Wherein, deltax and Deltay are respectively under the vehicle coordinate system at the current moment, and after a Deltat time, the position coordinates of the vehicle are obtained;
4) Cached updates
The latest N moments, namely t, coexist in the cache 0～ t n Every time there is a new data update, the new data is stored in t n In the corresponding cache, t is removed 0 Corresponding historical data, and sequentially shifting the rest cache data; maintaining the cached data scale, namely, the cached data at the historical moment cannot be too much, adding extra calculated amount and generating overlarge accumulated errors; the buffer data cannot be too little, and the influence of random errors on the final result is increased;
step five, at t n The vehicle coordinate system at the moment is taken as a benchmark, a reference coordinate system is established, t i Time-of-day vehicle coordinate system to t n The transformation matrix of the vehicle coordinate system at the moment is
Wherein θ i ＝Δθ i+1 +…+Δθ n ,θ i At t n Relative vehicle orientation at time t i Angle difference, Δθ, of the vehicle orientation at time i+1 At t i To t i+1 The angle of the car body changes at any time;
t i the estimated position of the time is set to (x) i ,y i ) From t i ～t n The information of the moment of time is deduced,
wherein Deltax is i And deltay i At t i From time to t i+1 The moving distance of the vehicle body at any time;
obtaining the state change of the vehicle at the ith moment relative to the current moment through the solution, wherein the obtained change amounts are displacement in the x and y directions and relative rotation angle of the vehicle body; after the relative state change is obtained, t is used as n The vehicle coordinate system at the moment is taken as a benchmark, a reference coordinate system is established, t i Time sum t n The longitude and latitude and course information of the moment can be directly obtained by the combined inertial navigation module, and then t can be obtained by calculation i The vehicle position at time t n Coordinate position in vehicle coordinates at a time
In the sixth step
And->
And comparing and outputting a judging result.
2. A vehicle condition based GPS diagnostic method according to claim 1, wherein the second time alignment procedure is as follows:
system Time stamp Time_s to receive GPS header frame data i And the corresponding GPS self-contained Time interval (Time_g) i+1 -Time_g i ) Accumulating and delivering the Time stamp Time_s of the subsequent GPS data under the system Time reference i+1 The calculation formula is as follows:
Time_s i+1 ＝Time_s i +(Time_g i+1 -Time_g i )
the Time of receipt is time_s i GPS data and time of Ts i And Ts i+1 If a uniform change assumption is made on the speed in the short term, time_s i The speed at time should be:
wherein the method comprises the steps of
For Time_s i Vehicle speed at time instant>
For time Ts i Vehicle speed at time,/->
For Ts i+1 Vehicle speed at time.
3. A vehicle condition based GPS diagnostic method according to claim 2, wherein the spatial alignment procedure is as follows: firstly, uniformly defining a coordinate system of a vehicle, taking the position of the mass center of the vehicle at the current moment as a circle center, pointing to the direction of a vehicle head as a y axis, pointing to the right side of the vehicle body perpendicular to the y axis as an x axis, and pointing to the direction of a vehicle roof perpendicular to the x axis and the y axis as a z axis, so as to establish the coordinate system of the mass center of the vehicle; wherein, the vector rotating along the positive direction of the coordinate axis is positive;
the data obtained from the sensor is based on the current sensor coordinate system, and is spatially aligned into the vehicle centroid coordinate system, the spatial alignment including rotation and translation of the coordinate system, defining the rotation matrix of the coordinate system as
The translation matrix of the coordinate system is +.>
There is->
Wherein->
Indicated at t k+Δ1 At the moment, the mass center coordinates of the vehicle in an s coordinate system; />
Indicated at t k+Δ1 At time, the vehicle centroid coordinates in the v coordinate system.
4. A vehicle state-based GPS diagnostic method according to claim 3, wherein the step six is performed on the calculation result
And->
Judging if->
Setting a correlation threshold c threshold For the calculation result, if the matching result deltas is smaller than the threshold value c threshold The GPS data is considered not to drift, if the matching result deltas is greater than the threshold value c threshold The GPS data is considered to drift and processing measures are required. />
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010422088.1A CN111665530B (en) | 2020-05-18 | 2020-05-18 | GPS diagnosis method based on vehicle state |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010422088.1A CN111665530B (en) | 2020-05-18 | 2020-05-18 | GPS diagnosis method based on vehicle state |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111665530A CN111665530A (en) | 2020-09-15 |
| CN111665530B true CN111665530B (en) | 2023-06-02 |
Family
ID=72383897
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010422088.1A Active CN111665530B (en) | 2020-05-18 | 2020-05-18 | GPS diagnosis method based on vehicle state |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111665530B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115201873B (en) * | 2022-09-06 | 2023-07-28 | 中冶智诚(武汉)工程技术有限公司 | Multi-system cooperative indoor and outdoor precise positioning system architecture and operation method thereof |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101334288A (en) * | 2008-08-07 | 2008-12-31 | 北京工业大学 | Public transport bus exact stop method based on standard line matching |
| EP2325607A1 (en) * | 2009-11-20 | 2011-05-25 | Sumitomo Rubber Industries, Ltd. | Vehicle estimate navigation appartus, vehicle estimate navigation method, and vehicle estimate navigation program |
| KR20150059420A (en) * | 2013-11-22 | 2015-06-01 | 에스케이플래닛 주식회사 | Route guidance service system, method and apparatus for fault diagonosis of gps in the system |
| CN104880722A (en) * | 2015-03-25 | 2015-09-02 | 清华大学 | GPS speed and position observation abnormal value detection method for unmanned aerial vehicle (UAV) |
| CN105300395A (en) * | 2014-07-11 | 2016-02-03 | 北京协进科技发展有限公司 | Navigation and positioning method and device |
| CN205161498U (en) * | 2015-09-29 | 2016-04-20 | 大连民族大学 | Long -range system of herding based on satellite positioning and electronic map |
| CN109975844A (en) * | 2019-03-25 | 2019-07-05 | 浙江大学 | A kind of anti-bleach-out process of GPS signal based on optical flow method |
| CN110418980A (en) * | 2017-03-17 | 2019-11-05 | 维宁尔美国公司 | Communication for high accuracy co-positioned solution |
| CN111152795A (en) * | 2020-01-08 | 2020-05-15 | 东南大学 | Model and parameter dynamic adjustment based adaptive vehicle state prediction system and prediction method |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US7756639B2 (en) * | 2003-10-06 | 2010-07-13 | Sirf Technology, Inc. | System and method for augmenting a satellite-based navigation solution |
| JP5898746B1 (en) * | 2014-09-29 | 2016-04-06 | 富士重工業株式会社 | Vehicle travel control device |
-
2020
- 2020-05-18 CN CN202010422088.1A patent/CN111665530B/en active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101334288A (en) * | 2008-08-07 | 2008-12-31 | 北京工业大学 | Public transport bus exact stop method based on standard line matching |
| EP2325607A1 (en) * | 2009-11-20 | 2011-05-25 | Sumitomo Rubber Industries, Ltd. | Vehicle estimate navigation appartus, vehicle estimate navigation method, and vehicle estimate navigation program |
| KR20150059420A (en) * | 2013-11-22 | 2015-06-01 | 에스케이플래닛 주식회사 | Route guidance service system, method and apparatus for fault diagonosis of gps in the system |
| CN105300395A (en) * | 2014-07-11 | 2016-02-03 | 北京协进科技发展有限公司 | Navigation and positioning method and device |
| CN104880722A (en) * | 2015-03-25 | 2015-09-02 | 清华大学 | GPS speed and position observation abnormal value detection method for unmanned aerial vehicle (UAV) |
| CN205161498U (en) * | 2015-09-29 | 2016-04-20 | 大连民族大学 | Long -range system of herding based on satellite positioning and electronic map |
| CN110418980A (en) * | 2017-03-17 | 2019-11-05 | 维宁尔美国公司 | Communication for high accuracy co-positioned solution |
| CN109975844A (en) * | 2019-03-25 | 2019-07-05 | 浙江大学 | A kind of anti-bleach-out process of GPS signal based on optical flow method |
| CN111152795A (en) * | 2020-01-08 | 2020-05-15 | 东南大学 | Model and parameter dynamic adjustment based adaptive vehicle state prediction system and prediction method |
Non-Patent Citations (6)
| Title |
|---|
| CIR机车综合无线通信设备故障处理;李少辉;《铁道通信信号》;20130731;第49卷(第7期);第50-53页 * |
| Linearized recursive least squares methods for real-time identification of tire–road friction coefficient;Mooryong Choi 等;《IEEE Transactions on Vehicular Technology》;20130425;第62卷(第7期);第2906-2918页 * |
| The implementation of OBD-II vehicle diagnosis system integrated with cloud computation technology;Jheng-Syu Jhou 等;《IEEE Transactions on Vehicular Technology》;20130425;第62卷(第7期);第9-12页 * |
| 基于H∞的车辆横向运动鲁棒控制;钱立军 等;《吉林大学学报(工学版)》;20151130;第45卷(第6期);第1757-1762页 * |
| 基于小波故障检测的INS/GPS导航系统信息融合技术;曹梦龙 等;《宇航学报》;20090930;第30卷(第5期);第1885-1890页 * |
| 汽车行驶道路侧向坡度估计;管欣 等;《吉林大学学报(工学版)》;20191130;第49卷(第6期);第1802-1809页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111665530A (en) | 2020-09-15 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US10360476B2 (en) | Sensor system comprising a fusion filter for common signal processing | |
| US7096116B2 (en) | Vehicle behavior detector, in-vehicle processing system, detection information calibrator, and in-vehicle processor | |
| AU772454B2 (en) | Methods and device for estimating lateral acceleration on an axis of a semitrailer or a trailer of a vehicle combination | |
| Bevly et al. | Integrating INS sensors with GPS velocity measurements for continuous estimation of vehicle sideslip and tire cornering stiffness | |
| Bevly et al. | The use of GPS based velocity measurements for measurement of sideslip and wheel slip | |
| Bevly et al. | The use of GPS based velocity measurements for improved vehicle state estimation | |
| Yang et al. | Magnetometer and differential carrier phase GPS-aided INS for advanced vehicle control | |
| CN107132563B (en) | Combined navigation method combining odometer and dual-antenna differential GNSS | |
| Melendez-Pastor et al. | A data fusion system of GNSS data and on-vehicle sensors data for improving car positioning precision in urban environments | |
| US11079237B2 (en) | Method for determining a relative position of a motor vehicle, position determination system for a motor vehicle and motor vehicle | |
| CN111665530B (en) | GPS diagnosis method based on vehicle state | |
| Brunker et al. | GNSS-shortages-resistant and self-adaptive rear axle kinematic parameter estimator (SA-RAKPE) | |
| KR20200130423A (en) | How to calibrate the gyrometer installed in the vehicle | |
| JP2021018112A (en) | Self position estimation device | |
| CN111469855A (en) | Vehicle motion parameter calculation method | |
| Gao et al. | High precision SINS/OD dead reckoning algorithm considering lever arm effect | |
| Ganguli et al. | Fault diagnostics for GPS-based lateral vehicle control | |
| JP2020080743A (en) | Vehicular posture estimation device | |
| US20220274640A1 (en) | Electronic power steering system rack force observer vehicle diagnostics | |
| US11465623B2 (en) | Semi-autonomous reversing of a follower vehicle | |
| CN115571156B (en) | Front vehicle transverse and longitudinal motion state joint estimation method based on sensor fusion | |
| Baer et al. | EgoMaster: A central ego motion estimation for driver assist systems | |
| Teoh et al. | Vehicle Localization Based On IMU, OBD2, and GNSS Sensor Fusion Using Extended Kalman Filter | |
| US20240043018A1 (en) | Method and electronic control system for ascertaining a distance travelled by a vehicle | |
| CN117416417A (en) | Vehicle direction control feedback system and method based on MEMS gyroscope |
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 |