True wind fusion algorithm and unmanned sailboat control system based on platform attitude correction

Abstract

The present invention provides a true wind fusion algorithm based on platform attitude correction and an unmanned sailboat control system. The algorithm converts the coordinate system of the ship-borne wind sensor into the earth coordinate system, performs filtering processing on the relative wind speed, relative wind direction, ship speed wind speed, and ship speed wind direction, and uses the filtered data to perform true wind speed fusion processing. The relative wind is corrected to the state when the sailboat is in a horizontal attitude by using the idea of ​​rotating coordinate conversion. Compared with other wind speed and wind direction compensation algorithms, the present invention does not rely on sensors or other hardware adjustments, making the unmanned sailboat sail control system simpler and more convenient.

Description

Technical Field

[0001] The present invention relates to the field of marine engineering technology, and in particular to a true wind fusion algorithm based on platform attitude correction and an unmanned sailboat control system. Background Art

[0002] For unmanned sailboat mobile observation platforms used in the ocean, wind is the power source of the unmanned sailboat. The most critical part of using wind is to be able to respond in time and make corresponding control actions according to different true wind directions. However, the true wind cannot be directly measured on unmanned sailboats and other moving platforms, and can only be obtained indirectly through vector model calculation. During the control process of unmanned sailboats, the wind direction and wind speed sensors are used to sense and detect wind direction and wind force data.

[0003] However, during the actual sailing process, the unmanned sailboat will experience bow pitch, roll and pitch movements, and the spatial position of the wind direction and wind speed sensor installed on the sailboat will change accordingly. The measurement accuracy of the ultrasonic wind direction and wind speed sensor is greatly affected by the platform posture. Therefore, there is an error between the measured value and the true value, which cannot meet the accuracy requirement of the unmanned sailboat control that uses wind direction and wind speed as control input, which will cause relatively large problems in the sail control of the sailboat and the control effect of the unmanned sailboat is not ideal.

[0004] In order to obtain a more accurate control effect of the unmanned sailboat, the wind direction and wind speed data detected by the wind need to be corrected and compensated.

[0005] In the prior art, the methods for compensating for wind direction and wind speed mainly include the following: (1) The motion compensation system of the wind direction and wind speed sensor on the mobile platform mainly compensates for the wind speed, but cannot compensate for the wind direction at the same time. (2) Although some algorithms have proposed analysis and compensation for wind speed measurement errors under the swaying state of the ship, they cannot fundamentally solve the problem of the influence of the platform posture on the wind direction and wind speed. Summary of the invention

[0006] The purpose of the present invention is to solve the above technical problems and provide a true wind fusion algorithm based on platform attitude correction and an unmanned sailboat control system, so as to compensate for the detected ocean wind and obtain data closer to the true wind for the control of the unmanned sailboat to obtain better control effect.

[0007] To achieve the above object, the technical solution adopted by the present invention is:

[0008] A true wind fusion algorithm based on platform attitude correction, the platform is equipped with a wind sensor, the wind direction detected by the wind sensor is defined as a relative wind direction, and the detected wind speed is defined as a relative wind speed. The fusion algorithm includes the following steps:

[0009] Coordinate transformation steps: Convert the shipborne wind sensor coordinate system to the geodetic coordinate system:

[0010]

[0011] Where: P is the vector in the earth coordinate system, P' is the wind vector measured by the wind sensor in the shipborne coordinate system, and R is the rotation orthogonal matrix;

[0012] in:

[0013]

[0014] α is the roll angle of the ship platform, β is the pitch angle of the ship platform, and γ is the bow angle of the ship platform;

[0015] The corrected wind vector expression is:

[0016] P=[V X ,V Y ,0];

[0017] Where: V X is the component of relative wind speed along the X axis after correction, V Y is the component of relative wind speed along the Y axis after correction;

[0018] The relative wind speed after correction is:

[0019]

[0020] The relative wind direction after correction is:

[0021]

[0022] True wind calculation steps: Calculate the true wind direction and true wind speed based on the corrected relative wind direction and the corrected relative wind speed:

[0023]

[0024]

[0025] Where: AWD is the relative wind direction after correction, V AWS is the corrected relative wind speed, T WD为 Calculated true wind direction, V TWS is the calculated gust wind speed, SWD is the ship speed and wind direction, V SWS is the ship speed and wind speed.

[0026] In some embodiments of the present invention, a wind speed filtering and speed fusion step is further included, which is used to filter the relative wind speed, relative wind direction, ship speed wind speed, and ship speed wind direction, and use the filtered relative wind speed, relative wind direction, ship speed wind speed, and ship speed wind direction to establish a speed fusion model:

[0027] The mathematical model of true wind speed fusion is established, and the state equation of the true wind speed fusion system is:

[0028] X(k+1)=Φ(k)X(k)+w(k);

[0029] Where: X(k) is the velocity matrix; φ(k) is the state matrix, w(k) is the process noise;

[0030] Take: x1 = V sws ,x2=V Aws ;

[0031] Pick:

[0032] Then we have:

[0033]

[0034] The observation equation of the true wind speed fusion system is:

[0035] Z(k)=h[k,X(k)]+V(k).

[0036] Where: V(k) is the observation noise, h[k,X(k)] is the nonlinear function of the state variable X.

[0037] The true wind speed calculation model is used as the observation model of the true wind speed fusion system:

[0038]

[0039] Where: V AWS is the relative wind speed after correction, V SWS is the ship speed and wind speed, AWD is the relative wind direction after correction, SWD is the ship speed and wind direction, V TWS True wind speed after correction;

[0040] For the linearized model, the extended Kalman filter recursive equation can be obtained according to the Kalman basic equation:

[0041]

[0042] P(k+1|k)=Φ(k+1|k)P(k|k)Φ T (k+1|k)+Q(k)

[0043] K(k+1)=P(k+1|k)HT (k+1)(H(k+1)P(k+1|k)H T (k+1)+R(k+1)) -1

[0044]

[0045] P(k+1)=[IK(k+1)H(k+1)]P(k+1|k)

[0046] in: represents the predicted true wind value at time k; The nonlinear function P(k+1|k) representing the optimal estimate at time k represents the prediction covariance at time k; P(k|k) represents the optimal estimate covariance at time k; φ(k+1|k) represents the state transfer matrix; Q(k) represents the variance of process noise; K(k+1) represents the Kalman filter gain; I represents the identity matrix; H(k+1) represents; the observation matrix R(k+1) represents the observation noise variance; Z(k+1) represents the true wind observation value at time k+1;

[0047] It is the product of the observation matrix and the true wind forecast value at time k;

[0048] The extended Kalman filter is used to filter and estimate the true wind speed, and the Jacobian matrix of the true wind observation equation is obtained:

[0049]

[0050] The true wind speed after filtering is:

[0051]

[0052]

[0053] In some embodiments of the present invention, an unmanned sailboat control system is further provided, which adopts the true wind fusion algorithm based on platform attitude correction described above.

[0054] The true wind fusion algorithm based on platform attitude correction and the unmanned sailboat control system provided by the present invention have the following beneficial effects:

[0055] 1. For the measurement error caused by the change of the sailboat's attitude and position, the present invention uses the idea of ​​rotating coordinate conversion to correct the relative wind to the state when the sailboat is in a horizontal attitude. Compared with other wind speed and direction compensation algorithms, the present invention does not rely on sensors or other hardware adjustments, making the unmanned sailboat sail control system simpler and more convenient.

[0056] 2. True wind data plays an important role in the path planning and autonomous navigation of unmanned sailboats. The present invention performs Kalman filtering on relative wind speed and direction, which greatly reduces the error.

[0057] 3. Based on the correction of relative wind direction and wind speed, the true wind data is integrated with the sailing wind speed data, and the error between the true wind data and the original true wind data is smaller, with higher precision and accuracy.

[0058] 4. Realize the measurement of true wind direction and wind speed, and provide better control input effect for the sail control system of unmanned sailboat. BRIEF DESCRIPTION OF THE DRAWINGS

[0059] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

[0060] Figure 1 Schematic diagram of wind generated by sailboat speed, relative wind speed, and true wind speed;

[0061] Figure 2 It is a schematic diagram of the coordinate system structure;

[0062] Figure 3 It is a schematic diagram of the vector transformation structure;

[0063] Figure 4 The figure is a logic flow chart of the method of the present invention. DETAILED DESCRIPTION

[0064] In order to make the technical problems, technical solutions and beneficial effects to be solved by the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0065] The present invention provides a true wind fusion algorithm based on platform attitude correction and an unmanned sailboat control system, which can process the wind direction and wind speed detected by the sensors carried by the unmanned sailboat platform, convert the detected wind into true wind, and use it for controlling the unmanned sailboat, so as to achieve a more accurate unmanned sailboat control effect.

[0066] Firstly, the basic principles of wind speed and direction detection of unmanned sailboat are explained.

[0067] The unmanned sailboat's perception of wind comes from the wind sensor on board the sailboat, which can detect wind speed and wind direction data. When the sailboat is sailing steadily, the wind sensed by the onboard wind sensor on board is the relative wind (or apparent wind speed), which incorporates the impact of the sailboat's six-degree-of-freedom motion on the wind.

[0068] The relative wind direction is recorded as AWD (Apparent Wind Direction), and the relative wind speed is recorded as V Aws The real wind in nature is the true wind, and its direction is recorded as TWD (True Wind Direction), and the wind speed is recorded as V Tws (True Wind Speed), the wind generated by the speed of a sailboat on the sea is equal to the speed of the ship, and its direction is opposite to the speed of the ship. The direction of the wind is recorded as SWD (SailBoatWindDirection), and the wind speed (sailing wind speed) is recorded as V Sws (SailBoat Wind Speed). Figure 1 The speed triangle shows the wind generated by the sailboat speed, the relative wind speed, and the true wind speed. When the ship speed is known, if one of the relative wind and the true wind is known, the other can be calculated from the speed triangle.

[0069] Establish a rectangular coordinate system on the geodetic plane. The wind direction and heading are both based on the true north of the earth as 0°, and the clockwise rotation angle increases to 360°, as shown in the figure above. The target is V Tws When controlling the sail of an unmanned sailboat, the input of its controller is the direction of the true wind, so the magnitude and direction of the true wind can be calculated by the cosine theorem.

[0070]

[0071]

[0072] Based on the above principles, the true wind fusion algorithm based on platform attitude correction provided by the first embodiment of the present invention mainly includes the following steps.

[0073] (1) Steps for sailboat attitude correction

[0074] Convert the shipborne wind sensor coordinate system to the geodetic coordinate system:

[0075]

[0076] Where: P is the vector in the earth coordinate system, P' is the wind vector measured by the wind sensor in the shipborne coordinate system, and R is the rotation orthogonal matrix.

[0077] The above coordinate transformation is performed based on the space rotation coordinate system, and the specific transformation principle is as follows.

[0078] The spatial rotation coordinate system is about the transformation between two coordinate systems.

[0079] refer to Figure 2 , where the coordinate system O-XYZ is defined as the coordinate system before the sailboat's attitude changes, and this coordinate system is the same as the earth's coordinate system; the O'-X'Y'Z' coordinate system is defined as the coordinate system after the sailboat's attitude changes, and the change of this coordinate system is affected by the six-degree-of-freedom motion of the ship, and the change coordinate system formed by the movement of the ship.

[0080] In the O-XYZ coordinate system, OX points to the bow direction, OY is perpendicular to the port side of the sailboat and points outward, the O-XY plane is at sea level, and OZ is perpendicular to the O-XY plane and points in the opposite direction of the center of the earth.

[0081] The wind direction and speed sensor is installed on the sailboat. The movement of the sailboat will change the spatial position of the wind sensor, resulting in measurement errors. The changes in the attitude and position of the sailboat include the bowing motion, the rolling motion and the pitching motion. The motion rotating around OX is the rolling motion, and the rotation angle is the roll angle Roll, recorded as α; the motion rotating around OY is the pitching motion, and the rotation angle is the pitch angle Pitch, recorded as β; the motion rotating around OZ is the bowing motion, and the rotation angle is the bowing angle Yaw, recorded as γ; For the measurement error caused by the change of the attitude and position of the sailboat, the idea of ​​rotating coordinate transformation can be used to correct the relative wind to the state when the sailboat is in a horizontal attitude.

[0082] The purpose of this step is to transform the vector in the earth coordinate system into the shipborne coordinate system affected by the six-degree-of-freedom motion.

[0083] For any vector P = (x, y, z) in the earth coordinate system, the rotation matrix R can be used to obtain P' = (x', y', z') in the ship coordinate system, that is, P = RP', where R is the rotation orthogonal matrix, that is, R T =R -1 The calculation of the rotation matrix R can be associated with the rotation of a vector in a two-dimensional plane, as shown in the figure, where the vector

[0084] Where θ is the angle between the vectors in the geodetic coordinate system and the rotating coordinate system, i represents the imaginary and real units, represents the algebraic representation of the wind vector before correction, Represents the algebraic representation of a rotation orthogonal matrix.

[0085] Therefore, there is the following vector conversion relationship:

[0086]

[0087] For a rotating coordinate system, if you rotate around that axis, the corresponding axis will not change. Therefore, the transformation formula for rotating around the Z axis in a three-dimensional coordinate system can be deduced as follows:

[0088]

[0089] Where R z is the rotation matrix when rotating around the Z axis. Similarly, it can be deduced that the rotation matrix when rotating around X and Y is:

[0090]

[0091]

[0092] Then the rotation matrix from the unmanned sailboat coordinate system to the earth coordinate system is:

[0093]

[0094] Therefore, the coordinate system transformation relationship mentioned above can be obtained.

[0095]

[0096] Then, the expression of the wind vector before correction is:

[0097] P'=[V' AWS ·sin(AWD'),V' AWS ·cos(AWD'),0];

[0098] Where: V' AWS is the wind speed measured by the shipborne wind sensor, and ADW' is the wind direction measured by the shipborne sensor.

[0099] The corrected relative wind vector expression is:

[0100] P=[V X ,V Y ,0];

[0101] Where: V X is the component of wind speed along the X axis after correction, V Y is the component of wind speed along the Y axis after correction;

[0102] The corrected relative wind speed V AWD , and the wind direction AWD of the relative wind after correction are:

[0103]

[0104]

[0105] Calculate the true wind direction and true wind speed based on the corrected relative wind direction and corrected relative wind speed:

[0106]

[0107]

[0108] (2) Wind direction filtering steps.

[0109] The wind direction filtering algorithm adopts the Kalman filtering principle. The following first introduces the working principle of the Kalman filtering principle in discrete nonlinear systems.

[0110] The dynamic equation of the discrete nonlinear system can be described as:

[0111]

[0112] Where X(k) is the state vector, Z(k) is the observation vector, f(·) and h(·) are the nonlinear state function and observation function of the system, W(k) and V(k) are the process noise and observation noise, respectively. They are both assumed to be Gaussian white noise with zero mean and are independent sequences.

[0113] The extended Kalman filter uses the local linear characteristics of nonlinear functions to locally linearize the nonlinear model. According to the above system state equation, the nonlinear state function f(·) is localized at the filter value. Doing a first-order Taylor expansion, we can get

[0114]

[0115] make:

[0116]

[0117]

[0118] Then the state equation is:

[0119] X(k+1)=Φ(k+1|k)X(k)+W(k)+φ(k)

[0120] According to the observation equation of the system, the nonlinear state function h(·) is transformed into Doing a first-order Taylor expansion, we can get

[0121]

[0122] make:

[0123]

[0124]

[0125] Then the observation equation is:

[0126] Z(k)=H(k)X(k)+y(k)+V(k)

[0127] For the linearized model, the extended Kalman filter recursive equation can be obtained according to the Kalman basic equation:

[0128]

[0129] P(k+1|k)=Φ(k+1|k)P(k|k)Φ T (k+1|k)+Q(k)

[0130] K(k+1)=P(k+1|k)H T (k+1)(H(k+1)P(k+1|k)H T (k+1)+R(k+1)) *1

[0131]

[0132] P(k+1)=[I*K(k+1)H(k+1)]P(k+1|k)

[0133] Compared with the basic equation of Kalman filtering, in the linearized system equation, the state transfer matrix Φ(k+1|k) and the observation matrix H(k+1) are replaced by the Jacobian matrices of f and h. Assume that the state variable has n dimensions, that is, X = [x1 x2 … x n ] T , then the corresponding Jacobian matrix is:

[0134]

[0135]

[0136] It further includes wind speed filtering and speed fusion steps, which are used to filter the relative wind speed, relative wind direction, ship speed wind speed, and ship speed wind direction, and use the filtered data to perform true wind speed fusion processing. The above extended Kalman filter algorithm is used to filter the true wind observation equation.

[0137] The mathematical model of true wind speed fusion is established, and the state equation of the true wind speed fusion system is:

[0138] X(k+1)=Φ(k)X(k)+w(k);

[0139] Among them: X(k) is the velocity matrix; φ(k) is the state matrix, and w(k) is the process noise. Process noise is a comprehensive and abstract description of uncertainty and random disturbances in the model building process, which can also be called estimation error.

[0140] Take: x1 = V sws ,x2=V Aws .

[0141] Pick:

[0142] Then we have:

[0143]

[0144] The system observation equation is:

[0145] Z(k)=h[k,X(k)]+V(k)

[0146] Where: V(k) is the observation noise, which is the noise of the wind speed and direction detection process, and h[k,X(k)] is the nonlinear function of the state variable X.

[0147] The true wind calculation model is used as the observation model of the true wind speed fusion system:

[0148]

[0149] Where: V AWS V is the relative wind speed monitored by the sensor after correction. SWS is the ship speed and wind speed, AWD is the relative wind direction monitored by the sensor after correction (corresponding to the direction of the P vector), SWD is the ship speed and wind direction, V TWS The true wind speed after correction.

[0150] For the linearized model, the extended Kalman filter recursive equation can be obtained according to the Kalman basic equation:

[0151]

[0152] P(k+1|k)=Φ(k+1|k)P(k|k)Φ T (k+1|k)+Q(k)

[0153] K(k+1)=P(k+1|k)H T (k+1)(H(k+1)P(k+1|k)H T (k+1)+R(k+1)) -1

[0154]

[0155] P(k+1)=[IK(k+1)H(k+1)]P(k+1|k)

[0156] in: represents the predicted true wind value at time k; represents the optimal estimate of the true wind at time k+1; The nonlinear function representing the optimal estimate at time k is: P(k+1|k) represents the prediction covariance at time k; P(k|k+1) represents the prediction covariance at time k; P(k|k) represents the optimal estimate covariance at time k; φ(k+1|k) represents the state transfer matrix; Q(k) represents the variance of the process noise; K(k+1) represents the Kalman filter gain; I represents the identity matrix; H(k+1) represents the observation matrix; H T (k+1) represents the transposed matrix of H(k+1); R(k+1) represents the observation noise variance; Z(k+1) represents the true wind observation value at time k+1;

[0157] It is the product of the observation matrix and the predicted value at time k.

[0158] The state equation of the system is linear, and the solution of Φ(k) does not need to be linearized, but the system observation equation is nonlinear. Therefore, the calculation of the true wind speed is a nonlinear estimation problem, and the extended Kalman filter can be used for filtering estimation.

[0159] The linearization method is used to linearize the true wind observation equation and calculate the Jacobian matrix of the true wind observation equation:

[0160]

[0161] The true wind speed after filtering is:

[0162]

[0163]

[0164] The wind direction calculation method after filtering is:

[0165]

[0166] The true wind speed and true wind direction after filtering are used to control the unmanned sailboat.

[0167] The second embodiment of the present invention further provides an unmanned sailboat control system, the control system includes a processor, the processor is equipped with the true wind fusion algorithm based on platform attitude correction described in the first embodiment. The unmanned sailboat control system can realize the control of unmanned sailboat path planning, autonomous navigation, etc., and provides a control input with higher precision and accuracy for the unmanned sailboat sail control system, thereby improving the control accuracy.

[0168] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A true wind fusion algorithm based on platform attitude correction, characterized in that: The platform is equipped with a wind sensor, the wind direction detected by the wind sensor is defined as the relative wind direction, and the detected wind speed is defined as the relative wind speed. The fusion algorithm includes the following steps: Coordinate transformation steps: Convert the shipborne wind sensor coordinate system to the geodetic coordinate system: Where: P is the vector in the earth coordinate system, P' is the wind vector measured by the wind sensor in the shipborne coordinate system, and R is the rotation orthogonal matrix; in: α is the roll angle of the ship platform, β is the pitch angle of the ship platform, and γ is the bow angle of the ship platform; The corrected wind vector expression is: P=[V X ,V Y ,0]; Where: V X is the component of relative wind speed along the X axis after correction, V Y is the component of relative wind speed along the Y axis after correction; The relative wind speed after correction is: The relative wind direction after correction is: True wind calculation steps: Calculate the true wind direction and true wind speed based on the corrected relative wind direction and the corrected relative wind speed: Where: AWD is the relative wind direction after correction, V AWS is the relative wind speed after correction, TWD is the calculated true wind direction, V TWS is the calculated gust wind speed, SWD is the ship speed and wind direction, V SWS is the ship speed and wind speed; It further includes wind speed filtering and speed fusion steps, which are used to filter the relative wind speed, relative wind direction, ship speed and wind speed, and ship speed and wind direction, and use the filtered data to perform true wind speed fusion processing: The mathematical model of true wind speed fusion is established, and the state equation of the true wind speed fusion system is: X(k+1)=Φ(k)X(k)+w(k); Where: X(k) is the velocity matrix; φ(k) is the state matrix, w(k) is the process noise; Take: x1 = V SWS ,x2=V AWS ; Pick: Then we have: The system observation equation is: Z(k)=h[k,X(k)]+V(k) Where: V(k) is the observation noise, h[k,X(k)] is the nonlinear function of the state variable X; The true wind speed calculation model is used as the observation model of the true wind speed fusion system: The observation equation of the true wind speed fusion system is: Where: V AWS is the relative wind speed after correction, V SWS is the ship speed and wind speed, AWD is the relative wind direction after correction, SWD is the ship speed and wind direction, V TWS True wind speed after correction; For the linearized model, the extended Kalman filter recursive equation can be obtained according to the Kalman basic equation: P(k+1|k)=Φ(k+1|k)P(k|k)Φ T (k+1|k)+Q(k) K(k+1)=P(k+1|k)H T (k+1)(H(k+1)P(k+1|k)H T (k+1)+R(k+1)) -1 P(k+1)=[IK(k+1)H(k+1)]P(k+1|k) in: represents the predicted true wind value at time k; represents the optimal estimate of the true wind at time k+1; The nonlinear function representing the optimal estimate at time k is P(k+1|k), which represents the prediction covariance at time k; P(k|k) represents the optimal estimate covariance at time k; φ(k+1|k) represents the state transfer matrix; Q(k) represents the variance of the process noise; K(k+1) represents the Kalman filter gain; I represents the identity matrix; H(k+1) represents the observation matrix; H T (k+1) represents the transposed matrix of H(k+1); R(k+1) represents the observation noise variance; Z(k+1) represents the true wind observation value at time k+1; It is the product of the observation matrix and the true wind forecast value at time k; The extended Kalman filter is used to filter and estimate the true wind speed, and the Jacobian matrix of the true wind observation equation is obtained: The true wind speed after filtering is:

2. An unmanned sailboat control system, characterized in that: Including the true wind fusion algorithm described in claim 1.

Images