DynoJEPP: Joint Estimation, Prediction and Planning in Dynamic Environments
This work addresses the problem of information flow corruption in joint estimation, prediction, and planning for autonomous robots, offering a solution that prevents unsafe behaviors in dynamic environments.
DynoJEPP introduces a factor-graph-based framework with directed factors to prevent prediction and planning from corrupting state estimation in dynamic environments. Without these directed factors, robots collide in the majority of experiments, demonstrating their critical role for safe navigation.
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.