Multi-unmanned-platform collaborative navigation positioning method and system based on LLE-ICP

Abstract

The invention discloses a multi-unmanned-platform collaborative navigation positioning method and system based on LLE-ICP (Link Level-Inductively Coupled Plasma). The system comprises an air unmanned platform, a ground unmanned platform, an airborne sensor and communication equipment. The method comprises the following steps: firstly, calculating position information and relative distance of each unmanned platform relative to a reference coordinate; then each unmanned platform calculates current position information of the unmanned platform by using data information of an inertial sensor, and carries out radio distance measurement through a communication module to obtain relative distance information with other unmanned platforms; the three-dimensional position is subjected to dimension reduction to form a two-dimensional coordinate, the coordinate is subjected to space conversion, and a converted two-dimensional coordinate point set is used as collaborative navigation data to be sent to each unmanned platform; and finally, compensating height positioning information of the unmanned platforms through an altimeter to realize multi-unmanned-platform collaborative navigation in a GNSS denial environment. According to the method, the precision and the stability of collaborative navigation of the multiple unmanned platforms in the GNSS denial environment are improved.

Description

Technical Field

[0001] The present invention relates to the field of air-ground collaborative navigation technology, and in particular to a multi-unmanned platform collaborative navigation and positioning method and system based on LLE-ICP. Background Art

[0002] Drones and single-machine unmanned vehicles usually use GNSS / INS combined navigation or visual navigation methods for positioning and navigation. This type of navigation method has high positioning accuracy when there is a GNSS signal. However, when the unmanned platform is in an environment with weak or no GNSS signal, the inertial device error cannot be corrected, and the positioning error will increase over time. In scenes such as dense forests, it is difficult for visual cameras to capture reliable feature points, and the SLAM solution to the unmanned platform's posture information is poor, resulting in the inability to constrain the error divergence of the inertial device. Therefore, in these scenarios, the unmanned platform's positioning accuracy will be poor.

[0003] Single drones and unmanned vehicles (UAVs) are limited by the performance of their sensors and computing units, making them unable to fully perceive the environment. Their limited endurance prevents them from performing missions for extended periods. Furthermore, their decision-making capabilities are limited, making it difficult for them to respond quickly in dynamic and uncertain environments. These factors make it difficult for a single UAV / vehicle to independently complete complex missions, such as battlefield search, environmental surveillance, and all-weather warning and long-range attack. These complex tasks often require multi-vehicle collaboration. Multi-vehicle collaborative navigation and positioning achieves more efficient mission execution by leveraging multiple unmanned systems. Leveraging communication networks, platforms share positioning, sensor data, and environmental information, enhancing overall perception. Collaborative navigation dynamically allocates tasks, allowing each UAV / vehicle to focus on specific areas or functions, improving coverage and responsiveness. Through mutual calibration and error compensation, positioning accuracy and navigation reliability are enhanced. Furthermore, compared to single UAVs / vehicles, multi-UAV / vehicle collaborative navigation and positioning can leverage data transmission between platforms to obtain additional observations, adding constraints to the system and reducing error uncertainty. Furthermore, data sharing between platforms can alleviate the cumulative error problem inherent in single-platform inertial navigation.

[0004] Current research on collaborative navigation and positioning mostly uses methods based on visual tracking or methods that use distance to solve formation information. However, visual tracking methods are easily affected by visual occlusion in complex cross-domain environments, while methods that use distance to solve formation information often require the unmanned swarm to maintain a good formation configuration. Poor geometric configuration will produce significant errors in the overall solution, which will limit the unmanned swarm's ability to perform tasks collaboratively. Therefore, it is urgent to develop a collaborative navigation and positioning method for multiple unmanned platforms that can suppress the error divergence of the navigation system and improve navigation accuracy, so as to achieve precise navigation and collaborative positioning of multiple unmanned platforms in complex environments with GNSS denial. Summary of the Invention

[0005] The object of the present invention is to provide a method and system for collaborative navigation and positioning of multiple unmanned platforms in a GNSS-denied environment, which can suppress error divergence of the navigation system and improve navigation accuracy.

[0006] The technical solution to achieve the purpose of the present invention is: a multi-unmanned platform collaborative navigation and positioning method based on LLE-ICP, first constructing a multi-unmanned platform collaborative navigation and positioning system based on LLE-ICP, the system includes an aerial unmanned platform, a ground unmanned platform, airborne sensors and communication equipment, and the aerial unmanned platform and the ground unmanned platform are both equipped with airborne sensors and communication equipment. The navigation and positioning method includes the following steps:

[0007] Step 1: Power on the LLE-ICP-based multi-unmanned platform collaborative navigation and positioning system, use a ground unmanned platform as the reference coordinate, and calculate the position information and relative distance of each unmanned platform relative to the reference coordinate;

[0008] Step 2: Each unmanned platform uses its own inertial sensor to collect data and transmit it to the processor on board the unmanned platform;

[0009] Step 3: Each unmanned platform uses its onboard processor to calculate its position based on the collaborative navigation data from the previous moment to obtain its current position information.

[0010] Step 4: Each unmanned platform performs radio ranging through the communication module on board to obtain the relative distance information with other unmanned platforms;

[0011] Step 5: Summarize the distance information between each two unmanned platforms to form a distance matrix. Combined with the inertial sensor coordinate matrix, the three-dimensional position is reduced to two-dimensional coordinates based on the LLE nonlinear data dimensionality reduction algorithm.

[0012] Step 6: Using the positioning coordinates provided by the inertial sensor as reference points, the coordinates are spatially transformed based on the scaled ICP iterative optimization algorithm to minimize the error. Finally, the transformed two-dimensional coordinate point set is sent to each unmanned platform as collaborative navigation data;

[0013] Step 7: Use the altimeter to compensate the altitude positioning information of the unmanned platform to achieve collaborative navigation of multiple unmanned platforms in a GNSS-denied environment.

[0014] A multi-unmanned platform collaborative navigation and positioning system based on LLE-ICP, which is used to implement the multi-unmanned platform collaborative navigation and positioning method based on LLE-ICP, including an aerial unmanned platform, a ground unmanned platform, airborne sensors and communication equipment;

[0015] The aerial unmanned platform and the ground unmanned platform are both equipped with airborne sensors and communication equipment, wherein the airborne sensors include inertial sensors and barometric altimeters for obtaining inertial sensor and altitude data; the communication equipment is used to transmit positioning information between the unmanned platforms, and measure the relative distance information between the unmanned platforms through data links and UWB, perform data dimensionality reduction processing based on LLE-ICP, use the relative distance information to assist inertial navigation to locate the relative position of each unmanned platform, and then locate the absolute position of each unmanned platform based on the position relationship.

[0016] A mobile terminal includes a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the program, the multi-unmanned platform collaborative navigation and positioning method based on LLE-ICP is implemented.

[0017] A computer-readable storage medium stores a computer program, which, when executed by a processor, implements the steps of the LLE-ICP-based multi-unmanned platform collaborative navigation and positioning method.

[0018] Compared with the existing technology, the present invention has the following significant advantages: (1) utilizing the ranging information between the unmanned platforms, using UWB or data link to measure the distance, and using the obtained distance data to assist the unmanned platform in navigation, thereby realizing the collaborative formation flight of multiple unmanned platforms in a GNSS-denied environment; (2) using distance information to assist positioning, suppressing the error divergence of inertial navigation, and improving the navigation and positioning accuracy of the system; (3) using a nonlinear data dimensionality reduction iterative algorithm based on LLE-ICP to constrain the error diffusion in the horizontal direction, and using an altimeter to compensate for the height positioning information of the unmanned platform, thereby improving the stability of the collaborative navigation of multiple unmanned platforms in a GNSS-denied environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1 This is a structural diagram of a multi-unmanned platform collaborative navigation and positioning system based on LLE-ICP in the present invention.

[0020] Figure 2 The figure is a flow chart of a multi-unmanned platform collaborative navigation and positioning method based on LLE-ICP of the present invention.

[0021] Figure 3 Schematic diagram of the iterative process of the LLE nonlinear data dimensionality reduction algorithm combined with the scaled ICP algorithm in the present invention.

[0022] Figure 4 This is an error analysis curve diagram of the collaborative navigation and positioning of multiple unmanned platforms No. 1 based on LLE-ICP in an embodiment of the present invention. DETAILED DESCRIPTION

[0023] The present invention provides a multi-unmanned platform collaborative navigation and positioning method and system based on LLE-ICP. LLE is a local linear embedding data dimensionality reduction algorithm, which captures the nonlinear structure of the data by retaining the linear relationship between each data point and its local neighbors, and finally uses the reconstruction weight of the high-dimensional input vector to calculate the low-dimensional embedding coordinates of the high-dimensional data. When facing the problems of nonlinear data and global geometric distortion, LLE can effectively discover the nonlinear structure of the data and reduce the impact of nonlinear errors on the dimensionality reduction results by mining the geometric relationship between each data point and its local neighbors. ICP is an iterative closest point algorithm, which is a classic point cloud registration algorithm, mainly used for point cloud matching such as vision and lidar, and aims to minimize the error between two sets of point sets by continuously matching point pairs and optimizing the transformation matrix.

[0024] The present invention addresses the problem of unmanned platform inertial navigation errors diverging rapidly and being unable to accurately locate and detect targets in complex environments with GNSS denial. The present invention uses distance assistance based on LLE-ICP to achieve air-ground collaborative positioning and navigation. Specifically, when the unmanned platform utilizes data link or UWB communication, it obtains distance information between each unmanned platform through a two-way, one-way pseudo-range measurement method to obtain the relative position of the platform. For the obtained relative distance information, considering that the distance value measured by the equipment is usually affected by factors such as environmental noise, reflection, and multipath effects, which lead to the occurrence of measurement nonlinear errors, the LLE nonlinear dimensionality reduction algorithm is used to map the relative position in three-dimensional space to a two-dimensional plane, retaining the main geometric structure information in the x-axis and y-axis directions and ignoring changes in the height direction. For the coordinates after dimensionality reduction, the scaled ICP iterative nearest point algorithm is used to make the dimensionality reduction result approach the target coordinate system. The distance assistance information finally obtained is integrated with the information solved by its own inertial navigation to constrain the divergence of the inertial navigation error, improve positioning accuracy, and achieve precise navigation in complex environments with GNSS denial. The multi-unmanned platform collaborative navigation and positioning method based on local linear embedding dimensionality reduction proposed in the present invention can realize collaborative navigation and positioning in complex environments such as hills and forests without relying on GNSS positioning information. It relies on the relative distance information between the unmanned platforms to suppress the error divergence of the navigation system, especially in terms of error constraints in the horizontal direction, and has strong anti-interference ability.

[0025] It is easy to understand that, based on the technical solution of the present invention, without changing the essential spirit of the present invention, a person skilled in the art can imagine various embodiments of the present invention. Therefore, the following specific embodiments and drawings are only exemplary descriptions of the technical solution of the present invention, and should not be regarded as the entirety of the present invention or as limitations or restrictions on the technical solution of the present invention. It should be noted that, unless otherwise specifically stated, the relative arrangement of components and steps, numerical expressions, and numerical values ​​described in these embodiments do not limit the scope of the present invention.

[0026] like Figure 1 As shown, the present invention provides a multi-unmanned platform collaborative navigation and positioning system based on LLE-ICP, including an aerial unmanned platform, a ground unmanned platform, airborne sensors and communication equipment;

[0027] The aerial unmanned platform and the ground unmanned platform are both equipped with airborne sensors and communication equipment, wherein the airborne sensors include inertial sensors and barometric altimeters for obtaining inertial sensor and altitude data; the communication equipment is used to transmit positioning information between the unmanned platforms, and measure the relative distance information between the unmanned platforms through data links and UWB, perform data dimensionality reduction processing based on LLE-ICP, use the relative distance information to assist inertial navigation to locate the relative position of each unmanned platform, and then locate the absolute position of each unmanned platform based on the position relationship.

[0028] As a specific example, the aerial unmanned platform and the ground unmanned platform can communicate with each other and exchange position information and distance information. Each aerial unmanned platform establishes a two-dimensional point coordinate set representing the relative position of each unmanned platform based on the distance information obtained from the communication data link, based on the LLE nonlinear data dimensionality reduction algorithm and the ICP iterative closest point algorithm, to constrain the divergence of inertial navigation errors and realize collaborative navigation and positioning of multiple aerial unmanned platforms.

[0029] like Figure 2 As shown, a multi-unmanned platform collaborative navigation and positioning method based on LLE-ICP includes the following steps:

[0030] Step 1: Power on the LLE-ICP-based multi-unmanned platform collaborative navigation and positioning system, use a ground unmanned platform as the reference coordinate, and calculate the position information and relative distance of each unmanned platform relative to the reference coordinate;

[0031] Step 2: Each unmanned platform uses its own inertial sensor to collect data and transmit it to the processor on board the unmanned platform;

[0032] Step 3: Each unmanned platform uses its onboard processor to calculate its position based on the collaborative navigation data from the previous moment to obtain its current position information.

[0033] Step 4: Each unmanned platform performs radio ranging through the communication module on board to obtain the relative distance information with other unmanned platforms;

[0034] Step 5: Summarize the distance information between each two unmanned platforms to form a distance matrix. Combined with the inertial sensor coordinate matrix, the three-dimensional position is reduced to two-dimensional coordinates based on the LLE nonlinear data dimensionality reduction algorithm.

[0035] In GNSS-denied environments, inertial navigation can exhibit height errors. This can be accurately measured using sensors onboard the unmanned platform, such as altimeters, barometers, or ground-based laser rangefinders. There are many ways to constrain height errors and improve accuracy. Therefore, to simplify the problem, a local linear embedding (LLE) algorithm can be used to reduce the 3D position to 2D coordinates, with only the x- and y-axis errors corrected and controlled.

[0036] After obtaining the distance measurement information between each pair of unmanned platforms, it is combined to form a distance matrix. Combined with the inertial navigation coordinate matrix, the LLE data dimensionality reduction algorithm is used to represent each data point in the high-dimensional space as a linear combination of its neighboring points, and this local linear combination relationship is maintained during dimensionality reduction. The steps for obtaining horizontal coordinates using the LLE algorithm are as follows:

[0037] Step 5.1: Find the neighboring points of each data point

[0038] The input is assumed to be a set of N three-dimensional coordinate points and an N*N distance matrix D. For each coordinate point obtained by the inertial sensor pose solution, the k nearest neighbors are found based on the matrix D to form a set N(i):

[0039] N(i)={j|d ij is one of the first K smallest values ​​and j≠i} (1)

[0040] Step 5.2: Reconstruct each data point using a linear combination of neighboring points

[0041] The reconstruction matrix W is constructed by the distance relationship between neighboring points, that is, for each coordinate point x i It is approximated by a linear combination of k neighboring points, namely:

[0042]

[0043] Where W ij is the weight, indicating the neighboring point x j Point x i The contribution of W ij It has two characteristics: sparsity and normalization. Sparsity means that if x j Not x i The neighboring point of ij =0; normalization is expressed as follows:

[0044]

[0045] The error ε(W) after approximate reconstruction can be expressed as:

[0046] ε(W)=|x i -∑ j∈N(i) Wij x j | 2 (4)

[0047] Based on the input distance matrix D, the local covariance matrix C is used i To re-express the geometric relationship between adjacent points, set the coordinate point x i The neighbor points of are N(i), then the local covariance matrix C i for:

[0048] C i =d ip +d iq -2d pq (5)

[0049] Where, d ip is the coordinate point x i and neighboring point x ip The distance between iq is the coordinate point x i and neighboring point x iq , d pq For the neighboring point x ip and neighboring point x iq the distance between them;

[0050] When solving the weights by minimizing the local covariance matrix, in order to minimize the reconstruction error, the error is rewritten as follows:

[0051] ε(W)=W i T C i W i (6)

[0052] In order to minimize the reconstruction error and satisfy the normalization constraint at the same time, the Lagrange multiplier method is used. First, the following Lagrange function is defined:

[0053]

[0054] Where λ is the Lagrange multiplier, which is used to deal with the constraints;

[0055] In order to minimize the Lagrangian function value, W ij Take the derivative and set it to 0:

[0056]

[0057] Where E is a vector whose elements are all 1. Simplifying formula (8) we can get:

[0058]

[0059] Multiply both sides of formula (9) by E T ,get:

[0060]

[0061] Arranging formula (10) we can get the expression of λ as follows:

[0062]

[0063] Substituting formula (11) into formula (9) yields:

[0064]

[0065] Since the right side of the equation (12) (E T C i W i ) / (E T E) is a scalar, so Equation (12) can be simplified as:

[0066] C i W i =E (13)

[0067] Solve the coordinate point x from equation (13) i Reconstruct the weights, normalize the weights of all coordinate points to get the reconstruction matrix W; Step 5.3, maintain the local linear relationship and perform dimensionality reduction to obtain the reduced dimensionality coordinates

[0068] In order to maintain these local linear relationships in low-dimensional space, a global cost matrix M is first constructed:

[0069] M=(IW)(IW) T (14)

[0070] Where I is the N×N identity matrix;

[0071] In order to find a low-dimensional embedding, it is necessary to perform eigenvalue decomposition on the matrix M. The eigenvalue decomposition process is as follows:

[0072] My=λy (15)

[0073] Where λ is the eigenvalue and y is the eigenvector;

[0074] Since the coordinates after dimensionality reduction are two-dimensional, the matrix consisting of the eigenvectors corresponding to the two smallest non-zero eigenvalues ​​is selected, that is, the coordinate matrix Y after dimensionality reduction:

[0075] Y=[y1,y2] (16)

[0076] This method does not assume any global models, such as linear or nonlinear mapping functions, when reducing the distance matrix to obtain two-dimensional coordinates on the horizontal plane. Instead, it directly optimizes the position of points in low-dimensional space through weighted relationships. This allows for more flexible adaptation to complex changes in global geometry and improved adaptability to irregular formation configurations. Furthermore, in complex GNSS-denied environments, for UWB or data link ranging systems, the LLE data dimensionality reduction algorithm, compared to the commonly used MDS data dimensionality reduction algorithm, reduces the nonlinear errors in the ranging data itself and eliminates the reliance of air-to-ground unmanned platforms on formation configurations.

[0077] Obtaining two-dimensional coordinates on a horizontal plane through the LLE data dimensionality reduction algorithm essentially represents a relative geometric relationship by optimizing the objective function, rather than a value in an absolute coordinate system. After dimensionality reduction, the coordinate values ​​obtained still have a certain degree of uncertainty due to degree-of-freedom transformations such as rotation, translation, and scaling. Therefore, to obtain more accurate coordinate results, a subsequent spatial coordinate conversion step is required, which involves rotating, translating, and scaling the dimensionality reduction results and calibrating them with the reference coordinate system or the reference point position of a known point.

[0078] Step 6: Using the positioning coordinates provided by the inertial sensor as reference points, the coordinates are spatially transformed based on the scaled ICP iterative optimization algorithm to minimize the error. Finally, the transformed two-dimensional coordinate point set is sent to each unmanned platform as collaborative navigation data;

[0079] Since the LLE data dimensionality reduction algorithm adopted emphasizes local relationships, the dimensionality reduction results often have certain symmetry and uncertainty. This symmetry is usually manifested in: rotational symmetry: the dimensionality reduction result can be rotated as a whole without affecting the local structure. Mirror symmetry: the dimensionality reduction result can be flipped as a whole without affecting the local relationship. Position uncertainty: the point set after dimensionality reduction may be translated or scaled as a whole. Although the point set after dimensionality reduction is consistent with the real point set in overall structure, the specific position between the point pairs may change, resulting in unclear point pair relationships. Therefore, when transforming the spatial coordinates, the scaled ICP iterative nearest point algorithm is combined to dynamically generate point pair relationships through nearest neighbor search to autonomously find the optimal point pair relationship for iteration. For example Figure 3 As shown in the figure, the two-dimensional coordinate space transformation flow chart is as follows:

[0080] Step 6.1: Calculate the optimal point pair relationship between the current point set R and the target point set T

[0081] Using the accelerated search method, the spatial partition structure of the target point set is constructed to reduce the search complexity. All points in the target area are traversed and the point R is set. k is a point in the two-dimensional coordinate point set R, and calculates the intersection of all points in the point set T with R k The Euclidean distance of the point with the smallest distance is selected as Rk Neighbor T k :

[0082]

[0083] Step 6.2: Define the error function

[0084] In the two-dimensional coordinate alignment problem, the goal is to find an optimal set of rotation matrices, translation vectors, and scaling factors to minimize the error between the dimensionality reduction result and the reference coordinates. Since there is no GNSS satellite positioning, the reference coordinates are the predicted information obtained by inertial navigation during the integrated navigation process or given by visual positioning.

[0085] Set the coordinate point in two-dimensional space after dimensionality reduction to R i =[r1,r2], the corresponding reference coordinate point is T i =[t1,t2], the translation transformation vector is Y, and the following translation transformation can be obtained:

[0086] R i ′=R i +Y (18)

[0087] Where R i ′ represents the coordinate point after translation transformation;

[0088] The two-dimensional plane rotation matrix Q is defined as follows:

[0089]

[0090] Set the rotation factor to s, then the entire rotation, translation and scaling transformation formula is:

[0091] T i =sQR i ′=sQ(R i + Y) = sQR i +sQY (20)

[0092] For the entire coordinate point set R and reference point set T after dimensionality reduction, the translation vector can also be obtained by the translation amount of the centroid point after rotation and translation as the final translation vector Y. The transformation formula of the entire point set is as follows:

[0093] T=sQR+Y (21)

[0094] After obtaining the change function, the required error function E(s,Q1,Y) is defined as:

[0095]

[0096] Where Q1 is the initial rotation matrix, which is generally set to the unit matrix; Y is the initial translation vector, which is generally set to 0; s is the initial scaling factor, which is generally set to 1;

[0097] Step 6.3: Decentralized processing of coordinate point sets

[0098] Center the coordinate point set to facilitate solving the shift vector; first calculate the centroid of the two sets of point sets:

[0099]

[0100] Where R c Represents the centroid of the coordinate point set after dimensionality reduction, T c is the centroid of the reference coordinate point set;

[0101] After finding the centroid, the two sets of points are decentralized:

[0102]

[0103] Where, Represents the coordinate point set after decentralized processing, Represents the reference coordinate point set after decentralized processing; Step 6.4, solve the rotation matrix, scaling factor and translation vector

[0104] The rotation matrix can be solved by singular value decomposition SVD. First, construct the following covariance matrix H:

[0105]

[0106] Perform singular value decomposition on formula (27):

[0107] H=UΣV T (28)

[0108] Where U and V are orthogonal matrices, and ∑ is a diagonal matrix;

[0109] The rotation matrix Q1 can be obtained:

[0110] Q1=VU T (29)

[0111] For the scaling factor, it can be solved by constructing the following partial derivatives:

[0112]

[0113] For the translation vector, after the rotation and scaling transformations are processed, the translation amount is determined by the center of mass:

[0114] Y=T c -sQ1R c (31)

[0115] Step 6.5: After the rotation, translation and scaling transformations, the error function value is calculated. If the error is greater than the preset threshold, the transformed set T is used as the new point set and a new optimal point pair relationship is found and operated again until the error is less than the threshold. The iteration stops and the parameters s, Q1 and Y at this time are substituted into formula (21) to obtain the final transformation matrix.

[0116] The core idea of ​​scaled ICP is to separate the optimization of point pair relationships from that of transformation parameters. Using nearest neighbor search, the optimal point pair relationship between the current point set R and the target point set T is found in each iteration. After fixing this point pair relationship, the transformation parameters are optimized. This significantly reduces the complexity of the optimization problem. The nonlinear component of the error function, namely the point pair relationship, is explicitly handled in the nearest neighbor search, while the optimization of the transformation parameters, namely rotation and translation, is a linear least squares problem with a straightforward analytical solution.

[0117] Step 7: Use the altimeter to compensate the altitude positioning information of the unmanned platform to achieve collaborative navigation of multiple unmanned platforms in a GNSS-denied environment.

[0118] The present invention also provides a mobile terminal, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the program, the LLE-ICP-based multi-unmanned platform collaborative navigation and positioning method is implemented.

[0119] The present invention also provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps in the LLE-ICP-based multi-unmanned platform collaborative navigation and positioning method.

[0120] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0121] Example

[0122] In this example, compensation simulation is performed based on the results of the multi-unmanned platform collaborative navigation and positioning method based on local linear embedding dimensionality reduction. The simulation conditions are as follows: 6 unmanned platforms, including 4 drones and 2 unmanned vehicles, are selected. The running time is 7.5 minutes, and the drones travel a distance of about 1000m. During the operation, the ranging simulation UWB ranging sensor error is 0.5m. Among them, the fourth drone is far away from the other drones, which can make the entire formation in an irregular configuration. The unmanned vehicle and the drone run together, running at a speed of 1.5m / s for the same time. The gyro zero drift of the inertial navigation device used is 10deg / h and the gyro noise is 0.01degs -1 Hz -1 / 2 , the accelerometer zero bias is 15μg, and the accelerometer noise is When using pure inertial navigation solution, the measurement error and the positioning error when using different dimensionality reduction algorithms combined with ICP iteration are as follows: Figure 4 shown.

[0123] As can be seen, the distance-assisted positioning error using LLE-ICP converges more slowly over time, significantly improving positioning accuracy compared to inertial navigation and MDS-ICP solutions. The LLE algorithm effectively reduces the interference of nonlinear errors and is not affected by formation configurations. Consequently, positioning jitter is significantly lower than that of the MDS algorithm, resulting in a 60.5% reduction in positioning error.

[0124] The above description is only a preferred specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any changes or substitutions that can be easily thought of by any technician familiar with this technical field within the technical scope disclosed by the present invention should be covered by the scope of protection of the present invention.

[0125] It should be understood that in order to simplify the present invention and help those skilled in the art understand the various aspects of the present invention, in the above description of the exemplary embodiments of the present invention, various features of the present invention are sometimes described in a single embodiment or described with reference to a single figure. However, the present invention should not be interpreted as if all the features included in the exemplary embodiments are essential technical features of the claims of this patent.

Claims

1. A multi-unmanned platform collaborative navigation and positioning method based on LLE-ICP, characterized in that: First, a multi-unmanned platform collaborative navigation and positioning system based on LLE-ICP is constructed. The system includes an aerial unmanned platform, a ground unmanned platform, airborne sensors and communication equipment. Both the aerial unmanned platform and the ground unmanned platform are equipped with airborne sensors and communication equipment. The navigation and positioning method includes the following steps: Step 1: Power on the LLE-ICP-based multi-unmanned platform collaborative navigation and positioning system, use a ground unmanned platform as the reference coordinate, and calculate the position information and relative distance of each unmanned platform relative to the reference coordinate; Step 2: Each unmanned platform uses its own inertial sensor to collect data and transmit it to the processor on board the unmanned platform; Step 3: Each unmanned platform uses its onboard processor to calculate its position based on the collaborative navigation data from the previous moment to obtain its current position information. Step 4: Each unmanned platform performs radio ranging through the communication module on board to obtain the relative distance information with other unmanned platforms; Step 5: Summarize the distance information between each two unmanned platforms to form a distance matrix. Combined with the inertial sensor coordinate matrix, the three-dimensional position is reduced to two-dimensional coordinates based on the LLE nonlinear data dimensionality reduction algorithm. Step 6: Using the positioning coordinates provided by the inertial sensor as reference points, the coordinates are spatially transformed based on the scaled ICP iterative optimization algorithm to minimize the error. Finally, the transformed two-dimensional coordinate point set is sent to each unmanned platform as collaborative navigation data; Step 7: Use the altimeter to compensate the altitude positioning information of the unmanned platform to achieve collaborative navigation of multiple unmanned platforms in a GNSS-denied environment.

2. The LLE-ICP-based multi-unmanned platform collaborative navigation and positioning method according to claim 1 is characterized in that: The LLE-based nonlinear data dimensionality reduction algorithm described in step 5 reduces the three-dimensional position to two-dimensional coordinates as follows: Step 5.1, find the neighboring points of each data point; Step 5.2: Reconstruct each data point using a linear combination of its neighboring points. Step 5.3: Maintain the local linear relationship and perform dimensionality reduction to obtain the dimensionality reduction coordinates.

3. The LLE-ICP-based multi-unmanned platform collaborative navigation and positioning method according to claim 2 is characterized in that: Step 5.1 is to find the neighboring points of each data point as follows: The input is assumed to be a set of N three-dimensional coordinate points and an N*N distance matrix D. For each coordinate point obtained by the inertial sensor pose solution, the k nearest neighbors are found based on the matrix D to form a set N(i): N(i)={j|d ij is one of the first K smallest values ​​and j≠i} (1).

4. The LLE-ICP-based multi-unmanned platform collaborative navigation and positioning method according to claim 3 is characterized in that: Step 5.2 reconstructs each data point using a linear combination of neighboring points as follows: The reconstruction matrix W is constructed by the distance relationship between neighboring points, that is, for each coordinate point x i It is approximated by a linear combination of k neighboring points, namely: Where W ij is the weight, indicating the neighboring point x j Point x i The contribution of W ij It has two characteristics: sparsity and normalization. Sparsity means that if x j Not x i The neighboring point of ij =0; normalization is expressed as follows: The error ε(W) after approximate reconstruction is expressed as: ε(W)=|x i -∑ j∈N(i) W ij x j | 2 (4) Based on the input distance matrix D, the local covariance matrix C is used i To re-express the geometric relationship between adjacent points, set the coordinate point x i The neighbor points of are N(i), then the local covariance matrix C i for: C i =d ip +d iq -2d pq (5) Where, d ip is the coordinate point x i and neighboring point x ip The distance between iq is the coordinate point x i and neighboring point x iq , d pq For the neighboring point x ip and neighboring point x iq the distance between them; When solving the weights by minimizing the local covariance matrix, in order to minimize the reconstruction error, the error is rewritten as follows: ε(W)=W i T C i W i (6) In order to minimize the reconstruction error and satisfy the normalization constraint at the same time, the Lagrange multiplier method is used. First, the following Lagrange function is defined: Where λ is the Lagrange multiplier used to handle the constraints; In order to minimize the Lagrangian function value, W ij Take the derivative and set it to 0: Where E is a vector whose elements are all 1. Simplifying formula (8) we get: Multiply both sides of formula (9) by E T ,get: Arranging formula (10) gives the expression of λ: Substituting formula (11) into formula (9) yields: Since the right side of the equation (12) (E T C i W i ) / (E T E) is a scalar, so Equation (12) is simplified to: C i W i =E (13) Solve the coordinate point x from equation (13) i Reconstruct the weights and normalize the weights of all coordinate points to obtain the reconstruction matrix W.

5. The LLE-ICP-based multi-unmanned platform collaborative navigation and positioning method according to claim 4 is characterized in that: In step 5.3, maintain the local linear relationship and perform dimensionality reduction to obtain the dimensionality reduction coordinates, as follows: In order to maintain these local linear relationships in low-dimensional space, a global cost matrix M is first constructed: M=(I-W)(I-W) T (14) Where I is the N×N identity matrix; In order to find a low-dimensional embedding, it is necessary to perform eigenvalue decomposition on the matrix M. The eigenvalue decomposition process is as follows: My=λy (15) Where λ is the eigenvalue and y is the eigenvector; Since the coordinates after dimensionality reduction are two-dimensional, the matrix consisting of the eigenvectors corresponding to the two smallest non-zero eigenvalues ​​is selected, that is, the coordinate matrix Y after dimensionality reduction: Y=[y1,y2] (16).

6. The LLE-ICP-based multi-unmanned platform collaborative navigation and positioning method according to claim 5 is characterized in that: The coordinates are spatially transformed based on the scaled ICP iterative optimization algorithm described in step 6 as follows: Step 6.1, calculate the optimal point pair relationship between the current point set R and the target point set T; Using the accelerated search method, the spatial division structure of the target point set is constructed, all points in the target area are traversed, and the point R is set. k is a point in the two-dimensional coordinate point set R, and calculates the intersection of all points in the point set T with R k The Euclidean distance of the point with the smallest distance is selected as R k Neighbor T k : Step 6.2, define the error function; In the two-dimensional coordinate alignment problem, the goal is to find an optimal set of rotation matrices, translation vectors, and scaling factors to minimize the error between the dimensionality reduction result and the reference coordinates. Since there is no GNSS satellite positioning, the reference coordinates are the predicted information obtained by inertial navigation during the integrated navigation process or given by visual positioning. Set the coordinate point in two-dimensional space after dimensionality reduction to R i =[r1,r2], the corresponding reference coordinate point is T i =[t1,t2], the translation transformation vector is Y, and the following translation transformation is obtained: R i ′=R i +Y (18) Where R i ′ represents the coordinate point after translation transformation; The two-dimensional plane rotation matrix Q is defined as follows: Set the rotation factor to s, then the entire rotation, translation and scaling transformation formula is: T i =sQR i ′=sQ(R i +Y)=sQR i +sQY (20) For the entire coordinate point set R and reference point set T after dimensionality reduction, the translation vector is the final translation vector Y obtained by the translation of the centroid point after rotation and translation. The transformation formula of the entire point set is as follows: T=sQR+Y (21) After obtaining the change function, the required error function E(s,Q1,Y) is defined as: Where Q1 is the initial rotation matrix, which is set to the unit matrix; Y is the initial translation vector, which is set to 0; s is the initial scaling factor, which is set to 1; Step 6.3: Decentralize the coordinate point set; Center the coordinate point set to solve the shift vector; First calculate the centroid of the two sets of points: Where R c Represents the centroid of the coordinate point set after dimensionality reduction, T c is the centroid of the reference coordinate point set; After finding the centroid, the two sets of points are decentralized: Where, Represents the coordinate point set after decentralized processing, Represents the reference coordinate point set after decentralized processing; Step 6.4, solve the rotation matrix, scaling factor and translation vector; The rotation matrix is ​​solved by the singular value decomposition method SVD. First, the following covariance matrix H is constructed: Perform singular value decomposition on formula (27): H=UΣV T (28) Where U and V are orthogonal matrices, and ∑ is a diagonal matrix; Obtain the rotation matrix Q1: Q1=VU T (29) For the scaling factor, the partial derivative is constructed as follows: For the translation vector, after the rotation and scaling transformations are processed, the translation amount is determined by the center of mass: Y=T c -sQ1R c (31) Step 6.5: After the rotation, translation and scaling transformations, the error function value is calculated. If the error is greater than the preset threshold, the transformed set T is used as the new point set, and the new optimal point pair relationship is found and operated again until the error is less than the threshold. The iteration stops and the parameters s, Q1 and Y at this time are substituted into formula (21) to obtain the final transformation matrix.

7. A multi-unmanned platform collaborative navigation and positioning system based on LLE-ICP, characterized by: The system is used to implement the LLE-ICP-based multi-unmanned platform collaborative navigation and positioning method as described in any one of claims 1 to 6, comprising an aerial unmanned platform, a ground unmanned platform, airborne sensors and communication equipment; The aerial unmanned platform and the ground unmanned platform are both equipped with airborne sensors and communication equipment, wherein the airborne sensors include inertial sensors and barometric altimeters for obtaining inertial sensor and altitude data; the communication equipment is used to transmit positioning information between the unmanned platforms, and measure the relative distance information between the unmanned platforms through data links and UWB, perform data dimensionality reduction processing based on LLE-ICP, use the relative distance information to assist inertial navigation to locate the relative position of each unmanned platform, and then locate the absolute position of each unmanned platform based on the position relationship.

8. The LLE-ICP-based multi-unmanned platform collaborative navigation and positioning system according to claim 7 is characterized in that: The aerial unmanned platforms and the ground unmanned platforms can communicate with each other and exchange position information and distance information. Each aerial unmanned platform establishes a two-dimensional point coordinate set representing the relative position of each unmanned platform based on the distance information obtained from the communication data link, using the LLE nonlinear data dimensionality reduction algorithm and the ICP iterative closest point algorithm, thereby constraining the divergence of inertial navigation errors and realizing collaborative navigation and positioning of multiple aerial unmanned platforms.

9. A mobile terminal comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein: When the processor executes the program, the multi-unmanned platform collaborative navigation and positioning method based on LLE-ICP is implemented as described in any one of claims 1 to 6.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the program is executed by the processor, the steps of the LLE-ICP-based multi-unmanned platform collaborative navigation and positioning method are implemented as described in any one of claims 1 to 6.

Images