UAV trajectory planning method, device, equipment and medium

Abstract

The present invention discloses a method, apparatus, device, and medium for unmanned aerial vehicle (UAV) trajectory planning. The method comprises obtaining phase difference measurement data between a first receiving device and a second receiving device at the current moment; calculating a cost surface based on the phase difference measurement value, then performing a grid search within a preset range centered on the UAV's initial position to find the UAV's initial estimated position; performing iterative least squares convergence on the local neighborhood of the corresponding grid point to complete the positioning of the UAV and obtain the UAV's position estimate at the current moment; establishing an optimization model using the UAV's heading angle as the state vector and minimizing the Cramer-Rao lower bound trajectory as the objective function, and converting the constrained optimization model into an unconstrained optimization model using a penalty function multiplier method; solving the unconstrained optimization model to obtain the UAV's optimal heading angle at the current moment; and calculating the next optimal track point based on the current UAV speed and the optimal heading angle. The present invention improves positioning accuracy and sensitivity.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned aerial vehicle (UAV) trajectory planning, and in particular relates to a method, device, equipment and medium for UAV trajectory planning. Background Art

[0002] Traditional trajectory planning techniques for emitter location often employ methods such as angle of arrival (AOA), time difference of arrival (TDOA), frequency difference of arrival (FDOA), and received signal strength indicator (RSSI). The trajectory planning process can be summarized as follows: Based on an analysis of positioning accuracy metrics and using these metrics as the objective function, a UAV trajectory optimization model is established, formulating the UAV trajectory planning problem as a nonlinear programming problem. By selecting optimal observation locations, the UAV can obtain high-quality, effective data in a shorter time without compromising system accuracy, thereby improving emitter location accuracy. Furthermore, the physical constraints of the UAV are considered to plan a reasonable and efficient trajectory for the UAV.

[0003] The limitation of traditional trajectory planning schemes for emitter positioning lies in the relatively mature trajectory planning technology for UAVs using multi-platform positioning systems such as AOA and TDOA, which can improve positioning accuracy and plan effective and flyable trajectories for UAVs. However, in the scenario of long-baseline positioning methods for single-platform UAVs, only a "generally optimal" trajectory is proposed, without considering the impact of the UAV's physical limitations on factors such as turning angles. In actual applications, the maneuverability of UAVs is generally difficult to achieve the spiral trajectory proposed in this paper, and the actual flyability of the trajectory is not rigorous. Summary of the Invention

[0004] The purpose of the present invention is to overcome the shortcomings of the existing technology and provide a method, device, equipment and medium for UAV trajectory planning. On the one hand, the positioning method based on long baseline interferometer is adopted to complete the rapid and high-precision positioning of long-distance targets. On the other hand, by implementing real-time trajectory planning for the UAV, the working efficiency and survivability of the UAV can be improved. Combining the advantages of the UAV platform and the long baseline positioning method, the combat effectiveness of the UAV can be maximized and the accuracy and sensitivity of reconnaissance positioning can be improved.

[0005] The object of the present invention is achieved through the following technical solutions:

[0006] A method for trajectory planning of an unmanned aerial vehicle (UAV), the method being implemented based on long baseline positioning, comprising providing a first receiving device on the UAV and providing a second receiving device within the communication range of the first receiving device, the method comprising:

[0007] Obtaining phase difference measurement data between the first receiving device and the second receiving device at the current moment;

[0008] Calculating a cost surface based on the phase difference measurement value, and then performing a grid search within a preset range centered on the initial position of the drone to find the initial estimated position of the drone;

[0009] Performing iterative least squares convergence on the local neighborhood of the grid points corresponding to the initial estimated position to complete the positioning of the UAV and obtain the positioning estimation result of the UAV at the current moment;

[0010] Taking the heading angle of the UAV as the state vector and minimizing the Cramer-Rao lower bound trace as the objective function, an optimization model is established. The penalty function multiplier method is used to transform the constrained optimization model into an unconstrained optimization model.

[0011] Solving the unconstrained optimization model to obtain the optimal heading angle of the UAV at the current moment;

[0012] The next optimal track point is calculated based on the current UAV speed and the optimal heading angle.

[0013] Furthermore, the method also includes setting the initial position, speed, maximum angular velocity, measurement step and baseline length of the drone before obtaining the phase difference measurement value of the first receiving device and the second receiving device at the current moment.

[0014] Furthermore, obtaining the phase difference measurement data between the first receiving device and the second receiving device at the current moment specifically includes:

[0015] The phase difference measurement value of the first receiving device and the second receiving device is obtained at the current moment, and all measurement values from the initial moment to the current moment are combined to obtain a phase difference measurement sequence.

[0016] Furthermore, the cost surface is calculated based on the phase difference measurement value, and then a grid search is performed within a preset range with the initial position of the drone as the center to find the initial estimated position of the drone, specifically including:

[0017] Divide the grid points within the preset range and set the search step size. The cost surface calculation formula includes:

[0018]

[0019] in, is the phase difference measurement, Δφ iis the noise-free phase difference calculated at each grid point, Δφ i The calculation formula includes:

[0020]

[0021] Where [x,y] T ∈S, S represents the set of possible radiation source locations on the grid, φ0 is the system error known through calibration, t i Indicates the measurement time corresponding to the i-th measurement value, x a1 (t i ) indicates that the first receiving device is at t i The horizontal coordinate of the time, x a2 (t i ) indicates that the second receiving device is at t i The horizontal axis of the time, y a1 (t i ) indicates that the first receiving device is at t i The vertical coordinate of the time, y a2 (t i ) indicates that the second receiving device is at t i The vertical coordinate of the moment;

[0022] When the actual phase difference measurement is equal to the noise-free phase difference at a certain location, the value of the cost surface will be minimized, indicating that this location is the location of the drone.

[0023] Furthermore, the method of establishing an optimization model with the heading angle of the UAV as the state vector and minimizing the trace of the Cramer-Rao lower bound as the objective function, and using the penalty function multiplier method to transform the constrained optimization model into an unconstrained optimization model specifically includes:

[0024] The state vector to be optimized is set to the heading angle of the UAV at the current moment;

[0025] Constraining the angular velocity of the drone, calculating the angular velocity of the drone at the current moment according to the measurement step length, wherein the constraint condition is that the angular velocity of the drone at the current moment is less than or equal to the maximum angular velocity;

[0026] The positioning estimation result is used to approximately calculate the minimum Cramer-Rao lower bound to obtain a constrained optimization model;

[0027] The penalty function multiplier method is used to construct the augmented objective function and transform the constrained optimization model into an unconstrained optimization model.

[0028] Furthermore, solving the unconstrained optimization model to obtain the optimal heading angle of the UAV at the current moment specifically includes:

[0029] The unconstrained optimization model is solved by adopting the PHR algorithm, and the unconstrained subproblems in the unconstrained optimization model are solved by adopting the quasi-Newton method.

[0030] Furthermore, the first receiving device and the second receiving device include antennas.

[0031] On the other hand, the present invention further provides a UAV trajectory planning device, which is used to implement any of the aforementioned UAV trajectory planning methods, and the device includes:

[0032] a phase difference measurement data acquisition module, wherein the phase difference measurement data acquisition module acquires the phase difference measurement data of the first receiving device and the second receiving device at a current moment;

[0033] An initial estimated position calculation module, wherein the module calculates a cost surface based on the phase difference measurement value, and then performs a grid search within a preset range centered on the initial position of the drone to find the initial estimated position of the drone;

[0034] A UAV positioning module, wherein the UAV positioning module performs iterative least squares convergence on a local neighborhood of the grid points corresponding to the initial estimated position to complete the positioning of the UAV and obtain an estimated positioning result of the UAV at the current moment;

[0035] A model building module, wherein the model building module uses the heading angle of the UAV as a state vector and minimizes the trace of the Cramer-Rao lower bound as an objective function to establish an optimization model, and adopts a penalty function multiplier method to convert the constrained optimization model into an unconstrained optimization model;

[0036] An optimal heading angle calculation module, wherein the optimal heading angle calculation module solves the unconstrained optimization model to obtain the optimal heading angle of the UAV at the current moment;

[0037] The optimal track point calculation module calculates the next optimal track point according to the current UAV speed and the optimal heading angle.

[0038] On the other hand, the present invention also provides a computer device, which includes a processor and a memory, wherein a computer program is stored in the memory, and the computer program is loaded and executed by the processor to implement any of the above-mentioned drone trajectory planning methods.

[0039] On the other hand, the present invention also provides a computer-readable storage medium, in which a computer program is stored. The computer program is loaded and executed by a processor to implement any of the above-mentioned UAV trajectory planning methods.

[0040] The beneficial effects of the present invention are:

[0041] (1) The present invention adopts a long-baseline interferometer positioning method to quickly and accurately locate distant radiation sources. At the same time, it introduces UAV trajectory planning technology, taking the trajectory minimization of the Cramer-Rao Lower Bound (CRLB) as the objective function, and solves the optimal trajectory point of the UAV in real time, thereby improving the positioning accuracy.

[0042] (2) The present invention takes into account the physical limitations of the UAV and uses the maximum angular velocity of the UAV as a constraint to plan a practical flight path for the UAV.

[0043] (3) The present invention adopts the multiplier method to solve the optimization problem, which does not require the value of the penalty parameter σ to tend to infinity. The algorithm is stable and converges quickly. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] Figure 1 This is a flow chart of a method for planning a UAV trajectory according to an embodiment of the present invention;

[0045] Figure 2 is a long baseline positioning scene graph used in an embodiment of the present invention;

[0046] Figure 3 is the trajectory planning result of the UAV according to the embodiment of the present invention;

[0047] Figure 4 Schematic diagram of the change of positioning error with measurement time according to an embodiment of the present invention;

[0048] Figure 5 This is a structural block diagram of the UAV trajectory planning device provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0049] The following describes the embodiments of the present invention through specific examples. Those skilled in the art can easily understand other advantages and effects of the present invention from the content disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments. The details in this specification can also be modified or changed based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the following embodiments and features in the embodiments can be combined with each other unless they conflict.

[0050] Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making any creative work shall fall within the scope of protection of the present invention.

[0051] The limitation of traditional trajectory planning schemes for emitter positioning lies in the relatively mature trajectory planning technology for UAVs using multi-platform positioning systems such as AOA and TDOA, which can improve positioning accuracy and plan effective and flyable trajectories for UAVs. However, in the scenario of long-baseline positioning methods for single-platform UAVs, only a "generally optimal" trajectory is proposed, without considering the impact of the UAV's physical limitations on factors such as turning angles. In actual applications, the maneuverability of UAVs is generally difficult to achieve the spiral trajectory proposed in this paper, and the actual flyability of the trajectory is not rigorous.

[0052] In order to solve the above technical problems, the following embodiments of the UAV trajectory planning method, device, equipment and medium of the present invention are proposed.

[0053] Example 1

[0054] Reference Figure 1 ,like Figure 1 The figure shows a flow chart of the UAV trajectory planning method provided in this embodiment, which specifically includes the following steps:

[0055] Step 1: Initialize system parameters. The system parameters to be initialized include: the initial position of the drone, velocity V, maximum angular velocity ψ max (unit is ° / sec), the measurement step length Δt of the drone, the baseline length L. The positions of antenna 1 and antenna 2 as receiving devices at time t [x a1 (t),y a1 (t)] T 、[x a2 (t),y a2 (t)] T , the position of antenna 1 is regarded as the position of the drone. In addition, the standard deviation of the phase difference measurement noise is known to be δ, the frequency of the received radiation source signal is f0Hz, and c represents the electromagnetic wave transmission rate.

[0056] Step 2: At time t, the UAV obtains the phase difference measurement data between the two antennas at the current track point.

[0057] Specifically, at the current time t, the phase difference measurement value between the two antennas is obtained And combined with all the measured values so far, the phase difference measurement sequence is obtained The measured values are corrupted by independent and identically distributed additive zero Gaussian white noise. The noise follows a Gaussian distribution with mean zero and standard deviation δ, which can be modeled as:

[0058]

[0059] Among them, Δφ i The true position of the radiation source Pe = [xt ,y t ] T function.

[0060] Step 3: Based on the phase difference measurement data in step 2, calculate the cost surface and perform a grid search within a certain range to find the initial estimated position of the radiation source.

[0061] Specifically, since the spatial interval between antennas is greater than half the signal wavelength, there is a phase wrapping effect, which leads to blurred phase difference measurement. The true phase difference Δφ∈[0,+∞) is wrapped into Δφ∈[0,2π), which is the actual phase difference measurement value. Therefore, a corrugated least squares cost surface will be generated when estimating the position of the radiation source.

[0062] Divide the grid points within a certain range and the search step size is d, then the cost surface is calculated as follows:

[0063]

[0064] is the phase difference measurement under the phase wrapping effect, i.e. the measurement value in step (2), Δφ i is the noise-free phase difference calculated at each grid point, Δφ i The calculation formula is as follows.

[0065]

[0066] Where [x,y] T ∈S, S represents the set of possible radiation source locations on the grid, φ0 is the system error known through calibration, t i Indicates the measurement time corresponding to the i-th measurement value.

[0067] When the actual phase difference measurement value is equal to the noise-free phase difference at a certain position, the value of the cost surface will be minimized, indicating that the position is the location of the target radiation source. To facilitate observation and analysis, in the present invention, the inverse of the cost surface is uniformly taken, that is, the cost surface is converted from minimization to maximization.

[0068] Step 4: Based on the initial estimated position in step 3, iterative least squares convergence is performed on the local neighborhood of the grid point to complete the positioning of the radiation source and obtain the positioning result of the drone at the current moment.

[0069] Specifically, the position data of the drone at each moment is known, and the phase difference is only a function of the actual radiation source position. This forms a vector parameter estimation problem, where the parameter x is the radiation source position and the observed value is the phase difference. The relationship between the two is nonlinear. The Gauss-Newton algorithm is used, and the initial value is the grid point corresponding to the maximum value of the cost surface in step (3). It is then iterated to converge to the least squares method.

[0070] During the least squares convergence, the estimated value of the radiation source position in each iteration is:

[0071]

[0072] Where n is the iteration time and H is expressed as follows:

[0073]

[0074] H is the Jacobian matrix of the phase difference with respect to the radiation source position, and T represents the transpose. is the prediction error caused by the Taylor series approximation when applying the Gauss-Newton method, I Δφ is the phase difference measurement noise covariance matrix, i.e. diag(δ 2 ,δ 2 ,…,δ 2 ) N×N .

[0075] H is obtained by taking each measurement value Δφ i ,i=1,2…,N pairs of x t and y t The derivative of is calculated analytically.

[0076]

[0077] From this, the derivative is numerically calculated as follows:

[0078]

[0079]

[0080] During iterative calculations, the current estimated result is used instead of x t ,y t After the iteration is completed, the final estimated result of the radiation source position at time t is obtained.

[0081] Step 5: Based on the phase difference measurement data and antenna position in step 2, the heading angle of the UAV is used as the state vector and the CRLB trace of the long baseline positioning is used as the objective function to establish an optimization model. Considering the maximum angular velocity limit of the UAV, the penalty function multiplier method is used to transform the constrained optimization model into an unconstrained optimization model.

[0082] Specifically, the state vector to be optimized is set to the heading angle of the drone at the current moment:

[0083] ρ=u(t)

[0084] The angular velocity of the UAV is constrained. Since the measurement step of the system is Δt, the angular velocity of the UAV at time t can be calculated by the following formula:

[0085]

[0086] The maximum angular velocity of the drone is ψ max , then the constraints are written as:

[0087] Determine the optimization model: The optimal heading angle obtained should minimize the trace of the CRLB matrix of the UAV at time t+1. The calculation formula of the CRLB matrix is as follows:

[0088]

[0089] The estimated position of the radiation source at time t is used to replace the true position of the radiation source to approximately calculate the CRLB.

[0090] Thus, the optimization model is obtained as follows:

[0091]

[0092]

[0093] The penalty function multiplier method is used to transform the constrained optimization problem in (53) into an unconstrained optimization problem.

[0094] Construct the augmented objective function:

[0095]

[0096] Where x = u(t),

[0097] Therefore, the optimization problem to be solved is transformed into:

[0098] minψ(x,λ k ,σ k )

[0099] Step 6: Use the PHR algorithm to implement the solution process of the multiplier method, and use the quasi-Newton method to solve the sub-problem, and finally obtain the optimal heading angle of the drone at the current moment.

[0100] Specifically, for the unconstrained optimization problem obtained in step 5, the PHR algorithm is used to solve it:

[0101] Select the initial value x0, given parameter σ1>0, θ∈(0,1), η>1, termination error 0≤ε=1. Let k=1.

[0102] Solve the subproblem. k-1 As the initial point, the quasi-Newton method is used to solve the unconstrained subproblem minψ(x,λ k ,σ k ), get the minimum point x k .

[0103] Test the termination condition. If β k ≤ε, then stop the iteration and output x k As the approximate minimum point of the original problem, otherwise it switches to the step of updating the penalty parameter, β k The calculation formula is as follows:

[0104]

[0105] Update the penalty parameter. If β k ≥θβ k-1 , let σ k+1 =ησ k , otherwise σ k+1 =σ k .

[0106] Update the multiplier vector. k+1 =max{0,λ k -g(x k )}.

[0107] Let k=k+1 and switch to solving the sub-problem.

[0108] Step 7: Calculate the next optimal track point based on the drone speed and optimal heading angle.

[0109] Specifically, according to the optimal heading angle u obtained in step 6 opt (t), get the coordinates of the next track point of the UAV (equivalent to antenna 1), the calculation formula is as follows:

[0110] x a1 (t+1)=x a1 (t)+V·Δt·cos(u opt (t))

[0111] y a1 (t+1)=y a1 (t)+V·Δt·sin(u opt (t))

[0112] Step 8: The drone moves to the track point obtained in step 7, time t = t + 1, and repeats steps 2 to 7 until the measurement time ends.

[0113] This embodiment uses long-baseline positioning. Based on the phase difference measurements between the two antennas, a grid search is performed on the cost surface to determine the grid point location of the radiation source. This point is then used as the initial value for least squares convergence to obtain the final estimated position. Simultaneously, while locating the radiation source, the objective function is to minimize the Cramer-Rao Lower Bound (CRLB). Taking the physical constraints of the drone into account, the next optimal track point is obtained, and a practical, flyable track is planned for the drone, enabling it to obtain high-quality, effective data in a relatively short period of time, thereby improving the positioning of the target radiation source. On the one hand, the positioning method based on long-baseline interferometry can achieve rapid and high-precision positioning of long-range targets. On the other hand, by implementing real-time track planning for the drone, the drone's operating efficiency and survivability can be improved. Combining the advantages of the drone platform and the long-baseline positioning method, the drone's operational effectiveness is maximized, and the accuracy and sensitivity of reconnaissance positioning are improved.

[0114] Example 2

[0115] In this embodiment, the target radiation source is located at The frequency of the radiation source signal is 10GHz, and the initial position of the drone is [0,0] T m, the initial velocity is along the x-axis. To plan a practical flight path for the drone, the specific implementation method is as follows:

[0116] Step 1: Set initialization parameters;

[0117] The drone flies at a constant speed, the speed is set to 16m / s, and the maximum angular velocity ψ max The measurement step of the drone is 1 second, the total measurement time is 100 seconds, the baseline length between the two antennas is 5 meters, and the system phase difference measurement error is 10 degrees, which obeys the zero-mean Gaussian distribution;

[0118] Step 2: Get the phase difference measurement value at the current moment. Assuming the initial time is 0 and the current time is t=50s, a total of 50 phase difference measurement values are accumulated.

[0119] Step 3: Calculate the cost surface based on the phase difference measurement data. Perform a grid search with a step size of 320m within an area of 80km × 80km centered on the origin of the coordinate system to find the grid point corresponding to the maximum value of the cost surface.

[0120] Step 4: Perform iterative least squares convergence on the grid points searched in step 3 to obtain the radiation source positioning result at the current moment;

[0121] Step 5: Use the current UAV heading angle as the optimization vector and the CRLB trace at the next moment as the objective function. Use the current estimated position of the radiation source instead of the actual position of the radiation source to approximately calculate the CRLB. Considering the maximum angular velocity limit of the UAV, the optimization model is established as follows:

[0122]

[0123]

[0124] The penalty function multiplier method is used to transform the constrained optimization model into an unconstrained optimization model J'=minψ(u(t),λ k ,σ k ).

[0125] Step 6: Use the PHR algorithm to solve the problem. The initial value x0 is the heading angle of the drone at the previous moment. The values of the parameters are: σ1 = 2, λ1 = 0.1, θ = 0.8, η = 2, ε = 1e-5. The optimal heading angle u at the current moment is obtained. opt (t).

[0126] Step 7: Get the drone's position at the next moment based on the drone's speed and heading angle at that moment.

[0127] x a1 (t+1)=x a1 (t)+16·Δt·cos(u opt (t))

[0128] y a1 (t+1)=y a1 (t)+16·Δt·sin(u opt (t))

[0129] Step 8: Move the drone to the position obtained in step 7, t = t + 1, and repeat steps 2 to 7 until the total measurement time is over.

[0130] Reference Figure 3-4 , Figure 3 is the trajectory planning result of the UAV in this embodiment, Figure 4 Schematic diagram of the change of positioning error with measurement time in an embodiment of the present invention.

[0131] from Figure 4 It can be seen that as the measurement time increases, the positioning error continues to converge and is much smaller than 1%R, meeting the requirements of high-precision positioning.

[0132] The drone trajectory planning method proposed in this embodiment can achieve higher positioning accuracy than conventional straight-line trajectory.

[0133] Example 3

[0134] Reference Figure 5 ,like Figure 5 The figure shows a block diagram of the structure of the UAV trajectory planning device provided in this embodiment, which specifically includes the following structures:

[0135] a phase difference measurement data acquisition module, wherein the phase difference measurement data acquisition module acquires the phase difference measurement data of the first receiving device and the second receiving device at a current moment;

[0136] An initial estimated position calculation module, wherein the module calculates a cost surface based on the phase difference measurement value, and then performs a grid search within a preset range centered on the initial position of the drone to find the initial estimated position of the drone;

[0137] A UAV positioning module, wherein the UAV positioning module performs iterative least squares convergence on a local neighborhood of the grid points corresponding to the initial estimated position to complete the positioning of the UAV and obtain an estimated positioning result of the UAV at the current moment;

[0138] A model building module, wherein the model building module uses the heading angle of the UAV as a state vector and minimizes the trace of the Cramer-Rao lower bound as an objective function to establish an optimization model, and adopts a penalty function multiplier method to convert the constrained optimization model into an unconstrained optimization model;

[0139] An optimal heading angle calculation module, wherein the optimal heading angle calculation module solves the unconstrained optimization model to obtain the optimal heading angle of the UAV at the current moment;

[0140] The optimal track point calculation module calculates the next optimal track point according to the current UAV speed and the optimal heading angle.

[0141] Example 4

[0142] This preferred embodiment provides a computer device that can implement the steps in any embodiment of the drone trajectory planning method provided in the embodiments of the present application. Therefore, the beneficial effects of the drone trajectory planning method provided in the embodiments of the present application can be achieved. Please refer to the previous embodiments for details and will not be repeated here.

[0143] Example 5

[0144] Those skilled in the art will appreciate that all or part of the steps in the various methods of the above embodiments can be accomplished through instructions, or by controlling related hardware through instructions. The instructions can be stored in a computer-readable storage medium and loaded and executed by a processor. To this end, an embodiment of the present invention provides a storage medium storing a plurality of instructions that can be loaded by a processor to execute the steps of any embodiment of the drone trajectory planning method provided in the embodiment of the present invention.

[0145] The storage medium may include a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk, etc.

[0146] Since the instructions stored in the storage medium can execute the steps in any of the UAV trajectory planning method embodiments provided in the embodiments of the present invention, the beneficial effects that can be achieved by any of the UAV trajectory planning methods provided in the embodiments of the present invention can be achieved. Please refer to the previous embodiments for details and will not be repeated here.

[0147] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A UAV trajectory planning method, characterized in that: The method is based on long baseline positioning and includes setting a first receiving device on the UAV and setting a second receiving device within the communication range of the first receiving device. The method includes: Obtaining phase difference measurement data between the first receiving device and the second receiving device at the current moment; A cost surface is calculated based on the phase difference measurement value, and then a grid search is performed within a preset range with the initial position of the UAV as the center to find the initial estimated position of the UAV, including: dividing the grid points within the preset range and setting the search step size. The calculation formula of the cost surface includes: in, is the phase difference measurement, Δφ i is the noise-free phase difference calculated at each grid point, Δφ i The calculation formula includes: Where [x,y] T ∈S, S represents the set of possible radiation source locations on the grid, φ0 is the system error known through calibration, t i Indicates the measurement time corresponding to the i-th measurement value, x a1 (t i ) indicates that the first receiving device is at t i The horizontal coordinate of the time, x a2 (t i ) indicates that the second receiving device is at t i The horizontal axis of the time, y a1 (t i ) indicates that the first receiving device is at t i The vertical coordinate of the time, y a2 (t i ) indicates that the second receiving device is at t i The vertical coordinate at the time, f0 represents the frequency of the received radiation source signal, and c represents the electromagnetic wave transmission rate; When the actual phase difference measurement value is equal to the noise-free phase difference at a certain position, the value of the cost surface will be minimized, indicating that the position is the location of the drone; Performing iterative least squares convergence on the local neighborhood of the grid points corresponding to the initial estimated position to complete the positioning of the UAV and obtain the positioning estimation result of the UAV at the current moment; Taking the heading angle of the UAV as the state vector and minimizing the Cramer-Rao lower bound trace as the objective function, an optimization model is established. The penalty function multiplier method is used to transform the constrained optimization model into an unconstrained optimization model. Solving the unconstrained optimization model to obtain the optimal heading angle of the UAV at the current moment; The next optimal track point is calculated based on the current UAV speed and the optimal heading angle.

2. The UAV trajectory planning method according to claim 1, wherein: The method further includes setting the initial position, speed, maximum angular velocity, measurement step length and baseline length of the drone before obtaining the phase difference measurement value between the first receiving device and the second receiving device at the current moment.

3. The UAV trajectory planning method according to claim 2, wherein: The obtaining of the phase difference measurement data between the first receiving device and the second receiving device at the current moment specifically includes: The phase difference measurement value of the first receiving device and the second receiving device is obtained at the current moment, and all measurement values from the initial moment to the current moment are combined to obtain a phase difference measurement sequence.

4. The UAV trajectory planning method according to claim 1, wherein: The method uses the heading angle of the UAV as the state vector and minimizes the trace of the Cramer-Rao lower bound as the objective function to establish an optimization model. The penalty function multiplier method is used to transform the constrained optimization model into an unconstrained optimization model. Specifically, the method includes: The state vector to be optimized is set to the heading angle of the UAV at the current moment; Constraining the angular velocity of the drone, calculating the angular velocity of the drone at the current moment according to the measurement step length, wherein the constraint condition is that the angular velocity of the drone at the current moment is less than or equal to the maximum angular velocity; The positioning estimation result is used to approximately calculate the minimum Cramer-Rao lower bound to obtain a constrained optimization model; The penalty function multiplier method is used to construct the augmented objective function and transform the constrained optimization model into an unconstrained optimization model.

5. The UAV trajectory planning method according to claim 4, wherein: Solving the unconstrained optimization model to obtain the optimal heading angle of the UAV at the current moment specifically includes: The unconstrained optimization model is solved by adopting the PHR algorithm, and the unconstrained subproblems in the unconstrained optimization model are solved by adopting the quasi-Newton method.

6. The UAV trajectory planning method according to claim 1, wherein: The first receiving device and the second receiving device include antennas.

7. A UAV trajectory planning device, characterized in that: The device is used to implement the UAV trajectory planning method according to any one of claims 1 to 6, and the device includes: a phase difference measurement data acquisition module, wherein the phase difference measurement data acquisition module acquires the phase difference measurement data of the first receiving device and the second receiving device at a current moment; The initial estimated position calculation module calculates a cost surface based on the phase difference measurement value, and then performs a grid search within a preset range with the initial position of the drone as the center to find the initial estimated position of the drone, including: dividing the grid points within the preset range and setting the search step size. The calculation formula of the cost surface includes: in, is the phase difference measurement, Δφ i is the noise-free phase difference calculated at each grid point, Δφ i The calculation formula includes: Where [x,y] T ∈S, S represents the set of possible radiation source locations on the grid, φ0 is the system error known through calibration, t i Indicates the measurement time corresponding to the i-th measurement value, x a1 (t i ) indicates that the first receiving device is at t i The horizontal coordinate of the time, x a2 (t i ) indicates that the second receiving device is at t i The horizontal axis of the time, y a1 (t i ) indicates that the first receiving device is at t i The vertical coordinate of the time, y a2 (t i ) indicates that the second receiving device is at t i The vertical coordinate at the time, f0 represents the frequency of the received radiation source signal, and c represents the electromagnetic wave transmission rate; When the actual phase difference measurement value is equal to the noise-free phase difference at a certain position, the value of the cost surface will be minimized, indicating that the position is the location of the drone; A UAV positioning module, wherein the UAV positioning module performs iterative least squares convergence on a local neighborhood of the grid points corresponding to the initial estimated position to complete the positioning of the UAV and obtain an estimated positioning result of the UAV at the current moment; A model building module, wherein the model building module uses the heading angle of the UAV as a state vector and minimizes the trace of the Cramer-Rao lower bound as an objective function to establish an optimization model, and adopts a penalty function multiplier method to convert the constrained optimization model into an unconstrained optimization model; An optimal heading angle calculation module, wherein the optimal heading angle calculation module solves the unconstrained optimization model to obtain the optimal heading angle of the UAV at the current moment; The optimal track point calculation module calculates the next optimal track point according to the current UAV speed and the optimal heading angle.

8. A computer device, characterized in that: The computer device includes a processor and a memory, wherein a computer program is stored in the memory, and the computer program is loaded and executed by the processor to implement the UAV trajectory planning method according to any one of claims 1 to 6.

9. A computer-readable storage medium, characterized in that The storage medium stores a computer program, which is loaded and executed by a processor to implement the drone trajectory planning method according to any one of claims 1 to 6.

Images