From 16444507c126f78cdb179271306ac48e922b5aac Mon Sep 17 00:00:00 2001 From: Capek System Safety & Robotic Solutions <96583804+MelkorBalrog@users.noreply.github.com> Date: Sat, 26 Jul 2025 19:11:02 -0400 Subject: [PATCH] docs: clarify pure pursuit formulas --- README.md | 353 +++++++++++++++++++++++++++--------------------------- 1 file changed, 177 insertions(+), 176 deletions(-) diff --git a/README.md b/README.md index 6e35b20..e546f09 100644 --- a/README.md +++ b/README.md @@ -56,18 +56,16 @@ The plots update in real time as the simulation runs. ### Physics Includes `KinematicsCalculator`, `ForceCalculator`, `DynamicsUpdater`, `CollisionDetector`, `VehicleCollisionSeverity`, `SurfaceFrictionManager` and `StabilityChecker`. These functions can be compiled to MEX for faster execution via `Scripts/Wrappers/generate_mex`. Key equations: -- Wheel slip ratio: \(\kappa = (\omega R - v_x) / \max(v_x, 0.1)\) -- Lateral slip angle: \(\alpha = \tan^{-1}(v_y / |v_x|)\) -- Aerodynamic drag: \(F_d = 0.5 \times C_d \times A \times \rho \times v^2\) +- Wheel slip ratio: $\kappa = (\omega R - v_x) / \max(v_x, 0.1)$ +- Lateral slip angle: $\alpha = \tan^{-1}(v_y / |v_x|)$ +- Aerodynamic drag: $F_d = 0.5 \times C_d \times A \times \rho \times v^2$ ### Graphics Objects such as `Vehicle3D`, `Road3D` and `World3D` render 3‑D scenes for the optional `Sim3DAnimator`. Screenshots may be saved automatically at each frame for later video generation. ### Mechanics Contains drivetrain and suspension models (`Engine`, `Transmission`, `BrakeSystem`, `Clutch`, `LeafSpringSuspension`, `Pacejka96TireModel`, etc.). The tire model uses the Pacejka 1996 formula: -```math -F_y = D \times \sin\bigl(C \times \tan^{-1}(B\alpha - E(B\alpha - \tan^{-1}(B\alpha)))\bigr) -``` +$F_y = D \times \sin\bigl(C \times \tan^{-1}(B\alpha - E(B\alpha - \tan^{-1}(B\alpha)))\bigr)$ where `B`, `C`, `D` and `E` are stiffness parameters. #### Mechanical Model Blocks @@ -75,15 +73,15 @@ The main mechanical components behave like interconnected black boxes with clear inputs and outputs. Their relationships can be visualized using Mermaid: ```mermaid flowchart LR - Throttle -->|"\theta_{th}"| Engine - Engine -->|"T_e,\omega_e"| Clutch - Wheels -->|"\omega_w"| Clutch + Throttle -->|"θ_th"| Engine + Engine -->|"T_e, ω_e"| Clutch + Wheels -->|"ω_w"| Clutch Clutch -->|"T_c"| Transmission Transmission -->|"T_w"| Differential - Differential -->|"T_{axle}"| Wheels + Differential -->|"T_axle"| Wheels BrakeSystem -->|"T_b"| Wheels - Ackermann -->|"\delta_i,\delta_o"| Wheels - Wheels -->|"\kappa,\alpha"| Pacejka[Pacejka Tire Model] + Ackermann -->|"δ_i, δ_o"| Wheels + Wheels -->|"κ, α"| Pacejka[Pacejka Tire Model] Pacejka -->|"F_x,F_y"| Chassis Suspension -->|"F_s"| Chassis Chassis -->|"u,v,r"| ForceCalc @@ -91,30 +89,20 @@ flowchart LR Dynamics -->|"RK4"| Motion ``` * **Engine** – accepts throttle position and produces engine torque - ```math - T_e = T_{curve}(\omega_e) \times \theta_{th} - ``` +$T_e = T_{curve}(\omega_e) \times \theta_{th}$ limited by `maxTorque`. * **Clutch** – transmits torque when engaged using - ```math - T_c = K_{clutch} \times (\omega_e - \omega_w) - ``` +$T_c = K_{clutch} \times (\omega_e - \omega_w)$ * **Transmission** – multiplies clutch torque by the selected gear ratio and final drive: - ```math - T_w = T_c \times gearRatio \times finalDriveRatio - ``` +$T_w = T_c \times gearRatio \times finalDriveRatio$ * **BrakeSystem** – converts the brake pedal command into braking torque applied to the wheels. - ```math - F_{brake} = u_b \times \mathrm{maxBrakingForce} \times \mathrm{brakeEfficiency} - ``` +$F_{brake} = u_b \times \mathrm{maxBrakingForce} \times \mathrm{brakeEfficiency}$ The resulting force is distributed between the axles according to the brake bias. * **LeafSpringSuspension** – generates suspension force - ```math - F_s = -K \times \Delta x - C \times v - ``` +$F_s = -K \times \Delta x - C \times v$ from spring displacement and velocity. * **AckermannGeometry** – maps steering wheel angle to left and right wheel angles for proper turning radii. @@ -125,25 +113,23 @@ flowchart LR ##### Interfaces Each mechanical block acts as a black box with defined inputs and outputs: -- **Throttle** – input: pedal command `u_{th}`; output: throttle position - `\theta_{th}`. -- **Engine** – input: `\theta_{th}`; output engine torque `T_e` as above. +- **Throttle** – input: pedal command `u_th`; output: throttle position + $\theta_{th}$. +- **Engine** – input: $\theta_{th}$; output engine torque $T_e$ as above. - **Clutch** – input: engagement state, engine and wheel speeds; - outputs transmitted torque `T_c`. -- **Transmission** – input: `T_c` and gear number; output wheel torque - `T_w` and updated gear ratio. + outputs transmitted torque $T_c$. +- **Transmission** – input: $T_c$ and gear number; output wheel torque + $T_w$ and updated gear ratio. - **BrakeSystem** – input: brake command `u_b`; output braking torque - ```math - T_b = u_b \times \mathrm{maxBrakingForce} \times \mathrm{brakeEfficiency} - ``` -- **LeafSpringSuspension** – inputs: spring deflection `\Delta x` and - velocity `v`; output suspension force `F_s`. -- **AckermannGeometry** – input: desired steering angle `\delta`; + $T_b = u_b \times \mathrm{maxBrakingForce} \times \mathrm{brakeEfficiency}$ +- **LeafSpringSuspension** – inputs: spring deflection $\Delta x$ and + velocity `v`; output suspension force $F_s$. +- **AckermannGeometry** – input: desired steering angle $\delta$; outputs inner and outer wheel angles calculated via - \(\tan\delta_{i,o} = L/(R \mp W/2)\) where `L` is wheelbase and + $\tan\delta_{i,o} = \tfrac{L}{R \mp W/2}$ where `L` is wheelbase and `W` track width. -- **Pacejka96TireModel** – inputs: slip angle `\alpha`, slip ratio `\kappa` - and normal load `F_z`; outputs lateral and longitudinal forces using the +- **Pacejka96TireModel** – inputs: slip angle $\alpha$, slip ratio $\kappa$ + and normal load $F_z$; outputs lateral and longitudinal forces using the formulas above. - **HitchModel** – inputs: tractor and trailer states; outputs hitch forces, moments and articulation angle integrated with RK4. @@ -155,55 +141,53 @@ inputs, outputs and a short physical model. ###### Throttle ```mermaid flowchart LR - u_th(["u_{th}"]) --> Throttle[Throttle] - Throttle --> theta_th(["\theta_{th}"]) + u_th(["u_th"]) --> Throttle[Throttle] + Throttle --> theta_th(["θ_th"]) ``` -*Inputs*: driver command `u_{th}` -*Outputs*: opening angle `\theta_{th}` +*Inputs*: driver command `u_th` +*Outputs*: opening angle $\theta_{th}$ The throttle filters the driver command and rate limits the change in opening. The actual valve position is computed via a saturation nonlinearity: -```math -\theta_{th} = \mathrm{sat}(u_{th}) -``` +$\theta_{th} = \mathrm{sat}(u_th)$ When the clutch is disengaged the delivered opening becomes -\(\theta_{adj}=\theta_{th}(1-e)\) where `e` is the clutch engagement percentage. +$\theta_{adj}=\theta_{th}(1-e)$ where `e` is the clutch engagement percentage. ###### Engine ```mermaid flowchart LR - theta_th(["\theta_{th}"]) + theta_th(["θ_th"]) load_torque(["T_{load}"]) theta_th --> Engine load_torque --> Engine Engine --> T_e(["T_e"]) - Engine --> omega_e(["\omega_e"]) + Engine --> omega_e(["ω_e"]) ``` -*Inputs*: `\theta_{th}`, load torque `T_{load}` -*Outputs*: engine torque `T_e`, engine speed `\omega_e` +*Inputs*: $\theta_{th}$, load torque $T_{load}$ +*Outputs*: engine torque $T_e$, engine speed $\omega_e$ -Represents a diesel engine whose torque curve \(T_e = f(\omega_e)\) is measured +Represents a diesel engine whose torque curve $T_e = f(\omega_e)$ is measured from test data. The instantaneous output is -\(T_e = f(\omega_e) \times \theta_{th}\) limited by `maxTorque`. Engine speed evolves as -\(\dot{\omega}_e = (T_e - T_{load})/I_e\) where `I_e` is engine inertia. +$T_e = f(\omega_e) \times \theta_{th}$ limited by `maxTorque`. Engine speed evolves as +$\dot{\omega}_e = \tfrac{T_e - T_{load}}{I_e}$ where `I_e` is engine inertia. ###### Clutch ```mermaid flowchart LR engage(["e"]) - omega_e(["\omega_e"]) - omega_w(["\omega_w"]) + omega_e(["ω_e"]) + omega_w(["ω_w"]) engage --> Clutch omega_e --> Clutch omega_w --> Clutch Clutch --> T_c(["T_c"]) ``` *Inputs*: engagement percentage `e`, engine and wheel speeds -*Outputs*: transmitted torque `T_c` +*Outputs*: transmitted torque $T_c$ Torque transfer depends on clutch engagement: -\(T_c = (1-e) \times T_{max}\) with `T_{max}` the clutch capacity. +$T_c = (1-e) \times T_{max}$ with `T_{max}` the clutch capacity. ###### Transmission ```mermaid @@ -214,13 +198,11 @@ flowchart LR gear --> Transmission Transmission --> T_w(["T_w"]) ``` -*Inputs*: `T_c`, selected gear `g` -*Outputs*: wheel torque `T_w` +*Inputs*: $T_c$, selected gear `g` +*Outputs*: wheel torque $T_w$ Wheel torque is amplified by the gear and final drive: -```math - T_w = T_c \times \mathrm{gearRatio}(g) \times finalDrive -``` +$T_w = T_c \times \mathrm{gearRatio}(g) \times finalDrive$ ###### BrakeSystem ```mermaid @@ -230,12 +212,10 @@ flowchart LR BrakeSystem --> F_brake(["F_{brake}"]) ``` *Inputs*: brake command `u_b` -*Outputs*: braking force `F_{brake}` +*Outputs*: braking force $F_{brake}$ The pedal command (u_b) is mapped to the total braking force: -```math -F_{brake} = u_b \times \mathrm{maxBrakingForce} \times \mathrm{brakeEfficiency} -``` +$F_{brake} = u_b \times \mathrm{maxBrakingForce} \times \mathrm{brakeEfficiency}$ This force is then split between the axles according to the brake bias. ###### LeafSpringSuspension @@ -247,32 +227,32 @@ flowchart LR vel --> Suspension Suspension --> F_s(["F_s"]) ``` -*Inputs*: spring deflection `\Delta x`, velocity `v` -*Outputs*: suspension force `F_s` +*Inputs*: spring deflection $\Delta x$, velocity `v` +*Outputs*: suspension force $F_s$ Suspension forces use a spring damper relation -\(F_s = -K \times \Delta x - C \times v\) and include load transfer from lateral/longitudinal +$F_s = -K \times \Delta x - C \times v$ and include load transfer from lateral/longitudinal acceleration. ###### AckermannGeometry ```mermaid flowchart LR - delta(["\delta"]) + delta(["δ"]) delta --> Ackermann - Ackermann --> delta_i(["\delta_i"]) - Ackermann --> delta_o(["\delta_o"]) + Ackermann --> delta_i(["δ_i"]) + Ackermann --> delta_o(["δ_o"]) ``` -*Input*: desired steering angle `\delta` -*Outputs*: inner/outer wheel angles `\delta_i`,`\delta_o` +*Input*: desired steering angle $\delta$ +*Outputs*: inner/outer wheel angles $\delta_i$, $\delta_o$ The geometry obeys -`\tan\delta_{i,o}=L/(R\mp W/2)` where `L` is wheelbase and `W` track width. +$\tan\delta_{i,o} = \tfrac{L}{R \mp W/2}$ where `L` is wheelbase and `W` track width. ###### Pacejka96TireModel and PacejkaMagicFormula ```mermaid flowchart LR - alpha(["\alpha"]) - kappa(["\kappa"]) + alpha(["α"]) + kappa(["κ"]) Fz(["F_z"]) alpha --> Tire kappa --> Tire @@ -280,11 +260,11 @@ flowchart LR Tire --> F_x(["F_x"]) Tire --> F_y(["F_y"]) ``` -*Inputs*: slip angle `\alpha`, slip ratio `\kappa`, normal load `F_z` -*Outputs*: tire forces `F_x`,`F_y` +*Inputs*: slip angle $\alpha$, slip ratio $\kappa$, normal load $F_z$ +*Outputs*: tire forces $F_x$, $F_y$ Both tire models implement the Pacejka equations to provide longitudinal and -lateral grip based on `\alpha` and `\kappa`. + lateral grip based on $\alpha$ and $\kappa$. ###### HitchModel ```mermaid @@ -292,16 +272,14 @@ flowchart LR states(["states"]) states --> Hitch Hitch --> F_h(["F_h"]) - Hitch --> delta(["\delta"]) + Hitch --> delta(["δ"]) ``` *Inputs*: tractor/trailer states *Outputs*: hitch forces and articulation angle The hitch imposes a rotational spring\–damper torque -```math -M_h = k_h \times \delta + c_h \times \dot\delta -``` -where `\delta` is the articulation angle between tractor and trailer. +$M_h = k_h \times \delta + c_h \times \dot\delta$ +where $\delta$ is the articulation angle between tractor and trailer. `HitchModel` integrates this angle with the same Runge\--Kutta 4 scheme used for the trailer yaw rate. @@ -354,18 +332,18 @@ flowchart LR Controllers --> th Controllers --> br Controllers --> st - th -->|"u_{th}"| Throttle - Throttle -->|"\theta_{th}"| Engine - Engine -->|"T_e,\omega_e"| Clutch - Wheels -->|"\omega_w"| Clutch + th -->|"u_th"| Throttle + Throttle -->|"θ_th"| Engine + Engine -->|"T_e, ω_e"| Clutch + Wheels -->|"ω_w"| Clutch Clutch -->|"T_c"| Transmission Transmission -->|"T_w"| Differential - Differential -->|"T_{axle}"| Wheels + Differential -->|"T_axle"| Wheels br -->|"u_b"| BrakeSystem BrakeSystem -->|"T_b"| Wheels - st -->|"\delta"| Ackermann - Ackermann -->|"\delta_i,\delta_o"| Wheels - Wheels -->|"\kappa,\alpha"| Pacejka[Pacejka Tire Model] + st -->|"δ"| Ackermann + Ackermann -->|"δ_i, δ_o"| Wheels + Wheels -->|"κ, α"| Pacejka[Pacejka Tire Model] Pacejka -->|"F_x,F_y"| ForceCalc Suspension -->|"F_s"| ForceCalc HitchModel -->|"F_h"| ForceCalc @@ -377,13 +355,11 @@ flowchart LR ``` The labels show the key state variables exchanged between the blocks. For -example the engine produces torque \(T_e = f(\omega_e) \times \theta_{th}\) which the +example the engine produces torque $T_e = f(\omega_e) \times \theta_{th}$ which the transmission multiplies to -```math -T_w = T_c \times \mathrm{gearRatio} \times finalDrive -``` +$T_w = T_c \times \mathrm{gearRatio} \times finalDrive$ Wheels -provide slip ratio `\kappa` and angle `\alpha` to the Pacejka tire model to +provide slip ratio `$\kappa$` and angle `\alpha` to the Pacejka tire model to compute `F_x` and `F_y`. These forces together with the suspension reaction `F_s` are summed by `ForceCalculator` yielding accelerations `a_x`, `a_y` and yaw moment `M_z`. `DynamicsUpdater` integrates the resulting @@ -410,21 +386,21 @@ flowchart LR ``` * **PID Speed Controller** computes acceleration using - \(a = K_p \times e + K_i \int e\,dt + K_d \times \dot e\) where the error \(e\) is the + $a = K_p \times e + K_i \int e\,dt + K_d \times \dot e$ where the error $e$ is the difference between desired and filtered speed. Cornering speed reduction is applied if the turn radius is small. * **ACC Controller** modifies the PID output when approaching a curve. When the - distance to a curve is below \(v \times t_{lookahead}\) the commanded speed is - reduced by a factor and jerk is limited to \(0.7 g\). + distance to a curve is below $v \times t_{lookahead}$ the commanded speed is + reduced by a factor and jerk is limited to $0.7 g$. * **Pure Pursuit Path Follower** predicts waypoints ahead of the vehicle and - computes the steering angle \(\delta = \tan^{-1}\frac{2L \times \sin\alpha}{d}\). The + computes the steering angle $\delta = \tan^{-1}\frac{2L \times \sin\alpha}{d}$. The angle passes through a Gaussian and low\-pass filter to reduce oscillations. * **Longitudinal Limiter** reads calibration curves from Excel to cap allowable acceleration and braking as functions of speed. * **Lateral Limiter** loads a steering limit curve from a file and clamps the requested wheel angle at high speed. * **Jerk Controller** bounds the change of acceleration and steering rate using - \(\Delta u_{max} = J_{max} \Delta t\). + $\Delta u_{max} = J_{max} \Delta t$. These modules exchange commands with the mechanical subsystems in the vehicle model diagram above. They also use the filter chain described later to smooth @@ -441,39 +417,74 @@ flowchart TD ACC -->|"limited accel"| LongLim Inputs -->|"path"| PP[Pure Pursuit] PP -->|"steer req"| LatLim - LongLim -->|"a_{cmd}"| Jerk - LatLim -->|"\delta_{lim}"| Jerk + LongLim -->|"a_cmd"| Jerk + LatLim -->|"δ_lim"| Jerk Jerk -->|"throttle"| Throttle Jerk -->|"steer"| Ackermann - Throttle -->|"\theta_{th}"| Engine + Throttle -->|"θ_th"| Engine Engine -->|"T_e"| Transmission Transmission -->|"T_w"| Differential - Differential -->|"T_{axle}"| Wheels - Ackermann -->|"\delta_i,\delta_o"| Wheels + Differential -->|"T_axle"| Wheels + Ackermann -->|"δ_i, δ_o"| Wheels Wheels -->|"state"| Feedback[Vehicle State] Feedback -->|"speed"| PID Feedback -->|"pos"| PP ``` +#### Pure Pursuit Algorithm +The path follower selects a lookahead point ahead of the vehicle to compute the +target steering angle. The classical pure pursuit method uses a single point at +distance $d$ and applies + +$$ +\delta = \tan^{-1}\frac{2L \sin \alpha}{d} +$$ + +where $L$ is the wheelbase and $\alpha$ the heading error. VDSS extends this by +projecting several future waypoints, averaging the resulting curvatures and then +filtering the command with Gaussian and low-pass stages before actuating the +Ackermann steering. + +The orientation and position after a preview time $t_p$ are estimated using +$$ +\hat{\theta} = \theta + \tfrac{v}{L}\tan(\delta)\, t_p +$$ +$$ +\hat{p} = p + v t_p [\cos \hat{\theta},\, \sin \hat{\theta}] +$$ +The controller searches the path ahead for a target point $(x_l, y_l)$ and computes +$$ +\alpha = \operatorname{atan2}(y_l - \hat{p}_y,\, x_l - \hat{p}_x) - \hat{\theta} +$$ +which yields the steering command +$$ +\delta_{pp} = \operatorname{atan2}(2L \sin \alpha,\, d_l). +$$ + +```mermaid +flowchart LR + path[Planned Path] --> Lookahead + Lookahead -->|"\alpha,d"| Curv[Compute Curvature] + Curv --> Gauss[Gaussian] + Gauss --> LowPass[Low-pass] + LowPass -->|"\delta"| Ackermann +``` + #### Control Limiters Both steering and longitudinal actuation are limited according to speed dependent curves. The curves are provided as Excel files so they can be tuned without modifying the code. *Longitudinal limits* -```math -a_{cmd} = \mathrm{clip}\bigl( a_{des},\; a_{min}(v),\; a_{max}(v) \bigr) -``` -Acceleration and braking bounds \(a_{max}(v)\) and \(a_{min}(v)\) are read from +$a_cmd = \mathrm{clip}\bigl( a_{des},\; a_{min}(v),\; a_{max}(v) \bigr)$ +Acceleration and braking bounds $a_{max}(v)$ and $a_{min}(v)$ are read from `accelCurve.xlsx` and `decelCurve.xlsx`. Desired acceleration is first passed through a Gaussian filter and then ramped over several steps to avoid jerks. *Lateral limits* -```math -\delta_{lim} = \mathrm{interp}(v,\, speedData,\, maxAngleData) -``` +$δ_lim = \mathrm{interp}(v,\, speedData,\, maxAngleData)$ `steerLimits.xlsx` contains pairs of vehicle speed and maximum steering angle. -The limiter clamps the requested angle to \(\pm\delta_{lim}\) each update. +The limiter clamps the requested angle to $\pmδ_lim$ each update. ### Tests MATLAB test functions under `tests/` validate controllers, vehicle dynamics and localization routines. Run `runtests('tests')` inside MATLAB to execute all unit tests. @@ -754,32 +765,32 @@ This project is licensed under the GNU General Public License v3. See the [LICEN scale = sqrt(J2980AssumedMaxMass / vehicleMass); thresholds = baseDV * scale; % baseDV from J2980 table ``` -Collision energy is computed as \(0.5 \times m \times \Delta v^2\) and compared with severity thresholds. This extension allows comparing truck crashes against J2980 passenger‑car limits by specifying `J2980AssumedMaxMass` in the GUI. +Collision energy is computed as $0.5 \times m \times \Delta v^2$ and compared with severity thresholds. This extension allows comparing truck crashes against J2980 passenger‑car limits by specifying `J2980AssumedMaxMass` in the GUI. ## Physics Models The blocks above are tied together using classical vehicle dynamics. The process for each simulation step is summarized below. 1. **Slip and Tire Forces** - - Slip ratio: \(\kappa = (\omega R - v_x)/\max(v_x,0.1)\) - - Slip angle: \(\alpha = \tan^{-1}(v_y / |v_x|)\) + - Slip ratio: $\kappa = (\omega R - v_x)/\max(v_x,0.1)$ + - Slip angle: $\alpha = \tan^{-1}(v_y / |v_x|)$ - Pacejka '96 formula gives the tire forces: - ```math - F_x = D_x \sin\bigl(C_x \tan^{-1}(B_x \kappa - E_x(B_x \kappa - \tan^{-1}(B_x \kappa)))\bigr) - F_y = D_y \sin\bigl(C_y \tan^{-1}(B_y \alpha - E_y(B_y \alpha - \tan^{-1}(B_y \alpha)))\bigr) - ``` + $$ + F_x = D_x \sin\bigl(C_x \tan^{-1}(B_x \kappa - E_x(B_x \kappa - \tan^{-1}(B_x \kappa)))\bigr) \\ + F_y = D_y \sin\bigl(C_y \tan^{-1}(B_y \alpha - E_y(B_y \alpha - \tan^{-1}(B_y \alpha)))\bigr) + $$ 2. **Force Summation** - - Aerodynamic drag: \(F_d = 0.5 \times \rho \times C_d \times A \times v^2\) - - Wheel force: \(F_x = T_w/R_w - F_{brake} - F_d\) where - \(F_{brake} = u_b \times \mathrm{maxBrakingForce} \times \mathrm{brakeEfficiency}\) + - Aerodynamic drag: $F_d = 0.5 \times \rho \times C_d \times A \times v^2$ + - Wheel force: $F_x = T_w/R_w - F_{brake} - F_d$ where + $F_{brake} = u_b \times \mathrm{maxBrakingForce} \times \mathrm{brakeEfficiency}$ - Net lateral force combines tire and suspension reactions. - - Yaw moment: \(M_z = l_f F_{yf} - l_r F_{yr}\) + - Yaw moment: $M_z = l_f F_{yf} - l_r F_{yr}$ 3. **Dynamics Update** - - Longitudinal: \(\tfrac{du}{dt} = F_x/m + r v\) - - Lateral: \(\tfrac{dv}{dt} = F_y/m - r u\) - - Yaw: \(\dot r = M_z / I_z\) + - Longitudinal: $\tfrac{du}{dt} = F_x/m + r v$ + - Lateral: $\tfrac{dv}{dt} = F_y/m - r u$ + - Yaw: $\dot r = M_z / I_z$ - `KinematicsCalculator` maps body velocities to global rates. 4. **RK4 Integration** @@ -793,7 +804,7 @@ for each simulation step is summarized below. ``` 5. **Energy Balance** - Collision analysis uses \(E_k = 0.5 \times m \times v^2\) to compare against severity + Collision analysis uses $E_k = 0.5 \times m \times v^2$ to compare against severity thresholds. This chain transforms driver inputs into forces and accelerations that are @@ -805,42 +816,42 @@ dynamics. Key formulas include: ### Kinematics - Distance under constant acceleration: - \(s = v_0 t + 0.5 \times a \times t^2\) + $s = v_0 t + 0.5 \times a \times t^2$ - Final velocity: - \(v = v_0 + a \times t\) -- Rotation matrix from body to world given roll `\phi`, pitch `\theta` and yaw - `\psi`: - \(R = R_z(\psi) R_y(\theta) R_x(\phi)\). + $v = v_0 + a \times t$ +- Rotation matrix from body to world given roll $\phi$, pitch $\theta$ and yaw + $\psi$: + $R = R_z(\psi) R_y(\theta) R_x(\phi)$. - Body velocities to world-frame position rates: - \(\dot x = u \cos\psi - v \sin\psi\), - \(\dot y = u \sin\psi + v \cos\psi\). + $\dot x = u \cos\psi - v \sin\psi$, + $\dot y = u \sin\psi + v \cos\psi$. - Lateral acceleration update: - \(a_{lat} = F_y / m\) + $a_{lat} = F_y / m$ - Roll dynamics internal to `KinematicsCalculator`: - \(\mathrm{rollAccel} = (M_{roll} - D_{roll} \times \mathrm{rollRate} - K_{roll} \times \mathrm{rollAngle}) / I_{roll}\) + $\mathrm{rollAccel} = (M_{roll} - D_{roll} \times \mathrm{rollRate} - K_{roll} \times \mathrm{rollAngle}) / I_{roll}$ ### Dynamics - Longitudinal motion: - \(\tfrac{du}{dt} = F_x/m + r v\) + $\tfrac{du}{dt} = F_x/m + r v$ - Lateral motion: - \(\tfrac{dv}{dt} = F_y/m - r u\) + $\tfrac{dv}{dt} = F_y/m - r u$ - Yaw rate derivative: - \(\dot r = M_z / I_z\) + $\dot r = M_z / I_z$ - Roll rate derivative: - \(\dot p = (\mathrm{momentRoll} - m g h \sin\phi - K_{roll}\phi - C_{roll} p) / I_{xx}\) + $\dot p = (\mathrm{momentRoll} - m g h \sin\phi - K_{roll}\phi - C_{roll} p) / I_{xx}$ - Tire slip ratio: - \(\kappa = (\omega R - v_x)/\max(v_x,0.1)\) + $\kappa = (\omega R - v_x)/\max(v_x,0.1)$ - Tire slip angle: - \(\alpha = \tan^{-1}(v_y / |v_x|)\) + $\alpha = \tan^{-1}(v_y / |v_x|)$ - Aerodynamic drag: - \(F_d = 0.5 \times \rho \times C_d \times A \times v^2\) - - Net longitudinal force: \(F_x = T_w/R_w - F_{brake} - F_d\) with - \(F_{brake} = u_b \times \mathrm{maxBrakingForce} \times \mathrm{brakeEfficiency}\) -- Translational kinetic energy: \(E_{trans} = 0.5 \times m \times (u^2 + v^2)\) -- Rotational kinetic energy: \(E_{rot} = 0.5 \times I \times \omega^2\) -- Yaw moment from tire forces: \(M_z = l_f F_{yf} - l_r F_{yr}\) + $F_d = 0.5 \times \rho \times C_d \times A \times v^2$ + - Net longitudinal force: $F_x = T_w/R_w - F_{brake} - F_d$ with + $F_{brake} = u_b \times \mathrm{maxBrakingForce} \times \mathrm{brakeEfficiency}$ +- Translational kinetic energy: $E_{trans} = 0.5 \times m \times (u^2 + v^2)$ +- Rotational kinetic energy: $E_{rot} = 0.5 \times I \times \omega^2$ +- Yaw moment from tire forces: $M_z = l_f F_{yf} - l_r F_{yr}$ - Newton's second law couples the forces and accelerations as - \(m \times a = \sum F\) and \(I \times \alpha = \sum M\) for translational and rotational + $m \times a = \sum F$ and $I \times \alpha = \sum M$ for translational and rotational dynamics. ### Integration @@ -859,7 +870,7 @@ motion every simulation step. vehicle from tire grip, braking torque, aerodynamic drag, suspension reaction and hitch constraints. `DynamicsUpdater.stateDerivative` converts these forces into linear and angular accelerations: -\(a_x = F_x/m\), \(a_y = F_y/m\) and \(\dot r = M_z/I_z\). `KinematicsCalculator` +$a_x = F_x/m$, $a_y = F_y/m$ and $\dot r = M_z/I_z$. `KinematicsCalculator` then maps body velocities to world-frame position rates. The RK4 loop integrates these derivatives so that position, velocity, orientation and roll state are all updated consistently each time step. @@ -868,9 +879,7 @@ The dynamic equations determine the accelerations from forces, while the kinemat ## Derivatives, Integrals and the Levant Formula The simulator repeatedly differentiates and integrates signals to turn driver commands into motion. Speed control uses a PID loop where -```math -a = K_p \times e + K_i \int e\,dt + K_d \times \frac{d e}{d t} -``` +$a = K_p \times e + K_i \int e\,dt + K_d \times \frac{d e}{d t}$ The derivative term is obtained with the **Levant differentiator**, a robust sliding-mode algorithm implemented in `LevantDifferentiator`. It estimates `d(error)/dt` even when the measured speed is noisy. The integral term collects @@ -915,24 +924,16 @@ more stable simulations. The mathematical form of each filter is: - **Rate limiter** - ```math - y_k = \min\bigl( \max(x_k,\,y_{k-1}-r_{\max} \Delta t),\; y_{k-1}+r_{\max} \Delta t \bigr) - ``` - with rate limit \(r_{\max}\). +$y_k = \min\bigl( \max(x_k,\,y_{k-1}-r_{\max} \Delta t),\; y_{k-1}+r_{\max} \Delta t \bigr)$ + with rate limit $r_{\max}$. - **Gaussian filter** - ```math - y_k = \sum_{i=-n}^{n} w_i x_{k-i} - ``` - where \(w_i\) are normalised Gaussian weights. +$y_k = \sum_{i=-n}^{n} w_i x_{k-i}$ + where $w_i$ are normalised Gaussian weights. - **Moving average** - ```math - y_k = \tfrac{1}{N} \sum_{i=0}^{N-1} x_{k-i} - ``` +$y_k = \tfrac{1}{N} \sum_{i=0}^{N-1} x_{k-i}$ - **Low-pass filter** - ```math - y_k = \alpha x_k + (1-\alpha) y_{k-1} - ``` +$y_k = \alpha x_k + (1-\alpha) y_{k-1}$ -Tuning parameters like \(r_{\max}\), window size \(N\) and coefficient \(\alpha\) +Tuning parameters like $r_{\max}$, window size $N$ and coefficient $\alpha$ are exposed in the GUI tabs so users can calibrate how aggressively commands are smoothed.