Path Planning Tolerant to Degraded Locomotion Conditions

Xiaoling Long, Sören Schwertfeger

This paper, presented at the IEEE International Symposium on Safety, Security, and Rescue Robotics (SSRR) 2019, tackles a common but often overlooked problem in mobile robotics: what happens when the robot’s hardware starts to fail?

TL;DR

Standard path planners assume a robot is in perfect health, but real-world conditions like flat tires or heavy payloads can make planned trajectories impossible to follow. This paper introduces an adaptive planning framework that learns how a robot actually moves in its current state by clustering real-time SLAM data into "motion primitives," allowing the robot to navigate successfully even under degraded locomotion conditions.

What Problem Does This Paper Address?

In ideal laboratory settings, a robot's wheel odometry is a reliable source of truth. However, in search and rescue or unstructured outdoor environments, things go wrong. Tires lose pressure, axes become loose, tracks de-track, or a heavy payload shifts the center of gravity.

Traditional navigation stacks (like the standard ROS move_base) are often "oblivious" to these changes. They issue commands based on a theoretical model of the robot. If a robot has a flat left tire, a command to "go straight" might result in a veer to the left. Because the wheel encoders might still show the wheels turning at the correct speed, the robot doesn't realize it's off-course until it potentially hits an obstacle or fails its mission. The authors aim to bridge this gap by making the path planner aware of the robot's actual, current physical capabilities.

Methods

The authors propose a three-step adaptive loop:

  1. Motion Primitive (MP) Generation: Instead of relying on a mathematical kinematic model, the system records actual robot motions. By pairing control inputs (velocity commands) with the resulting path (measured via SLAM rather than wheel odometry), the robot builds a library of what it is actually capable of doing.
  2. Clustering: To keep the computational load manageable, they use Affinity Propagation to cluster hundreds of recorded motions into a few dozen representative "Motion Primitives."
  3. MP-Based Planning: They use an A* search algorithm where the edges of the search graph are the learned motion primitives. This ensures that any planned path is, by definition, something the robot has recently proven it can execute.
  4. The Adaptive Feedback Loop: As the robot executes a plan, it constantly evaluates its performance. If the robot fails to reach the expected pose of a specific MP, that primitive is "downvoted" via a voting system. If the robot's locomotion changes (e.g., a tire pops mid-mission), the system detects the inconsistency, downvotes the old MPs, and incorporates new ones that reflect the current state.

Datasets

The researchers evaluated their approach using a Clearpath Jackal mobile robot equipped with a Velodyne VLP16 LIDAR and an IMU.

  • Environment: A 10m x 10m laboratory environment.
  • Baseline Data: They teleoperated the robot for 200 seconds to generate an initial library of 138 motion primitives, which were then clustered down to 40.
  • Localization: They used the BLAM (Berkeley Localization and Mapping) package for LIDAR-based SLAM to provide a ground truth for the robot's position that is independent of wheel odometry.

Key Insights and Ideas

  • SLAM as the Truth Source: The core "aha moment" is using SLAM to validate odometry. By comparing where the wheels say the robot is to where the LIDAR sees the robot is, the system can quantify locomotion degradation.
  • Kinodynamic Awareness without Manual Modeling: Modeling the physics of a "loose axis" or "unevenly pressurized tire" is incredibly difficult. By using a data-driven MP approach, the robot doesn't need to understand why it is veering; it only needs to know that "Command X results in Path Y."
  • Heuristic Punishment: The authors designed a heuristic that assigns higher costs to cells where the robot has previously failed. This encourages the planner to explore obstacle-free paths that are more compatible with the robot's current physical limitations.

Key Results

The authors compared their method against the standard ROS move_base (using the Dynamic Window Approach, or DWA). They tested four scenarios: Normal, Right Constrained (simulated failure), Left Constrained, and Overload.

  • Success Rate: In all degraded conditions (Right/Left Constrained and Overload), the standard move_base failed to reach the goal. The authors' adaptive method succeeded in every test.
  • Accuracy: In the "Right Constrained" test, the standard navigator ended 3.54 meters away from the goal. The proposed method reached within 0.73 meters.
  • Balanced vs. Unbalanced Degradation: The system proved effective not just for "veering" (unbalanced) but also for "sluggishness" (balanced degradation caused by overloading), where the robot simply cannot reach the speeds the planner expects.

Limitations and Future Work

  • Bootstrapping: The current system requires an initial period of teleoperation or "normal" navigation to build the MP library.
  • Computational Overhead: Planning in an MP-based search space is more complex than simple grid-based planning. While they used a penalty coefficient ($\beta$) to speed up the search, it may still struggle in very large, complex environments.
  • Fully Online Learning: The authors noted that moving toward a 100% online clustering system—where the robot learns entirely on the fly without a pre-obtained library—is the next logical step for true autonomy in unknown environments.

Summary updated

Abstract

Mobile robots, especially those driving outdoors and in unstructured terrain, sometimes suffer from failures and errors in locomotion, like unevenly pressurized or flat tires, loose axes or de-tracked tracks. Those are errors that go unnoticed by the odometry of the robot. Other factors that influence the locomotion performance of the robot, like the weight and distribution of the payload, the terrain over which the robot is driving or the battery charge could not be compensated for by the PID speed or position controller of the robot, because of the physical limits of the system. Traditional planning systems are oblivious to those problems and may thus plan unfeasible trajectories. Also, the path following modules oblivious to those problems will generate sub-optimal motion patterns, if they can get to the goal at all. In this paper, we present an adaptive path planning algorithm that is tolerant to such degraded locomotion conditions. We do this by constantly observing the executed motions of the robot via simultaneously localization and mapping (SLAM). From the executed path and the given motion commands, we constantly on the fly collect and cluster motion primitives (MP), which are in turn used for planning. Therefore the robot can automatically detect and adapt to different locomotion conditions and reflect those in the planned paths.
BibTeX
@inproceedings{long2019path,
  author = {Xiaoling Long and Sören Schwertfeger},
  title = {{Path Planning Tolerant to Degraded Locomotion Conditions}},
  booktitle = {SSRR},
  year = {2019},
  url = {https://openreview.net/forum?id=IqsbfMjXQo},
}

Loading related content...

Figure