diff --git a/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py b/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py index a70e30d378..d85fa98a84 100644 --- a/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py +++ b/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") diff --git a/docs/modules/path_tracking/lqr_speed_and_steering_control/lqr_speed_and_steering_control_main.rst b/docs/modules/path_tracking/lqr_speed_and_steering_control/lqr_speed_and_steering_control_main.rst index 68ea9c88b2..a0b145456c 100644 --- a/docs/modules/path_tracking/lqr_speed_and_steering_control/lqr_speed_and_steering_control_main.rst +++ b/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] `_ + +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: ~~~~~~~~~~~ diff --git a/docs/modules/path_tracking/lqr_speed_and_steering_control/lqr_steering_control_model.jpg b/docs/modules/path_tracking/lqr_speed_and_steering_control/lqr_steering_control_model.jpg new file mode 100644 index 0000000000..83754d5bb0 Binary files /dev/null and b/docs/modules/path_tracking/lqr_speed_and_steering_control/lqr_steering_control_model.jpg differ diff --git a/docs/modules/path_tracking/lqr_speed_and_steering_control/speed.png b/docs/modules/path_tracking/lqr_speed_and_steering_control/speed.png new file mode 100644 index 0000000000..ae99eb7ea3 Binary files /dev/null and b/docs/modules/path_tracking/lqr_speed_and_steering_control/speed.png differ diff --git a/docs/modules/path_tracking/lqr_speed_and_steering_control/x-y.png b/docs/modules/path_tracking/lqr_speed_and_steering_control/x-y.png new file mode 100644 index 0000000000..bff3f1a786 Binary files /dev/null and b/docs/modules/path_tracking/lqr_speed_and_steering_control/x-y.png differ diff --git a/docs/modules/path_tracking/lqr_speed_and_steering_control/yaw.png b/docs/modules/path_tracking/lqr_speed_and_steering_control/yaw.png new file mode 100644 index 0000000000..7f3d9c1d10 Binary files /dev/null and b/docs/modules/path_tracking/lqr_speed_and_steering_control/yaw.png differ diff --git a/docs/modules/path_tracking/lqr_steering_control/lqr_steering_control_main.rst b/docs/modules/path_tracking/lqr_steering_control/lqr_steering_control_main.rst index fcc9b5278a..baca7a33fc 100644 --- a/docs/modules/path_tracking/lqr_steering_control/lqr_steering_control_main.rst +++ b/docs/modules/path_tracking/lqr_steering_control/lqr_steering_control_main.rst @@ -8,6 +8,8 @@ control. .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/lqr_steer_control/animation.gif +`[Code Link] `_ + 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,6 +74,9 @@ 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: @@ -79,7 +84,7 @@ the state vector `x` and control input vector `u` are defined as follows: .. 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 diff --git a/tests/test_codestyle.py b/tests/test_codestyle.py index a7d11d270f..55e558c184 100644 --- a/tests/test_codestyle.py +++ b/tests/test_codestyle.py @@ -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' )