Implementation

Chassis

Due to limited access to fabrication resources because of the COVID-19 pandemic, any redesigns to the base had to be printed on a personal 3D printer.

Frame

We constructed the chassis out of slotted aluminum profiles and 1/4 laser-cut plywood, which is shown in Figure \ref{fig:bare_chassis}. Slotted aluminum was chosen for its strength-to-weight ratio, and it was used to bear the weight of the other components of our robot. Another advantage to using the slotted aluminum was its ease of assembly, as well as ease of adjustment in case of design changes. Figure \ref{fig:arm_location_1} shows the initial location of the shoulder joint of our robot arm, while Figure \ref{fig:arm_location_2} shows the updated location of our robot arm. This adjustment was made so that we could more easily reach the targets on the testbed, and was very easy to make because of the slotted aluminum profiles used in the frame.

Locomotion

To drive the wheels of the robot, we chose to use Hebi X8-9 Actuators. The driving factor behind this decision was that the we were also using Hebi actuators for our robot arm, and so it was much easier to plug/power two more Hebi Actuators both from a hardware integration perspective, as well as a software integration perspective. Since we were the only ShipBot team this year, we had access to enough Hebi Actuators to do this. This differential drive is shown in Figure \ref{fig:diff_drive}

Robot Arm

We decided to use Hebi actuators to actuate our arm. Hebi modules are smart actuators which are easy to integrate into a robot arm, have built in PID tuning mechanisms, can interface with ROS, and have a sufficient torque output. As a result, we decided that these actuators would be the easiest to integrate into our robotics system.

One drawback of these actuators is that they have serial elasticity, so they are not stiff or precise. As a result they tend to sag when the arm is carrying a load. However, they serial elasticity allows the robot arm to be flexible when the arm accidentally collides with an obstacle, which mitigates damage to both the environment and the robot arm.

The serial elasticity is a design element of the the HEBI actuators that allows them to detect the amount of force that they are applying, but time constraints prevented us from using force feedback while controlling the arm. Force feedback was not necessary for the switches and valves. However, during testing we noticed that it was difficult to flip the breakers with only position and velocity control. Further more, some of the HEBI modules would over heat when making repeated attempts to flip the breakers.

End-Effector

Since we chose to use a spring-loaded multi-pin end-effector, we had to ensure that the pins would have an interface with the springs. Initially, we planned to use E-style retaining rings. However, due to COVID-19, we didn't have access to a metal shop to machine the pins with a groove to work with this type of retaining ring. Because of this, we had to choose push-on retaining rings. Unfortunately, these types of retaining rings are much larger in diameter, meaning the pins could not be as close to each other as originally intended in order to avoid hardware interference. A cross section of the final end-effector design is shown below in Figure \ref{fig:ee_cross_section}. An open view of the inner spring enclosure is shown in Figure \ref{fig:ee_springs_on_parade}.

Base Localization

As stated in our Design Concepts, the base localization utilized two components: gmapping for map creation and AMCL for particle filter based localization within the map. With each of these components came a number of challenges in assuring a desirable localization estimate. For both of these components, we needed to get an initial Dead Reckoning estimate using the Hebi actuators.

Dead Reckoning

Dead Reckoning is a classic way to get a very rudimentary estimate of our state with only encoder feedback. The basic premise is to determine the linear (v) and angular (w) velocity and update the state using the following update rule:

x = x + v*cos(theta)*dt

y = y + v*sin(theta)*dt

theta = theta + w*dt

This method alone, however, does not produce an accurate enough estimate. To get a better estimate, we use a more robust integrator approximation, known as Runge-Kutta. This provides a better state estimate that can be used for both the map creation and the particle filter estimate.

Map Creation

Using the Dead Reckoned state estimate, we can utilize gmapping to create a map of the environment. To do this, our process required us to create a bag file where the robot remotely travels around the course, allowing us to collect both LIDAR and Hebi encoder data. By running gmapping and the bag file on a separate computer, we were able to create a map. Initially, our parameters were not ideal, resulting in the first map shown in figure \ref{fig:gmapping_map1}. After tuning the gmapping parameters, we were able to get a map with better resolution, as shown in figure \ref{fig:gmapping_map2}.

Particle Filter Localization

Our localization solution utilizes the Particle Filter to account for sensor noise. The particle filter is a method of estimating the state of the robot. To do this, the filter keeps track of the states of a large number of particles. In our case, our state is [x, y, th] or the 2D position and orientation. Each of the stored particles also have an associated weight to it. This weight tells us the likelihood that a specific particle is encoding the correct state of the robot. The particle filter simply updates the stored states and associated weights in two steps: (1) predict and (2) update.

The predict state takes in our movement commands (linear and angular velocities) and updates each of the stored states. For example, if our linear and angular velocities are v and w respectively, your state update would be th += w * dt, x += v*cos(th)*dt, y += v*sin(th)*dt for each of the particle states. For us, finding the linear and angular velocity commands of the robot is impossible with the cardboard mockup, as motors would be necessary to do so. As a result, we needed to utilize a simulated robot to test the filter.

The update state utilizes measurements to update the weights of each of the particles. In our case, these measurements are the IR sensor values. the update itself is based on a normalized version of Bayes Law.

To implement this, we used a generic particle filter found online (references in the source code) and re-purposed it to allow for our specific IR sensor measurements. To do this, we needed to create a shared coordinate system. The following image shows this coordinate system, as based on the course walls.

With the Dead Reckoned state estimate and the generated map, we can utilize AMCL to accurately determine the robot's state within the map. This is desired over purely Dead Reckoning due to the likelihood of drift with Dead Reckoning as well as the potential for the wheels to slip on the surface. In order to tune the AMCL parameters, we used the real robot, commanding the robot to move in a fixed manner, initially a straight line. We would visualize the estimate rviz. If we saw that the estimated LIDAR points matched the expected position on the map, we were able to conclude that our parameters were properly tuned. Below, in the figures, we can see an example of a situation where the estimated LIDAR points do align with the map and a situation where the estimated points do not align with the map.

In the tuning process, we identified that the LIDAR information often lags behind the state estimate, as the LIDAR is sending information at a lower update rate than the state estimate. As a result, there is some inherent lag to the particle filter. As a result, in determining whether our parameters result in proper convergence, we wait until the LIDAR values have settled after moving.

Base Planning

Our Base Planner went through two main iterations. Initially, the planner was developed to be open-loop. This meant that no feedback was involved. However, after developing and testing the planner, it was evident that due to slip in the environment, the error in the robot's movements using this open-loop planner was far too high. As a result, a closed-loop system was developed. To get from point A to point B using this system, as mentioned in the Design Concepts, there were three parts: a pre-rotation to point towards point B, move straight to point B, and a post-rotation to match the orientation of point B. This meant, there were two separate planners: a rotational planner and a linear planner.

Rotational Planner

The rotational planner was a bounded proportional controller. This meant that we would send angular velocity commands based on the following equation:

w = min(k_theta * (theta_des - theta_cur), theta_max)

By tuning the value of the proportionality constant, k_theta, we were able to get a fast and accurate response with the rotational planner.

Linear Planner

The linear planner used both a bounded proportional controller to determine linear velocities as well as pure pursuit to correct the trajectory by also determining a desired angular velocity. Similar to the rotational planner, our velocity was calculated using the following equation:

v = min(k_d * sqrt((x_des - x_ cur )^2 + (y_ des -y_ cur )^2}, v_max)

With pure pursuit, we could tune our lookahead distance and calculate the desired curvature gamma to stay on the straight path based on feedback from the base localization. Using this gamma, we could calculate an angular velocity w to be sent along with the linear velocity using the curvature property w = v * gamma.

Central Planner

The Central Planner utilizes the following logic to complete the mission specified in the Mission File.

Arm Planning

Interfacing with HEBI ROS Library

This first responsibility of the arm planner is to connect to the HEBI modules via the HEBI ROS library. This is done using a series of service calls to the HEBI ROS node which initialize the HEBI module group.

Kinematics

Even though our arm has 4 DOF, our kinematics treats it like a 3 DOF arm. The last joint, wrist 2, is controlled separately from the other three joints. This design allows us to use conventional RRR forward and inverse kinematics for our arm.

Trajectory Generation

Trajectory generator can create arbitrary linear work space trajectories. It does this by linearly interpolating between work space way points, then performing inverse kinematics on each of the interpolated way points. This method optimizes for precision and ease of implementation, but does not create a minimum jerk trajectory. In order to create protect the HEBI modules from jerking, we make sure that the robot arm moves at sufficiently slow speeds.

Executing Trajectories

At first, our arm planner would give our interpolated work space way points to the HEBI library, which would then create a minimum jerk trajectory based off of our way points and execute it. However, we found that the HEBI library was not accurate or predictable enough for our use case. Instead, the arm planner manually sends position and velocity commands in the configuration space to the HEBI modules at 200 MHZ. Manually sending the commands produces more accurate and reliable trajectories.

Interfacing with the Central Planner

In order to be a non-blocking function, we decided to wrap the functionality of the arm planner node in a ROS action server. This allows the central planner to make a non-blocking call to the arm planner node, perform other work while the arm planner executes its trajectory, then poll the arm planner to see when it has finished its work. Because the arm planner functionality is non-blocking, it allows our robot to do other tasks in parallel.

Vision System

The vision system interacts with the central planner and sends commands based on Target location and classified recent state.

Masking with Colorspace

To understand the shape and structure of the Target, we focus on using prior information of colors on the target objects and segment the object from image. A practical solution to image segmentation is utilizing the color spaces to match and mask. The matching is done by providing a target color and threshold-ing pixel intensity at either HSV or RGB channel space. As from the figure below it can be observed that HSV space has a spike of blue. We threshold on HSV space for each queried image frame to segment out objects, for both blue and orange objects. We then extract a blob from this segmentation and fit a circle with center and radius, as a key-point. Although this method is time-complex, its precision and accuracy is ~99% which is good for our application. In future, well trained deep learning methods can be employed for the task.

Object Detection and Classification

For the task of object detection and classification, we employ a Deep Learning model. We have about 30 frames annotated for training along with state-classes i.e On/Off for standing valves, one for shuttle and wheel valve, and only 3 for 3 breaker switches and we employ masking along with this to determine state. We chose YoloV3 as our base model. Vanilla YoloV3 pretrained/retrained initially doesnt perform very well. We used extra skip connections and added dialated convolutions into the YoloV3-SPP model which jumped the accuracy from ~70% to ~98% Precision. The model is trained with Pytorch. We were unsuccessful in using ONNX for exporting custom model to TensorRT, hence we wrote model code in C++ utilizing Nvidia's NvInfer library for Jetson device. Further we also use Quantization (FP16) to improve prediction speed. Hence for any new annotated data we train on pytorch, generate txt file of weights and compile a serialized engine which runs inference in real-time. The detection and classified model is named "Modified and Quantized YoloV3-SPP" (MYDT).

Object Tracking Strategy

We used a distance based tracker to track the bounding boxes (BBOX) for every new detection and keep on assigning the values according to the Functions of the tracker:

1) State generation: The state is generated based on both masking data and the detection of BBOX. The advantage of using a masking procedure is that you get exact location of the breaker switch due to its peculiar color.

2) Location generation: The MYDT does the major part of the job in detection of exact location.

For example, for a new image frame input to the tracker, the get_mask function will generate mask which will extract a blog of orange color. This blob key-point will have a center and radius. when this center is on top of the BBOX i.e the y-axis value of the keypoint center is greater than that of the BBOX center, it means that the breaker switch is on. When lower, it means that its off. Hence avoiding another extra class for MYDT training for classification.

Interfacing with Central Planner

The tracker directly talks to the central planner. As central planner sends a signal by publishing a topic, the tracker receives and forwards another signal to the MYDT. The MYDT is a TensorRT inference engine, it takes about average 0.53s for predictions on each new image. While the MYDT works, the tracker starts another parallel process of Masking. As the function of masking process is to generate mask based on colors, it identifies and stores a center keypoint and radius of the blob on a global variable. As soon as detector is ready with the prediction, it publishes a message consisting of anchor point and size (x, y, w, h) of each BBOX back to the tracker. Using the stored mask and stored detection, for every signal from central, the tracker publishes back the state and location of the target. This is made such that the central planner can send messages whenever it needs, and don't need to run the detector all the time. Hence reducing the gpu and memory load, needed for other important processes like mapping and trajectory generator of HEBIs. But note that this framework can run for any fps considering the tracker system uses BBOX interpolated tracking.

Integration and Evaluation

The integration of our components was split into the integration of our mechanical and hardware subsystems and the integration of our software subsystems. Although we did finish a majority of the mechanical integration prior to Spring Break, some of the mechanical integration as well as all of the software integration occurred after Spring Break. At this time, we were working remotely, and as such our procedure for integration had to change. Kenny was the only person with physical access to the robot.

To perform our integration, we setup the SSH and NX protocols to allow us to edit code and visualize the Jetson Nano's desktop remotely. After working on our code on our local machines, we would schedule times to meet with Kenny to test the code on the physical robot. This definitely proved to be doable, albeit more time consuming than normal.