Ship attitude estimation method and device, equipment, storage medium and program product

Abstract

The invention provides a ship attitude estimation method, device and equipment, a storage medium and a program product, which are applied to a ship equipped with an inertial measurement unit, and the method comprises the following steps: establishing a navigation coordinate system based on an environment where the ship is located, and establishing a ship body coordinate system based on the inertial measurement unit; establishing a model equation based on the state variables; establishing a measurement equation based on the state variable and the model equation; establishing a state-space equation based on the model equation and the measurement equation; determining measurement data of the inertial measurement unit, and determining an optimal value of a state variable based on the state-space equation and the measurement data; and determining the attitude angle of the ship based on the optimal value of the quaternion in the state variable so as to determine the attitude of the ship. According to the method, the model equation is established through the state variables, the measurement value is preset for the heading of the ship, and the measurement equation and the state space equation of the ship are established, so that the attitude angle of the ship is determined, the attitude of the ship in the movement process can be accurately measured, and the precision of ship attitude estimation is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of ship attitude, and in particular to a method, device, equipment, storage medium and program product for estimating ship attitude. Background Art

[0002] With the growth of offshore wind power installed capacity and the need for deep-sea oil and gas development, the demand for offshore installation, loading and unloading, and operation and maintenance is growing rapidly. Since ships will move in the horizontal and vertical planes under the action of wind, waves, and currents, offshore engineering operations will face huge challenges. In order to compensate for the movement of ships in the vertical plane, the equipment currently developed includes active compensation devices and passive compensation devices. For active compensation devices, all compensation commands need to be input with the movement of the ship in the vertical plane (roll, pitch, and heave), so the estimation of the ship's attitude (roll angle, pitch angle) is very important.

[0003] After long-term research, it was found that the ship's motion reference unit (MRU) can measure the ship's attitude and heave displacement. Its measurement accuracy depends on the measurement accuracy of the built-in accelerometer and gyroscope and the accuracy of the ship's attitude solution algorithm. The inertial measurement unit (IMU) and MRU are often combined to measure the ship's motion state. The IMU contains three accelerometers and three gyroscopes, which can measure the motion data of the carrier's six degrees of freedom, namely three accelerations and three angular velocities. To solve the attitude angle from the angular velocity, an integral operation is required. Due to the measurement deviation and noise of the gyroscope, the integral operation result will drift infinitely and cannot accurately estimate the attitude angle, that is, the accuracy of the attitude angle solution is not high, resulting in a large deviation in the ship's attitude estimation. Summary of the invention

[0004] In view of this, the present invention provides a method, device, equipment, storage medium and program product for estimating a ship attitude, which can solve the problem that when using IMU to estimate the ship attitude, the measurement deviation and noise of the gyroscope lead to low calculation accuracy of the ship attitude angle and inaccurate ship attitude estimation.

[0005] In order to solve the above technical problems, the present invention adopts the following technical solutions:

[0006] The present invention provides a method for estimating a ship attitude based on an inertial measurement unit, which is applied to a ship equipped with an inertial measurement unit, comprising:

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

[0008] A model equation is established based on state variables, including quaternions, the angular velocity of the ship, the linear acceleration of the ship, and the deviation of the inertial measurement unit. The quaternions are used to represent the rotational motion of the ship based on the navigation coordinate system.

[0009] Establishing measurement equations based on state variables and model equations;

[0010] Establish state space equations based on model equations and measurement equations;

[0011] determining measurement data of an inertial measurement unit, and determining optimal values ​​of state variables based on state space equations and the measurement data;

[0012] The attitude angle of the ship is determined based on the optimal value of the quaternion in the state variable to determine the attitude of the ship.

[0013] In one embodiment of the present invention, the measurement equation includes a first measurement equation, a second measurement equation and a third measurement equation.

[0014] The measurement equations are established based on the state variables and model equations, including:

[0015] Establishing a first measurement equation based on quaternion, linear acceleration, gravity acceleration of the ship and model equation;

[0016] Establishing a second measurement equation based on the angular velocity and the model equation;

[0017] The third measurement equation of the ship's pitch angle is established based on quaternion.

[0018] In one embodiment of the present invention, a first measurement equation is established based on quaternions, linear acceleration, gravity acceleration of the ship and a model equation, including:

[0019] Establish the conversion equation between the navigation coordinate system and the hull coordinate system based on quaternion;

[0020] A first measurement equation is established based on the conversion equation, the model equation, the linear acceleration and the gravitational acceleration.

[0021] In one embodiment of the present invention, the state space equation includes a state change equation and a state measurement equation.

[0022] The state space equations are established based on the model equations and the measurement equations, including:

[0023] Establish state change equation based on model equation;

[0024] Establish state measurement equation based on measurement equation;

[0025] An observer is set based on the state change equation and the state measurement equation, and the observer is used to observe the state variables.

[0026] In one embodiment of the present invention, determining the optimal value of the state variable based on the state space equation and the measurement data includes:

[0027] Discretize the state space equations based on the forward difference method;

[0028] Based on the discretized state space equations and measurement data, the optimal values ​​of the state variables are calculated through the extended Kalman filter model.

[0029] In one embodiment of the present invention, determining the attitude angle of the ship based on the optimal value of the quaternion in the state variable includes:

[0030] Convert the optimal value of the quaternion into Euler angles;

[0031] The attitude angle of the ship is determined based on the Euler angle.

[0032] In one embodiment of the present invention, establishing a hull coordinate system based on an inertial measurement unit includes:

[0033] The location of the inertial measurement unit is taken as the coordinate origin, the direction from the coordinate origin to the bow of the ship is taken as the x-axis, the direction from the coordinate origin to the starboard side of the ship is taken as the y-axis, the z-axis is determined based on the right-hand rule, and the hull coordinate system is established.

[0034] The present invention also provides a device for estimating a ship attitude, comprising:

[0035] An initialization module is used to establish a navigation coordinate system based on the environment where the ship is located and a hull coordinate system based on an inertial measurement unit. The navigation coordinate system and the hull coordinate system are spatial rectangular coordinate systems.

[0036] An acquisition module, used for acquiring measurement data of an inertial measurement unit;

[0037] A processing module, used for establishing a model equation based on state variables, the state variables including quaternions, the angular velocity of the ship, the linear acceleration of the ship and the deviation of the inertial measurement unit, the quaternions are used for representing the rotational motion of the ship based on the navigation coordinate system;

[0038] Establishing measurement equations based on state variables and model equations;

[0039] Establish state space equations based on model equations and measurement equations;

[0040] Determine optimal values ​​of state variables based on state space equations and measurement data;

[0041] The attitude angle of the ship is determined based on the optimal value of the quaternion in the state variable to determine the attitude of the ship.

[0042] The present invention also provides an electronic device, comprising:

[0043] Memory for storing computer programs;

[0044] The processor is used to execute the computer program in the memory to implement the above estimation method.

[0045] The present invention also provides a computer storage medium, wherein the computer storage medium stores a computer program, and when the computer program is executed by a processor, the processor executes the above estimation method.

[0046] The present invention also provides a computer program product. When the computer program product is run on an electronic device, the electronic device executes the above estimation method.

[0047] The above technical solution of the present invention has at least the following beneficial effects:

[0048] The method for estimating the attitude of a ship based on an inertial measurement unit according to an embodiment of the present invention establishes a model equation through state variables, presets a measurement value for the bow of the ship, establishes a measurement equation for the ship, and establishes a state space equation based on the model equation and the measurement equation, thereby determining the attitude angle of the ship. Compared with the prior art that does not use the linear acceleration of the accelerometer to solve the attitude angle of the ship, the present invention uses the linear acceleration when establishing the measurement equation, which can accurately measure the attitude of the ship during movement. And by presetting a measurement value for the bow of the ship, the present invention can estimate the relative change of the bow of the ship, which is beneficial to improve the accuracy of the ship attitude estimation. In addition, the present invention only needs to use the measurement data of the inertial measurement unit, avoids the use of a magnetometer for measurement, and can simplify the process of solving the ship attitude. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0050] Figure 2 The process of the method for estimating the ship attitude based on the inertial measurement unit according to an embodiment of the present invention is as follows Figure 1 ;

[0051] Figure 3 The process of the method for estimating the ship attitude based on the inertial measurement unit according to an embodiment of the present invention is as follows Figure 2 ;

[0052] Figure 4 A flow chart of establishing a measurement equation based on state variables and model equations according to an embodiment of the present invention;

[0053] Figure 5 A flow chart of establishing a first measurement equation according to an embodiment of the present invention;

[0054] Figure 6A flow chart of establishing a state space equation according to an embodiment of the present invention;

[0055] Figure 7 A flowchart of determining an optimal value of a state variable according to an embodiment of the present invention;

[0056] Figure 8 A schematic diagram of a gyroscope deviation in a simulation case according to an embodiment of the present invention;

[0057] Fig. 9 A schematic diagram of an estimated value of a ship's attitude angle in a simulation case according to an embodiment of the present invention;

[0058] Fig.10 A schematic diagram of a gyroscope deviation in a six-degree-of-freedom platform experiment according to an embodiment of the present invention;

[0059] Fig.11 A schematic diagram of the estimated value of the ship attitude angle in a six-degree-of-freedom platform experiment according to an embodiment of the present invention;

[0060] Fig.12 FIG. 4 is a block diagram of a SoC according to an embodiment of the present invention. DETAILED DESCRIPTION

[0061] In order to make the purpose, technical solution and advantages of the embodiment of the present invention clearer, the technical solution of the embodiment of the present invention will be clearly and completely described below in conjunction with the drawings of the embodiment of the present invention. Obviously, the described embodiment is a part of the embodiment of the present invention, not all of the embodiments. Based on the described embodiment of the present invention, all other embodiments obtained by ordinary technicians in this field belong to the scope of protection of the present invention.

[0062] To facilitate understanding of the technical solution of the present invention, the technical problem of the present invention is first further explained below.

[0063] The inertial measurement unit contains three accelerometers and three gyroscopes, which can measure the motion data of the carrier's six degrees of freedom, namely three accelerations and three angular velocities. To solve the attitude angle from the angular velocity, an integration operation is required. Due to the measurement deviation and noise of the gyroscope, the result of the integration operation will drift infinitely and the attitude angle cannot be accurately estimated. In other words, the accuracy of the attitude angle solution is not high, resulting in a large deviation in the ship's attitude estimation.

[0064] In order to solve the problem of inaccurate ship attitude angle solution caused by gyroscope measurement deviation and noise, in some embodiments, the measurement data of the magnetometer is added, the acceleration data of the accelerometer, the gyroscope angular velocity data and the magnetometer measurement data are fused, and the Kalman filter algorithm is used to estimate the ship attitude. This method constructs a gyroscope deviation model based on a low-cost MEMS attitude reference system, which is divided into a constant deviation and a random drift deviation, and uses a cubature Kalman filter algorithm to solve the ship attitude, thereby improving the calculation rate and the robustness of the filter. However, the magnetometer measures the earth's magnetic field strength, which is easily affected by magnetic interference, and the increase of the magnetometer's measurement data increases the complexity of the solution process.

[0065] In another embodiment, only the measurement data of the IMU is used, that is, three acceleration data and three angular velocity data are fused. When the ship is in a static state, the measurement data of the accelerometer includes the acceleration of gravity, and the roll angle and pitch angle can be accurately calculated. The method is based on the IMU ship attitude monitoring system and uses the acceleration of gravity to estimate the static roll angle and pitch angle, uses the Euler method to estimate the ship attitude by the angular velocity measured by the gyroscope, and uses the Kalman filter algorithm to fuse the two types of ship attitude estimation data to solve the optimal estimate of the ship attitude. However, when the ship is in a dynamic state, the measurement data of the accelerometer contains not only the acceleration of gravity, but also the linear acceleration, which will affect the accuracy of the ship attitude estimation.

[0066] That is to say, in order to solve the problem of attitude solution drift caused by gyroscope deviation, in the first embodiment, it is necessary to integrate the measurement data of magnetometers, etc., the solution process is more complicated, and the magnetometer is easily interfered by the magnetic field. In the second embodiment, when only IMU measurement data is used to solve the ship attitude, the linear acceleration of the ship in dynamic state is not considered, which has certain limitations and it is difficult to ensure the solution accuracy under complex sea conditions.

[0067] In order to solve the technical problems mentioned in the above embodiments, the present invention provides a method for estimating the attitude of a ship based on an inertial measurement unit, which adds the linear acceleration in the quaternion and IMU measurement data to the solution process, and can accurately measure the attitude of the ship during movement, which is conducive to improving the accuracy of ship attitude estimation. And avoid using a magnetometer for measurement, which can simplify the solution process of the ship attitude.

[0068] The following describes in detail the application scenarios of the method for estimating the ship attitude based on the inertial measurement unit according to the embodiment of the present invention with reference to the accompanying drawings.

[0069] refer to Figure 1 , Figure 1 FIG. 1 shows a scene diagram of a ship operating at sea according to an embodiment of the present invention. Figure 1As shown, when the ship 100 is operating at sea, the attitude of the ship 100 will continue to change due to the influence of the marine environment. In order to measure the attitude of the ship, an inertial measurement unit 200 is generally installed on the ship 100. The inertial measurement unit 200 is a device for measuring the three-axis attitude angle (or angular velocity) and acceleration of an object, and can be used to measure the attitude angle and acceleration of the ship 100 in three-dimensional space. Based on the measured data of the ship, the attitude of the ship is calculated, so that the movement of the ship can be accurately compensated to ensure the safety of offshore loading and unloading operations and personnel passage.

[0070] A method for estimating a ship attitude based on an inertial measurement unit according to an embodiment of the present invention will be described in detail below with reference to the accompanying drawings.

[0071] refer to Figure 2 , Figure 2 The flow chart of a method for estimating ship attitude based on an inertial measurement unit according to an embodiment of the present invention is shown. Figure 1 .like Figure 2 As shown, the method for estimating the ship attitude includes S201-S206, and the method is applied to a ship equipped with an inertial measurement unit (such as Figure 1 The ship 100 shown in the figure) wherein the inertial measurement unit can be arranged at the center of mass of the ship. The inertial measurement unit includes an accelerometer and a gyroscope. The accelerometer can measure the gravitational acceleration and linear acceleration of the ship, and the gyroscope can measure the angular velocity of the ship.

[0072] In S201, 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.

[0073] For example, Figure 1 As shown, the navigation coordinate system n and the hull coordinate system b are both spatial rectangular coordinate systems. As a reference coordinate system, the navigation coordinate system n does not change during the movement of the ship, that is, the navigation coordinate system n is stationary relative to the earth. n can be a North East Down (NED) coordinate system, that is, a NED coordinate system. The NED coordinate system is a reference coordinate system selected for navigation solution according to the needs of the navigation system during navigation. The NED coordinate system takes the north direction as the positive direction of the x-axis (such as Figure 1 Xn), the y-axis is facing due east (such as Figure 1 Yn), z-axis downward (such as Figure 1Zn). Because 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 navigation coordinate system n is regarded as an inertial system. The inertial system is the inertial field in classical physics and special relativity, which refers to the reference system in which Newton's laws of motion are valid. In other words, the navigation coordinate system n can describe space uniformly and isotropically, and can describe time uniformly. 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.

[0074] Further, in step S201, establishing a hull coordinate system based on the inertial measurement unit may include: taking the location of the inertial measurement unit as the coordinate origin, and taking the direction from the coordinate origin to the bow of the ship as the x-axis (such as Figure 1 Xb), with the direction from the origin to the starboard side of the ship as the y-axis (e.g. Figure 1 Yb), determine the z-axis based on the right-hand rule (such as Figure 1 The hull coordinate system changes with the movement of the ship, which helps to accurately describe the movement posture of the ship.

[0075] In S202, a model equation is established based on state variables, including quaternions, angular velocity of the ship, linear acceleration of the ship, and deviation of an inertial measurement unit.

[0076] For example, the quaternion includes four state variables, the angular velocity of the ship includes three state variables, the linear acceleration of the ship includes three state variables, and the deviation of the inertial measurement unit includes three state variables, that is, the number of state variables is 13.

[0077] Specifically, the model equations may include four. A mathematical model of quaternions and the angular velocity of the ship's rotation is established as the first model equation. A first-order dynamic model of the angular velocity of the ship's rotation is established as the second model equation. A dynamic model of the gyroscope deviation is established as the third model equation. A first-order dynamic model of the linear acceleration of the IMU is established as the fourth model equation. Among them, the quaternion can describe the rotational motion of the ship, that is, it represents the rotational motion of the ship based on the navigation coordinate system.

[0078] The first model equation for the relationship between the quaternion and the angular velocity of the ship's rotation is as follows:

[0079]

[0080] Where q=[q0,q1,q2,q3] T is the quaternion vector corresponding to the rotation from the navigation coordinate system n to the hull coordinate system b, is the value of the rotation speed of the hull coordinate system b relative to the navigation coordinate system n in the hull coordinate system b.

[0081] The true rotation rate of the ship can be assumed to be a first-order process, and the corresponding second model equation is as follows:

[0082]

[0083] Among them, c ωx 、c ωy and c ωz is a constant greater than 0, ζ ωx , ωy and ωz is the noise of the model.

[0084] The measurement deviation of the gyroscope can be assumed to be a very slow-changing process, so the corresponding third model equation is as follows:

[0085]

[0086] Among them, ρx , ρy and ρz is the noise of the model.

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

[0088]

[0089] Among them, c ax 、c ay and c az is a constant greater than 0, ζ ax , ay and az is the noise of the model.

[0090] In S203 , a measurement equation is established based on the state variables and the model equation.

[0091] For example, the four model equations and 13 state variables in step S202 can be used to establish measurement equations for the gyroscope, accelerometer and yaw angle, respectively. By establishing three measurement equations, it is helpful to accurately describe the influence of the state variables on the measurement data.

[0092] In S204 , a state space equation is established based on the model equation and the measurement equation.

[0093] For example, the state space equations can be formulated based on continuous time.

[0094] In S205 , measurement data of the inertial measurement unit is determined, and optimal values ​​of state variables are determined based on the state space equation and the measurement data.

[0095] For example, the measurement data is the data measured by the inertial measurement unit in continuous time. The measurement data includes the measured value of the sum of the ship's linear acceleration and gravitational acceleration in the hull coordinate system b and the measured value of the angular velocity in the hull coordinate system b. Among them, linear acceleration is a physical quantity that describes the rate of change of the magnitude and direction of the linear velocity of a rigid body with respect to time. Linear velocity is the speed at which any point on an object moves in a circular motion about a fixed axis. Gravitational acceleration is the acceleration caused by gravity on a freely falling object. Linear acceleration and gravitational acceleration can be measured by an accelerometer. Angular velocity describes how fast an object moves around the center of a circle. Angular velocity can be measured by a gyroscope.

[0096] The actual measurement data and the predicted values ​​obtained by solving the state space equations can be solved by the filter model to determine the optimal estimated values ​​(i.e., optimal values) of the 13 state variables. The filter model can be an extended Kalman filter model.

[0097] In S206, the attitude angle of the ship is determined based on the optimal value of the quaternion in the state variable to determine the attitude of the ship.

[0098] For example, the optimal estimate value (ie, the optimal value) of the quaternion may be converted into the attitude angle of the ship, so that the attitude of the ship at sea may be determined based on the attitude angle.

[0099] Reference below Figure 3 , Figure 3 The flow chart of a method for estimating ship attitude based on an inertial measurement unit according to an embodiment of the present invention is shown. Figure 2 .like Figure 3 As shown in the figure, first, the state change equation is established based on the quaternion, the rotational angular velocity of the ship, the gyroscope deviation and the linear acceleration of the ship. Among them, the quaternion, the rotational angular velocity of the ship, the gyroscope deviation and the linear acceleration of the ship are the 13 state variables of the system. Then, the measurement equations for the gyroscope, accelerometer and virtual heading angle (i.e., the heading angle) are established (i.e., Figure 2 The state space equation is established based on the state change equation and the measurement equation. The state space equation is solved by using the extended Kalman filter model, and the optimal estimate of the state variable is obtained by combining the actual measurement value of the inertial measurement unit, so as to calculate the attitude angle of the ship and estimate the attitude of the ship, which can further accurately compensate for the movement of the ship to ensure the safety of offshore loading and unloading operations and personnel passage.

[0100] In an embodiment of the present invention, a model equation is established through state variables, and a measurement value is preset for the bow of the ship, and a measurement equation of the ship is established. Based on the model equation and the measurement equation, a state space equation is established to determine the attitude angle of the ship. Compared with the prior art that does not use the linear acceleration of the accelerometer to solve the attitude angle of the ship, the present invention uses the linear acceleration of the ship when establishing the measurement equation, and incorporates the quaternion into the measurement equation, which can accurately measure the attitude of the ship during movement. And the present invention can estimate the relative change of the bow of the ship by presetting a measurement value for the bow of the ship, which is conducive to improving the accuracy of the ship attitude estimation. In addition, the present invention only needs to use the measurement data of the inertial measurement unit, avoids the use of a magnetometer for measurement, and can simplify the process of solving the ship attitude.

[0101] The following describes in detail the steps of the above embodiments of the present invention in conjunction with specific embodiments.

[0102] In one embodiment of the present invention, the measurement equation may include a first measurement equation, a second measurement equation, and a third measurement equation.

[0103] Combine the following Figure 2 , and refer to Figure 4 , to illustrate an embodiment of the present invention. Figure 4 A flow chart of establishing a measurement equation based on state variables and model equations according to an embodiment of the present invention is shown.

[0104] like Figure 2 and Figure 4 As shown, in one embodiment of the present invention, in S203, establishing a measurement equation based on the state variables and the model equation may include the following steps S401-S403.

[0105] Step S401: Establish a first measurement equation based on quaternion, linear acceleration, gravitational acceleration and model equation.

[0106] For example, a first measurement equation regarding the accelerometer is established based on the fourth model equation, quaternions, linear acceleration, and gravitational acceleration.

[0107] Step S402: Establish a second measurement equation based on the angular velocity and the model equation.

[0108] For example, a second measurement equation for the gyroscope is established based on the second model equation, the third model equation, and the angular velocity.

[0109] Because the angular velocity output by the gyroscope generally contains noise and deviation, it is not equal to the actual rotation rate of the ship. The corresponding second measurement equation is as follows:

[0110]

[0111] in, and is the measurement data of the three gyroscopes (i.e., predicted values), and is the measurement noise of the gyroscope.

[0112] Step S403: establishing a third measurement equation of the ship's pitch angle based on quaternions.

[0113] For example, in order to prevent the drift of the yaw angle, a virtual measurement value is added to the yaw angle, and the third measurement equation is:

[0114]

[0115] Combine the following Figure 4 , and refer to Figure 5 , to illustrate an embodiment of the present invention. Figure 5 A flow chart of establishing a first measurement equation according to an embodiment of the present invention is shown.

[0116] like Figure 4 and Figure 5 As shown, for S401 of an embodiment of the present invention, establishing a first measurement equation based on quaternions, linear acceleration, gravitational acceleration and a model equation may include the following steps S501-S502.

[0117] Step S501: establishing a conversion equation between the navigation coordinate system and the hull coordinate system based on quaternions.

[0118] For example, when the ship only has gravity acceleration, because gravity acceleration always points to the z-axis of the navigation coordinate system n, the roll and pitch of the ship can be reflected in the changes in the accelerometer measurement values. The coordinate transformation relationship (i.e., transformation equation) between the navigation coordinate system n and the hull coordinate system b is as follows:

[0119]

[0120] Step S502: Establish a first measurement equation based on the conversion equation, the model equation, the linear acceleration and the gravitational acceleration.

[0121] For example, the angular velocity output by the three accelerometers includes the influence of gravity acceleration and measurement noise. The corresponding output fourth model equation can be converted into the following:

[0122]

[0123] in, is the matrix in step S501, and is the noise of the model.

[0124] Based on the conversion equation in S501 and the fourth model equation described above, the relationship between the measurement values ​​of the three accelerometers and the quaternion vector (ie, the first measurement equation) is as follows:

[0125]

[0126] in, and is the measured data (i.e. predicted value) of the three accelerometers, g is the acceleration due to gravity, and is the measurement noise of the accelerometer.

[0127] By establishing the conversion equation and fusing the conversion equation with the model equation, the first measurement equation about the accelerometer can be obtained. The drift of the quaternion can be corrected by the measurement value of the accelerometer, which can further improve the accuracy of the ship attitude estimation.

[0128] Combine the following Figure 2 , and refer to Figure 6 , to illustrate an embodiment of the present invention. Figure 6 A flow chart of establishing a state space equation according to an embodiment of the present invention is shown. In an embodiment of the present invention, the state space equation includes a first state equation and a second state equation.

[0129] like Figure 2 and Figure 6 As shown, in S204, establishing a state space equation based on the model equation and the measurement equation may include the following steps S601-S603.

[0130] Step S601: Establish a state change equation based on the model equation.

[0131] For example, the state variables of the system include quaternions, the true rotation angular velocity of the ship, the measurement deviation of the gyroscope, and the true linear acceleration of the IMU, a total of 13 state variables. The first state equation is as follows:

[0132] x=[q0,q1,q2,q3,ω x ,ω y ,ω z ,ρ x ,ρ y ,ρ z ,a x ,a y ,a z ] T .

[0133] Step S602: Establish a state measurement equation based on the measurement equation.

[0134] For example, the sensor's measurement data includes three accelerometer measurements, three gyroscope measurements, and a virtual yaw angle, a total of 7 measurement data. The second state equation is as follows:

[0135]

[0136] Step S603: Setting an observer based on the state change equation and the state measurement equation.

[0137] For example, x0-x13 are used to correspond to the 13 state variables in step S601 respectively, and the four model equations in step S202 are integrated into the first state equation, and the first measurement equation in step S502, the second measurement equation in S402 and the third measurement equation in S403 are integrated into the state measurement equation, and the observer is obtained as follows:

[0138]

[0139] Among them, the observer is used to observe the motion state of the ship, that is, 13 system state variables.

[0140] Combine the following Figure 2 , and refer to Figure 7 , to illustrate an embodiment of the present invention. Figure 7 A flow chart of determining the optimal value of a state variable according to an embodiment of the present invention is shown.

[0141] like Figure 2 and Figure 7 As shown, in S205, determining the optimal value of the state variable based on the state space equation and the measurement data may include the following steps S701-S702.

[0142] Step S701: discretize the state space equation based on the forward difference method.

[0143] For example, based on the observer in step S603, the forward difference method is used to obtain the discrete state method as follows:

[0144]

[0145] in:

[0146]

[0147] Among them, the I matrix is ​​the identity matrix, and the M 4×4 、N 4×3 and C ω , C a The specific definitions are as follows:

[0148]

[0149] ω1=ω x ,ω2=ω y ,ω3=ω z .

[0150] H k+1 The matrix is ​​defined as follows:

[0151]

[0152] The specific matrix is ​​defined as follows:

[0153]

[0154] a1=a x ,a2=a y ,a4=a z -g,

[0155]

[0156] in, d=2(q1q2+q0q3).

[0157] Step S702: Based on the discretized state space equation and the measurement data, the optimal value of the state variable is calculated by using the extended Kalman filter model.

[0158] For example, based on the above discrete state change equation, the extended Kalman filter algorithm is used to update the optimal estimate of the system state according to the predicted value and actual measurement value of the system state (i.e., the measurement data of the IMU), and the optimal estimate of the system state includes the optimal value of the quaternion. In other words, by presetting the initial value of the state change equation, the discrete system state equation can predict the change of the system state, that is, based on the system state at time t-1, the system state at time t is predicted, and based on the predicted system state at time t, the measurement value of the IMU at time t and the measurement value of the virtual heading (i.e., the 7 measurement values ​​of the predicted state measurement equation) are predicted. According to the actual measurement value of the IMU at time t and the predicted measurement value at time t, the extended Kalman filter algorithm is used to update the optimal estimate of the system state at time t (i.e., the 13 state variables of the first state equation). In addition, based on the optimal estimate of the system state at time t, the optimal estimate of the system state at the next moment (i.e., time t+1) can be calculated.

[0159] The specific process of using the extended Kalman filter algorithm can also refer to the existing technology.

[0160] By discretizing the state space equations through the forward difference method and solving them based on the predicted values ​​and actual measured values ​​using the extended Kalman filter algorithm, the optimal values ​​of the state variables can be measured, thereby obtaining the optimal value of the quaternion, so that the ship attitude solution result is more accurate.

[0161] In one embodiment of the present invention, step S206, determining the attitude angle of the ship based on the optimal value of the quaternion in the state variable, may include: converting the optimal value of the quaternion into Euler angles, and determining the attitude angle of the ship based on the Euler angles.

[0162] The expression of the Euler angle from the navigation coordinate system n to the hull coordinate system b is as follows:

[0163]

[0164] in, is the roll angle, is the pitch angle, It is the bow rolling angle, which becomes the attitude angle of the ship after attitude conversion.

[0165] By converting quaternions into Euler angles and obtaining the ship's attitude angle information after attitude conversion, the ship's attitude during motion can be accurately measured, which helps to improve the accuracy of the ship's attitude solution process.

[0166] The following is an experimental test of the ship attitude estimation method based on the inertial measurement unit of the present invention.

[0167] Reference below Figure 8 and Fig. 9 , the value of the observer in a simulation case according to an embodiment of the present invention is described. Figure 8 A schematic diagram showing gyroscope deviation in a simulation case according to an embodiment of the present invention is shown. Fig. 9 A schematic diagram showing an estimated value of a ship attitude angle in a simulation case according to an embodiment of the present invention.

[0168] In one embodiment of the present invention, Figure 8 and Fig. 9 As shown in the figure, in the simulation case, the amplitude response operator (RAO) of an engineering ship is used as input, the JONSWAP wave spectrum is selected, and the method of superposition of finite regular waves is used to simulate and analyze the six-degree-of-freedom motion response of the ship under a significant wave height of 3.5 meters. The comparison results of the estimated gyro deviation and the actual deviation of the simulation case analysis are shown in Figure 1. Figure 8 It can be seen that the estimated deviation starts from the initial value, gradually approaches the actual deviation value after a short period of initialization, and finally the estimated deviation value stabilizes near the actual deviation value. The results show that not only the deviation estimation of the roll speed and pitch speed is accurate, but also the deviation of the bow is close to the actual deviation value, so this observer can accurately estimate the roll angle, pitch angle and bow angle of the ship.

[0169] In order to eliminate the influence of observer initialization, only the estimated value of the ship attitude in the steady state is shown. Fig. 9 The roll angle of (a) is Fig. 9 The pitch angle of (b), and Fig. 9 From the change of the bowing angle over time in (c), it can be seen that the estimated values ​​of different angles are almost consistent with the true values, and the error is limited to the engineering allowable range. In addition, in order to make the comparison and analysis more intuitive, Fig. 9 The error between the estimated value and the actual value is also shown, and it can be seen that the estimated errors of the ship's roll angle, pitch angle and bow angle are all small. In addition, the observer of the embodiment of the present invention is also very effective in estimating the ship's bow angle, which improves the accuracy of the ship's attitude solution process.

[0170] Specifically, the estimation error of the ship attitude angle in the simulation case is shown in Table 1 below.

[0171] Table 1. Ship attitude angle estimation error under simulation cases

[0172] Roll Pitch Bow Maximum angle (degrees) 1.9331 5.8704 1.888 Root mean square error (degrees) 0.0881 0.1024 0.0858 percentage(%) 4.5574 1.7443 4.5444

[0173] As shown in Table 1, when estimating the ship attitude angle in the simulation case, the roll angle and the bow angle do not exceed 5%, and the pitch angle does not exceed 2%, which ensures the accuracy of the ship attitude estimation.

[0174] Reference below Fig.10 and Fig.11 , the value of the observer in a six-degree-of-freedom platform experiment according to an embodiment of the present invention is described. Fig.10 A schematic diagram showing the gyroscope deviation in a six-degree-of-freedom platform experiment according to an embodiment of the present invention. Fig.11 A schematic diagram showing the estimated value of the ship attitude angle in a six-degree-of-freedom platform experiment according to an embodiment of the present invention is shown.

[0175] In one embodiment of the present invention, Fig.10 and Fig.11 As shown in the figure, in the six-degree-of-freedom platform experiment, due to the motion limitation of the six-degree-of-freedom platform device, the sea conditions with a significant wave height of 0.8 meters, a spectral peak period of 8s, and a wave direction angle of 20 degrees were selected to simulate and calculate the six-degree-of-freedom motion data of the ship. The six-degree-of-freedom platform was driven by the data, and the IMU was installed on the six-degree-of-freedom platform to obtain measurement data. Fig.10 The estimated deviation value of the gyroscope in the six-degree-of-freedom platform experiment gradually approaches the true value after initialization, with a small error, which further verifies the accuracy of the estimation method.

[0176] Fig.11 The comparison results between the estimated and true values ​​of the attitude angles in the six-degree-of-freedom platform experiment are shown in Figure 2. Fig.11As shown in the figure, the estimated values ​​of the roll angle (a), the pitch angle (b) and the bow angle (c) have a small deviation from the true value, which can better ensure the accuracy and help apply the observer to the actual attitude estimation of the ship, so as to further accurately compensate for the movement of the ship to ensure the stability and safety of the ship.

[0177] Specifically, the ship attitude angle estimation error table under the six-degree-of-freedom platform experiment is shown in Table 2 below.

[0178] Table 2. Estimation error of ship attitude angle under six-degree-of-freedom platform experiment

[0179] Roll Pitch Bow Maximum angle (degrees) 0.2912 1.1257 0.2785 Root mean square error (degrees) 0.0140 0.0485 0.0235 percentage(%) 4.8077 4.3084 8.4381

[0180] As shown in Table 2, when estimating the ship attitude angle under the six-degree-of-freedom platform experiment, the roll angle and pitch angle do not exceed 5%, and the bow angle does not exceed 10%, which are all within the allowable error range, further ensuring the accuracy of the ship attitude estimation.

[0181] The present invention also provides a device for estimating a ship's attitude, comprising an initialization module, an acquisition module and a processing module.

[0182] The initialization module is used to establish a navigation coordinate system based on the environment where the ship is located, and to establish a hull coordinate system based on an inertial measurement unit. The navigation coordinate system and the hull coordinate system are spatial rectangular coordinate systems.

[0183] The acquisition module is used to acquire the measurement data of the inertial measurement unit, and the measurement data includes the measurement value of the sum of the linear acceleration and the gravitational acceleration of the ship in the hull coordinate system and the measurement value of the angular velocity.

[0184] The processing module is used to establish a model equation based on state variables, the state variables include quaternions, the angular velocity of the ship, the linear acceleration of the ship and the deviation of the inertial measurement unit, the quaternion is used to represent the rotational motion of the ship based on the navigation coordinate system; establish a measurement equation based on the state variables and the model equation; establish a state space equation based on the model equation and the measurement equation; determine the optimal value of the state variable based on the state space equation and the measurement data; determine the attitude angle of the ship based on the optimal value of the quaternion in the state variable to determine the attitude of the ship.

[0185] It should be noted that the specific functions and implementation processes of each module in the embodiment of the present application can refer to the above embodiment. Figure 2-Figure 11 The methods explained will not be repeated here.

[0186] The present invention also provides an electronic device, comprising a memory and a processor.

[0187] The memory is used to store computer programs. The processor is used to execute the computer programs in the memory to achieve the above Figure 2-Figure 11 The estimation method explained.

[0188] The present invention also provides a computer storage medium, the computer storage medium stores a computer program, and when the computer program is executed by a processor, the processor executes the above Figure 2-Figure 11 The estimation method explained.

[0189] The present invention also provides a computer program product, which, when executed on an electronic device, enables the electronic device to execute the above Figure 2-Figure 11 The estimation method explained.

[0190] In summary, the method for estimating the attitude of a ship based on an inertial measurement unit of the present invention establishes a model equation through state variables, presets a measurement value for the bow of the ship, establishes the measurement equation of the ship, and establishes a state space equation based on the model equation and the measurement equation, thereby determining the attitude angle of the ship. The linear acceleration of the ship is used when establishing the measurement equation, and the attitude of the ship during movement can be accurately measured. Moreover, by presetting a measurement value for the bow of the ship, the relative change of the bow of the ship can be estimated, which is conducive to improving the accuracy of the ship attitude estimation. Moreover, the present invention only needs to use the measurement data of the inertial measurement unit, avoids the use of a magnetometer for measurement, and can simplify the process of solving the ship attitude.

[0191] Reference now Fig.12 , which is a block diagram of a SoC (System on Chip) 1300 according to an embodiment of the present application. Fig.12 In the FIG, similar components have the same reference numerals. In addition, the dashed boxes are optional features of more advanced SoCs. Fig.12 In the embodiment, SoC 1300 includes: an interconnect unit 1350, which is coupled to an application processor 1310; a system agent unit 1380; a bus controller unit 1390; an integrated memory controller unit 1340; a group or one or more coprocessors 1320, which may include integrated graphics logic, an image processor, an audio processor, and a video processor; a static random access memory (SRAM) unit 1330; and a direct memory access (DMA) unit 1360. In one embodiment, the coprocessor 1320 includes a dedicated processor, such as, for example, a network or communication processor, a compression engine, a GPGPU, a high throughput MIC processor, or an embedded processor.

[0192] The static random access memory (SRAM) unit 1330 may include one or more computer-readable media for storing data and / or instructions. The computer-readable storage medium may store instructions, specifically, temporary and permanent copies of the instructions. The instructions may include: when executed by at least one unit in the processor, causing the Soc 1300 to execute the instructions according to the above embodiments. Figure 2-Figure 11 The methods explained will not be repeated here.

[0193] The various embodiments of the mechanism disclosed in the present application can be implemented in hardware, software, firmware or a combination of these implementation methods. The embodiments of the present application can 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 memory and / or storage elements), at least one input device and at least one output device.

[0194] Program code can be applied to input instructions to perform the functions described in this application and generate output information. The output information can be applied to one or more output devices in a known manner. For the purposes of this application, 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.

[0195] Program code can be implemented with high-level programming language or object-oriented programming language to communicate with the processing system. When necessary, program code can also be implemented with assembly language or machine language. In fact, the mechanism described in this application is not limited to the scope of any specific programming language. In either case, the language can be a compiled language or an interpreted language.

[0196] In some cases, the disclosed embodiments may be implemented in hardware, firmware, software, or any combination thereof. The disclosed embodiments may also be implemented as instructions carried or stored on one or more temporary or non-temporary machine-readable (e.g., computer-readable) storage media, which may be read and executed by one or more processors. For example, instructions may be distributed over a network or through other computer-readable media. Therefore, machine-readable media may 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 disks, read-only memories (CD-ROMs), magneto-optical disks, read-only memories (ROMs), random access memories (RAMs), erasable programmable read-only memories (EPROMs), electrically erasable programmable read-only memories (EEPROMs), magnetic or optical cards, flash memory, or a tangible machine-readable memory for transmitting information (e.g., carrier waves, infrared signals, digital signals, etc.) using the Internet in electrical, optical, acoustic, or other forms of propagation signals. Therefore, machine-readable media include any type of machine-readable media suitable for storing or transmitting electronic instructions or information in a machine (e.g., computer) readable form.

[0197] In the accompanying drawings, some structural or method features may be shown in a specific arrangement and / or order. However, it should be understood that such a specific arrangement and / or order may not be required. Instead, in some embodiments, these features may be arranged in a manner and / or order different from that shown in the illustrative drawings. In addition, the inclusion of structural or method features in a particular figure does not mean that such features are required in all embodiments, and in some embodiments, these features may not be included or may be combined with other features.

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

[0199] It should be noted that in the examples and description of this patent, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, the elements defined by the sentence "including one" do not exclude the existence of other identical elements in the process, method, article or device including the elements.

[0200] Unless otherwise defined, the technical terms or scientific terms used in the present invention shall have the usual meanings understood by persons with ordinary skills in the field to which the present invention belongs. The words "first", "second" and similar words used in the present invention do not indicate any order, quantity or importance, but are only used to distinguish different components. Similarly, words such as "one" or "one" do not indicate quantity restrictions, but indicate the existence of at least one. Words such as "connect" or "connected" are not limited to physical or mechanical connections, but may include electrical connections, whether direct or indirect. "Up", "down", "left", "right" and the like are only used to indicate relative positional relationships. When the absolute position of the object being described changes, the relative positional relationship also changes accordingly.

[0201] The above are preferred embodiments of the present invention. It should be pointed out that, for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principles of the present invention. These improvements and modifications should also be regarded as within the scope of protection of the present invention.

Claims

1. A method for estimating ship attitude, applied to a ship equipped with an inertial measurement unit, It is characterized in that include: A navigation coordinate system is established based on the environment where the ship is located, and a hull coordinate system is established based on the inertial measurement unit, wherein both the navigation coordinate system and the hull coordinate system are spatial rectangular coordinate systems; Establishing a model equation based on state variables, wherein the state variables include quaternions, an angular velocity of the ship, a linear acceleration of the ship, and a deviation of the inertial measurement unit, wherein the quaternions are used to represent the rotational motion of the ship based on the navigation coordinate system; Establishing a measurement equation based on the state variable and the model equation; Establishing a state space equation based on the model equation and the measurement equation; determining measurement data of the inertial measurement unit, and determining an optimal value of the state variable based on the state space equation and the measurement data; The attitude angle of the ship is determined based on the optimal value of the quaternion in the state variable to determine the attitude of the ship.

2. The method for estimating a ship's attitude according to claim 1, It is characterized in that The measurement equations include a first measurement equation, a second measurement equation and a third measurement equation, The establishing of the measurement equation based on the state variable and the model equation comprises: Establishing the first measurement equation based on the quaternion, the linear acceleration, the gravitational acceleration of the ship and the model equation; Establishing the second measurement equation based on the angular velocity and the model equation; The third measurement equation of the pitch angle of the vessel is established based on the quaternion.

3. The method for estimating a ship's attitude according to claim 2, It is characterized in that The establishing the first measurement equation based on the quaternion, the linear acceleration, the gravitational acceleration of the ship and the model equation comprises: Establishing a conversion equation between the navigation coordinate system and the hull coordinate system based on the quaternion; The first measurement equation is established based on the conversion equation, the model equation, the linear acceleration, and the gravitational acceleration.

4. The method for estimating a ship's attitude according to claim 1 or 2, It is characterized in that The state space equation includes a state change equation and a state measurement equation. The establishing of the state space equation based on the model equation and the measurement equation comprises: Establishing the state change equation based on the model equation; Establishing the state measurement equation based on the measurement equation; An observer is set based on the state change equation and the state measurement equation, and the observer is used to observe the state variable.

5. The method for estimating a ship's attitude according to claim 1, It is characterized in that The determining the optimal value of the state variable based on the state space equation and the measurement data comprises: discretizing the state space equation based on a forward difference method; Based on the discretized state space equation and the measurement data, the optimal value of the state variable is calculated through an extended Kalman filter model.

6. The method for estimating a ship's attitude according to claim 1, It is characterized in that The determining the attitude angle of the ship based on the optimal value of the quaternion in the state variable comprises: Convert the optimal value of the quaternion into Euler angles; The attitude angle of the ship is determined based on the Euler angle.

7. The method for estimating a ship's attitude according to claim 1, It is characterized in that The establishing of a hull coordinate system based on the inertial measurement unit comprises: The inertial measurement unit is located as the coordinate origin, the direction from the coordinate origin to the bow of the ship is taken as the x-axis, the direction from the coordinate origin to the starboard side of the ship is taken as the y-axis, the z-axis is determined based on the right-hand rule, and the hull coordinate system is established.

8. A device for estimating a ship's attitude, It is characterized in that include: An initialization module, used to establish a navigation coordinate system based on the environment where the ship is located, and to establish a hull coordinate system based on an inertial measurement unit, wherein the navigation coordinate system and the hull coordinate system are spatial rectangular coordinate systems; An acquisition module, used for acquiring measurement data of the inertial measurement unit; a processing module, configured to establish a model equation based on state variables, wherein the state variables include quaternions, an angular velocity of the ship, a linear acceleration of the ship, and a deviation of the inertial measurement unit, wherein the quaternions are used to represent the rotational motion of the ship based on the navigation coordinate system; Establishing a measurement equation based on the state variable and the model equation; Establishing a state space equation based on the model equation and the measurement equation; determining an optimal value of the state variable based on the state-space equation and the measurement data; The attitude angle of the ship is determined based on the optimal value of the quaternion in the state variable to determine the attitude of the ship.

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

10. A computer storage medium, It is 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 estimation method according to any one of claims 1 to 7.

11. A computer program product, It is characterized in that When the computer program product runs on an electronic device, the electronic device is enabled to execute the estimation method according to any one of claims 1 to 7.

Images