# Safe and Robust Robot Maneuvers Based on Reach Control

## Abstract

In this paper, we investigate the synthesis of piecewise affine feedback controllers to address the problem of safe and robust controller design in robotics based on high-level controls specifications. The methodology is based on formulating the problem as a collection of reach control problems on a polytopic state space. Reach control has so far only been developed in theory and has not been tested experimentally on a real system before. Using a quadrocopter as our experimental platform, we show that these theoretical tools can achieve fast, albeit safe and robust maneuvers. In contrast to most traditional control techniques, the reach control approach does not require a predefined open-loop reference trajectory or spacial path. Experimental results on a quadrocopter show the effectiveness and robustness of this control approach. In a proof-of-concept demonstration, the reach controller is implemented in one translational direction while the other degrees of freedom are stabilized by separate controllers.

## 1Introduction

This paper proposes a novel framework for control of complex robotic maneuvers that simultaneously impose requirements of safety, fast response, and a desired sequence of events. We apply the framework to a simple side-to-side maneuver on a quadrocopter in order to expose the main features of the framework. The framework is based on using hybrid systems, event-based switching, and solving a collection of so-called reach control problems (RCP). The RCP has an extensive theoretical development, see [13], but it has been completely lacking in experimental validation. The primary goal of this paper is to illustrate, for the first time on a real system, the various strengths offered by the reach control approach.

Because our main application is quadrocopter maneuvering, we give an overview of current approaches, particularly placing our reach control approach within this literature. There are two predominant methods for control of quadrocopter maneuvers: timed trajectory tracking and path following. In timed trajectory tracking, an open-loop reference trajectory as a function of time is predefined. Then a controller that stabilizes the system to the trajectory is designed. For example, in [3], an impressive collection of aggressive quadrocopter maneuvers is featured using this approach. Timed trajectory tracking is the most common method in the literature [3]. On the other hand, difficulties arise in finding the open-loop reference trajectory. For example, in [1] it is first geometrically specified using splines, and then time parameterized so that the resulting reference trajectory is feasible. Also, while timed trajectory tracking can provide high-performance maneuvers, due to the open-loop nature of the reference signals, any additional unaccounted disturbances can quickly deteriorate performance.

In path following, a 3D spatial (untimed) path is specified in output space. Then an output stabilization method is used to keep the system on the path. Recent applications of path following to quadrocopter maneuvering include [5]. The benefit of this method compared to timed trajectory tracking is better control of transients: when the system deviates from the path, it must only steer back to the path rather than to a specific point in time. The difficulty in this method is again finding paths feasible for the dynamics and constraints.

In our approach, it is not required to generate a feasible timed trajectory or path. Rather, control specifications are given that restrict the dynamics to a feasible region of the state space. Further, these specifications inform on how the state must evolve in that region. The region is then partitioned into smaller regions (in our case simplices) and a feedback controller is designed for each region to guarantee correct evolution of the state. As such, our method has several significant advantages: 1) we bypass the construction of a reference trajectory or path; 2) we obtain feedback controllers, not open-loop controls; 3) we obtain controllers for the entire feasible region of operation, not only a neighborhood of a path; 4) we explicitly account for safety constraints and actuator limits. The main difficulty of our method (as it is implemented right now) is its application to high-dimensional systems, where the required state-space partitioning becomes more involved. In the future, advanced computational tools may be adopted from other fields to address this problem. On the other hand, this paper shows that high-order models can be reduced to simpler models with no degradation of performance. For related methods to our approach see [9]. Reach control has never been applied on a real system before. This work presents a proof-of-concept of its practical applicability.

## 2Methodology

In this section, we briefly outline the reach control methodology, which allows us to define high-level controls specifications, and results in safe and robust system behavior.

### 2.1System Model and Control Specifications

We assume that the system is modeled as a finite-dimensional, dynamical system, with possibly nonlinear dynamics:

where is the state and is the control input.

The objective of the controller is defined in terms of high-level safety and event specifications. In particular, the approach can handle the following types of specifications:

**Safety and Liveness** Safety and liveness constraints define the region in the state space that the system is allowed to visit. Safety constraints limit the dynamics to a safe regime of operation. Liveness constraints enforce fast, lively response. Our framework requires that the feasible region be a polytope , as shown in Figure 3.

**Desired Temporal Sequence** This specification describes the overall sequence of events or the overall motion of the system. A set of target states to be reached by the system must be defined. For more complex maneuvers, a sequence of target sets may be defined, and can be formalized by using automata from discrete event systems [20]. For example, Figures Figure 3 and ? show that the system must traverse the polytope clockwise by moving through a sequence of triangles , .

The specifications above can handle complex tasks, but can be computationally difficult for high-dimensional systems; a higher-level control architecture may help to decouple the complexity and will be illustrated in our quadrocopter example.

To summarize, the control specifications require determining a polytope, which describes the allowable states and a sequence of target sets. Given this data, the objective is to find a controller that ensures the states of the robotic system remain in the polytope while reaching the correct sequence of target sets. For a related example on complex control specifications in the context of reach control, see [16].

### 2.2Triangulation of the Polytope

To drive any initial state starting in to a target state while remaining in can be difficult. To systematize the design, we triangulate into a set of simplices and we specify a controller on each simplex, see Figure ?. Informally, the polytope is partitioned into triangles.

Formally, an -dimensional *simplex*, , is the convex hull of affinely independent points in ; it is the generalization of a triangle. A *facet* of a simplex is a boundary face of dimension . A *triangulation* is a partition of a set into simplices and is denoted as , see [19]. Then satisfies the properties:

and

is a lower-dimensional simplex of both and or the empty set for all .

Once a triangulation of has been specified, the next step of the design is to identify a sequence of simplices to be visited in order to be able to reach the target states.

Using the sequence of simplices, *exit facets* for each simplex are designated. The trajectories starting in the given simplex may only exit the simplex through the exit facets, while the remaining facets act as *restricted facets*.

On each simplex, it is assumed that the dynamics of the system are affine, having the form

where , , and the matrices have appropriate dimensions. If the dynamics are nonlinear, they may be linearized about some point in the simplex to yield the form .

### 2.3Control Design via the Reach Control Problem

Finally, controllers are designed for each simplex based on the *reach control problem* (RCP). This method ensures that closed-loop trajectories flow through the designated exit facets without crossing the restricted facets. The reach control problem formulation, its theoretical developments, and conditions for solvability are discussed in [13].

Below we summarize the procedure for determining a controller over an arbitrary simplex in our triangulation; see Figure 1 for a 2D example.

We define to be the index set for the vertices of the simplex. The coordinates of each vertex are denoted as , . For each of the vertices, , we must pick a corresponding . Each facet, , of the simplex is indexed by the vertex index that it does *not* contain, and each facet has an associated normal vector to describe its orientation, see Figure 1. We assume that there is at least one restricted facet, and index the restricted facets using .

To solve RCP on a given simplex, we must select controls at the vertices to satisfy the so-called *invariance conditions* [12]; that is,

This condition encodes that the velocity vector at each vertex points in the right direction so that trajectories leave the simplex through an exit facet while avoiding crossing the restricted facets. The feasibility of the inequalities in can be easily checked numerically via a linear program. If they are not feasible, then RCP is not solvable and the choice of restricted and exit facets must be modified. If they are feasible, then for a feasible choice of , , it can be shown that one can construct an affine feedback to be used over the entire simplex, see [10]. The affine feedback controller has the form

where and are obtained using

The final step is to check that the closed-loop system, , contains no equilibrium in the simplex. If so, then RCP is solved over the simplex. For a more detailed discussion on the design of RCP controllers, see [14].

In summary, the resulting control law over is a piecewise affine feedback with switching between controllers occurring at the boundaries between contiguous simplices.

## 3Application to a Quadrocopter Maneuver

We follow the methodology described earlier to design a controller for executing a simple side-to-side quadrocopter maneuver. Due to the complexity of the quadrocopter system, our overall control strategy relies on a standard quadrocopter control architecture, depicted in Figure 2, that decouples the various degrees of freedom, see Section 3.1. One degree of freedom, corresponding to the design of the Reach Controller in Figure 2, is responsible for controlling the side-to-side motion aspect of the maneuver and is the main point of focus in this paper. The remainder of the control architecture is standard and ensures that the quadrocopter executes the side-to-side motion while stabilizing the remaining degrees of freedom.

### 3.1Quadrocopter Model

The quadrocopter model is ubiquitous in the literature; see, for example, [2] or Chapter 4 of [4]; we refer the reader to those references for details. The vehicle dynamics are described by six degrees of freedom and are nonlinear. The translational position is measured in the inertial coordinate system as shown in Figure ?. The vehicle attitude is defined by the body-fixed frame and is represented by the -Euler angles, yaw, pitch, and roll, . The full state of the vehicle additionally includes the translational and rotational velocities of the body frame, represented in and represented in , respectively.

In our control architecture (Figure 2), we assume that the full state of the vehicle is measured. An onboard controller takes the desired pitch angle , roll angle , angular body velocity around the body’s -axis , and vertical velocity of the vehicle as inputs and calculates the required motor forces , . In our experiment, the onboard controller is an unmodifiable blackbox. In the offboard controller, a standard, nonlinear tracking controller (as, for example, proposed in [3]) is used for stabilizing the - and -coordinates of the vehicle as well as the yaw. We use the Reach Controller for the -direction.

We assume that the nonlinear controller successfully stabilizes the vehicle at , , and , , and provides the onboard controller inputs , , and . The equations governing the - and -motion of the vehicle are then given by

where is the gravitational constant and is the collective thrust normalized by the vehicle mass ,

with motor forces , , see Figures ? and Figure 2.

Since implies , equation gives , and with we have

To summarize this analysis, if we can define an profile, then with and this will produce a desired pitch angle signal to be input for the onboard controller (see again Figure 2). The signal will be constructed via the reach control methodology outlined earlier and this will yield a feedback .

This approach can be extended to three dimensions: similarly as above, each direction can be formulated as a single- or double-integrator, linear system. The challenge is in partitioning the state space. A recent method for complexity reduction involves working in output space [17].

### 3.2Control Specifications

In the next step of our methodology, we state the control specifications, which result in a polytope and a sequence of target sets, see Figure 3.

**Safety Specifications** To remain within the boundaries of the room, we require , see Figure 3. The maximum speed limitation imposes . For a safe turnaround, we impose a deceleration requirement close to the room’s boundaries; for compatibility with the RCP approach we use linear inequalities. We define a safe, minimum deceleration, , and obtain the following linear inequalities: , where we choose , see Figure 3. For safety to be ensured, all three inequalities must be simultaneously satisfied at all times:

Position: .

Speed: .

Deceleration: .

**Liveness Specifications** The liveness specifications ensure that the quadrocopter moves with sufficient speed when cruising between the ends of the room. Using our parameters as defined in Figure 3, we first define the inequality , which says the speed should be above the minimum. Next we define , and , with . These inequalities describe how the quadrocopter should accelerate from zero speed to the minimum speed and decelerate from the minimum speed to zero speed, respectively. For liveness to be ensured, any of the three inequalities must be satisfied at all times:

Speed: .

Acceleration: .

Deceleration: .

**Desired Temporal Sequence** We decompose this side-to-side maneuver into two opposing discrete modes: one in which the quadrocopter is moving from left to right, referred to as the L2R; and one in which the quadrocopter is moving from right to left, denoted by R2L. For the L2R and R2L modes respectively, we define target sets to be reached in order to transition to the opposite mode:

The sets are shown in Figure 3 (green lines). Assuming that the initial mode is L2R, the sequence of target sets to be crossed by the system trajectory is:

, , , , .

For the experiment, we fix the values of the polytope parameters in Figure 3 to: m, m, m, m/s, and m/s.

### 3.3Triangulation and Exit Facets

Our triangulation is shown in Figure ?. The vertices of the triangulation are uniquely labeled as , . The simplices are uniquely labeled as , . Although automated procedures for triangulating and solving RCP in conjunction are considered in [18], here the triangulation was naively generated by manually partitioning into simplices.

Next, to define the sequence of simplices to be visited, for each simplex we choose its restricted and exit facets. This information is also encoded in Figure ? via the red dashed lines. For the L2R mode, the facets were chosen so as to ensure that the resulting closed-loop vector field causes trajectories to reach the set . Due to symmetry, the R2L can be implemented trivially using the L2R mode’s design.

We remark that we also triangulated the non-liveness region (the orange region in Figure ?) in order to improve the robustness of our design. In the case that the system enters this region due to a large disturbance, the controllers defined on , , return the system to nominal behavior.

### 3.4Resulting Controller Design

Now we solve RCP on each simplex , . Equation showed that under perfect stabilization in the - and -directions, the dynamics in the -direction reduce to . For this double-integrator model, the dynamics over each simplex are

where , whose components are the -position and -velocity of the quadrocopter, respectively.

With the triangulation and exit facets presented, we now construct the controllers on each simplex. For each simplex , , we identify the three corresponding vertices , , , that form the simplex along with the corresponding exit facets . We design the control values at the three vertices such that the invariance conditions are satisfied. Note that larger control values result in more aggressive controls, and hence actuator constraints can also be incorporated. Finally, we use to obtain the feedback law . The controllers can be computed manually or using a numerical solver.

The resulting closed-loop dynamics for the L2R mode over and the non-liveness region is shown in Figure ?. As expected, trajectories above the -axis that start in remain inside and are guided towards the set , while any other non-nominal trajectory (starting in below the -axis or in the non-liveness region) eventually recovers and crosses . The closed-loop vector field for the R2L mode is obtained by oddly reflecting the L2R design about the origin.

We remark that to avoid discontinuities in the control when transitioning between simplices, we match the control values at the vertices along shared facets between contiguous simplices. For technical reasons related to solving RCP, it is necessary to introduce a discontinuity at [15].

## 4Experimental Results

Our experimental platform is the Parrot AR.Drone 2.0 running firmware version 2.3.3. We interface with the AR.Drone through ROS, an open-source robot operating system [21]. More precisely, we used ROS Hydro, installed on a 64-bit 12.04 Ubuntu version. In addition, we used the ROS *ardrone autonomy* package [21], version 1.3.1. All experiments were conducted with the indoor hull shown in Figure ?, which protects the vehicle propellers.

We demonstrate the successful execution of the desired side-to-side motion based on our RCP approach and compare it to the performance of a standard trajectory tracking controller [2], which guides the vehicle along a predefined, timed side-to-side trajectory. A video showing the experimental results can be found at: http://tiny.cc/quadrotorRCPx.

In our experiments, the following actions were performed for both trajectory tracking and the RCP approach: 1) nominal flight consisting of a few cycles of the L2R and R2L modes, 2) introducing a disturbance by manually holding the vehicle, and 3) introducing a disturbance by pushing the vehicle. At the top of Figures ? and ?, we show the position over time. The key difference that we observe is that when the quadrocopter is disturbed, the tracking approach fails the desired temporal sequence whereas the RCP approach does not. The middle plots show that the nominal behavior of both methods are comparable. The bottom plots show the significant degradation caused by the disturbances, where only in the tracking approach the trajectories exit the safety region by speeding up too much.

## 5Conclusion

We succeeded in experimentally demonstrating the first ever implementation of reach controllers on a real system. The result is a logically complex quadrocopter maneuver. The main advantages of the reach control approach are that it permits the incorporation of safety constraints, event sequences and logical constraints, and robustness to unmodeled disturbances, as shown in our comparison between the reach control and standard tracking approaches. The side-to-side maneuver shown here was mainly chosen to demonstrate the proof-of-concept, and so an extension would be to apply the RCP methodology to a more complex maneuver and compare to path following controllers.

### References

- A. P. Schoellig, F. Mueller, and R. D’Andrea, “Optimization-based iterative learning for precise quadrocopter trajectory tracking,”
*Autonomous Robots*, vol. 33, nos. 1-2, pp. 103-127, 2012. - A. P. Schoellig, M. Hehn, S. Lupashin, and R. D’Andrea, “Feasibility of motion primitives for choreographed quadrocopter flight”, in
*Proceedings of the American Control Conference (ACC)*, 2011, pp. 3843-3849. - S. Lupashin, M. Hehn, M. Mueller, A. P. Schoellig, M. Sherback, and R. D’Andrea, “A platform for aerial robotics research and demonstration: the Flying Machine Area,”
*Mechatronics*, vol. 24, no. 1, pp. 41-54, 2014. - A. LaViers and M. Egerstedt, “Controls and art: inquires at the intersection of the subjective and the objective,” Springer, 2014.
- D. Mellinger, N. Michael, and V. Kumar, “Trajectory generation and control for precise aggressive maneuvers with quadrotors,”
*The International Journal of Robotics Research*, vol. 31, no. 5, pp. 664-674, 2012. - E. Xargay, I. Kaminer, A. Pascoal, N. Hovakimyan, V. Dobrokhodov, V. Cichella, A. P. Aguiar, and R. Ghabcheloo, “Time-Critical Cooperative Path Following of Multiple Unmanned Aerial Vehicles over Time-Varying Networks,”
*Journal of Guidance, Control, and Dynamics*, vol. 36, no. 2, pp. 499-516, 2013. - A. Roza and M. Maggiore, “Path following controller for a quadrotor helicopter,” in
*Proceedings of the American Control Conference (ACC)*, 2012, pp. 4655 - 4660. - J. Gillula, G. Hoffmann, H. Huang, M. Vitus, and C. Tomlin, “Applications of hybrid reachability analysis to robotic aerial vehicles,”
*The International Journal of Robotics Research*, vol. 30, no. 3, pp. 335-354, 2011. - A. D. Ames, J. W. Grizzle, and P. Tabuada, “Control Barrier Function based Quadratic Programs with Application to Adaptive Cruise Control,” in
*Proceedings of the IEEE Conference on Decision and Control*, 2014, pp. 6271-6278. - L.C.G.J.M. Habets and J.H. van Schuppen, “A control problem for affine dynamical systems on a full-dimensional polytope,”
*Automatica*, vol. 40, no. 1, pp. 21-35, 2004. - L.C.G.J.M. Habets, P.J. Collins, and J.H. van Schuppen, “Reachability and control synthesis for piecewise-affine hybrid systems on simplices,”
*IEEE Transactions on Automatic Control*, vol. 51, no. 6, pp. 938-948, 2006. - B. Roszak and M. E. Broucke, “Necessary and sufficient conditions for reachability on a simplex,”
*Automatica*, vol. 42, no. 11, pp. 1913-1918, 2006. - M.E. Broucke, “Reach control on simplices by continuous state feedback,”
*SIAM Journal on Control and Optimization*, vol. 48, no. 5, pp. 3482-3500, 2010. - G. Ashford and M.E. Broucke. Design of reach controllers on simplices.
*IEEE Conference on Decision and Control*. Florence, Italy, Dec. 2013. - M.E. Broucke and M. Ganness, “Reach control on simplices by piecewise affine feedback,”
*SIAM Journal on Control and Optimization*, vol. 52, no. 5, pp. 3261 - 3286, 2014. - M. Vukosavljev and M. E. Broucke, “Control of a gantry crane: A reach control approach,” in
*Proceedings of the IEEE Conference on Decision and Control*, 2014, pp. 3609-3614. - Z. Kroeze and M. E. Broucke, “A Viability Approach to the Output Reach Control Problem,” in
*Proceedings of the American Control Conference*, 2016. - M. Kloetzer and C. Belta, “A fully automated framework for control of linear systems from temporal logic specifications,”
*IEEE Transactions on Automatic Control*, vol. 53, no. 1, pp. 287-297, 2008. - C. W. Lee, “Subdivisions and triangulations of polytopes,”
*Handbook of Discrete and Computational Geometry*, CRC Press Series Discrete Math. Appl., pp. 271-290, 1997. - R.J. Ramadge and W.M. Wonham. Supervisory control of a class of discrete event processes.
*SIAM J. Control and Optimization*. Vol. 25, no. 1, pp. 206-230, 1987. - Ardrone autonomy package, ver. 1.3.1, available at www.ros.org.