Navigation positioning method based on iteration strategy

Abstract

The invention discloses a navigation positioning method based on an iterative strategy, and the method comprises the steps: taking the sum of the actual distance, clock error and noise between an unmanned plane and a target position as a pseudo-range, building a positioning equation of pseudo-range observed quantity, carrying out the linear processing of the positioning equation through Taylor series expansion, and carrying out the linearization of an unmanned plane observation equation, a target function is defined according to a model prediction value and an observation value, the gradient of the target function is calculated, a Hessian matrix is fitted through a Jacobian matrix, and the gradient descent direction is determined; constructing an auxiliary function, and searching an optimal step length in the determined gradient descent direction; carrying out linear search by utilizing a Wolfe condition; performing iterative updating; carrying out convergence test; and adjusting a damping coefficient in the LM algorithm according to a linear search result to find an optimal solution to realize navigation positioning. According to the method, the accuracy and stability of navigation positioning are improved based on two iteration strategies, and the nonlinear problem in the navigation process is solved.

Description

Technical Field

[0001] The present invention relates to the technical field of positioning systems, and in particular to a navigation positioning method based on an iteration strategy. Background Art

[0002] Gauss-Newton iteration is a special form of Newton iteration, an iterative method in nonlinear regression models. It corrects the regression coefficients through multiple iterations to minimize the sum of squares of the residuals of the original model, thereby obtaining the optimal solution. However, the Gauss-Newton iteration algorithm can only converge to the ideal point when certain local conditions are met. As a suboptimal algorithm, it cannot achieve global convergence, and may even diverge when the initial value error is large, resulting in poor stability of the algorithm. It will only converge when the initial value is near the solution.

[0003] Levenberg-Marquardt optimization is also called Levenberg-Marquardt method, which was first published by Kenneth Levenberg. It was later rediscovered by statistician Donald Marquardt. In mathematics and computing, Levenberg-Marquardt algorithm can be used to solve nonlinear least squares problems. It can provide numerical solutions for nonlinear minimization (local minimum) and is an improved method for Gauss-Newton iteration. This method combines the advantages of Gauss-Newton algorithm and gradient descent method by setting the damping factor, ensuring the correctness of iterative search direction and the rationality of step size, and to a certain extent solves the problem of poor numerical stability of Gauss-Newton method leading to algorithm iteration convergence.

[0004] Ideally, the cost function should decrease during each iteration, that is,

[0005] But this is not always the case, and the error often increases with the iteration process. Therefore, linear search can be used to solve this problem, so that it proceeds in the direction of decreasing the cost function during the iterative estimation process to prevent the above situation from happening.

[0006] First, choose the step size appropriately: ,in, is the search step length, is the search direction. Then follow the direction of decreasing cost, that is

[0007] Need to Start looking for a suitable ,and , so that the appropriate search step size is selected and search direction satisfy ; Then select Wolfe rule to perform linear search; Finally update the iteration point . Summary of the invention

[0008] The present invention aims to solve one of the technical problems in the related art at least to a certain extent.

[0009] To this end, the present invention proposes a navigation and positioning method based on an iterative strategy, which uses the LM algorithm to solve the problem of poor numerical stability and improve the stability of the system; uses linear search to make the iteration converge along the direction of the minimum cost function, prevents the error from becoming larger and larger during the iteration process, and improves the accuracy of the system.

[0010] To achieve the above object, the present invention proposes a navigation positioning method based on an iterative strategy, comprising: The actual distance between the UAV and the target position, the sum of the clock error and the noise are regarded as pseudo-range to establish the positioning equation of pseudo-range observation, and the positioning equation is linearized by Taylor series expansion to linearize the UAV observation equation, and the objective function is defined according to the model prediction value and the observation value, and the gradient of the objective function is calculated, and the Hessian matrix is ​​fitted by the Jacobian matrix to determine the gradient descent direction; Construct an auxiliary function and find the optimal step size in the determined gradient descent direction; Use Wolfe conditions to perform linear search; Iterative updates; Convergence test; The damping coefficient in the LM algorithm is adjusted according to the results of the linear search to find the optimal solution for navigation and positioning.

[0011] The navigation and positioning method based on the iteration strategy of the embodiment of the present invention may have the following additional technical features: In one embodiment of the present invention, the actual distance between the UAV and the target position, the sum of the clock error and the noise are regarded as pseudo-range to establish a positioning equation of pseudo-range observation, and the positioning equation is linearized by Taylor series expansion to linearize the UAV observation equation, and the objective function is defined according to the model prediction value and the observation value, the gradient of the objective function is calculated, and the Hessian matrix is ​​fitted by the Jacobian matrix to determine the gradient descent direction, including: Considering the clock error and various noise measurement errors, the positioning equation established by the pseudorange observation is:

[0012]

[0013]

[0014]

[0015]

[0016] Linearize the observation equation and set the user's approximate coordinates to , take the corresponding correction number as ,Will , , Substituting into, performing Taylor series expansion, and ignoring high-order terms, the observation equation of the i-th UAV is linearized as follows:

[0017] in, , is the approximate distance from the i-th UAV to the user; is the unit vector in the direction of the i-th UAV connection line, expressed as:

[0018] Label the terms on the right side of the equation as:

[0019] Observable , The drone clock error is calculated based on the user's approximate location and the drone's location. And various delays According to the relevant model, is the free term of the observation equation; Rewrite the above formula as:

[0020] To measure the error caused by noise and other unmodeled factors, the objective function is defined as:

[0021] in, is the model prediction value, is the i-th observation value, which is converted into a vector form:

[0022] Among them, r is the residual vector:

[0023] Create the Jacobian matrix:

[0024] The gradient is expressed as:

[0025] In the Gauss-Newton algorithm, the Jacobian matrix is ​​used to fit the Hessian matrix:

[0026] Introduce a diagonal matrix:

[0027] At this time, the damping factor is introduced into the solution process : .

[0028] In one embodiment of the present invention, constructing an auxiliary function and finding an optimal step size in a determined gradient descent direction includes:

[0029] in, is the current iteration point, yes The descending direction at , the search step length is .

[0030] In one embodiment of the present invention, the Wolfe condition is used to perform a linear search, including: Introducing the Armijo criterion, assuming yes The descending direction at

[0031] The search step length satisfies the Armijo criterion, where is a constant that satisfies the curvature criterion:

[0032] in , the Wolfe condition is to satisfy both the Armijo criterion and the curvature criterion.

[0033] In one embodiment of the present invention, the iterative update includes: using a linear search method to determine the search step length , and then iteratively update the point ;in .

[0034] In one embodiment of the present invention, the convergence test includes: After each iteration, check whether the algorithm has converged; ,when All elements in are less than or equal to The cycle ends when .

[0035] In one embodiment of the present invention, adjusting the damping coefficient in the LM algorithm according to the result of the linear search includes: By scaling factor Sure:

[0036] is an initial damping factor used to control the effect of the Jacobian matrix on the solution update. v It is the adjustment multiple of the damping factor, which is adjusted dynamically during each iteration.

[0037] The navigation and positioning method based on the iteration strategy of the embodiment of the present invention is applied to the navigation and positioning system of various scenarios to solve the nonlinear problem in the navigation process, use the LM algorithm to solve the problem of poor numerical stability, and improve the stability of the system; use linear search to make the iteration converge along the direction of the minimum cost function, prevent the error from becoming larger and larger during the iteration process, and improve the accuracy of the system.

[0038] Additional aspects and advantages of the present invention will be given in part in the following description and in part will be obvious from the following description, or will be learned through practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] The above and / or additional aspects and advantages of the present invention will become apparent and easily understood from the following description of the embodiments in conjunction with the accompanying drawings, in which: Figure 1 is a flow chart of a navigation and positioning method based on an iteration strategy according to an embodiment of the present invention; Figure 2 It is a logic diagram of a navigation and positioning method based on an iteration strategy according to an embodiment of the present invention. DETAILED DESCRIPTION

[0040] It should be noted that, in the absence of conflict, the embodiments of the present invention and the features in the embodiments can be combined with each other. The present invention will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0041] In order to enable those skilled in the art to better understand the scheme of the present invention, the technical scheme in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work should fall within the scope of protection of the present invention.

[0042] The following describes a navigation and positioning method based on an iteration strategy according to an embodiment of the present invention with reference to the accompanying drawings.

[0043] Figure 1 is a navigation and positioning method based on an iteration strategy according to an embodiment of the present invention, such as Figure 1 As shown, the method includes: S1, the actual distance between the UAV and the target position, the sum of the clock error and the noise are regarded as pseudo-range to establish the positioning equation of pseudo-range observation, and the positioning equation is linearized by Taylor series expansion to linearize the UAV observation equation, and the objective function is defined according to the model prediction value and the observation value, the gradient of the objective function is calculated, and the Hessian matrix is ​​fitted by the Jacobian matrix to determine the gradient descent direction; S2, constructs an auxiliary function and searches for the optimal step size in the determined gradient descent direction; S3, linear search using Wolfe conditions; S4, iterative update; S5, convergence test; S6, adjusting the damping coefficient in the LM algorithm according to the result of the linear search to find the optimal solution to achieve navigation positioning.

[0044] It can be understood that the present invention provides a navigation positioning method based on an iterative strategy, which can be applied to various positioning scenarios. Figure 2 Said: In one embodiment of the present invention, a gradient descent direction is determined.

[0045] Specifically, only one scenario is listed here. In fact, this method is applicable to a variety of navigation and positioning scenarios. In the scenario related to UAV positioning, the sum of the true distance, clock error, and noise between multiple UAVs and the target position is regarded as similar to the pseudo-range in the satellite. Considering the measurement errors such as clock error and various noises, the positioning equation established by the pseudo-range observation is:

[0046]

[0047]

[0048]

[0049] …

[0050] Each pseudo-range measurement equation is a nonlinear function of the user's position. When performing positioning calculations, the user's position cannot be directly obtained. The observation equation must be linearized first. Set the user's approximate coordinates to , take the corresponding correction number as ,Will , , Substituting into the above equation, performing Taylor series expansion, and ignoring high-order terms, the observation equation of the i-th UAV can be linearized as

[0051] in, , is the approximate distance from the i-th UAV to the user; is the unit vector in the direction of the i-th UAV connection line, which can be expressed as

[0052] Label the terms on the right side of the formula as

[0053] Since the observed Known, The drone clock error can be calculated based on the user's approximate location and the drone's location. And various delays etc. can be calculated according to the relevant model, so is the free term of the observation equation.

[0054] Rewrite the above formula into

[0055] is the error caused by measurement noise and other unmodeled factors. Then define the objective function

[0056] in, is the model prediction value, is the i-th observation value. Converted into vector form, it is

[0057] Among them, r is the residual vector

[0058] Building the Jacobian Matrix

[0059] The gradient is expressed as

[0060] In the Gauss-Newton algorithm, the Jacobian matrix is ​​used to fit the Hessian matrix.

[0061] However, the result calculated by this method is not necessarily positive definite, that is, it cannot be guaranteed to be in a descending direction. Therefore, a diagonal matrix should be introduced to ensure that the result is positive definite:

[0062] At this time, the damping factor is introduced into the solution process

[0063]

[0064] In one embodiment of the present invention, an auxiliary function is constructed to find the optimal step length in a determined descending direction.

[0065]

[0066] in, is the current iteration point, yes The descending direction at , the search step length is .

[0067] In one embodiment of the present invention, the Wolfe condition is used to perform a linear search.

[0068] First, the Armijo criterion is introduced to ensure that each iteration is sufficiently reduced. yes The descending direction at .

[0069] The search step length satisfies the Armijo criterion, where is a constant. In practical applications, the parameter Usually a small positive number is chosen, such as , which makes the criterion easier to satisfy. From the geometric representation of the Armijo criterion, we can see that as long as Small enough to satisfy the Armijo criterion, which will result in insufficient drop. Too small, need to meet the curvature criteria:

[0070] in If the above formula is satisfied, the left side of the formula may be a large negative number, which means It has already approached the minimum value; the left side of the formula may also be a positive number, which indicates If the minimum value has been exceeded, it is reasonable to stop the linear search at this time. If the above formula is not satisfied, the left side of the formula is a small negative number, which means that Direction There is still a lot of room for decline, and we can continue to search for a better step length. In practical applications, according to The solution is different. The Wolfe condition satisfies both the Armijo criterion and the curvature criterion.

[0071] In one embodiment of the present invention, the search step length is determined by iterative updating using a linear search method. , and then iteratively update the point .in .

[0072] In one embodiment of the present invention, convergence test: After each iteration, it is checked whether the algorithm has converged.

[0073] From the above steps, we can see that ,when All elements in are less than or equal to The cycle ends when .

[0074] In one embodiment of the present invention, the damping coefficient is adjusted. According to the result of the linear search, it may be necessary to adjust the damping coefficient in the LM algorithm.

[0075] If the new iteration point provides a better function value reduction, then the damping factor needs to be reduced to allow a larger step size; if not, then the damping factor needs to be increased to ensure stability. If the damping factor is adjusted directly by multiplying or dividing by a fixed multiple, the step adjustment will be too large or too small, resulting in inaccurate estimation of the result and falling into a local optimum. Therefore, it is necessary to adjust the damping factor by a proportional factor. To determine:

[0076] is an initial damping factor used to control the effect of the Jacobian matrix on the solution update. v is the adjustment multiple of the damping factor, which is adjusted dynamically during each iteration. , the new parameter estimates provide a better fit, then accept the new set of parameter estimates and dynamically update . An adjustment factor based on how fast the error decreases is calculated. If the error decreases quickly, this adjustment factor will be close to 0, making Keep it the same or only decrease it slightly to maintain the Gauss-Newton nature of the algorithm. If the error decreases more slowly, the adjustment factor will be decreased The value of makes it tend to the direction of gradient descent. At the same time, it can also ensure It won't become too small, at least the original One quarter of the time, then reset v ; If the residual sum of squares increases or does not improve significantly, that is , that is, the new parameter estimates do not improve the results, then reject this set of new parameter estimates and update , to prevent too large a step size from causing the result to exceed the optimal solution, v Double the value, ready to make larger adjustments in the next iteration.

[0077] The navigation and positioning method based on the iteration strategy according to the embodiment of the present invention can provide efficient and accurate positioning services for drone positioning systems, air-space-ground positioning systems, etc. The method fully considers the challenges brought by the denial environment and ensures reliable user positioning under the most unfavorable conditions through navigation and positioning.

[0078] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "example", "specific example", or "some examples" etc. means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner. In addition, those skilled in the art may combine and combine the different embodiments or examples described in this specification and the features of the different embodiments or examples, without contradiction.

[0079] In addition, the terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first" and "second" may explicitly or implicitly include at least one of the features. In the description of the present invention, the meaning of "plurality" is at least two, such as two, three, etc., unless otherwise clearly and specifically defined.

Claims

1. A navigation and positioning method based on an iterative strategy, characterized in that: include: The actual distance between the UAV and the target position, the sum of the clock error and the noise are regarded as pseudo-range to establish the positioning equation of pseudo-range observation, and the positioning equation is linearized by Taylor series expansion to linearize the UAV observation equation, and the objective function is defined according to the model prediction value and the observation value, and the gradient of the objective function is calculated, and the Hessian matrix is ​​fitted by the Jacobian matrix to determine the gradient descent direction; Construct an auxiliary function and find the optimal step size in the determined gradient descent direction; Use Wolfe conditions to perform linear search; Iterative updates; Convergence test; The damping coefficient in the LM algorithm is adjusted according to the results of the linear search to find the optimal solution for navigation and positioning.

2. The method according to claim 1, characterized in that The actual distance between the UAV and the target position, the sum of the clock error and the noise are regarded as pseudo-range to establish the positioning equation of pseudo-range observation, and the positioning equation is linearized by Taylor series expansion to linearize the UAV observation equation, and the objective function is defined according to the model prediction value and the observation value, and the gradient of the objective function is calculated, and the Hessian matrix is ​​fitted by the Jacobian matrix to determine the gradient descent direction, including: Considering the clock error and various noise measurement errors, the positioning equation established by the pseudorange observation is: Linearize the observation equation and set the user's approximate coordinates to , take the corresponding correction number as ,Will , , Substituting into, performing Taylor series expansion, and ignoring high-order terms, the observation equation of the i-th UAV is linearized as follows: in, , is the approximate distance from the i-th UAV to the user; is the unit vector in the direction of the i-th UAV connection line, expressed as: Label the terms on the right side of the equation as: Observable , The drone clock error is calculated based on the user's approximate location and the drone's location. And various delays According to the relevant model, is the free term of the observation equation; Rewrite the above formula as: To measure the error caused by noise and other unmodeled factors, the objective function is defined as: in, is the model prediction value, is the i-th observation value, which is converted into a vector form: Among them, r is the residual vector: Create the Jacobian matrix: The gradient is expressed as: In the Gauss-Newton algorithm, the Jacobian matrix is ​​used to fit the Hessian matrix: Introduce a diagonal matrix: At this time, the damping factor is introduced into the solution process : 。 3. The method according to claim 2, characterized in that Construct auxiliary functions and find the optimal step size in the determined gradient descent direction, including: in, is the current iteration point, yes The descending direction at , the search step length is .

4. The method according to claim 3, characterized in that Use Wolfe conditions to perform linear searches, including: Introducing the Armijo criterion, assuming yes The descending direction at The search step length satisfies the Armijo criterion, where is a constant that satisfies the curvature criterion: in , the Wolfe condition is to satisfy both the Armijo criterion and the curvature criterion.

5. The method according to claim 4, characterized in that The iterative update includes: using a linear search method to determine the search step length , and then iteratively update the point ;in .

6. The method according to claim 5, characterized in that The convergence test includes: After each iteration, check whether the algorithm has converged; ,when All elements in are less than or equal to The loop ends when .

7. The method according to claim 6, characterized in that According to the results of the linear search, adjust the damping coefficient in the LM algorithm, including: By scaling factor Sure: is an initial damping factor used to control the effect of the Jacobian matrix on the solution update. v It is the adjustment multiple of the damping factor, which is adjusted dynamically during each iteration.

Images