Implemented Ivan LOS LQR in ROS2

motion/lqr_controller/config/lqr_config.yaml

@@ -2,6 +2,6 @@
   ros__parameters:
     lqr_controller:
       mass: 50.0
-      D: [10.0, 10.0, 10.0] # Damping coefficients
+      D: [10.0, 10.0, 5.0] # Damping coefficients
       Q: [10.0, 10.0, 5.0, 0.1, 1.0, 5.0]  # State costs for [x, y, heading, surge, sway, yaw]
       R: [1.0, 1.0, 1.0]  # Control cost weight

motion/lqr_controller/lqr_controller/asv_model.py

@@ -2,30 +2,52 @@
 
 class ASV():
 
-    def __init__(self, M, D):
-
+    def __init__(self, M: np.ndarray, D: np.ndarray):
         """
         Initialize some system matrices     
         """
-
-        #m = 50
-        #d = np.array([10, 10, 5])
-
-        #self.M = np.diag(m*np.ones(3))
-        #self.D = np.diag(d)
-
-
         self.M = M
         self.M_inv = np.linalg.inv(self.M)
         self.D = D
 
+    def linearize_model(self, heading: float) -> tuple[np.ndarray, np.ndarray]:
+        """
+        Get a linearization about some heading
+        """
+        J = np.array(
+            [[np.cos(heading), -np.sin(heading), 0],
+            [np.sin(heading), np.cos(heading), 0],
+            [0, 0, 1]]
+        )
 
-    def state_dot(self, state, tau_actuation, V_current = np.zeros(2)):
-
+        A = np.zeros((6,6))
+
+        A[:3,3:] = J
+        A[3:, 3:] = - self.M_inv @ self.D
+
+        B = np.zeros((6,3))
+        B[3:,:] = self.M_inv
+
+        return A, B
+
+    def RK4_integration_step(self, x: np.ndarray, u: np.ndarray, dt: float) -> np.ndarray:
         """
-        Derivative of the state calculated with the non-linear kinematix
+        Perform an integration step using the Runge-Kutta 4 integrator
         """
+        k1 = self.state_dot(x, u)
+        k2 = self.state_dot(x+dt/2*k1, u)
+        k3 = self.state_dot(x+dt/2*k2, u)
+        k4 = self.state_dot(x+dt*k3, u)
+
+        x_next = x + dt/6*(k1+2*k2+2*k3+k4)
+
 
+        return x_next
+
+    def state_dot(self, state: np.ndarray, tau_actuation: np.ndarray, V_current: np.ndarray = np.zeros(2)) -> np.ndarray:
+        """
+        Calculate the derivative of the state using the non-linear kinematics
+        """
         heading = state[2]
 
         J = np.array(
@@ -46,8 +68,9 @@ def state_dot(self, state, tau_actuation, V_current = np.zeros(2)):
         x_dot[0:2] += V_current # add current drift term at velocity level
 
         return x_dot
+
 
-    def linearize_model(self, heading):
+    def linearize_model(self, heading: float) -> tuple[np.ndarray, np.ndarray]:
         """
         Get a linearization about some heading
         """
@@ -67,7 +90,7 @@ def linearize_model(self, heading):
 
         return A, B
 
-    def RK4_integration_step(self, x, u, dt):
+    def RK4_integration_step(self, x: np.ndarray, u: np.ndarray, dt: float) -> np.ndarray:
 
         # integration scheme for simulation, implements the Runge-Kutta 4 integrator
 

motion/lqr_controller/lqr_controller/lqr_controller.py

@@ -12,8 +12,8 @@ def calculate_control_input(x, x_ref, K_LQR, K_r):
 
 class LQRController:
 
-    def __init__(self, m, D, Q, R):
-        self.heading_ref = 50*np.pi/180 # magic init number!!!
+    def __init__(self, m: float, D: list[float], Q: list[float], R: list[float]):
+        self.heading_ref = 30*np.pi/180 # magic init number!!!
 
         self.M = np.diag([m, m, m])
         self.M_inv = np.linalg.inv(self.M)
@@ -30,10 +30,39 @@ def __init__(self, m, D, Q, R):
         self.K_LQR = np.dot(np.dot(np.linalg.inv(self.R), self.B.T), self.P)
         self.K_r = np.linalg.inv(C@np.linalg.inv(self.B @ self.K_LQR - self.A) @ self.B)
 
+        ## Magic init Path parameters parameters
+        self.set_path([0, 0], [20, 20])
+        self.calculate_R_Pi_p()
+
+    def set_path(self, p0: list[float], p1: list[float]):
+        self.p0 = np.array(p0)
+        self.p1 = np.array(p1)
+
+    def calculate_R_Pi_p(self):
+        self.Pi_p = np.arctan2(self.p1[1]-self.p0[1], self.p1[0]-self.p0[0])
+        self.R_Pi_p = np.array(
+            [[np.cos(self.Pi_p), -np.sin(self.Pi_p)],
+            [np.sin(self.Pi_p), np.cos(self.Pi_p)]]
+        )
+
+    def calculate_LOS_x_ref(self, x: np.ndarray, look_ahead: float) -> np.ndarray:
+        """
+        generate reference at the look-ahead distance
+        """
+        p_asv = np.array([x[0], x[1]])
+        errors = self.R_Pi_p.T @ (p_asv - self.p0)
+        along_track_error = errors[0]
+        p_los_world = self.R_Pi_p @ np.array([along_track_error + look_ahead, 0]) + self.p0
+
+        x_ref = np.array([p_los_world[0], p_los_world[1], self.heading_ref])
+        return x_ref
+
     def run_ivan_sim(self):
-        x_init = np.array([-10, -10, 40*np.pi/180, 0, 0, 0])
+        x_init = np.array([10, -20, 40*np.pi/180, 0, 0, 0])
         x_ref = np.array([0, 0, self.heading_ref])
 
+        look_ahead = 5 # Look ahead distance
+
         T = 69.0        # Total simulation time
         dt = 0.01       # Time step
         num_steps = int(T / dt)  # Number of time steps
@@ -44,11 +73,22 @@ def run_ivan_sim(self):
         u_history = np.zeros((num_steps+1, self.B.shape[1]))
         x_ref_history = np.zeros((num_steps+1, np.shape(x_ref)[0]))
 
+        cross_track_error_history = np.zeros(num_steps+1)
+
         # Simulation loop
         x = x_init # Initial state
         for i in range(num_steps+1):
             #x_ref[i] = reference_function_const(i * dt)  # Update the reference at each time step
-            x_ref_history[i, :] = x_ref  # Update the reference at each time step
+            # x_ref_history[i, :] = x_ref  # Update the reference at each time step
+
+            # generate reference at the look-ahead distance
+            p_asv = np.array([x[0], x[1]])
+            errors = self.R_Pi_p.T @ (p_asv - self.p0)
+            along_track_error = errors[0]
+            p_los_world = self.R_Pi_p @ np.array([along_track_error + look_ahead, 0]) + self.p0
+
+            x_ref[:2] = p_los_world # Update the position reference at each time step
+            x_ref_history[i, :] = x_ref
 
             u = calculate_control_input(x, x_ref, self.K_LQR, self.K_r)  # Calculate control input 'u' at each time step
             x = self.asv.RK4_integration_step(x, u, dt)
@@ -75,12 +115,19 @@ def run_ivan_sim(self):
 
         plt.subplot(3, 1, 3)
         plt.scatter(x_history[:,0], x_history[:,1], label=f'Position')
-        plt.xlabel('Time')
-        plt.ylabel('Control Input Value')
+        plt.plot([self.p0[0], self.p1[0]], [self.p0[1], self.p1[1]], 'r-', label='Path')
+        plt.xlabel('x')
+        plt.ylabel('y')
         plt.legend()
 
         plt.tight_layout()
         plt.show()
+        plt.figure(1)
+        plt.plot(time, cross_track_error_history, label="cross track error")
+        plt.axis("equal")
+        plt.plot(time, np.zeros_like(time))
+        plt.legend()
+        plt.show()
 
 
 

motion/lqr_controller/package.xml

@@ -8,6 +8,7 @@
   <license>MIT</license>
 
   <depend>rclpy</depend>
+  <depend>geometry_msgs</depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>