6.2 Trajectory Generation
6.3.3 Catching a bouncing ball
This experiment demonstrates the ability to continuously replan in order to catch a bounc- ing ball. To enable this experiment a gripper was developed to throw and catch a golf ball from the underside of a quadrotor. The sides of the gripper can be opened simultaneously in order to drop the ball straight down or can be actuated independently in order to throw the ball to the left or to the right as shown in Fig. 6.7.
Here we describe a method for catching a ball at the peak of its flight when the z
velocity is zero. As the ball ascends into the gripper the gripper begins to close. The ball then reaches its peak inside the gripper and by the time the ball begins to descend the gripper has closed and the ball remains inside the gripper. A flowchart outlining the
Figure 6.6: Composite image of a single quadrotor flying through a thrown circular hoop. See attached video or http://tinyurl.com/pennquad.
method is shown in Fig. 6.8. As seen in this figure the method for catching a ball requires two main components, the prediction of the peak of the ball, theApogee Estimator, and the planning to reach the desired state, theTrajectory Generator.
New Ball Position Trajectory Generator Quality Apogee Estimate Safe Trajectory Apogee Estimator Vicon Controller
Figure 6.8: Flowchart for catching a ball.
Apogee Estimator
Here we describe our method for predicting when and where the ball will reach its apogee after a bounce. The first requirement is to find the current position and velocity of the ball. We cover the ball with reflective tape and use a Vicon Motion capture system to track it at 150 Hz. There is some noise in the Vicon measurements and Vicon occasionally loses track of the ball. Therefore we developed a method that estimates the state of the ball only if the system has maintained a quality track for at least some amount of time and if the noise is below some threshold.
At each time step we consider the lastn measurements (ti, xi,yi,zi) collected during the last0.07seconds of data. Note that we requirento be larger than some threshold. We assume that each measurement is subject to some error (xi, yi, zi). Our measurement model is shown in the equations below.
xi = x0+vx0ti+xi
yi = y0+vy0ti+yi
zi = z0+vz0ti− 12gt2i +zi
(6.7)
Note that here we assume the ball is undergoing parabolic flight under the acceleration of gravity. This implies that we do not attempt to estimate the state of the ball during a bounce event. We also ignore air drag in the model because at low speeds the air drag on the golf ball is negligible.
Using this measurement model the3nmeasurements can be written as: A x0 vx0 y0 vy0 z0 vz0 =Aβ =b+ (6.8)
whereAis a3n×6matrix andbis3nlength vector which both contain the measurements. Here is a 3n length vector containing the measurement noise. The intial velocites and positions are represented byβ which we estimate using a linear least squares method:
ˆ
β = (ATA)−1ATb (6.9)
In order to accept the estimate as a quality estimate the mean squared error, kAβˆ3n−bk2, must be less than some threshold. Given a quality estimate of the initial state of the ball we use a parabolic flight model to find the impact location and velocity of the ball. A golf ball was chosen because it is has a very high coefficient of restitution and also has a consistent bounce. The bouncing surface is a strong factor is determining the bounce of the ball. We use a steel lab benchtop that is 5 cm thick. Our impact model is shown in (6.10) where−and+superscripts represent pre- and post-impact velocities, respectively.
v+ x = v − x v+ y = vy− v+ z = −COR(v − z)v − z (6.10)
Note that we experimentally determined the coefficient of restitution as a linear function of the velocity of impact,COR(vz−). We assume that thex andy velocity are the same pre- and post-impact. However, we acknowledge that unmodeled disturbance is added to the ball when it bounces off the ground due to imperfections in the ball and the surface as well as spin on the ball. After calculating the post-impact velocities we use a parabolic flight model to predict the position and velocity of the ball at its apogee.
Trajectory Generator
When a quality apogee estimate is produced the Apogee Estimator feeds the apogee po- sition, velocity, and time to the Trajectory Generator. The Trajectory Generator computes a minimum snap trajectory from the current desired state of the quadrotor to this desired catch state. Note that we require the catch state to have acceleration and jerk of zero so that the catch is performed with the quadrotor at a level attitude with no angular velocity.
It is possible that the ball is thrown out of reach of the vehicle and we must account for this so the quadrotor does not attempt to follow trajectories that are unsafe. Therefore, after a trajectory is planned we find the maximum velocities, accelerations, jerks, and snaps for the planned polynomial trajectories along each axis. If any of these values exceed chosen safe thresholds then the trajectory is determined to be unsafe and is not executed.
Although it is ideal to reach the catch position at the velocity of the ball, it is possible to still catch the ball even if the vehicle does not move at the ball’s exact velocity. So, if the planned trajectory is unsafe we next plan a path to the catch position and allow the final velocity to be unspecified. This less constrained trajectory will have a lower cost and it is possible that it will be safe while the first planned trajectory was not. If this trajectory is safe, then it is executed. If not, then the quadrotor simply executes whatever safe trajectory it was already executing. Screenshots from a successful experimental trial are shown in Fig. 6.9.
Figure 6.9: Quadrotor catching a thrown ball after a bounce. The ball is highlighted in blue.
Chapter 7
Trajectory Generation with
Mixed-Integer Quadratic Programs for
Heterogeneous Quadrotor Teams
We present an algorithm for the generation of optimal trajectories for teams of hetero- geneous quadrotors in three-dimensional environments with obstacles. We formulate the problem using mixed-integer quadratic programs (MIQPs) where the integer constraints are used to enforce collision avoidance. The method allows for different sizes, capabilities, and varying dynamic effects between different quadrotors. Experimental results illustrate the method applied to teams of up to four quadrotors ranging from 65 to 962 grams and 21 to 67 cm in width following trajectories in three-dimensional environments with obstacles with accelerations approaching1g.
7.1
Introduction
Multi-rotor aerial vehicles have become increasingly popular robotic platforms because of their mechanical simplicity, dynamic capabilities, and suitability for both indoor and out- door environments. In particular, there have been many recent advances in the design [34],
control [54] and planning [35] for quadrotors, rotorcrafts with four rotors. In this paper we present a method for generating optimal trajectories for heterogeneous quadrotor teams like those shown in Fig. 7.1 in environments with obstacles.
Trajectories that quadrotors can follow quickly and accurately should be continuous up to the third derivative of position (or C3). This is because, for quadrotors, disconti- nuities in lateral acceleration require instantaneous changes in roll and pitch angles and discontinuities in lateral jerk require instantaneous changes in angular velocity. Finding
C3 trajectories requires planning in a high-dimensional search space which is impracti- cal for methods using reachability algorithms [33], incremental search techniques [52] or LQR-tree-based searches [75]. The problem is exacerbated when planning for multiple vehicles as this further expands the dimension of the search space.
This paper builds on our own previous work [55] in which we showed that the dy- namic model for the quadrotor is differentially flat. We used this fact to derive a trajectory generation algorithm that allows us to naturally embed constraints on desired positions, velocities, accelerations, jerks and inputs while satisfying requirements on smoothness of the trajectory. We extend that method in this work to include multiple quadrotors and obstacles. The method allows for different sizes, capabilities, and varying dynamic ef- fects between different quadrotors. We enforce collision avoidance using using integer constraints which transforms our quadratic program (QP) from [55] into a mixed-integer quadratic program (MIQP).
Our work also draws from the extensive literature on mixed-integer linear programs (MILPs) and their application to trajectory planning from Schouwenaars et al. [68, 70–72]. This body of work demonstrates the power and flexibility of integer constraints in simi- lar trajectory planning problems for both fixed-wing aerial vehicles and rotorcraft. A key difference in our approach is that we use piece-wise smooth polynomial functions to syn- thesize trajectories in the flat output space. Using piece-wise smooth polynomial functions allows us to enforce continuity between waypoints up to any desired derivative of position. Another difference in our work from this previous work on trajectory generation is the use
Figure 7.1: The kQuad65 (top), the Asctec Hummingbird [1] (middle), and the kQuad1000 (bottom).
of quadratic cost function resulting in a MIQP as opposed to a MILP.
The organization of the chapter is as follows. First, we present our trajectory genera- tion method for a single quadrotor in Sec. 7.2 and its extension to heterogeneous quadrotor teams in Sec. 7.3. In Sec. 7.4, we present experimental results for teams of up to four quadrotors ranging from 65 to 962 grams and 21 to 67 cm in width shown in Fig. 7.1 fol- lowing trajectories in three-dimensional environments with obstacles with accelerations approaching1g. Finally, in Sec.7.5, we offer some concluding remarks on this approach.