A micro-inertial / wheel speedometer integrated navigation method based on prior trajectory assistance

Abstract

The present invention discloses a micro-inertial / wheel speedometer combined navigation method based on prior trajectory assistance, which specifically includes the following steps: firstly, establish a prior map library; then perform prior trajectory assisted navigation initialization, including micro-inertial / wheel speedometer combined navigation relative to the map origin; rotate and translate the navigation trajectory to perform map matching to obtain the prior trajectory of the corresponding road section; reversely rotate and translate the prior trajectory to the map origin, and perform nearest point matching between the prior trajectory point and the combined navigation trajectory point; backtrack to perform combined navigation based on the position assistance of the nearest prior trajectory point; absolutize the corrected relative position and posture; finally, repeatedly use the absolute position to obtain the prior trajectory from the prior map library, and perform combined navigation based on the position assistance of the nearest prior trajectory point, until the navigation ends. The present invention effectively corrects the micro-inertial / wheel speedometer combined navigation result, and makes up for the defect that global positioning and orientation cannot be performed by relying on micro-inertial and wheel speedometer.

Description

Technical Field

[0001] The invention relates to a micro-inertial / wheel speedometer combined navigation method based on prior trajectory assistance, belongs to information fusion technology, and is particularly suitable for the field of combined navigation. Background Art

[0002] Micro-inertial / wheel speedometer combined navigation is a commonly used technical means for combined navigation of unmanned vehicles at present, with the advantages of good autonomy and strong anti-interference. However, absolute positioning and orientation cannot be achieved by relying solely on micro-inertial and wheel speedometers, and they are often combined with the Global Navigation Satellite System (GNSS). In the absence of satellite signals, in order to suppress the accumulation of position errors in the combined navigation of micro-inertial / wheel speedometers, laser radars, cameras, etc. have become commonly used auxiliary sensors. However, laser radars and cameras are easily affected by external factors such as the environment, and there is a problem of work interruption. Since the combined navigation trajectory of micro-inertial / wheel speedometers is similar to the real trajectory of the carrier, and the real trajectory of the carrier is similar to the prior trajectory such as the planned trajectory and the road network trajectory, this provides the possibility for the prior trajectory to assist the combined navigation of micro-inertial / wheel speedometers. Map matching provides a technical means to achieve the matching correspondence between the combined navigation trajectory and the prior trajectory. In addition, the assistance based on the map prior trajectory will make up for the defect that the combined navigation of micro-inertial / wheel speedometers cannot perform global positioning and orientation. Summary of the invention

[0003] In order to cope with the problems of inaccurate attitude, accumulated position error and inability to perform global positioning and orientation in micro-inertial / wheel speed meter combined navigation when there is no satellite signal and sensors such as lidar and camera cannot provide auxiliary navigation information, the present invention proposes a micro-inertial / wheel speed meter combined navigation method based on prior trajectory assistance.

[0004] The above-mentioned object of the present invention is achieved by the following technical solutions:

[0005] A micro-inertial / wheel speedometer combined navigation method based on prior trajectory assistance specifically comprises the following steps:

[0006] Step S1: Establish a regional prior map library, which contains road boundaries and prior trajectory points;

[0007] Step S2: Initialize the prior trajectory assisted navigation;

[0008] Step S3: using the absolute position output by the integrated navigation to obtain the prior trajectory from the prior map library, and obtaining the nearest matching point through nearest point matching to assist the micro-inertial / wheel speedometer integrated navigation, and repeating this step until the navigation is completed.

[0009] Further, the step S2 specifically includes the following process:

[0010] S2.1 The vehicle drives a distance in the area where the map library has been built, and performs micro-inertial / wheel speedometer combined navigation relative to the map origin to obtain the position trajectory of this distance relative to the map origin and the posture sequence relative to the map coordinate axis. The combined navigation adopts the classic Kalman filter algorithm, and the state space model of the combined system is:

[0011]

[0012]

[0013]

[0014] The system state vector X selects the misalignment angle φ and velocity error δv of the inertial navigation system n , positioning error δp n , gyroscope-related drift Accelerometer-related bias as the state, F is the system matrix, G is the system noise input matrix, W is the system noise vector, is the carrier system velocity calculated by micro-inertial navigation, is the load system speed measured by the wheel speed meter, and The difference is used as the observation vector Z of Kalman filtering, H is the measurement matrix, V is the observation noise vector, is the transformation matrix between the navigation coordinate system n and the carrier coordinate system b. The combined navigation process makes full use of the nonholonomic constraints of the vehicle;

[0015] S2.2 uses the similarity principle between the combined navigation trajectory of the micro-inertial / wheel speed meter and the real motion trajectory of the carrier, and performs map matching on the navigation trajectory after rotation and translation to obtain the prior trajectory points of the corresponding road section;

[0016] S2.3 uses the rotation and translation in step S2.2 to derotate and translate the prior trajectory to the map origin:

[0017]

[0018] in, is the global prior trajectory point, R and t are the rotation matrix and translation vector in step S2.2, respectively. is the prior trajectory point relative to the map origin obtained after inverse rotation and translation;

[0019] S2.4 performs the closest point matching between the prior trajectory point and the micro-inertial / wheel speedometer combined navigation trajectory point:

[0020]

[0021] in, is the jth micro-inertial / wheel speedometer combined navigation trajectory point relative to the map origin, is the prior trajectory point relative to the map origin of the ith point, and the point closest to the prior trajectory point in the combined navigation trajectory point is selected as the nearest matching point;

[0022] S2.5 backtracks to perform micro-inertial / wheel speedometer integrated navigation based on the position of the nearest prior trajectory point to obtain the corrected relative position and attitude. The integrated navigation still uses the classic Kalman filter algorithm, and the state vector and state equation are the same as those described in step S2.1. The measurement vector and measurement equation become

[0023]

[0024]

[0025] in, For Matching combined navigation position;

[0026] S2.6 performs global processing on the corrected relative position and attitude, that is, the absolute position and attitude are obtained after rotation and translation:

[0027]

[0028]

[0029] in, and are the combined navigation absolute position and attitude, and They are respectively the corrected combined navigation position and attitude relative to the map origin.

[0030] Further, the step S3 specifically includes the following process:

[0031] The absolute position output by the combined navigation is used to obtain the prior trajectory from the prior map library, and the nearest matching point is obtained through nearest point matching to assist the micro-inertial / wheel speedometer combined navigation, wherein the nearest point matching and the nearest matching point assisted micro-inertial / wheel speedometer combined navigation are the same as steps S2.4 and S2.5.

[0032] The present invention has the following advantages and beneficial effects:

[0033] (1) The present invention provides a solution for combined positioning and orientation based on prior trajectory, micro-inertia and wheel speed meter when there is no satellite signal and sensors such as laser radar and camera cannot provide auxiliary navigation information;

[0034] (2) The present invention utilizes the prior trajectory to assist in effectively correcting the micro-inertial / wheel speedometer combined navigation result, thus making up for the defect that global positioning and orientation cannot be performed by relying on micro-inertial and wheel speedometer.

[0035] (3) After completing the initialization of the prior trajectory assisted navigation, the present invention can assist the micro-inertial / wheel speedometer combined navigation online in real time. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] Figure 1 It is a principle block diagram of the micro-inertial / wheel speedometer combined navigation method based on prior trajectory assistance according to the present invention;

[0037] Figure 2 This is a schematic diagram of initialization of the micro-inertial / wheel speedometer combined navigation assisted by a priori trajectory according to the present invention;

[0038] Figure 3 Schematic diagram of a priori trajectory points and nearest matching points in the embodiment;

[0039] Figure 4 It is a comparison diagram of the prior trajectory, the real trajectory, the trajectory assisted by SINS / ODO and the prior trajectory in the embodiment;

[0040] Figure 5 A comparison diagram of horizontal positioning errors before and after prior trajectory auxiliary correction in the embodiment;

[0041] Figure 6 It is a comparison diagram of the posture error before and after the prior trajectory assisted correction in the embodiment. DETAILED DESCRIPTION

[0042] The technical solution provided by the present invention will be described in detail below in conjunction with the accompanying drawings and specific embodiments. It should be understood that the following specific implementation methods are only used to illustrate the present invention and are not used to limit the scope of the present invention.

[0043] Example 1: Reference Figure 1 and Figure 2 In one embodiment, the micro-inertial / wheel speedometer combined navigation method based on prior trajectory assistance mainly includes the following steps:

[0044] Step S1: Establish a regional prior map library, which includes road boundaries and prior trajectory points.

[0045] Step S2: Initialize the prior trajectory assisted navigation:

[0046] S2.1 The vehicle drives a distance in the area where the map library has been built, and performs micro-inertial / wheel speedometer combined navigation relative to the map origin to obtain the position trajectory of this distance relative to the map origin and the posture sequence relative to the map coordinate axis. Among them, the combined navigation adopts the classic Kalman filter algorithm, and the state space model of the combined system is

[0047]

[0048]

[0049]

[0050] The system state vector X selects the misalignment angle φ and velocity error δv of the inertial navigation system n , positioning error δp n , gyroscope-related drift Accelerometer-related bias as the state, F is the system matrix, G is the system noise input matrix, W is the system noise vector, is the carrier system velocity calculated by micro-inertial navigation, is the load system speed measured by the wheel speed meter, and The difference is used as the observation vector Z of Kalman filtering, H is the measurement matrix, V is the observation noise vector, is the transformation matrix between the navigation coordinate system n and the carrier coordinate system b. The combined navigation process makes full use of the nonholonomic constraints of the vehicle;

[0051] S2.2 uses the similarity principle between the combined navigation trajectory of the micro-inertial / wheel speed meter and the real motion trajectory of the carrier, and performs map matching on the navigation trajectory after rotation and translation to obtain the prior trajectory points of the corresponding road section;

[0052] S2.3 uses the rotation and translation in step S2.2 to derotate and translate the prior trajectory to the map origin:

[0053]

[0054] in, is the global prior trajectory point, R and t are the rotation matrix and translation vector in step S2.2, respectively. is the prior trajectory point relative to the map origin obtained after inverse rotation and translation;

[0055] S2.4 performs the closest point matching between the prior trajectory point and the micro-inertial / wheel speedometer combined navigation trajectory point:

[0056]

[0057] in, is the jth micro-inertial / wheel speedometer combined navigation trajectory point relative to the map origin, is the prior trajectory point relative to the map origin of the ith point, and the point closest to the prior trajectory point in the combined navigation trajectory point is selected as the nearest matching point;

[0058] S2.5 performs backtracking based on the micro-inertial / wheel speedometer integrated navigation assisted by the position of the most recent prior trajectory point to obtain the corrected relative position and attitude. Among them, the integrated navigation still uses the classic Kalman filter algorithm, the state vector and state equation are the same as those described in step S2.1, and the measurement vector and measurement equation become

[0059]

[0060]

[0061] in, For Matching combined navigation position;

[0062] S2.6 performs global processing on the corrected relative position and attitude, that is, the absolute position and attitude are obtained after rotation and translation:

[0063]

[0064]

[0065] in, and are the combined navigation absolute position and attitude, and They are respectively the corrected combined navigation position and attitude relative to the map origin.

[0066] Step S3: using the absolute position output by the integrated navigation to obtain the prior trajectory from the prior map library, and obtaining the nearest matching point through nearest point matching to assist the micro-inertial / wheel speedometer integrated navigation, and repeating this step until the navigation is completed.

[0067] The following are the simulation results of the embodiment. Figure 4 Comparison diagram of the prior trajectory, the carrier's true trajectory, the SINS / ODO trajectory, and the navigation trajectory assisted by the prior trajectory. Figure 5 Comparison diagram of horizontal positioning error before and after prior trajectory assisted correction. Figure 6 The comparison diagram of the heading error before and after the prior trajectory auxiliary correction is shown in the simulation results. The present invention can effectively correct the micro-inertial / wheel speed meter combined navigation results, and provides a solution for combined positioning and orientation based on the prior trajectory, micro-inertial and wheel speed meter.

[0068] It should be noted that the above embodiments are merely preferred embodiments of the present invention and are not intended to limit the protection scope of the present invention. Equivalent replacements or substitutions made on the basis of the above technical solutions all fall within the protection scope of the present invention.

Claims

1. A micro-inertial / wheel speedometer combined navigation method based on prior trajectory assistance, characterized in that: The specific steps include: Step S1: Establish a regional prior map library, which contains road boundaries and prior trajectory points; Step S2: Initialize the prior trajectory assisted navigation; Step S3: using the absolute position output by the integrated navigation to obtain the prior trajectory from the prior map library, and obtaining the nearest matching point through the nearest point matching to assist the micro-inertial / wheel speedometer integrated navigation, and repeating this step until the navigation is completed. The step S2 specifically includes the following process: S2.1 The vehicle drives a distance in the area where the map library has been built, and performs micro-inertial / wheel speedometer combined navigation relative to the map origin to obtain the position trajectory of this distance relative to the map origin and the posture sequence relative to the map coordinate axis. The combined navigation adopts the classic Kalman filter algorithm, and the state space model of the combined system is: The system state vector X selects the misalignment angle φ and velocity error δv of the inertial navigation system n , positioning error δp n , gyroscope-related drift Accelerometer-related bias as the state, F is the system matrix, G is the system noise input matrix, W is the system noise vector, is the carrier system velocity calculated by micro-inertial navigation, is the load system speed measured by the wheel speed meter, and The difference is used as the observation vector Z of Kalman filtering, H is the measurement matrix, V is the observation noise vector, is the transformation matrix between the navigation coordinate system n and the carrier coordinate system b. The combined navigation process makes full use of the nonholonomic constraints of the vehicle; S2.2 uses the similarity principle between the combined navigation trajectory of the micro-inertial / wheel speed meter and the real motion trajectory of the carrier, and performs map matching on the navigation trajectory after rotation and translation to obtain the prior trajectory points of the corresponding road section; S2.3 uses the rotation and translation in step S2.2 to derotate and translate the prior trajectory to the map origin: in, is the global prior trajectory point, R and t are the rotation matrix and translation vector in step S2.2, respectively. is the prior trajectory point relative to the map origin obtained after inverse rotation and translation; S2.4 performs the closest point matching between the prior trajectory point and the micro-inertial / wheel speedometer combined navigation trajectory point: in, is the jth micro-inertial / wheel speedometer combined navigation trajectory point relative to the map origin, is the prior trajectory point relative to the map origin of the ith point, and the point closest to the prior trajectory point in the combined navigation trajectory point is selected as the nearest matching point; S2.5 backtracks to perform micro-inertial / wheel speedometer integrated navigation based on the position of the nearest prior trajectory point to obtain the corrected relative position and attitude. The integrated navigation still uses the classic Kalman filter algorithm, and the state vector and state equation are the same as those described in step S2.

1. The measurement vector and measurement equation become in, For Matching combined navigation position; S2.6 performs global processing on the corrected relative position and attitude, that is, the absolute position and attitude are obtained after rotation and translation: in, and are the combined navigation absolute position and attitude, and They are respectively the corrected combined navigation position and attitude relative to the map origin.

Images