Unmanned aerial vehicle precision positioning and dynamic positioning optimization method based on elevation data

Abstract

The present application relates to the field of vehicle positioning, and discloses a method for precise positioning and dynamic positioning optimization of unmanned aerial vehicle to ground vehicle based on elevation data; the ray analytical expression of the camera line-of-sight vector in the NED coordinate system is obtained through the Beidou positioning and attitude information of the unmanned aerial vehicle photoelectric pod and the pixel position of the target; the digital elevation is introduced to search for the target latitude and longitude coarse positioning in the line-of-sight vector direction by using the dichotomy iterative height search; the intersection point of the terrain plane and the line-of-sight vector is obtained by using the local tangent plane method to solve the precise positioning according to the coarse positioning. The present application also proposes a terrain adaptive interactive multiple model filtering TA-IMM algorithm, which uses different motion models to describe the vehicle motion, and adaptively observes the noise and motion model transition probability based on the terrain undulation characteristics of the target neighborhood, and finally fuses and outputs the model state vector. The present application fully utilizes the digital elevation data, and overall realizes the precise positioning and dynamic positioning of unmanned aerial vehicle to ground vehicle with light hardware and high positioning accuracy.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of aerial imaging positioning and dynamic target state estimation, and particularly relates to a UAV ground vehicle fine positioning and dynamic positioning optimization method based on elevation data. BACKGROUND

[0002] The tracking and positioning technology of UAV aerial imaging on ground targets has been widely applied, such as regional security, target rescue, investigation of specific targets, latitude and longitude positioning and target moving speed calculation, further trajectory prediction of the target, and provision of point reference of the target. In this technical field, firstly, the latitude and longitude positioning accuracy of static targets under the visual angle of UAV is considered, and then how to use time series information to optimize the estimation of the state of dynamic targets under the dynamic condition of target movement is considered.

[0003] For the static positioning method of UAV on ground vehicles, most of the current methods use ranging method or height iteration method based on digital elevation data, the former needs heavy and high-cost photoelectric pod hardware, and the latter depends on the resolution of digital elevation data, and its positioning accuracy is limited in the case of low resolution of elevation data. For the dynamic positioning method of moving vehicles, the conventional long-short time sequence memory network needs a large number of training samples and computing power, and the general Kalman filter method cannot adapt to nonlinear systems, and its single motion model has limitations for describing complex compound motion patterns. SUMMARY

[0004] 1. Technical problems to be solved:

[0005] In view of the above technical problems, the present application provides a UAV ground vehicle fine positioning and dynamic positioning optimization method based on elevation data, which fully utilizes digital elevation data and realizes the UAV ground vehicle fine positioning and dynamic positioning optimization method with light weight hardware, high positioning accuracy and high stability.

[0006] 2. Technical solutions:

[0007] The UAV ground vehicle fine positioning and dynamic positioning optimization method based on elevation data, characterized in that it comprises:

[0008] Step one: through the Beidou positioning system, pose sensor and aerial camera in the photoelectric pod of the UAV, tracking and aerial photography of the ground moving vehicle target in the air; acquiring the vehicle image while recording the latitude, longitude, height and three-dimensional attitude angle of the aerial camera in real time;

[0009] Step two: using the line-of-sight vector method, the line-of-sight ray parameter equation of the connecting line between the aerial camera and the ground vehicle target is obtained in real time in the NED coordinate system based on the latitude, longitude, height, three-dimensional attitude angle of the aerial camera and the ground vehicle target image;

[0010] Step three: import the digital elevation data of the UAV, use the bisection method to solve the intersection of 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 target longitude and latitude at this moment;

[0011] Step four: taking the coarse positioning result as the center, a set of digital elevation points with a preset radius in the projection coordinate system is taken, and the least squares fitting is performed to obtain the analytical equation of the local tangent plane; the analytical equation of the local tangent plane is solved with the line-of-sight vector analytical expression of the aerial camera to obtain the static precise positioning result of the vehicle target longitude, latitude and height at this moment;

[0012] Step five: constructing a terrain adaptive interactive multiple model filter TA-IMM dynamic positioning optimization algorithm; the TA-IMM dynamic positioning optimization algorithm is based on the digital elevation data to fuse the two parallel motion models constructed, and the adaptive IMM algorithm is used for fusion processing to obtain the motion state prediction; wherein the parallel models are constructed as constant acceleration motion model and constant speed turning motion model; based on the elevation data, the local terrain quantity of the vehicle longitude and latitude neighborhood is obtained, the adaptive observation noise and adaptive model transition probability are calculated according to the terrain quantity, the adaptive observation noise and adaptive model transition probability obtained are used in the terrain adaptive IMM process to obtain the filtered observation data, the observation data are used to correct the predicted value to obtain the final vehicle coordinate current target motion state;

[0013] Step six: input the vehicle longitude, latitude and height in the static precise positioning result into the two parallel models of the TA-IMM algorithm for vehicle motion state prediction; based on the adaptive observation noise and adaptive model transition probability, the probability is updated and the state is fused; finally, the optimized vehicle dynamic positioning longitude, latitude, height and speed are output.

[0014] Further, step one specifically includes:

[0015] S11: at each static moment, the longitude λ u , the latitude φ u and the altitude h u of the photoelectric pod in the WGS84 coordinate system are obtained in real time by the Beidou satellite locator of the Beidou positioning system and the barometer;

[0016] S12: the three-dimensional attitude angle of the aerial camera is obtained in real time by the pose sensor, including the pitch angle θ, the roll angle and the heading angle ψ; the aerial camera performs image acquisition on the ground moving vehicle target 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.

[0017] Further, step two specifically includes:

[0018] 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 sight direction vector v of the connecting line between the aerial camera optical center and the ground vehicle in the camera coordinate system c ；

[0019] S22: convert the unit sight direction vector v in the camera coordinate system to the NED coordinate system to obtain the sight unit vector l: wherein the attitude angle composite rotation matrix R c The attitude angle composite rotation matrix R C2N is converted to the NED coordinate system to obtain the sight unit vector l: wherein the attitude angle composite rotation matrix R C2N As follows:

[0020] ;

[0021] ;

[0022] ;

[0023] (1);

[0024] As follows: Φ , R θ , R ψ respectively represent the rotation matrix corresponding to the roll angle , the pitch angle θ, and the heading angle ψ; then the sight unit vector l converted to the NED coordinate system is as follows:

[0025] (2);

[0026] In the above formula, l N , l E , and l D represent the north, east, and ground components of the sight unit vector in the NED coordinate system, respectively;

[0027] S23: the parameter equation P(s) of the sight ray of the connecting line between the aerial camera optical center and the ground vehicle at any height D A in the NED coordinate system is as follows:

[0028] (3);

[0029] In the above formula, s is the distance from the camera optical center to the intersection point with the plane of height D A on the sight ray; N(s), E(s), and D(s) represent the parameters of the north, east, and ground components of the sight ray parameter equation, respectively; in the above formula, P com represents the position of the aerial camera, and the corresponding coordinates are (0, 0, 0) T .

[0030] Furthermore, step three specifically includes:

[0031] S31: First, initialize the minimum elevation range Dmin and maximum elevation range Dmax of the area around the UAV; use the bisection method to select the elevation Di = (Dmin + Dmax) / 2, and use the line-of-sight ray parameter equation solution method in step S23 to solve the line-of-sight ray parameter equation P(s) on the plane of elevation Di.

[0032] S32: Based on the method of converting latitude and longitude increments using the Earth's radius of curvature, the latitude and longitude (φ) of the intersection point of the line-of-sight ray parameter equation P(s) and the plane at height Di are obtained. i , λ i Specifically, this includes:

[0033] First, calculate the distances N between the intersection point of the line-of-sight ray parametric equation P(s) and the plane at height Di, relative to the north, east, and ground direction of the UAV. g E g D g As shown in the following formula:

[0034] (4);

[0035] Then, the north, east, and ground distances are converted into latitude and longitude increments, at which point the radius of curvature R of the Earth's meridian is introduced. M (φ u ) and the radius of curvature R of the zonal loop N (φ u The latitude and longitude increments are used to characterize the eastward and northward distance increases, thus obtaining the latitude and longitude increments Δφ and Δλ:

[0036] (5);

[0037] (6);

[0038] In the above formula, a ， denoted as the Earth's semi-major axis, taken as 6,378,137 meters; e is the Earth's eccentricity, taken as 0.0818.

[0039] Finally, add the corresponding latitude and longitude increment to the current latitude and longitude of the drone to obtain the intersection latitude and longitude (φ). i , λ i );

[0040] S33: Obtain the actual terrain elevation h i = DEM(φ i , λ i ), calculate elevation Di and actual terrain elevation h i Height difference ΔD i = D i - hi ; preset height difference ΔD i convergence condition m, the following formula is updated iteration:

[0041] (7);

[0042] iterative height search until the iterative calculation of height difference ΔD i stop iteration within 10 meters, obtain the ground vehicle target based on elevation data coarse positioning latitude and longitude ( , ).

[0043] Further, step four specifically includes:

[0044] S41: with the vehicle target coarse positioning latitude and longitude ( , ) obtained in step three as the center, take the elevation data sampling points M in the preset radius R as follows:

[0045] (8);

[0046] In the above formula, (N i , E i ) is the corresponding coordinate of the vehicle target coarse positioning latitude and longitude ( , ) in the NED coordinate system; for the jth sampling point, its coordinate in the NED coordinate system is denoted as (N j , E j , D j ), where N j , E j , D j are the north distance, east distance and elevation value of the point relative to the origin of the NED coordinate system respectively;

[0047] S42: the local tangent plane equation formed by the elevation data sample points is solved and calculated by the least square method as follows:

[0048] ;

[0049] (9);

[0050] In the above formula, the design matrix A is an M row 3 column matrix, the ith row of which is composed of the local coordinates (x i , y i ) of the corresponding sampling point and the constant 1, that is, x i , y i , 1; the elevation vector D dem is an M-dimensional column vector, and the ith 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;

[0051] S43: the sight line ray parameter equation and the local tangent plane equation are solved simultaneously to obtain the distance parameter s* of the intersection point of the sight line ray and the plane, as follows:

[0052] ;

[0053] Solving:

[0054] (10);

[0055] S44: the static precise positioning results of the vehicle target in the undulating terrain at the current k time (φ * , λ * , h * ) are obtained by using the earth curvature radius conversion latitude and longitude increment method as in step S32.

[0056] Further, 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.

[0057] Further, step six specifically includes:

[0058] S61: establishing an adaptive observation noise according to the static precise positioning results (φ * , λ * , h * ) of the vehicle target at the current k time;

[0059] First, the standard deviation of the elevation fluctuation is calculated in the neighborhood of the position of the static precise positioning results :

[0060] (11);

[0061] In the above formula, h i is the height of the i-th elevation sampling point around the position; is the average elevation around the position; M is the number of sampling points; according to the standard deviation of the elevation fluctuation, the terrain adaptive observation noise R k is established as follows:

[0062] (12);

[0063] In the above formula, R0 is a preset reference observation noise; I is an identity matrix; μ RThe preset terrain influence coefficient is 0.01-0.1;

[0064] S62: According to the standard deviation of the height fluctuation , the terrain adaptive model transition probability based on adaptive terrain is established :

[0065] (13);

[0066] In the formula is the baseline transition probability from model i to model j; The preset terrain influence coefficient is 0.02-0.08 m -1 ; m represents the summation index, which traverses all models in the predefined motion model set, i.e., the constant acceleration motion model and the constant speed turning motion model, and in the formula, the value range of the summation index m is {1, 2}; model i and model j represent one of the constant acceleration motion model and the constant speed turning motion model;

[0067] S63: Before predicting the constant acceleration motion model and the constant speed turning motion model independently, the model interaction and state mixing step is first performed; based on the filtering result at the previous time, i.e., the (k-1) time, and the terrain adaptive model transition probability in step S62, a mixed initial state for the two motion models at the current k time is calculated; the mixed initial state includes a mixed probability , a mixed initial state , and a mixed initial covariance ;

[0068] S64: In the prediction process, the constant acceleration motion model and the constant speed turning motion model run in parallel, and the Kalman filter method and the extended Kalman filter algorithm are used for prediction, respectively; in the prediction process, the state vector X = x, y, v x , v y , a x , a y, ω T , wherein x and y represent the latitude and longitude position; v x , v y represent the latitude and longitude direction speed; a x , a y are the latitude and longitude direction acceleration; and ω is the angular velocity;

[0069] S65: The observation and update of the double model prediction result: the observation is the latitude and longitude of the target vehicle, and the observation function h(X) = (x, y) T ; according to the prior state estimation , the predicted observation vector of the two motion models at the k time is estimated As follows:

[0070] (14);

[0071] In the above formula, and respectively represent the target longitude and latitude coordinates in the predicted observation vector;

[0072] The difference between the real observation z k and the predicted observation is taken as the innovation ; the state and the covariance of the motion model are updated as follows:

[0073] ;

[0074] (15);

[0075] In the above formula, is the unit matrix; represents the prior state covariance matrix; represents the Kalman gain; H is the observation Jacobian matrix;

[0076] S66: fusion output; the motion model likelihood and the model probability are updated as follows:

[0077] ;

[0078] (16);

[0079] In the above formula, is the innovation covariance, is the normalization constant of the likelihood;

[0080] The fused model state and the model covariance are calculated as follows:

[0081] ;

[0082] (17);

[0083] In the above formula, represents the fused state vector;

[0084] The fused state is a weighted average of the outputs of the two motion models in the TA-IMM, and the longitude and latitude positioning results of the dynamically optimized vehicle target at time k and the current speed are obtained as the final output.

[0085] 3. Beneficial effects:

[0086] (1) The application discloses a kind of based on elevation data unmanned plane to ground vehicle precision positioning and dynamic positioning optimization method, first in the static precision positioning of ground vehicle target, in combination with dichotomy iterative method and local tangent plane method, the elevation of target is quickly searched while the limitation of insufficient elevation data resolution is made up.Secondly in the optimization method of dynamic positioning, using the digital elevation data in the neighborhood of static precision positioning result, terrain adaptive observation noise and model transition probability are designed, are added in multi-motion model filter and constitute new TA-IMM algorithm, according to the probability transition of motion model in filter guided by terrain characteristics, overall realization stable, high-precision unmanned plane to ground vehicle static and dynamic positioning.

[0087] (2) The application discloses a kind of based on elevation data unmanned plane to ground vehicle precision positioning and dynamic positioning optimization method, when ground vehicle passes through undulating terrain area, digital elevation data can be fully utilized, dichotomy iterative method height search and local tangent plane method are fused, single time positioning precision is effectively improved.Meanwhile, for the dynamic sequence of moving target, using TA-IMM algorithm based on digital elevation data can make it realize terrain adaptation in observation noise and model transition probability, finally the dynamic positioning precision of ground moving vehicle is optimized, and beneficial reference is provided for the specific position, moving direction and speed of target. BRIEF DESCRIPTION OF DRAWINGS

[0088] Figure 1 It is the overall flow chart of the unmanned plane to ground vehicle precision positioning and dynamic positioning optimization method based on elevation data of the application;

[0089] Figure 2 It is the hardware device and the unmanned plane photoelectric pod positioning flat ground vehicle under NED coordinate system of the application;

[0090] Figure 3 It is the local tangent plane method based on elevation data provided by the application;

[0091] Figure 4 It is the TA-IMM algorithm flow chart provided by the application;

[0092] Figure 5 It is the visualization trajectory graph of the motion target positioning in verification example of IMM-EKF algorithm and TA-IMM algorithm. DETAILED DESCRIPTION

[0093] The application will be specifically explained in combination with the drawings and embodiments.

[0094] Embodiment:

[0095] As attached Figure 1As shown, the elevation data-based unmanned aerial vehicle-to-ground vehicle fine positioning and dynamic positioning optimization method comprises:

[0096] Step one: through the Beidou positioning system, pose sensor and aerial camera in the unmanned aerial vehicle photoelectric pod, track and aerial photograph the ground moving vehicle target in the air; obtain the vehicle image while recording the latitude, longitude and height of the aerial camera and the three-dimensional attitude angle in real time;

[0097] As shown in the accompanying Figure 2 , the hardware devices in the unmanned aerial vehicle photoelectric pod include: Beidou positioning system, pose sensor IMU and aerial camera. The Beidou positioning system includes Beidou satellite positioner and barometric altimeter, which can obtain the latitude (φ u , λ u ) and altitude h u of the photoelectric pod in the WGS84 coordinate system in real time; the pose sensor IMU is used to obtain the three-dimensional attitude angle of the aerial camera in real time, including pitch angle θ, roll angle and heading angle ψ; the aerial camera collects images of the ground moving vehicle 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.

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

[0099] Step two: using the line-of-sight vector method, the latitude, longitude, height and three-dimensional attitude angle of the aerial camera and the vehicle target image are used to obtain the line-of-sight ray parameter equation of the line connecting the aerial camera and the ground vehicle in the NED coordinate system in real time.

[0100] Step two specifically includes:

[0101] 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 c of the line connecting the optical center of the aerial camera and the ground vehicle in the camera coordinate system;

[0102] In this step, the pixel coordinates (u, v) of the vehicle target in the image coordinate system are converted to the camera coordinate system coordinates using the intrinsic matrix K of the aerial camera as follows:

[0103] ;

[0104] The camera optical center and the ground vehicle are connected in the unit line-of-sight direction vector v in the camera coordinate system c As follows:

[0105] ;

[0106] In the above formula, v cx , v cy , and v cz are the unit line-of-sight direction vectors v c in the X, Y, and Z axes;

[0107] S22: The unit line-of-sight direction vector v c in the camera coordinate system is converted to the NED coordinate system by using the attitude angle composite rotation matrix R C2N to obtain the line-of-sight unit vector l: wherein the attitude angle composite rotation matrix R C2N As follows:

[0108] ;

[0109] ;

[0110] ;

[0111] (1);

[0112] As shown in the above formula R Φ , R θ , and R ψ respectively represent the rotation matrices corresponding to the roll angle , the pitch angle θ, and the heading angle ψ; then the line-of-sight unit vector l converted to the NED coordinate system is as shown in the following formula:

[0113] (2);

[0114] In the above formula, l N , l E , and l D respectively represent the northward, eastward, and downward components of the line-of-sight unit vector in the NED coordinate system;

[0115] S23: The line-of-sight ray of the space connecting the aerial camera optical center to the ground vehicle at an arbitrary height D A has the parametric equation P(s) in the NED coordinate system as follows:

[0116] (3);

[0117] In the formula, s is the distance from the camera optical center on the line-of-sight ray to the intersection point with the plane of height D A ; N(s), E(s), D(s) represent the parameters of the north, east, and ground components of the line-of-sight ray parameter equation, respectively; in the formula, P com represents the position of the aerial camera, and the corresponding coordinates are (0, 0, 0) T .

[0118] Step three: import the digital elevation data of the unmanned aerial vehicle, use the bisection method to solve the intersection latitude and longitude of the line-of-sight vector in the pointing direction with the specified height plane, and iteratively search the height to obtain the coarse positioning result of the vehicle target latitude and longitude at this moment; specifically including:

[0119] S31: first initialize the minimum value Dmin and the maximum value Dmax of the elevation range of the area around the unmanned aerial vehicle; use the bisection method to select the elevation Di = (Dmin + Dmax) / 2, and use the line-of-sight ray parameter equation solving method in step S23 to solve the elevation Di plane to obtain the line-of-sight ray parameter equation P(s);

[0120] In step S31, the ground constraint condition is: ; and then solve the parameter s: ; at this time, the effectiveness check is needed to confirm that the line-of-sight direction is downward: .

[0121] S32: according to the conversion of latitude and longitude increment method based on the earth curvature radius, the intersection latitude and longitude (φ i , λ i ) of the line-of-sight vector ray and the height Di plane is obtained; specifically including:

[0122] First, calculate the intersection point of the line-of-sight ray parameter equation P(s) and the height Di plane relative to the north, east, and ground directions N g , E g , D g , as follows:

[0123] (4);

[0124] Then convert the north, east, and ground distances into latitude and longitude increments, at this time, by introducing the earth meridian curvature radius R M (φ u ) and the prime vertical curvature radius R N (φ u ) to represent the latitude and longitude increments corresponding to the east and north distance increments, so as to obtain the latitude and longitude increments Δφ and Δλ:

[0125] (5);

[0126] (6);

[0127] In the above formula, a ， denoted as the Earth's semi-major axis, taken as 6,378,137 meters; e is the Earth's eccentricity, taken as 0.0818.

[0128] Finally, add the corresponding latitude and longitude increment to the current latitude and longitude of the drone to obtain the intersection latitude and longitude (φ). i , λ i ).

[0129] S33: Obtain the actual terrain elevation h i = DEM(φ i , λ i ), calculate elevation Di and actual terrain elevation h i Height difference ΔD i = D i - h i Preset height difference ΔD i Convergence conditions m is updated and iterated as follows:

[0130] (7);

[0131] Iterative height search until the height difference ΔD is calculated iteratively. i Stop iterating within 10 meters to obtain the coarse latitude and longitude coordinates of the ground vehicle target based on elevation data at this point. , ).

[0132] In step S33, in areas with undulating terrain, digital elevation data needs to be imported to guide precise positioning. The digital elevation data can be selected from published official data or reliable data obtained through self-surveyed measurements; it is usually in raster format, and the elevation of the corresponding location can be queried using latitude and longitude.

[0133] Step 4: Using the coarse positioning result as the center, 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 fine positioning result of the vehicle target's latitude, longitude, and altitude at this moment.

[0134] As attached Figure 3 As shown, in step four, after obtaining the coarse latitude and longitude based on the elevation data, the local tangent plane method is implemented to solve the problem of low resolution of the elevation data. Taking the coarse positioning result as the center, a set of digital elevation points within a radius R in the projected coordinate system is selected, and least squares fitting is performed to obtain the analytical equation of the local tangent plane. Then, the intersection point is obtained by solving the equation of the line-of-sight vector of the aerial camera to obtain the fine positioning result of the target's latitude, longitude, and altitude.

[0135] Step four specifically comprises the following steps:

[0136] S41: Taking the vehicle target rough positioning latitude and longitude (N , ) obtained in step three as the center, a number M of elevation data sampling points within a preset radius R are taken as follows:

[0137] (8);

[0138] In the above formula, (N i , E i ) is the corresponding coordinate of the vehicle target rough positioning latitude and longitude (N , ) in the NED coordinate system; for the jth sampling point, its coordinate in the NED coordinate system is denoted as (N j , E j , D j ), where Nj, Ej, Dj are respectively the northward distance, eastward distance, and elevation value of the point relative to the origin of the NED coordinate system. In this embodiment, a total of M elevation data sampling points within a radius R = 50 meters are taken, and Nj, Ej, Dj are all in units of meters.

[0139] S42: The local tangent plane equation formed by the elevation data sampling points is solved by using the least squares method as follows:

[0140] ;

[0141] (9);

[0142] In the above formula, the design matrix A is an M-row-by-3-column matrix, the ith row of which is composed of the local coordinates (x i , y i ) of the corresponding sampling point and the constant 1, that is, x i , y i , 1; the elevation vector D dem is an M-dimensional column vector, the ith element of which is the elevation value h i of the corresponding sampling point; a, b, c are respectively the corresponding parameters of the local tangent plane equation; N, E, D are variables of the plane equation, corresponding to the northward, eastward, and downward variables respectively;

[0143] In this embodiment, the design matrix A of the least squares method defined based on the M sampling points and the defined observed elevation vector D dem are as follows:

[0144] ;

[0145] .

[0146] S43: Solve the parametric equations of the line-of-sight ray and the local tangent plane equations simultaneously, as shown in the following equation:

[0147] ;

[0148] Solving for:

[0149] (10).

[0150] S44: Using the Earth's radius of curvature conversion latitude and longitude increment method as in S32, obtain the static precise positioning results (φ) of the vehicle target's latitude, longitude, and altitude in the undulating terrain at the current time k. * , λ * h * ).

[0151] 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 using digital elevation data. The motion state prediction is obtained by fusing these models using an adaptive IMM algorithm. 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 features in the vehicle's latitude and longitude neighborhood are obtained. The adaptive observation noise and adaptive model transition probability are calculated based on the terrain features. The obtained adaptive observation noise and adaptive model transition probability are used in the terrain-adaptive IMM process to obtain filtered observation data. The observation data is used to correct the predicted values ​​to obtain the final vehicle coordinates and the current target motion state.

[0152] 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.

[0153] Specifically, as shown in the attached document Figure 4 As shown, the TA-IMM dynamic positioning optimization algorithm takes the static fine positioning result as input, calculates the local terrain volume based on the elevation data at each observation point of the moving target, and constructs the adaptive observation noise and model transfer equation for terrain perception.

[0154] Step 6: Input the vehicle's latitude, longitude, and altitude from the static fine positioning results into 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; finally, output the optimized vehicle dynamic positioning latitude, longitude, altitude, and speed.

[0155] Step six specifically includes the following steps:

[0156] S61: As shown in Figure 4 , 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; : reference process noise covariance of model j; : reference observation noise covariance; : reference model transition probability matrix.

[0157] Specifically, as shown in Figure 4 , in this embodiment, at the kth moment, the local tangent plane method has obtained the longitude, latitude and height of the static precise positioning of the vehicle, and then the digital elevation model is imported to query the elevation value in the neighborhood of this position, and the mean value and standard deviation of the regional elevation fluctuation are calculated.

[0158] According to the static precise positioning result (φ * , λ * , h * ) of the vehicle target at the current kth moment, an adaptive observation noise is established;

[0159] First, the standard deviation of the elevation fluctuation in the neighborhood of the position where the static precise positioning result is located is calculated :

[0160] (11);

[0161] In the above formula, h i is the height of the i-th elevation sampling point around the position; is the average elevation around the position; M is the number of sampling points. In the above formula, is as follows:

[0162] ;

[0163] According to the standard deviation of the elevation fluctuation, the terrain adaptive observation noise R k is established as follows:

[0164] (12);

[0165] In the above formula, R0 is a preset reference observation noise; I is an identity matrix; μ R is a preset terrain influence coefficient on observation, which ranges from 0.01 to 0.1; this adaptive observation noise R k adjusts the observation noise according to the degree of terrain fluctuation, increases the observation noise in the area with large terrain fluctuation, thereby increasing the uncertainty of the observation part of the TA-IMM algorithm, and optimizing the filtering effect.

[0166] S62: According to the standard deviation of the elevation fluctuation , establish terrain adaptive model transition probability based on adaptive terrain model interaction :

[0167] (13)

[0168] in the above formula is the baseline transition probability from model i to model j; is a preset terrain influence coefficient, ranging from 0.02 to 0.08 m -1 ; m represents the summation index, which traverses all models in the predefined motion model set, i.e., the constant acceleration motion model and the constant speed turning motion model, and in the above formula, the value range of the summation index m is {1, 2}; model i and model j represent one of the constant acceleration motion model and the constant speed turning motion model; this terrain adaptive model transition probability reduces the transition probability when the terrain fluctuation standard deviation increases, ensuring that the model will not frequently switch under complex terrain.

[0169] S63: Before performing independent prediction on the constant acceleration motion model and the constant speed turning motion model, first perform the model interaction and state mixing step; this step is based on the filtering result at the last time, i.e., at time (k-1) and the terrain adaptive model transition probability in step S62, to calculate a mixed initial state for the two motion models at the current time k; the mixed initial state includes a mixing probability , a mixed initial state , and a mixed initial covariance ;

[0170] Specifically, for the two motion models of the TA-IMM algorithm, a model interaction function considering terrain adaptive transition probability is established, the filtering results of each model at the last time are mixed according to the probability to generate the starting state of the j model at the current time after model switching:

[0171] ;

[0172] ;

[0173] ;

[0174] wherein is the mixing probability, is the mixed initial state, is the mixed initial covariance, and in the above formula, the subscript k-1 represents the last time, i.e., the initial time;

[0175] S64: In the prediction process, the constant acceleration motion model and the constant speed turning motion model run in parallel, and Kalman filtering method and extended Kalman filtering algorithm are used for prediction respectively; state vector X = x, y, v x ,v y , a x , a y, ω T is used in the prediction process, wherein x, y represent the latitude and longitude position; v x , v y represent the latitude and longitude direction speed; a x , a y is the latitude and longitude direction acceleration; and ω is the angular velocity.

[0176] S641: Specifically, the constant acceleration motion model assumes that the target does constant acceleration motion, at which time the angular velocity ω is set to 0, and the state transition equation according to the physical model of the constant acceleration motion is as follows and the state transition matrix :

[0177] ;

[0178] ;

[0179] In the above formula, T is a sampling period, that is, the time interval between k-1 time and k time; is the hybrid initial state at the last time, that is, k-1 time;

[0180] The state of the target vehicle at this time k is predicted using the state transition equation and the covariance :

[0181] ;

[0182] In the above formula, represents the hybrid initial covariance of the target vehicle at k-1 time, represents the process noise covariance;

[0183] S642: Specifically, in the embodiment, the constant speed turning motion model assumes that the target does constant angular velocity motion, and the nonlinear state transition equation according to the physical model of the constant speed turning motion model is as follows and the state transition function :

[0184] ;

[0185] ;

[0186] In the above formula, ψ = atan2(vy , v x ) is the velocity direction angle, due to the nonlinearity of the constant velocity turning motion model state transition function, the Jacobian matrix is used to linearize it:

[0187] ;

[0188] In the above formula, represents the mixed initial state at time k-1.

[0189] The target vehicle state and covariance at time k are predicted using this state transition matrix:

[0190] .

[0191] S65: Observe and update the double model prediction results: the observation is the target vehicle latitude and longitude, and the observation function h(X) = (x, y) T ; according to the prior state estimate The predicted observation vector of the two motion models at time k is as follows:

[0192] (14)

[0193] In the above formula, and represent the target latitude and longitude coordinates in the predicted observation vector, respectively;

[0194] The difference between the true observation z k and the predicted observation is calculated as the innovation ;

[0195] ;

[0196] .

[0197] The Kalman gain of the model at this time is updated :

[0198] ;

[0199] The state and covariance of the motion model are updated as follows:

[0200] ;

[0201] (15);

[0202] In the above formula, is the unit matrix; represents the prior state covariance matrix; represents the Kalman gain; H is the observation Jacobian matrix;

[0203] S66: In the fusion output process, the motion model likelihood is updated as follows and the model probability :

[0204] ;

[0205] (16);

[0206] In the above formula, is the innovation covariance, is the normalization constant of the likelihood;

[0207] The fused model state and the model covariance :

[0208] ;

[0209] (17);

[0210] In the above formula, represents the fused state vector;

[0211] The fused state is a weighted average of the outputs of the two motion models in TA-IMM, obtaining the final output of the dynamic optimized vehicle target latitude and longitude positioning result and the current speed at time k.

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

[0213]

[0214] The first four components in the above formula are directly output as the longitude, latitude and speed components in the longitude and latitude directions of the target after dynamic optimization.

[0215] Verification example:

[0216] This verification example uses the IMM-EKF motion filtering algorithm and this algorithm for simulation test comparison. The IMM-EKF motion filtering algorithm is an optimized algorithm of the traditional IMM algorithm, which has certain adaptability to nonlinear motion patterns.

[0217] In this simulation verification example, the windows+python3.10 development platform is used for simulation test, a mountainous area with terrain undulations is selected to design the driving route of the vehicle, and the terrain standard deviation range m, specifically, the starting point of the target vehicle is set as (120.108200 °E, 31.413767 °N), and the motion sequence is: (1) moving at 16 m / s in the north for 10 seconds from the starting point. (2) uniformly decelerating to 4 m / s in 2 seconds. (3) moving at 4 m / s for 4 seconds. (4) uniformly angular velocity turns 90 ° east (linear velocity 4 m / s). (5) immediately after turning to the east, uniformly accelerating to 10 m / s in 2 seconds. (6) moving at 10 m / s for 20 seconds. (7) uniformly angular velocity turns 90 ° south (linear velocity 10 m / s). (8) moving at 10 m / s south for 15 seconds.

[0218] In the simulation verification example, the unmanned aerial vehicle follows the target vehicle from the rear, the absolute height is 200 m, the gimbal initial pitch angle is -35 °, the tracking distance is about 150 m, and the target is always kept in the image field of view. The camera resolution is 1980x1080, the data acquisition frame rate is 10 times per second, the DEM data resolution is 12.5 m, the Monte Carlo method is used to add random error, and the error obeys normal distribution. The initial model probability of the interacting multiple model is 0.6, 0.4, the transition probability matrix is 0.90, 0.10, 0.10, 0.90, and μ R is set to 0.06, is set to 0.06 m -1 .

[0219] In the simulation verification example, the motion target in the simulation condition is located by longitude and latitude by using the algorithm and the IMM-EKF motion filtering algorithm, and the motion positioning accuracy is compared. The comparison results are shown in Table 1.

[0220] Table 1, 2 kinds of motion filtering method positioning result table

[0221]

[0222] Figure 5 The visual trajectory diagram of the motion target positioning by the method and the IMM-EKF algorithm in the simulation example.

[0223] In the simulation verification example, it can be seen from the result table that the method is obviously superior to the IMM-EKF motion filtering algorithm in the average error, standard error, maximum error and RMS error indicators, and it can be seen from the visual trajectory diagram that the positioning trajectory of the method is closer to the true trajectory, and the positioning accuracy is higher. In summary, the method proposed in the application is superior to the general method.

[0224] Although the present application has been disclosed in its preferred embodiments with reference to the accompanying drawings, it is not intended to limit the present application thereto, and various changes or modifications can be made thereto by those skilled in the art without departing from the spirit and scope of the present application, and the scope of protection of the present application should be defined by the scope of protection of the claims.

Claims

1. A method for precise positioning and dynamic positioning optimization of an unmanned aerial vehicle based on elevation data, characterized in that: The application relates to a vehicle target dynamic positioning method based on a terrain adaptive interactive multi-model filter (TA-IMM) algorithm. Step one: tracking and aerial photographing of a ground moving vehicle target in the air by a Beidou positioning system, a pose sensor and an aerial photographing camera in an unmanned aerial vehicle photoelectric pod; real-time recording of the longitude, latitude and height of the aerial photographing camera and three-dimensional attitude angles while acquiring a vehicle image; Step two: real-time acquisition of a line-of-sight ray parameter equation of a line connecting the aerial photographing camera and the ground vehicle in an NED coordinate system by using a line-of-sight vector method on the basis of the longitude, latitude, height and three-dimensional attitude angles of the aerial photographing camera and the ground vehicle target image; Step three: introduction of digital elevation data of the unmanned aerial vehicle, solution of intersection longitude and latitude of a specified height plane in a line-of-sight vector direction by using a dichotomy method, and iterative height search to acquire a coarse positioning result of the vehicle target longitude and latitude at the moment; Step four: taking a digital elevation point set with a preset radius as a center in a projection coordinate system, performing least square fitting to obtain an analytical equation of a local tangent plane, and solving an intersection point of the analytical equation of the local tangent plane and an analytical expression of a line-of-sight vector of the aerial photographing camera to obtain a static precise positioning result of the vehicle target longitude, latitude and height at the moment; Step five: construction of a terrain adaptive interactive multi-model filter (TA-IMM) dynamic positioning optimization algorithm; the TA-IMM dynamic positioning optimization algorithm is used for fusing two parallel motion models which are constructed on the basis of digital elevation data, and a motion state prediction is obtained by using an adaptive IMM algorithm; the two parallel models are a constant acceleration motion model and a constant speed turning motion model; local terrain quantities of a vehicle longitude and latitude neighborhood are obtained on the basis of the elevation data, adaptive observation noise and adaptive model transition probability are calculated according to the terrain quantities, the adaptive observation noise and the adaptive model transition probability are used in a terrain adaptive IMM process to obtain filtered observation data, the observation data are used to correct a prediction value, and finally a current target motion state of a vehicle coordinate is obtained; Step six: input of the vehicle longitude, latitude and height in the static precise positioning result into the two parallel models of the TA-IMM algorithm for vehicle motion state prediction; probability updating and state fusion are carried out on the basis of the adaptive observation noise and the adaptive model transition probability, and finally optimized vehicle dynamic positioning longitude, latitude, height and speed are output.

2. The elevation data based unmanned aerial vehicle positioning and dynamic positioning optimization method according to claim 1, characterized in that: Step one specifically comprises the following steps. S11: At each static moment, the longitude λ u , the latitude φ u and the altitude h u of the photoelectric pod in the WGS84 coordinate system are obtained in real time by the Beidou satellite locator of the Beidou positioning system and the barometer. S12: The longitude λ u , the latitude φ u and the altitude h u of the photoelectric pod are converted into the local coordinate system of the photoelectric pod by the coordinate conversion function of the Beidou satellite locator. S13: The local coordinate system of the photoelectric pod is established by the Beidou satellite locator. S14: S12: The pose sensor acquires the three-dimensional attitude angle of the aerial camera in real time, including the pitch angle θ, the roll angle and the heading angle ψ; the aerial camera collects images of the ground moving vehicle target 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 elevation data based UAV-UGV precise positioning and dynamic positioning optimization method according to claim 2, characterized in that: Step two specifically comprises the following steps. 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 sight line direction vector v of the connection line between the aerial camera optical center and the ground vehicle in the camera coordinate system c ; S22: Convert the unit look direction vector v c Utilize the pose angle compound rotation matrix R C2N Convert to NED coordinate system to get look direction unit vector : Where the pose angle compound rotation matrix R C2N As follows: ； ； ； （1）； Ri = Rr(θi)Rp(θi)Ry(ψi) (1) Φ Rr(θi) = [cosθi 0 sinθi 0 -sinθi 1 cosθi] (2) θ Rp(θi) = [1 0 0 0 cosθi -sinθi 0 sinθi cosθi] (3) ψ Ry(ψi) = [cosψi -sinψi 0 sinψi cosψi 0 0 0 1] (4) where Rr(θi), Rp(θi), and Ry(ψi （2）； In the above formula, l N , l E , l D respectively represent the north, east, and ground components of the line-of-sight unit vector in the NED coordinate system. S23: Viewing ray of the space link of the aerial vehicle to any height D A The parametric equation P(s) of the viewing ray of the space link of the ground vehicle in the NED coordinate system is as follows: （3）； In the above formula, s is the distance from the camera optical center to the intersection point of the line-of-sight ray and the plane with height D A ; N(s), E(s), 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, and the corresponding coordinates are (0, 0, 0) T .

4. The elevation data based UAV-UGV precise positioning and dynamic positioning optimization method according to claim 3, characterized in that: Step three specifically comprises the following steps. S31: First, initialize the minimum value D of the height range of the area around the UAV min , the maximum value D of the height range max ; use the dichotomy to select the height D i = (D min + D max ) / 2, and use the line-of-sight ray parameter equation solving method in step S23 to solve the line-of-sight ray parameter equation P(s) for the plane of the height Di; S32: Based on the method of converting latitude and longitude increments using the Earth's radius of curvature, the latitude and longitude (φ) of the intersection point of the line-of-sight ray parameter equation P(s) and the plane at height Di are obtained. i , λ i Specifically, this includes: Firstly, the intersection point of the line-of-sight ray parameter equation P(s) and the height Di plane is calculated relative to the north, east and ground directions of the UAV N g , E g , D g , as follows: （4）； Then the north, east, and ground distances are converted into latitude and longitude increments, at which point the earth's meridian radius of curvature R M (φ u ) and prime vertical radius of curvature R N (φ u ) are introduced to characterize the latitude and longitude increments corresponding to the east and north distance increments, resulting in latitude and longitude increments Δφ and Δλ: （5）； （6）； In the above formula, a ， is the earth's semi-major axis, taken as 6378137 meters; e is the earth's eccentricity, taken as 0.0818; Finally, the intersection longitude and latitude (φ i , λ i ) are obtained by adding the corresponding longitude and latitude increments to the current longitude and latitude of the UAV. S33: Obtain actual terrain elevation h i = DEM(φ i , λ i ), calculate elevation Di and actual terrain elevation h i Height difference ΔD i = D i - h i ; preset height difference ΔD i Convergence condition of m, update iteration of the following formula is performed: （7）； Iterate height search until the difference in height calculated by iteration ΔD i Stop iteration within 10 meters, obtain the rough positioning of the ground vehicle target based on the elevation data latitude and longitude at this time , ) 5. The elevation data based UAV-UGV precise positioning and dynamic positioning optimization method according to claim 4, characterized in that: Step four specifically comprises the following steps. S41: Coarse positioning latitude and longitude of the vehicle target obtained in step three ( , Centered on a target area, M elevation data sampling points are selected within a preset radius R using the following formula: （8）； In the above formula, (N i , E i ) is the target coarse positioning latitude and longitude of the vehicle; (N , E ) is the corresponding coordinate in the NED coordinate system; for the jth sampling point, the coordinate in the NED coordinate system is denoted as (N j , E j , D j ), where N j , E j , and D j are the north distance, east distance, and elevation value of the point relative to the origin of the NED coordinate system, respectively. S42: a local tangent plane equation formed by elevation data sample points is solved by using a least square method as follows: ； （9）； In the above formula, the design matrix A is a matrix of M rows and 3 columns, the i-th row of which is composed of the local coordinates (x i ,y i ) of the corresponding sampling point and a constant 1, i.e., [x i ,y i ,1]; the elevation vector D dem is a column vector of M dimensions, the i-th element of which is the elevation value h i of the corresponding sampling point; 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: a distance parameter s* of an intersection point of a line-of-sight ray and a plane is solved by using a line-of-sight ray parameter equation and a local tangent plane equation as follows: ； The solution is as follows: （10）； S44: Obtain the static precise positioning results of the longitude and latitude and the height of the vehicle target in the undulating terrain at the current k moment (φ * , λ * , h * ) by using the earth curvature radius conversion longitude and latitude increment method as in step S32.

6. The elevation data based UAV-UGV precise positioning and dynamic positioning optimization method according to claim 5, characterized in that: In the TA-IMM dynamic positioning optimization algorithm, a Kalman filter (KF) is used for current target motion state prediction of the constant acceleration motion model, and an extended Kalman filter (EKF) is used for current target motion state prediction of the constant speed turning motion model.

7. The elevation data based UAV-UGV precise positioning and dynamic positioning optimization method according to claim 6, characterized in that: Step six specifically comprises the following steps. S61: input the static fine positioning result (φ * , λ * , h * ) of the vehicle target at the current k moment as the observation position of TA-IMM and establish adaptive observation noise; First, the standard deviation of the height fluctuation is calculated in the neighborhood of the position of the static fine positioning result : （11）； In the above formula, h i is the height of the i-th height sampling point around the position; is the average height around the position; M is the number of sampling points; and the terrain adaptive observation noise R k is established according to the standard deviation of the height fluctuation, and the following formula: （12）； In the above formula, R0 is a preset reference observation noise; I is a unit matrix; μ R is a preset terrain impact coefficient on observation, which ranges from 0.01 to 0.1; S62: Establishing a terrain adaptive model transfer probability based on a standard deviation of elevation relief , establishing a terrain adaptive model transfer probability based on a standard deviation of elevation relief : （13）； In the above formula is a reference transition probability from model i to model j; is a preset terrain influence coefficient, ranging from 0.02 to 0.08 m -1 ; m represents a summation index, which traverses all models in a predefined motion model set, i.e., a constant acceleration motion model and a constant speed turning motion model; in the above formula, the value range of the summation index m is {1, 2}; model i and model j represent one of the constant acceleration motion model and the constant speed turning motion model; S63: Before making independent predictions for the constant acceleration motion model and the constant velocity turning motion model, first perform a model interaction and state mixing step; this step is based on the filtering results at the previous time, i.e., time (k-1), and the terrain adaptive model transition probabilities in step S62, to calculate a mixed initial state for the two motion models at the current time k; the mixed initial state includes a mixed probability , mixed initial state , mixed initial covariance ; S64: In the prediction process, the constant acceleration motion model and the constant speed turning motion model run in parallel, respectively using Kalman filtering method and extended Kalman filtering algorithm for prediction; in the prediction process, the state vector X = [x, y, v x , v y ,a x , a y, ω] T is adopted, wherein x, y represent the latitude and longitude position; v x , v y represent the latitude and longitude direction speed; a x , a y is the latitude and longitude direction acceleration; ω is the angular velocity; S65: Observing and updating the double model prediction results: the observation is the longitude and latitude of the target vehicle, and the observation function is h(X) = (x, y) T ; according to the prior state estimation Predicting the prediction observation vector at time k of the two motion models As follows: （14）； In the above formulae, and respectively represent the target longitude and latitude coordinates in the predicted observation vector; The real observation z k is subtracted from the predicted observation to obtain the innovation ; the state of the motion model is updated as follows and the covariance : ； （15）； In the above formula, is the identity matrix; denotes the prior state covariance matrix; denotes the Kalman gain; H is the observation Jacobian matrix; S66: fusion output; update motion model likelihood as follows and model probability : ； （16）； In the above formula, is the innovation covariance, is the normalization constant of the likelihood. Computing the fused model state and model covariance : ； （17）； In the above formula, denotes the fused state vector; The fused state is a weighted average of outputs of two motion models in the TA-IMM, and finally output dynamic optimized vehicle target longitude and latitude positioning results and a current speed at the k moment are obtained.

Images