Ship positioning method and system based on multi-sensor fusion and electronic equipment

Abstract

The invention discloses a ship positioning method and system based on multi-sensor fusion and electronic equipment, and the method comprises the steps: obtaining ship positioning data of multiple sensors, carrying out the fusion processing of the data through a self-adaptive Kalman filtering algorithm, and generating a ship six-degree-of-freedom motion state estimation; based on the generated ship six-degree-of-freedom motion state estimation, horizontal position and heading data are extracted by using a coordinate transformation matrix; a Kalman filtering algorithm with adaptive parameter adjustment is adopted to perform low-frequency filtering processing on the horizontal position and heading data, and the low-frequency position and heading estimation of the ship are output, so that the ship positioning result is more stable and reliable.

Description

Technical Field

[0001] The present invention belongs to the field of dynamic positioning systems, and in particular relates to a ship positioning method, system and electronic equipment based on multi-sensor fusion. Background Art

[0002] With the rapid development of marine resource development and the shipping industry, the demand for precise positioning of ships in complex sea conditions is growing. Traditional ship positioning methods primarily rely on single sensors such as GPS or inertial measurement units (IMUs). These methods have significant limitations, including: limited accuracy of single sensors, GPS susceptibility to signal loss due to obstruction, and IMU cumulative errors, making them incapable of meeting the demand for long-term, high-precision positioning; insufficient adaptability to dynamic environments; traditional Kalman filter algorithms with fixed parameters struggle to adapt to dynamic disturbances such as waves and currents, resulting in large errors in low-frequency motion estimation; and low efficiency in multi-sensor fusion. Existing fusion algorithms fail to fully utilize the complementarity of multi-source data and lack effective bias calibration mechanisms, impacting the reliability of positioning results. In recent years, multi-sensor fusion technology has been gradually applied to ship positioning, improving positioning accuracy by fusing data from multiple sources, including GPS, IMUs, and compasses. However, the existing technology still has the following problems: the bias calibration is inaccurate, and the bias parameters of the IMU and compass are not dynamically calibrated, resulting in the accumulation of errors in the six-degree-of-freedom motion state estimation; the filtering parameters are fixed, and the covariance matrix cannot be automatically adjusted according to environmental changes, affecting the stability of low-frequency motion estimation; the data fusion strategy is single and does not take into account the dynamic changes in the sensor working state, and the fusion model lacks flexibility. Summary of the Invention

[0003] Based on this, it is necessary to provide a ship positioning method, system and electronic equipment based on multi-sensor fusion that can solve the above problems.

[0004] In a first aspect, the present application provides a ship positioning method based on multi-sensor fusion, comprising:

[0005] Acquire ship positioning data from multiple sensors and process it using sensor fusion technology to obtain a six-degree-of-freedom motion estimate of the ship;

[0006] According to the ship's six-degree-of-freedom motion estimation, the horizontal position and heading data are extracted using the coordinate transformation matrix;

[0007] The horizontal position and heading data are processed by the Kalman filter algorithm with adaptive parameter adjustment to obtain the low-frequency position and heading estimation of the ship.

[0008] In one embodiment, the ship positioning data is processed using sensor fusion technology to obtain a six-degree-of-freedom motion estimate of the ship, including:

[0009] The following formula is used to establish the state equation of the ship with six degrees of freedom:

[0010]

[0011] Among them, p n 、v n and a n Respectively represent the position, velocity and acceleration vector of the center of gravity in the NED coordinate system, θ n ,Ω n and A n They represent the vector forms of Euler angle, Euler angular velocity and Euler angular acceleration respectively. and They represent the basic offset of IMU acceleration, the basic offset of IMU angular velocity and the basic offset of compass angular velocity respectively. and Represents the corresponding white noise, O and I represent the 0 matrix and the identity matrix respectively.

[0012] In one embodiment, the measurement data is processed using sensor fusion technology to obtain a six-degree-of-freedom motion estimate of the vessel, further comprising:

[0013] According to the working status of sensors 1 to i, the corresponding equations in the state equation of the six degrees of freedom of the ship are added or subtracted using the following formula to obtain the measurement equation:

[0014]

[0015] Among them, the above measurement equation contains at least

[0016] Indicates the position vector in NED coordinates obtained by GPS measurement and conversion. and Represent the acceleration vector, Euler angle vector and Euler angular velocity vector measured by IMU respectively. and are the heading and heading angular velocity measured by the compass, Indicates the position vector of other sensor measurements converted to NED coordinates. represents the transformation matrix from the ship coordinate system to the NED coordinate system, φ, θ, and ψ represent the roll, pitch, and heading angles, respectively, and p GPS|b 、p IMU|b and p Other|b Respectively represent the onboard coordinates of the corresponding sensors, and Represent the deviations of acceleration, Euler angular velocity and heading angular velocity respectively.

[0017] In one embodiment, according to the working status of sensors 1 to i, the corresponding equations in the state equation of the six degrees of freedom of the ship are added or subtracted to obtain the measurement equation, including:

[0018] Use the following formulas to calculate the acceleration, angular velocity and angular velocity deviation of each IMU:

[0019]

[0020] Among them, b a，IMUi，out Represents the acceleration deviation output of the i-th IMU, Represents the acceleration value measured by the i-th IMU, a b Indicates the acceleration value in the ship coordinate system, R T represents the transpose of the rotation matrix, g represents the acceleration of gravity, p IMUi|b represents the position of the i-th IMU in the ship coordinate system, p b Indicates the position of the origin of the ship coordinate system in the NED coordinate system, b Ω，IMUi，out represents the angular velocity deviation output of the i-th IMU, Represents the angular velocity value measured by the i-th IMU, Ω n Indicates the angular velocity value in the NED coordinate system, b Ω，Gyroi，out represents the angular velocity deviation output value of the i-th compass, Represents the angular velocity value measured by the i-th compass.

[0021] In one embodiment, according to the working status of sensors 1 to i, the corresponding equations in the state equation of the six degrees of freedom of the ship are added or subtracted to obtain the measurement equation, which also includes:

[0022] Use the following formula to calculate the average of the IMU acceleration, angular velocity, and compass angular velocity deviation:

[0023]

[0024] in, represents the average acceleration deviation of the i-th IMU, n represents the number of calculations, b a，IMUi，out represents the acceleration deviation output of the i-th IMU, represents the average angular velocity deviation of the i-th IMU, b Ω，IMUi，out represents the angular velocity deviation output of the i-th IMU, represents the average angular velocity deviation of the i-th compass, b Ω，Gyroi，out Indicates the angular velocity deviation output of the i-th compass.

[0025] In one embodiment, horizontal position and heading data are processed by a Kalman filter algorithm with adaptive parameter adjustment to obtain low-frequency position and heading estimates of the ship, including:

[0026] The state equation of the Kalman filter is established using the following formula:

[0027]

[0028] Where M represents the mass matrix, D represents the linear damping matrix, η represents the column vector consisting of the north displacement, east displacement and bow direction of the low-frequency motion of the center of gravity of the ship in NED coordinates, and v n represents the velocity vector in the NED coordinate corresponding to η, b represents the offset force in the three component directions of η, W b represents the white noise of b, τ represents the known force vector of the hull in the ship coordinate system, which consists of the x-direction force, the y-direction force and the torque, ξ1 represents the displacement of the second-order simple harmonic motion of the center of gravity in the three component directions of η, ξ2 represents the velocity vector corresponding to η, Ω=diag([ω 01 ω 02 ω 03 ]),ω 01 、ω 02 and ω 03 Represents the motion frequency of the three component vectors of ξ1, Z w =diag([ζ1ζ2ζ3]), where ζ1, ζ2 and ζ3 represent the relative damping ratios in the three directions of ξ1, respectively. ξ represents the white noise of ξ, represents the transformation matrix, ψ represents the heading angle;

[0029] Use the following formula to create the measurement equation:

[0030]

[0031] in, represents the position and heading measurement values, η represents the column vector composed of the north displacement, east displacement and heading of the low-frequency motion of the ship's center of gravity in NED coordinates, v b represents the velocity vector of the ship in the ship coordinate system, b represents the offset force in the three component directions of η, ξ1 represents the displacement of the center of gravity in the three component directions of η, and ξ2 represents the velocity vector corresponding to η;

[0032] The horizontal position and heading data are processed using the established state equation and measurement equation to obtain the ship's low-frequency position and heading estimation.

[0033] In one embodiment, horizontal position and heading data are processed by a Kalman filter with automatic parameter adjustment to obtain low-frequency position and heading estimation of the ship, further comprising:

[0034] Collect time series of horizontal position and heading data;

[0035] Perform binomial fitting on the time series and obtain the square mean of the residuals;

[0036] Calculate the parameter adjustment coefficient based on the square mean of the residuals;

[0037] The covariance parameters of the Kalman filter are adjusted according to the parameter adjustment coefficients to generate low-frequency position and heading estimates.

[0038] In a second aspect, the present application also provides a ship positioning system based on multi-sensor fusion, comprising:

[0039] The data acquisition and preprocessing module is used to obtain ship positioning data from multiple sensors and process the ship positioning data using sensor fusion technology to obtain the ship's six-degree-of-freedom motion estimation;

[0040] The data conversion and feature extraction module is used to extract the horizontal position and heading data based on the ship's six-degree-of-freedom motion estimation using the coordinate transformation matrix;

[0041] The filtering and estimation module is used to process the horizontal position and heading data through the Kalman filtering algorithm with adaptive parameter adjustment to obtain the low-frequency position and heading estimation of the ship.

[0042] In a third aspect, the present application further provides an electronic device, including a memory and a processor, wherein the memory stores a computer program, and when the processor executes the computer program, the steps of the above-mentioned ship positioning method based on multi-sensor fusion are implemented:

[0043] In a fourth aspect, the present application further provides a computer-readable storage medium having a computer program stored thereon, wherein when the computer program is executed by a processor, the steps of the above-mentioned ship positioning method based on multi-sensor fusion are implemented:

[0044] The above-mentioned ship positioning method, system, and electronic equipment based on multi-sensor fusion effectively solve the problems of limited sensor accuracy and error accumulation in traditional technologies through the dynamic fusion and deviation collaborative calibration mechanism of multi-source sensor data. The adaptive extended Kalman filter architecture is used to dynamically integrate the measurement data of heterogeneous sensors such as GPS, IMU and compass. By calculating the sensor deviation mean in real time and updating the state equation, the influence of IMU zero bias drift and compass installation error on the six-degree-of-freedom estimation is overcome; the innovatively designed coordinate transformation matrix accurately extracts horizontal displacement and heading parameters in the NED coordinate system, and combines the adaptive Kalman filter algorithm including the second-order simple harmonic motion model to dynamically adjust the process noise covariance through residual analysis, thereby improving the stability of the ship's low-frequency motion estimation under wave interference. This technical solution realizes the autonomous reconstruction of the model in the state of sensor failure, so that the positioning system can still maintain reliable output when some sensors fail, and overall improves the positioning accuracy and system robustness in complex sea conditions. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] In order to more clearly illustrate the technical solutions in the embodiments of the present application or related technologies, the following briefly introduces the drawings required for use in the embodiments or related technical descriptions. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0046] Figure 1 This is a flow chart of a ship positioning method based on multi-sensor fusion according to the present invention;

[0047] Figure 2 A flowchart of a ship positioning method based on multi-sensor fusion according to the present invention;

[0048] Figure 3 This is a structural diagram of a ship positioning system based on multi-sensor fusion of the present invention. DETAILED DESCRIPTION

[0049] In order to make the purpose, technical solutions and advantages of this application more clear, the following further describes this application in detail with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain this application and are not intended to limit this application.

[0050] The present application discloses a ship positioning method, system, and electronic equipment based on multi-sensor fusion. The hardware environment consists of a multi-source sensing layer that integrates satellite positioning, inertial measurement, environmental perception, and other sensor groups, an edge computing layer, and a cloud collaboration layer. When the ship encounters wind and wave disturbances, sudden ocean current changes, or sensor anomalies, the computing terminal automatically integrates multi-dimensional sensor data, dynamically adjusts filter parameters, and compensates for positioning deviations. At the same time, data mutual verification and emergency positioning switching are achieved through redundant communication links and cloud servers, ensuring that stable navigation output can be maintained under extreme conditions such as satellite signal loss and single sensor failure, meeting the International Maritime Organization's functional safety standards for dynamic positioning systems and all-weather operation requirements.

[0051] In one embodiment, Figure 1 As shown, a ship positioning method based on multi-sensor fusion is provided. This embodiment uses the method applied to a local computing terminal on a ship as an example. It is understandable that the method can also be applied to a server, or to a system including a terminal and a server, and implemented through the interaction between the terminal and the server. In this embodiment, the method includes the following steps:

[0052] S101, acquiring ship positioning data from multiple sensors, and processing the ship positioning data using sensor fusion technology to obtain a six-degree-of-freedom motion estimate of the ship.

[0053] Ship positioning typically utilizes a variety of sensors, including the Global Positioning System (GPS), Inertial Measurement Unit (IMU), and compass. GPS measures and converts the position vector to North East Down (NED) coordinates, providing the ship's geographic location. The IMU measures acceleration vectors, Euler angle vectors, and Euler angular velocity vectors, reflecting the ship's motion and acceleration. The compass measures heading and heading angular velocity, helping to determine the ship's course. These multiple data sources describe the ship's motion from different perspectives. Sensor fusion technology integrates data from multiple sensors, such as by constructing a state equation for the ship's six degrees of freedom (DOF). This can be achieved by deriving measurement equations based on the operating status of each sensor. This unifies the data from different coordinate systems, accurately reflecting the ship's motion in the NED coordinate system and providing an estimate of the ship's six-DOF motion.

[0054] S102, based on the six-degree-of-freedom motion estimation of the ship, the horizontal position and heading data are extracted using a coordinate transformation matrix.

[0055] The motion of a ship in the ocean environment is extremely complex. Six-degree-of-freedom (6DOF) motion estimation can comprehensively describe the ship's motion state, typically including translational motion along the x, y, and z axes (corresponding to position, velocity, and acceleration), as well as rotational motion about these three coordinate axes. Rotational motion can be represented by Euler angles, Euler angular velocities, and Euler angular accelerations, comprehensively reflecting the ship's position, attitude, and changes in space. A coordinate transformation matrix can be used to convert motion data in the ship's coordinate system to the NED coordinate system. For example, horizontal position and heading data can be extracted from the 6DOF motion estimation data. Horizontal position data determines the ship's specific geographic location in the ocean, while heading data defines the ship's heading. During collision avoidance maneuvers, horizontal position data can be used to determine the relative position of the ship to other objects, such as other ships and reefs, while heading data helps predict the ship's motion trend. The combination of these two data allows the pilot to take timely and effective avoidance measures, ensuring safe navigation.

[0056] S103, processing the horizontal position and heading data by using a Kalman filter algorithm with adaptive parameter adjustment to obtain low-frequency position and heading estimation of the ship.

[0057] The Kalman filter is a linear optimal estimation algorithm based on a system's state-space model. It uses two steps, predicting and updating the system state, to continuously refine the estimate based on new measurement data. In ship positioning, the system state can be represented by information such as the ship's position and velocity, while measurement data comes from various sensors such as GPS and compass. The optimal estimate is derived by combining dynamic and measurement equations with the statistical characteristics of system and measurement noise. In practical applications, the navigation environment is complex and variable, and the statistical characteristics of system and measurement noise may change over time and with environmental changes. Adaptive parameter adjustment can be employed to adjust parameters in real time based on actual measurement data and changes in the system state. This allows the filter algorithm to better adapt to diverse environmental conditions and improve the accuracy and reliability of the estimate. During navigation, a ship's position and heading changes contain both high-frequency and low-frequency components. High-frequency components are typically short-term fluctuations caused by random disturbances such as waves and wind, while low-frequency components reflect the ship's true motion. By processing horizontal position and heading data with the Kalman filter algorithm, high-frequency noise can be filtered out to obtain low-frequency position and heading estimates. In the ship's automatic rudder control, low-frequency heading estimation can provide more stable heading information, allowing the ship to maintain the predetermined route more accurately; in the ship's positioning system, low-frequency position estimation can reduce the impact of random interference and improve positioning accuracy and reliability.

[0058] The present application discloses a ship positioning method, system and electronic equipment based on multi-sensor fusion, which obtains ship positioning data from multiple sensors, uses sensor fusion technology to obtain a six-degree-of-freedom motion estimate of the ship, and then uses a coordinate transformation matrix to extract horizontal position and heading data from the estimate. Finally, the extracted data is processed by a Kalman filter algorithm with adaptive parameter adjustment to obtain a low-frequency position and heading estimate of the ship, realizing dynamic fusion and deviation collaborative calibration of multi-source sensor data, overcoming the problems of limited accuracy of a single sensor, insufficient adaptability to dynamic environments, and low efficiency of multi-sensor fusion in traditional ship positioning methods, effectively improving the accuracy of ship positioning in complex sea conditions and the robustness of the system, and ensuring that the ship positioning results are more stable and reliable.

[0059] In one embodiment, the ship positioning data is processed using sensor fusion technology to obtain a six-degree-of-freedom motion estimate of the ship, including:

[0060] S201, use the following formula to establish the state equation of the ship's six degrees of freedom:

[0061]

[0062] Among them, p n 、v n and a n Respectively represent the position, velocity and acceleration vector of the center of gravity in the NED coordinate system, θ n ,Ω n and A n They represent the vector forms of Euler angle, Euler angular velocity and Euler angular acceleration respectively. and They represent the basic offset of IMU acceleration, the basic offset of IMU angular velocity and the basic offset of compass angular velocity respectively. and Represents the corresponding white noise, O and I represent the 0 matrix and the identity matrix respectively.

[0063] For example, p n 、v n and a n Represent the position, velocity and acceleration vectors of the ship's center of gravity in the NED coordinate system, which can accurately describe the position change, velocity and acceleration of the ship in space. n ,Ω n and A n These are the vector forms of Euler angles, Euler angular velocity, and Euler angular acceleration, respectively. Euler angles describe a ship's rotational attitude about three coordinate axes. Common Euler angle expressions include roll, pitch, and heading. Euler angular velocity and Euler angular acceleration describe the rate of change of a ship's rotational attitude. and These represent the basic offsets for IMU acceleration, IMU angular velocity, and compass angular velocity, respectively. Since the sensors themselves have errors, these offsets are used to compensate for the deviation between the sensor measurements and the true values, improving measurement accuracy. and represents the corresponding white noise, which has zero mean, constant variance, and independence at each moment. By accounting for the influence of white noise, the state equation can be made more realistic. O and I represent the zero matrix and the identity matrix, respectively. In matrix operations in the state equation, the zero matrix is used to indicate that certain variables have no direct coupling relationship, while the identity matrix is used to maintain the original values of certain variables. By establishing the state equation for the ship's six degrees of freedom in this way, the ship's motion state can be described using a mathematical model.

[0064] In one embodiment, the measurement data is processed using sensor fusion technology to obtain a six-degree-of-freedom motion estimate of the vessel, further comprising:

[0065] S301: Based on the working status of sensors 1 to i, the corresponding equations in the ship's six-degree-of-freedom state equation are added or subtracted using the following formula to obtain a measurement equation:

[0066]

[0067] Among them, the above measurement equation contains at least

[0068] Indicates the position vector in NED coordinates obtained by GPS measurement and conversion. and Represent the acceleration vector, Euler angle vector and Euler angular velocity vector measured by IMU respectively. and are the heading and heading angular velocity measured by the compass, Indicates the position vector of other sensor measurements converted to NED coordinates. represents the transformation matrix from the ship coordinate system to the NED coordinate system, φ, θ, and ψ represent the roll, pitch, and heading angles, respectively, and p GPS|b 、p IMU|b and p Other|b Respectively represent the onboard coordinates of the corresponding sensors, and Represent the deviations of acceleration, Euler angular velocity and heading angular velocity respectively.

[0069] Specifically, ship positioning relies on the coordinated work of multiple sensors. Different sensors may experience failures, signal interference, etc. during actual operation, and their working status may change at any time. For example, the GPS signal may be weakened or lost due to occlusion, and the IMU may cause inaccurate measurements due to its own error accumulation. The operating status of each sensor is monitored in real time, and the subsequent data processing method is adjusted according to its status to ensure the stable and reliable operation of the positioning system. If a GPS sensor works abnormally, the system can adjust the state equation to reduce the weight of the sensor data or temporarily not use its data to avoid erroneous data affecting the overall positioning accuracy. The measurement equation is obtained by adding and subtracting corresponding equations to the six-degree-of-freedom state equation of the ship, and integrating the relationship between the measurement data of each sensor and the ship's motion parameters. For example, the IMU measures acceleration in the ship's coordinate system. After the conversion matrix, combined with the gravity acceleration and the relevant parameters of the ship's coordinate system, the acceleration data can be converted to the NED coordinate system and fused and analyzed with other sensor data in the same reference system. GPS|b It is the position vector measured by GPS and converted to NED coordinates, providing basic geographic coordinate information for ship positioning; and It is the acceleration, Euler angle and Euler angular velocity vector measured by IMU, reflecting the ship's motion attitude and acceleration changes; and It is the heading and heading angular velocity measured by the compass, which can clearly indicate the ship's heading and turning changes; Other|b This is the position vector converted from other sensor measurements to NED coordinates, providing additional positioning information. These data, combined with the deviation information from each sensor, form a measurement equation that comprehensively reflects the ship's motion state. This provides key data support for the subsequent precise calculation of the ship's six-degree-of-freedom motion estimation, effectively improving ship positioning accuracy and system reliability.

[0070] In one embodiment, according to the working status of sensors 1 to i, the corresponding equations in the state equation of the six degrees of freedom of the ship are added or subtracted to obtain the measurement equation, including:

[0071] S401, use the following formula to calculate the acceleration, angular velocity and angular velocity deviation of each IMU:

[0072]

[0073] Among them, b a，IMUi，out represents the acceleration deviation output of the i-th IMU, Represents the acceleration value measured by the i-th IMU, a b Indicates the acceleration value in the ship coordinate system, R T represents the transpose of the rotation matrix, g represents the acceleration of gravity, p IMUi|b represents the position of the i-th IMU in the ship coordinate system, pb Indicates the position of the origin of the ship coordinate system in the NED coordinate system, b Ω，IMUi，out represents the angular velocity deviation output of the i-th IMU, Represents the angular velocity value measured by the i-th IMU, Ω n Indicates the angular velocity value in the NED coordinate system, b Ω，Gyroi，out represents the angular velocity deviation output value of the i-th compass, Represents the angular velocity value measured by the i-th compass.

[0074] For example, sensors can exhibit errors during actual measurement. These errors can be caused by various factors, including sensor accuracy limitations, inaccurate installation placement, and environmental interference. By calculating the acceleration and angular velocity of each IMU and the angular velocity deviation of the compass, the sensor measurements can be corrected, thereby improving the accuracy of the ship's six-degree-of-freedom motion estimation. In the subsequent sensor fusion process, using these corrected measurements can better reflect the ship's actual motion state, providing a more reliable basis for navigation, control, and positioning.

[0075] In one embodiment, according to the working status of sensors 1 to i, the corresponding equations in the state equation of the six degrees of freedom of the ship are added or subtracted to obtain the measurement equation, which also includes:

[0076] S501, calculate the average of the deviation of the IMU acceleration, angular velocity, and compass angular velocity using the following formula:

[0077]

[0078] in, represents the average acceleration deviation of the i-th IMU, n represents the number of calculations, b a，IMUi，out represents the acceleration deviation output of the i-th IMU, represents the average angular velocity deviation of the i-th IMU, b Ω，IMUi，out represents the angular velocity deviation output of the i-th IMU, represents the average angular velocity deviation of the i-th compass, b Ω，Gyroi，out Indicates the angular velocity deviation output of the i-th compass.

[0079] Specifically, this formula calculates the average deviation of the IMU's acceleration, angular velocity, and compass's angular velocity, providing a more accurate understanding of sensor deviations and improving the reliability of subsequent positioning data processing. The deviation measurements of a single sensor may be affected by random factors, such as momentary electromagnetic interference and mechanical vibration, causing fluctuations in the measured value. Calculating the average deviation from multiple measurements effectively reduces the impact of these random factors, ensuring that the resulting deviation data better reflects the sensor's true deviation. Based on the average deviation, subsequent sensor measurement data processing can be more accurately corrected, thereby improving the accuracy of the ship's positioning and the reliability of the system.

[0080] S601, in one embodiment, processing horizontal position and heading data using a Kalman filter algorithm with adaptive parameter adjustment to obtain a low-frequency position and heading estimate of the vessel, including:

[0081] The state equation of the Kalman filter is established using the following formula:

[0082]

[0083] Where M represents the mass matrix, D represents the linear damping matrix, η represents the column vector consisting of the north displacement, east displacement and bow direction of the low-frequency motion of the center of gravity of the ship in NED coordinates, and v n represents the velocity vector in the NED coordinate corresponding to η, b represents the offset force in the three component directions of η, W b represents the white noise of b, τ represents the known force vector of the hull in the ship coordinate system, which consists of the x-direction force, the y-direction force and the torque, ξ1 represents the displacement of the second-order simple harmonic motion of the center of gravity in the three component directions of η, ξ2 represents the velocity vector corresponding to η, Ω=diag([ω 01 ω 02 ω 03 ]),ω 01 、ω 02 and ω 03 Represents the motion frequency of the three component vectors of ξ1, Z w =diag([ζ1 ζ2 ζ3]), where ζ1, ζ2 and ζ3 represent the relative damping ratios in the three directions of ξ1, respectively. ξ represents the white noise of ξ, represents the transformation matrix, ψ represents the heading angle;

[0084] S602, establish the measurement equation using the following formula:

[0085]

[0086] in, represents the position and heading measurement values, η represents the column vector composed of the north displacement, east displacement and heading of the low-frequency motion of the ship's center of gravity in NED coordinates, v b represents the velocity vector of the ship in the ship coordinate system, b represents the offset force in the three component directions of η, ξ1 represents the displacement of the center of gravity in the three component directions of η, and ξ2 represents the velocity vector corresponding to η;

[0087] S603: Process the horizontal position and heading data using the established state equation and measurement equation to obtain the low-frequency position and heading estimation of the ship.

[0088] For example, the mass matrix M reflects the inertial characteristics of the ship in different directions, affecting the ship's response to force; the linear damping matrix D describes the linear damping effect on the ship during motion, such as water resistance, which gradually attenuates the ship's motion. The column vector η reflects the position and attitude of the ship in NED coordinates, v n The corresponding velocity vector describes the rate of change of the ship's position and attitude. The offset force b may be caused by environmental factors or the ship's own characteristics. The white noise W b Used to simulate the random part of the offset force, the known force vector τ represents the measurable or known external force acting on the ship. z It is a conversion matrix related to the heading angle, which is used to convert the forces and motions in the ship coordinate system to the NED coordinate system, thus achieving the unification of information between different coordinate systems. The column vector η, the displacement ξ1 of the second-order simple harmonic motion, and the velocity vector ξ2 are linked to the measured values, allowing the system state to be inferred from these measurements. The Kalman filter algorithm can be used to process the horizontal position and heading data, adjusting the ship's state estimate in real time based on new measurement data. This algorithm takes into account the low-frequency motion characteristics of the ship, resulting in a low-frequency position and heading estimate. This effectively filters out the effects of high-frequency noise, such as short-term fluctuations caused by waves, more accurately reflecting the ship's true motion trends and providing reliable information for applications such as navigation, control, and positioning.

[0089] In one embodiment, horizontal position and heading data are processed by a Kalman filter with automatic parameter adjustment to obtain low-frequency position and heading estimation of the ship, further comprising:

[0090] S701, collecting time series of horizontal position and heading data;

[0091] S702, performing binomial fitting on the time series to obtain the square mean of the residuals;

[0092] S703, calculating the parameter adjustment coefficient according to the square mean of the residual;

[0093] S704: Adjust the covariance parameters of the Kalman filter according to the parameter adjustment coefficient to generate low-frequency position and heading estimation.

[0094] Specifically, as a ship navigates, its horizontal position and heading constantly change. Sensors continuously collect this data, forming a time series. Collecting this data is the foundation for subsequent analysis and processing, as it contains dynamic information about the ship's motion. By collecting horizontal position and heading data at different times, we can fully record the ship's trajectory and directional changes, providing raw data for analyzing the ship's motion patterns. This data can be obtained from sensors such as GPS and compasses and accurately recorded in chronological order to ensure the continuity and accuracy of the time series. Binomial fitting is a mathematical method used to find a binomial curve that best fits the time series data. In ship positioning, the Kalman filter estimate is compared with the actual horizontal position and heading data. The difference between the two is the residual. The squared mean of the residuals is calculated by squaring and averaging the residuals, which measures the degree of deviation between the Kalman filter estimate and the actual value. A smaller squared mean of the residual indicates a closer Kalman filter estimate to the actual value, indicating a better estimate; a smaller squared mean of the residual indicates a significant deviation, requiring adjustment of the filter parameters. The squared mean of the residual reflects the estimation error of the Kalman filter model. Based on this value, the tuning coefficients are calculated to determine the adjustment magnitude of the Kalman filter covariance parameters. The calculation of the tuning coefficients is typically based on certain mathematical rules or algorithms, with adjustments made based on the mean squared residuals. If the mean squared residuals are large, the tuning coefficients increase the covariance parameter adjustment magnitude to further optimize the model. If the mean squared residuals are small, the tuning coefficients adaptively adjust the Kalman filter model based on the actual error. The covariance parameters influence the stability and accuracy of the Kalman filter estimation process. Adjusting the covariance parameters based on the calculated tuning coefficients optimizes the Kalman filter model's estimation of the ship's motion. After adjusting the covariance parameters, reprocessing the horizontal position and heading data using the Kalman filter can generate more accurate low-frequency position and heading estimates. By continuously adjusting the covariance parameters, the Kalman filter model better adapts to changes in the ship's motion, resulting in more accurate low-frequency position and heading estimates that better reflect the ship's actual motion, thereby improving the accuracy and reliability of ship positioning.

[0095] The present application discloses a ship positioning method, system and electronic equipment based on multi-sensor fusion. Through the dynamic fusion and deviation collaborative calibration mechanism of multi-source sensor data, it effectively solves the problems of limited sensor accuracy and error accumulation in traditional technologies. An adaptive extended Kalman filter architecture is used to dynamically integrate the measurement data of heterogeneous sensors such as GPS, IMU and compass. By calculating the sensor deviation mean in real time and updating the state equation, the influence of IMU zero bias drift and compass installation error on the six-degree-of-freedom estimation is overcome. The innovatively designed coordinate transformation matrix accurately extracts horizontal displacement and heading parameters in the NED coordinate system. Combined with the adaptive Kalman filter algorithm containing the second-order simple harmonic motion model, the process noise covariance is dynamically adjusted through residual analysis to improve the stability of the ship's low-frequency motion estimation under wave interference. This technical solution realizes the autonomous reconstruction of the model under sensor failure state, so that the positioning system can still maintain reliable output when some sensors fail, and overall improves the positioning accuracy and system robustness under complex sea conditions, ensuring that the ship positioning results are more stable and reliable, and meeting the International Maritime Organization's functional safety standards for dynamic positioning systems and all-weather operation requirements.

[0096] In order to further illustrate the solution of the embodiment of the present application, a specific example is given below.

[0097] In this example, a ship is used Figure 2 The ship positioning method based on multi-sensor fusion shown includes:

[0098] S1, use the following formula to fuse multi-sensor information to establish the ship's six-degree-of-freedom state equation:

[0099]

[0100] Among them, p n 、v n and a n Respectively represent the position, velocity and acceleration vector of the center of gravity in the NED coordinate system, θ n ,Ω n and A n They represent the vector forms of Euler angle, Euler angular velocity and Euler angular acceleration respectively. and They represent the basic offset of IMU acceleration, the basic offset of IMU angular velocity and the basic offset of compass angular velocity respectively. and Represents the corresponding white noise, O and I represent the 0 matrix and the identity matrix respectively.

[0101] S2, according to the working status of sensors 1 to i, use the following formula to add or subtract the corresponding equations in the state equation of the six degrees of freedom of the ship to obtain the measurement equation:

[0102]

[0103] in, Indicates the position vector in NED coordinates obtained by GPS measurement and conversion. and Represent the acceleration vector, Euler angle vector and Euler angular velocity vector measured by IMU respectively.

[0104] and are the heading and heading angular velocity measured by the compass, Indicates the position vector of other sensor measurements converted to NED coordinates.

[0105] represents the transformation matrix from the ship coordinate system to the NED coordinate system, φ, θ, and ψ represent the roll, pitch, and heading angles, respectively, and p GPS|b 、p IMU|b and p Other|b Respectively represent the onboard coordinates of the corresponding sensors, and Represent the deviations of acceleration, Euler angular velocity, and heading angular velocity, respectively. The number of each sensor type in the above measurement equation must be greater than or equal to 0. During the operation of the dynamic positioning system, the above measurement equation is continuously added and deleted based on the arrival of sensor data. To ensure system stability, the principle of minimum sensors must be adhered to. The measurement equation must contain at least the following equations, regardless of whether the corresponding sensors exist: At this point, if there is no corresponding sensor, a sensor measurement equation is still constructed, and the covariance of its measurement noise is set to a large value, such as 1×10 12 Depending on whether there are enough sensors to calculate position and heading, it is possible to know if position and heading are lost.

[0106] S3, estimates the six degrees of freedom motion of the ship using unscented Kalman filtering.

[0107] S4 calculates the acceleration offset, angular velocity offset, and angular velocity offset of the inertial measurement module (IMU) and the compass according to the six degrees of freedom of the ship, and feeds them back to the measurement equation, including:

[0108] S4.1. Calculate the acceleration, angular velocity, and angular velocity bias of each IMU using the following formulas:

[0109]

[0110] Among them, b a，IMUi，outrepresents the acceleration deviation output of the i-th IMU, Represents the acceleration value measured by the i-th IMU, a b Indicates the acceleration value in the ship coordinate system, R T represents the transpose of the rotation matrix, g represents the acceleration of gravity, p IMUi|b represents the position of the i-th IMU in the ship coordinate system, p b Indicates the position of the origin of the ship coordinate system in the NED coordinate system, b Ω，IMUi，out represents the angular velocity deviation output of the i-th IMU, Represents the angular velocity value measured by the i-th IMU, Ω n Indicates the angular velocity value in the NED coordinate system, b Ω，Gyroi，out represents the angular velocity deviation output value of the i-th compass, represents the angular velocity value measured by the i-th compass;

[0111] S4.2. Obtain n calculation results of acceleration, angular velocity, and compass angular velocity deviation, and calculate the average of the IMU acceleration, angular velocity, and compass angular velocity deviation using the following formula:

[0112]

[0113] in, represents the average acceleration deviation of the i-th IMU, n represents the number of calculations, b a，IMUi，out represents the acceleration deviation output of the i-th IMU, represents the average angular velocity deviation of the i-th IMU, b Ω，IMUi，out represents the angular velocity deviation output of the i-th IMU, represents the average angular velocity deviation of the i-th compass, b Ω，Gyroi，out represents the angular velocity deviation output of the i-th compass;

[0114] S4.3, feeding back the acceleration, angular velocity, compass angular velocity deviation and the average of the acceleration, angular velocity and compass angular velocity deviation to the measurement equation.

[0115] S5, the horizontal position and heading data are processed by the Kalman filter algorithm with adaptive parameter adjustment to obtain the low-frequency position and heading estimation of the ship, including:

[0116] S5.1, use the following formula to establish the state equation of the Kalman filter:

[0117]

[0118] Where M represents the mass matrix, D represents the linear damping matrix, η represents the column vector consisting of the north displacement, east displacement and bow direction of the low-frequency motion of the center of gravity of the ship in NED coordinates, and v n represents the velocity vector in the NED coordinate corresponding to η, b represents the offset force in the three component directions of η, W b represents the white noise of b, τ represents the known force vector of the hull in the ship coordinate system, which consists of the x-direction force, the y-direction force and the torque, ξ1 represents the displacement of the second-order simple harmonic motion of the center of gravity in the three component directions of η, ξ2 represents the velocity vector corresponding to η, Ω=diag([ω 01 ω 02 ω 03 ]),ω 01 、ω 02 and ω 03 Represents the motion frequency of the three component vectors of ξ1, Z w =diag([ζ1 ζ2 ζ3]), where ζ1, ζ2 and ζ3 represent the relative damping ratios in the three directions of ξ1, respectively. ξ represents the white noise of ξ, represents the transformation matrix, ψ represents the heading angle;

[0119] S5.2, establish the measurement equation using the following formula:

[0120]

[0121] in, represents the position and heading measurement values, η represents the column vector composed of the north displacement, east displacement and heading of the low-frequency motion of the ship's center of gravity in NED coordinates, v b represents the velocity vector of the ship in the ship coordinate system, b represents the offset force in the three component directions of η, ξ1 represents the displacement of the center of gravity in the three component directions of η, and ξ2 represents the velocity vector corresponding to η;

[0122] S5.3, using the established state equation and measurement equation to process the horizontal position and heading data to obtain the low-frequency position and heading estimation of the ship;

[0123] S5.4: When the position or heading obtained from step S2 is lost, the Kalman filter is dead reckoning. In this case, no automatic parameter adjustment is performed and the subsequent parameter adjustment steps are skipped.

[0124] S6, respectively collecting the time series of the horizontal position and heading data output in steps S3 and S5, wherein the length of the collected time window is T, the number of data points contained in it is N, and T is 8 wave periods;

[0125] S6.1, perform binomial fitting on each of the three sets of time series data output in step S3 to obtain the square mean of the residuals, where the norm of the residuals of any set of fitted data is normr, and the square mean of the residuals is RMS_a_z = normr / sqrt(N);

[0126] S6.2, perform binomial fitting on the three sets of data output in step S5: the low-frequency north position, the east position, and the heading. The norm of the residuals of any set of fittings is normr, and the square mean of the residuals RMS_a_l = normr / sqrt(N);

[0127] S6.3, performing binomial fitting on the three sets of high-frequency north position, east position, and heading data output in step S5, respectively. The norm of the residuals of any set of fittings is normr, and the square mean of the residuals RMS_a_w = normr / sqrt(N);

[0128] S6.4 performs binomial fitting on the three sets of data outputted from steps S3 and S5, namely the low-frequency north position, east position, and heading, respectively, and calculates the difference Dd between the data points obtained by the fitting equation and the data points obtained by the fitting equation in step 6, where the square mean of Dd is RMS_b_l = norm(Dd) / sqrt(N);

[0129] S6.5, calculate the parameter adjustment coefficient based on the square mean of the residual, square the data points output in step S6.1 respectively and calculate the average value to obtain RMS_wxi, and at the same time set the benchmark values of RMS_a_l, RMS_b_l and RMS_wxi to RMS_a_lm, RMS_b_lm and RMS_wxim.

[0130] S7, adjusting the covariance parameters of the Kalman filter according to the parameter adjustment coefficient to generate low-frequency position and heading estimates, wherein the parameter adjustment coefficient is calculated according to the following formula:

[0131]

[0132] Among them, the coefficients c_wxi and c_wb∈[0.96,1.04]

[0133] S7.1 The parameter covariance of the Kalman filter at the current time t in step S7 can be calculated from the covariance at the previous step t-1: in, and Set upper and lower limits according to actual conditions. The initial value of takes the upper limit, The initial value of is the lower limit.

[0134] It should be understood that, although the various steps in the flowcharts involved in the various embodiments described above are displayed in sequence according to the instructions of the arrows, these steps are not necessarily executed in sequence in the order indicated by the arrows. Unless otherwise specified herein, there is no strict order restriction on the execution of these steps, and these steps can be executed in other orders. Moreover, at least a portion of the steps in the flowcharts involved in the various embodiments described above can include multiple steps or multiple stages, and these steps or stages are not necessarily executed and completed at the same time, but can be executed at different times, and the execution order of these steps or stages is not necessarily to be carried out in sequence, but can be executed in turn or alternately with other steps or at least a portion of steps or stages in other steps.

[0135] Based on the same inventive concept, embodiments of the present application also provide a ship positioning system based on multi-sensor fusion for implementing the aforementioned ship positioning method based on multi-sensor fusion. The solution provided by this system is similar to the solution described in the aforementioned method. Therefore, the specific limitations of one or more embodiments of a ship positioning system based on multi-sensor fusion provided below can be found in the above-mentioned limitations of the ship positioning method based on multi-sensor fusion, and will not be further elaborated here.

[0136] In an exemplary embodiment, Figure 3 As shown, a ship positioning system based on multi-sensor fusion is provided, including:

[0137] The data acquisition and preprocessing module 11 is used to obtain the ship positioning data from multiple sensors and process the ship positioning data using sensor fusion technology to obtain the ship's six-degree-of-freedom motion estimation;

[0138] The data conversion and feature extraction module 12 is used to extract the horizontal position and heading data using the coordinate transformation matrix according to the six-degree-of-freedom motion estimation of the ship;

[0139] The filtering and estimation module 13 is used to process the horizontal position and heading data through a Kalman filtering algorithm with adaptive parameter adjustment to obtain the low-frequency position and heading estimation of the ship.

[0140] In one embodiment, an electronic device is provided, including a memory and a processor, wherein the memory stores a computer program, and when the processor executes the computer program, the steps of the ship positioning method based on multi-sensor fusion as described above are implemented.

[0141] In one embodiment, a computer-readable storage medium is provided, on which a computer program is stored. When the computer program is executed by a processor, the steps in the above-mentioned method embodiments are implemented.

[0142] For the device embodiments, since they basically correspond to the method embodiments, the relevant parts can be referred to the partial description of the method embodiments. The device embodiments described above are merely illustrative, wherein the components described as separate parts may or may not be physically separated, and the parts displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules can be selected according to actual needs to achieve the purpose of the disclosed solution. A person of ordinary skill in the art can understand and implement it without expending creative work.

[0143] The above-described embodiments merely represent several implementation methods of the embodiments of the present application. While the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the patent application. It should be noted that a person skilled in the art may make various modifications and improvements without departing from the concept of the embodiments of the present application, and these modifications and improvements fall within the scope of protection of the embodiments of the present application.

Claims

1. A ship positioning method based on multi-sensor fusion, characterized in that: The method comprises: Acquire ship positioning data from multiple sensors and process the ship positioning data using sensor fusion technology to obtain a six-degree-of-freedom motion estimate of the ship; Extracting horizontal position and heading data using a coordinate transformation matrix based on the six-degree-of-freedom motion estimation of the ship; The horizontal position and heading data are processed by a Kalman filter algorithm with adaptive parameter adjustment to obtain low-frequency position and heading estimation of the ship.

2. The method according to claim 1, characterized in that The processing of the ship positioning data using sensor fusion technology to obtain a six-degree-of-freedom motion estimate of the ship includes: The following formula is used to establish the state equation of the ship with six degrees of freedom: Among them, p n 、v n and a n Respectively represent the position, velocity and acceleration vector of the center of gravity in the NED coordinate system, θ n ,Ω n and A n They represent the vector forms of Euler angle, Euler angular velocity and Euler angular acceleration respectively. and They represent the basic offset of IMU acceleration, the basic offset of IMU angular velocity and the basic offset of compass angular velocity respectively. and Represents the corresponding white noise, O and I represent the 0 matrix and the identity matrix respectively.

3. The method according to claim 2, characterized in that The processing of the ship positioning data by using sensor fusion technology to obtain a six-degree-of-freedom motion estimate of the ship further includes: According to the working status of sensors 1 to i, the corresponding equations in the state equation of the six degrees of freedom of the ship are added or subtracted using the following formula to obtain the measurement equation: Among them, the above measurement equation contains at least Indicates the position vector in NED coordinates obtained by GPS measurement and conversion. and Represent the acceleration vector, Euler angle vector and Euler angular velocity vector measured by IMU respectively. and are the heading and heading angular velocity measured by the compass, Indicates the position vector of other sensor measurements converted to NED coordinates. represents the transformation matrix from the ship coordinate system to the NED coordinate system, φ, θ, and ψ represent the roll, pitch, and heading angles, respectively, and p GPS|b 、p IMU|b and p Other|b Respectively represent the onboard coordinates of the corresponding sensors, and Represent the deviations of acceleration, Euler angular velocity and heading angular velocity respectively.

4. The method according to claim 3, characterized in that According to the working status of sensors 1 to i, the corresponding equations in the state equation of the six degrees of freedom of the ship are added or subtracted to obtain the measurement equation, including: Use the following formulas to calculate the acceleration, angular velocity and angular velocity deviation of each IMU: Among them, b a，IMUi，out represents the acceleration deviation output of the i-th IMU, Represents the acceleration value measured by the i-th IMU, a b Indicates the acceleration value in the ship coordinate system, R T represents the transpose of the rotation matrix, g represents the acceleration of gravity, p IMUi|b represents the position of the i-th IMU in the ship coordinate system, p b Indicates the position of the origin of the ship coordinate system in the NED coordinate system, b Ω，IMUi，out represents the angular velocity deviation output of the i-th IMU, Represents the angular velocity value measured by the i-th IMU, Ω n Indicates the angular velocity value in the NED coordinate system, b Ω，Gyroi，out represents the angular velocity deviation output value of the i-th compass, Represents the angular velocity value measured by the i-th compass.

5. The method according to claim 3, characterized in that The method of adding or subtracting corresponding equations in the state equation of the six degrees of freedom of the ship according to the working status of sensors 1 to i to obtain the measurement equation further includes: Use the following formula to calculate the average of the IMU's acceleration, angular velocity, and the deviation of the compass's angular velocity: in, represents the average acceleration deviation of the i-th IMU, n represents the number of calculations, b a，IMUi，out represents the acceleration deviation output of the i-th IMU, represents the average angular velocity deviation of the i-th IMU, b Ω，IMUi，out represents the angular velocity deviation output of the i-th IMU, represents the average angular velocity deviation of the i-th compass, b Ω，Gyroi，out Indicates the angular velocity deviation output of the i-th compass.

6. The method according to claim 3, characterized in that The Kalman filter algorithm with adaptive parameter adjustment processes the horizontal position and heading data to obtain low-frequency position and heading estimation of the ship, including: The state equation of the Kalman filter is established using the following formula: Where M represents the mass matrix, D represents the linear damping matrix, η represents the column vector consisting of the north displacement, east displacement and bow direction of the low-frequency motion of the center of gravity of the ship in NED coordinates, and v n represents the velocity vector in the NED coordinate corresponding to η, b represents the offset force in the three component directions of η, W b represents the white noise of b, τ represents the known force vector of the hull in the ship coordinate system, which consists of the x-direction force, the y-direction force and the torque, ξ1 represents the displacement of the second-order simple harmonic motion of the center of gravity in the three component directions of η, ξ2 represents the velocity vector corresponding to η, Ω=diag([ω 01 ω 02 ω 03 ]),ω 01 、ω 02 and ω 03 Represents the motion frequency of the three component vectors of ξ1, Z w =diag([ζ1ζ2ζ3]), where ζ1, ζ2 and ζ3 represent the relative damping ratios in the three directions of ξ1, respectively. ξ represents the white noise of ξ, represents the transformation matrix, ψ represents the heading angle; Use the following formula to create the measurement equation: in, represents the position and heading measurement values, η represents the column vector composed of the north displacement, east displacement and heading of the low-frequency motion of the ship's center of gravity in NED coordinates, v b represents the velocity vector of the ship in the ship coordinate system, b represents the offset force in the three component directions of η, ξ1 represents the displacement of the center of gravity in the three component directions of η, and ξ2 represents the velocity vector corresponding to η; The horizontal position and heading data are processed using the established state equation and measurement equation to obtain low-frequency position and heading estimation of the ship.

7. The method according to claim 6, characterized in that The horizontal position and heading data are processed by the Kalman filter with automatic parameter adjustment to obtain the low-frequency position and heading estimation of the ship, and further includes: collecting a time series of the horizontal position and heading data; Perform binomial fitting on the time series to obtain the square mean of the residuals; Calculating the parameter adjustment coefficient according to the square mean of the residuals; The covariance parameters of the Kalman filter are adjusted according to the parameter adjustment coefficients to generate low-frequency position and heading estimation.

8. A ship positioning system based on multi-sensor fusion, characterized in that: The system comprises: A data acquisition and preprocessing module is used to obtain ship positioning data from multiple sensors and process the ship positioning data using sensor fusion technology to obtain a six-degree-of-freedom motion estimate of the ship; a data conversion and feature extraction module for extracting horizontal position and heading data using a coordinate transformation matrix based on the six-degree-of-freedom motion estimation of the ship; The filtering and estimation module is used to process the horizontal position and heading data through a Kalman filtering algorithm with adaptive parameter adjustment to obtain a low-frequency position and heading estimation of the ship.

9. An electronic device comprising a memory and a processor, wherein the memory stores a computer program, wherein: When the processor executes the computer program, the steps of the method according to any one of claims 1 to 7 are implemented.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 7 are implemented.

Images