DynoJEPP: Joint Estimation, Prediction and Planning in Dynamic Environments

TL;DR

DynoJEPP is a novel factor-graph framework that improves safety in dynamic environment navigation by controlling information flow, and it extends to cooperative scenarios.

Contribution

It introduces directed factors to prevent estimation corruption and enables cooperative object behavior modeling in trajectory planning.

Findings

Directed factors are crucial for safe navigation, preventing collisions.

Without them, robots frequently collide in experiments.

Cooperative DynoJEPP enhances prediction and planning with cooperative behaviors.

Abstract

DynoJEPP is a factor-graph-based framework that jointly formulates and simultaneously optimizes estimation, prediction, and planning in dynamic environments. In conventional factor-graph-based approaches that jointly formulate estimation, prediction, and planning, information from prediction and planning feeds back into state estimation, yielding corrupted estimates, undesired behaviors, and unsafe plans. To address this, DynoJEPP introduces a novel directed factor that enforces directional information flow within the factor graph, preventing prediction and planning from corrupting state estimation. We evaluate the impact of directed factors on inter-module interactions during navigation in both static and dynamic environments. Our results demonstrate that these factors are critical for safe operation, as without them, the robot collides in the majority of experiments. Building on this, we further introduce Cooperative DynoJEPP, which enables the ego robot to incorporate cooperative object behavior into its prediction and trajectory planning.