improve LQT steering and speed control document (#1047)

* 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

Author: AtsushiSakai

PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py

@@ -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")

docs/modules/path_tracking/lqr_speed_and_steering_control/lqr_speed_and_steering_control_main.rst

@@ -7,6 +7,133 @@ Path tracking simulation with LQR speed and steering control.
 
 .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/lqr_speed_steer_control/animation.gif
 
+`[Code Link] <https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py>`_
+
+Overview
+~~~~~~~~
+
+The LQR (Linear Quadratic Regulator) speed and steering control model implemented in `lqr_speed_steer_control.py` provides a simulation
+for an autonomous vehicle to track:
+
+1. A desired speed by adjusting acceleration based on feedback from the current state and the desired speed.
+
+2. A desired trajectory by adjusting steering angle based on feedback from the current state and the desired trajectory.
+
+by only using one LQT controller.
+
+Vehicle motion Model
+~~~~~~~~~~~~~~~~~~~~~
+
+The below figure shows the geometric model of the vehicle used in this simulation:
+
+.. image:: lqr_steering_control_model.jpg
+   :width: 600px
+
+The `e`, :math:`{\theta}`, and :math:`\upsilon` represent the lateral error, orientation error, and velocity error, respectively, with respect to the desired trajectory and speed.
+And :math:`\dot{e}` and :math:`\dot{\theta}` represent the rates of change of these errors.
+
+The :math:`e_t` and :math:`\theta_t`, and :math:`\upsilon` are the updated values of `e`, :math:`\theta`, :math:`\upsilon` and at time `t`, respectively, and can be calculated using the following kinematic equations:
+
+.. math:: e_t = e_{t-1} + \dot{e}_{t-1} dt
+
+.. math:: \theta_t = \theta_{t-1} + \dot{\theta}_{t-1} dt
+
+.. math:: \upsilon_t = \upsilon_{t-1} + a_{t-1} dt
+
+Where `dt` is the time difference and :math:`a_t` is the acceleration at the time `t`.
+
+The change rate of the `e` can be calculated as:
+
+.. math:: \dot{e}_t = V \sin(\theta_{t-1})
+
+Where `V` is the current vehicle speed.
+
+If the :math:`\theta` is small,
+
+.. math:: \theta \approx 0
+
+the :math:`\sin(\theta)` can be approximated as :math:`\theta`:
+
+.. math:: \sin(\theta) = \theta
+
+So, the change rate of the `e` can be approximated as:
+
+.. math:: \dot{e}_t = V \theta_{t-1}
+
+The change rate of the :math:`\theta` can be calculated as:
+
+.. math:: \dot{\theta}_t = \frac{V}{L} \tan(\delta)
+
+where `L` is the wheelbase of the vehicle and :math:`\delta` is the steering angle.
+
+If the :math:`\delta` is small,
+
+.. math:: \delta \approx 0
+
+the :math:`\tan(\delta)` can be approximated as :math:`\delta`:
+
+.. math:: \tan(\delta) = \delta
+
+So, the change rate of the :math:`\theta` can be approximated as:
+
+.. math:: \dot{\theta}_t = \frac{V}{L} \delta
+
+The above equations can be used to update the state of the vehicle at each time step.
+
+Control Model
+~~~~~~~~~~~~~~
+
+To formulate the state-space representation of the vehicle dynamics as a linear model,
+the state vector `x` and control input vector `u` are defined as follows:
+
+.. math:: x_t = [e_t, \dot{e}_t, \theta_t, \dot{\theta}_t, \upsilon_t]^T
+
+.. math:: u_t = [\delta_t, a_t]^T
+
+The linear state transition equation can be represented as:
+
+.. math:: x_{t+1} = A x_t + B u_t
+
+where:
+
+:math:`\begin{equation*} A = \begin{bmatrix} 1 & dt & 0 & 0 & 0\\ 0 & 0 & v & 0 & 0\\ 0 & 0 & 1 & dt & 0\\ 0 & 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 0 & 1\\\end{bmatrix} \end{equation*}`
+
+:math:`\begin{equation*} B = \begin{bmatrix} 0 & 0\\ 0 & 0\\ 0 & 0\\ \frac{v}{L} & 0\\ 0 & dt \\ \end{bmatrix} \end{equation*}`
+
+LQR controller
+~~~~~~~~~~~~~~~
+
+The Linear Quadratic Regulator (LQR) controller is used to calculate the optimal control input `u` that minimizes the quadratic cost function:
+
+:math:`J = \sum_{t=0}^{N} (x_t^T Q x_t + u_t^T R u_t)`
+
+where `Q` and `R` are the weighting matrices for the state and control input, respectively.
+
+for the linear model:
+
+:math:`x_{t+1} = A x_t + B u_t`
+
+The optimal control input `u` can be calculated as:
+
+:math:`u_t = -K x_t`
+
+where `K` is the feedback gain matrix obtained by solving the Riccati equation.
+
+Simulation results
+~~~~~~~~~~~~~~~~~~~
+
+
+.. image:: x-y.png
+   :width: 600px
+
+.. image:: yaw.png
+   :width: 600px
+
+.. image:: speed.png
+   :width: 600px
+
+
+
 References:
 ~~~~~~~~~~~
 

docs/modules/path_tracking/lqr_speed_and_steering_control/lqr_steering_control_model.jpg

@@ -8,6 +8,8 @@ control.
 
 .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/lqr_steer_control/animation.gif
 
+`[Code Link] <https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathTracking/lqr_steer_control/lqr_steer_control.py>`_
+
 Overview
 ~~~~~~~~
 
@@ -23,10 +25,10 @@ The below figure shows the geometric model of the vehicle used in this simulatio
 .. image:: lqr_steering_control_model.jpg
    :width: 600px
 
-The `e` and `theta` represent the lateral error and orientation error, respectively, with respect to the desired trajectory.
+The `e` and :math:`\theta` represent the lateral error and orientation error, respectively, with respect to the desired trajectory.
 And :math:`\dot{e}` and :math:`\dot{\theta}` represent the rates of change of these errors.
 
-The :math:`e_t` and :math:`theta_t` are the updated values of `e` and `theta` at time `t`, respectively, and can be calculated using the following kinematic equations:
+The :math:`e_t` and :math:`\theta_t` are the updated values of `e` and :math:`\theta` at time `t`, respectively, and can be calculated using the following kinematic equations:
 
 .. math:: e_t = e_{t-1} + \dot{e}_{t-1} dt
 
@@ -38,9 +40,9 @@ The change rate of the `e` can be calculated as:
 
 .. math:: \dot{e}_t = V \sin(\theta_{t-1})
 
-Where `V` is the vehicle speed.
+Where `V` is the current vehicle speed.
 
-If the :math:`theta` is small,
+If the :math:`\theta` is small,
 
 .. math:: \theta \approx 0
 
@@ -72,14 +74,17 @@ So, the change rate of the :math:`\theta` can be approximated as:
 
 The above equations can be used to update the state of the vehicle at each time step.
 
+Control Model
+~~~~~~~~~~~~~~
+
 To formulate the state-space representation of the vehicle dynamics as a linear model,
 the state vector `x` and control input vector `u` are defined as follows:
 
 .. math:: x_t = [e_t, \dot{e}_t, \theta_t, \dot{\theta}_t]^T
 
 .. math:: u_t = \delta_t
 
-The state transition equation can be represented as:
+The linear state transition equation can be represented as:
 
 .. math:: x_{t+1} = A x_t + B u_t
 

docs/modules/path_tracking/lqr_speed_and_steering_control/speed.png

@@ -54,7 +54,7 @@ def run_ruff(files, fix):
         return 0, ""
     args = ['--fix'] if fix else []
     res = subprocess.run(
-        ['ruff', f'--config={CONFIG}'] + args + files,
+        ['ruff', 'check', f'--config={CONFIG}'] + args + files,
         stdout=subprocess.PIPE,
         encoding='utf-8'
     )