Stretch 4 Autodocking

This work was built and open-sourced at Hello Robot. Please see hello-robot/stretch4_docking repo for the latest code.

The key improvement in this version of the docking station is the arrangement of the 3 hybrid markers. Our previous iteration used 2 bike reflectors on the dock pillar, which gave us a high-SNR signal easily isolated via intensity thresholding (i = 255) among the LiDAR’s 2M points/s, but it left the 3D pose of the docking station ambiguous. Using 3 markers lets us solve the orthogonal Procrustes problem, i.e. finding the optimal translation and rotation (SO3) that maps the marker’s centers from CAD to measured centroids. In effect, the robot knows the 3D pose of the docking station w.r.t. itself.

The isosceles arrangement also lets us efficiently identify the 3 markers, even in scenes with lots of retroreflective distractors. The algorithm I settled on is O(n^2), but through lazy evaluation, outperforms another O(n logn) algorithm I wrote. It works by finding the triangle’s “legs” (the 2 equal sides of an isosceles triangle) in one pass over i = 255 and then finding the triangle’s base in another pass over legs. Across the 3 homes I tested in, this detector exhibited a high true-positive accuracy on dozens of natural scenes. A particularly tough room was the bike room in my apartment complex since retroreflective vests happened to pass the isosceles test. It didn’t make it into the pipeline, but at one point, I trained a random forest on features like # points, covariance, RMS fit, etc. to classify candidate docks as true or false detections. In the end, an upright on the floor assumption ended up being the cheapest filter.

Pose estimation on registration alone is susceptible to artifacts like intensity blooming and interstitial noise. I ended up writing 7 different formulations of least squares/nonlinear optimization that takes in a variety of inputs (cropped point cloud, clustered marker points, plane fitting) and optimizes with various deadbands, regulators, and weightings. Then I manually annotated a dataset of point clouds to compare these optimizers. The best performing optimizer is what’s used in the autodocking procedure. One nice property of autodocking is that as the robot closes error w.r.t. the docking station, its view of the dock becomes cleaner and therefore its estimates get better.

With the docking station identified, we could technically stop here. Paired with a simple servo law and finite state machine, this is sufficient to dock and is what the previous version of autodocking did. However, this approach is limited: it knows where the dock is, but not what’s between the robot and the dock. If there’s something on the ground in its path, the robot would drive over it. So we build a local representation of the world by looking at a 2m radius around the robot. This map is 6x faster to create (thanks to Numba optimizations) compared to Nav2’s local map, which is fast enough to be used in the docking control loop.

The two lidars are mounted on the robot and can see its own base, so the shell shows up in the cloud as a ring of returns right where the robot is standing. Cropping out a cylinder around the base doesn’t clear it, because beams that graze the edge of the shell come back as points floating in midair well outside the base radius. Each lidar frame arrives organized into 128 rings, one per laser, so I walk along a ring in bearing order and look at the surface implied by each pair of neighboring points. If that surface sits at a very shallow angle to the beam, under about 30 degrees, it isn’t a real surface but the sensor skimming past an edge, so the point gets dropped. It removes about 800 of 115,000 points per lidar in 7ms, and the costmap comes out clean right up to the base.

With a docking goal and representation of obstacles around the robot, all that remains is closing the error to it and docking. For CPU-only docking, I used a simple SE2 servo law that brings the robot to a predock pose, followed by a low-stiffness translation that is guided by the dock’s guide rails into electrical contact. This servo law won’t run into obstacles but won’t attempt to travel around them.

For Stretch 4s with the Jetson installed, I implemented a sampling-based MPC controller that can fully dock the robot. It handles both obstacle avoidance and the contact interactions with the dock’s guide rails. My implementation is based on a popular control algorithm called MPPI. I wrote a CUDA kernel that parallelizes evaluation of thousands of sampled trajectories per second, allowing the controller to quickly explore a large space of possible future motions. What’s particularly impressive to me is that the model being used is a simple holonomic model; there’s no modeling of wheel slip, contact dynamics, etc., yet, paired with a stiffness schedule, the controller’s servoing-like error correction works well enough for docking on hardwood flooring, shag carpet, and other types of flooring.

To tell MPPI what a “good” trajectory looks like, I wrote a cost function consisting of individual terms expressed in the dock’s coordinate frame. The terms are 1) stay lined up with the dock’s centerline, 2) stop where the electrical contacts touch rather than short of them, and 3) treat the space around the guide rails as a keep-out that can only be exited by backing away from the dock. That last penalty is what stops the robot from taking the shortcut of sliding sideways onto the approach, which would roll a wheel over a rail. Something I found unintuitive was guessing at the right weights for these terms to cooperate, so I collected many scenes and had an idealized robot act out the maneuvers I wanted alongside the shortcuts I didn’t and compared their total cost directly.

I found having end-to-end latency (cloud in to control out) be as low as possible critical for the performance of the pipeline. I used Numba extensively throughout the codebase and have gotten it down to 40ms for CPU-only and 65ms for CPU+GPU docking. The following video shows the kind of artifacts I’d see when latency was >100ms:

Very excited to get this out there! Self-charging is an essential part of getting to unsupervised deployments, so I’m excited to see what experiments this enables for the Stretch community. The codebase is open-source and available; I’ve written the library as modules that can be reused in other projects. And last but not least, here’s a long cut compilation of the robot docking itself: