CN107300377B - A three-dimensional target localization method for rotary-wing UAV under the orbital trajectory - Google Patents

Abstract

The present invention discloses the rotor wing unmanned aerial vehicle objective localization method under a kind of track of being diversion, and using the single camera photographic subjects image being mounted on unmanned plane, and image is passed back to earth station；The marker with obvious characteristic is selected, and carries out visual identity；Then rotor wing unmanned aerial vehicle is diversion centered on the marker, carries out multipoint images measurement, calculates height and course deviation of the unmanned plane relative to landform where target based on the method for binocular vision model and the mutual iteration of linear regression model (LRM)；Next, any static or moving target in camera coverage may be selected in operator, realize that the three-dimensional of target is accurately positioned.The present invention is carried out in same primary aerial mission, and flight leading portion calculates course deviation and relative altitude, and flight back segment carries out three-dimensional accurate positioning；The present invention traditional triangulation location method under track that solves the problems, such as to be diversion can not calculate relative altitude, to realize the three-dimensional localization to target.

Description

A three-dimensional target localization method for rotary-wing UAV under the orbital trajectory

technical field

The invention belongs to the field of visual measurement, and in particular relates to a three-dimensional target positioning method of a rotary-wing unmanned aerial vehicle under a flying trajectory.

Background technique

Rotor-wing UAVs have been widely used in the fields of reconnaissance, agricultural insurance, environmental protection and post-disaster rescue due to their low cost, vertical take-off and landing, and hovering in the air.

The vision-based target positioning of the rotor UAV has become one of the current research hotspots. The visual method is used to locate the target in three dimensions. First, the relative height between the UAV and the target is determined by the triangulation method, and then the target can be positioned. . Considering the large heading deviation caused by the low-precision AHRS attitude reference system equipped with the rotor UAV, when using the image captured by the UAV for visual measurement, the light projected in the image will be offset to a certain extent. If the traditional triangulation method is used, the two sets of rays projected from the left and right views will be offset, and the relative height obtained by the solution will have a large error, so the relative height between the drone and the object cannot be accurately calculated, so it is impossible to calculate the relative height between the drone and the object. Effective 3D positioning of the target.

SUMMARY OF THE INVENTION

In view of this, the present invention provides a three-dimensional target positioning method of the rotor UAV under the flying trajectory, which can calculate the heading deviation, reduce the calculation error of the relative height, and improve the three-dimensional positioning of the target by the rotor drone. ability.

Beneficial effects:

(1) The method provided by the present invention can accurately calculate the heading deviation existing in the AHRS heading and attitude reference system for a rotor UAV equipped with a low-precision AHRS heading and attitude reference system, and then calculate the rotor unmanned aerial vehicle and the rotor unmanned aerial vehicle under the flying trajectory. The height between the terrain of the target, so as to realize the three-dimensional visual positioning of the target by the rotor UAV.

Description of drawings

Fig. 1 is the structure diagram of the three-dimensional positioning system of the rotor UAV target of the present invention;

Fig. 2 is the flow chart of the method provided by the present invention;

3 is a schematic diagram of a rotating view binocular vision model used in the present invention;

4 is a schematic diagram of a monocular camera ranging model used in the present invention;

Fig. 5 is the iterative process flow chart in the method provided by the present invention;

Fig. 6 is in the method provided by the present invention The data fitting curve;

Fig. 7 is in the method provided by the present invention The data fitting curve;

FIG. 8 is a positioning effect diagram of the method provided by the present invention.

Detailed ways

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

Build the following experimental platform to verify the effectiveness of the present invention, using a T650 quadrotor unmanned aerial vehicle, a notebook as the ground station, real-time communication can be carried out between the unmanned aerial vehicle and the ground station, the system structure is shown in Figure 1.

For UAVs, the aircraft is equipped with GPS positioning system, AHRS heading reference system, altimeter, wireless image transmission module and wireless data transceiver module, so that 3D Robotics' APM flight control system works in the self-stabilizing mode to ensure the UAV stable flight. A camera is installed at the nose position of the UAV, the top-down angle β is 45°, and the image is sent back to the ground station through the wireless image transmission module. The position, attitude and elevation information of the aircraft are transmitted to the ground station through the wireless data transceiver module.

The ground station takes the computer as the main body, runs algorithms such as the visual positioning of the UAV, and uses the USB interface to connect the wireless data transceiver module to realize the mutual communication between the UAV and the ground station.

Based on the experimental platform, as shown in Figure 2, a three-dimensional target positioning method of a rotary-wing UAV under a flying trajectory includes the following steps:

Step 1. After the system is started, use the camera mounted on the UAV to capture images and send the images back to the ground station;

Step 2: Select a static object with a clear outline as a marker from the returned image, and visually identify the marker;

The specific process of visually identifying the markers in step 2 is as follows:

Use the SIFT algorithm to identify the markers, obtain m feature points P ₁ , P ₂ . . . P _m-1 , P _m , and store these feature points as templates, where m is an integer;

Step 3: The rotor UAV flies around the marker, and uses the results of visual recognition to measure the marker at multiple points. Based on the mutual iteration method of the binocular vision model and the linear regression model, the relative value of the UAV is calculated. Altitude and heading deviation of the terrain on which the target is located;

The flowchart of step 3 is shown in Figure 5, and the specific process is as follows:

Step 3.1. The rotary-wing UAV uses visual recognition to measure N images in chronological order under the orbital flight trajectory, and uses the SIFT algorithm to extract the features of the current i-th image (1≤i≤N), and then use the template Match the feature points of the current image with the feature points of the current image to obtain w groups of matching points P ₁ , P ₂ ... P _w-1 , P _w (w≤m), and finally take the geometric center of these matching points P _f (f (f ≤w) represents the pixel position of the marker in the image, denoted as And record the measurement value when measuring the i-th image, including: the position of the drone shooting point O _i in the inertial reference frame {I} and attitude (ψ _i , θ _i , φ _i ), ψ _i , θ _i , φ _i are the azimuth, pitch and roll angles, respectively.

Step 3.2. Select any two images out of N images, there are a total of n groups, among which Taking the image measured earlier as the left view L, and the image measured later as the right view R, the binocular vision model of the rotated view is constructed, as shown in Figure 3.

Calculate the relative height h _j of the drone relative to the marker, 1≤j≤n

Among them, the pixel positions of the marker in the left and right views are respectively R _l , T _l are the rotation matrix and translation matrix of the UAV shooting point O _l corresponding to the left view relative to the inertial reference coordinate system, respectively,

Among them, ψ _l , θ _l , φ _l are the heading angle, pitch angle and roll angle of the left-view UAV shooting point O _l , respectively, ψ _r , θ _r , φ _r are the right-view UAV shooting point O, respectively _l heading angle, pitch angle and roll angle, δψ is the heading deviation, there is ψ _l =ψ _i -δψ(k), k is the number of iterations, θ _l =θ _i , φ _l =φ _i (1≤i＜N ), set the initial value δψ(0)=0;

R _r , T _r are the rotation matrix and translation matrix of the UAV shooting point _Or corresponding to the right view relative to the inertial reference coordinate system, respectively,

Among them, ψ _r =ψ _m -δψ(k), θ _r =θ _m , φ _r =φ _m (i<m≤N)

The coordinates of the drone shooting points corresponding to the left view and the right view in the inertial reference coordinate system are: and R, T are the rotation matrix and translation matrix of the camera coordinate system corresponding to the right view relative to the camera coordinate system corresponding to the left view, R=R _r R _l ^T , T=T _l -R ^T T _r =R _l (O _r -O _l ); M=[P _l ^-RT P _r P _l ×RT P _r ] ^{-1 T.}

Step 3.3. For the calculated relative heights h _j of the n groups, use the 3σ criterion to remove the gross error, and then calculate the average value of the n groups

Step 3.4, get the relative height Then, use the measured value of N points in step 3.2 and calculate the heading deviation δψ(k) based on the linear regression model;

Generally, [xyz] ^T , [x _p y _p z _p ] ^T represent the coordinates of the drone and the object in the inertial reference coordinate system {I}, respectively, and (x _f ′, y _f ′) represent the position of the object in the image Pixel position, f is the focal length of the camera, and the ranging model of the camera is

attitude matrix for

Among them, h' is the relative height between the UAV and the object, (ψ', θ', φ') represents the heading angle, pitch angle and roll angle of the UAV at a certain measurement point, among which, the pitch angle The measurement accuracy of θ′ and roll angle φ′ is high, and their errors are negligible, while the measurement of the heading angle ψ′ has a large heading deviation.

In this embodiment, in order to calculate the heading deviation of the heading angle, the measured values of N points of the markers captured by the drone at different positions are used, and the linear regression method is used to solve the problem. The specific calculation process is as follows: [x _G y _G z _G ] ^T represents the coordinates of the marker in the inertial reference coordinate system {I}, let [x _p y _p z _p ] ^T =[x _G y _G z _G ] ^T , is the average of the relative height of the drone and the marker, let Substitute into formula (4), we can get

Let the parameters θ=[θ _a , θ _b ] ^T , θ _a = [x _G , y _G ] ^T , θ _b = δψ(k), The measurement equations of position and attitude are equations (6) and (7), respectively:

z ₁ (i)=y ₁ (i)+v ₁ ,v ₁ to N(0,R ₁ ) (6)

Among them, v ₁ , v ₂ are measurement noises, and R ₁ , R ₂ are real symmetric positive definite matrices. Then formula (5) can be transformed into

in, For the attitude deviation, using Taylor expansion, Equation (8) becomes

From formula (8) and formula (9), we get

set matrix where a _1,3 ～a _2,5 represent the corresponding elements in the matrix A _i ; the matrix Among them, b _1,1 to b _2,3 represent the corresponding elements in the matrix B _i . In this embodiment, N-point visual measurement is performed on the same marker, so The corresponding matrices are A ₁ ,…, _AN , B ₁ ,…,B _N , and the following linear regression model is obtained through these measured values,

where I ₂ is a 2 × 2 identity matrix, and the noise is

V～N(0,R)

The covariance matrix is

The estimated parameter θ is

The heading deviation δψ(k) can be solved by formula (12).

Step 3.5, set e as a constant, if |δψ(k)-δψ(k-1)|<e, then get the final estimated value of relative height and heading deviation estimates And go to step 4; otherwise, go to step 3.2, substitute the current δψ(k) into the calculation formula of the heading angle of the left and right views, and find Thereby iterative calculation is performed.

Step 4. Under the condition that both the relative altitude and the heading deviation are effectively estimated, select any target in the field of view of the camera and obtain the measurement value of the target, and use the obtained heading deviation to calculate the true heading of the UAV, and then according to the real heading Heading and altitude estimates Realize the three-dimensional precise positioning of the target.

Specifically, it is assumed that the selected target is in the same plane as the marker, i.e. the estimated relative height It can be considered as the relative height of the UAV and the target, [x _t y _t z _t ] ^T represents the coordinates of the target in the inertial reference coordinate system {I}, there are Let [x _p y _p z _p ] ^T = [x _t y _t z _t ] ^T , Substitute the measured value of the target and the true heading of the UAV into formula (4), and calculate the coordinates of the target, so as to realize the three-dimensional positioning of the target.

The effectiveness of the iterative process will be described in detail below. Taking the circle around the flying trajectory as an example, the radius R=73m, the radian rad=1.5π, δψ=0,1,...,59,60deg, which can be obtained by formula (1). Corresponding 61 groups Then use the data fitting method to is the dependent variable, δψ is the independent variable, as shown in Figure 6, we get The mathematical relational expression for :

Similarly, let 36 groups of δψ are obtained by solving the formula (10), and then the data fitting method is used, with δψ as the dependent variable, is the independent variable, as shown in Figure 7, we get The mathematical relational expression for :

Assume e _δψ = δψ-δψ _t , where h _t , δψt are the true values of relative altitude and heading deviation, obtained from formula (9),

According to formula (10),

Among them, k ₁ and k ₂ are related parameters.

The relative height calculated by the binocular vision model is substituted into the linear regression equation, which can effectively calculate the heading deviation. Then, back-substituting the estimate of heading bias into the binocular vision model allows accurate calculation of relative altitude. Generally, the heading deviation of the AHRS system does not exceed 30deg, so |k ₂ |>k ₁ >0, and since k ₂ <0, according to equations (15) and (16), it can be known that after a finite step iteration, the relative altitude estimated value of and heading deviation estimates will converge to the true value.

Under the condition that the UAV is equipped with a camera, a flight test is carried out. The UAV performs image measurement on the marker under the orbital trajectory. The radius of the orbiting trajectory is R=73m, the arc rad=1.5π, and the UAV is relatively In the real height of the marker h _t = 45m, the flight speed V = 3.44m/s, f _GPS = 4Hz, the true value of the heading deviation δψ _t = 30deg, set e = 0.02deg, the effect of the method provided by the present invention is as shown in the table 1, Table 2, as shown in Figure 8. The errors e _h , e _δψ , _{e xy} , and ez listed in the table refer to the root mean square error.

Table 1 Iterative process

Table 2 Target positioning results

index Three-dimensional positioning of the present invention Relative height estimation error e_h/m 0.93 Heading estimation error e_δψ/deg 1.89 Positioning error e_xy/m 10.89 Positioning error e_z/m 0.43

To sum up, the above are only preferred embodiments of the present invention, and are not intended to limit the protection scope of the present invention. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included within the protection scope of the present invention.

Claims (4)

1. A three-dimensional target positioning method of a rotor unmanned aerial vehicle under a flying-around track is characterized by comprising the following steps:

step one, extracting a static object from an image shot by a rotor unmanned aerial vehicle as a marker;

secondly, the rotor unmanned aerial vehicle performs fly-around by taking the marker as a center, performs N-angle shooting on the marker in the fly-around process, and obtains a measured value of each shot image; n is a positive integer;

step three, grouping the N shot images in pairs, wherein each group of shot images utilizes rotationCalculating the relative height of the unmanned gyroplane relative to the marker by using the binocular vision model of the view, and then taking an average value of N groups as a relative height error in the current iteration turn k

When the relative altitude is calculated, the heading deviation delta psi (k) required by the binocular vision model of the rotated view is calculated by adopting the heading deviation delta psi (k-1) obtained by the previous iteration turn k-1, and the heading angle psi (k) required by the binocular vision model is updated to be psi (k) ═ psi_iδ ψ (k), wherein ψ_iThe course angle of the unmanned aerial vehicle when shooting the ith image is obtained; the initial value delta psi (0) is taken as 0;

step four, utilizing the relative height errorAnd the measured values of the N shot images are calculated to obtain the course deviation delta psi (k) of the current iteration turn k;

step five, judging whether the deviation between the delta psi (k) and the delta psi (k-1) is smaller than a set threshold value or not, and if so, taking the last iteration result as a relative height estimation valueAnd an estimate of course deviationAnd executing the step six; otherwise, adding 1 to the iteration turn k, and turning to the third step;

step six, calculating the real course of the rotor unmanned aerial vehicle by utilizing the estimated value of course deviation for any target in the visual field of the camera of the rotor unmanned aerial vehicle, and further calculating the real course and the height estimated value according to the real course and the height estimated valueRealizing three-dimensional positioning of the target;

in the fourth step, the specific way of calculating the heading deviation δ ψ (k) is as follows:

[x_Gy_Gz_G]^Tcoordinates representing the marker in an inertial reference frame { I }, are obtained

The ranging model of the camera is as follows:

wherein f is the focal length of the camera,to be the pose matrix of the drone at the time the ith image was taken,

1≤i≤N；

wherein,and (psi)_i,θ_i,φ_i) Shoot point O for unmanned aerial vehicle when shooting ith image_iPosition and attitude,. psi, in an inertial reference system { I }_i,θ_i,φ_iRespectively an azimuth angle, a pitch angle and a roll angle,is the pixel position of the marker in the ith image.

Let parameter θ be ═ θ_a,θ_b]^T，θ_a＝[x_G,y_G]^T，θ_b＝δψ(k)，The measurement equation set is

Wherein v is₁，v₂To measure noise, R₁，R₂For a true symmetric positive definite matrix, the formula (3) is transformed into

WhereinFor the attitude deviation, the equation (4) is changed to

From formula (4) and formula (5) to obtain

Setting matrixWherein a is_1,3～a_2,5Representation matrix A_iThe corresponding elements in (1);

matrix arrayWherein b is_1,1～b_2,3Is shown in matrix B_iThe corresponding elements in (1); a linear regression model was obtained as follows,

wherein, I₂Is a 2X 2 unit matrix, and has noise of V-N (0, R)

The covariance matrix is

The estimated value of the parameter theta is

The heading deviation δ ψ (k) can be solved by equation (8).

2. The method of claim 1, wherein the relative height h of the drone relative to the marker is a function of the distance between the drone and the marker_jAnd j is more than or equal to 1 and less than or equal to n, and the calculation method comprises the following steps:

wherein T ═ T_l-R^TT_r＝R_l(O_r-O_l)，M＝[P_l -R^TP_r P_l×R^TP_r]^-1T，R＝R_rR_l ^T；P_l、P_rPixel positions of the marker in the left view and the right view respectively; r and T are a rotation matrix and a translation matrix of a camera coordinate system corresponding to the right view relative to a camera coordinate system corresponding to the left view, R_l，T_lUnmanned aerial vehicle shooting points O corresponding to left views respectively_lRotation matrix and translation matrix with respect to an inertial reference frame, R_r，T_rUnmanned aerial vehicle shooting points O corresponding to right views respectively_rA rotation matrix and a translation matrix relative to an inertial reference frame.

3. A method of three-dimensional targeting of a rotary-wing drone according to claim 2, wherein the measurement of the marker pixel locations is by:

identifying the marker image obtained in the step one to obtain a plurality of characteristic points; and identifying each image in the process of flying around to obtain a plurality of characteristic points, matching the characteristic points of each image with the characteristic points of the marker image, and taking the geometric center of the matched points as the pixel position of the marker in the image.

4. The method of claim 1, wherein step three involves eliminating gross errors of relative height using a 3 σ criterion prior to averaging the N sets.