⚡ electrical engineering

HiCART-OMP: Human-inspired Collision Aware Trace Following Real Time and Online Motion Planner

This paper presents HiCART-OMP, a real-time, online motion planner that integrates human-inspired trace following with predictive multi-trajectory rollout to enable robots to safely and efficiently navigate dynamic, unstructured environments by decoupling end-effector path convergence from whole-body obstacle avoidance.

Original authors: Emilio Maranci, Salvatore D’Avella

Published 2026-08-26

📖 5 min read🧠 Deep dive

CC BY 4.0

Original authors: Emilio Maranci, Salvatore D’Avella

Original paper licensed under CC BY 4.0 (https://creativecommons.org/licenses/by/4.0/). ✨ This is an AI-generated explanation of the paper below. It is not written or endorsed by the authors. For technical accuracy, refer to the original paper. Read full disclaimer

Robots have long been masters of the factory floor, executing the same precise, repetitive motions in environments that never change. But as the vision of Industry 4.0 and 5.0 brings machines into shared spaces with people, the rules of engagement must change. In a world where humans move unpredictably and objects shift without warning, a robot cannot rely on a pre-written script. It must be able to see a new obstacle, decide in a split second how to move around it, and still arrive at its destination with the exact orientation needed to pick up a cup or tighten a screw. This requires a delicate balance: the robot must be reactive enough to dodge a sudden movement, yet smart enough to plan a path that doesn't get stuck in a dead end. For years, engineers have struggled to find a method that combines the speed of a reflex with the foresight of a map, often forcing a choice between safety and efficiency.

A team of researchers at the Scuola Superiore Sant'Anna in Pisa has developed a new system designed to solve this exact problem. They call it HiCART-OMP, a motion planner that allows a robot arm to navigate complex, changing environments in real time. The core idea behind their work is surprisingly human. When a person reaches for an object on a crowded shelf, they do not calculate a single, rigid line from their hand to the object. Instead, they form a general idea of the path, then continuously adjust their arm as they move, weaving around a coffee mug or a stack of books while keeping their eyes on the prize. The researchers translated this intuitive behavior into a mathematical framework that guides a robot's end-effector—the tool at the tip of the arm—along a flexible path that reshapes itself as the world changes.

The system works by first drawing a straight line in space from where the robot's hand currently is to where it needs to go. This line is not a rigid track but a living path made of many small points. As the robot moves, an external camera scans the environment and builds a digital map of everything it sees, including static furniture and moving people. If the system detects an obstacle near the path, it applies a virtual repulsive force that pushes the path points away, bending the route around the danger. Crucially, the robot's hand is guided to follow this bending line, while the rest of the robot's body is simultaneously steered away from collisions using a separate set of rules. This separation allows the hand to stay on course even as the robot's joints twist and turn to avoid hitting its own arm or nearby objects.

What sets this approach apart is how it thinks ahead. While many reactive systems only look at the immediate next step, HiCART-OMP constantly simulates multiple future possibilities. It generates several alternative paths that branch off from the current route, testing each one to see which would lead to the goal most safely and quickly. It evaluates these options based on how long the path is, how close it gets to obstacles, and whether the robot's joints are near their physical limits. If one of these alternative paths looks better than the current one, the system smoothly switches to it. This predictive capability prevents the robot from getting trapped in local dead ends, a common failure point for simpler systems that might push a robot into a corner and leave it unable to back out.

The researchers tested their method in both computer simulations and on real robots, including a seven-jointed arm from Franka Emika and a six-jointed arm from Doosan. They pitted their system against other state-of-the-art planners in scenarios designed to be difficult, such as navigating through narrow gaps between shelves or dodging moving objects that swing through the workspace. In static environments filled with tight constraints, the new planner succeeded in reaching the target every time, outperforming other methods that frequently got stuck or collided with themselves. In dynamic environments where objects were moving, the system achieved a success rate of nearly 86 percent, significantly higher than its competitors. While other systems often failed because they hesitated too long near obstacles or timed out while trying to find a way through, HiCART-OMP moved with a fluid confidence, often completing tasks in about three seconds less time than the others.

A key to this performance is how the system handles the robot's speed and safety. The researchers designed a special control mechanism that acts like a brake, automatically slowing the robot down as it gets closer to an obstacle. This ensures that even if the robot must pass very near a moving object, it does so with a controlled, gentle approach rather than a risky dash. Furthermore, the system prioritizes getting the hand to the correct location first, and only focuses on the precise angle of the hand once it is safely near the target. This allows the robot to be flexible and agile during the journey, only locking into a rigid final pose when it is safe to do so.

The results suggest that by mimicking the way humans plan and adjust their movements, robots can operate more safely and efficiently in unstructured spaces. The system does not require prior knowledge of the objects in the room; it builds its understanding of the world from scratch using camera data, refreshing its map sixty times a second. This allows it to react to changes as they happen, making it suitable for environments where people and machines work side by side. While the tests were conducted in controlled settings, the ability to generate smooth, collision-free motions at a speed of one thousand times per second indicates a significant step forward in making robots truly adaptable partners in the real world.

Technical Summary: HiCART-OMP

Problem Statement

Robots operating in human-centered, unstructured environments face a fundamental trade-off in motion planning: local reactive planners are responsive to dynamic changes but prone to local minima and myopic decisions, while long-horizon global planners offer optimality but suffer from prohibitive computational costs that hinder real-time responsiveness. Existing methods often struggle to balance global path optimality with local reactivity to dynamic obstacles, particularly when precise task-space accuracy and whole-body safety are required simultaneously. Furthermore, conventional artificial potential field (APF) methods frequently fail to reach target positions with adequate precision (GNRON problem) or get trapped in configurations where repulsive forces prevent convergence (RLMP problem).

Methodology

The proposed framework, HiCART-OMP (Human-inspired Collision Aware Trace Following Real Time and Online Motion Planner), addresses these challenges by combining local obstacle-driven path deformation with predictive multi-trajectory rollout. The approach is grounded in a human-motion model based on via postures and hierarchical task constraints.

Core Components

Cartesian Path Generation and Deformation:
- The planner initiates a straight-line path in Cartesian space from the current end-effector pose to the goal.
- Signed Distance Field (SDF): An external RGB-D camera (60 Hz) updates an Octomap, which is converted into an SDF. This provides real-time distance and gradient information regarding obstacles.
- Dynamic Path Shaping: The path is treated as a series of waypoints subject to two forces:
  - Repulsive Force ( $F_r$ ): Derived from the SDF gradient, pushing waypoints away from obstacles.
  - Elastic Force ( $F_e$ ): Acts between adjacent waypoints to maintain path smoothness and prevent "repulsion explosion."
- The number of waypoints is variable, allowing the path to expand or contract dynamically to circumvent obstacles while preserving smoothness.
Trajectory Rollout (Predictive Evaluation):
- To overcome local minima, the system generates $N_p$ alternative trajectories asynchronously.
- Deformation Primitives: Starting from a specific waypoint on the current path, a semicircular deformation primitive is applied, followed by scaling and rotation transformations around the axis pointing to the goal.
- Evaluation: Each candidate path is simulated over a long prediction horizon using the actual control law. A cost function evaluates paths based on success, path length, and minimum distance to obstacles. The best path is smoothly merged with the current trajectory.
Unified Control Law:
- Trace Controller: A custom closed-form control law ensures the end-effector converges to the dynamically generated path. It uses a polynomial approximation of the path and a Lyapunov-based stabilizing term.
- Task Prioritization: The control framework decouples end-effector convergence (primary task) from whole-body obstacle avoidance and orientation regulation (secondary tasks in the null space).
- Adaptive Orientation Weighting: The influence of the orientation task is modulated by an exponential factor based on the distance to the goal. Orientation is relaxed during navigation to facilitate obstacle avoidance and progressively enforced near the target.
- DCALS Pseudoinverse: A custom "Damped Collision-Aware Least Squares" pseudoinverse is employed. This modifies the standard damped least squares solution to explicitly penalize approaching velocities toward obstacles, ensuring safe, damped behavior near singularities and obstacles.
Real-Time Execution:
- The system operates at 1 kHz for velocity commands.
- The SDF is updated at 60 Hz.
- Trajectory rollout runs in parallel threads (up to 10 kHz control step simulation) without blocking the main execution.

Key Contributions

The paper identifies four primary contributions:

Human-Inspired Reformulation: A Cartesian reformulation of a human-motion model (via postures and hierarchical constraints) specifically adapted for reactive robot motion planning.
Unified Predictive-Reactive Strategy: Integration of SDF-based local path deformation with long-horizon trajectory rollout, allowing the planner to evaluate multiple candidate trajectories in real-time.
Decoupled Control Framework: A task-priority controller that guarantees end-effector convergence to the path while steering remaining links away from obstacles, overcoming the GNRON and RLMP limitations of traditional APF methods.
Safety Enhancements: The introduction of a customized DCALS pseudoinverse that reduces approach velocity near obstacles and includes emergency stopping conditions, alongside adaptive orientation weighting to ensure precise goal regulation.

Experimental Results

The method was validated in simulation (Gazebo) and on real robots (Franka Emika Panda and Doosan M0609) using the MotionBenchMaker suite and custom dynamic scenarios.

Static Environments: In the MotionBenchMaker "Bookshelf" and "Table Pick" scenarios, HiCART-OMP achieved a 100% success rate on Bookshelf and 75% on Table Pick, outperforming CHOMP, QPIK, and other baselines. It demonstrated superior ability to navigate tight clearances without self-collision.
Dynamic Environments: In nine dynamic scenarios involving moving obstacles, HiCART-OMP achieved an 85.71% success rate, outperforming STORM (78.57%) and RAMP (78.57%).
- Efficiency: HiCART-OMP completed tasks significantly faster (average 15.45s vs. ~18s for competitors) and followed shorter Cartesian paths (1.38m vs. 2.18m for RAMP).
- Safety: While RAMP maintained a larger average minimum distance to obstacles, HiCART-OMP operated safely in closer proximity without compromising task completion. Competitors frequently failed due to timeouts caused by oscillating near goals due to strong repulsive forces, whereas HiCART-OMP's DCALS pseudoinverse allowed it to dampen approach velocities and converge.

Significance and Claims

The authors claim that HiCART-OMP successfully bridges the gap between global optimality and local reactivity. By decoupling the end-effector's path convergence from the robot's whole-body obstacle avoidance, the method preserves task-space accuracy while maintaining safety in unstructured environments.

The paper asserts that the proposed framework overcomes the local minima and convergence failures typical of conventional potential field methods. The integration of predictive trajectory rollout allows the planner to anticipate future collisions and switch to safer paths, while the adaptive orientation weighting ensures that the robot can navigate complex, narrow spaces without being hindered by premature orientation constraints. The results suggest that the method provides a robust, real-time solution for collaborative robotics where safety, efficiency, and adaptability are paramount.