When exploring continuous space with planning algorithms, I recently utilized the kinematics of the robot, kinematic primitives (ie the controllable aspect of those kinematics- wheel/joint velocities or accelerations) and a given time delta t to calculate where the new pose of the robot would be. This allows me to explore a continuous space with algorithms like A*.

When doing this, amongst other things like collision checking and other constraints, I need to check to see if I have considered or reach this pose before now, or consider it's cost to see if I've found a shorter route. In discrete spaces this is easy - I can essentially use some methodology to deterministically identify that space (such as coordinates for a grid, for instance) and from there I can safely identify the pose as being reached before. For continuous spaces, this becomes a lot more difficult.

So far in my experimentation I have tried the following methods:

  1. The naive method, which I didn't really try, but present for completeness: Perform some "closeness" calculation of the given pose to all other poses, and determine if any are below a set threshold. Abandoned at first consideration as it will become progressively slower the longer exploration continues.

  2. Rounding the pose variables (ie x, y, theta orientation, joint values, etc) to a set decimal place, and utilizing the combinations thereof to create a hash such that similar-enough poses would generate the equivalent hash. Benefits from the O(1) lookup/set times. I tried this at first and results were never very good; it seemed that I could never find a good balance of determining

  3. Aggressively round to the nearest 0.XX5/0 (or similar, depending on your scale), and utilizing a series of hashes (defaultdicts, in python to simplify things) such that you can quickly check the existence/state of visiting this location prior. Say that we had the state variables $x$, $y$, and $\theta$, and our rounded versions were $x_r$, $y_r$, $\theta_r$, then we could have a series of nested hashes such that we can say visted[x_r][y_r][theta_r]. I've had success with this but requires significant testing to find an appropriate configuration to find a balance of allowing exploration of various orientations vs abandoning pre-considered spots.

  4. KD-Tree - This worked well when I didn't need to consider orientations/rotations. I suspect to deal with rotations that are continuous (a rotation at $\frac{3\pi}{2} + \pi$ simply becomes $\frac{\pi}{2}$) I could % $2\pi$ (IE in our previous example, $\frac{5\pi}{2} % 2\pi = \frac{\pi}{2}$) - but then you have issues with expressing proximity between a point just before $2\pi$ and just after $2\pi$ - ie near zero.

So essentially I have tried a few approaches, seen problems with all of them, and wish to know if there are other data structures or approaches to handle this piece of the problem. Alternatively, if there are other terms or methods that I'm simply ignorant of, I'd love to be point in the right direction for future reading.



Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.