Scribd Logo
Upload

Welcome to Scribd!

  • 38 views

    Dokumen

    Uploaded by

    Rizky Aditya
    This document provides code for implementing a linear quadratic regulator (LQR) controller to navigate a two-wheeled differential drive robot from its starting position to a desired goal position. It defines constants, functions for calculating state transition matrices, an LQR function to compute optimal control inputs, and a main function that uses LQR at each time step to iteratively plan the robot's motion until the goal is reached.

    Copyright:

    © All Rights Reserved
    Download as DOCX, PDF, TXT or read online from Scribd

    Dokumen

    Uploaded by

    Rizky Aditya
    38 views8 pages
    This document provides code for implementing a linear quadratic regulator (LQR) controller to navigate a two-wheeled differential drive robot from its starting position to a desired goal position. It defines constants, functions for calculating state transition matrices, an LQR function to compute optimal control inputs, and a main function that uses LQR at each time step to iteratively plan the robot's motion until the goal is reached.

    Original Title

    Dokumen (1)

    Copyright

    © © All Rights Reserved

    Available Formats

    DOCX, PDF, TXT or read online from Scribd

    Did you find this document useful?

    Is this content inappropriate?

    This document provides code for implementing a linear quadratic regulator (LQR) controller to navigate a two-wheeled differential drive robot from its starting position to a desired goal position. It defines constants, functions for calculating state transition matrices, an LQR function to compute optimal control inputs, and a main function that uses LQR at each time step to iteratively plan the robot's motion until the goal is reached.

    Copyright:

    © All Rights Reserved
    Download as DOCX, PDF, TXT or read online from Scribd
    Download as docx, pdf, or txt
    38 views8 pages

    Dokumen

    Uploaded by

    Rizky Aditya
    This document provides code for implementing a linear quadratic regulator (LQR) controller to navigate a two-wheeled differential drive robot from its starting position to a desired goal position. It defines constants, functions for calculating state transition matrices, an LQR function to compute optimal control inputs, and a main function that uses LQR at each time step to iteratively plan the robot's motion until the goal is reached.

    Copyright:

    © All Rights Reserved
    Download as DOCX, PDF, TXT or read online from Scribd
    Download as docx, pdf, or txt
    of 8

    Import numpy as np

    # Author: Addison Sears-Collins

    # https://automaticaddison.com

    # Description: Linear Quadratic Regulator example

    # (two-wheeled differential drive robot car)

    ######################## DEFINE CONSTANTS #####################################

    # Supress scientific notation when printing NumPy arrays

    Np.set_printoptions(precision=3,suppress=True)

    # Optional Variables

    Max_linear_velocity = 3.0 # meters per second

    Max_angular_velocity = 1.5708 # radians per second

    Def getB(yaw, deltat):

    “””

    Calculates and returns the B matrix

    3x2 matix - number of states x number of control inputs

    Expresses how the state of the system [x,y,yaw] changes

    From t-1 to t due to the control commands (i.e. control inputs).

    :param yaw: The yaw angle (rotation angle around the z axis) in radians

    :param deltat: The change in time from timestep t-1 to t in seconds

    :return: B matrix - 3x2 NumPy array

    “””

    B = np.array([ [np.cos(yaw)*deltat, 0],

    [np.sin(yaw)*deltat, 0],

    [0, deltat]])
    Return B

    Def state_space_model(A, state_t_minus_1, B, control_input_t_minus_1):

    “””

    Calculates the state at time t given the state at time t-1 and

    The control inputs applied at time t-1

    :param: A The A state transition matrix

    3x3 NumPy Array

    :param: state_t_minus_1 The state at time t-1

    3x1 NumPy Array given the state is [x,y,yaw angle] -

    [meters, meters, radians]

    :param: B The B state transition matrix

    3x2 NumPy Array

    :param: control_input_t_minus_1 Optimal control inputs at time t-1

    2x1 NumPy Array given the control input vector is

    [linear velocity of the car, angular velocity of the car]

    [meters per second, radians per second]

    :return: State estimate at time t

    3x1 NumPy Array given the state is [x,y,yaw angle] -

    [meters, meters, radians]

    “””

    # These next 6 lines of code which place limits on the angular and linear

    # velocities of the robot car can be removed if you desire.

    Control_input_t_minus_1[0] = np.clip(control_input_t_minus_1[0],

    -max_linear_velocity,

    Max_linear_velocity)

    Control_input_t_minus_1[1] = np.clip(control_input_t_minus_1[1],

    -max_angular_velocity,
    Max_angular_velocity)

    State_estimate_t = (A @ state_t_minus_1) + (B @ control_input_t_minus_1)

    Return state_estimate_t

    Def lqr(actual_state_x, desired_state_xf, Q, R, A, B, dt):

    “””

    Discrete-time linear quadratic regulator for a nonlinear system.

    Compute the optimal control inputs given a nonlinear system, cost matrices,

    Current state, and a final state.

    Compute the control variables that minimize the cumulative cost.

    Solve for P using the dynamic programming method.

    :param actual_state_x: The current state of the system

    3x1 NumPy Array given the state is [x,y,yaw angle] -

    [meters, meters, radians]

    :param desired_state_xf: The desired state of the system

    3x1 NumPy Array given the state is [x,y,yaw angle] -

    [meters, meters, radians]

    :param Q: The state cost matrix

    3x3 NumPy Array

    :param R: The input cost matrix

    2x2 NumPy Array

    :param dt: The size of the timestep in seconds -> float

    :return: u_star: Optimal action u for the current state

    2x1 NumPy Array given the control input vector is

    [linear velocity of the car, angular velocity of the car]


    [meters per second, radians per second]

    “””

    # We want the system to stabilize at desired_state_xf.

    X_error = actual_state_x – desired_state_xf

    # Solutions to discrete LQR problems are obtained using the dynamic

    # programming method.

    # The optimal solution is obtained recursively, starting at the last

    # timestep and working backwards.

    # You can play with this number

    N = 50

    # Create a list of N + 1 elements

    P = [None] * (N + 1)

    Qf = Q

    # LQR via Dynamic Programming

    P[N] = Qf

    # For i = N, ..., 1

    For i in range(N, 0, -1):

    # Discrete-time Algebraic Riccati equation to calculate the optimal

    # state cost matrix

    P[i-1] = Q + A.T @ P[i] @ A – (A.T @ P[i] @ B) @ np.linalg.pinv(

    R + B.T @ P[i] @ B) @ (B.T @ P[i] @ A)

    # Create a list of N elements

    K = [None] * N

    U = [None] * N
    # For i = 0, ..., N – 1

    For i in range(N):

    # Calculate the optimal feedback gain K

    K[i] = -np.linalg.pinv(R + B.T @ P[i+1] @ B) @ B.T @ P[i+1] @ A

    U[i] = K[i] @ x_error

    # Optimal control input is u_star

    U_star = u[N-1]

    Return u_star

    Def main():

    # Let the time interval be 1.0 seconds

    Dt = 1.0

    # Actual state

    # Our robot starts out at the origin (x=0 meters, y=0 meters), and

    # the yaw angle is 0 radians.

    Actual_state_x = np.array([0,0,0])

    # Desired state [x,y,yaw angle]

    # [meters, meters, radians]

    Desired_state_xf = np.array([2.000,2.000,np.pi/2])

    # A matrix

    # 3x3 matrix -> number of states x number of states matrix

    # Expresses how the state of the system [x,y,yaw] changes


    # from t-1 to t when no control command is executed.

    # Typically a robot on wheels only drives when the wheels are told to turn.

    # For this case, A is the identity matrix.

    # Note: A is sometimes F in the literature.

    A = np.array([ [1.0, 0, 0],

    [ 0,1.0, 0],

    [ 0, 0, 1.0]])

    # R matrix

    # The control input cost matrix

    # Experiment with different R matrices

    # This matrix penalizes actuator effort (i.e. rotation of the

    # motors on the wheels that drive the linear velocity and angular velocity).

    # The R matrix has the same number of rows as the number of control

    # inputs and same number of columns as the number of

    # control inputs.

    # This matrix has positive values along the diagonal and 0s elsewhere.

    # We can target control inputs where we want low actuator effort

    # by making the corresponding value of R large.

    R = np.array([[0.01, 0], # Penalty for linear velocity effort

    [ 0, 0.01]]) # Penalty for angular velocity effort

    # Q matrix

    # The state cost matrix.

    # Experiment with different Q matrices.

    # Q helps us weigh the relative importance of each state in the

    # state vector (X, Y, YAW ANGLE).

    # Q is a square matrix that has the same number of rows as

    # there are states.

    # Q penalizes bad performance.

    # Q has positive values along the diagonal and zeros elsewhere.


    # Q enables us to target states where we want low error by making the

    # corresponding value of Q large.

    Q = np.array([[0.639, 0, 0], # Penalize X position error

    [0, 1.0, 0], # Penalize Y position error

    [0, 0, 1.0]]) # Penalize YAW ANGLE heading error

    # Launch the robot, and have it move to the desired goal destination

    For i in range(100):

    Print(f’iteration = {i} seconds’)

    Print(f’Current State = {actual_state_x}’)

    Print(f’Desired State = {desired_state_xf}’)

    State_error = actual_state_x – desired_state_xf

    State_error_magnitude = np.linalg.norm(state_error)

    Print(f’State Error Magnitude = {state_error_magnitude}’)

    B = getB(actual_state_x[2], dt)

    # LQR returns the optimal control input

    Optimal_control_input = lqr(actual_state_x,

    Desired_state_xf,

    Q, R, A, B, dt)

    Print(f’Control Input = {optimal_control_input}’)

    # We apply the optimal control to the robot

    # so we can get a new actual (estimated) state.

    Actual_state_x = state_space_model(A, actual_state_x, B,

    Optimal_control_input)
    # Stop as soon as we reach the goal

    # Feel free to change this threshold value.

    If state_error_magnitude < 0.01:

    Print(“\nGoal Has Been Reached Successfully!”)

    Break

    Print()

    # Entry point for the program

    Main()

    You might also like