Rotation Matrix-Based Factor Graph Cooperative Localization Algorithm

Abstract

The present disclosure designs a rotation matrix-based factor graph cooperative localization algorithm. Firstly, the reasons of sudden increase in an error in an operation process of a conventional factor graph cooperative localization algorithm are analyzed; secondly, a rotation matrix is designed, and the size of a rotation angle is determined; then, a rotation matrix-based cooperative localization algorithm factor graph model is constructed, and a specific algorithm flow is designed; and finally, filtering fusion estimation is performed on position status information of a slave boat. Therefore, coordinate values of master and slave boats can be transformed within a factor graph in real time without changing the measurement accuracy of an inertial device in a system to cause the coordinates of the master and slave boats that participate in the calculation of the factor graph to be inconsistent, thereby solving the problem of sudden increase in a localization error of the conventional factor graph cooperative localization algorithm, and improving the robustness of the cooperative localization system.

Claims

1. A method, comprising the following steps: step 1: constructing a rotation matrix-based cooperative localization algorithm factor graph model; and step 2: establishing a rotation matrix-based cooperative localization algorithm, and filtering and updating position status information of a system.

2. The method according to claim 1, wherein in the step 1, rotation matrix factor nodes are added into a factor graph algorithm flow with the following concept: distance information of master and slave boats enter the factor graph algorithm flow by means of a node F.sub.i; a priori estimate of a position of the slave boat enters the factor graph algorithm flow by means of nodes A and B; then a position of the slave boat is rotated to change by means of a node T.sub.i; absolute position information of the master and slave boats are transformed into relative position information by means of nodes C.sub.i and D.sub.i; and finally, the position information of the master and slave boats are fused by means of a node E.sub.i.

3. The method according to claim 1, wherein in the step 2, a rotation matrix is constructed and a rotation angle is designed; a form of the rotation matrix is as follows: $C=\left[\begin{array}{cc}\cos \theta & \sin \theta \\ -\sin \theta & \cos \theta \end{array}\right],$ a determination method for a value of θ is as follows: supposing that coordinates of the master boat are (x.sub.m,y.sub.m) and coordinates of the slave boat are (x.sub.s,y.sub.s), the coordinates are transformed into (x′.sub.m,y′.sub.m) and (x′.sub.s,y′.sub.s) via a transformation matrix C through the following transformation process: $\left[\begin{array}{c}{x}_m^{\prime }\\ y_m^{\prime }\end{array}\right]=\left[\begin{array}{cc}\cos \theta & \sin \theta \\ -\sin \theta & \cos \theta \end{array}\right]\left[\begin{array}{c}{x}_m\\ y_m\end{array}\right]\left[\begin{array}{c}{x}_s^{\prime }\\ y_s^{\prime }\end{array}\right]=\left[\begin{array}{cc}\cos \theta & \sin \theta \\ -\sin \theta & \cos \theta \end{array}\right]\left[\begin{array}{c}{x}_s\\ y_s\end{array}\right],$ when estimated coordinates of the slave boat have an error, the following formula is obtained: $\left[\begin{array}{c}{x}_s^{\prime }+{\tilde{x}}^{\prime }\\ y_s^{\prime }+{\tilde{y}}^{\prime }\end{array}\right]=\left[\begin{array}{cc}\cos \theta & \sin \theta \\ -\sin \theta & \cos \theta \end{array}\right]\left[\begin{array}{c}{x}_s+\tilde{x}\\ y_s+\tilde{y}\end{array}\right],$ where {tilde over (x)} represents an error of the slave boat at an x axis of an original coordinate system; {tilde over (y)} represents an error of the slave boat at a y axis of the original coordinate system; {tilde over (x)}′ represents an error of the slave boat at the x axis after the transformation matrix; {tilde over (y)}′ represents an error of the slave boat at the y axis after the transformation matrix; an error term can be further calculated as: $\{\begin{array}{c}{\tilde{x}}^{\prime }=\tilde{x}\cos \theta +\tilde{y}\sin \theta \\ {\tilde{y}}^{\prime }=-\tilde{x}\sin \theta +\tilde{y}\cos \theta \end{array},$ a probability density function transmitted from a factor node E.sub.n to a variable node Δy.sub.n can be expressed as: $N\left(\Delta y_n,\pm \sqrt{d_n^2-{\left(\Delta x_n^{-}\right)}^2},\frac{{\left(\Delta x_n^{-}\right)}^2{\sigma }_{x_n^{-}}^2+d_n^2{\sigma }_{d_n}^2}{d_n^2-{\left(\Delta x_n^{-}\right)}^2}\right),$ where Δx.sub.n.sup.− represents an x coordinate difference between the master boat and the slave boat in a previous iteration process; and σ.sub.x.sub.n.sub.−.sup.2 represents a corresponding variance. an expectation value E(Δy.sub.n) of Δy.sub.n can be further expressed as: $E\left(\Delta y_n\right)=\pm \sqrt{d_n^2-{\left(\Delta x_n^{-}\right)}^2},$ a function f(x)=√d.sub.n.sup.2−x.sup.2 is set, and is subjected to first-order Taylor expansion to obtain: $f\left(x\right)\approx \sqrt{d_n^2-x_0^2}+\frac{x_0}{\sqrt{d_n^2-x_0^2}}\left(x-x_0\right),$ then: $f\left(x_0+\tilde{x}\right)-f\left(x_0\right)\approx \sqrt{d_n^2-x_0^2}+\frac{x_0}{\sqrt{d_n^2-x_0^2}}\tilde{x}-\sqrt{d_n^2-x_0^2}=\frac{x_0}{\sqrt{d_n^2-x_0^2}}\tilde{x},$ in combination with the above error term, that the following formula is obtained: $\{\begin{array}{c}\Delta \tilde{x}=\pm \frac{y_m^{\prime }-y_s^{\prime }}{\sqrt{d_n^2-{\left(y_m^{\prime }-y_s^{\prime }\right)}^2}}{\tilde{y}}^{\prime }\\ \Delta \tilde{y}=\pm \frac{x_m^{\prime }-x_s^{\prime }}{\sqrt{d_n^2-{\left(x_m^{\prime }-x_s^{\prime }\right)}^2}}{\tilde{x}}^{\prime }\end{array},$ in order to avoid a phenomenon of increase in the error, the following formula needs to be met: $\{\begin{array}{c}{d}_n^2-{\left(y_m^{\prime }-y_s^{\prime }\right)}^2\ne 0\\ d_n^2-{\left(x_m^{\prime }-x_s^{\prime }\right)}^2\ne 0\end{array},$ by means of a constraint relation of the formula d.sub.n.sup.2=Δx.sub.n.sup.2+Δy.sub.n.sup.2, the above formula is further simplified into: $\hspace{1em}\{\begin{array}{c}{x}_m^{\prime }-x_s^{\prime }\ne 0\\ y_m^{\prime }-y_s^{\prime }\ne 0\end{array},$ according to the above formulas, conditional expressions that θ should meet are as follows: $\theta \ne {\theta }_1,{\theta }_1=\arctan \left(\frac{y_s-y_m}{x_s-x_m}\right)$ $\theta \ne {\theta }_2,{\theta }_2=\arctan \left(\frac{x_s-x_m}{y_s-y_m}\right),$ in consideration of an actual situation, θ.sub.1≠θ.sub.2, and θ can be selected as:
θ=(θ.sub.1+θ.sub.2)/2.

Description

BRIEF DESCRIPTION OF FIGURES

[0044] FIG. 1 is a constructed rotation matrix-based cooperative localization algorithm factor graph model.

[0045] FIG. 2 is a movement path diagram of a master boat and a slave boat in a simulation experiment.

[0046] FIG. 3 is a graph showing a localization error in a simulation experiment.

DETAILED DESCRIPTION

[0047] The present disclosure is described in detail below in combination with specific embodiments.

[0048] The present disclosure designs a rotation matrix-based factor graph cooperative localization algorithm. Coordinate values of a master boat and a slave boat can be transformed within a factor graph in real time by means of introducing a rotation matrix to cause the coordinates of the master boat and the slave boat that participate in the calculation of the factor graph to be inconsistent, thereby avoiding the phenomenon of sudden increase in a cooperative localization error caused by the fact that the coordinates of the master and slave boats in a certain direction are close or the same in an operation process of the master and slave boats, improving the capability of a cooperative localization system and enhancing the robustness of the system. The objective of the present disclosure can be realized through the following steps:

[0049] 1. a rotation matrix-based cooperative localization algorithm factor graph model is constructed;

[0050] 2. a rotation matrix-based factor graph cooperative localization algorithm is established; and

[0051] 3. filtering fusion estimation is performed on a position status of the system by means of a transmitted probability density function.

[0052] In order to verify the effectiveness of the present disclosure, software is used to simulate the rotation matrix-based factor graph cooperative localization algorithm.

[0053] As shown in FIG. 1, the constructed rotation matrix-based cooperative localization algorithm factor graph model is illustrated. In the drawing, ({circumflex over (x)}.sub.k.sup.−,ŷ.sub.k.sup.−) represents one-step forecasting of the position status; (x,y) represents a position status after the filtering fusion estimation; represents a position status processed by the rotation matrix represented by the factor node T.sub.i; (Δx,Δy) represents relative position information between the master and slave boats; {circumflex over (d)}.sub.i represents measurement information between the master and slave boats; and d.sub.i represents d.sub.i=√{square root over (Δx.sup.2+Δy.sup.2)}.

[0054] FIG. 2 illustrates movement simulation of the master and slave boats. Simulation conditions: the master boat 1: initial position (x.sub.m1,y.sub.m1)=(−200 m, −400 m), speed 2 m/s, and heading 90°; the master boat 2: initial position (x.sub.m2,y.sub.m2)=(1800 m, −400 m), speed 2 m/s, and heading 90°; the slave boat: initial position (0 m, 0 m), speed 2 m/s, and heading 30°. During simulation, speed noise σ.sub.v.sup.2=(0.5 m/s).sup.2, acceleration noise σ.sub.α.sup.2=(0.01 m/s.sup.2).sup.2, gyroscope measurement noise σ.sub.ϕ.sup.2=(1°).sup.2 and hydroacoustic range observation noise σ.sub.d.sub.1.sup.2=σ.sub.d.sub.2.sup.2=(2 m).sup.2 of the slave boat are all irrelevant additive noise. A filtering algorithm is used to estimate and update position information, and a cycle for each filtering estimation is 1 s. Moreover, the extended Kalman filter (EKF) algorithm, the unscented Kalman filter (UKF) algorithm and the conventional factor graph cooperative localization (FGAS) algorithm are compared with the rotation matrix-based factor graph cooperative localization (RMFGAS) algorithm designed by the present disclosure.

[0055] It can be seen from FIG. 3 that when the system is operated to about 400 s, the localization error of the FGAS algorithm suddenly increases. According to the simulation conditions, elapsed time t when the y axis coordinates of the master boat and the slave boat are the same, can be calculated as:

[00014]$t=\frac{\Delta y_0}{\Delta v_y}=\frac{0-\left(-400\right)}{2-2\times \sin \left(30^{°}\right)}=400s$

where Δy.sub.0 represents an initial distance between the master boat and the slave boat in a y axis direction; and

[0056] Δv.sub.y represents a speed difference between the master boat and the slave boat in the y axis direction.

[0057] It can be seen that a common factor graph cooperative localization algorithm has the phenomenon of sudden increase in the localization error when the coordinates of the master and slave boats in a certain direction are close or the same. However, the rotation matrix-based factor graph cooperative localization algorithm provided by the present disclosure can always maintain a smaller cooperative error. At the same time, it can be seen that the overall cooperative performance of the RMFGAS algorithm is better than that of the EKF and the UKF. In addition, it should be noted that the localization errors of the RMFGAS algorithm, the EKF algorithm, and the UKF algorithm all increase first and then decrease. This is caused by changes in the observability of the system, and this problem cannot be solved by algorithms.

[0058] The above experiments verify the effectiveness of the rotation matrix-based factor graph cooperative localization algorithm of the present disclosure, the coordinate values of the master and slave boats can be transformed within the factor graph in real time without reducing the measurement accuracy of an inertial element in the cooperative system to cause the coordinates of the master and slave boats that participate in the calculation of the factor graph to be inconsistent, thereby solving the problem of sudden increase in the localization error of the conventional factor graph cooperative localization algorithm, which is caused when the coordinates of the master and slave boats in a certain direction are close or the same, and improving the robustness of the cooperative localization system.