How to make your robot go from A to B without hitting things (Pt. 2)
Aug 25, 2017
This is a second post about robot motion planning. You can find the first post about sampling-based planners over here. Being able to plan a path quickly while avoiding collisions is crucial for our roadmap.
Optimisation-based planner
The main difference with the sampling-based planner is that optimisation-based planners reason in continuous space. Those planners are usually a lot faster than the sampling-based planners. They don’t need to generate all the unnecessary states - checking states for collision is a costly operation. The main limitation of the control-based planner is in how it handles cluttered environments.
We define our problem - going from A to B without hitting things - as a constrained optimisation problem. Once the problem is formulated, there is a variety of solvers which can be used to find a solution - however we won’t delve too deep here.
So how can we define our planning problem as a constrained optimisation problem?
If we summarise our goal as an equation, we want α to be at point A at the start, and at point B at the end, which gives us:
However we don’t use equations - a robot never goes exactly where you want it to go and in any case you don’t know where it is exactly. But we’d rather like the robot to be as close as possible to those points at the given time, which gives us the following constraints:
So (3) and (4) means that α is close to A at the start, (5) and (6) that α is close to B at the end.
Finally we have the big blue obstacle to consider. In this very simple example it’s easy enough to model it using constraints, we simply don’t want alpha to be negative, or bigger than 180 degrees - otherwise our robot would collide with the obstacle. This gives us the following constraints:
So our problem is now constrained by those 8 inequalities. Obviously in a more complex scene, we’d have more constraints.
Now what is the function that we actually try to minimise? This depends on what we want our planner to do. A simple solution is to look for the shortest path to minimise the trajectory length; this gives us the following function which we want to minimise:
On a more advanced robot - one with multiple joints - we could also try to minimise the joint acceleration, to avoid reconfigurations as displayed below. A reconfiguration is when a robot changes its joint angles drastically to get to the next point in the trajectory as can be seen below.
Both those states are reaching the same point, if one goes from the one to the other, the robot “jumps” in space.
Solving constrained optimisation problems is a well studied field. Recently, a state of the art algorithm has been developed by Marc Toussaint in his KOMO planner . If you have any questions - or would like to collaborate on getting your planner working on our robots - feel free to tweet @UgoCupcic , he’s our Chief Technical Architect.