Visual sensing for developing autonomous behavior in snake robots

Snake robots are uniquely qualified to investigate a large variety of settings including archaeological sites, natural disaster zones, and nuclear power plants. For these applications, modular snake robots have been tele-operated to perform specific tasks using images returned to it from an onboard camera in the robots head. In order to give the operator an even richer view of the environment and to enable the robot to perform autonomous tasks we developed a structured light sensor that can make three-dimensional maps of the environment. This paper presents a sensor that is uniquely qualified to meet the severe constraints in size, power and computational footprint of snake robots. Using range data, in the form of 3D pointclouds, we show that it is possible to pair high-level planning with mid-level control to accomplish complex tasks without operator intervention.


I. INTRODUCTION
A successful field robot must be able to sense, plan, and act; three essential components of an autonomous system [1], [2].Research in snake robots has focused on the acting component; with various work being done to improve the control and fundamental understanding of snake locomotion [3], [4], [5], [6].The other two constituents of autonomy, sensing and planning, have been comparatively unexplored.Previous work on sensor assisted planning in snake robots has resulted in limited behaviors [7], [8], and in the case of Transeth, has been limited to planar cases [9].The motion primitive approach to planning used in Hatton's work produced more extensive behaviors, but was not assisted through the use of on-board sensors [10].This paper presents the first use of a on-board visual sensor specifically developed to enable 3D motion planning for snake robots.
This increase in sensing capability comes through the design of a small structured light sensor, designed to fit inside the head module of the CMU modular snake robot [11].This sensor makes it possible to map remote 3D environments (Figure 1).This range information, in the form of 3D pointclouds, is used to provide environmental data to the planning algorithm such as the location of obstacles.The planner then employs a motion primitive approach to enable task-specific autonomy.Automated pole climbing is presented as an novel example of automation using this framework.
All of these authors are with the Robotics Institute at Carnegie Mellon University, Pittsburgh, PA, U.S. Fig. 1: Modular Snake scanning the environment.The snake is partially curled to have a stable position.The snake head is lifted up to have a better viewing point and to allow the head to freely scan.The red line is the light from the laser.The inset shows the camera image (rotated for clarity).

II. BACKGROUND
A. Snake Robots Snake robots are highly-redundant mechanisms that use their many degrees of freedom to accomplish a wide variety of locomotion tasks [12], [13], [14].In addition to basic motions such as forward and lateral movement, snake robots excel at more complicated tasks, such as pole climbing and pipe navigation.The CMU modular snake robot used in this work has 16 degrees of freedom, alternating vertically and horizontally along its body, and has a width of 5.1 cm and a length of 94 cm [11].

B. Gaits
We typically control the motion of snake robots with gaits, cyclical controls that coordinate a system's internal degrees of freedom to produce net locomotion.Different gaits produce different motions, e.g.some gaits create net forward displacement whereas others result in planar rotation.For snake robots, one way to command gaits is by using a gait equation [5] to generate desired joint angle commands.The parameters in this gait equation define the shape of the propagating sine waves that travel along the snake's body.For the scope of this paper it is sufficient to say that the gaits used are each represented by a different set of these gait parameters.The gaits used in our model behavior include sidewinding, conical sidewinding, and pole climbing [6] [5].

C. Structured Light Sensors
Structured light is a proven technology that is well adapted for short range sensing [15].It is ideal for applications with stringent size and power constraints, which is the case with snake robots.Our recent research [16] demonstrated that structured light can be employed in exterior environments and can deal with optically difficult material like metals, which are likely to be encountered during search and rescue or inspection tasks.In our design the structured light is a laser line projected onto the environment and observed by a Black and White (B/W) camera.The line is extracted from the captured image (Figure 1) and the range data is calculated through triangulation.To get a full 3D point cloud either the laser (as was done in [16]) or the whole sensor needs to be physically rotated.In order to avoid any additional scanning mechanisms, we employ the snake robot's kinematics to sweep the head module while maintaining a stable support, one such configuration is shown in Figure 1.Each module can rotate by 180 • which allows for a wide scanning range and therefore a large 3D point coverage (Figure 4).Fig. 2: Sensor head on the CMU modular snake robot.A red laser is on the right and a B/W camera is on the left.They form the structured light sensor.On the bottom is a color camera and on the top are two LEDs for illumination.

III. MECHANICAL OVERVIEW
The mechanical architecture of the sensor head is similar to that of it's predecessor [11], modified to accommodate a structured light sensor and additional circuitry (Figure 2).In addition to a line generating laser and a B/W camera, there is a higher resolution color camera for teleoperation along with two LEDs for illumination in dark spaces (Figure 3).The sensor head connects to the snake robot using an unpowered modular head adapter and utilizes the same power, communication, and video feeds as the snake (12V, RS-485, and analog video).

A. Design Process
The sensor head module was designed through an iterative design process using rapid prototyping with a precision Stratasys 3D printer, decreasing both cost and production time.This allowed us to minimize custom made electronics and ensure the correct fit of components within the module's housing.The choice and positioning of internal components were governed by design constraints, including minimizing overall length and maximizing the spacing between the camera and laser.Maximizing the spacing between the camera and laser leads to a larger effective range and higher resolution at long distances.

B. Housing
The housing as well as all internal component mounts are machined out of aluminum alloys.All components thread on to a face plate via standoffs, allowing easy disassembly if maintenance or adjustment is required.The face plate slides into an outer shell, which in turn threads on to the modular head adapter.Both connections use bore seal o-rings to prevent ingress of water and debris.Technical specifications on the outer shell housing, as well as details on other important components are given in Table I.
Two laser cut acrylic windows are secured behind the face plate, compressing face seal o-rings (Figure 3).Using two acrylic windows avoids interference of light from the LEDs and laser to the cameras.Irregular geometry prevented the use of more scratch-resistant glass or sapphire windows at a reasonable cost.
The components are fixed onto the face plate using custom aluminum housings.The LED PCB is clamped into a recessed cavity by a heat sink which contacts the outer shell to efficiently transfer heat to the environment.Effective heat transfer between the PCB and clamp is facilitated by a thermal interface pad (Figure 3).To fit the larger, higher resolution bullet camera we used a combined housing assembly, which holds both cameras, their respective lenses, and a bandpass filter for the B/W camera.This combined housing aligns with the face plate using dowel pins and is secured with machine screws and standoffs.Each camera is aligned by a 3D printed cap which is sandwiched between the housing and the main PCB.The laser diode is held with a shaft collar style clamping fixture, which is secured with the camera housing.

C. Camera and LEDs
The teleoperation camera is a commercially available color bullet camera, removed from its original housing and put into the aforementioned camera assembly.We have currently installed a 3.6 mm S-mount lens to give a 79 degree field of view.The housing will fit between a 2.6 mm and a 8 mm lens, providing flexibility in adapting to different deployment environments.Due to space constraints, we reduced the number of LEDs to two; the standard head module contains four.The power of the LEDs can be controlled through the snake's communication bus [5].

D. Structured Light Sensor
The structured light sensor consists of a B/W version of the teleoperation camera and a line laser.The camera is fitted with a 6 mm S-mount lens.The laser driver is capable of digital TTL modulation into the MHz range.By combining this capability with the 60 Hz camera, we are able to switch the laser off with every other camera frame and perform background subtraction.This greatly reduces noise in the video frames fed to the 3D reconstruction code, which raises the signal to noise (S/N) ratio.If background subtraction is not necessary, we retain the option of operating at a higher frame rate for a faster scan or higher resolution point cloud.To further increase the S/N ratio in bright environments, a custom bandpass filter is fitted between the lens and camera sensor.

E. Additional Sensing
Additional sensors are located on the LED PCB and the main PCB.Both have temperature sensors, and the main board also houses all control and power circuits.As in each of the snake robot's modules, there is an IMU in the sensor head module.

A. Autonomous Pole Climbing
A current application of the sensor head described in Section III is object recognition in 3D scenes; in this case poles.Pole climbing is a robust behavior, but currently it requires a large amount of situational awareness on the part of the operator.Because the head of the snake is pointed away during the approach to the pole, climbing is difficult to initiate.This behavior is generally done where the operator has a clear view of both the snake and pole.The development of the 3D capable sensor head has allowed for the automation of the pole climbing behavior 1 .In this section we outline the basic principles behind this automation.
1) Localization: Using the snakes kinematics, the sensor head produces a 3D pointcloud of the surrounding environment.We use the Point Cloud Library 2 , a large repository for pointcloud manipulation and analysis algorithms, to process the visual information in the pointcloud.The algorithms utilized by our application include: cylinder and plane segmentation, voxel filtering, and a standard statistical filter.
Once the scan procedure has completed and a 3D pointcloud is available, several filters are applied to reduce noise and remove outlier points in the data (Figure 4).Each of the filters introduces an increase in computation, and thus the order and choice of filters applied is critical for fast and robust localization.The filtering starts with a passthrough filter, which removes any points outside a given bounding box.The sides of the bounding box are 2.5 m; which concur with the maximum range of the structured light sensor.An additional voxel filter is then used to reduce the size of the pointcloud.The final filter, which is also the slowest filter computationally, is the statistical filter, which removes noise in the pointcloud.
The filtered data is then segmented with modified versions of the PCL algorithms for plane and cylinder segmentation.Plane segmentation is used to remove the ground in the scene, which is necessary due to the unreliability of cylinder segmentation algorithms when large planes are present.Cylinder segmentation algorithms produce false positives corresponding to imaginary cylinders in the ground plane, especially with pointclouds generated by circular sweeping motions.We then apply the cylinder segmentation algorithm to the resulting pointcloud, revealing the location of the pole in the environment.The pole localization algorithm has a range of 2 m from the center module of the snake.Beyond this range, the decaying resolution of the structured light sensor prevents reliable detection.The cylinder segmentation algorithm is calibrated for poles with a diameter in the range of 5 cm to 20 cm, which corresponds to the size of poles that can be climbed by the snake robot. 1 A video showing autonomous pole climbing has been submitted with this paper.This video is also available via http://youtu.be/X3p10HWVHsc(This is an unlisted video and can only be seen using this link) 2 The Point Cloud Library (PCL) is a standalone, large scale, open project for 2D/3D image and point cloud processing.(www.pointcouds.org)2) Locomotion: Once the pole is localized, the snake proceeds to move towards it.The locomotion process is broken down into three sub-processes: rotation, displacement, and climbing.The snake first rotates to face the pole, and then moves toward the pole before finally climbing it.
It is important to note that, for a snake robot, it is difficult to assign a meaningful body frame that captures the net behavior in an intuitive way.In automated pole climbing, we represent the snake's body frame using the Virtual Chassis [4].This tool uses forward kinematics and sensor data from the IMUs inside each of the modules to accurately estimate the pose of the robot in the inertial frame.With an accurate body frame representation available, we can apply the intuition behind planing for simpler robots to the snake.A similar planning strategy to that presented here, that is built upon motion primitives, was explored in [10].In this previous work, however, the robot was assumed to have global localization.
To determine the magnitude of rotation and displacement necessary to approach the pole we model the snake kinematics as a two link manipulator with one revolute and one prismatic joint (Figure 5).Our planner finds inverse kinematic solutions to this two-link manipulator problem.
The rotation sub-process uses conical sidewinding [6].The macroscopic behavior of conical sidewinding can be thought of as a cone rolling on a flat surface, resulting in a rotation about its apex.In our case the cone is the snake robot, contorting its backbone curve to the cone surface, and the peak corresponds to the distal end of the robot.This method of rotating was chosen because it reduces the unmodeled effects of the robot's tether.At each timestep in our rotation sub-process, the Virtual Chassis allows us to compare successive body frames and extract the rotation between them.By executing conical sidewinding slowly we can integrate these small rotations into an estimate of the net rotation.Once we have reached the theta calculated by the planning algorithm, we stop the rotation sub-process.
After rotating, the axis along the length of the snake is perpendicular to the pole location as shown in Figure 5.The snake then locomotes towards the pole using a sidewinding gait.This biologically-inspired gait produces a net lateral motion of the snake robot [5].
The distance traveled while sidewinding is determined through the use of a simplified motion model for the snake introduced in [3].This model determines the distance traveled by integrating the small differential motions of modules in contact with the ground between timesteps.Though limited to certain gaits, this model proved to be reliable for this behavior.
The final component of this automated behavior is climbing.This complex behavior has previously been reduced to a series of pre-calculated motions [5].By contorting the shape of the snake robot into an arc, the snake can roll about the its longitudinal axis.We ensure that the snake has reached the pole, while maintaining this curved shape, by allowing the Fig. 5: Comparison between snake motion planning and two link manipulator system.The initial and final body frames for the snake are represented with B and B' respectively.snake to roll towards the pole for 5 seconds.The curvature of the snake can then be controlled to cause the snake to tightly wrap around the pole; taking a helical shape.Measurements of the motor currents in the modules can be used to determine when the snake is wrapped tightly enough to begin climbing the pole.Climbing is achieved by rolling within this helical shape [5].
3) Results: Automatic pole climbing was tested in ten trials with different required rotations.Each trial was denoted as either a failure or a success based on whether the snake robot was able to locate and climb the pole.The robot was successful in eight out of ten trials (Table II).It is important to note that although a pole can be located in the environment up to 2 m away, the robust operable range of this behavior is 1.5 m away from the pole.The net rotation calculation of the snake has an error of ±10 degrees, and thus success cannot be ensured outside this operable range.Trials 6 and 7 are examples of this limitation; in these trials the snake robot rotated too much and failed to reach the pole.TABLE II: Results for the testing of automatic pole climbing.The columns titled Distance and Magnitude of Rotation correspond to the parameters D and θ in Figure 5 V. CONCLUSIONS AND FUTURE WORK The purpose of this work was to develop the sensing capabilities required for autonomy on snake robots and to present an example behavior.The newly developed sensor head allowed for the collection of 3D pointclouds.These pointclouds were analyzed to locate specific environmental features, in this case poles.We then automated a pole climbing behavior, demonstrating the possible autonomy enabled by improved sensing.
We are currently developing a new generation of the sensor head.Design improvements will increase the resolution and range of the pointclouds; allowing for more robust pole localization as well as other autonomous capabilities.
Adding robustness to automatic pole climbing via the improvement of the motion planning algorithm is also possible.One probable addition is to increase the operable range of the behavior by chaining observation steps until a pole is close enough to ensure a successful climb.
The addition of the structured light sensor has given the CMU modular snake robot the sensing capabilities required for new promising behaviors.For example: scan registration and map building, essential first steps in the creation of a robust SLAM behavior.Other potential capabilities include autonomous pipe entry and traversal and 3D object reconstruction.

Fig. 4 :
Fig. 4: 3D data from structured light sensor.Colors have been added for clarity.

TABLE I :
Technical details for important components