Skip to content

3.2 Controls

Jump to bottom
Anthony Tan edited this page Sep 16, 2023 · 24 revisions

Controls

The control system's primary task is to listen to desired setpoint commands from the planner and continually generate an effort (force/torque) that is most likely to guide the AUV towards the desired setpoint.

Table of Contents

Overview

The controls package operates in two modes: State mode and Superimposer mode. In both cases, the control system runs an action server that listens to commands from an action client in the planner package. When the server receives a command, it is processed depending on the mode of operation.

State Mode

In State mode, the control system comprises three layers:

  1. State Server: The state server listens to state goals from the planner. State goals may contain four optional degrees of freedom: x, y, z, and orientation. These degrees of freedom are optional, meaning not all of them need to be enabled. If a degree of freedom is not enabled, the control system makes no effort to adjust the AUV's pose in that specific degree of freedom. For example, if the state server receives a state goal that only specifies a z position, the AUV will ascend or descend to the desired depth without controlling its x and y position or orientation. After receiving a goal, the state server publishes setpoints to the PID nodes. It monitors the progress of the goal and informs the planner when the goal pose has been achieved and maintained for a certain period.

  2. PID nodes: Detailed information about PID and quaternions is covered later in this document. There are four PID nodes: x, y, z, and quaternion (orientation). Each PID node is responsible for a specific degree of freedom. The x, y, and z PID nodes publish topics called global_x, global_y, and global_z, respectively. These topics represent the desired force to be exerted by the thrusters in the global coordinate frame. It's important to note that the effort published by the controls is body-framed, not global-framed. The transformation to the body frame is handled by the superimposer. The quaternion PID node is responsible for running PID controls on the AUV's rotation. Unlike the x, y, and z PID nodes, the quaternion PID node outputs three topics: roll, pitch, and yaw. These topics correspond to the x, y, and z components of the desired torque.

  3. Superimposer: The superimposer subscribes to the global_x, global_y, global_z, roll, pitch, and yaw topics and combines them into a single effort wrench. The global x, y, z forces are combined into a single global force vector and rotated to find the desired linear force within the AUV's body frame. This force constitutes the force component of the wrench. The torque component of the wrench is simply the roll, pitch, and yaw values. No further transformation is necessary for the torques.

Superimposer Mode

In Superimposer mode, the controls package consists of two layers:

  1. Effort Server: The effort server is an action server that listens to effort goals from the planner package. An effort goal consists of six optional effort components: force x, y, z, and torque x, y, z. If a specific degree of freedom is specified, that value is passed to the superimposer to be incorporated into the effort. For the force x, y, z degrees of freedom, the topics are surge, sway, and heave. These forces are body-framed. For the torque x, y, z degrees of freedom, the topics are roll, pitch, and yaw.

  2. Superimposer: The superimposer subscribes to the surge, sway, heave, roll, pitch, and yaw topics and combines them into a single effort vector.

It's worth noting that these modes of operation are not mutually exclusive and can be used simultaneously. For instance, you can use the state server to maintain the AUV's orientation while simultaneously using the effort server to apply a constant surge force.

Usage

Interacting with the controls package should be done using an action client in the planner. The planner features a Controller class that provides abstractions for all control actions. You can find this class in controller.py. In a mission plan, you can instantiate a controller object from controller.py and use its methods to control the AUV.

Launch Files

Dependencies

PID Control Simplified

In this section, we'll break down the PID (Proportional-Integral-Derivative) control algorithm into simpler terms, using the example of a chef trying to cook the perfect omelet, to help you understand how it guides our Autonomous Underwater Vehicle (AUV).

What is PID Control?

Think of PID control as a smart system that helps our AUV stay on course. It's similar to a chef who wants to cook the perfect omelet. The chef uses the stove's heat to control the cooking process, making sure the omelet is just right, not too runny and not too burnt.

Three Parts of PID Control

  1. Proportional (P) Part: Imagine the chef notices that one side of the omelet is cooking faster than the other. The chef adjusts the heat to balance it out. The P-part of PID control works the same way. It looks at the difference between where we want to be (the desired position) and where we are now (the current position), and it makes small adjustments to get us back on track.

  2. Integral (I) Part: Sometimes, the chef realizes that the pan is a bit too hot overall, causing the entire omelet to cook too quickly. The I-part helps by considering the overall cooking experience and making gradual adjustments. It ensures that any long-term cooking errors, like uneven heat, are corrected over time.

  3. Derivative (D) Part: Now, imagine the chef is whisking eggs and notices that the mixture is getting too frothy. The chef eases up on the whisking to prevent overdoing it. The D-part of PID control is similar; it looks at how quickly things are changing and makes sure our adjustments are smooth, preventing overshooting or overcorrection.

How PID Control Works in Our AUV

Our AUV uses PID controllers to keep track of different things:

  • X, Y, Z Controllers: These controllers monitor the AUV's position in the water: left-right, front-back, and up-down. If it's not where it's supposed to be, these controllers make sure we get back on course.

  • Quaternion Controller: This controller is responsible for maintaining the AUV's desired rotation.

Tuning the PID Controllers

Fine-tuning PID controllers is like the chef adjusting the stove's heat to get the perfect omelet. We adjust these settings to make sure our AUV moves smoothly and doesn't overreact. It's about finding the right balance to achieve the desired outcome, just like the chef aims for the perfect omelet every time.

Remember, getting the PID controllers just right takes some practice and fine-tuning, similar to how a chef perfects their omelet recipe. It ensures our AUV can complete its tasks underwater with precision, just like a chef creates a delicious omelet with the perfect texture and flavor.

Understanding Quaternions for Rotations

This section explains what quaternions are and how they are used to represent rotations in a straightforward and accessible way.

What are Quaternions?

Quaternions are mathematical objects used to describe rotations in three-dimensional space. Unlike other representations of rotations, such as Euler angles or matrices, quaternions offer some unique advantages, especially when dealing with computer graphics, robotics, and orientation control.

Why Quaternions?

Imagine you want to describe the orientation of an object, like the orientation of an AUV underwater. You might think of using angles to specify how it's rotated around its x, y, and z axes. While this works for many cases, it can lead to a problem known as "gimbal lock" in certain situations. Gimbal lock occurs when two of the axes align, making it challenging to define the object's orientation accurately.

Quaternions come to the rescue because they don't suffer from gimbal lock. They provide a more robust way to represent rotations and are especially useful in scenarios where precision and stability matter.

How Quaternions Work

A quaternion is typically written as "q = w + xi + yj + zk," where:

  • w is the scalar part.
  • x, y, and z are the vector part.

The scalar part (w) represents the rotation's angle, while the vector part (x, y, z) specifies the axis of rotation.

Using Quaternions for Rotations

Here's a simple analogy to understand how quaternions work for rotations:

Imagine you have a toy spaceship floating in space. To describe its orientation using a quaternion, you can think of the quaternion as a magical key that, when turned, makes the spaceship rotate in a particular direction and by a certain amount. The w part of the quaternion is like the amount you turn the key, and the x, y, and z parts determine the axis around which the spaceship rotates.

When you combine multiple rotations, you can multiply their corresponding quaternions together, just like turning a series of keys to orient your spaceship as you desire.

Benefits of Using Quaternions

  1. No Gimbal Lock: Quaternions avoid gimbal lock, which can be a problem when using other rotation representations.

  2. Smooth Interpolation: Quaternions provide smooth and predictable interpolation between different orientations, making animations and transitions look more natural.

  3. Efficient Calculations: Quaternion operations are computationally efficient, making them suitable for real-time applications like robotics and computer graphics.

In the context of our control package, quaternions are used to maintain and control the orientation of the Autonomous Underwater Vehicle (AUV). They help ensure that the AUV can accurately and stably maneuver in three-dimensional space underwater.

The Inverse of a Quaternion

What Is the Inverse?

The inverse of a quaternion is like the "opposite" or "undo" operation for rotations. When you multiply a quaternion by its inverse, the result is a quaternion representing no rotation at all. This is useful when you need to reverse a rotation or cancel out a previous rotation.

How to Calculate the Inverse

To find the inverse of a quaternion q, you negate its vector part (x, y, z) and leave the scalar part (w) unchanged. Mathematically:

q_inverse = -x * i - y * j - z * k + w

Practical Use

Imagine you've rotated an object using a quaternion, and now you want to bring it back to its original orientation. You can simply multiply the quaternion representing the rotation with its inverse, and the result will be the identity quaternion (no rotation).

Examples:

Lets say the world frame is a, the AUV's frame is b, and the desired body frame is c.

Ex 1:

If we know the quaternion from frame a to b = Qa_b, and the quaternion from frame b to c = Qb_c, how can we solve for the quaternion from a to c? Answer: Qa_c = Qa_b * Qb_c

Ex 2:

If we know the quaternion from frame a to b = Qa_b, and the quaternion from a to c = Qa_c, how can we solve for the quaternion from b to c? Answer: Qb_c = Qa_b^-1 * Qa_c

Quaternion PID

Unlike x,y,z PID, when running a PID loop on the orientation, you cannot split up the rotation into three independent components, they must be considered all at once. The first step is to solve for the error quaternion. This is the quaternion from the current orientation to the desired orientation. If we know q_nwu_auv and q_nwu_desired, then q_auv_desired is q_nwu_auv^-1 * q_nwu_desired. Now that we have q_auv_desired, we know how to rotate the auv to the desired frame. What's left is to actually perform this rotation. The x,y,z components of q_auv_desired are proportional to the actual axis of rotation expressed in axis angle form. Furthermore, the closer the auv is to its desired rotation, the closer these values approach zero.

Let eta be the x,y,z components of the quaternion. The control law is Kp * eta + Kd * w + Ki* integral(eta) where w is the error in desired angular velocity. The proportional term returns a torque value that rotates the auv toward its desired rotation. The derivative term gives a torque that opposes the error in angular velocity. For simple setpoint control, this adds a damping term to the velocity. If the AUV enters a wild spins somehow, this takes over and torques the auv out of the spin. The integral term ramps up when the auv's rotation gets stuck.

Sidebar

Clone this wiki locally