An unknown-pose vision beacon assisted underwater vehicle heading self-calibration method and system

Abstract

The application discloses a kind of unknown pose visual beacon assisted underwater vehicle heading self-calibration method and system.The method comprises: synchronously obtaining monocular image and inertial navigation data, solving visual relative pose based on global pose not pre-calibration, and the geometric parameter of coding label known fixed artificial visual beacon;When observing beacon for the first time continuously, according to the difference between the increment of visual relative heading angle and inertial navigation heading angle, the heading angle drift correction quantity is updated in combination with observation quality and motion state;The local pose of vehicle is recursively deduced using the corrected heading angle and velocity, and the stable beacon local pose is associated with identity label code and is stored;When observing beacon again, the map reference pose is kept unchanged, and long-term drift correction is carried out according to the difference between predicted relative heading angle and current visual relative heading angle.The application does not need to pre-calibrate beacon global pose, and can inhibit heading cumulative drift.

Description

Technical Field

[0001] This invention belongs to the field of navigation and vision-assisted inertial calibration technology for autonomous underwater vehicles (AUVs), specifically relating to a method and system for self-calibrating the heading of an AUV assisted by a visual beacon of unknown pose. The visual beacon referred to in this invention is an artificial visual beacon with known geometric dimensions and coded information. Background Technology

[0002] In marine surveys, seabed inspections, target searches, and long-term underwater operations, AUVs typically rely on inertial navigation modules to provide status information such as heading angle, velocity, and acceleration. Because the underwater environment cannot reliably receive satellite navigation signals, AUVs are prone to cumulative heading angle drift during long-term voyages. The heading angle refers to the yaw angle of the AUV around its local vertical axis, and this drift directly affects track maintenance, area coverage, path reproduction, and return-to-home accuracy.

[0003] In existing technologies, underwater heading angle correction typically relies on pre-measured acoustic base stations, external positioning systems, or fixed markers with known absolute bearings. These methods usually require complex underwater deployment and calibration processes, resulting in high deployment costs and maintenance difficulties when operating over large areas or in changing environmental conditions. If only inertial data is relied upon for recursion, heading angle drift will accumulate continuously after prolonged navigation, inspection, or operation, leading to significant heading angle errors when the AUV re-enters the target area or returns to the beacon deployment area.

[0004] Underwater visual beacons offer advantages such as simple structure, low cost, and ease of encoding and recognition, making them suitable as references for underwater local heading calibration. However, in actual deployment, the underwater position and orientation of artificial visual beacons are often difficult to measure accurately. Requiring the global x and y coordinates and yaw angle of each beacon to be known in advance significantly increases construction difficulty and reduces the applicability of the method. Therefore, a method is needed to self-calibrate the AUV's inertial navigation heading angle even when the global pose of the beacon is unknown, utilizing the beacon's static invariance, repeated observations, and continuous multi-frame observations. Furthermore, underwater image blurring, suspended particles, local overexposure or underexposure, and rapid AUV maneuvers can all reduce the stability of the visual relative pose; therefore, constraints on visual observation quality and AUV motion state are also necessary. Summary of the Invention

[0005] The purpose of this invention is to overcome the shortcomings of existing technologies, such as the difficulty in correcting heading angle drift during long-term operation of underwater AUVs, the need for pre-measurement of global pose when deploying artificial visual beacons, and insufficient utilization of visual information when first observing visual beacons with unknown poses. This invention provides a method and system for self-calibrating the heading of an underwater vehicle assisted by visual beacons with unknown poses. The method involves deploying artificial visual beacons with known geometric dimensions and unique codes at at least one location in the underwater operating area, navigation path, work area, or preset calibration area. It does not require prior acquisition of the beacon's x-coordinate, y-coordinate, and yaw angle in the global coordinate system. The AUV identifies the beacon through a monocular video stream and, combined with time-stamped heading angle, velocity, and acceleration data output by the inertial navigation module, corrects the inertial navigation heading angle drift using short-term continuous observation windows, local beacon maps, and re-observation constraints.

[0006] In a first aspect, the present invention provides a method for self-calibrating the heading of an underwater vehicle assisted by a visual beacon of unknown pose. This method utilizes at least one fixed artificial visual beacon whose global pose has not been pre-calibrated for heading self-calibration. The beacon is provided with an coded tag for identification, and the geometric dimensions of the coded tag and its installation relationship relative to the beacon body are known. The method includes:

[0007] A local map coordinate system is established based on the initial pose of the underwater vehicle; the camera on the underwater vehicle acquires monocular images, and the inertial navigation module outputs inertial navigation data including inertial navigation direction angle and velocity; the monocular images and inertial navigation data are synchronized according to timestamps.

[0008] The system identifies coded tags on beacons in monocular images, uses the geometric parameters of the coded tags and camera intrinsic parameters to solve the camera pose relative to the beacon, transforms it to obtain the visual relative pose of the underwater vehicle relative to the beacon, and extracts the visual relative heading angle from the visual relative pose.

[0009] When the same unrecorded beacon is observed in multiple consecutive frames, the first valid frame is used as a reference. The difference between the visual relative heading angle increment and the corresponding inertial navigation heading angle increment of each subsequent valid frame is taken as the short-term heading angle drift error. The error is then weighted and fused together with the visual observation quality of each valid frame and the motion state of the underwater vehicle to update the heading angle drift correction.

[0010] Using the underwater vehicle's speed and corrected inertial navigation heading angle, the current local pose of the underwater vehicle is recursively calculated. For unrecorded beacons, the local pose of the beacon is obtained based on the current local pose of the underwater vehicle and the visual relative pose. When the difference between the local poses of the beacons determined in adjacent frames within consecutive preset frames is less than a preset threshold, the local pose of the beacon is locked as the map reference pose of the corresponding beacon, and the corresponding beacon is recorded. The map reference pose is associated with the identity code of the corresponding beacon and stored.

[0011] When a recorded beacon is observed, the predicted relative heading angle is calculated based on the current local pose of the underwater vehicle and the map reference pose of the beacon. The predicted relative heading angle is subtracted from the current visual relative heading angle to obtain the long-term heading angle drift error. The heading angle drift correction is updated while keeping the map reference pose unchanged.

[0012] The inertial navigation heading angle is corrected using the heading angle drift correction.

[0013] Preferably, the artificial visual beacon includes a beacon body and multiple coded tags disposed on different sides of the beacon body; different coded tags have different tag numbers, and a mapping relationship between each tag number and the identification code of the beacon to which it belongs and the corresponding side is pre-established.

[0014] Preferably, when multiple coded tags of the same artificial visual beacon are identified in the same frame of monocular image, the visual relative pose corresponding to each coded tag is solved separately, and the visual relative poses are unified to the beacon coordinate system according to the installation relationship of the coded tags relative to the beacon body; the consistency of translation residuals and rotation residuals is checked on the unified visual relative poses, and the visual relative poses that pass the consistency check are weighted and fused according to the visual observation quality of each coded tag.

[0015] Preferably, for each frame of monocular image, the data with the smallest timestamp difference is determined in the inertial navigation data; when the timestamp difference is not greater than a preset synchronization threshold, the inertial navigation data with the closest timestamp is used or adjacent inertial navigation data are interpolated; when the timestamp difference is greater than the preset synchronization threshold, the visual correction observation corresponding to that frame of monocular image is discarded and inertial recursion is maintained.

[0016] Preferably, the effective frames are required to meet the following conditions: the same artificial visual beacon is identified within a consecutive preset number of frames; the recognition confidence of the coded tag is not lower than the confidence threshold; the reprojection error is not greater than the error threshold; the imaging area is not less than the area threshold; and the translational and rotational fluctuations of the visual relative pose of consecutive frames are not greater than the corresponding fluctuation thresholds. When the number of effective frames within a consecutive observation window is less than a preset number, the heading angle drift correction is not updated.

[0017] Preferably, the inertial navigation data also includes acceleration data; the visual observation quality is determined based on the recognition confidence of the coded tag, reprojection error, imaging area, and image sharpness; the underwater vehicle motion state is determined based on the equivalent angular velocity determined by adjacent inertial navigation heading angles and corresponding time differences, as well as the acceleration change in adjacent frames; the weight of each effective frame is determined based on the visual observation quality and the underwater vehicle motion state, and each short-term heading angle drift error is fused using a circular weighting method.

[0018] Preferably, a local beacon map is established using the identification code of the artificial visual beacon as an index. The local beacon map records at least the local position of the beacon, the local yaw angle of the beacon, the cumulative number of observations, and the cumulative observation quality. During the initial mapping process, the local pose of the beacon is updated with a preset gain based on the continuous observation results that meet the observation quality conditions. When the cumulative number of observations reaches a preset number and the stability of the local pose of the beacon reaches a preset condition, the map reference pose of the corresponding beacon is locked.

[0019] Preferably, the heading angle drift correction is updated using long-term heading angle drift error only when the currently identified identity code is consistent with the recorded beacon, the time synchronization error and visual observation quality meet the corresponding thresholds, the translational residual and rotational residual between the predicted relative pose and the current visual relative pose meet the corresponding thresholds, and the number of consecutive effective frames reaches a preset number. When multiple recorded beacons are observed at the same time, a circumferential consistency check is performed on the long-term heading angle drift errors corresponding to each beacon, and long-term heading angle drift errors that do not meet the consistency conditions are eliminated. The remaining long-term heading angle drift errors are then weighted and fused based on the cumulative observation quality of the corresponding beacon and the current visual observation quality.

[0020] Secondly, the present invention provides an underwater vehicle heading self-calibration system assisted by an unknown pose visual beacon, comprising at least one fixed artificial visual beacon whose global pose has not been pre-calibrated, and a monocular image acquisition module, an inertial navigation module, and a data processing module mounted on the underwater vehicle; the artificial visual beacon is provided with an coded tag for identification, the geometric dimensions of the coded tag and its installation relationship relative to the beacon body are known; the monocular image acquisition module and the inertial navigation module are respectively connected to the data processing module, and the data processing module is configured to execute the aforementioned underwater vehicle heading self-calibration method.

[0021] Preferably, the data processing module includes a time synchronization unit, a visual relative pose solving unit, a first observation short-term correction unit, a local beacon map unit, an inertial recursion unit, a re-observation long-term correction unit, and a heading output unit. The time synchronization unit is used to synchronize monocular images and inertial navigation data. The visual relative pose solving unit is used to solve the visual relative pose of the underwater vehicle relative to an artificial visual beacon. The first observation short-term correction unit is used to update the heading angle drift correction amount based on the short-term heading angle drift error. The local beacon map unit is used to establish and store the map reference pose of the beacon. The inertial recursion unit is used to recursively calculate the current local pose of the underwater vehicle. The re-observation long-term correction unit is used to update the heading angle drift correction amount based on the long-term heading angle drift error. The heading output unit is used to correct the inertial navigation heading angle using the heading angle drift correction amount.

[0022] Compared with the existing technology, the beneficial effects of this invention are:

[0023] 1. This invention does not require prior measurement of the x-coordinate, y-coordinate, and yaw angle of the artificial visual beacon in the global coordinate system. It only relies on the beacon's unique code, known geometric dimensions, and static invariance to establish a local beacon map in the underwater area where the beacon can be observed by the AUV, thus reducing the difficulty of beacon deployment and underwater construction calibration.

[0024] 2. When the present invention first stably observes a visual beacon in an unknown pose, it can use the reference frame of a short-term continuous observation window as a temporary reference. By using the difference between the visual relative heading angle increment and the inertial navigation heading angle increment in multiple consecutive frames, the present invention can achieve short-term heading angle drift constraint, avoid the visual information being idle during the first observation stage, and improve the calibration continuity during the effective observation period of the beacon.

[0025] 3. This invention does not require the underwater mission area to be fixedly divided into an operation area and a correction area. As long as the AUV observes the artificial visual beacon and meets the correction conditions, it can perform course drift correction. In a typical deployment mode, a dedicated correction area can also be set up, so that the AUV can navigate or operate in the non-correction area by relying on the corrected inertial navigation course. After entering the correction area, it can use the artificial visual beacon for re-observation correction. This is suitable for underwater missions with long endurance, periodic returns, or multiple crossings of the beacon deployment area.

[0026] 4. This invention uses a four-sided AprilTag ladder-type artificial visual beacon, which allows AUVs to observe the tag when approaching the beacon deployment location from different directions; when multiple side tags can be observed, the robustness of relative pose estimation is improved through multi-sided tag consistency check and weighted fusion.

[0027] 5. This invention records the local pose of unknown pose visual beacons through a local beacon map, and compares the map prediction observation with the current visual observation during subsequent re-observations. This transforms the unknown global pose beacon into a reusable long-term heading angle consistency constraint, suppressing the heading angle drift accumulated in beacon-free segments.

[0028] 6. This invention uses identification confidence, reprojection error, imaging area, image sharpness, learned quality score, and AUV motion state for observation screening and weight construction, which can reduce the impact of underwater image degradation, single-frame misidentification, and rapid maneuvering on heading angle correction results. Attached Figure Description

[0029] Figure 1 This is a schematic diagram illustrating the heading angle drift correction of an AUV operating between an artificial visual beacon deployment area and a non-beacon observation area in an embodiment of the present invention.

[0030] Figure 2This is a schematic diagram of the structure of the tiered four-sided AprilTag artificial visual beacon in an embodiment of the present invention.

[0031] Figure 3 This is a schematic diagram illustrating the time synchronization between monocular video stream and inertial navigation data in an embodiment of the present invention.

[0032] Figure 4 This is a schematic diagram of short-term heading constraints during the first observation of an unknown pose visual beacon in an embodiment of the present invention.

[0033] Figure 5 This is a schematic diagram of the local beacon map and the correction of the re-observation heading angle in an embodiment of the present invention. Detailed Implementation

[0034] The present invention will be further described below with reference to the accompanying drawings. It should be understood that the following embodiments are for illustrative purposes only and are not intended to limit the scope of protection of the present invention.

[0035] Example 1

[0036] A self-calibration method for the heading of an underwater vehicle assisted by a visual beacon in an unknown pose is described in three parts: beacon observation and data synchronization, short-term heading angle correction during the first observation, and long-term heading angle correction during re-observation and local beacon map construction.

[0037] Part 1: Beacon observation, relative pose solution and data synchronization.

[0038] Step 1.1: Determine the deployment area for artificial visual beacons and the AUV operating mode. For example... Figure 1 As shown, in this embodiment, multiple artificial visual beacons can be deployed along the AUV's navigation path, in the operational area, in a preset calibration area, or in a combination of these areas. The AUV's main workflow does not require the division of the operational and correction zones as a necessary condition. Visual heading angle correction is performed when an artificial visual beacon is observed and the correction conditions are met. When no artificial visual beacon meeting the conditions is observed, navigation and operations are conducted based on the heading angle, speed, and acceleration output by the inertial navigation module. As a typical deployment method, a dedicated correction zone can be set up on the seabed, and multiple artificial visual beacons can be deployed within this zone. The global position and global orientation of the artificial visual beacons do not need to be pre-measured after deployment. The AUV performs routine cruise, reconnaissance, or operational tasks in non-correction zones, and when entering a correction zone or passing the beacon deployment location, it estimates and corrects the long-term accumulated heading angle drift by re-observing the beacons.

[0039] Step 1.2: Configure the inertial navigation module and the monocular image acquisition module. In this embodiment, the inertial navigation module does not require modification of its internal algorithm and is used as a data source, outputting timestamps, heading angles, velocity, and acceleration. The image acquisition module is used as a monocular video stream data source, and its output is an image sequence with fixed resolution and a fixed frame rate. This embodiment does not limit the specific models of the inertial navigation module and the image acquisition module.

[0040] Step 1.3: Set up the artificial visual beacon structure. For example... Figure 2 As shown, the artificial visual beacon consists of a truncated pyramidal body and four AprilTag tags. The top surface of the truncated pyramidal body is a 60 cm × 60 cm square, the bottom surface is a 70 cm × 70 cm square, and the height of the body is 70 cm. All four sides are trapezoidal. The four AprilTag tags are respectively placed on the four trapezoidal sides, and each tag plane is coplanar with the corresponding trapezoidal side. Each AprilTag tag uses the tag36h11 encoding family, with a total tag size of 56.5 cm × 56.5 cm; after removing the outermost white silent area, the 8×8 encoding area size is 45.20 cm × 45.20 cm; the size of a single code element is 5.65 cm × 5.65 cm. The four AprilTag tags of the same beacon use different tag numbers, and a mapping relationship between each tag number, the unique number of the beacon to which it belongs, and the corresponding side sequence number is pre-established.

[0041] Step 1.4: Establish the beacon coordinate system and the tag coordinate system. Establish the beacon coordinate system B with the geometric center of the truncated pyramid body. The Z-axis of beacon coordinate system B is along the central axis of the truncated pyramid, and the X and Y axes are parallel to the two sides of the top or bottom surface, respectively. Establish tag coordinate systems T1, T2, T3, and T4 with the planes containing the four AprilTag tags, respectively. In this embodiment, superscripts represent the target coordinate system and subscripts represent the described coordinate system. That is, a homogeneous transformation matrix with superscript X and subscript Y represents the transformation of homogeneous coordinates in the Y coordinate system to the X coordinate system. Since the dimensions of the truncated pyramid body and the relative installation positions of the tags on the truncated pyramid body are known, the fixed transformation relationship from each tag coordinate system to the beacon coordinate system B can be predetermined after installation. When identifying the j-th tag, the relative pose from the camera coordinate system C to the beacon coordinate system B obtained from that tag is represented as:

[0042]

[0043] Where, transformation matrix The transformation matrix is ​​the transformation from the camera coordinate system C obtained from the j-th label to the beacon coordinate system B. Let T be the coordinate system of the j-th label. j Fixed transformation to beacon coordinate system B; transformation matrix Translate the camera coordinate system C obtained from visual inspection to the label coordinate system T. j The transformation.

[0044] Step 1.5: Unify single-tag observations to the beacon coordinate system and the body coordinate system A. After the AUV identifies the AprilTag tag on any side, it obtains the coordinates from the camera coordinate system C to the corresponding tag coordinate system T by solving the planar target pose, based on the tag corner image coordinates, camera intrinsic parameters, and the actual tag size. j The transformation from camera coordinate system C to beacon coordinate system B is obtained by using a fixed transformation between the tag coordinate system and the beacon coordinate system. The installation extrinsic parameters between camera coordinate system C and body coordinate system A are obtained through calibration after equipment installation. The transformation from body coordinate system A to beacon coordinate system B obtained from the j-th tag is as follows:

[0045]

[0046] in, Install extrinsic parameters in reverse order from the body coordinate system A to the camera coordinate system C; This is the installation extrinsic parameter for the camera coordinate system C to the body coordinate system A. This allows all label observations to be uniformly transformed to the same beacon coordinate system and body coordinate system A.

[0047] Step 1.6: Perform consistency checks and fusion on multi-label observations of the same beacon. If two or more side labels of the same artificial visual beacon are simultaneously identified in the same frame image, calculate the relative pose of each label and unify them to the beacon coordinate system B. Let the recognition confidence of the j-th label be η. j The root mean square error of corner reprojection is ε r,j The label imaging area is A j The attitude fluctuation in consecutive frames is σ. j Based on the above quantities, the visual observation quality q is constructed. j :

[0048]

[0049] Where, λ r and λ s A is a non-negative attenuation coefficient. ref This is the reference imaging area.

[0050] First, a consistency check is performed on the poses of each tag within the same frame. Let the translation vector and rotation matrix of the j-th tag be t, respectively. j and R jSelect the label with the highest observation quality as the initial reference, or first perform a coarse weighted fusion on all candidate observations to obtain the initial fusion translation t̅ and the initial fusion rotation R̅. The j-th label is retained in the valid label set Ω only if the translation residual and rotation residual satisfy the following equation. k In this context, the index for traversing a collection is represented by the subscript l:

[0051]

[0052] Where, τ t τ is the translation consistency threshold. R For rotation consistency thresholding, Log(·) represents mapping the relative rotation matrix to a rotation vector. For labels that pass the consistency check, the weight w is obtained based on observation quality normalization. j The translation amount is calculated using a weighted average:

[0053]

[0054] Each rotation matrix R j Convert to a symbolically unified unit quaternion u j Using the first valid quaternion as a reference; when the inner product of two quaternions is less than zero, invert one of the quaternions so that it lies in the same hemisphere. Fusion quaternion u̅ k via eig max (·) Eigenvalue decomposition operation: Take the normalized eigenvector corresponding to the largest eigenvalue of the weighted outer product matrix, and convert it into a fusion rotation matrix R̅. k :

[0055]

[0056] By the fusion rotation matrix R̅ k and the fused translation vector t̅ k The relative pose Z of the fused beacon in the k-th frame is constituted. k The above fusion method can avoid attitude abrupt changes caused by yaw angles crossing positive and negative boundaries and by directly averaging Euler angles.

[0057] Step 1.7: Complete camera calibration and underwater imaging parameter settings. Before tag detection, complete camera calibration under the conditions of the underwater camera's waterproof chamber, lens focal length, and underwater medium to obtain the underwater equivalent intrinsic parameter matrix, distortion parameters, and installation extrinsic parameters between the camera coordinate system C and the body coordinate system A. For cases where the refraction error of the planar observation window can be absorbed by the equivalent intrinsic parameter, distortion correction is directly performed using the underwater equivalent intrinsic parameter. For cases where the refraction error cannot be ignored, a refraction imaging model including the refraction interfaces of air, the observation window, and water is used to correct the corner back-projection rays.

[0058] Step 1.8: Preprocess and quality screening of underwater images. For each frame of underwater image, distortion correction, grayscale conversion, local contrast enhancement, and noise suppression are performed sequentially. In a preferred embodiment, contrast-limited adaptive histogram equalization is used to enhance the black-and-white boundaries of the labels, and edge-preserving filtering is used to suppress high-frequency noise caused by suspended particles in the water. The local contrast, sharpness, and overexposure ratio of the candidate label region are calculated. When the local contrast or sharpness is lower than the corresponding lower threshold, or the overexposure ratio is higher than the corresponding upper threshold, the frame is marked as a low-quality frame and is not used for attitude fusion and heading angle correction.

[0059] Step 1.9: A deep learning-assisted recognition module can be introduced under complex underwater image conditions. In a preferred embodiment, a deep learning-assisted recognition module is introduced before traditional AprilTag detection. The deep learning-assisted recognition module can employ a convolutional neural network, a lightweight object detection network, or a visual Transformer network, and is trained using underwater beacon image samples. For an input underwater image frame, the deep learning-assisted recognition module outputs artificial visual beacon ontology candidate boxes, AprilTag candidate boxes, and their corresponding detection confidence scores.

[0060] For candidate regions with a detection confidence level not lower than a preset threshold, a local image is cropped and local contrast enhancement, edge enhancement, scale normalization, and perspective correction are performed. Then, AprilTag encoding and recognition and corner localization are performed within the candidate region. The perspective correction is used to improve the decoding success rate of the encoded region. The corner coordinates used for pose calculation are still based on the corner positions in the original image coordinate system or the distortion-corrected image coordinate system. If the candidate region output by the deep learning-assisted recognition module overlaps with the traditional full-image AprilTag detection result, and the tag numbers obtained by the two are consistent and the corner reprojection error meets the threshold, then the observation is considered a valid visual observation. If the traditional full-image AprilTag detection does not obtain a valid result, but the candidate region given by the deep learning-assisted recognition module can complete AprilTag decoding and pose calculation after local enhancement, then the visual observation corresponding to the candidate region is retained. If the candidate region cannot complete AprilTag encoding and decoding or the corner reprojection error exceeds the threshold, then the candidate region is discarded.

[0061] The deep learning-assisted recognition module is used to improve the recall rate and observation quality assessment reliability of artificial visual beacons or AprilTag candidate regions in complex underwater images. It is not used as a direct output module for AUV heading angle or heading angle correction. The relative pose between the camera coordinate system and the tag coordinate system is still obtained by solving the planar target pose using the actual tag size, corner image coordinates, camera intrinsic parameters, and camera imaging model, thus maintaining the geometric interpretability and scale consistency of the visual pose estimation results.

[0062] Step 1.10: Synchronize the time between the monocular video stream and the inertial navigation data. For example... Figure 3 As shown, this embodiment performs time synchronization between the monocular video stream and inertial navigation data. The image acquisition module outputs an image timestamp t. k Image frame I(t) k The inertial navigation module outputs a time stamp t. i heading angle ψ imu (t i ), velocity v(t) i ) and acceleration a(t) i For each frame of image, first search for the data with the smallest time difference in the inertial navigation data sequence, and then calculate the minimum synchronization time difference δt. k :

[0063]

[0064] Where, τ sync The maximum allowable synchronization time difference. When δt k Not greater than τ sync When the image timestamp is between two adjacent inertial navigation timestamps and both sides of the data are valid, interpolation is used to obtain t. k Data at time. For heading angle interpolation, the shortest angle difference between adjacent heading angles is first calculated using the wrap function, and then interpolation is performed along this shortest angle difference to avoid erroneous jumps near the positive and negative π boundaries; velocity and acceleration are interpolated linearly. When δt k Greater than τ sync If adjacent inertial navigation data is lost, the corrected observation corresponding to that image frame is discarded, but the inertial recursion is retained. The synchronized data set is represented as follows:

[0065]

[0066] Among them, D k I(t) is the synchronization data group corresponding to the k-th frame image; k ) represents the k-th frame image; ψ imu (t k ) represents the inertial navigation output heading angle at the same moment, v(t) k ) represents the output velocity of the inertial navigation system, a(t) k The acceleration is the output of the inertial navigation system. The velocity is used for AUV pose recursion and re-observation prediction in the local map coordinate system. The velocity change is used to identify non-stationary conditions such as sudden stops and sharp turns. The acceleration and the difference between adjacent frames are used to determine the start and stop of the thrusters, collision disturbances and rapid maneuvers, and serve as motion state factors for visual correction weights.

[0067] Step 1.11: Define the coordinate system and angle units. In this embodiment, a body coordinate system A is established with the underwater vehicle body as the reference, with its X-axis pointing bow, Y-axis pointing starboard, and Z-axis pointing downwards; the camera coordinate system C is defined with the X-axis pointing to the right, the Y-axis pointing downwards, and the Z-axis pointing forward along the optical axis; the beacon coordinate system B is established at the geometric center of the tiered body; the local map coordinate system M is established from the initial state when the AUV first executes this method, or from the state when the AUV first enters the preset calibration area or first stably observes the artificial visual beacon. The heading angle referred to in this invention is the yaw angle of the body coordinate system A about the local vertical axis relative to the local map coordinate system M. All angles are uniformly represented in radians during the calculation process and can be converted to degrees for output or display. The heading angle difference is normalized to (-π, π) using the wrap function. When extracting the heading angle from the 3D rotation, the X-axis of the body coordinate system A is first projected onto the local map horizontal plane, and then the directional angle of this projection relative to the local map X-axis is calculated to reduce the coupling effect of roll and pitch angle changes on the heading angle extraction.

[0068] Part 2: Short-term heading angle correction during the first observation of a visual beacon with an unknown pose.

[0069] This section is based on the relative pose Z of the fused beacon in the k-th frame obtained in the first part. k and the corresponding synchronization data group D k The heading angle correction was completed within a short continuous observation window when the first observation of the unknown pose visual beacon was completed.

[0070] This section mainly includes the following steps:

[0071] Step 2.1: Determine the reference frame for the initial stable observation window. The initial stable detection frame must simultaneously meet the following conditions: within N consecutive... s The same artificial visual beacon was identified in all frames; the identification confidence level was not less than η. min The root mean square error of corner reprojection is no greater than ε. max The image area of ​​the tag is not less than A. min The translational and rotational fluctuations of the relative pose in consecutive frames shall not exceed the corresponding upper limits for translational and rotational fluctuations, respectively. As an optional setting, N... s Take 3 to 10 frames. The upper limits for translational and rotational fluctuations mentioned above are determined based on camera resolution, beacon observation distance, and water visibility. The first frame that meets the conditions is designated as the window reference frame k=0.

[0072] Step 2.2: Establish a temporary reference for a short-term continuous observation window and calculate the visual relative heading angle increment. For example... Figure 4As shown, when the AUV first stably observes an artificial visual beacon in an unknown pose, the relative pose Z0 of the fused beacon corresponding to the window reference frame is used as a temporary reference. For the k-th frame image within the same short-term continuous observation window W, the relative pose Z of the fused beacon is obtained. k And by Z0 and Z k The relative rotation between them, calculated visually, represents the relative heading angle increment, which can be expressed as:

[0073]

[0074] Where, Δψ vis (k) represents the visual relative heading angle increment of the k-th frame relative to the window reference frame; yaw(·) represents extraction from relative rotation according to the horizontal projection method described in Part 1. The continuous observation window ends when any exit condition is met: reaching the maximum number of frames N. w Or the maximum duration T w ;N consecutive loss of artificial visual beacons lost Frame; identification confidence level below η min The imaging area is smaller than A min Or the resolution is lower than C min The equivalent angular velocity exceeds ω max The change in acceleration between adjacent frames exceeds a jump Or the reprojection error continuously exceeds ε max If the number of valid frames is less than N min If the error is not detected, the window will not output the correction value; if the error fails at the beginning of the window, a stable reference frame will be searched again.

[0075] Where, η min A min C min ε max ω max and a jump These thresholds are defined as follows: lower limit for recognition confidence, lower limit for tag imaging area, lower limit for image sharpness, upper limit for reprojection error, upper limit for equivalent angular velocity, and upper limit for acceleration change between adjacent frames. These thresholds can be fixed calibration values ​​or adaptively adjusted based on water visibility, average image brightness, tag observation distance, historical observation quality, and inertial navigation data noise level. When the brightness of several consecutive frames decreases, local contrast decreases, or overall sharpness is low, the requirements for removing low-quality frames are increased or the visual correction weight is decreased. When the observation distance is close, the tag boundaries are stable, and the historical reprojection error is small, some image quality thresholds are relaxed to increase the number of effective observation frames. The threshold adaptation process only changes whether observations participate in statistics and their weights; it does not change the heading angle unit or coordinate system definition.

[0076] Step 2.3: Calculate the inertial navigation heading angle increment and short-time heading angle drift error. Within the same time window, calculate the inertial navigation heading angle increment based on the heading angle output by the inertial navigation module, and subtract the visual relative heading angle increment from the inertial navigation heading angle increment to obtain the short-time heading angle drift error, the expression of which is:

[0077]

[0078]

[0079] Where, Δψ imu (k) represents the inertial navigation heading angle increment; e Δψ (k) represents the short-term heading angle drift error; wrap represents the angle normalization function, used to normalize the angle difference to (-π, π). Before performing window statistics, outlier frames that are significantly inconsistent with the error distribution of the main window are removed using median absolute deviation or Huber loss to prevent misidentification of a single frame from directly changing the heading angle correction. In this embodiment, the short-term heading angle drift error is determined by the difference between the visual heading angle increment and the inertial navigation heading angle increment relative to the window reference frame, without depending on the global position and global orientation of the beacon. Therefore, even if a beacon with an unknown global pose is observed for the first time, a short-term heading angle drift constraint can be formed, avoiding the idleness of visual information in the initial observation stage.

[0080] Step 2.4: Construct a visual quality factor and perform weighted statistics on short-term heading angle drift errors. Perform weighted statistics on multiple short-term heading angle drift errors within a short-term continuous observation window. Let η be the confidence level for tag recognition in the k-th frame. k The reprojection error is ε k The label imaging area is A k The image sharpness of the label candidate region is C. k C k It can be determined by Laplacian variance, edge gradient energy, or the sharpness of the black-and-white boundary of the label; the visual quality factor Q is defined. k One basic form is:

[0081]

[0082] With the introduction of a deep learning-assisted recognition module, its output detection confidence score or learned quality score can also be used as a component of the visual observation quality factor. Let the learned quality score of the k-th frame be s. k s k Normalized to the [0,1] interval, it is used to characterize the credibility of the candidate region in this frame. The learned quality score can be obtained by one or more normalized combinations of detection confidence, candidate region integrity, boundary sharpness, and cross-frame stability. Then the visual quality factor can be modified as follows:

[0083]

[0084] Among them, Q k Q' is a regular visual quality factor constructed based on label recognition confidence, reprojection error, imaging area, and image sharpness. k The visual quality factor is the result of integrating the learning-based quality score, and its value ranges from [0,1]. When s k When the value is below a preset threshold, the observation in that frame will not participate in short-term continuous observation window statistics, multi-label pose fusion, and re-observation heading angle correction; when s k If the threshold is met but the observation quality is average, the observation in that frame can still be included in the calculation, but its weight will be reduced accordingly. When introducing a deep learning-assisted recognition module, the visual quality factor in the subsequent window comprehensive weight formula is taken as the visual quality factor after fusing the learned quality score, so that the learned quality score can participate in the calculation of the short-term heading angle correction.

[0085] In a preferred embodiment, image sharpness is included as an explicit component of the visual quality factor, and C is set as follows: ref For reference sharpness, the visual quality factor Q is... k In the above expression, the sharpness normalization factor min(1, C) is used. k / C ref When image blurring, edge diffusion, or suspended particles in water reduce the clarity of label boundaries, C k Decrease, visual quality factor Q k It then decreases.

[0086] Step 2.5: Output short-term heading angle correction based on AUV motion status. Calculate the equivalent angular velocity ω based on adjacent inertial navigation heading angles and time differences. k The motion state factor M is constructed using the equivalent angular velocity and the change in acceleration between adjacent frames. k :

[0087]

[0088] Where, λ ω and λ a Q is a non-negative attenuation coefficient. When the tag image is clear, the reprojection error is small, the recognition confidence is high, the imaging area meets the requirements, and the AUV movement is stable, Q... k and M k Larger values; when the image is blurry, the label is obscured, there is overexposure or underexposure, or the AUV rotates rapidly or experiences sudden acceleration changes, the corresponding weights are automatically reduced. The window's overall weight and short-term heading angle drift correction can be represented as follows:

[0089]

[0090] Where atan2 is the arctangent function in the four quadrants. When the sum of the effective weights is not less than the preset minimum weight w... min When the sum of effective weights is less than w, output the circumferential weighted short-time correction. min If the sine-weighted sum and the cosine-weighted sum are both close to zero, this window will not output the correction amount. s This is the first observation of the short-term correction gain. Before being used to update the heading angle drift correction, the window correction is first subject to single-time correction amplitude limiting and correction rate constraints; let the maximum single-time correction amplitude be δψ. max The maximum correction rate per unit time is γ max If the candidate correction exceeds the above constraints, the output will be based on the constraint boundaries. The heading angle drift correction can be updated between consecutive windows using first-order smoothing or moving average to avoid sudden jumps in the corrected heading angle caused by a single high-weight observation. If the current window does not meet the output conditions, the previous valid heading angle drift correction will be retained, or only its uncertainty will be propagated. The corrected heading angle is expressed as:

[0091]

[0092] Step 2.6: Perform consistency fusion of short-time heading angle correction results for multiple beacons. When multiple visible beacons exist within a window, calculate the short-time heading angle drift error sequence for each beacon. After consistency verification, perform circular weighted fusion based on the cumulative observation quality of each beacon within the current window. Thus, even if the global pose of the beacon is unknown, the AUV can utilize the relative changes in consecutive frames to form short-time heading angle drift constraints during the first stable observation of an unknown pose visual beacon.

[0093] Part 3: Local beacon map construction and long-term heading angle correction for re-observation.

[0094] Based on the fused beacon relative pose and synchronization data set formed in Part 1 and the short-term heading angle correction results formed in Part 2, this section completes the construction of local beacon maps and the long-term heading angle correction of re-observations.

[0095] Step 3.1: Add the first observed unknown pose visual beacons to the local beacon map. For example... Figure 5As shown, when observing a visual beacon with an unknown pose for the first time, this embodiment adds the beacon to the local beacon map based on the current local pose of the AUV, the camera mounting extrinsic parameters, and the camera's visual relative pose to the beacon. The first part first obtains the transformation from the camera coordinate system C to the beacon coordinate system B, and then inverses it to obtain the visual transformation from the beacon coordinate system B to the camera coordinate system C. Let the current transformation from the body coordinate system A to the local map coordinate system M be a transformation with superscript M and subscript A; the mounting extrinsic parameters from the camera coordinate system C to the body coordinate system A be a transformation with superscript A and subscript C; and the visual transformation from the beacon coordinate system B to the camera coordinate system C be a transformation with superscript C, subscript B, and a vis subscript. Then, the initial pose of the beacon in the local map is:

[0096]

[0097] The local beacon map uses a unique beacon number as an index, recording the beacon's local location, local yaw angle, complete 3D attitude, cumulative observation count, average reprojection error, and most recent valid observation time. It also saves the tag numbers, corresponding side numbers, and fixed transformation relationships from the tag coordinate system to the beacon coordinate system for each tag included in the beacon. This prevents different side tags of the same beacon from being repeatedly mapped and ensures that multi-tag fusion and re-observation association all point to the same beacon entity. During the initial mapping phase, subsequent continuous observations of the same beacon undergo consistency checks, and its local pose is updated with a small gain only when the observation quality meets a threshold. Once the cumulative observation count and pose stability reach preset conditions, the beacon is marked as a mature beacon, and its local pose is locked. Subsequent re-observations are used to update the AUV heading angle drift correction. The map reference pose of mature beacons remains unchanged, thus avoiding the absorption of inertial navigation heading angle drift into the local beacon map. Since the global pose of the beacon is unknown, the local beacon map may have a fixed overall rotation angle relative to the geographic coordinate system, but this fixed rotation angle does not affect the constraint on the long-term drift of the inertial navigation heading angle.

[0098] Step 3.2: Perform inertial recursion when no effective visual beacon is available. When the AUV is in a flight segment where no artificial visual beacon meeting the correction conditions has been observed, the heading and position are recursively calculated based on the corrected heading angle and the velocity and acceleration output by the inertial navigation module. The velocity is integrated over time to form a local displacement increment, and the acceleration is used to identify changes in motion state and constrain the reliability of the velocity recursion. When no available visual beacon is available, the heading angle drift correction remains in the previous effective correction state, or its uncertainty is propagated according to a preset random walk model, but the estimated mean deviation is not automatically changed based on the inertial navigation data without external constraints.

[0099] Step 3.3: Constructing predictive observations when re-observing mapped beacons. When the AUV re-observes a beacon that has been added to the local beacon map, first calculate the camera's predicted local pose based on the AUV's predicted local pose obtained through inertial recursion and the camera's extrinsic parameters. Then, combine this with the beacon pose recorded in the local beacon map to calculate the beacon's predicted relative pose. Subsequently, the mapping relationship between tag numbers and beacon numbers, time synchronization error, current visual observation quality, and translational and rotational residuals between the predicted relative pose and the current visual relative pose are checked. Only when the number association is consistent, each quality indicator and residual meets the corresponding threshold, and the number of consecutive valid frames reaches a preset number, is the re-observation correction condition satisfied. The predicted relative pose is expressed as:

[0100]

[0101] Step 3.4: Calculate the long-term heading angle drift error. Simultaneously, identify the beacon using the current video frame to obtain the actual visual relative pose. The difference in heading angle between the predicted relative pose and the actual visual relative pose is defined as the long-term heading angle drift error.

[0102] e ψ (k) = wrap(yaw( C T B,pred ) − yaw( C T B,vis ))

[0103] In this process, transformations with superscript C, subscript B, and the subscript "vis" represent the current visual observation, while transformations with superscript C, subscript B, and the subscript "pred" represent the local map prediction observation. The error sign is consistent with the heading angle deviation update direction. If multiple map-established beacons are re-observed at the same time, a circumferential median test is first performed on the long-term heading angle errors of each beacon, eliminating beacons whose errors differ from the main error by more than a threshold. Then, a weighted fusion is performed based on the cumulative map quality and the current visual quality. Adaptive correction coefficient K. k Determined based on visual quality factor and motion state factor:

[0104]

[0105] Where K0 is the base gain, K min and K max Here, represents the upper and lower limits of gain, and 'clip' represents the clipping function. Step 3.5: Update the long-term heading angle drift estimate and output the corrected heading angle: Use the long-term heading angle drift error and the adaptive correction coefficient to update the heading angle drift estimate b. ψ It also outputs the corrected heading angle.

[0106]

[0107]

[0108] Among them, b ψ (k) represents the heading angle drift correction at time k; K k e is the adaptive correction coefficient; ψ (k) represents the heading angle drift error obtained from long-term re-observation; ψ out (k) is the corrected output heading angle. Before the update, e... ψ (k) Perform wrap normalization and limit the maximum single correction amount to prevent heading angle jumps caused by erroneous associations or transient image anomalies. By establishing a long-term heading angle drift error between local map prediction observations and current visual observations, beacons with unknown global poses are transformed into reusable long-term heading angle consistency constraints after local mapping is completed, thereby suppressing the heading angle drift accumulated in beacon-free segments.

[0109] Step 3.6: Perform sliding window weighted optimization under continuous effective reobservation conditions. In one optional implementation, sliding window weighted optimization can be used within multiple consecutive effective beacon observation windows to jointly minimize the visual relative heading angle variation residual, the local map predicted heading angle residual, and the inertial navigation heading angle variation residual. The optimized state includes at least the AUV local heading angle, the heading angle drift correction, and the local heading angle of the observed beacon; it can be further extended to include the AUV's 3D local pose and the beacon's 3D local pose. Each residual is weighted according to visual quality, map cumulative quality, and motion state, and a robust loss function is used to reduce the impact of abnormal label observations.

[0110]

[0111] Where ρ is the robust loss function; w vis,k w map,k and w imu,k These are visual quality weights, cumulative map quality weights, and inertial recursion weights. Each weight is determined based on the current frame's visual quality factor, the cumulative observation quality of the corresponding beacon, and the AUV's motion state factor, and is used to reduce the impact of low-quality images, low-confidence map points, and rapid maneuvering on the optimization results.

[0112] The visual relative heading angle variation residual, the local map predicted heading angle residual, and the inertial navigation heading angle variation residual are expressed as follows:

[0113]

[0114] r imu,k = wrap(Δψ imu (k) + Δb ψ (k) −Δψ M (k))

[0115]

[0116] Where, Δψ vis (k) represents the change in heading angle obtained from visual relative pose within a continuous observation window; Δψ M (k) represents the change in the local heading angle of the AUV in the optimized state; ψ rel,vis (k) is the beacon's relative heading angle obtained from the current visual observation; ψ rel,pred (k) is the beacon-predicted relative heading angle obtained from the local beacon map and the AUV-predicted pose; Δψ imu (k) represents the change in heading angle output by the inertial navigation module; Δb ψ (k) represents the change in heading angle drift correction between adjacent states. All the angles mentioned above are expressed in radians and normalized to (-π, π) using the wrap function.

[0117] By solving the above optimization objective, the updated local heading angle and heading angle drift correction b of the AUV are obtained. ψ The result is used for subsequent heading angle output. If the number of valid frames within the sliding window is insufficient, the sum of valid weights is below the threshold, or the residual distribution does not meet the consistency check, then the current sliding window optimization is not performed, and the previous valid correction state is maintained or only its uncertainty is propagated.

[0118] Since artificial visual beacons are fixedly deployed and can be repeatedly observed, this invention can use the first continuous observation to form short-term heading angle drift constraints, and use local beacon maps and subsequent re-observations to form long-term heading angle consistency constraints, thereby suppressing the cumulative heading angle drift generated by the inertial navigation module during long-term operation, and improving the heading angle consistency and long-term heading angle accuracy of AUVs when running back and forth between beacon-observable areas and non-beacon-observable areas.

[0119] Example 2

[0120] An underwater vehicle heading self-calibration system assisted by an unknown pose visual beacon is provided for performing the method provided in Example 1. The system includes at least one artificial visual beacon, a monocular image acquisition module, an inertial navigation module, and a data processing module.

[0121] Artificial visual beacons are fixedly installed on the seabed, on fixed supports in the work area, or on underwater structures. Their global position and orientation do not need to be measured in advance. In this embodiment, the artificial visual beacon includes a corrosion-resistant beacon body and multiple coded tags installed on different sides of the beacon body. Each coded tag has a different tag number. The mapping relationship between each tag number, the identification code of the beacon to which it belongs, and the corresponding side, as well as the geometric dimensions of each coded tag and its installation relationship relative to the beacon body, are pre-stored in the data processing module.

[0122] The monocular image acquisition module includes a monocular camera fixedly mounted at the front of the underwater vehicle and a waterproof hull. The monocular camera is connected to the processing module via data interfaces such as Ethernet, USB, or MIPI to acquire monocular images including artificial visual beacons. The camera's intrinsic parameters and its extrinsic installation parameters relative to the underwater vehicle body are pre-calibrated and stored in the data processing module.

[0123] The inertial navigation module is fixedly installed inside the underwater vehicle and includes an inertial measurement unit and a navigation calculation unit. It outputs timestamped inertial navigation heading angle, velocity, and acceleration. The inertial navigation module connects to the data processing module via a serial interface, CAN bus, or Ethernet.

[0124] The data processing module employs an embedded computer located within the underwater vehicle, comprising a processor, runtime memory, non-volatile memory, clock circuitry, and communication interface. The non-volatile memory stores camera calibration parameters, coded tag geometric parameters, tag number mapping relationships, and local beacon maps. The processor is used for running time synchronization units, visual relative pose calculation units, initial observation short-term correction units, local beacon map units, inertial recursion units, re-observation long-term correction units, and heading output units. Each of these units can be implemented by the processor executing programs stored in the memory, eliminating the need for separate processors.

[0125] The time synchronization unit performs nearest neighbor matching or interpolation on the monocular image and inertial navigation data based on the timestamp; the visual relative pose solving unit identifies the coded tags, solves the visual relative pose based on the tag corner points, tag geometric dimensions and camera intrinsic parameters, and performs consistency checks and weighted fusion when multiple coded tags of the same beacon are identified in the same frame.

[0126] The initial observation short-term correction unit calculates the difference between the visual relative heading angle increment and the inertial navigation heading angle increment when the same unrecorded beacon is observed in multiple consecutive frames, and updates the heading angle drift correction based on the visual observation quality and the underwater vehicle's motion state. The local beacon map unit stores the beacon's local pose, cumulative observation count, and cumulative observation quality using an identification code as an index, and locks its map reference pose after the beacon's local pose meets the stability conditions.

[0127] The inertial recursive unit recursively calculates the current local pose based on the underwater vehicle's speed and the corrected inertial heading angle. The long-term correction re-observation unit, when it re-observes a recorded beacon and the observation quality, pose residual, and number of consecutive valid frames meet the corresponding conditions, compares the predicted relative heading angle with the current visual relative heading angle, and updates the heading angle drift correction while keeping the map reference pose unchanged.

[0128] The heading output unit uses the heading angle drift correction to correct the inertial navigation heading angle, and sends the corrected heading angle to the underwater vehicle's navigation controller via a serial interface, CAN bus, or Ethernet for track keeping, path tracking, or return-to-home control.

Claims

1. A method for self-calibrating the heading of an underwater vehicle assisted by a visual beacon of unknown pose, characterized in that, A heading self-calibration method is performed using at least one fixed artificial visual beacon with an uncalibrated global pose, wherein the beacon is equipped with an coded tag for identification; the method includes: A local map coordinate system is established based on the initial pose of the underwater vehicle; the camera on the underwater vehicle acquires monocular images, and the inertial navigation module outputs inertial navigation data including inertial navigation heading angle and velocity; the coded tags on the beacons in the monocular images are identified, and the pose of the camera relative to the beacons is solved using the geometric parameters of the coded tags and the camera intrinsic parameters, and the visual relative pose of the underwater vehicle relative to the beacons is obtained by transformation, and the visual relative heading angle is extracted; When the same unrecorded beacon is observed in multiple consecutive frames, the first valid frame is used as a reference. The difference between the visual relative heading angle increment and the corresponding inertial navigation heading angle increment of each subsequent valid frame is taken as the short-term heading angle drift error. The error is then weighted and fused in combination with the visual observation quality of each valid frame and the motion state of the underwater vehicle to update the heading angle drift correction. Using the underwater vehicle's speed and the corrected inertial navigation heading angle, the current local pose of the underwater vehicle is inertially recursively calculated. For unrecorded beacons, the local pose of the beacon is obtained based on the current local pose of the underwater vehicle and the visual relative pose. When the difference between the local poses of the beacons determined within consecutive preset frames is less than a preset threshold, the local pose of the beacon is locked as the map reference pose of the corresponding beacon, and the corresponding beacon is recorded. When a recorded beacon is observed, the predicted relative heading angle is calculated based on the current local pose of the underwater vehicle and the map reference pose of the beacon. The predicted relative heading angle is subtracted from the current visual relative heading angle to obtain the long-term heading angle drift error. The heading angle drift correction is updated while keeping the map reference pose unchanged. The inertial navigation heading angle is corrected using the heading angle drift correction.

2. The underwater vehicle heading self-calibration method according to claim 1, characterized in that, The artificial visual beacon includes a beacon body and multiple coded tags disposed on different sides of the beacon body; different coded tags have different tag numbers, and a mapping relationship between each tag number and the identification code of the beacon to which it belongs and the corresponding side is pre-established.

3. The underwater vehicle heading self-calibration method according to claim 2, characterized in that, When multiple coded tags of the same artificial visual beacon are identified in the same frame of monocular image, the visual relative pose corresponding to each coded tag is solved separately, and the visual relative poses are unified to the beacon coordinate system according to the installation relationship of the coded tags relative to the beacon body. The consistency of translation residuals and rotation residuals is checked on the unified visual relative poses, and the visual relative poses that pass the consistency check are weighted and fused according to the visual observation quality of each coded tag.

4. The underwater vehicle heading self-calibration method according to claim 1, characterized in that, For each frame of monocular image, the data with the smallest timestamp difference is determined in the inertial navigation data; when the timestamp difference is not greater than a preset synchronization threshold, the inertial navigation data with the closest timestamp is used or adjacent inertial navigation data are interpolated; when the timestamp difference is greater than the preset synchronization threshold, the visual correction observation corresponding to that frame of monocular image is discarded and inertial recursion is maintained.

5. The underwater vehicle heading self-calibration method according to claim 1, characterized in that, The effective frames satisfy the following conditions: the same artificial visual beacon is identified within a consecutive preset number of frames, the recognition confidence of the coded label is not lower than the confidence threshold, the reprojection error is not greater than the error threshold, the imaging area is not less than the area threshold, and the translational and rotational fluctuations of the visual relative pose of consecutive frames are not greater than the corresponding fluctuation thresholds. When the number of valid frames within a continuous observation window is less than a preset number, the heading angle drift correction is not updated.

6. The underwater vehicle heading self-calibration method according to claim 1, characterized in that, The inertial navigation data also includes acceleration; the visual observation quality is determined based on the recognition confidence of the coded tag, reprojection error, imaging area, and image sharpness; the underwater vehicle motion state is determined based on the equivalent angular velocity determined by adjacent inertial navigation heading angles and corresponding time differences, as well as the acceleration change in adjacent frames; the weight of each effective frame is determined based on the visual observation quality and the underwater vehicle motion state, and the various short-term heading angle drift errors are fused using a circular weighting method.

7. The underwater vehicle heading self-calibration method according to claim 1, characterized in that, A local beacon map is established using the identification code of the artificial visual beacon as an index. The local beacon map records at least the local beacon position, the local beacon yaw angle, the cumulative number of observations, and the cumulative observation quality. During the initial map building process, the local beacon pose is updated with a preset gain based on continuous observation results that meet the observation quality conditions. When the cumulative number of observations reaches a preset number and the stability of the local beacon pose meets a preset condition, the map reference pose of the corresponding beacon is locked.

8. The underwater vehicle heading self-calibration method according to claim 1, characterized in that, The heading angle drift correction is updated using long-term heading angle drift error only when the currently identified identity code is consistent with the recorded beacon, the time synchronization error and visual observation quality meet the corresponding thresholds, the translational residual and rotational residual between the predicted relative pose and the current visual relative pose meet the corresponding thresholds, and the number of consecutive valid frames reaches a preset number. When multiple recorded beacons are observed at the same time, a circumferential consistency check is performed on the long-term heading angle drift errors corresponding to each beacon, and long-term heading angle drift errors that do not meet the consistency conditions are eliminated. The remaining long-term heading angle drift errors are then weighted and fused based on the cumulative observation quality of the corresponding beacon and the current visual observation quality.

9. A self-calibration system for the heading of an underwater vehicle assisted by a visual beacon of unknown pose, characterized in that, The device includes at least one fixed artificial visual beacon with no pre-calibrated global pose, and a monocular image acquisition module, an inertial navigation module, and a data processing module mounted on the underwater vehicle. The artificial visual beacon has an coded tag for identification, the geometric dimensions of which and its installation relationship relative to the beacon body are known. The monocular image acquisition module and the inertial navigation module are respectively connected to the data processing module, which is configured to perform the underwater vehicle heading self-calibration method according to any one of claims 1 to 8.

10. The underwater vehicle heading self-calibration system according to claim 9, characterized in that, The data processing module includes a time synchronization unit, a visual relative pose solving unit, a first observation short-term correction unit, a local beacon map unit, an inertial recursion unit, a re-observation long-term correction unit, and a heading output unit. The time synchronization unit is used to synchronize monocular images and inertial navigation data. The visual relative pose solving unit is used to solve the visual relative pose of the underwater vehicle relative to artificial visual beacons. The first observation short-term correction unit is used to update the heading angle drift correction amount based on the short-term heading angle drift error. The local beacon map unit is used to establish and store the map reference pose of the beacons. The inertial recursion unit is used to recursively calculate the current local pose of the underwater vehicle; the long-term correction unit is used to update the heading angle drift correction based on the long-term heading angle drift error; and the heading output unit is used to correct the inertial navigation heading angle using the heading angle drift correction.

Images