An underwater backtracking inertial navigation method, system and device based on information constraints

Abstract

The present invention relates to the field of inertial navigation technology, and specifically discloses an underwater backtracking inertial navigation method, system and equipment based on information constraints, the method comprising: obtaining the initial attitude angle, initial speed, end speed, end position, angular velocity of the whole navigation process and specific force of the whole navigation process of the carrier during the underwater navigation process; carrying out inertial navigation according to the initial attitude angle, initial speed, angular velocity of the whole navigation process and specific force of the whole navigation process, and solving to obtain the attitude angle of the whole navigation process; carrying out reverse navigation according to the attitude angle, end speed and end position of the whole navigation process, and solving to obtain the speed of the whole navigation process and the position of the whole navigation process. The present invention realizes the inertial navigation of the whole process under the conditions that the initial attitude angle and speed of the carrier are known and the position is unknown, and the end speed and position are known and the attitude angle is unknown, and the navigation and positioning performance of the underwater carrier is effectively improved.

Description

Technical Field

[0001] The present invention relates to the field of inertial navigation technology, and in particular to an underwater backtracking inertial navigation method, system and equipment based on information constraints. Background Art

[0002] At present, underwater robots have become an important platform for underwater exploration. In addition to strong maneuverability, they also need to have navigation capabilities in complex environments. In this project, acoustic positioning systems, global navigation satellite systems, inertial navigation systems, etc. are mainly used to achieve the navigation of underwater robots. Among them, acoustic positioning and global navigation satellite systems are limited by hardware conditions and cannot be used in the entire area. Inertial navigation systems have become an irreplaceable way to measure navigation, guidance and control information for underwater robots due to the continuity, autonomy and high reliability of their navigation information.

[0003] An important task in the exploration of marine resources is to determine the location of marine resources underwater. Therefore, accurate navigation and positioning for underwater robots are crucial to the accuracy of obtaining scientific data. In the conventional technical route, there are two methods to achieve underwater inertial navigation: one is to know the attitude, speed and position information at the initial moment at the same time, and carry out forward navigation to obtain the real-time attitude, speed and position; the other is to know the attitude, speed and position information at the end moment at the same time, and use the stored inertial measurement data of the entire navigation process to carry out reverse navigation to calculate the attitude, speed and position. However, for underwater robots, it is difficult to obtain information such as the initial attitude angle, initial position, and attitude angle at the end moment only through the inertial measurement unit and GNSS receiver equipped, so it is impossible to use conventional methods to achieve underwater inertial navigation. Summary of the invention

[0004] The present invention aims to solve the problem that underwater inertial navigation can only be achieved when the attitude, speed and position information of the underwater carrier at the initial moment or the end moment is known. To this end, the present invention provides an underwater backtracking inertial navigation method, system and device based on information constraints. In view of the hardware limitations of small underwater robots, the inertial measurement unit and the Global Navigation Satellite System (GNSS) receiver are used to achieve full-process inertial navigation under the conditions that the attitude angle and speed of the carrier at the initial moment are known and the position is unknown, and the speed and position at the end moment are known and the attitude angle is unknown. Without introducing additional sensors, inertial navigation with limited reference information during the movement of the carrier can be achieved, which can effectively improve the navigation and positioning performance of the underwater robot.

[0005] The present invention provides an underwater backtracking inertial navigation method based on information constraints, and the technical solution adopted is as follows: comprising:

[0006] Obtaining the initial attitude angle, initial speed, final speed, final position, angular velocity of the entire navigation process, and specific force of the entire navigation process of the carrier during underwater navigation;

[0007] According to the initial attitude angle, the initial speed, the angular velocity of the navigation process and the relative force of the navigation process, inertial navigation is performed to calculate the attitude angle of the navigation process;

[0008] According to the attitude angle, end-time speed and end-time position of the navigation process, reverse navigation is carried out to calculate the speed of the navigation process and the position of the navigation process.

[0009] Furthermore, according to formula (1), inertial navigation is carried out to calculate the attitude angle of the whole navigation process.

[0010] (1)

[0011] in, Indicates navigation time, , Indicates the end time, during inertial navigation From 0 to , Represents the attitude matrix from the carrier coordinate system to the navigation coordinate system, express The first derivative of represents the angular velocity, express The antisymmetric matrix of represents the angular velocity of the navigation coordinate system relative to the inertial coordinate system, express The antisymmetric matrix of It represents the speed of the whole navigation process of the carrier in the navigation coordinate system. express The first derivative of Indicates comparison, represents the angular velocity of the navigation coordinate system caused by the rotation of the earth, It represents the angular velocity of rotation of the navigation coordinate system caused by the motion of the carrier, Represents the gravity component in the navigation coordinate system.

[0012] Furthermore, It contains the attitude angle of the carrier during the entire navigation process, and the attitude angle includes heading angle, pitch angle and roll angle.

[0013] Furthermore, , represents the eastward speed, represents the north speed, represents the celestial velocity, T represents the matrix transpose, , represents the acceleration due to gravity,

[0014] (2)

[0015] is the Earth's rotation angular rate, Indicates the latitude of the carrier in the geographic coordinate system. Represents the average radius of the Earth.

[0016] Furthermore, reverse navigation is carried out according to formula (3) to calculate the speed and position of the entire navigation process.

[0017] (3)

[0018] in, Indicates the position of the entire navigation process, , Indicates the longitude of the carrier in the geographic coordinate system. Indicates the height of the carrier in the geographic coordinate system. express The first derivative of express The negative matrix of express The negative matrix of express The negative matrix of express The negative matrix of .

[0019] Furthermore, the initial attitude angle is obtained by an inertial navigation initial alignment technology; at the initial moment, the carrier is stationary, and the initial speed is zero.

[0020] Furthermore, the end time velocity and the end time position are obtained by a GNSS receiver.

[0021] Furthermore, the angular velocity of the entire navigation process is measured by a gyroscope on the carrier, and the specific force of the entire navigation process is measured by an accelerometer on the carrier.

[0022] The present invention also provides an underwater backtracking inertial navigation system based on information constraints, and the technical scheme adopted is as follows: comprising: a data acquisition module, an inertial navigation solution module and a reverse navigation solution module connected in sequence;

[0023] A data acquisition module is used to obtain the initial attitude angle, initial speed, end speed, end position, angular velocity of the whole navigation process and specific force of the whole navigation process of the carrier during the underwater navigation process;

[0024] An inertial navigation solution module is used to perform inertial navigation according to the initial attitude angle, the initial speed, the angular velocity of the navigation process and the specific force of the navigation process, and solve the attitude angle of the navigation process;

[0025] The reverse navigation solution module is used to carry out reverse navigation according to the attitude angle, end time speed and end time position of the whole navigation process, and solve the speed and position of the whole navigation process.

[0026] The present invention also provides a computer device, including a memory and a processor, wherein the memory stores a computer program, and the processor implements the above-mentioned underwater backtracking inertial navigation method based on information constraints when executing the computer program.

[0027] The above one or more technical solutions in the embodiments of the present invention have at least one of the following technical effects:

[0028] The present invention aims at the situation that it is difficult to obtain all the attitude, speed and position information at the initial moment or the end moment due to the hardware limitation of the small underwater robot. The present invention uses the attitude angle and speed of the carrier at the initial moment, as well as the speed and position at the end moment, to carry out forward navigation solution and reverse navigation solution, and completes the solution of the attitude, speed and position of the whole process of carrier navigation. Under the conditions that the attitude angle and speed of the carrier at the initial moment are known and the position is unknown, and the speed and position at the end moment are known and the attitude angle is unknown, the whole process of inertial navigation is realized, and no additional sensors are introduced, so that the inertial navigation with limited reference information during the movement of the carrier can be realized, which can effectively improve the navigation and positioning performance of the underwater robot.

[0029] Additional aspects and advantages of the present invention will be given in part in the following description and in part will be obvious from the following description, or will be learned through practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0030] In order to more clearly illustrate the technical solutions in the present invention or the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0031] Figure 1 It is a flow chart of the method provided by the present invention. DETAILED DESCRIPTION

[0032] In order to make the purpose, technical scheme and advantages of the present invention clearer, the technical scheme of the present invention will be clearly and completely described below in conjunction with the drawings in the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments in the present invention, all other embodiments obtained by ordinary technicians in the field without creative work are within the scope of protection of the present invention. The following embodiments are used to illustrate the present invention, but cannot be used to limit the scope of the present invention.

[0033] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "example", "specific example", or "some examples" etc. means that the specific features, structures or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the embodiment of the present invention. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures or characteristics described may be combined in any one or more embodiments or examples in a suitable manner. In addition, those skilled in the art may combine and combine the different embodiments or examples described in this specification and the features of the different embodiments or examples, without contradiction.

[0034] Combine the following Figure 1 The present invention is further described in detail, and an underwater backtracking inertial navigation method, system and device based on information constraints of the present invention are described:

[0035] In this embodiment, Figure 1 As shown, an underwater backtracking inertial navigation method based on information constraints is provided, comprising the following steps:

[0036] Step 1: Obtain the initial attitude angle, initial speed, final speed, final position, angular velocity of the entire navigation process, and relative force of the entire navigation process of the carrier during underwater navigation.

[0037] The carrier of the present invention is mainly a small underwater robot or a vehicle. The carrier is only equipped with an inertial measurement unit and a GNSS receiver, or similar equipment. Therefore, only the initial moment attitude angle, the initial moment speed, the end moment speed, the end moment position, the angular velocity of the whole navigation process and the specific force of the whole navigation process can be collected, and the initial moment position and the end moment attitude angle are difficult to obtain. That is, the carrier is only equipped with equipment that can collect the initial moment attitude angle, the initial moment speed, the end moment speed, the end moment position, the angular velocity of the whole navigation process and the specific force of the whole navigation process.

[0038] In this embodiment, the underwater exploration navigation scenario is selected, and the underwater navigation process of the carrier is specifically as follows: at the initial moment, the carrier is located underwater and is in a stationary state, the carrier moves from underwater to the surface, and at the end moment, the carrier is located on the surface.

[0039] At the initial moment, the carrier is stationary, so the initial moment velocity is zero. The initial moment attitude angle is obtained by the inertial navigation initial alignment technology. The end moment velocity and end moment position are obtained by a GNSS receiver or a device with similar functions. The angular velocity of the whole navigation process is measured by the gyroscope on the carrier, and the specific force of the whole navigation process is measured by the accelerometer on the carrier.

[0040] Step 2: According to the initial attitude angle, initial speed, angular velocity of the navigation process and relative force of the navigation process, inertial navigation is performed to calculate the attitude angle of the navigation process.

[0041] Specifically, inertial navigation is carried out according to formula (1) to calculate the attitude angle of the entire navigation process.

[0042] (1)

[0043] in, Indicates navigation time, , Indicates the end time, during inertial navigation From 0 to , Represents the attitude matrix from the carrier coordinate system to the navigation coordinate system, express The first derivative of represents the angular velocity, express The antisymmetric matrix of represents the angular velocity of the navigation coordinate system relative to the inertial coordinate system, express The antisymmetric matrix of It represents the speed of the whole navigation process of the carrier in the navigation coordinate system. express The first derivative of Indicates comparison, represents the angular velocity of the navigation coordinate system caused by the rotation of the earth, It represents the angular velocity of rotation of the navigation coordinate system caused by the motion of the carrier, Represents the gravity component in the navigation coordinate system. Carrier coordinate system: b system, the carrier's center of gravity is the origin, and the three orthogonal coordinate axes point to the right, front, and top of the carrier respectively; Navigation coordinate system: n system, the carrier's center of gravity is the origin, and the three orthogonal coordinate axes point to the geographic east, north, and celestial direction respectively. Inertial coordinate system: i system, with the center of the earth as the origin, and the three orthogonal coordinate axes determined by the right-hand rule with the earth's vernal equinox and the rotation axis.

[0044] The attitude angle of the whole navigation process of the carrier is included in the attitude angle, which includes heading angle, pitch angle and roll angle. , represents the eastward speed, represents the north speed, represents the celestial velocity, T represents the matrix transpose, , Represents the acceleration due to gravity, which is a constant.

[0045] According to formula (2), we can calculate , and ,

[0046] (2)

[0047] in, is the Earth's rotation angular rate, Indicates the latitude of the carrier in the geographic coordinate system. Represents the average radius of the Earth. Since the exact latitude is unknown at this time, an approximate geographic latitude with errors can be used instead, and the impact on the calculation accuracy can be ignored.

[0048] It should be noted that in step 2, the speed calculation performed by formula (1) is only the speed of formula (2). Provide east and north velocity updates to complete the attitude solution. That is, although step 2 includes a velocity solution process, the ultimate goal is to carry out a forward attitude solution.

[0049] Step 3: According to the attitude angle, end-time speed and end-time position of the whole navigation process, reverse navigation is carried out to solve the speed of the whole navigation process and the position of the whole navigation process. So far, the attitude angle, speed and position of the whole navigation process of the carrier are calculated, and the inertial navigation of the carrier is realized. Using the attitude angle, speed and position of the whole navigation process of the carrier, the accurate position of the underwater target point to be explored can be determined.

[0050] Specifically, reverse navigation is carried out according to formula (3), and the speed and position of the whole navigation process are calculated.

[0051] (3)

[0052] In the reverse navigation process, is from to 0, Indicates the position of the entire navigation process, , Indicates the longitude of the carrier in the geographic coordinate system. Indicates the height of the carrier in the geographic coordinate system. express The first derivative of .

[0053] express The negative matrix of , express The negative matrix of .

[0054] express The negative matrix of express The negative matrix of , formula (2) is transformed into formula (4),

[0055] (4).

[0056] In summary, the carrier obtains the known initial attitude angle and velocity information underwater, and then the carrier keeps maneuvering until the navigation ends and floats to the surface to obtain the known speed and position information through the GNSS receiver. Based on this method, the attitude, velocity and position of the carrier in the whole navigation process can be solved, realizing underwater backtracking inertial navigation based on information constraints, and reducing the dependence of the navigation process on the underwater initial information.

[0057] This embodiment also provides an underwater backtracking inertial navigation system based on information constraints, and the technical solution adopted is as follows: comprising: a data acquisition module, an inertial navigation solution module and a reverse navigation solution module connected in sequence.

[0058] The data acquisition module is used to obtain the initial attitude angle, initial speed, end speed, end position, angular velocity of the entire navigation process and specific force of the entire navigation process of the carrier during the underwater navigation process.

[0059] The inertial navigation solution module is used to perform inertial navigation according to the initial attitude angle, the initial speed, the angular velocity of the navigation process and the relative force of the navigation process, and solve the attitude angle of the navigation process.

[0060] The reverse navigation solution module is used to carry out reverse navigation according to the attitude angle, end time speed and end time position of the whole navigation process, and solve the speed and position of the whole navigation process.

[0061] This embodiment also provides a computer device, including a memory and a processor, wherein the memory stores a computer program, and the processor implements the above-mentioned underwater backtracking inertial navigation method based on information constraints when executing the computer program.

[0062] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. An underwater backtracking inertial navigation method based on information constraints, characterized in that: include: Obtaining the initial attitude angle, initial speed, final speed, final position, angular velocity of the entire navigation process, and specific force of the entire navigation process of the carrier during underwater navigation; According to the initial attitude angle, the initial speed, the angular velocity of the navigation process and the relative force of the navigation process, inertial navigation is performed to calculate the attitude angle of the navigation process; According to formula (1), inertial navigation is carried out to calculate the attitude angle of the whole navigation process. （1） in, Indicates navigation time, , Indicates the end time, during inertial navigation From 0 to , Represents the attitude matrix from the carrier coordinate system to the navigation coordinate system, express The first derivative of represents the angular velocity, express The antisymmetric matrix of represents the angular velocity of the navigation coordinate system relative to the inertial coordinate system, express The antisymmetric matrix of It represents the speed of the whole navigation process of the carrier in the navigation coordinate system. express The first derivative of Indicates comparison, represents the angular velocity of the navigation coordinate system caused by the rotation of the earth, It represents the angular velocity of rotation of the navigation coordinate system caused by the motion of the carrier, Represents the gravity component in the navigation coordinate system; According to the attitude angle, the end time speed and the end time position of the navigation process, reverse navigation is carried out to calculate the speed of the navigation process and the position of the navigation process; The carrier is only equipped with equipment that can collect the attitude angle at the initial moment, the speed at the initial moment, the speed at the end moment, the position at the end moment, the angular velocity of the entire navigation process and the relative force of the entire navigation process; under the conditions that the attitude angle and speed of the carrier at the initial moment are known and the position is unknown, and the speed and position at the end moment are known and the attitude angle is unknown, the whole process of inertial navigation is realized.

2. The underwater backtracking inertial navigation method based on information constraints according to claim 1, characterized in that: It contains the attitude angle of the carrier during the entire navigation process.

3. The underwater backtracking inertial navigation method based on information constraints as claimed in claim 2 is characterized in that , represents the eastward speed, represents the north speed, represents the celestial velocity, T represents the matrix transpose, , represents the acceleration due to gravity, （2） is the Earth's rotation angular rate, Indicates the latitude of the carrier in the geographic coordinate system. Represents the average radius of the Earth.

4. The underwater backtracking inertial navigation method based on information constraints as claimed in claim 3 is characterized in that: According to formula (3), reverse navigation is carried out to calculate the speed and position of the whole navigation process. （3） in, Indicates the position of the entire navigation process, , Indicates the longitude of the carrier in the geographic coordinate system. Indicates the height of the carrier in the geographic coordinate system. express The first derivative of express The negative matrix of express The negative matrix of express The negative matrix of express The negative matrix of .

5. The underwater backtracking inertial navigation method based on information constraints according to claim 1, characterized in that: The initial moment attitude angle is obtained by an inertial navigation initial alignment technology; at the initial moment, the carrier is stationary, and the initial moment speed is zero.

6. The underwater backtracking inertial navigation method based on information constraints according to claim 1, characterized in that: The end time velocity and the end time position are obtained through a GNSS receiver.

7. The underwater backtracking inertial navigation method based on information constraints according to claim 1, characterized in that: The angular velocity of the entire navigation process is measured by the gyroscope on the carrier, and the specific force of the entire navigation process is measured by the accelerometer on the carrier.

8. An underwater backtracking inertial navigation system based on information constraints, characterized in that: Used to execute an underwater backtracking inertial navigation method based on information constraints as described in any one of claims 1 to 7, comprising: a data acquisition module, an inertial navigation solution module and a reverse navigation solution module connected in sequence; A data acquisition module is used to obtain the initial attitude angle, initial speed, end speed, end position, angular velocity of the whole navigation process and specific force of the whole navigation process of the carrier during the underwater navigation process; An inertial navigation solution module is used to perform inertial navigation according to the initial attitude angle, the initial speed, the angular velocity of the navigation process and the specific force of the navigation process, and solve the attitude angle of the navigation process; According to formula (1), inertial navigation is carried out to calculate the attitude angle of the whole navigation process. （1） in, Indicates navigation time, , Indicates the end time, during inertial navigation From 0 to , Represents the attitude matrix from the carrier coordinate system to the navigation coordinate system, express The first derivative of represents the angular velocity, express The antisymmetric matrix of represents the angular velocity of the navigation coordinate system relative to the inertial coordinate system, express The antisymmetric matrix of It represents the speed of the whole navigation process of the carrier in the navigation coordinate system. express The first derivative of Indicates comparison, represents the angular velocity of the navigation coordinate system caused by the rotation of the earth, It represents the angular velocity of rotation of the navigation coordinate system caused by the motion of the carrier, Represents the gravity component in the navigation coordinate system; A reverse navigation solution module is used to carry out reverse navigation according to the attitude angle, the speed at the end time and the position at the end time of the whole navigation process, and solve the speed of the whole navigation process and the position of the whole navigation process; The carrier is only equipped with equipment that can collect the attitude angle at the initial moment, the speed at the initial moment, the speed at the end moment, the position at the end moment, the angular velocity of the entire navigation process and the relative force of the entire navigation process; under the conditions that the attitude angle and speed of the carrier at the initial moment are known and the position is unknown, and the speed and position at the end moment are known and the attitude angle is unknown, the whole process of inertial navigation is realized.

9. A computer device comprising a memory and a processor, wherein the memory stores a computer program, wherein: When the processor executes the computer program, an underwater backtracking inertial navigation method based on information constraints as described in any one of claims 1 to 7 is implemented.

Images