Toward Image Based Visual Servoing for Aerial Grasping and Perching

Justin Thomas, Giuseppe Loianno, Koushil Sreenath, and Vijay Kumar

Abstract— This paper addresses the dynamics, control, plan-

ning, and visual servoing for micro aerial vehicles to perform
high-speed aerial grasping tasks. We draw inspiration from
agile, fast-moving birds, such as raptors, that detect, locate,
and execute high-speed swoop maneuvers to capture prey.
Since these grasping maneuvers are predominantly in the
sagittal plane, we consider the planar system and present
mathematical models and algorithms for motion planning
and control, required to incorporate similar capabilities in Fig. 1. A Red Kite swoops down and uses visual feedback to approach,
quadrotors equipped with a monocular camera. In particular, grasp, and retrieve food on the ground [8].
we develop a dynamical model directly in the image space,
show that this is a differentially-flat system with the image this task with a minimal sensor payload and consideration
features serving as flat outputs, outline a method for generating of the dynamics of the system [2], [3].
trajectories directly in the image feature space, develop a In scenarios like this, a monocular camera is an inexpen-
geometric visual controller that considers the second order sive and versatile sensor of choice, especially when com-
dynamics (in contrast to most visual servoing controllers that
assume first order dynamics), and present validation of our bined with an Inertial Measurement Unit (IMU) [4], [5]. Such
methods through both simulations and experiments. applications requiring control feedback using a single camera
motivate either Position Based Visual Servoing (PBVS) or
I. I NTRODUCTION Image Based Visual Servoing (IBVS) [6]. PBVS requires an
explicit estimation of the pose of the robot in the inertial
The dexterity and adaptability of living creatures far frame while IBVS acts directly using feedback from the
exceeds what we see in modern robots but provides great image coordinates. In particular, a single monocular camera
inspiration for avenues of research. For example, birds of is sufficient for visual servoing when there is some known
prey can not only cover great distances by flight, but they are geometry or structure in the environment. Further, visual
also excellent hunters and are able to perch and walk. Some servoing can even be effective in an unknown environment
raptors exhibit great speed and agility, and they use visual while remaining insensitive to camera calibration [7].
target recognition and perception-action loops to swoop Our goal is to ascribe to aerial vehicles the ability to fly
down to grasp their prey [1]. For example, see Fig. 1 for above, grasp, or perch on a target using vision. The platform
still images of a Red Kite swooping to capture a target. considered is a quadrotor micro aerial vehicle (MAV) which
Such capacities would be advantageous for robots in aerial is similar to a helicopter, but has four rotors [9]. The
manipulation tasks. For example, placing sensors quickly, quadrotor platform is appealing because of its mechanical
perching to save energy, and dynamic grasping to acquire simplicity, agility, and well understood dynamics [10].
and transport materials or other robots, are a few tasks that Despite the fact that it is underactuated, it is possible
increase the usefulness of aerial robots and require similar to design controllers that will guarantee convergence from
capabilities to those of raptors. Aerial robots, however, do almost any point on SE(3), the Euclidean motion group.
not yet have a comparable rich set of capabilities, and they In our group’s previous work, similar controllers have also
are limited partially by low strength/weight actuators, heavy been derived for a quadrotor carrying a cable-suspended
materials, and batteries with low energy density and low payload [10]. However, both of these approaches require full
specific power. In addition, our poor understanding of the knowledge of the state. Therefore, our goal is to use similar
perception-action loops required for agile flight and manipu- approaches with the dynamics of the system directly in the
lation remains a limiting factor. For grasping or perching, image plane (rather than in the Cartesian space) to develop
the robot must be able to detect the object of interest an IBVS controller based on visual features of a cylinder.
and subsequently use visual feedback to control the robot’s We will also demonstrate a method to generate dynamically-
motion. To maintain agility, the robot must also accomplish feasible trajectories in the image plane.
There are many excellent tutorials on visual servoing [11],
*We gratefully acknowledge the support of ARL grant W911NF-08-2- [6], [12], [13]. However, most approaches are limited to first-
0004 and ONR grants N00014-07-1-0829 and N00014-08-1-0696.
J. Thomas, G. Loianno, and V. Kumar are with the GRASP Lab, order or fully-actuated systems. For example, Taylor and
University of Pennsylvania, Philadelphia, PA, USA {jut, loiannog, Ostrowski demonstrated robustness to camera calibration,
kumar}@seas.upenn.edu but only considered a first-order system [7]. Cowan et al.
K. Sreenath is with the Depts. of Mechanical Engineering and
Robotics Institute, Carnegie Mellon University, Pittsburgh, PA, USA proved stability for second order systems, but assumed a fully
koushils@cmu.edu actuated system [14]. More recently, Hamel and Mahoney
 II. V ISION
In this section, we present an overview of the vision
system, outline the camera model, and derive the geometric
constraints on the cylinder detection in the image plane.
A. Problem Formulation
The problem is formulated in the sagittal plane, which
Fig. 2. Top: A bald eagle snatches a fish from the water [18]. Bottom: A
is the dominant plane of actuation for aerial grasping ma-
quadrotor dynamically grasps at 3 m/s using a custom gripper [2]. neuvers in nature and enables high-speed dynamic grasping
for aerial robots [2]. This also allows us to simplify the
problem. When considering the sagittal plane, two image
leveraged a spherical camera model and utilized backstep-
features are sufficient to establish a relationship between
ping to design non-linear controllers for a specific class of
the vision system and the pose of the robot. These features
underactuated second-order systems [15], [16]. As is typical
could be any two known points; however, it is not always
in backstepping, however, it is necessary to assume that the
possible to have such a structured environment in practice.
inner control loops are significantly faster than the outer
Conveniently, cylinders with known (or estimated) radii are
ones. Some preliminary efforts have been made to enable
fairly common and provide sufficient structure for visual
autonomous landing, but require an estimate of velocity in
servoing and grasping.
the inertial frame using an external motion capture system
We will use the following nomenclature. Let T be the
[17]. Therefore, there is a lack of IBVS controllers which can
homogeneous transformation matrix from the camera frame
handle the dynamic motion required for aggressive grasping
to the world frame, fi denote a focal length in the ith
and perching.
direction, ci be the center image pixel in the ith direction,
In this paper, we build upon existing IBVS literature,
and λ be an arbitrary scaling factor.
generalizing from the typical first-order fully actuated system
to a higher-order underactuated system, and we develop B. Camera Model
IBVS controllers for dynamic vision-based grasping and The camera is modeled using a standard pinhole perspec-
perching. In our previous work, we demonstrated a proof- tive camera model
of-concept bio-inspired gripper which, when attached to a  T so that a generic point in the world,
X Y Z 1 , is projected onto the image plane,
quadrotor, enabled dynamic grasping of stationary objects as  0 0
T
displayed in Fig. 2 while moving at 3 m/s (or 9 body lengths x y 1 , according to [20] such that
/ s) [2]. However, this work required a motion capture system
 
 0  X
[19] and therefore restricted such aggressive maneuvers to a x  Y 
structured environment. λ  y 0  = KP0 T −1   Z ,

1
The primary contribution of this paper is to enable 1
high-speed grasping maneuvers by developing a dynamical
where
model directly in the image space, showing that this is a  
differentially-flat system with the image features serving as fx 0 cx  
flat outputs, developing a geometric visual controller that K= 0 fy cy  , P0 = I3×3 03×1 .
considers the second order dynamics (in contrast to most 0 0 1
visual servoing controllers that assume first order dynam- From here on, we will use the calibrated image coordinates,
ics), and presenting validation of our methods through both    0 
simulations and experiments1 . x x
 y  = K −1  y 0  ,
The rest of the paper is structured as follows. In Section
II, a mapping from Cartesian space to the image space is 1 1
presented. Section III develops a dynamical model in the which are equivalent to the transformation and projection of
image space, while Section IV establishes that the system points in the world to an image plane with unity focal length
is differentially-flat, with the visual features being the flat and a centered coordinate system.
outputs, and presents a trajectory generation method. Section
V develops an IBVS controller for the full dynamical system C. Geometry
using geometric techniques. Finally, Sections VI and VII Let the image features be the points whose rays are tangent
present simulation and experimental results, respectively. to the cylinder and lie in the vertical plane. In contrast to
typical visual servoing approaches, these points are now a
1 It must be noted that grasping maneuvers are predominanty in the sagittal function of the position of the robot. Therefore, we cannot
plane and thus our developed models and algorithms for motion planning use the standard image Jacobian as in [12], which assumes
and control are based on a planar model (x − z plane). However, since the target points are stationary in the inertial frame.
the experimental system is 3D, we will use a Vicon-based motion capture
system to ensure stability for the yaw and the y-axis dynamics. The x − z We now formulate the mapping between the image fea-
dynamics will be stabilized thorugh our developed IBVS controller. tures and the robot pose. Let rq = (xq , zq )T ∈ R2 denote the
  
zq dΓ (rq ) ∂ dΓ (rq )
v̇ = = ṙq ≡ J ṙq ,
✓ xc xq dt ∂ ṙq dt
where J is the image Jacobian [22]. Note that J can be
v1,m zc f expressed as a function of either the image coordinates or
x
(xt,2 , zt,2 ) the position of the robot by using (3) and the fact that
z rq v2,m Γ is invertible. Having established a mapping between the
x Cartesian coordinates and the image coordinates, we will
(xt,1 , zt,1 ) next develop a dynamic model of the quadrotor system
Fig. 3. We assume, without loss of generality, that the target is located at directly in the image coordinates.
the origin and the quadrotor is located at (xq , zq ). The focal length of the
camera, fx , defines the location of the image plane relative to the quadrotor III. DYNAMICS
and the image coordinates are given by v1 and v2 . The optical ray tangent The dynamics of this quadrotor system are well known in
to the target intersects the target at (xt , zt ). The coordinate system of the
camera is indicated by xc and zc . literature. For simplicity, we restrict the robot to the vertical
(x − z) plane as shown in Fig. 4 and we assume the gripper
position of the quadrotor in the inertial frame with the target is massless (See [23] for the complete 3-D dynamic model).
cylinder at the origin, as shown in Fig. 3. Let Rt denote the We define
radius of the target cylinder, and rt,i ∈ R2 be a point on the
   
xq rq
circumference that has a tangent passing through the focal rq = , wq =
zq θ
point. With the camera at the same position as the quadrotor,
where rq is the position of the quadrotor and θ is the pitch
we have two geometric constraints,
angle. Then, the dynamics in the inertial frame take the form
krt k2 = Rt
Dẅ + C ẇ + G = F (4)
2 2
krq k2 = krq − rt k2 + Rt2 . 3×3
where D ∈ R is a diagonal inertial tensor because the
Knowing the radius of the cylinder, Rt , these equations have robot frame is aligned with the principal axes of the inertia.
two solutions which represent the two tangent points, In this case, centripetal and Coriolis terms, C ∈ R3×3 , are
zero. Gravity appears in G ∈ R3×1 , and F ∈ R3×1 is
 
s 2
Rt2  xq
  
−zq krq k
rt,i = ± − 1 . (1)
 
2 f Re2
krq k zq xq Rt2 F=
M
Unfortunately, the features in the image plane are coupled where R ∈ SO(2), f ∈ R is the total thrust, e2 =
with the attitude, and if not compensated, would not allow  T
0 1 , and M is the pitch moment generated from
for the desired attitude-decoupled mapping between the state the difference of thrusts between the front and rear rotors
of the robot and the image features. Therefore, we map as depicted in Fig. 4. Since the system has three degrees
the calibrated image coordinates to coordinates on a virtual of freedom, given by wq , and only two control inputs that
level image plane similar to [21] by rotating the camera appear in F, the system is underactuated.
coordinate system to a virtual frame where θ = 0. Then, the
f3
problem can be formulated in the rotated coordinate system
where the virtual image plane is level. The virtual calibrated f Re2
coordinates of the features can then be computed using (1) M f1
and   ✓
  xt,i rq
vi  0  z
λ  0  = P0 T −1  
 zt,i  (2)
1 x
1
Fig. 4. The total thrust is f = 4i=1 fi and is in the direction of Re2 .
P

with the appropriate transformation T ∈ SE(3). The virtual The moment, M , is the result of the thrust difference between f3 and f1 .
coordinates, v = [v1 , v2 ]T , in (2) provide two equations Now, ṙq and r̈q can be expressed as functions of the image
which can be solved to determine the robot and camera coordinates using the inverse of the image Jacobian, J. Then,
position as a function of the virtual image coordinates. the dynamics in (4) can be expressed in terms of the image
We also define the space S = {rq ∈ R2 | 2Rt ≤ krq k ≤
coordinates
Br , zq > 0}, such that the quadrotor’s position is bounded
˙ −1 v̇
 −1  −1 
J v̈ − J −1 JJ

below by 2Rt and bounded above by Br , and the quadrotor is J v̇
D +C +G=F
always above the horizontal plane. Then, there exists V ⊂ R2 θ̈ θ̇
and a smooth global diffeomorphism Γ : S −→ V such that which simplifies to:
1
 q 
krq k2
fx x z + R 2
− 1 ˙ −1 v̇
v̈ = J [f Re2 − GA ] + JJ (5)
v= 2  q q t 2
q Rt 2  ≡ Γ (rq ) , (3) m
zq − Rt2 xq zq − R 2 krq k
−1
t
2 Rt D3 θ̈ = M (6)
where GA denotes the upper left 2 × 2 block of G, D3 is constraints on attitude and the field of view must be written
the {3,3} element of D, and m is the mass of the robot. as inequalities in the virtual image plane. Having obtained
Equation (5) presents the translational dynamics directly in a trajectory in the image space, we next develop a visual
the image coordinates. Next, we will demonstrate that these controller that will track this trajectory.
coordinates form a set of flat outputs for the system, enabling
V. C ONTROL
trajectory design directly in the image space.
A. Attitude Controller
IV. D IFFERENTIAL F LATNESS AND P LANNING First, let Rd ∈ SO(2) denote the desired rotation matrix
In this section, we formulate the trajectory planning prob- defined by a desired attitude, θd . Then, we define attitude
lem in the image plane and compute trajectories that satisfy errors
the dynamic constraints (i.e. are dynamically feasible) in (5) 1
∨
eR = RdT R − RT Rd = sin(θ − θd )
and (6). The central idea relies on the differential flatness 2
(see [24] for definition) property of our system. We will eΩ = Ω − RT Rd Ωd = θ̇ − θ̇d
show that there is a set of so-called “flat outputs” such that
there is a diffeomorphism from the vector of state variables where ∨ is the “vee” map defined in [26]. These errors are
and inputs to the vector of flat outputs and its derivatives. In similar to [26] but simplified for the planar case. Also, we
practice, this means that dynamically feasible trajectories can define a configuration error function as
be generated by considering sufficiently smooth (the order is 1 
Ψ (R, Rd ) = tr I − RdT R .

defined by the map) trajectories in the space of flat outputs. In 2
this paper, we show that the image coordinates, v ∈ V ⊂ R2 , The attitude controller is then given as below.
serve as flat outputs. Proposition 1: [26, Prop. 1] (Exponential Stability of At-
First, there exists a diffeomorphism between the image titude Controlled Flight Mode) Consider the control moment
coordinates and the position of the robot, namely Γ as defined defined as
in (3). From (5), we get:
  M = −KR eR − KΩ eΩ + D3 θ̈d ,
f Re2 = mJ −1 v̈ − JJ ˙ −1 v̇ + GA ,
where KR and KΩ are positive scalars. Further, suppose the
initial conditions satisfy
and  
F1 Ψ(R(0), Rd (0)) < 2,
f = kFA k , θ = arctan .
F2 2 2
keΩ (0)k < kR (2 − Ψ(R(0), Rd (0))) .
D3
 
˙ −1 v̇ + GA . Further,
where FA = mJ −1 v̈ − JJ
Then, (eR , eΩ ) = (0, 0) is exponentially stable for the
1 closed-loop system.
f˙ = eT2 RT ḞA , θ̇ = eT1 RT ḞA .
f Proof: Follows from [26, Prop. 1]. See http://www.
jtwebs.net/ICRA-2014/ for details.
Since f appears in the denominator, we require that f > 0
so that the thrust is always positive. From (6) and one more B. Position Control
derivative, we obtain: Let errors in the image plane be defined by
1 T T 
M = D3 e1 R F̈A − 2f˙θ̇ . (7) ev = v − vd .
f
Then, using (5), the image space error dynamics are
Thus, all state variables and inputs can be written as func-
tions of the image coordinates and their derivatives. Since ˙ −1 v̇ − mv̈d .
mëv = f JRe2 − JGA + mJJ
we require the 4th derivative of the image coordinates (see
The visual servoing controller in then given as below.
(7)), the planned trajectories in the image plane must be at
Proposition 2: (Exponential Stability of Visual Feature
least C 4 continuous.
Controlled Flight Mode) Consider the total thrust component
Following previous work [25], [2], since the input M is
along the current body frame vertical axis defined by
an algebraic function of the fourth derivative (snap), it is
natural to plan smooth trajectories that minimize the snap of f = A · Re2 .
the trajectory using the cost functional,
where A = GA + mJ −1 [−Kp ev − Kd ėv + v̈d ], and the
Z tf
(4)
2 commanded attitude given by
Ji = vi (t) dt.
t0 A
Rc e2 = .
Choosing a polynomial basis allows the minimization to kAk
be formulated as a Quadratic Program (QP) [25]. Further, Then, the zero equilibrium (ev , ėv , eR , eΩ ) = (0, 0, 0, 0) is
equality constraints can be enforced and can be determined locally exponentially stable.
by desired robot positions (or velocities) using the Γ map Proof: See http://www.jtwebs.net/
or by previously measured image coordinates. Finally, all ICRA-2014/ for details.
 VI. S IMULATION R ESULTS Optical Ray Camera
Using the trajectory generation method outlined in Sec- Virtual
Optical Axis Optical
tion IV, we can generate sample trajectories directly in the
✓ Axis
image coordinates, representing a swooping maneuver. It is Image Plane
reasonable to specify a limit on the attitude, which enables Virtual “level”
vi,m Image Plane
the incorporation of linear visibility constraints, rather than
requiring non-linear visibility constraints when planning in
vi
the Cartesian space. A sample trajectory is shown in Fig. 5
Fig. 6. The measured image feature points, vi,m , which are affected by
(top), where the boundary conditions and intermediate way- θ, are projected onto a virtual level image plane to decouple the motion
point were computed using Γ, and with the derivatives in the from the attitude of the robot and determine the coordinates vi .
intermediate waypoint left unconstrained. Ground Station Robot
Next, using the generated desired trajectory in the image
Lateral and
plane, the controller from Section V is simulated on the Yaw Controller
dynamic model given by (5)-(6). The simulation is started f, ✓c Onboard
IBVS Dynamics
with an initial image coordinate error of 0.10m, and the Controller
resulting trajectory and error are plotted in Fig. 5. vm , ✓
Gumstix Camera
y, ẏ, rq
0.4 Vicon
v1
Positions

0.2
0 v2
v1,d Fig. 7. A camera captures images of the cylinder, which are sent to the
−0.2 Gumstix Overo Computer on Module (COM). The images are processed at
−0.4 v2,d 65 Hz using blob tracking; the boundaries of the cylinder are undistored,
0 1 2 3 4 calibrated, and sent back to a ground station along with the pitch as
measured from the IMU. From here, the ground station maps the points
0.2 to the virtual plane and computes desired control inputs using the IBVS
v1 controller. In parallel, Vicon feedback is used to close the loop on the lateral
0.1 v2
Errors

(y, ẏ) and yaw (ψ) dimensions of the robot. Then, the desired attitude is
0 v̇1 sent to the onboard controller, which uses the IMU to control the attitude
−0.1 v̇2 at 1 kHz.
−0.2
0 1 2 3 4
Image Coordinate

time (s) 0.5

Fig. 5. A sample trajectory in simulation. The simulated image coordinates, v1
vi , and the desired coordinates, vi,d , are in the top graph where there is 0 v2
an initial error of 0.1m in each coordinate. The feature errors and error v1,d
velocities are in the bottom graph. v2,d
−0.5
VII. E XPERIMENTAL VALIDATION 0 5 10 15
A Hummingbird quadrotor equipped with a global shutter time (s)
CaspaTM VL camera and Computer on Module from Gumstix Fig. 8. Experimental results of the feature coordinates in the virtual plane
[27] is used for experiments to fly down and grasp a cylin- for a “swooping” trajectory. The feature coordinates are denoted by vi and
the desired trajectory is given by vi,d .
der object. As mentioned earlier, grasping maneuvers are
predominantly in the sagittal plane and thus our developed
models and algorithms for motion planning and control are to improve the estimate of the image features and their
derivatives to compute J and J. ˙ A block diagram of the
planar. Since the experiments are on a 3D system, an external
Vicon-based motion capture system is used to stabilize system is shown in Fig. 7.
the yaw and lateral dynamics while our IBVS controller The stability of the controller was demonstrated through
stabilizes motion in the sagittal plane. several experiments including hovering, vertical trajectories,
Visual detection and tracking of the cylinder object runs “swooping” trajectories, and hovering above a moving cylin-
onboard the robot, is based on blob tracking using Freeman der. Here we present a “swooping” trajectory, which includes
chain coding, and is obtained using the C++ Visp library some components of the mentioned trajectories. See Fig. 8
[28]. When the object is in the image and rq ∈ S, the for the planned and actual trajectories in the virtual image
measured image points from the camera are mapped to the plane, Fig. 9 for the corresponding estimated and actual
virtual image plane using feedback from the IMU and the position in the inertial frame, and Fig. 10 for a sequence
transformation shown in Fig. 6, which is mathematically of still images from a sample experiment. The reader can
equivalent to observe the other trajectories in the attached video.
The aggression of our trajectories is limited beause of (i)
vi = tan (arctan (vi,m ) + θ)
slow feedback due to limited onboard sensing and computa-
where vi,m is the boundary of the cylinder in the actual tion, (ii) a limited optical resolution, and (iii) a small optical
calibrated image. The points in the virtual plane are filtered field of view.
 [5] Stephan Weiss, Davide Scaramuzza, and Roland Siegwart. Monocular-
1 SLAM-based navigation for autonomous micro helicopters in GPS-
Position (m)
denied environments. Journal of Field Robotics, 28(6):854–874,
xq,v
0.5 xq
November 2011.
[6] S. Hutchinson, G.D. Hager, and P.I. Corke. A tutorial on visual servo
zq,v control. IEEE Transactions on Robotics and Automation, 12(5):651–
0 zq 670, 1996.
[7] C.J. Taylor and J.P. Ostrowski. Robust vision-based pose control. In
0 5 10 15 Proceedings 2000 ICRA. Millennium Conference. IEEE International
Conference on Robotics and Automation. Symposia Proceedings (Cat.
time (s) No.00CH37065), volume 3, pages 2734–2740. IEEE, 2000.
[8] The Slow Mo Guys. ”Red Kites in Slow Motion”,
Fig. 9. Positions in the inertial frame for the experiment in Fig. 8. The
http://youtu.be/AYOx-iCMZhk.
vision estimates of the position (using Γ) are denoted by the “v” subscript.
[9] ”Ascending Technologies, GmbH,” http://www.asctec.de.
The ground truth only has the “q” subscript.
[10] Koushil Sreenath, Taeyoung Lee, and Vijay Kumar. Geometric Control
and Differential Flatness of a Quadrotor UAV with a Cable-Suspended
Load. In IEEE Conference on Decision and Control (CDC), pages
2269–2274, Florence, Italy, December 2013.
[11] B. Espiau, F. Chaumette, and P. Rives. A new approach to visual
servoing in robotics. IEEE Transactions on Robotics and Automation,
8(3):313–326, June 1992.
[12] Francois Chaumette and Seth Hutchinson. Visual servo control. I.
Basic approaches. IEEE Robotics & Automation Magazine, 13(4):82–
90, December 2006.
[13] F. Chaumette and S. Hutchinson. Visual servo control. II. Advanced
Fig. 10. Still images from a sample “swooping” trajectory using the vision- approaches [Tutorial]. IEEE Robotics & Automation Magazine,
based controller developed in this paper. The background has been washed 14(1):109–118, March 2007.
out slightly to improve visibility. [14] NJ Cowan, J.D. Weingarten, and D.E. Koditschek. Visual servoing via
navigation functions. IEEE Transactions on Robotics and Automation,
18(4):521–533, August 2002.
VIII. C ONCLUSION [15] Tarek Hamel and Robert Mahony. Visual Servoing of an Under-
Actuated Dynamic Rigid-Body System: An Image-Based Approach.
This paper demonstrates a first step towards autonomous IEEE Transactions on Robotics & Automation, 18(2):187–198, 2002.
dynamic grasping and manipulation for micro aerial vehicles [16] Tarek Hamel and Robert Mahony. Image based visual servo control
in unstructured environments. In particular, we considered for a class of aerial robotic systems. Automatica, 43(11):1975–1983,
November 2007.
a quadrotor system equipped with a monocular camera and [17] Daewon Lee, Tyler Ryan, and H. Jin. Kim. Autonomous landing of a
formulated the dynamics of the underactuated system directly VTOL UAV on a moving platform using image-based visual servoing.
in the virtual image plane. The system was demonstrated In 2012 IEEE International Conference on Robotics and Automation,
pages 971–976. IEEE, May 2012.
to be differential flat, with the image coordinates being the [18] Karen Bass, Brian Leith, Justin Anderson, Peter Bassett, Joe Stevens,
set of flat outputs. We presented a trajectory generation Hugh Pearson, and Jeff Turner. Nature’s Most Amazing Events.
method which guarantees dynamic feasibility and enables [DVD]. BBC Worldwide Ltd. Programs, 2009.
[19] ”Vicon Motion Systems, Inc,” http://www.vicon.com.
incorporating visual constraints as linear constraints. We [20] Yi Ma, Stefano Soatto, Jana Kosecka, and Shankar Sastry. An
developed a non-linear vision-based controller for trajectory Invitation to 3-D Vision: From Images to Geometric Models. Interdis-
tracking in the image space, presented a proof of stability, ciplinary Applied Mathematics. Springer, New York, 2004.
[21] Hamed Jabbari, Giuseppe Oriolo, and Hossein Bolandi. Dynamic
and provided validation of the controller both in simulation IBVS control of an underactuated UAV. 2012 IEEE International
and in experimentation on a quadrotor. A next step would Conference on Robotics and Biomimetics (ROBIO), pages 1158–1163,
be to generalize the control law to full three dimensions and December 2012.
[22] Hong Zhang and J.P. Ostrowski. Visual Servoing with Dynamics:
consider the yaw of the robot by using image moments to Control of an Unmanned Blimp. In International Conference on
detect the primary axis of the cylinder. Robotics and Automation, volume 1, pages 618–623. IEEE, 1999.
[23] Koushil Sreenath, Nathan Michael, and Vijay Kumar. Trajectory
ACKNOWLEDGMENT Generation and Control of a Quadrotor with a Cable-Suspended Load
A Differentially-Flat Hybrid System. In International Conference on
The authors would like to thank Kartik Mohta for his ex- Robotics and Automation, 2013.
pertise and assistance with the vision hardware and software. [24] R.M. Murray, M. Rathinam, and Willem Sluis. Differential flatness
of mechanical control systems: A catalog of prototype systems. In
R EFERENCES ASME International Congress and Exposition. Citeseer, 1995.
[1] Norma Venable. Birds of Prey. West Virginia University Extension [25] Daniel Mellinger and Vijay Kumar. Minimum snap trajectory gen-
Service, 1996. eration and control for quadrotors. In 2011 IEEE International
[2] Justin Thomas, Joe Polin, Koushil Sreenath, and Vijay Kumar. Avian- Conference on Robotics and Automation, pages 2520–2525. IEEE,
Inspired Grasping for Quadrotor Micro UAVs. In International Design May 2011.
and Engineering Technical Conferences & Computers and Information [26] Taeyoung Lee, Melvin Leok, and N.H. McClamroch. Control of
in Engineering Conference (IDETC/CIE), Portland, 2013. Complex Maneuvers for a Quadrotor UAV using Geometric Methods
[3] Matko Orsag, Christopher Korpela, and Paul Oh. Modeling and on SE(3). Asian Journal of Control, 2011.
Control of MM-UAV: Mobile Manipulating Unmanned Aerial Vehicle. [27] ”Gumstix, Inc.,” http://www.gumstix.com.
Journal of Intelligent & Robotic Systems, 69(1-4):227–240, August [28] Andrew I. Comport, Eric Marchand, Muriel Pressigout, and François
2012. Chaumette. Real-time markerless tracking for augmented reality: the
[4] Anastasios I Mourikis, Stergios I Roumeliotis, and Joel W Burdick. virtual visual servoing framework. IEEE transactions on visualization
SC-KF Mobile Robot Localization: A Stochastic Cloning Kalman and computer graphics, 12(4):615–28.
Filter for Processing Relative-State Measurements. IEEE Transactions
on Robotics, 23(4):717–730, August 2007.
 A PPENDIX so that
A. Attitude Controller ˙ −1 v̇ − mv̈d .
mëv = f JRe2 − JGA + mJJ (24)
The proof of the attitude controller is outlined below.
Proof: Follows from [26, Prop. 1], with the Lyapunov Defining
function f
eT2 RcT Re2 Re2 − Rc e2 ,



X=J (25)
D3 eT2 RcT Re2
VR = eΩ · eΩ + KR Ψ (R, Rd ) + c2 eR · eΩ , (8)
2 the error dynamics become
with c2 being a positive scalar, such that,  
f
zTθ Mθ zθ ≤ VR ≤ zTθ MΘ zθ , (9) mëv = J R c 2 +X
e
eT2 RcT Re2
V̇R ≤ −zTθ Wθ zθ , (10) ˙ −1 v̇ − mv̈d . (26)
− JGA + mJJ
T
where zθ = [keR k , keΩ k] , and Mθ , MΘ , and Wθ are Next, let
positive definite. f = A · Re2 (27)
B. Position Control
and the commanded attitude be defined by
To demonstrate the stability of the position controller, we
first set up the proof. Define Kp0 , Kd0 , B, α ∈ R as, A
Rc e2 = . (28)
kAk
Kp0 = m kJk J −1
Kp (11)
  Then, from the previous two equations, we have
Kd0 = kJk m J −1 ˙
Kd + J −1 (12)
f = kAk eT2 RcT Re2 . (29)
 
B = kJk kGA k + m J −1
v̈ d ˙
+ J −1 v̇ d
(13) Substituting this into (26) and using A, we have
kAk eT2 RcT Re2
 
α = keR k (14)
mëv = J Rc e2 + X
and define Wv1 , Wv2 , Wvθ , Wv ∈ R2×2 as eT2 RcT Re2
 c1 Kp  c1 −JGA + mJJ ˙ −1 v̇ − mv̈d (30)
c1 Kd
 
m B 0
Wv1 = c1 Kd m 2m , Wvθ = (15) = J (kAk Rc e2 ) + X
2m Kd − c1 B 0
" # −JGA + mJJ˙ −1 v̇ − mv̈d (31)
c1 αKp0 α c1 0 0


Wv2 = α c1 m0 2 m Kd + Kp (16) ˙ −1 v̇ − mv̈d
= JA + X − JGA + mJJ (32)
0
αKd0


2 m Kd + Kp
= −Kp ev − Kd ėv + X (33)
W v = W v1 − W v2 . (17)
which has the same form as (83) in [26]. We use the same
Suppose we choose positive constants c1 , Kp , Kd , KR , KΩ Lyapunov candidate, but in our image coordinates,
such that,
1 2 1 2
c21 Vv = Kp kev k + m kėv k + c1 ev · ėv . (34)
Kp > (18) 2 2
m  T
4 kWvθ k
2 Now, let zv = kev k , kėv k , then it follows that the
λmin (Wθ ) > (19) Lyapunov function Vv is bounded as
λmin (Wv )
Then, there exists positive constants γ1 , γ2 , γ3 , such that zTv Mv zv ≤ Vv ≤ zTv MV zv , (35)
kJk ≤ γ1 , J −1 ≤ γ2 , J −1 ˙ ≤ γ3 , and if initial 2×2
where Mv , MV ∈ R are defined as,
conditions and the desired trajectory satisfy    
1 Kp −c1 1 Kp c1
1 Mv = , MV = . (36)
α< , (20) 2 −c1 m 2 c1 m
mγ1 γ2
Then,
dist(vd (t), V c ) < kev (0)k , (21)
where V c is the complement of V , and dist(vd (t), V c ) = V̇v = Kp (ėv · ev ) + m (ëv · ėv )
inf t∈[0,∞),w∈V c kvd (t) − wk is the smallest distance be- + c1 (ev · ëv + ėv · ėv ) , (37)
tween a trajectory and a set, then the zero equilibrium
(ev , ėv , eR , eΩ ) = (0, 0, 0, 0) is locally exponentially stable. and incorporating (33),
Proof: We take an approach very similar to [26] to c1 Kp 2 2
V̇v = − kev k − (Kd − c1 ) kėv k
show that the controller is exponentially stable. Using (5), m
−c1 Kmd (ev · ėv ) + X · cm1 ev + ėv .


we can determine the image errors (38)
ëv = v̈ − v̈d (22)
1 ˙ −1 v̇ − v̈d
= J [f Re2 − GA ] + JJ (23)
m
Now, we establish a bound on X. From (25), where Wvθ , Wv are as defined in (15), (17). Since Wv =
T
f (Wv ) and Wv ∈ R2×2 , it is sufficient to show that
eT2 RcT Re2 Re2 − Rc e2



X=J T T (39) det(Wv ) > 0 and Wv (1, 1) > 0 in order to claim that
e2 Rc Re2
kAk Rc e2 · Re2 Wv > 0. Then, from the assumption on α in (20), we have
kXk ≤ kJk keR k (40) w11 > 0. This is reasonable since α is a functional on the
Rc e2 · Re2
attitude error such that α ∈ [0, 1]. Thus, the assumption in
≤ kJk kAk keR k (41) (20) is simply a bound on the attitude error. The determinant
−1
≤ kJk GA + mJ [−Kp ev − Kd ėv + v̈d ] can be expressed as a quadratic function of Kd such that
+J −1˙ [ėv + v̇d ] keR k (42) det(Wv ) = β0 + β1 Kd + β2 Kd2 (47)
≤ Kp0 kev k + Kd0 kėv k + B keR k


(43) and βi is a function of c1 , Kp , γ1 , γ2 , γ3 , and m. The critical
where Kp0 , Kd0 , B are as defined in (11)-(13), and from [26], point of the quadratic occurs when
0 ≤ keR k ≤ 1. Kp m Kp m + αc1 γ1 γ3
Next we will show that there exists positive constants Kd = + (48)
c1 c1 (1 − αγ1 γ2 m)
γ1 , γ2 , γ3 s.t., kJk ≤ γ1 , J −1 ≤ γ2 , and J −1 ˙ ≤ γ3 .
and has a value of
2
Since Γ is smooth (we only require C here), J is smooth on
Kp (1 − αγ1 γ2 m) Kp m − c21


the closed set S. This implies J is bounded on S, i.e., ∃γ1 > det(Wv ) = . (49)
0, s.t. kJk < γ1 . Next, since J is smooth and nonsingular m
In both equations, (1 − αγ1 γ2 m) > 0 as a result of the
on S, the inverse is well defined and is smooth on S, which
assumption in (20). Thus (48) is positive, and by (18), (49)
implies J −1 is bounded on S, i.e., ∃γ2 > 0, s.t. J −1 <
d −1 is positive and Wv0 > 0. Now, we consider the combined
γ2 . Next, observe that dt J (rq ) = ∂r∂ q J −1 (rq )ṙq is a
Lyapunov candidate for the translational and rotational error
composition of smooth functions on S, implying that it is
˙ dynamics, V = Vv + VR . From (9) and (35), we have,
bounded on S, i.e., ∃γ3 > 0, s.t. J −1 < γ3 .
Then, similar to [10], we can express V̇v as zTv Mv zv + zTθ Mθ zθ ≤ V ≤ zTθ MΘ zθ + zTv MV zv . (50)
  c
 T T
 ev 1
 Further, we see that
V̇v = − ev ėv Wv1 +X· ev + ėv
ėv m V̇ ≤ −zTv Wv zv + zTv Wvθ zθ − zTθ Wθ zθ , (51)
(44) 2
  ≤ −λmin (Wv ) kzv k + kWvθ k kzv k kzθ k
ev
≤ − ev ėTv Wv1
 T 
2
ėv −λmin (Wθ ) kzθ k , (52)
+Kp0 kev k keR k cm1 kev k + kėv k


and from (19), we have V̇ to be negative definite, and
+Kd0 kėv k keR k cm1 kev k + kėv k the zero equilibrium of the closed-loop system is locally


exponentially stable.
+B keR k cm1 kev k + kėv k . (45)

This can be written as,

V̇v ≤ −zTv Wv zv + zTv Wvθ zθ (46)