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

improve LQT steering and speed control document (#1047)

Browse Source 
* improve LQT steering and speed control

* improve LQT steering and speed control

* improve LQT steering and speed control doc

* improve LQT steering and speed control doc

* improve LQT steering and speed control doc

* improve LQT steering and speed control doc
This commit is contained in:
Atsushi Sakai
2024-07-16 23:02:31 +09:00
committed by GitHub
parent 26deca19b6
commit 1e101d1f8a
8 changed files with 149 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py
View File

@@ -55,6 +55,7 @@ def update(state, a, delta):
def pi_2_pi(angle):
return angle_mod(angle)
def solve_dare(A, B, Q, R):
"""
solve a discrete time_Algebraic Riccati equation (DARE)
@@ -221,8 +222,9 @@ def do_simulation(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")
@@ -290,6 +292,13 @@ def main():
plt.xlabel("x[m]")
plt.ylabel("y[m]")
plt.legend()
plt.subplots(1)
plt.plot(t, np.array(v)*3.6, label="speed")
plt.grid(True)
plt.xlabel("Time [sec]")
plt.ylabel("Speed [m/s]")
plt.legend()
plt.subplots(1)
plt.plot(s, [np.rad2deg(iyaw) for iyaw in cyaw], "-r", label="yaw")