Ship positioning method based on GNSS / DR factor graph combination

Abstract

The invention discloses a GNSS / DR factor graph combination-based ship positioning method, and relates to the technical field of ship positioning, and the method comprises the steps: obtaining the observation information of a DR system and a GNSS system; a traditional DR system is remodeled, track plotting is carried out, an error factor is established according to a sensor error of the DR system, and then a DR factor is constructed; according to the position increment of the DR system and the position information of the GNSS and the DR system, designing a self-adaptive fuzzy logic reasoning system to output a GNSS system observation covariance predicted value, and constructing a self-adaptive GNSS factor; the method comprises the following steps: establishing a factor graph model of a GNSS / DR integrated navigation system, adding factors into a factor graph, establishing a sliding window mechanism, performing marginalization processing when variables in a window reach a set size, constructing marginalized factors and adding the marginalized factors into the factor graph; and finally, solving an optimal estimation result of the ship position through global joint optimization. As a high-credibility positioning solution, the method provided by the invention can provide a navigation positioning result with higher precision for a ship and guarantee navigation safety.

Description

Technical Field

[0001] The present invention relates to the technical field of marine ship positioning, and in particular to a ship positioning method based on a GNSS / DR factor graph combination. Background Art

[0002] With the advancement of maritime navigation and positioning technology, the Global Navigation Satellite System (GNSS) plays a key role in providing ship positioning, navigation and timing (PNT) services. At the same time, in recent years, Maritime Autonomous Surface Ships (MASS) have become a research hotspot in the international maritime field, and highly reliable navigation and positioning technology has become an important basis for ensuring the safe navigation of MASS. However, the inherent vulnerability of GNSS has brought severe challenges to ship navigation and positioning.

[0003] The ship navigation and positioning method based on Dead Reckon (DR) can perform autonomous navigation and positioning of ships independently of the GNSS system, and has high accuracy in a short time. However, during the navigation of the ship, the integral errors of sensors such as the compass and the odometer will gradually accumulate, especially when considering the influence of external wind and current, its credibility is challenged. The ship integrated navigation system based on GNSS / DR is widely used due to its complementary advantages.

[0004] Existing ship GNSS / DR data fusion methods mostly use centralized filtering or federal filtering. Such methods rely on inherent mathematical models. When the sensor data is abnormal due to time-varying noise and observation errors in complex scenarios such as ships entering and leaving ports, the positioning accuracy is not high and the adaptive ability is weak. The factor graph optimization method associates system state information with navigation information, realizes data fusion based on the maximum a posteriori estimation theory, and uses global historical information to obtain better consistency results through multiple iterations, providing highly reliable positioning results for ships during navigation and improving the robustness of the ship's GNSS / DR integrated navigation system. Summary of the invention

[0005] According to the above-mentioned problems existing in the existing ship positioning technology, a ship positioning method based on the combination of GNSS / DR factor graph is provided. As a high-reliability positioning solution, it can provide ships with high-precision navigation and positioning results to ensure the safety of ships during navigation.

[0006] The technical means adopted by the present invention are as follows:

[0007] A ship positioning method based on a GNSS / DR factor graph combination comprises the following steps:

[0008] S1. Obtaining observation information of the DR system and observation information of the GNSS system; the observation information of the DR system includes the ship heading angle information output by the compass, the longitudinal speed, absolute water speed and absolute water direction information of the ship output by the speed log, and the absolute wind direction and absolute wind speed information output by the wind speed and direction meter; the observation information of the GNSS system includes the position information output by the satellite receiver;

[0009] S2. Perform dead reckoning based on the acquired DR system observation information and taking into account the influence of wind and water currents, wherein the dead reckoning is used to obtain the further position, true heading angle and true speed of the ship; construct a DR system sensor error factor based on the sensor error of the DR system; construct a DR factor based on the position, true heading angle, true speed and DR system sensor error obtained by dead reckoning;

[0010] S3, constructing new position information by taking the difference between the acquired GNSS system position information and the position information obtained by the DR system through dead reckoning; inputting the new position information and the position increment between the current moment and the previous moment obtained by the DR system through dead reckoning into an adaptive fuzzy logic inference system, wherein the adaptive fuzzy logic inference system outputs the observation covariance prediction value of the GNSS system based on the neural network, constructing an adaptive GNSS factor according to the acquired GNSS system position information and the observation covariance prediction value of the GNSS system, and suppressing the influence of abnormal observation values;

[0011] S4. Set the state variables of the GNSS / DR combined navigation system as variable nodes, establish the state transfer equations and observation equations of the state variables of the GNSS / DR combined navigation system, and construct the factor graph model of the GNSS / DR combined navigation system by combining the DR factor, the DR system sensor error factor and the adaptive GNSS factor; establish a sliding window to determine whether the number of variable nodes in the sliding window reaches the set window size. When the set size is reached, marginalize the earliest variable node in the sliding window, construct the marginalization factor and add it to the factor graph model, otherwise, do not perform marginalization; perform global joint optimization on the constructed factor graph model to solve the optimal estimation result.

[0012] Furthermore, the DR system sensor error factor is obtained according to the following formula:

[0013]

[0014] in, is the DR system sensor error state transfer equation, ζ k is the covariance matrix of the DR system sensor error, ck and c k-1 are the state variables of the DR system sensor errors at time k and (k-1) respectively.

[0015] Furthermore, the DR factor is calculated according to the following formula:

[0016]

[0017] Among them, x k and x k-1 are the DR system state variables at time k and (k-1), is the DR system observation information at time k, Ω k represents the covariance matrix, It represents the state transfer equation of the DR system, in which the DR system sensor error state variable c k-1 The prediction update is used as input to estimate and correct the sensor errors existing in the current DR system.

[0018] Furthermore, an adaptive fuzzy logic inference system is designed to process the position increment of the DR system and the position updates of the GNSS and DR systems to obtain the predicted value of the observation covariance matrix. The predicted value of the observation covariance of the GNSS system is used to construct an adaptive observation covariance matrix, and the adaptive GNSS factor is obtained according to the following formula:

[0019]

[0020] Among them, h k (x k ) represents the observation equation of the GNSS system, is the position information obtained by the satellite receiver at time k through pseudo-range differential analysis, represents the adaptive observation covariance matrix of the GNSS system.

[0021] Furthermore, the marginalization factor is obtained according to the following formula:

[0022]

[0023] Among them, γ represents the state variable of the marginalization factor node, μ γ and Λ γ represent the mean and covariance matrix after marginalization respectively.

[0024] Furthermore, the factor graph with the DR factor, the DR system sensor error factor, the adaptive GNSS factor and the marginalization factor is globally jointly optimized, including constructing the following optimization model:

[0025]

[0026] In the formula and denote the error functions of DR factor and adaptive GNSS factor, (γ-μ γ ) represents the error function of the marginalization factor. Finally, the Levenberg-Marquardt algorithm is used to solve the above least squares formula to obtain the optimal state estimation result.

[0027] Compared with the prior art, the present invention has the following advantages:

[0028] (1) Improve fusion accuracy: Compared with traditional filtering methods, the factor graph optimization method proposed in the present invention can use the information of all state quantities as much as possible for global joint optimization, which greatly improves the fusion positioning accuracy.

[0029] (2) Reducing algorithm complexity: The present invention introduces the sliding window concept to construct marginalized factor nodes into the factor graph, which greatly reduces the computational complexity and complexity of the algorithm while retaining effective information as completely as possible. BRIEF DESCRIPTION OF THE DRAWINGS

[0030] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, 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 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.

[0031] Figure 1 The figure is a flow chart of a ship positioning method based on a GNSS / DR factor graph combination in an embodiment of the present invention.

[0032] Figure 2 Schematic diagram of the principle of the adaptive fuzzy logic reasoning system in an embodiment of the present invention.

[0033] Figure 3 This is a factor graph model based on a GNSS / DR integrated navigation system provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0034] In order to enable those skilled in the art to better understand the scheme of the present invention, the technical scheme in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work should fall within the scope of protection of the present invention.

[0035] like Figure 1 As shown, the present invention provides a ship positioning method based on a GNSS / DR factor graph combination, comprising the following steps:

[0036] S1. Obtain observation information of the DR system and the GNSS system.

[0037] Specifically, the observation information of the DR system includes the ship's heading angle information output by the compass, the ship's longitudinal speed, absolute water speed and absolute water direction information output by the speed log, and the absolute wind direction and absolute wind speed information output by the wind speed and direction meter. The observation information of the GNSS system includes the position information output by the satellite receiver.

[0038] S2. Perform track calculation based on the acquired DR system observation information and taking into account the influence of wind, current and water flow, wherein the track calculation is used to obtain the further position, true heading angle and true speed of the ship; construct a DR system sensor error factor based on the sensor error of the DR system; construct a DR factor based on the position, true heading angle, true speed and DR system sensor error obtained by track calculation.

[0039] Specifically, this step mainly includes:

[0040] S210, performing dead reckoning based on the compass and odometer observation data obtained at time k to obtain the ship position at time k.

[0041] The recursive formula for traditional dead reckoning is as follows:

[0042]

[0043] Among them, x k and k They represent the coordinate position of the ship in the northeast coordinate system at time k, x k-1 and k-1 They represent the coordinate position of the ship in the northeast coordinate system at time (k-1), v k represents the longitudinal speed of the ship at time k, θ k It represents the ship's bow angle at time k. Without considering the influence of wind and water flow, the ship's bow angle can be assumed to be the heading angle.

[0044] The present invention considers the influence of wind and water flow and remodels the DR system.

[0045] First, consider the impact of wind flow on the DR system. According to the empirical formula, the wind pressure difference can be obtained. for:

[0046]

[0047] in is the wind sheer angle at time k, is the absolute wind speed at time k, is the longitudinal speed of the ship at time k. K represents the wind pressure difference coefficient, which is usually taken as 0.5.

[0048] According to wind pressure difference Update the ship heading angle at time k by the following formula and ship speed

[0049]

[0050]

[0051] Secondly, considering the influence of water flow on the DR system, the actual speed v' of the ship under the influence of wind and water flow can be obtained. k for:

[0052]

[0053] in, is the absolute water speed at time k.

[0054] So we can get the flow pressure difference β k for:

[0055]

[0056] Further, the true heading angle θ of the ship under the influence of wind and water flow is obtained k 'for:

[0057]

[0058] In summary, formula (1) can be updated as follows:

[0059]

[0060] S220, constructing a DR system sensor error factor.

[0061] Based on the error model characteristics of the compass and the odometer, the corresponding specific sensor error model formula is constructed as follows:

[0062]

[0063] Among them, ω k and ν k are the random zero bias error increments of the compass and the odometer at time k, usually white noise obeying Gaussian distribution, ε k and They represent the bias errors of the compass and odometer at time k respectively, which are obtained by adding the bias error at time (k-1) to the random bias error increment.

[0064] Constructing DR system sensor error factors The equation is as follows:

[0065]

[0066] in, is the DR system sensor error state transfer equation, ζ k is the covariance matrix of the DR system sensor error, c k and c k-1 are the state variables of the DR system sensor error at time k and (k-1) respectively. In this embodiment, the state variable c of the DR system sensor error at time k k This includes the zero bias errors of the compass and log at time k obtained in formula (9).

[0067] Specifically, the error function of the DR system sensor error factor can be expressed as:

[0068]

[0069] S230. Construct DR factor.

[0070] The DR factor is constructed based on the position, true speed and true heading angle information obtained by the above dead reckoning and the DR system sensor error state information. The specific node equation is:

[0071]

[0072] Among them, x k and x k-1 are the DR system state variables at time k and (k-1), is the observed value of the DR system at time k, Ω k represents the covariance matrix. It represents the state transfer equation of the DR system, in which the sensor error state variable of the DR system is used as input for prediction and update, which is used to estimate and correct the sensor error existing in the current DR system.

[0073] Specifically, the error function of the DR factor can be expressed as:

[0074]

[0075] In formula (12), Δp k-1,k , Δv k-1,k and Δh k-1,kare the position, true speed and true heading angle obtained by dead reckoning at time k, and is the predicted value of position, true speed and true heading angle obtained by track calculation at time k.

[0076] S3, construct new position information by taking the difference between the acquired GNSS system position information and the position information obtained by the DR system track dead reckoning; design an adaptive fuzzy logic reasoning system and output the observation covariance prediction value of the GNSS system based on the above position information and the position increment of the current moment compared to the previous moment obtained by the DR system track dead reckoning, and construct an adaptive GNSS factor based on the acquired GNSS system position information and the observation covariance prediction value of the GNSS system to suppress the influence of abnormal observations. Among them, the adaptive fuzzy logic reasoning system includes a fuzzification layer, a rule layer, a normalization layer, a consequent layer and an output layer, such as Figure 2 shown.

[0077] Due to the inherent fragility of the GNSS system, the time-varying noise and observation errors in complex environments will cause outliers in the GNSS data. Simply taking the observation covariance matrix as a priori value for state estimation will seriously affect the accuracy and robustness of the GNSS / DR integrated navigation system.

[0078] Specifically, the step includes:

[0079] S310, using an adaptive fuzzy logic inference system to estimate the observation covariance matrix prediction value of the GNSS system in real time, such as Figure 2 shown.

[0080] Assume that the position increment of the DR system at time k is in and are their position information at time k and (k-1) respectively, and R is a real number domain. And it is assumed that the position information of GNSS and DR system at time k is in is the position information of the GNSS system at time k.

[0081] Further, and All of them are used as inputs of the adaptive fuzzy logic reasoning system, and an adaptive fuzzy logic reasoning system is designed and constructed. The system specifically includes a fuzzification layer, a rule layer, a normalization layer, a consequent layer, and an output layer.

[0082] S311. In the fuzzification layer, the position increment or new information is used to calculate the fuzzy membership value in the system. The membership function formula used is as follows:

[0083]

[0084] in, Represents input x i fuzzy set; i represents the number of inputs, specifically, the inputs are x 1 =Δe DR , x 2 =Δe DIF ; n represents the number of membership functions for each input; tmf(x i ; a, b, c) represents the triangular membership function, {a, b, c} is the boundary parameter of the function to which each input belongs, ensuring that when Δe DR and Δe DIF After input into the function, there is a corresponding membership value.

[0085] S312. In the rule layer, the IF-THEN criterion is used to represent the linear relationship between input and output variables. The specific rules are as follows:

[0086]

[0087] Among them, R i Indicates the rules, and Respectively represent the input Δe DR and Δe DIF The fuzzy set in which it is located, i represents the rule number and i=1,2,…,n 2 , where n represents the number of membership functions for each input, α i It represents the GNSS observation covariance prediction value obtained after each rule is triggered. It represents the linear relationship parameter between the predicted value after each rule is triggered and the two input values.

[0088] S313, using the quadrature method in the normalization layer to calculate the trigger strength ω of each rule i , the calculation formula is as follows:

[0089]

[0090] in, and Indicates the membership value of the input fuzzy set in the triggered rule.

[0091] Then, the normalized trigger strength is calculated The formula is as follows:

[0092]

[0093] The denominator on the right side of the equation is the sum of the trigger strengths obtained after the rules are triggered.

[0094] S314: Calculate the weighted prediction value obtained after each rule is triggered in the post-processing layer The specific formula is as follows:

[0095]

[0096] S315, summarizing and calculating all weighted values ​​in the output layer, That is the observation covariance prediction value of the GNSS system. The specific formula is as follows:

[0097]

[0098] In the adaptive fuzzy logic reasoning system, the self-adaptation is based on neural adaptive methods such as back propagation. In order to make the designed system generalizable, it is necessary to use Δe DR and Δe DIF As input, The system is trained with neural network using the training set data as labels, and after training, the boundary parameters of the membership function of the corresponding input and the linear relationship parameters between each rule output and input are obtained.

[0099] At this point, through the adaptive fuzzy logic reasoning system with a five-layer structure, the observation covariance prediction value of the GNSS system can be obtained, the error of the GNSS system can be estimated in real time, and the time-varying noise and observation gross error in the position information of the GNSS system can be effectively estimated.

[0100] Then we can get the adaptive observation covariance matrix of the GNSS system

[0101]

[0102] Where I is the three-dimensional unit matrix.

[0103] S320, build adaptive GNSS factor The node equation is as follows:

[0104]

[0105] Among them, h k (x k ) represents the observation equation of the GNSS system, is the position information obtained by the satellite receiver at time k through pseudo-range differential analysis, represents the adaptive observation covariance matrix of the GNSS system.

[0106]

[0107] Among them, Δp k , Δv kand Δh k They represent the position, true speed and true heading angle obtained by track dead reckoning at time k respectively.

[0108] Specifically, the error function of the adaptive GNSS factor can be expressed as:

[0109]

[0110] in, Represents the position information of the GNSS system at time k.

[0111] S4. Add the DR factor, DR system sensor error factor and adaptive GNSS factor to the factor graph, construct a factor graph model of the GNSS / DR combined navigation system, and set the system state variables as variable nodes. In order to reduce the amount of calculation and algorithm complexity, consider establishing a sliding window to determine whether the number of variable nodes in the sliding window has reached the set size. When the number of variable nodes in the sliding window reaches the set size, marginalize the earliest system state in the sliding window, construct a marginalization factor and add it to the factor graph, otherwise no marginalization is performed. Perform global joint optimization on the constructed factor graph model, and use the constrained nonlinear optimization library to solve the optimal result of the system state. Specifically, it includes the following steps:

[0112] S410, construct a factor graph model of the GNSS / DR integrated navigation system, set the integrated navigation system state variables as variable nodes, add the DR factor, the DR system sensor error factor and the adaptive GNSS factor into the factor graph, such as Figure 3 As shown. The equations for constructing the integrated navigation system are as follows:

[0113]

[0114] in, is the state variable of the integrated navigation system at time k, Δp k =[Δx k ,Δy k ,Δz k ] T It represents the position error of the ship in the east, north and sky directions at time k. It represents the speed error of the ship in the east, north and sky directions at time k, Δh k represents the heading angle error of the ship at time k, and ε k They represent the zero bias errors of the speedometer and compass respectively, and F k-1 represents the system state transfer matrix, Q k represents the system state noise matrix, Z k represents the system observation value, H k represents the observation equation transfer matrix, V krepresents the observation noise matrix.

[0115] S420, setting the sliding window size to w, when the system state variables are continuously added and the window size is reached, the earliest system state in the window is marginalized using the Schur complement method, and when the window size is not reached, no marginalization is performed.

[0116] Constructing marginalization factors The specific formula is as follows:

[0117]

[0118] Among them, γ represents the state variable of the marginalization factor node, μ γ and Λ γ represent the mean and covariance matrix after marginalization using the Schur's complement method.

[0119] S430. According to the existing factor graph theory, the maximum a posteriori probability estimation formula can be used to minimize the residual of the entire system, and the least squares estimation formula is as follows:

[0120]

[0121] In the formula and denote the error functions of DR factor and adaptive GNSS factor, (γ-μ γ ) represents the error function of the marginalization factor. Finally, the Levenberg-Marquardt algorithm is used to solve the above least squares formula to obtain the optimal state estimation result.

[0122] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A ship positioning method based on a GNSS / DR factor graph combination, characterized in that: The following steps are involved: S1. Obtaining observation information of the DR system and observation information of the GNSS system; the observation information of the DR system includes the ship heading angle information output by the compass, the longitudinal speed, absolute water speed and absolute water direction information of the ship output by the speed log, and the absolute wind direction and absolute wind speed information output by the wind speed and direction meter; the observation information of the GNSS system includes the position information output by the satellite receiver; S2. Perform dead reckoning based on the acquired DR system observation information and taking into account the influence of wind and water currents, wherein the dead reckoning is used to obtain the further position, true heading angle and true speed of the ship; construct a DR system sensor error factor based on the sensor error of the DR system; construct a DR factor based on the position, true heading angle, true speed and DR system sensor error obtained by dead reckoning; S3, constructing new position information by taking the difference between the acquired GNSS system position information and the position information obtained by the DR system through dead reckoning; The position update information and the position increment of the current moment compared to the previous moment obtained by the DR system through dead reckoning are input into an adaptive fuzzy logic reasoning system, the adaptive fuzzy logic reasoning system outputs the observation covariance prediction value of the GNSS system based on the neural network, and constructs an adaptive GNSS factor according to the position information of the GNSS system and the observation covariance prediction value of the GNSS system to suppress the influence of abnormal observation values; S4, setting the state variables of the GNSS / DR integrated navigation system as variable nodes, establishing the state transfer equations and observation equations of the state variables of the GNSS / DR integrated navigation system, and constructing the factor graph model of the GNSS / DR integrated navigation system by combining the DR factor, the DR system sensor error factor and the adaptive GNSS factor; Establish a sliding window and determine whether the number of variable nodes in the sliding window reaches the set window size. When it reaches the set size, marginalize the earliest variable node in the sliding window, construct the marginalization factor and add it to the factor graph model. Otherwise, no marginalization is performed. The constructed factor graph model is globally jointly optimized to obtain the optimal estimation result.

2. A ship positioning method based on GNSS / DR factor graph combination according to claim 1, characterized in that: The DR system sensor error factor is calculated according to the following formula: in, is the DR system sensor error state transfer equation, ζ k is the covariance matrix of the DR system sensor error, c k and c k-1 are the state variables of the DR system sensor errors at time k and (k-1) respectively.

3. A ship positioning method based on GNSS / DR factor graph combination according to claim 2, characterized in that: The DR factor is calculated according to the following formula: Among them, x k and x k-1 are the DR system state variables at time k and (k-1), is the DR system observation information at time k, Ω k represents the covariance matrix, It represents the state transfer equation of the DR system, in which the DR system sensor error state variable c k-1 The prediction update is used as input to estimate and correct the sensor errors existing in the current DR system.

4. A ship positioning method based on GNSS / DR factor graph combination according to claim 3, characterized in that: An adaptive fuzzy logic inference system is designed to process the position increment of the DR system and the position updates of the GNSS and DR systems to obtain the predicted value of the observation covariance matrix. The predicted value of the observation covariance of the GNSS system is used to construct an adaptive observation covariance matrix, and the adaptive GNSS factor is obtained according to the following formula: Among them, h k (x k ) represents the observation equation of the GNSS system, is the position information obtained by the satellite receiver at time k through pseudo-range differential analysis, represents the adaptive observation covariance matrix of the GNSS system.

5. A ship positioning method based on GNSS / DR factor graph combination according to claim 4, characterized in that: The marginalization factor is obtained according to the following formula: Among them, γ represents the state variable of the marginalization factor node, μ γ and Λ γ represent the mean and covariance matrix after marginalization respectively.

6. A ship positioning method based on GNSS / DR factor graph combination according to claim 4, characterized in that: The factor graph with DR factor, DR system sensor error factor, adaptive GNSS factor and marginalization factor is globally jointly optimized, including constructing the following optimization model: In the formula and denote the error functions of DR factor and adaptive GNSS factor, (γ-μ γ ) represents the error function of the marginalization factor. Finally, the Levenberg-Marquardt algorithm is used to solve the above least squares formula to obtain the optimal state estimation result.

Images