Multi-unmanned ship data fusion method based on pose map

Abstract

The invention discloses a multi-unmanned ship data fusion method based on a pose map, and relates to the technical field of unmanned ships. For data collected by sensors such as a camera and a laser radar in an unmanned ship cluster, GPS information and observation information are fused, and a fusion algorithm for improving data alignment precision is designed. The alignment of the GPS information and the observation information of each unmanned ship in the time dimension is realized by adopting a linear interpolation technology, and the data time synchronization is ensured; and constructing a model based on the pose map, calculating the relative poses among the unmanned ships, and converting various sensor data to the same coordinate system to realize data alignment and fusion in space. According to the method, the influence of sea wave, water flow and other environmental disturbances on the attitude of the unmanned ship is considered, a six-degree-of-freedom pose is adopted for modeling, GPS information and observation information are jointly introduced into a pose graph, a closed-form Jacobian matrix corresponding to each error term is deduced, the graph model is iteratively solved through a nonlinear optimization method, and the attitude of the unmanned ship is calculated. And the precision and robustness of multi-source data fusion are effectively improved.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned ships, and in particular to a data fusion method for multiple unmanned ships based on a pose graph. Background Art

[0002] With the rapid development of unmanned vessel technology, the coordinated execution of multiple unmanned vessels (UAVs) has become a key development trend. In applications such as collaborative perception, environmental mapping, and surface inspection, data acquired by each UAV needs to be fused within a unified reference frame to form a complete and accurate representation of the environment. The key to achieving data fusion is accurately determining the relative pose relationships between the UAVs. Traditional pose estimation methods primarily rely on GPS information, which can provide basic positioning information in open waters. However, in complex environments such as ports, inland rivers, and near islands and reefs, GPS signals are susceptible to interference such as occlusion, resulting in significantly increased positioning errors. Furthermore, GPS lacks complete pose information, limiting the ability to align and fuse data based on geometric consistency. In recent years, vision-based observation techniques have made significant progress in surface scenarios. Image-based 6D pose estimation algorithms estimate the complete pose of the UAV in space by identifying feature points and establishing geometric relationships with the UAV model. These algorithms offer high update rates and rich pose information, demonstrating excellent performance in close-range collaboration and local perception.

[0003] Therefore, technicians in this field are committed to developing a multi-unmanned ship data fusion method based on pose graph. The present invention proposes a pose estimation method that integrates GPS information and visual observation information. While maintaining global positioning capabilities in wide-area scenarios, it introduces observation information to improve local estimation accuracy and ensure the continuity and stability of pose estimation. On this basis, a multi-unmanned ship collaborative estimation framework based on graph optimization is further constructed to uniformly integrate the observation information of each ship and obtain accurate relative pose relationships, thereby realizing spatial alignment and fusion of data. In addition, in order to improve the computational efficiency and numerical stability of the graph optimization process, the present invention derives a closed-form Jacobian matrix for the constructed pose graph structure, so that the optimization process can converge stably and quickly, providing efficient and reliable pose estimation support for multi-source data fusion tasks. Summary of the Invention

[0004] In view of the above-mentioned defects of the prior art, the technical problem to be solved by the present invention is to improve the alignment and fusion accuracy of multi-sensor data in an unmanned ship cluster.

[0005] To achieve the above object, the present invention provides a multi-unmanned vessel data fusion method based on a pose graph, comprising the following steps:

[0006] Step 1: Convert GPS data to the local coordinate system, and synchronize the GPS data of each unmanned vessel with the observation information;

[0007] Step 2: Construct a pose graph with the UAV pose as nodes and GPS data and observation information as edges.

[0008] Step 3: Calculate the Jacobian matrix of each edge of the pose graph;

[0009] Step 4: Iteratively solve the position of each unmanned ship;

[0010] Step 5: Fusion of multiple unmanned vessel data.

[0011] Furthermore, in step 1, the GPS data and observation information of each unmanned vessel are time synchronized by linear interpolation.

[0012] Furthermore, in step 2, the six-degree-of-freedom posture of the unmanned ship is used as a node.

[0013] Furthermore, the six-degree-of-freedom posture of the unmanned ship includes a three-dimensional position and a three-dimensional posture.

[0014] Furthermore, in step 2, the GPS data and the observation information are jointly constructed into an error term as a constraint relationship of the edge in the pose graph.

[0015] Furthermore, the pose graph uses the error between the estimated relative pose between two nodes and the actual observation value as the optimization objective function.

[0016] Furthermore, in step 4, the pose graph is iteratively solved by a nonlinear optimization algorithm.

[0017] Furthermore, in step 4, the Hessian matrix and gradient vector of the pose graph are calculated, and the pose of the unmanned ship is solved iteratively.

[0018] Furthermore, in step 5, the relative positions of the unmanned vessels are calculated and the sensor data are converted into the same coordinate system.

[0019] Furthermore, the sensor data includes camera data and lidar data carried by each unmanned vessel.

[0020] The data fusion process for surface unmanned vessels requires the unification of spatial reference frames based on the relative poses between each vessel. However, most existing relative pose measurement methods rely solely on GPS information or visual observation information, making it difficult to achieve high-precision results in environments with severe occlusion or dynamic interference. Although some studies have attempted to fuse GPS and observation information, most have been limited to fusion processing within a single vessel and have failed to achieve the coordinated utilization of observation information from multiple unmanned vessels, limiting the positioning accuracy and fusion capabilities of the overall system in large-scale collaborative tasks. This paper proposes a multi-unmanned vessel data fusion method based on pose graph optimization, which unifies the GPS information and visual observation information of each unmanned vessel in a graph optimization framework. This method uses the poses of the unmanned vessels as nodes in the pose graph, constructs edges using GPS and observation information to represent constraint relationships, and utilizes a graph optimization algorithm to efficiently solve the relative poses between the unmanned vessels, achieving information fusion across multiple platforms. The present invention first synchronizes the GPS and observation data acquired by multiple unmanned vessels in time to ensure consistency between different information sources under the same time reference. The pose of each unmanned vessel is then used as a node in the graph. Edges containing GPS and observation information are constructed as constraints, and the error between the estimated relative pose between two nodes and the actual observation is used as the optimization objective function. By performing nonlinear optimization on the entire graph structure, multi-source information can be integrated within a unified framework to obtain highly accurate relative pose estimation results.

[0021] In the collaborative process of multiple unmanned vessels, the effective fusion of data depends on the accurate acquisition and spatial alignment of the relative postures between each unmanned vessel. Existing methods mostly use three-dimensional posture parameters (lateral, longitudinal, yaw angle) to model the posture of unmanned vessels, and generally assume that the unmanned vessels have no changes or small changes in depth, pitch angle, and roll angle. However, in actual water surface environments, affected by complex disturbance factors such as water flow and waves, unmanned vessels usually exhibit six-degree-of-freedom motion characteristics. Traditional three-dimensional posture modeling methods are difficult to fully characterize their true posture, thereby reducing the accuracy of spatial alignment and fusion of multiple unmanned vessel data. In view of the characteristics of the posture changes of unmanned vessels in six-dimensional space, the present invention proposes a graph optimization method based on six-dimensional posture modeling. This method models the posture state of each unmanned vessel in a six-dimensional form (including three-dimensional position and three-dimensional posture) in the posture graph, and integrates the observation information from each unmanned vessel with GPS information. The relative six-dimensional posture between each unmanned vessel is jointly solved through a graph optimization algorithm, which significantly improves the posture estimation accuracy in complex environments and the adaptability of multi-source data fusion. The present invention first aligns the observation information of each unmanned vessel with GPS data based on a time synchronization mechanism. The six-dimensional pose of each unmanned vessel at each moment is used as an optimization variable in the pose graph. Subsequently, an error term for the six-dimensional relative pose between the unmanned vessels is constructed based on the observation information, and an error term for the two-dimensional position of the unmanned vessels is constructed based on the GPS data. By jointly optimizing these error functions, the six-dimensional pose variables of each unmanned vessel in the pose graph are guided to converge to the optimal solution, thereby accurately estimating the relative pose between unmanned vessels in a disturbed environment.

[0022] In the relative attitude estimation of unmanned ships based on pose graphs, the calculation of the Jacobian matrix is ​​a key step in achieving efficient iterative optimization. The current mainstream methods mostly use numerical differences to obtain the Jacobian matrix, which has problems such as the dependence of calculation accuracy on the perturbation step size, slow convergence speed, and instability in certain scenarios, which limits the practicality and real-time performance of the attitude estimation algorithm in actual complex environments. Based on the construction of a pose graph by fusing GPS information and observation information, the present invention derives the closed form Jacobian matrix of the error term with respect to the pose variables, thereby eliminating the need for numerical approximate calculations during the iterative solution process, significantly improving the stability and computational efficiency of the pose optimization process. After defining the error function of each edge in the pose graph, the present invention, based on Lie group theory and perturbation model, applies a small left perturbation to the pose variables, and combines the derivative definition to differentiate the error function to derive the closed Jacobian expression of the error term with respect to the six-dimensional pose variables, and then performs iterative updates of the pose based on it.

[0023] The present invention discloses a multi-unmanned vessel data fusion method based on a pose graph, which aims to improve the alignment and fusion accuracy of multi-sensor data in a cluster. For the data collected by sensors such as cameras and lidars in an unmanned vessel cluster, the present invention fuses GPS information and observation information, and designs a fusion algorithm to improve the accuracy of data alignment. The method first uses linear interpolation technology to achieve the alignment of GPS information and observation information collected by each unmanned vessel in the time dimension to ensure the time synchronization of the data; then, a model is constructed based on the pose graph to calculate the relative poses between each unmanned vessel, and the data of various sensors are converted to the same coordinate system to achieve spatial data alignment and fusion. In the relative pose estimation process, the present invention considers the influence of environmental disturbances such as waves and water currents on the posture of the unmanned vessel, adopts six-degree-of-freedom pose for modeling, and jointly introduces GPS information and observation information into the pose graph, derives the closed-form Jacobian matrix corresponding to each error term, and iteratively solves the graph model through a nonlinear optimization method, thereby effectively improving the accuracy and robustness of multi-source data fusion.

[0024] Compared with the prior art, the present invention has the following obvious substantial features and significant advantages:

[0025] 1. The method of the present invention integrates GPS and visual observation data of multiple unmanned ships, effectively improving the accuracy and robustness of pose estimation.

[0026] 2. By introducing six-dimensional pose modeling and graph optimization mechanisms, this invention can fully reflect the true motion characteristics of unmanned surface vessels in complex sea conditions, significantly improving the accuracy of relative pose estimation. Furthermore, by integrating multi-source information to construct a unified graph optimization model, the accuracy of the system's spatial alignment and fusion is enhanced.

[0027] 3. By deriving the closed-form Jacobian matrix, the present invention significantly improves the iteration efficiency and solution stability in the pose graph optimization process, effectively reducing the computational resource overhead.

[0028] The concept, specific structure and technical effects of the present invention will be further described below in conjunction with the accompanying drawings to fully understand the purpose, characteristics and effects of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] Figure 1 It is an algorithm flow chart of a preferred embodiment of the present invention;

[0030] Figure 2 It is a pose graph of a preferred embodiment of the present invention. DETAILED DESCRIPTION

[0031] The following describes several preferred embodiments of the present invention with reference to the accompanying drawings to make its technical content clearer and easier to understand. The present invention can be embodied in many different forms of embodiments, and the scope of protection of the present invention is not limited to the embodiments mentioned herein.

[0032] In the drawings, components with identical structures are denoted by the same reference numerals, and components with similar structures or functions are denoted by similar reference numerals. The size and thickness of each component shown in the drawings are arbitrary and are not limited by the present invention. For clarity, the thickness of components in some places in the drawings is appropriately exaggerated.

[0033] Aiming at the problem of data fusion of multiple unmanned ships, the present invention proposes a data alignment and fusion method based on pose graph. The method first performs coordinate conversion on the GPS data and converts it into the local coordinate system, and then synchronizes the GPS information and observation information of each unmanned ship through linear interpolation to ensure the consistency of the data in the time dimension. On this basis, a pose graph model with the six-degree-of-freedom pose of the unmanned ship as the graph node is constructed, and the GPS data and the observation data are jointly constructed to construct the error term as the constraint relationship of the edge in the graph. Furthermore, the Jacobian matrix corresponding to each edge is derived, and a nonlinear optimization algorithm is introduced to iteratively solve the pose graph, so as to obtain a high-precision relative pose estimate of each unmanned ship. Based on the estimation result, the multi-source sensor data such as cameras and lidars carried by each unmanned ship are uniformly converted to the same coordinate system to achieve data alignment and fusion in the spatial dimension. This method provides high-precision input support for collaborative perception and intelligent decision-making of multiple unmanned ships. The algorithm flow is as follows Figure 1 The specific steps are as follows:

[0034] (1) Step 1: Convert the GPS data to the local coordinate system and synchronize the GPS information and observation information of multiple unmanned vessels based on linear interpolation technology.

[0035] (11) Step 11: Construct the local coordinate system. Select an unmanned ship with stable movement and small changes in pitch and roll angles as the reference ship, and record its latitude and longitude as Lat loc ,Lon loc , the angle between the bow and the north direction is θ loc With the bow direction as the x-axis and the vertical upward direction as the z-axis, a right-handed coordinate system is constructed as the local coordinate system.

[0036] (12) Step 12: Coordinate conversion. Convert the GPS longitude and latitude data (Lat i ,Lon i ) is converted to (Lat loc ,Lon loc ) is the coordinate in the East-North-Sky (ENU) coordinate system with the origin

[0037]

[0038] Combined with the reference ship heading θ loc , and further convert it into coordinates in the local coordinate system

[0039]

[0040] (13) Step 13: Time alignment. Perform linear interpolation on the converted GPS data and observation data. Let the interpolation time be t∈(t k-1 ,t k ), where t k-1 and t k are the two most recent sampling moments before and after the data, respectively. The interpolated GPS position of the i-th unmanned ship is:

[0041]

[0042] For the i-th ship, the relative position of the j-th ship is obtained The interpolated data is:

[0043]

[0044] (2) Step 2: After each data synchronization, a pose graph is constructed based on GPS and observation information, and the error of each edge in the graph and the overall cost function are calculated.

[0045] (21) Step 21: Construct a pose graph. The pose graph uses the pose of each unmanned ship as a node and is expressed in the form of Lie algebra. Let the pose of the i-th unmanned ship ξ i =[τ i ,r i ] T , where τ i =[u i ,v i ,w i ] T and r i =[α i ,β i ,γ i ] T Represent the translation vector and attitude vector respectively. for ξ i The Lie algebraic form of the two is:

[0046]

[0047] in, In J imiddle, and [r i ] × They are vector ξ i and r i The matrix representation of θ i =||r i ||2 represents vector r i The position of each unmanned ship can be expressed as p i =J i τ i =[x i ,y i ,z i ] T When the i-th ship can observe the relative position T of the j-th ship ij When , the edge i→j is added to the pose graph. The set ε contains all such edges in the graph. The pose graph is as follows Figure 2 shown.

[0048] (22) Step 22: For edge i→j in the pose graph, calculate its observation error The observation error is defined as the pose observation value and pose estimates The difference between The superscript represents the transformation from Lie group to Lie algebra, Represents the pose estimate at each iteration.

[0049] (23) Step 23: Calculate the GPS error for each edge For edge i→j, first calculate the horizontal and vertical coordinate differences between unmanned ship i and unmanned ship j generated by the estimated value Then calculate the horizontal and vertical coordinate differences between unmanned ship i and unmanned ship j obtained by GPS GPS Error is the difference between the estimated difference and the GPS calculated difference, that is,

[0050] (24) Step 24: Calculate the error of each edge in the pose graph and the overall cost function. and GPS error Merge to get the error of each edge Then the sum of squared errors of all edges is used as the overall cost function F(ξ), which is expressed as

[0051] (3) Step 3: Calculate the Jacobian matrix of each edge of the pose graph by applying a small left perturbation. For the unmanned ship i in the edge i→j∈ε, its Jacobian matrix is in and The Jacobian matrices corresponding to the observation error and GPS error are

[0052] (31) Step 31: Calculate the Jacobian matrix of the observation error The observation error after applying a small left perturbation is:

[0053]

[0054] in represents the multiplication between Lie algebras, δξ i To be applied on ξ i A small left perturbation on . Using the adjoint representation, Performing Taylor expansion yields:

[0055]

[0056] Substituting the above expansion into We can get:

[0057]

[0058] Will By linear approximation, we can get:

[0059]

[0060] in, The small perturbation applied to the Lie group is mapped to the Lie algebra. According to the definition of derivative, the Jacobian matrix of the observation error with respect to the unmanned ship i is It can then be calculated as

[0061]

[0062] Similarly, the Jacobian matrix of the observation error with respect to the unmanned ship j is Can be calculated as:

[0063]

[0064] (32) Step 32: Calculate the Jacobian matrix of GPS error For the i-th unmanned ship ξ i , applying a small left perturbation δξ i =[δ u ,δ v ,δ w ,δ α ,δ β ,δ γ ] T After, position It can be calculated by the following formula:

[0065]

[0066] exist middle, The calculation formula is:

[0067]

[0068] in, The calculation formula is:

[0069]

[0070] When a perturbation is applied to only a single variable and the other variables are kept zero, Can be simplified to:

[0071]

[0072] Can be simplified to:

[0073]

[0074] Combined with the definition of derivative, we can calculate have to:

[0075]

[0076] Similarly, we can calculate have to:

[0077]

[0078] (4) Step 4: Iteratively solve the UAV pose. First, accumulate the square sum of all edge Jacobian matrix errors and use it to calculate the Hessian matrix H and gradient vector b. The calculation formula is:

[0079]

[0080] Among them, H ij and b i Represent the matrix blocks related to the unmanned ship i and j in H and b respectively. Then, the pose increment δξ is obtained by solving the linear equation system Hδξ=-b, and the pose is iteratively updated. The update formula is Repeat the above iteration until the until.

[0081] (5) Step 5: Multi-UAV data fusion. For the sensor data collected by the i-th UAV, it is first represented in point cloud format. The specific form is as follows:

[0082]

[0083] On this basis, combined with the unmanned ship pose information obtained above, the point cloud data collected by each unmanned ship is uniformly converted to the same coordinate system to achieve the fusion of multi-source data. After the conversion, the point cloud data of the i-th unmanned ship is expressed as:

[0084]

[0085] The preferred embodiments of the present invention have been described in detail above. It should be understood that numerous modifications and variations based on the concepts of the present invention are possible without inventive effort by those skilled in the art. Therefore, any technical solution that can be derived by one skilled in the art through logical analysis, reasoning, or limited experimentation based on the concepts of the present invention and the prior art should be within the scope of protection defined by the claims.

Claims

1. A method for fusion of multiple unmanned vessel data based on pose graph, characterized in that: The following steps are involved: Step 1: Convert GPS data to the local coordinate system, and synchronize the GPS data of each unmanned vessel with the observation information; Step 2: Construct a pose graph with the UAV pose as nodes and GPS data and observation information as edges. Step 3: Calculate the Jacobian matrix of each edge of the pose graph; Step 4: Iteratively solve the position of each unmanned ship; Step 5: Fusion of multiple unmanned vessel data.

2. The method for fusion of multiple unmanned vessels data based on pose graph according to claim 1, characterized in that: In step 1, the GPS data and observation information of each unmanned vessel are synchronized in time by linear interpolation.

3. The method for fusion of multiple unmanned vessels data based on pose graph according to claim 1, characterized in that: In step 2, the six-degree-of-freedom posture of the unmanned ship is used as a node.

4. The method for fusion of multiple unmanned vessels data based on pose graph according to claim 3, characterized in that: The six-degree-of-freedom posture of the unmanned ship includes a three-dimensional position and a three-dimensional posture.

5. The method for fusion of multiple unmanned ship data based on pose graph according to claim 1, characterized in that: In step 2, GPS data and observation information are combined to construct an error term as a constraint relationship of the edge in the pose graph.

6. The method for fusion of multiple unmanned vessels data based on pose graph according to claim 1, characterized in that: The pose graph uses the error between the estimated relative pose between two nodes and the actual observation value as the optimization objective function.

7. The method for fusion of multiple unmanned vessels data based on pose graph according to claim 1, characterized in that: In step 4, the pose graph is iteratively solved using a nonlinear optimization algorithm.

8. The method for fusion of multiple unmanned vessels data based on pose graph according to claim 1, characterized in that: In step 4, the Hessian matrix and gradient vector of the pose graph are calculated, and the pose of the unmanned ship is solved iteratively.

9. The method for fusion of multiple unmanned vessel data based on pose graph according to claim 1, characterized in that: In step 5, the relative positions of the unmanned vessels are calculated and the sensor data are converted into the same coordinate system.

10. The method for fusion of multiple unmanned vessels data based on pose graph according to claim 9, characterized in that: The sensor data includes camera data and lidar data carried by each unmanned vessel.

Images