Online Model Identification for Underwater Vehicles - 16299 Final Project

My final project for 16299.

1. A robotic submarine

Our vehicle is equipped with a few major systems that enable it to navigate through its environment.

• IMU - Measures linear acceleration and angular velocity. Onboard proprietary algorithms provide orientation estimates.
• DVL (Doppler Velocity Log) - Measures linear velocity.
• Thrusters (8x) - Applies force and torque to the vehicle.

To perform state estimation, our vehicle fuses linear velocity as measured by the DVL with orientation estimates from the IMU. By integrating velocity over time, we are able to keep track of our position. Since we can measure linear velocity directly using the DVL, with very high accuracy and an estimate of uncertainty that feeds into the EKF, our state estimation system can enable station-keeping over long periods of time. The performance is much better than what could be achieved by double-integrating linear acceleration.

To perform station-keeping, we use PD controllers on each of the 6 degrees of freedom of the vehicle to keep its pose as close to a target pose as possible. Because of hydrodynamic effects like buoyancy and drag, the dynamics of the vehicle are highly nonlinear. So, we decouple the nonlinearities in an inverse dynamics model that sits between the thrusters and the PD controller. Instead of commanding forces and torques directly, the PD controllers command an acceleration for a specific degree of freedom; the inverse dynamics model calculates the necessary wrench to achieve the commanded acceleration based on the current state of the vehicle.

We use the particular 6-DOF vector dynamics model published by Fossen in the Handbook of Marine Craft Hydrodynamics and Motion Control. In this model, the state consists of the orientation of the vehicle and its linear and angular velocities. The model uses 32 parameters, including mass, volume, centers of gravity and buoyancy, moments of inertia, and drag coefficients. Finding reasonable values for these parameters poses a significant challenge because many of them, like drag coefficients, cannot be directly measured or calculated from first principles. They must be estimated from data describing the motion of the vehicle instead.

Even if a good set of parameters is procured by some method, a particular problem of interest is what happens when the dynamics of the vehicle change during operation. For example, a gripper on the front of the vehicle might pick up a dense metal object, which increases the effective mass of the vehicle and shifts the center of gravity forward. The PD controllers have no ability to compensate for steady-state disturbances like this; with the current control stack, the vehicle would remain in a new equilibrium point, pitched forward and sunk below its target pose.

To alleviate this issue, we developed a system that is capable of performing online estimation of these model parameters. It runs in the background, watching the motion of the vehicle, and continually adjusts the model parameters to better match reality.

In brief, our system watches the wrench being applied to the vehicle and uses the current parameter estimates to calculate the expected acceleration due to the wrench. It then compares this expected acceleration to the resulting measured acceleration, and updates the parameter estimates to minimize the error between the two.

In practice, our implementation looks like a training routine for a neural network using gradient descent, except our "network" has a fixed structure defined by the model. We implemented the model in PyTorch using batched matrix operations, which allowed us to use Torch's autograd functionality to compute the gradient of the acceleration error with respect to each of the parameters.

Our control system runs at 50 Hz, so our implementation accumulates batches of 50 datapoints consisting of the state, wrench, and measured acceleration before updating once a second. An update step involves simply calculating the expected acceleration based on the states, wrenches, and current parameters, and then calculating the MSE error between the expected acceleration and the measured acceleration. We then step the parameters by some small amount in a direction opposite that of of the gradient.

We took advantage of the batched implementation of our model to run quick simulations of the performance of our system. We used the torchode library to compute the trajectories of a simulated vehicle.

Our simulation setup included a vehicle with an inverse dynamics model with parameters that perfectly match the simulation model. After some time, the simulation model parameters undergo a step change, rendering the inverse dynamics parameters incorrect. Without our model identification system updating the inverse dynamics parameters, the vehicle fails to regain its original pose. With the system running, the inverse dynamics parameters quickly update to match the simulation parameters, and the vehicle successfully maintains its pose.

Our software stack is based on ROS, so we converted our implementation into a ROS node. This node subscribes to the state estimation and wrench topics to observe the vehicle's state. When it has produced updated parameters, it calls a service to send them to the inverse dynamics controller. For the curious reader, our full codebase is available on Github; the relevant files are included here as well.

To validate our system on a real vehicle, we set up a test in the Field Robotics Center's water tank. We tied a large mass (a few pounds of scrap metal) to the front of the vehicle, and carried the load of the mass with a second rope to the surface. Once the vehicle was holding its position, we released the second rope, transferring the load to the vehicle and creating a step change in the mass and center of gravity of the vehicle.

We filmed the vehicle with the system engaged and disengaged, and found that its behavior matched our expectations in each case. With the system disengaged, the additional mass caused the vehicle to pitch forward and sink. With the system engaged, the vehicle still pitched forward and sank, but recovered after a few seconds and returned to its original pose. When we then removed the additional mass, the vehicle floated, showing that its inverse dynamics model now accounted for the additional mass added to the vehicle.