A tight coupling navigation method, system, computer device and storage medium

Abstract

The application discloses a kind of tight coupling navigation method, system, computer device and storage medium.The application calculates inertial factor residual, visual factor residual and global factor residual according to the inertial factor and visual factor obtained from inertial navigation system and the global factor obtained from long baseline positioning system, and constructs factor graph, adopts the tight coupling mode based on sliding window to fuse the inertial factor, visual factor and global factor in factor graph, constructs tight coupling model;According to inertial factor residual, visual factor residual and global factor residual, the tight coupling model is optimized, and the positioning result of the underwater autonomous vehicle is obtained.The application introduces global positioning information for underwater visual inertial navigation system, and tightly couples the inertial factor, visual factor and global factor of underwater autonomous vehicle, realizes the correction from local positioning trajectory to global positioning trajectory, and improves the precision and robustness of underwater autonomous vehicle positioning system.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of underwater autonomous vehicle positioning, and in particular to a tight coupling navigation method and system, a computer device and a storage medium. BACKGROUND

[0002] In the field of underwater autonomous vehicle positioning, the most widely used are visual inertial navigation system (VINS) and long baseline positioning system (LBL). The two types of sensors are complementary in advantages, and can realize high-precision autonomous navigation without relying on the external environment, but cannot provide global position information. In a more complex underwater environment, the availability of sensors is reduced. Long baseline positioning system (LBL) uses buoys on the sea surface to provide global positioning information to AUV through underwater sound propagation. Compared with electromagnetic waves, sound waves propagate more stably in underwater medium, improving positioning accuracy and navigation system stability. LBL is usually combined with inertial navigation system, DVL, etc. There is currently no effective solution to the fusion of VINS and LBL.

[0003] The traditional VINS system is highly dependent on the historical pose during underwater positioning without global positioning information, and requires a very accurate initial pose. In addition, in a large-scale underwater navigation scenario, the cumulative error cannot be eliminated due to the inability to form a closed loop. SUMMARY

[0004] The present application provides a tight coupling navigation method, system, computer device and readable storage medium to solve the technical problems of the existing visual inertial navigation system without global information, low underwater positioning accuracy, and lack of effective solution to the fusion of VINS and LBL, thereby improving the positioning accuracy of underwater autonomous vehicles and the stability of the navigation system.

[0005] To solve the above technical problems, in a first aspect, the present application embodiment provides a tight coupling navigation method applied to an underwater autonomous vehicle, the method comprising:

[0006] obtaining inertial factors and visual factors of a visual inertial navigation system, and calculating inertial factor residuals and visual factor residuals according to the inertial factors and visual factors respectively, the inertial factors comprising inertial state information of an IMU in the visual inertial navigation system, and the visual factors comprising inverse depths of image feature points;

[0007] obtaining global factors of a long baseline positioning system, and calculating global factor residuals according to the global factors, the global factors comprising slant ranges of the underwater autonomous vehicle and transponders in the long baseline positioning system;

[0008] constructing a factor graph according to the inertial factors, the visual factors and the global factors, and fusing the inertial factors, the visual factors and the global factors in the factor graph by using a sliding window based tightly coupled manner to construct a tightly coupled model;

[0009] optimizing the tightly coupled model according to the inertial factor residuals, the visual factor residuals and the global factor residuals to obtain the positioning result of the underwater autonomous vehicle.

[0010] In further embodiments, the constructing a factor graph according to the inertial factors, the visual factors and the global factors comprises:

[0011] taking the pose of the underwater autonomous vehicle in a world coordinate system as a node of the factor graph, the pose comprising a position and an orientation;

[0012] taking the inertial factors as first local constraints between two consecutive nodes of the factor graph;

[0013] taking the visual factors as second local constraints of each node of the factor graph, and taking the global factors as global constraints of each node of the factor graph.

[0014] In further embodiments, the fusing the inertial factors, the visual factors and the global factors in the factor graph by using a sliding window based tightly coupled manner to construct a tightly coupled model comprises:

[0015] obtaining a first relative pose of a camera coordinate system to a vehicle coordinate system in the visual-inertial navigation system;

[0016] obtaining a local positioning trajectory and a global positioning trajectory of the underwater autonomous vehicle by the visual-inertial navigation system and the long baseline positioning system respectively, aligning the local positioning trajectory and the global positioning trajectory to obtain a rotation matrix and a translation vector of the vehicle coordinate system to an ENU coordinate system, and calculating a second relative pose of the vehicle coordinate system to the ENU coordinate system according to the rotation matrix and the translation vector;

[0017] integrating the inertial factors, the visual factors and the global factors in the sliding window into a state vector based on the first relative pose and the second relative pose;

[0018] determining a sliding window time period, and constructing a tightly coupled model according to the state vectors in the sliding window time period.

[0019] In further embodiments, the aligning the local positioning trajectory and the global positioning trajectory to obtain a rotation matrix and a translation vector of the vehicle coordinate system to an ENU coordinate system comprises:

[0020] selecting a position point of the global positioning trajectory as a reference point, calculating global positioning information of the underwater autonomous vehicle corresponding to the reference point, taking the reference point as the origin of the ENU coordinate system;

[0021] Converting the global positioning information corresponding to the global positioning trajectory from the ECEF coordinate system to the ENU coordinate system to obtain the global positioning information of the underwater autonomous vehicle in the ENU coordinate system;

[0022] aligning the global positioning information of the underwater autonomous vehicle in the ENU coordinate system and the timestamp of the local positioning trajectory by using the interpolation method, to obtain the rotation matrix and translation vector of the carrier coordinate system to the ENU coordinate system in the visual inertial navigation system.

[0023] In further embodiments, the tight coupling model is optimized according to the inertial factor residual, the visual factor residual and the global factor residual to obtain the positioning result of the underwater autonomous vehicle, including:

[0024] Combining the inertial factor residual, the visual factor residual and the global factor residual into a sliding window to obtain a cost function of the tight coupling model;

[0025] Calculating the state vector that minimizes the cost function within the sliding window time period, and taking the calculated state vector as the positioning result of the underwater autonomous vehicle.

[0026] In further embodiments, the state vector is:

[0027]

[0028] wherein, represents the inertial state information of the IMU corresponding to the kth time; n represents the total number of inertial state information of the IMU within the sliding window time period; represents the first relative pose; represents the second relative pose; represents the inverse depth of the image feature point; m represents the number of feature points within the sliding window time period; represents the slant range between the underwater autonomous vehicle and the transponder; represents the number of received slant range information within the sliding window time period; , , respectively represent the position information, velocity information and rotation information of the visual inertial navigation system corresponding to the kth time; , represents the zero offset; represents the rotation matrix of the camera coordinate system to the carrier coordinate system in the visual inertial navigation system; denotes a translation vector from a camera coordinate system to a body coordinate system in a visual-inertial navigation system; denotes a rotation matrix from a body coordinate system to an ENU coordinate system in a visual-inertial navigation system; denotes a translation vector from a body coordinate system to an ENU coordinate system in a visual-inertial navigation system;

[0029] The cost function is:

[0030]

[0031] wherein, denotes an inertial factor residual error; denotes a visual factor residual error; denotes a global factor residual error; denotes an inertial factor measurement value at a time instant; denotes a visual factor measurement value at a time instant; denotes a global factor measurement value at a time instant.

[0032] In a second aspect, an embodiment of the present application provides a tightly coupled navigation system applied to an underwater autonomous vehicle, the system comprising:

[0033] a local positioning information acquisition unit configured to acquire an inertial factor and a visual factor of a visual-inertial navigation system, and to calculate an inertial factor residual error and a visual factor residual error according to the inertial factor and the visual factor respectively, the inertial factor comprising inertial state information of an IMU in the visual-inertial navigation system, and the visual factor comprising inverse depth of an image feature point;

[0034] a global positioning information acquisition unit configured to acquire a global factor of a long-baseline positioning system, and to calculate a global factor residual error according to the global factor, the global factor comprising slant range of the underwater autonomous vehicle to a transponder in the long-baseline positioning system;

[0035] a tightly coupled model construction unit configured to construct a factor graph according to the inertial factor, the visual factor and the global factor, and to fuse the inertial factor, the visual factor and the global factor in the factor graph in a sliding window based tightly coupled manner to construct a tightly coupled model;

[0036] a calculation optimization unit configured to optimize the tightly coupled model according to the inertial factor residual error, the visual factor residual error and the global factor residual error to obtain a positioning result of the underwater autonomous vehicle.

[0037] In a third aspect, the embodiments of the present application further provide a computer device, characterized by comprising a memory, a processor and a transceiver, which are connected through a bus; the memory is used for storing a set of computer program instructions and data, and can transmit the stored data to the processor; the processor can execute the program instructions stored in the memory to execute the method described above.

[0038] In a fourth aspect, the embodiments of the present application further provide a computer readable storage medium, characterized by storing a computer program, when the computer program is executed, the method described above is implemented.

[0039] The embodiments of the present application provide a tightly coupled navigation method, system, computer device and storage medium. According to the inertial factor and visual factor obtained from the inertial navigation system and the global factor obtained from the long baseline positioning system, the embodiments of the present application calculate the inertial factor residual, the visual factor residual and the global factor residual, and construct a factor graph. The embodiments of the present application fuse the inertial factor, the visual factor and the global factor in the factor graph in a tightly coupled manner based on a sliding window, and construct a tightly coupled model. According to the inertial factor residual, the visual factor residual and the global factor residual, the embodiments of the present application optimize the tightly coupled model, and obtain the positioning result of the underwater autonomous vehicle. The embodiments of the present application introduce global positioning information for an underwater visual inertial navigation system, tightly couple the inertial factor, the visual factor and the global factor of the underwater autonomous vehicle, realize correction from a local positioning trajectory to a global positioning trajectory, and improve the precision and robustness of the positioning system of the underwater autonomous vehicle. BRIEF DESCRIPTION OF DRAWINGS

[0040] Figure 1 is a tightly coupled navigation method step schematic diagram provided by the embodiments of the present application;

[0041] Figure 2 is a positioning geometric diagram of the underwater autonomous vehicle and the buoy point at adjacent time provided by the embodiments of the present application;

[0042] Figure 3 is a factor graph provided by the embodiments of the present application;

[0043] Figure 4 is a factor graph construction method step schematic diagram provided by the embodiments of the present application;

[0044] Figure 5 is a tightly coupled model construction method step schematic diagram provided by the embodiments of the present application;

[0045] Figure 6 is a rotation matrix and translation vector calculation method step schematic diagram of the carrier coordinate system to the ENU coordinate system in the visual inertial navigation system provided by the embodiments of the present application;

[0046] Figure 7 is a camera coordinate system, a carrier coordinate system and a long baseline positioning system coordinate system schematic diagram in a visual-inertial navigation system provided by an embodiment of the application;

[0047] Figure 8 is a step schematic diagram of a tight coupling navigation method S4 provided by an embodiment of the application;

[0048] Figure 9 is a tight coupling navigation system structure schematic diagram provided by an embodiment of the application;

[0049] Figure 10 is a computer device schematic diagram provided by an embodiment of the application. DETAILED DESCRIPTION

[0050] The embodiments of the application will be described in detail below with reference to the drawings. The embodiments are given only for illustrative purposes, and cannot be understood as limiting the application. The accompanying drawings are only for reference and illustration, and do not constitute a limitation on the scope of patent protection of the application. Based on the embodiments in the application, all other embodiments obtained by those of ordinary skill in the art without creative labor fall within the scope of protection of the application.

[0051] Please refer to Figure 1 In an embodiment of the application, a tight coupling navigation method includes the following steps S1-S4:

[0052] S1, obtain an inertial factor and a visual factor of a visual-inertial navigation system, and calculate an inertial factor residual and a visual factor residual according to the inertial factor and the visual factor respectively, the inertial factor includes inertial state information of an IMU in the visual-inertial navigation system, and the visual factor includes inverse depth of an image feature point.

[0053] A visual-inertial navigation system (VINS, visual-inertial system) is a navigation system that fuses camera and IMU sensor data to realize SLAM algorithm. The IMU sensor is a combination of accelerometer and gyroscope sensor, which is used to detect acceleration and angular velocity to represent motion and motion intensity.

[0054] The measured value of the inertial factor includes the bias of the platform, the noise linear acceleration and the angular velocity. Since the accelerometer works near the earth's surface, the linear acceleration measurement also contains the gravity component. Considering the noise measurement of the low-cost IMU sensor, the Coriolis force and centrifugal force caused by the earth's rotation are ignored in the formula of the IMU sensor. Therefore, the inertial measurement can be modeled as:

[0055] ,

[0056]

[0057] where, represents the acceleration output by the IMU sensor at time t; represents the angular velocity output by the IMU sensor at time t; represents the linear acceleration of the platform in the IMU sensor frame; represents the angular velocity of the platform in the IMU sensor frame; represents the attitude transformation matrix from the world coordinate system to the IMU coordinate system; represents the gravitational acceleration in the world coordinate system; , represents the additive noise in the accelerometer and gyroscope; represents the bias for the accelerometer and gyroscope.

[0058] Further, represents the output of the IMU sensor at time t, represents the linear acceleration and angular velocity of the platform in the IMU sensor frame. Assuming additive noise and both exhibit zero-mean Gaussian distribution, as , then the slowly varying bias related to the accelerometer and gyroscope is modeled as a random walk as follows:

[0059]

[0060] where, , .

[0061] Integrate the inertial measurement values over the time interval :

[0062]

[0063] where,

[0064]

[0065] includes relative position information, velocity information, and pose information.

[0066] The final inertial factor residual can be represented as:

[0067]

[0068] where, represents the carrier coordinate system; represents the relative position error; represents the velocity error; represents the relative rotation error of the three-dimensional Euclidean space; is the acceleration bias error; is the gravity bias error; represents is the projection of the system displacement at time t in the world coordinate system; represents is the projection of the system velocity at time t in the world coordinate system; , represents the measurement of the , is the integral position information and velocity information estimate value; returns the quaternion imaginary part; is a quaternion, representing the rotation from the IMU coordinate system to the world coordinate system; represents the multiplication between two quaternions; represents the underwater autonomous vehicle inertial factor measurement value, represents the estimated state of the underwater autonomous vehicle. In the embodiments of the present application, the represents the estimated state of the underwater autonomous vehicle.

[0069] In the embodiments of the present application, the visual measurement value is a set of sparse feature points extracted from the image frame, the corner points in the image are detected as feature points, and further an iterative sparse optical flow method is used for tracking. After distortion correction of the feature points, the projection process can be modeled as:

[0070]

[0071] wherein, represents the coordinates of the feature point in the image coordinate system; represents the three-dimensional coordinates of the feature point in the world coordinate system; and respectively represent the rotation matrix and the translation vector of the carrier coordinate system to the camera coordinate system; and respectively represent the rotation matrix and the translation vector of the world coordinate system to the carrier coordinate system; the projection equation of the camera coordinate system to the image coordinate system is a known quantity for each camera; represents the measurement noise.

[0072] For the feature point with inverse depth in the i-th frame, the feature point is observed again in the j-th frame, then the residual error of the two frames of images, the visual factor residual error, can be represented as:

[0073]

[0074] wherein, represents the position of the feature point r in the image coordinate at the jth frame time; is the inverse depth of the feature point r; represents a re-projection process; represents the 3D position of the feature point r in the world coordinate system at the jth frame time; estimated at the jth frame time; estimated at the jth frame time; , represents the rotation matrix and the translation vector of the ith frame image from the carrier coordinate system to the world coordinate system; , represents the rotation matrix and the translation vector of the camera coordinate system to the carrier coordinate system.

[0075] S2, obtaining a global factor of the long baseline positioning system, calculating a global factor residual according to the global factor, the global factor including the slant range of the underwater autonomous vehicle and the transponder in the long baseline positioning system.

[0076] The long baseline positioning system includes two parts, one part is a transceiver installed on a ship or an underwater autonomous vehicle, and the other part is a series of transponders with known positions fixed on the seabed, at least three.

[0077] In the embodiments of the present application, it is assumed that the position of the underwater autonomous vehicle is , and the positions of the four transponders on the seabed are wherein, , let the first transponder be the reference transponder. According to the position of the underwater autonomous vehicle calculated by the IMU sensor, the slant range of the underwater autonomous vehicle and the transponder is calculated, and the slant range of the other three transponders is subtracted from the slant range of the reference transponder to obtain the slant range difference based on the IMU sensor wherein, i=(1, 2, 3), denoted as an external observation value, that is:

[0078]

[0079] The external observation value is Taylor linearized relative to the true position of the underwater autonomous vehicle to obtain the measurement value of the slant range difference of the long baseline positioning system:

[0080] ,

[0081] ,

[0082] ,

[0083] .

[0084] The slant range difference of the above long baseline positioning system can also be obtained by inversely calculating the position of the buoy point combined with the positions of the underwater autonomous vehicle at the two time points, to obtain the estimated value of the slant range difference of the long baseline positioning system.

[0085] As shown in Figure 2 , the position of the buoy point is represented by , wherein , the position of the underwater autonomous vehicle at the previous time point is represented by , and A represents the position of the underwater autonomous vehicle at the current time point. Let OA be , AB be , and OB be . The angle between can be obtained from the motion posture of the underwater autonomous vehicle, and is represented by . Combined with the triangular relationship, we can obtain the slant range between the current position of the underwater autonomous vehicle and the buoy point:

[0086]

[0087] wherein

[0088] ,

[0089] ,

[0090] The slant range differences between the remaining three buoy points and the reference buoy point are calculated, and the estimated value of the slant range difference of the long baseline positioning system is obtained :

[0091]

[0092] wherein =1, 2, 3, represents the slant range between the current position of the underwater autonomous vehicle and the reference buoy point .

[0093] Therefore, from the measured value and the estimated value of the slant range difference, we can obtain the residual error of the LBL factor as:

[0094] .

[0095] S3, constructing a factor graph according to the inertial factor, the visual factor and the global factor, and fusing the inertial factor, the visual factor and the global factor in the factor graph in a tight coupling mode based on a sliding window to construct a tight coupling model.

[0096] In the embodiment of the present application, under the probabilistic model, the global factor of the underwater autonomous vehicle obtained from the long baseline positioning system, the inertial factor of the underwater autonomous vehicle calculated from the visual inertial navigation system and the visual factor are used to construct a factor graph, as shown in Figure 3 The factor graph provided by the embodiment of the present application is shown.

[0097] The method for constructing the factor graph includes the following steps, as shown in Figure 4

[0098] S301, taking the pose of the underwater autonomous vehicle in the world coordinate system as a node of the factor graph, the pose including position and direction.

[0099] S302, taking the inertial factor as a first local constraint between two consecutive nodes of the factor graph.

[0100] S303, taking the visual factor as a second local constraint of each node of the factor graph, and taking the global factor as a global constraint of each node of the factor graph.

[0101] In the embodiment of the present application, the factor graph takes the visual factor and the inertial factor obtained from the visual inertial navigation system as local constraints, and takes the global factor obtained from the long baseline positioning system as a global constraint. In the initialization stage, the correction from the local trajectory to the global trajectory is realized, and other factors can also be added as constraints.

[0102] In the embodiment of the present application, after the factor graph is constructed, the inertial factor, the visual factor and the global factor in the factor graph are fused in a tight coupling manner of a sliding window to construct a tight coupling model, as shown in Figure 5 The method for constructing the tight coupling model includes the following steps:

[0103] S304, obtaining a first relative pose of the camera coordinate system to the carrier coordinate system in the visual inertial navigation system.

[0104] S305, obtaining a local positioning trajectory and a global positioning trajectory of the underwater autonomous vehicle through the visual inertial navigation system and the long baseline positioning system respectively, aligning the local positioning trajectory and the global positioning trajectory, obtaining a rotation matrix and a translation vector of the carrier coordinate system to the ENU coordinate system in the visual inertial navigation system, and calculating a second relative pose of the carrier coordinate system to the ENU coordinate system according to the rotation matrix and the translation vector.

[0105] In the embodiment of the present application, the local positioning trajectory and the global positioning trajectory of the underwater autonomous vehicle obtained through the visual inertial navigation system and the long baseline positioning system respectively need to be aligned to calculate the rotation matrix ​translation vector As shown in Figure 6 the specific calculation method comprises the following steps:

[0106] S3051, select a position point of the global positioning trajectory as a reference point, calculate the global positioning information of the underwater autonomous vehicle corresponding to the reference point, and take the reference point as the origin of the ENU coordinate system.

[0107] S3052, convert the global positioning information corresponding to the global positioning trajectory from the ECEF coordinate system to the ENU coordinate system, and obtain the global positioning information of the underwater autonomous vehicle in the ENU coordinate system.

[0108] S3053, align the global positioning information of the underwater autonomous vehicle in the ENU coordinate system and the timestamps of the positioning information of the local positioning trajectory by using an interpolation method, to obtain a rotation matrix and a translation vector of the carrier coordinate system to the ENU coordinate system in the visual inertial navigation system.

[0109] In the embodiment of the application, ECEF coordinate system, ENU coordinate system, carrier coordinate system. As shown in Figure 7 , , and respectively represent the camera coordinate system, the carrier coordinate system and the long baseline positioning system coordinate system in the visual inertial navigation system. The long baseline positioning system uses the ECEF coordinate system.

[0110] First, select a position point from the global positioning trajectory obtained from the long baseline positioning system as a reference point The positioning information of the underwater autonomous vehicle corresponding to the reference point can be calculated by the long baseline positioning system, and the reference point is taken as the origin of the ENU coordinate system. The reference point can be selected as the positioning point after the long baseline positioning system is started for a certain time.

[0111] After the origin is determined, the ENU coordinate system is established, and the positioning information of the global positioning trajectory obtained from the long baseline positioning system is converted from the ECEF coordinate system used by the long baseline positioning system to the ENU coordinate system. The conversion formula is:

[0112] .

[0113] At this time, the underwater autonomous vehicle has two sets of trajectory positioning information, the global trajectory positioning information in the ENU coordinate system obtained by the long baseline positioning system , and the local trajectory positioning information obtained by the visual inertial navigation system The timestamps of the two sets of trajectory positioning information are aligned by using an interpolation method, and alignment calculation is performed:

[0114]

[0115] wherein s represents a scale factor.

[0116] Thus, the rotation matrix and the translation vector of the carrier coordinate system to the ENU coordinate system in the visual inertial navigation system are obtained, and alignment of the two trajectories is realized.

[0117] The second relative pose of the carrier coordinate system to the ENU coordinate system is calculated according to the rotation matrix and the translation vector.

[0118] S306, based on the first relative pose and the second relative pose, integrating the inertial factor, the visual factor and the global factor in the sliding window into a state vector.

[0119] The inertial factor data, the visual factor data and the global factor data are fused to realize estimation of the state of the underwater autonomous vehicle. In the embodiment of the present application, based on the first relative pose and the second relative pose, the inertial factor, the visual factor and the global factor information are fused by using a tight coupling method based on a sliding window, that is, the inertial factor, the visual factor and the global factor information in the sliding window are integrated into a state vector, which is represented by the following formula:

[0120]

[0121] wherein, represents the inertial state information of the IMU corresponding to the kth moment; n represents the total number of inertial state information of the IMU in the sliding window time period; represents the first relative pose; represents the second relative pose; represents the inverse depth of the image feature point; m represents the number of feature points in the sliding window time period; represents the slant range of the underwater autonomous vehicle and the transponder; represents the number of received slant range information in the sliding window time period; , , respectively represent the position information, the velocity information and the rotation information of the visual inertial navigation system corresponding to the kth moment; , represents a zero offset; represents the rotation matrix of the camera coordinate system to the carrier coordinate system; represents the translation vector of the camera coordinate system to the carrier coordinate system; represents the rotation matrix of the carrier coordinate system to the ENU coordinate system in the visual inertial navigation system; This represents the translation vector from the vehicle coordinate system to the ENU coordinate system in a visual inertial navigation system.

[0122] S307. Determine the sliding window time period, and construct a tightly coupled model based on the state vector within the sliding window time period.

[0123] The sliding window time period is determined to determine the total number of inertial state information of the IMU, the number of feature points, and the number of received slant range information within the sliding window time period, and a tightly coupled model is constructed based on the state vector within the sliding window time period.

[0124] S4. Optimize the tightly coupled model based on the inertial factor residual, visual factor residual, and global factor residual to obtain the positioning result of the underwater autonomous vehicle.

[0125] In embodiments of the present invention, such as Figure 8 As shown, step S4 includes:

[0126] S401. The inertial factor residual, visual factor residual, and global factor residual are merged into a sliding window to obtain the cost function of the tightly coupled model.

[0127] S402. Calculate the state vector that minimizes the cost function within the sliding window time period, and use the calculated state vector as the positioning result of the underwater autonomous vehicle.

[0128] The inertial factor residual, visual factor residual, and global factor residual are combined into a single sliding window to obtain the cost function of the tightly coupled model. The cost function formula is as follows:

[0129]

[0130] in, Represents the residual of the inertia factor; Represents the visual factor residual; Represents the global factor residual; express Measurement of the inertia factor at any given time; express Time-based visual factor measurement values; express Global factor measurement at any given time.

[0131] Based on the sliding window time period determined above, the state vector corresponding to minimizing the cost function within the sliding window time period is determined. This state vector is the output result of the tightly coupled model, and the output result is used as the positioning result of the underwater autonomous vehicle.

[0132] In the embodiment of the present application, in order to solve the technical problems that the existing visual inertial navigation system does not have global information, the underwater positioning accuracy is not high, and there is a lack of effective scheme for fusing VINS and LBL, a tightly coupled navigation method is provided. The present application first tightly couples visual inertial to obtain local positioning information and local trajectory of the underwater autonomous vehicle, then introduces the slant range information of the long baseline positioning system to obtain the global positioning information of several key points, and preliminarily corrects the local trajectory. Then, according to the inertial factor and visual factor obtained from the inertial navigation system and the global factor obtained from the long baseline positioning system, the inertial factor residual, the visual factor residual and the global factor residual are calculated, and a factor graph is constructed. The inertial factor, the visual factor and the global factor in the factor graph are fused in a tightly coupled manner based on a sliding window to construct a tightly coupled model. The tightly coupled model is optimized according to the inertial factor residual, the visual factor residual and the global factor residual, and the positioning result of the underwater autonomous vehicle is obtained. The present application introduces global positioning information into the underwater visual inertial navigation system, tightly couples the inertial factor, the visual factor and the global factor of the underwater autonomous vehicle, realizes the correction from the local positioning trajectory to the global positioning trajectory, and improves the accuracy and robustness of the underwater autonomous vehicle positioning system.

[0133] Correspondingly, as shown in Figure 9 , based on the tightly coupled navigation method, the embodiment of the present application further provides a tightly coupled navigation system, which comprises:

[0134] A local positioning information acquisition unit 1 is configured to acquire the inertial factor and the visual factor of the visual inertial navigation system, and calculate the inertial factor residual and the visual factor residual according to the inertial factor and the visual factor respectively. The inertial factor comprises the inertial state information of the IMU in the visual inertial navigation system, and the visual factor comprises the inverse depth of the image feature point.

[0135] A global positioning information acquisition unit 2 is configured to acquire the global factor of the long baseline positioning system, and calculate the global factor residual according to the global factor. The global factor comprises the slant range between the underwater autonomous vehicle and the transponder in the long baseline positioning system.

[0136] A tightly coupled model construction unit 3 is configured to construct a factor graph according to the inertial factor, the visual factor and the global factor, and fuse the inertial factor, the visual factor and the global factor in the factor graph in a tightly coupled manner based on a sliding window to construct a tightly coupled model.

[0137] An optimization calculation unit 4 is configured to optimize the tightly coupled model according to the inertial factor residual, the visual factor residual and the global factor residual, and obtain the positioning result of the underwater autonomous vehicle.

[0138] The specific definition of the tight coupling navigation system can refer to the definition of the tight coupling navigation method, which will not be repeated here. Those skilled in the art can realize that various modules and steps described in combination with the embodiments disclosed in the present application can be realized in hardware, software or combination of both. Whether the functions are realized in hardware or software depends on the specific application and design constraints of the technical solution. The skilled person can use different methods to realize the described functions for each specific application, but such implementation should not be considered beyond the scope of the present application.

[0139] As shown in Figure 10 The computer device provided by the embodiment of the present application includes a memory, a processor and a transceiver, which are connected through a bus; the memory is used to store a set of computer program instructions and data, and can transmit the stored data to the processor; the processor can execute the program instructions stored in the memory to perform the steps of the above-mentioned tight coupling navigation method.

[0140] The memory can include a volatile memory or a non-volatile memory, or can include both volatile and non-volatile memories; the processor can be a central processing unit, a microprocessor, a specific application integrated circuit, a programmable logic device or a combination thereof. By way of example but not limitation, the above-mentioned programmable logic device can be a complex programmable logic device, a field programmable logic gate array, a general array logic or any combination thereof.

[0141] In addition, the memory can be a physically independent unit, or can be integrated with the processor.

[0142] Those skilled in the art can understand that Figure 10 The structure shown in the figure is only a block diagram of part of the structure related to the scheme of the present application, and does not constitute a limitation on the computer device to which the scheme of the present application is applied. The specific computer device can include more or less components than those shown in the figure, or combine certain components, or have the same component arrangement.

[0143] In one embodiment, a computer readable storage medium is provided for storing one or more computer programs, the one or more computer programs including program codes for executing the above-mentioned tight coupling navigation method when the computer programs are run on a computer.

[0144] In the above embodiments, all or part of the methods can be implemented by software, hardware, firmware, or any combination thereof. When implemented by software, all or part of the methods can be implemented in the form of a computer program product. The computer program product includes one or more computer instructions. When the computer program instructions are loaded and executed on a computer, all or part of the processes or functions described in the embodiments of the present application are generated. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable devices. The computer instructions can be stored in a computer-readable storage medium or transferred from one computer-readable storage medium to another computer-readable storage medium, for example, the computer instructions can be transferred from one website, computer, server, or data center to another website, computer, server, or data center through a wired (such as coaxial cable, optical fiber, digital subscriber line) or wireless (such as infrared, wireless, microwave, etc.) manner. The computer-readable storage medium can be any available medium that can be accessed by a computer or a data storage device such as a server, data center, etc. that includes one or more available media sets. The available media can be a magnetic medium (for example, a floppy disk, a hard disk, a magnetic tape), an optical medium (for example, a DVD), or a semiconductor medium (for example, an SSD), etc.

[0145] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiments can be completed by instructing related hardware through a computer program. The computer program can be stored in a computer-readable storage medium. When the computer program is executed, it can include the processes of the above-mentioned embodiments.

[0146] The method, system, computer device and storage medium for tight coupling navigation in the embodiment introduce global positioning information for the underwater visual inertial navigation system, and tightly couple the inertial factor, the visual factor and the global factor of the underwater autonomous underwater vehicle to realize the correction from the local positioning trajectory to the global positioning trajectory, and improve the accuracy and robustness of the positioning system of the underwater autonomous underwater vehicle.

[0147] The above embodiments only express several preferred embodiments of the present application, which are described in a more specific and detailed manner, but cannot be understood as a limitation to the patent scope of the application. It should be pointed out that, for ordinary skilled in the art, several improvements and replacements can be made without departing from the technical principles of the present application, and these improvements and replacements should also be considered as the protection scope of the present application. Therefore, the protection scope of the present application patent should be subject to the protection scope of the claims.

Claims

1. A close-coupled navigation method applied to an underwater autonomous vehicle, characterized in that, The method comprises: obtaining an inertial factor and a visual factor of a visual-inertial navigation system, and calculating an inertial factor residual and a visual factor residual according to the inertial factor and the visual factor respectively, the inertial factor comprising inertial state information of an IMU in the visual-inertial navigation system, and the visual factor comprising inverse depth of an image feature point; obtaining a global factor of a long-baseline positioning system, and calculating a global factor residual according to the global factor, the global factor comprising slant range of a transponder in the long-baseline positioning system and the underwater autonomous vehicle; constructing a factor graph according to the inertial factor, the visual factor and the global factor, and fusing the inertial factor, the visual factor and the global factor in the factor graph in a tight coupling mode based on a sliding window to construct a tight coupling model; optimizing the tight coupling model according to the inertial factor residual, the visual factor residual and the global factor residual to obtain a positioning result of the underwater autonomous vehicle.

2. The close-coupled navigation method of claim 1, wherein, The constructing of the factor graph according to the inertial factor, the visual factor and the global factor comprises: taking a pose of the underwater autonomous vehicle in a world coordinate system as a node of the factor graph, the pose comprising a position and a direction; taking the inertial factor as a first local constraint between two continuous nodes of the factor graph; taking the visual factor as a second local constraint of each node of the factor graph, and taking the global factor as a global constraint of each node of the factor graph.

3. The close-coupled navigation method of claim 2, wherein, The fusing of the inertial factor, the visual factor and the global factor in the factor graph in the tight coupling mode based on the sliding window to construct the tight coupling model comprises: obtaining a first relative pose of a camera coordinate system to a carrier coordinate system in the visual-inertial navigation system; obtaining a local positioning trajectory and a global positioning trajectory of the underwater autonomous vehicle through the visual-inertial navigation system and the long-baseline positioning system respectively, aligning the local positioning trajectory and the global positioning trajectory to obtain a rotation matrix and a translation vector of the carrier coordinate system to an ENU coordinate system in the visual-inertial navigation system, and calculating a second relative pose of the carrier coordinate system to the ENU coordinate system according to the rotation matrix and the translation vector; integrating the inertial factor, the visual factor and the global factor in the sliding window into a state vector based on the first relative pose and the second relative pose; determining a time period of the sliding window, and constructing a tight coupling model according to the state vector in the time period of the sliding window.

4. The close-coupled navigation method of claim 3, wherein, The aligning of the local positioning trajectory and the global positioning trajectory to obtain the rotation matrix and the translation vector of the carrier coordinate system to the ENU coordinate system in the visual-inertial navigation system comprises: selecting a position point of the global positioning trajectory as a reference point, calculating global positioning information of the underwater autonomous vehicle corresponding to the reference point, and taking the reference point as an origin of the ENU coordinate system; converting the global positioning information corresponding to the global positioning trajectory from an ECEF coordinate system to an ENU coordinate system to obtain global positioning information of the underwater autonomous vehicle in the ENU coordinate system; The global positioning information of the underwater autonomous vehicle in the ENU coordinate system and the timestamps of the local positioning trajectory are aligned by using an interpolation method to obtain a rotation matrix and a translation vector of the carrier coordinate system to the ENU coordinate system in the visual-inertial navigation system.

5. The close-coupled navigation method of claim 3, wherein, The tight coupling model is optimized according to the inertial factor residual error, the visual factor residual error and the global factor residual error, and a positioning result of the underwater autonomous vehicle is obtained. The inertial factor residual error, the visual factor residual error and the global factor residual error are combined into a sliding window to obtain a cost function of the tight coupling model. A state vector that minimizes the cost function in the sliding window time period is calculated, and the calculated state vector is taken as the positioning result of the underwater autonomous vehicle.

6. The close-coupled navigation method of claim 5, wherein, The state vector is: wherein x k represents the inertial state information of the IMU corresponding to the kth moment; n represents the total number of inertial state information of the IMU within the sliding window time period; represents the first relative pose; represents the second relative pose; λ represents the inverse depth of the image feature point; m represents the number of feature points within the sliding window time period; ρ represents the slant range between the underwater autonomous vehicle and the transponder; l represents the number of slant range information received within the sliding window time period; respectively represent the position information, velocity information and rotation information of the visual inertial navigation system corresponding to the kth moment; b a , b g represents the zero offset; represents the rotation matrix of the camera coordinate system to the carrier coordinate system in the visual inertial navigation system; represents the translation vector of the camera coordinate system to the carrier coordinate system in the visual inertial navigation system; represents the rotation matrix of the carrier coordinate system to the ENU coordinate system in the visual inertial navigation system; represents the translation vector of the carrier coordinate system to the ENU coordinate system in the visual inertial navigation system; The cost function is: wherein, represents an inertial factor residual error; r R represents a visual factor residual error; r L represents a global factor residual error; represents a kth inertial factor measurement value; represents a kth visual factor measurement value; represents a kth global factor measurement value.

7. A tightly coupled navigation system, based on a visual-inertial navigation system and a long-baseline positioning system, characterized in that, The system is applied to an underwater autonomous vehicle and includes: A local positioning information acquisition unit is configured to acquire inertial factors and visual factors of a visual-inertial navigation system, and calculate inertial factor residual errors and visual factor residual errors according to the inertial factors and the visual factors, respectively. The inertial factors include inertial state information of an IMU in the visual-inertial navigation system, and the visual factors include inverse depths of image feature points. A global positioning information acquisition unit is configured to acquire global factors of a long baseline positioning system, and calculate global factor residual errors according to the global factors. The global factors include slant ranges of a transponder in the long baseline positioning system and the underwater autonomous vehicle. A tight coupling model construction unit is configured to construct a factor graph according to the inertial factors, the visual factors and the global factors, and fuse the inertial factors, the visual factors and the global factors in the factor graph by using a sliding window-based tight coupling method to construct a tight coupling model. A calculation and optimization unit is configured to optimize the tight coupling model according to the inertial factor residual error, the visual factor residual error and the global factor residual error, and obtain a positioning result of the underwater autonomous vehicle.

8. A computer device, comprising: The computer readable storage medium stores a computer program, and when the computer program is executed, the method of any one of claims 1 to 6 is implemented.

9. A computer-readable storage medium, characterized in that: The computer readable storage medium stores a computer program, and when the computer program is executed, the method of any one of claims 1 to 6 is implemented.

Images