⚡ electrical engineering

Surgical Robot, Path Planning, Joint Space, Riemannian Manifolds

This paper proposes a Riemannian manifold-based path planning method in joint space for surgical robots that utilizes gradient descent to efficiently navigate non-concave abdominal surfaces and minimize joint angle movement compared to traditional position-space calculations.

Original authors: Yoshiki Yamamoto, Maina Sogabe, Shunichi Hirahara, Toshiki Kaisaki, Tetsuro Miyazaki, Kenji Kawashima

Published 2026-03-17

📖 5 min read🧠 Deep dive

CC BY 4.0

Original authors: Yoshiki Yamamoto, Maina Sogabe, Shunichi Hirahara, Toshiki Kaisaki, Tetsuro Miyazaki, Kenji Kawashima

Original paper licensed under CC BY 4.0 (http://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

Imagine you are trying to thread a needle while wearing thick, clumsy oven mitts, but the needle is actually a delicate surgical tool inside a patient's body. This is the challenge of Minimally Invasive Surgery (MIS). The surgeon makes a tiny hole (the "port") and inserts a robotic arm. The problem? The arm has to pivot around that tiny hole, and the inside of the body is a bumpy, crowded landscape full of soft organs that can't be touched.

If the robot moves too wildly, it might rip the tissue or get stuck in a weird angle. Traditionally, computers plan the robot's path by looking at the 3D space (like a map of the room). But this is like trying to navigate a maze by only looking at the walls from the outside; it's slow, computationally heavy, and often leads to awkward, jerky movements.

This paper proposes a clever new way to think about the problem: Stop looking at the room; look at the robot's joints.

Here is the breakdown of their solution using simple analogies:

1. The "Joint Space" vs. The "Room Map"

The Old Way (Position Space): Imagine you are trying to walk from your front door to the kitchen. You look at a map of the house, avoiding furniture. If the furniture is shaped weirdly (like a U-shape), the computer has to do a massive amount of math to figure out how to squeeze through.
The New Way (Joint Space): Instead of looking at the room, the computer looks at your body's joints (your knees, hips, and elbows). It asks: "How much do I need to bend my knee to avoid the chair?"
The Magic Trick: The authors realized that while the inside of the body (the organs) looks like a messy, concave cave (like a bowl), when you translate that shape into the language of the robot's joints, it often looks like a smooth, bumpy hill.

2. The Riemannian Manifold: The "Stretchy Trampoline"

The paper uses a fancy math term called a Riemannian Manifold. Think of this as a stretchy trampoline that the robot's joints are walking on.

In a normal room, walking in a straight line is easy.
On this trampoline, the fabric is stretched tight near the "danger zones" (the organs).
If the robot tries to walk too close to an organ, the trampoline fabric pulls it back, making that path feel "expensive" or impossible to walk on.
Because the "danger zones" in the joint-space trampoline look like smooth hills (non-concave), the robot can simply roll a ball down the hill (a method called gradient descent) to find the easiest, safest path. It doesn't need to search every nook and cranny; it just follows the slope.

3. The "Edge Cost" Function: The Toll Booth

To make sure the robot doesn't just take the shortest path but also the smoothest path, the authors invented a "Toll Booth" system.

Every time the robot considers taking a step, the computer checks two things:
1. Distance: How far is this step?
2. The "Toll": How close is this step to an organ?
If the step is near an organ, the "Toll" is huge. The robot will naturally choose a slightly longer path that keeps it far away from the organs, rather than a short path that risks a collision.
This also ensures the robot doesn't twist its joints into painful, awkward angles (reducing the "range of motion").

4. The Experiment: The "Bladder" Test

The researchers tested this on a fake surgery scenario (simulating a gallbladder or bladder removal).

The Setup: They had a robotic arm try to move from a starting point to a target organ inside a fake body cavity.
The Result:
- Old Method (Room Map): The robot took a path that required it to twist its joints wildly. It was like a contortionist trying to reach a cookie jar.
- New Method (Joint Space Trampoline): The robot found a path that was much smoother. It moved its joints less, kept a safer distance from the organs, and didn't have to twist itself into knots.

Why This Matters

Think of the difference between driving a car through a city using a GPS that only knows the streets (Old Method) versus a GPS that knows exactly how your car's suspension, steering, and engine work (New Method).

By planning the path based on the robot's joints rather than just the space, the robot:

Moves smoother: Less jerky motion means less stress on the patient's body.
Thinks faster: It's easier to roll a ball down a hill than to solve a complex maze.
Is safer: It naturally avoids the "danger zones" without needing to be told every single time.

In short, this paper teaches the robot to "feel" its way through the surgery by understanding its own body mechanics, rather than just staring at a map of the room. This could lead to robots that perform surgeries with the grace of a dancer rather than the clumsiness of a drunk uncle.

1. Problem Statement

Minimally Invasive Surgery (MIS) using robotic systems faces significant challenges in autonomous path planning:

Kinematic Constraints: Surgical robots operate through a fixed insertion port (trocar), creating a "remote center of mechanism" (RCM) constraint. The robot's movement is restricted to a cone-like volume, leading to complex joint angle limitations.
Obstacle Complexity: The abdominal cavity and organ surfaces are often non-concave (convex or mixed). Traditional path planning in position space (Cartesian space) struggles with these geometries, often requiring computationally expensive sampling methods (like PRM or RRT) or suffering from coarse collision checks.
Joint Motion Limits: Standard planners often generate trajectories that maximize joint movement, potentially causing mechanical stress, exceeding joint limits, or increasing the risk of collision with the port.

The core problem is to find an optimal, collision-free trajectory for the surgical robot that minimizes joint motion and computational cost while respecting the complex geometry of the abdominal cavity and organs.

2. Methodology

The authors propose a novel approach that shifts path planning from position space to joint space by modeling the configuration space as a Riemannian manifold.

A. Riemannian Metric Definition

Instead of treating joint space as a flat Euclidean space, the authors define a composite Riemann metric $G(q)$ that incorporates both kinematic constraints and obstacle avoidance.

Kinematic Term ( $G_q$ ): Derived from the forward kinematics of the 5-DOF holder arm. It accounts for the relationship between the three primary joints ( $q_1, q_2, q_3$ ) and the dependent joints ( $q_4, q_5$ ), effectively measuring the "distance" in joint space while considering the full 5-DOF configuration.
Obstacle Term ( $G_{obs}$ ): A barrier function that imposes an infinite cost as the robot configuration approaches the forbidden boundary ( $\partial Q_a$ ). This is defined as:
$G_{obs}(q) = \frac{\sigma}{\|q - q_a\|} I$
where $q_a$ is the closest point on the obstacle boundary in joint space.

B. Obstacle Mapping in Joint Space

A critical insight of the paper is the geometric transformation of obstacles:

Position Space: Organ surfaces and cavity walls can be concave, making gradient-based optimization difficult (prone to local minima).
Joint Space: Through the mapping of the robot's kinematics, these concave obstacles in position space often transform into non-concave (convex) surfaces in joint space.
Algorithm: The authors approximate the collision boundary in joint space using Delaunay triangulation. They partition the boundary into convex patches. For any given configuration, the closest obstacle point is found efficiently using a greedy search on these convex patches, rather than a full global search.

C. Path Planning Algorithm

Graph Construction: A sparse graph is created by sampling collision-free configurations ( $V_q$ ) in joint space.
Edge Cost Function: The cost of an edge between two nodes is calculated using the Riemann metric and a penalty for proximity to obstacles:
$c(q_\alpha, q_\beta) = \sqrt{(q_\beta - q_\alpha)^T G(\bar{q}) (q_\beta - q_\alpha)} \left[ 1 + \frac{\sigma_q}{\|\bar{q} - q_a(\bar{q})\|^2} \right]$
Search: Dijkstra's algorithm is used to find the minimum-cost path on the graph.
Smoothing: The discrete path is converted into a smooth continuous trajectory using cubic spline interpolation.

3. Key Contributions

Riemannian Manifold Formulation: The paper introduces a method to transform position space into a Riemannian manifold in joint space, allowing for the definition of a geodesic path that naturally respects mechanical constraints.
Convexity in Joint Space: The authors demonstrate that concave obstacles in the physical abdominal cavity often become convex (or combinations of convex surfaces) in the robot's joint space. This property allows for the use of efficient gradient descent and greedy search methods, significantly reducing computational complexity compared to position-space planning.
Edge Cost Function: A specific cost function is defined that balances the shortest path in joint space (minimizing motion) with a barrier penalty to prevent collisions.
Experimental Validation: The method is validated on simulated surgical scenarios (cholecystectomy and prostatectomy) using a 5-DOF holder robot, comparing performance against traditional position-space planning.

4. Experimental Results

The proposed method was tested on a 5-DOF surgical robot arm using mimic organs (gallbladder and bladder) within a concave body cavity.

Geometric Transformation: Experiments confirmed that concave organ shapes in position space (e.g., the bladder) map to convex shapes in joint space, validating the theoretical premise.
Performance Metrics: The proposed method was compared against position-space planning using three indices:
- Mean Insertion Angle ( $\psi_{ave}$ ): The proposed method achieved 31.8° vs. 39.5° (Position Space).
- Max Insertion Angle ( $\psi_{max}$ ): The proposed method achieved 40.3° vs. 47.7°.
- Rate of Change ( $\dot{\psi}_{RMS}$ ): The proposed method achieved 0.214 deg/mm vs. 0.256.
Interpretation: Lower angle values indicate that the robot arm stays more vertical relative to the body wall, reducing torsion forces on the trocar port and minimizing the range of joint motion. The proposed method successfully generated trajectories with reduced joint motion and lower stress on the insertion port.

5. Significance and Future Work

Significance: This work provides a theoretical and practical framework for semi-autonomous surgical robots. By leveraging the geometric properties of joint space, it solves the "curse of dimensionality" and non-convexity issues common in traditional motion planning. The reduction in joint motion is crucial for patient safety (reducing tissue trauma at the port) and robot longevity.
Future Work: The authors plan to:
- Clarify the necessary conditions for convex representation in joint space for more complex obstacles.
- Develop collision avoidance methods beyond spline interpolation (e.g., dynamic obstacle avoidance).
- Address interference between multiple robot arms (multi-robot coordination).
- Apply the method to actual intracorporeal environments with highly complex, non-standard shapes.

In conclusion, the paper successfully demonstrates that transforming the path planning problem into a Riemannian manifold in joint space offers a computationally efficient and mechanically superior solution for MIS robots, enabling safer and more autonomous surgical procedures.