github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-04-22 03:00:22 -04:00
Code Issues Packages Projects Releases Wiki Activity

Enhance lqr steering control docs (#1015)

Browse Source 
* enhance lqr steering control docs

* enhance lqr steering control docs

* enhance lqr steering control docs

* enhance lqr steering control docs

* improve lqr steering control docs
This commit is contained in:
Atsushi Sakai
2024-05-25 23:31:54 +09:00
committed by GitHub
parent cfb736316a
commit 5751829bab
4 changed files with 111 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
PathTracking/lqr_steer_control/lqr_steer_control.py
View File

@@ -55,7 +55,7 @@ def update(state, a, delta):
return state
def PIDControl(target, current):
def pid_control(target, current):
a = Kp * (target - current)
return a
@@ -70,10 +70,11 @@ def solve_DARE(A, B, Q, R):
solve a discrete time_Algebraic Riccati equation (DARE)
"""
X = Q
maxiter = 150
Xn = Q
max_iter = 150
eps = 0.01
for i in range(maxiter):
for i in range(max_iter):
Xn = A.T @ X @ A - A.T @ X @ B @ \
la.inv(R + B.T @ X @ B) @ B.T @ X @ A + Q
if (abs(Xn - X)).max() < eps:
@@ -178,7 +179,7 @@ def closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal):
dl, target_ind, e, e_th = lqr_steering_control(
state, cx, cy, cyaw, ck, e, e_th)
ai = PIDControl(speed_profile[target_ind], state.v)
ai = pid_control(speed_profile[target_ind], state.v)
state = update(state, ai, dl)
if abs(state.v) <= stop_speed:
@@ -202,8 +203,9 @@ def closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal):
if target_ind % 1 == 0 and show_animation:
plt.cla()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.plot(cx, cy, "-r", label="course")
plt.plot(x, y, "ob", label="trajectory")
plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")