Hi everyone,
We are developing a custom 8-thruster holonomic AUV running ROS 2 Humble. We have integrated a Water Linked DVL A50 for dead-reckoning position holding and cross-track error correction during forward navigation.
We are encountering a persistent issue: The vehicle moves perfectly straight for the first 5 to 10 meters, but after that distance, it starts drifting to the right and continuously crab-walking (strafing).
Screen Recording / Video of the Issue: (Insert your YouTube/Google Drive video link here)
System Architecture:
-
Vehicle Platform: Custom 8-thruster vector configuration AUV (BlueRobotics T200 thrusters)
-
Onboard Computer: NVIDIA Jetson Orin NX (16GB)
-
DVL: Water Linked DVL A50 (Acoustic bottom-track + dead-reckoning enabled)
-
Sensors: External IMU/Compass (Heading) + MS5837 Depth Sensor
-
Software: ROS 2 Humble node handling Depth, Heading, Pitch/Roll, and DVL-based Position Control.
Problem Description:
-
Initial State (0 to 10m):
-
The vehicle descends to target depth, locks the reference point/heading, and starts navigating forward along the line (lin_x > 0).
-
For the first 5 to 10 meters, cross-track error is minimal and the trajectory is straight.
-
-
Onset of Drift (> 10m):
-
Gradually, a cross-track error accumulates to the right.
-
The controller attempts to correct this error by applying sway correction (strafe_corr), which causes the vehicle to crab-walk (strafe laterally) continuously while still maintaining its nose toward the target heading.
-
Over distance, the sway correction magnitude grows, leading to severe lateral drift.
-
Our Controller Logic (auv_controller.py):
Below is the relevant snippet of our ROS 2 position control loop handling Cross-Track Error calculation and frame transformation:
Python
def _position_control(self, dt):
if not self.pos_hold_enabled or not self.dvl_ready:
self._reset_pos_pid()
return 0.0, 0.0
# ... [Safety & Depth Hold Checks] ...
moving = abs(self.lin_x) > 1e-3
strafing = abs(self.lin_y) > 1e-3
turning = (abs(self.ang_z) > 1e-3) or self.circle_mode
# Reset position reference on mode changes
mode = (moving, strafing, turning)
if (mode != self._prev_pos_mode) or (not self.pos_ref_set):
self._capture_pos_ref()
self._reset_pos_pid()
self._prev_pos_mode = mode
# Inertial frame position deviation from frozen reference
dx = self.dvl_x - self.ref_x
dy = self.dvl_y - self.ref_y
if moving:
# Cross-track error calculation: Ignore along-track error during forward motion
live_yaw = self._frame_yaw_deg()
lpsi = math.radians(live_yaw)
lx, ly = math.cos(lpsi), math.sin(lpsi)
along = dx * lx + dy * ly
dx -= along * lx
dy -= along * ly
# Transform error vector (-dx, -dy) into Vehicle Body Frame
ex, ey = -dx, -dy
cpsi = math.radians(self._frame_yaw_deg())
c, s = math.cos(cpsi), math.sin(cpsi)
err_fwd = ex * c + ey * s
err_right = -ex * s + ey * c # + err_right commands sway to the right
# --- SWAY (STRAFE) PID CONTROL ---
vel_fresh = (time.time() - self.dvl_vel_last_t) < DVL_VEL_TIMEOUT_S
v_right = self.dvl_vy_sign * self.dvl_vy if vel_fresh else 0.0
e = err_right
if abs(e) < self.pos_deadband:
e = 0.0
self.y_integral = 0.0
self.y_integral = _clamp(self.y_integral + e * dt, POS_I_MAX)
# PID using velocity damping (v_right) for D-term
raw = (self.y_Kp * e + self.y_Ki * self.y_integral - self.y_Kd * v_right)
raw = self._apply_deadzone(raw, self.strafe_min_pwm)
strafe_corr = self.strafe_sign * _clamp(raw, self.strafe_max)
return surge_corr, strafe_corr
Questions for the Forum:
-
Heading/Yaw Alignment Sensitivity: Could a small misalignment between the IMU/Compass heading and the DVL internal orientation cause along-track displacement to continuously leak into cross-track error over longer distances (>10m)?
-
Dynamic Waypoint Projection: Is projecting cross-track error using the vehicle’s live yaw inside a continuous leg structurally flawed compared to fixing a static 2D line equation (Ax + By + C = 0) from t0?
-
Hydrodynamic / Thruster Allocation Asymmetry: When applying continuous sway commands to correct drift, does thruster asymmetry typically induce yaw torque that degrades dead-reckoning performance?
-
DVL A50 Configuration: Are there specific filtering or acoustic beam lock parameters in the DVL A50 web interface we should adjust for pool environments?
We would deeply appreciate any recommendations, insights into frame transformation math, or tuning advice!
Thanks in advance!
