Ugo Cupcic
Sep 15, 2016 · 3 min read
Image for post
Image for post

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?

Image for post
Image for post
Single joint robot, planning from A to B, obstacle in blue.

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:

Image for post
Image for post
goals as equations

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:

Image for post
Image for post
goal represented as 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:

Image for post
Image for post
obstacle represented as 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:

Image for post
Image for post
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.

Image for post
Image for post
Image for post
Image for post
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 me @ugocupcic.

Welcome to a place where words matter. On Medium, smart voices and original ideas take center stage - with no ads in sight. Watch
Follow all the topics you care about, and we’ll deliver the best stories for you to your homepage and inbox. Explore
Get unlimited access to the best stories on Medium — and support writers while you’re at it. Just $5/month. Upgrade

Get the Medium app

A button that says 'Download on the App Store', and if clicked it will lead you to the iOS App store
A button that says 'Get it on, Google Play', and if clicked it will lead you to the Google Play store