Ship six-degree-of-freedom motion sensing and predicting method and device and storage medium

Abstract

The invention provides a ship six-degree-of-freedom motion sensing and predicting method and device and a storage medium, and belongs to the technical field of ship motion sensing. Establishing a first measurement equation based on the rotation quaternion and the measurement value; based on the first system state change equation and the first measurement equation, the rotation angular velocity and the linear acceleration of the ship are obtained; establishing a six-degree-of-freedom state-space equation based on the rotation angular velocity and the linear acceleration within the preset duration; based on the rotation angular velocity and the linear acceleration of the ship, the optimal estimation of the system state is determined by adopting an extended Kalman filtering algorithm and a state-space equation, and the six-degree-of-freedom motion response condition of the ship is determined; and according to the six-degree-of-freedom motion response condition of the ship and the six-degree-of-freedom second system state change equation, predicting the six-degree-of-freedom motion change within the specified time. According to the method, the six-degree-of-freedom motion of the ship can be accurately estimated, and short-term six-degree-of-freedom motion of the ship can be predicted.

Description

Technical Field

[0001] The present invention relates to the technical field of ship motion perception, and particularly relates to a method, device and storage medium for six-degree-of-freedom ship motion perception and prediction. Background Art

[0002] With the gradual development of offshore resource development and utilization towards the deep and far sea, harsher sea conditions will pose greater challenges to engineering operations such as offshore installation, loading and unloading, and operation and maintenance. Active wave compensation equipment can mitigate the impact caused by harsh sea conditions on ships. This equipment is the core equipment to ensure the safety, stability and efficiency of maritime personnel passage and equipment transfer. The core technology of this type of equipment is active wave compensation technology, which compensates for the impact brought by ship motion in the reverse direction by precisely controlling the motion of each mechanism of the equipment. The ship dynamic positioning system can control the low-frequency motion of the ship in three degrees of freedom: surge, sway and yaw. However, there are still wave-frequency motions of the ship in six degrees of freedom. The wave-frequency motion data is the input of the active wave compensation algorithm, and the accuracy of ship motion perception will directly affect the compensation effect.

[0003] At present, a ship motion reference unit (MRU) can, based on the measurement data of three accelerometers and three gyroscopes, through the processing of the built-in motion perception algorithm of the product, output the motion data of the ship in three degrees of freedom, including roll angle, pitch angle and heave displacement. However, special offshore engineering operations require high-precision ship six-degree-of-freedom motion data. Ship six-degree-of-freedom motion perception and prediction have always been a difficult problem. Therefore, it is necessary to solve this problem. Summary of the Invention

[0004] In view of this, the present invention provides a method, device and storage medium for six-degree-of-freedom ship motion perception and prediction, which can accurately obtain the motion conditions of the ship in six degrees of freedom and can predict the motion conditions of the ship in six degrees of freedom within a predetermined time.

[0005] To solve the above technical problems, the present invention adopts the following technical solutions:

[0006] The method for six-degree-of-freedom ship motion perception and prediction according to an embodiment of the present invention includes:

[0007] Establish a navigation coordinate system based on the environment where the ship is located, and establish a hull coordinate system based on the inertial measurement unit. Both the navigation coordinate system and the hull coordinate system are spatial rectangular coordinate systems;

[0008] Establish a first system state change equation based on the navigation coordinate system, the hull coordinate system, rotation quaternion, rotation angular velocity, gyroscope deviation and linear acceleration, wherein the rotation quaternion is used to represent the rotational motion of the ship based on the navigation coordinate system;

[0009] Obtain the measurement values of the inertial measurement unit, and establish a first measurement equation based on the rotation quaternion and the measurement values, where the measurement values include gyroscope measurement values, accelerometer measurement values, and virtual heading measurement values;

[0010] Based on the first system state change equation and the first measurement equation, obtain the rotational angular velocity and linear acceleration of the ship, where the rotational angular velocity and the linear acceleration are values in the navigation coordinate system;

[0011] Based on the rotational angular velocity and the linear acceleration within a preset time duration, establish a six-degree-of-freedom state space equation, and the six-degree-of-freedom state space equation includes a second system state change equation for six degrees of freedom;

[0012] Based on the rotational angular velocity and linear acceleration of the ship, use the extended Kalman filter algorithm and the six-degree-of-freedom state space equation to determine the optimal estimate of the system state, and determine the motion response of the ship in six degrees of freedom;

[0013] According to the motion response of the ship in six degrees of freedom and the second system state change equation, predict the motion changes in six degrees of freedom within a specified time.

[0014] According to an embodiment of the present invention, establishing the first measurement equation based on the quaternion and the measurement data further includes:

[0015] Based on the quaternion, the angular velocity of the ship, the linear acceleration of the ship, and the gyroscope bias in the inertial measurement unit, establish a model equation;

[0016] Based on the model equation, the quaternion, and the measurement data, establish the first measurement equation.

[0017] According to an embodiment of the present invention, the first measurement equation includes an accelerometer measurement equation, a gyroscope measurement equation, and a virtual heading measurement equation.

[0018] Establishing the first measurement equation based on the model equation, the quaternion, and the measurement data includes:

[0019] Based on the quaternion, the measured value of the linear acceleration, the measured value of the gravitational acceleration, and the model equation, establish the accelerometer measurement equation;

[0020] Based on the measured value of the angular velocity and the model equation, establish the gyroscope measurement equation;

[0021] Based on the quaternion, establish the virtual heading measurement equation for the yaw angle of the ship.

[0022] According to an embodiment of the present invention, obtaining the rotational angular velocity and linear acceleration of the ship based on the first system state change equation and the first measurement equation includes:

[0023] Using the extended Kalman filter algorithm, the first state change equation, and the first measurement equation to estimate the system state, and solving for the changes in the roll angle, pitch angle, and heading angle of the ship relative to the local inertial system to obtain the rotational angular velocity and linear acceleration of the ship.

[0024] According to an embodiment of the present invention, establishing a six-degree-of-freedom state space equation based on the rotational angular velocity and the linear acceleration within a preset time period includes:

[0025] Collect the time series of the rotational angular velocity and the linear acceleration of the three axes within a preset time period, perform time-frequency transformation on the six time series respectively using the inverse Fourier transform to obtain the transformed frequency spectrum series, and find the frequency combinations corresponding to all local peaks of each frequency spectrum series;

[0026] Using the displacements and velocities of multiple sine signals in the frequency combination as system state variables, and using the rotational angular velocity and linear acceleration of the ship as measurement data, establish the six-degree-of-freedom state space equation.

[0027] According to an embodiment of the present invention, establishing a first system state change equation based on the navigation coordinate system, the hull coordinate system, the rotation quaternion, the rotational angular velocity, the gyroscope bias, and the linear acceleration includes:

[0028] Establish a mathematical model based on the navigation coordinate system, the hull coordinate system, the rotation quaternion, and the rotational angular velocity;

[0029] Establish a first-order dynamic model of the rotational angular velocity based on the navigation coordinate system, the hull coordinate system, and the rotational angular velocity;

[0030] Establish a dynamic model of the gyroscope bias based on the navigation coordinate system, the hull coordinate system, and the gyroscope bias, and

[0031] Establish a first-order dynamic model of the linear acceleration based on the navigation coordinate system, the hull coordinate system, and the linear acceleration.

[0032] According to an embodiment of the present invention, the rotational angular velocity measured by the gyroscope includes noise and a deviation value from the true rotational angular velocity of the ship;

[0033] The acceleration measured by the accelerometer includes gravitational acceleration, linear acceleration, and measurement noise.

[0034] According to an embodiment of the present invention, estimating the system state by using the extended Kalman filter algorithm, the first state change equation, and the first measurement equation, and calculating the changes of the roll angle, pitch angle, and heading angle of the ship relative to the local inertial system to obtain the rotational angular velocity and linear acceleration of the ship, including:

[0035] Using the Euler formula of forward difference to discretize the state space equation of continuous time to obtain the extended Kalman filter model;

[0036] Based on the extended Kalman filter model, selecting the initial value and covariance matrix of the system state variables, the first state change equation error matrix, and the first measurement equation error matrix, and obtaining the rotational angular velocity and linear acceleration of the ship according to the predicted value and actual measurement value of the first system state change equation, and updating the system state.

[0037] In a second aspect, the present invention also discloses an electronic device, including: a memory for storing instructions executed by one or more processors of the electronic device, and a processor for executing the method described in the embodiment of the first aspect.

[0038] In a third aspect, the present invention also discloses a computer-readable storage medium, on which instructions are stored, and when the instructions are executed on an electronic device, the electronic device is enabled to execute the method described in the embodiment of the first aspect.

[0039] At least one of the above technical solutions of the present invention has the following beneficial effects:

[0040] According to the ship six-degree-of-freedom motion perception and prediction method of the embodiment of the present invention, taking the rotation quaternion, rotational angular velocity, gyroscope bias, linear acceleration at the IMU installation location, etc. as the system state, introducing a virtual heading measurement value, not only can estimate the system state, but also can calculate the changes of the roll angle, pitch angle, and heading angle of the ship relative to the local inertial system. More importantly, based on the rotational angular velocity and linear acceleration of the ship in the navigation coordinate system, a system state space equation of the ship six degrees of freedom is established to estimate the motion state of the ship six degrees of freedom. At the same time, based on the system state change equations of six degrees of freedom combined by multiple sine signals, the six-degree-of-freedom motion of the ship in a short time can be predicted. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] Figure 1 It is a scene diagram of a ship operating at sea according to an embodiment of the present invention;

[0042] Figure 2 It shows a flowchart of the ship six-degree-of-freedom motion perception and prediction method according to an embodiment of the present invention;

[0043] Figure 3aFlowchart of establishing the first state - space equation for ship attitude estimation according to an embodiment of the present invention;

[0044] Figure 3b Flowchart of establishing the state - space equation for six degrees of freedom according to an embodiment of the present invention;

[0045] Figure 4 Schematic diagram of the ship heave motion spectrum and all local peaks according to an embodiment of the present invention;

[0046] Figure 5 Schematic diagram of the comparison between the estimated values and the true values of the ship attitude angle and the heading angle according to an embodiment of the present invention;

[0047] Figure 6 Schematic diagram of the ship attitude angle, heading angle estimation errors and prediction errors according to an embodiment of the present invention;

[0048] Figure 7 Schematic diagram of the comparison between the estimated values and the true values of the ship translational displacement according to an embodiment of the present invention;

[0049] Figure 8 Schematic diagram of the ship translational displacement estimation errors and prediction errors according to an embodiment of the present invention;

[0050] Fig. 9 Schematic diagram of the ship six - degree - of - freedom motion estimation errors and prediction errors according to an embodiment of the present invention;

[0051] Fig.10 Block diagram of the SoC (System on Chip) according to an embodiment of the present invention. Detailed implementation manners

[0052] To make the objectives, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are some, but not all, of the embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art based on the described embodiments of the present invention fall within the scope of protection of the present invention.

[0053] To facilitate the understanding of the technical solutions of the present invention, the technical problems of the present invention will be further described below.

[0054] A commonly used Inertial Measurement Unit (IMU) includes three accelerometers and three gyroscopes, which can measure the motion data of six degrees of freedom of a carrier, namely three accelerations and three angular velocities. To solve for the attitude angle from the angular velocity, an integration operation is required. Due to measurement biases and noise in the gyroscopes, the result of the integration operation will drift rapidly and infinitely, and the attitude angle cannot be accurately estimated. Therefore, the measured values of the three accelerometers are needed to correct the drift of the quaternion. Due to the existence of gravitational acceleration, changes in the ship's attitude angle will cause changes in the measured data of the three accelerometers. Therefore, most Marine Reference Units (MRUs) can output relatively accurate roll and pitch angles of the ship. After calculating the roll and pitch angles, the heave acceleration of the ship can be calculated, and then the heave displacement of the ship can be estimated through integration and filtering algorithms.

[0055] However, the characteristics of gravitational acceleration can only ensure the accuracy of the ship's motion in the vertical plane (roll angle, pitch angle, and heave displacement), and cannot estimate the ship's heading, and thus cannot estimate the surge displacement and sway displacement of the ship.

[0056] In some embodiments, one way to solve the above problems is to add the measured data of a magnetometer, fuse the acceleration data of the accelerometers, the angular velocity data of the gyroscopes, and the measured data of the magnetometer, and use the Kalman filter algorithm to estimate the roll angle, pitch angle, and heading of the ship. However, the magnetometer measures the Earth's magnetic field intensity and is susceptible to magnetic interference. Moreover, this patent only gives the solution method for the rotational motion of the ship and does not cover the solution method for the translational displacement of the ship. In addition, to solve the problem of attitude solution drift caused by the bias of the gyroscopes, it is necessary to fuse the measured data of magnetometers and other sensors. The solution process is relatively complex, and the magnetometer is easily affected by magnetic interference. The existing technology can only sense the motion of three degrees of freedom of the ship, namely roll angle, pitch angle, and heave displacement, and cannot output the motion data of six degrees of freedom of the ship. Also, the existing technology mainly focuses on solving the problem of estimating the current motion state of the ship and cannot achieve short-term prediction of the ship's motion. It is also extremely important and can indirectly solve the problem of the response lag of the actuator of the active wave compensation equipment.

[0057] To solve the above technical problems, an embodiment of the present invention proposes a method for ship six-degree-of-freedom motion perception and prediction. Taking rotation quaternion, rotation angular velocity, gyroscope bias, linear acceleration at the installation location of the inertial measurement unit (IMU), etc. as system states, introducing a virtual heading measurement value, establishing a first system state change equation and a first measurement equation, and using the extended Kalman filter algorithm to estimate the system state to determine the ship's rotation angular velocity and linear acceleration. Perform frequency-domain analysis on the time-domain data of six states such as the ship's rotation angular velocity and three-axis accelerations over a period of time. Taking linear acceleration and angular velocity as measurement variables, establish the system state space equation for each degree of freedom, and use the extended Kalman filter algorithm to estimate the ship's six-degree-of-freedom motion and predict the ship's six-degree-of-freedom motion in the short term, solving the situation where the prior art cannot accurately calculate the ship's six-degree-of-freedom motion, and by predicting the ship's six-degree-of-freedom motion, the response lag problem of the actuator of the active wave compensation equipment can be indirectly solved.

[0058] The following will combine with the accompanying drawings to elaborate in detail on the method for ship six-degree-of-freedom motion perception and prediction according to the embodiments of the present invention.

[0059] Reference Figure 1 , Figure 1 shows a scene diagram of a ship operating at sea according to an embodiment of the present invention.

[0060] As Figure 1 shown, this scene diagram shows the state of ship 100 operating at sea. When ship 100 operates at sea, affected by the marine environment, the attitude of ship 100 will constantly change. To measure the ship's attitude, an inertial measurement unit 200 is installed on ship 100. Among them, the inertial measurement unit 200 can be set at the centroid of the ship. The inertial measurement unit 200 is a device for measuring the three-axis angular velocity and acceleration of an object, including an accelerometer and a gyroscope. The accelerometer can measure the ship's gravitational acceleration and linear acceleration, and the gyroscope can measure the ship's angular velocity. Through the inertial measurement unit 200, the rotation angular velocity and acceleration of ship 100 in three-dimensional space can be measured. Then, based on the measurement data of the inertial measurement unit 200, the motion response of the ship's six degrees of freedom is perceived and predicted, so as to use the motion compensation equipment to accurately compensate the ship's motion to ensure the stability and safety of special engineering operations.

[0061] Reference Figure 2 , Figure 2 shows a flowchart of the method for ship six-degree-of-freedom motion perception and prediction according to an embodiment of the present invention. As Figure 2 shown, this method includes S210 - S270.

[0062] S210. Establish a navigation coordinate system based on the environment where the ship is located, and establish a hull coordinate system based on the inertial measurement unit. Both the navigation coordinate system and the hull coordinate system are right-handed Cartesian coordinate systems.

[0063] For example, as Figure 1 shown, in the embodiments of the present invention, two coordinate systems are used, namely the navigation coordinate system n fixed on the earth and the hull coordinate system b fixed on the ship. The coordinate system n is fixed on the earth and is a north-east-earth coordinate system. The positive direction of the x-axis is the due north direction, the y-axis points to the due east direction, and the z-axis points downward. Since the earth's rotation speed is very small, the influence of the earth's rotation can be included in the category of sensor noise, so the coordinate system n is regarded as an inertial system. The second coordinate system is the body coordinate system b fixed on the ship and moving and rotating with the ship. Its coordinate origin is located at the installation position of the inertial measurement unit, where the x-axis points to the bow of the ship, the y-axis points to the starboard, and the z-axis follows the right-hand coordinate system rule. The inertial system, that is, the inertial field in classical physics and special relativity, refers to the reference system in which Newton's laws of motion hold. That is to say, the navigation coordinate system n can uniformly and isotropically describe space and can uniformly describe time. The hull coordinate system b is established based on the inertial measurement unit. During the movement of the ship, the inertial measurement unit changes with the movement of the ship, that is, the hull coordinate system b also changes with the movement of the ship.

[0064] S220. Establish a first system state change equation based on the navigation coordinate system, the hull coordinate system, the rotation quaternion, the rotation angular velocity, the gyroscope bias, and the linear acceleration, where the rotation quaternion is used to represent the rotational motion of the ship based on the navigation coordinate system.

[0065] In the embodiments of the present invention, in order to avoid problems such as singularity and normalization when using Euler angles to describe the rotational motion of a rigid body, quaternions are used to describe the rotational motion of the ship. At the same time, considering the linear acceleration when the ship is in motion, a mathematical model of the quaternion and the ship's rotation angular velocity, a first-order dynamic model of the ship's rotation angular velocity, a dynamic model of the gyroscope bias, and a first-order dynamic model of the linear acceleration at the installation location of the IMU are established.

[0066] S230. Obtain the measurement values of the inertial measurement unit, and establish a first measurement equation based on the rotation quaternion and the measurement values.

[0067] Among them, the measurement values include gyroscope measurement values, accelerometer measurement values, and virtual heading measurement values.

[0068] In an embodiment of the present invention, the measurement data of the sensor IMU includes the measurement values of three accelerometers and the measurement values of three gyroscopes. At the same time, in order to prevent the drift of the yaw angle, a virtual measurement value is added to the heading, forming seven measurement data, and a first measurement equation for the gyroscope, accelerometer, and yaw angle is established. This method can not only estimate the system state of the ship, but also calculate the changes of the roll angle, pitch angle, and heading angle of the ship relative to the local inertial system.

[0069] S240. Based on the first system state change equation and the first measurement equation, the rotational angular velocity and linear acceleration of the ship are obtained. Among them, the rotational angular velocity and linear acceleration are values in the navigation coordinate system.

[0070] In some embodiments of the present invention, after determining the measurement data of the IMU, the extended Kalman filter algorithm is used to estimate the system state, calculate the changes of the roll angle, pitch angle, and heading angle of the ship relative to the local inertial system, and calculate the values of the rotational angular velocity and linear acceleration of the ship in the navigation coordinate system.

[0071] S250. Based on the rotational angular velocity and linear acceleration within a preset time period, a state space equation with six degrees of freedom is established. Among them, the state space equation with six degrees of freedom includes a second system state change equation and a second measurement equation with six degrees of freedom.

[0072] S260. Based on the rotational angular velocity and linear acceleration of the ship, the extended Kalman filter algorithm and the state space equation with six degrees of freedom are used to determine the optimal estimate of the system state and determine the motion response of the ship with six degrees of freedom.

[0073] S270. According to the motion response of the ship with six degrees of freedom and the second system state change equation, the motion changes with six degrees of freedom within a specified time are predicted.

[0074] According to the method of the embodiment of the present invention, the accuracy and effectiveness in estimating the roll angle, pitch angle, and yaw angle of the ship can be improved, and at the same time, the six-degree-of-freedom motion in the short term can be accurately predicted.

[0075] It can be understood that the method of the embodiments of the present invention can be applied to electronic devices, including but not limited to mobile phones, large-screen devices, smart TVs, wearable devices, tablets (Pads), desktop computers, computers, wearable devices (such as smart watches, smart glasses, helmets), virtual reality (VR) devices, augmented reality (AR) devices, wireless devices in industrial control, wireless devices in self-driving, wireless devices in smart grids, wireless devices in transportation safety, wireless devices in smart cities, wireless devices in smart homes, servers, and so on.

[0076] The following will combine the accompanying drawings and specific embodiments to elaborate in detail on the calculation methods in each step of the above embodiments of the present invention.

[0077] Refer to Figure 3a , Figure 3a which shows a flowchart of establishing the first state space equation for ship attitude estimation according to an embodiment of the present invention. As Figure 3a shown, first, a first state change equation is established based on quaternions, the rotational angular velocity of the ship, gyroscope bias, and the linear acceleration of the ship. Then, a second measurement equation (i.e., the measurement equation) regarding the gyroscope, accelerometer, and virtual heading angle (i.e., yaw angle) is established. Based on the first state change equation and the first measurement equation, the first state space equation for ship attitude estimation is established, and the extended Kalman filter model is used to solve the first state space equation to obtain the current rotational angular velocity and linear acceleration of the ship. This will be further described in the following steps.

[0078] In S220, a first system state change equation is established based on the navigation coordinate system, the hull coordinate system, the rotation quaternion, the rotational angular velocity, the gyroscope bias, and the linear acceleration.

[0079] In this step, the rotational motion of the ship is described using quaternions, and at the same time, considering the linear acceleration of the ship when it is in motion, a mathematical model of the quaternion and the rotational angular velocity of the ship, a first-order dynamic model of the rotational angular velocity of the ship, a dynamic model of the gyroscope bias, and a first-order dynamic model of the linear acceleration at the IMU installation location are established.

[0080] The measurement data of the sensor include the measurement values of three accelerometers, the measurement values of three gyroscopes, and a virtual yaw angle value, for a total of 7 measurement data. The state variables of the system include quaternion, the true rotational angular velocity of the ship, the measurement bias of the gyroscope, and the linear acceleration, for a total of 13 state variables. The relationships between these parameters are described below.

[0081] The mathematical relationship between the quaternion and the rotational motion of the ship is shown in the following equation:

[0082]

[0083] where \(q = [q 0 ,q 1 ,q 2 ,q 3 T is the quaternion vector corresponding to the rotation from coordinate system n to coordinate system b, and is the value of the rotational velocity of coordinate system b relative to coordinate system n in coordinate system b.

[0084] The true rotational rate of the ship can be assumed to be a first-order process, and the corresponding differential equation is as follows:

[0085]

[0086] where \(c ωx , c ωy , and c ωz are constants greater than 0, and \(\zeta ωx , \(\zeta ωy , and \(\zeta ωz are the noises of the model.

[0087] The measurement bias of the gyroscope can be assumed to be a process with a very slow change rate, so its corresponding random walk process is as follows:

[0088]

[0089] where \(\zeta ρx , \(\zeta ρy , and \(\zeta ρz are the noises of the model.

[0090] The true linear acceleration of the IMU can be assumed to be a first-order process, and the corresponding differential equation is as follows:

[0091]

[0092] where \(c ax , c ay , and c az are constants greater than 0, and \(\zeta ax , \(\zeta ay , and \(\zeta az ​is the noise of the model.

[0093] In S230, the measurement values of the inertial measurement unit are obtained, and the first measurement equation is established based on the rotation quaternion and the measurement values.

[0094] Specifically, the first measurement equations for the gyroscope, accelerometer, and heading are established. In the embodiments of the present invention, the measurement data of the sensor IMU includes the measurement values of three accelerometers and the measurement values of three gyroscopes. At the same time, in order to prevent the drift of the yaw angle, a virtual measurement value is added to the heading, that is Seven measurement data are formed, and the gyroscope measurement equation, accelerometer measurement equation, and yaw angle measurement equation are established.

[0095] Because the angular velocity output by the gyroscope generally includes noise and bias, which is not equal to the true rotation rate of the ship, the corresponding mathematical relationship is as follows:

[0096]

[0097] Among them, and are the measurement data of the three gyroscopes, and are the measurement noises of the gyroscope.

[0098] Quaternion coordinate transformation matrix The coordinate transformation relationship between the navigation coordinate system n and the hull coordinate system b is expressed as:

[0099]

[0100] The angular velocity output by the three accelerometers includes the influence of gravitational acceleration and measurement noise, and the corresponding output model is as follows:

[0101]

[0102] Therefore, the relationship between the measurement values of the three accelerometers and the quaternion vector is shown as follows:

[0103]

[0104] Among them, and are the measurement data of the three accelerometers, g is the gravitational acceleration, and are the measurement noises of the accelerometer.

[0105] Relationship between virtual heading measurement and quaternion As follows:

[0106]

[0107] In an embodiment of the present invention, it is possible to The continuous-time state-space equation is established as follows:

[0108] The system state variable is the vector x = [q 0 , q 1 , q 2 , q 3 , ω x , ω y , ω z , ρ x , ρ y , ρ z , a x , a y , a z T , and the measurement data is the vector The state equation in continuous time is:

[0109]

[0110] Furthermore, a specific observer can be designed as follows:

[0111]

[0112] In S240, based on the first system state change equation and the first measurement equation, the rotational angular velocity and linear acceleration of the ship are obtained.

[0113] Determine the measurement data of the IMU, use the extended Kalman filter algorithm to estimate the system state, solve the changes of the ship's roll angle, pitch angle, and heading angle relative to the local inertial system, and calculate the values of the ship's rotational angular velocity and linear acceleration in the navigation coordinate system.

[0114] In practical applications, the state space equation in continuous time (described in S230) is discretized using the forward difference Euler formula. After discretization, a standard extended Kalman filter model can be obtained as follows:

[0115]

[0116] where I is the identity matrix, M 4×4 , N 4×3 and C ω , C a are specifically defined as follows:

[0117]

[0118] ω 1 = ω x , ω 2 = ω y , ω 3 = ω z ​

[0119] Among them, the H k matrix is defined as:

[0120]

[0121] In the formula,

[0122]

[0123] a 1 = a x , a 2 = a y , a 4 = a z - g

[0124]

[0125] d = 2(q 1 q 2 + q 0 q 3 )

[0126] Select the initial values of the system state variables and the covariance matrix, the first state change equation error matrix and the first measurement equation error matrix, and update the optimal estimate of the system state according to the predicted value and the actual measurement value of the state change equation.

[0127] To describe the accuracy of ship attitude observation, it is necessary to convert the quaternion into Euler angles. After attitude conversion, the attitude angle information of the ship is obtained. The Euler angle expression from coordinate system to coordinate system b is as follows:

[0128] The Euler angle expression from coordinate system n to coordinate system b is as follows:

[0129]

[0130] Among them, is the roll angle, is the pitch angle, is the heading angle.

[0131] The expression of the angular velocity of rotation from coordinate system n to coordinate system b in coordinate system n is as follows:

[0132]

[0133] Among them, and are the values of the ship's angular velocity of rotation in coordinate system n.

[0134] In S250, a state space equation with six degrees of freedom is established based on the rotational angular velocity and linear acceleration within a preset time duration. Among them, the state space equation with six degrees of freedom can be referred to as the second state space equation.

[0135] Reference Figure 3b , Figure 3b FIG. shows a flowchart of establishing a state space equation with six degrees of freedom according to an embodiment of the present invention. As Figure 3b shown, first, the rotational angular velocity or linear acceleration of a certain degree of freedom within a preset time duration. Then, perform time-frequency transformation on the time series within the preset time duration to determine all the main frequencies of the frequency spectrum sequence. Using the sine signal corresponding to each main frequency and its first derivative as state variables, establish a second system state change equation. Using the sine signal corresponding to each main frequency and its first derivative as state variables, and the angular velocity or linear acceleration as measurement data, establish a second measurement equation. Determine the second state space equation based on the second system state change equation and the second measurement equation, which will be further described below.

[0136] In an embodiment of the present invention, time series of six motion states such as rotational angular velocity and triaxial acceleration within a period of time are collected. Perform inverse Fourier transform on the six time series respectively to find the frequency values corresponding to all local peaks of the motion response energy spectrum of each degree of freedom.

[0137] Among them, collecting the time series of rotational angular velocity and triaxial acceleration within a period of time is expressed as where j represents the jth degree of freedom among the six degrees of freedom of the ship, k represents discrete time, and n T is the number of data points included in the time series.

[0138] Perform time-frequency transformation on the six time series respectively using inverse Fourier transform. The transformed frequency spectrum sequence is Find the frequency combinations corresponding to all local peaks of each frequency spectrum sequence, expressed as where i represents the ith main frequency, and n j represents the number of frequencies included in the jth frequency spectrum sequence.

[0139] In an embodiment of the present invention, taking the displacements and velocities of multiple sine signals as system state variables, and the rotational angular velocity and linear acceleration of the ship as measurement data, establish a state space equation for each degree of freedom (state space equation with six degrees of freedom). Among them, the state space equation with six degrees of freedom includes a second system state change equation and a second measurement equation. The specific establishment process is as follows:

[0140] According to the random wave theory, the six-degree-of-freedom motion of the ship can be regarded as a time series composed of sine signals of multiple main frequencies, that is:

[0141]

[0142] where d j (t) is the displacement (angle) of the six - degree - of - freedom motion of the ship, is the (angular) acceleration of the six - degree - of - freedom motion of the ship. For a sinusoidal signal with a given frequency, the (angular) acceleration is inversely proportional to the displacement (angle).

[0143] Taking one of the degrees of freedom as an example, the second - order system state - change equation is as follows:

[0144]

[0145] In the formula, i = 1, 2, …, n j

[0146] For the rotation angle, the rotational angular velocity in the coordinate system n is the measured value, and its second - order measurement equation is as follows:

[0147] v j = H z z j , where H z is a 1×2n j vector, expressed as:

[0148]

[0149] For the translational displacement, the linear acceleration in the coordinate system n is the measured value, and its measurement equation is as follows:

[0150] v j = H z z j , where H z is a 1×2n j vector, expressed as:

[0151]

[0152] In S260, based on the rotational angular velocity and linear acceleration of the ship, the extended Kalman filter algorithm and the state - space equation are used to determine the optimal estimate of the system state, and to determine the motion response of the six degrees of freedom of the ship. Among them, the motion response of the six degrees of freedom includes the values corresponding to the surge, sway, heave, roll, pitch, and yaw of the ship.

[0153] In the embodiment of the present invention, according to the sampling period T, the continuous - time second - order state - change equation and second - order measurement equation are discretized to obtain the discrete state - space equation as follows:

[0154]

[0155] Select the initial values of appropriate system state variables and covariance matrices, the error matrix of the state transition equation, and the error matrix of the measurement equation. Update the optimal estimate of the system state based on the predicted value of the state transition equation and the actual measurement value.

[0156] The estimation of the six-degree-of-freedom motion state of the ship is as follows:

[0157]

[0158] In S270, according to the motion response of the six degrees of freedom of the ship and the system state space equation, predict the motion changes of the six degrees of freedom within a specified time.

[0159] According to Discrete system state space equations, based on the best estimate of the current state of the system, prediction The six-degree-of-freedom motion state of the ship within a certain number of time steps in step S250, the expression is as follows:

[0160]

[0161] Through the above calculations, the six-degree-of-freedom motion state and the predicted state of the ship are obtained.

[0162] Taking an engineering ship as an example, the six-degree-of-freedom motion perception and prediction method of the embodiment of the present invention will be further described below.

[0163] In the embodiment of the present invention, taking the Response Amplitude Operator (RAO) of an engineering ship as the input, selecting the JONSWAP wave spectrum, and using the method of superposition of a finite number of regular waves, a simulation analysis is carried out with the ship under the condition of a significant wave height of 3.5 meters as an example to obtain the six-degree-of-freedom motion response of the ship.

[0164] Using the method described in S210 - S270 in the above embodiment, calculate the six-degree-of-freedom motion state of the ship. First, compare and analyze the estimated values and true values of the ship's attitude angle and heading angle.

[0165] Generate the heave displacement information of the ship through simulation case analysis, and perform the inverse Fourier transform on its six time series, as Figure 4 shown, Figure 4 shows the heave motion spectrum of the ship and a schematic diagram of all local peaks. The peak detection algorithm can output the amplitude and the characteristic frequency f i j of each sine wave. According to the characteristic frequency of each waveform and the phase spectrum obtained from the inverse Fourier transform, the corresponding phase is obtained. These parameters can initialize the extended Kalman filter, and through multiple iterations, the error is gradually reduced, so as to obtain accurate heave motion information of the ship.

[0166] Refer to Figure 5 , Figure 5 shows a comparison graph of the roll angle, pitch angle, and heading angle of a ship estimated by two observers according to embodiments of the present invention. Among them, Method 1 represents the estimated value of the motion state corresponding to S240, and Method 2 represents the estimated value of the motion state corresponding to S260 (the same hereinafter). To eliminate the influence of observer initialization, only the estimated values of ship motion in the steady state are shown. As Figure 5 shown, it can be seen from the figure that the estimated values of both methods are almost coincident with the true values, and the error is limited within the range allowed by the engineering.

[0167] For a more intuitive comparative analysis, refer to Figure 6 , Figure 6 which shows an analysis curve graph of the estimated error and prediction error of the ship attitude angle and heading angle according to embodiments of the present invention. It can be seen from Figure 6 that the estimated error and prediction error of the ship attitude angle and heading angle are both small. The observer also has a very significant effect on estimating the yaw angle of the ship, can better ensure the accuracy, and is applied to the actual attitude estimation of the ship. Moreover, in the present invention, the analysis result of the prediction error of the ship motion state shows that this observer can accurately predict the attitude angle and yaw angle in the short term.

[0168] Next, the estimated value and the true value of the translational displacement of the ship are analyzed and compared in combination with data.

[0169] Refer to Figure 7 and Figure 8 , Figure 7 which shows the comparison result of the translational displacement value of the ship estimated by the observer according to an embodiment of the present invention and the true value. Figure 8 shows the comparison result of the translational displacement value of the ship estimated by the observer according to another embodiment of the present invention. It can be seen from Figure 7 that the estimated values of the surge, sway, and heave motions of the ship are almost coincident with the true values. It can be seen from Figure 8 that the translational displacement error of the ship remains within an acceptable range, and the ship six-degree-of-freedom motion perception algorithm proposed by the present invention has a high accuracy for estimating the translational displacement of the ship.

[0170] Next, the analysis of the estimation error and prediction error of the ship six-degree-of-freedom motion is carried out in combination with data.

[0171] Refer to Fig. 9 , Fig. 9The maximum amplitudes of the six-degree-of-freedom motion of the ship in the embodiments of the present invention, as well as the estimation errors and prediction error values of the two methods, are shown. In the simulation case analysis, the estimation errors of the three attitude angles of Method 1 are all less than 4%, which are limited within the allowable error range. Method 2 estimates the six-degree-of-freedom motion of the ship. The estimation errors of the three attitude angles are all less than 2%, the estimation errors of the three translational displacements are all less than 4%, and the estimation error of the heave displacement is less than 0.07 m, that is, less than 3% of the maximum heave amplitude, and high-precision heave motion information can be obtained. From the perspective of the prediction error values, the prediction errors of the three attitude angles are all less than 2%, and the prediction errors of the three translational displacements are all less than 4%, indicating the effectiveness and accuracy of this method in predicting the short-term six-degree-of-freedom motion of the ship.

[0172] In summary, based on the mathematical relationship between quaternions and the rotational motion of the ship, as well as the characteristics of gravitational acceleration, the present invention also considers the linear acceleration when the ship is in motion, and adds virtual yaw measurement data. The designed ship attitude observer can effectively estimate the changes of the ship attitude angles and the yaw angle relative to the local inertial system. By using the inverse Fourier transform method to perform frequency-domain analysis on the time-domain data of six states such as the ship's rotational angular velocity and three-axis acceleration over a period of time, find the frequencies corresponding to all local peaks of the energy spectrum, regard the time-domain motion response of the ship as composed of sine signals with multiple frequencies, use the displacement and velocity of each sine signal as system state variables, and use linear acceleration and angular velocity as measurement variables to establish the first system state space equation for each degree of freedom, and use the extended Kalman filter algorithm to estimate the six-degree-of-freedom motion of the ship. At the same time, based on the second system state change equation combined with multiple sine signals, predict the short-term six-degree-of-freedom motion of the ship. The simulation case analysis verifies the accuracy and effectiveness of this method in estimating the roll angle, pitch angle, and yaw angle of the ship, and can accurately predict the short-term six-degree-of-freedom motion at the same time. The estimation accuracy of the ship's heave displacement reaches 3% of the maximum heave amplitude, and the prediction error of the ship's translational displacement is less than 4%, with relatively small errors, which has certain engineering reference value for meeting the ship's offshore engineering operations.

[0173] The present invention also provides an electronic device, including:

[0174] a memory for storing instructions executed by one or more processors of the device, and

[0175] a processor for executing the method explained in the above embodiments in combination with Figures 1 to 9 what is explained.

[0176] The present invention also provides a computer-readable storage medium. The computer-readable storage medium stores a computer program. When the computer program is run by a processor, the processor is caused to execute the method explained in the above embodiments Figures 1 to 9 what is explained.

[0177] The present invention also provides a computer program product containing instructions, which, when the computer program product runs on an electronic device, enables a processor to execute the Figures 1 to 9 method explained in the above embodiments.

[0178] Now refer to Fig.10 , which shows a block diagram of a System on Chip (SoC) 1000 according to an embodiment of the present invention. In Fig.10 , similar components have the same reference numerals. Additionally, the dashed boxes are optional features of a more advanced SoC. In Fig.10 , the SoC 1000 includes: an interconnect unit 1050, which is coupled to an application processor 1010; a system agent unit 1080; a bus controller unit 1090; an integrated memory controller unit 1040; one or a group of one or more coprocessors 1020, which may include integrated graphics logic, an image processor, an audio processor, and a video processor; a Static Random Access Memory (SRAM) unit 1030; and a Direct Memory Access (DMA) unit 1060. In one embodiment, the coprocessor 1020 includes a dedicated processor, such as, for example, a network or communication processor, a compression engine, a General Purpose Computing on GPU (GPGPU), a Many Integrated Core (MIC) processor with high throughput, or an embedded processor, etc.

[0179] The Static Random Access Memory (SRAM) unit 1030 may include one or more computer-readable media for storing data and / or instructions. Instructions may be stored in the computer-readable storage medium, specifically, temporary and permanent copies of the instructions are stored. The instructions may include: when executed by at least one unit in the processor, enabling the Soc 1000 to execute the processing method according to the above embodiments, and specifically, the interaction method explained in the above embodiments Figure 1 and Fig. 9 , which will not be elaborated here.

[0180] Embodiments of the mechanisms disclosed in the present invention may be implemented in hardware, software, firmware, or a combination of these implementation methods. Embodiments of the present invention may be implemented as a computer program or program code executed on a programmable system, which includes at least one processor, a storage system (including volatile and non-volatile memories and / or storage elements), at least one input device, and at least one output device.

[0181] Program code can be applied to the input instructions to perform the various functions described in the present invention and generate output information. The output information can be applied to one or more output devices in a known manner. For the purposes of the present invention, a processing system includes any system having a processor such as, for example, a digital signal processor (DSP), a microcontroller, an application specific integrated circuit (ASIC), or a microprocessor.

[0182] The program code can be implemented in a high-level procedural language or an object-oriented programming language in order to communicate with the processing system. When needed, the program code can also be implemented in an assembly language or a machine language. In fact, the mechanisms described in the present invention are not limited to the scope of any particular programming language. In any case, the language can be a compiled language or an interpreted language.

[0183] In some cases, the disclosed embodiments can be implemented in hardware, firmware, software, or any combination thereof. The disclosed embodiments can also be implemented as instructions carried or stored on one or more transitory or non-transitory machine-readable (e.g., computer-readable) storage media, which can be read and executed by one or more processors. For example, the instructions can be distributed via a network or via other computer-readable media. Thus, a machine-readable medium can include any mechanism for storing or transmitting information in a machine (e.g., computer) readable form, including but not limited to, floppy disks, optical disks, optical discs, compact discs read only memory (CD-ROMs), magneto-optical discs, read only memory (ROM), random access memory (RAM), erasable programmable read only memory (EPROM), electrically erasable programmable read only memory (EEPROM), magnetic or optical cards, flash memory, or tangible machine-readable memories for transmitting information (e.g., carrier waves, infrared signals, digital signals, etc.) in electrical, optical, acoustic, or other forms using the Internet. Thus, a machine-readable medium includes any type of machine-readable medium suitable for storing or transmitting electronic instructions or information in a machine (e.g., computer) readable form.

[0184] In the drawings, some structural or method features may be shown in a particular arrangement and / or order. However, it should be understood that such a particular arrangement and / or ordering may not be required. Rather, in some embodiments, these features may be arranged in a different manner and / or order than shown in the illustrative drawings. Additionally, the inclusion of a structural or method feature in a particular figure does not imply that such a feature is required in all embodiments, and in some embodiments, these features may not be included or may be combined with other features.

[0185] It should be noted that each unit / module mentioned in the device embodiments of the present invention is a logical unit / module. Physically, a logical unit / module can be a physical unit / module, a part of a physical unit / module, or implemented as a combination of multiple physical units / module. The physical implementation manner of these logical units / modules themselves is not the most important. The combination of the functions implemented by these logical units / modules is the key to solving the technical problems proposed by the present invention. In addition, in order to highlight the innovative part of the present invention, the above device embodiments of the present invention do not introduce units / modules that are not closely related to solving the technical problems proposed by the present invention. This does not mean that there are no other units / modules in the above device embodiments.

[0186] It should be noted that in the examples and descriptions of this patent, relational terms such as "first" and "second" are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any actual relationship or order between these entities or operations. Moreover, the terms "comprise", "include" or any other variant thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements not only includes those elements but also other elements not expressly listed, or elements inherent to such process, method, article or device. Without further limitation, an element defined by the statement "comprising an" does not exclude the presence of additional identical elements in the process, method, article or device including the element.

[0187] Although the present invention has been illustrated and described by referring to some preferred embodiments of the present invention, those of ordinary skill in the art should understand that various changes can be made to it in form and detail without departing from the spirit and scope of the present invention.

Claims

1. A method for sensing and predicting six-degree-of-freedom motion of a ship, characterized in that: include: A navigation coordinate system is established based on the environment in which the ship is located, and a hull coordinate system is established based on an inertial measurement unit, wherein both the navigation coordinate system and the hull coordinate system are spatial rectangular coordinate systems; Establishing a first system state change equation based on the navigation coordinate system, the hull coordinate system, a rotation quaternion, a rotation angular velocity, a gyroscope deviation and a linear acceleration, wherein the rotation quaternion is used to represent the rotational motion of the ship based on the navigation coordinate system; Acquire measurement values ​​of the inertial measurement unit, and establish a first measurement equation based on the rotation quaternion and the measurement values, wherein the measurement values ​​include gyroscope measurement values, accelerometer measurement values, and virtual heading measurement values; Based on the first system state change equation and the first measurement equation, the rotational angular velocity and the linear acceleration of the ship are obtained, wherein the rotational angular velocity and the linear acceleration are values ​​in the navigation coordinate system; Based on the rotational angular velocity and the linear acceleration within a preset time length, a state space equation of six degrees of freedom is established, wherein the state space equation of six degrees of freedom includes a second system state change equation and a second measurement equation of six degrees of freedom; Based on the ship's rotational angular velocity and linear acceleration, the extended Kalman filter algorithm and the state-space equations of the six degrees of freedom are used to determine the optimal estimate of the system state, and the motion response of the ship's six degrees of freedom are determined; based on the motion response of the ship's six degrees of freedom and the second system state change equation, the motion changes of the six degrees of freedom within a specified time are predicted.

2. The method for sensing and predicting six-degree-of-freedom motion of a ship according to claim 1, characterized in that: The establishing of the first measurement equation based on the quaternion and the measurement data further includes: Establishing a model equation based on the quaternion, the angular velocity of the ship, the linear acceleration of the ship, and the deviation of the inertial measurement unit; The first measurement equation is established based on the model equation, the quaternion and the measurement data.

3. The method for sensing and predicting six-degree-of-freedom motion of a ship according to claim 2, characterized in that: The first measurement equation includes an accelerometer measurement equation, a gyroscope measurement equation and a virtual bow vector measurement equation. The establishing of a first measurement equation based on the model equation, the quaternion and the measurement data comprises: Establishing an accelerometer measurement equation based on the quaternion, the measured value of the linear acceleration, the measured value of the gravitational acceleration and the model equation; Establishing a gyroscope measurement equation based on the measured value of the angular velocity and the model equation; A virtual bow vector measurement equation of the bow pitch angle of the ship is established based on the quaternion.

4. The method for sensing and predicting the six-degree-of-freedom motion of a ship according to claim 1, characterized in that: The obtaining of the rotational angular velocity and linear acceleration of the ship based on the first system state change equation and the first measurement equation includes: The extended Kalman filter algorithm, the first system state change equation and the first measurement equation are used to estimate the system state, solve the changes of the ship's roll angle, pitch angle and heading angle relative to the local inertial system, and obtain the ship's rotational angular velocity and linear acceleration.

5. The method for sensing and predicting the six-degree-of-freedom motion of a ship according to claim 1, characterized in that: The state space equation of six degrees of freedom is established based on the rotation angular velocity and the linear acceleration within a preset time period, including: Collect the time series of the rotation angular velocity and the linear acceleration of the three axes within a preset time length, use inverse Fourier transform to perform time-frequency transform on the six time series respectively, obtain the transformed spectrum sequence, and find the frequency combination corresponding to all local peaks of each spectrum sequence; The state space equation of six degrees of freedom is established by taking the displacement and velocity of multiple sinusoidal signals in the frequency combination as system state variables and taking the rotation angular velocity and linear acceleration of the ship as measurement data.

6. The method for sensing and predicting the six-degree-of-freedom motion of a ship according to claim 1, characterized in that: The first system state change equation is established based on the navigation coordinate system, the hull coordinate system, the rotation quaternion, the rotation angular velocity, the gyroscope deviation and the linear acceleration, including: Establishing a mathematical model based on the navigation coordinate system, the hull coordinate system, the rotation quaternion and the rotation angular velocity; Establishing a first-order dynamic model of the rotational angular velocity based on the navigation coordinate system, the hull coordinate system and the rotational angular velocity; establishing a dynamic model of the gyroscope deviation based on the navigation coordinate system, the hull coordinate system and the gyroscope deviation, and A first-order dynamic model of the linear acceleration is established based on the navigation coordinate system, the hull coordinate system and the linear acceleration.

7. The method for sensing and predicting six-degree-of-freedom motion of a ship according to claim 1, characterized in that: The rotational angular velocity measured by the gyroscope includes noise and a deviation value from the true rotational angular velocity of the ship; The acceleration measured by the accelerometer includes gravity acceleration, linear acceleration and measurement noise.

8. The method for sensing and predicting the six-degree-of-freedom motion of a ship according to claim 1, characterized in that: The method of estimating the system state by using the extended Kalman filter algorithm, the first system state change equation and the first measurement equation, solving the changes of the ship's roll angle, pitch angle and heading angle relative to the local inertial system, and obtaining the rotational angular velocity and linear acceleration of the ship includes: The continuous-time state space equation is discretized using the Euler formula of forward difference to obtain the extended Kalman filter model; Based on the extended Kalman filter model, the initial values ​​and covariance matrix of the system state variables, the error matrix of the first state change equation and the error matrix of the first measurement equation are selected, and the rotational angular velocity and linear acceleration of the ship are obtained according to the predicted value and actual measured value of the first state change equation, and the system state is updated.

9. An electronic device, characterized in that: include: Memory for storing computer programs; A processor, configured to execute the computer program in the memory to implement the method according to any one of claims 1 to 8.

10. A computer storage medium, characterized in that: The computer storage medium stores a computer program, and when the computer program is executed by a processor, the processor executes the method according to any one of claims 1 to 8.

Images