3.2 Papers
3.2.2 Paper II
Multi 3D Camera Mapping for Predictive and Reflexive Robot Manipulator Trajectory Estimation
Having a system consisting of a robot manipulator and multiple calibrated 3D cameras allows the system to observe the workspace and detect moving obstacles.
By utilising this information, the robot can adapt to changing dynamic conditions.
This work aims at solving the issue of relatively slow trajectory recalculation pro-cess of existing methods when an unexpected obstacle appears in very close prox-imity to the robot, which sometimes can lead to a collision.
Our setup consisted of having in total four different 3D cameras. It allowed for redundancy as well as ensuring the workspace is seen from many angles, so obstacles do not obstruct the field-of-view. However, with every camera providing a point cloud, the amount of data to be processed was too large for a single com-puter to provide real-time performance. To address this issue, but still keeping the necessary accuracy, the point clouds were pre-filtered, then merged by using the calibration data, and position fine-tuning using the iterative closest point (ICP) method and then converted to an octomap. Octomaps can have a different resolu-tion in different parts of the map allowing to have a finer resoluresolu-tion on the points of interest, while background objects, which are out of reach for the robot could be set to have a coarse resolution, thus saving a significant amount of processing power.
Once the map of the workspace was created, the robot itself had to be removed from the octomap to prevent false-positive self-collision instances. It has been done by taking its joint encoder information and fitting a simplified model using finely fit cylinders and overlaying it in the volumetric octomap. Any voxels that are covered by the estimated robot model were removed from the octomap. What was left was a collision map representing static structure obstacles, like a table and a robot cart, as well as any dynamic obstacles that come into a field-of-view of the cameras.
By using a long-term observation of the workspace, a danger map was
con-structed. It was based on analysing the frequency of the objects occupying the
workspace and a cost function based on a logarithmic decay calculates the
likeli-hood that this part of the map will be free or obstructed long-term. By utilising
this information, the predictive behaviour part of the robot motion planner
calcu-lates the trajectory, which takes into consideration the length of the path together
with the risk that the area being crossed might be obstructed. The planner is based
on the RRT-Connect algorithm [31].
(a) Overview of the algorithm. Green ovals rep-resent the sensing part, blue rectangles - process-ing part, yellow hexagons - motion plannprocess-ing and grey rectangle (dashed borders) - motion execu-tion. Reflexive motion planning marked as a yel-low hexagon (dashed borders) overrides the pre-dictive motion planning when an unexpected ob-stacle gets close to the robot.
Start Pos
Goal Pos Reactive Planning
Obstacle Stop and
replan point
(b) Reactive Behaviour Planning: When the ob-stacle is present, the robot stops and recalculates the path. Occupied workspace is never crossed.
Start Pos
Goal Pos
Danger Map Predictive Planning
(c) Our method: the trajectory fully avoids, but gets close to the medium risk area in the danger map. If detour is not large, a safe path is chosen over a risky one.
Start Pos
Goal Pos
Danger Map
(d) Our method: the trajectory crosses the low risk area in the danger map.
Start Pos
Goal Pos
Danger Map Obstacle Predictive + Reflexive Planning
(e) Our method: Predictive and Reflexive Plan-ning combined, when an unexpected obstacle gets very close to the robot.
Figure 3.5. Algorithm overview for the robot reflexive-predictive behaviour model.
To react quickly and avoid any unexpected obstacles that appear in a very close proximity to the robot, a reflexive behaviour model was used. It was inspired by how many of us would pull back an arm if we touch anything hot or sharp, even without understanding or observing what happened. Instead of calculating a new path, the model retraces some previously-executed trajectory points, while a new and safe route is being recalculated using a predictive model in parallel.
Before the execution, a check is made to ensure that retracing the previous path is free of obstacles. Otherwise, a movement vector facing away from the detected obstacle is used as a reflexive behaviour trajectory. The algorithm is explained in Figure 3.6.
Start Pos
Goal Pos Obstacle
Backtracking avoidance move
Obstacle movement Initial robot trajectory Invalidated robot trajectory Replanned trajectory Alternative avoidance move Collision object
movement estimation Collision checking areas for avoidance moves Trace of the obstacle’s previous positions
Figure 3.6. Reflexive Behaviour. The planned robot trajectory (solid green line) is blocked by a moving obstacle (marked in red). The first option is to backtrack on the executed path (pink dotted line) until the collision risk is over. If backtracking still results in a collision, the second option is to use the alternative avoidance move and move to the direction opposite from where the obstacle is approaching (dotted orange line). In the meantime, an alternative collision-free trajectory to the goal position is calculated and executed (blue dashed line).