Underwater terrain matching and integrated navigation system and method

Abstract

The invention provides an underwater terrain matching and integrated navigation system and method, an underwater terrain matching navigation method and an underwater terrain matching / inertial integrated navigation method are fused, and under the condition of no terrain data, a large amount of historical terrain data does not need to be accumulated, and the terrain data and inertial data acquired in real time are used for integrated navigation, so that the navigation efficiency is improved. The inertial navigation error is estimated and corrected, so that the error divergence of pure inertial navigation is inhibited, and the navigation precision is improved.

Description

Technical Field

[0001] This invention belongs to the field of underwater navigation, specifically relating to an underwater terrain matching and integrated navigation system and method. Background Technology

[0002] Inertial navigation systems (INS) use built-in gyroscopes and accelerometers to sense the angular velocity and linear acceleration of a carrier. Through multiple integrations, they can obtain the carrier's position, velocity, and attitude. This process requires no external intervention, radiates low energy, and possesses strong stealth and adaptability, making it widely used in aerospace, precision surveying, and other fields. However, the accumulation of errors over time makes INS unsuitable for long-duration underwater applications, necessitating the introduction of alternative navigation methods to suppress these errors. With the advancement of sensor technologies such as sonar and underwater cameras, high-frequency topographic measurement provides a technical path for terrain-matching positioning. Traditional underwater terrain matching methods are derived from airborne terrain matching systems. Aircraft measure the elevation data of a section of terrain along their flight path and perform correlation calculations to obtain the aircraft's position information. Therefore, traditional terrain-matching navigation requires prior acquisition of underwater terrain data within the carrier's movement area. This data is then correlated with other high-precision navigation results to form a database. During actual navigation, real-time detected terrain data is matched with this database to obtain the positioning result, which is then used to correct inertial navigation errors. Underwater terrain matching / inertial navigation has become one of the important solutions for long-endurance, high-stealth, and high-precision navigation of various underwater vehicles, and has been widely promoted and applied.

[0003] In his dissertation, "Research on Seafloor Terrain Matching Navigation of AUVs under Multi-Sensor Conditions," Chen Pengyun disclosed a terrain matching navigation method. This dissertation utilizes multibeam echolocation to detect seafloor topography, establishing electronic nautical chart data and multibeam echolocation data to form a terrain database. During real-time navigation, it proposes a seafloor terrain matching positioning method based on maximum a posteriori estimation (MAP) based on multibeam echolocation, and a target-hunting terrain matching navigation method based on Bayesian filtering based on low-energy detection. The research was validated through sea trial data, and the results show that the positioning accuracy meets the navigation and positioning requirements of underwater vehicles. However, the method requires extensive surveying work to construct the terrain database, and the seafloor topography is susceptible to interference, which reduces the database's effectiveness and hinders its widespread application.

[0004] In his dissertation, "Research on Autonomous Cognition and Path Planning Algorithms for Navigable Areas in Underwater Active SLAM," Cai Qingnan disclosed a SLAM-based underwater terrain matching navigation method. This dissertation utilizes multibeam sonar terrain scanning and proposes a non-parametric seabed topographic map navigability characterization algorithm based on elevation difference. It also clusters the non-parametric features of the seabed topography to pre-determine the geographical location information of the navigable area for navigation. During actual navigation, this method constructs the seabed topographic map. The size of the map increases with the expansion of the operating area, leading to a surge in computational load. Therefore, this method is suitable for navigation within an effective range and requires a high-performance computer to support the computational demands. Summary of the Invention

[0005] The purpose of this invention is to provide an underwater terrain matching and integrated navigation system and method to solve the problem that traditional terrain matching navigation requires prior acquisition of underwater terrain data within the vehicle's movement area, leading to significant inconvenience and high measurement costs during implementation. This invention, without the need for accumulating large amounts of historical terrain data, utilizes real-time acquired terrain data and inertial data for integrated navigation, estimating and correcting inertial navigation errors, thereby suppressing pure inertial navigation error divergence and improving navigation accuracy.

[0006] This invention provides an underwater terrain matching and integrated navigation system and method, which integrates an underwater terrain matching navigation method and an underwater terrain matching / inertial integrated navigation method. The specific solution is as follows:

[0007] Leveraging the high accuracy of inertial navigation over short periods, the system matches real-time measured terrain data with historical terrain data from the surrounding timeframe to obtain changes in heading and position, which are then used to correct the inertial navigation results. The system primarily consists of a forward terrain measurement device, a backward terrain measurement device, and an inertial measurement device. The forward terrain measurement device is installed at the front of the underwater vehicle, the backward terrain measurement device at the rear, and the inertial measurement device close to the forward terrain measurement device. The underwater vehicle moves parallel to the horizontal plane, and the measurement directions of the forward and backward terrain measurement devices are perpendicular to the vehicle's forward direction, respectively measuring the distance between the terrain within the detection range and the terrain measurement device. The X-axis of the forward terrain measurement coordinate system... iq Y iq The axes are respectively related to the X-axis of the carrier coordinate system. b Y b The axes are parallel; the X-axis of the backward topographic survey coordinate system is parallel. ih Y ih The plane containing the axis and the X coordinate system of the forward topographic survey. iq Y iq The planes containing the axes are parallel, and the angle Δφ between the two coordinate systems is... qhThe data can be obtained in advance through calibration; the inertial measurement equipment outputs the angular velocity and acceleration in the carrier coordinate system according to a fixed period τ2 (τ1 is an integer n1 times τ2), and the system performs inertial navigation recursive calculation according to a fixed period τ2 to complete the inertial navigation function; the forward and backward topographic surveying equipment perform synchronous data acquisition and matching according to a fixed period τ1. After successful matching, navigation observations such as the change in heading angle, longitude, and latitude at the corresponding time can be obtained, and combined navigation calculation is performed using a Kalman filter in a fixed period τ1; when the feedback correction period τ3 (τ3 is an integer n2 times τ1) is reached, the inertial navigation error state vector estimated by the combined navigation is used to perform feedback correction on the inertial navigation results to achieve the effect of inertial navigation error suppression.

[0008] The beneficial effects of this invention are as follows:

[0009] This invention eliminates the need for prior acquisition of an underwater topographic database; integrated navigation can be achieved simply by collecting topographic data in real time, effectively suppressing error divergence in heading angle and velocity. Compared to other disclosed methods, this invention is easier to promote and utilize, saves significant surveying costs for topographic databases, and is highly economical. Attached Figure Description

[0010] The accompanying drawings, which form part of this application, are used to provide a further understanding of the invention. The illustrative embodiments of the invention and their descriptions are used to explain the invention and do not constitute an improper limitation of the invention.

[0011] Figure 1 This is a schematic diagram of the system composition of the present invention;

[0012] Figure 2 This is a schematic diagram of the terrain and terrain surveying equipment of the present invention;

[0013] Figure 3 This is a schematic diagram showing the relationship between the forward topographic measurement coordinate system and the carrier coordinate system of this invention;

[0014] Figure 4 This is a schematic diagram of the carrier and the changes in heading and position of the present invention;

[0015] Figure 5 This is a flowchart of the method of the present invention;

[0016] Figure 6 This is a flowchart of the acquisition and matching process for forward and backward terrain data in this invention. Detailed Implementation

[0017] To enable those skilled in the art to better understand the present invention, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of the present invention.

[0018] This embodiment provides an underwater terrain matching and integrated navigation system and method, such as Figure 1 As shown, the system mainly consists of a forward topographic surveying device, a backward topographic surveying device, and an inertial measurement device. The forward topographic surveying device is installed at the front end of the underwater vehicle, the backward topographic surveying device is installed at the rear end of the underwater vehicle, and the inertial measurement device is installed close to the forward topographic surveying device. The underwater vehicle moves parallel to the horizontal plane, and the measurement directions of the forward and backward topographic surveying devices are perpendicular to the direction of the vehicle's movement. The distances between the topography within the detection range and the topographic surveying devices are measured respectively. Figure 2 As shown; X of the forward topographic survey coordinate system iq Y iq The axes are respectively related to the X-axis of the carrier coordinate system. b Y b The axes are parallel, and the relationship between their coordinate systems is as follows: Figure 3 As shown; X of the backward topographic survey coordinate system ih Y ih The plane containing the axis and the X coordinate system of the forward topographic survey. iq Y iq The planes containing the axes are parallel, and the angle Δφ between the two coordinate systems is... qh The data can be obtained beforehand through calibration; the inertial measurement equipment outputs the angular velocity and acceleration in the carrier coordinate system according to a fixed period τ2 (τ1 is an integer n1 times τ2). The system performs inertial navigation recursive calculations according to the fixed period τ2 to complete the inertial navigation function; the forward and backward topographic surveying equipment synchronously acquire and match data according to a fixed period τ1. After successful matching, navigation observations such as the change in heading angle, longitude, and latitude at the corresponding time can be obtained, such as... Figure 4 As shown, the Kalman filter is used to perform integrated navigation calculations at a fixed period τ1; when the feedback correction period τ3 (τ3 is an integer n2 times τ1) is reached, the inertial navigation error state vector estimated by the integrated navigation is used to perform feedback correction on the inertial navigation results, thereby achieving the effect of suppressing inertial navigation errors.

[0019] The implementation process of the system of this invention is as follows: Figure 5 As shown, the specific steps are as follows:

[0020] (1) Initialize navigation parameters:

[0021] (1.1) Construct the 15-dimensional standard inertial navigation error propagation equation:

[0022]

[0023] The physical definition of the state vector X is as follows:

[0024]

[0025] Where φ E φ N φ U For attitude angle error, δV E δV N δV U For velocity error, δL, δλ, and δh are for position error, and ε x ε y ε z For angular velocity error, For acceleration error, and the Kalman filter state vector All variables in the array are initialized to 0.

[0026] (1.2) All elements of the system state accumulation matrix F' are set to 0.

[0027] (1.3) Initialize the covariance matrix of the Kalman filter:

[0028]

[0029] The initial values ​​of the variables on the diagonal of the covariance matrix P0 are determined based on the corresponding initial value error or engineering experience.

[0030] (1.4) Initialize the Kalman filter system noise matrix:

[0031]

[0032] The initial values ​​of the variables on the diagonal of the system noise matrix Q are determined based on the angular velocity and acceleration error noise characteristics output by the inertial measurement equipment.

[0033] (1.5) Initialize inertial navigation parameters, including pitch angle θ ins (0), Roll angle γ ins (0), heading angle ψ ins (0) Eastward speed VE ins (0), Northward speed VN ins (0), Aerial velocity VU ins (0), Latitude L ins (0), Longitude λ ins (0), Height hins (0) and other inertial navigation parameters are assigned values ​​using the measurement results of the external system.

[0034] (1.6) The discrete timing parameters t and k are set to 0.

[0035] (2) Perform inertial measurement data acquisition to obtain the angular velocity of the carrier in the carrier coordinate system at the current moment. and acceleration Increment the value of the discrete timing parameter t by 1.

[0036] (3) Perform inertial navigation recursive calculation:

[0037] (3.1) The inertial navigation parameters θ from the previous moment ins (t-1), γ ins (t-1), ψ ins (t-1), VE ins (t-1), VN ins (t-1), VU ins (t-1), L ins (t-1), λ ins (t-1), h ins (t-1) Using the inertial navigation algorithm and the angular velocity in the current vehicle coordinate system and acceleration Updated to θ ins (t), γ ins (t), ψ ins (t), VE ins (t), VN ins (t), VU ins (t), L ins (t), λ ins (t), h ins (t).

[0038] (3.2) Using θ ins (t), γ ins (t), ψ ins (t), VE ins (t), VN ins (t), VU ins (t), L ins (t), λ ins (t), h ins Update the inertial navigation error propagation equation matrix F(t) and the system state accumulation matrix F':

[0039] F'=F'+F(t·τ2)

[0040] (3.3) Calculate the integrated navigation results:

[0041]

[0042]

[0043] Where the pitch angle θ zh (·), Roll angle γ zh (·), heading angle ψ zh (·), Eastward speed VE zh (·), Northbound speed VN zh (·), Celestial velocity VU zh (·), Latitude L zh (·), Longitude λ zh (·), Height h zh (·) represents the navigation result of the integrated navigation at the corresponding time.

[0044] (4) If the current time is the time of terrain data acquisition, increment the value of discrete timing parameter k by 1 and proceed to step (5); otherwise, proceed to step (2).

[0045] (5) Collect and match forward and backward terrain data to obtain the number of successfully matched frames N of forward terrain data. q Number of frames N that were successfully matched with the backward terrain data h , to obtain N q The dimensional heading change vector Δψ q Time interval vector ΔT q , to obtain N h The dimensional heading change vector Δψ h Time interval vector ΔT h Longitude change vector Δλ h Latitude change vector ΔL h For detailed implementation procedures, please refer to [the relevant documentation / reference]. Figure 6 The implementation process is as follows:

[0046] (5.1) Acquire the forward terrain data matrix I at the current moment. q (x,y,k).

[0047] (5.2) Acquire the backward terrain data matrix I at the current time. h (x,y,k).

[0048] (5.3) The counter variable temp is assigned the value 0.

[0049] (5.4) Number of frames N with successful forward terrain matching q The value is assigned to 0.

[0050] (5.5) Number of frames N for successful backward terrain matching h The value is assigned to 0.

[0051] (5.6) Employ feature extraction algorithms with rotation invariance and scale invariance (such as SIFT, SURF, etc.) to extract features from I. q (x,y,k) feature points are extracted to obtain I q The feature point coordinate set A of (x, y, k) q and the corresponding feature descriptor vector set M q :

[0052]

[0053] Where a q For from I q (x,y,k) represents the number of feature points extracted, where K is the dimension of the feature descriptor.

[0054] (5.7) The same feature extraction algorithm as in step (5.6) is used to process the backward terrain data matrix I. h (x,y,k) feature points are extracted to obtain I h The feature point coordinate set A of (x, y, k) h and the corresponding feature descriptor vector set M h :

[0055]

[0056] Where a h For from I h (x,y,k) represents the number of feature points extracted, where K is the dimension of the feature descriptor.

[0057] (5.8) If the value of temp is equal to the length of the terrain data queue Nw (Nw is an integer, determined according to engineering experience), then the collection and matching process of forward and backward terrain data ends and proceeds to step (6); otherwise, proceed to step (5.9).

[0058] (5.9) The same feature extraction algorithm as in step (5.6) is used to extract features from the historical forward terrain data matrix I. q Extract feature points from (x,y,k-temp-1) to obtain I q The feature point coordinate set B and the corresponding feature descriptor vector set N of (x, y, k-temp-1):

[0059] B = [(x1,y1),…,(x b ,y b )]

[0060] N = [(n 11 ,n 12 …,n 1K ),…,(n b1 ,n b2 …,n bK )]

[0061] Where b is from I q (x,y,k-temp-1) represents the number of feature points extracted, where K is the dimension of the feature descriptor.

[0062] (5.10) If b is less than 2, proceed to step (5.24); otherwise, proceed to step (5.11).

[0063] (5.11) If a q If the value is less than 2, proceed to step (5.17); otherwise, proceed to step (5.12).

[0064] (5.12) Calculate the feature descriptor vector set M q The distance set D between all vectors N q :

[0065]

[0066] (5.13) Extract the distance set D q The two distances D with the smallest values ​​in the middle qe and D qf (D qe ≤D qf ):

[0067]

[0068] Where E qm and E qn D qe The corresponding feature descriptor vector sets M q And the index number of N, F qm and F qn D qf The corresponding feature descriptor vector sets M q And the index number of N.

[0069] (5.14) If D qe and D qf If all values ​​are less than the threshold Th1 (Th1 is an engineering experience parameter), proceed to step (5.15); otherwise, proceed to step (5.17).

[0070] (5.15) Number of frames N that were successfully matched with the forward terrain data q Increment the value by 1.

[0071] (5.16) Calculate the change in heading Δψ q (N q ), time interval ΔT q (N q ):

[0072]

[0073] ΔT q (N q ) = temp + 1

[0074] in Belongs to feature point coordinate set A q , It belongs to the feature point coordinate set B.

[0075] (5.17) If a h If the value is less than 2, proceed to step (5.24); otherwise, proceed to step (5.18).

[0076] (5.18) Calculate the feature descriptor vector set M h The distance set D between all vectors N h :

[0077]

[0078] (5.19) Extract the distance set D h The two distances D with the smallest values ​​in the middle he and D hf (D he ≤D hf ):

[0079]

[0080] Where E hm and E hn D he The corresponding feature descriptor vector sets M h And the index number of N, F hm and F hn D hf The corresponding feature descriptor vector sets M h And the index number of N.

[0081] (5.20) If D he and D hf If all values ​​are less than the threshold Th2 (Th2 is an engineering experience parameter), proceed to step (5.21); otherwise, proceed to step (5.24).

[0082] (5.21) Number of frames N that were successfully matched with the backward terrain data h Increment the value by 1.

[0083] (5.22) Calculate the change in heading Δψ h (N h ), time interval ΔT h (N h ):

[0084]

[0085] ΔT h (N h ) = temp + 1

[0086] in Belongs to feature point coordinate set A h , Belongs to the feature point coordinate set B, Δφ qh The installation deviation angle of the forward and backward topographic surveying equipment in the heading plane.

[0087] (5.23) Calculate the change in longitude Δλ h (N h ), latitude change ΔL h (N h ):

[0088]

[0089] c1=cos(ψ ins (t-(temp+1)*n1))

[0090] s1=sin(ψ ins (t-(temp+1)*n1))

[0091] c2=cos(ψ ins (t)+Δφ qh )

[0092] s2=sin(ψ ins (t)+Δφ qh )

[0093] ΔX(N h )=c1·X q (N h )-s1·Y q (N h )-c2·X h (N h )+s2·(Y h (N h )+W qh )

[0094] ΔY(N h )=s1·X q (N h )+c1·Y q (N h )-s2·X h (N h )-c2·(Y h (N h )+Wqh )

[0095]

[0096] Among them W qx W qy These are the pixel values ​​of the forward topographic surveying equipment in the X and Y axes, respectively, Θ qx Θ qy It refers to the measurement angle range of the forward topographic surveying equipment in the X and Y axis directions, Δφ. qh W represents the installation deviation angle of the forward and backward topographic surveying equipment in the heading plane. qh The distance between the forward and backward topographic surveying equipment is given by R, where R is the length of the Earth's semi-major axis, e is the Earth's ellipsoidal flattening, and ψ is the distance between the forward and backward topographic surveying equipment. ins (·), L ins (·), h ins (·) represents the heading angle, latitude, and altitude calculated by inertial navigation at the corresponding moment.

[0097] (5.24) Increment the value of the counter variable temp by 1, and proceed to step (5.8).

[0098] (6) The Kalman filter is used for integrated navigation calculation. The specific implementation process is as follows:

[0099] (6.1) Update the discretized state equation matrix Φ k / k-1 :

[0100] Φ k / k-1 =I 15×15 +F′

[0101] (6.2) Update the discretized system noise matrix Q k-1 :

[0102]

[0103] (6.3) Update the estimated vector

[0104]

[0105] (6.4) Update the covariance matrix P k / k-1 :

[0106]

[0107] (6.5) If N q With N h If the sum is greater than 0, proceed to process (6.6); otherwise, proceed to process (6.11).

[0108] (6.6) Establish the observation equation matrix H k :

[0109]

[0110] (6.7) Establish the observation vector Z k :

[0111]

[0112] (6.8) Establish the observation noise matrix vector R k :

[0113]

[0114] Where R φq R φh R Lh R λh Determined based on engineering experience.

[0115] (6.9) Update the state estimation vector Covariance matrix P k :

[0116]

[0117] P k =(I 15×15 -K k ·H k )·P k / k-1

[0118] (6.10) Proceed to step (6.12).

[0119] (6.11) Update the state estimation vector Covariance matrix P k :

[0120]

[0121] P k =P k / k-1

[0122] (6.12) All elements of the system state accumulation matrix F' are set to 0, the integrated navigation solution ends and proceeds to step (7).

[0123] (7) If the current time is the feedback correction time, proceed to process (8); otherwise, proceed to process (2).

[0124] (8) Perform feedback correction:

[0125]

[0126] (9) Proceed to step (2).

[0127] The above description of the disclosed embodiments enables those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. An underwater terrain matching and integrated navigation system, characterized in that, include: Forward topographic surveying equipment, backward topographic surveying equipment, and inertial measurement equipment; The forward topographic measurement device is installed at the front end of the underwater vehicle, the backward topographic measurement device is installed at the rear end of the underwater vehicle, and the inertial measurement device is installed close to the forward topographic measurement device. The underwater vehicle moves parallel to the horizontal plane. The forward and backward topographic surveying equipment are perpendicular to the direction of the vehicle's movement, and the distance between the topography within the detection range and the topographic surveying equipment is measured respectively. X of the forward topographic survey coordinate system iq Y iq The axes are respectively related to the X-axis of the carrier coordinate system. b Y b The axes are parallel; The X coordinate system of the backward topographic surveying system ih Y ih The plane containing the axis and the X coordinate system of the forward topographic survey. iq Y iq The planes containing the axes are parallel, and the angle Δφ between the two coordinate systems is... qh This can be obtained in advance through calibration; The inertial measurement device outputs the angular velocity and acceleration in the carrier coordinate system according to a fixed period τ2. The system then performs inertial navigation recursive calculations according to a fixed period τ2 to complete the inertial navigation function.

2. The underwater terrain matching and integrated navigation system according to claim 1, characterized in that, The forward and backward topographic surveying equipment synchronously acquire and match data according to a fixed period τ1. After successful matching, navigation observations such as the change in heading angle, longitude, and latitude at the corresponding time can be obtained, and combined navigation calculations are performed using a Kalman filter at the fixed period τ1.

3. The underwater terrain matching and integrated navigation system according to claim 2, characterized in that, The τ1 is n1 times τ2; n1 is a positive integer.

4. The underwater terrain matching and integrated navigation system according to claim 2, characterized in that, When the feedback correction period τ3 is reached, the inertial navigation result is corrected by using the inertial navigation error state vector estimated by the integrated navigation system, thereby achieving the effect of suppressing inertial navigation error. Here, τ3 is n2 times τ1, and n2 is a positive integer.

5. A navigation method for an underwater terrain matching and integrated navigation system according to any one of claims 1-4, characterized in that, Includes the following steps: Step 1: Navigation parameter initialization; Step 2: Inertial measurement data acquisition, obtain the angular velocity and acceleration of the carrier in the carrier coordinate system at the current moment, and increment the value of the discrete timing parameter t by 1; Step 3: Inertial navigation recursive solution; Step 4: Determine if the current time is for terrain data acquisition. If so, increment the value of the discrete timing parameter k by 1 and proceed to step 5; otherwise, proceed to step 2. Step 5: Collect and match forward and backward terrain data to obtain the number N frames that were successfully matched with the forward terrain data. q Number of frames N that were successfully matched with the backward terrain data h , to obtain N q The dimensional heading change vector Δψ q Time interval vector ΔT q , to obtain N h The dimensional heading change vector Δψ h Time interval vector ΔT h Longitude change vector Δλ h Latitude change vector ΔL h ; Step 6: Perform integrated navigation solution using a Kalman filter; Step 7: If the current time is the feedback correction time, proceed to step 8; otherwise, proceed to step 2. Step 8: Perform feedback correction and return to Step 2.

6. The navigation method according to claim 5, characterized in that, Step 5 specifically involves: Step 5.1: Acquire the forward terrain data matrix I at the current moment. q (x,y,k); Step 5.2: Acquire the backward terrain data matrix I at the current moment. h (x,y,k); Step 5.3: Assign the value 0 to the counter variable temp; Step 5.4: Number of frames N where forward terrain matching was successful q The value is assigned to 0; Step 5.5: Number of frames N where backward terrain matching was successful h The value is assigned to 0; Step 5.6: Use a feature extraction algorithm with rotation invariance and scale invariance to extract features from I. q (x,y,k) feature points are extracted to obtain I q The feature point coordinate set A of (x, y, k) q and the corresponding feature descriptor vector set M q ; Where a q For from I q (x,y,k) represents the number of feature points extracted, where K is the dimension of the feature descriptor. Step 5.7: Use the same feature extraction algorithm as in step (5.6) to process the backward terrain data matrix I. h (x,y,k) feature points are extracted to obtain I h The feature point coordinate set A of (x, y, k) h and the corresponding feature descriptor vector set M h ; Step 5.8: If the value of temp is equal to the length Nw of the terrain data queue, the collection and matching process of the forward and backward terrain data ends and proceeds to step 6; otherwise, proceed to step 5.

9. Step 5.9: Use the same feature extraction algorithm as in Step 5.6 to process the historical forward terrain data matrix I. q Extract feature points from (x,y,k-temp-1) to obtain I q The feature point coordinate set B and the corresponding feature descriptor vector set N of (x,y,k-temp-1); where Nw is an integer, determined based on engineering experience; N=[(n 11 ,n 12 …,n 1K ),…,(n b1 ,n b2 …,n bK )], Where b is from I q (x,y,k-temp-1) represents the number of feature points extracted, where K is the dimension of the feature descriptor. Step 5.10: If b is less than 2, proceed to step 5.24; otherwise, proceed to step 5.

11. Step 5.11: If a q If the value is less than 2, proceed to step (5.17); otherwise, proceed to step (5.12). Step 5.12: Calculate the feature descriptor vector set M q The distance set D between all vectors N q : Step 5.13: Extract the distance set D q The two distances D with the smallest values ​​in the middle qe and D qf (D qe ≤D qf ): Where E qm and E qn D qe The corresponding feature descriptor vector sets M q And the index number of N, F qm and F qn D qf The corresponding feature descriptor vector sets M q and the index number of N; Step 5.14: If D qe and D qf If all values ​​are less than the threshold Th1, proceed to step 5.15; otherwise, proceed to step 5.

17. Th1 is an engineering experience parameter. Step 5.15: Number of frames N where forward terrain data was successfully matched q Increment the value by 1; Step 5.16: Calculate the change in heading Δψ q (N q ), time interval ΔT q (N q ): ΔT q (N q )=temp+1 in Belongs to feature point coordinate set A q , It belongs to the feature point coordinate set B; Step 5.17: If a h If the value is less than 2, proceed to step 5.24; otherwise, proceed to step 5.

18. Step 5.18: Calculate the feature descriptor vector set M h The distance set D between all vectors N h : Step 5.19: Extract the distance set D h The two distances D with the smallest values ​​in the middle he and D hf (D he ≤D hf ): Where E hm and E hn D he The corresponding feature descriptor vector sets M h And the index number of N, F hm and F hn D hf The corresponding feature descriptor vector sets M h and the index number of N; Step 5.20: If D he and D hf If all values ​​are less than the threshold Th2, proceed to step 5.21; otherwise, proceed to step 5.

24. Th2 is an engineering empirical parameter. Step 5.21: Number of frames N where backward terrain data was successfully matched h Increment the value by 1; Step 5.22: Calculate the change in heading Δψ h (N h ), time interval ΔT h (N h ): ΔT h (N h )=temp+1 in Belongs to feature point coordinate set A h , Belongs to the feature point coordinate set B, Δφ qh The installation deviation angle of the forward and backward topographic surveying equipment in the heading plane; Step 5.23: Calculate the change in longitude Δλ h (N h ), latitude change ΔL h (N h ); Step 5.24: Increment the value of the counter variable temp by 1, and proceed to step 5.

8.

7. The navigation method according to claim 5, characterized in that, Step 6 specifically includes: Step 6.1: Update the discretized state equation matrix Φ k / k-1 : F k / k-1 =I 15×15 +F'; Step 6.2: Update the discretized system noise matrix Q k-1 : Step 6.3: Update the estimated vector Step 6.4: Update the covariance matrix P k / k-1 : Step 6.5: If N q With N h If the sum is greater than 0, proceed to step (6.6); otherwise, proceed to step (6.11). Step 6.6: Establish the observation equation matrix H. k : Step 6.7: Establish the observation vector Z k : Step 6.8: Establish the observation noise matrix vector R k : Where R φq R φh R Lh R λh Determined based on engineering experience; Step 6.9: Update the state estimation vector Covariance matrix P k : P k =(I 15×15 -K k ·H k )·P k / k-1 Step 6.10: Proceed to Step 6.12; Step 6.11: Update the state estimation vector Covariance matrix P k : P k =P k / k-1 ; Step 6.12: System state accumulation matrix F ' All elements are assigned the value 0, the combined navigation solution ends and proceeds to step 7.

8. The navigation method according to any one of claims 5-7, characterized in that, Integrated navigation can be achieved simply by collecting terrain data in real time, and the divergence of errors in heading angle and speed can be effectively suppressed.

Images