CN101833761A - UAV pose estimation method based on cooperative target feature lines - Google Patents

Abstract

The invention discloses a method for estimating the pose of an unmanned aerial vehicle based on characteristic lines of a cooperation target, which is characterized in that it comprises the following steps: (1) performing image preprocessing on an input image to obtain two sets of parallel lines of the cooperation target; (2) Analyze and calculate the vanishing line equation for the extracted parallel lines, and obtain the slope of the vanishing line, thereby calculating the roll angle of the UAV; (3) directly obtain the yaw angle of the UAV according to the image, and convert the known The four point coordinates are substituted into the relationship equation between the UAV camera coordinate system and the world coordinate system to calculate the remaining four pose parameters. The invention realizes the accurate calculation of the position and attitude information of the drone.

Description

UAV pose estimation method based on cooperative target feature lines

technical field

The invention relates to a method in the technical field of image processing, and specifically discloses a method for estimating the pose of an unmanned aerial vehicle based on characteristic lines of cooperative targets.

Background technique

In the navigation and landing guidance of unmanned aerial vehicles, the most widely used is to use the GPS/SINS integrated navigation system combined with the altimeter to complete. However, GPS is controlled by people. Once the GPS is turned off or we receive error codes, the UAV will not be able to navigate and land autonomously, or even cause the UAV to be damaged due to wrong landing guidance. Therefore, we must have autonomous control. Navigation and landing guidance systems. Only in this way can the requirements of unmanned aerial vehicle navigation and all-weather autonomous precise landing be met in wartime. Therefore, in order to improve the anti-jamming ability of our UAVs, solve the problem that GPS is controlled by people, and improve the survivability of UAVs, it is necessary to equip our UAVs with a set of guidance systems that have strong anti-jamming capabilities and can independently navigate and land. system. In fact, the all-weather autonomous navigation and landing of unmanned aerial vehicles is also one of the key issues that many countries in the world urgently need to solve.

With the development of image processing technology and optical camera technology, machine vision navigation technology has become a research hotspot in the field of UAV autonomous navigation. Visual navigation technology uses sensors to obtain images, and obtains UAV navigation and positioning attitude parameters through image processing. Visual sensors have the advantages of portability, low power consumption, and small size. In addition, visual navigation can solve the problem that GPS is controlled by other countries and is susceptible to interference. Potential dangerous problems, and has the advantages of moderate accuracy and low cost. Therefore, the present invention proposes a navigation method based on infrared cooperative targets to realize the all-weather autonomous and precise landing of the UAV.

Contents of the invention

The object of the present invention is to propose a kind of UAV pose estimation method based on cooperative target feature line, by the position of vanishing point and vanishing line on the image surface uniquely determined by the direction of this straight line or plane, they and The spatial attitude of the camera has a definite relationship to calculate the attitude information of the UAV.

The present invention is achieved by taking the following technical solutions:

A method for estimating the pose of an unmanned aerial vehicle based on cooperative target feature lines, characterized in that it comprises the following steps:

(1) Perform image preprocessing on the input image to obtain two sets of parallel lines of the cooperation target;

(2) Analyze and calculate the vanishing line equation to the extracted parallel lines, obtain the slope of the vanishing line, thereby calculate the roll angle of the UAV;

(3) Obtain the yaw angle of the UAV directly according to the image, and substitute the known four point coordinates into the relationship equation between the UAV camera coordinate system and the world coordinate system to calculate the other four pose parameters.

The aforesaid UAV pose estimation method based on cooperative target feature line is characterized in that: in above-mentioned steps (1), at first detect the edge of cooperative target with sobel edge detection method, then carry out contour chain code tracking to edge, Two sets of parallel lines are extracted.

Technical effect of the present invention is as follows:

The invention relates to a method in the technical field of image processing, and specifically discloses a method for estimating the pose of an unmanned aerial vehicle based on characteristic lines of cooperative targets. Its positions on the image plane for the vanishing point and vanishing line are uniquely determined by the direction of the straight line or plane, and they have a definite relationship with the spatial attitude of the camera to calculate the attitude of the UAV. Accurate calculation of UAV pose information is realized.

Description of drawings

Fig. 1 is the relation of relevant coordinate system;

Figure 2 is two sets of parallel lines of the cooperation target;

Figure 3 Schematic diagram of yaw angle.

Detailed ways

The present invention will be further described in detail below in combination with specific embodiments.

As shown in Figure 2, calculate the vanishing line equation of the two sets of parallel lines of the cooperative target; then, get the slope of the vanishing line, thereby calculating the roll angle; as shown in Figure 3, get the yaw angle of the UAV; The known four point coordinates are substituted into the relationship equation between the UAV camera coordinate system and the world coordinate system to calculate the other four pose parameters.

As shown in Figure 1, WCS is the world coordinate system, the origin is established at a point on the plane where the landing icon is located, and the xw-yw plane is parallel to the ground; CCS is the camera coordinate system, the origin is established at the optical center, the yc axis coincides with the optical axis, and the xc The -zc plane is parallel to the image plane; ICS is the image coordinate system, and the origin is located at (0, 0, f) in the camera coordinate system (f is the focal length of the camera), which is a two-dimensional coordinate system. According to the above definition, the conversion relationship between the point (xw, yw, zw) on the landing icon and its projection (x1, z1) on the image plane can be obtained. The relationship between the world coordinate system and the camera coordinate system is as follows:

CCS-WCS $\left(\begin{array}{c}{x}_c\\ {\mathrm{the\; y}}_c\\ z_c\end{array}\right)=R\left(\begin{array}{c}{x}_w\\ {\mathrm{the\; y}}_w\\ z_w\end{array}\right)+T---\left(1\right)$

Among them, T is a three-dimensional translation vector, which is the coordinate of the origin of the world coordinate system in the camera coordinate system. The relationship between the camera coordinate system and the image coordinate system is as follows:

ICS-CCS

其中θ、φ和ψ分别是俯仰角、滚转角和偏航角。设

rotation matrix

where θ, φ, and ψ are the pitch, roll, and yaw angles, respectively. set up

In Fig. 2, two sets of parallel lines are respectively L11, L12 and L21, L22, and the vanishing points formed by the two sets of parallel lines on the image plane are respectively v ₁ (x ₁ , z ₁ ) (obtained from the set of parallel lines L11 ) and v ₂ (x ₂ , z ₂ ) (obtained from the group of parallel lines L21), then connect these two points to obtain the vanishing line formed by the plane on the image plane.

Let the equation of the straight line be expressed as:

L11: y=k ₁ x+b ₁ z=0

(3)

L21: y=k ₂ x+b ₂ z=0

Then according to the definition of the vanishing point, taking a point p(x _p , y _p , 0) on L11, we have:

对y取极限y→∞，同时代入x_p＝(y_p-b₁)/k₁，得：

Taking the limit y→∞ for y, and substituting x _p =(y _p -b ₁ )/k ₁ at the same time, we get:

Similarly, for L21:

Connecting the above two points, and substituting the value indicated by A-I, the equation of the vanishing line is obtained:

x□sinφ-z□cosφ-tan θ□f＝0, from which the slope of the vanishing line k＝tanφ can be obtained. Then the roll angle can be obtained. Then substitute the known four point coordinates into the relationship equation (1) between the UAV camera coordinate system and the world coordinate system to calculate the remaining four pose parameters.

The above has disclosed the present invention with preferred embodiments, but it is not intended to limit the present invention, and all technical solutions obtained by adopting equivalent replacement or equivalent transformation methods fall within the protection scope of the present invention.

Claims (4)

1. UAV position and orientation method of estimation based on the cooperative target characteristic curve is characterized in that may further comprise the steps:

(1) image to input carries out two groups of parallel lines that the image pre-service obtains cooperative target;

(2) the parallel lines analysis meter that extracts is calculated the hachure equation that goes out, draw the slope of the hachure that goes out, thereby calculate the roll angle of unmanned plane;

(3) directly obtain the crab angle of unmanned plane according to image, the relation equation of known four point coordinate substitution unmanned plane camera coordinate systems and world coordinate system is calculated all the other four pose parameters.

2. the UAV position and orientation method of estimation based on the cooperative target characteristic curve according to claim 1, it is characterized in that: in above-mentioned steps (1), at first detect the edge of cooperative target, then the edge is carried out the profile chain code following, extract two groups of parallel lines with the sobel edge detection method.

3. the UAV position and orientation method of estimation based on the cooperative target characteristic curve according to claim 2 is characterized in that: in above-mentioned steps (2), the described hachure equation that goes out is: x sin φ-z cos φ-tan θ f=0; The hachure slope that goes out is: k=tan φ, and wherein θ, φ are respectively the angle of pitch, roll angle, f is a focal length of camera.

4. the UAV position and orientation method of estimation based on the cooperative target characteristic curve according to claim 3 is characterized in that: in above-mentioned steps (3), the relation equation of described unmanned plane camera coordinate system and world coordinate system is:

Described T is the D translation vector, is the coordinate of world coordinate system initial point in camera coordinate system,

θ, φ and ψ are respectively the angle of pitch, roll angle and crab angle.

Images