A geomagnetic bionavigation-based inertial navigation position correction method and device

Abstract

The application discloses a kind of based on geomagnetic bionavigation's inertial navigation position correction method and device.The application utilizes the position and geomagnetic information of several geomagnetic reference points measured in advance, when underwater vehicle travels to the vicinity of geomagnetic reference point nearby sea area, with the difference value of real-time measured geomagnetic information and geomagnetic information of geomagnetic reference point minimum as guide, drive underwater vehicle along the direction of geomagnetic reference point maneuver, complete the correction of inertial navigation position information.The application does not need to construct high-resolution high-precision terrain / geomagnetic / gravity map, easy to implement.

Description

TECHNICAL FIELD

[0001] The present application relates to the underwater navigation technical field, specifically relates to a kind of inertial navigation position correction method and device based on geomagnetic bionavigation. BACKGROUND

[0002] Inertial navigation system has independent autonomy, navigation information is complete, strong concealment and other advantages, and is the main means for underwater vehicle to provide autonomous navigation information guarantee.However, inertial navigation system has the technical characteristics that error diverges with time from principle, and system performance will continuously decline with time, so underwater vehicle must be floated to obtain the accurate position information of satellite navigation to correct inertial navigation error after a period of use, seriously restricts the operation task efficiency of underwater vehicle.

[0003] In order to realize long-time, high-precision autonomous navigation of underwater vehicle, underwater accurate auxiliary positioning means is needed, and external high-precision position information is used to compensate the error of inertial navigation system, mainly including underwater acoustic positioning and geophysical field navigation.However, the use of underwater acoustic positioning has prerequisite and has great safety hazard;Geophysical field navigation needs prior database, and use condition is harsh.Therefore, it is urgent to develop a safe and reliable, independent and autonomous auxiliary navigation means not dependent on prior database. SUMMARY

[0004] Therefore, the present application provides a kind of inertial navigation position correction method and device based on geomagnetic bionavigation, which can effectively realize the inertial navigation position correction of underwater vehicle by using individual geomagnetic reference point measured in advance and geomagnetic information measured by underwater vehicle in real time, and can be applied and popularized in underwater navigation field.

[0005] The inertial navigation position correction method based on geomagnetic bionavigation of the present application comprises:

[0006] Step 1, set a plurality of underwater geomagnetic reference points, obtain the position information and geomagnetic characteristic quantity information of the geomagnetic reference points;

[0007] Step 2, calculate the difference between the position information measured by inertial navigation and the position information of stored geomagnetic reference points during the running of underwater vehicle, if the difference is less than the set threshold value, it is determined that the underwater vehicle has moved to the sea area near the geomagnetic reference point, and step 3 is executed;

[0008] Step 3, the underwater vehicle measures the three-dimensional geomagnetic field intensity of the current position in real time; taking the minimum difference of the three-dimensional geomagnetic field intensity of the current position and the geomagnetic reference point position as the target, the motion direction of the underwater vehicle at the next time is obtained, and the difference between the actual motion direction and the target heading is taken as the feedback input of the heading drive control to navigate and control the underwater vehicle to approach the direction of the geomagnetic reference point; when the intensity difference approaches 0, it is determined that the geomagnetic reference point has been reached; the geomagnetic reference point position is replaced by the inertial navigation position information to realize the inertial navigation position correction.

[0009] Preferably, in step 1, the magnetic sensor and satellite navigation antenna are placed on the same wooden base, and the position information and geomagnetic characteristic quantity information of the geomagnetic reference point are measured in the form of a surface ship towing.

[0010] In step 3, the underwater vehicle releases the magnetic sensor and measures the geomagnetic information of the current position in real time in the form of towing.

[0011] Preferably, in step 3, the target heading H0 of the underwater vehicle is calculated according to the underwater vehicle position information measured in real time by the inertial navigation and the stored geomagnetic reference point position information.

[0012]

[0013] Wherein, R n represents the curvature radius of the earth meridian, R e represents the curvature radius of the earth prime vertical, Δx represents the eastward position difference, and Δy represents the northward position difference.

[0014] Taking the minimum difference of the three-dimensional geomagnetic field intensity of the current underwater vehicle position and the geomagnetic reference point position as the constraint condition, the motion direction of the underwater vehicle at the next time is obtained:

[0015] H k+1 = minf(B k ) (9)

[0016] Wherein, is the three-dimensional geomagnetic field intensity of the underwater vehicle position measured in real time; (B x , B y , B z ) is the three-dimensional geomagnetic field intensity of the geomagnetic reference point position.

[0017] The difference between the actual motion direction of the underwater vehicle and the target heading is taken as the feedback input of the heading drive control to navigate and control the underwater vehicle to approach the direction of the geomagnetic reference point.

[0018] The application also provides an inertial navigation position correction device using the above method, comprising a magnetic sensor, a storage unit, a real-time calculation unit and a motion control unit.

[0019] The magnetic sensor is used to measure the geomagnetic information of the current position.

[0020] The storage unit is used to store the position information and geomagnetic characteristic information of the geomagnetic reference point.

[0021] The real-time calculation unit is used to calculate the difference between the position information of the inertial navigation measurement and the position information of the geomagnetic reference point stored in the storage unit, and when the difference is less than a set threshold, the motion control unit is started.

[0022] The motion control unit aims to minimize the difference between the geomagnetic information of the current position measured by the magnetic sensor and the geomagnetic information of the geomagnetic reference point stored in the storage unit, and performs navigation control until the underwater vehicle travels to the geomagnetic reference point; the inertial navigation position information is replaced by the position information of the geomagnetic reference point, and the correction of the inertial navigation position of the underwater vehicle is completed.

[0023] Advantages:

[0024] (1) The application only needs to measure a few geomagnetic reference points in advance, and does not need to construct a high-resolution high-precision terrain / geomagnetic / gravity map, which has strong operability.

[0025] (2) The application measures the geomagnetic information in the form of towing, which can effectively avoid the influence of geomagnetic characteristic measurement error (magnetic interference).

[0026] (3) The application has strong universality and can be expanded to other carriers. BRIEF DESCRIPTION OF DRAWINGS

[0027] Figure 1 The method flowchart of the application is provided. DETAILED DESCRIPTION

[0028] The application will be described in detail below in conjunction with the drawings and examples.

[0029] The application provides an inertial navigation position correction method and device based on geomagnetic biomimetic navigation, which uses individual geomagnetic reference points measured in advance and real-time measured geomagnetic information of the underwater vehicle to drive the underwater vehicle to maneuver along the direction of the geomagnetic reference point, and realizes the correction of the inertial navigation position information. The components of the device of the application include a storage unit, a real-time calculation unit and a motion control unit, and the execution flowchart is as shown in Figure 1 The execution flowchart includes the following steps:

[0030] Step 1: Set underwater geomagnetic reference points, obtain the position information and geomagnetic characteristic information of the geomagnetic reference points, and store them in the storage unit of the underwater vehicle.

[0031] The embodiment adopts the same wooden base for placing the magnetic sensor and satellite navigation antenna, adopts the form of water surface ship dragging, measures the position information and geomagnetic characteristic quantity information of several geomagnetic reference points, and specifically:

[0032] Step 1.1: measuring the three-dimensional geomagnetic field intensity vector B of the geomagnetic reference point by using the magnetic sensor:

[0033] B = [B x B y B z ] T (1)

[0034] In the formula, B x represents the east component of the geomagnetic field intensity of the geomagnetic reference point, B y represents the north component of the geomagnetic field intensity of the geomagnetic reference point, and B z represents the vertical component of the geomagnetic field intensity of the geomagnetic reference point.

[0035] Step 1.2: measuring the position information P of the geomagnetic reference point by using satellite navigation:

[0036] P = [Lλ] T (2)

[0037] In the formula, L represents the latitude information of the geomagnetic reference point, and λ represents the longitude information of the geomagnetic reference point.

[0038] Step 1.3: constructing the geomagnetic reference point information G(P, B) with the position information P as the horizontal coordinate and the three-dimensional geomagnetic field intensity information B as the vertical coordinate, and storing the measured several geomagnetic reference point information into the storage unit of the underwater vehicle.

[0039] Step 2: the real-time calculation unit determines whether the underwater vehicle moves to the sea area near the geomagnetic reference point according to the position information measured by the inertial navigation of the underwater vehicle and the pre-established geomagnetic reference point, and decides whether to start the geomagnetic bionic navigation under the condition of not affecting the navigation safety and operation task of the underwater vehicle;

[0040] Step 2.1: reading the underwater vehicle position information measured by the inertial navigation in the real-time calculation unit, and calculating the position difference Δr between the underwater vehicle and the geomagnetic reference point in real time:

[0041]

[0042] In the formula, L represents the latitude information of the underwater vehicle measured by the inertial navigation, and λ represents the longitude information of the underwater vehicle measured by the inertial navigation.

[0043] Step 2.2: Determine whether the geomagnetic bionic navigation algorithm is started by comparing the position difference calculated in step 2.1 with the size of the set threshold value δ:

[0044] If Δr < δ, it is determined that the underwater vehicle has moved to the sea area near the geomagnetic reference point, and steps 3-4 are executed to start the geomagnetic bionic navigation algorithm and correct the inertial navigation position;

[0045] If Δr ≥ δ, it is determined that the underwater vehicle is far away from the sea area near the geomagnetic reference point, and the geomagnetic bionic navigation algorithm is not started, and the original navigation plan is continued.

[0046] Step 3: Release the magnetic sensor carried by the underwater vehicle, measure the geomagnetic information of the current position in real time in a towing manner, and calculate the target maneuvering direction of the underwater vehicle;

[0047] Step 3.1: Release the magnetic sensor carried by the underwater vehicle, measure the three-dimensional geomagnetic field intensity vector of the current position in a towing manner

[0048]

[0049] In the formula, Bx represents the east component of the geomagnetic field intensity at the position of the underwater vehicle, By represents the north component of the geomagnetic field intensity at the position of the underwater vehicle, Bz represents the vertical component of the geomagnetic field intensity at the position of the underwater vehicle.

[0050] Step 3.2: Calculate the target heading H0 of the underwater vehicle according to the inertial navigation real-time measured position information of the underwater vehicle and the geomagnetic reference point information stored in the storage unit in advance:

[0051]

[0052] In the formula, R n R represents the curvature radius of the meridian of the earth, R e Δx represents the east position difference, and Δy represents the north position difference.

[0053] Step 4: Start the geomagnetic bionic navigation, that is, use the three-dimensional geomagnetic field intensity difference between the current position of the underwater vehicle and the position of the geomagnetic reference point as the guide, drive the underwater vehicle to approach the direction of the geomagnetic reference point through the motion control unit; when it is determined that the underwater vehicle travels to the geomagnetic reference point, use the reference point position to replace the inertial navigation position information to realize the correction of the inertial navigation position, and end the geomagnetic bionic navigation;

[0054] Step 4.1: Establish the target function of the three-dimensional geomagnetic field intensity at the current time:

[0055]

[0056] In the formula, k represents the current time, and f(B) k ) represents the objective function of the three-dimensional geomagnetic field intensity at the current moment.

[0057] Step 4.2: Using the minimization of the objective function as a constraint, determine the motion direction of the underwater vehicle at the next moment:

[0058] H k+1 =minf(B k (9)

[0059] Step 4.3: Using the difference between the actual direction of motion and the target heading as the feedback input for heading drive control, when all three components of the objective function approach 0, it is determined that the three-dimensional geomagnetic field strength at the current position is infinitely close to the three-dimensional geomagnetic field strength at the target position (reference point position), and the heading drive control process ends.

[0060] f(B k )<ε (10)

[0061] In the formula, ε represents a three-dimensional vector consisting of three elements that tend to 0;

[0062] Step 4.4: At the end of the heading drive control process, the inertial navigation position is corrected by replacing the inertial navigation position information with the geomagnetic reference point position.

[0063]

[0064] In the formula, This indicates the current latitude information of the underwater vehicle. This indicates the current longitude of the underwater vehicle.

[0065] In summary, the above are merely preferred embodiments of the present invention and are not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for inertial navigation position correction based on geomagnetic biomimetic navigation, characterized in that, include: Step 1: Set up several underwater geomagnetic reference points and obtain their location information and geomagnetic characteristic quantity information; Step 2: During the underwater vehicle's movement, the difference between the position information measured by the inertial navigation system and the stored geomagnetic reference point position information is calculated. If the difference is less than a set threshold, it is determined that the vehicle has moved to the sea area near the geomagnetic reference point, and step 3 is executed. Step 3: The underwater vehicle measures the three-dimensional geomagnetic field strength at its current location in real time. With the goal of minimizing the difference in three-dimensional geomagnetic field strength between the current location and the geomagnetic reference point, the underwater vehicle's direction of motion for the next moment is determined. The difference between the actual direction of motion and the target heading is used as the feedback input for heading-driven control to perform navigation control on the underwater vehicle, driving it to approach the geomagnetic reference point. When the intensity difference approaches 0, it is determined that the underwater vehicle has reached the geomagnetic reference point. The geomagnetic reference point position is then used to replace the inertial navigation position information to achieve inertial navigation position correction.

2. The method as described in claim 1, characterized in that, In step 1, the magnetic sensor and the satellite navigation antenna are placed on the same wooden base and towed by a boat on water to measure the position information and geomagnetic characteristic information of the geomagnetic reference point. In step 3, the underwater vehicle releases a magnetic sensor and uses a towing method to measure the geomagnetic information of the current location in real time.

3. The method as described in claim 1, characterized in that, In step 3, the underwater vehicle's target heading H0 is calculated based on the underwater vehicle's real-time position information measured by inertial navigation and the stored geomagnetic reference point position information. Where λ represents the longitude information of the geomagnetic reference point, L represents the longitude information of the underwater vehicle measured by inertial navigation; L represents the latitude information of the geomagnetic reference point. Represents the latitude information of an underwater vehicle measured by inertial navigation; R n R represents the radius of curvature of the Earth's meridian. e Δx represents the radius of curvature of the Earth's circumference, Δy represents the difference in position to the east, and Δy represents the difference in position to the north. Using the minimum difference in the three-dimensional geomagnetic field intensity between the current position of the underwater vehicle and the geomagnetic reference point as a constraint, determine the direction of motion of the underwater vehicle at the next moment: H k+1 =minf(B k ) (9) in, The three-dimensional geomagnetic field intensity at the location of the underwater submersible is measured in real time; (B) x B y B z () represents the three-dimensional geomagnetic field strength at the location of the geomagnetic reference point; The difference between the actual direction of motion of the underwater vehicle and the target heading is used as the feedback input for heading drive control. The underwater vehicle is then guided to approach the geomagnetic reference point.

4. An inertial navigation position correction device employing the method described in any one of claims 1 to 3, characterized in that, It includes a magnetic sensor, a storage unit, a real-time computing unit, and a motion control unit; Among them, the magnetic sensor is used to measure the geomagnetic information of the current location; The storage unit is used to store the location information and geomagnetic characteristic quantity information of the geomagnetic reference point; The real-time calculation unit is used to calculate the difference between the position information measured by the inertial navigation and the position information of the geomagnetic reference point stored in the storage unit. When the difference is less than a set threshold, the motion control unit is activated. The motion control unit aims to minimize the difference between the geomagnetic information of the current position measured by the magnetic sensor and the geomagnetic information of the geomagnetic reference point stored in the storage unit, and performs navigation control until the underwater vehicle reaches the geomagnetic reference point; then it replaces the inertial navigation position information with the position information of the geomagnetic reference point to complete the correction of the underwater vehicle's inertial navigation position.

Images