A visual-IMU fusion pose solution method for wave compensation of a wind power operation and maintenance ship

Abstract

The application discloses a visual-IMU fusion pose solution method for wave compensation of a wind power operation and maintenance ship, the method uses a visual-IMU sensor as a wind power operation and maintenance ship pose detection device, wave disturbances of six degrees of freedom, including cross sway, longitudinal sway, heave, roll, pitch and yaw, are collected by the pose detection device, six-degree-of-freedom wave compensation values required by a wave compensation platform are calculated, and compensation is carried out. The application uses an IMU and an RGB-D camera fusion mode to detect the six-degree-of-freedom pose of the ship, can effectively solve the problem that a single IMU sensor cannot accurately solve three degrees of freedom displacement, such as cross sway, longitudinal sway and heave, and thus full compensation of wave disturbances of six degrees of freedom is realized.

Description

Technical Field

[0001] This invention belongs to the field of ship wind power operation and maintenance, and specifically relates to a visual-IMU fusion pose calculation method for wave compensation of wind power operation and maintenance ships. Background Technology

[0002] Offshore wind turbine maintenance vessels experience six degrees of freedom of motion due to ocean waves, posing safety hazards when using walkways to transfer maintenance personnel to the wind turbine platform. Onboard active wave compensation devices can isolate wave disturbances to a certain extent, ensuring the safety of maintenance personnel boarding the vessel.

[0003] Shipborne active wave compensation devices need to obtain the six degrees of freedom attitude of the ship under the action of waves. Traditional ship attitude detection devices usually use IMU, but IMU can only obtain relatively accurate roll, pitch and yaw attitude angles in a short time. It cannot detect the precise sway, pitch and heave displacement, and therefore cannot fully compensate for disturbances in all six degrees of freedom. Summary of the Invention

[0004] This invention overcomes the shortcomings of the prior art and provides a visual-IMU fusion pose calculation method for wave compensation of wind power operation and maintenance vessels.

[0005] The technical solution to achieve the purpose of this invention is as follows: Firstly, this invention provides a visual-IMU fusion pose calculation method for wave compensation of wind power operation and maintenance vessels, comprising the following steps:

[0006] Step 1: Place the Stewart platform on the wind turbine maintenance vessel and build a boarding bridge on the upper platform to connect to the wind turbine platform;

[0007] Step 2: Place a ship pose detection device, consisting of an RGB camera and an IMU, at the center point of the Stewart platform; place a visual target on the wind power platform.

[0008] Step 3: The complementary filtering method is used to calculate the three-degree-of-freedom attitude angles of the IMU; the three-degree-of-freedom attitude angles and three-degree-of-freedom displacements of the camera are calculated by visual method, and the attitude angles of the camera and the IMU are fused by extended Kalman filtering to obtain six-degree-of-freedom pose information.

[0009] Step 4: When the wind power maintenance vessel is affected by waves and generates six degrees of freedom motion, the posture of the Stewart platform is controlled to compensate for the disturbance of the six degrees of freedom of the waves, so that the upper platform remains stationary relative to the wind power platform.

[0010] Furthermore, the Stewart platform described in step 1, which is fixed to the ship's deck, will experience the same attitude changes as the ship's hull due to wave disturbances:

[0011] X t =[x t y t z t φ αt φ βt φ γt ] T

[0012] Where X t This represents the actual value of the six-degree-of-freedom attitude change of the ship caused by wave disturbance.

[0013] x t y t z t φ αt φ βt φ γt These represent the actual values ​​of the six degrees of freedom attitude changes of the ship caused by wave disturbance: sway, pitch, heave, roll, pitch, and yaw.

[0014] Furthermore, the wind power platform mentioned in step 2 is a fixed wind power platform, and the visual target is fixed on the wind power platform, regarded as the world coordinate system (O). w ,X w ,Y w Z w ), located within the camera's field of view, with the camera coordinate system being (O c ,X c ,Y c Z c ).

[0015] Furthermore, in step 3, the attitude angles of the camera and the IMU are fused using extended Kalman filtering. First, complementary filtering is performed on the raw IMU data to calculate the IMU attitude information.

[0016]

[0017] in, These represent the estimated roll, pitch, and yaw angles of the IMU after complementary filtering, respectively.

[0018] The camera performs corner detection and tracking on the target. Based on the pixel coordinates of the corners in each frame, the camera's pose information at the current moment can be uniquely determined.

[0019] X′=[xyz φ′ α φ′ β φ′ γ ] T

[0020] Where x, y, z, φ′ α , φ′ β , φ′γ These represent the sway, pitch, and heave displacements calculated by the camera, as well as the roll, pitch, and yaw angles, respectively.

[0021] Furthermore, in step 3, the vision and IMU are first synchronized in time and space before data fusion, and the vision detection data is expanded to the same frequency as the IMU by interpolation.

[0022] φ′ calculated by the camera α , φ′ β , φ′ γ With IMU solution By performing fusion, compensating for gyroscope drift, and then superimposing the x, y, and z values ​​calculated by the camera, we obtain:

[0023] X = [xyz φ α φ β φ γ ] T

[0024] Where φ α φ β φ γ X represents the fused three-axis attitude angles, and X represents the fused six-DOF pose.

[0025] Extended Kalman filtering is used to fuse the attitude angles calculated by the camera and IMU to compensate for gyroscope drift, and a state estimation equation model is established:

[0026]

[0027] Where, ω αb ω βb ω γb These represent the three-axis attitude angle offsets of the IMU gyroscope, ω and ω', respectively. α ω β ω γ dt represents the current three-axis attitude angle and angular velocity values, respectively, and dt represents the sampling time interval;

[0028] The covariance prediction equation is:

[0029]

[0030] in,

[0031] g(x k )=[φ α +(ω α -ω αb )dt φ β +(ω α -ω αb )dt φ γ +(ωα -ω αb )dt ω αb ω βb ω γb ] T P k-1,k-1 Let P be the covariance matrix of the previous time step. k,k-1 Used to calculate the covariance matrix at the next time step;

[0032] The process noise covariance matrix is:

[0033]

[0034] Q α Q β Q γ Q αb Q βb Q γb These represent the angular velocity variance calculated by the IMU and the angular velocity variance calculated by the camera, respectively.

[0035] Calculate the residuals based on the camera's calculated values:

[0036] y = z k -Hx k

[0037] Where z k =[φ′ α φ′ β φ′ γ ] T The three-axis attitude angles calculated for the camera, where y is the difference between the actual observation and the estimated value, and x is the angle between the actual observation and the estimated value. k The estimated state value;

[0038]

[0039] H is the observation matrix. Since the attitude angles calculated by the camera are three-dimensional, while the state estimation equation is six-dimensional, the dimensions need to be unified through the H matrix.

[0040] The state equation is updated using the Kalman gain, where the Kalman gain is calculated as follows:

[0041]

[0042] Where R is the measurement noise of the camera;

[0043] The state update equation and covariance update equation are established as follows:

[0044] X = X - +Ky

[0045] P k,k =(IK kH)P k,k-1

[0046] X represents the updated state, i.e., the fused attitude angle [φ]. α φ β φ γ ] T X _ P is an intermediate state variable. k,k Let be the covariance matrix at the next time step.

[0047] Furthermore, the Stewart platform described in step 4, through the pose detection device, detects the six-degree-of-freedom disturbance of the ship caused by the wave effect as follows:

[0048] X = [xyz φ α φ β φ γ ] T

[0049] Based on the output value of the pose detection device, the required extension and retraction of the six electric cylinders is calculated using the Stewart platform inverse kinematics algorithm to compensate for the disturbance of the six degrees of freedom caused by the waves, so that the upper platform remains stationary relative to the wind power platform.

[0050] In a second aspect, the present invention provides an electronic device including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the steps of the method described in the first aspect.

[0051] Thirdly, the present invention provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the method described in the first aspect.

[0052] Compared with the prior art, the significant advantages of the present invention are:

[0053] (1) The attitude angles calculated by IMU and vision are integrated, which effectively reduces the drift phenomenon of IMU after long-term use.

[0054] (2) Since the IMU cannot obtain accurate displacement, the present invention obtains the ship's sway, pitch, and heave displacement by camera calculation, which can achieve full compensation for disturbances in all six degrees of freedom. Attached Figure Description

[0055] Figure 1 This is a diagram of the Stewart platform architecture.

[0056] Figure 2 A schematic diagram of a wind power maintenance vessel equipped with a Stewart wave compensation platform.

[0057] Figure 3A schematic diagram illustrating the use of interpolation to expand camera data.

[0058] Figure 4 This is a schematic diagram of complementary filtering and IMU data fusion.

[0059] Figure 5 This diagram illustrates the data structure for fusing IMU and camera data using extended Kalman filtering.

[0060] Figure 6 This is a diagram of wave compensation structure for wind power operation and maintenance vessels based on vision-IMU fusion. Detailed Implementation

[0061] The present invention will now be described in further detail with reference to the accompanying drawings and specific embodiments.

[0062] A vision-IMU fusion pose calculation method for wave compensation of wind power operation and maintenance vessels includes the following steps:

[0063] Step 1: Place the Stewart platform on the wind turbine maintenance vessel and build a boarding bridge on the upper platform to connect to the wind turbine platform.

[0064] Step 2: Place a ship pose detection device, consisting of an RGB camera and an IMU, at the center point of the Stewart platform. Place a visual target on the wind power platform.

[0065] Step 3: The complementary filtering method is used to calculate the three-degree-of-freedom attitude angles of the IMU; the three-degree-of-freedom attitude angles and three-degree-of-freedom displacements of the camera are calculated by visual method, and the attitude angles of the camera and the IMU are fused by extended Kalman filtering to obtain six-degree-of-freedom pose information.

[0066] Step 4: When the wind power maintenance vessel is affected by waves and generates six degrees of freedom motion, the posture of the Stewart platform is controlled to compensate for the disturbance of the six degrees of freedom of the waves, so that the upper platform remains stationary relative to the wind power platform.

[0067] like Figure 1 As shown, the Stewart platform consists of six electric cylinders connecting the upper and lower platforms. The upper platform can be controlled to achieve the desired pose using an inverse kinematics algorithm, while the pose of the upper platform can be solved using a forward kinematics algorithm based on the extension and retraction of the electric cylinders.

[0068] like Figure 2As shown, a wind power operation and maintenance vessel equipped with a wave compensation platform includes a Stewart wave compensation platform and a boarding bridge. The lower platform of the Stewart platform is fixed to the deck; the boarding bridge starts from the upper platform of the Stewart platform and ends on the fixed wind power platform. Through corresponding control algorithms, the disturbance of waves on the upper platform can be compensated, thereby keeping the boarding bridge stable and safely transporting wind power operation and maintenance personnel.

[0069] The wave compensation platform requires real-time acquisition of the ship's six-degree-of-freedom pose under wave disturbance. The camera and IMU are placed at the center point of the Stewart platform, and the pose is calculated using a vision-IMU fusion method.

[0070] In this embodiment, the IMU sampling frequency is set to 50Hz, and the camera sampling frequency is set to 25Hz. Figure 3 As shown, the camera data is augmented to 50Hz using interpolation. Assuming that the camera moves at a constant speed between two frames, the data at each moment can be calculated from the data of the previous two moments.

[0071] The method described above uses complementary filtering to calculate the attitude angle of the IMU, such as... Figure 4 As shown, an IMU typically consists of a gyroscope, an accelerometer, and a magnetometer. The gyroscope can directly obtain the three-axis angular velocity values ​​of the IMU, and after integration, the attitude angles of the three degrees of freedom can be obtained. The accelerometer obtains the roll and pitch angles of the IMU, but because the direction of gravity coincides with the Z-axis, the yaw angle cannot be obtained. The magnetometer can obtain the yaw angle value of the IMU. The gyroscope has better high-frequency characteristics, while the accelerometer and magnetometer have better low-frequency characteristics. In this embodiment, a high-pass filter is used to filter the gyroscope data, and a low-pass filter is used to filter the accelerometer and magnetometer data, fusing them to obtain a more accurate roll, pitch, and yaw angle.

[0072] In this embodiment, the visual target is a rectangular object to be detected, fixed on the wind power platform, and the target is regarded as a fixed coordinate system. The four corner points of the rectangle are detected by a corner detection algorithm. Since the four corner points are far apart, the nearest neighbor matching principle is used to track the four corner points in adjacent frames. The PNP algorithm is used to calculate the displacement of the camera itself in real time in six degrees of freedom: sway, pitch, heave, roll, pitch, and yaw.

[0073] The aforementioned method uses extended Kalman filtering to fuse the attitude angles of the camera and the IMU, such as... Figure 5 As shown. In this embodiment, the attitude angle data after IMU complementary filtering is used as the predicted value, and the attitude angle data calculated by vision is used as the measured value to establish a state estimation equation model:

[0074]

[0075] Where, ω αb ω βb ω γb These represent the three-axis attitude angle offsets of the IMU gyroscope, ω and ω', respectively. α ω β ω γ dt represents the current three-axis attitude angle and angular velocity values, respectively, and dt represents the sampling time interval.

[0076] The covariance prediction equation is:

[0077]

[0078] in,

[0079] g(x k )=[φ α +(ω α -ω αb )dt φ β +(ω α -ω αb )dt φ γ +(ω α -ω αb )dt ω αb ω βb ω γb ] T

[0080] The process noise covariance matrix is:

[0081]

[0082] Calculate the residuals based on the camera's calculated values:

[0083] y = z k -Hx k

[0084] Where z k =[φ′ α φ′ β φ′ γ ] T The three-axis attitude angles calculated for the camera are y, where y is the difference between the actual observed value and the estimated value.

[0085]

[0086] H is the observation matrix. Since the attitude angles calculated by the camera are three-dimensional, while the state estimation equation is six-dimensional, the dimensions need to be unified through the H matrix.

[0087] The state equation is updated using the Kalman gain, where the Kalman gain is calculated as follows:

[0088]

[0089] Where R represents the camera's measurement noise.

[0090] The state update equation and covariance update equation are established as follows:

[0091] X = X - +Ky

[0092] P k,k =(IK k H)P k,k-1

[0093] The fused attitude angles [φ] are obtained α φ β φ γ ] T .

[0094] Figure 6 This is a structural diagram of wave compensation for wind power operation and maintenance vessels based on vision-IMU fusion. The wave compensation method for wind power operation and maintenance vessels based on vision-IMU fusion used in this invention can effectively overcome the drift problem caused by long-term use of IMU for roll, pitch, and yaw angles compared to a single IMU; and can also compensate for the three degrees of freedom of sway, pitch, and heave that IMU cannot accurately measure.

[0095] The above description is only a preferred embodiment of the present invention. It should be noted that those skilled in the art can make several improvements and modifications without departing from the principle of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention.

Claims

1. A visual-IMU fusion pose calculation method for wave compensation of wind power operation and maintenance vessels, characterized in that, Includes the following steps: Step 1: Place the Stewart platform on the wind turbine maintenance vessel and build a boarding bridge on the upper platform to connect to the wind turbine platform; Step 2: Place a ship pose detection device, consisting of an RGB camera and an IMU, at the center point of the Stewart platform; place a visual target on the wind power platform. Step 3: The complementary filtering method is used to calculate the three-degree-of-freedom attitude angles of the IMU; the three-degree-of-freedom attitude angles and three-degree-of-freedom displacements of the camera are calculated by visual method, and the attitude angles of the camera and the IMU are fused by extended Kalman filtering to obtain six-degree-of-freedom pose information. Step 3 involves fusing the camera and IMU attitude angles using extended Kalman filtering. First, complementary filtering is applied to the raw IMU data to calculate the IMU attitude information. ； in, These represent the estimated roll, pitch, and yaw angles of the IMU after complementary filtering, respectively. The camera performs corner detection and tracking on the target. Based on the pixel coordinates of the corners in each frame, the camera's pose information at the current moment can be uniquely determined. ； in, These represent the sway, pitch, and heave displacements and the roll, pitch, and yaw angles calculated by the camera, respectively. Before data fusion, the vision and IMU are synchronized in time and space, and the vision detection data is expanded to the same frequency as the IMU by interpolation. Camera calculation With IMU solution The data is then fused to compensate for gyroscope drift, and then superimposed with the camera's calculated values. get: ； in The fused three-axis attitude angles The fused six-DOF pose; Extended Kalman filtering is used to fuse the attitude angles calculated by the camera and IMU to compensate for gyroscope drift, and a state estimation equation model is established: ； in, These represent the three-axis attitude angle offsets of the IMU gyroscope. These represent the current three-axis attitude angle and angular velocity values, respectively. Indicates the sampling time interval; The covariance prediction equation is: ； in, , , , Let be the covariance matrix of the previous time step. Used to calculate the covariance matrix at the next time step; The process noise covariance matrix is: ； in These represent the angular velocity variance calculated by the IMU and the angular velocity variance calculated by the camera, respectively. Calculate the residuals based on the camera's calculated values: ； in The three-axis attitude angles calculated for the camera. The difference between the actual observed value and the estimated value. The estimated state value; ； The observation matrix is ​​used because the camera's calculated attitude angles are three-dimensional, while the state estimation equation is six-dimensional. Therefore, it is necessary to... Unified dimension of the matrix; The state equation is updated using the Kalman gain, where the Kalman gain is calculated as follows: ； Where R is the measurement noise of the camera; The state update equation and covariance update equation are established as follows: , ； The updated state, i.e., the fused attitude angles. , This is an intermediate state quantity. Let be the covariance matrix at the next time step; Step 4: When the wind power maintenance vessel is affected by waves and generates six degrees of freedom motion, the posture of the Stewart platform is controlled to compensate for the disturbance of the six degrees of freedom of the waves, so that the upper platform remains stationary relative to the wind power platform.

2. The visual-IMU fusion pose calculation method for wave compensation of wind power operation and maintenance vessels according to claim 1, characterized in that, The Stewart platform described in step 1 is fixed to the ship's deck and will experience the same attitude changes as the ship's hull due to wave disturbances. ； in This represents the actual value of the six-degree-of-freedom attitude change of the ship caused by wave disturbance. These represent the actual values ​​of the six degrees of freedom attitude changes of the ship caused by wave disturbance: sway, pitch, heave, roll, pitch, and yaw.

3. The visual-IMU fusion pose calculation method for wave compensation of wind power operation and maintenance vessels according to claim 1, characterized in that, The wind power platform mentioned in step 2 is a fixed wind power platform, and the visual target is fixed on the wind power platform and regarded as the world coordinate system. Located within the camera's field of view, the camera coordinate system is .

4. The visual-IMU fusion pose calculation method for wave compensation of wind power operation and maintenance vessels according to claim 1, characterized in that, The Stewart platform described in step 4, through the pose detection device, detects the following six-degree-of-freedom disturbances caused by waves affecting the ship: ； Based on the output value of the pose detection device, the required extension and retraction of the six electric cylinders is calculated using the Stewart platform inverse kinematics algorithm to compensate for the disturbance of the six degrees of freedom caused by the waves, so that the upper platform remains stationary relative to the wind power platform.

5. An electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the steps of the method as described in any one of claims 1-4.

6. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the program is executed by the processor, it implements the steps of the method as described in any one of claims 1-4.

Images