Navigation method based on real-time measurement of geomagnetic field

Abstract

The present invention discloses a navigation method based on real-time measurement of geomagnetic field. Based on two types of magnetic field information, namely magnetic field intensity and magnetic field inclination, a two-dimensional gradient approximation algorithm is used to realize long-distance geographic navigation; according to the geomagnetic intensity and inclination information of the known destination, the navigation body measures the geomagnetic field information of the navigation trajectory in real time during the navigation process, and predicts the heading angle according to the gradient information of the magnetic field intensity and inclination. The gradient information is continuously updated during the navigation process until the navigation of the navigation body is realized. The present invention can also maintain the stability of navigation when there are errors in geomagnetic intensity measurement, inclination calculation, step length estimation and heading deflection direction.

Description

Technical Field

[0001] The invention relates to a long-distance navigation method, and in particular to a navigation method based on real-time measurement of the geomagnetic field. Background Art

[0002] Existing global navigation satellite systems (GNSS), such as BDS, GPS, GLONASS and Galileo, can provide all-weather, real-time navigation and positioning services for many applications. However, for AUVs working in the deep sea, GNSS is difficult to access because the attenuation of seawater limits the coverage of electromagnetic waves. GNSS navigation methods have great limitations in deep sea environments.

[0003] Underwater navigation systems usually use inertial navigation systems (INS), simultaneous localization and mapping (SLAM) using imaging sonar or optical sensors, or acoustic measurement systems and magnetic field mapping systems (MMS) instead of GNSS. However, MMS and SLAM methods require maps of the area of ​​interest, and the mapping process is very difficult in ocean scenarios. The cumulative effect of INS errors limits the operating time, and more errors will exist during long-distance navigation.

[0004] The two-dimensional gradient approximation algorithm based on geomagnetic declination and inclination can achieve long-distance and long-operation-time navigation, but this method also has disadvantages: geomagnetic declination is the angle between the geomagnetic north and the horizontal component of the geomagnetic field. Without the help of GNSS or other precision instruments, the measurement and calibration of geomagnetic north is very difficult. Due to the cumulative effect of errors, the accuracy of the inertial gyroscope is also difficult to maintain reliability during long-term operation. Summary of the invention

[0005] In order to solve the problems existing in the background technology, the present invention proposes a navigation method based on real-time measurement of the geomagnetic field, and designs a new long-distance navigation method by using the geomagnetic intensity and geomagnetic inclination, thereby avoiding the problem that the directional error of the fiber optic gyroscope directly affects the measurement of the geomagnetic declination and the estimation of the gradient, thereby further improving the robustness and practicability of the navigation method.

[0006] The technical solution adopted by the present invention is:

[0007] The navigation method method includes the following steps:

[0008] 1) Get the navigation starting point of the navigation body as the initial path point P 0 , waypoint P 0 Along the first random navigation angle Go forward any first distance L 1 Arrive at the first waypoint P 1 ; Path point P 1Along a random second navigation angle Go forward any second distance L 2 Arrive at the second waypoint P 2 ;

[0009] 2) From the currently reached i-th path point P i The navigation angle obtained by processing the navigation angle estimation method of the i+1th Go forward any i+1th distance L i+1 Arrive at the next waypoint P i+1 , and the i+1th path point P i+1 As the current target point, i is updated to i+1, where i is a positive integer greater than or equal to 2;

[0010] 3) Update the target point according to the geomagnetic field information until the geomagnetic field information of the target point is consistent with the geomagnetic field information of the navigation end point, end the navigation, connect each path point in order from small to large according to the sequence number of the path point, and use the polyline connected by all the path points as the navigation path of the navigation body to realize the navigation of the navigation body.

[0011] In step 1), the initial path point P is obtained. 0 , the first path point P 1 And the second path P 2 The geomagnetic field information includes geomagnetic intensity and geomagnetic inclination.

[0012] In the step 3), updating the target point according to the geomagnetic field information specifically comprises: determining whether the geomagnetic field information of the current target point is the same as the geomagnetic field information of the navigation end point;

[0013] If yes, end navigation;

[0014] Otherwise, return to step 2).

[0015] In the step 2), the navigation angle obtained by processing the i+1th navigation angle estimation method is Specifically:

[0016] The i+1th path point P i+1 Corresponding navigation angle Passing the i-th path point P i 、The i-1th path point P i-1 and the i-2th path point P i-2 , and use the following formula to get, the formula is set as follows:

[0017]

[0018]

[0019]

[0020] in, is the i+1th path point P i+1 The corresponding estimated navigation angle, I d is the geomagnetic inclination of the navigation end point, F d is the geomagnetic intensity of the navigation end point, F i is the geomagnetic intensity of the ith path point, I i is the geomagnetic inclination angle of the ith path point, dF and dI are the geomagnetic intensity gradient and geomagnetic inclination gradient respectively, dF x and dF y is the geomagnetic intensity gradient in different directions, L is the advance distance, dI x and dI y is the geomagnetic inclination gradient in different directions, n is the navigation angle correction parameter, and the value of the navigation angle correction parameter is 0 or 1;

[0021] The value of the above navigation angle correction parameter is determined by the following formula:

[0022]

[0023]

[0024]

[0025] in, is the unit vector of the predicted navigation direction at the i+1th path point. When the navigation angle correction parameter n=0, the geomagnetic intensity and geomagnetic inclination (F) of the i+2th path point are calculated by the above formula when the navigation angle correction parameter n is 0. ′i+2 , I′ i+2 ); When the navigation angle correction parameter n = 1, the above formula is used to calculate the geomagnetic intensity and geomagnetic inclination (F′) of the i+2th path point when the navigation angle correction parameter n is 1. i+2 , I′ i+2 ), by comparing the geomagnetic intensity and geomagnetic inclination obtained in the two cases with the geomagnetic intensity and geomagnetic inclination of the i+1th path point, the actual navigation angle is determined.

[0026] The actual navigation angle is determined as follows:

[0027] When the navigation angle correction parameter n is 0, if the geomagnetic intensity and geomagnetic inclination of the i+2th path point are closer to the geomagnetic intensity and geomagnetic inclination of the end point than the geomagnetic intensity and geomagnetic inclination of the i+1th path point, then the navigation angle correction parameter n is 0, otherwise the navigation angle correction parameter n is 1; after determining the value of the navigation angle correction parameter n, the estimated navigation angle corresponding to the i+1th path point is As the actual navigation angle of the i+1th waypoint

[0028] In the step 3), after the target point is updated, a canonical state estimation method is used for posterior processing.

[0029] The geomagnetic intensity and geomagnetic inclination are obtained through magnetic sensors.

[0030] The navigation body is one of an indoor robot and a cable-free underwater robot.

[0031] The beneficial effects of the present invention are:

[0032] 1. Real-time measurement of geomagnetic field information during navigation trajectory can achieve long-distance navigation without any other auxiliary information and calibration;

[0033] 2. By using geomagnetic intensity instead of geomagnetic declination, the complexity of the navigation system is further simplified.

[0034] 3. The navigation stability can be maintained even when there are errors in geomagnetic intensity measurement, inclination calculation, step length estimation and heading deflection. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 For navigation methods with different random errors E dir 、E nott 、E int 、E inc 、E step The simulated trajectory diagram.

[0036] Figure 2 Fig. 2 Experimental setup and simulation results for the navigation method. DETAILED DESCRIPTION

[0037] The present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.

[0038] Embodiments of the present invention are as follows:

[0039] like Figure 1 As shown, the method of the present invention is specifically implemented by using a magnetic field sensor to obtain the geomagnetic intensity and inclination information of the destination based on the geomagnetic north coordinate system of the y-axis (F d , I d), which allows navigation to reach a destination from any starting location.

[0040] 1) Get the navigation starting point of the navigation body as the initial path point P 0 , waypoint P 0 Along the first random navigation angle Go forward any first distance L 1 Arrive at the first waypoint P 1 ; Path point P 1 Along a random second navigation angle Go forward any second distance L 2 Arrive at the second waypoint P 2 ;

[0041] 2) From the currently reached i-th path point P i The navigation angle obtained by processing the navigation angle estimation method of the i+1th Go forward any i+1th distance L i+1 Arrive at the next waypoint P i+1 , and the i+1th path point P i+1 As the current target point, i is updated to i+1, where i is a positive integer greater than or equal to 2;

[0042] 3) Update the target point according to the geomagnetic field information until the geomagnetic field information of the target point is consistent with the geomagnetic field information of the navigation end point, end the navigation, connect each path point in order from small to large according to the sequence number of the path point, and use the polyline connected by all the path points as the navigation path of the navigation body to realize the navigation of the navigation body.

[0043] In step 1), get the initial path point P 0 , the first path point P 1 And the second path P 2 The geomagnetic field information includes geomagnetic intensity and geomagnetic inclination, which is obtained through magnetic sensors.

[0044] In step 3), the target point is updated according to the geomagnetic field information by determining whether the current target point is the navigation end point. If so, the navigation is terminated; otherwise, the process returns to step 2).

[0045] After updating the target point, the canonical state estimation method is used for posterior processing.

[0046] Specifically, the initial path point P 0 , the first path point P 1 , and the second path point P 2 The geomagnetic intensity and inclination distribution of the triangular area formed by the three-point connection line is expressed by a two-dimensional plane equation:

[0047]

[0048]

[0049] The two-dimensional plane has the x-axis perpendicular to the geomagnetic north and the y-axis perpendicular to the geomagnetic north. F is the geomagnetic intensity, I is the geomagnetic inclination, and dF x and dF y are the gradients of the geomagnetic intensity in the x-axis and y-axis directions, dI x and dI y are the gradients of the geomagnetic inclination in the x-axis and y-axis directions respectively, C is a constant term, (x 0 ,y 0 ) are the path point coordinates.

[0050] In step 2), the navigation angle obtained by processing the i+1th navigation angle estimation method is Specifically: the i+1th path point P i+1 Corresponding navigation angle Passing the i-th path point P i 、The i-1th path point P i-1 and the i-2th path point P i-2 , and use the following formula to get, the formula is set as follows:

[0051]

[0052]

[0053]

[0054] in, is the i+1th path point P i+1 The corresponding estimated navigation angle, I d is the geomagnetic inclination of the navigation end point, F d is the geomagnetic intensity of the navigation end point, F i is the geomagnetic intensity of the ith path point, I i is the geomagnetic inclination angle of the ith path point, dF and dI are the geomagnetic intensity gradient and geomagnetic inclination gradient respectively, dF x and dF y is the geomagnetic intensity gradient in different directions, L is the advance distance, dI x and dI y is the geomagnetic inclination gradient in different directions, n is the navigation angle correction parameter, the value of n is 0 or 1, and the value of n is determined by the following formula:

[0055]

[0056]

[0057]

[0058] in, is the unit vector of the predicted navigation direction at the i+1th path point. When the navigation angle correction parameter n=0, the geomagnetic intensity and geomagnetic inclination (F′) of the i+2th path point when the navigation angle correction parameter n is 0 are calculated by the above formula. i+2 , I′ i+2 ); When the navigation angle correction parameter n = 1, the above formula is used to calculate the geomagnetic intensity and geomagnetic inclination (F′) of the i+2th path point when the navigation angle correction parameter n is 1. i+2 , I′ i+2 ), by comparing the geomagnetic intensity and geomagnetic inclination obtained in the two cases with the geomagnetic intensity and geomagnetic inclination of the i+1th path point;

[0059] When the navigation angle correction parameter n is 0, if the geomagnetic intensity and geomagnetic inclination of the i+2th path point are closer to the geomagnetic intensity and geomagnetic inclination of the end point than the geomagnetic intensity and geomagnetic inclination of the i+1th path point, then the navigation angle correction parameter n is 0, otherwise the navigation angle correction parameter n is 1. After determining the value of the navigation angle correction parameter n, the estimated navigation angle corresponding to the i+1th path point is As the actual navigation angle of the i+1th waypoint

[0060] The navigation body in this embodiment is an indoor robot or a cable-free underwater robot.

[0061] Since the measurement and calibration of geomagnetic north is difficult to achieve, and the equipment has an error accumulation effect, it is difficult to maintain accuracy reliability under long-term operation. The coordinate system used by the geomagnetic intensity-inclination navigation mechanism of the present invention is optimized to geomagnetic north instead of geographic north, which is easier to obtain during the navigation process. The magnetic field intensity is used instead of the geomagnetic declination, which further simplifies the complexity of the navigation system. Only the magnetic field information of geomagnetic intensity and geomagnetic inclination is used as the basis for long-distance navigation. Compared with the method based on geomagnetic declination, it is more convenient. The measurement of geomagnetic intensity is much easier than the measurement of magnetic declination that requires precise geographic north, which can further simplify the implementation of the navigation system.

[0062] like Figure 2 As shown, the experimental setting and simulation results of the navigation method of the present invention are demonstrated:

[0063] The simulation was carried out using MATLAB and the heading was given Interference, strength measurement error (F k , I k), geomagnetic inclination calculation and Monte Carlo simulation of step length L are used to verify the feasibility and robustness, practicality and performance of the navigation method of the present invention; wherein the errors are respectively the heading error E dir , geomagnetic intensity measurement error E int 、Geomagnetic inclination error E inc and step error E step .

[0064] The experimental setup was implemented in MATLAB. Before the navigation process, the F and I matrices of the region of interest were generated using the world magnetic model. matrix and I matrix The sampling period is 0.15° in both latitude and longitude; by retrieving F matrix and I matrix Simulate the real-time measurement of geomagnetic intensity and geomagnetic inclination.

[0065] First, E dir The error is an inevitable directional error in the navigation process, which will cause the navigation to deviate from the expected course and will also affect the accuracy and tendency of the geomagnetic intensity gradient estimation; the geomagnetic intensity measurement error E int It may be caused by daily fluctuations in the Earth's magnetic field and the resolution of the sensor system; another unavoidable error that may interfere with the navigation process is caused by E step The error is caused by the cumulative effect of the inertial acceleration sensor errors, which will lead to inaccurate heading direction in the equations for estimating the geomagnetic intensity and geomagnetic inclination gradient.

[0066] Figure 1 The square mark in the figure is the starting point, and the triangle mark is the end point; the curves in the figure are the contour lines of geomagnetic intensity and geomagnetic inclination, the curves with circles and triangles are the navigation tracks, and the circles and triangles are the points of each step in the navigation.

[0067] From all the trajectories, it can be seen that the heading direction in the first two steps is randomly selected. However, after the third step, the heading direction approaches the end point with the gradient of geomagnetic intensity and geomagnetic inclination.

[0068] Finally, all the interferences analyzed above are introduced into the MATLAB simulation, where E step = ±20%, E int = ±12nT, E inc = ±0.033°, E dir The error E of geomagnetic intensity measurement is analyzed in the figure. int In the simulation process, random measurement errors are int= ±12nT; at the same time, this error in measuring the geomagnetic intensity will lead to an error in calculating the geomagnetic inclination; taking the area in the figure as an example, assuming that the geomagnetic field B H The horizontal component ranges from 1×10 4 to 3.5×10 4 nT change, z-axis intensity B Z From -2.5×10 4 to 1.4×10 4 nT changes. Therefore, when B H =1×10 4 nT,B Z =-2.5×10 4 At nT, the maximum inclination calculation error caused by the ±12nT intensity measurement error is ±0.033°:

[0069]

[0070] The results show that the measurement error of geomagnetic intensity E int = ±12nT, which has little effect on the navigation process. This intensity measurement error will lead to the calculation error of the geomagnetic inclination E inc ; The simulation trajectory shows that the step length E is introduced during the navigation process step Random error within the range of ±20%, this level of step error is acceptable; as the direction error increases, the trajectory gradually deviates from the interference-free path. Although the navigation efficiency is affected by the error E dir , but the navigation trajectory can always reach the end point, which is different from the step error E step and intensity measurement error (E int and E inc ) compared with the simulation results show that the heading error E dir The impact on navigation efficiency is the worst. dir When it increases to relatively high levels, the navigation trajectory deviates significantly from the interference-free route. There are some places in the simulation where the direction is wrong, but it is corrected as the local gradient is estimated.

[0071] It can be seen that the long-distance navigation method designed by the present invention using the geomagnetic intensity and geomagnetic inclination avoids the problem that the directional error of the fiber optic gyroscope directly affects the measurement of the geomagnetic declination and the estimation of the gradient, thereby further improving the robustness and practicality of the navigation method.

Claims

1. A navigation method based on real-time measurement of the geomagnetic field, characterized in that: The method comprises the following steps: 1) Get the navigation starting point of the navigation body as the initial path point P0, and the path point P0 is along the random first navigation angle Go forward any first distance L1 to reach the first path point P1; path point P1 along the random second navigation angle Go forward any second distance L2 to reach the second path point P2; In the step 1), the geomagnetic field information of the initial path point P0, the first path point P1 and the second path point P2 is obtained, and the geomagnetic field information includes geomagnetic intensity and geomagnetic inclination; 2) From the currently reached i-th path point P i The navigation angle obtained by processing the navigation angle estimation method of the i+1th Go forward any i+1th distance L i+1 Arrive at the next waypoint P i+1 , and the i+1th path point P i+1 As the current target point, i is updated to i+1, where i is a positive integer greater than or equal to 2; In the step 2), the navigation angle obtained by processing the i+1th navigation angle estimation method is Specifically: The i+1th path point P i+1 Corresponding navigation angle Passing the i-th path point P i 、The i-1th path point P i-1 and the i-2th path point P i-2 , and use the following formula to get, the formula is set as follows: in, is the i+1th path point P i+1 The corresponding estimated navigation angle, I d is the geomagnetic inclination of the navigation end point, F d is the geomagnetic intensity of the navigation end point, F i is the geomagnetic intensity of the ith path point, I i is the geomagnetic inclination angle of the ith path point, dF and dI are the geomagnetic intensity gradient and geomagnetic inclination gradient respectively, dF x and dF y is the geomagnetic intensity gradient in different directions, L is the advance distance, dI x and dI y is the geomagnetic inclination gradient in different directions, n is the navigation angle correction parameter, and the value of the navigation angle correction parameter is 0 or 1; The value of the above navigation angle correction parameter is determined by the following formula: in, is the unit vector of the predicted navigation direction at the i+1th path point, When the navigation angle correction parameter n=0, the geomagnetic intensity and geomagnetic inclination (F′) of the i+2th path point when the navigation angle correction parameter n is 0 are calculated by the above formula: i+2 ,I′ i+2 ); When the navigation angle correction parameter n=1, the geomagnetic intensity and geomagnetic inclination (F′) of the i+2th path point when the navigation angle correction parameter n is 1 are calculated by the above formula: i+2 ,I′ i+2 ), The actual navigation angle is determined by comparing the geomagnetic intensity and geomagnetic inclination obtained in the two cases with the geomagnetic intensity and geomagnetic inclination of the i+1th path point; 3) Update the target point according to the geomagnetic field information until the geomagnetic field information of the target point is consistent with the geomagnetic field information of the navigation end point, end the navigation, connect each path point in order from small to large according to the sequence number of the path point, and use the polyline connected by all the path points as the navigation path of the navigation body to realize the navigation of the navigation body.

2. The navigation method based on real-time measurement of the geomagnetic field according to claim 1, characterized in that: In the step 3), updating the target point according to the geomagnetic field information specifically comprises: determining whether the geomagnetic field information of the current target point is the same as the geomagnetic field information of the navigation end point; If yes, end navigation; Otherwise, return to step 2).

3. The navigation method based on real-time measurement of the geomagnetic field according to claim 1, characterized in that: The actual navigation angle is determined as follows: When the navigation angle correction parameter n is 0, if the geomagnetic intensity and geomagnetic inclination of the i+2th path point are closer to the geomagnetic intensity and geomagnetic inclination of the end point than the geomagnetic intensity and geomagnetic inclination of the i+1th path point, then the navigation angle correction parameter n is 0, otherwise the navigation angle correction parameter n is 1; after determining the value of the navigation angle correction parameter n, the estimated navigation angle corresponding to the i+1th path point is As the actual navigation angle of the i+1th waypoint 4. The navigation method based on real-time measurement of the geomagnetic field according to claim 1, characterized in that: In the step 3), after the target point is updated, a canonical state estimation method is used for posterior processing.

5. The navigation method based on real-time measurement of the geomagnetic field according to claim 1, characterized in that: The geomagnetic intensity and geomagnetic inclination are obtained through magnetic sensors.

6. The navigation method based on real-time measurement of the geomagnetic field according to claim 1, characterized in that: The navigation body is one of an indoor robot and a cable-free underwater robot.

Images