Unmanned aerial vehicle to-ground vehicle fine positioning and dynamic positioning optimization method based on elevation data

Abstract

The invention relates to the field of vehicle positioning, and discloses an unmanned aerial vehicle to-ground vehicle fine positioning and dynamic positioning optimization method based on elevation data. Obtaining a ray analytic expression of a camera sight line vector in an NED coordinate system through Beidou positioning and attitude information of a photoelectric pod of the unmanned aerial vehicle and a pixel position of a target; importing a digital elevation, and carrying out iterative height search in a line-of-sight vector direction by using a dichotomy to obtain target longitude and latitude coarse positioning; and solving the intersection point of the terrain plane and the sight line vector by using a local tangent plane method according to the coarse positioning to obtain fine positioning. The invention further provides a terrain adaptive interactive multi-model filtering TA-IMM algorithm, vehicle motion is described by adopting different motion models, and model state vectors are finally fused and output based on terrain fluctuation characteristics of a target neighborhood, adaptive observation noise and motion model transition probability. According to the method, digital elevation data are fully utilized, and unmanned aerial vehicle-to-ground vehicle fine positioning and dynamic positioning with lightweight hardware and high positioning precision are realized on the whole.

Description

Technical Field

[0001] The present invention relates to the technical field of aerial imaging positioning and dynamic target state estimation, and in particular to a method for optimizing precise positioning and dynamic positioning of a UAV-to-ground vehicle based on elevation data. Background Art

[0002] UAV aerial imaging technology for tracking and locating ground targets has been widely used in applications such as regional security, target rescue, and reconnaissance of specific targets. It also allows for longitude and latitude positioning, as well as target velocity calculation. This allows for trajectory prediction and provides a reference for target locations. This technical area primarily requires considering the longitude and latitude positioning accuracy of static targets from the drone's perspective. Furthermore, it requires considering how to use time series information to optimize the state estimation of dynamic targets in the case of dynamic target movement.

[0003] For static positioning of UAVs against ground vehicles, most current methods use either odometry or highly iterative methods based on digital elevation data. The former requires heavy, expensive optoelectronic pod hardware, while the latter relies on the resolution of digital elevation data, limiting positioning accuracy when the elevation data resolution is low. For dynamic positioning of moving vehicles, conventional long-short temporal memory networks require a large number of training samples and computing power, while conventional Kalman filtering methods are not adaptable to nonlinear systems, and their single motion model is limited in describing complex, compound motion patterns. Summary of the Invention

[0004] 1. Technical problems to be solved: In response to the above technical problems, the present invention provides a method for optimizing the precise positioning and dynamic positioning of UAV-to-ground vehicles based on elevation data. This method makes full use of digital elevation data and generally realizes a method for optimizing the precise positioning and dynamic positioning of UAV-to-ground vehicles with lightweight hardware, high positioning accuracy and high stability.

[0005] 2. Technical solution: The method for optimizing precise positioning and dynamic positioning of a UAV-to-ground vehicle based on elevation data is characterized by comprising: Step 1: Track and photograph a moving vehicle in the air using the BeiDou positioning system, attitude sensor, and aerial camera in the drone's optoelectronic pod. While capturing vehicle images, the camera's latitude, longitude, altitude, and 3D attitude angle are recorded in real time. Step 2: Using the line of sight vector method to obtain the line of sight ray parameter equation of the line connecting the aerial camera and the ground vehicle in real time in the NED coordinate system based on the latitude and longitude, altitude, three-dimensional attitude angle of the aerial camera and the ground vehicle target image; Step 3: Import the drone's digital elevation data and use the bisection method to find the longitude and latitude of the intersection point with the specified height plane in the direction of the sight vector. Then iterate the height search to obtain the coarse positioning result of the vehicle's target longitude and latitude at this moment. Step 4: Using the coarse positioning result as the center, select a set of digital elevation points within a preset radius in the projected coordinate system and perform a least squares fit to obtain the analytical equation of the local tangent plane. This analytical equation is then combined with the analytical expression of the aerial camera's line of sight vector to find the intersection point, thus obtaining the static fine positioning result of the vehicle's target latitude, longitude, and altitude at that moment. Step 5: Construct a terrain-adaptive interactive multi-model filtering TA-IMM dynamic positioning optimization algorithm; the TA-IMM dynamic positioning optimization algorithm is based on the fusion of two parallel motion models constructed by digital elevation data, and the motion state prediction is obtained by using the adaptive IMM algorithm fusion process; the parallel models constructed are a constant acceleration motion model and a constant speed turning motion model; based on the elevation data, the local terrain volume of the vehicle's latitude and longitude neighborhood is obtained, and the adaptive observation noise and adaptive model transition probability are calculated based on the terrain volume. The obtained adaptive observation noise and adaptive model transition probability are used in the terrain-adaptive IMM process to obtain filtered observation data, which are used to correct the prediction value to obtain the final vehicle coordinate current target motion state; Step 6: Input the vehicle's latitude, longitude, and altitude from the static precise positioning results into the two parallel models of the TA-IMM algorithm to predict the vehicle's motion state; perform probability updates and state fusion based on adaptive observation noise and adaptive model transition probabilities; and finally output the optimized vehicle's dynamic positioning latitude, longitude, altitude, and speed.

[0006] Furthermore, step one specifically includes: S11: At each static moment, the longitude λ of the optoelectronic pod in the WGS84 coordinate system is obtained in real time through the Beidou satellite locator and barometer of the Beidou positioning system. u , latitude φ u and altitude h u ; S12: The posture sensor obtains the three-dimensional posture angle of the aerial camera in real time, including the pitch angle θ and the roll angle and heading angle ψ; the aerial camera collects images of ground moving vehicle targets in real time, and the built-in target tracking algorithm outputs the pixel coordinates (u, v) of the vehicle in the image coordinate system in real time.

[0007] Furthermore, step 2 specifically includes: S21: Convert the pixel coordinates (u, v) of the vehicle target in the image coordinate system to the camera coordinate system, and obtain the unit line of sight direction vector v of the line connecting the optical center of the aerial camera and the ground vehicle in the camera coordinate system c； S22: The unit sight direction vector v in the camera coordinate system c Using the attitude angle composite rotation matrix R C2N Convert to NED coordinate system to get the sight unit vector l: where the attitude angle composite rotation matrix R C2N As follows: ; ; ; (1); As above formula R Φ 、R θ 、R ψ Represents the roll angle , the rotation matrix corresponding to the pitch angle θ and the heading angle ψ; then the line of sight unit vector l is obtained by converting it to the NED coordinate system as shown below: (2); In the above formula, l N ,、l E ,、l D They represent the north, east, and ground components of the sight line unit vector in the NED coordinate system respectively; S23: Aerial camera optical center to arbitrary height D A The parametric equation P(s) of the line of sight of the space connecting the ground vehicles in the NED coordinate system is as follows: (3); In the above formula, s is the distance from the optical center of the camera to the height D on the line of sight ray. A The distance between the plane intersection points; N(s), E(s), and D(s) represent the parameters of the north, east, and ground components of the line of sight ray parameter equation respectively; in the above formula, P com Represents the position of the aerial camera, where the corresponding coordinates are (0, 0, 0) T .

[0008] Furthermore, step three specifically includes: S31: First, initialize the minimum value Dmin and the maximum value Dmax of the elevation range around the drone; use the dichotomy method to select the elevation Di = (Dmin + Dmax) / 2, and use the line-of-sight ray parameter equation solution method in step S23 for the plane at elevation Di to obtain the line-of-sight ray parameter equation P(s); S32: According to the method of converting the longitude and latitude increments of the earth's curvature radius, the longitude and latitude (φ) of the intersection of the line of sight ray parameter equation P(s) and the plane with a height of Di are obtained. i ,λi ): Specifically including: First, calculate the north, east, and ground distances N of the intersection of the line of sight ray parameter equation P(s) and the plane at height Di relative to the drone. g 、E g 、D g , as follows: (4); Then the north, east and ground distances are converted into longitude and latitude increments by introducing the radius of curvature of the earth meridian R M (φ u ) and the radius of curvature of the yoke R N (φ u ) is used to characterize the longitude and latitude increments corresponding to the easting and northing distances, thereby obtaining the longitude and latitude increments Δφ and Δλ: (5); (6); In the above formula, a ， is the semi-major axis of the Earth, which is 6378137 meters; e is the eccentricity of the Earth, which is 0.0818; Finally, add the current latitude and longitude of the drone to the corresponding longitude and latitude increment to obtain the longitude and latitude of the intersection point (φ i ,λ i ); S33: Get the actual terrain elevation h i = DEM(φ i , λ i ), calculate the elevation Di and the actual terrain elevation h i Height difference ΔD i = D i -h i ;Preset height difference ΔD i The convergence condition of m, and perform the following update iterations: (7); Iterate the height search until the iteratively calculated height difference ΔD i The iteration stops within 10 meters, and the coarse positioning latitude and longitude of the ground vehicle target based on the elevation data is obtained ( , ).

[0009] Furthermore, step four specifically includes: S41: The vehicle target coarse positioning latitude and longitude obtained in step 3 ( , ) as the center, the following formula is used to obtain M elevation data sampling points within the preset radius R: (8); In the above formula, (N i , E i ) is the vehicle target coarse positioning longitude and latitude ( , ) in the NED coordinates; for the jth sampling point, its coordinates in the NED coordinate system are marked as (N j , E j , D j ), where N j 、E j 、D j They are the northing distance, easting distance, and elevation of the point relative to the origin of the NED coordinate system; S42: Solve the local tangent plane equation formed by the elevation data sample points using the least squares method as follows; ; (9); In the above formula, the design matrix A is a matrix with M rows and 3 columns, and its i-th row consists of the local coordinates of the corresponding sampling point (x i ,y i ) and a constant 1, that is, x i ,y i , 1; elevation vector D dem is an M-dimensional column vector, whose i-th element is the elevation value h of the corresponding sampling point i ; a, b, c are the corresponding parameters of the local tangent plane equation; N, E, D are the variables of the plane equation, corresponding to the north, east and ground variables respectively; S43: Combine the parametric equation of the line of sight ray and the local tangent plane equation as follows to solve the distance parameter s* between the line of sight ray and the plane intersection, as follows: ; The solution is: (10); S44: Using the method of converting the earth curvature radius into longitude and latitude increments as in step S32, the static precise positioning result (φ) of the vehicle target in the undulating terrain at the current time k is obtained. * , λ * , h * ).

[0010] Furthermore, in the TA-IMM dynamic positioning optimization algorithm, the constant acceleration motion model uses a Kalman filter KF to predict the current target motion state; and the constant speed turning motion model uses an extended Kalman filter EKF to predict the current target motion state.

[0011] Furthermore, step six specifically includes: S61: Based on the static precise positioning result (φ * ,λ * , h * ) establish adaptive observation noise; First, calculate the elevation standard deviation in the neighborhood of the static precise positioning result. : (11); In the above formula, h i is the height of the i-th elevation sampling point around the location; is the average elevation around the location; M is the number of sampling points; based on the standard deviation of elevation fluctuation, the terrain adaptive observation noise R is established k As follows: (12); In the above formula, R0 is the preset reference observation noise; I is the unit matrix; μ R is the preset terrain influence coefficient on observation, which ranges from 0.01 to 0.1; S62: Based on the standard deviation of elevation fluctuation , establish the terrain adaptive model transfer probability based on the model interaction of adaptive terrain : (13); In the above formula is the baseline transition probability from model i to model j; It is the preset terrain influence coefficient, which ranges from 0.02 to 0.08m -1 ; m represents the sum index, which traverses all models in the predefined motion model set, namely the constant acceleration motion model and the constant speed turning motion model. The value range of the sum index m in the above formula is {1, 2}; model i, model j represents one of the constant acceleration motion model and the constant speed turning motion model; S63: Before independently predicting the constant acceleration motion model and the constant speed turning motion model, a model interaction and state mixing step is first performed. This step calculates a mixed initial state for the two motion models at the current k moment based on the filtering result at the previous moment (k-1) and the terrain adaptive model transition probability in step S62. The mixed initial state includes the mixing probability , mixed initial state , the initial covariance of the mixture ; S64: During the prediction process, the constant acceleration motion model and the constant speed turning motion model run in parallel, using the Kalman filter method and the extended Kalman filter algorithm for prediction respectively; the state vector X = x, y, v is used in the prediction process. x ,v y , a x , a y, ω T , where x, y represent the longitude and latitude positions; v x , v y Indicates the speed in the longitude and latitude directions; a x , a y is the acceleration in the longitude and latitude directions; ω is the angular velocity; S65: Observe and update the dual model prediction results: the observation quantity is the latitude and longitude of the target vehicle, and the observation function h(X)=(x, y) T ; According to the prior state estimate Predict the predicted observation vectors of the two motion models at time k As follows: (14); In the above formula, and Respectively represent the target latitude and longitude coordinates in the predicted observation vector; The real observation z k and predicted observations The difference between ; Update the state of the motion model as follows and covariance : ; (15); In the above formula, is the identity matrix; represents the prior state covariance matrix; represents the Kalman gain; H is the observation Jacobian matrix; S66: Fusion output; update the motion model likelihood as follows and model probability : ; (16); In the above formula, is the innovation covariance, is the normalization constant of the likelihood; Calculate the fused model state and model covariance : ; (17); In the above formula, Represents the fused state vector; The fused state is the weighted average of the outputs of the two motion models in TA-IMM, and the final output is the dynamically optimized latitude and longitude positioning results and current speed of the vehicle target at time k.

[0012] 3.Beneficial effects: (1) The present invention discloses a method for optimizing the precise positioning and dynamic positioning of UAV ground vehicles based on elevation data. First, the bisection iteration method and the local tangent plane method are combined in the static precise positioning of ground vehicle targets to quickly search the target's elevation while compensating for the limitation of insufficient resolution of elevation data. Secondly, in the optimization method for dynamic positioning, the digital elevation data in the neighborhood of the static precise positioning result is used to design terrain-adaptive observation noise and model transfer probability. These are added to the multi-motion model filter to form a new TA-IMM algorithm. The probability transfer of the motion model in the filter is guided by the terrain characteristics, thereby achieving overall stable and high-precision static and dynamic positioning of UAV ground vehicles.

[0013] (2) The present invention discloses a method for optimizing the precise positioning and dynamic positioning of ground vehicles using UAVs based on elevation data. When a ground vehicle passes through an area with undulating terrain, the method can fully utilize digital elevation data, integrate the binary iterative height search method and the local tangent plane method, and effectively improve the single-time positioning accuracy. At the same time, for dynamic sequences of moving targets, the TA-IMM algorithm based on digital elevation data can achieve terrain adaptation in terms of observation noise and model transition probability, ultimately optimizing the dynamic positioning accuracy of ground moving vehicles and providing a useful reference for the specific location, movement direction, and speed of the target. BRIEF DESCRIPTION OF THE DRAWINGS

[0014] Figure 1 This is a general flow chart of the method for optimizing precise positioning and dynamic positioning of UAV-to-ground vehicles based on elevation data of the present invention; Figure 2 This is a schematic diagram of the hardware device involved in the present invention and the positioning of the UAV optoelectronic pod on a flat ground vehicle in the NED coordinate system; Figure 3 Schematic diagram of the local tangent plane method based on elevation data provided by the present invention; Figure 4 Flowchart of the TA-IMM algorithm provided by the present invention; Figure 5 This is a visualization of the trajectory of the moving target positioning in the verification example of the IMM-EKF algorithm and the TA-IMM algorithm. DETAILED DESCRIPTION

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

[0016] Example: As attached Figure 1 As shown in FIG, the method for optimizing the precise positioning and dynamic positioning of a UAV-to-ground vehicle based on elevation data includes: Step 1: Track and photograph a moving vehicle in the air using the BeiDou positioning system, attitude sensor, and aerial camera in the drone's optoelectronic pod. While capturing vehicle images, the camera's latitude, longitude, altitude, and 3D attitude angle are recorded in real time. As attached Figure 2 As shown in the figure, the hardware devices in the UAV optoelectronic pod include: Beidou positioning system, attitude sensor IMU and aerial camera. The Beidou positioning system includes Beidou satellite locator and barometric altimeter, which can obtain the longitude and latitude (φ) of the optoelectronic pod in the WGS84 coordinate system in real time. u , λ u ) and altitude h u The IMU is used to obtain the three-dimensional attitude angle of the aerial camera in real time, including the pitch angle θ and the roll angle and heading angle ψ; the aerial camera collects images of ground moving vehicles in real time, and the built-in target tracking algorithm can output the pixel coordinates (u, v) of the vehicle in the image in real time.

[0017] As attached Figure 2 As shown in the figure, the aerial camera collects images of the ground moving vehicle in real time. At each frame moment, the drone records the spatial position of the optoelectronic pod at the current moment (φ u , λ u , h u ), optoelectronic pod three-dimensional attitude angle The pixel coordinates (u, v) of the vehicle target center at this moment in the image are transmitted to the central processing unit for data processing. In the figure, Δφ and Δλ represent the longitude and latitude increments, respectively.

[0018] Step 2: For the latitude and longitude, altitude, three-dimensional attitude angle of the aerial camera and the ground vehicle target image, use the line of sight vector method to obtain the line of sight ray parameter equation of the line connecting the aerial camera and the ground vehicle in real time in the NED coordinate system.

[0019] Step 2 specifically includes: S21: Convert the pixel coordinates (u, v) of the vehicle target in the image coordinate system to the camera coordinate system, and obtain the unit line of sight direction vector v of the line connecting the optical center of the aerial camera and the ground vehicle in the camera coordinate system c ; In this step, the intrinsic parameter matrix K of the aerial camera is used to transform the pixel coordinates (u, v) of the vehicle target in the image coordinate system to obtain the coordinates in the camera coordinate system as follows: : ; The unit sight direction vector v of the line connecting the camera optical center and the ground vehicle in the camera coordinate system c As follows: ; In the above formula, v cx 、v cy 、v cz is the unit sight direction vector v c Components in the X, Y, and Z axes; S22: The unit sight direction vector v in the camera coordinate system c Using the attitude angle composite rotation matrix R C2N Convert to NED coordinate system to get the sight unit vector l: where the attitude angle composite rotation matrix R C2N As follows: ; ; ; (1); As above formula R Φ 、R θ 、R ψ Represents the roll angle , the rotation matrix corresponding to the pitch angle θ and the heading angle ψ; then the line of sight unit vector l is obtained by converting it to the NED coordinate system as shown below: (2); In the above formula, l N ,、l E ,、l D They represent the north, east, and ground components of the sight line unit vector in the NED coordinate system respectively; S23: Aerial camera optical center to arbitrary height D A The parametric equation P(s) of the line of sight of the space connecting the ground vehicles in the NED coordinate system is as follows: (3); In the above formula, s is the distance from the optical center of the camera to the height D on the line of sight ray. A The distance between the plane intersection points; N(s), E(s), and D(s) represent the parameters of the north, east, and ground components of the line of sight ray parameter equation respectively; in the above formula, P com Represents the position of the aerial camera, where the corresponding coordinates are (0, 0, 0)T .

[0020] Step 3: Import the drone's digital elevation data, use the bisection method to solve the longitude and latitude of the intersection point with the specified height plane in the direction of the line of sight vector, and iterate the height search to obtain the coarse positioning result of the vehicle's target longitude and latitude at this moment; specifically, it includes: S31: First, initialize the minimum value Dmin and the maximum value Dmax of the elevation range around the drone; use the dichotomy method to select the elevation Di = (Dmin + Dmax) / 2, and use the line-of-sight ray parameter equation solution method in step S23 for the plane at elevation Di to obtain the line-of-sight ray parameter equation P (s); In step S31, the ground constraint conditions are: ; Then solve the parameter s: ; Now you need to do a validity check to confirm that the line of sight is pointing downwards: .

[0021] S32: According to the method of converting the longitude and latitude increments of the earth's curvature radius, the longitude and latitude (φ) of the intersection of the sight vector ray and the plane with a height of Di are obtained. i ,λ i ): Specifically including: First, calculate the north, east, and ground distances N of the intersection of the line of sight ray parameter equation P(s) and the plane at height Di relative to the drone. g 、E g 、D g , as follows: (4); Then the north, east and ground distances are converted into longitude and latitude increments by introducing the radius of curvature of the earth meridian R M (φ u ) and the radius of curvature of the yoke R N (φ u ) is used to characterize the longitude and latitude increments corresponding to the easting and northing distances, thereby obtaining the longitude and latitude increments Δφ and Δλ: (5); (6); In the above formula, a ， is the semi-major axis of the Earth, which is 6378137 meters; e is the eccentricity of the Earth, which is 0.0818; Finally, add the current latitude and longitude of the drone to the corresponding longitude and latitude increment to obtain the longitude and latitude of the intersection point (φ i ,λ i ).

[0022] S33: Get the actual terrain elevation h i = DEM(φi , λ i ), calculate the elevation Di and the actual terrain elevation h i Height difference ΔD i = D i -h i ;Preset height difference ΔD i The convergence condition of m, and perform the following update iterations: (7); Iterate the height search until the iteratively calculated height difference ΔD i The iteration stops within 10 meters, and the coarse positioning latitude and longitude of the ground vehicle target based on the elevation data is obtained ( , ).

[0023] In step S33, in areas with undulating terrain, digital elevation data needs to be imported to guide precise positioning. Digital elevation data can be selected from published official data or reliable data obtained through self-survey. It is usually in raster format, and the elevation of the corresponding location can be queried by longitude and latitude.

[0024] Step 4: Centered on the coarse positioning result, take a set of digital elevation points within a preset radius in the projected coordinate system and perform least squares fitting to obtain the analytical equation of the local tangent plane. Combine the analytical equation of the local tangent plane with the analytical expression of the aerial camera's line of sight vector to find the intersection point, thus obtaining the static precise positioning result of the vehicle's target latitude, longitude, and altitude at that moment.

[0025] As attached Figure 3 As shown in Figure 4, after obtaining the coarse positioning latitude and longitude based on the elevation data, the local tangent plane method is implemented to address the low resolution of the elevation data. Centered on the coarse positioning result, a set of digital elevation points within a radius R is taken in the projected coordinate system and a least squares fit is performed to obtain the analytical equation of the local tangent plane. This is then combined with the line of sight vector equation of the aerial camera to find the intersection point, resulting in the precise positioning results of the target latitude, longitude, and altitude.

[0026] Step 4 specifically includes the following steps: S41: The vehicle target coarse positioning latitude and longitude obtained in step 3 ( , ) as the center, the following formula is used to obtain M elevation data sampling points within the preset radius R: (8); In the above formula, (N i , E i ) is the vehicle target coarse positioning longitude and latitude ( , ) in the NED coordinates; for the jth sample point, its coordinates in the NED coordinate system are marked as (Nj , E j , D j ), where Nj, Ej, and Dj are the northing distance, easting distance, and elevation of the point relative to the origin of the NED coordinate system, respectively. In this embodiment, a total of M elevation data sample points within a radius of R = 50 meters are selected, and the units of Nj, Ej, and Dj are all meters.

[0027] S42: Solve the local tangent plane equation formed by the elevation data sample points using the least squares method as follows; ; (9); In the above formula, the design matrix A is a matrix with M rows and 3 columns, and its i-th row consists of the local coordinates of the corresponding sampling point (x i ,y i ) and a constant 1, that is, x i ,y i , 1; elevation vector D dem is an M-dimensional column vector, whose i-th element is the elevation value h of the corresponding sampling point i ; a, b, c are the corresponding parameters of the local tangent plane equation; N, E, D are the variables of the plane equation, corresponding to the north, east and ground variables respectively; In this embodiment, the design matrix A of the least squares method defined by M sampling points and the observed elevation vector D defined dem As follows: ; .

[0028] S43: Combine the parametric equation of the line of sight ray and the local tangent plane equation as follows to solve the distance parameter s* between the line of sight ray and the intersection of the plane, as follows: ; The solution is: (10).

[0029] S44: Using the method of converting the earth curvature radius into longitude and latitude increments as in S32, the static precise positioning result of the longitude and latitude and altitude of the vehicle target in the undulating terrain at the current time k is obtained (φ * , λ * , h * ).

[0030] Step 5: Construct a terrain-adaptive interactive multi-model filtering TA-IMM dynamic positioning optimization algorithm; the TA-IMM dynamic positioning optimization algorithm is based on the fusion of two parallel motion models constructed by digital elevation data, and the motion state prediction is obtained by using the adaptive IMM algorithm fusion processing; the parallel models constructed are a constant acceleration motion model and a constant speed turning motion model; based on the elevation data, the local terrain quantity of the vehicle's latitude and longitude neighborhood is obtained, and the adaptive observation noise and adaptive model transfer probability are calculated according to the terrain quantity. The obtained adaptive observation noise and adaptive model transfer probability are used in the terrain adaptive IMM process to obtain filtered observation data, and the observation data is used to correct the prediction value to obtain the final vehicle coordinate current target motion state.

[0031] Specifically, the constant acceleration motion model uses a Kalman filter KF to predict the current target motion state; the constant speed turning motion model uses an extended Kalman filter EKF to predict the current target motion state; Specifically, as attached Figure 4 As shown in the figure, the TA-IMM dynamic positioning optimization algorithm takes the static precise positioning results as input, calculates the local terrain quantity based on the elevation data at each observation point of the moving target, and constructs the terrain-aware adaptive observation noise and model transfer equations.

[0032] Step 6: Input the vehicle's latitude, longitude, and altitude from the static precise positioning results into the two parallel models of the TA-IMM algorithm to predict the vehicle's motion state; perform probability updates and state fusion based on adaptive observation noise and adaptive model transition probabilities; and finally output the optimized vehicle's dynamic positioning latitude, longitude, altitude, and speed.

[0033] Step six specifically includes the following steps: S61: If Figure 4 As shown, in this embodiment, the following variables are initialized: : Initial state estimate of model j; : initial noise covariance matrix of model j; : initial probability of model j; : baseline process noise covariance of model j; : baseline observation noise covariance; : Baseline model transition probability matrix.

[0034] Specifically, such as Figure 4 As shown, in this embodiment, at the kth moment, the local cutting plane method has obtained the latitude, longitude and altitude of the static precise positioning of the vehicle, and then imported the digital elevation model to query the elevation value of the neighborhood of this location, and calculate the average value and standard deviation of the regional elevation fluctuation.

[0035] According to the static precise positioning result of the vehicle target at the current k moment (φ * ,λ * , h * ) establish adaptive observation noise; First, calculate the elevation standard deviation in the neighborhood of the static precise positioning result. : (11); In the above formula, h i is the height of the i-th elevation sampling point around the location; is the average elevation around the location; M is the number of sampling points. As follows: ; According to the standard deviation of elevation fluctuation, the terrain adaptive observation noise R is established. k As follows: (12); In the above formula, R0 is the preset reference observation noise; I is the unit matrix; μ R is the preset terrain influence coefficient on observation, which ranges from 0.01 to 0.1; the adaptive observation noise R k The observation noise is adjusted according to the degree of terrain undulation, and the observation noise is increased in areas with large terrain undulations, thereby increasing the uncertainty of the observation part of the TA-IMM algorithm and optimizing the filtering effect.

[0036] S62: Based on the standard deviation of elevation fluctuation , establish the terrain adaptive model transfer probability based on the model interaction of adaptive terrain : (13); In the above formula is the baseline transition probability from model i to model j; It is the preset terrain influence coefficient, which ranges from 0.02 to 0.08m -1 ; m represents the sum index, which traverses all models in the predefined motion model set, namely the constant acceleration motion model and the constant speed turning motion model. The value range of the sum index m in the above formula is {1, 2}; model i and model j represent one of the constant acceleration motion model and the constant speed turning motion model; the terrain-adaptive model transition probability decreases when the standard deviation of the terrain undulation increases, ensuring that the model will not switch frequently under complex terrain.

[0037] S63: Before independently predicting the constant acceleration motion model and the constant speed turning motion model, a model interaction and state mixing step is first performed. This step calculates a mixed initial state for the two motion models at the current k moment based on the filtering result at the previous moment (k-1) and the terrain adaptive model transition probability in step S62. The mixed initial state includes the mixing probability , mixed initial state , the initial covariance of the mixture ; Specifically, for the two motion models of the TA-IMM algorithm, a model interaction function is established that takes into account the terrain adaptive transition probability. The filtering results of each model at the previous moment are mixed according to probability to generate the starting state of model j at the current moment after model switching: ; ; ; in is the mixing probability, is the initial state of the mixture, is the initial covariance of the mixture, and the subscript k-1 in the above formula represents the previous moment, that is, the initial moment; S64: During the prediction process, the constant acceleration motion model and the constant speed turning motion model run in parallel, using the Kalman filter method and the extended Kalman filter algorithm for prediction respectively; the state vector X = x, y, v is used in the prediction process. x ,v y , a x , a y, ω T , where x, y represent the longitude and latitude positions; v x , v y Indicates the speed in the longitude and latitude directions; a x , a y is the acceleration in the longitude and latitude directions; ω is the angular velocity.

[0038] S641: Specifically, the constant acceleration motion model assumes that the target moves with constant acceleration. At this time, the angular velocity ω is set to 0. According to the physical model of constant acceleration motion, the state transition equation is as follows: and the state transition matrix : ; ; In the above formula, T is the sampling period, that is, the time interval from time k-1 to time k; is the initial state of the mixture at the previous moment, i.e., moment k-1; Use the state transition equation to predict the target vehicle state at time k and covariance : ; In the above formula, represents the mixed initial covariance of the target vehicle at time k-1, represents the process noise covariance; S642: Specifically, in this embodiment, the constant speed turning motion model assumes that the target moves at a constant angular velocity. The nonlinear state transfer equation according to the physical model of the constant speed turning motion model is as follows: and the state transition function : ; ; In the above formula, ψ = atan2(v y , v x ) is the velocity direction angle. Due to the nonlinearity of the state transfer function of the constant speed turning motion model, it is necessary to use the Jacobian matrix to linearize it: ; In the above formula, represents the initial state of the mixture at time k-1.

[0039] Use this state transfer matrix to predict the target vehicle state and covariance at time k: .

[0040] S65: Observe and update the dual model prediction results: the observation quantity is the latitude and longitude of the target vehicle, and the observation function h(X)=(x, y) T ; According to the prior state estimate Predict the predicted observation vectors of the two motion models at time k As follows: (14) In the above formula, and Respectively represent the target latitude and longitude coordinates in the predicted observation vector; The true observation z is calculated as follows k and predicted observations The difference is taken as the new information ; ; .

[0041] At this time, the Kalman gain of the updated model : ; Update the state of the motion model as follows and covariance : ; (15); In the above formula, is the identity matrix; represents the prior state covariance matrix; represents the Kalman gain; H is the observation Jacobian matrix; S66: During the fusion output process, the motion model likelihood is updated as follows and model probability : ; (16); In the above formula, is the innovation covariance, is the normalization constant of the likelihood; Calculate the fused model state and model covariance : ; (17); In the above formula, Represents the fused state vector; The fused state is the weighted average of the outputs of the two motion models in TA-IMM, and the final output is the dynamically optimized latitude and longitude positioning results and current speed of the vehicle target at time k.

[0042] In this embodiment, after state fusion, the fused state vector is:

[0043] The first four components are directly output as the dynamically optimized longitude, latitude and velocity components of the target in the longitude and latitude directions at this moment.

[0044] Verification example: This verification example uses the IMM-EKF motion filtering algorithm to conduct simulation experiments and comparisons with this algorithm. The IMM-EKF motion filtering algorithm is an optimization algorithm of the traditional IMM algorithm, which has certain adaptability to nonlinear motion patterns.

[0045] In this simulation verification example, the windows+python3.10 development platform was used for simulation experiments. A mountainous forest area with undulating terrain was selected to design the vehicle's driving route. The terrain standard deviation range was m. Specifically, the target vehicle's starting latitude and longitude are set to (120.108200°E, 31.413767°N). The motion sequence is as follows: (1) Starting from the starting point, move north at 16 m / s for 10 seconds. (2) Decelerate uniformly to 4 m / s within 2 seconds. (3) Move at 4 m / s for 4 seconds. (4) Turn eastward 90° at a constant angular velocity (linear velocity 4 m / s). (5) Immediately after turning east, accelerate uniformly to 10 m / s within 2 seconds. (6) Move at 10 m / s for 20 seconds. (7) Turn southward 90° at a constant angular velocity (linear velocity 10 m / s). (8) Move southward at 10 m / s for 15 seconds.

[0046] In this simulation, the drone follows a target vehicle from behind at an absolute altitude of 200 meters. The initial gimbal pitch angle is -35°, and the tracking distance is approximately 150 meters, always keeping the target within the image field of view. The camera resolution is 1980×1080, the data acquisition frame rate is 10 times per second, and the DEM data resolution is 12.5 meters. A Monte Carlo method is used to add random errors, and the errors follow a normal distribution. The initial model probabilities of the interacting multi-model are 0.6 and 0.4, the transition probability matrix is ​​0.90, 0.10, 0.10, 0.90, and μ R Set to 0.06, Set to 0.06m -1 .

[0047] In this simulation verification example, this algorithm and the IMM-EKF motion filtering algorithm are used to simulate the longitude and latitude positioning of the moving target under simulation conditions, and their motion positioning accuracy is compared. The comparison results are shown in Table 1.

[0048] Table 1. Positioning results of two motion filtering methods

[0049] Figure 5 This is a visualization trajectory diagram of the moving target positioning in this simulation example using the method of the present invention and the IMM-EKF algorithm.

[0050] In this simulation verification example, the results table shows that this method significantly outperforms the IMM-EKF motion filtering algorithm in terms of mean error, standard error, maximum error, and RMS error. Furthermore, the visualized trajectory graph shows that the positioning trajectory of the method of the present invention is closer to the true trajectory and has higher positioning accuracy. In summary, the method proposed by the present invention outperforms the general methods used in the comparison.

[0051] Although the present invention has been disclosed above with reference to preferred embodiments, these are not intended to limit the present invention. Any person skilled in the art will readily be able to make various changes or modifications without departing from the spirit and scope of the present invention. Therefore, the scope of protection of the present invention shall be determined by the scope of protection of the claims of this application.

Claims

1. A method for optimizing precise positioning and dynamic positioning of UAV-to-ground vehicles based on elevation data, characterized by: include: Step 1: Track and photograph a moving vehicle in the air using the BeiDou positioning system, attitude sensor, and aerial camera in the drone's optoelectronic pod. While capturing vehicle images, the camera's latitude, longitude, altitude, and 3D attitude angle are recorded in real time. Step 2: Using the line of sight vector method to obtain the line of sight ray parameter equation of the line connecting the aerial camera and the ground vehicle in real time in the NED coordinate system based on the latitude and longitude, altitude, three-dimensional attitude angle of the aerial camera and the ground vehicle target image; Step 3: Import the drone's digital elevation data and use the bisection method to find the longitude and latitude of the intersection point with the specified height plane in the direction of the sight vector. Then iterate the height search to obtain the coarse positioning result of the vehicle's target longitude and latitude at this moment. Step 4: Using the coarse positioning result as the center, select a set of digital elevation points within a preset radius in the projected coordinate system and perform a least squares fit to obtain the analytical equation of the local tangent plane. This analytical equation is then combined with the analytical expression of the aerial camera's line of sight vector to find the intersection point, thus obtaining the static fine positioning result of the vehicle's target latitude, longitude, and altitude at that moment. Step 5: Construct a terrain-adaptive interactive multi-model filtering TA-IMM dynamic positioning optimization algorithm; the TA-IMM dynamic positioning optimization algorithm is based on the fusion of two parallel motion models constructed by digital elevation data, and the motion state prediction is obtained by using the adaptive IMM algorithm fusion process; the parallel models constructed are a constant acceleration motion model and a constant speed turning motion model; based on the elevation data, the local terrain volume of the vehicle's latitude and longitude neighborhood is obtained, and the adaptive observation noise and adaptive model transition probability are calculated based on the terrain volume. The obtained adaptive observation noise and adaptive model transition probability are used in the terrain-adaptive IMM process to obtain filtered observation data, which are used to correct the prediction value to obtain the final vehicle coordinate current target motion state; Step 6: Input the vehicle latitude, longitude and altitude from the static precise positioning results into the two parallel models of the TA-IMM algorithm to predict the vehicle's motion state. Probability update and state fusion are performed based on adaptive observation noise and adaptive model transition probability; finally, the optimized vehicle dynamic positioning latitude, longitude, altitude, and speed are output.

2. The method for optimizing precise positioning and dynamic positioning of a UAV-to-ground vehicle based on elevation data according to claim 1, characterized in that: Step 1 specifically includes: S11: At each static moment, the longitude λ of the optoelectronic pod in the WGS84 coordinate system is obtained in real time through the Beidou satellite locator and barometer of the Beidou positioning system. u , latitude φ u and altitude h u ; S12: The posture sensor obtains the three-dimensional posture angle of the aerial camera in real time, including the pitch angle θ and the roll angle and heading angle ψ; the aerial camera collects images of ground moving vehicle targets in real time, and the built-in target tracking algorithm outputs the pixel coordinates (u, v) of the vehicle in the image coordinate system in real time.

3. The method for optimizing precise positioning and dynamic positioning of a UAV-to-ground vehicle based on elevation data according to claim 2, characterized in that: Step 2 specifically includes: S21: Convert the pixel coordinates (u, v) of the vehicle target in the image coordinate system to the camera coordinate system, and obtain the unit line of sight direction vector v of the line connecting the optical center of the aerial camera and the ground vehicle in the camera coordinate system c ; S22: The unit sight direction vector v in the camera coordinate system c Using the attitude angle composite rotation matrix R C2N Convert to NED coordinate system to get the sight unit vector :The attitude angle composite rotation matrix R C2N As follows: ； ； ； （1）； As above formula R Φ 、R θ 、R ψ Represents the roll angle , the rotation matrix corresponding to the pitch angle θ and the heading angle ψ; then the line of sight unit vector l is obtained by converting it to the NED coordinate system as shown below: （2）； In the above formula, l N 、l E 、l D They represent the north, east, and ground components of the sight line unit vector in the NED coordinate system respectively; S23: Aerial camera optical center to arbitrary height D A The parametric equation P(s) of the line of sight of the space connecting the ground vehicles in the NED coordinate system is as follows: （3）； In the above formula, s is the distance from the optical center of the camera to the height D on the line of sight ray. A The distance between the plane intersection points; N(s), E(s), and D(s) represent the parameters of the north, east, and ground components of the line of sight ray parameter equation respectively; in the above formula, P cam Represents the position of the aerial camera, where the corresponding coordinates are (0, 0, 0) T .

4. The method for optimizing precise positioning and dynamic positioning of a UAV-to-ground vehicle based on elevation data according to claim 3 is characterized by: Step three specifically includes: S31: First initialize the minimum elevation range D of the area around the drone min , maximum value D max ; Use dichotomy to select elevation D i =(D min +D max ) / 2, use the line-of-sight ray parameter equation solving method in step S23 for the plane at elevation Di to obtain the line-of-sight ray parameter equation P(s); S32: According to the method of converting the longitude and latitude increments of the earth's curvature radius, the longitude and latitude (φ) of the intersection of the line of sight ray parameter equation P (s) and the plane with a height of Di are obtained. i ,λ i ): Specifically including: First, calculate the north, east, and ground distances N of the intersection of the line of sight ray parameter equation P(s) and the plane at height Di relative to the drone. g 、E g 、D g , as follows: （4）； Then the north, east and ground distances are converted into longitude and latitude increments by introducing the radius of curvature of the earth meridian R M (φ u ) and the radius of curvature of the yoke R N (φ u ) is used to characterize the longitude and latitude increments corresponding to the easting and northing distances, thereby obtaining the longitude and latitude increments Δφ and Δλ: （5）； （6）； In the above formula, a ， is the semi-major axis of the Earth, which is 6378137 meters; e is the eccentricity of the Earth, which is 0.0818; Finally, add the current latitude and longitude of the drone to the corresponding longitude and latitude increment to obtain the longitude and latitude of the intersection point (φ i ,λ i ); S33: Get the actual terrain elevation h i = DEM(φ i , λ i ), calculate the elevation Di and the actual terrain elevation h i Height difference ΔD i =D i -h i ;Preset height difference ΔD i The convergence condition of m, and perform the following update iterations: （7）； Iterate the height search until the iteratively calculated height difference ΔD i The iteration stops within 10 meters, and the coarse positioning latitude and longitude of the ground vehicle target based on the elevation data is obtained ( , ).

5. The method for optimizing precise positioning and dynamic positioning of a UAV-to-ground vehicle based on elevation data according to claim 4 is characterized in that: Step 4 specifically includes: S41: The vehicle target coarse positioning latitude and longitude obtained in step 3 ( , ) as the center, the following formula is used to obtain M elevation data sampling points within the preset radius R: （8）； In the above formula, (N i , E i ) is the vehicle target coarse positioning longitude and latitude ( , ) in the NED coordinates; for the jth sampling point, its coordinates in the NED coordinate system are marked as (N j , E j , D j ), where N j 、E j 、D j They are the northing distance, easting distance, and elevation of the point relative to the origin of the NED coordinate system; S42: Solve the local tangent plane equation formed by the elevation data sample points using the least squares method as follows; ； （9）； In the above formula, the design matrix A is a matrix with M rows and 3 columns, and its i-th row consists of the local coordinates of the corresponding sampling point (x i ,y i ) and a constant 1, that is, [x i ,y i , 1]; elevation vector D dem is an M-dimensional column vector, whose i-th element is the elevation value h of the corresponding sampling point i ; a, b, c are the corresponding parameters of the local tangent plane equation; N, E, D are the variables of the plane equation, corresponding to the north, east and ground variables respectively; S43: Combine the parametric equation of the line of sight ray and the local tangent plane equation as follows to solve the distance parameter s* between the line of sight ray and the plane intersection, as follows: ； The solution is: （10）； S44: Using the method of converting the earth curvature radius into longitude and latitude increments as in step S32, the static precise positioning result (φ) of the vehicle target in the undulating terrain at the current time k is obtained. * , λ * , h * ).

6. The method for optimizing precise positioning and dynamic positioning of a UAV-to-ground vehicle based on elevation data according to claim 5, characterized in that: In the TA-IMM dynamic positioning optimization algorithm, the constant acceleration motion model uses the Kalman filter KF to predict the current target motion state; the constant speed turning motion model uses the extended Kalman filter EKF to predict the current target motion state.

7. The method for optimizing precise positioning and dynamic positioning of a UAV-to-ground vehicle based on elevation data according to claim 6, characterized in that: Step 6 specifically includes: S61: Based on the static precise positioning result (φ * ,λ * , h * ) as the observation position input of TA-IMM and establish the adaptive observation noise; First, calculate the elevation standard deviation in the neighborhood of the static precise positioning result. : （11）； In the above formula, h i is the height of the i-th elevation sampling point around the location; is the average elevation around the location; M is the number of sampling points; based on the standard deviation of elevation fluctuation, the terrain adaptive observation noise R is established k As follows: （12）； In the above formula, R0 is the preset reference observation noise; I is the unit matrix; μ R is the preset terrain influence coefficient on observation, which ranges from 0.01 to 0.1; S62: Based on the standard deviation of elevation fluctuation , establish the terrain adaptive model transfer probability based on the model interaction of adaptive terrain : （13）； In the above formula is the baseline transition probability from model i to model j; It is the preset terrain influence coefficient, which ranges from 0.02 to 0.08m -1 ; m represents the sum index, which traverses all models in the predefined motion model set, namely the constant acceleration motion model and the constant speed turning motion model. The value range of the sum index m in the above formula is {1, 2}; model i, model j represents one of the constant acceleration motion model and the constant speed turning motion model; S63: Before independently predicting the constant acceleration motion model and the constant speed turning motion model, a model interaction and state mixing step is first performed. This step calculates a mixed initial state for the two motion models at the current k moment based on the filtering result at the previous moment (k-1) and the terrain adaptive model transition probability in step S62. The mixed initial state includes the mixing probability , mixed initial state , the initial covariance of the mixture ; S64: During the prediction process, the constant acceleration motion model and the constant speed turning motion model run in parallel, using the Kalman filter method and the extended Kalman filter algorithm for prediction respectively; in the prediction process, the state vector X = [x, y, v x , v y ,a x , a y, ω] T , where x, y represent the latitude and longitude positions; v x , v y Indicates the speed in the longitude and latitude directions; a x , a y is the acceleration in the longitude and latitude directions; ω is the angular velocity; S65: Observe and update the dual model prediction results: the observation quantity is the latitude and longitude of the target vehicle, and the observation function h(X)=(x, y) T ; According to the prior state estimate Predict the predicted observation vectors of the two motion models at time k As follows: （14）； In the above formula, and Respectively represent the target latitude and longitude coordinates in the predicted observation vector; The real observation z k and predicted observations The difference between ; Update the state of the motion model as follows and covariance : ； （15）； In the above formula, is the identity matrix; represents the prior state covariance matrix; represents the Kalman gain; H is the observation Jacobian matrix; S66: Fusion output; update the motion model likelihood as follows and model probability : ； （16）； In the above formula, is the innovation covariance, is the normalization constant of the likelihood; Calculate the model state after fusion and model covariance : ； （17）； In the above formula, Represents the fused state vector; The fused state is the weighted average of the outputs of the two motion models in TA-IMM, and the final output is the dynamically optimized latitude and longitude positioning results and current speed of the vehicle target at time k.

Images