Integrated navigation fixed solution state detection method and computer system

Abstract

The invention belongs to the technical field of navigation, and particularly relates to an integrated navigation fixed solution state detection method and a computer system. The method comprises the following steps: when positioning data of a current frame output by integrated navigation is in a fixed solution state, acquiring an increment between the positioning data of the current frame and the positioning data of a previous frame output by the integrated navigation, and when an absolute value of the increment is greater than a hopping threshold, considering that hopping occurs and taking position information of a vehicle when the hopping occurs as a hopping position; the increment comprises a height increment and / or a transverse increment perpendicular to the driving direction of the vehicle; if the distance between the current position of the vehicle and the jumping position is smaller than or equal to a preset distance threshold value and the deviation of the current position of the vehicle in the direction of the increment is too large compared with the position before jumping, it is considered that the state of the fixed solution output by the current integrated navigation is not credible. The technical problem that the positioning result output by integrated navigation in the prior art is abnormal and cannot meet the requirement of automatic driving is solved.

Description

Technical Field

[0001] The invention belongs to the field of navigation technology, and in particular relates to a combined navigation fixed solution state detection method and a computer system. Background Art

[0002] At present, in the autonomous driving industry, integrated navigation equipment is an indispensable and important component. Usually, when using the posture data output by the integrated navigation equipment, the reliability and accuracy of the data are judged by the status flag output by the equipment itself. However, in the autonomous driving industry, autonomous driving vehicles often enter open and unobstructed lanes from elevated lanes, or enter elevated or other obstructed lanes from open and unobstructed lanes. The use environment of the integrated navigation equipment is frequently switched. When the environment is switched, the output status flag of the integrated navigation equipment will often be a credible state, but the output posture data is inaccurate. If the accuracy of the posture data is judged only by the status flag output by the integrated navigation equipment, it will cause abnormal braking of the autonomous driving vehicle, or the vehicle will deviate from the lane, resulting in the unavailability of the autonomous driving function or even dangerous situations. Therefore, it is necessary to filter the data before using the posture data output by the integrated navigation equipment, and the accuracy of the data cannot be judged by the status flag alone, so as to ensure that the posture data used is reliable and accurate, and improve the stability of the autonomous driving system and the safety of driving.

[0003] For example, the Chinese invention patent application publication number CN118131258A discloses a high-precision positioning method based on multi-source fusion technology, which uses laser SLAM and integrated navigation to achieve high-precision positioning. However, the above solution does not detect the fixed solution output by the integrated navigation. Therefore, when a "pseudo-fixed solution" appears in the integrated navigation, the output fusion positioning result will be abnormal and cannot meet the needs of autonomous driving.

[0004] A pseudo-fixed solution refers to a positioning data whose positioning state is a fixed solution output by the combined navigation. However, when the vehicle's environment changes between an unobstructed environment and an obstructed environment, the positioning data output by the combined navigation cannot reach the accuracy of a fixed solution, but the positioning state is still the positioning data of a fixed solution. Summary of the invention

[0005] The purpose of the present invention is to provide a combined navigation fixed solution state detection method and a computer system to solve the technical problem that the positioning results output by the combined navigation in the prior art will be abnormal and cannot meet the requirements of autonomous driving.

[0006] In order to solve the above technical problems, the present invention provides a technical solution of a method for detecting a fixed solution state of an integrated navigation, which is: a method for detecting a fixed solution state of an integrated navigation, the method comprising:

[0007] When the current frame positioning data output by the combined navigation is in a fixed solution state, an increment between the positioning data of the current frame and the previous frame output by the combined navigation is obtained, and when the absolute value of the increment is greater than the jump threshold, it is considered that a jump occurs and the position information of the vehicle when the jump occurs is used as the jump position; the increment includes a height increment and / or a lateral increment perpendicular to the driving direction of the vehicle;

[0008] If the distance between the vehicle's current position and the jump position is less than or equal to the preset distance threshold and the deviation of the vehicle's current position in the direction of the increment compared to the position before the jump is too large, the fixed solution state of the current integrated navigation output is considered unreliable.

[0009] The beneficial effect of the above technical solution is that the technical solution of a combined navigation fixed solution state detection method of the present invention belongs to a pioneering invention. The present invention judges the continuity of the positioning data output by the combined navigation in the fixed solution state to identify the "pseudo-fixed solution". If the positioning state output by the combined navigation is reliable, the positioning data output by the combined navigation should be continuous without jumps in the fixed solution state. Therefore, the present invention judges the accuracy of the current positioning data by judging whether there is a jump in the lateral or height direction of the positioning data output by the combined navigation in the fixed solution state, and can filter out unreliable data to improve the accuracy of the positioning data output by the combined navigation device. The present invention solves the technical problem that the positioning results output by the combined navigation in the prior art will be abnormal and cannot meet the needs of autonomous driving.

[0010] Furthermore, the deviation of the current position of the vehicle from the position before the jump in the direction of the increment is too large according to the following method:

[0011] Depending on whether the current jump direction is opposite to the previous jump direction, the jump flag decreases or increases the set value accordingly;

[0012] If the value of the jump flag is not the initial value, it is considered that the deviation of the current position of the vehicle in the direction of the increment is too large compared to the position before the jump.

[0013] Further, the increment is obtained according to the positioning data in a stable fixed solution state;

[0014] The stable fixed solution state is obtained in the following manner: when the positioning state of the positioning data output by the integrated navigation changes from a non-fixed solution state to a fixed solution state, after waiting for a set time, the positioning data output by the integrated navigation is considered to be a stable fixed solution state.

[0015] Furthermore, if the distance between the current position of the vehicle and the jump position is less than or equal to the distance threshold and the value of the current jump flag is the initial value, it is considered that the fixed solution state of the current integrated navigation output is credible.

[0016] Furthermore, if the distance between the current position of the vehicle and the jump position is greater than the distance threshold, it is considered that the fixed solution state output by the current integrated navigation is credible.

[0017] Furthermore, depending on whether the direction of this jump is opposite to the direction of the last jump, the jump flag is correspondingly reduced or increased by a set value in the following manner: if the direction of this jump is opposite to the direction of the last jump, the jump flag is reduced by 1; otherwise, the jump flag is increased by 1.

[0018] Furthermore, the increment is an increment in a vehicle body coordinate system.

[0019] Furthermore, the increment in the vehicle body coordinate system is obtained by performing coordinate transformation on the increment in the global coordinate system.

[0020] Furthermore, if the fixed solution state output by the current integrated navigation is credible, the value of the jump flag is set to the initial value, and then the integrated navigation outputs the position and posture of the fixed solution state.

[0021] The present invention also provides a technical solution for a computer system: a computer system comprising a processor, wherein the processor is used to execute a computer program to implement the steps of the combined navigation fixed solution state detection method as described above. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] Figure 1 The present invention is a flowchart of a method for detecting a fixed solution state of combined navigation according to an embodiment of the present invention. DETAILED DESCRIPTION

[0023] The present invention determines the continuity of the positioning data output by the combined navigation in the fixed solution state to identify the "pseudo fixed solution". If the positioning state output by the combined navigation is reliable, the positioning data output by the combined navigation should be continuous without jumps in the fixed solution state. Therefore, the present invention determines the accuracy of the current positioning data by determining whether there is a jump in the lateral or height direction of the positioning data output by the combined navigation in the fixed solution state, and can filter out unreliable data to improve the accuracy of the positioning data output by the combined navigation device. The present invention solves the technical problem in the prior art that the positioning results output by the combined navigation will be abnormal and cannot meet the requirements of autonomous driving.

[0024] Combined navigation fixed solution state detection method embodiment:

[0025] When the positioning data output by the combined navigation is used as a data source for positioning output, the status flag of the current combined navigation output can be used to determine whether the combined navigation output is in a fixed solution state. If the positioning state output by the combined navigation is reliable, the positioning data output by the combined navigation should be continuous and without jumps in the fixed solution state. The accuracy of the current positioning data can be determined by determining whether there are jumps in the lateral or height directions of the positioning results output in the fixed solution state. If there are no jumps, the current output position data can be considered to be credible. If there are jumps, the current combined navigation output is a pseudo-fixed solution, and the output positioning data is not accurate and credible. This output can be filtered to improve the accuracy of the positioning data output by the combined navigation device.

[0026] Specifically, Figure 1 As shown, the combined navigation fixed solution state detection method of this embodiment includes:

[0027] 1. After receiving the current frame combined navigation data, determine whether the positioning state of the combined navigation is a fixed solution state. If the positioning data output by the current frame combined navigation is a fixed solution state, count_fix starts counting. When count_fix is ​​greater than the set threshold (i.e. the set time), start calculating the increment of the positioning data output by the current frame combined navigation and the previous frame combined navigation in the global coordinate system to avoid the instability of the positioning data when entering the fixed solution state and affecting the calculation results. If the current frame output by the combined navigation is other solution states, other states are output normally:

[0028]

[0029] Among them, Δxrtk, Δyrtk and Δzrtk are the lateral increment, longitudinal increment and height increment of the integrated navigation output in the global coordinate system respectively; xrtk current ,yrtk current and zrtk current They are the lateral component, longitudinal component and height component of the combined navigation output of the current frame in the global coordinate system; xrtk pre ,yrtk pre and zrtk pre They are respectively the lateral component, longitudinal component and height component of the previous frame combined navigation output in the global coordinate system.

[0030] In this embodiment, the increment is the current frame data minus the previous frame data. In other implementations, the increment may also be the previous frame data minus the current frame data, but it is necessary to ensure that the calculation method of the increment remains unchanged during the entire algorithm execution process.

[0031] 2. According to the attitude angle, pitch angle θ, roll angle γ, and heading angle ψ output by the combined navigation of the current frame, calculate the rotation matrix of the current navigation coordinate system to the vehicle coordinate system (optionally simplify the matrix)

[0032]

[0033] 3. Use the rotation matrix calculated in step 2 to The increment of the positioning data calculated in step 1 in the global coordinate system is converted to the vehicle body coordinate system, and the increment of the positioning data in the vehicle body coordinate system is calculated:

[0034]

[0035] Among them, Δxrtk b , Δyrtk b and Δzrtk b They are respectively the lateral increment, longitudinal increment and height increment of the combined navigation output in the vehicle coordinate system.

[0036] 4. Monitor the Δxrtk calculated in step 3 b and Δzrtk b , when Δzrtk b or Δzrtk b When the absolute value of is greater than the set jump threshold, it is considered that there is one jump, and the jump flag mode_change is increased by 1. If each subsequent jump is in the opposite direction to the previous jump, the jump flag mode_change is reduced by 1, otherwise it is increased by 1, and the position of the jump time is recorded as (x1, y1) (i.e., the jump position).

[0037] In this embodiment, the direction of the jump is indicated by the positive or negative value of the increment.

[0038] In this embodiment, the lateral increment Δxrtk is obtained at the same time b and the height increment Δzrtk b , but when judging, Δxrtk b and Δzrtk b If any one of the above is greater than the transition threshold, a transition is considered to have occurred. The increase or decrease of the transition flag is as follows:

[0039] For example: If the last jump, Δxrtk b and Δzrtk b The absolute values ​​of Δxrtk are greater than the transition threshold (i.e., transitions occur in both directions), and b and Δzrtk b The jump direction is positive.

[0040] ① If the jump is also Δxrtk b and Δzrtk b Both have changed, and Δxrtk b and Δzrtk b The jump directions are all positive. At this time, since both the lateral increment and the height increment have jumped, it is considered that two jumps have occurred, and the jump directions of the lateral increment and the height increment are the same as the previous jump directions, then the jump flag mode_change increases by 2 (both the lateral increment and the height increment increase mode_change by 1, and mode_change increases by 2 in total).

[0041] ② If this jump occurs, Δxrtk b and Δzrtk b All of them have jumped, Δxrtk b The jump direction is positive, Δzrtk b The jump direction is in the opposite direction. At this time, it is also considered that two jumps occur; however, Δxrtk b The jump direction is the same as the last Δxrtk b The jump direction is the same, and Δzrtk b The jump direction is the same as the last Δzrtk b If the jump direction is opposite, the jump flag mode_change remains unchanged (Δxrtk b Make mode_change increase by 1, and Δzrtk b Decrease mode_change by 1, and after superposition, mode_change remains unchanged).

[0042] In summary, in one jump, the lateral increment Δxrtk b and the height increment Δzrtk b They respectively affect the increase or decrease of the jump mark mode_change, and the superposition of the two effects on the jump mark is the change amount of the jump mark this time.

[0043] In other embodiments, only the lateral increment Δxrtk may be obtained. b and the height increment Δzrtk b 5. Calculate the distance between the positioning point output by the current integrated navigation and (x1, y1) saved in step 5. If the distance is greater than the preset distance threshold, set mode_change to the initial value and output the current integrated navigation state as the fixed solution state. If the distance is less than the distance threshold, proceed to the next step.

[0044] 6. Determine whether the current mode_change value is equal to the initial value. If they are equal, it is considered that the fixed solution state of the current combined navigation output is true and reliable, and the positioning state of the current combined navigation data is output as a fixed solution. If the mode_change value is not equal to the initial value, it is considered that the fixed solution state of the current combined navigation output is unreliable, and the positioning state of the current combined data is output as a pseudo-fixed solution state. In this way, the data output by the combined navigation can be filtered to improve the credibility of the combined navigation output.

[0045] In other implementations, a jump flag may be set for the lateral increment and the height increment respectively, and both the lateral increment and the height increment only affect the change of the corresponding jump flag. When both jump flags are initial values, mode_change is considered to be the initial value.

[0046] Computer System Embodiment:

[0047] A computer system includes a processor, the processor is used to execute a computer program to implement the steps of the above-mentioned integrated navigation fixed solution state detection method. The specific integrated navigation fixed solution state detection method has been introduced in detail in the above-mentioned integrated navigation fixed solution state detection method embodiment, and will not be repeated here.

[0048] The present invention has the following characteristics:

[0049] The present invention innovatively uses the positioning data output by the combined navigation to determine the reliability of the combined navigation output data by the lateral and height increments in the vehicle coordinate system. It can improve the reliability of the combined navigation positioning data in the fusion positioning algorithm that uses the combined navigation as the positioning data source, improve the accuracy of the fusion positioning algorithm, and improve the positioning stability of the vehicle when entering an open environment from an obstructed environment.

[0050] The present invention innovatively proposes that the output of the combined navigation fixed solution has a pseudo-fixed state, which can reduce the system abnormality risk caused by directly using the positioning state output by the combined navigation to determine whether the positioning data is a fixed solution and directly using the positioning data of the pseudo-fixed solution, thereby improving the driving safety of the vehicle and the reliability of the system.

[0051] Finally, it should be noted that the above is only a preferred embodiment of the present invention and is not intended to limit the present invention. Although the present invention is described in detail with reference to the aforementioned embodiments, those skilled in the art can still modify the technical solutions recorded in the aforementioned embodiments without creative work, or replace some of the technical features therein with equivalents. Any modification, equivalent replacement, improvement, etc. made within the spirit and principles of the present invention shall be included in the protection scope of the present invention.

Claims

1. A method for detecting the state of a fixed solution of an integrated navigation system, characterized in that: The method includes: When the current frame positioning data output by the combined navigation is in a fixed solution state, an increment between the positioning data of the current frame and the previous frame output by the combined navigation is obtained, and when the absolute value of the increment is greater than the jump threshold, it is considered that a jump occurs and the position information of the vehicle when the jump occurs is used as the jump position; the increment includes a height increment and / or a lateral increment perpendicular to the driving direction of the vehicle; If the distance between the vehicle's current position and the jump position is less than or equal to the preset distance threshold and the deviation of the vehicle's current position in the direction of the increment compared to the position before the jump is too large, the fixed solution state of the current integrated navigation output is considered unreliable.

2. The method for detecting the fixed solution state of integrated navigation according to claim 1, characterized in that: The deviation of the vehicle's current position from the position before the jump in the direction of the increment is too large according to the following method: Depending on whether the current jump direction is opposite to the previous jump direction, the jump flag decreases or increases the set value accordingly; If the value of the jump flag is not the initial value, it is considered that the deviation of the current position of the vehicle in the direction of the increment is too large compared to the position before the jump.

3. The method for detecting the fixed solution state of integrated navigation according to claim 1, characterized in that: The increment is obtained according to the positioning data in a stable fixed solution state; The stable fixed solution state is obtained in the following manner: when the positioning state of the positioning data output by the integrated navigation changes from a non-fixed solution state to a fixed solution state, after waiting for a set time, the positioning data output by the integrated navigation is considered to be a stable fixed solution state.

4. The method for detecting the fixed solution state of integrated navigation according to claim 1, characterized in that: If the distance between the current position of the vehicle and the jump position is less than or equal to the distance threshold and the value of the current jump flag is the initial value, it is considered that the fixed solution state of the current integrated navigation output is credible.

5. The method for detecting the fixed solution state of integrated navigation according to claim 1, characterized in that: If the distance between the current position of the vehicle and the jump position is greater than the distance threshold, it is considered that the fixed solution state output by the current integrated navigation is credible.

6. The method for detecting the fixed solution state of integrated navigation according to claim 2, characterized in that: Depending on whether the direction of this jump is opposite to the direction of the last jump, the jump flag decreases or increases the set value accordingly: if the direction of this jump is opposite to the direction of the last jump, the jump flag decreases by 1; otherwise, the jump flag increases by 1.

7. The method for detecting the fixed solution state of integrated navigation according to claim 1, characterized in that: The increment is an increment in the vehicle body coordinate system.

8. The method for detecting the fixed solution state of integrated navigation according to claim 7, characterized in that: The increment in the vehicle body coordinate system is obtained by performing coordinate transformation on the increment in the global coordinate system.

9. The method for detecting the fixed solution state of integrated navigation according to claim 4 or 5, characterized in that: If the fixed solution state output by the current integrated navigation is credible, the value of the jump flag is set to the initial value, and then the integrated navigation outputs the position and posture of the fixed solution state.

10. A computer system comprising a processor, characterized in that: The processor is used to execute a computer program to implement the steps of the combined navigation fixed solution state detection method according to any one of claims 1 to 9.

Images