{'fraction_non_alphanumeric': 0.030061928392732643, 'fraction_numerical': 0.004921461674117213, 'mean_word_length': 4.66016713091922, 'pattern_counts': {'":': 0, '<': 1, '<?xml version=': 0, '>': 0, 'https://': 1, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 1, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
Exploiting collisions for sampling-based multicopter motion planning

Multicopters with collision-resilient designs can operate with trajectories involving collisions. This paper presents a sampling-based method that can exploit collisions for better motion planning. The method is built upon the basis of the RRT* algorithm and takes advantages of fast motion primitive generation and collision checking for multicopters. It generates collision states by detecting potential intersections between motion primitives and obstacles, and connects these states with other sampled states to form collision-inclusive trajectories. We show that allowing collision helps improve the performance of the sampling-based planner in narrow spaces like tunnels. Finally, an experiment of tracking the trajectory generated by the collision-inclusive planner is presented.


I. INTRODUCTION
Autonomous systems often need to find feasible trajectories between desired states. One effective approach to the problem is to sample states in the state space and connect them with feasible (in terms of both input-feasible and collision-free) trajectory pieces. Methods using this approach, such as rapidly exploring random trees (RRT) [1] and probabilistic road maps (PRM) [2], are referred to as sampling-based methods. In particular, RRT*, a variant of RRT, has gained great popularity due to its unique asymptotic optimality characteristic [3]. Researchers have extended the RRT* algorithm to work with dynamic systems with differential constraints [4] and have developed different sampling methods [5] and heuristics guiding the sampling process [6], [7] to increase RRT*'s rate of convergence.
One key step of sampling-based motion planning is checking collisions. Trajectories colliding with obstacles are usually discarded by planners. However, in recent years, many autonomous systems that can survive collisions have been developed, such as the collision-resilient aerial vehicles in [8] and [9]. As these vehicles can fly trajectories that are not collision-free, they can plan their motion in an extended feasible state space, which usually leads to better trajectories with less cost or duration time. This idea of exploiting collisions for better trajectories is discussed in [10], which also proposes a method to find collision-inclusive optimal trajectories with mixed integer programming. The method is later experimentally verified in [11]. Furthermore, collision is shown to be beneficial to stochastic optimal steering problems, as contact with environment helps decrease the uncertainty of the state estimation [12].
We are interested in generating trajectories in the extended collision-inclusive state space with a sampling-based method for multicopters. Our method is based on RRT* algorithm and takes advantage of the differential flatness of multicopter  dynamics, which enables rapid generation of minimum jerk motion primitives [13] and efficient collision detection [14]. Via detecting collisions and predicting the states of the vehicle after the collisions, the method can generate collision states and potentially connect them with other sampled states to form collision-inclusive trajectories.
Planning with collisions introduces an interesting tradeoff. On one hand, more computation time is required for detecting and generating states with collisions. On the other hand, the planner will no longer discard samples deemed infeasible due to collisions. Hence, the planner can add samples to its exploring tree more efficiently. We find that the benefit of planning with collision is likely to outweigh its computation cost in narrow environments such as tunnels, where collision is likely to take place and collision-exclusive planners are forced to discard most of its samples.
The contribution of this paper is as follows. We present a sampling-based motion planner that can find collisioninclusive trajectories for multicopters. We demonstrate that the planner is likely to generate better result than collisionexclusive planners in narrow spaces with a Monte Carlo test on a tunnel example. We also experimentally validated the planned collision-inclusive trajectory with our tensegrity multicopter [15], shown in Fig. 1. Our method can also be adapted to work with other collision-resilient multicopters by replacing the collision model in this paper with other vehicle-specific models.

II. MOTION PRIMITIVE AND COLLISION DETECTION
This section introduces two important building blocks of the collision-inclusive motion planner, the motion primitive generator for multicopters and the rapid collision detector for such primitives. These tools are originally presented in [13] and [14]. Here we only introduce the key results and explain how they are adapted to work with our motion planner.

A. Multicopter motion primitive generator
The multicopter motion primitive generator is a computationally efficient tool that can generate and check the inputfeasibility of about one million motion primitives in a second on a modern computer. This enables us to rapidly connect sampled states to form feasible trajectories.
The generator computes a thrice differentiable trajectory which guides the multicopter from an initial state at time t 0 to a final state at time t f , while minimizing the cost function where j is the jerk of the quadcopter at time t and is defined as the third order derivative of the position. The result trajectory is a fifth order polynomial, where x(t) is the position at time t and x 0 ,ẋ 0 ,ẍ 0 are position, velocity and acceleration at the start of motion primitive. a 0 , a 1 a 2 ∈ R 3 and they are vector parameters describing the trajectory computed as a linear function of the initial state and the final state. Let P = MOTIONPRIMITIVE((s 0 , t 0 ), (s f , t f )) be the function generating the motion primitive connecting state s 0 and s f between time t 0 and t f . A state is defined as the combination of position, velocity and acceleration of the vehicle. We define C(P) as the function evaluating the cost of the motion primitive. For each generated motion primitive, we can check if the inputs for the multicopter to implement the primitive satisfy bounds on the minimum and maximum total thrust and the magnitude of the angular velocity. We define INPUTFEASIBLE(P) as the function checking the input feasibility of the motion primitive. A method for quickly implementing the check is also given in [13], where we refer the reader for further reading.

B. Rapid Collision Detection
The rapid collision detection algorithm [14] checks if a generated motion primitive will collide with convex obstacles in the environment. Non-convex obstacles may be approximated by defining them as a union of convex obstacles.
For a given motion primitive starting at time t 0 and ends at time t f , the algorithm first checks if its begin position x(t 0 ), end position x(t f ) and the mid-time position x(t split ) is inside an obstacle. Here t split = (t 0 + t f )/2. If not, it then checks if the primitive crosses a separation plane between the obstacle and x(t split ). This separation plane is defined as the tangential plane of obstacle surface that includes a point p, which is located in the obstacle and has minimum distance to x(t split ). If the whole primitive does not cross the separation plane, it is guaranteed to be free of collision. If the primitive crosses the separation plane, we can bisect it into two sections, x(t 0 ) → x(t split ) and x(t split ) → x(t f ) and repeat the above checking process on the section(s) crossing the separation plane. An illustration of this idea is shown in Fig 2.(a) and Fig 2.  (a). The detector checks if the primitive stays on the same side of the separation plane. If not, it bisects the primitive and checks either half that crosses the plane. (b). If the primitive piece stays on the same side of the separation plane, we know the primitive is free of collision. (c). We can estimate the collision time by keep bisecting the primitive until the section crossing the separation plane is shorter than a certain duration threshold.
In [14], the recursion terminates once any part of the primitive is found lying inside the obstacle. We modify the recursion termination rule to compute the time of the collision. When checking for collision, we can keep bisecting the primitive until the time of the checked primitive section crossing the separation plane is smaller than a certain threshold. We can thus estimate the time of collision as the beginning time of this primitive section, as shown in Fig. 2.(c). We define COLLISIONFREE(P) as the function checking if the primitive collides with any obstacle in the space. We also denote COLLISIONTIME(P) as the function finding the time of the first collision between a given primitive and the obstacles in the environment.

III. ALGORITHM DESCRIPTION
This section introduces the algorithm of the samplingbased method for collision-inclusive motion planning. The algorithm searches for a potentially collision-inclusive trajectory connecting the start state s 0 and the goal state s f . The algorithm is based on the RRT* [3] and it repeatedly samples states from the state space and attempts to connect them to T , the exploring tree keeping track of previously sampled feasible states and optimal paths leading to them. Our collision-inclusive planner makes two key changes to the original RRT* algorithm. First, a collision state generation step is introduced to allow states involving collisions to be considered. Second, instead of connecting sampled states with lines, we connect them with input-feasible primitives described in Section II. For the rest of this section, we will first introduce how key steps of the original RRT* algorithm are modified to find collision-inclusive trajectories for multicopters. Then, we will present the whole planner and discuss the cost and benefits of planning with collisions.

A. Collision model
We define the state of the vehicle by its position, velocity and acceleration, i.e. s = [x,ẋ,ẍ]. Given a pre-collision state s, the function COLLISIONMODEL(s) predicts the post collision state s + = [x + ,ẋ + ,ẍ + ]. This model depends on the vehicle design and the material of contact surface and may vary among different multicopters. A model for the vehicle in Fig. 1 is given in Section V.

B. Connecting nodes as state-time pairs
We define a node n as a pair of vehicle state and end time, n = (s, t). This indicates that it takes the vehicle a time t to reach s from the start. When a node represents a state at collision, it will be coupled with a post-collision node containing the same time and a post-collision state predicted by the collision model. Denote P C(n) as the function accessing the post-collision node of n. Define CONNECT(n 1 , n 2 ) as the process of generating the primitive connecting two nodes, as shown in Algorithm 1. Moreover, we define the cost of node, COST(n) as the lowest cost of the feasible path found from start to n. return MOTIONPRIMITIVE(n 1 , n 2 )

C. Generating sample states
For each step, the planner generates a node sample n s via a random process. With possibility η f , the goal sampling rate, the state of n s is set as the goal state and with possibility (1-η f ), the state of n s is uniformly sampled from the state space. The time t of n s is sampled uniformly from [0, t * end ] where t * end is the time of the shortest feasible trajectory from the start state to the goal state the algorithm has found. This value is initiated with an overestimate of the shortest feasible trajectory time and then updated throughout the planning. We use function SAMPLE() to denote the above process.

D. Collision node generation
After the sampled node is generated, we find the node in T whose time is before the sampled node and can be connected to it with a primitive of the lowest cost. Denote this node as the closest node. Then, we check if the primitive connecting the closest node to the sampled node collides with any obstacle in the environment. If the primitive is free of collision, we use the sampled node directly for future optimal connection attempts. However, if the primitive collides with any obstacle in the environment. We generate a collision node which has the collision time and the state of the vehicle right before collision. Meanwhile, we use the collision model to predict the post-collision state. This process is illustrated in Fig.3 and we define the process as the GETCOLLISIONNODE(n s ) function, with its implementation detailed in Algorithm 2.

E. Connect along minimum cost path and rewire
After sampling and collision node generation, we want to connect the generated node to a best feasible parent node in T so that its cost is minimized. If such a feasible parent can be found, we will add the generated node to T . Afterwards, we rewire the tree to ensure the optimality of all connections. The process is shown in Algorithm 3.

Connect to nodes prior in time
Connect to nodes later in time (a) (b) Fig. 3: A graphic illustration of the collision node generation process. After node ns is sampled, we connect it with its closest node nc. If a collision takes place, we generate a pre-collision node with the time and the state right before the collision. We also predict its state after the collision and set it as the post-collision node for possible future connections. t n ← COLLISIONTIME(P) 7: s n ← P(t n ) 8: n n ← (s n , t n ) 9: P C(n n ) ← (COLLISIONMODEL(s n ), t n ) 10: return n n Notice that comparing to the original RRT*, we are not generating a "near neighbor" set to decrease the number of connections. The reason behind this decision is that the metric of "distance" between two nodes is the cost of the primitive connecting them and such cost cannot be calculated before the connection attempt. However, for planning tasks that involve a large number of nodes, we can keep track of the k th smallest feasible primitive cost. Following the suggestion of [3], we set k = 2e · log(|T |), where |T | refers to the number of nodes in T . We discard primitive candidates whose cost are larger than the k th smallest cost without checking input-feasibility and collision. This pre-screening helps decrease computation time and has an effect similar to the "near neighbor" screening in the original RRT*.

F. Full collision-inclusive sampling-based planner
Now, we combine the previous parts and present the full planner as Algorithm 4. After running the planner, we can find the best end node as the node in T with an end state that has a smallest time. Then, we can recover the best trajectory via backtracking from the best end node to the start.
Planning with collision brings two benefits. First, allowing collisions extends the feasible state space that the samplingbased planner can search in. As a result, given enough time, the collision-inclusive planner will converge to a better trajectory than the one generated by the collsion-exclusive planner (i.e. the planner without the collision node generation process). Second, sampled nodes will not be discarded due to infeasibility caused by collisions. Hence, the collisioninclusive planner may add nodes to T more efficiently. for n in T with time prior to n n do 5: P ← CONNECT(n, n n ) 6: if not INPUTFEASIBLE(P) then continue 7: if not COLLISIONFREE(P) then continue 8: if COST(n) + C(P) < COST(n n ) then 9: COST(n n ) ← COST(n)+ C(P) 10: PARENT(n n ) ← n 11: if PARENT(n n ) is not null then if n n is added to T then REWIRE(n n ) However, planning with collision also comes with its cost. First, the process of generating collision nodes takes additional computation time. Second, the collision node generation process may slow the expansion of the exploring random tree under certain circumstances. An example is illustrated in Fig. 4. Nodes n a and n b are in T and a node n s is sampled on the right side of the obstacle. In Fig. 4.(a), with the collision node generation process, n s will be connected with its closest node, n a and a collision node is generated on the left side of the obstacle. If the process is disabled, as in Fig. 4.(b), the sampled node will be connected with n b and added directly to T . We see that with the collision node (a). During the collision node generation process, a collision node nn will be generated on the left of the obstacle and added to T . (b). If the collision node generation is disabled, ns will be added to T directly. As a result, T can expand to the right of the obstacle in this step. generation, T can no longer reach the right of obstacle at this step and its expansion is slowed down in this example.

IV. ILLUSTRATIVE EXAMPLE: PLAN IN A TUNNEL
In this section, we present an illustrative example to showcase the trade-off between the cost and the benefits of planning with collisions and illustrate why the collisioninclusive planner may perform better than collision-exclusive planner in narrow spaces.
In this 2D example, the vehicle starts at position (1,2)[m] and is in a 1m wide tunnel. It needs to move in the positive x-direction for 3.5 meters to leave the tunnel and then move in an open space to get to a goal position at (4,5)[m]. We run both the collision-inclusive planner and the collisionexclusive planner on this problem for 10 5 times and compare the results generated. Fig. 5 snapshots the planners after computing for 0.1s. We observe that the collision-inclusive planner generates a trajectory colliding with the tunnel wall. Moreover, its nodes in T are mainly collision nodes. This phenomenon is expected. The tunnel is very narrow, so most of the primitives generated involve collisions. As a result, collision nodes are likely to be generated during the planning process. Moreover, if we compare the number of collision-free nodes in the open space right of the tunnel, we notice that the collision-exclusive planner generates more nodes in the open space than the collision-inclusive planner, indicating a faster expansion of the exploring tree in that region. This echoes the discussion in Section III that the collision node generation process can slow down the expansion of the exploring tree. Now, we compare the performance of the planners with the median of the minimum feasible trajectory time the planners have found among all trials. Fig. 6.(a) plots the median of the best trajectory time found versus computation time. It shows that the collision-inclusive planner finds a better trajectory than the collision-exclusive planner under the same time limit. The relatively better performance can be credited to the two benefits of collision-inclusive planning. First, allowing collisions extends the feasible state space. Second, collisioninclusive planner can add nodes to T more efficiently, because no nodes are discarded due to infeasibility caused by collision. Fig. 6.(b) shows the median of the numbers of nodes in T versus computation time. The figure shows that collision-inclusive planner can add nodes to T with a higher speed. The relatively low efficiency of the collision-exclusive planner suggests that most of its sampled nodes are deemed as infeasible due to collisions and cannot be added to T . This example shows that the collision-inclusive planner is likely to outperform the collision-exclusive planner when the vehicle is in narrow spaces or spaces crowded with obstacles, which are usually the scenarios that collision-resilient vehicles are designed for.

TRAJECTORIES
This section demonstrates the experiment of tracking planned collision-inclusive and collision-exclusive trajectories generated by our planner with a collision-resilient tensegrity multicopter [15]. Experiment video: https:// youtu.be/pKcqjTDxi90.

A. Collision model for the test platform
We predict that the post-collision position stays the same as the pre-collision position, x + = x. For post collision velocity, we use an empirical model similar to the one in [16] for the non-sliding case. The model predicts the velocity component normal to the obstacle with a linear function: Where e is the coefficient of restitution andẋ n andẋ + n are velocity component normal to the obstacle before and after the collision. Moreover, the model predicts that the ratio between tangential impulse and normal impulse is proportional to the incidence angle, which is the angle between the precollision velocity vector and the normal vector of the obstacle surface. As a result, we havė where κ is a constant.ẋ t andẋ + t are velocity component tangential to the obstacle before and after the collision. With experiments, we identify e = 0.43 and κ = 0.20 for our model.
The post-collision acceleration is dependent on the attitude of the vehicle after collision, which can be hard to predict due to the large torque disturbance the vehicle may experience during the collision process. As a result, we assumeẍ = 0, which corresponds to a hovering status, and treat the difference between the true attitude after collision and the hovering attitude as an initial attitude error for the trajectory piece after the collision. As multicopters have responsive attitude controllers, this error is expected to be corrected in a negligible time.

B. Improving the tracking of collision trajectories
Due to the torque disturbance during the collision, the vehicle can rotate with a large angular velocity after the collision. To mitigate the tracking error caused by this, we temporarily (for 0.3s) increase the gains of the attitude and angular rates controller of the vehicle after the collision.

C. Tracking experiment
An image sequence of the tracking experiment is shown in Fig. 7 and Fig. 8 shows the planned trajectories and tracking results for both collision-inclusive and collision-exclusive cases. The trajectory makes a U-turn to avoid an obstacle in the center of the space. The obstacle separates the space into two parts, connected by a 1m gap.
For both scenarios, the multicopter can successfully follow the reference to reach the end goal. However, we observe tracking error starting at the tip of U-turn for both tracking attempts. For the collision-exclusive case, the error is caused by aggressive maneuver. For the collision-inclusive case, the collision introduces a torque disturbance that is not fully captured by the collision model. This makes the state of the vehicle deviates from the reference state and causes the  tracking error after the collision. This experiment verifies that trajectories generated by the collision-inclusive planner can be tracked successfully, and also suggests that better tracking of the collision trajectory can be achieved through decreasing the impact of torque disturbance on the system during collisions, either via physical designs or control strategies.

VI. CONCLUSIONS
In this paper we present a sampling-based motion planner that can exploit collisions to generate better trajectories for multicopters. The planner samples collisions as impacts between generated motion primitives and obstacles, and connect collision states with other sampled states to form collision-inclusive trajectories.
Planning with collisions has two benefits. First, allowing collisions extends the feasible state space. Second, sampled states are no longer discarded due to infeasibility caused by collisions. Thus, the rate of adding samples to the exploring tree may be increased. Collision-inclusive planning also comes with its cost. The process of generating collision node requires additional computation time and it may also slow down the expansion of the exploring tree. We illustrate with an example that the benefits of planning with collision are likely to outweigh the cost when searching for trajectories in narrow environments, where most generated trajectory pieces involve collisions.
We have also experimentally tracked trajectories generated by our planner. Experiment result indicates that a major source of tracking error comes from the disturbance that is not captured by the collision model. This could be mitigated by designing a short and aggressive recovery trajectory piece to decrease the variance of the state after collision. Such trajectory piece can also make the planner less dependent on the accuracy of collision models and hence makes it easier to apply the planner on different platforms.