I’ll try to keep this short and sweet.
One approach that is often used to solve these problems in simulation is a Rapidly-Exploring Random Tree. To give at least a little credibility to my post, I’ll admit I studied these, and motion planning was my research lab’s area of expertise (probabilistic motion planning).
The canonical paper to read on these is Steven LaValle’s Rapidly-exploring random trees: A new tool for path planning, and there have been a million papers published since that all improve on it in some way.
First I’ll cover the most basic description of an RRT, and then I’ll describe how it changes when you have dynamical constraints. I’ll leave fiddling with it afterwards up to you:
Terminology
“Spaces”
The state of your spaceship can be described by its 3-dimension position (x, y, z) and its 3-dimensional rotation (alpha, beta, gamma) (I use those greek names because those are the Euler angles).
state space is all possible positions and rotations your spaceship can inhabit. Of course this is infinite.
collision space are all of the “invalid” states. i.e. realistically impossible positions. These are states where your spaceship is in collision with some obstacle (With other bodies this would also include collision with itself, for example planning for a length of chain). Abbreviated as C-Space.
free space is anything that is not collision space.
General Approach (no dynamics constraints)
For a body without dynamical constraints the approach is fairly straightforward:
- Sample a state
- Find nearest neighbors to that state
- Attempt to plan a route between the neighbors and the state
I’ll briefly discuss each step
Sampling a state
Sampling a state in the most basic case means choosing at random values for each entry in your state space. If we did this with your space ship, we’d randomly sample for x, y, z, alpha, beta, gamma across all of their possible values (uniform random sampling).
Of course way more of your space is obstacle space than free space typically (because you usually confine your object in question to some “environment” you want to move about inside of). So what is very common to do is to take the bounding cube of your environment and sample positions within it (x, y, z), and now we have a lot higher chance to sample in the free space.
In an RRT, you’ll sample randomly most of the time. But with some probability you will actually choose your next sample to be your goal state (play with it, start with 0.05). This is because you need to periodically test to see if a path from start to goal is available.
Finding nearest neighbors to a sampled state
You chose some fixed integer > 0. Let’s call that integer k. Your k nearest neighbors are nearby in state space. That means you have some distance metric that can tell you how far away states are from each other. The most basic distance metric is Euclidean distance, which only accounts for physical distance and doesn’t care about rotational angles (because in the simplest case you can rotate 360 degrees in a single timestep).
Initially you’ll only have your starting position, so it will be the only candidate in the nearest neighbor list.
Planning a route between states
This is called local planning. In a real-world scenario you know where you’re going, and along the way you need to dodge other people and moving objects. We won’t worry about those things here. In our planning world we assume the universe is static but for us.
What’s most common is to assume some linear interpolation between the sampled state and its nearest neighbor. The neighbor (i.e. a node already in the tree) is moved along this linear interpolation bit by bit until it either reaches the sampled configuration, or it travels some maximum distance (recall your distance metric).
What’s going on here is that your tree is growing towards the sample. When I say that you step “bit by bit” I mean you define some “delta” (a really small value) and move along the linear interpolation that much each timestep. At each point you check to see if you the new state is in collision with some obstacle. If you hit an obstacle, you keep the last valid configuration as part of the tree (don’t forget to store the edge somehow!) So what you’ll need for a local planner is:
- Collision checking
- how to “interpolate” between two states (for your problem you don’t need to worry about this because we’ll do something different).
- A physics simulation for timestepping (Euler integration is quite common, but less stable than something like Runge-Kutta. Fortunately you already have a physics model!
Modification for dynamical constraints
Of course if we assume you can linearly interpolate between states, we’ll violate the physics you’ve defined for your spaceship. So we modify the RRT as follows:
- Instead of sampling random states, we sample random controls and apply said controls for a fixed time period (or until collision).
Before, when we sampled random states, what we were really doing was choosing a direction (in state space) to move. Now that we have constraints, we randomly sample our controls, which is effectively the same thing, except we’re guaranteed not to violate our constraints.
After you apply your control for a fixed time interval (or until collision), you add a node to the tree, with the control stored on the edge. Your tree will grow very fast to explore the space. This control application replaces linear interpolation between tree states and sampled states.
Sampling the controls
You have n jets that individually have some min and max force they can apply. Sample within that min and max force for each jet.
Which node(s) do I apply my controls to?
Well you can choose at random, or your can bias the selection to choose nodes that are nearest to your goal state (need the distance metric). This biasing will try to grow nodes closer to the goal over time.
Now, with this approach, you’re unlikely to exactly reach your goal, so you need to define some definition of “close enough”. That is, you will use your distance metric to find nearest neighbors to your goal state, and then test them for “close enough”. This “close enough” metric can be different than your distance metric, or not. If you’re using Euclidean distance, but it’s very important that you goal configuration is also rotated properly, then you may want to modify the “close enough” metric to look at angle differences.
What is “close enough” is entirely up to you. Also something for you to tune, and there are a million papers that try to get you a lot closer in the first place.
Conclusion
This random sampling may sound ridiculous, but your tree will grow to explore your free space very quickly. See some youtube videos on RRT for path planning. We can’t guarantee something called “probabilistic completeness” with dynamical constraints, but it’s usually “good enough”. Sometimes it’ll be possible that a solution does not exist, so you’ll need to put some logic in there to stop growing the tree after a while (20,000 samples for example)
More Resources:
Start with these, and then start looking into their citations, and then start looking into who is citing them.
- Kinodynamic RRT*
- RRT-Connect