Autonomous surface vessels operating in urban waterways must coordinate with other vessels in confined spaces while complying with traffic regulations, often without knowing their intentions. We present a decentralized motion planner based on model predictive path integral control, a sampling-based model predictive control scheme. The planner jointly computes a trajectory for the ego vessel and a prediction of the trajectories of the other vessels, and therefore accounts for their interaction. Traffic regulations are encoded as discontinuous costs, which the sampling-based formulation admits without a smooth approximation. We evaluate the method in maps taken from real sections of Amsterdam's canals, in encounters between two vessels and in scenarios with four interacting vessels. Across 780 randomized runs the method reaches every goal without collisions or deadlocks, and outperforms an optimization-based planner designed for the same domain, as well as a version of the proposed planner that does not account for the interaction. We further validate the planner on two scaled vessel prototypes in an outdoor environment, where planning and localization run onboard at 10 Hz, and head-on and crossing encounters are resolved without collisions or rule violations.
Figures & tables
Fig. 1 : Crossing at an intersection in simulation. The ego vessel (blue) runs the proposed planner; the other vessel (orange) does not communicate. Faded outlines show the preceding motion. The ego vessel knows its own state, a global path (dashed green) and the position and velocity of the other vessel. It places a local goal (purple circle) ahead of itself on the global path, estimates the goal of the other vessel (red star) with a constant velocity model, and samples input sequences over the horizon for both vessels. This yields its own planned trajectory (solid purple) and the trajectory it expects from the other vessel (dashed red); only the first input of its own sequence is applied.
Fig. 2 : Structure of the planner. At every time step each agent observes the states of all agents in the scene. The local goal of the ego agent (agent i in the figure) is taken from the global planner, while the local goals of the remaining agents are estimated from their states and the occupancy map. The resulting multi-agent problem is solved as if it were centralized, producing an input sequence and a trajectory for every agent, of which only the first input ui,0∗ is applied to the ego agent. The panels behind represent the other agents, each running the same structure on its own state.
Fig. 3 : Two vessels running the decentralized planner without communication. Each vessel plans its own trajectory (solid line in its color) and the one it expects from the other (dashed line in its color). Each vessel uses its own local goal (filled circle) and its estimated goal of the other vessel (outlined circle). Although the goals are estimated with a constant velocity model, both agents plan a cooperative maneuver.
Fig. 4 : Encounter geometries penalized by the regulation cost. The shaded region is the starboard side of the ego vessel within the radius kregion , in which the checks apply. Left: the ego vessel does not give way to a vessel approaching from starboard. Right: the ego vessel avoids an oncoming vessel to port.
Fig. 5 : Left: the local goal pg of the ego vessel is found by moving backward along the global path (faded circles) until the point lies within a radius rpg of the vessel. Right: the goal of another vessel follows from a constant velocity model; when the resulting point pg′ falls on a static obstacle, the goal is moved back along the velocity direction to the first free point pg .
Param.
Description
Sim.
Real
K
Number of samples
1000/2000
1000
J
Ancillary controllers
4
4
T
Horizon [steps]
100
100
δT
Time step [s]
0.1
0.1
λ0
Initial inverse temperature
15
15
ηmin , ηmax
Sample count bounds
3, 6
3, 6
TABLE I : Parameters used in simulation and in the real-world experiments.
Fig. 6 : Crossing at the Bloemgracht-Lijnbaansgracht intersection resolved by the decentralized method without communication (left) and by the decentralized method with constant velocity model prediction (right). On the left, the give-way vessel (blue) passes behind the vessel with right of way and complies with the regulation. On the right, the vessel with right of way slows down and the give-way vessel crosses ahead of it, which does not comply with the regulation.
Fig. 7 : Crossing at the Bloemgracht-Lijnbaansgracht intersection from the same initial conditions. Left: decentralized without communication; the give-way vessel (blue) passes astern of the vessel with right of way. Right: decentralized with prediction; the vessel with right of way slows down and the give-way vessel crosses ahead of it.
Method
Succ.- Dead.- Coll.
Rule Viol.
Avg. Time [s]
Total Avg. Dist. [m]
BG
Centralized
100 - 0 - 0
0
25.44
76.50
Dec. Comm.
100 - 0 - 0
0
25.59
76.83
Dec. No Comm.
100 - 0 - 0
0
25.84
76.34
Dec. Prediction
92 - 2 - 6
48
29.98
81.34
RA-MPCC
52 - 37 - 11
4
55.93
67.64
BGLG
Centralized
100 - 0 - 0
0
28.00
90.37
TABLE II : Results of 100 runs per scenario with two agents and randomized initial conditions and goals, with K=1000 .
Fig. 8 : Four vessels navigating with the decentralized method without communication in the Herengracht, the Prinsengracht and the Bloemgracht-Lijnbaansgracht intersection. The vessel outlines are drawn at regular intervals along the trajectory and are to scale.
Method
Succ.- Dead.- Coll.
Rule Viol.
Avg. Time [s]
Total Avg. Dist. [m]
HG
Centralized
20 - 0 - 0
0
44.70
233.58
Dec. Comm.
20 - 0 - 0
0
40.50
226.77
Dec. No Comm.
20 - 0 - 0
2
56.10
268.51
Dec. Prediction
9 - 4 - 7
60
38.50
211.43
PG
Centralized
20 - 0 - 0
0
50.29
261.74
Dec. Comm.
20 - 0 - 0
0
50.67
264.21
TABLE III : Results of 20 runs per scenario with four agents and randomized initial conditions and goals, with K=2000 for the MPPI-based planners.
Number of agents
2
3
4
5
Mean [ms]
81.1
128.8
172.4
211.9
Median [ms]
76.5
124.2
169.3
212.0
Std [ms]
16.8
26.7
32.4
50.5
TABLE IV : Solve time of the centralized variant for an increasing number of agents in an open crossing, with K=2000 . Mean, median and standard deviation are computed over the control cycles of five runs.
Fig. 9 : Quarterscale Roboat.
Fig. 10 : Two autonomous agents perform a head-on scenario using our proposed decentralized IA-MPPI. The encounter is resolved according to navigation rules. Localization and planning are performed on-board. Lines in each vessel’s color are the trajectories it planned for itself and for the other vessel.
Fig. 11 : Two autonomous agents perform a crossing scenario using our proposed decentralized IA-MPPI. The encounter is resolved according to navigation rules. Localization and planning are performed on-board. Lines in each vessel’s color are the trajectories it planned for itself and for the other vessel.