Multi-dimensional Planning for Autonomous UAVs
Introduction
Planning in general becomes increasingly expensive as the number of dimensions in the planner's state space is increased. A question that comes to mind when approaching this problem is whether the state space must maintain the same number of dimensions throughout the plan. In applications where this condition is not mandatory I believe that reducing the dimensionality, or number of dimensions, of the state space in some areas while keeping the desired dimensionality in others would help reduce the computational cost for any given plan. As such, plans generated by this method should be computed faster than plans generated using a state space with uniform dimensionality. This is desirable in situations where the time to compute plans is an important factor, such as planning on the fly during exploration missions, where the terrain explored may not be known in advance. The focus of this project was to develop a planner that computed trajectories for rotor craft UAVs that uses a state space with non uniform dimensionality.
Multi-dimensional State-space
There are two types of states that the Multi-dimensional State-space is composed of. These are states consisting of four and three dimensions, which will be referred to as high and low dimensional states respectively. Both states contain x,y,z coordinates, but high dimensional states also contain a yaw component, which is discretized into 16 evenly spaced angles. It should also be noted that the x,y,z coordinates are uniformly discretized in a three-dimensional grid. The planner determines if a state should be high or low by comparing the distance between that state and the UAV from a given distance threshold. If the distance is below the threshold then the state is assumed to be high dimensional. Otherwise, it is assumed to be a low dimensional state.
Diagram showing how state-space is divided.
Connections between high dimensional states are formed through the use of motion primitives containing the same dimensions in x,y,z and yaw. A motion primitive can be thought of as a dense set of way-points between two states meant to imitate a feasible action for a given platform, such as a quadrotor. The costs assigned to varying motion primitives are scaled in terms of time required to achieve them, not relative distance. Connections between low dimensional states are formed if they border each other in a three-dimensional grid, which shall be referred to as a 26-connected grid.
Diagram showing visual representation of motion primitives and connected grid.
Connections between high and low dimensional cells becomes a little more complicated and all depends on which state is looking for successors. If a given state is high-dimensional, then it looks for its successors using motion primitives. Otherwise, it will look for successors using a 26-connected grid. Below is some simple pseudo code showing how successors are formed for a given stating.
The following shows a visual representation of the above pseudo code.
Diagram displaying connection between cells.
ARA*
Anytime Repairing A*, or ARA* [1], is a variation of A* with some notable differences. It initially attempts to return a suboptimal solution within a specified amount of time, given an initial epsilon value. Afterwards, with remaining time available, it attempts to improve the cost of the original solution while gradually reducing the initial epsilon value received. The cost of the solution generated is sub-optimally bounded by epsilon, meaning that the cost is guaranteed to be no more that the optimal cost solution times the epsilon value.
Experimental Setup
The planner was tested using a previously setup simulation environment with a quadrotor which already had previously developed trajectory following and perception code. The planner starts with an empty map and updates this through use of vertical and horizontal lidars in simulation, which are passed to perception and fed into the planner. Although the environment itself is static, there is some disparity present between runs with the same start and goal locations. This makes it necessary to perform multiple runs with the same start and goal locations in order to show consistent results. The size of the environment from the planner's perspective was approximately 320x250x30 cells. An overhead view can be seen below.
Picture showing overhead view of simulation environment.
Trials were run using both the multi-dimensional planner and the uniform-dimensional planner. The uniform-dimensional planner is like the multi-dimensional planner in every aspect, except that its state-space is only composed of high-dimensional states. The purpose of running trials with both planners was to gauge the performance of using a multi-dimensional state-space over uniformly-dimensional state-space. The planners were both given the same start and goal locations as shown by the above diagram and were expected to continue re-planning until the quadrotor reached the goal position. Both planners were run 20 times from the start position to each of the goal positions. The amount of time allocated for each plan was one second, and the initial epsilon value passed to both planners was 5. The high dimensional radius set for the multi-dimensional planner was 40 cells.
Results
The statistics for both planners are shown in the tables below. Each row represents the statistics recorded for a specific planner moving to a specific goal from 20 trial runs. The settled epsilon value refers to the epsilon that was reached during a one second planner interval and it can be assumed that a lower value means a better cost path.
The multi-dimensional planner shows some improvement over the normal planner in terms of expansions, and settled epsilon value in the two cases shown. To the first goal it expanded on average 32% less states than the uniform-dimensional planner, and to the second goal it expanded on average 28% less states than the uniform-dimensional planner. This means that if both planners were used with regular A*, or weighted A*, the plans generated by the multi-dimensional planner should take less time to compute than the those generated by the uniform-dimensional planner.
To the first goal, the epsilon value that the multi-dimensional planner settled on was 15% less than the the epsilon value that the uniform-dimensional planner settled on. To the second goal, the epsilon value that the multi-dimensional planner settled on was 25% less than the epsilon value that the uniform-dimensional planner settled on. This means that the sub-optimal plans generated by the multi-dimensional planner were closer to optimal than those generated by the uniform-dimensional planner. Of course, one must take into account that the optimal plans generated by both planners are not the same, since their respective state-spaces are not identical.
The diagram below provides an example of when the two planners do not produce the same optimal plan. The uniform-dimensional planner uses a smooth turning motion around the corner, while the multi-dimensional planner does not. The smooth turning motion is a motion primitive that the uniform-dimensional planner can make use of in that area. Since the area around the corner falls outside the multi-dimensional planner's high-dimensional range, it cannot not make use of this same motion for that same area.
Diagram displaying visual differences between optimal plans generated by both planners along a corridor.
Conclusion and Future Work
In this project I have developed a multi-dimensional planner for generating trajectories for UAVs in a simulated environment. I have also shown that multi-dimensional planning appears to have some performance gains over planning with a uniformly-dimensional state space. More specifically, it generates plans with less expansions without removing a dimension entirely from the state-space. In the future I hope to push the limits of the planner to much larger environments that would otherwise not be practical for use with planners that only make use of one set of dimensions.
I think a reasonable extension to this work could be an approach that uses multiple layers of states of varying dimensionality. Another extension could be to come up with a different set of criteria for determining the dimensionality of states.
Video
The multi-dimensional planner was also run in a larger 500x500x30 cell environment. A link to the video can be found below.
References
[1] Maxim Likhachev, Geoff Gordon and Sebastian Thrun, " ARA*: Anytime A* with Provable Bounds on Sub-Optimality, " Advances in Neural Information Processing Systems 16 (NIPS), MIT Press, Cambridge, MA, 2004.
My comments...
Anirudha Majumdar
I liked the video of the robots kicking the ball.
I suggest he tries to use his algorithm on actual robots.
Avik De
I liked his description of splines.
I suggest he tries to continue his project on an actual robot arm.
Ben Charrow
I liked the video he did of the pr2 with scarab.
Maybe use different robots.
Caitlin Powers
I liked her description of the control law used.
I suggest getting it to work on a wam arm.
Fei Miao
I liked her description of rrt.
I suggest getting it to work with real robots.
Jason Owens
I liked his overall presentation.
I suggest that he continues to improve his backpack detection by adding a learning component.
Kartik Mohta
I liked the quadrotor simulation he showed.
I suggest he tries to run the same algorithm using real quadrotors.
Menglong Zhu
I like how his algorithm could help the pr2 read written text on a white board.
I can't think of anything to suggest and thought he overall did an excellent job.
Philip Dames
I liked the simulation he showed.
I suggest he tries to improve the speed of his program.
Steve McGill
I liked the video of the two robots picking up the table.
I suggest he tries to get the same two robots to pick up another robot of similar size and weight next.