[American Institute of Aeronautics and Astronautics AIAA Guidance, Navigation, and Control (GNC) Conference - Boston, MA ()] AIAA Guidance, Navigation, and Control (GNC) Conference

Implementation of Fast MPC with a Quadrotor for

Obstacle Avoidance

Colin Greatwood∗

and Arthur Richards†

Department of Aerospace Engineering, University of Bristol, Bristol, UK

This paper proposes a realtime guidance technique for an autonomous vehicle movingthrough obstacles. The method combines a geometric convexification technique with ModelPredictive Control (MPC). Several recent advances in MPC are exploited in order toachieve efficient computation. Experimental results are given using a quadrotor in aninstrumented flying arena to show the feasibility of real-time control.

I. Introduction

This paper introduces a method for path planning and control of quadrotors within a non-convex obstaclefield. The method uses online optimization within a Model Predictive Control (MPC) framework, takingadvantage of fast MPC1 to provide a real-time controller on embedded hardware. Trajectory generationin the presence of obstacles is NP-hard2 and has been the subject of considerable algorithm development,including randomized methods,3,4 integer programming5 and nonlinear optimization.6–8

The method introduced in this paper adopts a two stage process to avoid high computational require-ments. Like Ref. 9, a local, convex optimization problem is derived from the harder global problem. Otherapproaches along these lines include following tunnels,10 receding horizon optimization11 or combining pathgeneration with dynamic optimization12 or feasibility testing.13 The authors’ approach here is to decomposethe problem geometrically to form a local convex problem and then deploy a quadratic program (QP) op-timization. Notable features are the use of code generation for embedded implementation and the use of ahighly efficient QP solver to exploit structure in the problem.

The optimizer used for MPC is based upon the work by Wang and Boyd1 and is shown to exhibit goodperformance at 20Hz. Flight tests were conducted in the Bristol Robotics Laboratory’s flying arena using anAR.Drone14 quadrotor, for which a second order linear model is identified. Comparisons are made againstequivalent controllers that do not employ the key parts of the proposed MPC formulation, such as posingthe MPC as a regulation problem, illustrating the impact on performance.

Quadrotors are popular test beds for autonomous vehicle research. Instrumented flying facilities havebeen developed, including MIT’s Raven,15 Stanford’s STARMAC,16 Pennsylvania’s GRASP lab17 and ETH’sFlying Machine Arena,13 all actively researching autonomous capabilities for quadrotors and other vehicles.

Section II introduces the high level path planner that provides a convex operating space for the MPCoptimizations. The construction of the MPC algorithm used is detailed in Section III. Section IV providesan overview of the experimental infrastructure as well as the identified mathematical model of the AR.Dronebefore providing experimental results. Finally Section V draws conclusions and presents the future researchgoals.

Nomenclature

Oi Obstacle ipminx Left hand extent of operating regionpmaxx Right hand extent of operating region

∗Research Assistant, Email [email protected]†Senior Lecturer, Email [email protected], Senior Member AIAA.

1 of 11

American Institute of Aeronautics and Astronautics

Dow

nloa

ded

by P

UR

DU

E U

NIV

ER

SIT

Y o

n Se

ptem

ber

21, 2

013

| http

://ar

c.ai

aa.o

rg |

DO

I: 1

0.25

14/6

.201

3-47

90

AIAA Guidance, Navigation, and Control (GNC) Conference

August 19-22, 2013, Boston, MA

AIAA 2013-4790

Copyright © 2013 by the American Institute of Aeronautics and Astronautics, Inc. All rights reserved.

pminy Lower extent of operating regionpmaxy Top extent of operating regionrx x Location of vehiclery y Location of vehiclerS Target positionr(k) Location of vehicle at time ku(k) Control input at time kw(k) Disturbance at time kw(k) Disturbance estimate at time kx(k) Vehicle’s state at time kxS Target statexmini Left hand extent of obstacle ixmaxi Right hand extent of obstacle iymini Lower extent of obstacle iymaxi Top extent of obstacle i

II. Path Planning

The outline of the quadrotor control scheme used in this work is shown in Figure 1. The quadrotor isstabilized by its own onboard controller, taking velocity demand inputs. Directly controlling the movementof the quadrotor is an MPC controller, which is introduced in Section III. The MPC controller commandsthe quadrotor to move to setpoints provided by the path planner. The MPC method used here employsQuadratic Programming (QP), which requires a convex solution space to be described in terms of linearconstraints. The “Convexification” block determines a convex obstacle-free operating region forming theconstraints for the MPC. Within this convex operating space, a target setpoint is also provided to the MPCcontroller. The calculation of the convex operating region and provision of the setpoint are described in thefollowing section. This paper assumes fixed altitude and hence 2-D motion.

Vitus et al. propose two methods for decomposing a non-convex solution space into a sequence of convexpolytopes in their Tunnel MILP approach.10 This approach reduces the complexity of the MILP but not tothe point of convexity, i.e. still a MILP, making it unsuited to embedded solution. Augugliaro et al. studiedthe problem of multi-vehicle avoidance resulting in a convex Quadratic Program (QP).9 The new approachesin the following two sections also result in a QP, but tailored to obstacles representing an indoor environmentrather than other vehicles. A limitation of convexification is that the vehicle could conceivably get stuckdown a ‘dead-end’. Solutions to this problem include the incorporation of a higher-level global planner11

but this is beyond the scope of this paper.

II.A. Orthogonal Convexification

The first convexification method presented here assumes that all obstacles are rectangular and orthogonal.Coupled with the simple dynamics of the MAV (Micro Air Vehicle) (§IV.B), this enables two separate MPCprograms to be executed in parallel - one for each lateral axis. Figure 2(a) shows an example of an obstaclefield, with the destination in the top right hand corner. The rectangular operating region, for which thevehicle is constrained to by the MPC, is constructed in a two stage process. First, define the ith obstacleOi ⊂ <2

Oi =

{x ∈ <2 | Ix ≤

[xmaxi

ymaxi

], Ix ≥

[xmini

ymini

]},∀i ∈ No (1)

where xmini and xmax

i define the left and right hand sides of the obstacles; similarly ymaxi and ymin

i are thetop and bottom. Let pmin

x and pmaxx be the left and right hand side of the operating rectangle and pmin

y and

2 of 11


Dow

nloa

ded

by P

UR

DU

E U

NIV

ER

SIT

Y o

n Se

ptem

ber

21, 2

013

| http

://ar

c.ai

aa.o

rg |

DO

I: 1

0.25

14/6

.201

3-47

90


Vicon dSpace MicroAutoBox Parrot AR.Drone

Position measurements

Quadrotor commands MPC

Convexifi-

cation

State estimator

Virtual sensor

Ethern

et

Setpoint

Operating region

State estimate

Visible obstacles

Figure 1. Control Scheme

pmaxy be the top and bottom. Then, for a vehicle position of [rx, ry]T the rectangle faces are defined by

pmaxx = min

ixmini , ∀i ∈ {1..No} | xmin

i ≥ rx (2)

pminx = max

ixmaxi , ∀i ∈ {1..No} | xmax

i ≤ rx (3)

pmaxy = min

iymini , ∀i ∈ {1..No} | ymin

i ≥ ry (4)

pminy = max

iymaxi , ∀i ∈ {1..No} | ymax

i ≤ ry (5)

This provides a small operating rectangle such as that shown in Figure 2(a), where the right hand side ofthe operating rectangle is to the left of all the left faces of the obstacles that are to the right of the vehicle’scurrent position. This logic defines the other three faces, and a feasible convex space is provided.

The second stage of the rectangular operating region construction is to expand in the direction of thedestination, which is demonstrated in Figure 2(b). The expansion direction is the axis along which thetarget is furthest from the current position. If the rectangle cannot be expanded in that direction, due to anobstacle, the other axis is used. If expansion is not possible in either direction then no expansion is made.Figures 2(c) and 2(d) show the same two stage process for a different initial position.

Finally, the path planner provides the current setpoint position, rS , to the MPC controller. If the targetis within the operating rectangle then the setpoint is the target. Otherwise, the setpoint is the nearest pointto the target within the operating rectangle.

3 of 11


Dow

nloa

ded

by P

UR

DU

E U

NIV

ER

SIT

Y o

n Se

ptem

ber

21, 2

013

| http

://ar

c.ai

aa.o

rg |

DO

I: 1

0.25

14/6

.201

3-47

90


0 0.2 0.4 0.6 0.8 10

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

(a) Local convex operating region in green (b) Local target (triangle) in convex operating region

0 0.2 0.4 0.6 0.8 10

0.1

0.2

0.3

0.4

0.5

0.6

0.7

0.8

0.9

1

(c) Subsequent operating region (d) Expansion of subsequent operating region

Figure 2. Example operating regions

II.B. Non-orthogonal Convexification

The first convexification method decoupled the x and y axes of motion. This is reasonable for the dynamicsof the MAVs used but assumes an orthogonal representation of the world, i.e. aligned rectangular obstacles.This is clearly limiting, although it could be acceptable for some indoor flight applications. In this section,a different convexification method is developed that is suitable for arbitrary operating environments butmaintains a fixed number of constraints for MPC. It is assumed that the MAV has a limited sensing radiusand the MPC trajectory is constrained to remain within a currently-visible obstacle-free region.18 Themodified operating regions are shaped in order to maximize manoeuvring space in the direction of travel,but limited to only the observed free space.

For good manoeuvrability in the direction of travel, the basic shape of the operating region is chosen to bea cone, expanding from the current position to the limit of the sensor visibility. This is shown in Figure 3(a).To allow some margin for error in the position of the MAV, the region is then expanded rearwards, as shownin Figure 3(b). Finally, two faces are added to the front of the operating region, as shown in Figure 3(c).This forms the basic quadrilateral shape of the operating region; this is subsequently reduced in size untilno obstacles are enclosed within the region. It is only the first time step that is necessarily coincidentwith the MAV’s current position, i.e. the location of the sensor. Constraints for timesteps further aheadin the trajectory are centred at the predicted location of the vehicle at that time, based on the trajectoryfrom the previous time step. This helps performance and aims for the “tail feasibility” property, i.e. thatthe continuation of the previous solution is a feasible option, that ensures stability of MPC. Figure 3(d)demonstrates an example of an operating region centred away from the sensing radius centre.

4 of 11


Dow

nloa

ded

by P

UR

DU

E U

NIV

ER

SIT

Y o

n Se

ptem

ber

21, 2

013

| http

://ar

c.ai

aa.o

rg |

DO

I: 1

0.25

14/6

.201

3-47

90


The obstacles in this decomposition method are represented by faces. This means that it is not necessaryto have information about the entirety of obstacles, but just the faces that intersect the volume of spacevisible by the MAV.

MAV PositionVisible radiusInitial operating region

(a) Core shape of operating region

MAV PositionVisible radiusExtended operating region

(b) Core shape expanded rearwards

P

A

B

C

D

MAV PositionVisible radiusConvex operation region

(c) Front two faces added

P

Pfuture

A

B

C

D

MAV PositionMAV Posn at future time stepVisible radiusConvex operation region

(d) Core shape at future time step

Figure 3. Basic shape of operating region

III. Application of Fast MPC

Model Predictive Control19 is used to drive the quadrotor to the setpoint while respecting operatingconstraints on velocity and control. This is implemented using the “Fast MPC” formulation of Wang andBoyd1 to achieve fast solution times.

In theory, it is possible to treat this scenario as a regulation problem, taking the setpoint position asthe origin and converting all states to a setpoint-relative frame before passing to the MPC. However, thiswas found to give significant problems when the setpoint changed, often leading to loss of feasibility of theinterior point optimizer. Instead, the method of Ref. 20 has been adopted, treating the problem as one oftracking a piecewise-constant setpoint, using the modifications of Ref. 21 to give offset-free tracking in the

5 of 11


Dow

nloa

ded

by P

UR

DU

E U

NIV

ER

SIT

Y o

n Se

ptem

ber

21, 2

013

| http

://ar

c.ai

aa.o

rg |

DO

I: 1

0.25

14/6

.201

3-47

90


presence of disturbance.The system dynamics are modelled as a discrete-time linear time-invariant system with a disturbance,

x(k + 1) = Ax(k) +B∆u(k) + w(k) (6)

r(k) = Cx(k) (7)

where x(k) ∈ <n is the state at time step k, r(k) is the position and w(k) ∈ <n is the disturbance. Thecontrol input here is the input change, ∆u(k) = u(k)− u(k − 1), and the applied control u(k − 1) is one ofthe elements of the state x(k). Section IV.B describes the numerical model of the quadrotor dynamics. Forthe first, orthogonal, convexification scheme, two decoupled MPCs are employed, each modeling one axis ofmotion. For the second scheme, both axes are combined into a single dynamic model and controller. Bothstate and input are subject to constraints:

Fxx(k) + Fu∆u(k) ≤ f (8)

The constraints representing the velocity and obstacles are implemented as soft constraints using penaltyfunctions.22 This allows constraints to be violated if there is no alternative, avoiding infeasibility due tonumerical issues or disturbances when operating close to constraints.

The MPC optimizer solves a quadratic program (QP), at each time step, to find the input sequenceU(k) = (∆u(k), . . . ,∆u(k + T )) that minimizes a cost defined as

J =

T∑j=0

(x(k + j)− xS(k))TQ(x(k + j)− xS(k)) + ∆u(k + j)TR∆u(k + j) (9)

The current target state xS(k) is found by solving the following equations21 for a disturbance-invariant stateat the desired position rS(k): [

I −AC

]xS(k) =

(w(k)

rS(k)

)(10)

where w(k) is a disturbance estimate found from the prediction error. The tracking cost (9) can be rewrittenas the equivalent cost:

J =

T∑j=0

x(k + j)TQx(k + j) + ∆u(k + j)TR∆u(k + j)− 2xS(k)TQx(k + j) (11)

where the constant term has been omitted, since it makes no difference to the optimization. Setting the finallinear cost weight −2xS(k)TQ = qT puts the problem in the same form as for Fast MPC1 and the reader isdirected to that paper for details on how the QP is solved, including fast computation of the Newton step.

One significant modification is made to the MPC formulation from Ref. 1 in order to enforce terminalequality constraints. For the quadrotor, it is convenient and effective to require the terminal velocity to bezero, in accordance with the idea of “safe” receding horizon control.23 Fortunately, the addition of terminalequality constraints does not change the sparsity patterns exploited for fast computation in Ref. 1. Thedetails of the modification are given in Appendix A.

The MPC optimizer was programmed in a Simulink embedded MATLAB function for use with auto codegeneration. One of the key features of Fast MPC is the exploitation of the structure of the matrices. Theauto code generation used, however, does not support sparse matrices. It was therefore critical to createspecific functions for the relevant matrix multiplications.

IV. Flight Test Results

This section provides experimental results for the developed path planner, where an AR.Drone is com-manded to fly past several obstacles in the recently commissioned flying arena at the BRL. Section IV.Aintroduces the arena, Section IV.B provides details on the AR.Drone model used and the experimental resultsare shown in Section IV.C.

6 of 11


Dow

nloa

ded

by P

UR

DU

E U

NIV

ER

SIT

Y o

n Se

ptem

ber

21, 2

013

| http

://ar

c.ai

aa.o

rg |

DO

I: 1

0.25

14/6

.201

3-47

90


IV.A. Flight Research Facility

The flying lab at the Bristol Robotics Laboratory measures approximately 15×12×4 metres and is equippedwith ten Vicon24 motion capture cameras. Control algorithm development is carried out in Simulink andcompiled to run on dSpace MicroAutoBox II25 realtime hardware, which is fitted with a 900Mhz PowerPCprocessor.

The hardware enables a rapid-prototyping platform for control experiments. The software ‘tool-chain’includes Simulink blocks that were developed to communicate with both the AR.Drone for control and theVicon system for position information and state estimation. Bespoke applications were written to automatethe AR.Drone network connections and allow optional manual control over-ride during experimentation.

IV.B. Quadrotor Modelling

System ID was performed on the lateral translation of the AR.Drone running version 1.7.4 firmware, enablingconstruction of state estimators and MPC controllers. The fore/aft and sideways motions were found to haveidentical responses to control inputs. A second order transfer function for the response was found as

G(s) =12

0.35s2 + 2s+ 1(12)

It was also found that the input was saturated at around 0.37. The transfer function was verified by applyingvarying levels of step inputs up to 0.37 and comparing them against the vehicle’s velocity output, the resultsare shown in Figure 4

0 0.5 1 1.5 2 2.5 3 3.50

0.5

1

1.5

2

2.5

3

3.5AR.Drone step responses for forward inputs

Time (s)

Vel

ocity

(m

/s)

ux = 0.1ux = 0.2ux = 0.3ux >= 0.37

TF = 12 / (0.35s2 + 2s + 1)

(a) Three second period

0.05 0.1 0.15 0.2 0.250

0.05

0.1

0.15

0.2

0.25

0.3

AR.Drone step responses for forward inputs

Time (s)

Vel

ocity

(m

/s)

ux = 0.1ux = 0.2ux = 0.3ux >= 0.37

TF = 12 / (0.35s2 + 2s + 1)

(b) Short period response

Figure 4. Verifying AR.Drone transfer function against flight data

The position data was measured at 100Hz by Vicon; the MPC controller used in this work operates at20Hz. Figure 4(b) shows how the second order response lines up with the flight data at these time scales.Some offset is observed, but the results were deemed acceptable.

As previously introduced, the MPC formulation requires a discretized linear state space representationof the vehicle, hence from Equation 12, the system matrices are found to be

A =

1.0000 0.0499 0.0011 0.0007

0 0.9967 0.0434 0.0390

0 −0.1241 0.7485 1.4894

0 0 0 1.0000

(13)

B = [0.0007, 0.0390, 1.4894, 1.0000]T (14)

C = [1, 0, 0, 0]T (15)

D = [0, 0, 0, 0]T (16)

7 of 11


Dow

nloa

ded

by P

UR

DU

E U

NIV

ER

SIT

Y o

n Se

ptem

ber

21, 2

013

| http

://ar

c.ai

aa.o

rg |

DO

I: 1

0.25

14/6

.201

3-47

90


The first three states of the model represent the AR.Drone’s position, velocity and acceleration. The finalstate provides the control state (due to the ∆u(k) formulation).

IV.C. Results and Comparisons

A set of six obstacles were hard coded into the path planner and velocity limits of ±0.5ms−1 were imposedon the quadrotor. A sensing radius of 2m was assumed. The obstacles were all increased in size by 0.125min both axes to allow a safety margin for avoidance. Figure 5(a) shows the route taken by the vehicle usingthe orhtogonal convexification, starting in the bottom left hand corner. Due to the two stage rectangularoperating region scheme, the quadrotor can be seen to fly in a straight line along the y direction abouthalfway through the flight, even after clearing the obstacle on the right hand side. It is only once the targetis further away in the x direction that the operating region is expanded in that direction and the headingof the quadrotor changes. Figure 5(b) shows the velocity plot for the experiment, demonstrating that thevelocity constraint was satisfied.

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 0

0.5

1

1.5

2

2.5

3

3.5

4

4.5

5

x−position (m)

y−po

sitio

n (m

)

(a) Route taken around obstacles

0 5 10 15 20 25−0.1

0

0.1

0.2

0.3

0.4

0.5

0.6

Time / seconds

Vel

ocity

/ m

s−1

X velocityY velocity

(b) Satisfaction of 0.5ms−1 velocity constraint

Figure 5. Experimental results with AR.Drone

Figure 6 compares the position prediction error at each step for (a) the setpoint tracking controllerdeveloped here and (b) a controller regulating to the origin in the frame centred on the setpoint. Notice thesignificant spikes in the regulation case, which coincide with changes in setpoint. At these times the optimizerfailed to converge due to the large shifts in the reference frame making the initial solution infeasible. Thisillustrates the utility of the tracking controller in using a consistent frame of reference throughout.

A study was conducted to determine how many Newton step iterations should be performed. Figure 7(a)shows the increase in computation time required for increasing the number of iterations. Figure 7(b) showsthe velocity profiles for three trajectories for a range of Newton step iterations, demonstrating the constrainthandling quality due to the 0.5ms−1 limit. The results back up the findings of Wang and Boyd,1 who findthat acceptable control can be achieved while using only a small number of iterations.

The computational expense of moving to the non-orthogonal decomposition method increased due tocombining two MPCs into one higher order MPC. To compare the two, the number of Newton iterationswas fixed at 6. It was found that the decoupled MPC, using the first decomposition method had a meanturnaround time of 12ms and a peak of 18ms. The single MPC with the second, non-orthogonal decom-position method had a mean turnaround time of 26ms and a peak of 34ms. This shows that savings canindeed be made by reducing the problem to one MPC per axis, but at the cost of reduced flexibility in posingconstraints.

One potential improvement that could be made to the implementation of the single MPC program wouldbe to take advantage of the structure of the A and B matrices. Similar to other matrices in Fast MPC, theseare block diagonal for the decoupled axes of the vehicle. As auto code generation does not automatically takeadvantage of sparse matrices, the matrix multiplications with these will be more expensive than necessary.

8 of 11


Dow

nloa

ded

by P

UR

DU

E U

NIV

ER

SIT

Y o

n Se

ptem

ber

21, 2

013

| http

://ar

c.ai

aa.o

rg |

DO

I: 1

0.25

14/6

.201

3-47

90


0 5 10 15 20 25−600

−500

−400

−300

−200

−100

0

100

Time / seconds

Pre

dict

ion

erro

r (m

m)

X errorY error

(a) Tracking: Target as setpoint

0 5 10 15 20 25−600

−500

−400

−300

−200

−100

0

100

Time / seconds

Pre

dict

ion

Err

or (

mm

)

X errorY error

(b) Regulation: Target as origin

Figure 6. Evaluation of prediction error in MPC, due to different setpoint formulations

4 6 8 10 12 14 16 18 200

10

20

30

40

50

60

70

Number of Newton iterations

Mea

n tu

rnar

ound

tim

e / m

s

(a) Turnaround time versus Newton iterations

0 2 4 6 8 10 12 14 16 18 20−1

−0.5

0

0.5

14 Newton Iterations

X VelocityY Velocity

0 2 4 6 8 10 12 14 16 18 20−1

−0.5

0

0.5


Vel

ocity

/ m

s−1

0 2 4 6 8 10 12 14 16 18 20−1

−0.5

0

0.5


Time / s

(b) Velocity profiles for different Newton iteration counts

Figure 7. Computational cost and solution quality for different Newton iteration counts

The savings that can be made by extracting just the non-zero elements for multiplication will be identifiedin future work.

V. Conclusions and Future Work

The simple decomposition methods used for path planning in this paper proved to have good performanceavoiding obstacles in a non-convex solution space. When coupled with efficient MPC techniques, it was shownthat realtime operations with online optimization was achievable on modest development hardware.

The path planning methodology does not enable the vehicle to escape from a dead end corridor. Futurework aims to address this issue using the idea of building a list of explored polytopes to enable backtracking.More advanced methods will also be explored such as the incorporation of learning or a higher level pathplanner in a receding horizon or tunnel-like approach.

Acknowledgements

The work presented here was funded by the UK Ministry of Defence CDE (Centre for Defence Enterprise)

9 of 11


Dow

nloa

ded

by P

UR

DU

E U

NIV

ER

SIT

Y o

n Se

ptem

ber

21, 2

013

| http

://ar

c.ai

aa.o

rg |

DO

I: 1

0.25

14/6

.201

3-47

90


References

1Y. Wang and S. Boyd, “Fast model predictive control using online optimization,” Control Systems Technology, IEEETransactions on, vol. 18, no. 2, pp. 267–278, 2010.

2J. H. Reif, “Complexity of the movers problem and generalizations,” in 20th IEEE Symposium on the Foundations ofComputer Science, pp. 421–427, 1979.

3S. M. LaValle and J. J. Kuffner, “Randomized kinodynamic planning,” in Proceedings of International Conference onRobotics and Automation, 1999.

4I. Garcia and J. How, “Trajectory optimization for satellite reconfiguration maneuvers with position and attitude con-straints,” in American Control Conference, 2005. Proceedings of the 2005, pp. 889–894, IEEE, 2005.

5A. G. Richards and J. P. How, “Aircraft trajectory planning with collision avoidance using mixed integer linear program-ming,” in Proceedings of American Control Conference, 2002.

6M. B. Milam, K. Mushambi, and R. M. Murray, “A new computational approach to real-time trajectory generation forconstrained mechanical systems,” in Proceedings of the IEEE Conference on Decision and Control, pp. 845–851, 2000.

7F. Borrelli, D. Subramanian, A. U. Raghunathan, and L. T. Biegler, “MILP and NLP techniques for centralized trajectoryplanning of multiple unmanned vehicles,” in Proceedings of the American Control Conference, 2006.

8I. Cowling, O. Yakimenko, J. Whidborne, and A. Cooke, “Direct method based control system for an autonomousquadrotor,” Journal of Intelligent & Robotic Systems, vol. 60, pp. 285–316, 2010.

9F. Augugliaro, A. Schoellig, and R. D’Andrea, “Generation of collision-free trajectories for a quadrocopter fleet: A se-quential convex programming approach,” in Intelligent Robots and Systems (IROS), 2012 IEEE/RSJ International Conferenceon, pp. 1917 –1922, oct. 2012.

10M. Vitus, V. Pradeep, G. Hoffmann, S. Waslander, and C. Tomlin, “Tunnel-milp: Path planning with sequential convexpolytopes,” in AIAA Guidance, Navigation, and Control Conference, 2008.

11J. S. Bellingham, A. G. Richards, and J. P. How, “Receding horizon control of autonomous vehicles,” in Proceedings ofthe American Control Conference, 2002.

12G. Hoffmann, S. Waslander, and C. Tomlin, “Quadrotor helicopter trajectory tracking control,” in AIAA Guidance,Navigation and Control Conference and Exhibit, Honolulu, Hawaii, pp. 1–14, Citeseer, 2008.

13M. Hehn and R. D?Andrea, “Quadrocopter trajectory generation and control,” in Proceedings of the IFAC world congress,2011.

14“AR.Drone quadrotor.” http://ardrone.parrot.com.15J. How, B. Bethke, A. Frank, D. Dale, and J. Vian, “Real-time indoor autonomous vehicle test environment,” Control

Systems, IEEE, vol. 28, pp. 51 –64, april 2008.16G. Hoffman, D. G. Rajnarayan, S. L. Waslander, D. Dostla, J. S. Jang, and C. J. Tomlin, “The stanford testbed of

autonomous rotorcraft for multi-agent control (starmac),” in Proceedings of the 23rd Digital Avionics Systems Conference,2004.

17N. Michael, D. Mellinger, Q. Lindsey, and V. Kumar, “The grasp multiple micro-uav testbed,” Robotics & AutomationMagazine, IEEE, vol. 17, no. 3, pp. 56–65, 2010.

18M. Deittert, A. G. Richards, and G. Mathews, “Receding horizon control in unknown environments: Experimentalresults,” in IEEE International Conference on Robotics and Automation (ICRA), (Anchorage, Alaska), May 2010.

19J. M. Maciejowski, Predictive Control with Constraints. Prentice Hall, 2002.20D. Limon, I. Alvarado, T. Alamo, and E. Camacho, “MPC for tracking piecewise constant references for constrained

linear systems,” Automatica, vol. 44, no. 9, pp. 2382–2387, 2008.21U. Maeder, F. Borrelli, and M. Morari, “Linear offset-free model predictive control,” Automatica, vol. 45, no. 10, pp. 2214

– 2222, 2009.22A. G. Richards, “Fast model predictive control with soft constraints,” in European Control Conference, (Zurich), July

2013.23T. Schouwenaars, J. P. How, and E. Feron, “Receding horizon path planning with implicit safety guarantees,” in Pro-

ceedings of the American Control Conference, 2004.24“Vicon motion systems.” http://www.vicon.com/.25“dSpace MicroAutoBox.” http://www.dspace.com/en/pub/home/products/hw/micautob.cfm.

A. Terminal Equality Constraints in Fast MPC

This appendix describes how to add a set of `F terminal equality constraints EFx(k + T ) = eF to theFast MPC formulation of Wang and Boyd.1 Adopting the same notation as in that paper, starting fromtheir (6), the problem is converted to a nonlinear optimization in the form

minimize zTHz + gT z + κφ(z) (17)

subject to Cz = b (18)

where z is an amalgamation of all the decision variables z = (∆u(k)T , x(k)T , . . .∆u(k + T − 1)Tx(T )T )T

and κφ(z) is a barrier function representing the inequalities. With the added inequalities, this means that

10 of 11


Dow

nloa

ded

by P

UR

DU

E U

NIV

ER

SIT

Y o

n Se

ptem

ber

21, 2

013

| http

://ar

c.ai

aa.o

rg |

DO

I: 1

0.25

14/6

.201

3-47

90


the matrices C and b now have the structures

C =

−B I · · · 0 0 0

0 −A · · · 0 0 0...

.... . .

......

...

0 0 · · · −A −B I

0 0 · · · 0 0 EF

b =

Ax(k) + w(k)

w(k)...

w(k)

eF

Note that the extra `F equality constraints in the problem will require a corresponding extra `F elements inthe dual variable ν. The modified Schur complement Y = CΦ−1CT used to solve for the step in ν is givenby

Y =

Y11 Y12 · · · 0 0 0

Y21 Y22 · · · 0 0 0...

.... . .

......

...

0 0 · · · YT−1,T−1 YT−1,T 0

0 0 · · · YT,T−1 YTT YFC

0 0 · · · 0 Y TFC YFF

where YFC = QTE

TF and YFF = EFYFC and all other submatrices are calculated as in Ref. 1. Note that this

retains the same sparsity pattern as the original Y so the same solution approach is followed: the Cholesyfactor of Y is

L =

L11 0 · · · 0 0 0

L21 L22 · · · 0 0 0...

.... . .

......

...

0 0 · · · LT−1,T−1 0 0

0 0 · · · LT,T−1 LTT 0

0 0 · · · 0 LFC LFF

where LFC is found by solving LTTLFC = YFC and then LFF is found by the Cholesky factorization LFFL

TFF =

YFF − LFCLTFC . Then the solution of the equation Y∆ν = β can then proceed as in Ref. 1 making use

of the Cholesky factorization L of Y . The modifications to include the equality constraints are simply andextra step on the end of each stage in the process of solving for ∆ν.

11 of 11


Dow

nloa

ded

by P

UR

DU

E U

NIV

ER

SIT

Y o

n Se

ptem

ber

21, 2

013

| http

://ar

c.ai

aa.o

rg |

DO

I: 1

0.25

14/6

.201

3-47

90


Documents

[American Institute of Aeronautics and Astronautics AIAA Guidance, Navigation, and Control (GNC) Conference - Boston, MA ()] AIAA Guidance, Navigation, and Control (GNC) Conference