Reward-Based Collision-Free Algorithm for Trajectory Planning of Autonomous Robots

TL;DR

The paper tackles reward-based mission planning for autonomous robots where a sequence of weighted waypoints must be chosen to maximize collected rewards under obstacle avoidance and robot dynamics. It introduces a two-layer framework: a genetic algorithm that optimizes waypoint sequences with a penalty-based fitness function, and a trajectory generator using differential flatness and clothoid curves to ensure dynamically feasible, curvature-continuous paths between waypoints; obstacle-free subpaths are established via A*, and the genetic crossover leverages dynamic time warping with extended convex combinations and projection. Key contributions include a two-strategy crossover for variable-length discrete sequences, a differential-flatness-based mapping from flat outputs to states/inputs, constraint-violation metrics integrated into the fitness, and extensive hardware validation on a ground vehicle, a quadrotor, and a quadruped, plus a time-complexity study showing empirical $O(n^2)$ scaling. The results demonstrate improved scalability and trajectory smoothness over jerk/snap-based methods and competitive performance against MINLP solvers, highlighting practical impact for real-world autonomous tasks with limited resources and varying waypoint rewards. The framework supports flexible task prioritization, robust collision avoidance, and efficient planning suitable for embedded deployment, with future directions including dynamic obstacles, multi-robot extensions, and real-time re-planning.

Abstract

This paper proposes a novel mission planning algorithm for autonomous robots that selects an optimal waypoint sequence from a predefined set to maximize total reward while satisfying obstacle avoidance, state, input, derivative, mission time, and distance constraints. The formulation extends the prize-collecting traveling salesman problem. A tailored genetic algorithm evolves candidate solutions using a fitness function, crossover, and mutation, with constraint enforcement via a penalty method. Differential flatness and clothoid curves are employed to penalize infeasible trajectories efficiently, while the Euler spiral method ensures curvature-continuous trajectories with bounded curvature, enhancing dynamic feasibility and mitigating oscillations typical of minimum-jerk and snap parameterizations. Due to the discrete variable length optimization space, crossover is performed using a dynamic time-warping-based method and extended convex combination with projection. The algorithm's performance is validated through simulations and experiments with a ground vehicle, quadrotor, and quadruped, supported by benchmarking and time-complexity analysis.

Figures (22)

• Figure 1: Quadruped robot trajectory through weighted waypoints without a specific predetermined order, considering collision avoidance and total distance constraints. Larger blue dots represent higher rewards, while red boxes indicate obstacles. The trajectory maximizes the collected reward by prioritizing higher-reward waypoints, skipping lower-weighted ones, while satisfying the distance constraint.
• Figure 2: Illustration of problem formulation elements: environment $\mathcal{E}$, obstacles $\mathcal{O}_i$, obstacle-free space $\mathcal{F}$, waypoints $\boldsymbol{q}_i$ with associated rewards $w_{\boldsymbol{q}_i}$, and obstacle free path $p(s)$ connecting the waypoints of the sequence $s=\{\boldsymbol{q}_1,\boldsymbol{q}_2,\boldsymbol{q}_4,\boldsymbol{q}_5\}$ in order.
• Figure 3: Flowchart of the proposed algorithm based on classical Genetic Algorithms (GA).
• Figure 4: Fitness function flowchart.
• Figure 5: Alignment example between the one-dimensional sequences $s_1$, $s_2$. The optimal alignment of the two sequences is found by the warping path that follows the minimum distance matrix $D$ described by the color bar.