In this study, bolded variables represent vectors or matrices and non-bolded variables represent scalars. The body-fixed frame standard for ROS vehicles follows Forward–Left–Up (FLU) relative to an East–North–Up (ENU) inertial frame, whereas ArduSub’s body-fixed frame is Forward–Right–Down (FRD), referencing a North–East–Down (NED) inertial frame. In robotics applications that use the ROS, there are certain guidelines that exists known as the ROS Enhancement Proposals (REPs). Generally, the REPs are guidelines to follow that help developers integrate with the larger ROS ecosystem. They serve as valuable touchstones which provide common threads for the community to latch on to, and standardize many fundamental things that help demystify the open-source landscape. For example, the convention that all ROS vehicles follow FLU-ENU is because of REP 103 [
28]. There is a longstanding issue that those using the ROS in maritime applications were urged to use FLU-ENU frames by REP 103, but the rest of the industry followed the FRD-NED, also known as forward–starboard–down, FSD-NED, frame conventions. A new REP, 156, puts this issue to rest and advises that ROS users in maritime applications preserve FLU-ENU as the primary frames, but also maintain secondary frames with the appended suffixes “
_fsd” or “
_ned” [
29]. Readers are also encouraged to follow the ROS Maritime Working Group updates on the ROS Discourse forums [
30]. Moving forward, in this section all discussion and theory will reference an FRD-NED/FSD-NED frame if applicable.
The thruster configuration of the BROV2 Heavy can be seen in
Figure 3: horizontal azimuthal thrusters 1–4 create surge, sway, and yaw motions; vertical thrusters 5–8 create heave, roll, and pitch motions; and thrusters are numbered with indices that are referenced from this point onward. The symbols for vehicle states in the inertial frame are
, where linear components are grouped in
and angular components are grouped in
. The symbols for velocities in the body-fixed frame are
, where linear components are grouped in
and angular components are grouped in
. The symbols for forces and torques are grouped together in a single vector
, where
are linear forces and
are angular torques.
2.2.1. Thruster Allocation Matrix Overview
ROVs like the BROV2 owe their high manoeuvrability to the multiple thrusters that their boxy frame carries. Each of these thrusters work in tandem to impart control forces and torques on the body of the vehicle to carry out movements and stabilize the vehicle.
For the BROV2 Heavy equipped with eight thrusters, this can be expressed quantitatively,
where
is the six-by-one vector of control forces and torques,
is the six-by-eight thruster allocation matrix [
32], and
is the eight-by-one vector of the force contributions of the eight thrusters. The constant matrix
is a linear transformation that maps the force and torque inputs of the motors
to the overall resultant forces and torques on the vehicle body
.
The first three rows of
transform the eight thruster input forces into the resultant body linear forces and are constructed column-wise with
where
is the angle of the
i-th thruster in the horizontal plane of the vehicle and
is the angle of the
i-th thruster in the perpendicular upright plane (see
Figure 3). The vector described in Equation (
2) encodes the geometric orientation of the
i-th thruster in space and can be considered a type of unit vector for forces. All the force unit vectors for the eight thrusters’ orientations put together form the top three rows of
, which when given an input
, outputs the linear forces (i.e., the first three entries of
).
Figure 3 displays the FRD body frame coordinate axes, relative locations, and orientations of the thruster configuration. Thruster 1 is annotated as an example of how
is defined: the angle
follows the right-hand convention for clockwise yaw rotation about the downwards-pointing z-axis;
is not visible in the figure but also follows the right-hand convention for pitch rotation about the y-axis.
The last three rows of
map the thruster force inputs to the resultant body torques and are constructed column-wise with
where
is the location of the
i-th thruster relative to the vehicle’s center of mass, and a cross product is performed with it and the first three rows of the
i-th column of
determined earlier in Equation (
2). Similar to how Equation (
2) is a unit vector for forces, Equation (
3) is a unit vector for torques. All the torque unit vectors for the eight thrusters are put together to form the bottom three rows of
, which when given an input
, output the angular torques (last three entries of
).
Using the vehicle system geometry presented in [
16] and concatenating the eight vectors resulting from Equations (
2) and (
3), the total thruster allocation matrix for the BROV2 Heavy is given in Equation (
4) rounded to two decimal places. Given a vector
containing the variable thrust of all eight thrusters, Equations (
1) and (
4) can determine the control forces and torques
resulting from the thrusters on the BROV2 body.
Now, consider if some desired forces and torques on the BROV2 were given and the necessary motor activations
needed to be calculated instead, as is the case of an autopilot mechanism; the inverse thruster allocation matrix would be required. This is
where
is the pseudo-inverse of the rectangular matrix
. The actual matrix computed from the pseudo-inverse of Equation (
4) has values of
The matrix above can transform an input vector of desired forces and torques on the vehicle body into the required motor activations to achieve them. These arrays , , , and have compatible physical units in the Meters–Kilogram–Second (MKS) system of measurements. With these definitions in place, the process of implementing the changes in the software shall be explained next.
2.2.2. Implementation
In implementing custom changes to ArduSub, it is important that modifications are precise and that the rest of the codebase’s functionality is preserved. The locations in the code handling individual thruster allocation were identified to be within the files
SIM_Submarine.cpp and
AP_Motors6DOF.cpp. The former carries out calculations for state updates of the SITL virtual vehicle due to motor inputs and contains a matrix
In this equation, a hat is used to distinguish
from the matrix
. The latter contains a matrix for distributing pilot control inputs to the individual thruster activations with the form
where
is the transpose of Equation (
7). Upon closer inspection of the two matrices, both are organized in the same fashion as the thruster allocation matrix and its inverse, and are used for similar purposes:
is to
as
is to
. Where the zero entries in
and
are the entries close to zero in
and
, the nonzero entries also share the same positive or negative sign. However, the hatted matrices lack the dimensionality that the theoretical ones possess because they do not implicitly encode physical units and were intended for a heuristic control scheme instead of a quantitative one.
Figure 5 provides a simple flow chart visualization of the following description. Every iteration of the software control loop checks the
RC_Input channel for the six-by-one vector of PWM signals ranging from
to
, centered on
where values
are positive values and
are negative values. The six entries correspond to surge, sway, heave, roll, pitch, and yaw commands and are each normalized to a
to
range that becomes the user input vector
. This vector
is then read into
AP_Motors6DOF.cpp where the function
motor_command() calculates the thruster allocation; Algorithm 1 details this loop and shall be discussed shortly.
Calculations performed in the primary loop of
AP_Motors6DOF.cpp take the forces and torques requested by
and determine how the eight individual thrusters should activate to serve these commands. The resulting eight-by-one vector of these calculations
is analogous to the forces from Equation (
5). Each entry represents one of the eight thrusters and is normalized to a
to
range. The entries of
are converted once more to PWM signals in the
to
range and output as
to the
RC_Output channel. Once in
RC_Output, the PWMs are distributed throughout the respective ESCs corresponding to the eight individual thrusters. Ultimately, this activates the thrusters that generate forces and torques on the vehicle.
To understand how exactly
is used, Algorithm 1 provides a step-by-step clarification. As of the ArduSub 4.5.0 beta1 release on 22 February 2024, the Ardupilot/ArduSub source code still utilizes Algorithm 1 found in
AP_Motors6DOF.cpp for the manual control mode [
33]. The matrix is used as a constant parameter that helps transform user input
into motor activation
. The function performing the computation is
motor_command(). Within the function, there are various forms of
which serve as buffer variables used inside the function. The first usage is introduced on lines 2 and 9, with the form
, where the left superscript
j can be either 1 for roll, pitch, and heave control or 2 for yaw, surge, and sway control and the right subscript “max” labels its purpose as a normalizing variable. The second type of usage is introduced on lines 4 and 11, with the form
, where the left superscript
j again can be either 1 for roll, pitch, and heave control or 2 for yaw, surge, and sway control and the right subscript
i is an index indicating the
i-th thruster which is being calculated for. The groupings of forces in
terms correspond to those generated primarily by the vertical thrusters and groupings of forces in
correspond to those generated primarily by the azimuthal thrusters.
Algorithm 1: Original Manual Control Algorithm |
![Applsci 14 07453 i001]() |
The function motor_command() consists of three loops, two for calculating preliminary output values based on inputs and the third loop for constructing the output. The first loop and second loop are identical except that the first loop calculates for roll, pitch, and heave control and the second loop calculates for yaw, surge, and sway control. The first two loops step through the thrusters in order and scale outputs with respect to the corresponding parts of with respect to the corresponding user inputs of and sum them together in the buffer (lines 4 and 11). These numbers are then checked against the current value of and replaced with the value of the buffer if it is larger (lines 5 and 12). After all eight thrusters have been looped over twice, the final third loop normalizes with respect to the final values of and adds the vertical and azimuthal ratios together (line 17). The trimming function constrain() checks that each value of is in the range of to (line 18); if a value is outside the bounds, the function chooses the limit closest. Then, the values of are scaled up to appropriate PWMs (line 19), and finally sent to RC_Output which relays the signals to the respective ESCs.
If a user were to command any combination of movement, the vehicle would respond with all requested movements, but all would be scaled with respect to the largest commanded force or torque. The overall effect of Algorithm 1 is that it generates intuitive vehicle responses that a pilot will recognize as matching joystick inputs. This heuristic method is a good way to prevent cases of motor saturation where the vehicle appears to be unresponsive due to motors being maxed out. However, the exact value of the resulting forces/torques, , will not directly correlate to the magnitude of the input commands, which is necessary for automatic control laws. Instead, quantification of forces should be preserved, and saturation avoided through selection of an adequate control bandwidth.
Thus, with this assessment of the original control implementation and the goal of implementing a PD control law, a new simple algorithm is determined which can replace the pre-existing framework without disrupting anything else or introducing unnecessary complexity.
In order for the new manual control algorithm to be integrated seamlessly, without altering processes on either end of the software control loop, it must use the same inputs and produce the same outputs. The main challenge is incorporating physical units that will allow the autopilot to command specific quantities of forces and torques. To achieve this,
replaces
and new parameters and functions are introduced that encode the BROV2 hardware limitations while preserving the control architecture. These new parameters are
,
, and
, which are a six-by-one vector containing the maximum forces and torques that the BROV2 is capable of, eighty-one normalized PWMs and eighty-one corresponding thrusts produced by a T200 thruster. The contents of
and
are experimental data collected by Blue Robotics [
34] and graphed in
Figure 6 and used as a lookup table, utilized by the FMU thrust distribution and the SITL hydrodynamic model. The values of
are determined analytically by solving for
using Equation (
1) given
, constructed of maximum thrusts according to the lookup table in combinations producing solely surge, sway, heave, roll, pitch, or yaw manoeuvrers. This results in
for forces and
for torques. To use input signals from
RC_Input of
, the signals are converted to units of forces and torques with
, and transformed with
to find
directly. Mathematically, these operations are
Algorithm 2 shows how Equation (
9) is accomplished in ArduSub step by step. Using the new parameters
,
, and
, the same inputs
are used and the same outputs
are produced. However, now
originates from a hybrid autopilot which has calculated physical forces and torques (that have been normalized with respect to
), as opposed to interpreting inputs heuristically. Linear DOF control is handled with an open-loop controller that directly passes inputs into
. Angular control is handled with a closed-loop PD control law that interprets an AR pilot’s gestures with error feedback, before passing inputs into
.
Section 4 shall discuss the control law in detail.
The new function
motor_command_new() uses one software loop; first, it iteratively performs Equation (
9) (line 3) to determine
. This is then input into the function
find_bounding_indicies(), which determines the closest indices of
below (
low) and above (
high) the force of
(line 4). In the event that
is outside the range of
, the
index will equal the
index and default to either
for negative forces or
for positive forces (lines 5 through 12). For all other instances, the bounding indices will be used to perform linear interpolation that produces a normalized value of
(lines 13 through 15). This value is trimmed (line 16) and then converted to PWMs (line 18) to finally be sent to
RC_Output.
It is important to emphasize that while this new algorithm accounts for vehicle limitations, it depends on inputs from the hybrid autopilot that encode vehicle limitations as well.
Algorithm 2: New Manual Control Algorithm |
![Applsci 14 07453 i002]() |
2.2.3. Simulation Discussion
The module that enables devices to interface with ArduSub is known as the hardware abstraction layer (HAL). Each supported variety of hardware has a unique library; Pixhawk uses the AP_HAL_ChibiOS library, whereas our simulation uses AP_HAL_SITL. Once the vehicle’s low-level hardware is able to communicate using ArduSub, it is able to access the shared libraries for general-purpose sensors, state estimation, and motor control. For example, the motor libraries that control the thrusters on the BROV2 share a common code with the libraries used to control thrusters on a quadcopter. In the case of a simulated virtual mission, a special SITL HAL is utilized to emulate a physical FMU that does not require a physical device to be in the loop; shared libraries further upstream remain the same regardless of the hardware connected to the HAL.
ArduSub uses the Python script sim_vehicle.py to invoke the SITL HAL that on one branch generates the virtual firmware and by default on a separate branch generates the virtual environment for submersible vehicles. At runtime, the latest state of the codebase is built and must pass through a robust verification process that checks that the raw code is written properly and will be able to be flashed to an actual vehicle FMU. This capability that seamlessly bridges virtual firmware testing and real-world vehicles is invaluable.
The other branch called by default when
sim_vehicle.py is run generates the SITL virtual environment from
SIM_Submarine.cpp and its header. Contained in these files are the hydrodynamic model of the BROV2 and control allocation algorithms (see
Section 2.2.5) for the state updates of the virtual vehicle. Alternatively, users may use Gazebo instead of
SIM_Submarine.cpp to perform these calculations. Gazebo is a powerful simulation environment that renders 3D dynamic scenes that represent robots and their environments, simulating physics, collisions, and sensors through an ever-expanding library of plugins. Current versions of Gazebo have plugins for thrusters, buoyancy, and hydrodynamics, including added mass effects [
35]. Added mass characterizes the inertia of water that needs to be displaced by objects as they accelerate through water, increasing the effective mass. All of this amounts to a high-fidelity simulation that also uses the ArduSub virtual firmware backend.
Despite these appealing characteristics, researchers are likely to encounter challenges in using Gazebo. For projects using ROS 1 (like the subject of this work), there are not any well-documented methods for using Gazebo with ArduSub SITL; the guide available on the ArduSub website [
36] is outdated. Versions of Gazebo that include the high-fidelity plugins described run only on ROS 2, which requires operating systems that do not support ROS 1. Using Gazebo also requires a computer with hardware capabilities able to run the 3D simulation at the desired refresh rate, which can be a considerable bottleneck. However, for those who are not limited by hardware or to ROS 1, there is an actively maintained, at the time of this writing, ArduSub SITL with Gazebo for ROS 2 called Orca4 [
37].
In this study, we will not be using Gazebo and will be exploring and updating the default
sim_vehicle.py simulation model in
SIM_Submarine.cpp. While the default ArduSub SITL environment is a lower fidelity simulation without a 3D visualization component, we update it to use the same hydrodynamic coefficients that Gazebo would, and it can be expected to produce similar results for simple underwater movement not involving collision. The physical properties of the generic vehicle defined as the “
Submarine” object, meant to represent the BROV2, in the SITL source code was inaccurate because it used several crude approximations, underestimated damping effects, and was missing added mass. For a vehicle like the BROV, there is an expected abundance of acceleration and deceleration because it is likely to be performing many stop-and-go manoeuvrers; therefore, a missing added mass factor amounts to significant errors. The specific updates are catalogued in
Section 2.2.4. The virtual frame representing the BROV2 uses the heuristic thruster allocation matrix described in Equation (
7). Nonzero elements are appropriately distributed and the signs are correct, but, as was mentioned, values do not correspond to physical units. The fix for this is to replace the thruster allocation matrix with Equation (
4), which was calculated earlier.
The simulation code calculated vehicle accelerations similar to Algorithm 1, where the thruster allocation matrix was broken into three element vector pieces of the linear and angular components to be looped over for each motor. Linear and angular accelerations were originally calculated by dividing the thrust and torque by vehicle mass and moment of inertia; this operation was treated as a flat scaling operation where the reciprocal of the mass or inertia was multiplied by each element. In our modified code, an element-wise division function was implemented to allow dividing by the different values of the updated combined inertia and added mass vector in a single operation. The angular acceleration calculations originally approximated the vehicle as a sphere with all thrusters equidistant from the center of mass; this was corrected to model the actual geometric locations of the BROV2 thrusters as described in the thruster allocation matrix of Equation (
4). Drag was calculated using the standard drag equation, where the coefficient of drag was set to that of a sphere. This might be an acceptable approximation, but was also implemented erroneously; the fix for this is described in
Appendix A.
2.2.4. Vehicle Simulation Model Update
The original ArduSub vehicle model is inaccurate and produces unrealistic behavior.
Appendix B introduces the full hydrodynamic model that the updates are based on as well as a method for determining control gains for a critically damped linear system approximation which is relevant to
Section 4. As discussed in
Section 2.2.1, the thruster allocation matrix used by SITL to calculate state updates was updated from Equation (
7) to Equation (
4). See the later
Section 3 for a description of the experimental testing that was performed to determine the yaw hydrodynamic model of the BROV2. All 6DOF drag models were updated to follow the generalized form of Equation
11. In order to generate simulation results that have practical usability, that yaw hydrodynamic hydrodynamic model updated yaw and pitch degrees of freedom identically, whereas coefficients in roll were scaled to around 86% of the former to match the proportionality suggested by [
17]. Missing added mass parameters were introduced. Linear degree of freedom parameters or other untested parameters were updated to the values used in [
16]. Vehicle mass was changed from
to
and the center of buoyancy with respect to the center of gravity was changed from
to
. First-order linear drag coefficients were introduced
, as well as first-order angular drag coefficients
. Second-order drag was incorporated with
Ns
2/m and second-order angular drag coefficients with
Ns
2/rad
2. The linear and angular added mass vectors were also introduced, with values of
kg m
2 and
kg m
2/rad, respectively.
Other minor changes included a correction of the application of the transformation matrix of the inertial-to-body frame
to
; previously, this was applied to
. Also, thruster force output calculations were updated to reference a lookup table, see
Section 2 and
Figure 6. In the next sections, the updates to the underlying hydrodynamic models of the autopilot and SITL produce accurate results useful for designing controllers that operate in real water settings.