DDFV

Linear Quadratic Regulator (LQR) With Python Code Example – Aut... https://automaticaddison.com/linear-quadratic-regulator-lqr-with-pytho...
1 of 25 09/07/2021 03:21
2 of 25 09/07/2021 03:21
3 of 25 09/07/2021 03:21
4 of 25 09/07/2021 03:21
5 of 25 09/07/2021 03:21
6 of 25 09/07/2021 03:21
7 of 25 09/07/2021 03:21
γ
ω
8 of 25 09/07/2021 03:21
9 of 25 09/07/2021 03:21
10 of 25 09/07/2021 03:21
11 of 25 09/07/2021 03:21
12 of 25 09/07/2021 03:21
13 of 25 09/07/2021 03:21
14 of 25 09/07/2021 03:21
15 of 25 09/07/2021 03:21
16 of 25 09/07/2021 03:21
17 of 25 09/07/2021 03:21
18 of 25 09/07/2021 03:21
19 of 25 09/07/2021 03:21
1 import numpy as np
2
3 # Author: Addison Sears-Collins
4 # https://automaticaddison.com
5 # Description: Linear Quadratic Regulator example
6 # (two-wheeled differential drive robot car)
7
8 ######################## DEFINE CONSTANTS ###############################
9 # Supress scientific notation when printing NumPy arrays
10 np.set_printoptions(precision=3,suppress=True)
11
12 # Optional Variables
13 max_linear_velocity = 3.0 # meters per second
14 max_angular_velocity = 1.5708 # radians per second
15
16 def getB(yaw, deltat):
17 """
18 Calculates and returns the B matrix
19 3x2 matix ---> number of states x number of control inputs
20 of 25 09/07/2021 03:21
20
21 Expresses how the state of the system [x,y,yaw] changes
22 from t-1 to t due to the control commands (i.e. control inputs).
23
24 :param yaw: The yaw angle (rotation angle around the z axis) in radia
25 :param deltat: The change in time from timestep t-1 to t in seconds
26
27 :return: B matrix ---> 3x2 NumPy array
28 """
29 B = np.array([ [np.cos(yaw)*deltat, 0],
30 [np.sin(yaw)*deltat, 0],
31 [0, deltat]])
32 return B
33
34
35 def state_space_model(A, state_t_minus_1, B, control_input_t_minus_1):
36 """
37 Calculates the state at time t given the state at time t-1 and
38 the control inputs applied at time t-1
39
40 :param: A The A state transition matrix
41 3x3 NumPy Array
42 :param: state_t_minus_1 The state at time t-1
43 3x1 NumPy Array given the state is [x,y,yaw angle] --->
44 [meters, meters, radians]
45 :param: B The B state transition matrix
46 3x2 NumPy Array
47 :param: control_input_t_minus_1 Optimal control inputs at time t-
48 2x1 NumPy Array given the control input vector is
49 [linear velocity of the car, angular velocity of the car]
50 [meters per second, radians per second]
51
52 :return: State estimate at time t
55 """
56 # These next 6 lines of code which place limits on the angular and li
57 # velocities of the robot car can be removed if you desire.
58 control_input_t_minus_1[0] = np.clip(control_input_t_minus_1[
59
60
61 control_input_t_minus_1[1] = np.clip(control_input_t_minus_1[
62
63
64 state_estimate_t = (A @ state_t_minus_1) + (B @ control_input_t_minus
65
66 return state_estimate_t
67
68 def lqr(actual_state_x, desired_state_xf, Q, R, A, B, dt):
69 """
70 Discrete-time linear quadratic regulator for a nonlinear system.
71
72 Compute the optimal control inputs given a nonlinear system, cost mat
73 current state, and a final state.
74
75 Compute the control variables that minimize the cumulative cost.
76
77 Solve for P using the dynamic programming method.
21 of 25 09/07/2021 03:21
78
79 :param actual_state_x: The current state of the system
82 :param desired_state_xf: The desired state of the system
85 :param Q: The state cost matrix
86 3x3 NumPy Array
87 :param R: The input cost matrix
88 2x2 NumPy Array
89 :param dt: The size of the timestep in seconds -> float
90
91 :return: u_star: Optimal action u for the current state
92 2x1 NumPy Array given the control input vector is
93 [linear velocity of the car, angular velocity of the car]
94 [meters per second, radians per second]
95 """
96 # We want the system to stabilize at desired_state_xf.
97 x_error = actual_state_x - desired_state_xf
98
99 # Solutions to discrete LQR problems are obtained using the dynamic
100 # programming method.
101 # The optimal solution is obtained recursively, starting at the last
102 # timestep and working backwards.
103 # You can play with this number
104 N = 50
105
106 # Create a list of N + 1 elements
107 P = [None] * (N + 1)
108
109 Qf = Q
110
111 # LQR via Dynamic Programming
112 P[N] = Qf
113
114 # For i = N, ..., 1
115 for i in range(N, 0, -1):
116
117 # Discrete-time Algebraic Riccati equation to calculate the optim
118 # state cost matrix
119 P[i-1] = Q + A.T @ P[i] @ A - (A.T @ P[i] @ B) @ np.linalg.pinv(
120 R + B.T @ P[i] @ B) @ (B.T @ P[i] @ A)
121
122 # Create a list of N elements
123 K = [None] * N
124 u = [None] * N
125
126 # For i = 0, ..., N - 1
127 for i in range(N):
128
129 # Calculate the optimal feedback gain K
130 K[i] = -np.linalg.pinv(R + B.T @ P[i+1] @ B) @ B.T @ P[i
131
132 u[i] = K[i] @ x_error
133
134 # Optimal control input is u_star
135 u_star = u[N-1]
22 of 25 09/07/2021 03:21
136
137 return u_star
138
139 def main():
140
141 # Let the time interval be 1.0 seconds
142 dt = 1.0
143
144 # Actual state
145 # Our robot starts out at the origin (x=0 meters, y=0 meters), and
146 # the yaw angle is 0 radians.
147 actual_state_x = np.array([0,0,0])
148
149 # Desired state [x,y,yaw angle]
150 # [meters, meters, radians]
151 desired_state_xf = np.array([2.000,2.000,np.pi/2])
152
153 # A matrix
154 # 3x3 matrix -> number of states x number of states matrix
155 # Expresses how the state of the system [x,y,yaw] changes
156 # from t-1 to t when no control command is executed.
157 # Typically a robot on wheels only drives when the wheels are told to
158 # For this case, A is the identity matrix.
159 # Note: A is sometimes F in the literature.
160 A = np.array([ [1.0, 0, 0],
161 [ 0,1.0, 0],
162 [ 0, 0, 1.0]])
163
164 # R matrix
165 # The control input cost matrix
166 # Experiment with different R matrices
167 # This matrix penalizes actuator effort (i.e. rotation of the
168 # motors on the wheels that drive the linear velocity and angular vel
169 # The R matrix has the same number of rows as the number of control
170 # inputs and same number of columns as the number of
171 # control inputs.
172 # This matrix has positive values along the diagonal and 0s elsewhere
173 # We can target control inputs where we want low actuator effort
174 # by making the corresponding value of R large.
175 R = np.array([[0.01, 0], # Penalty for linear velocity effort
176 [ 0, 0.01]]) # Penalty for angular velocity effort
177
178 # Q matrix
179 # The state cost matrix.
180 # Experiment with different Q matrices.
181 # Q helps us weigh the relative importance of each state in the
182 # state vector (X, Y, YAW ANGLE).
183 # Q is a square matrix that has the same number of rows as
184 # there are states.
185 # Q penalizes bad performance.
186 # Q has positive values along the diagonal and zeros elsewhere.
187 # Q enables us to target states where we want low error by making the
188 # corresponding value of Q large.
189 Q = np.array([[0.639, 0, 0], # Penalize X position error
190 [0, 1.0, 0], # Penalize Y position error
191 [0, 0, 1.0]]) # Penalize YAW ANGLE headin
192
193 # Launch the robot, and have it move to the desired goal destination
23 of 25 09/07/2021 03:21
194 for i in range(100):

195 print(f'iteration = {i} seconds')
196 print(f'Current State = {actual_state_x}')
197 print(f'Desired State = {desired_state_xf}')
198
199 state_error = actual_state_x - desired_state_xf
200 state_error_magnitude = np.linalg.norm(state_error)
201 print(f'State Error Magnitude = {state_error_magnitude}'
202
203 B = getB(actual_state_x[2], dt)
204
205 # LQR returns the optimal control input
206 optimal_control_input = lqr(actual_state_x,
207 desired_state_xf,
208 Q, R, A, B, dt)
209
210 print(f'Control Input = {optimal_control_input}')
211
212
213 # We apply the optimal control to the robot
214 # so we can get a new actual (estimated) state.
215 actual_state_x = state_space_model(A, actual_state_x, B,
216 optimal_control_input)
217
218 # Stop as soon as we reach the goal
219 # Feel free to change this threshold value.
220 if state_error_magnitude < 0.01:
221 print("\nGoal Has Been Reached Successfully!")
222 break
223
224 print()
225
226 # Entry point for the program
227 main()
24 of 25 09/07/2021 03:21
25 of 25 09/07/2021 03:21

DDFV

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

DDFV

Uploaded by

Copyright:

Available Formats

Linear Quadratic Regulator (LQR) With Python Code Example – Aut... https://automaticaddison.com/linear-quadratic-regulator-lqr-with-pytho...

194 for i in range(100):

You might also like