## Planning for Formations of Motion-Constrained Vehicles

### Approach and Motivation

The objective is to maintain a static formation defined over curvilinear coordinates, which are a coordinate system in which the coordinate lines may be curved. The formation is defined relative to a reference agent. An illustration of a formation along curvilinear coordinates is shown above.

If airplane L is the leader,then the middle blue line corresponds to the 'x' axis of the curvilinear system, and the line perpendicular to the airplane is the 'y' axis. To maintain a static formation, each follower aircraft must maintain p_i distance along the leader's curved trajectory and q_i distance along the perpendicular direction, as in the figure above. Finally, the variable d_L denotes the distance the reference agent has covered and s_i = d_L + p_i denotes the distance that the projection of follower i has covered along the leader's trajectory.

### Respecting the Control Limits

There is an important issue that arises when the leader’s trajectory is computed in real time. It is possible that the controls for the follower will violate the control limits for the vehicle. In the following picture, the green lines represent the controls of the aircraft when there is no consideration of the motion limitations (represented by the red horizontal lines). The blue lines represent the result of our work, where the formation is maintained while the constraints are also respected.

### Dynamic Formations

### Acquiring a Formation

### Multi-Level Formation Roadmaps for Dynamic Collision Avoidance

Teams of robots can utilize formations to accomplish a task, such as maximizing the observability of an environment while maintaining connectivity. In a cluttered space, however, it might be necessary to automatically change formation to avoid obstacles. This work proposes a path planning approach for non-holonomic robots, where a team dynamically switches formations to reach a goal without collisions. The method introduces a multi-level graph, which can be constructed offline. Each level corresponds to a different formation and edges between levels allow for formation transitions. All edges satisfy curvature bounds and clearance requirements from obstacles.

Given an environment with static obstacles (1), a collision-free roadmap G(V,E) for a holonomic system (2) can be computed using various alternatives, such as sampling-based planners. The input graph must satisfy the typical requirements of a roadmap, be directed graph and it is desirable to have a sparse roadmap and maximize distance from the obstacles. The roadmap should contain paths with obstacle clearance that satisfies the requirements of the least conservative formation, at least two nodes with more than three edges, else it will be a line or a loop graph, that robots would not be able to change their direction along the graph. Once the input graph is available, Fs copies are created for the different levels, each one corresponding to a specific static formation (3). It is necessary to edit each level so that it satisfies curvature constraints arising from the corresponding formation (4). The graph still may not satisfy collision avoidance requirements. The initial graph is computed only for one agent. Each formation requires a different clearance around the leader’s configuration. Thus, additional parts of the graph must be pruned. The clearance along edges computed given the corresponding formation for each level. The clearance on the right side of the leader can be different than the clearance on the left side depending the formation. The parts of the graph that don’t satisfy the minimum clearance are pruned. The input graph is directed, which means that for an asymmetric formation only one direction may be pruned (5). Up to this point, multiple levels for different formations have been computed. Each level is different due to different clearance and curvature limits and may also be disconnected. The capability of the team to switch between static formations is utilized so as to reconnect different graph levels. Two different types of transitions are available. The different levels can either be connected along straight edges, or through circular arcs (6). If you want to know more about the multi-level roadmap download the paper here

Using quadratic Bezier curves, a sequence of two curvature-bounded curves can be computed, which can connect any two configurations. These curves are used to connect the initial and final configuration with points on the level that corresponds to the initial/final formation respectively. The experiments evaluate the percentage of desired formation maintenance and the length of the path. Green path: the robots have to use the specific formation as much as they can. Red path: the robots can switch formations in order to return the shortest path.

### Results

The following pictures provide the error between the desired formation and the achieved formation at each simulation step. The left picture displays the distance from ground truth, while the right figure shows the difference in orientation.

The next set of pictures provide the error for the dynamic formation(left picture) and the acquisition of a formation(right picture) at each simulation step. The error is the distance between the desired formation and the achieved formation.

##### Related Publications

- Krontiris A., Louis S., Bekris KE., "Multi-Level Formation Roadmaps for Collision-Free Dynamic Shape Changes with Non-holonomic Teams", International Conference on Robotics and Automation (ICRA-12), Minnesota, MN, May, 2012.
- Krontiris A., Louis S., Bekris KE., "General Dynamic Formations for Non-Holonomic Systems Along Planar Curvilinear Coordinates", International Conference on Robotics and Automation (ICRA-11), Shanghai, China, May, 2011.
- Krontiris A., Louis S., Bekris K E., "Simulating Planar Aircraft Formations Along Curvilinear Coordinates", Third International Conference on Motion in Games, Zeist, Netherlands, Nov., 2010