A multi-modal sensor extrinsic parameter automatic calibration method and system

Abstract

The present application relates to the technical field of data fusion, and specifically relates to a multi-modal sensor external parameter automatic calibration method and system, which comprises the following steps: acquiring a data frame sequence of a first sensor on a ship body and a second sensor on a mast and horizontal plane velocity data of a second sensor position, performing time derivative operation on the velocity data to obtain acceleration components and calculating a synthetic vector amplitude to identify a disturbance time period, obtaining a horizontal displacement sequence through time integration of the velocity data, combining vertical height to calculate a mast swing angle change sequence, converting the mast swing angle change sequence into a sensor space posture offset angle sequence, and finally performing coordinate transformation operation on the initial external parameter calibration parameters to obtain corrected external parameter calibration parameters. The present application can identify and compensate position offsets caused by external disturbances on sensor fixed structures in real time, and ensure the accuracy and reliability of multi-sensor data fusion.

Description

Technical Field

[0001] This invention relates to the field of data fusion technology, specifically to an automatic calibration method and system for the extrinsic parameters of multimodal sensors. Background Technology

[0002] Multimodal sensors are sensing devices that collect and fuse two or more physical quantities or environmental information. For example, sensors used in autonomous driving applications include LiDAR and vision cameras. Each sensor has its own observation coordinate system. When autonomous vehicles or ships integrate these sensors, the data from multiple sensors needs to be fused before use. The role of multimodal sensors is to integrate the advantages of multiple sensor types to improve recognition accuracy and precision. For instance, LiDAR is suitable for high-precision 3D maps and can accurately measure distances and locate objects; infrared sensors are suitable for low visibility and offer good night vision; and vision cameras can acquire rich texture and color information, which, combined with AI technology, can accurately identify object categories. In the same sensing space, calibrating the coordinate systems of all sensors and calculating the coordinate mapping relationship between their own coordinate systems is the extrinsic parameter calibration process. Extrinsic parameters include relative translation and rotation.

[0003] Multiple sensors are rigidly mounted on autonomous driving devices, each with its own local coordinate system. Extrinsic parameter calibration is needed to determine the pose transformation relationships between the sensors and relative to the autonomous driving device, and to unify the data from different sensors into a global coordinate system. Inaccurate pose relationships can lead to redundancy or contradictions in multi-sensor information, resulting in incorrect judgments about the environment and the sensor's own state, thus affecting subsequent planning decisions. Once the extrinsic parameters of multiple sensors are calibrated, they are generally not recalibrated unless the positions of multiple sensors change significantly. For example, existing unmanned surface vessels (USVs) used in water quality monitoring, disaster relief, patrol, and reconnaissance typically have lidar and cameras mounted on the mast or superstructure. Because ships are constantly exposed to waves and wind, their extrinsic parameters are prone to slow drift, necessitating a solution for extrinsic parameter calibration after sensor displacement caused by external physical disturbances. Summary of the Invention

[0004] (1) Technical problems to be solved

[0005] The purpose of this invention is to provide a method and system for automatic calibration of extrinsic parameters of multimodal sensors, so as to solve the problem of positional displacement caused by external disturbances to the fixed structure of the sensor in real time, and ensure the accuracy and reliability of multi-sensor data fusion.

[0006] (2) Technical solution

[0007] To achieve the above objectives, in one aspect, the present invention provides an automatic calibration method for the extrinsic parameters of a multimodal sensor, the method comprising:

[0008] Acquire the first sensor data frame sequence of the first sensor installed on the hull and the second sensor data frame sequence of the second sensor installed on the mast. The first and second sensor data frame sequences are continuously obtained through a fixed sampling frequency and marked with timestamp sequences. Acquire the data frame sequences of the second sensor along the X and Y axes in the horizontal direction. Shaft velocity data sequence and Shaft velocity data sequence, the Shaft velocity data sequence and The shaft speed data sequence is acquired by a speed sensor installed at the same location as the second sensor. Shaft velocity data sequence and The shaft velocity data sequence is time-synchronized with the second sensor data frame sequence;

[0009] right Shaft velocity data sequence and The time derivatives of the shaft velocity data sequences were calculated respectively. Axial acceleration component sequence and Calculate the axis acceleration component sequence. Axial acceleration component sequence and The composite vector amplitude sequence of the axial acceleration component sequence in the horizontal plane is used to mark the time period when the composite vector amplitude sequence exceeds a preset disturbance threshold as the disturbance time period sequence;

[0010] Obtain the vertical height of the second sensor on the mast and through Shaft velocity data sequence and The horizontal displacement sequence corresponding to the vertical height is obtained by time integration of the shaft velocity data sequence within the disturbance time period sequence. The mast swing angle change sequence is calculated based on the horizontal displacement sequence and the vertical height. The mast swing angle change sequence is then converted into the spatial attitude offset angle sequence of the second sensor relative to the first sensor.

[0011] The corresponding time series of disturbances Shaft velocity data sequence The spatial offset sequence is obtained by combining the axis velocity data sequence and the spatial attitude offset angle sequence. The spatial offset sequence is then subjected to coordinate transformation with the initial extrinsic calibration parameters to obtain the corrected extrinsic calibration parameters.

[0012] Furthermore, the method for calculating the mast swing angle change sequence based on the horizontal displacement sequence and the vertical height includes:

[0013] The vertical height of the second sensor on the mast is recorded as follows: Obtain the first time interval within the perturbation time period sequence. The corresponding time Shaft speed data is denoted as and Shaft speed data is denoted as ,in The value ranges from 1 to the total number of sampling points within the perturbation time period sequence. Integer variables;

[0014] The time interval corresponding to the fixed sampling frequency is denoted as . ,right Shaft speed data and The shaft velocity data are integrated over time to calculate the time from the start of the disturbance period to the [missing value]. At that moment Horizontal displacement in the axial direction and in Horizontal displacement in the axial direction :

[0015] ;

[0016] ;

[0017] in From 1 to Integer variables; Horizontal displacement sequence in axial direction and The horizontal displacement sequences along the axial direction are processed by moving average filtering, with the filter window length set to the total number of sampling points. Divide by the preset segmentation coefficient and round down to obtain the filtered result. Axial smooth displacement sequence and Axial smooth displacement sequence ;

[0018] Calculate the first Horizontal composite displacement amplitude at time 1 :

[0019] ;

[0020] By vertical height Combined horizontal displacement amplitude The ratio calculation of the arctangent function value to the first The initial swing angle of the mast relative to the vertical direction at that moment :

[0021] ;

[0022] Establish a physical constraint model for the mast's swaying, and calculate the difference in the initial sway angle between two adjacent moments, denoted as the rate of angle change. :

[0023] ;

[0024] When the rate of change of angle When the absolute value exceeds the preset angle change rate threshold, it is determined that there is an abnormal disturbance in the data at that moment;

[0025] The initial swing angle corresponding to the moment when abnormal disturbance exists. The corrected mast swing angle is obtained by replacing the initial swing angle with the weighted average of the initial swing angles at adjacent, non-abnormal times. ;

[0026] The mast swing angle corrected for all moments within the perturbation time series. Arranged in chronological order to form a sequence of changes in the mast's swing angle.

[0027] Furthermore, the initial swing angle corresponding to the moment when abnormal disturbances occur... Methods for replacing the initial swing angles with a weighted average of adjacent, non-abnormal time points include:

[0028] For each moment where an abnormal disturbance occurs, its index position in the sequence is obtained and denoted as . And identify the index position of the previous non-abnormal time. and the index position at the next non-abnormal time. ;

[0029] Calculate time ratio parameters The corrected mast sway angle was calculated using linear interpolation. ,in and These are the initial swing angles at the previous and subsequent non-abnormal times, respectively.

[0030] Furthermore, the method for converting the mast swing angle change sequence into a spatial attitude offset angle sequence of the second sensor relative to the first sensor includes:

[0031] Obtain the installation offset vector of the second sensor relative to the fixed point at the bottom of the mast, the installation offset vector including a radial distance component. and axial height component ;

[0032] Corrected mast swing angle Decomposed into pitch angle components around a fixed point at the base of the mast. and roll angle component ,in It is an integer variable whose value ranges from 1 to the total number of sampling points within the perturbation time period sequence;

[0033] The spatial coordinates of the second sensor under the mast swing state are represented as a three-dimensional vector in the coordinate system of the fixed point at the bottom of the mast. Axis components are , Axis components are , Axis components are ,in , , These are the three-dimensional coordinate components of the mast bottom fixed point in the ship's coordinate system;

[0034] The spatial displacement vector sequence is calculated by the difference in three-dimensional coordinates before and after the spatial position transformation of the second sensor, forming a spatial attitude offset angle sequence of the second sensor relative to the first sensor.

[0035] Furthermore, the method for obtaining the corrected extrinsic calibration parameters by performing coordinate transformation operations between the spatial offset sequence and the initial extrinsic calibration parameters includes:

[0036] Obtain the initial extrinsic calibration parameters of the first and second sensors, wherein the initial extrinsic calibration parameters include the initial rotation matrix. and initial translation vector ;

[0037] Establish an optimization objective function for multi-sensor extrinsic parameter calibration, and obtain the first... Measurement error per data frame Time interval and signal-to-noise ratio ,pass Calculate the first The weight coefficients of each data frame are used to establish the objective function. ,in This represents the total number of valid data frames within the perturbation time period sequence. For the first sensor in the first The target point coordinate vector at each moment For the second sensor in the first The source point coordinate vector at each moment. This is the corrected rotation matrix. The corrected translation vector. The characteristic time constant of the mast oscillation;

[0038] The objective function is solved by constraint optimization using the Lagrange multiplier method, and the optimal rotation matrix is ​​calculated using the iterative nearest-point algorithm to obtain the corrected extrinsic calibration parameters. and ,in For dynamic rotation correction matrix, This is the dynamic translation correction vector.

[0039] Based on the same inventive concept, this invention also provides an automatic calibration system for the extrinsic parameters of a multimodal sensor, the system comprising:

[0040] The data acquisition module is used to acquire the first sensor data frame sequence of the first sensor installed on the hull and the second sensor data frame sequence of the second sensor installed on the mast. The first and second sensor data frame sequences are continuously acquired through a fixed sampling frequency and marked with timestamp sequences. The module also acquires the data frame sequences corresponding to the X and Y axes of the second sensor in the horizontal plane. Shaft speed data sequence and Shaft velocity data sequence, the Shaft speed data sequence and The shaft speed data sequence is acquired by a speed sensor installed at the same location as the second sensor. Shaft speed data sequence and The shaft velocity data sequence is time-synchronized with the second sensor data frame sequence;

[0041] The disturbance analysis module is used to analyze... Shaft speed data sequence and The time derivatives of the shaft velocity data sequences were calculated respectively. Axial acceleration component sequence and Calculate the sequence of axis acceleration components. Axial acceleration component sequence and The composite vector amplitude sequence of the axial acceleration component sequence in the horizontal plane is used to mark the time period when the composite vector amplitude sequence exceeds a preset disturbance threshold as the disturbance time period sequence;

[0042] Angle offset calculation module is used to obtain the vertical height of the second sensor on the mast and... Shaft speed data sequence and The horizontal displacement sequence corresponding to the vertical height is obtained by time integration of the shaft velocity data sequence within the disturbance time period sequence. The mast swing angle change sequence is calculated based on the horizontal displacement sequence and the vertical height. The mast swing angle change sequence is then converted into the spatial attitude offset angle sequence of the second sensor relative to the first sensor.

[0043] The extrinsic parameter compensation calibration module is used to calibrate the parameters corresponding to the disturbance time period sequence. Shaft velocity data sequence The spatial offset sequence is obtained by combining the axis velocity data sequence and the spatial attitude offset angle sequence. The spatial offset sequence is then subjected to coordinate transformation with the initial extrinsic calibration parameters to obtain the corrected extrinsic calibration parameters.

[0044] Furthermore, the system also includes:

[0045] The vertical height of the second sensor on the mast is recorded as follows: Obtain the first time interval within the perturbation time period sequence. The corresponding time Shaft speed data is denoted as and Shaft speed data is denoted as ,in The value ranges from 1 to the total number of sampling points within the perturbation time period sequence. Integer variables;

[0046] The time interval corresponding to the fixed sampling frequency is denoted as . ,right Shaft speed data and The shaft velocity data are integrated over time to calculate the time from the start of the disturbance period to the [missing value]. At that moment Horizontal displacement in the axial direction and in Horizontal displacement in the axial direction :

[0047] ;

[0048] ;

[0049] in From 1 to Integer variables; Horizontal displacement sequence in axial direction and The horizontal displacement sequences along the axial direction are processed by moving average filtering, with the filter window length set to the total number of sampling points. Divide by the preset segmentation coefficient and round down to obtain the filtered result. Axial smooth displacement sequence and Axial smooth displacement sequence ;

[0050] Calculate the first Horizontal composite displacement amplitude at time 1 :

[0051] ;

[0052] By vertical height Combined horizontal displacement amplitude The ratio calculation of the arctangent function value to the first The initial swing angle of the mast relative to the vertical direction at that moment :

[0053] ;

[0054] Establish a physical constraint model for the mast's swaying, and calculate the difference in the initial sway angle between two adjacent moments, denoted as the rate of angle change. :

[0055] ;

[0056] When the rate of change of angle When the absolute value exceeds the preset angle change rate threshold, it is determined that there is an abnormal disturbance in the data at that moment;

[0057] The initial swing angle corresponding to the moment when abnormal disturbance exists. The corrected mast swing angle is obtained by replacing the initial swing angle with the weighted average of the initial swing angles at adjacent, non-abnormal times. ;

[0058] The mast swing angle corrected for all moments within the perturbation time series. Arranged in chronological order to form a sequence of changes in the mast's swing angle.

[0059] Furthermore, the system also includes:

[0060] The time difference correction module is used to obtain the index position in the sequence for each time with an abnormal disturbance, denoted as . And identify the index position of the previous non-abnormal time. and the index position at the next non-abnormal time. ;

[0061] Calculate time ratio parameters The corrected mast sway angle was calculated using linear interpolation. ,in and These are the initial swing angles at the previous and subsequent non-abnormal times, respectively.

[0062] Furthermore, the system also includes:

[0063] The coordinate transformation module is used to obtain the installation offset vector of the second sensor relative to the fixed point at the bottom of the mast, the installation offset vector including a radial distance component. and axial height component ;

[0064] Corrected mast swing angle Decomposed into pitch angle components around a fixed point at the base of the mast. and roll angle component ,in It is an integer variable whose value ranges from 1 to the total number of sampling points within the perturbation time period sequence;

[0065] The spatial coordinates of the second sensor under the mast swing state are represented as a three-dimensional vector in the coordinate system of the fixed point at the bottom of the mast. Axis components are , Axis components are , Axis components are ,in , , These are the three-dimensional coordinate components of the mast bottom fixed point in the ship's coordinate system;

[0066] The spatial displacement vector sequence is calculated by the difference in three-dimensional coordinates before and after the spatial position transformation of the second sensor, forming a spatial attitude offset angle sequence of the second sensor relative to the first sensor.

[0067] Furthermore, the system also includes:

[0068] The coordinate synthesis module is used to obtain the initial extrinsic calibration parameters of the first and second sensors, wherein the initial extrinsic calibration parameters include the initial rotation matrix. and initial translation vector ;

[0069] Establish an optimization objective function for multi-sensor extrinsic parameter calibration, and obtain the first... Measurement error per data frame Time interval and signal-to-noise ratio ,pass Calculate the first The weight coefficients of each data frame are used to establish the objective function. ,in This represents the total number of valid data frames within the perturbation time period sequence. For the first sensor in the first The target point coordinate vector at each moment For the second sensor in the first The source point coordinate vector at each moment. This is the corrected rotation matrix. The corrected translation vector. The characteristic time constant of the mast oscillation;

[0070] The objective function is solved by constraint optimization using the Lagrange multiplier method, and the optimal rotation matrix is ​​calculated using the iterative nearest-point algorithm to obtain the corrected extrinsic calibration parameters. and ,in For dynamic rotation correction matrix, This is the dynamic translation correction vector.

[0071] (3) Beneficial effects

[0072] Compared with the prior art, the beneficial effect of the present invention is to ensure the accuracy and reliability of multi-sensor data fusion by identifying and compensating for the positional displacement caused by external disturbances to the sensor fixing structure in real time. Attached Figure Description

[0073] Figure 1 This is a block diagram of an automatic calibration method for extrinsic parameters of a multimodal sensor according to Embodiment 1 of the present invention. Detailed Implementation

[0074] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0075] Before providing examples, it's necessary to describe the application scenario of this invention. In a hydrological monitoring activity prior to a flood peak, an automatic monitoring vessel equipped with millimeter-wave radar, lidar, and a vision camera is deployed. The use of multiple sensors has its practical applications: lidar offers high accuracy, collects a large amount of information, and is sensitive to target size, but it's unsuitable for harsh weather conditions; millimeter-wave radar, while less accurate than lidar, directly provides target position, distance, velocity, and acceleration information, and exhibits high reliability even in harsh environments; the vision camera is primarily used for on-site situation gathering and feature object recognition. The millimeter-wave radar is mounted on the hull and has two target perception modes: Object mode and Cluster mode, which can be switched in real-time during perception. When sensing surrounding obstacles, the millimeter-wave radar enters Object mode, directly returning the position, distance, velocity, and acceleration information of multiple targets, thus assisting in the vessel's obstacle avoidance function within a small area; when automatically berthing or unberthing, the millimeter-wave radar enters Cluster mode to identify the embankment, outputting a two-dimensional sparse point cloud for embankment identification, further assisting in the vessel's automatic berthing and unberthing functions. Multiple lidar and vision cameras are mounted on the hull and stern mast, requiring a wider field of view to identify objects including hydrological conditions and higher-precision obstacle recognition (with some autonomous driving capabilities). During hydrological monitoring before the flood peak, the current is turbulent, accompanied by strong winds that disturb the unmanned vessel. Under these conditions, the pre-calibrated extrinsic parameters may be misidentified due to coordinate system errors caused by the strong disturbances, leading to data fusion errors from multiple sensors. Essentially, the external disturbances to the unmanned vessel cause the sensor mast to sway back and forth. Different intensities of external disturbances result in varying amplitudes of this swaying. The sensor data generated during this swaying process, along with the data from the ship's sensors, are not uniformly regular but rather disordered. Simply finding the midpoint of the sway to determine the mast's original position has limitations. Therefore, it is necessary to promptly identify and assess the impact of the disturbances on the parameters and select sensor data at the corresponding time points for automatic extrinsic parameter calibration and optimization based on the disturbance's effect on the parameters.

[0076] Example 1: As Figure 1 As shown in the figure, this embodiment provides an automatic calibration method for the extrinsic parameters of a multimodal sensor, the method comprising:

[0077] Acquire the first sensor data frame sequence of the first sensor installed on the hull and the second sensor data frame sequence of the second sensor installed on the mast. The first and second sensor data frame sequences are continuously obtained through a fixed sampling frequency and marked with timestamp sequences. Acquire the data frame sequences of the second sensor along the X and Y axes in the horizontal direction. Shaft velocity data sequence and Shaft velocity data sequence, the Shaft velocity data sequence and The shaft speed data sequence is acquired by a speed sensor installed at the same location as the second sensor. Shaft velocity data sequence and The shaft velocity data sequence is time-synchronized with the second sensor data frame sequence;

[0078] right Shaft velocity data sequence and The time derivatives of the shaft velocity data sequences were calculated respectively. Axial acceleration component sequence and Calculate the axis acceleration component sequence. Axial acceleration component sequence and The composite vector amplitude sequence of the axial acceleration component sequence in the horizontal plane is used to mark the time period when the composite vector amplitude sequence exceeds a preset disturbance threshold as the disturbance time period sequence;

[0079] Obtain the vertical height of the second sensor on the mast and through Shaft velocity data sequence and The horizontal displacement sequence corresponding to the vertical height is obtained by time integration of the shaft velocity data sequence within the disturbance time period sequence. The mast swing angle change sequence is calculated based on the horizontal displacement sequence and the vertical height. The mast swing angle change sequence is then converted into the spatial attitude offset angle sequence of the second sensor relative to the first sensor.

[0080] The corresponding time series of disturbances Shaft velocity data sequence The spatial offset sequence is obtained by combining the axis velocity data sequence and the spatial attitude offset angle sequence. The spatial offset sequence is then subjected to coordinate transformation with the initial extrinsic calibration parameters to obtain the corrected extrinsic calibration parameters.

[0081] For example, in a hydrological monitoring mission during the flood season in the middle reaches of the Yangtze River, an unmanned monitoring vessel was dispatched to collect and monitor flood peak data. A millimeter-wave radar was installed at the bottom of the vessel as the first sensor, and a solid-state lidar was installed at the top of the stern mast as the second sensor. The vertical height of the mast from the deck to the lidar installation location was 3.2 meters. The fixed sampling frequency for both the millimeter-wave radar and the solid-state lidar was set to 20 frames per second, with a time interval of 0.05 seconds. Starting at the mission start time (recorded as time zero), data was continuously collected for 300 seconds. The millimeter-wave radar acquired 6000 frames of data from the first sensor, and the solid-state lidar acquired 6000 frames of data from the second sensor. Each frame was marked with a GPS-timed timestamp with microsecond accuracy. Simultaneously, a dual-axis MEMS velocity sensor installed at the same location as the solid-state lidar synchronously recorded the horizontal X-axis and Y-axis velocities. The MEMS velocity sensor also sampled at 20 frames per second, with the time synchronization error between the MEMS and solid-state lidar data frames controlled within 1 millisecond. The dual-axis MEMS velocity sensor acquires velocity data by measuring inertial motion, with a measurement range of -5 m / s to +5 m / s and a measurement accuracy of 0.01 m / s. The initial extrinsic calibration parameters are recorded as follows: the spatial position of the solid-state lidar relative to the millimeter-wave radar: X-axis positive 0.80 m, Y-axis positive 0.30 m, Z-axis positive 3.20 m; rotational attitude: pitch angle ±2.0 degrees, roll angle ±1.0 degrees, and yaw angle 0 degrees. These initial extrinsic calibration parameters were obtained using a high-precision total station at a calibration site before the unmanned monitoring vessel left the factory. During calibration, the vessel was stationary and free from external disturbances.

[0082] The calculation method involves dividing the difference in velocity data between two adjacent frames by a fixed time interval of 0.05 seconds to obtain the X-axis acceleration component sequence and the Y-axis acceleration component sequence. Taking frames 100 to 101 as an example, the X-axis velocity in frame 100 is 0.12 m / s and the Y-axis velocity is 0.09 m / s, while in frame 101, the X-axis velocity is 0.18 m / s and the Y-axis velocity is 0.13 m / s. Therefore, the X-axis acceleration component in frame 101 is calculated as 0.18 minus 0.12 divided by 0.05, which equals 1.20 m / s², and the Y-axis acceleration component is calculated as 0.13 minus 0.09 divided by 0.05, which equals 0.80 m / s². The horizontal plane composite vector amplitude is calculated for the X-axis and Y-axis acceleration components of each frame. The composite vector amplitude for frame 101 is the square root of 1.20² plus 0.80², resulting in 1.44 m / s². A preset disturbance threshold of 0.50 m / s² was set. This threshold was chosen because the horizontal acceleration of a ship under normal navigation conditions is usually below 0.30 m / s², while the acceleration increases significantly when encountering strong winds or turbulent disturbances. 0.50 m / s² can effectively distinguish between normal navigation and disturbance states. The entire sequence of 6000 frames of synthetic vector amplitude was traversed, and a disturbance state was determined when the synthetic vector amplitude exceeded 0.50 m / s². Statistical analysis identified three consecutive disturbance time periods: the first period was from frame 500 to 800 (301 frames), the second period was from frame 1200 to 1600 (401 frames), and the third period was from frame 2300 to 2800 (501 frames). The three disturbance time period sequences contained a total of 1203 sampling points, corresponding to a disturbance duration of 60.15 seconds. The reason for choosing the disturbance time periods is that mast swaying mainly occurs during periods of strong external disturbance. At this time, the sensor data collected contains true position and orientation offset information, while during calm periods, the mast remains basically stationary and does not require correction. The three disturbance time periods correspond to three significant external disturbance events encountered by the monitoring vessel: the first is a lateral gust of wind lasting 15 seconds, the second is a rapid water flow impact lasting 20 seconds, and the third is a combined effect of wind and waves lasting 25 seconds.

[0083] The vertical height of the second sensor, the solid-state lidar, on the mast was determined to be 3.2 meters. This vertical height was obtained by measuring vertically from the ship's deck plane to the center point of the lidar base with a measuring tape, with the measurement error controlled within 0.01 meters. For the identified disturbance time period sequence, time integration was performed on the X-axis velocity data sequence and the Y-axis velocity data sequence to calculate the horizontal displacement. Taking frames 500 to 800 of the first disturbance time period as an example, frame 500 was taken as the integration start time, and the initial value of the horizontal displacement was set to zero. At frame 501, the X-axis velocity was 0.15 meters per second, with a time interval of 0.05 seconds, and the accumulated horizontal displacement in the X-axis direction was 0.15 multiplied by 0.05, which equals 0.0075 meters; at frame 502, the X-axis velocity was 0.18 meters per second, and the accumulated horizontal displacement in the X-axis direction was 0.0075 plus 0.18 multiplied by 0.05, which equals 0.0165 meters, and so on, accumulating frame by frame. By frame 550, 50 integration operations had been performed, resulting in a cumulative displacement of 0.082 meters in the X-axis direction and 0.061 meters in the Y-axis direction. By frame 800, the end of the first disturbance period, 301 integration operations had been performed, with a maximum cumulative displacement of 0.247 meters in the X-axis direction and 0.183 meters in the Y-axis direction, forming a horizontal displacement sequence within the disturbance period. The second and third disturbance periods restarted integration calculations from their respective starting frames. By frame 1600, after 401 integrations in the second period, the maximum X-axis displacement was 0.195 meters and the maximum Y-axis displacement was 0.221 meters. By frame 2800, after 501 integrations in the third period, the maximum X-axis displacement was 0.289 meters and the maximum Y-axis displacement was 0.176 meters. The integration operation used a discrete-time summation method, essentially treating the velocity within each time interval as a constant value for rectangular integration. This method can obtain high-precision displacement estimates when the sampling frequency is sufficiently high.

[0084] The change in mast swing angle is calculated based on the horizontal displacement and vertical height of 3.2 meters at each sampling point. Taking frame 550 as an example, the horizontal composite displacement amplitude is obtained by combining the X-axis displacement of 0.082 meters and the Y-axis displacement of 0.061 meters. This is calculated as the square root of 0.082 (0.006724) plus the square root of 0.061 (0.003721), which is 0.010445, equal to 0.102 meters. The mast swing angle is calculated by dividing the horizontal composite displacement of 0.102 meters by the vertical height of 3.2 meters, yielding an arctangent function. The ratio is 0.0319, and the arctangent is 1.83 degrees. This calculation is based on the small-angle approximation assumption. When the swing angle is less than 10 degrees, the trajectory of the mast tip can be approximated as a circular arc, and the ratio of horizontal displacement to vertical height equals the tangent of the swing angle. At frame 800, the horizontal composite displacement is 0.247 squared (0.061009) plus 0.183 squared (0.033489), summed to 0.094498, which takes the square root of 0.307 meters. The corresponding swing angle is 0.307 divided by 3.2 (0.0959 arctangent), equal to 5.48 degrees. The swing angles of 1203 disturbance sampling points are calculated sequentially, forming a sequence of mast swing angle changes, ranging from 0.35 degrees to 5.82 degrees. The mast swing angle is then converted into the spatial attitude offset angle of the second sensor relative to the first sensor. This conversion process requires calculating the pitch and roll components based on the displacement directions along the X and Y axes. At frame 550, an X-axis displacement of 0.082 meters corresponds to a pitch angle component with an arctangent of 0.0256 (0.082 divided by 3.2), which equals 1.47 degrees. A Y-axis displacement of 0.061 meters corresponds to a roll angle component with an arctangent of 0.0191 (0.061 divided by 3.2), which equals 1.09 degrees. At frame 800, the pitch angle component has an arctangent of 0.0772 (0.247 divided by 3.2), reaching 4.42 degrees, and the roll angle component has an arctangent of 0.0572 (0.183 divided by 3.2), reaching 3.27 degrees. Traversing all disturbance time periods yields a sequence of spatial attitude offset angles. The pitch angle offset ranges from -3.15 degrees to +4.88 degrees, the roll angle offset ranges from -2.23 degrees to +3.67 degrees, and the yaw angle offset remains within a small range of -0.5 degrees to +0.5 degrees because the mast primarily swings within the vertical plane. Negative angles occur when the mast swings in the opposite direction. For example, during the mast's return swing after the first disturbance period ends, the X-axis displacement is -0.095 meters at frame 820, corresponding to a pitch angle of -2.98 degrees.

[0085] The X-axis velocity data sequence, Y-axis velocity data sequence, and spatial attitude offset angle sequence corresponding to the disturbance time period sequence are integrated into a spatial offset sequence. This sequence contains 1203 sets of data, each set containing six parameters: timestamp, X-axis velocity, Y-axis velocity, pitch offset, roll offset, and yaw offset. The spatial offset sequence is then subjected to coordinate transformation calculations with the initial extrinsic calibration parameters. The initial translation vectors are 0.80 meters for the X-axis, 0.30 meters for the Y-axis, and 3.20 meters for the Z-axis; the initial rotation angles are 2.0 degrees for pitch, 1.0 degrees for roll, and 0 degrees for yaw. During the coordinate transformation, the spatial offset in frame 550 corrected the translation vector to 0.80 + 0.082 = 0.882 meters for the X-axis, 0.30 + 0.061 = 0.361 meters for the Y-axis, and 3.20 meters for the Z-axis due to the smaller vertical displacement. The rotation angles were corrected to 2.0 + 1.47 = 3.47 degrees for the pitch angle, 1.0 + 1.09 = 2.09 degrees for the roll angle, and 0 + 0.3 = 0.3 degrees for the yaw angle. An optimization objective function was established, and a weighted average was applied to 1203 sets of data. Data sets with disturbance acceleration between 0.50 and 1.00 m / s² were weighted at 0.80 (482 sets), data sets with disturbance acceleration between 1.00 and 2.00 m / s² were weighted at 0.50 (536 sets), and data sets with disturbance acceleration exceeding 2.00 m / s² were weighted at 0.20 (185 sets). The weights are set based on the principle that the smaller the disturbance intensity, the closer the sensor pose is to the true offset state and the lower the measurement noise. However, when the disturbance intensity is too large, the mast may undergo elastic deformation or the MEMS velocity sensor may experience measurement saturation, thus reducing the weights. The optimization calculation process uses the least squares method, which calculates the weighted sum of 1203 sets of corrected translation vectors and rotation angles according to their weights, and then divides the sum of the weights to obtain the corrected extrinsic calibration parameters. Specifically, the X-axis translation component is calculated as follows: the sum of X-axis values ​​from 482 sets of data multiplied by 0.80, the sum of X-axis values ​​from 536 sets of data multiplied by 0.50, and the sum of X-axis values ​​from 185 sets of data multiplied by 0.20. This sum is then divided by the total weights: 482 multiplied by 0.80, 536 multiplied by 0.50, and 185 multiplied by 0.20 equals 839.6. This yields a corrected translation vector of 0.823 meters for the X-axis, 0.284 meters for the Y-axis, and 3.187 meters for the Z-axis. The corrected rotation angles are 2.34 degrees for pitch, 1.18 degrees for roll, and 0.08 degrees for yaw. Using these corrected extrinsic parameters, multi-sensor fusion is performed on 2000 frames of data collected over the next 100 seconds. The solid-state lidar point cloud is then transformed into a millimeter-wave radar coordinate system using the corrected extrinsic parameters for target localization. The verification method involves pre-deploying five reflective buoys in the monitored waters as ground-based true value calibration points. The buoy positions are measured using differential GPS with centimeter-level accuracy.Comparing the identified buoy positions with the ground truth values ​​from 2000 frames of data, the target positioning error decreased from an average of 0.385 meters before correction to 0.087 meters, the maximum error decreased from 0.621 meters to 0.143 meters, and the standard deviation of the error decreased from 0.152 meters to 0.038 meters. This proves that the correction of the external parameter calibration parameters effectively compensated for the sensor pose shift caused by disturbances, enabling the multi-sensor data fusion accuracy to meet the technical requirement of hydrological monitoring tasks for target positioning accuracy better than 0.10 meters.

[0086] Furthermore, the method for calculating the mast swing angle change sequence based on the horizontal displacement sequence and the vertical height includes:

[0087] The vertical height of the second sensor on the mast is recorded as follows: Obtain the first time interval within the perturbation time period sequence. The corresponding time Shaft speed data is denoted as and Shaft speed data is denoted as ,in The value ranges from 1 to the total number of sampling points within the perturbation time period sequence. Integer variables;

[0088] The time interval corresponding to the fixed sampling frequency is denoted as . ,right Shaft speed data and The shaft velocity data are integrated over time to calculate the time from the start of the disturbance period to the [missing value]. At that moment Horizontal displacement in the axial direction and in Horizontal displacement in the axial direction :

[0089] ;

[0090] ;

[0091] in From 1 to Integer variables; Horizontal displacement sequence in axial direction and The horizontal displacement sequences along the axial direction are processed by moving average filtering, with the filter window length set to the total number of sampling points. Divide by the preset segmentation coefficient and round down to obtain the filtered result. Axial smooth displacement sequence and Axial smooth displacement sequence ;

[0092] Calculate the first Horizontal composite displacement amplitude at time 1 :

[0093] ;

[0094] By vertical height Combined horizontal displacement amplitude The ratio calculation of the arctangent function value to the first The initial swing angle of the mast relative to the vertical direction at that moment :

[0095] ;

[0096] Establish a physical constraint model for the mast's swaying, and calculate the difference in the initial sway angle between two adjacent moments, denoted as the rate of angle change. :

[0097] ;

[0098] When the rate of change of angle When the absolute value exceeds the preset angle change rate threshold, it is determined that there is an abnormal disturbance in the data at that moment;

[0099] The initial swing angle corresponding to the moment when abnormal disturbance exists. The corrected mast swing angle is obtained by replacing the initial swing angle with the weighted average of the initial swing angles at adjacent, non-abnormal times. ;

[0100] The mast swing angle corrected for all moments within the perturbation time series. Arranged in chronological order to form a sequence of changes in the mast's swing angle.

[0101] For example, in the aforementioned hydrological monitoring task, for the three identified disturbance time periods, the change sequence of the mast swing angle within each time period is calculated, and the vertical height of the second sensor, i.e., the solid-state lidar, on the mast is recorded as 3.2 meters. This parameter remains constant throughout the calculation process. A detailed explanation is provided using frames 500 to 800 of the first disturbance time period as an example. This time period contains 301 sampling points, with a total number of sampling points recorded as 301. For the first moment within this disturbance time period, i.e., frame 500, the X-axis velocity data is 0.15 m / s and the Y-axis velocity data is 0.11 m / s; for the second moment, i.e., frame 501, the X-axis velocity data is 0.18 m / s and the Y-axis velocity data is 0.14 m / s; and for the third moment, i.e., frame 502, the X-axis velocity data is 0.21 m / s and the Y-axis velocity data is 0.16 m / s. Similarly, at time 50 (frame 549), the X-axis velocity data is 0.24 m / s and the Y-axis velocity data is 0.18 m / s; at time 51 (frame 550), the X-axis velocity data is 0.26 m / s and the Y-axis velocity data is 0.20 m / s. A fixed sampling frequency of 20 frames per second corresponds to a time interval of 0.05 seconds, which is used as a fixed parameter in all integration calculations.

[0102] The horizontal displacements in the X-axis and Y-axis directions were calculated by performing time integration on the X-axis and Y-axis velocity data at 0.05-second intervals, from the start of the disturbance period (frame 500) to each subsequent moment. For the first moment (frame 500), since it is the integration starting point, the horizontal displacements in both the X-axis and Y-axis directions are zero. For the second moment (frame 501), the horizontal displacement in the X-axis direction is calculated as 0.15 multiplied by 0.05, equaling 0.0075 meters, and the horizontal displacement in the Y-axis direction is calculated as 0.11 multiplied by 0.05, equaling 0.0055 meters. For the third moment (frame 502), the horizontal displacement in the X-axis direction is calculated as 0.0075 plus 0.18 multiplied by 0.05, equaling 0.0165 meters, and the horizontal displacement in the Y-axis direction is calculated as 0.0055 plus 0.14 multiplied by 0.05, equaling 0.0125 meters. Continuing with the cumulative integration, for the 51st time point (frame 550), after 50 cumulative integrations, the horizontal displacement in the X-axis direction is calculated as 0.15 x 0.05 + 0.18 x 0.05 + 0.21 x 0.05, accumulated sequentially to 0.24 x 0.05, resulting in 0.082 meters; the horizontal displacement in the Y-axis direction is calculated as 0.11 x 0.05 + 0.14 x 0.05 + 0.16 x 0.05, accumulated sequentially to 0.18 x 0.05, resulting in 0.061 meters. For the 301st time point (frame 800), after 300 cumulative integrations, the horizontal displacement in the X-axis direction is 0.247 meters, and the horizontal displacement in the Y-axis direction is 0.183 meters. Using the same method, the horizontal displacement in the second perturbation period (frames 1200 to 1600) and the third perturbation period (frames 2300 to 2800) were calculated. At the 401st moment of the second period (frame 1600), the horizontal displacement in the X-axis direction was 0.195 meters and the horizontal displacement in the Y-axis direction was 0.221 meters. At the 501st moment of the third period (frame 2800), the horizontal displacement in the X-axis direction was 0.289 meters and the horizontal displacement in the Y-axis direction was 0.176 meters.

[0103] The horizontal displacement sequences along the X-axis and Y-axis are processed using moving average filtering. The filter window length is set to the total number of sampling points divided by a preset segmentation coefficient and then rounded down. For the first disturbance time period, the total number of sampling points is 301, and the preset segmentation coefficient is set to 10. Therefore, the filter window length is 301 divided by 10, which equals 30.1, and then rounded down to 30. The purpose of moving average filtering is to eliminate high-frequency noise and measurement errors accumulated during velocity integration, making the displacement curve smoother and more accurately reflecting the actual swing trajectory of the mast. For the horizontal displacement sequence along the X-axis, moving average filtering is applied. For times 15 to 286, the average value of 30 sampling points (15 points before and after each time step) is used as the filtered value. For example, at time 51 (frame 550), 30 horizontal displacement values ​​in the X-axis direction are obtained from time 36 to time 65, which are 0.051 m, 0.054 m, 0.057 m, 0.088 m, and 0.091 m respectively. Summing these 30 values ​​and dividing by 30 yields a filtered smoothed X-axis displacement of 0.076 m. Similarly, a moving average filter is applied to the horizontal displacement sequence in the Y-axis direction; the filtered smoothed Y-axis displacement at time 51 (frame 550) is 0.055 m. For the first 15 and last 15 time points, since sufficient sampling points are unavailable, the average of the available sampling points is used for filtering. After filtering, smoothed X-axis and Y-axis displacement sequences are obtained, with a sequence length of 301 sampling points. The filter window length for the second disturbance period is 401 divided by 10, which equals 40.1, and then rounded down to 40. The filter window length for the third disturbance period is 501 divided by 10, which equals 50.1, and then rounded down to 50.

[0104] The horizontal composite displacement amplitude at each time point is calculated. For time point 51 (frame 550), the filtered smoothed displacement in the X-axis direction is 0.076 meters, and the smoothed displacement in the Y-axis direction is 0.055 meters. The horizontal composite displacement amplitude is calculated as the square root of 0.076 squared (0.005776) plus 0.055 squared (0.003025), summed to 0.008801, which is 0.094 meters. For time point 101 (frame 600), the filtered smoothed displacement in the X-axis direction is 0.132 meters, and the smoothed displacement in the Y-axis direction is 0.098 meters. The horizontal composite displacement amplitude is calculated as the square root of 0.132 squared (0.017424) plus 0.098 squared (0.009604), summed to 0.027028, which is 0.164 meters. For the 301st time point (frame 800), the filtered smoothed displacement along the X-axis is 0.241 meters, and the smoothed displacement along the Y-axis is 0.179 meters. The horizontal composite displacement amplitude is calculated as the square root of 0.241 (0.058081) plus the square root of 0.179 (0.032041), summed to 0.090122, which is 0.300 meters. The horizontal composite displacement amplitude is calculated sequentially for all 301 sampling points, forming a horizontal composite displacement amplitude sequence.

[0105] The initial swing angle of the mast relative to the vertical direction at each moment is obtained by calculating the arctangent function value using the ratio of the vertical height of 3.2 meters to the horizontal composite displacement amplitude. For the 51st moment (frame 550), the horizontal composite displacement amplitude of 0.094 meters divided by the vertical height of 3.2 meters equals 0.0294, and the calculated arctangent function value is 1.68 degrees, which is the initial swing angle at the 51st moment. For the 101st moment (frame 600), the horizontal composite displacement amplitude of 0.164 meters divided by the vertical height of 3.2 meters equals 0.0513, and the arctangent function value is 2.94 degrees. For the 301st moment (frame 800), the horizontal composite displacement amplitude of 0.300 meters divided by the vertical height of 3.2 meters equals 0.0938, and the arctangent function value is 5.36 degrees. The initial swing angles at 301 sampling points are calculated sequentially, ranging from 0.28 degrees to 5.36 degrees.

[0106] A physical constraint model for mast swaying is established. The difference in the initial sway angle between two adjacent moments is calculated as the rate of change of angle. For the second moment (frame 501), the initial sway angle is 0.32 degrees, and for the first moment (frame 500), the initial sway angle is 0.28 degrees. The rate of change of angle is calculated as 0.32 - 0.28 = 0.04 degrees per frame. For the 51st moment (frame 550), the initial sway angle is 1.68 degrees, and for the 50th moment (frame 549), the initial sway angle is 1.62 degrees. The rate of change of angle is calculated as 1.68 - 1.62 = 0.06 degrees per frame. For the 52nd moment (frame 551), the initial sway angle is 1.75 degrees, and the rate of change of angle is calculated as 1.75 - 1.68 = 0.07 degrees per frame. A preset angle change rate threshold of 0.50 degrees per frame was set. This threshold is based on the premise that, under normal mast swing conditions, the angle change between adjacent frames should be continuous, with the angle change rate typically not exceeding 0.30 degrees per frame. When the angle change rate exceeds 0.50 degrees per frame, it indicates that the data at that moment may be affected by sudden strong disturbances or sensor measurement noise, and needs to be determined as an abnormal disturbance. Traversing the angle change rate sequence of 301 sampling points, for the 158th moment (frame 657), the initial swing angle is 3.82 degrees, and for the 157th moment (frame 656), the initial swing angle is 3.21 degrees. The angle change rate is calculated as 3.82 minus 3.21 equals 0.61 degrees per frame. The absolute value of this value, 0.61 degrees per frame, exceeds the preset angle change rate threshold of 0.50 degrees per frame, thus determining that the data at the 158th moment contains an abnormal disturbance. Continuing the inspection, at time 159 (frame 658), the initial swing angle was 3.95 degrees, with an angle change rate of 3.95 minus 3.82 equaling 0.13 degrees per frame, which did not exceed the threshold. Among the 301 sampling points, 17 times were identified as having abnormal disturbances: times 158, 172, 189, 203, 221, 235, 248, 259, 267, 274, 281, 286, 290, 294, 297, 299, and 301.

[0107] The initial swing angle corresponding to the moment with abnormal disturbance is replaced with the weighted average of the initial swing angles of the adjacent non-abnormal moments. For moment 158, i.e., frame 657, the index position of this moment in the sequence is 158. The preceding non-abnormal moment is identified as moment 157, with an index position of 157 corresponding to an initial swing angle of 3.21 degrees. The following non-abnormal moment is moment 159, with an index position of 159 corresponding to an initial swing angle of 3.95 degrees. The time scale parameter is calculated as 158 minus 157 divided by 159 minus 157, i.e., 1 divided by 2 equals 0.5. The corrected mast swing angle is calculated by linear interpolation as 3.21 plus 0.5 multiplied by 3.95 minus 3.21, i.e., 3.21 plus 0.5 multiplied by 0.74 equals 3.21 plus 0.37 equals 3.58 degrees. The initial swing angle of 3.82 degrees at moment 158 ​​is replaced with the corrected mast swing angle of 3.58 degrees. For the 172nd moment, i.e., the 671st frame, the index position is 172. The previous non-abnormal moment is the 171st moment, with an index position of 171, corresponding to an initial swing angle of 4.15 degrees. The next non-abnormal moment is the 173rd moment, with an index position of 173, corresponding to an initial swing angle of 4.28 degrees. The time scale parameter is 172 minus 171 divided by 173 minus 171, which equals 1 divided by 2, which equals 0.5. The corrected mast swing angle is 4.15 plus 0.5 multiplied by the difference between 4.28 and 4.15, i.e., 4.15 plus 0.5 multiplied by 0.13 equals 4.15 plus 0.065, which equals 4.22 degrees. For the 301st moment (frame 800), since it is the end of the sequence and there are no subsequent non-abnormal moments, the initial swing angles of the two previous non-abnormal moments, the 299th and 300th moments, are taken as 5.28 degrees and 5.32 degrees respectively. The corrected mast swing angle is calculated using extrapolation: 5.32 + 5.32 - 5.28 = 5.36 degrees. This correction and replacement is performed sequentially on the 17 abnormal moments, resulting in the corrected mast swing angle sequence, with a sequence length of 301 sampling points.

[0108] The corrected mast swing angles for all moments within the perturbation time period are arranged chronologically to form a mast swing angle change sequence. The first perturbation time period, from frame 500 to frame 800, contains 301 angle values, arranged chronologically as follows: frame 500 corresponds to 0.28 degrees, frame 501 to 0.32 degrees, frame 502 to 0.36 degrees, then to frame 550 to 1.68 degrees, frame 600 to 2.94 degrees, frame 657 (corrected) to 3.58 degrees, and frame 800 (corrected) to 5.36 degrees. The same method is used for the second perturbation time period, from frame 1200 to frame 1600. This time period has 401 sampling points and a filter window length of 40. 23 abnormal moments are identified and corrected, resulting in a mast swing angle change sequence containing 401 angle values, ranging from 0.31 degrees to 5.58 degrees. The third disturbance period, from frames 2300 to 2800, was processed. During this period, there were 501 sampling points and a filter window length of 50. 29 abnormal moments were identified and corrected, resulting in a mast swing angle variation sequence containing 501 angle values, ranging from 0.26 degrees to 6.12 degrees. The mast swing angle variation sequences from the three disturbance periods were merged to obtain a complete mast swing angle variation sequence with a total of 1203 angle values. This sequence accurately reflects the mast swing of the monitoring vessel during the three external disturbances encountered in the 300-second data acquisition process, providing accurate angle data for subsequent calculation of the spatial attitude offset angle sequence of the second sensor relative to the first sensor.

[0109] Furthermore, the initial swing angle corresponding to the moment when abnormal disturbances occur... Methods for replacing the initial swing angles with a weighted average of adjacent, non-abnormal time points include:

[0110] For each moment where an abnormal disturbance occurs, its index position in the sequence is obtained and denoted as . And identify the index position of the previous non-abnormal time. and the index position at the next non-abnormal time. ;

[0111] Calculate time ratio parameters The corrected mast sway angle was calculated using linear interpolation. ,in and These are the initial swing angles at the previous and subsequent non-abnormal times, respectively.

[0112] For example, in the calculation of the mast swing angle change sequence, for the identified moments with abnormal disturbances, the weighted average of the initial swing angles of adjacent non-abnormal moments is used for replacement. Taking the first disturbance time period from frame 500 to frame 800 as an example, a total of 17 moments with abnormal disturbances were identified in this time period, namely moments 158, 172, 189, 203, 221, 235, 248, 259, 267, 274, 281, 286, 290, 294, 297, 299, and 301. Correction and replacement processing is performed for each moment with abnormal disturbances.

[0113] For the 158th time point with an abnormal disturbance, i.e., frame 657, the index position of this time point in the sequence is obtained as 158. Identifying the preceding non-abnormal time point, checking backwards from time point 157, the angle change rate of time point 157 (frame 656) is 0.26 degrees, not exceeding the threshold of 0.50 degrees per frame, thus it is determined to be a non-abnormal time point. The initial swing angle corresponding to index position 157 at this time point is 3.21 degrees. Identifying the following non-abnormal time point, checking backwards from time point 159, the angle change rate of time point 159 (frame 658) is 0.13 degrees, not exceeding the threshold per frame, thus it is determined to be a non-abnormal time point. The initial swing angle corresponding to index position 159 at this time point is 3.95 degrees. Calculating the time ratio parameter, subtracting the index position of the preceding non-abnormal time point 157 from the current abnormal time point index position 158 yields 1, and subtracting the index position of the preceding non-abnormal time point 157 from the index position of the following non-abnormal time point 159 yields 2, with a time ratio parameter of 0.5. The corrected mast swing angle was calculated using linear interpolation. The baseline value was the initial swing angle of 3.21 degrees at the previous non-abnormal moment. The interpolation increment was 0.37 degrees, calculated by multiplying the time scale parameter of 0.5 by the difference in initial swing angles between the two non-abnormal moments (0.74 degrees). The corrected mast swing angle was 3.58 degrees. The initial swing angle of 3.82 degrees at the 158th moment was then replaced with the corrected mast swing angle of 3.58 degrees. The linear interpolation method was used because mast swing is a continuous physical process, and the angles between adjacent moments should have a smooth transition. Interpolating using the angle values ​​from non-abnormal moments can effectively eliminate measurement noise and angle jumps caused by sudden disturbances.

[0114] For the 172nd time with an abnormal disturbance, i.e., frame 671, the index position is 172. The previous time without an anomaly, index position 171, corresponds to an initial swing angle of 4.15 degrees, and the next time without an anomaly, index position 173, corresponds to an initial swing angle of 4.28 degrees. With a time scale parameter of 0.5, the corrected mast swing angle is 4.22 degrees, replacing the initial swing angle of 4.67 degrees. For the 189th time, index position 189, the previous time without an anomaly, index position 188, corresponds to an initial swing angle of 4.52 degrees, and the next time without an anomaly, index position 190, corresponds to an initial swing angle of 4.61 degrees. With a time scale parameter of 0.5, the corrected mast swing angle is 4.57 degrees, replacing the initial swing angle of 5.08 degrees. For index position 203 at time 203, the previous non-abnormal time index position 202 corresponds to an initial swing angle of 4.78 degrees, and the next non-abnormal time index position 204 corresponds to an initial swing angle of 4.85 degrees. With a time scale parameter of 0.5, the corrected mast swing angle is 4.82 degrees, replacing the initial swing angle of 5.32 degrees.

[0115] For the 221st time point with an abnormal disturbance, i.e., frame 720, the index position is 221. The previous non-abnormal time point, index position 220, corresponds to an initial swing angle of 5.02 degrees, and the next non-abnormal time point, index position 222, corresponds to an initial swing angle of 5.06 degrees. With a time scale parameter of 0.5, the corrected mast swing angle is 5.04 degrees, replacing the initial swing angle of 5.58 degrees. For the 235th time point, index position 235, the previous non-abnormal time point, index position 234, corresponds to an initial swing angle of 5.18 degrees, and the next non-abnormal time point, index position 236, corresponds to an initial swing angle of 5.21 degrees. With a time scale parameter of 0.5, the corrected mast swing angle is 5.20 degrees, replacing the initial swing angle of 5.72 degrees. For the 248th time point, index position 248, the previous non-abnormal time point, index position 247, corresponds to an initial swing angle of 5.26 degrees, and the next non-abnormal time point, index position 249, corresponds to an initial swing angle of 5.28 degrees. The corrected mast swing angle is 5.27 degrees. For index position 259 at time 259, the preceding normal time index position 258 corresponds to an initial swing angle of 5.31 degrees, and the following normal time index position 260 corresponds to an initial swing angle of 5.32 degrees. The corrected mast swing angle is 5.32 degrees. For index position 267 at time 267, the preceding normal time index position 266 corresponds to an initial swing angle of 5.33 degrees, and the following normal time index position 268 corresponds to an initial swing angle of 5.34 degrees. The corrected mast swing angle is 5.34 degrees.

[0116] For the 274th time point with an abnormal disturbance, i.e., frame 773, the index position is 274. The previous non-abnormal time point, index position 273, corresponds to an initial swing angle of 5.34 degrees, and the next non-abnormal time point, index position 275, corresponds to an initial swing angle of 5.32 degrees. With a time scale parameter of 0.5, the corrected mast swing angle is 5.33 degrees, reflecting that the mast has begun to swing back from its maximum swing angle, replacing the initial swing angle of 5.86 degrees. For the 281st time point, index position 281, the previous non-abnormal time point, index position 280, corresponds to an initial swing angle of 5.29 degrees, and the next non-abnormal time point, index position 282, corresponds to an initial swing angle of 5.26 degrees. The corrected mast swing angle is 5.28 degrees, reflecting a gradually decreasing swing trend. For index position 286 at time 286, the preceding normal time index position 285 corresponds to an initial swing angle of 5.22 degrees, and the following normal time index position 287 corresponds to an initial swing angle of 5.18 degrees. The corrected mast swing angle is 5.20 degrees. For index position 290 at time 290, the preceding normal time index position 289 corresponds to an initial swing angle of 5.14 degrees, and the following normal time index position 291 corresponds to an initial swing angle of 5.09 degrees. The corrected mast swing angle is 5.12 degrees, replacing the initial swing angle of 5.68 degrees.

[0117] For the 294th time point with abnormal disturbance, i.e., frame 793, the index position is 294. The previous non-abnormal time point, index position 293, corresponds to an initial swing angle of 5.03 degrees, and the next non-abnormal time point, index position 295, corresponds to an initial swing angle of 4.96 degrees. With a time scale parameter of 0.5, the corrected mast swing angle is 5.00 degrees, replacing the initial swing angle of 5.51 degrees. For the 297th time point, index position 297, the previous non-abnormal time point, index position 296, corresponds to an initial swing angle of 4.88 degrees, and the next non-abnormal time point, index position 298, corresponds to an initial swing angle of 4.79 degrees. The corrected mast swing angle is 4.84 degrees, replacing the initial swing angle of 5.35 degrees. For index position 299 at time 299, the previous non-abnormal time index position 298 corresponds to an initial swing angle of 4.79 degrees, and the next non-abnormal time index position 300 corresponds to an initial swing angle of 4.68 degrees. The corrected mast swing angle is 4.74 degrees, replacing the initial swing angle of 5.21 degrees.

[0118] For the 301st moment with an abnormal disturbance, i.e., frame 800, the index position is obtained as 301. This moment is the end of the sequence. The previous non-abnormal moment is identified as the 300th moment, with an initial swing angle of 4.68 degrees corresponding to index position 300. However, there are no subsequent non-abnormal moments for reference. A special processing method is used to obtain the corrected mast swing angle of 4.74 degrees at the 299th moment and the initial swing angle of 4.68 degrees at the 300th moment. The angle change trend is calculated to be -0.06 degrees. This trend is extended to the 301st moment, and the corrected mast swing angle is 4.62 degrees, replacing the initial swing angle of 5.36 degrees. This processing method is based on the continuous physical law of mast swing. The angle change of the mast should remain smoothly decreasing during the swing process. Extrapolating the angle change trend of the previous two moments at the end of the sequence can avoid abrupt angle changes that do not conform to the physical law. This processing method is reasonable in practical applications because mast swing is an inertial motion, and the motion state of adjacent moments has continuity.

[0119] The same method was used to process the 23 abnormal moments identified in frames 1200 to 1600 of the second disturbance time period. For moment 159 of this period, corresponding to frame 1358, the index position of the previous non-abnormal moment (158) corresponds to an initial swing angle of 3.46 degrees, and the index position of the next non-abnormal moment (160) corresponds to an initial swing angle of 3.72 degrees. With a time scale parameter of 0.5, the corrected mast swing angle is 3.59 degrees, replacing the initial swing angle of 4.08 degrees. For moment 185 of this period, corresponding to frame 1384, the index position of the previous non-abnormal moment (184) corresponds to an initial swing angle of 4.12 degrees, and the index position of the next non-abnormal moment (186) corresponds to an initial swing angle of 4.26 degrees. The corrected mast swing angle is 4.19 degrees. For the 401st moment of this segment, which is the 1600th frame, as the end point of the sequence, the index position 400 of the previous non-abnormal moment corresponds to an initial swing angle of 5.43 degrees. The angle change trend of the previous two moments is calculated to be -0.08 degrees, and the corrected mast swing angle is 5.35 degrees, replacing the initial swing angle of 5.91 degrees.

[0120] The system processes 29 anomalous moments identified during the third disturbance time period, from frames 2300 to 2800. For moment 190 in this period, corresponding to frame 2489, the initial swing angle is 4.87 degrees at index position 189 of the preceding non-anomalous moment and 5.02 degrees at index position 191 of the following non-anomalous moment. With a time scale parameter of 0.5, the corrected mast swing angle is 4.95 degrees, replacing the initial swing angle of 5.61 degrees. For moment 218 in this period, corresponding to frame 2517, the initial swing angle is 5.38 degrees at index position 217 of the preceding non-anomalous moment and 5.45 degrees at index position 219 of the following non-anomalous moment. The corrected mast swing angle is 5.42 degrees. For the 501st moment of this segment, i.e., the 2800th frame, as the end of the sequence, the index position 500 of the previous non-abnormal moment corresponds to an initial swing angle of 5.97 degrees. The angle change trend of the previous two moments is calculated to be -0.06 degrees, and the corrected mast swing angle is 5.91 degrees, replacing the initial swing angle of 6.48 degrees. After completing the correction and replacement of all 69 abnormal moments, a complete corrected mast swing angle change sequence of 1203 angle values ​​across three disturbance time periods is obtained. This sequence effectively eliminates the angle jumps caused by sudden strong disturbances and MEMS velocity sensor measurement noise, ensuring the continuity and physicality of the mast swing angle data. The corrected angle sequence presents a smooth swing curve in the time domain, which conforms to the actual motion law of the mast under wind and wave disturbances.

[0121] Furthermore, the method for converting the mast swing angle change sequence into a spatial attitude offset angle sequence of the second sensor relative to the first sensor includes:

[0122] Obtain the installation offset vector of the second sensor relative to the fixed point at the bottom of the mast, the installation offset vector including a radial distance component. and axial height component ;

[0123] Corrected mast swing angle Decomposed into pitch angle components around a fixed point at the base of the mast. and roll angle component ,in It is an integer variable whose value ranges from 1 to the total number of sampling points within the perturbation time period sequence;

[0124] The spatial coordinates of the second sensor under the mast swing state are represented as a three-dimensional vector in the coordinate system of the fixed point at the bottom of the mast. Axis components are , Axis components are , Axis components are ,in , , These are the three-dimensional coordinate components of the mast bottom fixed point in the ship's coordinate system;

[0125] The spatial displacement vector sequence is calculated by the difference in three-dimensional coordinates before and after the spatial position transformation of the second sensor, forming a spatial attitude offset angle sequence of the second sensor relative to the first sensor.

[0126] For example, after correcting the mast swing angle change sequence, the sequence is converted into a spatial attitude offset angle sequence of the second sensor relative to the first sensor. This allows for the acquisition of the installation offset vector of the second sensor (i.e., the solid-state lidar) relative to the mast's bottom fixed point. This installation offset vector was precisely measured and recorded during sensor installation. The mast's bottom fixed point is located on the ship's deck plane. The solid-state lidar is mounted on a sensor bracket at the top of the mast, with a radial offset between the center of the sensor bracket and the mast's central axis. The horizontal distance from the center point of the solid-state lidar base to the mast's central axis is measured using a measuring tape; the radial distance component is 0.15 meters. The vertical distance from the mast's bottom fixed point to the center point of the solid-state lidar base is measured using a measuring tape; the axial height component is 3.20 meters. Obtaining the installation offset vector is necessary because the solid-state lidar is not mounted on the mast's central axis but rather offset via the sensor bracket. When the mast swings, the sensor's actual spatial position is affected not only by the mast swing angle but also by the radial offset, requiring accurate calculation of the sensor's three-dimensional coordinates through spatial geometric transformation.

[0127] The corrected mast swing angle is decomposed into pitch and roll components around a fixed point at the mast base. Taking frames 500 to 800 of the first disturbance time period as an example, this time period contains 301 sampling points. For the 51st moment, i.e., frame 550, the corrected mast swing angle is 1.68 degrees. Combining the smoothed displacement of 0.076 meters in the X-axis direction and 0.055 meters in the Y-axis direction at this moment, the pitch and roll components are calculated. The pitch component is calculated using the arctangent function with a vertical height of 3.2 meters after smoothing the X-axis position, resulting in 1.36 degrees. The roll component is calculated using the arctangent function with a vertical height of 3.2 meters after smoothing the Y-axis position, resulting in 0.98 degrees. For the 101st moment (frame 600), the corrected mast swing angle is 2.94 degrees, with a smoothed displacement of 0.132 meters in the X-axis direction corresponding to a pitch component of 2.36 degrees, and a smoothed displacement of 0.098 meters in the Y-axis direction corresponding to a roll component of 1.75 degrees. For the 301st moment (frame 800), the corrected mast swing angle is 4.62 degrees, with a smoothed displacement of 0.241 meters in the X-axis direction corresponding to a pitch component of 4.31 degrees, and a smoothed displacement of 0.179 meters in the Y-axis direction corresponding to a roll component of 3.20 degrees. The pitch and roll components are calculated sequentially for 301 sampling points, forming pitch and roll component sequences.

[0128] The spatial coordinates of the second sensor under the mast swing state are represented as a three-dimensional vector in the coordinate system of the fixed point at the mast bottom. The coordinate system of the fixed point at the mast bottom has the fixed point at the mast bottom as the origin, the X-axis points towards the bow, the Y-axis points towards the port side of the ship, and the Z-axis points vertically upward. For the 51st moment, i.e., the 550th frame, the pitch angle component is 1.36 degrees, the roll angle component is 0.98 degrees, the radial distance component is 0.15 meters, and the axial height component is 3.20 meters. Calculating the X-axis component of the three-dimensional vector, the radial distance of 0.15 meters multiplied by the sine of the pitch angle of 1.36 degrees (0.0237) yields 0.00356 meters, and then multiplied by the cosine of the roll angle of 0.98 degrees (0.9999) yields 0.00356 meters. The X-axis coordinate of the fixed point at the mast bottom in the ship's coordinate system is 0.80 meters, and the X-axis component is 0.80356 meters. Calculate the Y-axis component: the radial distance of 0.15 meters multiplied by the sine of the roll angle of 0.98 degrees (0.0171) yields 0.00257 meters, then multiplied by the cosine of the pitch angle of 1.36 degrees (0.9997) yields 0.00257 meters. The Y-axis coordinate of the mast base fixed point in the ship's coordinate system is 0.30 meters, and the Y-axis component is 0.30257 meters. Calculate the Z-axis component: the axial height of 3.20 meters multiplied by the cosine of the pitch angle of 1.36 degrees (0.9997) yields 3.1990 meters, then multiplied by the cosine of the roll angle of 0.98 degrees (0.9999) yields 3.1987 meters. The Z-axis coordinate of the mast base fixed point in the ship's coordinate system is 0 meters, and the Z-axis component is 3.1987 meters. At that moment, the spatial coordinates of the second sensor were 0.80356 meters on the X-axis, 0.30257 meters on the Y-axis, and 3.1987 meters on the Z-axis.

[0129] For the 101st moment (frame 600), the pitch angle component is 2.36 degrees and the roll angle component is 1.75 degrees. Calculate the X-axis component: the radial distance of 0.15 meters multiplied by the sine of the pitch angle of 2.36 degrees (0.0412) and the cosine of the roll angle of 1.75 degrees (0.9995) yields 0.00617 meters. Adding the X-axis coordinate of the mast base fixed point (0.80 meters) gives 0.80617 meters. Calculate the Y-axis component: the radial distance of 0.15 meters multiplied by the sine of the roll angle of 1.75 degrees (0.0305) and the cosine of the pitch angle of 2.36 degrees (0.9991) yields 0.00457 meters. Adding the Y-axis coordinate of the mast base fixed point (0.30 meters) gives 0.30457 meters. The Z-axis component is calculated as follows: axial height 3.20 meters multiplied by the cosine of pitch angle 2.36 degrees (0.9991) multiplied by the cosine of roll angle 1.75 degrees (0.9995) to obtain 3.1943 meters. Adding the Z-axis coordinate of the mast base fixed point (0 meters) gives 3.1943 meters. At this moment, the spatial coordinates of the second sensor are: X-axis 0.80617 meters, Y-axis 0.30457 meters, Z-axis 3.1943 meters. For the 301st moment (800th frame), the pitch angle component is 4.31 degrees and the roll angle component is 3.20 degrees. The X-axis component is calculated as: radial distance 0.15 meters multiplied by the sine of pitch angle 4.31 degrees (0.0752) multiplied by the cosine of roll angle 3.20 degrees (0.9984) to obtain 0.01126 meters. Adding 0.80 meters gives 0.81126 meters. The Y-axis component is calculated as follows: radial distance 0.15 meters multiplied by the sine of roll angle 3.20 degrees (0.0558), then by the cosine of pitch angle 4.31 degrees (0.9972), resulting in 0.00835 meters. Adding 0.30 meters yields 0.30835 meters. The Z-axis component is calculated as follows: axial height 3.20 meters multiplied by the cosine of pitch angle 4.31 degrees (0.9972), then by the cosine of roll angle 3.20 degrees (0.9984), resulting in 3.1823 meters. Adding 0 meters yields 3.1823 meters. At this moment, the spatial coordinates of the second sensor are: X-axis 0.81126 meters, Y-axis 0.30835 meters, Z-axis 3.1823 meters.

[0130] The spatial displacement vector sequence was calculated by taking the difference in three-dimensional coordinates before and after the spatial position transformation of the second sensor. The initial spatial coordinates of the second sensor in a undisturbed state were 0.80 meters on the X-axis, 0.30 meters on the Y-axis, and 3.20 meters on the Z-axis, corresponding to the initial extrinsic parameter calibration parameters. At time 51 (frame 550), the X-axis component of the spatial displacement vector was 0.80356 meters minus 0.80 meters, equaling 0.00356 meters; the Y-axis component was 0.30257 meters minus 0.30 meters, equaling 0.00257 meters; and the Z-axis component was 3.1987 meters minus 3.20 meters, equaling -0.0013 meters. At this time, the spatial displacement vector was 0.00356 meters in the positive X-axis direction, 0.00257 meters in the positive Y-axis direction, and 0.0013 meters in the negative Z-axis direction, indicating that the sensor shifted horizontally towards the bow and port side, while slightly descending vertically. This descent was caused by the mast swaying, leading to the sensor deviating from the vertical axis. For the 101st time point (600th frame), the X-axis component of the spatial displacement vector is 0.00617 meters, the Y-axis component is 0.00457 meters, and the Z-axis component is -0.0057 meters. For the 301st time point (800th frame), the X-axis component is 0.01126 meters, the Y-axis component is 0.00835 meters, and the Z-axis component is -0.0177 meters. The spatial displacement vectors of these 301 sampling points are calculated sequentially to form a spatial displacement vector sequence.

[0131] By combining the spatial displacement vector sequence, pitch angle component sequence, and roll angle component sequence, a spatial attitude offset angle sequence of the second sensor relative to the first sensor is formed. For the 51st moment, i.e., the 550th frame, the spatial attitude offset includes position offset and angle offset. The position offset is 0.00356 meters on the X-axis, 0.00257 meters on the Y-axis, and -0.0013 meters on the Z-axis. The angle offset is 1.36 degrees for pitch, 0.98 degrees for roll, and 0.62 degrees for yaw, calculated from the direction angles of the X-axis and Y-axis displacements. For the 101st moment, i.e., the 600th frame, the position offset is 0.00617 meters on the X-axis, 0.00457 meters on the Y-axis, and -0.0057 meters on the Z-axis. The angle offset is 2.36 degrees for pitch, 1.75 degrees for roll, and 1.08 degrees for yaw. For the 301st moment, i.e. the 800th frame, the position offset is 0.01126 meters on the X-axis, 0.00835 meters on the Y-axis, and -0.0177 meters on the Z-axis. The angle offset is 4.31 degrees for pitch, 3.20 degrees for roll, and 1.45 degrees for yaw. The same method was used to process the second disturbance time period (frames 1200 to 1600) and the third disturbance time period (frames 2300 to 2800). The positional offset at the 401st moment of the second segment was 0.00875 meters on the X-axis, 0.00992 meters on the Y-axis, and -0.0158 meters on the Z-axis. The angular offsets were 2.48 degrees for pitch, 2.81 degrees for roll, and 1.32 degrees for yaw. The positional offset at the 501st moment of the third segment was 0.01297 meters on the X-axis, 0.00790 meters on the Y-axis, and -0.0209 meters on the Z-axis. The angular offsets were 3.68 degrees for pitch, 2.24 degrees for roll, and 1.18 degrees for yaw. A complete spatial attitude offset angle sequence containing 1203 time points was generated. This sequence accurately reflects the spatial position and attitude changes of the solid-state lidar caused by mast swaying during three disturbance time periods, providing precise offset data for subsequent coordinate transformation calculations to correct extrinsic parameter calibration parameters. The pitch angle offset range in the spatial attitude offset angle sequence is -3.15 degrees to +4.88 degrees, the roll angle offset range is -2.23 degrees to +3.67 degrees, and the yaw angle offset ranges from -0.82 degrees to +1.45 degrees. The maximum position offset is 0.0138 meters in the X-axis direction, 0.0116 meters in the Y-axis direction, and -0.0218 meters in the Z-axis direction. Although these offsets seem small numerically, they can lead to the cumulative amplification of target positioning errors in high-precision multi-sensor fusion applications, and must be compensated for through extrinsic parameter calibration correction.

[0132] Furthermore, the method for obtaining the corrected extrinsic calibration parameters by performing coordinate transformation operations between the spatial offset sequence and the initial extrinsic calibration parameters includes:

[0133] Obtain the initial extrinsic calibration parameters of the first and second sensors, wherein the initial extrinsic calibration parameters include the initial rotation matrix. and initial translation vector ;

[0134] Establish an optimization objective function for multi-sensor extrinsic parameter calibration, and obtain the first... Measurement error per data frame Time interval and signal-to-noise ratio ,pass Calculate the first The weight coefficients of each data frame are used to establish the objective function. ,in This represents the total number of valid data frames within the perturbation time period sequence. For the first sensor in the first The target point coordinate vector at each moment For the second sensor in the first The source point coordinate vector at each moment. This is the corrected rotation matrix. The corrected translation vector. The characteristic time constant of the mast oscillation;

[0135] The objective function is solved by constraint optimization using the Lagrange multiplier method, and the optimal rotation matrix is ​​calculated using the iterative nearest-point algorithm to obtain the corrected extrinsic calibration parameters. and ,in For dynamic rotation correction matrix, This is the dynamic translation correction vector.

[0136] For example, in the aforementioned embodiment scenario, after calculating the spatial attitude offset angle sequence of the second sensor relative to the first sensor, the spatial offset sequence is transformed with the initial extrinsic calibration parameters to obtain the corrected extrinsic calibration parameters. The initial extrinsic calibration parameters of the first sensor (millimeter-wave radar) and the second sensor (solid-state lidar) are obtained. These parameters are measured at the calibration site before the unmanned monitoring vessel leaves the factory. The initial extrinsic calibration parameters include an initial rotation matrix and an initial translation vector. The initial rotation matrix describes the rotation relationship between the solid-state lidar coordinate system and the millimeter-wave radar coordinate system, corresponding to a three-dimensional rotation transformation with a pitch angle of 2.0 degrees, a roll angle of 1.0 degrees, and a yaw angle of 0 degrees. The initial translation vector describes the spatial displacement relationship between the origins of the two sensor coordinate systems, corresponding to 0.80 meters on the X-axis, 0.30 meters on the Y-axis, and 3.20 meters on the Z-axis. The initial extrinsic calibration parameters accurately reflect the relative pose of the two sensors when the hull is stationary and undisturbed. However, after encountering wind and waves that cause the mast to swing, these parameters no longer accurately describe the actual relative pose. Dynamic correction is required to obtain corrected extrinsic calibration parameters that conform to the current state.

[0137] An optimization objective function for multi-sensor extrinsic parameter calibration was established. This function comprehensively considers the impact of measurement error, time interval, and signal-to-noise ratio on calibration accuracy. For 1203 valid data frames within three disturbance time periods, relevant parameters were calculated frame by frame. For the 51st data frame (frame 550), the measurement error was obtained. The measurement error was calculated by the difference in detection results of the same target point by millimeter-wave radar and solid-state lidar. At this moment, a preset reflective buoy was located 50 meters ahead of the monitoring vessel as a reference target. The millimeter-wave radar detected the buoy at a distance of 50.08 meters, while the solid-state lidar detected it at a distance of 50.12 meters. The absolute value of the difference between the two was 0.04 meters, which is the measurement error. The time interval for this frame was also obtained. The time interval refers to the length of time between this data frame and the start of the disturbance time period. Frame 550 was 50 frames away from the start of frame 500, corresponding to a time interval of 2.5 seconds. The signal-to-noise ratio (SNR) of this frame is obtained. The SNR is calculated as the ratio of the reflection intensity to the background noise intensity of the solid-state lidar point cloud data. The average reflection intensity of this frame's point cloud is 180, the background noise intensity is 12, and the SNR is 15.0. Using the weighting coefficient calculation formula, the numerator is the SNR of 15.0 divided by the measurement error of 0.04 meters, resulting in 375. The denominator is an exponential function of the natural constant, where the exponent is the time interval of 2.5 seconds divided by the characteristic time constant. The characteristic time constant is set to 5.0 seconds to reflect the decay period of the mast swing, with an exponent value of 0.5. The natural constant 2.718 raised to the power of 0.5 equals 1.649. The weighting coefficient is 375 divided by 1.649, which equals 227.4. The design principle of this weighting coefficient is that data frames with higher SNR and smaller measurement errors are more reliable and should be given greater weight. Conversely, data frames farther from the start of the disturbance have lower representativeness due to the gradual decay of the mast swing and should be given less weight. This physical process is simulated using an exponential decay function.

[0138] For the 101st data frame (600th frame), the measurement error is 0.06 meters, the time interval is 5.0 seconds, and the signal-to-noise ratio is 14.2. The weighting coefficient is calculated as follows: SNR 14.2 divided by measurement error 0.06 meters equals 236.7. Time interval 5.0 seconds divided by characteristic time constant 5.0 seconds equals exponent value 1.0. The natural constant raised to the power of 1.0 is 2.718. The weighting coefficient is 236.7 divided by 2.718, which equals 87.1. For the 301st data frame (frame 800), the measurement error is 0.08 meters, the time interval is 15.0 seconds, and the signal-to-noise ratio (SNR) is 13.5. The weighting coefficient is calculated as follows: SNR 13.5 divided by measurement error 0.08 meters, resulting in 168.75; time interval 15.0 seconds divided by characteristic time constant 5.0 seconds, resulting in exponent value 3.0; natural constant 3.0 to the power of 20.09; weighting coefficient 168.75 divided by 20.09 equals 8.4. The weighting coefficients are calculated sequentially for all 1203 data frames, ranging from 5.2 to 358.6. Data frames with larger weighting coefficients are mainly concentrated in the early stages of disturbances and moments with higher measurement quality; these data frames will play a dominant role in subsequent optimization calculations.

[0139] The goal is to minimize the weighted sum of squared errors between the target point coordinates detected by the first sensor and the source point coordinates detected by the second sensor after extrinsic parameter transformation. For the 51st data frame (frame 550), the target point coordinate vector detected by the first sensor is 50.02 meters on the X-axis, 0.15 meters on the Y-axis, and 0.08 meters on the Z-axis, represented in the millimeter-wave radar coordinate system. The source point coordinate vector detected by the second sensor is 49.18 meters on the X-axis, -0.12 meters on the Y-axis, and -3.08 meters on the Z-axis, represented in the solid-state lidar coordinate system. The source point coordinates are rotated using a corrected rotation matrix, and then a corrected translation vector is added to obtain the transformed coordinates of the source point in the millimeter-wave radar coordinate system. The Euclidean distance between the target point coordinate vector and the transformed source point coordinate vector is calculated. The square of this distance is multiplied by a weighting coefficient of 227.4 to obtain the contribution value of this data frame to the objective function. The contribution values ​​of all 1203 data frames are summed to obtain the total objective function value. The optimization objective is to minimize the total objective function value by adjusting the modified rotation matrix and the modified translation vector.

[0140] The objective function is solved using the Lagrange multiplier method with constraints including the orthogonality requirement of the rotation matrix (i.e., the product of the transpose of the rotation matrix and itself equals the identity matrix) and the determinant of the rotation matrix equals 1. These constraints ensure that the rotation transformation does not change the length and chirality of the spatial vector. A Lagrange function is established, and the objective function and constraints are combined using Lagrange multipliers. Partial derivatives of the Lagrange function with respect to the elements of the rotation matrix, the translation vector elements, and the Lagrange multipliers are calculated, and all partial derivatives are set to zero to obtain a system of equations. The optimal rotation matrix is ​​calculated using an iterative nearest-point algorithm. This algorithm starts with an initial rotation matrix, rotates all source point coordinates of the second sensor using the current rotation matrix, and finds the nearest point correspondence between each transformed source point and the target point of the first sensor. Based on these points, the updated rotation matrix and translation vector are calculated, and the iteration is repeated until the objective function value converges. After 18 iterations, the objective function value converged from the initial 2847.6 to the final 156.3. The gradual decrease in the objective function value during the iteration process indicates that the optimization process is effective.

[0141] The corrected extrinsic calibration parameters are obtained. The corrected rotation matrix is ​​obtained by multiplying the dynamic rotation correction matrix by the initial rotation matrix. The dynamic rotation correction matrix corresponds to an additional rotation of 0.34 degrees for pitch, 0.18 degrees for roll, and 0.08 degrees for yaw, representing the angle correction amount relative to the initial calibration state. The corrected rotation matrix corresponds to a total pitch angle of 2.0 degrees plus 0.34 degrees equaling 2.34 degrees, a total roll angle of 1.0 degrees plus 0.18 degrees equaling 1.18 degrees, and a total yaw angle of 0 degrees plus 0.08 degrees equaling 0.08 degrees. The corrected translation vector is obtained by multiplying the initial translation vector by the dynamic rotation correction matrix and then adding the dynamic translation correction vector. The additional displacement produced by the dynamic rotation correction matrix acting on the initial translation vector is 0.021 m on the X-axis, 0.008 m on the Y-axis, and -0.015 m on the Z-axis. The dynamic translation correction vector is 0.002 m on the X-axis, -0.024 m on the Y-axis, and 0.002 m on the Z-axis. The corrected translation vector has the following components: X-axis component: 0.80 m + 0.021 m + 0.002 m = 0.823 m; Y-axis component: 0.30 m + 0.008 m - 0.024 m = 0.284 m; Z-axis component: 3.20 m - 0.015 m + 0.002 m = 3.187 m. The final calibration parameters for the external parameters were determined as follows: translation vector X-axis 0.823 m, Y-axis 0.284 m, Z-axis 3.187 m, and rotation angles 2.34 degrees pitch, 1.18 degrees roll, and 0.08 degrees yaw.

[0142] The corrected extrinsic parameters were used to perform multi-sensor fusion verification on 2000 frames of data collected over a subsequent 100 seconds. The verification method involved pre-deploying five reflective buoys as ground-based ground calibration points in the monitored water area. Buoy positions were measured using differential GPS: Buoy 1 was located at 30°28′36.251″N, 114°18′22.183″E; Buoy 2 was located at 30°28′38.472″N, 114°18′22.183″E; and Buoy 2 was located at 30°28′38.472″N, 114°18′22.183″E. At 114°18′24.596″, buoy 3's position is 30°28′40.318″N, 114°18′26.827″E; buoy 4's position is 30°28′42.095″N, 114°18′29.134″E; and buoy 5's position is 30°28′44.261″N, 114°18′31.502″E. The GPS measurement accuracy reaches the centimeter level. The buoy point cloud coordinates detected by the solid-state lidar are transformed to the millimeter-wave radar coordinate system by correcting the extrinsic parameter calibration parameters. Then, combined with the ship's own position and heading information, they are transformed to the global coordinate system and compared with the ground truth value measured by differential GPS. For buoy 1, the distance error between the fused positioning result before correction and the ground truth value is 0.42 meters; after correction, the distance error is reduced to 0.09 meters. For buoy 2, the error was 0.51 meters before correction and 0.11 meters after correction. For buoy 3, the error was 0.38 meters before correction and 0.08 meters after correction. For buoy 4, the error was 0.29 meters before correction and 0.06 meters after correction. For buoy 5, the error was 0.35 meters before correction and 0.10 meters after correction. Statistical analysis of all buoy detection results across 2000 frames of data showed that the target positioning error decreased from an average of 0.385 meters before correction to 0.087 meters after correction, a reduction of 77.4%. The maximum error decreased from 0.621 meters before correction to 0.143 meters after correction, and the standard deviation of the error decreased from 0.152 meters before correction to 0.038 meters after correction. This indicates that the corrected data fusion accuracy was significantly improved and its stability was enhanced. The correction of the external parameter calibration parameters effectively compensated for the sensor pose shift caused by the mast swing due to wind and wave disturbances, enabling the multi-sensor data fusion accuracy to meet the technical requirement of hydrological monitoring tasks for target positioning accuracy better than 0.10 meters, and verifying the effectiveness and practicality of the external parameter automatic calibration method.

[0143] Example 2: Based on the same inventive concept, this example also provides an automatic calibration system for the extrinsic parameters of a multimodal sensor, the system comprising:

[0144] The data acquisition module is used to acquire the first sensor data frame sequence of the first sensor installed on the hull and the second sensor data frame sequence of the second sensor installed on the mast. The first and second sensor data frame sequences are continuously acquired through a fixed sampling frequency and marked with timestamp sequences. The module also acquires the data frame sequences corresponding to the X and Y axes of the second sensor in the horizontal plane. Shaft velocity data sequence and Shaft velocity data sequence, the Shaft velocity data sequence and The shaft speed data sequence is acquired by a speed sensor installed at the same location as the second sensor. Shaft velocity data sequence and The shaft velocity data sequence is time-synchronized with the second sensor data frame sequence;

[0145] The disturbance analysis module is used to analyze... Shaft velocity data sequence and The time derivatives of the shaft velocity data sequences were calculated respectively. Axial acceleration component sequence and Calculate the axis acceleration component sequence. Axial acceleration component sequence and The composite vector amplitude sequence of the axial acceleration component sequence in the horizontal plane is used to mark the time period when the composite vector amplitude sequence exceeds a preset disturbance threshold as the disturbance time period sequence;

[0146] Angle offset calculation module is used to obtain the vertical height of the second sensor on the mast and... Shaft velocity data sequence and The horizontal displacement sequence corresponding to the vertical height is obtained by time integration of the shaft velocity data sequence within the disturbance time period sequence. The mast swing angle change sequence is calculated based on the horizontal displacement sequence and the vertical height. The mast swing angle change sequence is then converted into the spatial attitude offset angle sequence of the second sensor relative to the first sensor.

[0147] The extrinsic parameter compensation calibration module is used to calibrate the parameters corresponding to the disturbance time period sequence. Shaft velocity data sequence The spatial offset sequence is obtained by combining the axis velocity data sequence and the spatial attitude offset angle sequence. The spatial offset sequence is then subjected to coordinate transformation with the initial extrinsic calibration parameters to obtain the corrected extrinsic calibration parameters.

[0148] Furthermore, the system also includes:

[0149] The vertical height of the second sensor on the mast is recorded as follows: Obtain the first time interval within the perturbation time period sequence. The corresponding time Shaft speed data is denoted as and Shaft speed data is denoted as ,in The value ranges from 1 to the total number of sampling points within the perturbation time period sequence. Integer variables;

[0150] The time interval corresponding to the fixed sampling frequency is denoted as . ,right Shaft speed data and The shaft velocity data are integrated over time to calculate the time from the start of the disturbance period to the [missing value]. At that moment Horizontal displacement in the axial direction and in Horizontal displacement in the axial direction :

[0151] ;

[0152] ;

[0153] in From 1 to Integer variables; Horizontal displacement sequence in axial direction and The horizontal displacement sequences along the axial direction are processed by moving average filtering, with the filter window length set to the total number of sampling points. Divide by the preset segmentation coefficient and round down to obtain the filtered result. Axial smooth displacement sequence and Axial smooth displacement sequence ;

[0154] Calculate the first Horizontal composite displacement amplitude at time 1 :

[0155] ;

[0156] By vertical height Combined horizontal displacement amplitude The ratio calculation of the arctangent function value to the first The initial swing angle of the mast relative to the vertical direction at that moment :

[0157] ;

[0158] Establish a physical constraint model for the mast's swaying, and calculate the difference in the initial sway angle between two adjacent moments, denoted as the rate of angle change. :

[0159] ;

[0160] When the rate of change of angle When the absolute value exceeds the preset angle change rate threshold, it is determined that there is an abnormal disturbance in the data at that moment;

[0161] The initial swing angle corresponding to the moment when abnormal disturbance exists. The corrected mast swing angle is obtained by replacing the initial swing angle with the weighted average of the initial swing angles at adjacent, non-abnormal times. ;

[0162] The mast swing angle corrected for all moments within the perturbation time series. Arranged in chronological order to form a sequence of changes in the mast's swing angle.

[0163] Furthermore, the system also includes:

[0164] The time difference correction module is used to obtain the index position in the sequence for each time with an abnormal disturbance, denoted as . And identify the index position of the previous non-abnormal time. and the index position at the next non-abnormal time. ;

[0165] Calculate time ratio parameters The corrected mast sway angle was calculated using linear interpolation. ,in and These are the initial swing angles at the previous and subsequent non-abnormal times, respectively.

[0166] Furthermore, the system also includes:

[0167] The coordinate transformation module is used to obtain the installation offset vector of the second sensor relative to the fixed point at the bottom of the mast, the installation offset vector including a radial distance component. and axial height component ;

[0168] Corrected mast swing angle Decomposed into pitch angle components around a fixed point at the base of the mast. and roll angle component ,in It is an integer variable whose value ranges from 1 to the total number of sampling points within the perturbation time period sequence;

[0169] The spatial coordinates of the second sensor under the mast swing state are represented as a three-dimensional vector in the coordinate system of the fixed point at the bottom of the mast. Axis components are , Axis components are , Axis components are ,in , , These are the three-dimensional coordinate components of the mast bottom fixed point in the ship's coordinate system;

[0170] The spatial displacement vector sequence is calculated by the difference in three-dimensional coordinates before and after the spatial position transformation of the second sensor, forming a spatial attitude offset angle sequence of the second sensor relative to the first sensor.

[0171] Furthermore, the system also includes:

[0172] The coordinate synthesis module is used to obtain the initial extrinsic calibration parameters of the first and second sensors, wherein the initial extrinsic calibration parameters include the initial rotation matrix. and initial translation vector ;

[0173] Establish an optimization objective function for multi-sensor extrinsic parameter calibration, and obtain the first... Measurement error per data frame Time interval and signal-to-noise ratio ,pass Calculate the first The weight coefficients of each data frame are used to establish the objective function. ,in This represents the total number of valid data frames within the perturbation time period sequence. For the first sensor in the first The target point coordinate vector at each moment For the second sensor in the first The source point coordinate vector at each moment. This is the corrected rotation matrix. The corrected translation vector. The characteristic time constant of the mast oscillation;

[0174] The objective function is solved by constraint optimization using the Lagrange multiplier method, and the optimal rotation matrix is ​​calculated using the iterative nearest-point algorithm to obtain the corrected extrinsic calibration parameters. and ,in For dynamic rotation correction matrix, This is the dynamic translation correction vector.

[0175] It should be noted that the specific methods by which each module performs operations in the system described in the above embodiments have been described in detail in the embodiments related to the method, and will not be elaborated here.

[0176] Finally, it should be noted that although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art can still modify the technical solutions described in the foregoing embodiments or make equivalent substitutions for some of the technical features. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A method for automatic calibration of extrinsic parameters of a multimodal sensor, characterized in that, The method includes: Acquire the first sensor data frame sequence of the first sensor installed on the hull and the second sensor data frame sequence of the second sensor installed on the mast. The first and second sensor data frame sequences are continuously obtained through a fixed sampling frequency and marked with timestamp sequences. Acquire the data frame sequences of the second sensor along the X and Y axes in the horizontal direction. Shaft velocity data sequence and Shaft velocity data sequence, the Shaft velocity data sequence and The shaft speed data sequence is acquired by a speed sensor installed at the same location as the second sensor. Shaft velocity data sequence and The shaft velocity data sequence is time-synchronized with the second sensor data frame sequence; right Shaft velocity data sequence and The time derivatives of the shaft velocity data sequences were calculated respectively. Axial acceleration component sequence and Calculate the sequence of axis acceleration components. Axial acceleration component sequence and The composite vector amplitude sequence of the axial acceleration component sequence in the horizontal plane is used to mark the time period when the composite vector amplitude sequence exceeds a preset disturbance threshold as the disturbance time period sequence; Obtain the vertical height of the second sensor on the mast and through Shaft velocity data sequence and The horizontal displacement sequence corresponding to the vertical height is obtained by time integration of the shaft velocity data sequence within the disturbance time period sequence. The mast swing angle change sequence is calculated based on the horizontal displacement sequence and the vertical height. The mast swing angle change sequence is then converted into the spatial attitude offset angle sequence of the second sensor relative to the first sensor. The corresponding time series of disturbances Shaft velocity data sequence The spatial offset sequence is obtained by combining the axis velocity data sequence and the spatial attitude offset angle sequence. The spatial offset sequence is then subjected to coordinate transformation with the initial extrinsic calibration parameters to obtain the corrected extrinsic calibration parameters. The method for obtaining corrected extrinsic calibration parameters by performing coordinate transformation operations between the spatial offset sequence and the initial extrinsic calibration parameters includes: Obtain the initial extrinsic calibration parameters of the first and second sensors, wherein the initial extrinsic calibration parameters include the initial rotation matrix. and the initial translation vector ; Establish an optimization objective function for multi-sensor extrinsic parameter calibration, and obtain the first... Measurement error per data frame Time interval and signal-to-noise ratio ,pass Calculate the first The weight coefficients of each data frame are used to establish the objective function. ,in This represents the total number of valid data frames within the perturbation time period sequence. For the first sensor in the first The target point coordinate vector at each moment For the second sensor in the first The source point coordinate vector at each time step. This is the corrected rotation matrix. The corrected translation vector. The characteristic time constant of the mast's oscillation; The objective function is solved by constraint optimization using the Lagrange multiplier method, and the optimal rotation matrix is ​​calculated using the iterative nearest-point algorithm to obtain the corrected extrinsic calibration parameters. and ,in For dynamic rotation correction matrix, This is the dynamic translation correction vector.

2. The method for automatic calibration of extrinsic parameters of a multimodal sensor according to claim 1, characterized in that, The method for calculating the mast swing angle change sequence based on the horizontal displacement sequence and the vertical height includes: The vertical height of the second sensor on the mast is recorded as follows: Obtain the first time interval within the perturbation time period sequence The corresponding time Shaft speed data is denoted as and Shaft speed data is denoted as ,in The value ranges from 1 to the total number of sampling points within the perturbation time period sequence. Integer variables; The time interval corresponding to the fixed sampling frequency is denoted as . ,right Shaft speed data and The shaft velocity data are integrated over time to calculate the time from the start of the disturbance period to the [missing value]. At that moment Horizontal displacement in the axial direction and in Horizontal displacement in the axial direction : ； ； in From 1 to Integer variables; Horizontal displacement sequence in axial direction and The horizontal displacement sequences along the axial direction are processed by moving average filtering, with the filter window length set to the total number of sampling points. Divide by the preset segmentation coefficient and round down to obtain the filtered result. Axial smooth displacement sequence and Axial smooth displacement sequence ; Calculate the first Horizontal composite displacement amplitude at time 1 : ； By vertical height Combined horizontal displacement amplitude The ratio calculation of the arctangent function value to the first The initial swing angle of the mast relative to the vertical direction at that moment : ； Establish a physical constraint model for the mast's swaying, and calculate the difference in the initial sway angle between two adjacent moments, denoted as the rate of angle change. : ； When the rate of change of angle When the absolute value exceeds the preset angle change rate threshold, it is determined that there is an abnormal disturbance in the data at that moment; The initial swing angle corresponding to the moment when abnormal disturbance exists. The corrected mast swing angle is obtained by replacing the initial swing angle with the weighted average of the initial swing angles at adjacent, non-abnormal times. ; The mast swing angle corrected for all moments within the perturbation time series. Arranged in chronological order to form a sequence of changes in the mast's swing angle.

3. The method for automatic calibration of extrinsic parameters of a multimodal sensor according to claim 2, characterized in that, The initial swing angle corresponding to the moment when abnormal disturbance exists. Methods for replacing the initial swing angles with a weighted average of adjacent, non-abnormal time points include: For each moment where an abnormal disturbance occurs, its index position in the sequence is obtained and denoted as . And identify the index position of the previous non-abnormal time. and the index position at the next non-abnormal time. ; Calculate time ratio parameters The corrected mast sway angle was calculated using linear interpolation. ,in and These are the initial swing angles at the previous and subsequent non-abnormal times, respectively.

4. The method for automatic calibration of extrinsic parameters of a multimodal sensor according to claim 2, characterized in that, The method for converting the mast swing angle change sequence into a spatial attitude offset angle sequence of the second sensor relative to the first sensor includes: Obtain the installation offset vector of the second sensor relative to the fixed point at the bottom of the mast, the installation offset vector including a radial distance component. and axial height component ; Corrected mast swing angle Decomposed into pitch angle components around a fixed point at the base of the mast. and roll angle component ,in It is an integer variable whose value ranges from 1 to the total number of sampling points within the perturbation time period sequence; The spatial coordinates of the second sensor under the mast swing state are represented as a three-dimensional vector in the coordinate system of the fixed point at the bottom of the mast. Axis components are , Axis components are , Axis components are ,in , These are the three-dimensional coordinate components of the mast bottom fixed point in the ship's coordinate system; The spatial displacement vector sequence is calculated by the difference in three-dimensional coordinates before and after the spatial position transformation of the second sensor, forming a spatial attitude offset angle sequence of the second sensor relative to the first sensor.

5. An automatic calibration system for extrinsic parameters of a multimodal sensor, characterized in that, The system includes: The data acquisition module is used to acquire the first sensor data frame sequence of the first sensor installed on the hull and the second sensor data frame sequence of the second sensor installed on the mast. The first and second sensor data frame sequences are continuously acquired through a fixed sampling frequency and marked with timestamp sequences. The module also acquires the data frame sequences corresponding to the X and Y axes of the second sensor in the horizontal plane. Shaft velocity data sequence and Shaft velocity data sequence, the Shaft velocity data sequence and The shaft speed data sequence is acquired by a speed sensor installed at the same location as the second sensor. Shaft velocity data sequence and The shaft velocity data sequence is time-synchronized with the second sensor data frame sequence; The disturbance analysis module is used to analyze... Shaft velocity data sequence and The time derivatives of the shaft velocity data sequences were calculated respectively. Axial acceleration component sequence and Calculate the sequence of axis acceleration components. Axial acceleration component sequence and The composite vector amplitude sequence of the axial acceleration component sequence in the horizontal plane is used to mark the time period when the composite vector amplitude sequence exceeds a preset disturbance threshold as the disturbance time period sequence; Angle offset calculation module is used to obtain the vertical height of the second sensor on the mast and... Shaft velocity data sequence and The horizontal displacement sequence corresponding to the vertical height is obtained by time integration of the shaft velocity data sequence within the disturbance time period sequence. The mast swing angle change sequence is calculated based on the horizontal displacement sequence and the vertical height. The mast swing angle change sequence is then converted into the spatial attitude offset angle sequence of the second sensor relative to the first sensor. The extrinsic parameter compensation calibration module is used to calibrate the parameters corresponding to the disturbance time period sequence. Shaft velocity data sequence The spatial offset sequence is obtained by combining the axis velocity data sequence and the spatial attitude offset angle sequence. The spatial offset sequence is then subjected to coordinate transformation with the initial extrinsic calibration parameters to obtain the corrected extrinsic calibration parameters. The coordinate synthesis module is used to obtain the initial extrinsic calibration parameters of the first and second sensors, wherein the initial extrinsic calibration parameters include the initial rotation matrix. and the initial translation vector ; Establish an optimization objective function for multi-sensor extrinsic parameter calibration, and obtain the first... Measurement error per data frame Time interval and signal-to-noise ratio ,pass Calculate the first The weight coefficients of each data frame are used to establish the objective function. ,in This represents the total number of valid data frames within the perturbation time period sequence. For the first sensor in the first The target point coordinate vector at each moment For the second sensor in the first The source point coordinate vector at each time step. This is the corrected rotation matrix. The corrected translation vector. The characteristic time constant of the mast's oscillation; The objective function is solved by constraint optimization using the Lagrange multiplier method, and the optimal rotation matrix is ​​calculated using the iterative nearest-point algorithm to obtain the corrected extrinsic calibration parameters. and ,in For dynamic rotation correction matrix, This is the dynamic translation correction vector.

6. The automatic calibration system for extrinsic parameters of a multimodal sensor according to claim 5, characterized in that, The system also includes: The vertical height of the second sensor on the mast is recorded as follows: Obtain the first time interval within the perturbation time period sequence The corresponding time Shaft speed data is denoted as and Shaft speed data is denoted as ,in The value ranges from 1 to the total number of sampling points within the perturbation time period sequence. Integer variables; The time interval corresponding to the fixed sampling frequency is denoted as . ,right Shaft speed data and The shaft velocity data are integrated over time to calculate the time from the start of the disturbance period to the [missing value]. At that moment Horizontal displacement in the axial direction and in Horizontal displacement in the axial direction : ； ； in From 1 to Integer variables; Horizontal displacement sequence in axial direction and The horizontal displacement sequences along the axial direction are processed by moving average filtering, with the filter window length set to the total number of sampling points. Divide by the preset segmentation coefficient and round down to obtain the filtered result. Axial smooth displacement sequence and Axial smooth displacement sequence ; Calculate the first Horizontal composite displacement amplitude at time 1 : ； By vertical height Combined horizontal displacement amplitude The ratio calculation of the arctangent function value to the first The initial swing angle of the mast relative to the vertical direction at that moment : ； Establish a physical constraint model for the mast's swaying, and calculate the difference in the initial sway angle between two adjacent moments, denoted as the rate of angle change. : ； When the rate of change of angle When the absolute value exceeds the preset angle change rate threshold, it is determined that there is an abnormal disturbance in the data at that moment; The initial swing angle corresponding to the moment when abnormal disturbance exists. The corrected mast swing angle is obtained by replacing the initial swing angle with the weighted average of the initial swing angles at adjacent, non-abnormal times. ; The mast swing angle corrected for all moments within the perturbation time series. Arranged in chronological order to form a sequence of changes in the mast's swing angle.

7. The automatic calibration system for extrinsic parameters of a multimodal sensor according to claim 6, characterized in that, The system also includes: The time difference correction module is used to obtain the index position in the sequence for each time with an abnormal disturbance, denoted as . And identify the index position of the previous non-abnormal time. and the index position at the next non-abnormal time. ; Calculate time ratio parameters The corrected mast sway angle was calculated using linear interpolation. ,in and These are the initial swing angles at the previous and subsequent non-abnormal times, respectively.

8. The automatic calibration system for extrinsic parameters of a multimodal sensor according to claim 6, characterized in that, The system also includes: The coordinate transformation module is used to obtain the installation offset vector of the second sensor relative to the fixed point at the bottom of the mast, the installation offset vector including a radial distance component. and axial height component ; Corrected mast swing angle Decomposed into pitch angle components around a fixed point at the base of the mast. and roll angle component ,in It is an integer variable whose value ranges from 1 to the total number of sampling points within the perturbation time period sequence; The spatial coordinates of the second sensor under the mast swing state are represented as a three-dimensional vector in the coordinate system of the fixed point at the bottom of the mast. Axis components are , Axis components are , Axis components are ,in , , These are the three-dimensional coordinate components of the mast bottom fixed point in the ship's coordinate system; The spatial displacement vector sequence is calculated by the difference in three-dimensional coordinates before and after the spatial position transformation of the second sensor, forming a spatial attitude offset angle sequence of the second sensor relative to the first sensor.

Images