Académique Documents
Professionnel Documents
Culture Documents
expressed in Fb , Ω = ωb∧ = [0, −r, q; r, 0, −p; −q, p, 0] ∀R ∈ SO(3), RT Ṙ ∈ so(3), and ṘT Ṙ + RT R̈ ∈ so(3).
is the tensor form of ωb . The torque on the quadrotor is Proof: Take the time derivative of RT R = I to get
˙
R R + RT Ṙ = 0. Hence we have (RT Ṙ)T + RT Ṙ =
T
given by τ = [τx , τy , τz ]T in the body frame Fb , J is
the inertia matrix of the quadrotor, and h = [x, y, z]T 0, satisfying the so(3) condition for RT Ṙ. Differentiate
is the position of the quadrotor in Fw . The quadrotor’s (RT Ṙ)T + RT Ṙ = 0 to get ((R˙T )Ṙ + RT R̈)T + ((R˙T )Ṙ +
inputs are the total thrust and the three torques µ = RT R̈) = (ṘT Ṙ + RT R̈)T + (ṘT Ṙ + RT R̈) = 0, implies
[fz , τx , τy , τz ]T , and the system has a 12 dimensional state, that ṘT Ṙ + RT R̈ ∈ so(3).
ξ = [x, y, z, vx , vy , vz , ψ, θ, φ, p, q, r]T . III. V ECTOR F IELD F OLLOWING
In many scenarios, it is desirable for a robot to follow
C. Endogenous Transformation a given velocity vector field, for example for navigation,
Consider the flat outputs consisting of position and yaw collision avoidance, connectivity maintenance, flocking, and
angle σ = [σ1 , σ2 , σ3 , σ4 ]T = [x, y, z, ψ]T . In this sec- herding applications. In this section, we describe our method
tion we give the functions β(·) and γ(·) such that, given for deriving the time derivatives of the flat outputs from a
... .... given velocity vector field. This is the main contribution of
(σ, σ̇, σ̈, σ , σ ), we can find the associated state ξ and input
µ. The following theorem summarizes the results of [10]. our work.
Theorem 1: The quadrotor dynamics (3)–(6), with state The key to our method is to find the required time
... ....
ξ = [x, y, z, vx , vy , vz , ψ, θ, φ, p, q, r]T and input µ = derivatives (σ̇, σ̈, σ , σ ) by taking spatial derivatives of
[fz , τx , τy , τz ]T , are differentially flat, with the flat outputs the vector field. With these time derivatives, the states and
inputs of the quadrotor can be generated from the endoge-
σ = [σ1 , σ2 , σ3 , σ4 ]T := [x, y, z, ψ]T , (7) nous transformation as described in Theorem 1. The SE(3)
controller described in [10] and [8] is then used to stabilize
...
such that ξ = β(σ, σ̇, σ̈, σ ), with the quadrotor along the required trajectory ξ(t). Here we
only consider vector fields specifying spatial velocity, hence
[x, y, z, vx , vy , vz , ψ]T = β1:7 (σ, σ̇) the yaw angle σ4 is irrelevant. We arbitrarily set σ4 (t) ≡ 0,
= [σ1 , σ2 , σ3 , σ̇1 , σ̇2 , σ̇3 , σ4 ]T and the expressions for the time derivatives of σ1:3 (t) are
θ = β8 (σ, σ̇, σ̈) = atan2(βa , βb ) (8) computed with the following theorem.
p Theorem 2: The flat output derivatives required to com-
φ = β9 (σ, σ̇, σ̈) = atan2(β... c, βa2 + βb2 ) ... ....
pute the endogenous transformation σ̇1:3 , σ̈1:3 , σ 1:3 , σ 1:3 at
[p, q, r]T = β T ∨
10:12 (σ, σ̇, σ̈, σ ) = (R Ṙ) , any point x in a vector field V (x) can be calculated by
where
σ̇1:3 (x) = V (x)
βa = − cos σ4 σ̈1 − sin σ4 σ̈2
σ̈ (x) = J (σ̇ (x), x)σ̇ (x)
1:3 1:3 1:3
... (11)
βb = −σ̈3 + g (9) σ 1:3 (x) = J (σ̈1:3 (x), x)σ̈1:3 (x)
.... ... ...
βc = − sin σ4 σ̈1 + cos σ4 σ̈2 , σ 1:3 (x) = J ( σ 1:3 (x), x) σ 1:3 (x),
where J (f (x), x) denotes the Jacobian matrix of the func-
Quadrotor trajectory
tion f (x). Limit cycle
Proof: σ̇1:3 (x) = V (x) is true by definition.
1
Then σ̈1:3 (x) = d(σ̇1:3 dt
(x))
= ∂(σ̇1:3
∂x
(x)) d(x(t))
dt =
J (σ̇1:3 (x), x)σ̇1:3 (x), and the procedure can be recursively
altitude = −z
0.5
applied to obtain the expressions for the third and forth time
derivatives.
0 −0.5
The method is applicable to 1D, 2D, or 3D vector fields, 0
but we show examples only with 2D vector fields for clarity. 0.5
1
For example, consider a velocity vector field given by the 0 0.5 1 1.5 2 2.5
1.5
equation
Fig. 4. The 2D vector field is the same with Fig. 3(a) but translated to
x2 x1 [0.9, 1.5, −0.5]T , with a constant altitude 0.5m. The trajectory converges
V (x1 , x2 ) = − 0.5 sin(x21 + x22 − 1) , (12) to the circle (x − 0.9)2 + (y − 1.5)2 = 1, with z = −0.5, as expected.
−x1 x2
−0.5
0
−0.5
0
−0.5
0
−0.5
−x1 x2
−1 −1 −1 −1
−1.5
−1 0 1
−1.5
−1 0 1
−1.5
−1 0 1
−1.5
−1 0 1 as shown in Fig. 5. The trajectory of the quadrotor is shown
in Fig. 6. Again, the first two images show a pre-planned
(a) Velocity (b) Acceleration (c) Jerk (d) Snap
take-off trajectory, and the quadrotor enters the vector field
Fig. 3. Vector field as in (12), with scaled vector length for plotting. at the third image.
Using the endogenous transformation described in Sec. II, 1.5 1.5 Spiral path
we then obtain the states and inputs for the quadrotor to be
1 1
used as a control reference to control it along the desired
0.5 0.5
vector field. Specifically, for the vector field in (12), the
simulation in Sec. IV and the experiment in Sec. V will show 0 0
−1.5 −1.5
−1.5 −1 −0.5 0 0.5 1 1.5 −1.5 −1 −0.5 0 0.5 1 1.5
IV. S IMULATION
(a) Logarithmic spiral with a = (b) A path following the vector
To illustrate the concept, in this section we consider simu- 1.5, b = 0.2, and θ → ∞. field as eq. (13) with b = 0.2.
lation with a dynamical model of the quadrotor flying in three Fig. 5. Starts from [−1.5, 0]T in the vector field, the path eventually spires
different velocity vector fields: 1), limit cycle; 2), logarithmic to [0, 0]T , showing the same trajectory as the logarithmic spiral.
spiral and 3), obstacle avoidance. Sec. V gives the results of
hardware experiments with a quadrotor executing our control
C. Obstacle Avoidance
method in the same three vector fields.
In this scenario, the quadrotor travels to a destination
point, with an obstacle in its path. The destination generates
A. Limit Cycle an attracting field to pull the quadrotor to it, while the
As in (12) and Fig. 3, the vector field is designed to obstacle generates a repelling field to push the quadrotor
cause the quadrotor to converge to a circle centered at away.
[0.9, 1.5, −0.5]T . The trajectory of the quadrotor is shown The attracting and repelling vector fields in Fig. 7 are
in Fig. 4, with the quadrotor image drawn at various time formulated from the gradient of a 2D Gaussian G(x) =
T 2
instants to give an indication of pitch and roll angles. The Ae(−(x−x0 ) (x−x0 ))/2σ . Taking the gradient, the attracting
quadrotor does not actually enter the vector field until the vector field of the destination at xa is defined as va (x) =
G(x)
third image. Before then, it is executing a planned maneuver σ 2 (xa − x), ∀x. And the repelling vector field of the
G(x)
to take off and enter the vector field with the appropriate obstacle at xr is defined as vr (x) = kx−x rk
(x − xr ), ∀x,
velocity. such that k x − xr k≥ rob , where rob is the minimum
Quadrotor trajectory Quadrotor trajectory
Logarithmic spiral Vector flow path
1 1
altitude = −z
0.5
0.5
altitude = −z
0 −0.5
0 −0.5 0
0 0.5
0.5 1
1 0 1.5
0.5 1 1.5 2
0 1.5 2.5
0.5 1 1.5 2 2.5
(a) The virtual obstacle is at [0.6, 1.2, −0.5]T , the quadrotor pass
it from left.
Fig. 6. The quadrotor starts from [0.9, 0, −0.5]T to follow the vector
field, and spire to the center [0.9, 1.5, −0.5]T , to which the vector field is
translated. The quadrotor achieves a trajectory the same with the logarithmic
spiral as in Fig. 5. Quadrotor trajectory
Vector flow path
2 Start
Destination
1.8 Path 0.5
altitude = −z
1.6
1.4
0 −0.5
1.2 0
1 0.5
1
0.8 0 1.5
0.5 1 1.5 2 2.5
0.6
0.4 (b) The virtual obstacle is at [1.0, 1.2, −0.5]T , the quadrotor pass
0.2 it from right.
0
0 0.5 1 1.5 2 2.5 3 Fig. 8. The quadrotor avoids the obstacle with different trajectories when
the virtual obstacles (black circle) are at different positions.
V. E XPERIMENTS
Hardware experiments were conducted with a
K500 quadrotor, from KMel Robotics (http:
//kmelrobotics.com/). The position and orientation
of the quadrotor are observed by an OptiTrack motion
capture system (http://www.naturalpoint.com/
optitrack/) at the default update rate 120Hz and Fig. 9. System diagram, the markers are used for tracking.
obtained by MATLAB via Java scripts reading UDP packets
from the OptiTrack. The velocity and angular velocity are All experiments were conducted corresponding to the
computed by numerical differentiation from the position and same setup as in the simulations in Sec. IV. As in the
Euler angles, as well as with an Extended Kalman Filter simulations, the quadrotor takes off and flies into the vector
(EKF) in MATLAB. field by following a predefined trajectory, which is generated
The UDP packet parsing, the EKF algorithm, and the from spline curves [20],[12]. The final state of this trajectory
SE(3) controller are implemented in C language and are is designed to match the vector field so that the quadrotor
called by MATLAB via the MEX interface to speed up the can transition smoothly into from the trajectory following to
calculation, making it possible to run the algorithm online the vector field following control.
without degrading the OptiTrack update rate, even when
flying multiple quadrotors. Meanwhile, the K500 receives A. Limit Cycle
the desired thrust, Euler angles and Euler angles’ rate as the The quadrotor first flies to [0.5, 0.5, −0.5]T , which is
inputs, and executes its onboard SO(3) controller based on outside of the limit cycle (x − 0.9)2 + (y − 1.5)2 = 1, with
a predefined trajectory, then in the vector field following, it yaw angle and its derivatives to the second order from the
converges to the limit cycle as expected, with a small position vector field, and explore extensions to multiple quadrotors
error. Along the cycle, the quadrotor flies at a constant speed interacting with vector fields. Also, The conditions of the
1.4m/s, as in Fig. 10(a). vector field under which the quadrotor dynamics can satisfy
should be studied.
R EFERENCES
[1] Nora Ayanian and Vijay Kumar. Decentralized feedback controllers for
multi-agent teams in environments with obstacles. IEEE Transactions
on Robotics, 26(5):878 – 887, October 2010.
[2] J. Cortés, S. Martı́nez, T. Karatas, and F. Bullo. Coverage control
for mobile sensing networks. IEEE Transactions on Robotics and
Automation, 20(2):243–255, April 2004.
(a) Composite image of a sin- (b) Composite image of a single [3] D. V. Dimarogonas and K. H. Johansson. Decentralized connectivity
gle quadrotor flying a limit cycle, quadrotor flying a logarithmic spi- maintenance in mobile networks with bounded inputs. In Robotics
http://tinyurl.com/qbus6c7. ral, http://tinyurl.com/kx2meq4. and Automation, 2008. ICRA 2008. IEEE International Conference
on, pages 1507–1512, 2008.
Fig. 10. A single quadrotor flying a limit cycle and a logarithmic spiral [4] L. Habets and J. H. van Schuppen. A control problem for affine
path at a constant altitude 0.5m. dynamical systems on a full-dimensional polytope. Automatica,
40(1):21–35, 2004.
[5] A. Jadbabaie, J. Lin, and A. S. Morse. Coordination of groups
B. Logarithmic Spiral of mobile autonomous agents using nearest neighbor rules. IEEE
Transactions on Automatic Control, 48(6):988–1001, June 2003.
The quadrotor first flies to [0.9, 0, −0.5]T where it enters [6] O. Khatib. Real-time obstacle avoidance for manipulators and mobile
the vector field, as shown in Fig. 10(b). The maximum speed robots. The international journal of robotics research, 5(1):90–98,
1986.
in this case is 1.4m/s, and the quadrotor quickly slows down [7] M. Kloetzer and C. Belta. A fully automated framework for control of
when approaching the center of the logarithmic spiral. linear systems from temporal logic specifications. Automatic Control,
IEEE Transactions on, 53(1):287–297, 2008.
C. Obstacle Avoidance [8] Taeyoung Lee, M Leoky, and N Harris McClamroch. Geometric
tracking control of a quadrotor uav on se (3). In Decision and
We present two experiments to demonstrate the obstacle Control (CDC), 2010 49th IEEE Conference on, pages 5420–5425.
avoidance with the vector field following strategy. In each IEEE, 2010.
experiment, the virtual obstacle is presented at a different [9] P. Martin, R. M. Murray, and P. Rouchon. Flat systems, equivalence,
and trajectory generation. Technical report, California Institute of
position. In the vector field, the quadrotor flies from the same Technology, Control and Dynamical Systems, April 2003.
start position at [0.2, 0, −0.5]T to the same destination at [10] Daniel Mellinger and Vijay Kumar. Minimum snap trajectory genera-
[1.5, 2.5, −0.5]T . tion and control for quadrotors. In Robotics and Automation (ICRA),
2011 IEEE International Conference on, pages 2520–2525. IEEE,
2011.
[11] Nathan Michael, Daniel Mellinger, Quentin Lindsey, and Vijay Ku-
mar. The grasp multiple micro-uav testbed. Robotics & Automation
Magazine, IEEE, 17(3):56–65, 2010.
[12] Richard M Murray. Optimization-based control. California Institute
of Technology, CA, 2009.
[13] R. Olfati-Saber and R. R. Murray. Consensus problems in networks of
agents with switching topology and time-delays. IEEE Transactions
on Automatic Control, 49(9):1520–1533, September 2004.
[14] C. Reynolds. Flocks, herds, and schools: A distributed behavioral
(a) The virtual obstacle (b) The virtual obstacle model. Computer Graphics, 21:25–34, 1987.
is at [0.6, 1.2, −0.5]T , is at [1.0, 1.2, −0.5]T , [15] E. Rimon and D. E. Koditschek. Exact robot navigation using artificial
http://tinyurl.com/o475fna. http://tinyurl.com/qdmucrm. potential functions. IEEE Transactions on Robotics and Automation,
8(5):501–518, 1992.
Fig. 11. Composite image of a single quadrotor avoiding an obstacle at [16] M. Schwager, B. Julian, M. Angermann, and D. Rus. Eyes in the sky:
different positions. Decentralized control for the deployment of robotic camera networks.
Proceedings of the IEEE, 99(9):1541–1561, September 2011.
[17] M. Schwager, D. Rus, and J. J. Slotine. Unifying geometric, prob-
abilistic, and potential field approaches to multi-robot deployment.
VI. C ONCLUSIONS AND F UTURE W ORK International Journal of Robotics Research, 30(3):371–383, March
In this paper we persent a method for controlling a 2011.
[18] Brian L Stevens and Frank L Lewis. Aircraft control and simulation.
quadrotor robot along a desired vector field. This is difficult 2003.
because quadrotors have complex nonlinear dynamics. It [19] J. Van den Berg, M. Lin, and D. Manocha. Reciprocal velocity obsta-
is useful because many algorithms for robot navigation, cles for real-time multi-agent navigation. In International Conference
on Robotics and Automation (ICRA), pages 1928–1935, 2008.
collision avoidance, surveillance, flocking, and herding give a [20] Fujio Yamaguchi and Fujio Yamaguchi. Curves and surfaces in
vector field for a robot to follow to accomplish its task. Our computer aided geometric design. Springer-Verlag Berlin, 1988.
control method leverages the tools of differential flatness, [21] M. M. Zavlanos and G. J. Pappas. Potential fields for maintaining
connectivity of mobile networks. IEEE Transactions on Robotics,
and obtains the desired velocity, acceleration, jerk, and snap 23(4):812–816, August 2007.
of the quadrotor directly from the vector field. The method
is demonstrated in simulations and experiments with three
different vector fields. In the future we will consider the