Obstacle Avoidance for 2D Quadrotor with Hanging Load

Ayush Agrawal, Nathan Bucki, Prasanth Kotaru, David Meister, and Martin Xu

Abstract— In this project, we use tools from optimal controlto generate and stabilize feasible and safe trajectories for atwo-dimensional model of a quadrotor with a cable suspendedpayload in cluttered environments. Due to the high degreeof underactuation and nonlinear dynamics of the system,optimization-based techniques, for specifying control objectivesand complex desired behaviors, are better suited over tradi-tional control design methods for these systems. In particular,we develop our trajectory generation scheme using recent meth-ods developed within the optimization-based collision avoidanceframework, where obstacles are modeled as polyhedra and non-differentiable collision avoidance constraints are reformulatedinto smooth nonlinear constraints. We then use a controller inthe Model Predictive Control (MPC) framework to track thattrajectory and compare it with an infinite-time horizon LQRcontroller.

I. INTRODUCTIONIn recent years, small-scale unmanned aerial vehicles such

as quadrotors have shown great potential in applicationsincluding load transportation in urban environment as well asremote areas. The problem of using a suspended payload isparticularly interesting since this allows for more dynamicmaneuvers as opposed to the problem where the load isrigidly attached to the quadrotor frame (the latter, in fact,leads to a higher moment of inertia leading to a more slug-gish response). Adding a cable suspended payload, however,leads to additional control challenges due to (a) high degreeof underactuation, (b) nonlinear dynamics of the system and(c) input and state constraints.

In this project, we approached the problem by first testingour trajectory planner and controllers on a 2D quadrotorwithout a hanging load. Our initial tests used only simple ob-stacles and defined the quadrotor as a point mass for collisiondetection purposes. Next, we considered the quadrotor as afull-dimensional object by defining a bounding box aroundthe vehicle that is constrained to never intersect the obstacles.Finally, we added the hanging load with a similar boundingbox and associated collision avoidance constraints.

Because the collision avoidance constraints are ini-tially non-differentiable, we reformulate the constraints intosmooth nonlinear constraints using strong duality as pre-sented in [2]. This allows us to perform non-convex optimiza-tion using IPOPT in order to generate an optimal trajectoryfor the system.

After verifying our model, planner, and controllers, wetested our system in various scenarios as shown in Section V.

II. DYNAMICS DERIVATIONThe dynamics for the quadrotor with a suspended payload

(Figure 1) are defined based on the model presented in [1].

Project Video can be found here: https://youtu.be/BfGQMpfwSIc

Fig. 1. 2D Model of Quadrotor with Load

TABLE IVARIABLE DEFINITIONS FOR QUADROTOR WITH LOAD

Variable DescriptionyL, zL Load position

φL Load angle w.r.t. zl Cable length

mL Load massyQ = yL− l sinφL Quadrotor positionzQ = zL + l cosφL

φQ Quadrotor angle w.r.t. ymQ Quadrotor massJQ Quadrotor moment of inertialQ Quadrotor arm lengthF Total Thrust from propellersM Total Moment from propellers

F1, F2 Thrust from individual propellers

However, after deriving the equations of motion by usingboth the Newton and Lagrange methods, we discovered andcorrected several errors in the equations of motion presentedin [1]. Table I shows how the variables are defined, and thecorrected equations of motion are given in (1).

X =

yLzLφLφQ

− mQmQ+mL

lφ 2L sinφL

mQmQ+mL

lφ 2L cosφL−g00

︸ ︷︷ ︸

fvec

+

0 00 00 00 0

−sinφLmQ+mL

cos(φQ−φL) 0cosφL

mQ+mLcos(φQ−φL) 0

1mQl sin(φQ−φL) 0

0 1JQ

︸ ︷︷ ︸

gvec

[FM

],

(1)[FM

]=

[1 1−lQ lQ

][F1F2

](2)

X =[yL zL φL φQ yL zL φL φQ

](3)

III. OPTIMAL TRAJECTORY GENERATION

We define the control problem in two parts: first we gen-erate an optimal trajectory that includes obstacle avoidanceconstraints, and then we track the generated trajectory usingdifferent controllers. The following details our formulationof the optimal trajectory generation problem.

The obstacles are defined by (4), and the polyhedrarepresenting the quadrotor are defined by (5), where R(xk)and t(xk) are the respective rotation matrices and translationvectors representing the rotation and translation of the poly-hedra at time k. We use one bounding box to define the sizeof the quadrotor body and a second bounding box to definethe size of the load.

O(m) = {y ∈ Rn : A(m)y≤ b(m)}, (4)

E(xk) = R(xk)B+ t(xk), B := {y : Gy≤ g}. (5)

We reformulate the obstacle avoidance constraints usingstrong duality as shown in [2]. This gives the optimizationproblem in (6), where the variables are defined as in table II,k = 0, . . . ,N and m = 0, . . . ,M.

TABLE IIVARIABLE DEFINITIONS FOR TRAJECORY GENERATION

Variable DescriptionTopt Timestep lengthxk State vector at time kuk Control input vector at time k

λ(m)k , µ

(m)k Dual variables for obstacle m at time k

q1, q2 Timestep cost tuning parametersR Control input cost tuning parameterN Total number of timestepsM Total number of obstacles

f (xk,uk) System dynamicsh(xk,uk) Input and state constraints

τF Final timexF Goal state

minTopt ,x,u,λλλ ,µµµ

q1Topt +q2T 2opt +

N

∑k=0

uTk Ruk

s.t. x0 = x(0), xN+1 = xF ,τF = NToptxk+1 = f (xk,uk), h(xk,uk)≤ 0,−g>µ

(m)k +(A(m) t(xk)−b(m))>λ

(m)k > 0,

G>µ(m)k +R(xk)

>A(m)>λ(m)k = 0,

‖A(m)>λ(m)k ‖∗ ≤ 1, λ

(m)k ≤ 0, µ

(m)k ≤ 0.

(6)We chose our cost function such that the time it takes to

reach the final state is minimized. Because a constraint onthe final state of the system is included, we did not includeany stage cost based on the state vector at each timestep.Thus, we seek to minimize some weighted sum of the timeit takes to reach the goal position and the magnitude of thecontrol inputs used at each timestep.

IV. TRACKING CONTROL

We tested the ability of both an MPC and infinite hori-zon discrete time LQR controller to track the trajectoriesgenerated by our optimization problem for several differentscenarios.

A. DLQR Controller

The LQR controller was intended to serve as a baselineto which we could compare our MPC controller, and wasdefined as a full state feedback controller that minimizes thefollowing cost function. Qd and Rd are chosen to be diagonalmatrices that result in reasonable tracking responses. x is thegenerated optimal trajectory to be tracked.

J =12

∞

∑k=0

{e(k)T Qde(k)+uT (k)Rdu(k)

}(7)

e(k) = x(k)− x(k). (8)

The system is linearized and discretized about the tra-jectory at each timestep, giving an A and B matrix withwhich we calculate the optimal feedback gain K. Finally, theoptimal control input obtained from trajectory generation isadded in as a feedforward term,

u(k) =−Ke(k)+ u(k). (9)

B. MPC tracking

Because the MPC controller is intended to run in real timeon the system, we make some relaxations to the optimizationproblem given in (6). Namely, the MPC program does notinclude collision constraints and has a cost function basedon the control effort and state deviation from the optimaltrajectory. Equation (10) gives the optimization problemsolved by the MPC tracking controller at time-step k, withhorizon N, for tracking the trajectory x.

u∗k = argminx,u

N

∑i=0

(xi− xk+i)T Q(xi− xk+i)+(uT

i − uk+i)R(ui− uk+i)

s.t. x0 = x(k),xi+1 = f (xi, ui)+A(xi− xi)+B(ui− ui),xiL ≤ xi ≤ xiUuiL ≤ ui ≤ uiU .

(10)MATLAB ode45 solver is used to simulate the forward

dynamics. The dynamics x = f (x,u) are simulated betweenthe time-span [tk, tk+1] using the input u∗k calculated in (10).Note that linearized dynamics (about the reference trajectorygenerated in section III) are used in the MPC control,whereas the complete non-linear dynamics are used in thesimulations.

V. PERFORMANCE

We tested our trajectory generator and tracking controllerson three difference scenarios: flying through a window,swinging the mass into an inverted pendulum configuration,and flying between two offset triangles. Figure 2 shows

the optimal trajectory generated by our trajectory generationprogram for the window obstacle scenario.

We tested our tracking controllers by changing either thecontrol input limits or the dynamics of the system. Figure 3shows the performance of the two tracking controllers whentighter control input constraints are imposed than duringtrajectory generation. Because the MPC controller is ableto take the new control input constraints into account, itoutperforms the LQR controller. Figure 4 shows how thecontrol inputs saturate during the simulation for each trackingcontroller.

Figure 5 shows how the tracking controllers respondto a 10% increase in weight of the hanging load duringsimulation. Although both controllers perform roughly thesame, the MPC controller has slightly less tracking errorthan the LQR controller.

Animations of the trajectory and plots for the other twotest cases are shown in our presentation.

Fig. 2. Generated trajectory with window obstacle. The solver is given aninitial trajectory that is kinematically but not dynamically feasible.

VI. CONCLUSIONS

Reformulating the obstacle constraints using strong dualityallowed us to generate trajectories capable of avoiding obsta-cles while minimizing a weighted sum of control inputs andtrajectory execution time. We used both an MPC and infinitehorizon discrete time LQR controller to track the generatedtrajectory for three difference scenarios, which showed thesuperior tracking ability of of the MPC controller comparedto the LQR controller in cases with tighter input constraintsand model errors.

REFERENCES

[1] Koushil Sreenath, Nathan Michael, and Vijay Kumar. Trajectorygeneration and control of a quadrotor with a cable-suspended load – adifferentially-flat hybrid system. In IEEE International Conference onRobotics and Automation (ICRA), 2013.

[2] Xiaojing Zhang, Alexander Liniger, and Francesco Borrelli.Optimization-based collision avoidance. arXiv preprintarXiv:1711.03449, 2017.

Fig. 3. Y-Z Trajectory of load when using either LQR or MPC trackingcontroller. Deviations from reference trajectory are due to tighter constraintson the control input of the tracking controllers.

Fig. 4. Control inputs over time for both tracking controllers. The inputssaturate around t = 3, but the MPC controller performs actions to bring itcloser to the desired trajectory than the LQR controller as shown by thesmall dips from the control input limit.

Fig. 5. Comparison of trajectory tracking error between LQR and MPCcontrollers with model uncertainty. During simulation the mass of the loadwas increased by 10%