CN111665530A - GPS (global positioning system) diagnosis method based on vehicle state - Google Patents

GPS (global positioning system) diagnosis method based on vehicle state Download PDF

Info

Publication number
CN111665530A
CN111665530A CN202010422088.1A CN202010422088A CN111665530A CN 111665530 A CN111665530 A CN 111665530A CN 202010422088 A CN202010422088 A CN 202010422088A CN 111665530 A CN111665530 A CN 111665530A
Authority
CN
China
Prior art keywords
vehicle
time
data
front wheel
gps
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.)
Granted
Application number
CN202010422088.1A
Other languages
Chinese (zh)
Other versions
CN111665530B (en
Inventor
李珺
冯冲
黄立明
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Tage Idriver Technology Co Ltd
Original Assignee
Beijing Tage Idriver Technology Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Beijing Tage Idriver Technology Co Ltd filed Critical Beijing Tage Idriver Technology Co Ltd
Priority to CN202010422088.1A priority Critical patent/CN111665530B/en
Publication of CN111665530A publication Critical patent/CN111665530A/en
Application granted granted Critical
Publication of CN111665530B publication Critical patent/CN111665530B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

The invention discloses a GPS diagnosis method based on vehicle state, comprising the following steps: acquiring basic information; time alignment of sensor feedback data; spatial alignment of sensor feedback data; caching data at historical time and maintaining scale unchanged; presume the present position of vehicle according to the information of the historical moment; and comparing the result with the positioning result of the GPS data and outputting a judgment result. The diagnosis method provided by the invention is suitable for the unmanned environment of the mining area, and can be used for presuming the pose change condition of the vehicle in a 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 historical time to the current time when the vehicle moves; and then, whether the GPS data is abnormal or not can be judged by comparing the GPS positioning information with the GPS positioning information, so that the decision-making error caused by the fact that the drifting GPS data is referenced subsequently for positioning is avoided.

Description

GPS (global positioning system) diagnosis method based on vehicle state
Technical Field
The invention relates to the field of automobile active safety, in particular to a GPS (global positioning system) 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 on-board sensors to perform simple mathematical calculations on the measured signals to obtain the relevant vehicle operating conditions. And secondly, a high-precision sensor is used for directly measuring the running state of the relevant vehicle (such as a Global Navigation Satellite System (GNSS), particularly a high-precision Global Positioning System (GPS) and the like). The third method is a model method, namely, the estimation of the running state of the automobile is realized by performing kinematic or dynamic modeling on the running process of the automobile and simultaneously performing a proper filtering estimation algorithm by taking the information of the low-cost vehicle-mounted sensor as observation information. In a mine area, GPS information is generally used for positioning and planning a track of a vehicle, and due to the fact that the road condition of the mine area in an open mine area is poor and the GPS signals of a road below a mine pit are poor sometimes and easy to drift, the vehicle can not normally run or serious results are brought.
The method comprises the steps of detecting abnormal behaviors of a vehicle, starting to detect the vehicle behaviors and prompt early warning if the abnormal behaviors of the vehicle change, judging the vehicle state immediately when the vehicle reaches the final state, and providing the final early warning, so that the vehicle detection time is greatly shortened, early warning and reporting to a management center are facilitated in time, events are rapidly processed, and secondary accidents are prevented. Therefore, the method and the device have the advantages that the abnormal behavior detection of the vehicle is carried out in real time, so that accidents are prevented, the running efficiency of the vehicle is improved, and the low-carbon safe travel target is realized. Therefore, the research on the automatic detection algorithm of the abnormal data of the vehicle and the dynamic feedback thereof have become an important research direction of researchers.
Most of the existing technical schemes directly use the GPS data to carry out the state observation value of the vehicle, and do not evaluate the reliability and stability of the GPS data, and in fact, the GPS data is not continuously stable and has no errors. If the wrong positioning information is used for controlling the vehicle, avoiding obstacles and the like, great system faults and potential safety hazards are inevitably brought. Patent CN 108399743A discloses a method and a flow for detecting abnormal behaviors of vehicles on a highway based on GPS data; patent CN 102556075A discloses a vehicle running state estimation method based on improved extended kalman filtering; patent CN 108136867A) discloses a vehicle location point forwarding method for an autonomous vehicle, the flow chart of the technical solution 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 diagnosis of the GPS data is not involved, and the precision of all default GPS data is higher. It is only mentioned in patent CN 108136867A that the second position related to the front wheel is calculated based on the direction of movement and the first position related to the rear wheel, but the accuracy of the front wheel angle, the difference between front wheel drive and rear wheel drive, and no correlation calculation of the historical position information are considered.
Disclosure of Invention
The invention aims to solve the problems and the defects in the prior art, and provides a method for estimating the state of an unmanned vehicle and diagnosing a GPS (global positioning system) in an open-pit mine, which is suitable for the unmanned environment of the mine and can be used for estimating the pose change condition of the vehicle in a time period from a certain historical time to the current time when the vehicle moves, after a series of data processing and calculation, according to the self parameters (speed and front wheel turning angle) in the time period. And then, whether the GPS data is abnormal or not can be judged by comparing the GPS data with the GPS positioning information, and the decision-making error caused by the fact that the drifting GPS data is referenced subsequently for positioning is avoided, which is particularly important compared with the automatic driving display of the mining area depending on the GPS information.
In order to achieve the purpose, the invention adopts the following technical scheme:
a GPS diagnostic method based on a vehicle state, characterized by comprising the steps of:
the method comprises the following steps: acquiring basic information;
step two: time alignment of sensor feedback data;
step three: spatial alignment of sensor feedback data;
step four: caching data at historical time and maintaining scale unchanged;
step five: presume the present position of vehicle according to the information of the historical moment;
step six: and comparing the result with the positioning result of the GPS data and outputting a judgment result.
Further, in the first step, the front wheel steering angle information of the vehicle and the vehicle speed information of the vehicle are obtained through a vehicle drive-by-wire interface, and the longitude and latitude and the course information of the vehicle are obtained through a combined inertial navigation module; the input data of the front wheel turning angle needs to be subjected to stability judgment and processing.
Further, the stability judgment and processing process of the input data of the front wheel turning angle is as follows:
the front wheel turning angle is phi, firstly, the difference value is judged before and after the front wheel turning angle is phi, if phi is phii+2-φi+1>Delta phi, a quadratic equation of the rotation angle of the front wheel and delta t in 20 time intervals is fitted by using a least square method, wherein phii+2Is the front wheel angle value of the current time, phii+1The front wheel rotation angle value at the previous moment is shown, and delta phi is the maximum change value of the front wheel rotation angle in an interval;
the current front wheel steering angle value y is calculated by the time interval x of the two front wheel steering angle data, the process is as follows,
Figure BDA0002496671340000021
wherein y (x)i) Front wheel angle value, y, estimated for time iiThe front wheel steering angle value measured at the moment i, Q is the difference value of the estimated front wheel steering angle and the measured front wheel steering angle, and a, b and c are coefficients in a front wheel steering angle estimation formula;
the value of a, b, c when Q is minimum is found, and the partial derivative of Q to a, b, c is firstly found:
Figure BDA0002496671340000031
Figure BDA0002496671340000032
Figure BDA0002496671340000033
the following can be obtained by calculation:
Figure BDA0002496671340000034
Figure BDA0002496671340000035
Figure BDA0002496671340000036
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, if the observed value of the front wheel rotation angle received currently exceeds the threshold value, the front wheel rotation angle value at the current moment is replaced by the method, and the normal operation of the algorithm is guaranteed.
Further, in the second step, the time alignment process is as follows:
receiving the system Time stamp Time _ s of the first frame data of the GPSiTime interval (Time _ g) associated with corresponding GPSi+1-Time_gi) Accumulating to recur the timestamp Time _ s of the subsequent GPS data under the system Time referencei+1The calculation formula is as follows:
Time_si+1＝Time_si+(Time_gi+1-Time_gi)
the Time of reception is Time _ siIs Ts for GPS data and timeiAnd Tsi+1On the assumption that the speed in the short term changes uniformly, then Time _ siThe velocity at time should be:
Figure BDA0002496671340000037
wherein
Figure BDA0002496671340000038
Is Time _ siTime of dayThe speed of the vehicle at the moment of time,
Figure BDA0002496671340000039
is a time TsiThe speed of the vehicle at the moment of time,
Figure BDA00024966713400000310
is Tsi+1The vehicle speed at the moment.
Further, in the third step, the spatial alignment process is as follows: firstly, a coordinate system of the vehicle is defined in a unified mode, the center of mass position of the vehicle at the current moment is taken as the center of a circle, the direction pointing to the head of the vehicle is taken as the y axis, the direction pointing to the right side of the vehicle body perpendicular to the y axis is taken as the x axis, the direction pointing to the roof of the vehicle perpendicular to the x axis and the y axis is taken as the z axis, and the center of mass coordinate system of the vehicle. Wherein, the vector rotating along the coordinate axis in the positive direction is positive;
the data acquired from the sensors is based on the current sensor coordinate system, the data is spatially aligned into the vehicle centroid coordinate system, the spatial alignment includes rotation and translation of the coordinate system, and a rotation matrix of the coordinate system is defined as
Figure BDA0002496671340000041
The translation matrix of the coordinate system is
Figure BDA0002496671340000042
Then there is
Figure BDA0002496671340000043
Wherein
Figure BDA0002496671340000044
Is shown at tk+Δ1At the moment, the vehicle mass center coordinate under the s coordinate system;
Figure BDA0002496671340000045
is shown at tk+Δ1And (5) at the moment, the vehicle mass center coordinate under the v coordinate system.
Further, the fourth step of receiving the latest vehicle information including the front wheel turning angle, the vehicle speed, the longitude, the latitude and the heading angle, and calculating the vehicle state relative to the previous time after receiving the information,
firstly, the turning radius of the vehicle at the current moment is calculated according to the corner of the front wheel and the wheelbase of the vehicle body:
Figure BDA0002496671340000046
wherein radii is the turning radius at the center of the vehicle body, L is the vehicle wheelbase, and phi is the front wheel turning angle;
the turning radius at the center of the wheel base of the rear wheel of the vehicle is as follows:
Figure BDA0002496671340000047
1) calculating vehicle cornering angular velocity
Figure BDA0002496671340000048
Where ω is the vehicle body turning angle velocity vvehicleFeeding back the vehicle speed for the vehicle;
2) integral of angle
Δθ＝ω×Δt
Wherein, the delta theta is the angle rotated by the vehicle body at the current angular speed within a delta t time; Δ t is tiAnd ti+1The time interval of (c);
3) position estimation
Δy＝radii×sin(|ω×Δt|)
Δx＝radii×(1-cos(ω×Δt))
The position coordinates of the vehicle are respectively obtained after a time of delta t under the vehicle coordinate system of the current moment by delta x and delta y;
4) updating of a cache
The latest N times (t) coexist in the cache0～tn) The history data information of (2) storing new data into t every time there is new data updatenIn the corresponding cache, t is eliminated0Corresponding historical data, and sequentially shifting the rest cache data; maintaining cached data sizeThat is, too much data cached at the historical time cannot be made, extra calculation amount is increased, and too large accumulated error is generated; and the buffer data can not be too little, and the influence of random errors on the final result is increased.
Further, step five is performed according to tnEstablishing a reference coordinate system t based on the vehicle coordinate system at the momentiVehicle coordinate system of time tnThe transformation matrix of the vehicle coordinate system of the moment is
Figure BDA0002496671340000051
Wherein, thetai＝Δθi+1+…+Δθn,θiIs tnVehicle orientation at time tiAngular difference of vehicle orientation at time, Δ θi+1Is tiTo ti+1The angle of the vehicle body changes at any moment;
tithe estimated time position is set to (x)i,yi) From ti～tnThe information of the time is calculated out,
Figure BDA0002496671340000052
wherein, Δ xiAnd Δ yiIs tiTime ti+1The moving distance of the vehicle body at any moment;
obtaining the state change of the vehicle at the ith moment relative to the state change of the current moment through the solution, wherein the obtained variable quantities are the displacement in the x direction and the y direction and the relative rotation angle of the vehicle body; after obtaining the relative change of the state, the value is tnEstablishing a reference coordinate system t based on the vehicle coordinate system at the momentiTime t andnthe longitude and latitude and course information of the moment can be directly acquired by the combined inertial navigation module, and then t can be obtained by calculationiVehicle position at time tnCoordinate position under vehicle coordinates at time
Figure BDA0002496671340000053
Further, step six, the calculation results are compared
Figure BDA0002496671340000054
And
Figure BDA0002496671340000055
make a determination if
Figure BDA0002496671340000056
Setting an association threshold cthresholdFor the above calculation result, if the matching result Δ s is smaller than the threshold value cthresholdConsidering that the GPS data does not drift, and if the matching result deltas is larger than the threshold cthresholdThe 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 be used for accurately predicting GPS information by using vehicle body state information at historical time, does not increase data of other measuring sensors, and is simple and efficient.
(2) The invention obtains the relative movement distance and angle of the vehicle by carrying out accumulated integration on the movement state of the vehicle at the historical moment
(3) The invention applies the principle of least square to judge and process the fault of the stability of the front wheel steering angle data in the calculation process and aligns various data in time.
(4) The invention relates the vehicle displacement and rotation calculated by the method to a uniform reference coordinate system to obtain the relative variation of the vehicle posture in a period of time, and then compares the relative variation with the result obtained by the GPS positioning information to judge whether the GPS data is accurate or not
Drawings
Fig. 1 is a flowchart of a method for forwarding a location point of an autonomous vehicle according to the prior art.
FIG. 2 is a flow chart of a GPS diagnostic method based on vehicle conditions provided by the present invention.
Fig. 3 is a time alignment diagram of a GPS diagnostic method based on vehicle status 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 shows a relative movement distance deviation of a vehicle according to a GPS diagnostic method for a vehicle state according to the present invention.
Detailed Description
The following describes embodiments of the present invention in detail and clearly with reference to the examples and the accompanying drawings.
A GPS diagnostic method based on vehicle status, as shown in fig. 2, includes the following steps:
the method comprises the following steps: and acquiring basic information.
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 course information of the vehicle are obtained through a combined inertial navigation module. The data transmitted by the two devices have errors in space and time, and the original data needs to be processed firstly.
The data of the front wheel corner may jump abnormally, but because the subsequent calculation is a correlation and accumulation process, if frame loss occurs in the middle, the final determination result is greatly influenced, so that stability judgment and processing need to be performed on the input data, and the following process is a process for processing the front wheel corner.
The front wheel turning angle is phi, firstly, the difference value is judged before and after the front wheel turning angle is phi, if phi is phii+2-φi+1>Delta phi, a quadratic equation of the rotation angle of the front wheel and delta t in 20 time intervals is fitted by using a least square method, wherein phii+2Is the front wheel angle value of the current time, phii+1The front wheel steering angle value at the previous moment is delta phi, which is the maximum change value of the front wheel steering angle in an interval.
The time interval x of the two front wheel steering angle data in the following formula is used for calculating the value y of the current front wheel steering angle,
Figure BDA0002496671340000071
wherein y (x)i) Front wheel angle value, y, estimated for time iiThe front wheel steering angle value measured at the moment i, Q is the difference value between the estimated and measured front wheel steering angles, and a, b and c are coefficients in a front wheel steering angle estimation formula.
The value of a, b, c when Q is minimum is found, and the partial derivative of Q to a, b, c is firstly found:
Figure BDA0002496671340000072
Figure BDA0002496671340000073
Figure BDA0002496671340000074
the following can be obtained by calculation:
Figure BDA0002496671340000075
Figure BDA0002496671340000076
Figure BDA0002496671340000077
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 observed value of the front wheel rotation angle received currently exceeds the threshold value, the value of the front wheel rotation angle at the current moment is replaced by the method, so that the normal operation of the algorithm is ensured.
Step two: time alignment of sensor feedback data.
Since the GPS data and the wire control feedback data may not be sent from the same state of the vehicle body, and obviously, many problems occur when the data in different states are used for calculation, the two parts of data are firstly aligned to the same stateUnder a time reference. The specific implementation process is that firstly, the system Time stamp Time _ s of the first frame data of the GPS is receivediTime interval (Time _ g) associated with corresponding GPSi+1-Time_gi) Accumulating to recur the timestamp Time _ s of the subsequent GPS data under the system Time referencei+1The calculation formula is as follows:
Time_si+1＝Time_si+(Time_gi+1-Time_gi)
similarly, as shown in fig. 3, the data fed back by the line control needs to be time-synchronized through the above operations, that is, the system timestamp of the first frame of data is obtained, and then is accumulated with the time interval of the subsequently received data, so as to deliver the timestamp under the system time reference. The two types of data time stamps are converted to the system time reference, each piece of data is provided with the time stamp with the same time reference after the operation, and the sequence of the received data types can be easily judged. Then, different types of data are Time-aligned to the same type, for example, the received Time is Time _ siIs Ts for GPS data and timeiAnd Tsi+1On the assumption that the speed in the short term changes uniformly, then Time _ siThe velocity at time should be:
Figure BDA0002496671340000081
wherein
Figure BDA0002496671340000082
Is Time _ siThe speed of the vehicle at the moment in time,
Figure BDA0002496671340000083
is a time TsiThe speed of the vehicle at the moment of time,
Figure BDA0002496671340000084
is composed of
Figure BDA0002496671340000085
Vehicle speed at time of day。
And replacing the speed data with front wheel steering angle data to perform the same operation, so as to realize the time alignment of the front wheel steering angle data.
Step three: spatial alignment of sensor feedback data.
Firstly, a coordinate system of the vehicle is defined in a unified mode, the center of mass position of the vehicle at the current moment is taken as the center of a circle, the direction pointing to the head of the vehicle is taken as the y axis, the direction pointing to the right side of the vehicle body perpendicular to the y axis is taken as the x axis, the direction pointing to the roof of the vehicle perpendicular to the x axis and the y axis is taken as the z axis, and the center of mass coordinate system of the vehicle. Wherein, the vector rotating along the coordinate axis in the positive direction is positive.
The data acquired from the sensors is based on the current sensor coordinate system, and the data needs to be spatially aligned to the vehicle centroid coordinate system, the spatial alignment includes rotation and translation of the coordinate system, and the rotation matrix of the coordinate system is defined as
Figure BDA0002496671340000086
The translation matrix of the coordinate system is
Figure BDA0002496671340000087
Then there is
Figure BDA0002496671340000088
Wherein
Figure BDA0002496671340000089
Is shown at tk+Δ1At the moment, the vehicle mass center coordinate under the s coordinate system;
Figure BDA00024966713400000810
is shown at tk+Δ1And (5) at the moment, the vehicle mass center coordinate under the v coordinate system.
Particularly, when the calculation is performed, when referring to the vehicle body centroid coordinate system at different times, the GPS data, the speed and other data at different times need to be calculated according to the motion information (including longitude and latitude, speed and the like) of the vehicle and the motion information (including longitude and latitude, speed and the like) of other vehicles to obtain the motion information of the vehicle at the current time relative to other historical times.
Step four: and caching the data at the historical moment and maintaining the scale unchanged.
And receiving the vehicle information at the latest moment, wherein the vehicle information comprises the corner of the front wheel, the vehicle speed, the longitude, the latitude and the heading angle. After receiving the information, the vehicle state calculation at the previous time is performed. The displacement and rotation of the vehicle at the current moment relative to the last moment are mainly calculated.
Firstly, the turning radius of the vehicle at the current moment is calculated according to the corner of the front wheel and the wheelbase of the vehicle body:
Figure BDA00024966713400000811
wherein radii is the turning radius at the center of the vehicle body, L is the vehicle wheelbase, and phi is the front wheel turning angle.
The turning radius at the center of the wheel base of the rear wheel of the vehicle is as follows:
Figure BDA0002496671340000091
5) calculating vehicle cornering angular velocity
Figure BDA0002496671340000092
Where ω is the vehicle body turning angle velocity vvehicleAnd feeding back the vehicle speed for the vehicle.
6) Integral of angle
Δθ＝ω×Δt
Wherein, the delta theta is the angle rotated by the vehicle body at the current angular speed within a delta t time; Δ t is tiAnd ti+1The time interval of (c).
7) Position estimation
Δy＝radii×sin(|ω×Δt|)
Δx＝radii×(1-cos(ω×Δt))
And respectively determining the position coordinates of the vehicle after a time delta t under the vehicle coordinate system of the current moment by delta x and delta y.
8) Updating of a cache
The latest N times (t) coexist in the cache0～tn) The history data information of (2) storing new data into t every time there is new data updatenIn the corresponding cache, t is eliminated0And corresponding historical data and residual cache data are sequentially shifted. The size of the cached data is maintained, that is, the cached data at the historical moment cannot be too much, extra calculation amount is added, and overlarge accumulated error is generated. And the buffer data can not be too little, and the influence of random errors on the final result is increased.
Step five: and estimating the current position of the vehicle according to the information of the historical moment.
With tnEstablishing a reference coordinate system t based on the vehicle coordinate system at the momentiVehicle coordinate system of time tnThe transformation matrix of the vehicle coordinate system of the moment is
Figure BDA0002496671340000093
Wherein, thetai＝Δθi+1+…+Δθn,θiIs tnVehicle orientation at time tiAngular difference of vehicle orientation at time, Δ θi+1Is tiTo ti+1The angle of the vehicle body changes at all times.
tiThe estimated time position is set to (x)i,yi) From ti～tnThe information of the time is calculated out,
Figure BDA0002496671340000101
wherein, Δ xiAnd Δ yiIs tiTime ti+1The moving distance of the vehicle body at that time.
Through the solution, the state change of the vehicle at the ith moment relative to the state change of the current moment can be obtained, and the obtained change amounts are the displacement in the x direction and the y direction and the relative rotation angle of the vehicle body.
Obtaining the calculated relative variation of the stateThen, with tnEstablishing a reference coordinate system t based on the vehicle coordinate system at the momentiTime t andnthe longitude and latitude and course information of the moment can be directly acquired by the combined inertial navigation module, and then t can be obtained by calculationiVehicle position at time tnCoordinate position under vehicle coordinates at time
Figure BDA0002496671340000102
Step six: and comparing the result with the positioning result of the GPS data and outputting a judgment result.
Respective calculation results
Figure BDA0002496671340000103
And
Figure BDA0002496671340000104
make a determination if
Figure BDA0002496671340000105
Setting an association threshold cthresholdFor the above calculation result, if the matching result Δ s is smaller than the threshold value cthresholdConsidering that the GPS data does not drift, and if the matching result deltas is larger than the threshold cthresholdThe GPS data is considered to drift and some processing measures are required. Fig. 4 and 5 show the actual effect of the data testing by the method. The dotted line in fig. 4 represents each time t of the vehicle calculated by using the methodiAnd angle change of n time intervals before, and the solid line is t of GPS dataiAnd the heading angle change of the first n time intervals, it can be seen that the two are very close. Fig. 5 is the offset distance calculated through the sixth step, and it can be seen that the calculated position is less than 1 meter as a whole compared to the GPS positioning result when the vehicle travels at a speed of 5 m/s.
It will be apparent to those skilled in the art that various modifications and improvements can be made to the embodiments of the present invention without departing from the inventive concept thereof, and these modifications and improvements are intended to be within the scope of the invention.

Claims (8)

1. A GPS diagnostic method based on a vehicle state, characterized by comprising the steps of:
the method comprises the following steps: acquiring basic information;
step two: time alignment of sensor feedback data;
step three: spatial alignment of sensor feedback data;
step four: caching data at historical time and maintaining scale unchanged;
step five: presume the present position of vehicle according to the information of the historical moment;
step six: and comparing the result with the positioning result of the GPS data and outputting a judgment result.
2. The GPS diagnosis method based on the vehicle state as claimed in claim 1, wherein, the first step is that the front wheel steering angle information of the vehicle and the vehicle speed information of the vehicle are obtained through a vehicle drive-by-wire interface, and the longitude and latitude and the heading information of the vehicle are obtained through a combined inertial navigation module; the input data of the front wheel turning angle needs to be subjected to stability judgment and processing.
3. The GPS diagnostic method according to claim 2, wherein the stability of the input data of the front wheel turning angle is judged and processed as follows:
the front wheel turning angle is phi, firstly, the difference value is judged before and after the front wheel turning angle is phi, if phi is phii+2-φi+1>Delta phi, a quadratic equation of the rotation angle of the front wheel and delta t in 20 time intervals is fitted by using a least square method, wherein phii+2Is the front wheel angle value of the current time, phii+1The front wheel rotation angle value at the previous moment is shown, and delta phi is the maximum change value of the front wheel rotation angle in an interval;
the current front wheel steering angle value y is calculated by the time interval x of the two front wheel steering angle data, the process is as follows,
Figure FDA0002496671330000011
wherein y (x)i) Front wheel angle value, y, estimated for time iiThe front wheel steering angle value measured at the moment i, Q is the difference value of the estimated front wheel steering angle and the measured front wheel steering angle, and a, b and c are coefficients in a front wheel steering angle estimation formula;
the value of a, b, c when Q is minimum is found, and the partial derivative of Q to a, b, c is firstly found:
Figure FDA0002496671330000012
Figure FDA0002496671330000013
Figure FDA0002496671330000021
the following can be obtained by calculation:
Figure FDA0002496671330000022
Figure FDA0002496671330000023
Figure FDA0002496671330000024
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, if the observed value of the front wheel rotation angle received currently exceeds the threshold value, the front wheel rotation angle value at the current moment is replaced by the method, and the normal operation of the algorithm is guaranteed.
4. The GPS diagnosis method according to claim 3, wherein in the second step, the time alignment process is as follows:
receiving the system Time stamp Time _ s of the first frame data of the GPSiTime interval (Time _ g) associated with corresponding GPSi+1-Time_gi) Accumulating to recur the timestamp Time _ s of the subsequent GPS data under the system Time referencei+1The calculation formula is as follows:
Time_si+1＝Time_si+(Time_gi+1-Time_gi)
the Time of reception is Time _ siIs Ts for GPS data and timeiAnd Tsi+1On the assumption that the speed in the short term changes uniformly, then Time _ siThe velocity at time should be:
Figure FDA0002496671330000025
wherein
Figure FDA0002496671330000026
Is Time _ siThe speed of the vehicle at the moment in time,
Figure FDA0002496671330000027
is a time TsiThe speed of the vehicle at the moment of time,
Figure FDA0002496671330000028
is Tsi+1The vehicle speed at the moment.
5. The GPS diagnosis method based on the vehicle state according to claim 4, wherein the spatial alignment process of the third step is as follows: firstly, uniformly defining a coordinate system of a vehicle, and establishing the coordinate system of the center of mass of the vehicle by taking the center of mass of the vehicle at the current moment as the center of a circle, pointing to the head of the vehicle 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 roof perpendicular to the x-axis and the y-axis as a z-axis; wherein, the vector rotating along the coordinate axis in the positive direction is positive;
the data acquired from the sensors is based on the current sensor coordinate system, and the data is passed through spaceAligning into a vehicle centroid coordinate system, spatially aligning including rotation and translation of the coordinate system, defining a rotation matrix of the coordinate system as
Figure FDA0002496671330000031
The translation matrix of the coordinate system is
Figure FDA0002496671330000032
Then there is
Figure FDA0002496671330000033
Wherein
Figure FDA0002496671330000034
Is shown at tk+Δ1At the moment, the vehicle mass center coordinate under the s coordinate system;
Figure FDA0002496671330000035
is shown at tk+Δ1And (5) at the moment, the vehicle mass center coordinate under the v coordinate system.
6. The GPS diagnosis method based on vehicle state as claimed in claim 5, wherein said step four, receiving the latest vehicle information including the turning angle of the front wheel, the vehicle speed, the longitude, the latitude, the heading angle, and calculating the vehicle state with respect to the previous time after receiving the above information,
firstly, the turning radius of the vehicle at the current moment is calculated according to the corner of the front wheel and the wheelbase of the vehicle body:
Figure FDA0002496671330000036
wherein radii is the turning radius at the center of the vehicle body, L is the vehicle wheelbase, and phi is the front wheel turning angle;
the turning radius at the center of the wheel base of the rear wheel of the vehicle is as follows:
Figure FDA0002496671330000037
1) calculating vehicle cornering angular velocity
Figure FDA0002496671330000038
Where ω is the vehicle body turning angle velocity vvehicleFeeding back the vehicle speed for the vehicle;
2) integral of angle
Δθ＝ω×Δt
Wherein, the delta theta is the angle rotated by the vehicle body at the current angular speed within a delta t time; Δ t is tiAnd ti+1The time interval of (c);
3) position estimation
Δy＝radii×sin(|ω×Δt|)
Δx＝radii×(1-cos(ω×Δt))
The position coordinates of the vehicle are respectively obtained after a time of delta t under the vehicle coordinate system of the current moment by delta x and delta y;
4) updating of a cache
The latest N times (t) coexist in the cache0～tn) The history data information of (2) storing new data into t every time there is new data updatenIn the corresponding cache, t is eliminated0Corresponding historical data, and sequentially shifting the rest cache data; maintaining the scale of the cached data, namely, too much cached data at the historical moment cannot be realized, increasing extra calculation amount and generating overlarge accumulated error; and the buffer data can not be too little, and the influence of random errors on the final result is increased.
7. The GPS diagnostic method based on the vehicle state as claimed in claim 6, wherein the step five, with tnEstablishing a reference coordinate system t based on the vehicle coordinate system at the momentiVehicle coordinate system of time tnThe transformation matrix of the vehicle coordinate system of the moment is
Figure FDA0002496671330000041
Wherein, thetai＝Δθi+1+…+Δθn,θiIs tnVehicle orientation at time tiAngular difference of vehicle orientation at time, Δ θi+1Is tiTo ti+1The angle of the vehicle body changes at any moment;
tithe estimated time position is set to (x)i,yi) From ti～tnThe information of the time is calculated out,
Figure FDA0002496671330000042
wherein, Δ xiAnd Δ yiIs tiTime ti+1The moving distance of the vehicle body at any moment;
obtaining the state change of the vehicle at the ith moment relative to the state change of the current moment through the solution, wherein the obtained variable quantities are the displacement in the x direction and the y direction and the relative rotation angle of the vehicle body; after obtaining the relative change of the state, the value is tnEstablishing a reference coordinate system t based on the vehicle coordinate system at the momentiTime t andnthe longitude and latitude and course information of the moment can be directly acquired by the combined inertial navigation module, and then t can be obtained by calculationiVehicle position at time tnCoordinate position under vehicle coordinates at time
Figure FDA0002496671330000043
8. The GPS diagnosis method based on vehicle state according to claim 7, wherein the sixth step is for the calculation result
Figure FDA0002496671330000044
And
Figure FDA0002496671330000045
make a determination if
Figure FDA0002496671330000046
Setting an association threshold cthresholdFor the above calculation result, if the matching result Δ s is smaller than the threshold value cthresholdConsidering that the GPS data does not drift, and if the matching result deltas is larger than the threshold cthresholdThe GPS data is considered to drift and processing measures are required.
CN202010422088.1A 2020-05-18 2020-05-18 GPS diagnosis method based on vehicle state Active CN111665530B (en)

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 true CN111665530A (en) 2020-09-15
CN111665530B 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)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115201873A (en) * 2022-09-06 2022-10-18 中冶智诚(武汉)工程技术有限公司 Multi-system collaborative indoor and outdoor precise positioning system architecture and operation method thereof

Citations (11)

* Cited by examiner, † Cited by third party
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
US20090326809A1 (en) * 2003-10-06 2009-12-31 Colley Jaime B System and method for augmenting a satellite-based navigation solution
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
US20160090100A1 (en) * 2014-09-29 2016-03-31 Fuji Jukogyo Kabushiki Kaisha Driving control apparatus for vehicle
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

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20090326809A1 (en) * 2003-10-06 2009-12-31 Colley Jaime B System and method for augmenting a satellite-based navigation solution
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
US20160090100A1 (en) * 2014-09-29 2016-03-31 Fuji Jukogyo Kabushiki Kaisha Driving control apparatus for vehicle
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)

* Cited by examiner, † Cited by third party
Title
JHENG-SYU JHOU 等: "The implementation of OBD-II vehicle diagnosis system integrated with cloud computation technology", 《IEEE TRANSACTIONS ON VEHICULAR TECHNOLOGY》 *
MOORYONG CHOI 等: "Linearized recursive least squares methods for real-time identification of tire–road friction coefficient", 《IEEE TRANSACTIONS ON VEHICULAR TECHNOLOGY》 *
曹梦龙 等: "基于小波故障检测的INS/GPS导航系统信息融合技术", 《宇航学报》 *
李少辉: "CIR机车综合无线通信设备故障处理", 《铁道通信信号》 *
管欣 等: "汽车行驶道路侧向坡度估计", 《吉林大学学报(工学版)》 *
钱立军 等: "基于H∞的车辆横向运动鲁棒控制", 《吉林大学学报(工学版)》 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115201873A (en) * 2022-09-06 2022-10-18 中冶智诚(武汉)工程技术有限公司 Multi-system collaborative indoor and outdoor precise positioning system architecture and operation method thereof
CN115201873B (en) * 2022-09-06 2023-07-28 中冶智诚(武汉)工程技术有限公司 Multi-system cooperative indoor and outdoor precise positioning system architecture and operation method thereof

Also Published As

Publication number Publication date
CN111665530B (en) 2023-06-02

Similar Documents

Publication Publication Date Title
CN106289275B (en) Unit and method for improving positioning accuracy
CN107084743B (en) Offset and misalignment compensation for six degree of freedom inertial measurement units using GNSS/INS data
US7096116B2 (en) Vehicle behavior detector, in-vehicle processing system, detection information calibrator, and in-vehicle processor
US9183463B2 (en) Orientation model for a sensor system
EP2519803B1 (en) Technique for calibrating dead reckoning positioning data
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
EP2219044A1 (en) Navigation method, navigation system, navigation device, vehicle provided therewith and group of vehicles
CN108345021B (en) Doppler radar assisted GPS/INS vehicle speed measurement method
CN107076559B (en) Method and system for matching navigation systems
CN111196233B (en) System and method for detecting lane keeping performance of a vehicle
EP4345421A2 (en) Method for calibrating sensor parameters based on autonomous driving, apparatus, storage medium, and vehicle
KR20190040818A (en) 3D vehicular navigation system using vehicular internal sensor, camera, and GNSS terminal
CN115060257B (en) Vehicle lane change detection method based on civil-grade inertia measurement unit
JP2017219429A (en) Vehicle-purpose travelling control device and its control method
KR20200130423A (en) How to calibrate the gyrometer installed in the vehicle
JP7203805B2 (en) Analysis of localization errors of moving objects
CN111665530A (en) GPS (global positioning system) diagnosis method based on vehicle state
KR102109953B1 (en) Apparatus and method for estimating position of vehicle using yawrate sensor
Lee Mobile robot localization using optical mice
CN110411463A (en) On-vehicle navigation apparatus receives the emergency navigational system and method under star failure state
JP2021018112A (en) Self position estimation device
Teoh et al. Vehicle Localization Based On IMU, OBD2, and GNSS Sensor Fusion Using Extended Kalman Filter
CN112298197B (en) Vehicle and vehicle control method
US20240043018A1 (en) Method and electronic control system for ascertaining a distance travelled by a vehicle

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