|
FhSim
3.1.0
Marine systems simulation
|
|
|
FhSim
3.1.0
Marine systems simulation
|
Horizontal guidance for an undulating survey AUV: a waypoint track, an unwrapped heading reference, a cruise speed and a battery-aware return.
Collaboration diagram for AUV undulation mission control:The horizontal half of the guidance layer, and the companion of AuvUndulateDiveControl, which owns the vertical half. This class walks a list of waypoints, emits the Heading and Speed references that the heading and speed autopilots follow, watches the battery, and turns the vehicle round in time to reach the recovery position with a stated reserve still in it.
| Phase | Phase port | What is commanded |
|---|---|---|
Transit | 0 | The next waypoint in the list, at CruiseSpeed. |
Return | 1 | RecoveryPosition, at CruiseSpeed. |
Finished | 2 | Speed = 0, the heading held, EndMission = 1. |
Transit becomes Return when the waypoint list runs out, or earlier when the battery gate below says the vehicle must start home now. Return becomes Finished on arriving within WaypointRadius of RecoveryPosition. Finished is terminal, and EndMission latches at one and never goes back.
FhSim cannot stop a simulation on a signal**, so EndMission is a flag for the observer and for an FMU host, not a control of the run. A scenario using this class must set TEnd long enough to cover the whole mission, and will simply sit at zero speed after the vehicle is home.
Auv.Heading wraps at \(\pm\pi\) and PidController subtracts its reference and its measurement plainly, so a reference on the far side of the seam from the measurement produces an error of nearly \(2\pi\) and a full-scale rudder command in the wrong direction. The fix is one extra inport on a class that has to exist anyway:
Heading - psiMeasured is then in \((-\pi, \pi]\) by construction at every heading, and a plain PID is correct everywhere. The reference the port carries is the representative of the bearing that lies nearest the current measurement, so it is not** confined to \((-\pi, \pi]\) itself: on a southbound leg it will read 3.19 rad.
Worth stating precisely, because the obvious assertion about this port is false. The emitted reference is congruent to the bearing modulo \(2\pi\) at every instant, so
wrap(Heading(t) - Heading(t - dt)) is small whenever the bearing turns slowly — the reference is continuous **modulo \(2\pi\)**;Heading(t) - Auv.Heading(t), the error the PID actually sees, is small and continuous — which is the property the whole exercise is for;Heading(t) - Heading(t - dt) itself is not small across the seam. When the measurement* wraps from \(+\pi\) to \(-\pi\), as it does repeatedly on a southbound track, the reference wraps with it and the raw difference is \(2\pi\).Making the raw reference continuous instead — unwrapping against the previously emitted value rather than against the measurement — would let the reference wind arbitrarily far from the wrapped measurement and put the \(2\pi\) spike straight back into the PID. The formula above is the one that keeps the error right, and the error is what matters. AuvUndulateMissionControl_Test asserts all three statements.
The gate asks whether the vehicle can still reach RecoveryPosition and arrive with BatteryAtEndOfMission joules left:
metresPerJoule is measured from the flown track, not computed from CruiseSpeed and the hotel load.** A nominal figure is wrong here by a wide margin, and 0037 — The undulation climbs and dives at 0.7 of u sin(DiveAngle), and the pitch loop needs 40 s to get there says by how much: an undulating vehicle makes only about 0.7 of the vertical speed its commanded DiveAngle implies, so the same water column costs about 1.4 times the track length and 1.4 times the energy, and the pitch loop spends roughly 40 s of every leg still turning over. Turns, current, fin drag and the hotel load all land in the measurement for free and in no nominal formula at all.
Before any distance has been made good the ratio is unknown, energyToRecovery is taken as zero, and the gate degenerates to the plain reserve test Battery <= BatteryAtEndOfMission. That is the conservative direction only in the sense that it cannot fire early; a mission whose recovery position is far away at t = 0 should not rely on the first few samples.
The gate is evaluated in Transit only. Once the vehicle is on its way home there is nothing left to decide, and a gate that could fire again would only chatter.
The class holds no integrator states. The waypoint index, the phase, the latch and every output are members written only in AcceptedStep, once per UpdatePeriod, exactly as SeineOperation holds its operational phase and AuvUndulateDiveControl its direction. Switching evaluated inside OdeFcn or inside an output function would also be evaluated at rejected trial times, and the mission would then depend on steps the integrator threw away.
That property is also what keeps the control stack free of algebraic loops. The wiring Auv -> Mission -> HeadingPid -> Auv is cyclic on paper, but every output port of this class and of PidController returns a member written only in AcceptedStep, so there is no direct feedthrough anywhere in the cycle. Reading Auv.Heading to do the unwrap is safe for exactly this reason.
Because the switching depends on where the accepted steps fall, a regression input must fix StepMax.
The measured turn radius of the suggested vehicle at cruise is 13.3 m, so a radius much below one turning diameter makes the vehicle circle a waypoint it cannot close on. The default 15 m is that number rounded up. It is also the radius at which the mission is declared over, so it is the accuracy of the recovery position.
SpeedPid.InitOutput is a feed-forward and not a tuning knob: it must be the propulsion power that holds CruiseSpeed, 30 W for the suggested vehicle at 1.5 m/s. At the wrong value the speed loop spends tens of seconds unwinding an integrator that started in the wrong place.
| Name | Width | Description @addoption{Pos, 3, The vehicle position in NED, m. Only the north and east components are used; depth belongs to AuvUndulateDiveControl. } @addoption{Battery, 1, The energy left in the battery, J. } @addoption{Heading, 1, The measured vehicle heading, rad, wrapped into minus pi to pi. Used only to choose the branch of the emitted reference. } |
|---|
| Name | Width | Description @addoption{Speed, 1, The commanded forward speed through water, m/s. CruiseSpeed while the mission runs and zero once it is finished. } @addoption{Heading, 1, The commanded heading, rad, unwrapped against the measurement and therefore NOT confined to minus pi to pi. } @addoption{EndMission, 1, One once the vehicle has reached the recovery position, zero before that. It latches. } @addoption{WaypointIndex, 1, Diagnostic. The index of the waypoint being flown towards, counting from zero. } @addoption{DistanceToWaypoint, 1, Diagnostic. The horizontal distance to the point being flown towards, m. } @addoption{Phase, 1, Diagnostic. Zero while transiting, one while returning, two once finished. } |
|---|
| Name | Width | Description @addoption{WaypointsX, n, The north coordinates of the waypoints, m. Mandatory, and at least one. } @addoption{WaypointsY, n, The east coordinates of the waypoints, m. Mandatory, and the same length as WaypointsX. } @addoption{RecoveryPosition, 2, The north and east coordinates the vehicle returns to, m. Defaults to the first waypoint. } @addoption{WaypointRadius, 1, The distance within which a waypoint counts as reached, m. Default 15, which is about one turning diameter of the suggested vehicle. } @addoption{CruiseSpeed, 1, The commanded forward speed through water, m/s. Default 1.5. } @addoption{BatteryAtEndOfMission, 1, The energy the vehicle must still have on arriving at the recovery position, J. Default 2.7e5, that is 15 percent of the suggested 1.8e6 J battery. } @addoption{LoopWaypoints, 1, One to restart the waypoint list instead of returning when it runs out, so that only the battery gate ends the survey. Default zero. } @addoption{UpdatePeriod, 1, The time between evaluations of the law, s. Default 1. } |
|---|
This SimObject has no states and therefore no initial conditions. It starts in Transit towards waypoint zero, commanding CruiseSpeed and a heading of zero; the heading is replaced by a real reference at the first accepted step.
This SimObject is referred to as Auv/UndulateMissionControl
1.9.8