A New Combined Vision Technique for Micro Aerial Vehicle Pose Estimation

Abstract

In this work, a new combined vision technique (CVT) is proposed, comprehensively developed, and experimentally tested for stable, precise unmanned micro aerial vehicle (MAV) pose estimation. The CVT combines two measurement methods (multi- and mono-view) based on different constraint conditions. These constraints are considered simultaneously by the particle filter framework to improve the accuracy of visual positioning. The framework, which is driven by an onboard inertial module, takes the positioning results from the visual system as measurements and updates the vehicle state. Moreover, experimental testing and data analysis have been carried out to verify the proposed algorithm, including multi-camera configuration, design and assembly of MAV systems, and the marker detection and matching between different views. Our results indicated that the combined vision technique is very attractive for high-performance MAV pose estimation.

1. Introduction

In recent years, micro aerial vehicles have shown amazing capability in performing certain difficult tasks and movements, such as exact landing [1], team cooperation [2], building blocks [3], etc. In performing these tasks, state estimation for the aerial vehicles must be precisely obtained and be carried out in a real-time format because any small error or delay may lead to a complete failure. Therefore, the perception of environment and pose of MAVs is important for precise and real-time control. However, in a conventional GPS (Global Position System)-Inertial system, GPS is not reliable enough to deal with the task due to signal blockage or multi-path scattering. The inertial measurement unit (IMU) is able to offer nice pose measurements but suffers from accumulative error over time. Therefore, it needs frequent correction [4]. Although Light Detection and Ranging (LIDAR) can be used to explore the surrounding environment and calculate the relative pose of a MAV, its size and weight are out of the range of the load capacity of a micro aerial robot.

In comparison, the most-adopted method follows a vision-based approach, which is able to provide enough accuracy to satisfy the needs for these MAV tasks, such as take-off, landing, indoor navigation, etc. State estimation for the MAV can be defined as the process of tracking the three-dimensional (3D) pose of the vehicle. The vision-based approach has been intensively studied and recently developed into many different methods categorized by different state estimation methods. According to the arrangement of cameras, the vision-based state estimation can be roughly divided into two categories: onboard navigation and on-ground navigation. Onboard navigation refers to a MAV that tracks some structured or natural object to achieve self-state by using onboard vision, such as relative target and landmark navigation [5,6,7], visual odometry (VO) [8,9], simultaneous localization and mapping (SLAM) [10,11], etc. In the early stage of onboard navigation, relative target and landmark navigation were used to estimate the relative MAV pose via onboard vision [5,6,7], where the structure of the reference targets/landmarks is known. Then methods based on structure from motion (SFM), SLAM, and VO have been proposed one by one. Using the methods, the MAV pose can be estimated by tracking some typical features from the natural scene. Without the transformation from image plane to 3D space, bio-inspired optical flow techniques have been also applied to directly infer the MAV pose and motion depending on only the information in images [12,13]. Alternatively, a MAV calculates its pose by an external visual measuring system, which is usually called on-ground navigation. In such a system, one or more cameras are distributed in the surrounding and have the ability to capture the image data about MAVs in real time. Normally as a testbed, on-ground navigation is applied to test the algorithms of robot global positioning, control, and planning. In this situation, either carrying load or computation capacity is no longer considered to be a problem. Even though current studies mainly focus on onboard vision, like the above-mentioned methods, on-ground navigation is worth studying as well [14,15,16,17,18]. However, these systems used only the triangulation method in stereo camera configuration or the PNP (perspective-n-point) method in single camera configuration.

For the on-ground navigation method, we have developed a new combined vision technique to significantly enhance the real-time MAV pose estimation, and built an on-ground visual system to test it. In this CVT, two positioning strategies with different constraints have been integrated by a particle filter framework at the same time: multi-view triangulation and monocular perspective-3-point (P3P) algorithms. The multi-view triangulation algorithm is based on the intersection of different views, while the monocular P3P technique is based on the shape change of a rigid body in the image. The visual system is expected to have a more stable performance when these two location methods (mono-view P3P and multi-view triangulation) are considered simultaneously by a PF framework.

The remainder of this paper is organized as follows: Section 2 reviews the related work of visual measurement in MAV navigation. A summary of the visual system including hardware and software is presented in Section 3. In the hardware part, the MAV and camera configurations are introduced. In the software part, the feature point detection and multi-camera calibration are also described. Section 4 illustrates the proposed CVT visual algorithm which is used to calculate MAV poses by integrating the two different methods. Finally, experiment testing results are demonstrated and conclusions will be drawn in Section 5 and Section 6, respectively.

2. Previous Research in On-Ground Visual Navigation for MAV Pose Estimation

MAV pose estimation in different applications and many advanced strategies to achieve this task have been intensively studied for the past two decades. On-ground visual navigation is a commonly used strategy for motion capture, which employs a set of external cameras with known camera parameters to track and locate MAV. These cameras are usually installed around to observe the flight in space simultaneously. This strategy is usually applicable to a limited space but with good accuracy.

For example, the VICON system (Vicon Motion Systems Ltd., Oxford, UK), a commercial product, is able to track and locate more than one robot at the same time. The system exhibits a good performance of positioning accuracy and processing rate. Based on this system, MAV control and navigation have been well demonstrated in [19,20,21], where MAVs are controlled by the feedback of the visual system to dance, play musical instruments, build houses, etc. However, such a configuration requires multiple high-speed external IR cameras and is too expensive. An indoor test-bed named RAVEN (Real-time indoor Autonomous Vehicle test ENvironment), based on VICON system, has been developed by the Aerospace Control Lab [22] for autonomous navigation of multiple rotorcrafts, where vision is the only manner used for MAV pose estimation. The test-bed acquired pose information by detecting and locating markers installed around MAV. The markers are usually beacons or reflection spheres which are easy to recognize by infrared camera. Therefore, such a system has a high resolution for location positioning and has been used to perform complicated tasks, such as autonomous landing, charging, fancy flying, and multi-MAV motion planning.

Martínez et al. [14] proposed two visual manners including onboard vision and on-ground vision to enable MAV automation. One detects and tracks the planar structure by an onboard camera. The other one is a 3D reconstruction of the position of MAV by an on-ground camera system. The on-ground camera system consists of three visible light cameras, by which the color markers attached to the flying vehicle can be tracked and located. However, the results are rough because the markers on the aircrafts are too large and the view is limited. In the work by Faessler et al. [15], the MAV pose estimation was done with the P3P algorithm. An infrared camera is used as the major sensor in such a system to assist detection. However, it takes a certain calculation time to distinguish markers attached to different positions of the aerial body, as they look the same in the infrared image. Different from the above, Hyondong et al. [16] employed a visible light camera instead of an infrared camera to detect and track indoor MAV pose with color balls as the markers. Since onboard markers are attached to the pre-defined location and have different color features, the position and attitude of MAV could be recognized by color feature and triangulation. In addition, an extended Kalman filter (EKF) framework integrated with MAV dynamic was introduced to improve the performance of the visual measurement system. For example in [17], two stationary and upward-looking cameras were placed on the ground to track four black balls attached to the helicopter. The errors between the positions of the tracked balls and pre-specified references are taken as the visual feedback control input. A pair of ground and onboard cameras was employed for the problem of quadrotor pose estimation [18]. The two cameras were set to face each other to estimate the full 6-degrees-of-freedom (DOF) pose of MAVs.

Different from the above-mentioned works, this paper did not pay too much attention to the system configuration, but focused on how to make use of the image data from a visual system to archive a stable pose estimation. In this work we will try to consider the two kinds of location methods (mono-view P3P and multi-view triangulation) simultaneously by a PF framework.

3. Description of the Visual Measurement System

The on-ground visual measurement system is designed to test the proposed CVT and includes the cameras, image grabber, PC processor, and self-built MAV with markers. The major focuses in this study are calibration of the multi-camera system, MAV system design, and MAV detection.

3.1. Multi-Camera System Calibration

Before starting the measuring work, the multi-camera system should be calibrated to acquire a mapping between the features in the real 3D space and the 2D image. By considering the basic pinhole camera model, the calibration setups are separated into two setups. The first setup is for intrinsic parameter calibration where each camera is individually calibrated with the Bouguet camera calibration toolbox [23]. The second setup is for extrinsic parameter calibration carried out by the multi-camera self-calibration toolbox [24].

In our measurement system (see Figure 1), four analog CCD (Charge Coupled Device) cameras are connected to a PC station via a general 8-channel image grabber. So the data source from the cameras can be captured synchronously and processed in real-time with the help of the OpenCV library. At first, we use several image chessboards from different camera views and calibrate the four intrinsic parameters of every camera, including the focal length (f_x, f_y) and principal point (μ₀, ν₀). These parameters constitute the internal matrix (K-matrix) in the imaging model, which can be written as:

$$s{\tilde{x}}_{image}=K[R|t]{\tilde{X}}_{world}=P{\tilde{X}}_{world}, where K=\left[\begin{array}{ccc}{f}_x& 0& u_0\\ 0& f_y& v_0\\ 0& 0& 1\end{array}\right]$$

(1)

where ${\tilde{x}}_{image}$ is the 2D image point represented by a homogeneous vector ${\left(\mu ,\nu ,1\right)}^T$, ${\tilde{X}}_{world}$ is the 3D world point represented by a homogeneous four-vector ${\left(X,Y,Z,1\right)}^T$, and s is a scalar representing the depth information. R is the rotation transform matrix and t is the translation transform matrix from the world frame to camera frame. R, t, and K-matrix constitute the mapping between the 3D world and 2D image, which can be called the Projective matrix (P-matrix).

Once K-matrix is acquired, only seven parameters remain unknown for every camera, including the scalar s, 3-vector r, and 3-vector t, respectively, in the P-matrix. All the image points, 3D-world points, and camera projectives from all the cameras can be put into the following equation:

$$W_s=\left[\begin{array}{ccc}{s}_1^1\left[\begin{array}{c}{u}_1^1\\ v_1^1\\ 1\end{array}\right]& \cdots & s_n^1\left[\begin{array}{c}{u}_n^1\\ v_n^1\\ 1\end{array}\right]\\ ⋮& ⋮& ⋮\\ s_1^m\left[\begin{array}{c}{u}_1^m\\ v_1^m\\ 1\end{array}\right]& \cdots & s_n^m\left[\begin{array}{c}{u}_n^m\\ v_n^m\\ 1\end{array}\right]\end{array}\right]={\left[\begin{array}{c}{P}^1\\ ⋮\\ P^m\end{array}\right]}_{3m\times 4}{\left[\begin{array}{ccc}{\tilde{X}}_1& \cdots & {\tilde{X}}_n\end{array}\right]}_{4\times n}$$

(2)

Based on the equation above, W_s can be factorized to recover the projective motion P and the projective shape X if enough noiseless points $({\mu }_j^i, {\nu }_j^i)$ can be collected. During the calibration of extrinsic parameters, the bright spots will be waved through the working volume of the camera system, as shown in Figure 2a. Besides intrinsic parameters, the coordinates of the bright spots in all images are the known information. The detailed computation and calibration processes have been previously explained [24]. With the multi-camera self-calibration toolbox, not only the extrinsic parameters of all cameras but also the coordinates of bright spots in the 3D space can be recovered. This is shown in Figure 2b.

3.2. MAV Design and Detection

As shown in Figure 3, the MAV system is built with only one airframe, four speed controllers and rotors and propellers, onboard microcontroller, communication module, and IMU sensor. In the system, the main part for control and information processing is operated by a computer (PC) on the ground. The onboard microcontroller, STM32F103 (STMicroelectronics, Shanghai, China), is in charge of receiving speed command from the ground PC to generate corresponding Pulse-Width Modulation (PWM) for the speed controllers to control the four motors. Also, the microcontroller can be utilized to capture the inertial information from the IMU sensor and send it to the ground PC in real time. The IMU, MPU9150 (InvenSense, San Jose, CA, US), is a unique sensor on the drone, which is continuously sending measurements of the attitude angle, acceleration, and angular velocity to the microcontroller at 100 Hz. The module Xbee PRO 900HP S3B is responsible for the communication between the onboard and ground PC, which is able to work in full-duplex mode. A joystick is connected to the ground PC by a simulator and is used to change the flight state of the MAVs. In order to conveniently and efficiently test the control system in the lab, the quadrotor is mounted on a plank via a cardan joint so that its state can be flexibly adjusted without flying far away.

As color is one of the most distinct features in the environment, we set three different color balls around the airframe as markers for detection. Firstly, Gaussian filtering is done with a five-by-five patch for every image to remove some sharp and unnecessary information. Because of different colors, the three balls could be well segmented from the scene in HSV (hue, saturation, value) space. However, the measurements in the images from different views (cameras) are not independent with each other in the multi-camera configuration. It should be noted that there is an epipolar geometry constraint between every two fixed views. The constraint allows the correspondence of image point x from one view and image point x’ from the other view, which can be written as:

$${\tilde{x}}^{\prime }F\tilde{x}=0$$

(3)

where F is the fundamental matrix that contains the information of the two cameras’ intrinsic and extrinsic parameters and can be acquired in advance according to Equation (4),

$$F={K^{\prime }}^{-T}R{K}^T{\left[K{R}^{T}t\right]}_{\times }$$

(4)

where K′ and K are the intrinsic matrices of the two cameras, while R and t represent the 3 × 3 rotation matrix and the 3 × 1 translation matrix. From this equation, we can determine the corresponding x′ in the other view along the epipolar line $\overrightarrow{l}=\mathit{F}\overrightarrow{x}$ once the point x in one view is found. As a result, the detection in all views takes less processing time and improves noise immunity. The results of the marker detection are shown in Figure 4a–e, showing that the detection is steady and the mean error is less than one pixel.

4. Pose Estimation

4.1. Marker Location and MAV Pose Computation

It is noted that there is a unique solution to determine the position of a 3D point in the two-view geometry by triangulation. Given the parameters of one camera, every point in the image can be used by the two equations to determine the three unknown coordinates of the corresponding point in 3D space according to the projection model (Equation (2)). Therefore, in the case of two or more than two views, there are more than enough equations to obtain the solution. The more equations that are considered, the higher accuracy the solution has. The fundamentals of the triangulation method are shown in Figure 5a. In this method, the color marker represented by point X can be located in the 3D world based on the knowledge of the corresponding positions in the image planes taken by Camera 1–Camera N. The detailed computation is shown in Equation (5), which is a combination of all the projective equations,

$${\left[\begin{array}{ccc}{s}^1{\overrightarrow{x}}^1& \cdots & s^N{\overrightarrow{x}}^N\end{array}\right]}^T={\left[\begin{array}{c}{P}^1\\ ⋮\\ P^N\end{array}\right]}_{3N\times 4}\overrightarrow{X}$$

(5)

where ${\overrightarrow{x}}^i$ denotes the homogeneous coordinate of the marker in image plane i, ${\overrightarrow{P}}^i$denotes the projective matrix of camera i, and $\overrightarrow{X}$ denotes the homogeneous coordinate of the marker in the 3D world.

At the same time, by using the known relative relations among the three color markers on the rigid body, the marker position and the rigid body pose relative to the camera frame can be derived. This method is a so-called P3P problem, where the only given information is the actual distances between each two markers. The details of the P3P method are shown in Figure 5b. The image point $mi({\mu }_i, {\nu }_i)$ with focal normalization is calculated with Equation (6). The side lengths of the triangle structured from the markers M1, M2, and M3 are denoted by a, b, and c, respectively. In fact, the vector ${[x_{ci}, x_{ci}, 1]}^T$ has the same direction with the unit vector ${\overrightarrow{e}}_i$, which is expressed in Equation (7).

$${\left[\begin{array}{ccc}{x}_{ci}& y_{ci}& 1\end{array}\right]}^T=K^{-1}{\left[\begin{array}{ccc}{u}_i& v_i& 1\end{array}\right]}^T$$

(6)

$${\overrightarrow{e}}_i=\frac{1}{\sqrt{x_{1ci}^2+y_{1ci}^2+1}}{\left[\begin{array}{ccc}{x}_{ci}& y_{ci}& 1\end{array}\right]}^T$$

(7)

Consequently, the cosine of the angles between the three unit vectors could be determined by Equation (8). Therefore, the three equations about the distance d₁, d₂, and d₃ between the points M1–M3 and the center of camera can be obtained according to the cosine law, as described in Equation (8). The equations can be solved by linear iteration with two real solutions of d₁–d₃.

$$\{\begin{array}{l}\cos \alpha =e_2^T{e}_3\\ \cos \beta =e_1^T{e}_3\\ \cos \gamma =e_1^T{e}_2\end{array}$$

(8)

$$\{\begin{array}{l}{d}_2^2+d_3^2-2d_2{d}_3\cos \alpha =a^2\\ d_1^2+d_3^2-2d_1{d}_3\cos \beta =b^2\\ d_1^2+d_2^2-2d_1{d}_2\cos \gamma =c^2\end{array}$$

(9)

Finally, the positions of the three markers M1–M3 relative to the camera reference frame can be determined by Equation (10). Since the camera orientation in the 3D world frame has been calibrated in advance, the positions of the three markers M1–M3 relative to the 3D world frame can be also calculated.

$${\overrightarrow{M}}_i=d_i{\overrightarrow{e}}_i, \mathrm{i}=1,2,3$$

(10)

Because the positions of markers M1–M3 on the MAV are given, the pose of the MAV relative to the 3D world reference can be determined from the discussion above. The MAV pose contains both position and attitude, as shown in Figure 5c. The center of the markers M1 and M2 is defined as the MAV position. In the rotation matrix $\overrightarrow{R}=\left[\overrightarrow{n}, \overrightarrow{o}, \overrightarrow{a}\right]$, the three vectors, $\overrightarrow{n}$, $\overrightarrow{o}$, and $\overrightarrow{a}$ represent the ${\overrightarrow{x}}_b$, ${\overrightarrow{y}}_b$, ${\overrightarrow{z}}_b$ direction of the body frame in the 3D world coordinates, respectively. As shown in Equation (11), the MAV body reference frame $\left\{{\overrightarrow{x}}_b,{\overrightarrow{y}}_b,{\overrightarrow{z}}_b\right\}$ can be built with the vector from the center between M2 and M3 to M1 as ${\overrightarrow{x}}_b$, and the vector from M2 to M3 as ${\overrightarrow{y}}_b$, and ${\overrightarrow{z}}_b$ to be determined by the right-hand rule,

$$\{\begin{array}{c}{\overrightarrow{n}}^{\prime }={\overrightarrow{M}}_1-\frac{1}{2}({\overrightarrow{M}}_2+{\overrightarrow{M}}_3)\\ \overrightarrow{n}=\frac{{\overrightarrow{n}}^{\prime }}{\Vert {\overrightarrow{n}}^{\prime }\Vert }\\ {\overrightarrow{o}}^{\prime }={\overrightarrow{M}}_3-{\overrightarrow{M}}_2\\ \begin{array}{l}\overrightarrow{o}=\frac{{\overrightarrow{o}}^{\prime }}{\Vert {\overrightarrow{o}}^{\prime }\Vert }\\ \overrightarrow{a}=\overrightarrow{n}\times \overrightarrow{o}\end{array}\end{array}$$

(11)

where $\overrightarrow{a}$ can be determined by the cross product of $\overrightarrow{n}$ and $\overrightarrow{o}$ following the orthogonality of R. Now, the rotation and translation relationship between the body frame and the world frame has been derived. The three attitude angles (roll, pitch, yaw) of the MAV can be solved by the Rodriguez transformation expressed in Equation (12):

$${\left[\begin{array}{ccc}\phi & \theta & \psi \end{array}\right]}^T=Rodriguez({\overrightarrow{R}}_{3\times 3})$$

(12)

where $\psi$, $\phi$ and $\theta$ represent yaw, pitch, and roll angle, respectively. It should be noted that each angle needs to be adjusted to a different range so that it can be consistent with the IMU output. In general, for a rigid body (MAV) the three attitude angles (roll, pitch, and yaw) should have a defined range and change direction. For example, the range of the roll angle is set to [−180°, 180°], the range of the pitch angle is set to [−90°, 90°], and the range of the yaw angle is set to [0°, 360°]. However, the computation result acquired directly from the visual system is [0°, 360°]. So it is necessary that the two sets of attitudes are adjusted to be consistent. The adjustment is also called “normalization”.

4.2. Pose Estimation

As the measurement is taken from the visual system, the MAV pose is continuously calculated from multi-view triangulation and mono-view P3P, which will be used to correct or update the state of MAVs in a fusion framework. In general, inertial measurements taken at high rate (100 Hz–2 kHz) are fused with lower rate exteroceptive updates from vision, or GPS. For the MAV pose estimation, conventional approaches are based on the indirect formulations of Extended or Unscented Kalman Filters and Particle Filters (PF). For a multiple-camera measurement of one quantity at an approximate same time, the framework in this work is based on the indirect formulation of an iterated PF, where the state prediction is driven by IMU.

With $\mathit{I}=\{{\overrightarrow{e}}_1,{\overrightarrow{e}}_2,{\overrightarrow{e}}_3\}$ as a right-hand inertial frame, $\mathit{B}=\{{\overrightarrow{b}}_1,{\overrightarrow{b}}_2,{\overrightarrow{b}}_3\}$ defines a body-fixed frame with its center at the center of mass of the vehicle. The ground 3D-world reference in the visual system is set to be consistent with the inertial frame. The motion model of the rigid body can be derived based on the kinematic equation of attitude and Newton’s equation of motion, as expressed in Equation (13). ${\overrightarrow{\xi }}_I=(x,y,z)$ is the position of the center of mass of the vehicle in frame I. ${\overrightarrow{v}}_I$ is the linear velocity in frame I. $\overrightarrow{\Omega }={(p,q,r)}^T$ is the angular velocity of the airframe with respect to the frame B. The attitude of the rigid body is given by the rotation matrix ${\mathit{R}}_B^I(\phi ,\theta ,\psi ):\mathit{B}\to \mathit{I}$.

$$\{\begin{array}{c}\dot{{\overrightarrow{\xi }}_I}={\overrightarrow{v}}_I\\ \dot{{\overrightarrow{v}}_I}=g{\overrightarrow{e}}_3+\raisebox{1ex}{$1$}\!\left/ \!\raisebox{-1ex}{$m$}\right.{\mathit{R}}_B^I\mathit{F}\\ {\dot{\mathit{R}}}_B^I={\mathit{R}}_B^I{\overrightarrow{\Omega }}_{\times }\\ \mathit{I}\dot{\overrightarrow{\Omega }}=-{\overrightarrow{\Omega }}_{\times }\times \mathit{I}\overrightarrow{\Omega }+\mathit{\tau }\end{array}$$

(13)

The operator ${(\cdot )}_{\times }$ maps $\overrightarrow{\Omega }$ into an anti-symmetric matrix. m represents the rigid object’s mass. $\mathit{I}\in {ℝ}^{3\times 3}$ represents the inertia matrix which is a constant. The vectors F and $\mathit{\tau }\in \left\{\mathit{B}\right\}$ represent the principal non-conservative forces and moments applied to the rotorcraft airframe by the aerodynamics of the rotors, respectively. It is difficult for a PF to deal with high dimensional states. This is because the filter is likely to be divergent as the dimensions increase. The estimated state of a MAV consists of the linear velocity $(\dot{x}, \dot{y}, \dot{z})$, the attitude angle $(\phi , \theta , \psi )$ in the inertial frame, the acceleration, and the angular velocity in the body frame. The acceleration and the angular velocity measured by the IMU are supposed to be the sum of the real value, a bias noise, and a Gaussian noise. Therefore, the number of the PF state is 12, as expressed in the following,

$${\overrightarrow{x}}_k={\left(\dot{x}, \dot{y}, \dot{z}, \phi , \theta , \psi , a_{bx}, a_{by}, a_{bz}, p,q,r\right)}^T$$

(14)

Measurements of the MAV state include its position $\left(x,y,z\right)$ and attitude $\left(\phi ,\theta ,\psi \right)$ in the inertial reference coordinate system. Measurement noises are complicated and mainly come from camera calibration, marker detection, and varying environmental conditions. A particle filter is applied to deal with the noises, because it is capable of modeling and canceling the random noise. Such a particle filter is able to simulate the true state of the MAV and many measurements for one state by using different testing methods with the large number of particles in the filter. Therefore, it is used to estimate the state of the MAV in this work.

Similar to other filter frameworks [25,26,27], our estimation based on PF mainly consists of the predicting and the updating periods. The detailed process is shown in Figure 6. During the predicting period, the state of every particle evolves from x_k to x_{k + 1} driven by the IMU based on the motion model discussed above, with fixed weights for the moment. This is followed by the updating period, when the weights of all particles are adjusted more than once. The weights are proportional to the separation of the predicted state and the measurement in every updating step. At first, the weights of all particles are adjusted one by one based on the measurement results from each camera. Then, after the updating period, the particle weights based on multi- and mono-view measurements are normalized. As a result, the state of each particle is estimated by accumulating the product of the particle and its weight. In addition, the particles with little weight will be abandoned, while particles with large weight will be resampled. As a result, the particles to be measured are always enough in the procedure.

5. Experiments and Analyses

The vision-based pose estimation algorithm is tested in our laboratory with a small quadrotor system shown in Figure 7. A visual system is set up with four cameras and a PC with an image grabber. As shown in Figure 7, the quadrotor is flying in the view of the visual system. Its position and attitude relative to the ground 3D reference frame can be calculated by the above-mentioned procedure. The low-cost visible light cameras employed in the system are distributed around a 4 m × 3 m × 3 m testing room.

5.1. Pose Computation Results

When the positions of the three markers in the image are available, the location of the MAV can be calculated by multi- or mono-view geometry. As shown in Figure 8A,B, the position and attitudes of the testing MAV are calculated by using the two methods, respectively. On account of the P3P method introduced in Section 4, two set of solutions are obtained and are discriminative with red and blue lines in Figure 8B. Even though the two solutions of the position calculation are almost similar, the small disparity would give rise to a large difference on the following attitude angle calculations. It could be observed that one of the two solutions is more close to the solution of the triangulation displayed in Figure 8A. The other solution could be discarded using the triangulation solution as the reference. As a result, this provides a solving method to distinguish the two solutions from P3P. Then by calculating the position $\left(x,y,z\right)$ and the attitudes $\left(roll,pitch,yaw\right)$ continuously, the corresponding histograms about the multi-and mono-view measurements are shown in Figure 9 and Figure 10, respectively. This information could help us understand in advance some performance of the two location methods, such as stability, if the results of the methods are regarded as measurements of the filter framework. Through the histograms, each measured variable, whether it is position or attitude, is centered on a fixed value and changes around it.

In Figure 11, we compared the estimated attitude angles with the attitude and heading reference system (AHRS) to verify the validity of the visual system. The precision of AHRS is 0.5°–1° and the resolution is 0.1°. As shown in Figure 11, both the IMU and visual measurements are obtained and displayed at the same time when the quadrotor is fixed on the plank via cardan lock. The results indicated that the visual measurements including roll, pitch, and yaw angles are quite consistent with the IMU data. This comparison indicated that the proposed technique is quite accurate.

5.2. MAV Pose Estimation

It is necessary to model and study the noise in the two measurements from the visual system in order to finally achieve an accurate design parameter of the filter. The noise model from the measurements has been obtained by analyzing the histograms shown in Figure 9 and Figure 10. In this work, the noises of the process and measurement are considered as approximately a zero-mean Gaussian distribution.

Especially, more particles are required when the estimation is expected to hold an approximate real state, and disastrous computation also happens with it. So after numerous attempts, the particle number N is set to 1000. As a result, the cycle time for the visual measurement system is 0.421 s, using the Matlab platform on a PC with a 1.6 GHz Intel i5 processor. Figure 12 shows the results of state estimation using the PF measurements from the triangulation module and IMU while the quadrotor is hovering in the air. The mean value of the state of the quadrotor is thought to be approximately zero. As a consequence, the standard deviations for the estimated state and the original measurements are [0.0580, 0.1475, 0.0138, 0.0161, 0.0230, 0.0197, 1.7052, 0.9917, 6.1615, 118.4207, 71.4597, 44.4818] and [0.3067, 0.8531, 0.2053, 0.0335, 0.0380, 0.0364, 3.5187, 3.2062, 7.7488, 121.5115, 76.2109, 52.0670] respectively. The results shown in Figure 12 indicate that the state estimation by the proposed technique is more stable than the measurement directly obtained by a visual system or IMU.

Meanwhile, another experiment of real-time pose estimation has also been carried out while the quadrotor is flying freely in a control region of the visual system. In this test, the position and heading of the quadrotor are remotely controlled by a ground station and its attitude is autonomously stabilized by the onboard IMU. The result of a part of the recovered flight trajectory estimated by the PF framework is shown in Figure 13. In order to be observed well, it should be noted that the Z-axis of the 3D world (inertial) frame has been converted to the opposite direction and points upwards.

From the results and discussion above, the proposed CVT enables a stable estimation when the flying quadrotor is within the view of the designed vision system. Additionally, the camera view or image noise may lead to different errors in each location method based on different constraints. It is helpful to achieve a stable result when the solutions from triangulation and P3P are referred to by a filter.

6. Conclusions

In summary, we have presented a combined vision technique and built a visual measurement system for MAV pose estimation. In this work, an indoor visual measurement system has been developed and introduced, including multi-camera parameter calibration, MAV detection, and design of a quadrotor. The unique contribution of this paper is the proposed CVT in which two visual positioning methods can be integrated in a PF framework to obtain a stable estimated pose. With the designed visual measurement system, some interesting proof-of-concept tests have been carried out. The results show that the proposed CVT can provide stable pose estimation for MAVs. In our future plan, the proposed system will be assembled into an on-ground navigation platform to guide the MAV for safe and precise landing. It can also be considered as a low-cost and high-performance flight test-bed, which can be applied to study the dynamics, control, and navigation of MAVs.