Recent Advances on Mathematical Modeling and Control Methods for Complex Vehicle SystemsView this Special Issue
Quad-Rotor Helicopter Autonomous Navigation Based on Vanishing Point Algorithm
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.
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 .
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.  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.  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.  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: Section 2 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.
is the ground coordinate, and 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:
Where straight lines , , and represent the parallel lines of , , and separately. Lines and are the projection lines while lines and 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 , and they can be expressed as where , , , and 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 .
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 .
Figure 2 shows the mapping relationship between point in the three-dimension space and point in the two-dimension image , and we can obtain where represents focal length of visual system, and point can also be expressed as the sum of several vectors in the directions of , , and coordinate axes . Using the attributes of , , and to express the base vector of the three axes, in the meantime vector can be expressed as , so the vanishing point can be calculated by
The indoor flight environment for quad-rotor helicopter navigation is shown as Figure 3(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.
(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:
In the two-dimensional space, it can be expressed as where is the blur radius which can be calculated by ; 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
The direction of vector stands for the direction having max changing rate, and the modulus of the gradient can be calculated by
The direction of the gradient is where is the included angle formed by gradient and -axis.
So the sample gradient model can be calculated by where and stand for the base vectors about -axis negative direction and -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
Partial derivative can be calculated by where the constant values and and then the above and can be expressed as the below convolution template: Neighbourhood pixel point can be expressed as 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.
Figure 3(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 Figure 4 shows, for any three points , , and in the straight line in the image space, they will form three sinusoidal curves , , and while they are mapped into the parameter space, and the intersection point of the three curve is corresponding to the straight line in the image 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 or pixels and maps them into one point in the parameter space, then uses a set with each element containing both a real-valued vector and an integer score to represent the parameter space, and updates the set at each step by the point mapped from the randomly picked pixels. Finally, system finds accumulated cells of the detected curves .
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 Figure 5.
The interfering accumulations caused by disturbance pixels can be significantly improved in RTH method. For Figure 5, HT method uses a 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 , where is the number of straight lines system that has been detected and is integer. Each line is expressed by two endpoints ,.
For endpoints set as , the angle between each line and horizontal coordinate axis can be calculated by
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 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 is 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 used to mean the ratio of points inside the circular error of vanishing point accounts for the percentage of total points, where and are both positive integers.
As Figure 6(a) shows, there exist many intersections caused by straight lines, and these intersections are enlarged in order to be seen clearly; for example and (inside the circular error of vanishing point) and , , , and (outside the circular error of vanishing point) consist in straight line . If 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.
(a) Illustration of coverage rate and error radius of vanishing point
(b) Experimental results about coverage affected by the threshold VT and HT
The 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, , 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.
Figure 6(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 making 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 , so the number of their intersections is , and it is stored as the initial value in the intersection array.
Any two straight lines in image can by expressed as Points and are on a straight line while points and are on another straight line. Then distance between the two lines can be calculated by While is 0, it means straight lines and form parallel line; otherwise, they are intersection lines, and then the coordinate of their intersection point can be calculated by
Calculate all the intersections by the above equations, where is the number of all intersection points, and their coordinates are expressed as , respectively. Those intersection points whose coordinate does not exist in the range of to are undesirable ones, and they should be deleted, where and 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
As is the number of iterative process, the value is where (<1) is the convergence coefficient, represents radius of convergence, and 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 of and and calculate the according to and .
Step 2. The new radius of convergence is .
Step 3. Calculate the mass center of the intersection points set as and regard it as the vanishing point.
Step 4. For all the elements in the intersection point set as , calculate the distance between intersection point and vanishing point.
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 Figure 7. 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.
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.
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 is and the vanishing point coordinate is , so the error for the navigation can be expressed as
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 : where is the difference between current value and real value, represents accumulated error sum for the period ranges from beginning of current close-loop control to current control, stands for difference between the current control difference and last control difference in the process of the same close-loop control, and 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, is the position difference of -axis used to control the vertical flight, which is corresponding the height control of quad-rotor helicopter, and is the position difference of -axis used to control the horizontal flight, which is corresponding the yaw angle and roll angle control of quad-rotor helicopter : where is defined by where is the error threshold of integral separation while, under the condition of -axis and -axis, the is and , 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 ( pix, 15 fps) and bottom camera ( 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 . 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 .
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 Figures 10(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.
(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 deviation
Experiment 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 Figures 10(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 about , , and 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.
|(a) Flight test of coordinate axes|
|(b) Flight test of coordinate axes|
|(c) Flight test of coordinate axes|
From 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 Figure 3(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.
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 Figure 13.
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 ; 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 , 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 . Consequently, the proposed algorithm has better real-time ability than the other two vanishing point detection algorithms.
Figure 14 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].
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.
Conflict of Interests
The authors declare that there is no conflict of interests regarding the publication of this paper.
This work is partly supported by the National Natural Science Foundation of China (no. 61101121), the National High Technology Research and Development Program (863 Program) (no. 2013AA102505), the Key Laboratory Project Funds of Shenyang ligong University under Grant no. 4771004kfs03, and the Educational Committee of Liaoning Province Science and Technology Research Projects under Grant no. L2013096. The authors thank Jan Vitek, Ales Plsek, and Lei Zhao for their help while the first author studied at the Purdue University as a Visiting Scholar from September, 2010, to September, 2012. The authors also thank the anonymous reviewers for their valuable comments.
G. Hoffmann, D. G. Rajnarayan, S. L. Waslander, D. Dostal, J. S. Jang, and C. J. Tomlin, “The Stanford testbed of autonomous rotorcraft for multiagent control,” in Proceedings of the 23rd Digital Avionics Systems Conference, vol. 2, October 2004.View at: Google Scholar
M. Achtelika, A. Bachrach, R. He, S. Prentice, and N. Roy, “Autonomous navigation and exploration of a quadrotor helicopter in GPS-denied indoor environments,” in Proceedings of the Robotics: Science and Systems, 2008.View at: Google Scholar
R. Hartley and A. Zisserman, Multiple View Geometry in Computer Vision, Cambridge University Press, Cambridge, UK, 2nd edition, 2003.
A. Minagawa and N. Tagawa, “Vanishing point and vanishing line estimation with line clustering,” IEICE Transactions on Information & Systems, vol. 7, pp. 1574–1582, 2000.View at: Google Scholar