A method for simultaneous field map extension and autonomous positioning of a drone

Abstract

The application discloses a method for simultaneously expanding a field map and autonomously positioning a UAV track, which comprises the following steps: a UAV randomly moves along a straight line for a period of time, and preliminarily positions all beacon positions; a path planning is performed on a flight track of the UAV in a near field; the path planning in the near field is segmented, the UAV positions the beacons in a relative coordinate system during flight in each segment of the track, and through matching with the beacons in a geographic coordinate system, the field map of the beacons in the geographic coordinate system is expanded, and the UAV track is positioned; in a far field beacon area, a flight track of the UAV is planned; according to the planned track in the far field, the UAV flies along the planned track and simultaneously positions the surrounding beacons in a relative coordinate system, and through matching with the beacons in a geographic coordinate system, the expanded field map of the beacons in the far field and the positioning result of the UAV track are obtained. The application can accurately capture the beacons during UAV flight, continuously expand and construct the field map of the beacons, and simultaneously realize autonomous positioning of the UAV track.

Description

Technical Field

[0001] This invention belongs to the field of passive positioning technology, specifically relating to a method for simultaneous field map expansion and autonomous positioning of UAV trajectory. Background Technology

[0002] Satellite navigation is a widely used real-time positioning technology with numerous applications in transportation, meteorological surveying, and other fields. However, satellite navigation signals are easily blocked by buildings, causing navigation and positioning results to fail. Furthermore, satellite navigation signals are susceptible to electromagnetic interference, transmitting incorrect signal data and resulting in inaccurate signals at the receiving end.

[0003] Current research proposes beacon-based navigation and positioning schemes, which can effectively overcome the vulnerability of satellite navigation signals and enhance positioning performance in areas where satellite navigation has failed by using pre-set beacon nodes. However, for unknown beacons, beacon node positioning is needed to map the auxiliary beacon positions. In practical applications, there are many unknown beacons such as mobile base stations and unauthorized radio broadcasts. Utilizing these beacons for positioning and navigation can effectively compensate for the shortcomings of satellite navigation and achieve more accurate positioning performance. Although traditional inertial navigation positioning methods can achieve continuous UAV trajectory positioning without external factors, accurate positioning over long periods requires intermittent hovering calibration. For far-field UAV positioning, frequent hovering calibration reduces mission efficiency. Therefore, it is necessary to study methods for mapping the accurate positioning results of relative coordinate system inertial navigation to the geographic coordinate system after hovering calibration at any far-field location. Summary of the Invention

[0004] The technical problem to be solved by the present invention is to address the shortcomings of the prior art by providing a method for simultaneous field map expansion and autonomous UAV trajectory localization, which utilizes beacon matching to achieve beacon field map expansion and autonomous UAV trajectory localization.

[0005] To achieve the above-mentioned technical objectives, the technical solution adopted by the present invention is as follows:

[0006] A method for simultaneous field map expansion and autonomous UAV trajectory localization includes:

[0007] Step 1: The UAV moves randomly along a straight line and uses inertial navigation positioning and array direction finding data to initially locate the positions of all beacons within the field map;

[0008] Step 2: Based on the initially located beacon position, perform path planning for the near-field beacon UAV flight trajectory to obtain the near-field planned path;

[0009] Step 3: Segment the near-field planned path. When the UAV flies along each segment, it locates the beacon in the relative coordinate system. By matching the beacon with the beacon in the geographic coordinate system, the geographic coordinate system beacon field map is expanded and the UAV trajectory is located, allowing it to enter the far-field beacon area.

[0010] Step 4: In the far-field beacon area, plan the UAV flight trajectory to obtain the far-field planned path;

[0011] Step 5: According to the far-field planned path, the UAV flies along the planned trajectory and simultaneously performs relative coordinate system positioning on the surrounding beacons. By matching with the beacons in the geographic coordinate system, the expanded far-field beacon field map and the UAV trajectory positioning results are obtained.

[0012] To optimize the above technical solution, the specific measures also include:

[0013] In step 1 above, the drone moves randomly at a constant speed along a straight line, and the trajectory of the straight line is as follows: , The inertial navigation positioning result is ,in, Let x and y be the coordinates of the i-th trajectory point. For the number of trajectory points, The inertial navigation positioning result for the i-th trajectory point The x and y coordinates in the text;

[0014] On a straight trajectory, the array provides direction-finding values ​​for different beacons relative to true north. , , The number of beacons is unknown, and their locations are represented as...

[0015] Based on the inertial navigation positioning results and The preliminary location of the beacon is as follows:

[0016]

[0017] in, This indicates pseudo-inverse computation. This represents the measurement matrix related to the direction finding values. This represents the observation matrix related to the measurements and inertial navigation positioning.

[0018]

[0019]

[0020] Calculate the distance from the beacon to the straight line trajectory:

[0021]

[0022] The nearest beacon to the trajectory is selected as:

[0023]

[0024] make Based on the above calculation results, a data storage matrix is ​​constructed:

[0025] .

[0026] The relationship between the above inertial navigation positioning results and the actual trajectory is as follows:

[0027]

[0028] in, This is the drone's actual speed. For velocity measured by inertial navigation, , For UAV inertial navigation to measure acceleration, This represents the time interval between trajectory points.

[0029] Step 2 above is based on the beacon location. , , To determine the number of near-field beacons, path planning is performed, assuming the path planning points are... , , To determine the number of path points for near-field planning, construct the following Fisher information matrix:

[0030]

[0031] in, This represents a first-order biased guide.

[0032]

[0033]

[0034] In the formula, Let be the direction-finding angle of the l-th beacon at the i′-th trajectory point; For the first Path planning point inertial navigation positioning results;

[0035] The Cramer-Rao limit is obtained based on the Fisher information matrix:

[0036]

[0037] Define perturbation ,exist Find the position with the smallest Cramérault bound. Location, to obtain near-field planning path:

[0038]

[0039] in, This indicates that the Craméro limit is at the 1st The partial derivative matrix of the trace points.

[0040] .

[0041] Step 3 above describes the near-field path planning trajectory based on a fixed number of trajectory points. Divided into In each segment, the inertial navigation positioning result for each segment of the drone's flight path is as follows: , , ;

[0042] Based on the inertial navigation positioning results and array direction finding data, calculate the first position in the relative coordinate system. Segment trajectory for the first Estimation results of the beacon locations:

[0043]

[0044] in, This indicates pseudo-inverse computation. Indicates the first The trajectory of segment about the first Measurement matrix of beacon direction finding values Indicates the first The trajectory of segment about the first The observation matrix of beacon measurements and inertial navigation positioning.

[0045]

[0046]

[0047] In the formula, Let be the direction-finding angle of the l-th beacon at the end point of the k-th segment of the trajectory;

[0048] Calculate the distance from the beacon to the k-th segment of the trajectory:

[0049]

[0050] The nearest beacon to the k-th segment of the trajectory is selected as:

[0051]

[0052] make Matching of beacon positions between relative coordinate system and geographic coordinate system:

[0053]

[0054] in, The matched beacon geographic coordinates, For the location of the beacon in the geographic coordinate system, The position of the beacon relative to the coordinate system;

[0055] Matching the trajectory positions between relative coordinate systems and geographic coordinate systems:

[0056]

[0057] in, The geographic coordinates of the matched drone trajectory;

[0058] After the above calculation is completed, through Achieve iterative updates;

[0059] The latest calculation With data storage matrix The corresponding ( ) compare, if the latest If the value is smaller, use the latest one. , Update the storage data column corresponding to the data storage matrix.

[0060] The relationship between the inertial navigation positioning result and the actual trajectory position for each of the above segments is as follows:

[0061]

[0062] in, Let be the velocity at the (i′+1)th trajectory point. , For the actual speed of the drone, The inertial navigation system measures the acceleration at the (i′+1)th trajectory point. This represents the time interval between trajectory points.

[0063] Step 4 above involves planning the drone's trajectory in the far-field beacon area, where the far-field beacon is... , , This refers to the number of near-field beacons. For the number of far-field beacons, ;

[0064] Assume the path planning point is , , To determine the number of points for near-field and far-field path planning, the following Fisher information matrix is ​​constructed:

[0065]

[0066] in, This represents a first-order biased guide.

[0067]

[0068]

[0069] in, In the first At the i-th trajectory point, for the i-th The direction finding angle of a far-field beacon;

[0070] The Cramer-Rao limit is obtained based on the Fisher information matrix:

[0071]

[0072] exist Find the position with the smallest Cramérault bound. Location, to obtain the far-field planned path:

[0073]

[0074] in, This indicates that the Craméro limit is at the 1st The partial derivative matrix of the trace points.

[0075] .

[0076] In step 5 above, based on the far-field planned path, the UAV flies along the planned trajectory. The relationship between the inertial navigation positioning result and the actual trajectory position is as follows:

[0077]

[0078] in, This is the inertial navigation positioning result for the i′′-th trajectory point in the far field; For the far field The velocity at each trajectory point ;

[0079] The estimated positions of surrounding beacons in the relative coordinate system are obtained as follows:

[0080]

[0081] in, Indicates the far-field trajectory with respect to the first... Measurement matrix of beacon direction finding values Indicates the far-field trajectory with respect to the first... The observation matrix of beacon measurements and inertial navigation positioning.

[0082]

[0083]

[0084] in, To determine the starting point of the far-field trajectory for the first... The direction finding angle of a beacon; To the end point of the far-field trajectory for the first The direction finding angle of a beacon;

[0085] Calculate the distance from the beacon to the trajectory:

[0086]

[0087] The beacons are sorted by distance from smallest to largest as follows:

[0088]

[0089] In the formula, Indicates will The elements in the array are arranged in ascending order, and the beacon positions from closest to farthest from the trajectory are returned.

[0090] Assuming to use Match each beacon. The total number of beacons participating in the matching. The starting index for the beacons participating in the matching;

[0091] Select the beacon closest to the far-field trajectory:

[0092]

[0093] Let the relative coordinate system coordinates The beacon positions for relative coordinate systems and geographic coordinate systems are matched as follows:

[0094]

[0095] In the formula, The matched beacon geographic coordinates are used to create an expanded far-field beacon field map. express The first of the matrix The third row contains geographic coordinate system beacon location data;

[0096] The trajectory position matching between the relative coordinate system and the geographic coordinate system is as follows:

[0097]

[0098] In the formula, The geographic coordinates of the matched far-field UAV trajectory;

[0099] The latest calculation and The corresponding ( ) compare, if the latest If the value is smaller, use the latest one. , Update the storage data column corresponding to the data storage matrix.

[0100] The present invention has the following beneficial effects:

[0101] This invention uses an array to locate beacon positions and simultaneously sense changes in UAV position; it achieves precise UAV trajectory positioning in a relative coordinate system based on inertial navigation data, and constructs a beacon distribution field map in a relative coordinate system using array direction-finding data.

[0102] This invention fully utilizes beacon positioning and matching technology, using a beacon distribution field map in a relative coordinate system to match and expand an existing field map in a geographic coordinate system. At the same time, it realizes the mapping of the UAV trajectory in the relative coordinate system to the geographic coordinate system, enabling accurate capture of beacons during UAV flight and continuous expansion and construction of the beacon field map. While the beacon field map is being expanded, the UAV trajectory in the relative coordinate system is also mapped to the geographic coordinate system, realizing autonomous positioning of the UAV trajectory.

[0103] Based on traditional inertial navigation positioning methods, this invention can achieve trajectory positioning starting from any position in the far field, which has significant application value. Attached Figure Description

[0104] Figure 1 This is a flowchart of the method of the present invention;

[0105] Figure 2 This is a scene diagram of the present invention;

[0106] Figure 3 This is a simulation effect diagram of the present invention. Detailed Implementation

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

[0108] This invention provides a method for simultaneous field map expansion and autonomous UAV trajectory localization, such as... Figure 1As shown, the UAV moves along a straight line to initially determine the positions of all beacons; based on the beacon positions, the UAV's near-field flight trajectory is planned; the UAV locates the previously located beacon and the nearest beacon on each segment of the near-field trajectory, and matches it with the previously located beacon, realizing the expansion of the beacon field map in the geographic coordinate system and autonomous UAV trajectory positioning; based on the far-field beacon positions, the UAV's flight trajectory is planned; the UAV matches the less nearest beacon with the historical expanded beacon on the far-field trajectory, realizing the expansion of the far-field beacon field map in the geographic coordinate system and autonomous UAV trajectory positioning. This method uses an array as the UAV detection device to perform beacon positioning and matching and autonomous UAV trajectory positioning. Specifically, the method includes the following steps:

[0109] Step 1: The UAV moves randomly along a straight line and uses inertial navigation positioning and array direction finding data to initially locate the positions of all beacons within the field map;

[0110] Step 2: Based on the initially located beacon position, perform path planning for the near-field beacon UAV flight trajectory to obtain the near-field planned path;

[0111] Step 3: Segment the near-field planned path. When the UAV flies along each segment, it locates the beacon in the relative coordinate system. By matching the beacon with the beacon in the geographic coordinate system, the geographic coordinate system beacon field map is expanded and the UAV trajectory is located, allowing it to enter the far-field beacon area.

[0112] Step 4: In the far-field beacon area, plan the UAV flight trajectory to obtain the far-field planned path;

[0113] Step 5: According to the far-field planned path, the UAV flies along the planned trajectory and simultaneously performs relative coordinate system positioning on the surrounding beacons. By matching with the beacons in the geographic coordinate system, the expanded far-field beacon field map and the UAV trajectory positioning results are obtained.

[0114] In this embodiment, in step 1, the UAV enters the field map, moves randomly along a straight line, and initially locates the positions of all beacons within the field map, as follows:

[0115] The drone moves randomly at a constant speed along a straight line. The trajectory of this straight line is represented as follows: , The inertial navigation positioning result is expressed as follows: The relationship between the inertial navigation positioning result and the actual trajectory points is as follows:

[0116]

[0117] in, This is the actual speed of the drone; the drone's inertial navigation system measures the acceleration as follows: The time interval between trajectory points is Then the velocity measured by the inertial navigation system is , .

[0118] The direction finding values ​​of different beacons relative to true north on a straight trajectory array are... , The beacon location is unknown, indicated as The preliminary location of the beacon is as follows:

[0119]

[0120] in, This indicates pseudo-inverse computation. This represents the measurement matrix related to the direction finding values. This represents the observation matrix relating to the measurements and inertial navigation positioning.

[0121]

[0122]

[0123] Calculate the distance from the beacon to the straight line trajectory:

[0124]

[0125] Therefore, the nearest beacon to the trajectory is defined as:

[0126]

[0127] In the above formula, Returns to make the expression The variable with the smallest value. For ease of subsequent iterations, let... Simultaneously, a data storage matrix is ​​constructed based on the above calculation results:

[0128]

[0129] In this embodiment, step 2 involves path planning for the near-field beacon drone's orbital positioning trajectory based on the preliminary beacon location; the path planning for the near-field drone's flight trajectory is as follows:

[0130] according to , Perform path planning, assuming the path planning points are... , Construct the following Fisher information matrix (FIM):

[0131]

[0132] in, Indicates a first-order biased guide;

[0133]

[0134] In the above formula, , For the first The path planning point inertial navigation positioning results.

[0135] Define the Cramer-Rao lower bound (CRLB):

[0136]

[0137] In order to Find the position with the smallest Cramérault bound. Location, definition ,but

[0138]

[0139] in, This indicates that the Craméro limit is at the 1st The partial derivative matrix of the trace points;

[0140] In this embodiment, step 3, based on the near-field planned path, involves the UAV performing relative coordinate system positioning on the previous positioning beacon and the nearest beacon in each trajectory segment (the UAV positions the previous positioning beacon and the nearest beacon in the relative coordinate system while flying on each trajectory segment), matching with the previous positioning beacon, and simultaneously expanding the geographic coordinate system beacon field map and UAV trajectory positioning, as detailed below:

[0141] The path planning trajectory is based on a fixed number of trajectory points. Divided into The inertial navigation positioning results for each segment of the trajectory are as follows: , , .

[0142] The relationship between the inertial navigation positioning result and the actual trajectory position for each segment is as follows:

[0143]

[0144] in, , .

[0145] We can obtain the first relative coordinate system. Segment trajectory for the first Estimation results of the beacon locations:

[0146]

[0147] in, Indicates the first The trajectory of segment about the first Measurement matrix of beacon direction finding values Indicates the first The trajectory of segment about the first The observation matrix of beacon measurements and inertial navigation positioning;

[0148]

[0149]

[0150] Calculate the distance from the beacon to the k-th segment of the trajectory:

[0151]

[0152] Therefore, the nearest beacon to the k-th segment of the trajectory is defined as:

[0153]

[0154] To facilitate subsequent iterations, let The formula for matching the beacon positions of relative coordinate systems and geographic coordinate systems is as follows:

[0155]

[0156] The formula for matching the trajectory position between the relative coordinate system and the geographic coordinate system is as follows:

[0157]

[0158] After the above calculation is completed, .

[0159] The latest calculation and The corresponding ( ) compare, if the latest If the value is smaller, then use the latest one. , Update the data storage matrix corresponding to the first Each column stores data. This is understandable. subscript Used to select numbers in the range 1 to L. subscript Indicates selecting the first The column values ​​correspond to the newly calculated numerical indices, making them easy to read and retrieve.

[0160] In this embodiment, step 4 involves planning the UAV's flight trajectory in the far-field beacon region, as detailed below:

[0161] In the far-field beacon area, the drone trajectory is planned. The far-field beacon is... , , This refers to the number of near-field beacons. For the number of far-field beacons, Assume the path planning point is , Construct the Fisher information matrix (FIM) as follows:

[0162]

[0163] in, Indicates a first-order biased guide;

[0164]

[0165] In the above formula, .

[0166] Define the Cramer-Rao lower bound (CRLB):

[0167]

[0168] In order to Find the position with the smallest Cramérault bound. Location, definition ,but

[0169]

[0170] in, This indicates that the Craméro limit is at the 1st The partial derivative matrix of the trace points;

[0171]

[0172] In this embodiment, step 5 involves the UAV performing relative coordinate system positioning on surrounding beacons based on the far-field planned path (the UAV flies along the planned path while simultaneously performing relative coordinate system positioning on surrounding beacons), by comparing the nearest beacons with the nearest ones. Matching each beacon with a geographic coordinate system beacon yields the expanded far-field beacon field map and the UAV trajectory positioning results, as detailed below:

[0173] The relationship between the inertial navigation positioning result and the actual trajectory position is as follows:

[0174]

[0175] in, , .

[0176] The beacon position estimation results in the relative coordinate system can be obtained:

[0177]

[0178] in, Indicates the far-field trajectory with respect to the first... Measurement matrix of beacon direction finding values Indicates the far-field trajectory with respect to the first... The observation matrix of beacon measurements and inertial navigation positioning;

[0179]

[0180]

[0181] in, To determine the starting point of the far-field trajectory for the first... The direction finding angle of a beacon; To the end point of the far-field trajectory for the first The direction finding angle of a beacon;

[0182] Calculate the distance from the beacon to the trajectory:

[0183]

[0184] Therefore, the beacons are ordered in ascending order of distance.

[0185]

[0186] In the above formula, Will The elements in the array are sorted in ascending order, and the beacon positions are returned in ascending order of distance from the trajectory. Here, it is assumed that the following method is used... Match each beacon.

[0187] Therefore, the nearest beacon to the far-field trajectory is defined as:

[0188]

[0189] To facilitate subsequent iterations, let The formula for matching beacon positions between relative coordinate systems and geographic coordinate systems is as follows:

[0190]

[0191] In the above formula, express The first of the matrix The beacon location data in the third row of the column.

[0192] The formula for matching the trajectory position between the relative coordinate system and the geographic coordinate system is as follows:

[0193]

[0194] The latest calculation and The corresponding ( ) compare, if the latest If the value is smaller, then use the latest one. , Update the corresponding stored data columns in the data storage matrix. This involves using variables. Selecting a value from the corresponding column of Data indicates a comparison with a newly calculated value for the same beacon, with the index of the new calculated value being consistent.

[0195] To verify the effectiveness of the algorithm described in this invention, the following demonstration is performed using MATLAB simulation analysis.

[0196] The scene diagram of the present invention is as follows Figure 2 As shown, the UAV enters the positioning area along a straight line, initially determines the positions of all beacons, and then expands the beacon field map and achieves autonomous positioning while flying according to the path planning results.

[0197] The simulation effect diagram of the present invention is as follows: Figure 3 As shown, the inertial navigation acceleration measurement error in the simulation is 0.1 m / s². 2 The direction finding angle error is 2°; the near-field beacon position is... , , The location of the far-field beacon is The far-field UAV trajectory localization and matching beacon is a simulation image. and Simulation results show that the method of this invention can accurately construct the beacon field map and achieve autonomous positioning of the UAV trajectory at two locations, with a far-field autonomous positioning error of 0.6228m. The formula for calculating the error is as follows:

[0198] .

[0199] The above are merely preferred embodiments of the present invention. The scope of protection of the present invention is not limited to the above embodiments. All technical solutions falling within the scope of the present invention's concept are within the scope of protection of the present invention. It should be noted that for those skilled in the art, any improvements and modifications made without departing from the principles of the present invention should be considered within the scope of protection of the present invention.

Claims

1. A method for simultaneous field map expansion and autonomous UAV trajectory localization, characterized in that, include: Step 1: The UAV moves randomly along a straight line and uses inertial navigation positioning and array direction finding data to initially locate the positions of all beacons within the field map; Step 2: Based on the initially located beacon position, perform path planning for the near-field beacon UAV flight trajectory to obtain the near-field planned path; Step 3: Segment the near-field planned path. When the UAV flies along each segment, it locates the beacon in the relative coordinate system. By matching the beacon with the beacon in the geographic coordinate system, the geographic coordinate system beacon field map is expanded and the UAV trajectory is located, allowing it to enter the far-field beacon area. Step 4: In the far-field beacon area, plan the UAV flight trajectory to obtain the far-field planned path; Step 5: According to the far-field planned path, the UAV flies along the planned trajectory and simultaneously performs relative coordinate system positioning on the surrounding beacons. By matching with the beacons in the geographic coordinate system, the expanded far-field beacon field map and the UAV trajectory positioning results are obtained.

2. The method for simultaneous field map expansion and autonomous UAV trajectory localization according to claim 1, characterized in that, In step 1, the drone moves randomly and at a constant speed along a straight line. The trajectory of the straight line is as follows: , The inertial navigation positioning result is ,in, Let x and y be the coordinates of the i-th trajectory point. For the number of trajectory points, The inertial navigation positioning result for the i-th trajectory point The x and y coordinates in the figure; On a straight trajectory, the array provides direction-finding values ​​for different beacons relative to true north. , , The number of beacons is unknown, and their locations are represented as... ; Based on the inertial navigation positioning results and The preliminary location of the beacon is as follows: in, This indicates pseudo-inverse computation. This represents the measurement matrix related to the direction finding values. This represents the observation matrix relating to the measurements and inertial navigation positioning. Calculate the distance from the beacon to the straight line trajectory: The nearest beacon to the trajectory is selected as: make Based on the above calculation results, a data storage matrix is ​​constructed: 。 3. The method for simultaneous field map expansion and autonomous UAV trajectory localization according to claim 2, characterized in that, The relationship between the inertial navigation positioning result and the actual trajectory is as follows: in, This is the drone's actual speed. For velocity measured by inertial navigation, , For UAV inertial navigation to measure acceleration, This represents the time interval between trajectory points.

4. The method for simultaneous field map expansion and autonomous UAV trajectory localization according to claim 2, characterized in that, Step 2: Based on the beacon location , , To determine the number of near-field beacons, path planning is performed, assuming the path planning points are... , , To determine the number of path points for near-field planning, construct the following Fisher information matrix: in, Indicates a first-order biased guide; In the formula, In the first At the i-th trajectory point, for the i-th The direction finding angle of a beacon; For the first Path planning point inertial navigation positioning results; The Cramer-Rao limit is obtained based on the Fisher information matrix: Define perturbation ,exist Find the position with the smallest Cramérault bound. Location, to obtain near-field planning path: in, This indicates that the Craméro limit is at the 1st The partial derivative matrix of the trace points; 。 5. The method for simultaneous field map expansion and autonomous UAV trajectory localization according to claim 4, characterized in that, Step 3: Plan the near-field path trajectory according to a fixed number of trajectory points. Divided into In each segment, the inertial navigation positioning result for each segment of the drone's flight path is as follows: , , ; Based on the inertial navigation positioning results and array direction finding data, calculate the first position in the relative coordinate system. Segment trajectory for the first Estimation results of the beacon locations: in, This indicates pseudo-inverse computation. Indicates the first The trajectory of segment about the first Measurement matrix of beacon direction finding values Indicates the first The trajectory of segment about the first The observation matrix of beacon measurements and inertial navigation positioning; In the formula, Let be the direction-finding angle of the l-th beacon at the end point of the k-th segment of the trajectory; Calculate the distance from the beacon to the k-th segment of the trajectory: The nearest beacon to the k-th segment of the trajectory is selected as: make Matching of beacon positions between relative coordinate system and geographic coordinate system: in, The matched beacon geographic coordinates, For the location of the beacon in the geographic coordinate system, The position of the beacon relative to the coordinate system; Matching the trajectory positions between relative coordinate systems and geographic coordinate systems: in, The geographic coordinates of the matched drone trajectory; After the above calculation is completed, through Achieve iterative updates; The latest calculation With data storage matrix The corresponding ( ) compare, if the latest If the value is smaller, use the latest one. , Update the data storage matrix corresponding to the first Each column stores data.

6. The method for simultaneous field map expansion and autonomous UAV trajectory localization according to claim 5, characterized in that, The relationship between the inertial navigation positioning result and the actual trajectory position for each segment is as follows: in, Let be the velocity at the (i′+1)th trajectory point. , For the actual speed of the drone, The inertial navigation system measures the acceleration at the (i′+1)th trajectory point. This represents the time interval between trajectory points.

7. The method for simultaneous field map expansion and autonomous UAV trajectory localization according to claim 5, characterized in that, Step 4: In the far-field beacon area, plan the drone trajectory. The far-field beacon is... , , This refers to the number of near-field beacons. For the number of far-field beacons, ; Assume the path planning point is , , To determine the number of points for near-field and far-field path planning, the following Fisher information matrix is ​​constructed: in, Indicates a first-order biased guide; in, In the first At the i-th trajectory point, for the i-th The direction finding angle of a far-field beacon; The Cramer-Rao limit is obtained based on the Fisher information matrix: exist Find the position with the smallest Cramérault bound. Location, to obtain the far-field planned path: in, This indicates that the Craméro limit is at the 1st The partial derivative matrix of the trace points; 。 8. The method for simultaneous field map expansion and autonomous UAV trajectory localization according to claim 7, characterized in that, Step 5: Based on the far-field planned path, the UAV flies along the planned trajectory. The relationship between the inertial navigation positioning result and the actual trajectory position is as follows: in, This is the inertial navigation positioning result for the i′′-th trajectory point in the far field; For the far field The velocity at each trajectory point ; The estimated positions of surrounding beacons in the relative coordinate system are obtained as follows: in, Indicates the far-field trajectory with respect to the first... Measurement matrix of beacon direction finding values Indicates the far-field trajectory with respect to the first... The observation matrix of beacon measurements and inertial navigation positioning; in, To determine the starting point of the far-field trajectory for the first... The direction finding angle of a beacon; To the end point of the far-field trajectory for the first The direction finding angle of a beacon; Calculate the distance from the beacon to the trajectory: The beacons are sorted by distance from smallest to largest as follows: In the formula, Indicates will The elements in the array are arranged in ascending order, and the beacon positions from closest to farthest from the trajectory are returned. Assuming to use Match each beacon. The total number of beacons participating in the matching. The starting index for the beacons participating in the matching; Select the beacon closest to the far-field trajectory: Let the relative coordinate system coordinates The beacon positions for relative coordinate systems and geographic coordinate systems are matched as follows: In the formula, The matched beacon geographic coordinates are used to create an expanded far-field beacon field map. express The first of the matrix The third row contains geographic coordinate system beacon location data; The trajectory position matching between the relative coordinate system and the geographic coordinate system is as follows: In the formula, The geographic coordinates of the matched far-field UAV trajectory; The latest calculation and The corresponding ( ) compare, if the latest If the value is smaller, use the latest one. , Update the storage data column corresponding to the data storage matrix.

Images