High-precision fusion method of displacement and attitude of unmanned watercraft based on adaptive Kalman filter

Abstract

The present invention discloses a high-precision fusion method for unmanned vehicle displacement attitude based on adaptive Kalman filtering, belonging to the field of ship perception and positioning technology. The method comprises: constructing a discrete-time state-space model of the navigation data state using discrete integration based on the navigation data state; obtaining the system state equation using the rotation matrix and the median theorem; obtaining the observation transfer matrix using coordinate system information, the length of the geodetic line between two points, and the azimuth angle, and then obtaining the state observation equation in combination with noise data; obtaining the unmanned vehicle displacement attitude information based on the system state equation and the state observation equation using an adaptive Kalman filtering algorithm; detecting the unmanned vehicle predicted data using a sliding window method; and correcting the predicted data by substituting the Kalman filter when an anomaly is detected. The present invention improves the accuracy, reliability, and robustness of the unmanned vehicle displacement attitude information acquisition through state modeling, fusion algorithm, anomaly detection, and anomaly correction.

Description

Technical Field

[0001] The present invention belongs to the technical field of ship perception and positioning, and particularly relates to a high-precision fusion method for the displacement posture of an unmanned boat based on adaptive Kalman filtering. Background Art

[0002] With rapid technological advancements, unmanned surface vessels (UAVs) are becoming increasingly widely used in fields such as scientific research, environmental protection, and rescue operations. With their high degree of automation, efficiency, and flexibility, UAVs are gradually replacing traditional manned vessels, enabling them to perform a variety of complex and dangerous tasks. For example, in scientific research, UAVs can be used to collect ocean data and monitor the environment. In environmental protection, they can conduct water quality testing and track pollution sources. In rescue missions, they undertake search and rescue operations and transport supplies, with autonomous docking and undocking being a key technology.

[0003] The docking and undocking process for an unmanned vehicle is typically divided into two phases. Taking docking as an example, the first phase is from the mission start point to the berthing point. Once the unmanned vehicle reaches the mission start point, it typically slows down and begins entering the narrow waters. During this phase, the unmanned vehicle's control system's primary task is to control the unmanned vehicle to avoid various static and dynamic obstacles in the narrow waters. The second phase is after the unmanned vehicle reaches the berthing point and begins berthing. During this phase, the unmanned vehicle's speed is further reduced, and its navigational mission no longer requires obstacle avoidance. Since the berth is typically only slightly larger than the unmanned vehicle, the primary navigational task during this phase is to safely enter the berth and berth. Although the primary navigational tasks vary during both phases, accurate displacement and attitude data of the unmanned vehicle are required to support the unmanned vehicle control module in making timely and appropriate control decisions.

[0004] Currently, the mainstream method for acquiring displacement and attitude information is generally based on data fusion between GPS and IMU. The most widely used multi-source sensor information fusion algorithm is the Kalman filter series. However, traditional Kalman filter series algorithms are mostly designed based on the Gaussian noise assumption. They are often severely interfered with by the non-Gaussian noise interference generated by the complex hydrological environment in which unmanned boats operate autonomously. As a result, the accuracy and stability of the final fused data are unable to meet the requirements of autonomous docking and undocking missions.

[0005] Therefore, there is an urgent need to propose a more accurate and robust method for obtaining high-precision displacement posture of unmanned boats to solve the technical problem of low accuracy and stability of GPS and IMU data fusion results under the influence of complex environmental interference such as wind, waves and currents in the existing technology. The present invention is proposed to solve this problem. Summary of the Invention

[0006] In view of the shortcomings of the background technology, the purpose of the present invention is to propose a high-precision fusion method for the displacement and attitude of an unmanned boat based on adaptive Kalman filtering. The system state equation and state observation equation are constructed based on GPS and IMU, and the displacement and attitude information of the unmanned boat is obtained based on adaptive Kalman filtering. At the same time, an auxiliary measurement noise covariance matrix is ​​set, and the variational Bayesian algorithm, Wasserstein distance and sliding window method are combined to detect and correct anomalies. The method solves the current problems of inaccurate acquisition of the displacement and attitude information of the unmanned boat and deviation caused by imperfect measurement noise processing.

[0007] The technical solution adopted by the present invention is:

[0008] A high-precision fusion method for displacement and attitude of an unmanned vehicle based on adaptive Kalman filtering includes the following steps:

[0009] S1) Construct the system state equation and state observation equation of the unmanned boat based on GPS and IMU;

[0010] Sa1) Based on GPS and IMU, the navigation data state, acceleration, and attitude angle change rate are acquired, and a discrete-time state-space model of the navigation data state is constructed using discrete integration. The system state equation is derived by converting the data into a north-east navigation coordinate system using a rotation matrix and updating the position using the mean value theorem.

[0011] Sa2) The observation transfer matrix is ​​derived from the coordinate system information obtained by GPS, the length of the geodetic line between two points, and the azimuth angle. The observation transfer matrix is ​​used to convert the autonomous vehicle's own rectangular coordinate system into the northeast sky navigation coordinate system. The state observation equation is then obtained by combining the noise data.

[0012] S2) Acquire the displacement and attitude information of the unmanned boat based on the adaptive Kalman filter, use the sliding window method to detect, and use statistical methods to correct outliers;

[0013] Sb1) Based on the system state equation and state observation equation, the adaptive Kalman filter algorithm is used to predict the state of the UAV at the next moment and obtain the displacement and posture information of the UAV;

[0014] Sb2) Setting the auxiliary measurement noise covariance matrix, calculating the fixed value of the measurement noise covariance matrix using the variational Bayesian algorithm, quantifying the similarity of the covariance matrix using the Wasserstein distance based on the estimated value of the measurement noise covariance matrix, and detecting the unmanned vehicle prediction data using the sliding window method;

[0015] Sb3) When anomalies in the predicted data are detected, the statistical similarity measurement is used to correct the outliers, the optimal solution is obtained by maximizing the distance, and the Kalman filter is used to correct the measurement noise covariance matrix.

[0016] Preferably, in step Sa1), the specific process of obtaining the system state equation is as follows: ① Under the multi-source information fusion framework, obtain the navigation data state through GPS, and use the inertial measurement unit to directly measure the acceleration of the unmanned boat and attitude angle change rate , and the discrete time state space model of each navigation data state of the unmanned boat is calculated using the discrete integral formula. The navigation data state includes the position of the unmanned boat ,speed and attitude angle , the following formula is expressed as:

[0017] (1)

[0018] (2)

[0019] (3)

[0020] in, express The position of the unmanned boat at all times, express The position of the unmanned boat at all times, Indicates the time interval, express Time speed, express Time acceleration; express Time speed, express Momentary attitude angle, express Momentary attitude angle, express The attitude angle change rate at all times; the position, velocity, and attitude angle correspond to the three dimensional components of the x, y, and z directions in three-dimensional space, and are integrated with the navigation data state into a nine-dimensional vector , the specific formula is as follows:

[0021] (4)

[0022] in 、 、 Indicates the position of the unmanned boat in the northeast navigation coordinate system, 、 、 It represents the speed of the unmanned boat in the northeast navigation coordinate system, 、 、 It represents the attitude angle of the unmanned boat in the northeast navigation coordinate system. Represents the transposition operation; ② With the help of the heading change of the unmanned boat, the inertial coordinate system is rotated, and the rotation matrix from the inertial coordinate system to the northeast sky navigation coordinate system is constructed to obtain the attitude of the northeast sky navigation coordinate system. The differential equation of the rotation matrix with respect to time is as follows:

[0023] (5)

[0024] In the formula Indicates that the inertial coordinate system is equivalent to the attitude of the northeast sky navigation coordinate system, Represents the output representation of angular velocity in the inertial coordinate system, Represents a three-dimensional vector Perform cross product processing and expand it into matrix, Represents the time derivative of the attitude of the inertial coordinate system relative to the northeast sky navigation coordinate system, represents the body coordinate system of the unmanned boat, Indicates the Northeast Sky navigation coordinate system, represents the inertial coordinate system; where, let is a three-dimensional vector. According to the rules of cross multiplication, it can be expanded into Matrix, the calculation is as shown in formula (6):

[0025] (6)

[0026] In the formula Indicates along Angular velocity along the axis, Indicates along Angular velocity along the axis, Indicates along The angular velocity in the direction of the axis; by differentiating the differential equation of formula (5), the attitude calculation result of the inertial measurement unit is obtained, as shown in formula (7):

[0027]

[0028] In the formula express The attitude of the inertial coordinate system relative to the northeast sky navigation coordinate system at the moment, express The attitude of the inertial coordinate system relative to the northeast sky navigation coordinate system at the moment, Indicates from Time has come The attitude change of the inertial measurement unit at the moment; among them, The calculation of is determined by integrating the angular velocity output by the gyroscope in the inertial measurement unit, as shown in the following formula (8):

[0029] (8)

[0030] In the formula Represents a discrete time series time, Represents a discrete time series moment; assuming arrive At the moment, the inertial measurement unit conforms to the fixed-axis rotation, that is, the acceleration direction does not change, then formula (8) holds true; ③ The update of the position of the unmanned boat is based on the update of the speed. The differential equation of the speed update is shown in the following formula (9):

[0031] (9)

[0032] In the formula represents the differential of velocity, Represents the acceleration value output from the inertial measurement unit, Indicates the gravitational acceleration in the Northeast Sky navigation coordinate system, Represents the sensor-related identifier; after calculating the differential equation of formula (9), it is transformed into a recursive form, as follows:

[0033] (10)

[0034] In the formula Indicates that the unmanned boat is The speed in the Northeast Sky navigation coordinate system at all times, Indicates from Time has come The speed change in the Northeast Sky navigation coordinate system at all times, express The sensor outputs the relevant identification at each moment; among them, The calculation of is as follows:

[0035] (11)

[0036] Combined with the mean value theorem, the unmanned boat is The position in the Northeast Navigation Coordinate System at all times The corresponding calculation process is shown in formula (12):

[0037] (12)

[0038] In the formula Indicates that the unmanned boat is The position is always in the Northeast Sky navigation coordinate system. Unmanned boat The speed in the Northeast Sky navigation coordinate system at all times, where The value range is arrive ④ After the above steps, the inertial measurement unit is used to collect data to calculate the position of the surface unmanned boat required in the fusion algorithm ,speed and attitude angle The data is used to construct the system state equation required for information fusion. The calculation process is shown in formula (13):

[0039] (13)

[0040] In the formula Indicates the system status from Transfer to The state transition matrix, Indicates external influences, is the control input matrix, Indicates Time has come The process noise in time, Indicates that the unmanned boat is The system state vector at time t.

[0041] Preferably, in step Sa2), the specific steps of obtaining the state observation equation are as follows: based on geodesy, the longitude and latitude coordinate system information obtained by GPS is converted into a coordinate system, and during the conversion process, the first position data collected by GPS is used as the origin of the coordinate system ( a,b,c ) to build its own rectangular coordinate system, and the subsequent current state latitude and longitude data obtained is ( B,L,H ), converted into the northeast sky navigation coordinate system as ( X,Y,Z );Use latitude and longitude information to obtain ( a,b,c )and( B, L,H ) The length of the geodesic between two points and azimuth , the observation transfer matrix from the observation data of the unmanned boat to the current state is obtained. The calculation process of the conversion relationship is as follows formula (14):

[0042] (14)

[0043] In the formula It represents the length of the geodetic line between two points obtained by using the longitude and latitude information, reflecting the distance the UAV moves between these two locations; Indicates the bearing between two points, representing the direction angle from the first point to the subsequent point; Indicates the altitude value of the first point obtained by GPS; Represents the altitude value of a subsequent point obtained by GPS; based on the system state equation and combined with the noise parameter, the state observation equation is obtained as shown in formula (15):

[0044] (15)

[0045] In the formula represents the observation transfer matrix, represents the noise parameter, Indicates the observation data provided by GPS at the current time.

[0046] Preferably, in step Sb1), the specific steps of obtaining the displacement posture information of the unmanned boat are as follows:

[0047] Based on the system state equation and state observation equation, the adaptive Kalman filter algorithm is used to predict the state of the surface unmanned boat at the next moment: through the calculation results of the previous moment and the acceleration and angular velocity data collected by the inertial measurement unit at the next moment, the measurement update system state vector at the current moment is obtained with the help of formula (13), combined with the state transfer matrix obtained by formula (15) and calculated by formula (16) The covariance matrix at the moment is substituted into formula (17) to obtain the Kalman gain parameter , using formula (18), the weight between the GPS observation value and the IMU measurement prediction calculation value is balanced by the Kalman gain parameter to obtain the state vector at the current moment, and finally updated by formula (19) The covariance matrix at the moment is used to complete the prediction process of the optimal state estimation. The specific calculation steps are as follows:

[0048] (16)

[0049] (17)

[0050] (18)

[0051] (19)

[0052] In the formula yes The covariance matrix of the time instant; Is the system status from Transfer to The state transition matrix of is the process noise covariance matrix, which represents the variance of the process noise; is the Kalman filter gain parameter; is the observation transfer matrix; is the measurement noise covariance matrix; It is the predicted updated system state vector, that is, the current state of the unmanned boat system calculated by the data collected by the inertial measurement unit; It is the measurement and update system state vector, that is, the final state of the unmanned boat system at the current moment; It is the observation data provided by GPS at the current moment; yes The covariance matrix of the moments; is the transpose operation.

[0053] Preferably, in step Sb2), the specific steps of detecting the unmanned boat prediction data are as follows: setting the auxiliary measurement noise covariance matrix, and at the same time, combining the variational Bayesian algorithm to calculate the fixed value of the measurement noise covariance matrix , as follows:

[0054] (20)

[0055] (twenty one)

[0056] (twenty two)

[0057] In the formula and yes The degree of freedom parameter and inverse scale matrix at the moment; and are the corresponding prior parameters respectively; is the auxiliary measurement noise covariance matrix; is the dimension of the vector; It is the observation data provided by GPS at the current moment; is the observation transfer matrix; It is the measurement and update system state vector, that is, the current state of the unmanned boat system; is the covariance matrix at the current moment; is a transposition operation; the prior parameters in formula (20) are calculated using the forgetting factor method, and the calculation formula is as follows:

[0058] (twenty three)

[0059] (twenty four)

[0060] In the formula It is the forgetting factor; is the degree of freedom parameter at the previous moment; is the dimension of the vector; is the inverse proportional matrix of the previous moment;

[0061] Substitute formulas (20), (23), and (24) into formula (21) to calculate the estimated value of the actual measurement noise covariance matrix: , as shown in the following formula (25):

[0062] (25)

[0063] In the formula is the designed auxiliary measurement noise covariance matrix; It is the forgetting factor; It's the last moment The measurement noise covariance matrix of ; It's the last moment The measurement noise covariance matrix of ;

[0064] Assuming the measurement noise distribution as well as The corresponding covariance matrix is 、 ,but 、 The calculation process of the Wasserstein distance between is as follows:

[0065] (26)

[0066] In the formula yes 、 Wasserstein distance between them; is the covariance matrix traces; is the covariance matrix traces; is a matrix and trace of the product;

[0067] In formula (26), when using Wasserstein distance to evaluate the similarity of noise distribution, the following properties are obtained:

[0068] Property 1: obtained by formula , that is, the Wasserstein distance is symmetric. This feature ensures that when the Wasserstein distance is used for detection, the detection result is only 、 The value of is related and is not affected by other factors;

[0069] Property 2: If and only if When , the Wasserstein distance has a minimum value of 0, and its value increases monotonically as the noise distribution distance increases;

[0070] The sliding window method is used to calculate the additional measurement noise covariance matrix to determine whether the measurement noise covariance matrix has been changed. The calculation process is shown in formula (27):

[0071] (27)

[0072] In the formula express , represents the additional measurement noise covariance matrix calculated by the sliding window method; express Time speed; Indicates device operation; represents the observation transfer matrix; express The covariance matrix of the time instant; express estimated value of; represents the observation transfer matrix;

[0073] Assume that the true value of the measurement noise covariance matrix is , Represents the constructed auxiliary measurement noise covariance matrix and the additional measurement noise covariance matrix The true value of the Wasserstein distance in the calculation process satisfies the following formula:

[0074] (28)

[0075] The threshold is set based on the result calculated by formula (28), and the predicted data and the measurement noise covariance matrix are tested as follows:

[0076] (29)

[0077] (30)

[0078] (31)

[0079] When the calculation result does not satisfy formula (29), the data is abnormal; when the calculation result does not satisfy formula (30) or (31), the measurement noise covariance matrix is ​​abnormal; otherwise, the data is normal.

[0080] Preferably, in step Sb3), the specific steps of correcting the abnormality are as follows:

[0081] Use statistical similarity measurement to correct outliers, then the vector The similarity measure between is calculated as follows:

[0082] (32)

[0083] In the formula is the similarity function, is the Euclidean norm, is the joint probability density distribution function, is a vector Similarity measure between ;

[0084] In the state update, the calculation is maximized and between and and The distance between them is as follows:

[0085] (33)

[0086] The upper right corner of the formula represents the optimal solution; express The square root of express The square root of represents the similarity function of the measurement model; represents the function that takes the maximum value; express The probability density function of Indicates that the following expression is maximized The optimal solution of Approximately a Gaussian distribution, the optimal solution problem of formula (33) is converted into a maximum distance solution problem, and the modified calculation process is as follows:

[0087] (34)

[0088] In the formula express An intermediate calculated value of the measurement noise covariance matrix obtained after correction; represents the similarity function; Represents matrix product Perform trace operations;

[0089] Then define the auxiliary function as follows:

[0090] (35)

[0091] Combining formulas (20) and (21), we get the following calculation formula:

[0092] (36)

[0093] parameter The calculation of is transformed using formula (21):

[0094] (37)

[0095] Combining formulas (35), (36), and (37), the optimal solution of the measurement noise covariance matrix is ​​calculated. At the same time, combining formulas (21) and (37), the optimal value of the measurement noise covariance matrix is ​​calculated to reduce the impact of the surrounding complex environment on the calculation of parameter values ​​during the calculation process; it is substituted into the Kalman filter update process for correction.

[0096] Compared with the existing technology, the present invention proposes a high-precision fusion method for the displacement and posture of an unmanned vehicle based on adaptive Kalman filtering. The advantages of this method are:

[0097] The present invention provides a high-precision fusion method for the displacement and attitude of an unmanned vehicle based on adaptive Kalman filtering, which has many significant advantages, as follows:

[0098] (1) In the process of constructing the system state equation, the navigation data state is calculated by discrete integration, and the coordinate system is transformed by the rotation matrix, and the position is calculated by velocity update. The components of the position, velocity, and attitude angle of the unmanned boat in three-dimensional space are fully considered. The constructed nine-dimensional state vector can fully characterize the motion state of the unmanned boat, ensuring the integrity and accuracy of the state description. The state observation equation uses GPS position information and obtains the observation transfer matrix through geodetic transformation of the coordinate system. The noise effect is fully considered to make the observation model more suitable for the actual application scenario and provide a reliable observation basis for subsequent filtering fusion. (2) The adaptive Kalman filter algorithm is used to predict and update the state of the unmanned boat based on the constructed system state equation and state observation equation, which can effectively fuse IMU and GPS data. The Kalman gain parameter is used to balance the weights of the two, solving the problem of insufficient accuracy of single sensor data and improving the accuracy of state estimation. In view of the characteristics of IMU with cumulative errors and GPS being easily interfered by the environment, the fusion algorithm achieves complementary advantages, so that the unmanned boat can still obtain relatively accurate displacement and attitude information in a complex water surface environment. (3) The auxiliary measurement noise covariance matrix is ​​introduced and combined with the variational Bayes algorithm to detect outliers. The prior parameters are calculated through the forgetting factor, so that the estimation of the noise covariance matrix can adapt to data changes and improve the sensitivity to abnormal data. The similarity of the covariance matrix is ​​quantified by the Wasserstein distance, which has excellent properties such as symmetry and monotonically increasing. It can accurately judge whether the data or matrix results are abnormal, avoid the interference of abnormal data on state estimation, and enhance the robustness of the system in complex environments. (4) When an anomaly is detected, the statistical similarity measure is used for correction, and the optimal solution problem is converted into a maximum distance solution. The optimal solution of the measurement noise covariance matrix is ​​calculated and substituted into the Kalman filter update process, which can effectively correct the outlier. This correction mechanism ensures that the stability and accuracy of the filtering algorithm can be maintained in the presence of abnormal data, ensuring the reliability of the displacement attitude information of the unmanned boat and enabling the system to work continuously and stably. (5) The entire method uses a variational Bayesian algorithm to adaptively estimate the noise covariance matrix. Combined with an anomaly detection and correction mechanism, the system can dynamically adjust parameters based on actual data conditions and adapt to changes in sensor noise characteristics in the water surface environment. Whether the GPS signal is blocked or the IMU drift is aggravated, this method can maintain good performance through adaptive adjustment and is suitable for various complex water surface operation scenarios.

[0099] In summary, the present invention significantly improves the accuracy, reliability and robustness of the acquisition of displacement posture information of unmanned boats through state modeling, fusion algorithm, anomaly detection and anomaly correction, and has strong practical application value. BRIEF DESCRIPTION OF THE DRAWINGS

[0100] Figure 1This is a flow chart of the high-precision fusion method of the unmanned boat displacement posture based on adaptive Kalman filtering of the present invention;

[0101] Figure 2 Schematic diagram of the conversion from the inertial coordinate system to the navigation coordinate system of the present invention;

[0102] Figure 3 This is a block diagram of the anomaly detection and correction mechanism of the present invention;

[0103] Figure 4 This is the overall architecture diagram of the algorithm of the present invention;

[0104] Figure 5 This is the standard trajectory diagram of the simulation data of the present invention;

[0105] Figure 6 This is a GPS measurement data trajectory diagram simulated by the present invention;

[0106] Figure 7 This is a visualization of the adaptive Kalman filter fusion result of the present invention;

[0107] Figure 8 This is a visual graph of the fusion results of the algorithm of the present invention;

[0108] Figure 9 This is an error analysis diagram of the adaptive Kalman filter fusion result of the present invention;

[0109] Figure 10 This is the error analysis diagram of the fusion result of the algorithm of the present invention. DETAILED DESCRIPTION

[0110] The following will be combined with the drawings in the embodiments of this application to further clearly and completely describe the technical solutions in the embodiments of this application. It should be noted that the embodiments described are only part of the embodiments of this application, not all of the embodiments. Based on the embodiments in this application, all other embodiments obtained by those skilled in the art without making any creative work shall fall within the scope of protection of this application.

[0111] In order to make the invention objectives, technical solutions and advantages of this application clearer, the embodiments of this application are further described in detail in conjunction with the drawings in the specification: In order to more clearly understand the above-mentioned objectives, features and advantages of the present invention, the advantages of the present invention will be further illustrated by comparing the embodiments in conjunction with the drawings and specific implementation methods.

[0112] The present invention proposes a high-precision fusion method for the displacement and attitude of an unmanned boat based on adaptive Kalman filtering. Figure 1 As shown, the steps of the method are described in detail:

[0113] S1) Construct the system state equation and state observation equation of the unmanned vehicle based on GPS and IMU; Sa1) Obtain navigation data state, acceleration, and attitude angle change rate based on GPS and IMU, and construct a discrete time state space model of the navigation data state using discrete integration; Use the rotation matrix to transform into the north-east navigation coordinate system, and use the median value theorem to update the position to obtain the system state equation;

[0114] Specifically, in step Sa1), the specific process of obtaining the system state equation is as follows: ① When the unmanned boat is performing berthing and unberthing operations, the working conditions of its control module are time-varying and the motion state is nonlinear; under the multi-source information fusion framework, the navigation data state is obtained using GPS, and the acceleration of the unmanned boat is directly measured using the inertial measurement unit. and attitude angle change rate , and the discrete time state space model of each navigation data state of the unmanned boat is calculated using the discrete integral formula. The navigation data state includes the position of the unmanned boat ,speed and attitude angle , the following formula is expressed as:

[0115] (1)

[0116] (2)

[0117] (3)

[0118] in, express The position of the unmanned boat at all times, express The position of the unmanned boat at all times, Indicates the time interval, express Time speed, express Time acceleration; express Time speed, express Momentary attitude angle, express Momentary attitude angle, express Attitude angle change rate at all times;

[0119] Integrate the navigation data state, correspond the position, velocity, and attitude angle to the three dimensional components of the x, y, and z directions in three-dimensional space, and integrate them with the navigation data state into a nine-dimensional vector , the specific formula is as follows:

[0120] (4)

[0121] in 、 、 Indicates the position of the unmanned boat in the northeast navigation coordinate system, 、 、 It represents the speed of the unmanned boat in the northeast navigation coordinate system, 、 、 It represents the attitude angle of the unmanned boat in the northeast navigation coordinate system. Represents a transpose operation;

[0122] ② When the UAV starts to move, the inertial measurement unit is used to measure the acceleration and angular velocity. Usually, the data measured by the inertial measurement unit is in the inertial coordinate system, that is, the body coordinate system; and other navigation information is reflected in the north-east navigation coordinate system. Therefore, the data obtained by the inertial unit sensor is converted into a coordinate system through the rotation matrix to obtain the acceleration and angular velocity data of the UAV in the north-east navigation coordinate system; Figure 2 As shown in the figure, with the help of the heading change of the unmanned boat, the inertial coordinate system is rotated, and the rotation matrix from the inertial coordinate system to the northeast sky navigation coordinate system is constructed to obtain the attitude of the northeast sky navigation coordinate system. The differential equation of the rotation matrix with respect to time is as follows:

[0123] (5)

[0124] In the formula Indicates that the inertial coordinate system is equivalent to the attitude of the northeast sky navigation coordinate system, Represents the output representation of angular velocity in the inertial coordinate system, Represents a three-dimensional vector Perform cross product processing and expand it into matrix, Represents the time derivative of the attitude of the inertial coordinate system relative to the northeast sky navigation coordinate system, represents the body coordinate system of the unmanned boat, Indicates the Northeast Sky navigation coordinate system, represents the inertial coordinate system;

[0125] Among them, is a three-dimensional vector. According to the rules of cross multiplication, it can be expanded into Matrix, the calculation is as shown in formula (6):

[0126] (6)

[0127] In the formula Indicates along Angular velocity along the axis, Indicates along Angular velocity along the axis, Indicates along Angular velocity along the axis;

[0128] By solving the differential equation of formula (5), the attitude calculation result of the inertial measurement unit is obtained, which is expressed as formula (7):

[0129] (7)

[0130] In the formula express The attitude of the inertial coordinate system relative to the northeast sky navigation coordinate system at the moment, express The attitude of the inertial coordinate system relative to the northeast sky navigation coordinate system at the moment, Indicates from Time has come The attitude changes of the inertial measurement unit at each moment;

[0131] in, The calculation of is determined by integrating the angular velocity output by the gyroscope in the inertial measurement unit, as shown in the following formula (8):

[0132] (8)

[0133] In the formula Represents a discrete time series time, Represents a discrete time series time;

[0134] Assume that arrive At this moment, the inertial measurement unit conforms to fixed-axis rotation, that is, the acceleration direction does not change, then formula (8) holds; and the inertial measurement units currently used are all high-frequency, and are approximately regarded as fixed-axis rotation between two measurement intervals;

[0135] ③ The calculation of the position of the unmanned boat is based on the speed update. The differential equation of speed update is shown in the following formula (9):

[0136] (9)

[0137] In formula (9), represents the differential of velocity, Represents the acceleration value output from the inertial measurement unit, It represents the acceleration of gravity in the Northeast Sky navigation coordinate system. Represents the sensor-related identifier; by integrating the differential equation of formula (9) and rewriting it into a recursive form, the formula is as follows:

[0138] (10)

[0139] In the formula Indicates that the unmanned boat is The speed in the Northeast Sky navigation coordinate system at all times, Indicates from Time has come The speed change in the Northeast Sky navigation coordinate system at all times, express Moment sensor (inertial measurement unit) output related identification;

[0140] in, The calculation of is as follows:

[0141] (11)

[0142] Combined with the mean value theorem, the unmanned boat is The position in the Northeast Navigation Coordinate System at all times The corresponding calculation process is shown in formula (12):

[0143] (12)

[0144] In the formula Indicates that the unmanned boat is The position is always in the Northeast Sky navigation coordinate system. Unmanned boat The speed in the Northeast Sky navigation coordinate system at all times, where The value range is arrive between moments;

[0145] ④After the above steps, the inertial measurement unit is used to collect data to calculate the position of the surface unmanned boat required in the fusion algorithm ,speed and attitude angle The data is used to construct the system state equation required for information fusion. The calculation process is shown in formula (13):

[0146] (13)

[0147] In the formula Indicates the system status from Transfer to The state transition matrix, Indicates external influences, is the control input matrix, Indicates Time has come The process noise in time, Indicates that the unmanned boat is The system state vector at time t.

[0148] Sa2) The observation transfer matrix is ​​derived from the coordinate system information obtained by GPS, the length of the geodetic line between two points, and the azimuth angle. The observation transfer matrix is ​​used to convert the autonomous vehicle's own rectangular coordinate system into the northeast sky navigation coordinate system. The state observation equation is then obtained by combining the noise data.

[0149] Specifically, in step Sa2), the specific steps of obtaining the state observation equation are as follows:

[0150] The position information data of the unmanned boat is directly observed through the GPS sensor; the position information data observed by GPS is longitude and latitude data, while the data collected by IMU is in the body coordinate system of the unmanned boat. Therefore, the longitude and latitude information obtained by GPS needs to be converted into a coordinate system based on geodesy to obtain the GPS description of the displacement attitude information of the unmanned boat during the autonomous docking and undocking process; in the conversion process, the first position data collected by GPS is used as the origin of the coordinate system ( a,b,c ) to build its own rectangular coordinate system, and the current latitude and longitude data obtained later is ( B,L,H ), after conversion, the Northeast Sky navigation coordinate system is ( X,Y,Z );Use latitude and longitude information to obtain ( a,b,c )and( B,L,H ) The length of the geodesic between two points and azimuth , the observation transfer matrix from the observation data of the unmanned boat to the current state is obtained, and the calculation process of the conversion relationship is as follows formula (14):

[0151] (14)

[0152] In the formula It represents the length of the geodetic line between two points obtained by using the longitude and latitude information, reflecting the distance the UAV moves between these two locations; Indicates the bearing between two points, representing the direction angle from the first point to the subsequent point; Indicates the altitude value of the first point obtained by GPS; Indicates the height value of a subsequent point obtained by GPS. In actual operation, due to the multipath effect caused by large metal facilities on the water surface and the surrounding shore, the data collected by GPS is often contaminated by noise. Based on the system state equation and combined with the noise parameter, the state observation equation is obtained as shown in formula (15):

[0153] (15)

[0154] In the formula represents the observation transfer matrix, represents the noise parameter, Indicates the observation data provided by GPS at the current time.

[0155] S2) Acquire the displacement and attitude information of the unmanned boat based on the adaptive Kalman filter, use the sliding window method to detect, and use statistical methods to correct outliers;

[0156] Sb1) Based on the system state equation and state observation equation, the adaptive Kalman filter algorithm is used to predict the state of the UAV at the next moment and obtain the displacement and posture information of the UAV;

[0157] Specifically, in step Sb1), the specific steps for obtaining the displacement attitude information of the unmanned boat are as follows: Based on the system state equation and the state observation equation, the state of the surface unmanned boat at the next moment is predicted using the adaptive Kalman filter algorithm: First, the measurement update system state vector at the current moment is obtained by using the calculation results of the previous moment and the acceleration and angular velocity data collected by the inertial measurement unit at the next moment with the help of formula (13), and the state transfer matrix obtained by combining formula (15) and the calculation by formula (16) is obtained. The covariance matrix at the moment is substituted into formula (17) to obtain the Kalman gain parameter , using formula (18), the weight between the GPS observation value and the IMU measurement prediction calculation value is balanced by the Kalman gain parameter to obtain the state vector at the current moment, and finally updated by formula (19) The covariance matrix at the moment completes the prediction of the optimal state estimate and the update correction process; the specific calculation steps are as follows:

[0158] (16)

[0159] (17)

[0160] (18)

[0161] (19)

[0162] In the formula yes The covariance matrix of the time instant; Is the system status from Transfer to The state transition matrix of is the process noise covariance matrix, which represents the variance of the process noise; is the Kalman filter gain parameter; is the observation transfer matrix; is the measurement noise covariance matrix; It is the predicted updated system state vector, that is, the current state of the unmanned boat system calculated by the data collected by the inertial measurement unit; It is the measurement and update system state vector, that is, the final state of the unmanned boat system at the current moment; It is the observation data provided by GPS at the current moment; yes The covariance matrix of the time instant; is the transpose operation; in the above parameters, the measurement noise covariance matrix It is a pre-set parameter that characterizes the estimation accuracy of GPS. However, in actual work engineering, due to the interference of multipath effects, the estimation accuracy of GPS often changes, and its estimation accuracy is different at different times. The value of the measurement noise covariance matrix should be adjusted in time in formula (17), and the calculated Kalman gain parameter is not easy to deviate, and this deviation is brought into formula (18), affecting the final optimal estimation of the system state at the current moment.

[0163] Sb2) setting an auxiliary measurement noise covariance matrix, combining a variational Bayesian algorithm to calculate a fixed value of the measurement noise covariance matrix, combining an estimated value of the measurement noise covariance matrix with a Wasserstein distance to quantify the similarity of the covariance matrix, and using a sliding window method to detect the unmanned vehicle prediction data; specifically, in step Sb2), the specific steps of detecting the unmanned vehicle prediction data are as follows:

[0164] Set the auxiliary measurement noise covariance matrix, and at the same time, combine the variational Bayesian algorithm to fix the value of the measurement noise covariance matrix , as follows:

[0165] (20)

[0166] (twenty one)

[0167] (twenty two)

[0168] In the formula and is the degree of freedom parameter at the moment and the inverse proportional matrix; and are the corresponding prior parameters respectively; is the designed auxiliary measurement noise covariance matrix; is the dimension of the vector; It is the observation data provided by GPS at the current moment; is the observation transfer matrix; It is the measurement and update system state vector, that is, the final state of the unmanned boat system at the current moment; yes The covariance matrix of the time instant; is the transpose operation;

[0169] The corresponding prior parameters in formula (20) are calculated using the forgetting factor method, and the calculation formula is as follows:

[0170] (twenty three)

[0171] (twenty four)

[0172] In the formula It is the forgetting factor; is the degree of freedom parameter at the previous moment; is the dimension of the vector; is the inverse proportional matrix of the previous moment; Substitute formulas (20), (23), and (24) into formula (21) to calculate the estimated value of the actual measurement noise covariance matrix , is the weighted sum of previous statistical information and current observation information, as shown in the following formula (25):

[0173] (25)

[0174] In the formula is the designed auxiliary measurement noise covariance matrix; It is the forgetting factor; It's the last moment The measurement noise covariance matrix of ; It's the last moment The measurement noise covariance matrix is ​​assuming that the measurement noise distribution as well as Corresponding covariance matrix 、 ,but 、 The calculation process of the Wasserstein distance between is as follows:

[0175] (26)

[0176] In the formula yes 、 Wasserstein distance between them; is the covariance matrix traces; is the covariance matrix traces; is a matrix and The trace of the product; in formula (26), when using Wasserstein distance to evaluate the similarity of noise distribution, it has the following properties:

[0177] Property 1: obtained by formula , that is, the Wasserstein distance is symmetric. This feature ensures that when the Wasserstein distance is used for detection, the detection result is only 、 The value of is related and is not affected by other factors;

[0178] Property 2: If and only if When , the Wasserstein distance has a minimum value of 0, and its value increases monotonically as the noise distribution distance increases, providing a basis for the subsequent threshold setting. The sliding window method is used to calculate the additional measurement noise covariance matrix to determine whether the measurement noise covariance matrix has been changed. The calculation process is shown in formula (27):

[0179] (27)

[0180] In the formula for , represents the additional measurement noise covariance matrix calculated by the sliding window method; express Time speed; Indicates device operation; represents the observation transfer matrix; express The covariance matrix of the time instant; express estimated value of; represents the observation transfer matrix;

[0181] Assume that the true value of the measurement noise covariance matrix is , Represents the constructed auxiliary measurement noise covariance matrix and the additional measurement noise covariance matrix The true value of the Wasserstein distance in the calculation process satisfies the following formula:

[0182] (28)

[0183] In practical applications, since the value of the real measurement noise covariance matrix is is unknown, so an estimate in steady state is used , according to the obtained result, the threshold is set, and the prediction data and the measurement noise covariance matrix are tested as follows:

[0184] (29)

[0185] (30)

[0186] (31)

[0187] When the calculation result does not satisfy formula (29), the data is abnormal; when the calculation result does not satisfy formula (30) or (31), the measurement noise covariance matrix is ​​abnormal; otherwise, the data is normal.

[0188] Sb3) When anomalies in the predicted data are detected, the statistical similarity measurement is used to correct the outliers, the optimal solution is obtained by maximizing the distance, and the Kalman filter is used to correct the measurement noise covariance matrix.

[0189] Specifically, in step Sb3), the specific steps of correcting the anomaly are as follows: when abnormal data or noise covariance matrix is ​​detected, appropriate correction and improvement are performed to improve the accuracy of the algorithm calculation results; the outliers are corrected using a statistical similarity measurement method, then the vector The similarity measure between is calculated as follows:

[0190] (32)

[0191] In the formula is the similarity function, is the Euclidean norm, is the joint probability density distribution function, is a vector Similarity measure between ;

[0192] In the state update, the calculation is maximized and between and and The distance between them is as follows:

[0193] (33)

[0194] The upper right corner of the formula represents the optimal solution; express The square root of express The square root of represents the similarity function of the measurement model; represents the function that takes the maximum value; express The probability density function of Indicates that the following expression is maximized The optimal solution of

[0195] Will Approximately a Gaussian distribution, the optimal solution problem of the above formula (33) is converted into a maximum distance solution problem, and the modified calculation process is as follows:

[0196] (34)

[0197] In the formula express An intermediate calculated value of the measurement noise covariance matrix obtained after correction; represents the similarity function; Represents matrix product Perform trace operations;

[0198] Then define the auxiliary function as follows:

[0199] (35)

[0200] Combining formulas (20) and (21), we get the following calculation formula:

[0201] (36)

[0202] parameter The calculation of is transformed using formula (21):

[0203] (37)

[0204] Combining formulas (35), (36), and (37), the optimal solution of the measurement noise covariance matrix is ​​calculated. At the same time, combining formulas (21) and (37), the optimal value of the measurement noise covariance matrix is ​​calculated to reduce the impact of the surrounding complex environment on the calculation of parameter values ​​during the calculation process; it is substituted into the Kalman filter update process to correct outliers and improve the result fusion.

[0205] The specific simulation experiments of the present invention are described in detail with reference to the technical solutions and the accompanying drawings as follows:

[0206] A high-precision fusion method for the displacement and attitude of an unmanned boat based on adaptive Kalman filtering is proposed. The unmanned boat is used as the simulation research object, the simulated berthing and unberthing routes are designed, the simulated GPS and IMU data are generated, the detection and correction mechanism and the overall algorithm architecture are as follows: Figure 3 、 4 As shown, the accuracy of the method of the present invention is verified.

[0207] In the design of the experimental trajectory, a nearly S-shaped trajectory was chosen to simulate the UAV's motion during turns and straight travel. To generate simulation data, corresponding initial parameters were set, including the initially unknown longitude and latitude. Based on the proposed method for acquiring UAV displacement and attitude information based on adaptive Kalman filtering, a simulated docking and undocking route was designed using the UAV as the simulation object, generating simulated GPS and IMU data to verify the accuracy of the proposed method.

[0208] A nearly S-shaped trajectory was chosen for the experimental trajectory design, simulating the UAV's turning and straight-ahead motion. To generate simulation data, the corresponding initial parameters needed to be set, including the initially unknown longitude and latitude, the three-axis velocities in the Northeast Navigation coordinate system, and the attitude angles. These are shown in Table 1 below.

[0209] Table 1 Initial parameters of the unmanned boat simulation trajectory

[0210] Lat(deg) Lon(deg) Alt(m) Vx(m / s) Vy(m / s) Vz(m / s) Yaw(deg) Pitch(deg) Roll(deg) 32 120 0 10 0 0 0 0 0

[0211] In order to realize the trajectory design of the simulated unmanned boat, the motion trajectory is divided into three segments. The parameters such as duration, turning angle, acceleration, etc. are designed for each segment as shown in Table 2:

[0212] Table 2 Segmented control parameters of unmanned boat simulation motion

[0213]

[0214] According to the data in Table 2, the entire motion process is set to 30s, the heading angle change rate and acceleration are set as shown in the table, and the entire motion is set to a nonlinear change motion process. The standard motion trajectory of the generated data is as follows Figure 5 As shown, Figure 6 The figure shows the trajectory visualization of the unmanned boat generated by simulation based on GPS data in the northeast celestial coordinate system.

[0215] In order to demonstrate the accuracy of the fusion results of the algorithm proposed in this invention, the adaptive Kalman filter algorithm is used as a comparison algorithm to detect the effect. The fusion results of the two algorithms are expressed in the Northeast Sky navigation coordinate system as follows Figure 7 、 8 As shown, the error analysis diagram of the fusion results of the two algorithms is shown as follows Figure 9 、 10 It can be seen that the algorithm provided by the present invention effectively improves the accuracy of the finally acquired displacement attitude information data of the unmanned boat.

[0216] Although the preferred embodiments of the present application have been described, those skilled in the art may make additional changes and modifications to these embodiments once they have learned the basic creative concept. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the present application.

[0217] Obviously, those skilled in the art may make various changes and modifications to this application without departing from the spirit and scope of this application. Thus, if these modifications and variations of this application fall within the scope of the claims of this application and their equivalents, this application is intended to include these modifications and variations.

Claims

1. A high-precision fusion method for displacement and attitude of unmanned boat based on adaptive Kalman filtering, characterized by: include: S1) Construct the system state equation and state observation equation of the unmanned boat based on GPS and IMU; Sa1) Obtain navigation data status, acceleration, and attitude angle change rate based on GPS and IMU, and construct a discrete time state space model of the navigation data status using discrete integration; By using the rotation matrix to transform into the north-east navigation coordinate system, the position is updated in combination with the middle value theorem to obtain the system state equation; Sa2) The observation transfer matrix is ​​derived from the coordinate system information obtained by GPS, the length of the geodetic line between two points, and the azimuth angle. The observation transfer matrix is ​​used to convert the autonomous vehicle's own rectangular coordinate system into the northeast sky navigation coordinate system. The state observation equation is then obtained by combining the noise data. S2) Acquire the displacement and attitude information of the unmanned boat based on the adaptive Kalman filter, use the sliding window method to detect, and use statistical methods to correct outliers; Sb1) Based on the system state equation and state observation equation, the adaptive Kalman filter algorithm is used to predict the state of the UAV at the next moment and obtain the displacement and posture information of the UAV; Sb2) Setting the auxiliary measurement noise covariance matrix, calculating the fixed value of the measurement noise covariance matrix using the variational Bayesian algorithm, quantifying the similarity of the covariance matrix using the Wasserstein distance based on the estimated value of the measurement noise covariance matrix, and detecting the unmanned vehicle prediction data using the sliding window method; Sb3) When anomalies in the predicted data are detected, the statistical similarity measurement is used to correct the outliers, the optimal solution is obtained by maximizing the distance, and the Kalman filter is used to correct the measurement noise covariance matrix.

2. The high-precision fusion method for displacement and posture of an unmanned vehicle based on adaptive Kalman filtering according to claim 1 is characterized in that: In the step Sa1), the specific process of obtaining the system state equation is as follows: ① Obtain navigation data status through GPS and use inertial measurement unit to directly measure the acceleration of the unmanned boat and attitude angle change rate , the discrete time state space model of each navigation data state of the unmanned boat is calculated using the discrete integral formula. The navigation data state includes the position of the unmanned boat ,speed and attitude angle , the following formula is expressed as: (1) (2) （3） in, express The position of the unmanned boat at all times, express The position of the unmanned boat at all times, Indicates the time interval, express Time speed, express momentary acceleration; express Time speed, express Momentary attitude angle, express Momentary attitude angle, express Attitude angle change rate at all times; The position, velocity, and attitude angle correspond to the three dimensional components of the x, y, and z directions in three-dimensional space, and are integrated with the navigation data state into a nine-dimensional vector , the formula is as follows: (4) in 、 、 Indicates the position of the unmanned boat in the northeast navigation coordinate system, 、 、 It represents the speed of the unmanned boat in the northeast navigation coordinate system, 、 、 It represents the attitude angle of the unmanned boat in the northeast navigation coordinate system. Represents a transpose operation; ② With the help of the heading change of the unmanned boat, the inertial coordinate system is rotated, and the rotation matrix from the inertial coordinate system to the northeast sky navigation coordinate system is constructed to obtain the attitude of the northeast sky navigation coordinate system. The differential equation of the rotation matrix with respect to time is as follows: (5) In the formula Indicates that the inertial coordinate system is equivalent to the attitude of the northeast sky navigation coordinate system, Represents the output representation of angular velocity in the inertial coordinate system, Represents a three-dimensional vector Perform cross product processing, Represents the time derivative of the attitude of the inertial coordinate system relative to the northeast sky navigation coordinate system, represents the body coordinate system of the unmanned boat, Indicates the Northeast Sky navigation coordinate system, represents the inertial coordinate system; Among them, is a three-dimensional vector. According to the rules of cross multiplication, it can be expanded into Matrix, the calculation is as shown in formula (6): （6） In the formula Indicates along Angular velocity along the axis, Indicates along Angular velocity along the axis, Indicates along Angular velocity in the direction of the axis; By solving the differential equation of formula (5), the attitude calculation result of the inertial measurement unit is obtained, which is expressed as formula (7): （7） In the formula express The attitude of the inertial coordinate system relative to the northeast sky navigation coordinate system at the moment, express The attitude of the inertial coordinate system relative to the northeast sky navigation coordinate system at the moment, Indicates from Time has come The attitude changes of the inertial measurement unit at each moment; in, The calculation of is determined by integrating the angular velocity output by the gyroscope in the inertial measurement unit, as shown in the following formula (8): (8) In the formula Represents a discrete time series time, Represents a discrete time series time; Assume that arrive At the moment, the inertial measurement unit conforms to fixed-axis rotation, that is, the acceleration direction does not change, then formula (8) is established; ③ The update of the position of the unmanned boat is based on the update of the speed. The differential equation of the speed update is shown in the following formula (9): (9) In the formula represents the differential of velocity, Represents the acceleration value output from the inertial measurement unit, It represents the acceleration of gravity in the Northeast Sky navigation coordinate system. Indicates sensor-related identifiers; After calculating the differential equation of formula (9), it is transformed into a recursive form, and the formula is as follows: （10） In the formula Indicates that the unmanned boat is The speed in the Northeast Sky navigation coordinate system at all times, Indicates from Time has come The speed change in the Northeast Sky navigation coordinate system at all times, express The sensor outputs the relevant identification at the moment; in, The calculation of is as follows: (11) Combined with the mean value theorem, the unmanned boat is The position in the Northeast Navigation Coordinate System at all times The corresponding calculation process is shown in formula (12): (12) In the formula Indicates that the unmanned boat is The position is always in the Northeast Sky navigation coordinate system. Unmanned boat The speed in the Northeast Sky navigation coordinate system at all times, where The value range is arrive between moments; ④After the above steps, the inertial measurement unit is used to collect data to calculate the position of the unmanned boat required in the fusion algorithm ,speed and attitude angle The data is used to construct the system state equation required for information fusion. The calculation process is shown in formula (13): (13) In the formula Indicates the system status from Transfer to The state transition matrix, Indicates external influences, is the control input matrix, Indicates Time has come The process noise in time, Indicates that the unmanned boat is The system state vector at time t.

3. The high-precision fusion method for displacement and posture of an unmanned vehicle based on adaptive Kalman filtering according to claim 1 is characterized in that: In step Sa2), the specific steps of obtaining the state observation equation are as follows: Based on geodesy, the longitude and latitude coordinate information obtained by GPS is converted into a coordinate system, and the first position data collected by GPS is used as the origin of the coordinate system ( a,b,c ) to build its own rectangular coordinate system, and then obtain the current state latitude and longitude data as ( B,L,H ), converted into the northeast sky navigation coordinate system as ( X,Y,Z );Use latitude and longitude information to obtain ( a,b,c )and( B,L,H ) The length of the geodesic between two points and azimuth , the observation transfer matrix from the observation data of the unmanned boat to the current state is obtained, and the conversion relationship is calculated as follows (14): （14） In the formula It represents the length of the geodetic line between two points obtained by using the longitude and latitude information, reflecting the distance the UAV moves between these two locations; Indicates the bearing between two points, representing the direction angle from the first point to the subsequent point; Indicates the altitude value of the first point obtained by GPS; Indicates the altitude value of a subsequent point obtained by GPS; Based on the system state equation and combined with the noise parameter, the state observation equation is shown in formula (15): （15） In the formula represents the observation transfer matrix, represents the noise parameter, Indicates the observation data provided by GPS at the current time.

4. The high-precision fusion method for displacement and posture of an unmanned vehicle based on adaptive Kalman filtering according to claim 1 is characterized in that: In step Sb1), the specific steps of obtaining the displacement posture information of the unmanned boat are as follows: By using the calculation results of the previous moment and the acceleration and angular velocity data collected by the inertial measurement unit at the next moment, the measurement update system state vector at the current moment is obtained with the help of formula (13), combined with the state transfer matrix obtained by formula (15) and calculated by formula (16) The covariance matrix at the moment is substituted into formula (17) to obtain the Kalman gain parameter , using formula (18), the weight between the GPS observation value and the IMU measurement prediction calculation value is balanced by the Kalman gain parameter to obtain the state vector at the current moment, and finally updated by formula (19) The covariance matrix at the moment is used to complete the prediction process of the optimal state estimation. The specific calculation steps are as follows: (16) （17） (18) (19) In the formula yes The covariance matrix of the time instant; Is the system status from Transfer to The state transition matrix of is the process noise covariance matrix, which represents the variance of the process noise; is the Kalman filter gain parameter; is the observation transfer matrix; is the measurement noise covariance matrix; It is the predicted updated system state vector, that is, the current state of the unmanned boat system calculated by the data collected by the inertial measurement unit; It is the measurement and update system state vector, that is, the final state of the unmanned boat system at the current moment; It is the observation data provided by GPS at the current moment; yes The covariance matrix of the time instant; is the transpose operation.

5. The high-precision fusion method for displacement and posture of an unmanned vehicle based on adaptive Kalman filtering according to claim 1 is characterized in that: In step Sb2), the specific steps for detecting the unmanned boat prediction data are as follows: Set the auxiliary measurement noise covariance matrix, and at the same time, combine the variational Bayesian algorithm to fix the value of the measurement noise covariance matrix The calculation is as follows: （20） (21) (22) In the formula and yes The degree of freedom parameter and inverse scale matrix at the moment; and are the corresponding prior parameters respectively; is the auxiliary measurement noise covariance matrix; is the dimension of the vector; It is the observation data provided by GPS at the current moment; is the observation transfer matrix; It is the measurement and update system state vector, that is, the current state of the unmanned boat system; is the covariance matrix at the current moment; is the transpose operation; Among them, the prior parameters in formula (20) are calculated using the forgetting factor as follows: （23） (24) In the formula It is the forgetting factor; is the degree of freedom parameter at the previous moment; is the dimension of the vector; is the inverse proportional matrix of the previous moment; Substitute formulas (20), (23), and (24) into formula (21) to calculate the estimated value of the actual measurement noise covariance matrix: , as shown in the following formula (25): (25) In the formula is the designed auxiliary measurement noise covariance matrix; It is the forgetting factor; It's the last moment The measurement noise covariance matrix of ; It's the last moment The measurement noise covariance matrix of ; Assuming the measurement noise distribution as well as The corresponding covariance matrix is 、 ,but 、 The calculation process of the Wasserstein distance between is as follows: (26) In the formula yes 、 Wasserstein distance between them; is the covariance matrix traces; is the covariance matrix traces; is a matrix and trace of the product; In formula (26), when using Wasserstein distance to evaluate the similarity of noise distribution, the following properties are obtained: Property 1: obtained by formula , that is, the Wasserstein distance is symmetric. This feature ensures that when the Wasserstein distance is used for detection, the detection result is only 、 The value of is related and is not affected by other factors; Property 2: If and only if When , the Wasserstein distance has a minimum value of 0, and its value increases monotonically as the noise distribution distance increases; The sliding window method is used to calculate the additional measurement noise covariance matrix to determine whether the measurement noise covariance matrix has been changed. The calculation process is shown in formula (27): （27） In the formula express , represents the additional measurement noise covariance matrix calculated by the sliding window method; express Time speed; Indicates device operation; represents the observation transfer matrix; express The covariance matrix of the time instant; express estimated value of; represents the observation transfer matrix; Assume that the true value of the measurement noise covariance matrix is , Represents the constructed auxiliary measurement noise covariance matrix and the additional measurement noise covariance matrix The true value of the Wasserstein distance in the calculation process satisfies the following formula: (28) The threshold is set based on the result calculated by formula (28), and the predicted data and the measurement noise covariance matrix are tested as follows: （29) (30) (31) When the calculation result does not satisfy formula (29), the data is abnormal; when the calculation result does not satisfy formula (30) or (31), the measurement noise covariance matrix is ​​abnormal; otherwise, the data is normal.

6. The high-precision fusion method for displacement and posture of an unmanned vehicle based on adaptive Kalman filtering according to claim 1 is characterized in that: In step Sb3), the specific steps for correcting the abnormality are as follows: The outliers are corrected using statistical similarity metrics. The similarity metrics are calculated as follows: （32） In the formula is the similarity function, is the Euclidean norm, is the joint probability density distribution function, is a similarity measure; In the state update, the calculation is maximized and between and and The distance between them is as follows: （33） The upper right corner of the formula represents the optimal solution; express The square root of express The square root of represents the similarity function of the measurement model; represents the function that takes the maximum value; express The probability density function of Indicates that the following expression is maximized The optimal solution of Will Approximately a Gaussian distribution, the optimal solution problem of formula (33) is converted into a maximum distance solution problem, and the modified calculation process is as follows: (34) In the formula express An intermediate calculated value of the measurement noise covariance matrix obtained after correction; represents the similarity function; Represents matrix product Perform trace operations; Then define the auxiliary function as follows: (35) Combining formulas (20) and (21), we get the following calculation formula: （36） parameter The calculation of is transformed using formula (21): (37) Combining formulas (35), (36), and (37), the optimal solution of the measurement noise covariance matrix is ​​calculated. At the same time, combining formulas (29) and (37), the optimal value of the measurement noise covariance matrix is ​​calculated to reduce the impact of the surrounding complex environment on the calculation of parameter values ​​in the calculation process; Substitute it into the Kalman filter update process for correction.

Images