28-12-2015, 03:43 PM
C onsumer-grade technology seen in cameras and phones has led to the price–performance ratio falling dramatically over the last decade. We are seeing a similar trend in robots that leverage this technology. A recent development is the interest
of companies such as Google, Apple, and Qualcomm in high-end communication devices equipped with such sensors as cameras and inertial measurement units (IMUs) and with sig-nificant computational capability. Google, for instance, is de-veloping a customized phone equipped with conventional as well as depth cameras. This article explores the potential for the rapid integration of inexpensive consumer-grade elec-tronics with the off-the-shelf robotics technology for automa-tion in homes and offices. We describe how standard hardware platforms (robots, processors, and smartphones) can be integrated through simple software architecture to build autonomous quadrotors that can navigate and map un-known, indoor environments. We show how the quadrotor can be stabilized and controlled to achieve autonomous flight and the generation of three-dimensional (3-D) maps for ex-ploring and mapping indoor buildings with application to smart homes, search and rescue, and architecture. This opens up the possibility for any consumer to take a commercially available robot platform and a smartphone and automate the process of creating a 3-D map of his/her home or office.
The price–performance ratio of processors, sensors, and net-working infrastructure, which has dropped significantly over the last decade, has led to new applications founded on the conver-gence of computation, sensing, and computing. A recent General Electric report [1] calls this convergence the industrial Internet and suggests that the potential macroeconomic benefit from the industrial Internet could be comparable with the eco-nomic productivity gain attributable to the Internet revolution of the late 20th century. Others call it the Internet of Things and predict an economic impact in the tens of trillions of dollars [2]. More than 75% of business leaders surveyed predicted a direct impact of this technology on their business [3]. This conver-gence also holds great promise for automation with robots, which “emphasizes efficiency, productivity, quality, and reliabili-ty, focusing on systems that operate autonomously, often in structured environments over extended periods” [4].
In this article, we address this confluence of technologies enabling automation in the context of aerial robotics, a field that has also seen dramatic advances over the last decade. The same drop in price–performance ratio of processors and sen-sors has fueled the development of micro unmanned aerial ve-hicles (UAVs) that are between 0.1 and 1 m in length and 0.1–2 kg in mass. These low-cost platforms are easy to manu-facture in contrast to the expensive UAVs used for military ap-plications. The number of Predators and Global Hawks is estimated to be around 1,000. Growth in the consumer elec-tronics industry (millions or billions of components at low cost) has resulted in inexpensive hardware for sensing and computation. These advances, coupled with open-source tools for building robots like quadrotors [5], have led to innovative low-cost toys and hobby kits (e.g., diydrones.org). The real op-portunity for robotics and automation is in leveraging these tools (and the convergence of computation, sensing, and com-munication) to develop economical (compared with the mili-tary counterparts), functional, and robust aerial robots. These can be used in such tasks as inspection [6], interaction with the environment [7], [8], search and rescue [9], construction [10], [11], and mapping of homes and offices.
Early quadrotor vehicles were primarily experimental sys-tems, but improved design and software tools have lead to sig-nificant increases in reliability and reductions in cost. Today, quadrotors have reached the maturity of consumer-grade de-vices. To reiterate, this is in large part due to the decreasing price–performance ratio of sensors for autonomous navigation, including global positioning systems (GPSs), cameras, IMUs, and laser scanners [7], [12]–[16]. In this context, low-cost range sensors offer an attractive alternative to high-end laser scanners and 3-D cameras for applications such as indoor navigation and mapping, surveillance, and autonomous robotics.
Consumer-grade range sensing technology has led to many devices becoming available on the market, like the Microsoft Kinect sensor and the ASUS Xtion sensor (PrimeSense 2010; see Figure 1). The richness of the provided data and the low cost of the sensor have attracted many researchers from the fields of mapping, 3-D modeling, and reconstruction. The ASUS Xtion sensor boasts a lower weight than the first genera-tion of red, green, blue, and depth (RGB-D) cameras (around 70 g without the external casing). (While this specific sensor is no longer available, there are others under development that are likely to be available in the future.) It does not require ex-ternal power beyond a standard universal serial bus (USB) port, and it is quite compact. Consequently, sensors in this form factor have received significant attention from the scien-tific community, particularly for environment mapping and monitoring applications with UAVs [16], [17].
This article addresses the use of an off-the-shelf quadrotor platform with a commercial off-the-shelf (COTS) RGB-D camera (see Figure 2). The current algorithms based on RGB-D cameras need a large platform and specific customized hardware processors that are not widely available. These algo-rithms generally only work without limitation on laptop or desktop computers [18], [19]. Moreover, there is still a gap be-tween the use of complex algorithms in the research field and their use by naive humans for everyday applications. Google’s Project Tango has bridged these gaps by creating a prototype [20] smartphone [Google (http://www.google.com) 2014; see Figure 3]. The smartphone incorporates an enhanced RGB-D camera with a fisheye lens that has a field of view of 170c and a depth sensor able to capture a dense set of point clouds. It also incorporates customized hardware and software designed to track full 3-D motion while concurrently creating a map of the environment using visual odometry and structure from mo-tion algorithms. These sensors allow the phone to make more than a quarter of a million 3-D measurements every second,updating its position and orientation in real time and combin-ing the data into a single 3-D model of the surrounding space. In comparison, previous works on implementing vision-based algorithms on camera phones are based on marker tracking [21] and localization algorithms [22]. These algorithms are de-signed for augmented-reality applications; they employ camer-as with a limited frame rate and with a small field of view. For these reasons, they are not suitable to deal with the long-term operations and large navigation coverage areas needed in ro-botic tasks.
In this article, a complete architecture representing a first step toward autonomous aerial robot flight with a camera phone is presented. It represents the first “plug-and-play” inte-gration of a consumer product with an off-the-shelf aerial robot to enable autonomy with possible onboard localization, mapping, and control. Thus, it is representative a new class of affordable smart devices that can potentially lower the barrier to automation in homes by providing services for localization,state estimation, control, and mapping. In the future, end users may be able to utilize their smartphone device to autonomous-ly control an aerial platform and to add new functionalities. The first contribution of this work is the development of a quadrotor platform equipped with the Google Project Tango [20] smartphone sensor and a small processor unit. The sec-ond contribution is the vehicle’s control based on smartphone localization estimation. A nonlinear controller guarantees the exponential stability of the platform, which is able to follow trajectories in 3-D space. Finally, the fusion of the phone’s pose with inertial sensor measurements allows for an increased rate of state estimation, and, thus, it enables fast motions.
Our platform of choice was a quadrotor due to its mechanical simplicity [5] and ease of control. Moreover, its ability to oper-ate in confined spaces, hover at any given point in space, and perch or land on a flat surface makes it a very attractive aerial platform with tremendous potential. A description of the proposed hardware and software architecture is presented here.
The experimental platform shown in Figure 5 is made from COTS components and is equipped with an AutoPilot board consisting of an IMU and a user-programmable ARM7 micro-controller. The main computation unit on board is an Odroid-XU (http://www.hardkernel.com) with a 1.7-GHz Samsung Exynos 5 Octa processor with 2 GB of random ac-cess memory (RAM), a 16-GB embedded multimedia control-ler (eMMC) module, and a 802.11n Wi-Fi transceiver. The only other addition to this setup is a forward-pointing Project Tango [20] smartphone from Google and a USB 3.0 cable for communication between the Odroid-XU and the Tango phone. The total mass of the platform is 900 g. The software framework is presented in the “Software Architecture” section.
It should be noted that our experimental setup is indepen-dent of the specifics of the employed embedded board. The processor usage is estimated to be 35% of the total available central processing unit (CPU), which suggests that a smaller and less powerful embedded processor will suffice. However, to guarantee a reliable setup and to reuse the same configura-tion for other robotic tasks, we choose to use the Odroid-XU board. The price is similar to other embedded platforms in the same class. Recent developments by Odroid suggest the possibility of using the less powerful Odroid-W board since the price is four times lower compared with the Odroid-XU, but the ODROID-W has been recently released on the market.
The Odroid-XU performs the following tasks:
●● sensor fusion between Tango’s pose and the vehicle’s IMU
●● nonlinear position-based control.
While both tasks could conceivably have been performed on the Tango device, we used an independent processor to facili-tate the ease of prototyping and to ensure a more reliable ap-proach to state estimation and control at a fixed rate of 100 Hz.
A Java application routine is enabled on the phone for pose streaming using the user datagram protocol (UDP). The Odroid-XU runs a robot operating system (ROS)-based ar-chitecture (http://www.ros.org). The UDP packets are re-ceived by a ROS node and are subsequently converted into ordinary ROS messages. It should be pointed out that the pre-sented strategy allows the vehicle to run all the algorithms on board. The base station is responsible only for visualization and handling user interaction.
Modeling
A quadrotor is a system made of four identical rotors and pro-pellers located at the vertices of a square. The first and the third propellers rotate clockwise, and the second and the fourth propellers rotate counterclockwise (see Figure 6).
Dynamic Model
Let us consider an inertial reference frame denoted by
of mass (COM) of the vehicle denoted by "b 1;
dynamic model of the vehicle can be expressed as
xo = v,
mvo =-Rxe 3 + mge3,
o t
R = RX,
o
JX + X # JX = M,
where x ! R3 is the Cartesian position of the vehicle ex-
pressed in the inertial frame, v ! R3 is the velocity of the vehicle in the inertial frame, m ! R is the mass, X ! R3 is the angular velocity in the body-fixed frame, and J ! R3 is the in-
ertia matrix with respect to the body frame. The hat symbol t$ denotes the skew-symmetry operator according to
xyt = x # y for all x, y ! R3, g is the standard gravitational
acceleration, and e3 =60 0 T M ! R 3
1@ . The total moment
along all axes of the body-fixed frame and the thrust x ! R are control inputs of the plant. The dynamics of rotors and propellers are neglected, and it is assumed that the force of
each propeller is directly controlled. The total thrust, x = /4j =1 f j, acts in the direction of the z-axis of the body- fixed frame, which is orthogonal to the plane defined by the centers of the four propellers. The relationship between single-motor force f j , the total thrust x, and the total moment M can be written as
x V R 1 1 1 1V f1
W S 0 -d W S W
SM1W S 0 dW S f2W (2)
W =S d 0 -d 0 W S W,
M2 S f3
W S W
c -c c W S W
M3 -c X T f4
X T X
Table 1. A glossary of important symbols.
f j ! R Force produced by the jth propeller
x ! R Sum of forces produced by all four propellers
M ! R3 Moments generated by propellers around
x ! R3 body axes Position of robot’s COM
R ! SO(3) Rotation matrix of the vehicle with respect to
Rc ! SO (3)the inertial frame Commanded rotation matrix
m ! R Mass of the vehicle
J ! R Rotational inertia of robot about its COM
X ! R3 Angular velocity of robot in the body frame
Xc ! R3 Commanded angular velocity of robot
a ! R3 in the body frame Linear acceleration of robot in the body frame
g ! R Gravitational acceleration
d ! R Distance of each rotor from the COM
xd ! R3 Desired position
3 Desired velocity
xo d ! R3
xpd ! R Desired acceleration
e x , ev ! R3 Translational errors
e R, eX ! R3 Attitude errors
x ! R13 State estimation vector
ab ! R3 Accelerometer biases
u ! R6 Estimator input
where c a constant value and d is the distance from the COM
to the center of each rotor in the b 1, b2 plane. For nonzero values of d, (2) can be inverted. Our assumption that x and M are the inputs of the plant is, therefore, valid.
Vehicle Control
In most previous works, a back-stepping approach to control is used because the attitude dynamics can be assumed to be faster than the dynamics governing the position and linear-
ized controllers are used for both loops [5], [13]. In this article, because we need to model large excursions from the hover position for robustness, we use a nonlinear controller
based on the work of [23] and [24].
The control inputs x and M are chosen as
M =-k R eR - k X e X + X # JX
t < R c Xc - R
- J ^ X R
x =-^ -k x e x -kv ev - mge 3 +mxpdh $ Re3,
where xpd is the desired acceleration; g is the gravity acceleration; k x , kv, k R , and kX are positive definite terms; and the subscript $ c denotes a commanded value. The quantities
1 < < 0
e R = ^ Rc R - R R c h
2
represent the orientation and angular rate errors, respectively,
whereas translation errors are represented by
e x = x - x d , ev = xo - xo d .
The symbol . 0 represent the vee map so (3) " R 3, which is
the inverse of the operation denoted with the t$ symbol. If the initial attitude error is less than 90°, the zero equilibrium of the tracking errors is exponentially stable, i.e.,
6e x ; ev; e X; eR@ / 60; 0; 0; 0@. Furthermore, if the initial attitude error is between 90° and 180°, then the zero equilibrium of the tracking errors is almost globally exponentially
attractive. See [23] for convergence and stability analysis and [24] for experimental results.
State Estimation
To enable onboard control, an unscented Kalman filter (UKF) estimator is responsible for fusing vision and inertial data. This process allows us to estimate the full pose at 100 Hz,
similar to [12]. The vehicle’s state is estimated by combining the six-degrees-of-freedom pose from the Project Tango [20] smartphone device and the vehicle’s IMU. In the model, the state is represented by
x = 6x T T
v
where the quaternion is represented by the vector
U =6q 0 q 1
and the accelerometer biases by
ab =
6a b x ab y abz@ .
The prediction step uses the input linear acceleration and an-
gular velocity measurements given by the IMU
u = 6XT aT@T, v = 6vXT vaT vbT@T, xk +1 = f^ xk, uk, vkh,
where vk represents the process noise the we assume to be
Gaussian white noise.
Finally, the Tango pose estimates are used to update the
state estimate. We have a linear measurement model
zk = Hxk +hk,
where hk is the observation noise, which is again assumed to be
Gaussian white noise. The measurement model is linear since
the filter’s update is given by the smartphone device, which pro-
vides the absolute position and orientation of the vehicle.
Experimental Results
The experimental tests have been performed in the General Robotics, Automation Sensing, and Perception (GRASP) Lab [5] at the University of Pennsylvania. The considered
working area is a volume of 5 # 4 # 5 m3, and it is composed of 20 T040 motion-capture-system cameras. TheVicon motion-capture system (http://www.vicon.com) pro-
vides a state estimate for the quadrotor, which is considered ground truth in this context. It can be run at or below 375 Hz. The standard deviation for single-marker position
estimates is 50 nm, which is well beyond the accuracy required for flight. Finally, it is a robust system since the provided software assumes that the markers in the models are
rigidly attached, which enables the system to maintain tracking even if all but one camera are occluded. Two sets of tests are proposed—the first considers the stabilization of
the quadrotor at a desired pose, even in the presence of external disturbances, and the second tests the system’s ability to follow a generated trajectory. In both experiments, the
feedback control signal is obtained from the estimation filtering mentioned in the “State Estimation” section at 100 Hz. The results, including controller performance and the precision of the estimation with respect to a Vicon motion capture system, are provided.
System Stabilization
In this experiment, the system is set to hover in a defined 3-D position in space. To show the effectiveness of the proposed controller and estimation pipeline, perturbations are
applied to the vehicle. The vehicle promptly returns to the set point, showing the stability of the controller presented. This behavior is shown in Figure 7, where after the taking-
off phase (the first 5 s), the vehicle reaches the specified altitude and the goal position is set to p = 0.2 2.2 1.2 T m,
T 6 @
while b1d =61 0 0@ . Then, three consecutive perturbations of around 0.7 m along the z-axis, y-axis, and x-axis are noticeable (see orange circles in Figure 7). The corresponding velocity values are shown in Figure 8. After each perturbation (highlighted by the orange dashed el-lipse), the vehicle controller commands changes in velocity to allow the system to return to its original position. The peak velocities are around 2 m/s, and, in each case, the con-troller takes less than 3 s to stabilize, suggesting a closed-loop control bandwidth of around 0.25–0.5 Hz. During each disturbance, the estimator is able to identify the pose and velocity of the system (this is noticeable just comparing the estimation with respect to the Vicon data in Figures 7–9) and reacts promptly, once released, increasing the spatial velocity to return to its hovering position. The performance is compared with the motion capture system, and root mean square error (RMSE) values are reported in Table 2. These results can be considered a first benchmark for this new device. It exhibits strong localization perfor-mances, and, thus, the COTS components are suitable for robotic tasks. The orientation error is evaluated according to [25] as
W^R , Rdh = 12 tr ^I - R Td Rh,
where R is the vehicle’s rotation and Rd the desired one. As shown in Table 2, the proposed filtering technique is able to keep the errors at the same order of magnitude as the Tango de-vice, but with the additional benefit of increased estimation rate, since the pose and the velocity are updated at 100 Hz. In Table 3,
we summarize the velocity RMSE for the three Cartesian com-ponents with respect to the Vicon motion-capture system. The error is small, with an average value of 0.06 m/s.
Autonomous Navigation
In this experiment, the system is set to follow a trajectory in 3-D space. In this way, the dynamic properties of the system can be fully tested. The results confirm the localization prop-erties of the presented architecture and fully validate the pro-posed approach for autonomous navigation in an unknown environment. The trajectory is generated according to previ-ous works [24], [26]. Since the input M is an algebraic func-tion of the fourth derivative of the position (snap), it is convenient to plan smooth trajectories that minimize the snap of the trajectory using the cost functional.
where nd is a constant value, and t0 and t f the inital and final time, respectively. This minimization problem can be formulat-ed as a quadratic program [24]. Furthermore, equality con-straints can be enforced and can be determined by desired robot positions (or velocities). A trajectory of 9 s, presenting an oval shape (see Figure 10), is generated passing through four points
in space (P1 = 6-1 - 1. 5 -1.2@T, P2 = 60 -0.5 -1.2@T,
P3 = 60 0.5 -1.2@T, P4 = 6-1 - 1 .5 -1.2@T, for the x, y,
Table 2. The position and orientation RMSE of the Tango estimation and of the estimation filtering technique compared to Vicon.
Position Tango Estimation (m) UKF Estimation (m)
x 0.0165 0.0195
y 0.0320 0.0296
z 0.0274 0.0299
Orientation Tango Estimation UKF Estimation
W^R , Rdh 0.0627 0.091
Table 3. The velocity RMSE of the estimation filtering technique compared to Vicon.
Component UKF Estimation (m/s)
x 0.0651
y 0.0533
z 0.0788
In Tables 4 and 5, the RMSE values of the Cartesian position and velocity (shown in Figures 11 and 12, respectively),with respect to the planned trajectory, are reported. The error
is relatively small, with maximum values of around 6 cm, and similar to the error obtained during the stabilization task. This confirms the feasibility of using this new device and
the presented approach for real robotic tasks. In the multimedia content available at https://www.youtube
watch?v=WuPZeD2J3pw, the extensive experiments are presented, with different trajectory profiles and maximum velocities reaching values up to 1.5 m/s.
Conclusions
In this article, we demonstrate the hardware and software architecture with the underlying algorithms to enable the plug-and-play functionality with a smartphone and quadrotor. In addition, we demonstrated the autonomous navigation of a quadrotor platform. With data from an external motion-capture system to measure ground truth and desired trajectory values, we show that the robot can navigate in 3-D at speeds of 1–1.5 m/s with an average error of 4 cm in position. Furthermore, we have shown that the system is robust and can recover from external disturbances of 0.7 m. Both these facts show that the system is suitable for reliable indoor navigation. A simple user interface, possibly from another handheld platform, can be used to guide the robot to designated points in a map or to generate a 3-D map. This opens up the vision of a flying robot for every human user (the number of cell phones in the world is already comparable with the world population), and the potential of using aerial robots to generate 3-D maps of buildings, localize sensors in smart buildings, take pictures from different viewpoints, and assist humans in search and rescue applications. Thus, we strongly believe that smartphones will have a strong impact in real life, in terms of advances in the automation of processes such as construction, package delivery, and interaction between humans. The interest of companies such as Google, Apple, and Qualcomm in smartphones equipped with such sensors as 3-D cameras and IMUs suggests that this vision is likely to become a reality.
While the prototype described in this article requires the use of an additional processor to facilitate integration, there is no reason why we should not be able to use the onboard pro-cessor on the smartphone. In the near future, this would allow any user the possibility to use and control any aerial robotic platform with his or her own smartphone. With a robust and accurate control system for autonomous flight, questions of mapping and obstacle avoidance at moderate to high speeds (2–4 m/s) are realistic avenues for future research. Finally, we wish to use multiple devices to enable cooperative localization and mapping of the environment, as already developed for RGB-D cameras (see Figures 13 and 14).
Acknowledgments
This work was supported in part by the TerraSwarm Research Center, one of six centers supported by the STARnet phase of the Focus Center Research Program, a Semiconductor Research Corporation program sponsored by MARCO and the U.S. Defense Advanced Research Projects Agency; Air Force Office of Scientific Research grant FA9550-10-1-0567; Army Research Laboratory grants W911NF-08-2-0004 and W911NF-10-2-0016; Office of Naval Research grants N00014-07-1-0829, N00014-14-1-0510, N00014-09-1-1051, N00014-11-1-0725, and N00014-09-1-103; National Science Foundation grants PFI-1113830, IIS-1138847, and IIS-1328805; and the UPS Foundation.
of companies such as Google, Apple, and Qualcomm in high-end communication devices equipped with such sensors as cameras and inertial measurement units (IMUs) and with sig-nificant computational capability. Google, for instance, is de-veloping a customized phone equipped with conventional as well as depth cameras. This article explores the potential for the rapid integration of inexpensive consumer-grade elec-tronics with the off-the-shelf robotics technology for automa-tion in homes and offices. We describe how standard hardware platforms (robots, processors, and smartphones) can be integrated through simple software architecture to build autonomous quadrotors that can navigate and map un-known, indoor environments. We show how the quadrotor can be stabilized and controlled to achieve autonomous flight and the generation of three-dimensional (3-D) maps for ex-ploring and mapping indoor buildings with application to smart homes, search and rescue, and architecture. This opens up the possibility for any consumer to take a commercially available robot platform and a smartphone and automate the process of creating a 3-D map of his/her home or office.
Automation of Aerial Robotics
The price–performance ratio of processors, sensors, and net-working infrastructure, which has dropped significantly over the last decade, has led to new applications founded on the conver-gence of computation, sensing, and computing. A recent General Electric report [1] calls this convergence the industrial Internet and suggests that the potential macroeconomic benefit from the industrial Internet could be comparable with the eco-nomic productivity gain attributable to the Internet revolution of the late 20th century. Others call it the Internet of Things and predict an economic impact in the tens of trillions of dollars [2]. More than 75% of business leaders surveyed predicted a direct impact of this technology on their business [3]. This conver-gence also holds great promise for automation with robots, which “emphasizes efficiency, productivity, quality, and reliabili-ty, focusing on systems that operate autonomously, often in structured environments over extended periods” [4].
In this article, we address this confluence of technologies enabling automation in the context of aerial robotics, a field that has also seen dramatic advances over the last decade. The same drop in price–performance ratio of processors and sen-sors has fueled the development of micro unmanned aerial ve-hicles (UAVs) that are between 0.1 and 1 m in length and 0.1–2 kg in mass. These low-cost platforms are easy to manu-facture in contrast to the expensive UAVs used for military ap-plications. The number of Predators and Global Hawks is estimated to be around 1,000. Growth in the consumer elec-tronics industry (millions or billions of components at low cost) has resulted in inexpensive hardware for sensing and computation. These advances, coupled with open-source tools for building robots like quadrotors [5], have led to innovative low-cost toys and hobby kits (e.g., diydrones.org). The real op-portunity for robotics and automation is in leveraging these tools (and the convergence of computation, sensing, and com-munication) to develop economical (compared with the mili-tary counterparts), functional, and robust aerial robots. These can be used in such tasks as inspection [6], interaction with the environment [7], [8], search and rescue [9], construction [10], [11], and mapping of homes and offices.
Early quadrotor vehicles were primarily experimental sys-tems, but improved design and software tools have lead to sig-nificant increases in reliability and reductions in cost. Today, quadrotors have reached the maturity of consumer-grade de-vices. To reiterate, this is in large part due to the decreasing price–performance ratio of sensors for autonomous navigation, including global positioning systems (GPSs), cameras, IMUs, and laser scanners [7], [12]–[16]. In this context, low-cost range sensors offer an attractive alternative to high-end laser scanners and 3-D cameras for applications such as indoor navigation and mapping, surveillance, and autonomous robotics.
Consumer-grade range sensing technology has led to many devices becoming available on the market, like the Microsoft Kinect sensor and the ASUS Xtion sensor (PrimeSense 2010; see Figure 1). The richness of the provided data and the low cost of the sensor have attracted many researchers from the fields of mapping, 3-D modeling, and reconstruction. The ASUS Xtion sensor boasts a lower weight than the first genera-tion of red, green, blue, and depth (RGB-D) cameras (around 70 g without the external casing). (While this specific sensor is no longer available, there are others under development that are likely to be available in the future.) It does not require ex-ternal power beyond a standard universal serial bus (USB) port, and it is quite compact. Consequently, sensors in this form factor have received significant attention from the scien-tific community, particularly for environment mapping and monitoring applications with UAVs [16], [17].
This article addresses the use of an off-the-shelf quadrotor platform with a commercial off-the-shelf (COTS) RGB-D camera (see Figure 2). The current algorithms based on RGB-D cameras need a large platform and specific customized hardware processors that are not widely available. These algo-rithms generally only work without limitation on laptop or desktop computers [18], [19]. Moreover, there is still a gap be-tween the use of complex algorithms in the research field and their use by naive humans for everyday applications. Google’s Project Tango has bridged these gaps by creating a prototype [20] smartphone [Google (http://www.google.com) 2014; see Figure 3]. The smartphone incorporates an enhanced RGB-D camera with a fisheye lens that has a field of view of 170c and a depth sensor able to capture a dense set of point clouds. It also incorporates customized hardware and software designed to track full 3-D motion while concurrently creating a map of the environment using visual odometry and structure from mo-tion algorithms. These sensors allow the phone to make more than a quarter of a million 3-D measurements every second,updating its position and orientation in real time and combin-ing the data into a single 3-D model of the surrounding space. In comparison, previous works on implementing vision-based algorithms on camera phones are based on marker tracking [21] and localization algorithms [22]. These algorithms are de-signed for augmented-reality applications; they employ camer-as with a limited frame rate and with a small field of view. For these reasons, they are not suitable to deal with the long-term operations and large navigation coverage areas needed in ro-botic tasks.
In this article, a complete architecture representing a first step toward autonomous aerial robot flight with a camera phone is presented. It represents the first “plug-and-play” inte-gration of a consumer product with an off-the-shelf aerial robot to enable autonomy with possible onboard localization, mapping, and control. Thus, it is representative a new class of affordable smart devices that can potentially lower the barrier to automation in homes by providing services for localization,state estimation, control, and mapping. In the future, end users may be able to utilize their smartphone device to autonomous-ly control an aerial platform and to add new functionalities. The first contribution of this work is the development of a quadrotor platform equipped with the Google Project Tango [20] smartphone sensor and a small processor unit. The sec-ond contribution is the vehicle’s control based on smartphone localization estimation. A nonlinear controller guarantees the exponential stability of the platform, which is able to follow trajectories in 3-D space. Finally, the fusion of the phone’s pose with inertial sensor measurements allows for an increased rate of state estimation, and, thus, it enables fast motions.
System Architecture
Our platform of choice was a quadrotor due to its mechanical simplicity [5] and ease of control. Moreover, its ability to oper-ate in confined spaces, hover at any given point in space, and perch or land on a flat surface makes it a very attractive aerial platform with tremendous potential. A description of the proposed hardware and software architecture is presented here.
Hardware Architecture
The experimental platform shown in Figure 5 is made from COTS components and is equipped with an AutoPilot board consisting of an IMU and a user-programmable ARM7 micro-controller. The main computation unit on board is an Odroid-XU (http://www.hardkernel.com) with a 1.7-GHz Samsung Exynos 5 Octa processor with 2 GB of random ac-cess memory (RAM), a 16-GB embedded multimedia control-ler (eMMC) module, and a 802.11n Wi-Fi transceiver. The only other addition to this setup is a forward-pointing Project Tango [20] smartphone from Google and a USB 3.0 cable for communication between the Odroid-XU and the Tango phone. The total mass of the platform is 900 g. The software framework is presented in the “Software Architecture” section.
It should be noted that our experimental setup is indepen-dent of the specifics of the employed embedded board. The processor usage is estimated to be 35% of the total available central processing unit (CPU), which suggests that a smaller and less powerful embedded processor will suffice. However, to guarantee a reliable setup and to reuse the same configura-tion for other robotic tasks, we choose to use the Odroid-XU board. The price is similar to other embedded platforms in the same class. Recent developments by Odroid suggest the possibility of using the less powerful Odroid-W board since the price is four times lower compared with the Odroid-XU, but the ODROID-W has been recently released on the market.
Software Architecture
The Odroid-XU performs the following tasks:
●● sensor fusion between Tango’s pose and the vehicle’s IMU
●● nonlinear position-based control.
While both tasks could conceivably have been performed on the Tango device, we used an independent processor to facili-tate the ease of prototyping and to ensure a more reliable ap-proach to state estimation and control at a fixed rate of 100 Hz.
A Java application routine is enabled on the phone for pose streaming using the user datagram protocol (UDP). The Odroid-XU runs a robot operating system (ROS)-based ar-chitecture (http://www.ros.org). The UDP packets are re-ceived by a ROS node and are subsequently converted into ordinary ROS messages. It should be pointed out that the pre-sented strategy allows the vehicle to run all the algorithms on board. The base station is responsible only for visualization and handling user interaction.
Modeling
A quadrotor is a system made of four identical rotors and pro-pellers located at the vertices of a square. The first and the third propellers rotate clockwise, and the second and the fourth propellers rotate counterclockwise (see Figure 6).
Dynamic Model
Let us consider an inertial reference frame denoted by
of mass (COM) of the vehicle denoted by "b 1;
dynamic model of the vehicle can be expressed as
xo = v,
mvo =-Rxe 3 + mge3,
o t
R = RX,
o
JX + X # JX = M,
where x ! R3 is the Cartesian position of the vehicle ex-
pressed in the inertial frame, v ! R3 is the velocity of the vehicle in the inertial frame, m ! R is the mass, X ! R3 is the angular velocity in the body-fixed frame, and J ! R3 is the in-
ertia matrix with respect to the body frame. The hat symbol t$ denotes the skew-symmetry operator according to
xyt = x # y for all x, y ! R3, g is the standard gravitational
acceleration, and e3 =60 0 T M ! R 3
1@ . The total moment
along all axes of the body-fixed frame and the thrust x ! R are control inputs of the plant. The dynamics of rotors and propellers are neglected, and it is assumed that the force of
each propeller is directly controlled. The total thrust, x = /4j =1 f j, acts in the direction of the z-axis of the body- fixed frame, which is orthogonal to the plane defined by the centers of the four propellers. The relationship between single-motor force f j , the total thrust x, and the total moment M can be written as
x V R 1 1 1 1V f1
W S 0 -d W S W
SM1W S 0 dW S f2W (2)
W =S d 0 -d 0 W S W,
M2 S f3
W S W
c -c c W S W
M3 -c X T f4
X T X
Table 1. A glossary of important symbols.
f j ! R Force produced by the jth propeller
x ! R Sum of forces produced by all four propellers
M ! R3 Moments generated by propellers around
x ! R3 body axes Position of robot’s COM
R ! SO(3) Rotation matrix of the vehicle with respect to
Rc ! SO (3)the inertial frame Commanded rotation matrix
m ! R Mass of the vehicle
J ! R Rotational inertia of robot about its COM
X ! R3 Angular velocity of robot in the body frame
Xc ! R3 Commanded angular velocity of robot
a ! R3 in the body frame Linear acceleration of robot in the body frame
g ! R Gravitational acceleration
d ! R Distance of each rotor from the COM
xd ! R3 Desired position
3 Desired velocity
xo d ! R3
xpd ! R Desired acceleration
e x , ev ! R3 Translational errors
e R, eX ! R3 Attitude errors
x ! R13 State estimation vector
ab ! R3 Accelerometer biases
u ! R6 Estimator input
where c a constant value and d is the distance from the COM
to the center of each rotor in the b 1, b2 plane. For nonzero values of d, (2) can be inverted. Our assumption that x and M are the inputs of the plant is, therefore, valid.
Vehicle Control
In most previous works, a back-stepping approach to control is used because the attitude dynamics can be assumed to be faster than the dynamics governing the position and linear-
ized controllers are used for both loops [5], [13]. In this article, because we need to model large excursions from the hover position for robustness, we use a nonlinear controller
based on the work of [23] and [24].
The control inputs x and M are chosen as
M =-k R eR - k X e X + X # JX
t < R c Xc - R
- J ^ X R
x =-^ -k x e x -kv ev - mge 3 +mxpdh $ Re3,
where xpd is the desired acceleration; g is the gravity acceleration; k x , kv, k R , and kX are positive definite terms; and the subscript $ c denotes a commanded value. The quantities
1 < < 0
e R = ^ Rc R - R R c h
2
represent the orientation and angular rate errors, respectively,
whereas translation errors are represented by
e x = x - x d , ev = xo - xo d .
The symbol . 0 represent the vee map so (3) " R 3, which is
the inverse of the operation denoted with the t$ symbol. If the initial attitude error is less than 90°, the zero equilibrium of the tracking errors is exponentially stable, i.e.,
6e x ; ev; e X; eR@ / 60; 0; 0; 0@. Furthermore, if the initial attitude error is between 90° and 180°, then the zero equilibrium of the tracking errors is almost globally exponentially
attractive. See [23] for convergence and stability analysis and [24] for experimental results.
State Estimation
To enable onboard control, an unscented Kalman filter (UKF) estimator is responsible for fusing vision and inertial data. This process allows us to estimate the full pose at 100 Hz,
similar to [12]. The vehicle’s state is estimated by combining the six-degrees-of-freedom pose from the Project Tango [20] smartphone device and the vehicle’s IMU. In the model, the state is represented by
x = 6x T T
v
where the quaternion is represented by the vector
U =6q 0 q 1
and the accelerometer biases by
ab =
6a b x ab y abz@ .
The prediction step uses the input linear acceleration and an-
gular velocity measurements given by the IMU
u = 6XT aT@T, v = 6vXT vaT vbT@T, xk +1 = f^ xk, uk, vkh,
where vk represents the process noise the we assume to be
Gaussian white noise.
Finally, the Tango pose estimates are used to update the
state estimate. We have a linear measurement model
zk = Hxk +hk,
where hk is the observation noise, which is again assumed to be
Gaussian white noise. The measurement model is linear since
the filter’s update is given by the smartphone device, which pro-
vides the absolute position and orientation of the vehicle.
Experimental Results
The experimental tests have been performed in the General Robotics, Automation Sensing, and Perception (GRASP) Lab [5] at the University of Pennsylvania. The considered
working area is a volume of 5 # 4 # 5 m3, and it is composed of 20 T040 motion-capture-system cameras. TheVicon motion-capture system (http://www.vicon.com) pro-
vides a state estimate for the quadrotor, which is considered ground truth in this context. It can be run at or below 375 Hz. The standard deviation for single-marker position
estimates is 50 nm, which is well beyond the accuracy required for flight. Finally, it is a robust system since the provided software assumes that the markers in the models are
rigidly attached, which enables the system to maintain tracking even if all but one camera are occluded. Two sets of tests are proposed—the first considers the stabilization of
the quadrotor at a desired pose, even in the presence of external disturbances, and the second tests the system’s ability to follow a generated trajectory. In both experiments, the
feedback control signal is obtained from the estimation filtering mentioned in the “State Estimation” section at 100 Hz. The results, including controller performance and the precision of the estimation with respect to a Vicon motion capture system, are provided.
System Stabilization
In this experiment, the system is set to hover in a defined 3-D position in space. To show the effectiveness of the proposed controller and estimation pipeline, perturbations are
applied to the vehicle. The vehicle promptly returns to the set point, showing the stability of the controller presented. This behavior is shown in Figure 7, where after the taking-
off phase (the first 5 s), the vehicle reaches the specified altitude and the goal position is set to p = 0.2 2.2 1.2 T m,
T 6 @
while b1d =61 0 0@ . Then, three consecutive perturbations of around 0.7 m along the z-axis, y-axis, and x-axis are noticeable (see orange circles in Figure 7). The corresponding velocity values are shown in Figure 8. After each perturbation (highlighted by the orange dashed el-lipse), the vehicle controller commands changes in velocity to allow the system to return to its original position. The peak velocities are around 2 m/s, and, in each case, the con-troller takes less than 3 s to stabilize, suggesting a closed-loop control bandwidth of around 0.25–0.5 Hz. During each disturbance, the estimator is able to identify the pose and velocity of the system (this is noticeable just comparing the estimation with respect to the Vicon data in Figures 7–9) and reacts promptly, once released, increasing the spatial velocity to return to its hovering position. The performance is compared with the motion capture system, and root mean square error (RMSE) values are reported in Table 2. These results can be considered a first benchmark for this new device. It exhibits strong localization perfor-mances, and, thus, the COTS components are suitable for robotic tasks. The orientation error is evaluated according to [25] as
W^R , Rdh = 12 tr ^I - R Td Rh,
where R is the vehicle’s rotation and Rd the desired one. As shown in Table 2, the proposed filtering technique is able to keep the errors at the same order of magnitude as the Tango de-vice, but with the additional benefit of increased estimation rate, since the pose and the velocity are updated at 100 Hz. In Table 3,
we summarize the velocity RMSE for the three Cartesian com-ponents with respect to the Vicon motion-capture system. The error is small, with an average value of 0.06 m/s.
Autonomous Navigation
In this experiment, the system is set to follow a trajectory in 3-D space. In this way, the dynamic properties of the system can be fully tested. The results confirm the localization prop-erties of the presented architecture and fully validate the pro-posed approach for autonomous navigation in an unknown environment. The trajectory is generated according to previ-ous works [24], [26]. Since the input M is an algebraic func-tion of the fourth derivative of the position (snap), it is convenient to plan smooth trajectories that minimize the snap of the trajectory using the cost functional.
where nd is a constant value, and t0 and t f the inital and final time, respectively. This minimization problem can be formulat-ed as a quadratic program [24]. Furthermore, equality con-straints can be enforced and can be determined by desired robot positions (or velocities). A trajectory of 9 s, presenting an oval shape (see Figure 10), is generated passing through four points
in space (P1 = 6-1 - 1. 5 -1.2@T, P2 = 60 -0.5 -1.2@T,
P3 = 60 0.5 -1.2@T, P4 = 6-1 - 1 .5 -1.2@T, for the x, y,
Table 2. The position and orientation RMSE of the Tango estimation and of the estimation filtering technique compared to Vicon.
Position Tango Estimation (m) UKF Estimation (m)
x 0.0165 0.0195
y 0.0320 0.0296
z 0.0274 0.0299
Orientation Tango Estimation UKF Estimation
W^R , Rdh 0.0627 0.091
Table 3. The velocity RMSE of the estimation filtering technique compared to Vicon.
Component UKF Estimation (m/s)
x 0.0651
y 0.0533
z 0.0788
In Tables 4 and 5, the RMSE values of the Cartesian position and velocity (shown in Figures 11 and 12, respectively),with respect to the planned trajectory, are reported. The error
is relatively small, with maximum values of around 6 cm, and similar to the error obtained during the stabilization task. This confirms the feasibility of using this new device and
the presented approach for real robotic tasks. In the multimedia content available at https://www.youtube
watch?v=WuPZeD2J3pw, the extensive experiments are presented, with different trajectory profiles and maximum velocities reaching values up to 1.5 m/s.
Conclusions
In this article, we demonstrate the hardware and software architecture with the underlying algorithms to enable the plug-and-play functionality with a smartphone and quadrotor. In addition, we demonstrated the autonomous navigation of a quadrotor platform. With data from an external motion-capture system to measure ground truth and desired trajectory values, we show that the robot can navigate in 3-D at speeds of 1–1.5 m/s with an average error of 4 cm in position. Furthermore, we have shown that the system is robust and can recover from external disturbances of 0.7 m. Both these facts show that the system is suitable for reliable indoor navigation. A simple user interface, possibly from another handheld platform, can be used to guide the robot to designated points in a map or to generate a 3-D map. This opens up the vision of a flying robot for every human user (the number of cell phones in the world is already comparable with the world population), and the potential of using aerial robots to generate 3-D maps of buildings, localize sensors in smart buildings, take pictures from different viewpoints, and assist humans in search and rescue applications. Thus, we strongly believe that smartphones will have a strong impact in real life, in terms of advances in the automation of processes such as construction, package delivery, and interaction between humans. The interest of companies such as Google, Apple, and Qualcomm in smartphones equipped with such sensors as 3-D cameras and IMUs suggests that this vision is likely to become a reality.
While the prototype described in this article requires the use of an additional processor to facilitate integration, there is no reason why we should not be able to use the onboard pro-cessor on the smartphone. In the near future, this would allow any user the possibility to use and control any aerial robotic platform with his or her own smartphone. With a robust and accurate control system for autonomous flight, questions of mapping and obstacle avoidance at moderate to high speeds (2–4 m/s) are realistic avenues for future research. Finally, we wish to use multiple devices to enable cooperative localization and mapping of the environment, as already developed for RGB-D cameras (see Figures 13 and 14).
Acknowledgments
This work was supported in part by the TerraSwarm Research Center, one of six centers supported by the STARnet phase of the Focus Center Research Program, a Semiconductor Research Corporation program sponsored by MARCO and the U.S. Defense Advanced Research Projects Agency; Air Force Office of Scientific Research grant FA9550-10-1-0567; Army Research Laboratory grants W911NF-08-2-0004 and W911NF-10-2-0016; Office of Naval Research grants N00014-07-1-0829, N00014-14-1-0510, N00014-09-1-1051, N00014-11-1-0725, and N00014-09-1-103; National Science Foundation grants PFI-1113830, IIS-1138847, and IIS-1328805; and the UPS Foundation.