Vision-Based Pose Estimation of UAV from Line Correspondences

(1)

Procedia

Engineering

Procedia Engineering 00 (2011) 000–000 www.elsevier.com/locate/procedia

Advanced in Control Engineering and Information Science

Vision-based pose estimation of UAV

from line correspondences

Fei Li

a

_{, Da-quan Tang}

a

_{, Ning Shen}

a,

_a*

a. Department of Control Engineering, Naval Aeronautical and Astronautical University, Yantai, China 264001 Abstract

In order to assist landing of Unmanned Aircraft Vehicle (UAV), a vision-based method is proposed, which only makes use of the two edge lines and the threshold line on the runway. In the earlier stage, through seeking the geometric constraints among the three lines, the coordinates of the points of intersection of the three lines in the camera frame are solved, and the vectors of the three lines in the camera frame can be obtained using the vanishing point. The attitudes are solved by the Umeyama method, and the position vector of the UAV can be calculated using the coplanarity characteristic. In the later stage, the threshold line is invisible, the yaw、the pitch、the cross position and the altitude can be estimated by two edge lines. Simulation results show the proposed algorithm is accurate and fast.

Selection and/or peer-review under responsibility of [CEIS 2011] Keywords: vision, land, vanishing point, pose estimation

1. Introduction

Vision-assisted landing of UAV is significant in the military, one important technique of which is estimating the pose in the process of landing based on the runway image captured by the camera fixed in the UAV. Vision-assisted landing attracts much research. Ivan [1] estimates the pose based on the projective matrix between the initial frame and runway and homography matrix between adjacent frames, so the error is prone to be accumulated. Andrew [2] determined the relative location of the runway as an image by performing image registration against a stack of images in which the location of the runway is

*Corresponding author. Tel.:+8615966513353

E-mail address:[email protected].

Open access under CC BY-NC-ND license.

(2)

known, and the method uses the SIFT features about the terrain surrounding the runway, which can steer the UAV towards the runway even before the runway itself becomes visible, but extracting the SIFT features costs much time. Sara [3] proposed a single-camera method for position and attitude estimation based on the runway edge and horizon, but the extraction of horizon is easily disturbed by mountains and tall building. Liu [4] developed a two-camera approach using arbitrary three points on the runway edges, but wing bending can reduce the precision, and the extraction of point is sensitive to noise.

In recent years, there are some conflicts in most research: more features depended on to assist landing, higher accuracy comes out, but more time employed. So it is the goal of auto- landing to make use of fewer features to try for higher accuracy. The most obvious features of the runway is two edge lines and a cross landing threshold line, and the extraction of lines is less sensitive to noise than points. It gets some study that pose being estimated according to only three lines. An algorithm based on geometry constraints of three lines is presented by Qin [5], but the lines are non-coplanar, which differs with the location of the lines of the runway. Afterward, a closed-form method to determine object attitudes with respect to the camera for three coplanar lines is proposed [6]. When there are two solutions for depth of one nodal point, the distances between optical center and the two points of intersection should be contrasted, but this contrast is unrealistic while landing. To overcome the above shortcomings, this paper utilizes the imaging geometry of the runway to acquire the attitudes and position of UAV.

When the UAV is very close to runway, the threshold line is invisible, so only some parameters of pose can be calculated in the condition of roll angle provided by IMU.

2. The description of runway imaging

The runway imaging is described in Fig.1. Two edge lines and the cross threshold line have been detected, while are the points of intersection and the width of runway is . Runway frame has its origin at the center of , and -axis is defined towards the centreline of runway while towards the right, and downwards. Image plane frame has its origin at the center of the image plane, while the cross axis is u , and the longitudinal axis is . Camera frame rigidly attached to the aircraft, has its origin at the projection center, also called pinhole. -axis lies on the optical axis, while orientations of x are consistent with u 、 v . One point is projected onto the image plane with coordinates and depth which is the distance between optical center and . We consider a pin-hole camera model, so the corresponding camera coordinates are

3 1 l l 、 l2 c 3 2 P P 、 L c cy x w w w wx y z o w x 2 3 P P i k w z w y c y v oc z c z c、 ) , i i v i P ( i u p Pi ( , , ) i i i i i i i i c i k_mu k_mv k_mf P = (1)

where intermediate variable 2 2 2

i i

i f u v

m = + + .

According to the imaging theory [7], the projection of the pair of parallel lines will intersect at called vanishing point, and besides .

3 1 l l 、 ) , (up vp p ocp//l1//l3

From the above analysis, three geometry constraints can be obtained as followed: (1) l₁_⊥l₂，₍₂₎ P2P3 =L，（3）l1// l3

(3)

c o c y c z c x w o w y _z_w w x 1 l 2 l 3 l 2 P 3 P 1 P 4 P p 3 p p₂ 4 p 1 p

Fig.1 imaging of runway

3. The pose estimation in the earlier stage

It is assumed that the pose of camera relative to UAV is known, so we do nothing but seeking the pose of camera with respect to the runway. This part is based upon the threshold line visible.

Firstly, after the depth of are inferred from the three constraints, we can get the camera coordinates and the camera vectors of the three lines. Secondly, the attitudes of camera relative to runway are determinated by the three corresponding lines. Lastly, the coplanar equation is used for acquiring position of camera.

3 2 P P、 c c _P P2、 3 3.1. Calculation of depth

The vector lies withl2 P2c、P3c : ( , , )

3 3 2 2 3 3 3 2 2 2 3 3 3 2 2 2 2 k_mu k_mu k_mv k_mv k_mf k_mf l = − − − . l1 is parallel with ) , , (u v f p

oc p p . From constraint (1), we can obtain:

( ) ( ) ( ) 0 3 3 3 2 2 2 3 3 3 2 2 2 3 3 2 2 ₋ _⋅ ₊ ₋ ₊ ₋ ₌ p v m v k m v k u m u k m u k f f m k m k (2)

The relationship between and is expressed as followed: k2 k3

(3) 3 2 sk k = where p p p p v v u u f v v u u f m m s 3 3 2 2 2 2 2 3 + + + + = .

Constraint (2) decides the following equation:

2 2 2 3 2 3 2 3 3 2 2 2 2 2 2 2 2 2 3 2 3 2 3 3 2 2 2 2 2 2 2 2 2 3 2 3 2 3 2 2 2 2 2 2 ) ( ) ( ) ( L v u f v k v u f v k v u f u k v u f u k v u f f k v u f f k = + + − + + + + + − + + + + + − + + (4)

Substituting (3) into (4), we get an quadratic equation about , and the solution about can be solved. is always in front of the camera, so :

2 k 2 2 k 2 P k2 >0

(4)

2 3 3 2 2 2 3 3 2 2 2 2 3 2 2 2 ) ( ) ( ) 1 ( m s m f u m su m v m sv m L k − + − + − = (5)

3.2. The estimation of camera attitudes

According to the last section, after is computed, the camera coordinates and the camera vector in the camera frame corresponding to can be determined. The vector is equal to

2 k c i P c l1 c l2 l2 ocp. The

orthonormal basis in the camera frame is ( , )

2 2 1 1 c c c c l l l l

A = , with its homologous basis ( , )

2 2 1 1 l l l l B = in the

runway frame. The rotation matrix R is decided by equation RB=A, which can be changed into the following least-squares problem:

min A −RB 2, subject to

(6) 3

I R

RT ₌

The constrained least-squares problem [8] can be solved by Umeyama [9]. The Umeyama solution proceeds as followed: Let _UDVTbe the singular value decomposition of _A_BT ₍_UUT ₌_VVT ₌_I_,_D₌

) 0 ),

(d d1≥d2≥d3 ≥

diag i . When rank(ABT) ≥2, the rotation matrix can be determined.

(7) T USV R = where (8) ⎩ ⎨ ⎧ < − ≥ = 0 ) det( ), 1 , 1, 1 ( 0 ) det( , 3 T T AB diag AB I S

When rank (_A_BT₎₌₂_,_S_{must be chosen as}

(9)⎩ ⎨ ⎧ − = − = = 1 ) det( ) det( ), 1 ,1 ,1 ( 1 ) det( ) det( , 3 V U diag V U I S

3.3. The estimation of camera position

While the rotation matrix is acquired, usually we can compute the translation vector T according to the basic perspective projection equation . The robust of this method is poor, because only one point is used. The extraction of line is steadier than point. Xiaohu [10] gets the translation vector according to coplanar characteristic.

T RP Pic= i+

As shown in Fig.2, 3-D line 、 its corresponding image plane line and the optical center lie in the same plane what is called interpretation plane. li l′i

(5)

c o c z yc c x i D _P_i i s i m i d w o w y w x w z i l i l′ ) , ( TR

Fig.2 imaging of line coplanarity

Define coplanar parameter T

i i T i i i mm K =rsrs + r r , where

{

i mi

}

r r , s i l

is an orthonormal basis of the 2-dimentional interpretation plane. So a point located on line can decide the coplanar equation: Pi

(10) ) (RP T K T RPi+ = i i+

An arbitrary point is chosen on each line when we gain the three lines on the runway, so the translation vector, the camera coordinates of the origin of runway frame, is calculated by

i i i i i K I RP K I T

∑

= − = − − = 3 1 3 1 3 1 3 )) ( ) ( ( (11)

Then the position of the camera in the runway frame can be estimated by equation (12) as followed:

T_′₌₋R−1T (12)

4. The pose estimation in the later stage

When UAV is very close to runway, the cross threshold line runs out of the image plane, so the above three-line algorithm is invalidated.

The rotationR =R(γ)R(θ)R(ψ), where R(γ) is roll matrix, R(θ) is pitch matrix and R(ψ)is yaw matrix. So the relationship between the unit vector

1 1

l

l _{in the runway and its corresponding vector} p o p o c c

can be expressed as followed:

1 1 ) ( ) ( ) ( l l R R R p o p o c c ₌ _γ _θ _ψ (13)

where the roll angle is acquired from IMU, therefore the yaw angle and the pitch angle can be offered by equation (13).

When UAV near the runway, the longitudinal distance will not affect the landing as the runway is very long, so we can assume that the longitudinal distance of UAV is zero. The lateral position and altitude can be determined by the two edges [3].

(6)

The initial condition of the simulation is as followed: The width of runway is 30m. The focal length of camera is 12.5mm with the size of the pixel is 9.9um×9.9um and the size of the image plane is 658×494. UAV slides along 3°gliding angle standing to the rate of 40m/s with the initial position (0,-60,-1000) relative to the runway frame. When the threshold line is invisible, the roll angle is assumed to be zero. In the process of simulation, the edges and the threshold line have been already detected with Gaussian noise of 2 pixel variance.

In the earlier stage of landing, small pitch angle of camera relative to runway cannot affect pose estimation. But with the approach to runway, the image plane coordinates of the vanishing point is larger and larger, and especially when the terminal guidance, the coordinates approach infinity, which disturbs pose estimation. Increasing the pitch angle of camera with respect to runway is the resolution of the above trouble, so we make the pitch being 3°when the threshold line visible, and being 10°when invisible.

As shown in Fig.3 and Fig.4, all errors are confined in the limited bounds, which validate the correctness of the algorithm proposed. In the earlier stage, the yaw and the pitch vary with the error of 0.1 degree, and the roll with 0.8 degree. The cross error and the altitude error are both less than 0.5m. Because of the long runway, the longitudinal error less than 10m is inessential. Besides, we can sun up from Fig.3: Shorter the distance to runway, smaller all the errors being.

As shown in Fig.4 about the errors in the later stage, the pitch and the yaw is less than 0.1°. The cross direction error is less than 0.2m, and so was the altitude.

Through the above analysis, the method we put forward is provided with high accuracy. As for operating time, in the earlier stage, dealing with each frame costs 1.3ms averagely, and 0.2ms in the later stage, so our method can operate in real-time.

6. Conclusion

In this paper, a vision-based algorithm is presented. In the earlier stage, the cross threshold line and two edge lines are used to estimate the attitudes and position of UAV. When the UAV is very close to runway and the threshold line is invisible, the yaw、the pitch、the cross position and the altitude can be calculated in the condition of roll angle being offered by IMU. The simulation results show that the method proposed can operate in real-time with high accuracy.

(7)

Fig.4 Errors of pose in the later stage

Acknowledgements

Sincere thanks to Shen Ning and Yin Gao-yang, who provide support to the research.

References

[1] Ivan F Mondragon, Pascual Campoy, Carol Martinez. 3D pose estimation based on planar object tracking for UAVs control. IEEE International Conference on Robotics and Automation Anchorage Convention District，2010：35-41

[2] Andrew Miller, Mubarak Shah, Don Harper. Landing a UAV on a Runway using image registration. IEEE International Conference on Robitics and Automation. Pasadena,2008:182-187

[3] Sara S, Gomi H, Ninomiya T, et al.. Position and attitude estimation using image processing of runway. AIAA Aerospace Sciences Meeting and Exhibit. Reno,NV.2000:1-10

[4] Tianshu Liu, Gary Fleming. Videgrammetric Determination of Aircraft Position and Attitude for Vision-Based Autonomous Landing. 44th AIAA Aerospace Sciences Meeting and Exhibit .Reno, Nevada.2006

[5] Qin Lijuan, Zhu Feng. Novel algorithm for real-time pose estimation from line coorespondences based on geometric constraints. Chinese Journal of Scientific Instrument, 2006, 27(6):2213-2252

[6] Qin Lijuan, Cao Dongzhi, Hu Yulan, et al.. Algorithm for Attitude Determination from Three Door-like Lines. IEEE International Conference on information and Automation.Zhangjiajie,China.2008：1359-1363

[7] Richard Hartley, Andrew Zisserman, Multiple View Geometry in Computer Vision. Cambridge University Press, England, 2003

[8] O.Faugeras, Three-Dimensional Computer Vision. MIT Press, 1993

[9] Shinji Umeyama. Least-Squares Estimation of Transformation Parameters Between Two Point Patterns. IEEE Transactions on Pattern Analysis and Machine Intelligence.1991，13（4）：376-380

[10] Xiaohu Zhang, Kunpeng Wang, Zheng Zhang et al.. A new line-based orthogonal iteration pose estimation algorithm. In Proceedings of International Conference on Information Engineering and Computer Science, Wuhan, 2009