Path planning for visual servoing

Question

I am at moment trying to implement a visual servoing application. the robot I am using is a UR5, and TCP has a stereo camera mounted on to it. The idea is to move the end effector according to the object being tracked.

The path-planning algorithm for this system should comply with some rules.

1. The path which it creates should be collision free, and always keep the object being tracked at sight at all time.

Having a path that keeps the object in sight has been bit of problem. Sometime will the end effector rotate around itself, messing up measurements taken and thus the tracking itself.

2. It should be able to maneuver away from static obstacles.

A Possible solution?

I thought of a possible solution. Since my current state and desired state is defined by two different sphere, A possible solution would be to create a straight line between each center of each sphere, and between the current position and desired position, such that a straight path in between could be computed easily. which always keeps itself oriented toward the object. Problems is that I am not sure how I should handle collision here.

Update Or use it as a heuristic for a heuristic based path planning?

Answer 1

I would recommend using an RRT or FMT sampling based path planner. The basic idea is to sample your state space and build a tree which connects your starting state to the goal state. Each time you connect two samples, you check for a collision: if there is no collision then you add the connection to your tree search, otherwise you move on.

It is your responsibility to write a collision checker routine, since this is a fairly application specific task. If you add some more detail to your set-up I could make the answer more specific (are you using an occupancy grid? Are there dynamic constraints?).

If I'm not mistaken you can represent an occluded view as a collision, in which case the algorithm will find a path which gets from start to finish without occlusions. Your sphere's idea is not too dissimilar from how the proof for the FMT algorithm works. For FMT (which I am more familiar with), a recent dissertation is here (look at "Avoiding plume impingement" as a way to avoid occlusions), and a complete description is here, with open source implementation here.

You might also be interested in "next best view" planners.