Look as You Leap:
Planning Simultaneous Motion and Perception for High-DoF Robots
Qingxi Meng and Emiliano Flores and Carlos Quintero-Peña and Peizhu Qian and Zachary Kingston and Shannan K. Hamlin and Vaibhav Unhelkar and Lydia E. Kavraki
IEEE Transactions on Robotics (T-RO), 2026
 

Supplementary video (2:34) — all simulation and real-robot experiments. Watch on YouTube ↗

 

Abstract: — Most common tasks for robots in dynamic spaces require that the environment is regularly and actively perceived. The perception task considered in this work can represent a broad range of robot perception objectives, including object detection, human activity recognition, and human face detection. For example, a service robot may need to continuously detect and localize a target object during manipulation, while an assistive robot in a healthcare setting may need to maintain reliable perception of a human face or activity for interaction and safety monitoring. These tasks impose perception constraints on the robot motion. However, solving motion and perception tasks simultaneously is challenging, as these objectives often impose conflicting requirements. Furthermore, while robots must react quickly to changes in the environment, directly evaluating the quality of perception (e.g., object detection confidence) is often expensive or infeasible at runtime. This problem is especially important in human-centered environments, such as homes and hospitals, where effective perception is essential for safe and reliable operation. In this work, we address the challenge of solving motion planning problems for high-degree-of-freedom (DoF) robots from a start to a goal configuration with continuous perception constraints under both static and dynamic environments. Our solution is a GPU-parallelized perception-score-guided probabilistic roadmap planner with a neural surrogate model (PS-PRM). Unlike existing active perception-, visibility-aware or learning-based planners, our work jointly considers perception tasks and constraints when searching for a solution to the motion planning problem. Our method uses a neural surrogate model to approximate perception scores, incorporates them into a roadmap-based solution, and leverages GPU parallelism to enable efficient online replanning in dynamic settings. We demonstrate that our planner, evaluated on high-DoF robots, outperforms RL- and trajectory-optimization-based baseline methods in both static and dynamic environments in both simulation and real-robot experiments.



The paper can be found on IEEE Xplore and arXiv.  

The authors are affiliated to the Department of Computer Science at Rice University, Houston, TX, USA. Vaibhav Unhelkar and Lydia E. Kavraki are also with the Ken Kennedy Institute, Rice University. Shannan K. Hamlin is with the Houston Methodist Academic Institute, Houston, TX, USA. Peizhu Qian is currently with the University of Houston. Zachary Kingston is currently with Purdue University.  
Corresponding author can be contacted at qm15 (at) rice (dot) edu. This work was supported in part by NSF 2326390 and NSF CCF-2336612.


Motivating Example

Fig 1: A UR5 on a differential-drive base moves through a cluttered environment while tracking a monitor with its wrist-mounted camera. Top: the robot's trajectory (fading right to left) as it maintains a high detection rate. Bottom: camera views for the configurations shown above.

 

Method: — PS-PRM builds a probabilistic roadmap on a manifold defined by object-centered visibility constraints, scores every configuration with a learned neural surrogate model, refines those scores with ray-cast occlusion checking, and searches for the path that best trades off motion cost against perception quality. Because the surrogate is defined over the camera-to-target transform in SE(3) rather than over robot joint values, it is robot- and environment-agnostic and can be reused across platforms without retraining.

Camera Model

Fig 2: The camera model: line of sight (orange) and camera frustum (purple), shown against a cup intersecting each. These two visibility constraints define the manifold within the robot's free configuration space on which PS-PRM samples.

 
PS-PRM Pipeline

Fig 3: The PS-PRM pipeline. Bottom: environment with candidate configurations. Middle: PRM generation, neural surrogate scoring, and occlusion checking run in parallel to select the optimal path. Top: camera views confirming differing perception scores.

 
Neural Surrogate and Occlusion Checking

Fig 4: Perception-aware evaluation via neural surrogate model and occlusion-aware ray tracing. Left to right: environment with target (human) and occluder (orange box); surrogate-predicted perception score field; ray-traced occlusion refinement; camera views at points A and B, showing lower scores under occlusion.

 
GPU-Parallelized Scoring

Fig 5: Parallel perception-score evaluation for a batch of robot configurations. Camera poses are computed via forward kinematics; occlusion checking and neural surrogate scoring run concurrently; outputs combine into a final perception score (0–1) per configuration. Batching this pipeline on the GPU is what enables online replanning in dynamic scenes.

 

Simulation benchmark: — Planning and perception results for the Stretch robot in a simulated home environment, averaged over 2,500 planning problems. Both PS-PRM variants and their neural-surrogate counterparts reach detection rates above 70%, confidence scores above 0.75, and tracking rates around 90%, while the neural surrogate cuts preprocessing time by roughly 10× at comparable perception quality.

Method Motion Planning Object Tracking
Preprocess Time (s) Plan. Time (s) Total Time (s) Path Len. (rad) Succ. Rate Det. Rate Conf. Score Track Rate
Post0.0510.0516.27799.7%20.9%0.53157.9%
Rejection1.2641.26410.37998.9%29.7%0.59974.0%
Manifold-frustum0.4950.49511.14198.8%41.0%0.59780.9%
Manifold-line2.8592.8597.47890.1%48.9%0.48267.4%
Visibility-Aware TrajOpt0.1520.1525.6293.0%35.7%0.56164.4%
RL-Based Active Perception169.80.404170.20421.7557.3%75.5%0.63480.0%
PS-PRM-frustum291.70.014291.71416.68896.4%73.7%0.76189.8%
PS-PRM-line341.50.018341.51815.00795.7%80.1%0.79291.0%
Neural-PS-PRM-frustum32.080.01732.09715.81497.1%72.8%0.75989.8%
Neural-PS-PRM-line31.790.02131.81113.51896.3%79.6%0.77190.4%

Table 1: Planning and perception results for the Stretch robot in a simulated home environment, averaged over 2,500 planning problems. "Preprocess/Plan./Total Time" are pre-planning, planning, and combined durations; "Path Len." and "Succ. Rate" are motion planning metrics; "Det. Rate," "Conf. Score," and "Track Rate" are object-tracking metrics.

 
Simulated Trajectory

Fig 6: Example PS-PRM trajectory in the simulated home environment. Robot traces and camera images are sampled along the path, fading in as the robot nears the goal. Snapshots show YOLO confidence scores and ground-truth bounding boxes. The planner selects a trajectory that stays slightly farther from the monitored object, trading a marginally longer path for a better viewing angle.

 
Baseline Comparison

Fig 7: Qualitative comparison in the simulated home environment. Each row shows one method's trajectory (left) with the camera views sampled along it (right): Post keeps the TV in view only intermittently, Manifold-frustum improves coverage but at low confidence, and PS-PRM-line maintains detection with consistently higher confidence scores throughout the motion.

 

Video 1: Simulated home environment. The Post and Manifold-Frustum baselines lose the TV from view or hold it at low confidence; PS-PRM-Line plans against the predicted perception score and improves the detection rate by 1.8×. Watch on YouTube ↗

 

Human tracking in a nursing setting: — A Fetch robot plans from a start to a goal configuration while continuously detecting the faces of two masked nurses, using RetinaFace confidence as the perception score. The midpoint between the two people is used as the monitoring point.

Nursing Environment

Fig 8: Simulated nursing environment: the robot plans start-to-goal while tracking both masked nurses. Manifold-line takes a shorter path (orange) that passes behind the nurses and yields poor visibility; Neural-PS-PRM-line takes a longer, more cluttered path (blue) that keeps both faces in view.

 
Method Motion Planning Human Tracking
Plan. Time (s) Path Len. (rad) Det. Rate (1 face) Det. Rate (2 faces) Conf. Score
Manifold-line5.316.8931.53%0.00%0.49
Neural-PS-PRM-line32.9111.5198.40%63.29%0.81

Table 2: Comparison of Manifold-line and Neural-PS-PRM-line in a simulated nursing environment. "Plan. Time" includes PRM construction for Neural-PS-PRM-line. "Det. Rate (1/2 face)" is the detection rate for one or both nurses; "Conf. Score" is the average face-detection confidence.

 

Video 2: Simulated nursing environment. Manifold-Line passes behind the nurses and loses their faces; Neural-PS-PRM-Line takes a longer route that keeps both faces in view, improving the face detection rate by 2.1×. Watch on YouTube ↗

 
Conflicting Targets

Fig 9: Distance-based priority under conflicting perception objectives: with the two nurses spatially separated, high-quality perception of both at once is infeasible, so the Stretch robot shifts attention from Nurse 1 (green) to Nurse 2 (blue) as it moves left to right.

 
Narrow Passage

Fig 10: Planning through a narrow passage: the distance-based priority metric shifts which object the camera focuses on as the robot crosses, while sampling-based planning solves the narrow-passage motion.

 

Video 3: Planning across two rooms with two candidate targets. The distance-based priority metric hands attention from the first object to the second as the robot passes through the narrow passage. Watch on YouTube ↗

 

Planning in a dynamic environment: — The robot must reach one of two goal locations while continuously detecting a human face. As the person turns, PS-PRM re-scores the roadmap on the GPU and replans online, switching goals to preserve visibility.

Dynamic Planning

Fig 11: Dynamic planning for human face detection: as the human rotates, the Stretch 2 robot replans from the shelf (Goal 1, orange) to the cabinet (Goal 2, blue) to preserve visibility. Neural-PS-PRM-line's online replanning achieves much higher detection rates than static planning.

 
Method Motion Planning Face Detection
Plan. Time (s) #Plans Path Len. (rad) Det. Rate Conf. Score
Manifold-line0.1715.4874.13%0.55
Neural-PS-PRM-line2.492.1210.5390.53%0.73

Table 3: Comparison of Manifold-line and Neural-PS-PRM-line in a dynamic, human-aware scenario. "#Plans" is the average number of replans during execution; "Det. Rate" and "Conf. Score" are face-detection rate and average confidence.

 

Real-robot experiments: — We validate PS-PRM on Hello Robot's Stretch 2 and a UR5 manipulator across four real-world scenarios: static object tracking, dynamic human tracking in a nurse training setting, human tracking with a moving obstacle and a moving goal, and a grasping task constrained by human activity recognition.

Real Robot Cup Detection

Fig 12: Real-robot cup-detection experiment: motion paths for Manifold-line (orange) and PS-PRM-line (blue), with camera views for each shown left and right. The left path is shorter but the cup is occluded by the monitor; PS-PRM-line takes the longer route around the table and keeps the cup detected in significantly more frames.

 

Video 4: Real-robot occlusion test. Manifold-Line takes the short path and loses the cup behind the monitor; PS-PRM-Line takes the longer unobstructed route, improving the detection rate by 2.1× by avoiding occlusion. Watch on YouTube ↗

 
Real Robot Nurse Tracking

Fig 13: Real-robot face-detection experiment in a nurse training scenario: as the nurse rotates, the Stretch 2 robot replans from the left cabinet (Goal 1, orange) to the right cabinet (Goal 2, blue) to preserve visibility, using Neural-PS-PRM-line's online replanning.

 

Video 5: Real-robot dynamic human tracking in a nurse training scenario. The robot starts toward the left cabinet, then replans online to the right cabinet as the nurse turns, selecting the goal that preserves visibility of her face. Watch on YouTube ↗

 
Moving Obstacle and Goal

Fig 14: Dynamic human, obstacle, and goal movement: the Stretch 2 robot (blue) switches paths as the person rotates (B), replans around a moved obstacle (C), and replans again as the goal cabinet moves (D). The planner reaches execution rates of up to 2 Hz and generates an average of 8 trajectories per run, more than the 3 environment changes.

 

Video 6: Real-robot replanning under three separate disturbances: the person rotates, a cabinet is moved into the robot's path mid-execution, and the goal cabinet itself is relocated. Poses are tracked with an OptiTrack system. Watch on YouTube ↗

 
UR5 Activity Recognition

Fig 15: Perception-constrained planning for human activity recognition on a 6-DoF UR5. Without perception costs (left, short path), the person is largely occluded by the wooden board and the drinking action is missed; the Neural-PS-PRM trajectory (right) detours above the board to maintain visibility and recovers the full activity timeline. Trajectories are dotted, red when occluded and green when visible.

 

Video 7: PS-PRM on higher-DoF systems. First the 9-DoF mobile manipulator of Fig 1 tracking a monitor through the blind-spot alley, then the 6-DoF UR5 maintaining human activity recognition around an occluding board. Watch on YouTube ↗

 

Rice University    Houston    TX    USA