CN112731483B - Method for judging RTK abnormal value in automatic driving integrated navigation system - Google Patents
Method for judging RTK abnormal value in automatic driving integrated navigation system Download PDFInfo
- Publication number
- CN112731483B CN112731483B CN202011471789.0A CN202011471789A CN112731483B CN 112731483 B CN112731483 B CN 112731483B CN 202011471789 A CN202011471789 A CN 202011471789A CN 112731483 B CN112731483 B CN 112731483B
- Authority
- CN
- China
- Prior art keywords
- rtk
- automatic driving
- odometer
- automobile
- navigation
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 52
- 230000002159 abnormal effect Effects 0.000 title claims abstract description 15
- 238000004364 calculation method Methods 0.000 claims abstract description 28
- 230000001133 acceleration Effects 0.000 claims description 30
- 239000011159 matrix material Substances 0.000 claims description 9
- 238000005070 sampling Methods 0.000 claims description 9
- 230000009466 transformation Effects 0.000 claims description 7
- IAZDPXIOMUYVGZ-UHFFFAOYSA-N Dimethylsulphoxide Chemical compound CS(C)=O IAZDPXIOMUYVGZ-UHFFFAOYSA-N 0.000 claims description 3
- 230000005856 abnormality Effects 0.000 abstract description 3
- 238000005516 engineering process Methods 0.000 description 7
- 238000009825 accumulation Methods 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 206010063385 Intellectualisation Diseases 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000005336 cracking Methods 0.000 description 1
- 230000001186 cumulative effect Effects 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
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/01—Satellite radio beacon positioning systems transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/13—Receivers
- G01S19/35—Constructional details or hardware or software details of the signal processing chain
- G01S19/37—Hardware or software details of the signal processing chain
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/43—Determining position using carrier phase measurements, e.g. kinematic positioning; using long or short baseline interferometry
- G01S19/44—Carrier phase ambiguity resolution; Floating ambiguity; LAMBDA [Least-squares AMBiguity Decorrelation Adjustment] method
Abstract
The invention belongs to the field of automatic driving vehicle-mounted integrated navigation positioning, and particularly relates to a method for judging an RTK abnormal value in an automatic driving integrated navigation system, which comprises the following steps: establishing a judging reference by utilizing the inertial and odometer combined navigation parameters, and judging the effectiveness of the RTK positioning precision; when the RTK positioning accuracy is judged to be invalid, dead reckoning predicts the current position information of the automatic driving automobile; after the RTK resolving position of the automatic driving automobile fails for a long time, judging the validity of the RTK resolving position information; and correcting the combined navigation parameters of the inertia and the odometer when the RTK calculation position information is judged to be effective. The invention can judge the abnormality of RTK navigation in time under the condition that the RTK signal is blocked, and further switch the route of obtaining navigation parameters in time, thereby avoiding the automatic driving automobile from deviating from the correct driving direction.
Description
Technical Field
The invention belongs to the field of automatic driving vehicle-mounted integrated navigation positioning, and particularly relates to a method for judging an abnormal RTK value in an automatic driving inertia/odometer/RTK automatic driving integrated navigation system.
Background
The rapid development of science and technology promotes the pace of the mechanized age, and provides unprecedented opportunities for the automatic driving field of automation, intellectualization and mechanization. The rise of the automatic driving field requires various high-precision core technical support, wherein one of the most critical cores is a vehicle-mounted integrated navigation fusion positioning technology, which replaces human eyes, hands and feet to operate and control the automobile automatically, so as to realize a series of operations such as path planning, behavior decision and the like of the automobile.
With the continuous maturity of the vehicle navigation technology, although opportunities and platforms are provided for the automatic driving field, some problems still exist in the core technology to be solved and the scheme to be optimized. Taking an RTK positioning technology as an example, although the GPS carrier differential principle is used to realize centimeter-level high-precision positioning at present, the RTK navigation is affected by signals, environment and the like to cause the phenomenon of cracking of local area navigation positioning precision, so that the precision requirement of automatic driving navigation positioning cannot be met. In this part of the area it would be desirable to provide autopilot car navigation parameters using the short term accuracy of the inertial/odometer feature. However, since the accurate feedback of the RTK signal is not blocked, the abnormality of the RTK navigation cannot be timely judged, and thus the route of obtaining the navigation parameters cannot be timely switched, which may cause the automatic driving automobile to deviate from the correct driving direction.
Therefore, in order to avoid the occurrence of the above phenomena, it is necessary and indispensable to provide a method for determining the abnormal RTK value in an autopilot inertial/odometer/RTK integrated navigation system, and the research will promote the development of autopilot car technology to a certain extent, and will also provide a new solution for solving the same type of engineering problems.
Disclosure of Invention
In order to solve the above problems, the present invention provides a method for determining an abnormal RTK value in an autopilot integrated navigation system. In order to determine the effectiveness of RTK positioning in the running process of an automatic driving automobile, firstly, a determination reference is established by utilizing an inertia/odometer combined navigation parameter, when the speed and the acceleration of the automatic driving automobile calculated by the RTK do not meet a threshold value determined by an inertia/odometer combined navigation calculation result, RTK information is determined to be abnormal, then the position of the carrier automobile at the current moment is estimated by utilizing the characteristic of high short-term positioning precision of the inertia/odometer combined navigation, and the calculation result is used as the pose of the automatic driving automobile at the current moment. However, due to the lack of cumulative divergence of the errors over time in integrated inertial/odometer navigation, the reference reliability established by the inertial/odometer will decrease when the RTK is inactive for a long period of time. In order to accurately judge RTK information, when the RTK is abnormal for a long time, the characteristics of high measurement precision and high gesture precision of the distance between two adjacent points of an inertia/odometer are utilized, the distance and the heading of the two adjacent points calculated by the RTK are compared, when a certain threshold value is met, the RTK signal is considered to be recovered to be normal, the combined navigation result of the inertia/odometer is corrected by utilizing the RTK information, and the corrected result is used as a condition for judging whether the RTK signal is abnormal or not until the whole automatic driving process is finished.
The invention provides a method for judging an abnormal value of an RTK in an automatic driving integrated navigation system, which is characterized by comprising the following steps:
s1: establishing a judging reference by utilizing the inertial and odometer integrated navigation parameters, and judging the effectiveness of RTK positioning accuracy in the automatic driving vehicle-mounted integrated navigation, wherein the specific process is as follows:
s11: establishing a speed threshold value of combined navigation of inertia and an odometer;
s12: establishing an acceleration threshold value of combined navigation of inertia and an odometer;
s13: judging the validity of the RTK calculation position information;
s2: when the RTK positioning precision in the automatic driving vehicle-mounted integrated navigation is invalid, dead reckoning predicts the current position information of the automatic driving vehicle, and the specific process is as follows:
s21: performing gesture calculation of the automatic driving automobile based on the combined navigation of the inertia and the odometer;
s22: estimating the position of the automatic driving automobile at the current moment based on the odometer position information difference principle;
s3: after the RTK calculation position of the automatic driving automobile fails for a long time, judging the effectiveness of the RTK positioning precision;
s4: when the RTK positioning accuracy is judged to be effective, correcting the inertial and odometer combined navigation parameters, and then taking the corrected inertial and odometer combined navigation parameters as a new condition for evaluating whether the RTK positioning accuracy is effective.
Further, the specific process of step S11 is as follows:
defining the speed of an RTK-resolved autopilot vehicle as v RTK (t) acceleration of a RTK (t); the speed of the automatic driving automobile obtained by utilizing the differential primary understanding calculation of inertia and an odometer is v DR (t) acceleration of a DR (t) and the resolution of the odometer is m, the velocity error δv caused by the resolution of the odometer DR The method comprises the following steps:
δv DR ≤m (1)
defining the real speed of an automatic driving automobile as v (t), the RTK resolving speed error as δv (t), the data sampling rate of inertia and an odometer as f, the data sampling rate of RTK as q, and when the maximum value of the RTK positioning error is a due to unstable signals, the RTK resolving speed error δv (t) satisfies the following formula under ideal conditions:
in order to ensure the positioning precision requirement of the automatic driving automobile, under the condition of considering the error of the RTK device, the maximum speed error of the RTK allowed by the automatic driving automobile meets the following conditions:
wherein deltas is the maximum allowable positioning error,
speed v of automatic driving automobile obtained by differential original understanding calculation of inertia and odometer DR (t) speed v of an autonomous car resolved with RTK RTK And (t) when the following formula (4) is satisfied, the RTK calculation position information is considered to be accurate:
v DR (t)-v RTK (t)≤|δv(t)|+|δv DR |＝w (4)
where w is the speed solution error threshold within an acceptable range.
Further, the specific process of step S12 is:
defining an allowed RTK calculated acceleration error within the positioning accuracy of the automatic driving automobile as δa (t), wherein when the speed of the automatic driving automobile does not exceed the maximum speed, the RTK calculated acceleration error meets the following conditions:
wherein s represents the time period [ t ] i ，t i-1 ]The real driving distance of the automatic driving automobile;
and (3) finishing the formula (5) to obtain the default acceleration which is a fixed value in the sampling time interval:
when the RTK calculated acceleration error δa (t) satisfies the equation (6), the RTK calculated position information is considered to be accurate.
Further, in step S13, the validity judgment condition of the RTK calculated position information in the autopilot vehicle-mounted integrated navigation is:
where b is the maximum acceleration allowed during the driving of the autopilot.
Further, the specific process of step S21 is:
defining a coordinate system of the automatic driving automobile as a carrier system, marking as a b system, setting a heading angle of the automatic driving automobile as psi, a pitch angle as theta and a roll angle as gamma; defining a geographic coordinate system as a navigation system, marking as an n system, and calculating a coordinate transformation matrix of the navigation system and a carrier system according to a coordinate system transformation principleThe method comprises the following steps:
from four-element solutionCoordinate transformation matrix of carrier system and navigation systemThe method comprises the following steps:
wherein q 0 、q 1 、q 2 、q 3 Is the coefficient of four elements, and the coefficient of four elements,
order theT 12 ＝2(q 1 q 2 -q 0 q 3 )，T 13 ＝2(q 1 q 3 +q 0 q 2 )，T 21 ＝2(q 1 q 2 +q 0 q 3 )，T 23 ＝2(q 2 q 3 -q 0 q 1 )，T 31 ＝2(q 1 q 3 -q 0 q 2 )，T 32 ＝2(q 2 q 3 +q 0 q 1 )，Then record
Since the rotation process from n system to b system always maintains the rectangular coordinate systemIs an orthogonal matrix:
then the attitude information of the automatically driven car is calculated:
further, the specific process of step S22 is:
the coordinates defining the left wheel of an autonomous car are a (x l ,y l ) The coordinates of the right wheel are B (x r ,y r ) Left wheel angular velocity w l The angular velocity of the right wheel is w r The linear speeds of the left wheel and the right wheel are v respectively l 、v r The center point coordinate of the axis is M (x, y),
by installing two photoelectric encoders on the wheels at two sides of the automobile, the running distance of the two wheels of the automobile is reversely solved according to the pulse number output by the encoders, and the running distance of the left photoelectric encoder in unit time delta t is set to be delta s l The travel distance of the right wheel photoelectric encoder in unit time delta t is delta s r The linear speeds of the left and right wheels are:
wherein DeltaN l 、ΔN r The pulse numbers output by the left wheel photoelectric encoder and the right wheel photoelectric encoder in the unit time delta t are respectively; p is the number of pulses output per wheel revolution; d is the diameter of the wheel of the vehicle,
then the central axis center point speed v of two wheels of the automobile M The method comprises the following steps:
let the current time t of the automatic driving car calculated according to inertia i Is θ (t) i ) From the last time t i-1 To the current time t i Is (t) i ,t i-1 ) The method comprises the following steps:
Δθ(t i ,t i-1 )＝Δθ(t i )-Δθ(t i-1 ) (15)
if the geographic position of the central axis center point of the two wheels at the last moment is (x) M (t i-1 ),y M (t i-1 ) The position of the automatically driven automobile at the current moment is:
in order to ensure real-time performance, the time interval between two data adoption points of the automatic driving automobile is very small, and the time interval is close to 0, namely:
wherein, c is a constant value,
then equation (16) reduces to:
wherein x is M (t i ),y M (t i ) And the geographic position coordinates of the automatic driving automobile at the current moment.
Further, the specific process of step S3 is as follows:
setting the geographical coordinates of the positioning point at the current moment of the RTK to be (x) after long-time failure 1 (t i ),y 1 (t i ) The geographical coordinates of the positioning point at the last moment are (x) 1 (t i-1 ),y 1 (t i-1 ) And the distance increment from the current moment to the two points at the last moment, which is acquired by the odometer, is as follows:
the distance increment of two adjacent points acquired by the RTK is:
wherein R is N Represents the radius of the longitude circle of the earth, R M Representing the radius of the latitude circle of the earth,
defining the attitude angle of inertia and odometer solutions as θ DR (t i ,t i-1 ) The attitude angle calculated by RTK is theta RTK (t i ,t i-1 ) When the two resolving parameters meet the following formulas (21) and (22), the RTK signal is considered to be recovered to be normal,
δ(Δs(t i ,t i-1 ))＝|Δs RTK (t i ,t i-1 )-Δs DR (t i ,t i-1 )|∈(δs min ,δs max ) (21)
δθ(t i ,t i-1 )＝|θ RTK (t i ,t i-1 )-θ DR (t i ,t i-1 )|∈(δθ min ,δθ max ) (22)
wherein, delta (deltas (t) i ,t i-1 ) For RTK and inertial and odometer combined navigation positioning difference δs min For the confidence interval minimum, δs max For the confidence interval maximum, δθ (t i ,t i-1 ) Navigation attitude error delta theta for RTK and combination of inertia and odometer min Is the minimum value of confidence interval of the attitude credibility, delta theta max Is the maximum value of confidence intervals of the gesture credibility.
The invention has the beneficial effects that:
when the automatic driving automobile drives under the complex environment condition, the method can judge whether the RTK positioning is effective or not when the signal condition is weaker, and if so, the automatic driving automobile vehicle navigation information is provided according to the RTK technology; if not, inertial/odometer dead reckoning is used to provide the navigation information required for the driving process of the automatic driving automobile. Therefore, the invention can judge the abnormality of RTK navigation in time under the condition that the RTK signal is blocked, and further switch the route of navigation parameter acquisition in time, thereby avoiding the automatic driving automobile from deviating from the correct driving direction.
Drawings
FIG. 1 is a general flow chart of a method for determining an abnormal RTK value in an autopilot inertial/odometer/RTK integrated navigation system according to an embodiment of the present invention;
FIG. 2 is a schematic diagram illustrating the determination of whether an RTK signal is abnormal according to an embodiment of the present invention;
fig. 3 is a schematic diagram of automatic driving car dead reckoning based on the differential principle of the odometer according to an embodiment of the present invention.
Detailed Description
The invention will be further described with reference to the accompanying drawings and examples, it being understood that the examples described below are intended to facilitate an understanding of the invention and are not intended to limit the invention in any way.
Fig. 1 shows a general flowchart of a method for judging an abnormal RTK value in an autopilot inertial/odometer/RTK integrated navigation system according to an embodiment of the present invention, and specifically includes the following steps:
s1: and establishing a judging reference by using the inertial/odometer integrated navigation parameters, and judging the effectiveness of RTK positioning accuracy in the automatic driving vehicle-mounted integrated navigation. The specific process is as follows:
s11: establishing a speed threshold value of the combined inertial/odometer navigation;
in order to ensure normal running of the automatic driving automobile, the embodiment requires that the positioning accuracy of the vehicle navigation is within 20cm, namely, a confidence region of a positioning result of the automatic driving automobile at the current moment can be set to be a circle with radius r=20cm, and the maximum positioning error is allowed to be deltas. It is considered that the RTK positioning error is required to be not more than 20cm at maximum, that is, Δs.ltoreq.20cm, and when the range is exceeded, the RTK calculation position information is considered to be abnormal.
Let the speed of the RTK-resolved autopilot car be v RTK (t) acceleration of a RTK (t). The speed of the automatic driving automobile obtained by utilizing the differential primary understanding calculation of the inertia/odometer is v DR (t) acceleration of a DR (t), and the resolution of the odometer is m=0.01 km/h. At this time, the velocity error δv caused by the resolution of the odometer DR The method comprises the following steps:
δv DR ≤0.01km/h≈0.003m/s (1)
the data sampling rate f=100 Hz, the rtk data sampling rate q=10 Hz, defining the inertia/odometer, and the speed of the autonomous car is between 0 and 60km/h. Taking the maximum speed as an example, when the real speed v (t) of the automatic driving automobile is 60km/h, the RTK solving speed error is set as δv (t), and when the maximum value of the RTK positioning error caused by signal instability is 20cm, the RTK solving speed error δv (t) satisfies the following formula in an ideal state:
because the RTK device has a system error, the RTK product can reach the positioning precision of 2-3cm under the influence of the system error at the present stage, and the extremely bad is 5cm. Therefore, in order to ensure the positioning accuracy requirement of the automatic driving automobile, under the condition of considering the error of the RTK device, the maximum speed error of the RTK allowed by the automatic driving automobile should meet the following conditions:
thus, the velocity v calculated by the combined inertial/odometer navigation DR (t) velocity v calculated with RTK RTK When (t) satisfies the following equation (4), it can be considered that the RTK calculated position information is accurate:
v DR (t)-v RTK (t)≤|δv(t)|+|δv DR |＝1.5003m/s (4)
s12: establishing an acceleration threshold value of the inertial/odometer combined navigation;
according to the limit of the automatic driving automobile, the acceleration of the automobile is set to be not more than 3m/s 2 . If the allowed RTK calculated acceleration error in the positioning precision of the automatic driving automobile is δa (t), when the speed of the automobile is not more than 60km/h, the RTK calculated acceleration error meets the following requirements:
wherein s represents a time period[t i ，t i-1 ]The true driving distance of the automatic driving automobile.
Since the acceleration of the autopilot belongs to discrete data, the default acceleration is a fixed value in the sampling time interval, and the following formula (5) can be obtained:
|δa(t)|≤15m/s 2 (6)
from equation (6), it can be seen that the error δa (t) between the acceleration of the automated driving vehicle calculated by the RTK and the acceleration calculated by the inertia/mileage calculation is 15m/s 2 Within this, the RTK solution location information can be considered accurate.
S13: and judging the validity of the RTK calculation position information.
According to the analysis of the step S11 and the step S12, when the RTK signal is unstable in the running process of the automatic driving automobile, under the positioning accuracy requirement of 20cm, the maximum error of the speed of the automatic driving automobile calculated by the allowed RTK and the speed calculated by the inertia/mileage calculation is 1.5003m/S; whereas the maximum error of the allowed RTK-resolved acceleration of the autonomous car with the inertia/odometry-resolved acceleration is 15m/s 2 . Since the acceleration or the speed is used as the judging condition of whether the RTK calculation position information is effective or not, the judging condition is a function of time and is a result of the reverse thrust with the positioning accuracy requirement, so that in order to ensure the positioning accuracy of the automatic driving automobile as much as possible, the step S11 and the step S12 should be satisfied at the same time, the judging condition of the effectiveness of the RTK positioning accuracy in the automatic driving vehicle-mounted integrated navigation is as follows:
|δv(t)|+|δv DR |≤1.5003m/s and|δa(t)|≤15m/s 2 and a DR (t)＜3m/s 2 (7)
s2: and when the RTK positioning precision in the automatic driving vehicle-mounted integrated navigation is invalid, dead reckoning predicts the current position information of the automatic driving vehicle. The specific process is as follows:
s21: and carrying out gesture calculation of the automatic driving automobile based on the combined navigation of the inertia and the odometer.
Defining a coordinate system of the automatic driving automobile as a carrier system, marking as a b system, setting a heading angle of the automatic driving automobile as psi, and pitchingThe angle is theta, and the roll angle is gamma; defining a geographic coordinate system as a navigation system, namely a northeast coordinate system, marking as an n-system, and calculating a coordinate conversion matrix of the navigation system and a carrier system according to a coordinate system conversion principleThe method comprises the following steps:
based on four elements, coordinate transformation matrix of carrier system (b system) and navigation system (n system) can be calculatedThe method comprises the following steps:
wherein q 0 、q 1 、q 2 、q 3 Is a four element coefficient.
Order theT 12 ＝2(q 1 q 2 -q 0 q 3 )，T 13 ＝2(q 1 q 3 +q 0 q 2 )，T 21 ＝2(q 1 q 2 +q 0 q 3 )，T 23 ＝2(q 2 q 3 -q 0 q 1 )，T 31 ＝2(q 1 q 3 -q 0 q 2 )，T 32 ＝2(q 2 q 3 +q 0 q 1 )，Then record
Since the rotation process from n system to b system always maintains the rectangular coordinate systemIs an orthogonal matrix:
the attitude information of the automatically driven automobile can be calculated:
the gesture of the automatic driving automobile is calculated through the calculation.
S22: and estimating the position of the automatic driving automobile at the current moment based on the odometer position information difference principle.
As shown in fig. 3, the coordinates of the left wheel of the automated driving automobile are defined as a (x l ,y l ) The coordinates of the right wheel are B (x r ,y r ) Left wheel angular velocity w l The angular velocity of the right wheel is w r The linear speeds of the left wheel and the right wheel are v respectively l 、v r The center point coordinate of the axis is M (x, y), theta 1 Representing the course angle variation of the auto-driving automobile wheel in unit time; knowing θ by the nature of triangles 2 ＝θ 1 ；θ 3 ＝θ 1 。
The working principle of the odometer is that two photoelectric encoders are arranged on wheels on two sides of an automobile, and the driving distance of the two wheels of the automobile is reversely solved according to the pulse number output by the encoders. Let the travel distance of the left wheel photoelectric encoder in unit time delta t be delta s l The travel distance of the right wheel photoelectric encoder in unit time delta t is delta s r . The linear speeds of the left and right wheels are:
wherein DeltaN l 、ΔN r The pulse numbers output by the left wheel photoelectric encoder and the right wheel photoelectric encoder in delta t time are respectively; p is the number of pulses output per wheel revolution; d is the wheel diameter.
Then the central axis center point speed v of two wheels of the automobile M The method comprises the following steps:
let the current time t of the automatic driving car calculated according to inertia i Is θ (t) i ) From the last time t i-1 To the current time t i Is (t) i ,t i-1 ) The method comprises the following steps:
Δθ(t i ,t i-1 )＝Δθ(t i )-Δθ(t i-1 ) (15)
if the geographic position of the central axis center point of the two wheels at the last moment is (x) M (t i-1 ),y M (t i-1 ) The position of the automatically driven automobile at the current moment is:
to ensure real-time performance, the data update frequency of the automatic driving automobile is generally quite high (at 1000 kHz), that is, the time interval between two data utilization points is quite small, and the time interval is close to 0. Namely:
wherein c is a constant value.
Then equation (16) can be reduced to:
in summary, the geographic location of the autonomous car at the current time based on the inertial/odometer dead reckoning solution is (x M (t i ),y M (t i ))。
S3: and judging the validity of the RTK positioning precision after the automatic driving automobile RTK solving position fails for a long time.
When the RTK calculation position information is invalid for a long time, the accuracy cannot measure the validity of the RTK calculation position information because errors of inertial/odometer combined navigation are accumulated for a long time, and a step length and course constraint judging method is adopted for accurately analyzing the validity of the RTK calculation position information.
The RTK has the characteristics of high positioning precision and heading precision when signals are sufficient, the reason that the odometer diverges for a long time is caused by the error of the odometer scale, the scale error can be accumulated with time, and the accumulation of inertial heading error also indirectly causes the error accumulation of the odometer, but the RTK has the characteristic of high short-term precision. The whole thought of this step is: on the basis of accurate positioning of the inertia/odometer at the upper point, the error introduced to the next point is only doubled in odometer scale error and weak heading error angle, namely the accuracy of distance measurement information of two adjacent points is still very high, and the heading accuracy is high. Similarly, after the RTK is recovered, the positioning error of two adjacent points is small, and the heading precision is high.
Based on the analysis, after the RTK resolving position fails for a long time, if the distance between two adjacent points of the RTK location and the location of two adjacent points of the inertia/odometer are judged to meet a certain threshold, and the heading gesture meets a certain threshold, the RTK signal is considered to be recovered to be normal.
Setting the geographical coordinates of the positioning point at the current moment of the RTK to be (x) after long-time failure 1 (t i ),y 1 (t i ) The geographical coordinates of the positioning point at the last moment are (x) 1 (t i-1 ),y 1 (t i-1 ) And the distance increment from the current moment to the two points at the last moment, which is acquired by the odometer, is as follows:
the distance increment of two adjacent points acquired by the RTK is:
wherein R is N Represents the radius of the longitude circle of the earth, R M Representing the latitude circle radius of the earth.
Defining the attitude angle of the inertial/odometer solution as θ DR (t i ,t i-1 ) The attitude angle calculated by RTK is theta RTK (t i ,t i-1 ) When the two resolving parameters satisfy the following formulas (21) and (22), the RTK signal is considered to be recovered to be normal.
δ(Δs(t i ,t i-1 ))＝|Δs RTK (t i ,t i-1 )-Δs DR (t i ,t i-1 )|∈(δs min ,δs max ) (21)
δθ(t i ,t i-1 )＝|θ RTK (t i ,t i-1 )-θ DR (t i ,t i-1 )|∈(δθ min ,δθ max ) (22)
Wherein, delta (deltas (t) i ,t i-1 ) For RTK and inertial/odometer combined navigation positioning difference δs min For the confidence interval minimum, δs max For the confidence interval maximum, δθ (t i ,t i-1 ) For RTK and inertia/odometer combined navigation attitude error, delta theta min Is the minimum value of confidence interval of the attitude credibility, delta theta max Is the maximum value of confidence intervals of the gesture credibility.
S4: when the RTK positioning accuracy is judged to be effective, the attitude and the position of the combined navigation parameter of the inertia/odometer are corrected, and then the corrected inertia/odometer parameter is used as a new condition for evaluating whether the RTK positioning accuracy is effective.
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 judging method of RTK abnormal value in automatic driving integrated navigation system is characterized in that the method comprises the following steps:
s1: establishing a judging reference by utilizing the inertial and odometer integrated navigation parameters, and judging the effectiveness of RTK positioning accuracy in the automatic driving vehicle-mounted integrated navigation, wherein the specific process is as follows:
s11: establishing a speed threshold value of combined navigation of inertia and an odometer;
s12: establishing an acceleration threshold value of combined navigation of inertia and an odometer;
s13: judging the validity of the RTK calculation position information;
s2: when the RTK positioning precision in the automatic driving vehicle-mounted integrated navigation is invalid, dead reckoning predicts the current position information of the automatic driving vehicle, and the specific process is as follows:
s21: performing gesture calculation of the automatic driving automobile based on the combined navigation of the inertia and the odometer;
s22: estimating the position of the automatic driving automobile at the current moment based on the odometer position information difference principle;
s3: after the RTK calculation position of the automatic driving automobile fails for a long time, judging the effectiveness of the RTK positioning precision;
s4: when the RTK positioning accuracy is judged to be effective, correcting the inertial and odometer combined navigation parameters, and then taking the corrected inertial and odometer combined navigation parameters as a new condition for evaluating whether the RTK positioning accuracy is effective or not;
the specific process of step S11 is as follows:
defining the speed of an RTK-resolved autopilot vehicle as v RTK (t) acceleration of a RTK (t); the speed of the automatic driving automobile obtained by utilizing the differential primary understanding calculation of inertia and an odometer is v DR (t) acceleration of a DR (t) and the resolution of the odometer is m, the speed due to the resolution of the odometerError δv DR The method comprises the following steps:
δv DR ≤m (1)
defining the real speed of an automatic driving automobile as v (t), the RTK resolving speed error as δv (t), the data sampling rate of inertia and an odometer as f, the data sampling rate of RTK as q, and when the maximum value of the RTK positioning error is a due to unstable signals, the RTK resolving speed error δv (t) satisfies the following formula under ideal conditions:
in order to ensure the positioning precision requirement of the automatic driving automobile, under the condition of considering the error of the RTK device, the maximum speed error of the RTK allowed by the automatic driving automobile meets the following conditions:
wherein deltas is the maximum allowable positioning error,
speed v of automatic driving automobile obtained by differential original understanding calculation of inertia and odometer DR (t) speed v of an autonomous car resolved with RTK RTK And (t) when the following formula (4) is satisfied, the RTK calculation position information is considered to be accurate:
v DR (t)-v RTK (t)≤|δv(t)|+|δv DR v＝w (4)
wherein w is the speed solution error threshold within an acceptable range;
the specific process of step S12 is as follows:
defining an allowed RTK calculated acceleration error within the positioning accuracy of the automatic driving automobile as δa (t), wherein when the speed of the automatic driving automobile does not exceed the maximum speed, the RTK calculated acceleration error meets the following conditions:
wherein,s denotes a time period [ t ] i ，t i-1 ]The real driving distance of the automatic driving automobile;
and (3) finishing the formula (5) to obtain the default acceleration which is a fixed value in the sampling time interval:
when the RTK calculation acceleration error delta a (t) meets the formula (6), the RTK calculation position information is considered to be accurate;
in step S13, the validity judgment condition of the RTK solution position information in the autopilot vehicle-mounted integrated navigation is:
where b is the maximum acceleration allowed during the driving of the autopilot.
2. The method according to claim 1, wherein the specific process of step S21 is:
defining a coordinate system of the automatic driving automobile as a carrier system, marking as a b system, setting a heading angle of the automatic driving automobile as psi, a pitch angle as theta and a roll angle as gamma; defining a geographic coordinate system as a navigation system, marking as an n system, and calculating a coordinate transformation matrix of the navigation system and a carrier system according to a coordinate system transformation principleThe method comprises the following steps:
coordinate transformation matrix of carrier system and navigation system is calculated according to four elementsThe method comprises the following steps:
wherein q 0 、q 1 、q 2 、q 3 Is the coefficient of four elements, and the coefficient of four elements,
order theT 12 ＝2(q 1 q 2 -q 0 q 3 )，T 13 ＝2(q 1 q 3 +q 0 q 2 )，T 21 ＝2(q 1 q 2 +q 0 q 3 )，T 23 ＝2(q 2 q 3 -q 0 q 1 )，T 31 ＝2(q 1 q 3 -q 0 q 2 )，T 32 ＝2(q 2 q 3 +q 0 q 1 )，Then record
Since the rotation process from n system to b system always maintains the rectangular coordinate systemIs an orthogonal matrix:
then the attitude information of the automatically driven car is calculated:
3. the method according to claim 1, wherein the step S22 comprises the following specific steps:
the coordinates defining the left wheel of an autonomous car are a (x l ,y l ) The coordinates of the right wheel are B (x r ,y r ) Left wheel angular velocity w l The angular velocity of the right wheel is w r The linear speeds of the left wheel and the right wheel are v respectively l 、v r The center point coordinate of the axis is M (x, y),
by installing two photoelectric encoders on the wheels at two sides of the automobile, the running distance of the two wheels of the automobile is reversely solved according to the pulse number output by the encoders, and the running distance of the left photoelectric encoder in unit time delta t is set to be delta s l The travel distance of the right wheel photoelectric encoder in unit time delta t is delta s r The linear speeds of the left and right wheels are:
wherein DeltaN l 、ΔN r The pulse numbers output by the left wheel photoelectric encoder and the right wheel photoelectric encoder in the unit time delta t are respectively; p is the number of pulses output per wheel revolution; d is the diameter of the wheel of the vehicle,
then the central axis center point speed v of two wheels of the automobile M The method comprises the following steps:
let the current time t of the automatic driving car calculated according to inertia i Is θ (t) i ) From the last time t i-1 To the current time t i Is (t) i ,t i-1 ) The method comprises the following steps:
Δθ(t i ,t i-1 )＝Δθ(t i )-Δθ(t i-1 ) (15)
if the geographic position of the central axis center point of the two wheels at the last moment is (x) M (t i-1 ),y M (t i-1 ) The position of the automatically driven automobile at the current moment is:
in order to ensure real-time performance, the time interval between two data adoption points of the automatic driving automobile is very small, and the time interval is close to 0, namely:
wherein, c is a constant value,
then equation (16) reduces to:
wherein x is M (t i ),y M (t i ) And the geographic position coordinates of the automatic driving automobile at the current moment.
4. A method according to claim 3, wherein the step S3 is specifically performed by:
setting the geographical coordinates of the positioning point at the current moment of the RTK to be (x) after long-time failure 1 (t i ),y 1 (t i ) The geographical coordinates of the positioning point at the last moment are (x) 1 (t i-1 ),y 1 (t i-1 ) And the distance increment from the current moment to the two points at the last moment, which is acquired by the odometer, is as follows:
the distance increment of two adjacent points acquired by the RTK is:
wherein R is N Represents the radius of the longitude circle of the earth, R M Representing the radius of the latitude circle of the earth,
defining the attitude angle of inertia and odometer solutions as θ DR (t i ,t i-1 ) The attitude angle calculated by RTK is theta RTK (t i ,t i-1 ) When the two resolving parameters meet the following formulas (21) and (22), the RTK signal is considered to be recovered to be normal,
δ(Δs(t i ,t i-1 ))＝|Δs RTK (t i ,t i-1 )-Δs DR (t i ,t i-1 )|∈(δs min ,δs max ) (21)
δθ(t i ,t i-1 )＝|θ RTK (t i ,t i-1 )-θ DR (t i ,t i-1 )|∈(δθ min ,δθ max ) (22)
wherein, delta (deltas (t) i ,t i-1 ) For RTK and inertial and odometer combined navigation positioning difference δs min For the confidence interval minimum, δs max For the confidence interval maximum, δθ (t i ,t i-1 ) Navigation attitude error delta theta for RTK and combination of inertia and odometer min Is the minimum value of confidence interval of the attitude credibility, delta theta max Is the maximum value of confidence intervals of the gesture credibility.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011471789.0A CN112731483B (en) | 2020-12-14 | 2020-12-14 | Method for judging RTK abnormal value in automatic driving integrated navigation system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011471789.0A CN112731483B (en) | 2020-12-14 | 2020-12-14 | Method for judging RTK abnormal value in automatic driving integrated navigation system |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112731483A CN112731483A (en) | 2021-04-30 |
| CN112731483B true CN112731483B (en) | 2024-04-09 |
Family
ID=75601995
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011471789.0A Active CN112731483B (en) | 2020-12-14 | 2020-12-14 | Method for judging RTK abnormal value in automatic driving integrated navigation system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112731483B (en) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114485725A (en) * | 2021-12-22 | 2022-05-13 | 深圳元戎启行科技有限公司 | Data anomaly detection method, automatic driving platform and computer readable storage medium |
| CN114475655A (en) * | 2022-01-29 | 2022-05-13 | 智道网联科技(北京)有限公司 | Early warning method and device for automatic driving and computer readable storage medium |
| CN115079619B (en) * | 2022-07-27 | 2022-11-15 | 智道网联科技(北京)有限公司 | Circuit for V2X device, control method, V2X device and vehicle |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20060102016A (en) * | 2005-03-22 | 2006-09-27 | 현대자동차주식회사 | A dead reckoning sensor correction system of navigation system on vehicle and method thereof |
| CN110221333A (en) * | 2019-04-11 | 2019-09-10 | 同济大学 | A kind of error in measurement compensation method of vehicle-mounted INS/OD integrated navigation system |
| CN110631574A (en) * | 2018-06-22 | 2019-12-31 | 北京自动化控制设备研究所 | inertia/odometer/RTK multi-information fusion method |
| CN110779521A (en) * | 2019-11-12 | 2020-02-11 | 成都中科微信息技术研究院有限公司 | Multi-source fusion high-precision positioning method and device |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| FR2878954B1 (en) * | 2004-12-07 | 2007-03-30 | Sagem | HYBRID INERTIAL NAVIGATION SYSTEM BASED ON A CINEMATIC MODEL |
| US20080071476A1 (en) * | 2006-09-19 | 2008-03-20 | Takayuki Hoshizaki | Vehicle dynamics conditioning method on MEMS based integrated INS/GPS vehicle navigation system |
-
2020
- 2020-12-14 CN CN202011471789.0A patent/CN112731483B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR20060102016A (en) * | 2005-03-22 | 2006-09-27 | 현대자동차주식회사 | A dead reckoning sensor correction system of navigation system on vehicle and method thereof |
| CN110631574A (en) * | 2018-06-22 | 2019-12-31 | 北京自动化控制设备研究所 | inertia/odometer/RTK multi-information fusion method |
| CN110221333A (en) * | 2019-04-11 | 2019-09-10 | 同济大学 | A kind of error in measurement compensation method of vehicle-mounted INS/OD integrated navigation system |
| CN110779521A (en) * | 2019-11-12 | 2020-02-11 | 成都中科微信息技术研究院有限公司 | Multi-source fusion high-precision positioning method and device |
Non-Patent Citations (3)
| Title |
|---|
| "Integrity monitoring for Positioning of intelligent transport systems using integrated RTK-GNSS, IMU and vehicle odometer";Ahmed El-Mowafy;《IET Intelligent Transport Systems》;1-8 * |
| "弹载大视场星惯组合深度融合导航技术";宋凝芳等;《航空学报》;第41卷(第8期);1-12 * |
| "捷联惯导/里程计组合导航方法";张小跃等;《北京航空航天大学学报》;第39卷(第7期);922-926 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112731483A (en) | 2021-04-30 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112731483B (en) | Method for judging RTK abnormal value in automatic driving integrated navigation system | |
| EP2519803B1 (en) | Technique for calibrating dead reckoning positioning data | |
| CN100578153C (en) | Calibration method for vehicle speed measuring instrument | |
| CN103033184B (en) | Error correction method, device and system for inertial navigation system | |
| CN105509738A (en) | Inertial navigation/Doppler radar combination-based vehicle positioning and orientation method | |
| CN101201255A (en) | Vehicle combined navigation system based on intelligent navigation algorithm | |
| CN101556160B (en) | Onboard navigation system and method capable of realizing vehicle speed signal self-learning | |
| CN104197958B (en) | Speedometer calibration method based on laser velocimeter dead reckoning system | |
| CN109470276B (en) | Odometer calibration method and device based on zero-speed correction | |
| CN104215262A (en) | On-line dynamic inertia sensor error identification method of inertia navigation system | |
| CN104515527A (en) | Anti-rough error integrated navigation method under non-GPS signal environment | |
| CN114966629A (en) | Vehicle body laser radar external reference calibration method based on EKF algorithm framework | |
| CN115060257A (en) | Vehicle lane change detection method based on civil-grade inertia measurement unit | |
| CN114136310A (en) | Inertial navigation system error autonomous inhibition system and method | |
| CN113092822B (en) | Online calibration method and device of laser Doppler velocimeter based on inertial measurement unit | |
| CN101545781A (en) | Method for determining pulse equivalent of speedometer in on-board integrated navigation | |
| CN111220151B (en) | Inertia and milemeter combined navigation method considering temperature model under load system | |
| CN112254728A (en) | Method for enhancing EKF-SLAM global optimization based on key road sign | |
| CN110987023B (en) | Inertial navigation dynamic alignment method | |
| CN114111792B (en) | Vehicle-mounted GNSS/INS/odometer integrated navigation method | |
| CN113063441B (en) | Data source correction method and device for accumulated calculation error of odometer | |
| CN114935345A (en) | Vehicle-mounted inertial navigation mounting angle error compensation method based on pattern recognition | |
| CN114370885A (en) | Error compensation method and system for inertial navigation system | |
| CN114379577A (en) | Method and device for generating driving track | |
| CN111665530A (en) | GPS (global positioning system) diagnosis method based on vehicle state |
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 |