RapidQuadcopterCollisionDetection
None
view repo
We present an algorithm for quickly detecting whether certain polynomial trajectories in time intersect with convex obstacles. The algorithm is used in conjunction with an existing multicopter trajectory generation method to achieve rapid, obstacle-aware motion planning in environments with both static convex obstacles and dynamic convex obstacles whose boundaries do not rotate. In general, this problem is difficult because the presence of convex obstacles makes the feasible space of trajectories nonconvex. The performance of the algorithm is benchmarked using Monte Carlo simulations, and experimental results are presented that demonstrate the use of the method to plan collision-free multicopter trajectories in milliseconds in environments with both static and dynamic obstacles.
READ FULL TEXT VIEW PDF
Real-world environments are inherently uncertain, and to operate safely ...
read it
We present a novel multicopter trajectory planning algorithm (RAPPIDS) t...
read it
In this paper, we propose a novel optimization-based trajectory planner ...
read it
This paper presents MADER, a 3D decentralized and asynchronous trajector...
read it
Navigating complex urban environments safely is a key to realize fully
a...
read it
Small Unmanned Aircraft Systems (sUAS) will be an important component of...
read it
In this paper, we address the trajectory planning problem in uncertain
n...
read it
None
A key enabler of the use of autonomous systems in real-world situations is a fast method for generating state and input feasible trajectories between desired states. This problem is know as the motion planning problem, and is a well researched area that includes numerous methods for the generation of such trajectories. In particular, sampling-based methods such as rapidly exploring random trees (RRT) [1], probabilistic roadmaps (PRM) [2], and fast marching trees (FMT) [3], have been used with great success to construct collision-free paths between desired states. Such methods are typically performed by sampling the state space of the system and attempting to connect feasible sampled nodes with simple trajectories that do not collide with obstacles. Performing collision detection on both the sampled nodes and the trajectories that connect them is often considered the most computationally expensive step in the motion planning process, and will be the focus of this paper.
Previous work focusing on the reduction of collision detection time includes [4], which presents an algorithm that involves using distance information from previously sampled nodes to avoid performing explicit node-obstacle collision detection when possible. In [5] this idea is adapted to reduce collision detection time for multicopter trajectories by computing overlapping collision-free spheres around the trajectory based on the maximum velocity of the vehicle and distance to the nearest obstacle at each sample point.
Rather than generating a number of multicopter trajectories and then checking each one for collisions, the authors of [6] first compute a series of overlapping, obstacle-free polyhedrons and then generate a series of trajectory segments that remain inside the polyhedrons. In [7], the authors take a similar approach by using an octree-based representation of the environment in order to enforce corridor constraints on each trajectory segment generated. A third approach to collision avoidance for aggressive flight is explored in [8], where a dense set of alternative trajectories to some desired trajectory are precomputed, allowing for one of the alternative trajectories to be chosen if a collision is predicted along the desired trajectory. In this case, a collision is determined by comparing the distance of each point along the discretized candidate trajectory to the points in a point cloud generated by a laser scanner.
In contrast to methods concerned only with planning in static environments, the authors of [9] leverage sequential convex programming to compute trajectories for multiple quadcopters that do not collide, allowing for dynamic formation changes. In [10] a method for dynamic obstacle collision avoidance is presented that models the obstacles as ellipsoids and incorporates them as nonconvex constraints in a model predictive controller.
In this paper we are interested in reducing the computational time required to find feasible trajectories for multicopters in order to enable high-speed flight in cluttered, unknown environments (e.g. when navigating a forest). Fast trajectory generation is a requirement in these scenarios due to the limited range of onboard sensors and limited onboard computational power. For example, obstacles can often be occluded or unexpectedly change position, requiring an immediate, agile response to avoid a collision if flying at high speeds. Furthermore, due to the constrained onboard computational power of aerial vehicles, efficient algorithms are often required in order to achieve acceptable performance.
To this end, we propose a computationally efficient method for quickly evaluating whether a candidate trajectory collides with obstacles in the environment. We limit ourselves to evaluating multicopter trajectories similar to those described in [11], which describe the vehicle’s position as a fifth order polynomial in time. These trajectories result in the minimum average jerk over the duration of the trajectory, and are particularly useful because they can be generated and checked for input feasibility with little computation. Unlike other methods that require a discrete time representation of the vehicle trajectory (and thus a number of collision checks that depend on the time step used for discretization), our method leverages a continuous time representation of the trajectory to rapidly perform collision detection.
We follow [11]
in modeling the multicopter as a six degree of freedom rigid body with acceleration
(written in the inertial coordinate frame) and orientation , whererepresents the rotation matrix that rotates vectors in the body-fixed frame to the inertial frame. Gravity is denoted
(written in the inertial frame), and the mass-normalized total thrust force acts in the direction, where is the body-fixed thrust direction. We assume that the angular velocity of the vehicle is controlled by a high-bandwidth low-level controller such that the angular velocity converges very rapidly to a desired value, and may thus be treated as an input to the system. The translational dynamics and attitude dynamics of the multicopter are then(1) |
where
is the skew-symmetric form of the angular velocity vector
so that for any vector .Using this model, it can be shown that kinematically feasible polynomial trajectories in time can be generated using the differential flatness property of multicopter dynamics [12]. Specifically, we plan trajectories by defining the components of the jerk as second order polynomials in time between time and . As described in [11], this results in trajectories that minimize the average jerk over the trajectory. The thrust and angular velocity are then written as a function of and as follows.
(2) |
where and are the components of angular velocity perpendicular to the thrust direction (i.e. the roll and pitch rates). Note that the angular velocity in the direction does not affect the translational motion of the vehicle, and is taken to be for the rest of the paper.
The position and velocity of the multicopter are defined as and respectively, and are both in and written in the inertial frame. Let , , and be the position, velocity, and acceleration of the vehicle at the start of the trajectory. Because the minimum average jerk trajectory is achieved when each component of the jerk is a second order polynomial in time, the trajectories of the states of the system follow as
(3) |
where , , are linear functions of , , and .
A method for quickly checking whether a given trajectory satisfies bounds on the minimum and maximum thrust and the magnitude of the angular velocity is given in [11], to which we refer the reader for further discussion.
In this section we describe the collision detection algorithm. All obstacles are assumed to be convex; nonconvex obstacles may be approximated by defining them as a union of convex obstacles. In general, the presence of convex obstacles results in the feasible space being nonconvex, making the trajectory generation and collision detection problem difficult to perform using traditional optimization methods.
We first review a method used to check whether a polynomial trajectory lies on one side of a plane, which is defined by a point and unit normal (both written in the inertial frame). The distance of the trajectory from the plane can be computed as
(4) |
Furthermore, the critical points of can be computed by differentiating with respect to and finding the roots of the resulting equation:
(5) |
where
(6) |
The trajectory is defined only between and , so the critical points of occur between and include the start and end of the candidate trajectory. The set of critical points is then defined as
(7) |
If the distance at each critical point is positive, this indicates that does not cross the plane. Because is a fourth order polynomial in time, its roots can be found in closed form, meaning that can be found with very little computation.
We now extend this method to detect collisions with convex obstacles. The given convex obstacle and polynomial trajectory are required to have the following two properties. First, it must be possible to check whether a specific point is inside . Second, assuming , it must be possible to define a separating plane between and (defined with point unit normal ). Thus, if is found to not cross the separating plane, it is guaranteed to not collide with .
Algorithm 1 leverages this property to verify whether an individual segment of a given trajectory is in collision with a given obstacle. The algorithm begins by checking whether the end points of the trajectory and are inside the obstacle, followed by a call to CheckSection, which returns whether the given section is feasible, infeasible, or whether the feasibility of the section cannot be determined. For each call to CheckSection, a time between and is chosen which divides the trajectory in two. We choose to be the average of and , as it will evenly divide the trajectory into two sub-trajectories in time.
For each section checked recursively by CheckSection, is first checked for feasibility, and then the minimum resolution of the section is checked. The parameter serves to terminate the algorithm early in order to prevent excessive time being spent checking any particular candidate trajectory, and limits the recursive depth of the algorithm. This end condition can be reached in the case where the candidate trajectory passes sufficiently close to the obstacle, requiring the trajectory to be split into a large number of sub-trajectories to be checked.
Next, the unit normal and location of a separating plane are found. Although there are many possible ways to find a separating plane, in our implementation we choose such that it is the minimum distance point to located in . The unit normal of the plane is then chosen such that the resulting plane lies on the obstacle boundary at and points from to . The times at which the critical points of the distance of the trajectory from the resulting separating plane occur are then computed by solving the corresponding fourth order polynomial given by (5). Once is computed, the two sections of the trajectory occurring before and after are each checked for feasibility. Let be the elements of in and be the elements of in .
The section of the trajectory between and is first checked for feasibility by iterating forward in time over and checking whether each critical point of lies on the feasible side of the separating plane. If a critical point is found to lie on the obstacle side of the plane, the section between the previous critical point (already determined to be on the feasible side of the plane) and cannot be guaranteed to be feasible and is recursively checked with CheckSection. Finally, the section of the trajectory between and is checked for feasibility in a similar manner by iterating backwards in time over . A graphical representation of Algorithm 1 is shown in Figure 1.
Note that Algorithm 1 treats as the trajectory of a point. In order to detect collisions between and a real multicopter, we define a sphere of radius that contains the vehicle, and enlarge by in each direction. Additionally, because polynomials of order greater than four do not have closed form solutions except in special cases, greater computation time would be required to find the critical points of any higher order position trajectories (e.g. as used in [13]).
In this section we provide two simulations used to benchmark the performance of the proposed algorithm.^{1}^{1}1An implementation can be found at https://github.com/nlbucki/RapidQuadcopterCollisionDetection The algorithm was implemented in C++ and compiled with GCC version 5.4.0 with the highest speed optimization settings. All simulations were ran as a single thread on a laptop with a 1.80GHz Intel i7-8550U processor.
First, a Monte Carlo simulation was conducted in order to characterize the computational time required to perform collision detection on a single candidate trajectory. The methods of [11] are used to generate the candidate trajectories and check whether the generated trajectory satisfies some given input bounds. Any trajectory requiring a mass-normalized thrust that is not between and or that requires an angular velocity of greater than is discarded.
Candidate trajectories are generated with a fixed initial position of
. The final position, initial and final velocity, and initial and final acceleration along each axis are generated from uniform distributions over the intervals (
, ), (, ), and (, ) respectively. The length of time of the trajectory is sampled uniformly at random between and . A sphere with radius sampled uniformly at random on (, ) and positions sampled uniformly at random on (, ) along each axis is used as an obstacle. The minimum collision detection time per section is chosen to be .For such trials, the average time required to detect collisions was , and of the candidate trajectories, 95.99% did not collide with the obstacle. Table I shows the computation time required depending on whether the trajectory was found to be feasible, infeasible, or of indeterminable feasibility.
Feasible | Infeasible | Indeterminable | |
Fraction of trajectories | 95.99% | 4.01% | % |
Collision detection time |
A second Monte Carlo simulation involving generating collision free trajectories that bring the vehicle to rest was also conducted. This scenario is of interest in the case where, for example, the vehicle must perform an emergency stopping maneuver (e.g. when an unexpected obstacle appears in the path of the vehicle while flying at high speed). The simulation is run in batches of 100 candidate trajectories, where each candidate trajectory starts from the same initial state and ends at rest at a position sampled uniformly at random along each axis on (, ). For each batch, the initial position of the vehicle is constrained to be (, , ), the initial velocity and acceleration in the x-direction are sampled uniformly at random on (, ) and , ) respectively, and the initial velocity and acceleration in the y- and z-directions are sampled uniformly at random on (, ) and (, ) respectively. The length of time of the candidate trajectories is sampled uniformly at random between and . The positions and orientations of the obstacles, represented as five long rectangular prisms, are fixed as shown in Figure 2, which additionally shows the candidate trajectories of a single batch.
One million batches were simulated. The average time required to find the first collision free trajectory was . On average, each trajectory required to generate, to check for satisfaction of constraints on the total thrust and angular velocity, and to detect any collisions with the five obstacles. For each batch, an average of 60.2% of generated trajectories were collision free.
In the previous section we showed that our algorithm is capable of detecting collisions between given quadcopter trajectories and static convex obstacles in an environment. This method is easily extended to detect collisions with dynamic obstacles whose boundaries do not rotate, and whose position trajectories are described by fifth order or below polynomial in time. Example applications of this method include detecting collisions between two quadcopters with different trajectories or between a projectile and a moving quadcopter.
Let be the predicted trajectory of the center of a given obstacle. The relative position of the obstacle and the quadcopter at any time is then
(8) |
where each component of will a polynomial in time if each component of both and are also polynomials in time.
Recall that we model the quadcopter as a sphere with radius . A collision between the quadcopter and the dynamic obstacle occurs if intersects with an obstacle centered at the origin of the same size as the dynamic obstacle but enlarged by in each direction. Because the boundary of the obstacle is required to not rotate, the same methods described in Section III may be used to detect collisions between and the enlarged obstacle centered at the origin. Obstacles with boundaries that do rotate may be straight-forwardly encoded by enclosing them convex shapes with boundaries that do not rotate (e.g. spheres) at the penalty of introducing conservatism to the collision detection.
This section presents experimental results where the proposed algorithm is used to enable a quadcopter to avoid unexpected static and dynamic obstacles. For the static obstacle case, we interrupt the motion of the quadcopter while following a trajectory by placing a surface in the path of the vehicle, forcing it to rapidly plan a new trajectory to avoid the collision and bring the vehicle to rest. For the dynamic obstacle case, we throw a projectile at the vehicle while it is following a trajectory to a goal position, again forcing it to rapidly plan a new trajectory to avoid the projectile and then continue to the original goal position. All experiments can be viewed in the attached video.
The quadcopter has a mass of , and receives thrust and angular velocity commands at via radio from an offboard laptop. Collision detection and trajectory generation is performed using the same laptop as used for benchmarking in Section IV. The position and attitude of the quadcopter and obstacles are measured using a motion capture system at .
During each controller time step, we check whether any collisions are predicted to occur between the quadcopter and the obstacle used for each experiment. If a collision is detected, a new trajectory is generated that is not predicted to collide with the obstacle and ends at rest with zero velocity and zero acceleration. We sample candidate end positions for the avoidance trajectory uniformly at random in a rectangular space and sample durations of the avoidance trajectory uniformly at random from to . While tracking the avoidance trajectory, we continue to check for predicted collisions and generate a new avoidance trajectory if necessary.
When searching for feasible trajectories during both experiments, we generate and evaluate candidate trajectories for , and at the end of the allocated time choose the trajectory with the minimum average jerk that satisfies all state and input constraints. We choose the trajectory with the minimum average jerk in order to favor less aggressive trajectories. When evaluating each candidate trajectory, we first compute the average jerk of the trajectory and reject it if it has a higher average jerk than any previously found state and input feasible trajectory. Next, we use the methods of [11] to check that the total mass-normalized thrust remains between and and that the maximum angular velocity remains bellow , as these are the physical limits of the experimental vehicle. We then check that the candidate trajectory stays within a box of to prevent the vehicle from flying into the ceiling, floor, or walls. Finally, we check that the candidate trajectory does not collide with any obstacles using Algorithm 1 with .
For the static obstacle avoidance experiment, we define the static obstacle as a rectangular prism measuring
, which includes both the increase in size in each direction necessary to account for the true size of the quadcopter and a small buffer to account for trajectory tracking and estimation errors. The quadcopter tracks two predefined trajectories that result in a roughly circular motion with an average speed of
, and checks these trajectories for collisions with the rectangular prism at each controller time step. The obstacle is then moved by hand in front of the vehicle about before the vehicle would pass, causing a collision to be predicted and an avoidance trajectory to be generated that brings the vehicle to rest without colliding with the obstacle. Figure 3 shows a sequence of images from the experiment. For the experiment shown in Figure 3, 14,610 candidate avoidance trajectories were evaluated in the allocated after first predicting a collision with the obstacle (recall that the controller runs with a period).For the dynamic obstacle avoidance experiment, we throw a projectile at the vehicle while it is performing a rest to rest maneuver from some initial position to some final position . If a collision between the quadcopter and the projectile is predicted, the quadcopter rapidly plans a trajectory to avoid the projectile. The projectile is thrown by hand, meaning that its trajectory can only be predicted by the system after it has been thrown. The position and velocity
of the projectile at the current controller time step are estimated using position measurements received from the motion capture system and a Kalman filter. The position of the projectile
is then predicted over a five second time horizon as a quadratic function of time that depends on and :(9) |
The minimum allowable distance between the center of mass of the projectile and the center of the quadcopter is chosen to be , which is chosen such that there is at least separation between the quadcopter and projectile to account for any trajectory tracking and estimation errors. During each controller time step, we check whether the projectile is predicted to collide with the quadcopter by checking whether their relative position ever enters a sphere of radius centered at the origin, and begin generating an avoidance trajectories if a collision is detected. While evaluating candidate avoidance trajectories, we not only check that the avoidance trajectory will not collide with the projectile during the maneuver, but also check that the projectile will not collide with the quadcopter after the quadcopter has reached the end position of the avoidance trajectory.
While tracking the avoidance trajectory, we try to generate sample return trajectories at each controller time step that bring the quadcopter from its current state to rest at the originally desired end position . Durations of the candidate return trajectories are sampled between and . Once a feasible trajectory is found that does not collide with the projectile and ends at , the avoidance trajectory is interrupted and the vehicle begins tracking the return trajectory. Figure 3 shows a sequence of images from the experiment. For the experiment shown in Figure 3, 2,371 candidate avoidance trajectories were evaluated in the allocated after first predicting a collision with the projectile.
In this paper we presented a method for quickly detecting whether a polynomial trajectory collides with a convex obstacle. This method can be applied to both static convex obstacles and dynamic obstacles whose boundaries do not rotate. We used the proposed algorithm to perform rapid collision detection of multicopter trajectories, which can be modeled by fifth order polynomials in time. The ability to rapidly assess whether a given trajectory will collide with obstacles allows for a collision-free trajectory to be found in a short period of time by generating and checking many candidate trajectories for collisions. Because such a large number of the candidate trajectories can be generated and evaluated in such a short period of time, the vehicle is able to plan collision-free trajectories within milliseconds. This enables the vehicle to avoid obstacles that suddenly appear while the vehicle is flying at high speeds and to avoid projectiles thrown at high speeds.
This material is based upon work supported by the Berkeley Fellowship for Graduate Study. The experimental testbed at the HiPeRLab is the result of contributions of many people, a full list of which can be found at hiperlab.berkeley.edu/members/.
Comments
There are no comments yet.