Route planning & control
Verified terrain becomes a checked path, then actuator commands. Gazebo motion feeds the next planning and control decision.
Personal underwater waypoint planning uses the Mission depth limits. A wider simulation limit preserves the terrain and swept-footprint checks below.
Processing flow
- 1. Goals + measured state + terrain
Transform geographic goals into the active local frame. Join confirmed terrain tiles on a common grid; leave missing coverage unknown.
- 2. Geographic and coastal guidance
Find land-avoiding guidance and port approach corridors. Geographic clearance alone does not establish safe water depth.
- 3. Check the swept vehicle footprint
Expand the hull by its tracking margin. Clip floor triangles against each test square to find the highest floor, then reject land, insufficient clearance or missing data. Maximum-filter bounds accelerate deep-water tests.
- 4. Direct segment → A* detour
Try a checked straight segment first. Otherwise search an eight-neighbour lattice with distance cost and a Euclidean heuristic. Enforce corner, radius, length and time limits.
- 5. Shorten and revalidate
Accept shortcuts through the same swept-footprint tests and preserve protected harbour approach points. Record checked waypoints and minimum clearance; return an error when no safe route is found.
- 6. Speed policy → propulsion
The automatic-voyage leader brakes for bends and heading error, then uses ramped PI speed control and differential yaw thrust. The follower looks ahead along the leader’s measured trail; tilt and closing speed constrain commands.
- 7. Measured feedback → next update
Gazebo applies forces and reports motion. Monitor progress, stability and terrain/sensor coverage; pause before unverified coverage. New goals or changed conditions can require replanning.
The LIVE voyage uses mapped coastal connections as temporary guidance. When measured movement brings the vessel at least 50 m closer to the course target than the connection’s final hint, that guidance is released and the onward route is planned again. Every new propulsion corridor still requires mapped-land and numeric-depth approval.
Planning technology catalogue
| Layer | Method and data | Scope |
|---|---|---|
| Geographic route | Port/arrival targets, passage gates and land polygons produce long-distance guidance and coastal connections. | Land avoidance does not by itself establish bathymetric clearance. Port catalogue coordinates and navigable arrival targets are separate. |
| Terrain field | Confirmed numeric grids are joined in a common frame. Exact triangle clipping finds the highest represented floor inside a query square; a SciPy maximum-filter upper bound accelerates clearly deep regions. | Unknown or out-of-bounds terrain remains unavailable; visual satellite colour is not used as depth. |
| Swept clearance | Sample each segment at no more than 1 m spacing and enlarge the query footprint by half that separation. Use the deeper endpoint for a depth-changing leg. | This is a conservative terrain-envelope test, not full six-degree-of-freedom future hull-motion prediction. |
| Local search | Try the direct segment, then eight-neighbour A* with distance cost, Euclidean heuristic and conservative corner checks. Shortcuts pass the same segment test. | Standard planner defaults are 4 m lattice spacing, 600 m search radius, 2,000 m route length and 25 s planning budget; callers can select different bounds. |
| Vehicle allowance | Surface: 3.5 m hull radius, initially 4 m tracking margin, 1.5 m floor clearance. Underwater: 0.5 m hull radius, 2 m tracking margin, 1 m floor clearance. | Personal WAM-V fallback can narrow tracking margin to 2 m without relaxing its depth clearance. These are planner allowances, not a hydrographic accuracy certificate. |
| Result and monitoring | Return checked waypoints, selected margin, grid/budget information and minimum represented clearance. Runtime corridor/coverage checks can stop execution. | Planning failure, tracking departure and missing coverage are distinct conditions. A route's existence does not guarantee successful control in the chosen environment. |
The local planner is terrain-based. AIS markers, another vessel in a video, or an available LiDAR topic do not establish dynamic-obstacle avoidance or COLREG-compliant behaviour. Such behaviour belongs to the controller selected for the experiment.
LIVE resident coastal recovery
WWW-POSIM searches the loaded numeric terrain with mapped-land exclusions. The ordinary 1.5 km shore band constrains the search itself. If that band has no checked onward corridor, a finite bypass can reach 2 km offshore, then rejoin the ordinary band at its endpoint.
The endpoint must advance the onward course by at least 500 m, reduced to 10% of the remaining distance within 5 km of the target. The corridor retains the 200 m shallow-water exclusion and 2 m minimum represented depth, with a maximum length of 12 km. Native execution checks each bounded portion again.
Optional coastal rounding first tests the next course direction. When that direction has no checked corridor, resident recovery retries the original course vertex within the same 60-second search budget. It preserves a committed escape goal. Local escape distance alone does not reset the voyage progress deadline.
New recovery endpoints stay outside recently escaped areas. A fully checked onward corridor may cross one and continue beyond it. Routes making less than 50 m of onward progress keep the area excluded throughout their path. This history constraint does not change the physical hull-safety check.
If local recovery cannot advance around a headland, a mapped coastal connection supplies persistent discovery bends. The search uses the loaded OSM exclusion within the 64 km land window, a 250 m grid, a 300 m mapped-land margin and the ordinary 1.5 km coastal band. Its endpoint must gain at least 5 km toward the original course target, reduced to 10% of the remaining distance near that target. The selected connection stays fixed across local replans and coordinator restarts.
This connection supplies direction, not represented water depth. Each propelled portion still passes the resident numeric-depth and native corridor checks. Progress follows the remaining length of that unchanged connection only while the current corridor is approved; circles and replacement goals cannot reset its progress deadline.
After repeated regional recovery attempts and ten minutes without measured progress, LIVE can search a 16 km terrain window for a complete connection toward the course. It keeps the selected bends in order and approves each propulsion segment against the current native terrain and mapped land.
After prolonged stalled recovery, LIVE can use a checked open-water corridor. Once movement resumes, it periodically tries to rejoin mapped shore and follow it onward. Shore discovery checks a 64 km window and can extend to 128 km; these distances do not expand numeric-depth coverage. Each propulsion segment still requires mapped-land and numeric-depth approval. A fixed port approach remains mandatory; the vessel transitions to its entry on a checked corridor.
Controller paths and feedback
| Path | Input → implementation → actuation | Research interpretation |
|---|---|---|
| Personal ArduPilot | Guided targets/manual inputs → ArduRover or ArduSub SITL through MAVROS and the Gazebo JSON interface → configured actuator channels. | Navigation source, EKF state, mode, arming and actuator mapping affect the result. GNSS & navigation distinguishes sensor truth and fused estimates. |
| ROS 2 direct control | User ROS node → permitted command topics and selected actuator mapping → Gazebo forces. | The user supplies the control law. Publishing a target or velocity is not equivalent to measuring achieved motion. |
| Automatic-voyage leader | Checked waypoints → bend braking, heading-error speed reduction, acceleration ramp, clamped-integral PI speed control and differential yaw thrust. | Native feedback uses measured body motion. The nominal control period is 0.05 s; heartbeat loss inhibits commands. |
| Automatic-voyage follower | Leader's measured trail and separation/closing rate → look-ahead guidance, speed/turn limits and braking → propulsion. | The nominal following-gap policy is 38 m between hull origins. It is a demonstration controller, not a fleet collision-avoidance proof. |
| Voyage recovery | Explicit recovery policy can request a bounded physical tow, retaining mass, contact and drag. | Record recovery mode separately from ordinary autonomous propulsion. The leader's tow is limited to 900 N and 0.8 m/s requested recovery speed. |
In sessions with GNSS configured, missions revalidate the route from the measured position after arming and navigation settling, before sending the first destination. Settling requires fresh FCU local pose and valid GNSS fusion or accepted external sensor fusion. The receiver’s raw GPS fix remains a separate status; see GNSS & navigation.
Personal WAM-V missions use input shaping by default (GUID_OPTIONS=0). With no explicit heading, they refresh the original position target every two simulation seconds outside the 0.75 m arrival band and request zero speed and zero yaw rate inside it. A saved profile with bit 64 set retains ArduRover's S-curve waypoint planner. BlueROV2 refreshes position goals every two simulation seconds using ArduSub's waypoint controller. Telemetry freshness uses wall-clock age; navigation and terrain-clearance checks continue throughout the leg.
For a powered boat receiving a fresh Guided target with zero speed and zero yaw rate, the packaged ArduRover keeps its speed PID active to oppose forward and backward drift. This controls speed; correction toward the authored position still starts outside the arrival band. Sailboats, expired targets and other control modes retain their configured stopping behavior.
Once the acceleration-limited target is zero, the speed controller clears its integral at the first measured stop. Outside the configured stop-speed band, it also clears retained integral that would accelerate the boat in its current direction of drift. Opposing braking integral and bias within the stop-speed band remain active.
The default personal profile uses WP_SPEED=0.5 m/s, ATC_STOP_SPEED=0.02 m/s and ATC_STR_RAT_I=0.
The SITL plugin interpolates sensor states between completed physics steps, targeting 200 Hz for Rover and 1000 Hz for Sub. Changing the physics interval changes the number of intermediate samples. Each interval ends at the original physical state and timestamp. Physical resolution remains determined by the physics timestep.
The public WAM-V demo selects input shaping (GUID_OPTIONS=0) and refreshes the original waypoint every two simulation seconds. It uses WP_RADIUS=0.2 m, LOIT_RADIUS=0.1 m and LOIT_SPEED_GAIN=1.0 for approach and wave-disturbed station keeping. Mission arrival still requires 0.75 m horizontal accuracy. Personal workspaces retain their selected controller parameters.
Checkpoint restoration and floating-origin changes also issue state/coordinate commands. A coordinate rebase transforms terrain/path/trail positions while preserving measured velocities; it must not be differentiated as physical travel. Keep recovery, reset and rebase events when assessing tracking error or energy consumption.
Implementation: POSIM-Routing, POSIM-Control native controllers. WWW-POSIM supplies session orchestration, controller selection and personal mission guards.
Interpretation
The native leader/follower algorithms belong to the automatic world voyage. Personal ArduPilot and ROS 2 direct-control sessions use their selected command path. Route clearance is conditional on the represented terrain and vehicle settings.
Personal WAM-V waypoint missions reserve a 3.5 m hull radius and a 4 m tracking margin. If the start or passage is blocked by that allowance, planning retries with a 2 m tracking margin. The 1.5 m water-depth clearance remains required. During the mission, leaving the selected tracking corridor stops the mission.
Related documentation and implementation
LIVE world voyage · Control an installed workspace · Terrain generation algorithms
POSIM-Routing: navigation.py, planning.py and coastal/port modules. POSIM-Control: native/Pilot.cc, native/Follower.cc and Python speed/stability policies. Use the revisions selected by the application.