Ocean buoy attitude test method based on solar and geomagnetic double vector auxiliary gyroscope

Abstract

This invention discloses a method for testing the attitude of marine buoys based on a solar-geomagnetic dual-vector gyroscope. The method includes: acquiring sky images via a camera, identifying solar spots and extracting the center pixel coordinates using the gray-scale centroid method; calculating the measured solar vector in the carrier coordinate system from camera intrinsic parameters and the installation matrix; calculating the solar reference vector using latitude, longitude, and time; acquiring the measured geomagnetic vector in the carrier coordinate system using a magnetometer; calculating the geomagnetic reference vector in the geographic coordinate system; constructing the Davenport matrix using the Wahba problem and solving for the eigenvectors to obtain the optimal attitude quaternion observations; establishing a model with quaternions and gyroscope zero bias as state variables; fusing the observations with the gyroscope angular rate measurements using an extended Kalman filter; and estimating the buoy's three-dimensional attitude in real time and compensating for gyroscope drift. This invention offers high accuracy and robustness, making it particularly suitable for buoy platforms operating in long-term marine environments.

Description

Technical Field

[0001] This invention relates to the field of inertial navigation technology, and in particular to a method for testing the attitude of marine buoys based on a solar geomagnetic dual-vector assisted gyroscope. Background Technology

[0002] Ocean buoys are key platforms for acquiring marine environmental data (such as waves, currents, temperature, and salinity), and the sensor data they carry usually require precise attitude information for correction.

[0003] Currently, buoy attitude measurement mainly relies on Global Navigation Satellite System (GNSS) attitude modules or high-precision inertial navigation systems (INS). GNSS attitude measurement methods are limited by the finite size of the buoy, resulting in insufficient measurement accuracy due to short baselines and susceptibility to multipath effects. High-precision INS are expensive and power-consuming, making them unsuitable for long-term unattended buoy platforms. While low-cost microelectromechanical systems (MEMS) sensors can be used for attitude estimation, accelerometers are susceptible to non-gravitational acceleration interference in dynamic environments, leading to large attitude calculation errors.

[0004] Therefore, there is an urgent need for an attitude measurement scheme suitable for ocean buoys that combines long-term stability and environmental adaptability. Summary of the Invention

[0005] The problem to be solved by this invention is to provide a method for testing the attitude of marine buoys based on a solar geomagnetic dual-vector gyroscope, so as to improve the accuracy and robustness of buoy attitude measurement.

[0006] This invention adopts the following technical solution: a method for testing the attitude of marine buoys based on a solar geomagnetic dual-vector assisted gyroscope, comprising the following steps:

[0007] S1. Constructing Reference Vectors and Measurement Vectors: Obtaining the Solar Reference Vector in the Geographic Coordinate System With the geomagnetic reference vector The corresponding solar measurement vectors are obtained through cameras and magnetometers on ocean buoys. With geomagnetic measurement vector ;

[0008] S2. Solving the initial attitude based on the Wahba problem: Constructing a weighted outer product matrix using the reference vectors and measurement vectors of the sun and geomagnetism. And thus construct the Davenport matrix. By solving its eigenvectors, we obtain attitude quaternion observations that characterize the rotation from the geographic coordinate system to the carrier coordinate system.

[0009] S3. Using attitude quaternions and gyroscope zero bias as state vectors: Establish a state prediction model. The state equations include equations for updating attitude quaternions based on gyroscope measurements minus estimated zero bias and noise. A covariance matrix is ​​also established considering the influence of process noise. Design an extended Kalman filter, using the attitude quaternion observations as the filtered observations, construct the observation equation and perform filtering recursion, and establish the covariance matrix considering the influence of observation noise. The optimal attitude quaternion of the ocean buoy and the gyroscope zero bias are estimated and output in real time by fusing the gyroscope angular rate measurement value.

[0010] S4. Fault-tolerant processing: When the solar geomagnetic vector is determined to be invalid, it automatically switches to fault-tolerant processing mode and uses the gyroscope to perform recursive attitude estimation.

[0011] Specifically, in step S1, the ocean buoy is equipped with a camera array and a main control chip. The camera array includes five cameras installed in front, behind, to the left, right and above the buoy. The main control chip is equipped with a magnetometer and a gyroscope.

[0012] Sky images were acquired using a camera array, solar flares were identified, and their center pixel coordinates were extracted using the grayscale centroid method.

[0013] ;

[0014] ;

[0015] in, For pixels grayscale value, Its pixel coordinates, Center pixel coordinates This represents the total number of pixels within the light spot area;

[0016] The solar vector in the carrier coordinate system is obtained by combining camera intrinsic parameters and installation matrix calculation. Data from magnetometers and gyroscopes were collected to measure the geomagnetic vector in volume coordinates. .

[0017] Specifically, in step S2, the initial pose is solved based on the Wahba problem, including:

[0018] 1. Calculate the weighted outer product sum matrix. , is represented as:

[0019] ;

[0020] Wherein, the superscript T indicates transpose. , These are the weighting coefficients for the camera and magnetometer; if the solar vector fails (e.g., at night, due to obstruction), set... =0; if the geomagnetic vector fails (e.g., due to magnetic field interference), let... =0.

[0021] 2. Construct the Davenport matrix , is represented as:

[0022] ;

[0023] ;

[0024] ;

[0025] ;

[0026] in, , , For the intermediate matrix, For the elements in matrix B, It is a 3×3 identity matrix;

[0027] 3. Calculate the Davenport matrix Maximum eigenvalue And its corresponding unit eigenvector, which is the optimal attitude quaternion representing the rotation from the geographic coordinate system to the carrier coordinate system. .

[0028] Specifically, in step S3, an extended Kalman filter (EKF) is constructed for data fusion, including:

[0029] 1. Define the system state vector as follows:

[0030] ;

[0031] in, A quaternion representing the attitude rotation from the geographic coordinate system to the carrier coordinate system; This is the zero bias of the three-axis gyroscope.

[0032] 2. Construct a state prediction model:

[0033] Define the gyroscope measurement model:

[0034] ;

[0035] in, This refers to the angular rate output by the gyroscope. For the true angular velocity, For measuring noise.

[0036] Define the kinematic equations of quaternions , is represented as:

[0037] ;

[0038] The true angular rate is approximated by subtracting the current zero-bias estimate from the gyroscope measurement. :

[0039] ;

[0040] Define the prior state quaternion:

[0041] ;

[0042] in, It is an obliquely symmetric matrix for angular velocity. Let be the posterior pose quaternion at time k-1. Let be the prior pose quaternion at time k. The sampling time interval, It is a 4×4 identity matrix;

[0043] The zero-biased dynamic model is modeled as a random walk process:

[0044] ;

[0045] in, This represents the change of zero bias over time. To drive the noise, the power spectral density is .

[0046] After discretization, we get:

[0047] ;

[0048] in, The gyroscope has zero bias at time k. Let k be the random walk noise vector biased at time k.

[0049] 3. State equations and Jacobian matrix:

[0050] Combining quaternions and zero biases into a state vector, the state equation is expressed as:

[0051] ;

[0052] in, For process noise, The process noise covariance matrix is... This refers to the angular rate output by the gyroscope. , Let k be the state vector at time k and k+1.

[0053] For EKF, the state transition matrix is ​​obtained through linearization. :

[0054] ;

[0055] in, Jacobi updated for quaternions; The effect of zero bias error on quaternion updates.

[0056] The process noise covariance matrix is:

[0057] ;

[0058] in, It is the process noise covariance matrix of the quaternion state.

[0059] 4. Construct the measurement equation:

[0060] The observation equation is expressed as:

[0061] ;

[0062] in, To observe the noise, Let k be the observation vector at time k. For the observation matrix:

[0063] ;

[0064] This indicates that the observation is only related to the attitude quaternion and is independent of the zero bias.

[0065] 5. Filtering recursion:

[0066] The prediction process is as follows:

[0067] ;

[0068] ;

[0069] in, For the posterior state estimate at time k-1, Let k be the prior state estimate at time k. Let be the prior covariance matrix at time k. The function is a nonlinear state transition function. The state estimation error covariance matrix;

[0070] The update process is as follows:

[0071] ;

[0072] ;

[0073] ;

[0074] in, To observe the noise covariance matrix, For Kalman gain, Let be the observation vector at time k.

[0075] Specifically, in step S3, the process noise covariance matrix of the extended Kalman filter... Covariance matrix of observation noise Calibration is performed based on the typical motion characteristics of ocean buoys, sensor measurement noise characteristics, and environmental interference levels.

[0076] Specifically, in step 4, when the solar geomagnetic vector is determined to be invalid, the system automatically switches to fault-tolerant processing mode and uses a gyroscope for attitude estimation.

[0077] Compared with the prior art, the present invention, employing the above technical solution, has the following technical effects:

[0078] 1. High accuracy: This invention obtains the optimal observation attitude through the Wahba problem and uses Kalman filtering to fuse high-frequency gyroscope data, effectively suppressing noise and compensating for gyroscope drift, resulting in small long-term attitude drift.

[0079] 2. Strong robustness: The method of this invention can still operate in a degraded manner when a certain sensor in the system fails, adapting to complex marine environments.

[0080] 3. Good real-time performance: The method of this invention has a moderate amount of computation and can run in real time on a float embedded processor. Attached Figure Description

[0081] Figure 1 This is a flowchart of the marine buoy attitude testing method of the present invention.

[0082] Figure 2 This is a top view of the external structure of the buoy in an embodiment of the present invention.

[0083] Figure 3 This is a cross-sectional view of the internal structure of the buoy in an embodiment of the present invention.

[0084] Figure 4 The diagram shows the simulated attitude estimation results of roll angle, pitch angle, and yaw angle in an embodiment of the present invention. Detailed Implementation

[0085] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of the application will be further described in detail below with reference to the accompanying drawings. The described embodiments are only a part of the embodiments involved in this invention. All non-innovative embodiments based on these embodiments by other researchers in the art are within the protection scope of this invention. Furthermore, the step numbers in the embodiments of this invention are only set for ease of explanation and do not limit the order of the steps. The execution order of each step in the embodiments can be adaptively adjusted according to the understanding of those skilled in the art.

[0086] In one embodiment of the present invention, taking a certain type of cylindrical marine meteorological buoy as an example, a marine buoy attitude measurement system for implementing the method of the present invention is provided, comprising:

[0087] A camera used to measure the solar vector in the carrier's coordinate system;

[0088] A triaxial magnetometer is used to measure the geomagnetic vector in the carrier coordinate system.

[0089] A three-axis MEMS gyroscope is used to measure the angular rate of a carrier.

[0090] The main control processor is configured to execute the steps of the method of the present invention;

[0091] The power module supplies power to the above components.

[0092] The specific hardware configuration is as follows:

[0093] Camera: A camera with 5 lenses featuring a wide field of view (±120°) (Hikvision MV-CS050-10GC), with a field of view exceeding 180°, such as... Figure 2 As shown. It includes: a first camera 1 mounted on the front of the buoy, a third camera 3 mounted on the rear of the buoy, a second camera 2 mounted on the left side of the buoy, a fourth camera 4 mounted on the right side of the buoy, and a fifth camera 5 mounted above the buoy. It is used to output the angle data of the solar vector in the sensor head coordinate system, which is converted into a vector using built-in calibration parameters. .

[0094] Main control unit 6: Installed below the buoy, such as Figure 3 As shown, a low-power embedded microprocessor (STM32H7 series) is used to run the attitude calculation and filtering algorithm.

[0095] Triaxial magnetometer: Employs an integrated temperature-compensated MEMS magnetometer (HMC5883L) to output magnetic field strength data in the carrier coordinate system, which is then corrected to obtain the geomagnetic vector. .

[0096] Three-axis MEMS gyroscope: A low-noise, low-drift gyroscope (BMI088) is selected to output angular rate measurements. .

[0097] Auxiliary units include a GPS / BeiDou module (used to obtain precise time and position for calculating reference vectors), a low-power data storage device, and a satellite communication module.

[0098] When installing all sensors, their sensitive axes should be aligned as closely as possible with the buoy carrier coordinate system (defined as: X-axis pointing to the buoy head, Y-axis pointing to the starboard side, and Z-axis pointing vertically downwards to form a right-hand system). Installation error calibration should be performed using a laboratory 3D turntable, and the calibration matrix should be stored in the non-volatile memory of the main control unit.

[0099] Furthermore, based on the above system, ocean buoy attitude testing is conducted, the process of which is as follows: Figure 1 As shown, after powering on, follow these steps:

[0100] S1: Constructing the reference vector and measurement vector

[0101] 1. System initialization:

[0102] Read sensor calibration parameters; initialize extended Kalman filter (EKF) state vector. Initialize the error covariance matrix. Process noise matrix and observation noise matrix .

[0103] 2. Data Acquisition and Preprocessing:

[0104] Obtain the current UTC time and buoy geographic coordinates (longitude and latitude) from GPS; simultaneously read the raw data from the magnetometer and gyroscope.

[0105] Geomagnetic reference vector calculation: Based on the geomagnetic model, the local geomagnetic field intensity components are obtained, and after normalization, the reference geomagnetic vector is obtained. .

[0106] Solar reference vector Calculation: The solar altitude angle in the geographic coordinate system is calculated using an ephemeris algorithm based on the time and geographic coordinates. and azimuth This leads to the reference solar vector:

[0107] ;

[0108] If the solar altitude angle If the angle is less than 0°, the solar vector is deemed invalid.

[0109] Solar measurement vector calculate:

[0110] 1) Image acquisition and preprocessing

[0111] The camera captures sky images at a fixed frame rate (e.g., 10Hz);

[0112] The original image is denoised using median filtering and then converted to grayscale.

[0113] An adaptive threshold segmentation algorithm is used to extract areas in the image whose brightness exceeds a set threshold and identify them as candidate sunspots.

[0114] If multiple highlighted areas exist, the area that best matches the characteristics of the sun is selected based on features such as area and shape.

[0115] If the sun's image point appears in multiple camera images, the calculation result of the camera whose image point is closest to the center of the image and is the most complete is selected as the output.

[0116] 2) Subpixel positioning at the solar center

[0117] For the selected solar spot regions, the coordinates of the solar center were calculated using the gray-scale centroid method. :

[0118] ;

[0119] in, For pixels grayscale value, Its pixel coordinates, This represents the total number of pixels within the light spot area.

[0120] 3) Transformation from image plane coordinates to camera coordinate system

[0121] Based on the camera intrinsic parameter matrix :

[0122] ;

[0123] in, The x-coordinate of the principal point The y-coordinate of the principal point The focal length is in the x-direction. The focal length is in the y-direction;

[0124] Convert pixel coordinates to coordinates in the normalized camera coordinate system. :

[0125] ;

[0126] Normalizing the obtained vector yields the vector of the sun in the camera coordinate system:

[0127] .

[0128] 4) Lens distortion correction

[0129] Extracted solar center pixel coordinates Convert to ideal coordinates in normalized camera coordinate system :

[0130] ;

[0131] Calculate the square of the radial distance :

[0132] ;

[0133] The distortion correction model is used to calculate the corrected normalized coordinates. :

[0134] ;

[0135] in, , The radial distortion coefficient is... , These are the tangential distortion coefficients, and these parameters are obtained through prior laboratory calibration.

[0136] Corrected normalized coordinates Remapping back to the pixel coordinate system yields the corrected solar center pixel coordinates. :

[0137] ;

[0138] After correction By substituting the image plane coordinates into the camera coordinate system as described in 3) above, the direction of the solar vector without distortion can be obtained, thereby significantly improving the measurement accuracy of the solar vector.

[0139] 5) Transformation from camera coordinate system to carrier coordinate system

[0140] When the camera is mounted on the buoy, there is a fixed rotational relationship between its coordinate system and the carrier coordinate system. A high-precision turntable is used, and calibration is obtained by measuring a solar simulator or external reference light source under multiple known directions. .

[0141] The solar vector in the camera coordinate system Transform to the carrier coordinate system:

[0142] .

[0143] 6) Validity judgment

[0144] When the sun cannot be identified in the image (e.g., at night, or when it is obscured by thick clouds) or the calculated solar altitude angle is below a threshold (e.g., below the horizon), the system will mark the solar vector as invalid.

[0145] S2: Solving the initial attitude based on the Wahba problem

[0146] Set weights and ( , These are the weighting coefficients corresponding to the camera and magnetometer, which can usually be set according to the sensor accuracy. If the solar vector fails (e.g., at night, due to obstruction), they can be set to... =0, if the geomagnetic vector fails (e.g., due to magnetic field interference), it can be set to 0. =0.

[0147] Calculate the matrix using the method described above. And construct the Davenport matrix. Call the eigenvalue decomposition function of the symmetric matrix to solve. Maximum eigenvalue And its corresponding unit eigenvector. This eigenvector is the optimal attitude quaternion representing the rotation from the geographic coordinate system to the vehicle coordinate system. .

[0148] S3: Construct an Extended Kalman Filter (EKF) for data fusion.

[0149] 1. State Definition

[0150] Define the system state vector as follows:

[0151] ;

[0152] in, A quaternion representing the attitude rotation from the geographic coordinate system to the carrier coordinate system; This is the zero bias of the three-axis gyroscope.

[0153] 2. State prediction model

[0154] The gyroscope measurement model is as follows:

[0155] ;

[0156] in, This refers to the angular rate output by the gyroscope. For the true angular velocity, For measuring noise.

[0157] The kinematic equations for quaternions are:

[0158] ;

[0159] The true angular rate is approximated by subtracting the current zero-bias estimate from the gyroscope measurement:

[0160] ;

[0161] Prior state quaternions:

[0162] .

[0163] The zero-biased dynamic model is modeled as a random walk process:

[0164] ;

[0165] in, This represents the change of zero bias over time. To drive the noise, the power spectral density is .

[0166] After discretization, it is represented as:

[0167]

[0168] 3. State equations and Jacobian matrix

[0169] Combining quaternions and zero biases into a state vector, the state equation can be written as:

[0170] ;

[0171] in, This refers to the angular rate output by the gyroscope. For process noise, the covariance matrix is: .

[0172] For EKF, linearization is required to obtain the state transition matrix. :

[0173] ;

[0174] in, Jacobi updated for quaternions; The effect of zero bias error on quaternion updates.

[0175] The process noise covariance matrix is:

[0176]

[0177] in, It is the process noise covariance matrix of the quaternion state.

[0178] 4. Construct measurement equations

[0179] The observation equation is:

[0180]

[0181] in, To observe the noise, Let k be the observation vector at time k; observation matrix for:

[0182] ;

[0183] This indicates that the observation is only related to the attitude quaternion and is independent of the zero bias.

[0184] 5. Filtering recursion

[0185] The prediction process is represented as follows:

[0186] ;

[0187] ;

[0188] The update process is represented as follows:

[0189] ;

[0190] ;

[0191] ;

[0192] Among them, the process noise covariance matrix Covariance matrix of observation noise Calibration is performed based on the typical motion characteristics of ocean buoys, sensor measurement noise characteristics, and environmental interference levels.

[0193] 6. Attitude Output and Application

[0194] The optimal estimated quaternion Converted into three-axis attitude angles, and output via satellite communication or local storage.

[0195] S4: Fault Tolerance

[0196] When the solar geomagnetic vector is determined to be invalid, the system automatically switches to fault-tolerant processing mode and uses a gyroscope for attitude estimation.

[0197] In summary, this invention designs a method for testing the attitude of marine buoys based on a solar geomagnetic dual-vector gyroscope. Its superior performance is verified through MATLAB simulation, with calculated roll angle error of ±0.6°, pitch angle error of ±0.6°, and yaw angle error of ±0.8° under steady-state conditions. Figure 4 As shown.

[0198] The above description is only a preferred embodiment of the present invention. It should be noted that for those skilled in the art, several improvements and modifications can be made 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 method for testing the attitude of an ocean buoy based on a solar geomagnetic dual-vector assisted gyroscope, characterized in that, Includes the following steps: S1. Obtain the solar reference vector in the geographic coordinate system. With the geomagnetic reference vector The corresponding solar measurement vectors are obtained through cameras and magnetometers on ocean buoys. With geomagnetic measurement vector ; S2. Solve for the initial attitude based on the Wahba problem. Construct a weighted outer product matrix using the reference vectors and measurement vectors of the sun and geomagnetism. And thus construct the Davenport matrix. By solving its eigenvectors, we obtain attitude quaternion observations that characterize the rotation from the geographic coordinate system to the carrier coordinate system. S3. Using attitude quaternions and gyroscope zero bias as state vectors, establish a state prediction model. The state equations include equations for updating attitude quaternions based on angular rates after subtracting estimated zero bias and noise from gyroscope measurements. A covariance matrix is ​​also established considering the influence of process noise. ; An extended Kalman filter is designed, using the attitude quaternion observations as the filtered observations. An observation equation is constructed, and a filtering recursion is performed. The covariance matrix is ​​established considering the influence of observation noise. The optimal attitude quaternion of the ocean buoy and the gyroscope zero bias are estimated and output in real time by fusing the gyroscope angular rate measurement value. S4. When the solar geomagnetic vector is determined to be invalid, automatically switch to fault-tolerant processing mode and use the gyroscope to perform recursive attitude estimation.

2. The method for testing the attitude of a marine buoy according to claim 1, characterized in that, In step S1, the ocean buoy is equipped with a camera array and a main control chip; The camera array includes five cameras mounted on the front, rear, left, right, and top of the buoy. These cameras are used to acquire sky images, identify solar flares, extract the center pixel coordinates using the gray-scale centroid method, and calculate the solar vector in the carrier coordinate system by combining camera intrinsic parameters and the mounting matrix. ; The main control chip is equipped with a magnetometer and a gyroscope, used to collect data from the magnetometer and gyroscope to obtain the geomagnetic vector in the body coordinate system. .

3. The method for testing the attitude of a marine buoy according to claim 1, characterized in that, In step S2, the weighted outer product sum matrix , is represented as: ； Wherein, the superscript T indicates transpose. , These are the weighting coefficients for the camera and magnetometer; if the solar vector fails, let... =0; if the geomagnetic vector fails, assume =0.

4. The method for testing the attitude of a marine buoy according to claim 3, characterized in that, In step S2, the Davenport matrix is ​​constructed. , is represented as: ； ； ； ； in, , , For the intermediate matrix, For matrix The elements in It is a 3×3 identity matrix; By calculating the Davenport matrix Maximum eigenvalue The quaternion, along with its corresponding unit eigenvector, represents the optimal attitude quaternion for rotation from the geographic coordinate system to the carrier coordinate system. .

5. The method for testing the attitude of a marine buoy according to claim 4, characterized in that, In step S3, the state vector is represented as: ； in, A quaternion representing the attitude rotation from the geographic coordinate system to the carrier coordinate system; This is the zero bias of the three-axis gyroscope.

6. The method for testing the attitude of a marine buoy according to claim 5, characterized in that, In step S3, the state prediction model is constructed as follows: Define the gyroscope measurement model: ； in, This refers to the angular rate output by the gyroscope. For the true angular velocity, For measuring noise. Quaternion kinematic equations , is represented as: ； The true angular rate is approximated by subtracting the current zero-bias estimate from the gyroscope measurement. : ； The zero-biased dynamic model is modeled as a random walk process: ； in, This represents the change of zero bias over time. To drive the noise, the power spectral density is .

7. The method for testing the attitude of a marine buoy according to claim 5, characterized in that, In step S3, the state equation is expressed as: ； in, For process noise, The process noise covariance matrix is... This refers to the angular rate output by the gyroscope. , Let be the state vectors at time k and time k+1; For the extended Kalman filter, the state transition matrix is ​​obtained through linearization. : ； in, Jacobi updated for quaternions; The effect of zero bias error on quaternion updates.

8. The method for testing the attitude of a marine buoy according to claim 7, characterized in that, In step S3, the observation equation is expressed as: ； in, To observe the noise, Let k be the observation vector at time k. This is the observation matrix.

9. The method for testing the attitude of a marine buoy according to claim 7, characterized in that, In step S3, the filtering recursion and prediction process is as follows: ； ； in, For the posterior state estimate at time k-1, For the prior state estimate at time t, Let be the prior covariance matrix at time k. The function is a nonlinear state transition function. The state estimation error covariance matrix; The update process is as follows: ； ； ； in, To observe the noise covariance matrix, For Kalman gain, Let be the observation vector at time k.

10. The method for testing the attitude of a marine buoy according to claim 1, characterized in that, In step S3, the process noise covariance matrix of the extended Kalman filter... Covariance matrix of observation noise The calibration is performed based on the typical motion characteristics of ocean buoys, sensor measurement noise characteristics, and environmental interference levels.

Images