4. Vehicle Dynamics and Physical Modeling

While geometric kinematics are sufficient for flat, low-speed navigation, agricultural environments introduce significant physical constraints. Terrain slopes, high inertia, and low-adhesion surfaces generate substantial lateral forces. To accurately simulate the robot's behavior—especially its tendency to slip and drift—we must compute its motion using the fundamental principles of rigid body dynamics.

This section details the Newton-Euler derivations that govern the vehicle's rotational acceleration and slip angles, directly reflecting the computational core of the simulator.

4.1. The Fundamental Principle of Dynamics in Rotation

When the vehicle steers, the tires generate lateral forces that create a rotational moment around the Center of Gravity (CoG). According to Euler's equation for rigid body rotation, the sum of these moments dictates the yaw acceleration \(\ddot{\theta}\).

Let \(I_z\) be the yaw moment of inertia, \(L_F\) and \(L_R\) be the distances from the CoG to the front and rear axles respectively, and \(F_{yF}\) and \(F_{yR}\) be the lateral forces generated by the tires.

The mechanical steering angles \(\delta_F\) and \(\delta_R\) rotate the wheels relative to the chassis. Therefore, only the component of the tire force that is perpendicular to the vehicle's longitudinal axis contributes to the yaw moment.

Applying the rotational dynamic equilibrium:

\[ I_z \cdot \ddot{\theta} = L_R \cdot F_{yR} \cos(\delta_R) - L_F \cdot F_{yF} \cos(\delta_F) \]

Isolating the yaw acceleration \(\ddot{\theta}\) yields the exact equation computed in the simulation:

\[ \ddot{\theta} = \frac{1}{I_z} \Big( L_R \cdot F_{yR} \cos(\delta_R) - L_F \cdot F_{yF} \cos(\delta_F) \Big) \]

To obtain the vehicle's yaw rate \(\dot{\theta}\) and absolute heading \(\theta\) for the current discrete time step \(dt\), we perform a first-order numerical Euler integration:

\[ \begin{aligned} \dot{\theta}_{k+1} &= \dot{\theta}_k + \ddot{\theta}_k \cdot dt \\ \theta_{k+1} &= \theta_k + \dot{\theta}_{k+1} \cdot dt \end{aligned} \]

Dynamic_Mouvement.sce

Theta_Double_Dot = (1/Iz) * (-LF * F_Lat_Force * cos(F_Steer_A) + LR * R_Lat_Force * cos(R_Steer_A)) ;

Theta_Dot = Theta_Dot + Theta_Double_Dot * dt ;

Theta = Theta + Theta_Dot * dt ;

4.2. Global Slip Angle Dynamics

In extreme off-road conditions, the vehicle does not move perfectly in the direction it is pointing. The entire chassis drifts. This drift is quantified by the global vehicle slip angle \(\beta\), which is the angle between the chassis' longitudinal axis and the actual CoG velocity vector (\(V_{CoG}\)).

To find how this drift evolves (\(\dot{\beta}\)), we apply Newton's Second Law of Motion (\(F = m \cdot a\)) projected onto the axis strictly perpendicular to the velocity vector.

The absolute lateral acceleration \(a_{\perp}\) perpendicular to the path of the CoG is composed of the change in the slip angle itself plus the yaw rate of the vehicle:

\[ a_{\perp} = V_{CoG} (\dot{\beta} + \dot{\theta}) \]

Multiplying this acceleration by the vehicle mass \(M\) gives the sum of external forces perpendicular to the velocity:

\[ M \cdot V_{CoG} (\dot{\beta} + \dot{\theta}) = \sum F_{\perp} \]

Three external forces act perpendicularly to the movement:

Gravity on a slope: If the terrain has a lateral slope angle (\(\alpha_{slope}\)), gravity pulls the vehicle sideways: \(M \cdot g \sin(\alpha_{slope})\).
Front tire force: Projected relative to the velocity vector: \(-F_{yF} \cos(\beta - \delta_F)\).
Rear tire force: Projected relative to the velocity vector: \(-F_{yR} \cos(\beta - \delta_R)\).

(Note: The tire forces are negative because they act as restoring forces opposing the outward drift).

Assembling the complete dynamic equation:

\[ M \cdot V_{CoG} (\dot{\beta} + \dot{\theta}) = M \cdot g \sin(\alpha_{slope}) - F_{yF} \cos(\beta - \delta_F) - F_{yR} \cos(\beta - \delta_R) \]

To isolate the slip angle derivative \(\dot{\beta}\), we divide the entire equation by \(M \cdot V_{CoG}\) and subtract \(\dot{\theta}\):

\[ \dot{\beta} = \frac{1}{M \cdot V_{CoG}} \Big( M \cdot g \sin(\alpha_{slope}) - F_{yF} \cos(\beta - \delta_F) - F_{yR} \cos(\beta - \delta_R) \Big) - \dot{\theta} \]

Distributing the fraction yields the exact differential equation evaluated in the simulator to compute Vehicle_Beta_Dot:

\[ \dot{\beta} = \frac{g \sin(\alpha_{slope})}{V_{CoG}} - \frac{1}{M \cdot V_{CoG}} \Big( F_{yF} \cos(\beta - \delta_F) + F_{yR} \cos(\beta - \delta_R) \Big) - \dot{\theta} \]

Just like the yaw rate, the global slip angle \(\beta\) is then updated via Euler integration:

\[ \beta_{k+1} = \beta_k + \dot{\beta}_k \cdot dt \]

Dynamic_Mouvement.sce

// Safty to avoid to divise by 0 when the robot close to stop
if Speed > 0.25
    CoG_Speed  = Speed / cos(Vehicle_Beta) ;
    Vehicle_Beta_Dot   = (-1/(M * CoG_Speed)) * ((F_Lat_Force) * cos(Vehicle_Beta - F_Steer_A)...
                         + (R_Lat_Force) * cos(Vehicle_Beta - R_Steer_A)) + (g*sin(Slope_Rad) / CoG_Speed) - Theta_Dot ;
    Vehicle_Beta       = Vehicle_Beta + Vehicle_Beta_Dot * dt ;

4.3. Axle Slip Angles

The lateral forces \(F_{yF}\) and \(F_{yR}\) used above do not depend on the global slip angle \(\beta\), but on the local slip angles at each specific axle (\(\beta_F\) and \(\beta_R\)). Because the vehicle is rotating while translating, the front axle "swipes" laterally faster than the CoG, while the rear axle moves slower.

We must derive the true local velocity vector angles (\(\alpha_F\) and \(\alpha_R\)) relative to the chassis. First, we decompose the CoG velocity into longitudinal (\(V_x\)) and lateral (\(V_y\)) components relative to the chassis:

\[ \begin{aligned} V_x &= V_{CoG} \cos(\beta) \\ V_y &= V_{CoG} \sin(\beta) \end{aligned} \]

Using solid kinematics, the lateral velocity at the front axle includes the yaw rotation contribution (\(+L_F \dot{\theta}\)), while the rear axle subtracts it (\(-L_R \dot{\theta}\)). Assuming the longitudinal velocity is identical across the chassis (\(V_{xF} = V_{xR} = V_x\)):

\[ \begin{aligned} V_{yF} &= V_{CoG} \sin(\beta) + L_F \dot{\theta} \\ V_{yR} &= V_{CoG} \sin(\beta) - L_R \dot{\theta} \end{aligned} \]

The absolute velocity angle at the front axle \(\alpha_F\) is the arctangent of its lateral over longitudinal velocities:

\[ \alpha_F = \arctan \left( \frac{V_{yF}}{V_x} \right) = \arctan \left( \frac{V_{CoG} \sin(\beta) + L_F \dot{\theta}}{V_{CoG} \cos(\beta)} \right) \]

By dividing the numerator terms by the denominator, we simplify this into the exact formula utilized in the code:

\[ \alpha_F = \arctan \left( \frac{V_{CoG} \sin(\beta)}{V_{CoG} \cos(\beta)} + \frac{L_F \dot{\theta}}{V_{CoG} \cos(\beta)} \right) = \arctan \left( \tan(\beta) + \frac{L_F \dot{\theta}}{V_{CoG} \cos(\beta)} \right) \]

Finally, a local tire slip angle is defined as the difference between this true velocity angle and the mechanical steering angle (\(\beta_{slip} = \alpha - \delta\)). Applying this to both axles provides the final required states:

\[ \begin{aligned} \beta_F &= \arctan \left( \tan(\beta) + \frac{L_F \dot{\theta}}{V_{CoG} \cos(\beta)} \right) - \delta_F \\ \beta_R &= \arctan \left( \tan(\beta) - \frac{L_R \dot{\theta}}{V_{CoG} \cos(\beta)} \right) - \delta_R \end{aligned} \]

Dynamic_Mouvement.sce

// Safty to avoid to divise by 0 when the robot close to stop
if Speed > 0.25
    Front_Beta = atan( (tan(Vehicle_Beta)) + (LF * Theta_Dot) / (CoG_Speed * cos(Vehicle_Beta))) - F_Steer_A ;
    Rear_Beta  = atan( (tan(Vehicle_Beta)) - (LR * Theta_Dot) / (CoG_Speed * cos(Vehicle_Beta))) - R_Steer_A ;

4.4. Tire/Ground Contact Models

With the local slip angles determined, the simulator evaluates the actual lateral forces generated by the tires. The engine supports two distinct contact models depending on the required fidelity.

4.4.1. Linear Model

For basic debugging and near-zero slip conditions, a linear approximation is used. The tire acts as a simple torsional spring where the lateral force is strictly proportional to the local slip angle multiplied by a Cornering Stiffness constant \(C\):

\[ \begin{aligned} F_{yF} &= C \cdot \beta_F \\ F_{yR} &= C \cdot \beta_R \end{aligned} \]

Dynamic_Mouvement.sce

F_Lat_Force = C * Front_Beta;
R_Lat_Force = C * Rear_Beta;

4.4.2. Pacejka's Magic Formula

For true agricultural fidelity, the linear model is inadequate because it fails to cap the friction limit, allowing infinite grip. To simulate realistic loss of traction and non-linear tire saturation, the simulator implements Pacejka's Magic Formula. The specific parameters and empirical coefficients used in this model are adapted from agricultural robotics research, specifically detailed in the study: Etude, modélisation et validation du contact roue/sol pour la simulation de véhicules et robots mobiles sous le logiciel Adams.

The fundamental Magic Formula evaluates the lateral force \(F_y\) as a function of the slip angle \(\beta\) (converted to degrees):

\[ F_y = D \cdot \sin \Big( C \cdot \arctan \big( B \cdot \beta_{deg} - E \cdot (B \cdot \beta_{deg} - \arctan(B \cdot \beta_{deg})) \big) \Big) \]

The shape of this curve is governed by dynamic variables that depend strictly on the Normal Force (\(F_z\)) pushing the tire into the ground. In this framework, the vertical load distribution is modeled statically based on the geometric CoG location:

\[ \begin{aligned} F_{zF} &= M \cdot g \cdot \left(\frac{L_R}{L}\right) \\ F_{zR} &= M \cdot g \cdot \left(\frac{L_F}{L}\right) \end{aligned} \]

Using nine empirical coefficients (\(a_0\) through \(a_8\)), the core Pacejka parameters (\(B, C, D, E\)) are continuously evaluated for each axle:

a) Peak Factor (\(D\)): Determines the absolute maximum grip available.

\[ D = a_1 F_z^2 + a_2 F_z \]

b) Shape Factor (\(C\)): Controls the overall bounds of the sine transition.

\[ C = a_0 \]

c) Stiffness Factor (\(B\)): Governs the initial slope of the curve (similar to the linear model's stiffness). To prevent numerical instability at zero loads, a microscopic safeguard constant (1e-6) is added to the denominator.

\[ B = \frac{a_3 \sin(a_4 \arctan(a_5 F_z))}{a_0 \cdot D + 10^{-6}} \]

d) Curvature Factor (\(E\)): Dictates the drop-off rate of grip once the peak limit is exceeded.

\[ E = a_6 F_z^2 + a_7 F_z + a_8 \]

By resolving this equation at every simulation step, the system perfectly captures the transition from high-grip turning to uncontrolled skidding.

Dynamic_Mouvement.sce

a0 = 1.3   ;    // Shape Factor
a1 = -8    ;    // Second order coefficient to model the peak adhesion curve
a2 = 100   ;    // First order coefficient to model the peak adhesion curve
a3 = 200   ;    // Stiffness coefficient B, dictates the slope of the force curve at the origin
a4 = 1.82  ;    // Adjusts the stiffness evolution according to the vertical load Fz
a5 = 0.208 ;    // Fine-tunes the stiffness based on the applied vertical load on the wheel
a6 = 0     ;    // Shape factor E, controls the curvature at the peak of the curve
a7 = 0.354 ;    // Manages the transition between the elastic zone and the pure slip zone
a8 = 5     ;    // Adapts the transition based on the applied weight on the wheel

// --------- Normal Forces
F_Fz = (LR / L) * M * g ;   // Front normal force
R_Fz = (LF / L) * M * g ;   // Rear normal force

// Equation : Lateral_Force = D * sin(C * atan(B*x - E*(B*x - atan(B*x)))), with a0 = C

//  --------- Front Wheel
D_Front = a1 * (F_Fz^2) + (a2 * F_Fz) ;
B_Front = (a3 * sin(a4 * atan(a5 * F_Fz) ))  / (a0 * D_Front + 1e-6) ;
E_Front = a6 * (F_Fz^2) + ( (a7 * F_Fz) + a8) ; 

//  --------- Rear Wheel
D_Rear = a1 * (R_Fz^2) + (a2 * R_Fz) ; 
B_Rear = (a3 * sin(a4 * atan(a5 * R_Fz) )) / (a0 * D_Rear + 1e-6) ;
E_Rear = a6 * (R_Fz^2) + ( (a7*R_Fz) + a8) ;

// Radian to degree conversion
Front_Beta_Deg = Front_Beta * 180 / %pi ; 
Rear_Beta_Deg  = Rear_Beta  * 180 / %pi ;

// Lateral Forces
F_Lat_Force = D_Front * sin(a0 * atan(B_Front * ((1 - E_Front) * Front_Beta_Deg + (E_Front / B_Front) * atan(B_Front * Front_Beta_Deg)))) ;
R_Lat_Force = D_Rear  * sin(a0 * atan(B_Rear  * ((1 - E_Rear)  * Rear_Beta_Deg  + (E_Rear  / B_Rear ) * atan(B_Rear  * Rear_Beta_Deg))))  ;