3D model based ladder tracking using vision and laser point cloud data

This paper presents 3D model based industrial ladder tracking using vision and laser point cloud data for the ladder climbing task of a humanoid robot ATLAS in the DARPA Robotics Challenge. A virtual visual servoing algorithm with a moving edge detector is used for visual 3D ladder tracking to obtain 6D pose of ladder relative to the robot. An iterative closest point algorithm, which is suitable for 6D pose recognition with laser point cloud data, is used for initialization and failure recovery of the visual 3D tracking algorithm. For each loop of the visual tracker, With 6D pose from previous image frame or from laser point cloud data, a virtual image of the 3D ladder geometric model is first generated by projective back-projection. Then, the moving edge detector is applied to find the displacement of edge features in the virtual and real image. Image Jacobian is calculated to obtain the gradient of the 6D pose with respect to displacement of edges features. Then, the visual virtual servoing algorithm is used to obtain the 6D pose of the ladder iteratively according to the image Jacobian and feature error. The iterative closest point algorithm with laser point cloud data is executed to get the 6D pose globally if necessary for reinitialization or recovering from tracking failure. The 3D ladder tracker has been verified both in drcsim/Gazebo simulation environment and with real data.