A vector map-based calibration precision verification method for integrated navigation system of satellite and inertial unit

Abstract

The application relates to a kind of vector map-based verification method of calibrating precision of integrated navigation system of satellite and inertial unit, including dynamic lateral precision verification and static lateral / longitudinal precision verification two parts, dynamic lateral precision verification includes: data acquisition;The trajectory point of the real-time driving of vehicle is projected on vector map;Dynamic lateral precision error is calculated.The static lateral / longitudinal precision verification includes: data acquisition;The position of the real-time stop point of vehicle is projected on vector map;Static lateral / longitudinal precision error is calculated.The application takes the vector map of automatic driving vehicle as prior information, the process is simple, has strong flexibility, and the requirement to scene is lower, only needs the help of vector map, can project output data on map, can also directly reflect the position deviation of vehicle in map while showing quantitative result.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of automatic driving multi-sensor calibration, and particularly relates to a vector map-based calibration precision verification method for a GNSS / INS integrated navigation system. BACKGROUND

[0002] It is understood that positioning technology is one of the core technologies of automatic driving. Automatic driving positioning technology mainly includes three types: the first type is signal-based positioning, such as obtaining positioning information through global satellite GNSS, WIFI, FM microwave, etc.; the second type is environment feature matching-based positioning, such as vision or laser radar-based positioning; and the third type is inertial positioning relying on inertial sensors to obtain positioning information. With the development of automatic driving technology, pure reliance on a certain positioning method cannot meet the demand of high-precision positioning of automatic driving vehicles. Among them, the GNSS / INS integrated navigation system effectively overcomes the problems of insufficient positioning signals of GNSS satellites in complex environments and insufficient navigation accuracy of inertial navigation for a long time while combining the respective advantages and disadvantages of satellite navigation and inertial navigation.

[0003] The GNSS / INS integrated navigation system has now become an important positioning unit indispensable for automatic driving vehicles. The GNSS / INS integrated navigation system is generally installed at the center of the rear axle of the vehicle, and the coordinate system of the calibrated GNSS / INS integrated navigation system is consistent with the vehicle body coordinate system. Accurate GNSS / INS integrated navigation system calibration is an important prerequisite for ensuring good positioning accuracy of the vehicle during driving. However, there are many factors affecting the accuracy of GNSS / INS integrated navigation system calibration in actual situations, such as large installation angle deviation, leading to difficulty in converging the calibration results; long-time operation of the vehicle, causing the integrated navigation system to loosen; antenna installation error, and arm value measurement error, etc. However, since the GNSS / INS integrated navigation output data are high-frequency attitude and position values of the carrier motion, it is difficult to visually present like images and point clouds, and there is a lack of true values for reference and comparison, so from the data aspect, these problems are difficult to expose and the calibration accuracy is difficult to evaluate.

[0004] The existing GNSS / INS integrated navigation system calibration precision verification methods mainly include:

[0005] (1) GNSS / INS integrated navigation system calibration precision verification method based on positioning and orientation system. This method uses the positioning and orientation system, and the vehicle to be verified moves according to the preset motion trajectory and speed, and verifies the calibration precision of the integrated navigation system by comparing and analyzing the output data of the vehicle-mounted integrated navigation system and the positioning and orientation system. The verification result of this method depends on the accuracy of the positioning and orientation system, and since the accuracy of the positioning and orientation system has limitations, it can only be applied to large deviation precision verification.

[0006] (2)High-precision turntable-based verification method for calibration accuracy of integrated navigation system. This method places the target to be measured on the turntable by controlling the rotation angle of the turntable, and verifies the calibration accuracy of the integrated navigation system by comparing and analyzing the output data of the integrated navigation system and the high-precision turntable. This method is not applicable to the integrated navigation system fixed on the vehicle, and the operation is complex, and the verification accuracy is high in cost.

[0007] It is found that the patent with publication number CN112393743A discloses a combined navigation verification system and method of physical motion test mode. The method verifies the calibration accuracy of the integrated navigation system by comparing and analyzing the output data of the vehicle-mounted integrated navigation system and the positioning and orientation system. The verification result of this method depends on the accuracy of the positioning and orientation system, and due to the limitations of the accuracy of the positioning and orientation system, it can only be used for precision verification of large deviations.

[0008] Publication number CN110702144A discloses a method for verifying the integrated navigation system of the strapdown inertial and GPS satellite combined navigation system mounted on an aircraft. The method fixes the integrated navigation system to be verified on the aircraft and performs integrated navigation calculation during flight. This method depends on the aircraft mounting and has a complex process, and is not suitable for autonomous vehicles.

[0009] In summary, the existing calibration accuracy verification methods for integrated navigation systems mostly use high-precision turntables and other equipment for high-precision measurement, and the operation process is complex and costly. Most of them use independent integrated navigation system design, and it is difficult to apply the existing methods to the integrated navigation system installed on autonomous vehicles. At the same time, most of them use data analysis methods to present the results, which is not intuitive.

[0010] Therefore, in order to solve the problem of verifying the calibration accuracy of the integrated navigation system of the autonomous vehicle, it is necessary to study a calibration verification method for the integrated navigation system of the autonomous vehicle, which is simple to operate, high in verification accuracy and low in cost. This method is of great significance for high-precision positioning of autonomous vehicles. SUMMARY

[0011] The purpose of the present application is to solve the problem of complex operation, low precision and high cost in verifying the calibration accuracy of the integrated navigation system of the autonomous vehicle by providing a vector map-based verification method for the calibration accuracy of the integrated navigation system.

[0012] In order to achieve the above purpose, the present application provides a vector map-based verification method for the calibration accuracy of the integrated navigation system, which includes dynamic lateral accuracy verification and static lateral / longitudinal accuracy verification. The method of dynamic lateral accuracy verification is as follows:

[0013] S101, data collection - the vehicle travels along a long straight line, and data of a vehicle inertial navigation system is collected;

[0014] S102, according to the positioning information output by the inertial navigation system, the trajectory point of the vehicle is projected onto the vector map;

[0015] S103, the dynamic lateral accuracy error e is calculated according to the following formula,

[0016]

[0017] In the formula, d i is the distance from the current point to the fitted straight line, n is the number of points, and i is the index of the current point;

[0018] The method for verifying the static lateral / longitudinal accuracy is as follows:

[0019] S201, data collection - the vehicle stops along a long straight line, and data of the inertial navigation system is collected;

[0020] S202, according to the positioning information output by the inertial navigation system, the real-time stop point position of the vehicle is projected onto the vector map;

[0021] S203, the static lateral / longitudinal accuracy error e is calculated according to the following formula,

[0022] e lat = d

[0023]

[0024] In the formula, e lat is the static lateral accuracy error, d is the distance from the reverse stop point to the straight line passing through the current point, e lon is the static longitudinal accuracy error, and d 12 is the distance between the forward stop point and the reverse stop point.

[0025] The application further adopts the following technical scheme:

[0026] In the step S101, the vehicle driving includes route 1 and route 2, wherein the route 1 refers to the vehicle driving forward for dozens of meters while closely adhering to one side of the lane line, and the route 2 refers to the vehicle driving from the end point of the route 1 to the start point of the route 1 in a reverse direction while closely adhering to the same side of the lane line adhered by the route 1.

[0027] In the step 102, the data output by the inertial navigation system in real time is projected onto the vector map to obtain two dynamic trajectory points, and the forward trajectory point set is recorded as P1, and the reverse trajectory point set is recorded as P2.

[0028] In step 103, after traversing the forward trajectory point set, it is judged whether there is an uncalculated trajectory point, if not, the dynamic lateral accuracy error is directly calculated; if there is, the K closest trajectory points in the reverse trajectory set to the current point are searched, a straight line equation is fitted using the K trajectory points, then the distance from the current point to the fitted straight line is calculated, and the calculated distance is added to the distance obtained at the last time, and after the addition, it is judged again whether there is an uncalculated trajectory point in the forward trajectory point set.

[0029] Preferably, the K closest trajectory points (p i (x i ,y i ,z i )∈P1 to the current point p i in the reverse trajectory point set P2 are searched, and a straight line equation is fitted using the K trajectory points, to obtain a straight line direction vector The distance from the current point to the fitted straight line is calculated according to the following formula

[0030] Wherein, x i ,y i ,z i are the x, y, z coordinate values of the current point p i in the x, y, z three directions of the integrated navigation system, A, B, C are direction vectors of the spatial straight line equation, and D is a straight line parameter of the spatial straight line equation. The general formula of the spatial straight line equation is Ax+By+Cz+D=0.

[0031] In step 201, when the vehicle stops along the long straight road, a stop line in the lateral direction of the road is set, and the stop line is perpendicular to the direction of the road lane line.

[0032] The vehicle stops with the front and rear wheels close to the same side lane line, and the rear wheel center of the vehicle presses the stop line.

[0033] In step 202, the data output in real time by the integrated navigation system is projected onto the vector map to obtain two static stop points, and the forward stop point is recorded as p1 and the reverse stop point is recorded as p2.

[0034] In step 203, the closest road vector point set to the forward stop point in the vector map is searched, the K closest points to the forward stop point in the road vector point set are searched, a straight line is fitted using the K points to obtain the direction vector of the road, and then the lateral or longitudinal distance from the forward stop point to the reverse stop point is calculated using the direction vector of the road, which is the static lateral or longitudinal error.

[0035] Preferably, the forward stop point p1 is taken as the current point, and the closest lane line vector point set L1 to the forward stop point p1 in the vector map is searched.

[0036] Search for the K closest points to the forward stop point in the point set L1;

[0037] Use the K points to fit a straight line equation to obtain the straight line direction vector Calculate the spatial straight line equation passing through the current point Ax+By+Cz+D=0, where ABC is the direction vector and D is the straight line parameter;

[0038] Then calculate the distance of the reverse stop point p2 to the straight line equation of the current point The distance d is the static lateral accuracy error e lat = d;

[0039] Use the Pythagorean theorem to obtain the longitudinal accuracy error

[0040] In the formula, A, B, and C are the direction vectors of the spatial straight line equation, D is the straight line parameter of the spatial straight line equation, x, y, and z are the three-dimensional coordinates of the current point, and the general form of the spatial straight line equation is Ax+By+Cz+D=0, d 12 is the distance between the positive stop point and the reverse stop point.

[0041] The advantages of the present application are as follows:

[0042] (1) The present application uses the vector map of the autonomous vehicle as prior information, and can perform accuracy verification on a long straight road with lane lines. The verification process is simple, and the vehicle does not need to be taken to a fixed scene and assisted by a specific device to complete the accuracy verification, which has strong flexibility.

[0043] (2) The present application uses natural scenes, has low requirements for scenes, and only needs to collect data on a flat road during the calibration accuracy verification process, without the need to disassemble and assemble the integrated navigation system, and can be applied to any type of autonomous vehicle.

[0044] (3) The present application uses a vector map to project the output data onto the map, which can not only show the quantitative results but also intuitively reflect the position deviation of the vehicle in the map. BRIEF DESCRIPTION OF DRAWINGS

[0045] The present application will be further described below with reference to the accompanying drawings.

[0046] Figure 1 It is a schematic diagram of the installation position of the vehicle integrated navigation system of the present application.

[0047] Figure 2 It is a schematic diagram of the vehicle driving of the dynamic lateral accuracy verification of the present application.

[0048] Figure 3 It is a flowchart of the dynamic lateral accuracy verification of the present application.

[0049] Figure 4 This is a schematic diagram of a vehicle stopped for static lateral / longitudinal accuracy verification according to the present invention.

[0050] Figure 5 This is a flowchart for the static / longitudinal accuracy verification of the present invention.

[0051] Figure 6 This is a visualization of the dynamic horizontal accuracy verification results of the present invention.

[0052] Figure 7 This is a visualization of the static horizontal / vertical accuracy verification results of this invention. Detailed Implementation

[0053] Example 1

[0054] like Figure 1 As shown, the satellite-inertial navigation system used in this embodiment is installed at the center of the vehicle's rear axle. The vehicle coordinate system is defined as front-left-upper, meaning the X-axis direction is the vehicle's direction of travel, the Y-axis direction is the vehicle's lateral leftward direction, and the Z-axis direction is perpendicular to the ground plane pointing towards the sky. A properly calibrated satellite-inertial navigation system maintains the same coordinate system as the vehicle coordinate system.

[0055] A method for verifying the calibration accuracy of a vector map-based satellite-inertial navigation system mainly includes two parts: dynamic lateral accuracy verification and static lateral / longitudinal accuracy verification. The dynamic lateral accuracy verification displays the vehicle's dynamic trajectory on the vector map in a visual and real-time manner and calculates the dynamic lateral accuracy error. The principle is that the vehicle travels the same trajectory in both directions. Since the satellite-inertial navigation system is installed at the center of the vehicle's rear axle, the two trajectories output by a well-calibrated satellite-inertial navigation system should theoretically overlap. The lateral calibration accuracy is verified by comparing the lateral distance between the two dynamic trajectories.

[0056] Static lateral / longitudinal accuracy verification: The vehicle's stationary position is displayed on the vector map, and the static lateral / longitudinal accuracy errors are calculated. The principle of static lateral accuracy verification is that the vehicle stops close to the lane line in both directions. Since the satellite-inertial navigation system is installed at the center of the vehicle's rear axle, the lateral distance between the two stopping points output by a well-calibrated satellite-inertial navigation system is theoretically zero. The lateral calibration accuracy is verified by comparing the lateral distance between the two stationary positions. The principle of static longitudinal accuracy verification is that a stop line perpendicular to the lane line is set in the lateral direction of the road. The vehicle stops in both directions along the lane line, with the center of the rear wheels touching the stop line. The longitudinal distance between the two stopping points output by a well-calibrated satellite-inertial navigation system is theoretically zero. The longitudinal calibration accuracy is verified by comparing the longitudinal distance between the two stationary positions.

[0057] like Figure 2As shown, during the dynamic lateral accuracy verification of the vehicle, the vehicle travels in both directions, with the vehicle traveling in a straight line along one side of the lane line, and then traveling along the same lane line once in each direction.

[0058] like Figure 3 As shown, the dynamic lateral accuracy verification method in the calibration accuracy verification of a satellite-inertial integrated navigation system based on vector maps includes the following steps:

[0059] S101. Acquire dynamic data from the satellite-inertial navigation system. After data acquisition is initiated, the vehicle travels along one side of the lane line of a long straight road for tens of meters, then travels along the same lane line in both directions once each, collecting data from the satellite-inertial navigation system.

[0060] S102. Based on the positioning information output by the satellite-inertial navigation system, project the real-time output data of the satellite-inertial navigation system onto the vector map to obtain two dynamic trajectory points. The forward trajectory point set is denoted as P1, and the reverse trajectory point set is denoted as P2.

[0061] S103. Calculate the dynamic lateral accuracy error. Traverse the forward trajectory point set P1 and determine if there are any uncalculated trajectory points. If there are uncalculated trajectory points in the forward trajectory point set P2, search for points in the reverse trajectory point set P2 that are a distance from the current point p. i (x i ,y i ,z i The K nearest trajectory points (where the current point p) ∈ P1 i For the uncalculated points found after traversing the forward trajectory point set, x i ,y i ,z i They are the current point p. i In the satellite-inertial navigation system, the x, y, and z coordinates are used, and the straight line equation is fitted using K trajectory points to obtain the straight line direction vector. A, B, and C are all direction vectors of the equation of a straight line in space. Then, the distance d from the current point to the fitted line is calculated. i , D is the linear parameter of the spatial linear equation. The general form of the spatial linear equation is Ax + By + Cz + D = 0. The calculated distance from the current point to the fitted line is accumulated with the distance obtained at the previous moment. After accumulation, it is necessary to return to the positive trajectory point set to re-determine whether there are any uncalculated trajectory points in the positive trajectory point set.

[0062] If there are no uncalculated trajectory points in the forward trajectory point set, then the dynamic lateral precision error is calculated directly. That is: statistically calculate d. i , thus obtaining the lateral accuracy error Where n is the number of points, and i is the index of the current point. The results of the dynamic lateral accuracy verification are shown below. Figure 6 .

[0063] like Figure 4 As shown, the vehicle stops in both directions. First, a stop line perpendicular to the lane line is set on the road. The vehicle stops close to the edge of the lane line, with the center of the rear wheel resting on the stop line. The vehicle stops once in each direction along the same lane line.

[0064] like Figure 5 As shown, the static horizontal / vertical accuracy verification method in the calibration accuracy verification of a satellite-inertial integrated navigation system based on vector maps includes the following steps:

[0065] S201. Acquire static data from the satellite-inertial navigation system. After starting data acquisition, the vehicle moves along one side of the lane line of a long straight road, with the center of the vehicle's rear wheels resting on the stop line in the transverse direction of the road. The stop line is perpendicular to the direction of the lane lines. The vehicle stops once in each direction along the same lane line, and data from the satellite-inertial navigation system is collected.

[0066] S202. Based on the positioning information output by the satellite-inertial navigation system, project the real-time output data of the satellite-inertial navigation system onto the vector map to obtain two static stopping points. The forward stopping point is denoted as p1, and the reverse stopping point is denoted as p2.

[0067] S203. Calculate the static lateral / longitudinal accuracy error. Using the positive stop point p1 as the current point, search the vector map for the lane line vector point set L1 that is closest to the positive stop point p1. Search within point set L1 for the K points closest to the positive stop point. Fit the straight line equation using the K points to obtain the straight line direction vector. Calculate the equation of the line passing through the current point. Then calculate the distance from the reverse stopping point p2 to the current point. The distance d is the lateral accuracy error e. lat =d; Calculate the distance d between two stopping points in the forward and reverse directions. 12 Using the Pythagorean theorem, the longitudinal accuracy error is obtained. In the formula, A, B, and C are all direction vectors of the spatial straight line equation, D is the straight line parameter of the spatial straight line equation, and x, y, and z are the three-dimensional coordinates of the current point in the satellite-inertial navigation system. The general form of the spatial straight line equation is Ax + By + Cz + D = 0. The results of static lateral / longitudinal accuracy verification are shown in [the provided text]. Figure 7 .

[0068] This invention is based on high-precision vector map verification, which is more intuitive and accurate when projected onto the map; it does not rely on fixed scenes or high-precision measurement equipment, requires no disassembly, and is highly flexible; it provides multiple verification methods, including dynamic verification of lateral accuracy and static lateral / longitudinal accuracy verification, which can be switched arbitrarily according to needs; the operation process is simple, the time consumption is short, and it can quickly locate inertial navigation calibration problems.

[0069] It should be noted that the execution order of the above steps is determined by their inherent logic and function. The execution order is sufficient to achieve the desired result of the technical solution disclosed in this patent, and should not impose any limitations or constraints on the implementation of this invention and its embodiments. In addition to the above embodiments, this invention may have other implementation methods. All technical solutions formed by equivalent substitution or equivalent transformation fall within the protection scope claimed by this invention.

Claims

1. A method for verifying the calibration accuracy of a satellite-inertial integrated navigation system based on vector maps, characterized in that, It includes two parts: dynamic lateral accuracy verification and static lateral / longitudinal accuracy verification. The method for dynamic lateral accuracy verification is as follows: S101, Data Acquisition—The vehicle travels along a long straight road, and the data from the vehicle's satellite and inertial navigation system is collected. S102. Based on the positioning information output by the satellite-inertial navigation system, project the real-time trajectory points of the vehicle onto the vector map; project the real-time data output by the satellite-inertial navigation system onto the vector map to obtain two dynamic trajectory points, and record the set of positive trajectory points as follows. The set of points in the reverse trajectory is ; S103. Calculate the dynamic lateral accuracy error according to the following formula. , ， In the formula, This represents the distance from the current point to the fitted line. For the number of points, i The index of the current point; After traversing the forward trajectory point set, determine whether there are any uncalculated trajectory points. If not, directly calculate the dynamic lateral precision error. If so, search for the K nearest trajectory points to the current point in the reverse trajectory point set, and fit a straight line equation using the K trajectory points. Then calculate the distance from the current point to the fitted line, and accumulate the calculated distance with the distance obtained in the previous time step. After the accumulation, re-determine whether there are any uncalculated trajectory points in the forward trajectory point set. In the reverse trajectory point set Search distance from current point Recent A trajectory point, then use By fitting the equation of a straight line to each trajectory point, the direction vector of the straight line can be obtained. The distance from the current point to the fitted line is calculated using the following formula. , In the formula, These are the x, y, and z coordinates of the current point in the satellite-inertial navigation system, respectively. A, B, and C are the direction vectors of the spatial straight line equation, and D is the straight line parameter of the spatial straight line equation. The method for static horizontal / vertical accuracy verification is as follows: S201, Data Acquisition—The vehicle stops along a long straight road, and data is collected from the satellite-inertial navigation system. S202. Based on the positioning information output by the satellite-inertial navigation system, project the real-time stopping point of the vehicle onto the vector map. S203. Calculate the static horizontal / vertical accuracy error according to the following formula. ， ， In the formula, This refers to static lateral accuracy error. This is the distance from the reverse stopping point to the straight line passing through the current point. This refers to static longitudinal accuracy error. This is the distance between the forward and reverse stopping points.

2. The calibration accuracy verification method for a satellite-inertial integrated navigation system based on vector maps according to claim 1, characterized in that, In step S101, the vehicle travel includes route 1 and route 2. Route 1 refers to the vehicle traveling forward for tens of meters while closely following one side of the lane line. Route 2 refers to the vehicle starting from the end of route 1 and traveling in the opposite direction to the beginning of route 1, during which the vehicle closely follows the same side lane line of route 1.

3. The calibration accuracy verification method for a satellite-inertial integrated navigation system based on vector maps according to claim 1, characterized in that, In step S201, when the vehicle stops along the long straight road, a stop line is set in the lateral direction of the road, and the stop line is perpendicular to the direction of the road lane lines. The vehicle stops with its front and rear wheels touching the stop line, keeping close to the lane lines on the same side.

4. The calibration accuracy verification method for a satellite-inertial integrated navigation system based on a vector map according to claim 3, characterized in that, In step S202, the real-time output data of the satellite-inertial navigation system is projected onto a vector map to obtain two static stopping points, and the forward stopping point is recorded as... The reverse stop point is .

5. The calibration accuracy verification method for a satellite-inertial integrated navigation system based on vector maps according to claim 4, characterized in that, In step S203, the set of road vector points closest to the forward stop point is searched in the vector map. The K points closest to the forward stop point are searched in the set of road vector points. The direction vector of the road is obtained by fitting a straight line with the K points. Then, the lateral or longitudinal distance from the forward stop point to the reverse stop point is calculated using the direction vector of the road, which is the static lateral or longitudinal error.

6. The calibration accuracy verification method for a satellite-inertial integrated navigation system based on vector maps according to claim 5, characterized in that, With positive stop point Using the current point as an example, search for the positive stopping point in the vector map. The nearest lane line vector point set ; In point set Search for the closest point to the positive stopping point One point; use By fitting the equation of a straight line to each point, the direction vector of the line can be obtained. Calculate the equation of the spatial line passing through the current point: Ax + By + Cz + D = 0, where A, B, and C are the direction vectors of the spatial line equation, and D is the line parameter of the spatial line equation. Then calculate the distance from the reverse stopping point to the current point using the equation of the line. The distance d is the static lateral accuracy error. ; Calculate the distance between the two stopping points (positive and negative). Using the Pythagorean theorem, the longitudinal accuracy error is obtained. , In the formula, x, y, and z are the three-dimensional coordinates of the current point. This is the distance between the positive and negative stopping points.

Images