From 9a48d1d89a11621f22ec176daf4028c0ce0ae812 Mon Sep 17 00:00:00 2001 From: Cyprian Date: Wed, 9 Oct 2024 17:51:45 +0200 Subject: [PATCH] LQR controller for pitch yaw and surge with working and stable values. --- .../scripts/velocity_controller_lqr.py | 117 ++++++++++-------- 1 file changed, 66 insertions(+), 51 deletions(-) diff --git a/control/velocity_controller_lqr/scripts/velocity_controller_lqr.py b/control/velocity_controller_lqr/scripts/velocity_controller_lqr.py index 9d031bb25..9908b44b5 100644 --- a/control/velocity_controller_lqr/scripts/velocity_controller_lqr.py +++ b/control/velocity_controller_lqr/scripts/velocity_controller_lqr.py @@ -29,15 +29,20 @@ def quaternion_to_euler_angle(w, x, y, z): return X, Y, Z +# A function to convert the angle to the smallest signed angle +def ssa(angle): + if angle > np.pi: + angle -= 2 * np.pi + elif angle < -np.pi: + angle += 2 * np.pi + return angle # Function to calculate the coriolis matrix -def calculate_coriolis_matrix(self, pitch_rate, yaw_rate): - return np.array([[1.629 * pitch_rate, 0], [0, 1.769 * yaw_rate]]) - +def calculate_coriolis_matrix(self, pitch_rate, yaw_rate, sway, heave): + return np.array([[0.2,-30*sway*0.01, -30*heave*0.01], [30*sway*0.01, 0, 1.629 * pitch_rate], [30*heave*0.01 ,1.769 * yaw_rate, 0]]) #----------------------------------------------------------------Controller Node---------------------------------------------------------------- - class VelocityLQRNode(Node): def __init__(self): @@ -56,50 +61,57 @@ def __init__(self): self.topic_subscriber = self.create_subscription( Odometry, "/nucleus/odom", self.nucleus_callback, 10) - self.guidance_values = np.array([np.pi / 4, np.pi / 4 + self.guidance_values = np.array([0.4, -np.pi / 8, -np.pi / 4 ]) # guidance values TEMPORARY self.guidance_values_aug = np.array( - [np.pi / 4, np.pi / 4, 0.0]) # augmented guidance values TEMPORARY + [0.4, -np.pi / 8, -np.pi / 4, 0.0, 0.0, 0.0]) # augmented guidance values TEMPORARY # TODO: state space model, Anders showed me the chapter in the book from page 55 on for this - self.M = np.array([[1.629, 0], - [0, 1.769]]) # mass matrix with mass = 30kg + self.M = np.array([[30, 0.6, 0], + [0.6, 1.629, 0], + [0, 0, 1.769]]) # mass matrix with mass = 30kg + self.M_inv = np.linalg.inv(self.M) # inverse of mass matrix - self.C = np.array([[0, 0], [0, 0]]) + + self.C = np.zeros((3, 3)) # Coriolis matrix + self.A = np.eye(3) # depending on number of states + self.B = np.eye(3) # depending on number of control inputs - self.A = np.eye(2) # depending on number of states - self.B = np.eye(2) # depending on number of control inputs - - self.A_aug = np.eye(3) # Augmented A matrix - self.B_aug = np.eye(3) # Augmented B matrix + self.A_aug = np.eye(6) # Augmented A matrix + self.B_aug = np.eye(6, 3) # Augmented B matrix # LQR controller parameters - self.Q = np.diag([300, 25]) # state cost matrix - self.R = np.eye(2) * 0.5 # control cost matrix - - P_I = 0.5 # Augmented state cost for pitch - Y_I = 0.0 # Augmented state cost for yaw + self.Q = np.diag([100, 100, 25]) # state cost matrix + self.R = np.diag([0.5, 0.5, 0.5]) # control cost matrix - self.Q_aug = np.block([[self.Q, np.zeros((2, 1))], - [np.zeros((1, 2)), - P_I]]) # Augmented state cost matrix + self.I_surge = 0.5 # Augmented state cost for surge + self.I_pitch = 0.5 # Augmented state cost for pitch + self.I_yaw = 0.1 # Augmented state cost for yaw + + self.Q_aug = np.block([[self.Q, np.zeros((3, 3))], + [np.zeros((3, 3)), np.diag([self.I_surge, self.I_pitch, self.I_yaw])]]) # Augmented state cost matrix - self.z = 0.0 + self.z_surge = 0.0 + self.z_pitch = 0.0 + self.z_yaw = 0.0 # Augmented states for surge, pitch and yaw - # State vector 1. pitch, 2. yaw - self.states = np.array([0, 0]) + # State vector 1. surge, 2. pitch, 3. yaw + self.states = np.array([0.0, 0.0, 0.0]) #---------------------------------------------------------------Callback Functions--------------------------------------------------------------- def nucleus_callback(self, msg: Odometry): # callback function - dummy, self.states[0], self.states[1] = quaternion_to_euler_angle( + dummy, self.states[1], self.states[2] = quaternion_to_euler_angle( msg.pose.pose.orientation.w, msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z) - def guidance_callback( - self, - msg: Float32MultiArray): # Function to read data from guidance + self.states[0] = msg.twist.twist.linear.x + + self.C = calculate_coriolis_matrix( + self, msg.twist.twist.angular.y, msg.twist.twist.angular.z, msg.twist.twist.linear.y, msg.twist.twist.angular.z) #coriolis matrix + + def guidance_callback(self, msg: Float32MultiArray): # Function to read data from guidance # self.guidance_values = msg.data pass @@ -111,47 +123,50 @@ def LQR_controller(self): # The LQR controller function # Coriolis matrix #self.C = calculate_coriolis_matrix(self, msg.twist.twist.angular.x, msg.twist.twist.angular.z) - self.C = calculate_coriolis_matrix( - self, 1.0, 1.0) # TEMPORARY linearization of the coriolis matrix self.A = -self.M_inv @ self.C self.B = self.M_inv - self.A_aug = np.block([[self.A, np.zeros((2, 1))], - [-1, 0, 0]]) # Augmented A matrix for pitch - self.B_aug = np.block([[self.B], [0, 0]]) # Augmented B matrix + # Augment the A and B matrices for integrators for surge, pitch, and yaw + self.A_aug = np.block([[self.A, np.zeros((3, 3))], [-np.eye(3), np.zeros((3,3))]]) # Integrators added for surge, pitch, and yaw + self.B_aug = np.block([[self.B], [np.zeros((3, 3))]]) # Control input does not affect integrators directly # CT LQR controller from control library python self.K, self.S, self.E = ct.lqr(self.A, self.B, self.Q, self.R) - self.K_aug, self.S_aug, self.E_aug = ct.lqr(self.A_aug, self.B_aug, - self.Q_aug, self.R) + self.K_aug, self.S_aug, self.E_aug = ct.lqr(self.A_aug, self.B_aug, self.Q_aug, self.R) + + surge_error = self.guidance_values[0] - self.states[0] # Surge error no need for angle wrapping + pitch_error = ssa(self.guidance_values[1] - self.states[1]) # Apply ssa to pitch error + yaw_error = ssa(self.guidance_values[2] - self.states[2]) # Apply ssa to yaw error + + # self.get_logger().info(f"{surge_error}, {pitch_error}, {yaw_error}") + + # Update the integrators for surge, pitch, and yaw - # Control input like: u = -self.K * (desired-states) - #u = self.K @ (self.guidance_values - self.states) - self.z = self.z + self.guidance_values[0] - self.states[0] - u_aug = self.K_aug @ (self.guidance_values_aug - - np.block([self.states, self.z])) + self.z_surge += self.I_surge*surge_error*1.5 # surge integrator + self.z_pitch += self.I_pitch*pitch_error*2 # pitch integrator + self.z_yaw += self.I_yaw*yaw_error # yaw integrator - # Control input - #msg.torque.y = u[0] - #msg.torque.z = u[1] + # Augmented state vector including integrators + u_aug = -self.K_aug @ (-surge_error, -pitch_error, -yaw_error, self.z_surge, self.z_pitch, self.z_yaw) # Augmented control input # Augmented control input - msg.torque.y = u_aug[0] - msg.torque.z = u_aug[1] + msg.force.x = u_aug[0] + msg.torque.y = u_aug[1] + msg.torque.z = u_aug[2] #Publish the control input self.publisherLQR.publish(msg) + self.get_logger().info( - f"Pitch input: {msg.torque.y}, Yaw input: {msg.torque.z}") + f"Surge input: {msg.force.x} Pitch input: {msg.torque.y}, Yaw input: {msg.torque.z}") def publish_states(self): msg = Float32MultiArray() - # DATA: 1: pitch, 2: yaw, 3: guidance pitch, 4: guidance yaw + # DATA: 1: surge 2: pitch, 3: yaw, 4: guidance surge 5: guidance pitch, 6: guidance yaw msg.data = [ - self.states[0], self.states[1], self.guidance_values[0], - self.guidance_values[1] - ] + self.states[0], self.states[1], self.states[2], self.guidance_values[0], self.guidance_values[1], + self.guidance_values[2]] self.publisher_states.publish(msg)