# Quad-Rotor Helicopter Autonomous Navigation Based on Vanishing Point Algorithm

Authors: Jialiang Wang; Hai Zhao; Yuanguo Bi; Xingchi Chen; Ruofan Zeng; Shiliang Shao
Journal: Journal of Applied Mathematics
 (2014)

---

## Abstract

Quad-rotor helicopter is becoming popular increasingly as they can well implement many flight missions in more challenging environments, with lower risk of damaging itself and its surroundings. They are employed in many applications, from military operations to civilian tasks. Quad-rotor helicopter autonomous navigation based on the vanishing point fast estimation (VPFE) algorithm using clustering principle is implemented in this paper. For images collected by the camera of quad-rotor helicopter, the system executes the process of preprocessing of image, deleting noise interference, edge extracting using Canny operator, and extracting straight lines by randomized hough transformation (RHT) method. Then system obtains the position of vanishing point and regards it as destination point and finally controls the autonomous navigation of the quad-rotor helicopter by continuous modification according to the calculated navigation error. The experimental results show that the quad-rotor helicopter can implement the destination navigation well in the indoor environment.


---

## Body

## 1. Introduction

As things get further away from us, they seem smaller. When they get far enough away, distances become ever tinier and so form a single point called vanishing point. In one-point perspective, all the horizontal lines go straight across, while the lines that move away from us—the sides of boxes, the road we are on, or the railway lines in front of us—all converge towards the center of the picture [1].So the vanishing point contains much three-dimension information of the real environment, which can help to understand the real environment especially containing many parallel lines. The position estimation of vanishing point is widely used in applications such as navigation of robot, three-dimensional reconstruction, and camera calibration, and it has already become the hot research topic nowadays.In the recent years, quad-rotor helicopter received an increasing attention from the research community. Many authors focused on the autonomous navigation control of them. Hoffmann et al. [2] presented a model-based algorithm for autonomous flying with their STARMAC-quad rotor. Their system flied outdoors and utilized GPS and inertial measurement. Achtelika et al. [3] developed an indoor autonomous quad rotor equipped with a laser range scanner and cameras enabling autonomous hovering in a constraint indoor environment. Kendoul et al. [4] developed vision system for autonomous navigation 3D localization and control of small aerial vehicles based on optic flow.Vanishing point is very important because it contains abundant three-dimension information for the indoor environment, and the innovation for this paper is that it focuses on the implementation of autonomous navigation using vanishing point algorithm and VPFE algorithm based on clustering principle in the indoor environment.This paper is organized as follows: Section2 presents system model of quad-rotor helicopter and its physical parameters. Section 3 describes the principle of vanishing point algorithm. Section 4 presents the implementation process of proposed VPEE algorithm in detail. Section 5 describes the navigation based on improved PID control. Section 6 presents experimental results in the indoor environment in terms of execution time and flight deviation. Conclusions are given at the end of the paper.


## 2. System Model of Quad-Rotor Helicopter

The quad rotor has the flight motions such as vertical take-off and landing, pitching, rolling, and yawing, which are implemented by motor speed control so as to make system generate different force on each propeller.(
O
g
,
X
g
,
Y
g
,
Z
g
) is the ground coordinate, and (O,X,Y,Z) is the quad-rotor helicopter coordinate as Figure 1 shows. Quad-rotor helicopter coordinate is used to control the state of motion, while ground coordinate is used to adjust the position and posture of the quad-rotor helicopter. From the analysis about transformation of above two coordinates, we can get the transformation equation as formula (1) shows: (1)[XgYgZg]=(cos⁡ψsinψ0-sinψcos⁡ψ0001)·(1000sinφcos⁡φ0-cos⁡φsinφ)·(-sinθcos⁡θ0-cos⁡θ-sinθ0001)·[XYZ]=(-cos⁡ψsinθ-sinψsinφcos⁡θcos⁡ψcos⁡θ-sinψsinθsinφsinψcos⁡φsinψsinθ-cos⁡ψsinφcos⁡θ-sinψcos⁡θ-cos⁡ψsinθsinφcos⁡ψcos⁡φcos⁡θcos⁡φcos⁡φsinθsinφ)·[XYZ],Figure 1
Transformation relationship between quad-rotor helicopter and ground coordinate.Where straight linesL1, L2, and L3 represent the parallel lines of OX, OY, and OgXg separately. Lines O′P1 and O′P2 are the projection lines while lines OX and OY mapped into the ground coordinate. θ, φ, and ψ mean included angles as Figure 1 shows.Besides, for the whole quad-rotor helicopter, the input variable is rotate speed of 4 electric motors [5], and they can be expressed as
(2)u1=F1+F2+F3+F4,u2=F4-F2,u3=F3-F1,u4=F1-F2+F3-F4,
where u1, u2, u3, and u4 represent the lifting force, rolling force, pitching force, and drifting force, respectively. In the meanwhile, the input variable contains 3 other pieces of poseinformation of θ, φ, and ψ for the whole system [6].


## 3. Principle of Vanishing Point Algorithm

### 3.1. Model of Vanishing Point

A group of parallel lines in three-dimension (3D) space can be mapped into some intersection lines in two-dimension (2D) image, and the intersection point formed by these intersection lines is called vanishing point [7].Figure2 shows the mapping relationship between point A(x0,y0,z0) in the three-dimension space and point B(x0′,y0′,z0′) in the two-dimension image [8], and we can obtain
(3)(x0′,y0′,z0′)=(x0fy0,z0fy0,f),
where f represents focal length of visual system, and point p(x0,y0,z0) can also be expressed as the sum of several vectors in the directions of x, y, and z coordinate axes [9]. Using the attributes of i, j, and k to express the base vector of the three axes, in the meantime vector p¯ can be expressed as p¯=x0i¯+y0j¯+z0k¯, so the vanishing point v can be calculated by
(4)v¯=xvi¯+yvj¯+fk¯.Figure 2
A point in 3D space mapped into 2D image.The indoor flight environment for quad-rotor helicopter navigation is shown as Figure3(a), and the vanishing point can be obtained by the above calculation, and then it is set as the destination point navigation for quad-rotor helicopter in the indoor environment.Edge detection of indoor flight environment.
(a)
Image of indoor environment
(b)
Execution result of canny algorithm


### 3.2. Straight Line Extraction of Destination Region

Detecting the parallel lines existing in the test environment is the premise for calculating the vanishing point, so this procedure is very important. It contains the below process: preprocessing of image, deleting noise interference, edge extracting using Canny operator, and extracting lines by RHT method.


#### 3.2.1. Preprocessing of Image

While using histogram equalization method to improve contrast of overall situation, the luminance information can well be reflected in the histogram, and it is also effective for foreground and background no matter it is too bright or too dark. The most important thing is that histogram equalization is a reversible operation, which means only knowing the function expression of equalization, and then system can recover the original histogram with lower computational complexity.


#### 3.2.2. Canny Operator

Canny operator is the optimization approximation operator obtained by measuring signal to noise ratio (SNR) and location product. Its principle in nature is to execute smooth image operation by using Gaussian function and calculating max derivative through first order differential operator. The process of edge detection is used to delete much useless information. In fact, it is used to detect points having obvious brightness variation, because these points include much information such as discontinuous depth and surface direction, change of material attribute, and scene illumination. Canny algorithm includes the below 4 procedures.(
1) Gaussian Filter Denoising. Gaussian blur is a filter for image penumbra, and it calculates the conversion for each pixel in the image by using below normal distribution:
(5)G(r)=12πσ2Ne-r2/2σ2.In the two-dimensional space, it can be expressed as(6)G(u,v)=12πσ2e-(u2+v2)/2σ2,
where r is the blur radius which can be calculated by r2=u2+v2; besides σ is the standard deviation of the above normal distribution, and it is used to control the smoothness of Gaussian filter parameters.System executes convolution operation for original image and Gaussian smoothing template and then obtains an image having the slightest obscure. So image noise is reduced and the level of detail is also decreased after this process.(
2) Gradient Operator Sobel. The edges in image may point to different directions, so the Canny algorithm uses 4 masks to detect the margin of horizontal verticality and diagonals lines. The convolution value for original image and any mask is calculated and stored.The first derivative of one two-dimension equation can be regarded as the gradient, and its vector can be expressed as(7)L¯(x,y)=[L¯xL¯y]=[∂f/∂x∂f/∂y].The direction of vectorL¯(x,y) stands for the direction having max changing rate, and the modulus of the gradient can be calculated by
(8)|L¯(x,y)|=L¯x2+L¯y2.The direction of the gradient is(9)δ(x,y)=arctan(Ly¯Lx¯),
where δ is the included angle formed by gradient and x-axis.So the sample gradient model can be calculated by(10)L¯x=f[i,j+1]-f[i,j],Ly¯=f[i,j]-f[i+1,j],
where i and j stand for the base vectors about y-axis negative direction and x-axis positive direction, respectively.Canny edge detection function adopts a kind of gradient operator Sobel. Each pixel in the image uses this operator, and then the corresponding gradient vector or normal vector will be generated. The operator contains two three-dimensional matrices, and the modulus value of gradient can be expressed as(11)H=Lx2+Ly2.Partial derivative can be calculated by(12)Lx=(β·n3+n2+n4)-(α·n7+n0+n6),Ly=(β·n1+n0+n2)-(α·n5+n4+n6),
where the constant values α=2 and β=2 and then the above Lx and Ly can be expressed as the below convolution template:
(13)Lx=|-101-202-101|,Ly=|121000-1-2-1|.
Neighbourhood pixel point can be expressed as
(14)Lnb=|m0m1m2m7[i,j]m3m6m5m4|.
So the emphasis on calculating Sobel operator is to obtain the central pixel in the area whose size is similar to the template.(
3) Control of Gradient Value. The system compares the gray level and direction of current pixel with the pixel gradient value whose position is two pixels prior to current pixel, if it is smaller than the pixel gradient value of prior pixel, and then it would be set as 0, which means it is not an edge.(
4) Partition of Lag Threshold. The system could obtain the point whose gradient is changing by the above principle, but for those having high luminance gradient, there would exist some interference factors to judge whether they are edges, so a threshold is introduced to solve this problem. To be exact, Canny uses two lag thresholds of high and low threshold. If the important edge is continuous curve in the image, then the system can track the obscure part of the given curve and avoid to regard these noise pixels which actually do not form curve as edges.The system begins this process with a large threshold, firstly marks these luminance gradients having high edge probability, and then tracks them in the whole image by the calculated direction information. For the process of edge tracking, the system uses the low threshold to control tracking path and makes the tracking path finally return the beginning position along with the obscure part.Figure3(a) shows the indoor environment captured by the front camera of quad-rotor helicopter. System stores the detecting results in the form of binary image, and Figure 3(b) is the edge effect picture using the above edge detecting process. Experimental results show the basic edge information can be successfully extracted.


#### 3.2.3. Randomized Hough Transformation (RHT) Method Used to Extract Straight Lines

The main idea used to detect straight lines is to establish a mapping between image coordinate space and parameter coordinate space. Many pixels in the image space are mapped into the parameter space, if these pixels are mapped into the same point in the parameter coordinate, and then the counter will be increased by 1. The point’s coordinate having largest counter value is corresponding to a straight line in the image space.As Figure4 shows, for any three points P1, P2, and P3 in the straight line Y=a0x+b0 in the image space, they will form three sinusoidal curves L1, L2, and L3 while they are mapped into the parameter space, and the intersection point P0 of the three curve is corresponding to the straight line L0 in the image space.Figure 4
The mapping relation for a line in the image space and parameter space.This paper uses improved randomized Hough transformation for the quad-rotor helicopter by using randomized sampling pixels in the image space and mapping them into parameter space. System firstly randomly picks two orn pixels and maps them into one point in the parameter space, then uses a set P with each element containing both a real-valued vector and an integer score to represent the parameter space, and updates the set P at each step by the point mapped from the randomly picked pixels. Finally, system finds accumulated cells of the detected curves [10].RHT method uses many-to-one mapping so as to avoid huge amount of calculations in the traditional Hough transformation (HT) method using one-to-many mapping.The system uses dynamic list structure and allocates corresponding memory space to these parameters only accumulated by the many-to-one mapping. The experimental result about using RHT method to extract straight lines is shown as Figure5.Figure 5
Straight lines extraction.The interfering accumulations caused by disturbance pixels can be significantly improved in RTH method. For Figure5, HT method uses a 320*240 array while RHT method uses the storage of only around 36 cells. Besides, the computing time of HT method is about 2.4 times larger than that of RHT method.Consequently, RTH method has the advantages of infinite parameter apace, small storage, and high computation speed, which can better satisfy the real-time control of the quad-rotor helicopter.


## 3.1. Model of Vanishing Point

A group of parallel lines in three-dimension (3D) space can be mapped into some intersection lines in two-dimension (2D) image, and the intersection point formed by these intersection lines is called vanishing point [7].Figure2 shows the mapping relationship between point A(x0,y0,z0) in the three-dimension space and point B(x0′,y0′,z0′) in the two-dimension image [8], and we can obtain
(3)(x0′,y0′,z0′)=(x0fy0,z0fy0,f),
where f represents focal length of visual system, and point p(x0,y0,z0) can also be expressed as the sum of several vectors in the directions of x, y, and z coordinate axes [9]. Using the attributes of i, j, and k to express the base vector of the three axes, in the meantime vector p¯ can be expressed as p¯=x0i¯+y0j¯+z0k¯, so the vanishing point v can be calculated by
(4)v¯=xvi¯+yvj¯+fk¯.Figure 2
A point in 3D space mapped into 2D image.The indoor flight environment for quad-rotor helicopter navigation is shown as Figure3(a), and the vanishing point can be obtained by the above calculation, and then it is set as the destination point navigation for quad-rotor helicopter in the indoor environment.Edge detection of indoor flight environment.
(a)
Image of indoor environment
(b)
Execution result of canny algorithm


## 3.2. Straight Line Extraction of Destination Region

Detecting the parallel lines existing in the test environment is the premise for calculating the vanishing point, so this procedure is very important. It contains the below process: preprocessing of image, deleting noise interference, edge extracting using Canny operator, and extracting lines by RHT method.


### 3.2.1. Preprocessing of Image

While using histogram equalization method to improve contrast of overall situation, the luminance information can well be reflected in the histogram, and it is also effective for foreground and background no matter it is too bright or too dark. The most important thing is that histogram equalization is a reversible operation, which means only knowing the function expression of equalization, and then system can recover the original histogram with lower computational complexity.


### 3.2.2. Canny Operator

Canny operator is the optimization approximation operator obtained by measuring signal to noise ratio (SNR) and location product. Its principle in nature is to execute smooth image operation by using Gaussian function and calculating max derivative through first order differential operator. The process of edge detection is used to delete much useless information. In fact, it is used to detect points having obvious brightness variation, because these points include much information such as discontinuous depth and surface direction, change of material attribute, and scene illumination. Canny algorithm includes the below 4 procedures.(
1) Gaussian Filter Denoising. Gaussian blur is a filter for image penumbra, and it calculates the conversion for each pixel in the image by using below normal distribution:
(5)G(r)=12πσ2Ne-r2/2σ2.In the two-dimensional space, it can be expressed as(6)G(u,v)=12πσ2e-(u2+v2)/2σ2,
where r is the blur radius which can be calculated by r2=u2+v2; besides σ is the standard deviation of the above normal distribution, and it is used to control the smoothness of Gaussian filter parameters.System executes convolution operation for original image and Gaussian smoothing template and then obtains an image having the slightest obscure. So image noise is reduced and the level of detail is also decreased after this process.(
2) Gradient Operator Sobel. The edges in image may point to different directions, so the Canny algorithm uses 4 masks to detect the margin of horizontal verticality and diagonals lines. The convolution value for original image and any mask is calculated and stored.The first derivative of one two-dimension equation can be regarded as the gradient, and its vector can be expressed as(7)L¯(x,y)=[L¯xL¯y]=[∂f/∂x∂f/∂y].The direction of vectorL¯(x,y) stands for the direction having max changing rate, and the modulus of the gradient can be calculated by
(8)|L¯(x,y)|=L¯x2+L¯y2.The direction of the gradient is(9)δ(x,y)=arctan(Ly¯Lx¯),
where δ is the included angle formed by gradient and x-axis.So the sample gradient model can be calculated by(10)L¯x=f[i,j+1]-f[i,j],Ly¯=f[i,j]-f[i+1,j],
where i and j stand for the base vectors about y-axis negative direction and x-axis positive direction, respectively.Canny edge detection function adopts a kind of gradient operator Sobel. Each pixel in the image uses this operator, and then the corresponding gradient vector or normal vector will be generated. The operator contains two three-dimensional matrices, and the modulus value of gradient can be expressed as(11)H=Lx2+Ly2.Partial derivative can be calculated by(12)Lx=(β·n3+n2+n4)-(α·n7+n0+n6),Ly=(β·n1+n0+n2)-(α·n5+n4+n6),
where the constant values α=2 and β=2 and then the above Lx and Ly can be expressed as the below convolution template:
(13)Lx=|-101-202-101|,Ly=|121000-1-2-1|.
Neighbourhood pixel point can be expressed as
(14)Lnb=|m0m1m2m7[i,j]m3m6m5m4|.
So the emphasis on calculating Sobel operator is to obtain the central pixel in the area whose size is similar to the template.(
3) Control of Gradient Value. The system compares the gray level and direction of current pixel with the pixel gradient value whose position is two pixels prior to current pixel, if it is smaller than the pixel gradient value of prior pixel, and then it would be set as 0, which means it is not an edge.(
4) Partition of Lag Threshold. The system could obtain the point whose gradient is changing by the above principle, but for those having high luminance gradient, there would exist some interference factors to judge whether they are edges, so a threshold is introduced to solve this problem. To be exact, Canny uses two lag thresholds of high and low threshold. If the important edge is continuous curve in the image, then the system can track the obscure part of the given curve and avoid to regard these noise pixels which actually do not form curve as edges.The system begins this process with a large threshold, firstly marks these luminance gradients having high edge probability, and then tracks them in the whole image by the calculated direction information. For the process of edge tracking, the system uses the low threshold to control tracking path and makes the tracking path finally return the beginning position along with the obscure part.Figure3(a) shows the indoor environment captured by the front camera of quad-rotor helicopter. System stores the detecting results in the form of binary image, and Figure 3(b) is the edge effect picture using the above edge detecting process. Experimental results show the basic edge information can be successfully extracted.


### 3.2.3. Randomized Hough Transformation (RHT) Method Used to Extract Straight Lines

The main idea used to detect straight lines is to establish a mapping between image coordinate space and parameter coordinate space. Many pixels in the image space are mapped into the parameter space, if these pixels are mapped into the same point in the parameter coordinate, and then the counter will be increased by 1. The point’s coordinate having largest counter value is corresponding to a straight line in the image space.As Figure4 shows, for any three points P1, P2, and P3 in the straight line Y=a0x+b0 in the image space, they will form three sinusoidal curves L1, L2, and L3 while they are mapped into the parameter space, and the intersection point P0 of the three curve is corresponding to the straight line L0 in the image space.Figure 4
The mapping relation for a line in the image space and parameter space.This paper uses improved randomized Hough transformation for the quad-rotor helicopter by using randomized sampling pixels in the image space and mapping them into parameter space. System firstly randomly picks two orn pixels and maps them into one point in the parameter space, then uses a set P with each element containing both a real-valued vector and an integer score to represent the parameter space, and updates the set P at each step by the point mapped from the randomly picked pixels. Finally, system finds accumulated cells of the detected curves [10].RHT method uses many-to-one mapping so as to avoid huge amount of calculations in the traditional Hough transformation (HT) method using one-to-many mapping.The system uses dynamic list structure and allocates corresponding memory space to these parameters only accumulated by the many-to-one mapping. The experimental result about using RHT method to extract straight lines is shown as Figure5.Figure 5
Straight lines extraction.The interfering accumulations caused by disturbance pixels can be significantly improved in RTH method. For Figure5, HT method uses a 320*240 array while RHT method uses the storage of only around 36 cells. Besides, the computing time of HT method is about 2.4 times larger than that of RHT method.Consequently, RTH method has the advantages of infinite parameter apace, small storage, and high computation speed, which can better satisfy the real-time control of the quad-rotor helicopter.


## 3.2.1. Preprocessing of Image

While using histogram equalization method to improve contrast of overall situation, the luminance information can well be reflected in the histogram, and it is also effective for foreground and background no matter it is too bright or too dark. The most important thing is that histogram equalization is a reversible operation, which means only knowing the function expression of equalization, and then system can recover the original histogram with lower computational complexity.


## 3.2.2. Canny Operator

Canny operator is the optimization approximation operator obtained by measuring signal to noise ratio (SNR) and location product. Its principle in nature is to execute smooth image operation by using Gaussian function and calculating max derivative through first order differential operator. The process of edge detection is used to delete much useless information. In fact, it is used to detect points having obvious brightness variation, because these points include much information such as discontinuous depth and surface direction, change of material attribute, and scene illumination. Canny algorithm includes the below 4 procedures.(
1) Gaussian Filter Denoising. Gaussian blur is a filter for image penumbra, and it calculates the conversion for each pixel in the image by using below normal distribution:
(5)G(r)=12πσ2Ne-r2/2σ2.In the two-dimensional space, it can be expressed as(6)G(u,v)=12πσ2e-(u2+v2)/2σ2,
where r is the blur radius which can be calculated by r2=u2+v2; besides σ is the standard deviation of the above normal distribution, and it is used to control the smoothness of Gaussian filter parameters.System executes convolution operation for original image and Gaussian smoothing template and then obtains an image having the slightest obscure. So image noise is reduced and the level of detail is also decreased after this process.(
2) Gradient Operator Sobel. The edges in image may point to different directions, so the Canny algorithm uses 4 masks to detect the margin of horizontal verticality and diagonals lines. The convolution value for original image and any mask is calculated and stored.The first derivative of one two-dimension equation can be regarded as the gradient, and its vector can be expressed as(7)L¯(x,y)=[L¯xL¯y]=[∂f/∂x∂f/∂y].The direction of vectorL¯(x,y) stands for the direction having max changing rate, and the modulus of the gradient can be calculated by
(8)|L¯(x,y)|=L¯x2+L¯y2.The direction of the gradient is(9)δ(x,y)=arctan(Ly¯Lx¯),
where δ is the included angle formed by gradient and x-axis.So the sample gradient model can be calculated by(10)L¯x=f[i,j+1]-f[i,j],Ly¯=f[i,j]-f[i+1,j],
where i and j stand for the base vectors about y-axis negative direction and x-axis positive direction, respectively.Canny edge detection function adopts a kind of gradient operator Sobel. Each pixel in the image uses this operator, and then the corresponding gradient vector or normal vector will be generated. The operator contains two three-dimensional matrices, and the modulus value of gradient can be expressed as(11)H=Lx2+Ly2.Partial derivative can be calculated by(12)Lx=(β·n3+n2+n4)-(α·n7+n0+n6),Ly=(β·n1+n0+n2)-(α·n5+n4+n6),
where the constant values α=2 and β=2 and then the above Lx and Ly can be expressed as the below convolution template:
(13)Lx=|-101-202-101|,Ly=|121000-1-2-1|.
Neighbourhood pixel point can be expressed as
(14)Lnb=|m0m1m2m7[i,j]m3m6m5m4|.
So the emphasis on calculating Sobel operator is to obtain the central pixel in the area whose size is similar to the template.(
3) Control of Gradient Value. The system compares the gray level and direction of current pixel with the pixel gradient value whose position is two pixels prior to current pixel, if it is smaller than the pixel gradient value of prior pixel, and then it would be set as 0, which means it is not an edge.(
4) Partition of Lag Threshold. The system could obtain the point whose gradient is changing by the above principle, but for those having high luminance gradient, there would exist some interference factors to judge whether they are edges, so a threshold is introduced to solve this problem. To be exact, Canny uses two lag thresholds of high and low threshold. If the important edge is continuous curve in the image, then the system can track the obscure part of the given curve and avoid to regard these noise pixels which actually do not form curve as edges.The system begins this process with a large threshold, firstly marks these luminance gradients having high edge probability, and then tracks them in the whole image by the calculated direction information. For the process of edge tracking, the system uses the low threshold to control tracking path and makes the tracking path finally return the beginning position along with the obscure part.Figure3(a) shows the indoor environment captured by the front camera of quad-rotor helicopter. System stores the detecting results in the form of binary image, and Figure 3(b) is the edge effect picture using the above edge detecting process. Experimental results show the basic edge information can be successfully extracted.


## 3.2.3. Randomized Hough Transformation (RHT) Method Used to Extract Straight Lines

The main idea used to detect straight lines is to establish a mapping between image coordinate space and parameter coordinate space. Many pixels in the image space are mapped into the parameter space, if these pixels are mapped into the same point in the parameter coordinate, and then the counter will be increased by 1. The point’s coordinate having largest counter value is corresponding to a straight line in the image space.As Figure4 shows, for any three points P1, P2, and P3 in the straight line Y=a0x+b0 in the image space, they will form three sinusoidal curves L1, L2, and L3 while they are mapped into the parameter space, and the intersection point P0 of the three curve is corresponding to the straight line L0 in the image space.Figure 4
The mapping relation for a line in the image space and parameter space.This paper uses improved randomized Hough transformation for the quad-rotor helicopter by using randomized sampling pixels in the image space and mapping them into parameter space. System firstly randomly picks two orn pixels and maps them into one point in the parameter space, then uses a set P with each element containing both a real-valued vector and an integer score to represent the parameter space, and updates the set P at each step by the point mapped from the randomly picked pixels. Finally, system finds accumulated cells of the detected curves [10].RHT method uses many-to-one mapping so as to avoid huge amount of calculations in the traditional Hough transformation (HT) method using one-to-many mapping.The system uses dynamic list structure and allocates corresponding memory space to these parameters only accumulated by the many-to-one mapping. The experimental result about using RHT method to extract straight lines is shown as Figure5.Figure 5
Straight lines extraction.The interfering accumulations caused by disturbance pixels can be significantly improved in RTH method. For Figure5, HT method uses a 320*240 array while RHT method uses the storage of only around 36 cells. Besides, the computing time of HT method is about 2.4 times larger than that of RHT method.Consequently, RTH method has the advantages of infinite parameter apace, small storage, and high computation speed, which can better satisfy the real-time control of the quad-rotor helicopter.


## 4. Implementation of Vanishing Point Algorithm

Vanishing point is obtained by evaluating the straight lines extracted from collected image. As in the indoor environment, there inevitably existed some disturbing lines, so the emphasis on this vanishing point algorithm is to reduce the effect of error component and estimate the exact position of vanishing point.


### 4.1. The Vanishing Point Algorithm

(
1) Deleting Unreasonable Straight Line. There is a straight line set as C{l0,l1,…,lk∣k=0,…,M}, where M is the number of straight lines system that has been detected and k is integer. Each line is expressed by two endpoints {(xs0,ys0),(xe0,ye0)},{(xs1,ys1),(xe1,ye1)},…,{(xsM,ysM),(xeM,yeM)}.For endpoints set asCp, the angle between each line and horizontal coordinate axis can be calculated by
(15)deg⁡=arctan|y1-y0||x1-x0|.There are some straight lines in the indoor scenario such as the edges of ceilings, the edges of floors, etc., and these lines are disturbing lines in the calculation of the vanishing point. Therefore, the removal of these lines is essential to accurately calculate the vanishing point and alleviate the error of the algorithm.Unreasonable lines in the image are deleted by means of judging the deg angle. As OpenCV library function is used by this system, so the value ranges from 0 to 360 degrees after transformation. Judging about whether a line is close to horizontal or vertical straight lines depends on(16)f(deg⁡)=1,VT>|90-deg⁡|,VT>|270-deg⁡|,HT>|deg⁡|,HT>|180-deg⁡|,
where VT (vertical line threshold) and HT (horizontal line threshold), respectively, represent the vertical and horizontal angle thresholds used to delete undesired lines.In order to obtain the optimized value of VT and HT, we do the below test experiments so as to make the coverage rate to achieve the max value. Supposing the error radius isr for the vanishing point, so these straight lines will generate some points both inside and outside the circular error of vanishing point. The coverage rate is defined as Ii/(Ii+Ok) used to mean the ratio of points inside the circular error of vanishing point accounts for the percentage of total points, where i and k are both positive integers.As Figure6(a) shows, there exist many intersections caused by straight lines, and these intersections are enlarged in order to be seen clearly; for example I1 and I2 (inside the circular error of vanishing point) and O1, O2, O3, and O4 (outside the circular error of vanishing point) consist in straight line L1. If L1 is deleted, then these six points are all deleted, which will affect the coverage rate. So using the threshold of horizontal line and vertical line is necessary so as to reduce the calculation work and improve coverage rate.Error radius of vanishing point and coverage rate affected by the threshold VT and HT.
(a)
Illustration of coverage rate and error radius of vanishing point
(b)
Experimental results about coverage affected by the threshold VT and HTThe values of VT and HT are both integers so as to lower the calculation. While VT and HT are both 0, which means no straight lines are deleted in this case,VT+VH>90, which means all straight lines will be deleted. So choosing a suitable value to make the algorithm have the best accuracy is the key point. Figure 6(b) shows the test results about the relationship of VT and HT affecting the coverage rate. The corresponding value of VT and HT is 10 and 15, respectively, while the coverage rate reaches to its peak value.Figure6(b) shows the coverage rate affected by the thresholds of VT and HT. Because VT and HT both vary from 0 to 90 degrees, so there are total 810 image frames (while accuracy is 1 degree) in Figure 6(b). As the coverage rate will affect the outcome accuracy of vanishing point, which directly affects the flight deviation, the experiment of these 810 image frames well reflects the flight deviation according to the thresholds VT and HT while the number of images frames increase.For those lines makingf(deg⁡)=1 is undesirable, so those lines should be deleted, and they are marked as blue in Figure 5.(
2) Calculating the Intersection Point. After deleting undesirable lines, the number of lines in the line set is M, so the number of their intersections is N=M·(M-1)/2, and it is stored as the initial value in the intersection array.Any two straight lines in image can by expressed as(17)A1x+B1y+C1=0,A2x+B2y+C2=0.
Points A(x1,y1) and B(x2,y2) are on a straight line while points C(x3,y3) and D(x4,y4) are on another straight line. Then distance d between the two lines can be calculated by
(18)d=(x4-x3)·(y2-y1)-(x2-x1)·(y4-y3).
While d is 0, it means straight lines AB and CD form parallel line; otherwise, they are intersection lines, and then the coordinate of their intersection point can be calculated by
(19)x0=x1·(x4-x3)·(y2-y1)+(x2-x1)·(x4-x3)·(y3-y1)-x3d·(x2-x1)·(y4-y3),y0=y1·(x2-x1)·(y4-y3)+(x3-x1)·(y2-y1)·(y4-y3)+y3d·(x4-x3)·(y2-y1).Calculate all the intersections by the above equations, wheren is the number of all intersection points, and their coordinates are expressed as (x1,y1),(x2,y2),…,(xn,yn), respectively. Those intersection points whose coordinate does not exist in the range of (0,0) to (h,w) are undesirable ones, and they should be deleted, where h and w are the height and weight of the image.The system uses Canny algorithm to obtain the edge of the collected image and then adopts RHT method to extract straight lines. From the analysis of abundant practical flight experimental results, we can know that the successful rate about collecting elements of intersection point set can reach to above 92.5%.


### 4.2. Proposed VPFE Algorithm Based on Clustering Principle

As the bottleneck for quad-rotor helicopter navigation is the speed of image processing, it includes the calculation of every pixel in the image. In order to improve the real-time ability of visual navigation, it is important to eliminate some unnecessary calculations.A new vanishing point fast estimation (VPFE) algorithm is proposed to evaluate the vanishing points and obtain a navigation destination point finally. In fact, it is 1-mean cluster process. The system deals with all the points in the intersection set by the method of convergence. Specifically, the system firstly deletes those points whose distances to mass center are larger than cluster radius, and then adjusts value of the cluster radius by the method of convergence. The above procedure is executed repeatedly, and finally the mass center is obtained as the vanishing point. The total number of iterative procedure can be calculated by(20)R·pn<Re.Asn is the number of iterative process, the value is
(21)n=⌊log⁡ReR⌋+1,
where p(<1) is the convergence coefficient, R represents radius of convergence, and Re is a dynamical value that varies from each cluster process, which stands for the deviation of mass center about each cluster process; finally, it is the radius of convergence used to describe the region how much vanishing point will be located. In order to facilitate the calculation, an element amount threshold NT is preset.The procedure of algorithm is described as follows.Step 1.
Set the initialization values ofR and Re and calculate the n according to R and Re.Step 2.
The new radius of convergence isp·R.Step 3.
Calculate the mass center of the intersection points set asC and regard it as the vanishing point.Step 4.
For all the elements in the intersection point set asC, calculate the distance between intersection point and vanishing point.Step 5.
If the number of elements in the intersection points is smaller than NT or the amount of iterative process is larger thann, then go to Step 6; otherwise, return to Step 2.Step 6.
Output value of the mass center in the intersection point set is regarded as the position of the vanishing point.The verification result of the above algorithm is shown as Figure7. The green points are the undesirable ones who do not meet the requirement of threshold in the convergence process, and these points should be deleted. The red marker is the vanishing point obtained from the above VPFE algorithm, and the corresponding image in the indoor environment collected by the front camera of the quad-rotor helicopter is shown as Figure 8.Figure 7
Clustering processing of VPFE algorithm.Figure 8
Collected image of the indoor environment.


## 4.1. The Vanishing Point Algorithm

(
1) Deleting Unreasonable Straight Line. There is a straight line set as C{l0,l1,…,lk∣k=0,…,M}, where M is the number of straight lines system that has been detected and k is integer. Each line is expressed by two endpoints {(xs0,ys0),(xe0,ye0)},{(xs1,ys1),(xe1,ye1)},…,{(xsM,ysM),(xeM,yeM)}.For endpoints set asCp, the angle between each line and horizontal coordinate axis can be calculated by
(15)deg⁡=arctan|y1-y0||x1-x0|.There are some straight lines in the indoor scenario such as the edges of ceilings, the edges of floors, etc., and these lines are disturbing lines in the calculation of the vanishing point. Therefore, the removal of these lines is essential to accurately calculate the vanishing point and alleviate the error of the algorithm.Unreasonable lines in the image are deleted by means of judging the deg angle. As OpenCV library function is used by this system, so the value ranges from 0 to 360 degrees after transformation. Judging about whether a line is close to horizontal or vertical straight lines depends on(16)f(deg⁡)=1,VT>|90-deg⁡|,VT>|270-deg⁡|,HT>|deg⁡|,HT>|180-deg⁡|,
where VT (vertical line threshold) and HT (horizontal line threshold), respectively, represent the vertical and horizontal angle thresholds used to delete undesired lines.In order to obtain the optimized value of VT and HT, we do the below test experiments so as to make the coverage rate to achieve the max value. Supposing the error radius isr for the vanishing point, so these straight lines will generate some points both inside and outside the circular error of vanishing point. The coverage rate is defined as Ii/(Ii+Ok) used to mean the ratio of points inside the circular error of vanishing point accounts for the percentage of total points, where i and k are both positive integers.As Figure6(a) shows, there exist many intersections caused by straight lines, and these intersections are enlarged in order to be seen clearly; for example I1 and I2 (inside the circular error of vanishing point) and O1, O2, O3, and O4 (outside the circular error of vanishing point) consist in straight line L1. If L1 is deleted, then these six points are all deleted, which will affect the coverage rate. So using the threshold of horizontal line and vertical line is necessary so as to reduce the calculation work and improve coverage rate.Error radius of vanishing point and coverage rate affected by the threshold VT and HT.
(a)
Illustration of coverage rate and error radius of vanishing point
(b)
Experimental results about coverage affected by the threshold VT and HTThe values of VT and HT are both integers so as to lower the calculation. While VT and HT are both 0, which means no straight lines are deleted in this case,VT+VH>90, which means all straight lines will be deleted. So choosing a suitable value to make the algorithm have the best accuracy is the key point. Figure 6(b) shows the test results about the relationship of VT and HT affecting the coverage rate. The corresponding value of VT and HT is 10 and 15, respectively, while the coverage rate reaches to its peak value.Figure6(b) shows the coverage rate affected by the thresholds of VT and HT. Because VT and HT both vary from 0 to 90 degrees, so there are total 810 image frames (while accuracy is 1 degree) in Figure 6(b). As the coverage rate will affect the outcome accuracy of vanishing point, which directly affects the flight deviation, the experiment of these 810 image frames well reflects the flight deviation according to the thresholds VT and HT while the number of images frames increase.For those lines makingf(deg⁡)=1 is undesirable, so those lines should be deleted, and they are marked as blue in Figure 5.(
2) Calculating the Intersection Point. After deleting undesirable lines, the number of lines in the line set is M, so the number of their intersections is N=M·(M-1)/2, and it is stored as the initial value in the intersection array.Any two straight lines in image can by expressed as(17)A1x+B1y+C1=0,A2x+B2y+C2=0.
Points A(x1,y1) and B(x2,y2) are on a straight line while points C(x3,y3) and D(x4,y4) are on another straight line. Then distance d between the two lines can be calculated by
(18)d=(x4-x3)·(y2-y1)-(x2-x1)·(y4-y3).
While d is 0, it means straight lines AB and CD form parallel line; otherwise, they are intersection lines, and then the coordinate of their intersection point can be calculated by
(19)x0=x1·(x4-x3)·(y2-y1)+(x2-x1)·(x4-x3)·(y3-y1)-x3d·(x2-x1)·(y4-y3),y0=y1·(x2-x1)·(y4-y3)+(x3-x1)·(y2-y1)·(y4-y3)+y3d·(x4-x3)·(y2-y1).Calculate all the intersections by the above equations, wheren is the number of all intersection points, and their coordinates are expressed as (x1,y1),(x2,y2),…,(xn,yn), respectively. Those intersection points whose coordinate does not exist in the range of (0,0) to (h,w) are undesirable ones, and they should be deleted, where h and w are the height and weight of the image.The system uses Canny algorithm to obtain the edge of the collected image and then adopts RHT method to extract straight lines. From the analysis of abundant practical flight experimental results, we can know that the successful rate about collecting elements of intersection point set can reach to above 92.5%.


## 4.2. Proposed VPFE Algorithm Based on Clustering Principle

As the bottleneck for quad-rotor helicopter navigation is the speed of image processing, it includes the calculation of every pixel in the image. In order to improve the real-time ability of visual navigation, it is important to eliminate some unnecessary calculations.A new vanishing point fast estimation (VPFE) algorithm is proposed to evaluate the vanishing points and obtain a navigation destination point finally. In fact, it is 1-mean cluster process. The system deals with all the points in the intersection set by the method of convergence. Specifically, the system firstly deletes those points whose distances to mass center are larger than cluster radius, and then adjusts value of the cluster radius by the method of convergence. The above procedure is executed repeatedly, and finally the mass center is obtained as the vanishing point. The total number of iterative procedure can be calculated by(20)R·pn<Re.Asn is the number of iterative process, the value is
(21)n=⌊log⁡ReR⌋+1,
where p(<1) is the convergence coefficient, R represents radius of convergence, and Re is a dynamical value that varies from each cluster process, which stands for the deviation of mass center about each cluster process; finally, it is the radius of convergence used to describe the region how much vanishing point will be located. In order to facilitate the calculation, an element amount threshold NT is preset.The procedure of algorithm is described as follows.Step 1.
Set the initialization values ofR and Re and calculate the n according to R and Re.Step 2.
The new radius of convergence isp·R.Step 3.
Calculate the mass center of the intersection points set asC and regard it as the vanishing point.Step 4.
For all the elements in the intersection point set asC, calculate the distance between intersection point and vanishing point.Step 5.
If the number of elements in the intersection points is smaller than NT or the amount of iterative process is larger thann, then go to Step 6; otherwise, return to Step 2.Step 6.
Output value of the mass center in the intersection point set is regarded as the position of the vanishing point.The verification result of the above algorithm is shown as Figure7. The green points are the undesirable ones who do not meet the requirement of threshold in the convergence process, and these points should be deleted. The red marker is the vanishing point obtained from the above VPFE algorithm, and the corresponding image in the indoor environment collected by the front camera of the quad-rotor helicopter is shown as Figure 8.Figure 7
Clustering processing of VPFE algorithm.Figure 8
Collected image of the indoor environment.


## 5. Navigation Based on PID Control

The implementation of quad-rotor helicopter navigation is composed of image data collection and receiving control command, which forms a closed-loop control system [11, 12]. Besides, the control command calculated by PID control forms another closed-loop control processing as Figure 9 shows.Figure 9
The workflow of quad-rotor helicopter navigation control.In this experiment, the destination point is the vanishing point for the quad-rotor helicopter, and the image central position collected by the quad rotor is its current position. The quad-rotor helicopter modifies the image central position according to the vanishing position. Suppose the image central position isO(xo,yo) and the vanishing point coordinate is VP(xvp,yvp), so the error for the navigation can be expressed as
(22)dx=xvp-xo,dy=yvp-yo.The system adapts to PID to implement the process of navigation by close-loop control. In fact, the effect of PID control is to decrease the difference of current position and the vanishing point position till the difference reaches to a certain threshold [13]:
(23)V=KP·e+KI·∑e+KD·de,
where e1 is the difference between current value and real value, e2 represents accumulated error sum for the period ranges from beginning of current close-loop control to current control, e3 stands for difference between the current control difference and last control difference in the process of the same close-loop control, and V is the real parameter to control the quad rotor. This navigation process is implemented by adjusting the three attitude angles of yaw, roll, and pitch.The adjustment includes direction of three-dimension axis,dy is the position difference of y-axis used to control the vertical flight, which is corresponding the height control of quad-rotor helicopter, and dx is the position difference of x-axis used to control the horizontal flight, which is corresponding the yaw angle and roll angle control of quad-rotor helicopter [14]:
(24)ei=dxi=xvi-xoout=KP·ei+α·KI·∑ei+KD·(ei-ei-1),
where α is defined by
(25)α={0,|ei|>ε,1,|ei|<ε,
where ε is the error threshold of integral separation while, under the condition of y-axis and z-axis, the ei is dyi=yvi-yo and dzi=zvi-zo, respectively.Due to the fact that the control of quad-rotor helicopter belongs to complex nonlinear control, so in the process of PID control, while biased error became larger, the current integral separation parameter should be set as 0 to close integral action in order to eliminate shake, and while biased error became smaller, the current integral separation parameter should be set as 1 so as to open integral action.In order to test the performance of improved PID control, indoor flight experiments about testing flight angle (roll, pitch, and yaw) and flight deviation (horizontal and vertical) are executed.The quad-rotor helicopter is AR.Drone which is developed by Parrot Corporation in France, and it is designed based on four-axis intelligent controlling by WIFI, and it has the sensors such as three-axis accelerometer, two-axis gyrometer, one-axis yaw gyrometer, and ultrasonic wave height meter. Besides, it has a front camera (320*240 pix, 15 fps) and bottom camera (176*144 pix, 60 fps). AR.Drone has an ARM9 microcontroller which is based on Linux OS development platform.Quad-rotor helicopter collects the sensors data such as gyroscope, accelerometer, and altimeter and system further calculates the navigation data such as posture information of pitch, roll, yaw, and height. The quad rotor sends these navigation data and video streaming to PC through WIFI [15]. Then PC refreshes the current navigation data and handles the video streaming including decoding, video navigation, and image handling algorithms. After PC calculating the navigation error according to the output of visual navigation, then system sends the navigation error to the PID controller and finally obtains the output of flight control command. The output is sent to quad rotor by WIFI to control its flight effectively [16].Experiment 1.
While the quad-rotor helicopter hovers in the indoor environment, disturbance is imposed to the three angles of roll angle, pitch angle, and yaw angle at the time 22 s. The response process was reflected as Figures10(a), 10(b), and 10(c) show. The experimental results show that the improved PID control is better than original PID control in terms of overshoot and adjustment time.Performance comparison about flight angle and flight deviation.
(a)
Flight test of roll angel
(b)
Flight test of pitch angel
(c)
Flight test of yaw angle
(d)
Flight test of vertical deviation
(e)
Flight test of horizontal deviationExperiment 2.
While the quad-rotor helicopter hovers in the indoor environment, disturbance is imposed in the direction of horizontal and vertical at the time 22 s. The response process was reflected as Figures10(d) and 10(e) show. The experimental results show that the improved PID control is also better than original PID control as to overshoot and adjustment time.Experiment 3.
The quad-rotor helicopter was flied to the coordinate of destination point (3,3,3) from the coordinate of original point (1,1,1) in order to show the performance test of flight stability for the improved PID. The experimental results aboutx, y, and z coordinate axes are shown in Figures 11(a), 11(b), and 11(c). The experimental results show that the improved PID control is also better than original PID control in terms of overshoot and adjustment time.Performance test of flight stability.
(a)
Flight test ofX coordinate axes
(b)
Flight test ofY coordinate axes
(c)
Flight test ofZ coordinate axesFrom the analysis of the above experimental results, no matter whether disturbance is imposed in the quad-rotor helicopter, we can know that the improved PID can well conduct the stability of flight for the quad-rotor helicopter in the indoor environment compared with the original PID.


## 6. Flight Experimental Results in the Indoor Environment

In order to test the performance of the VPFE algorithm, we test the flight of the quad-rotor helicopter in the indoor environment as Figure3(a) shows. Figure 12 shows two frames of the software display image in PC. Each image describes the video image collected by the quad rotor after the corresponding handling by the above algorithm. A smaller picture in the right bottom of the image is used to show the flight scene of the quad-rotor helicopter at the corresponding time point.Flight test in the indoor environment.
(a)
(b)In the below experiments, we measure the system efficiency in terms of algorithm execution time, and the experimental results about calculating vanishing point for these continuous 150 frames are shown in Figure13.Figure 13
The efficiency of VPFE algorithm.As we can see from the results, the average execution time for the whole algorithm is 26.46 ms, which can meet the indoor flight environment for AR.Drone whose video flowing returning rate is 15 fps [17, 18].There are two other kinds of algorithm used to detect the vanishing point.(1)Spatial Alternation Technique. Information of image coordinate space is mapped into another coordinate space, and then operations of deleting straight lines are executed. Gaussian ball transformation method is proposed by Tayebi and McGilvray [18]; after the process of mapping, the straight lines and spatial location information of vanishing point is decreased at the cost of missing the information of straight line length and the distance between straight line and vanishing point. In the meantime, the calculation accuracy is affected by the accuracy of accumulate units.(2)Statistical Estimation Method. Parameter of lines is estimated by the feature points in the edge of image, and these parameters are used to calculate the vanishing point, or cost function is constructed using vanishing point and feature points of edge [19], but in the practical application, the algorithm is having higher computation complexity and lower computational efficiency.This paper proposes algorithm mainly using straight line information. Straight line information is directly used to obtain the vanishing point in the image, and then the intersection of these straight lines is calculated, and this process is implemented by calling the matrix operations function in the OpenCV. Finally clustering principle is used to obtain the vanishing point so as to reduce the computation complexity of the algorithm.The average execution time of proposed algorithm is about 26.46 ms. Compared with the other two vanishing point detection algorithms, the experimental results show that the execution time is separately about 13.7% and 21.3% shorter than Gaussian ball transformation method and statistical estimation method [20]. Consequently, the proposed algorithm has better real-time ability than the other two vanishing point detection algorithms.Figure14 shows the horizontal and vertical deviations between vanishing point and optic center for these continuous 150 image frames. In this experiment, the flight speed for the quad-rotor helicopter is constant value (0.4 m/s). As we can see from the experimental results, the quad-rotor helicopter can be well controlled during the whole process of autonomous navigation flight in the indoor environment  [21, 22].Figure 14
The horizontal deviation of autonomous navigation.


## 7. Conclusions

After the implementation of preprocessing of image, deleting noise interference, edge extracting using Canny operator, and extracting straight lines by RHT, the system finally obtains the position of vanishing point through the proposed VPFE algorithm and effectively operates the autonomous navigation of the quad-rotor helicopter by continuous calculation about the navigation error. The experiment shows that it can implement the destination navigation toward the calculated vanishing point well in the indoor environment, and the experimental results show that its average execution time for the whole algorithm is 26.46 ms per image frame, besides the flight deviation error can well be controlled into the acceptable range, so it can meet the requirements of real-time and good stability in the indoor environment.



---

*Source: 567057-2014-07-22.xml*