State Machine in EGO-Planner

Finite state machine in EGO-Planner

EGO-Planner / EGO-Swarm

Original version with B-spline trajectory optimization

FSM

states:

INIT
WAIT_TARGET
SEQUENTIAL_START
GEN_NEW_TRAJ
REPLAN_TRAJ
EXEC_TRAJ
EMERGENCY_STOP

graph TD
 INIT --> force_return
 INIT --> WAIT_TARGET
 WAIT_TARGET --> force_return
 WAIT_TARGET --> SEQUENTIAL_START
 SEQUENTIAL_START --> EXEC_TRAJ
 SEQUENTIAL_START --> SEQUENTIAL_START
 GEN_NEW_TRAJ --> EXEC_TRAJ
 GEN_NEW_TRAJ --> GEN_NEW_TRAJ
  REPLAN_TRAJ --> EXEC_TRAJ
 REPLAN_TRAJ --> REPLAN_TRAJ
 EXEC_TRAJ --> WAIT_TARGET
 EXEC_TRAJ --> REPLAN_TRAJ
 EXEC_TRAJ --> force_return
 EMERGENCY_STOP --> GEN_NEW_TRAJ
 A([collision]) --> EMERGENCY_STOP

Single agent Replan

collision occurs
reach the goal or beyond the replan threshold
after waiting 0.8s for emergency stop

Multi-agent Replan

broadcast a given strategy at a given frequency
check collision as soon as traj received from broadcast network
generate a new collision free trajectory when collision detected => due to latency or packet losses*

Deadlock is not under consideration

EDG-TEAM

Joint optimize all trajectories => consider influence of the current agent on all trajectory gradients
Grouping mechanism
- Based on Euclidean distance (4.0 meters)
- If grouping: ECBS(centralized, computed on min-index-agent in the group) + MINCO Opt
- If not grouping: Single-agent Planning

How to share map?

share state, including
- start p, v, a;
- target p, v, a;
- local map bound min and max;
- map data
How to share trajectory?
1. publish to board cast network /boardcast_traj_from_planner, receive message from /boardcast_traj_to_planner
How to optimize among all these trajectories?

run a group optimization on one single agent, design joint cost function considering other agent’s trajectory.

FSM

            graph TD
            INIT --> force_return
INIT --> WAIT_TARGET
WAIT_TARGET --> force_return
WAIT_TARGET --> SEQUENTIAL_START
SEQUENTIAL_START --> EXEC_TRAJ
SEQUENTIAL_START --> SEQUENTIAL_START
GEN_NEW_TRAJ --> EXEC_TRAJ
GEN_NEW_TRAJ --> GEN_NEW_TRAJ
 REPLAN_TRAJ --> EXEC_TRAJ
REPLAN_TRAJ --> REPLAN_TRAJ
EXEC_TRAJ --> WAIT_TARGET
EXEC_TRAJ --> REPLAN_TRAJ
EXEC_TRAJ --> force_return
EMERGENCY_STOP --> GEN_NEW_TRAJ
A([collision]) --> EMERGENCY_STOP
CEN_REPLAN_TRAJ -.-> REPLAN_TRAJ
CEN_REPLAN_TRAJ -.-> CEN_EXEC_TRAJ
CEN_EXEC_TRAJ -.-> WAIT_TARGET
B([group check]) -.-> CEN_EXEC_TRAJ
B -.-> CEN_REPLAN_TRAJ