A Stabilizing Gyroscopic Obstacle Avoidance Controller forUnderactuated Systems

Gowtham Garimella1, Matthew Sheckells2 and Marin Kobilarov1

Abstract— This work addresses the problem of controlling anunderactuated system to a goal state in a cluttered environment.To this end, a gyroscopic obstacle avoidance controller isproposed for a class of underactuated systems, which takeas inputs body torques and a thrust force along some knownbody-fixed axis. The Lyapunov stability of the controller in thepresence of obstacles is shown in theory and verified throughsimulations. Care is taken to ensure stability even when thesystem has only a finite obstacle detection radius, and a novelgyroscopic obstacle avoidance gain selection is designed whichsmoothly attenuates the steering force as the robot headsaway from an obstacle. Furthermore, a gyroscopic steeringforce tailored to two primitive obstacles, namely cylinders andspheres, is proposed. Simulations of a quadrotor system and ananosatellite system validate that the controller is stable undera large number of obstacles and smoothly converges to the goalstate.

I. INTRODUCTION

Navigation of underactuated dynamic robotic systems ina cluttered environment is considered a nontrivial task.Challenges arise due to the inability of the system to instan-taneously produce a force in any desired direction. The classof underactuated systems considered in this work are rigidbodies with controls given by body torques and a thrust forcealong with some body-fixed axis. These types of systemsare common in robotics and include quadrotors, satellites,underwater and surface vehicles. For instance, quadrotorsare becoming important for a variety of applications such assearch and rescue [1], mapping [2], and package delivery [3].Quadrotors have four axis aligned rotors which provide thrustalong the rotor’s axis and torque along the body axes. Be-sides, small satellites such as cubesats [4] are enabling low-cost testbeds for applications such as formation flying [5] andother autonomous operations [6]. The CubeSats consideredin this work have an attitude control system (ACS) and asingle thruster which produces the thrust force along somebody-fixed direction.

Many control techniques have been developed for suchtypes of vehicles. These include deterministic feedback lin-earizing controllers for quadrotors in [7], [8], [9], [10] as wellas Lyapunov-stable controllers in the presence of boundedexternal disturbances [11], [12], [13], [14].

In addition to standard point stabilization, requiring prov-ably stable obstacle avoidance significantly complicates the

1Gowtham Garimella and Marin Kobilarov are with the Department ofMechanical Engineering Johns Hopkins University, 3400 N Charles Str,Baltimore, MD 21218, USA ggarime1|marin@jhu.edu

2Matthew Sheckells is with the Department of Computer Science JohnsHopkins University, 3400 N Charles Str, Baltimore, MD 21218, USAmshescke1@jhu.edu

control design. While a backstepping obstacle avoidancecontroller has been proposed in [15], it focused on twoschemes, namely a mass point model and a safety ball modelto generate waypoints away from an obstacle. The proposedcontroller, therefore, does not explicitly include obstacleconstraints, and no proof of obstacle-aware convergence isavailable. Traditionally, obstacle avoidance for fully actuatedsystems has been considered through gyroscopic avoidanceand navigation field [16] approaches. The navigation fieldapproach generates an artificial potential field with a globalminimum at the goal and a maximum at the obstacles, but thedesign of such a potential field is non-trivial. The navigationfield approach has been used for obstacle avoidance ofquadrotors in [17], [18]. Unlike the potential field methods,the dynamic window approach [19] is a local method thatmerges a Model Predictive Control (MPC) approach andpotential field methods to find a control trajectory in theaccessible space that maximizes a utility function.

In contrast to requiring a potentially complex nonlinearpotential function, the gyroscopic avoidance approach han-dles obstacles by applying a steering force to the robotwithout increasing the Lyapunov energy of the system [20].The applied steering force, therefore, does not affect theLyapunov stability of the controller. This method is semi-globally convergent to the goal state in the presence ofunknown convex obstacles. Gyroscopic avoidance has beensuccessfully applied to create flocking behavior in a multi-agent system [21] and to control an Unmanned GroundVehicle (UGV) [22]. A gyroscopic force added to a potentialfield approach was applied to quadrotor swarm formationin [23], but only a simplified kinematic model for thequadrotor was considered.

This work extends the gyroscopic avoidance approach [20](originally developed for fully actuated systems) to under-actuated dynamical systems in 3D workspaces through abackstepping technique. To ensure convergence in the pres-ence of multiple obstacles, a novel obstacle avoiding steeringfunction has been designed to enable smooth transitionsbetween colliding and non-colliding directions of motions.Furthermore, to ensure stability even when the system hasa finite obstacle detection radius, a smooth obstacle controlgain is employed. Two types of 3D obstacles are considered:cylinders and spheres. Many real world obstacles can bemodeled using a combination of these primitives.

We demonstrate the ability of the method to performobstacle avoidance in two challenging simulated scenarios.The controller is first employed on a quadrotor and shownto converge to a goal position while avoiding a forest of

2016 IEEE 55th Conference on Decision and Control (CDC)ARIA Resort & CasinoDecember 12-14, 2016, Las Vegas, USA

978-1-5090-1837-6/16/$31.00 ©2016 IEEE 5010

trees modeled as cylinders with spherical canopies. A similarexample involving a nanosatellite has shown convergenceto a goal position while avoiding space debris modeledas spheres. To the best of the authors’ knowledge, this isthe first controller providing convergence for these types ofunderactuated systems in complex scenarios.

The rest of the paper is organized as follows. In §IIwe specify the dynamics of the class of underactuatedsystems considered. In §III, a desired gyroscopic controlleris designed for the translational dynamics. This controlleris extended to the class of underactuated systems throughbackstepping in §IV. Next, the design of gyroscopic ob-stacle avoidance gains specific to cylindrical, and sphericalobstacles are specified in §V. Finally, simulations of aquadrotor and nanosatellite in non-trivial scenarios are shownin section VI. The proof for stable collision avoidance isderived in Appendix.

II. DYNAMICS OF UNDERACTUATED SYSTEMS

We consider underactuated systems modeled as rigid bod-ies with position p ∈ R3 and velocity p in a fixed inertialframe, orientation matrix R ∈ SO(3) and body-fixed angularvelocity ω ∈ R3. The control inputs for the system are thebody torques τ ∈R3 and thrust force ut ∈R in some knownbody-fixed direction e ∈R3. The system is subject to knownexternal forces given by f ∈R3 and no external torques. Thedynamics is

mp = Reut + f , (1)R = Rω, (2)Jω = Jω×ω + τ, (3)

where m is the mass and J is the rotational inertia.Our goal is to design a Lyapunov-stable controller achiev-

ing a given desired goal position pd with zero velocitypd = 0 while avoiding obstacles. To accomplish this, wefirst design a gyroscopic obstacle avoidance controller forthe translational dynamics of the underactuated system. Theresulting “desired” control forces for this controller cannotbe directly achieved due to under actuation. Therefore, weperform a backstepping procedure which closes the loop instages and ultimately achieves stability. We next describe thetranslational and gyroscopic parts of the controller derivation.

III. GYROSCOPIC AVOIDANCE

We first design a gyroscopic avoidance controller for theposition coordinates. Let g ∈ R3 denote the translationalinput force. For clarity, let the system’s translational statecombining position and velocity be denoted by x= [pT , pT ]T .The translational dynamics is then given by

x = Ax+B(g+ f ), (4)

A =

[0 I3×30 0

], B =

[0

1m I3×3

]. (5)

The underactuated dynamics considered in (1-3) is an ex-tension of this subsystem, where the control force is given

as the thrust vector g = Reu and the thrust direction Re iscontrolled by the rotational dynamics of the system.

The design of the controller starts with defining the errorz0 ∈ R6 between the state x and the desired state xd =[pT

d ,0T ]T , given by

z0 = x− xd .

For a standard linear system considered in (4-5), a Lyapunovstable feedback control law can be achieved using a desiredforce gd ∈ R3 given by

gd =−Kzo− f , (6)K = [Kp, Kv], (7)

where Kp,Kv ∈ R3×3 are positive definite matrices corre-sponding to proportional and derivative gains, respectively.

Gyroscopic avoidance is equivalent to adding force termswhich steer the system around the obstacles. This obstacleavoidance is achieved by adding the desired force perpen-dicular to both the current velocity and the steering axis ofthe obstacle. The control law is thus augmented accordingto

gd =−Kz0− f −G(x)p, (8)

where the matrix G(x) ∈ R3×3 is skew-symmetric. Thismatrix instantaneously rotates the velocity away from an ob-stacle. Equivalently, the steering force G(x)p can be regardedas being perpendicular to the velocity of the robot.

For stability analysis it will be useful to define the storagefunction

V0 =12

zT0 Pz0, (9)

where

P =

[Kp 00 mI3×3

]. (10)

We then have

V0 =12

zT0 P(Ax+B(g+ f ))+

12(Ax+B(g+ f ))T Pz0.

If the system if fully actuated, the input force g can be set tothe desired control force gd and it would be possible to showasymptotic stability. Yet, the input force for an underactuatedsystem is restricted only along the single body directionRe and cannot be set to the desired force gd . Hence, theabove controller is extended to the full underactuated systemdynamics using a backstepping procedure as described next.

IV. BACKSTEPPING PROCEDURE

We next close the loop in stages using backstepping.The controller designed in (6) requires the control forceon the system g to be equal to a desired control force gdthat stabilizes the system. The desired force gd cannot beachieved directly for underactuated systems since the controlforce can only be applied along some known body directionRe. Instead, the difference between the applied force g anddesired force gd is considered as an error z1 ∈R3 defined by

z1 = g−gd ,

5011

to be further suppressed in the backstepping procedure. Withthis definition, we have

V0 = z0Qz0 +(BT Pz0)T (g−gd),

where

Q =

[0 00 −Kv

].

Next, a new Lyapunov candidate is defined which includeserror z1 as

V1 =V0 +12

zT1 z1,

with time-derivative given by

V1 = z0Qz0 + zT1 (g− gd +BT Pz0),

which can be expressed as

V1 = z0Qz0− kz1zT1 z1 + zT

1 z2,

where z2 = g−ad and ad = gd−BT Pz0−kz1z1. The variablead is a desired value for g which cannot be achieved by theunderactuated systems. Thus, continuing the backsteppingprocedure a new Lyapunov candidate which includes theerror between the desired and actual values of g is givenas

V2 =V1 +12

zT2 z2,

with derivative

V2 = z0Qz0− kz1zT1 z1 + zT

2 (g− ad + z1).

The desired value of g which ensures V2 is negative semi-definite is denoted as bd , and is computed as

bd = ad− z1− kz2z2,

bd = gd−BT Pz0− kz1 z1− z1− kz2z2,

where

gd =−[0 G(x)]z0−2[0 G(x)]z0−Kz0,

and

z0 = A2x+AB( f +g)+Bg.

For the underactuated systems, the derivative g can beexpanded as

g = R[eu+ ˆωeu+2ωeu+ ω2eu].

Finally, the control inputs τ, u can be chosen to satify thedesired bd by setting

τ = J[e× (RT bd− ω2eu−2ω u)/u]−Jω×ω, (11)

u = eT (RT bd−ω2eu−2ω u). (12)

Note that this controller does not directly control the thrustforce u, but instead controls u. The state of quadrotor systemis extended by u, u to account for this. The controller fortorque τ shown in (11) has a singularity when the thrust forcegoes to zero u= 0. This is not a problem in practice since thevehicle always produces positive force u while navigating.

Proof of Stability:

Let the error state for the system be given by z =[zT

0 ,zT1 ,z

T2 ]

T . Using the control law described in (11), thederivative of Lyapunov Function V2 is evaluated as

V2 =12(zT

0 Pz0 + zT1 z1 + zT

2 z2), (13)

V2 = zT0 Qz0− kz1zT

1 z1− kz2zT2 z2 = zT Kz, (14)

with K =

Q 0 00 kz1 00 0 kz2

, (15)

where the matrix K is negative semidefinite. The largestinvariant set [24] with respect to the quadrotor dynamics,when V2 = 0 is given by z = 0. Assuming V2 is a C1

smooth function and assuming the initial error state z(t = 0),controls τ(t),u(t), and states x(t) are bounded, the systemwill asymptotically converge to z = 0 according to Lasalle’sinvariance principle [24].The boundedness of the states andcontrols is ensured only if the robot does not collide withan obstacle. The proof for collision avoidance is provided insection VII-A which guarantees boundedness of the controlinputs. The smoothness of V2 is ensured by a smooth controllaw, which implies the steering forces must be C2 smooth.The design of proper steering forces is discussed in thefollowing section.

V. OBSTACLE AVOIDANCE COEFFICIENTS

In the backstepping controller shown in (11), the specificform of gyroscopic avoidance matrix G(x) is not specified. Inthis section, we discuss the form of G(x) suitable for obstacleavoidance.

The obstacle avoidance matrix G(x) is decomposed intoa sum of obstacle avoidance matrices Gi(x) correspondingto individual obstacles. Each individual obstacle avoidancematrix is composed of angular obstacle avoidance gaink1(θi), radial obstacle avoidance gain k2(di), and the steeringaxis ei(x). In summary, we have

G(x) = ∑Gi(x), Gi(x) = k1(θi)k2(di)ei(x). (16)

where the hat operator · maps the steering axis to a skewsymmetric matrix as

e =

0 −e3 e2e3 0 −e1−e2 e1 0

.The angular obstacle avoidance gain reduces the magni-

tude of steering force as the robot heads away from theobstacle, while the radial obstacle avoidance gain forcesthe magnitude of steering force to be inversely proportionalto the distance between the robot and obstacle. In addi-tion, the steering force should be negligible beyond a userspecified distance from the obstacle known as the detectionradius, since the obstacle avoidance is not active beyondthis distance. The second order derivatives of G are used inthe backstepping control law (11). Therefore, the obstacleavoidance gains and steering axis direction should be C2

smooth functions.

5012

The obstacles considered in this work are either cylindersor spheres. The choice of obstacle avoidance gain andsteering axis for the two obstacles are discussed next.

A. Cylinders

A cylinder is specified by the major axis a, radius r,and a point on the major axis op. The cylinder obstaclesare assumed to have infinite length. The steering axis e(x)for a cylindrical obstacle is chosen along the major axisa which applies steering correction around the major axisof the cylinder. There is an ambiguity on the sign of theprincipal axis of the cylinder, which corresponds to drivingto the right or left of the cylinder. The sign of the steeringaxis is determined according to (17-20). According to thisapproach, the robot avoids the cylinder by steering right ifit is heading to the right of the cylinder, and the oppositewhen heading left of the cylinder. The steering axis e(x) ismathematically computed as

v = p− (aT p)a, (17)∆p = op− p, (18)

d = ∆p− (aT∆p)a, (19)

e(x) = sign(aT (d× v))a. (20)

The velocity v is the projected robot velocity onto the planeperpendicular to the major axis of the cylinder a⊥. Similarly,the displacement vector d is the projection of the vectorconnecting the robot center to the point on the major axis ofthe cylinder onto the plane a⊥.

The sign function is not differentiable, hence it is notsuitable for the backstepping control law. A scaled sigmoidfunction which is a smooth approximation of the sign func-tion is chosen instead. The smooth steering axis is given by:

e(x) = (2S(aT (d× v))−1)a,

S(t) =1

1+ e−kt .

The angular obstacle avoidance gain is chosen to decayexponentially with the angle between the robot heading andobstacle θ . The mathematical form of the angular gain isdescribed as

k1(θ) = exp [katt (θ −1)] , (21)

θ =dT v

‖d‖‖v+λa‖. (22)

The variable θ is the cosine of the angle between theprojected velocity v and the projected displacement vectord, and katt ∈ R+ adjusts the sensitivity of the steering forceto the robot’s heading. When the magnitude of the projectedvelocity approaches zero, the angle between d and v isundefined. A velocity vector in the null space of projectedvelocity is added to the projected velocity in (22) to avoidthis singularity. This regularizing term also ensures that thederivatives of k1(θ) remain bounded.

The specific form of the radial obstacle avoidance gain isshown below:

k2(d) = kobsS(rd + r−‖d‖− ε)

‖d‖− r. (23)

A sigmoid function is used as an smooth approximation ofthe step function to suppress the radial gain beyond the finitedetection radius rd . The value of ε is adjusted such thatthe radial obstacle avoidance gain at the detection radiusis negligible. The gain kobs scales the steering effort. Anappropriate value for kobs can guarantee obstacle avoidanceas discussed in appendix V.

The complete obstacle avoidance matrix for a cylindricalobstacle is written as

G(x) = kobsekatt (θ−1)

‖d‖− rS(rd + r−‖d‖− ε)S(aT (v×d))a.

B. Spheres

A sphere is specified by it’s center op and radius r. Unlikea cylinder, the steering axis for a sphere is not constant, andis chosen based on the robot velocity and displacement vectorfrom the robot to the center of the sphere. Given the velocityof the robot v = p, the distance to the sphere d = op− p, theform of steering axis chosen as

e(x) =d× v‖d× v‖

.

When the robot is heading towards the obstacle, the crossproduct of d × v goes to zero, which creates unboundedderivatives of the steering axis. To avoid this issue, a regu-larized value of norm of the cross product is used. The normof the cross product can be written as

‖d× v‖=√‖d‖2‖v‖2− (dT v)2.

The regularized value can then be formulated as

‖d× v‖reg =√‖d‖2‖v‖2(1+ kreg)− (dT v)2.

The gain kreg ensures that the norm ‖d× v‖reg is non-zeroeven when the robot is heading towards the obstacle.

The radial obstacle avoidance gain k2(d) is the same asthat of cylinder obstacle. Similar to the cylinder case, thesteering gain chosen for a spherical obstacle as k1(θ) =exp [katt(θ −1)], where θ the angle between the robot head-ing and obstacle is given by

θ =dT v‖d‖‖v‖

.

The steering obstacle avoidance gain and the steering axis arenot defined when the robot velocity is zero. This singularityis avoided by setting the obstacle avoidance to zero when thevelocity of the robot is below a threshold δ . The completeobstacle avoidance matrix G for a sphere is written as

G(x) =

{kobs

exp[katt (θ−1)]‖d‖−r

S(rd+r−‖d‖−ε)‖d×v‖reg

d× v if ‖v‖> δ ,

0 otherwise.

5013

0 10 20

time(s)

-20

-10

0m

x

xd 0 10 20

time(s)

-20

-10

0

y

yd 0 10 20

time(s)

0

5

10

z

zd

0 10 20

time(s)

0

2

4

m/s

x

xd

0 10 20

time(s)

0

2

4 y

yd

0 10 20

time(s)

-0.50

0.51

1.5z

zd

0 10 20

time(s)

-4-20246

Nm τx

0 10 20

time(s)

-4-2024

τy

0 10 20

time(s)

-1

0

1

τz

0 10 20

time(s)

4

6

N

u

5 10 15 20 25

200400600800

100012001400

V

5 10 15 20 25

-6000

-4000

-2000V

Fig. 2. History of the quadrotor position, velocity, controls, and Lyapunovfunction V for the quadrotor simulation

VI. NUMERICAL SIMULATIONS

The backstepping controller designed in section IV istested on a quadrotor and a nanosatellite in simulation.The underactuated dynamics in (1) has been discretizedusing a semi-implicit scheme [25] and integrated using Eulerintegration.

A finite detection radius of 3 meters is applied to theobstacles. The backstepping parameters have been chosen toensure the controls are bounded, and the system convergessmoothly to the goal.

A. Quadrotor in a Dense Forest

A standard quadrotor model is used, with the thrust alignedwith the body-fixed z-axis of the system and with gravity asthe only external force, i.e., f = (0,0,−9.81m). The massof the quadrotor is 0.5 Kg and the moment of inertia ischosen as a diagonal matrix J = diag([.003, .003, .005]). Theobstacle scene used for testing is that of a forest with treeobstacles as shown in Fig. 1. Each tree obstacle consists of acylindrical trunk and a spherical canopy. A total of 23 treesare generated on a grid spanning a 20m×20m region. Thetrees are perturbed randomly from the grid centers. The goalof the controller is to reach the desired position shown inFig. 1 starting from the opposite side of the grid.

One can observe that the quadrotor smoothly reaches thegoal while avoiding obstacles as illustrated in Fig. 1. Thestate, control, and Lyapunov energy history of the quadrotortrajectory are shown in Fig. 2. Note that the Lyapunovenergy function asymptotically approaches zero with nodiscontinuities, even though the obstacle detection radius ofthe system is finite.

B. Satellite among Space Debris

For the second example, consider a nanosatellite equippedwith an attitude control system and a single thruster. Thesatellite is placed in an environment of space debris modeled

0 200 400

time(s)

-8-6-4-20

m

x

xd 0 200 400

time(s)

-8-6-4-20

y

yd 0 200 400

time(s)

0

5

10

z

zd

0 200 400

time(s)

0

0.05

m/s

x

xd

0 200 400

time(s)

0

0.05

0.1 y

yd

0 200 400

time(s)

0

0.05

0.1z

zd

0 200 400

time(s)

-1

0

1

Nm τx

0 200 400

time(s)

-1

0

1

τy

0 200 400

time(s)

-1

0

1

τz

0 200 400

time(s)

0.02

0.04

N

u

100 200 300 400 500

0.5

1

1.5

V

100 200 300 400 500

-0.03

-0.02

-0.01 V

Fig. 3. History of the nanosatellite position, velocity, controls, andLyapunov function V for satellite simulation

as spheres, and no external forces are included in thesimulation. The satellite is tasked to navigate to the desiredposition shown in 1 while avoiding the obstacles. A totalof 48 obstacles are generated using a cuboid grid in a cubicspace of 9m×9m×9m. The obstacles are randomly perturbedaround the grid centers.

Similarly to the quadrotor scenario, the satellite is able tostabilize to the goal while avoiding obstacles as demonstratedin Fig. 1. A detailed state, control, and Lyapunov energyhistory of the satellite are shown in Fig. 3. The Lyapunovenergy function asymptotically approaches zero even as newobstacles enter or leave the detection radius of the system.

VII. CONCLUSIONS

A backstepping Lyapunov stable controller using gyro-scopic obstacle avoidance has been designed for underactu-ated systems. The ability of the controller to handle finiteobstacle detection radius and the underactuated dynamicsin the presence of obstacles has been shown in theory, andsimulations verified the capability of the controller to avoidobstacles and converge to a goal in complex scenarios. Fur-thermore, the obstacle coefficient design approach employedin this work can be extended to include new primitives suchas finite-length cylinders, finite planes, and ellipsoids. Thebackstepping procedure can also be extended to stabilizethe system under bounded external disturbances as explainedin [11]. Future work will concentrate on implementing thegyroscopic obstacle avoidance controller on a real systemand showing that the convergence guarantees hold.

APPENDIX

A. Proof of Obstacle Avoidance

In this section, the appropriate choice of scaling gain onthe distance obstacle avoidance kobs shown in (23) requiredto guarantee obstacle avoidance of the system is presented.Several assumptions are required for finding the gain. Tostart with it is assumed that there is a single obstacle in the

5014

b) c)

Fig. 1. The figures (a) and (b) show a quadrotor navigating through a dense cluster of trees using the proposed controller with a finite obstacle detectionradius. Figure (a) shows the top view of the quadrotor, where the obstructing obstacles have been removed for a clear view of the trajectory. Figure (b)shows a side view of the same trajectory swooping under the canopies to reach the goal. A satellite navigating through space debris using the proposedcontroller is illustrated in (c). The thrust vector of the satellite is shown by a pointed cone at the bottom of the satellite.

environment. Further, the gain matrices Kp and Kv used in thedesired force gd in (8) are assumed to be constant matrices asin kpI3×3,kvI3×3, and we set m= 1 for simplicity. If the robotis not moving p = 0, it is assumed that the robot will notcollide with an obstacle. The proof for obstacle avoidanceis explained through the principle of contradiction similar tothe proof shown in [20].

Let the system collide with the obstacle at tc with non-zero velocity p(tc) 6= 0. The dynamics before the collisionduring the interval I = [tc−∆t, t−c ] is considered. The closedloop translational dynamics of the underactuated system canbe written as

p = g+ f =−kp(p− pd)− (kvI3×3 +G)p+ z1. (24)

Integrating the dynamics for the interval I gives p(t−c ) as

p(t−c ) = e−kv∆tRG(t−c , t−c −∆t)p(tc−∆t)+ e(∆t), (25)

e(∆t) =∫ t−c

t−c −∆te−kv(t−c −τ)RG(t−c ,τ)(−kp∆p+ z1)dτ, (26)

where RG(t,τ) is given by solving ddt RG(t,τ) =

−G(x)RG(t,τ) with RG(τ,τ) = I [26]. The obstacleavoidance matrix G(x) has the form shown in (16). Thesteering axis ei(x) is assumed to be constant during thesmall time ∆t. For cylindrical obstacle, the steering axis ischosen to be the major axis of cylinder, hence it is constantduring ∆t. For a sphere, this is a valid approximationassuming the displacement vector does not change duringthe interval I. Under this assumption, the rotation matrixRG(t,τ) can be simplified as

ddt

RG(t,τ) =−k1(θ)k2(d)eRG(t,τ),

RG(t,τ) = e−[∫ t

τ k1(θ)k2(d)dt]e = Re(ψ(t,τ)),

ψ(t,τ) =∫ t

τ

k1(θ)k2(d)dt.

The form of Re(ψ(t,τ)) implies that the obstacle avoidancematrix induces a rotation about the steering axis e, wheree is assumed to be constant during ∆t and the amount ofrotation is given by the angle ψ(t,τ).

Let the initial value of the Lyapunov function (13) beVmax , V2(t = 0). It has been shown in (14), that the Lya-punov energy is non-increasing over time. The error vectore(∆t) from (26) is shown to be O(∆t) (i.e the norm of thevector is O(∆t)) as follows

‖e(∆t)‖ ≤∫ t−c

tc−∆t‖(−kp∆p+ z1)‖dτ,

≤∫ t−c

tc−∆tkp‖∆p‖+‖z1‖dτ,

≤√

Vmax(√

2kp +1)∆t, using (10,13).

Using this result, the velocity at t−c from (25) can be writtenas

p(t−c ) = e−kv∆tRe(ψ(t−c , t−c −∆t))p(tc−∆t)+O(∆t). (27)

Since the Lyapunov energy is non-increasing over time and12 m‖ p‖2 <V (t)≤Vmax according to (13,10), the norm of thevelocity of the system is upper bounded as ‖ p(t)‖<

√2Vmax.

This implies the projected velocity v(t) shown in (17 shouldalso have the same property, i.e. ‖v(t)‖ ≤

√2Vmax. The

derivative of the projected distance vector d(t) is evaluated tobe the negative of the projected velocity d(t) = −v(t). Theprojected distance at the time of collision is equal to theradius of the obstacle ‖d(tc)‖= r. The projected distance attc−∆t can be bounded as

‖d(tc−∆t)‖ ≤√

2Vmax∆t + r. (28)

The projected distance is assumed to be continuously gettingcloser to the obstacle as in

‖d(t)‖ ≤ ‖d(tc−∆t),‖ ∀t ∈ I. (29)

Since the robot is heading towards the obstacle, the abso-lute value of the heading of the robot from the obstaclewill be less than π/2. Hence, the angular avoidance gainduring the interval I can be bounded as k1(θ(t)) ≥ e−katt .The robot is also assumed to be within the detection ra-dius. Hence, the obstacle avoidance gain is simplified ask2(d(p(t))) = kobs/(‖d(p(t))‖− r). A lower bound on the

5015

rotation ψ(t−c , t−c −∆t) is found using above results as

ψ(t−c , t−c −∆t) =∫ t−c

tc−∆tk1(θ(t))k2(d(t))dt,

≥∫ t−c

tc−∆te−katt

kobs

‖d(t)‖− rdt,

≥ e−kattkobs√2Vmax

, using (28-29).

If kobs is chosen as kobs ≥ πekatt√

2Vmax, the lower boundon the rotation induced by the obstacle avoidance matrix isgiven by ψ(t−c , t−c −∆t)≥ π . This bound is not dependent ontime left to collision ∆t. Using (27), the velocity at the timeof collision is rotated around the obstacle axis by more than180 degrees from the time tc−∆t when the robot is expectedto be heading towards the obstacle. Thus at time tc when therobot collision occurs, the robot is not heading towards theobstacle which is a contradiction to our initial assumptionthat the robot collided with the obstacle at nonzero velocityfor which the heading angle needs to be towards the obstacle.Even with a small time to collision, the robot can completelyavoid the obstacle with the appropriate gain selection of kobs.In practice, this a very conservative gain and smaller valuesthan this have achieved satisfactory collision avoidance.

REFERENCES

[1] S. Gupte, P. I. T. Mohandas, and J. M. Conrad, “A survey of quadrotorunmanned aerial vehicles,” in 2012 Proceedings of IEEE Southeastcon,March 2012, pp. 1–6.

[2] S. Siebert and J. Teizer, “Mobile 3d mapping for surveying earthworkprojects using an unmanned aerial vehicle (UAV) system,” Automationin Construction, vol. 41, pp. 1–14, 2014.

[3] “Prime air,” http://www.amazon.com/b?node=8037720011, 2015.[4] S. Rainer, “Status and trends of small satellite missions for earth

observation,” Acta Astronautica, vol. 66, no. 1, pp. 1–12, 2010.[5] G. C. Caillibot Eric and D. Kekez, “Formation flying demonstration

missions enabled by canx nanosatellite technology,” in Proceedingsof the AIAA/USU Conference on Small Satellites, 13th Annual FrankJ. Redd Student Scholarship Competition, 2005. [Online]. Available:http://digitalcommons.usu.edu/smallsat/2005/all2005/42/

[6] M. Kobilarov and S. Pellegrino, “Trajectory planning for cubesat short-time-scale proximity operations,” Journal of Guidance, Control, andDynamics, vol. 37, no. 2, pp. 566–579, 2014.

[7] E. Altug, J. P. Ostrowski, and R. Mahony, “Control of a quadrotorhelicopter using visual feedback,” in 2002 IEEE International Con-ference on Robotics and Automation. Proceedings. ICRA’02., vol. 1.IEEE, 2002, pp. 72–77 vol.1.

[8] S. A. Al-Hiddabi, “Quadrotor control using feedback linearization withdynamic extension,” in 6th International Symposium on Mechatronicsand its Applications, 2009. ISMA’09. IEEE, March 2009, pp. 1–3.

[9] K. H. J. Lee. Daewon and S. Shankar, “Feedback linearization vs.adaptive sliding mode control for a quadrotor helicopter,” InternationalJournal of control, Automation and systems, vol. 7, no. 3, pp. 419–428,2009.

[10] H. Voos, “Nonlinear control of a quadrotor micro-UAV usingfeedback-linearization,” in 2009 IEEE International Conference onMechatronics. ICM 2009. IEEE, April 2009, pp. 1–6.

[11] M. Kobilarov, “Trajectory tracking of a class of underactuated systemswith external disturbances.” in 2013 American Control Conference(ACC), June 2013, pp. 1044–1049.

[12] R. Mahony and T. Hamel, “Robust trajectory tracking for a scalemodel autonomous helicopter,” International Journal of Robust andNonlinear Control, vol. 14, no. 12, pp. 1035–1059, 2004.

[13] S. Bouabdallah and R. Siegwart, “Backstepping and sliding-modetechniques applied to an indoor micro quadrotor,” in 2005 IEEEInternational Conference on Robotics and Automation, 2005. ICRA2005. IEEE, 2005, pp. 2247–2252.

[14] M. G. O. G. V. Raffo and F. R. Rubio, “Backstepping/nonlinear H∞ control for path tracking of a quadrotor unmanned aerial vehicle,”in 2008 American Control Conference. IEEE, June 2008, pp. 3356–3361.

[15] S. H. Geng Qingbo and Q. Hu, “Obstacle avoidance approachesfor quadrotor UAV based on backstepping technique,” in 2013 25thChinese Control and Decision Conference (CCDC). IEEE, May 2013,pp. 3613–3617.

[16] E. Rimon and D. E. Koditschek, “Exact robot navigation usingartificial potential functions,” IEEE Transactions on Robotics andAutomation, vol. 8, no. 5, pp. 501–518, 1992.

[17] H. K. Chang Kai, Xia Yuanqing and M. Dailiang, “Obstacleavoidance and active disturbance rejection control for a quadrotor,”Neurocomputing, vol. 190, pp. 60 – 69, 2016. [Online]. Available:http://www.sciencedirect.com/science/article/pii/S0925231216000801

[18] A. Budiyanto, A. Cahyadi, T. B. Adji, and O. Wahyunggoro, “UAVobstacle avoidance using potential field under dynamic environment,”in 2015 International Conference on Control, Electronics, RenewableEnergy and Communications (ICCEREC),, Aug 2015, pp. 187–192.

[19] P. Ogren and N. E. Leonard, “A convergent dynamic window approachto obstacle avoidance,” IEEE Transactions on Robotics, vol. 21, no. 2,pp. 188–195, April 2005.

[20] D. E. Chang and J. E. Marsden, “Gyroscopic forces and collisionavoidance with convex obstacles,” in New trends in nonlinear dynamicsand control and their applications. Springer, 2003, pp. 145–159.

[21] D. E. Chang, S. C. Shadden, J. E. Marsden, and R. Olfati-Saber,“Collision avoidance for multiple agent systems,” in 42nd IEEEConference on Decision and Control (CDC), 2003., vol. 1, December2003, pp. 539–543 Vol.1.

[22] D. Vissiere, D. E. Chang, and N. Petit, “Experiments of trajectorygeneration and obstacle avoidance for a ugv,” in 2007 AmericanControl Conference (ACC), July 2007, pp. 2828–2835.

[23] F. S. Haibo Min and F. Niu, “Decentralized UAV formation trackingflight control using gyroscopic force,” in IEEE International Con-ference on Computational Intelligence for Measurement Systems andApplications, 2009. CIMSA ’09., May 2009, pp. 91–96.

[24] H. K. Khalil and J. Grizzle, Nonlinear systems. Prentice hall NewJersey, 1996, vol. 3.

[25] M. Kobilarov, “Discrete optimal control on lie groups and applica-tions to robotic vehicles,” in 2014 IEEE International Conference onRobotics and Automation (ICRA), May 2014, pp. 5523–5529.

[26] C.-T. Chen, Linear system theory and design. Oxford UniversityPress, Inc., 1995.

5016