Sommario

L’elaborato descrive la progettazione e l’implementazione di un sistema di guida autonoma simulato. Il sistema integra tre componenti principali: un controllore di sterzo che combina l’algoritmo Pure Pursuit con un termine feedforward basato sul modello cinematico Ackermann e un PID di feedback per la correzione degli errori residui, un sistema di sterzo a quattro ruote (4WS) con transizione continua tra controfase e fase in funzione della velocità, e un PID di velocità adattivo con look-ahead sulla curvatura.

La gestione della velocità è evoluta da una selezione binaria del target a un velocity planning continuo, che mappa proporzionalmente la curvatura rilevata in una velocità target, migliorando la stabilità del veicolo a scapito di un incremento del tempo sul giro L’ottimizzazione del parametro brake_threshold ha evidenziato come una soglia di frenata nulla attivi i freni meccanici in modo eccessivo, mentre un valore di 2.0 m/s recupera tempo sul giro. L’utilizzo del passo reale del veicolo più accurato nel feedforward Ackermann ha prodotto un ulteriore miglioramento del tempo sul giro. L’analisi evidenzia come dettagli parametrici se pur minimi contribuiscano in modo determinante alla creazione di un sistema stabile, riflettendo come ogni singola variabile influenzi la dinamica complessiva. Un sistema di logging CSV e uno script Python con Matplotlib effettuano l’analisi delle prestazioni.

Introduzione e Contesto Generale

La guida autonoma rappresenta uno dei campi di ricerca più attivi nell’ingegneria robotica e dei sistemi di controllo. Un veicolo autonomo deve essere in grado di percepire l’ambiente circostante, pianificare un percorso e seguirlo con precisione, il tutto in tempo reale e con un margine di errore ridotto al minimo.

Il presente progetto si colloca in questo contesto e ha come obiettivo la realizzazione di un sistema di guida autonoma simulato all’interno del motore di gioco Godot 4, utilizzando il linguaggio GDScript. La scelta di un ambiente di simulazione consente di testare e validare algoritmi di controllo in modo sicuro e ripetibile, prima di un eventuale trasferimento su hardware reale.

Il sistema sviluppato è composto da tre macro-componenti principali:

1. Controllo Sterzo: un controllore di sterzo che combina l’algoritmo Pure Pursuit per la selezione del punto di riferimento, un termine feedforward basato sul modello cinematico Ackermann per il calcolo geometrico dell’angolo di sterzo, e un PID di feedback per la correzione degli errori residui.

2. Controllo Velocità Adattivo con 4WS: un sistema di gestione della velocità che anticipa le curve e coordina lo sterzo di tutte e quattro le ruote (Four Wheel Steering) per migliorare la manovrabilità a bassa velocità e la stabilità ad alta velocità.

3. Path Planning: costruzione e smoothing del percorso da seguire, a partire dal percorso definito nell’ambiente di simulazione.

Il veicolo simulato è basato sul modello fisico VehicleBody3D di Godot, che implementa una fisica realistica del veicolo includendo sospensioni, attrito delle ruote e inerzia. Il percorso è definito tramite un nodo Path3D e viene elaborato all’avvio per generare una sequenza di waypoint equidistanti.

Il Controllore PID

Teoria del Controllo PID

Il controllore PID (Proportional-Integral-Derivative) è uno degli strumenti più utilizzati nei sistemi a controllo automatico. La sua semplicità implementativa, unita alla robustezza e alla flessibilità, lo rende adatto a una vasta gamma di applicazioni, dal controllo industriale alla robotica.

Il principio fondamentale è la retroazione negativa (feedback): il sistema misura continuamente la differenza tra il valore desiderato e il valore attuale (variabile controllata), detta errore, e agisce su di essa per minimizzarla.

L’uscita del controllore PID è definita dalla seguente equazione:

\[\begin{equation} u(t) = K_P \cdot e(t) \;+\; K_I \int_0^t e(\tau)\,d\tau \;+\; K_D \cdot \frac{de(t)}{dt} \label{eq:pid} \end{equation}\]

dove:

• \(e(t)\) è l’errore istantaneo al tempo \(t\);

• \(K_P\) è il guadagno proporzionale;

• \(K_I\) è il guadagno integrale;

• \(K_D\) è il guadagno derivativo;

• \(u(t)\) è il segnale di controllo applicato al sistema.

Componente Proporzionale

Il termine proporzionale produce un’azione di controllo direttamente proporzionale all’errore corrente:

\[\begin{equation} u_P(t) = K_P \cdot e(t) \end{equation}\]

Un valore elevato di \(K_P\) rende il sistema più reattivo, ma può causare oscillazioni intorno al setpoint (overshoot). Un valore troppo basso produce una risposta lenta e un errore a regime (steady-state error).

Componente Integrale

Il termine integrale accumula l’errore nel tempo ed elimina l’errore a regime che il solo termine proporzionale non riesce a correggere:

\[\begin{equation} u_I(t) = K_I \int_0^t e(\tau)\,d\tau \end{equation}\]

In implementazione discreta (come nel caso del presente progetto a ogni frame di simulazione), l’integrale viene approssimato come:

\[\begin{equation} I_k = I_{k-1} + e_k \cdot \Delta t \end{equation}\]

È necessario implementare un meccanismo di anti-windup per evitare che l’integrale cresca illimitatamente quando l’attuatore è in saturazione.
L’integrazione condizionata (conditional integration): l’integrale si aggiorna solo quando il sistema non è in saturazione, bloccandosi esattamente nel momento in cui l’attuatore raggiunge il suo limite fisico:

\[\begin{equation} I_k = \begin{cases} I_{k-1} + e_k \cdot \Delta t & \text{se } |u_{k-1}| < u_{max} \\ I_{k-1} & \text{altrimenti (saturazione)} \end{cases} \end{equation}\]

Con questo approccio l’integrale smette di crescere nell’esatto momento in cui non può più avere effetto sull’uscita, eliminando completamente l’overshoot causato dal windup al termine della saturazione.

Componente Derivativo

Il termine derivativo reagisce alla velocità di variazione dell’errore, anticipando le correzioni e smorzando le oscillazioni:

\[\begin{equation} u_D(t) = K_D \cdot \frac{de(t)}{dt} \end{equation}\]

In forma discreta:

\[\begin{equation} u_D^{(k)} = K_D \cdot \frac{e_k - e_{k-1}}{\Delta t} \end{equation}\]

Implementazione Discreta nel Progetto

Nel contesto della simulazione, il PID viene eseguito a ogni frame fisico
(funzione _physics_process(delta)). L’equazione discretizzata utilizzata è:

\[\begin{equation} u_k = K_P \cdot e_k \;+\; K_I \cdot I_k \;+\; K_D \cdot \frac{e_k - e_{k-1}}{\Delta t} \end{equation}\]

I valori dei guadagni sono stati calibrati empiricamente tramite test iterativi nella simulazione, ottenendo i valori riportati in Tabella 2.1.

Progettazione

Algoritmo Pure Pursuit e Sterzo

Il Rabbit Point

L’algoritmo Pure Pursuit seleziona un punto di riferimento mobile sul percorso, detto rabbit point (o lookahead point), posto a una distanza fissa \(L_{ahead}\) davanti al veicolo. L’obiettivo del controllore è orientare il veicolo verso questo punto.

Il rabbit point \(\mathbf{r}\) viene identificato come il primo waypoint che si trova ad almeno \(L_{ahead}\) metri dalla posizione attuale del veicolo.

Il parametro \(L_{ahead} = 7.0\;\text{m}\) è stato calibrato sperimentalmente: valori troppo bassi causano oscillazioni sul rettilineo, valori troppo alti rendono il sistema insensibile alle curve strette.

Calcolo dell’Errore Angolare

Per poter alimentare il PID dello sterzo, è necessario calcolare l’errore di orientamento (o angolare). L’errore angolare rappresenta la differenza tra la direzione attuale del veicolo e la direzione da raggiungere verso il rabbit point che segna il waypoint target.

A tale scopo, si applica la cinematica del controllo polare di posizione. Siano (\(x_R, y_R\)) le coordinate attuali dell’auto e \(\theta_R\) il suo orientamento rispetto al sistema di riferimento globale. Sia inoltre (\(x_T, y_T\)) la posizione del punto target (il rabbit generato dal Pure Pursuit). Per prima cosa, si calcola l’angolo assoluto \(\theta_T\) del vettore che unisce il centro di massa dell’auto al target \(T\): \[\begin{equation} \theta_T = \text{arctan2}(y_T - y_R, x_T - x_R) \end{equation}\] Successivamente, l’errore di rotta \(\theta_{error}\) viene calcolato come differenza circolare tra l’angolo verso il target e l’orientamento attuale dell’auto: \[\begin{equation} \theta_{error} = \theta_T \ominus \theta_R \end{equation}\] L’operatore \(\ominus\) rappresenta la differenza circolare, necessaria per mantenere l’angolo normalizzato nell’intervallo [\(-\pi,+\pi\)] ed evitare discontinuità nel controllo.

Nell’implementazione pratica, poiché il veicolo si muove nel piano orizzontale tridimensionale, il calcolo trigonometrico con normalizzazione viene risolto in modo efficiente tramite la funzione signed_angle_to, che calcola direttamente l’angolo di errore \(\theta_{error}\) restituendo l’angolo con segno tra il vettore direzionale del muso dell’auto e il vettore verso il target, mentre il segno dell’angolo indica direttamente il verso di rotazione.

var car_fwd = global_transform.basis.z
car_fwd.y = 0.0
var to_rabbit = rabbit_pos - global_transform.origin
to_rabbit.y = 0.0
var angle_error = car_fwd.signed_angle_to(to_rabbit.normalized(), 
                                            Vector3.UP)

Applicazione del PID allo Sterzo

Il calcolo dell’angolo di sterzo adotta un’architettura feedforward + feedback: un termine geometrico derivato dal modello cinematico Ackermann fornisce l’angolo nominale per la traiettoria corrente, mentre un controllore PID corregge gli errori residui non catturati dalla geometria.

Termine Feedforward: Modello Cinematico Ackermann

Il modello cinematico Ackermann con raggio di curvatura centrale, lega l’angolo di sterzo anteriore alla geometria della curva tramite il passo del veicolo \(L\).

Dato l’errore angolare \(\theta_{error}\) verso il rabbit point, la distanza di lookahead \(L_{ahead}\),
e \(L=2.819\;\text{m}\) che è il passo della Mustang, l’angolo di sterzo nominale si ottiene come: \[\begin{equation} \alpha_{ff}(k) = \arctan\!\left(\frac{2L\,\sin\bigl(\theta_{error}(k)\bigr)}{L_{ahead}}\right) \label{eq:ackermann_ff} \end{equation}\]

La formula [eq:ackermann_ff] non è la semplice equazione di Ackermann, ma nasce dalla fusione di due contributi distinti: la cinematica del veicolo (modello Ackermann) e la geometria dell’algoritmo Pure Pursuit.
Per far percorrere al veicolo un arco di raggio \(R\), l’angolo di sterzo deve soddisfare \(\tan(\alpha_f) = L/R\); sostituendo l’espressione del raggio in funzione di \(\theta_{error}\) e \(L_{ahead}\) si ottiene la formula  [eq:ackermann_ff].

Passo 1 — Modello cinematico Ackermann. La cinematica di Ackermann lega il raggio di curvatura \(R\), il passo \(L\) e l’angolo di sterzo \(\alpha_f\): \[\begin{equation} R = \frac{L}{\tan(\alpha_f)} \;\Longrightarrow\; \alpha_f = \arctan\!\left(\frac{L}{R}\right) \end{equation}\] Questa equazione descrive cosa deve fare il veicolo: dato un raggio \(R\) da percorrere, restituisce l’angolo di sterzo fisicamente necessario. Il problema è che durante la guida il raggio \(R\) non è noto direttamente, infatti il controllore sa solo dove si trova il rabbit point.

Passo 2 — Geometria del Pure Pursuit. L’algoritmo Pure Pursuit calcola il raggio \(R\) dell’arco che parte dall’asse posteriore del veicolo e passa esattamente per il rabbit point. Dalla geometria del cerchio, applicando la relazione trigonometrica tra la corda di lookahead \(L_{ahead}\) e il raggio si ottiene: \[\begin{equation} R = \frac{L_{ahead}}{2\,\sin(\theta_{error})} \end{equation}\] Il raggio dipende dall’errore angolare verso il rabbit point e dalla distanza di lookahead: questo è il contributo geometrico dell’algoritmo di inseguimento.

Passo 3 — Fusione delle due formule. Sostituendo il raggio \(R\) ricavato geometricamente dal Pure Pursuit all’interno della formula cinematica di Ackermann si ottiene un’unica espressione che integra sia la geometria della traiettoria sia le caratteristiche fisiche del veicolo: \[\begin{equation} \tan(\alpha_{ff}) = \frac{L}{R} = \frac{L}{\dfrac{L_{ahead}}{2\,\sin(\theta_{error})}} = \frac{2L\,\sin(\theta_{error})}{L_{ahead}} \end{equation}\] da cui, applicando l’arcotangente, si ricava la [eq:ackermann_ff]. Quando il rabbit point è allineato con il muso del veicolo (\(\theta_{error} = 0\)) il feedforward produce \(\alpha_{ff} = 0\); all’aumentare dell’errore angolare l’angolo cresce in modo non lineare, saturando naturalmente per errori elevati.

Termine Feedback: Controllore PID

Il PID corregge gli errori residui non catturati dal feedforward: derive sistematiche, attrito asimmetrico, imprecisioni del modello fisico.

• Il termine proporzionale (\(K_P \cdot e_k\)) produce un’azione di controllo direttamente proporzionale all’errore istantaneo.

• Il termine integrale (\(K_I \cdot I_k\)) per lo sterzo è nullo inffatti l’introduzione di un’azione integrale sull’angolo di sterzo avrebbe rischiato di causare instabilità e fenomeni di wind-up in uscita dalle curve, mantenendo le ruote sterzate anche quando l’errore polare era ormai azzerato.

• Il termine derivativo (\(K_D \cdot \dot{e}_k\)) Allo stesso modo, si è esclusa l’azione derivativa poiché, non essendoci dinamiche da smorzare, avrebbe reso il comando di sterzata inutilmente ’scattoso’ in caso di rapide variazioni dell’errore angolare.

Schema Complessivo

L’angolo target è la somma del feedforward geometrico e dell’uscita PID, saturata e filtrata con un filtro passa-basso:

\[\begin{align} \alpha_{ff}(k) &= \arctan\!\left(\frac{2L\,\sin(\theta_{error}(k))}{L_{ahead}}\right) \\ u_{PID}(k) &= K_P \cdot \theta_{error}(k) + K_I \cdot I(k) + K_D \cdot \frac{\theta_{error}(k) - \theta_{error}(k-1)}{\Delta T} \\ \alpha_{target}(k) &= \text{Sat}\!\left(\alpha_{ff}(k) + u_{PID}(k),\; -\alpha_{max},\; +\alpha_{max}\right) \\ \alpha_c(k) &= \alpha_c(k-1) + \alpha_{speed} \cdot \Delta T \cdot \bigl(\alpha_{target}(k) - \alpha_c(k-1)\bigr) \end{align}\]

Il termine \(\alpha_{speed} \cdot \Delta T\) controlla la velocità di variazione dello sterzo: con \(\alpha_{speed} = 8.0\) e \(\Delta T \approx 0.016\;\text{s}\) (60 Hz), il fattore di interpolazione per frame risulta pari a circa \(0.13\) (\(8.0 \cdot 0.016 = 0.128 \approx 0.13\)), garantendo una transizione reattiva ma sufficientemente fluida per la stabilità fisica del veicolo.
Se \(\alpha_{speed} \cdot \Delta T \approx 0.13\) allora \(\alpha_c(k) = \alpha_c(k-1) + 0.13 \cdot (\alpha_{target}(k) - \alpha_c(k-1))\)

var angle_error = car_fwd.signed_angle_to(to_rabbit.normalized(),
                                           Vector3.UP)
# Feedforward: modello cinematico Ackermann
var wheelbase = 2.819  # passo Ford Mustang 1969 (m)
var alpha_ff  = atan(2.0 * wheelbase * sin(angle_error) / L_ahead)

# Feedback: PID sull'errore residuo
angle_integral += angle_error * delta
angle_integral  = clamp(angle_integral, -PI, PI)
var angle_change = (angle_error - prev_angle_error) / delta
prev_angle_error = angle_error
var pid_out = P_factor * angle_error \
            + I_factor * angle_integral \
            + D_factor * angle_change

# Sterzo totale: feedforward + feedback
var target_steer = clamp(alpha_ff + pid_out, -max_steer, max_steer)
current_steering = lerp(current_steering, target_steer,
                        steer_speed * delta)

Sistema di Sterzo a Quattro Ruote (4WS)

I veicoli tradizionali sterzano soltanto con le ruote anteriori. Questa configurazione impone un raggio di curvatura minimo determinato dalla geometria del veicolo e limita la manovrabilità alle basse velocità. I sistemi 4WS (Four Wheel Steering) aggiungono la capacità di sterzare anche le ruote posteriori, con benefici diversi in funzione della velocità:

• Bassa velocità - Controfase: le ruote posteriori girano in direzione opposta alle anteriori. Il centro istantaneo di rotazione si avvicina al veicolo, riducendo il raggio di curvatura e aumentando la manovrabilità.

• Alta velocità - In fase: le ruote posteriori girano nella stessa direzione delle anteriori. Il veicolo tende a traslare lateralmente invece di ruotare bruscamente, aumentando la stabilità direzionale e riducendo il sovrasterzo.

Modello Matematico del 4WS

L’angolo di sterzo posteriore \(\alpha_r\) è calcolato in funzione dell’angolo anteriore \(\alpha_f\) e della velocità \(v\):

\[\begin{equation} \alpha_r = \alpha_f \cdot r_{max} \cdot \varphi(|v|) \end{equation}\]

dove \(r_{max}\) è il rapporto massimo tra sterzo posteriore e anteriore, e \(\varphi(|v|)\) è il fattore di fase, definito come:

\[\begin{equation} \varphi(|v|) = \begin{cases} -1 & \text{se } v \leq v_{low} \quad \text{(controfase massima)} \\ \displaystyle\frac{v - v_{low}}{v_{high} - v_{low}} \cdot 2 - 1 & \text{se } v_{low} < v < v_{high} \quad \text{(transizione lineare)} \\ +1 & \text{se } v \geq v_{high} \quad \text{(in fase massima)} \end{cases} \end{equation}\]

Il passaggio da \(\varphi = -1\) a \(\varphi = +1\) avviene linearmente tra le soglie \(v_{low}\) e \(v_{high}\). Esiste un valore intermedio di velocità \(v_0 = (v_{low} + v_{high})/2\) in cui \(\varphi = 0\), ovvero le ruote posteriori non sterzano (comportamento identico a un veicolo 2WS standard).

func calculate_rear_steering(front_steer: float, speed: float) -> float:
    var abs_speed = abs(speed)
    var phase_factor: float
    if abs_speed <= max_speed_counterphase_active:
        phase_factor = -1.0
    elif abs_speed >= min_speed_phase_active:
        phase_factor = 1.0
    else:
        var t = (abs_speed - max_speed_counterphase_active) \
              / (min_speed_phase_active - max_speed_counterphase_active)
        phase_factor = lerp(-1.0, 1.0, t)
    return front_steer * rear_steer_ratio_max * phase_factor

Limite Dinamico dello Sterzo Posteriore

Per ragioni di sicurezza e stabilità, l’angolo massimo delle ruote posteriori viene ridotto progressivamente all’aumentare della velocità tramite una ulteriore interpolazione lineare, attiva per velocità superiori ai 20 m/s:

\[\begin{equation} \alpha_{r,lim}(v) = \begin{cases} \alpha_{r,low} & \text{se } v \leq 20\;\text{m/s} \\ \alpha_{r,low} + t \cdot (\alpha_{r,high} - \alpha_{r,low}) & \text{altrimenti} \end{cases} \end{equation}\]

con \(t = \text{clamp}\!\left(\frac{v - 20}{v_{max} - 20},\; 0,\; 1\right)\). L’angolo posteriore effettivo generato dal controllore viene quindi limitato fisicamente dal blocco di saturazione:

\[\begin{equation} \alpha_r = \text{clamp}\!\left(\alpha_f \cdot r_{max} \cdot \varphi(|v|),\; -\alpha_{r,lim},\; +\alpha_{r,lim}\right) \end{equation}\]

L’introduzione di questa limitazione dinamica è necessaria per compensare i vincoli cinematici del modello sterzante ad alte velocità. Nel modello Ackermann di base, le velocità non sono indipendenti, ma la velocità angolare del veicolo (o di imbardata) \(\omega\) dipende direttamente dalla velocità lineare \(v\) e dall’angolo di sterzata secondo la relazione \(\omega=v/R\).

A velocità moderate (sotto i 20 m/s), il sistema utilizza il limite di saturazione standard \(\alpha_{r,low} (\approx 4°)\), che garantisce un’elevata manovrabilità sia durante la sterzata in controfase (riducendo il raggio di curvatura) sia all’inizio della transizione in fase.

Al crescere della velocità lineare, mantenere un angolo di sterzata posteriore costante comporterebbe un incremento lineare della velocità di rotazione del veicolo, portandolo al sovrasterzo e alla perdita di aderenza. Per cui, avvicinandosi alla velocità massima (\(\approx 57\;\text{m/s}\)), l’angolo consentito viene saturato a un valore molto più restrittivo \(\alpha_{r,high} (\approx 0.8°)\): le ruote posteriori in fase lavorano con angoli piccolissimi, sufficienti per un cambio di corsia fluido ma troppo piccoli per indurre rotazioni attorno all’asse verticale che causerebbero lo sbandamento.

Infine, l’interpolazione lineare tra le due soglie garantisce che la restrizione dello sterzo avvenga in modo continuo, evitando le forti oscillazioni che una soglia netta provocherebbe a causa dell’inerzia del veicolo.

@export var V_MAX_REF: float = 56.94  # riferimento fisso, indipendente da target_speed
...
var dynamic_limit = limit_rear_steer_low_speed
if forward_speed > 20.0:
    var t = clamp((forward_speed - 20.0) / (V_MAX_REF - 20.0), 0.0, 1.0)
    dynamic_limit = lerp(limit_rear_steer_low_speed, limit_rear_steer_high_speed, t)

var target_rear    = calculate_rear_steering(current_steering, forward_speed)
target_rear        = clamp(target_rear, -dynamic_limit, dynamic_limit)
current_rear_steer = lerp(current_rear_steer, target_rear, steer_speed * delta)

if rear_left_wheel:  rear_left_wheel.steering  = current_rear_steer
if rear_right_wheel: rear_right_wheel.steering = current_rear_steer

Controllo Adattivo della Velocità

Un aspetto critico della guida autonoma è la gestione della velocità in prossimità delle curve. Affrontare una curva stretta ad alta velocità causa la perdita di aderenza e l’uscita dal percorso. Il sistema implementa un meccanismo di look-ahead sulla curvatura: prima di ogni frame, vengono analizzati i prossimi waypoint per stimare la curvatura e decidere se accelerare o frenare.

Calcolo della Curvatura

Per garantire che il veicolo adatti la propria velocità alle caratteristiche del tracciato, è necessario stimare la geometria del percorso davanti all’auto. Poiché la traiettoria è definita in modo discreto da una sequenza di punti (waypoint), la curvatura in ogni punto viene approssimata calcolando l’angolo di deviazione tra i segmenti adiacenti.

Dato un waypoint generico \(\mathbf{w}_i\), si considerano il vettore del segmento in ingresso \(\mathbf{w}_i - \mathbf{w}_{i-1}\) e il vettore del segmento in uscita \(\mathbf{w}_{i+1} - \mathbf{w}_i\). Normalizzando questi vettori e calcolandone il prodotto scalare, si ottiene il coseno dell’angolo compreso. Applicando l’arcocoseno si estrae l’angolo di deviazione \(\kappa_i\):

\[\begin{equation} \kappa_i = \arccos\!\left( \frac{(\mathbf{w}_i - \mathbf{w}_{i-1})}{|\mathbf{w}_i - \mathbf{w}_{i-1}|} \cdot \frac{(\mathbf{w}_{i+1} - \mathbf{w}_i)}{|\mathbf{w}_{i+1} - \mathbf{w}_i|} \right) \end{equation}\]

Se i tre waypoint sono perfettamente allineati, il prodotto scalare è pari a 1 e \(\kappa_i = 0\) (tratto rettilineo, \(\arccos(1) = 0\)). Al crescere della deviazione direzionale, \(\kappa_i\) aumenta, indicando una curva più stretta.

Analisi Predittiva (Lookahead)

Per il controllo non è sufficiente conoscere la curvatura nel punto attuale: occorre anticipare le curve future per applicare una decelerazione calcolata dal PID di velocità prima dell’ingresso in curva. Sia \(j^*\) l’indice del waypoint attualmente più vicino al veicolo e \(N_{prev}\) la dimensione della finestra di predizione. La curvatura massima imminente viene calcolata come:

\[\begin{equation} \kappa_{max} = \max_{i = j^*+1}^{j^*+N_{prev}} \kappa_i \end{equation}\]

Il valore \(\kappa_{max}\) viene utilizzato per abbassare dinamicamente la velocità target del PID di velocità prima dell’ingresso in curva, prevenendo velocità di imbardata eccessive e la perdita di aderenza.

var max_curv_deg = 0.0
for i in range(wp_idx + 1, min(waypoints.size() - 1, wp_idx + preview_wp + 1)):
    var v1 = (waypoints[i] - waypoints[i-1]); v1.y = 0.0
    var v2 = (waypoints[i+1] - waypoints[i]); v2.y = 0.0
    if v1.length() > 0.01 and v2.length() > 0.01:
        var dot = clamp(v1.normalized().dot(v2.normalized()), -1.0, 1.0)
        var curv_deg = rad_to_deg(acos(dot))
        if curv_deg > max_curv_deg: max_curv_deg = curv_deg

Selezione della Velocità Target

Nella versione iniziale del sistema, la velocità target veniva selezionata in modo binario in base a \(\kappa_{max}\):

\[\begin{equation} v_{target} = \begin{cases} v_{curve} & \text{se } \kappa_{max} > \kappa_{thresh} \\ v_{straight} & \text{altrimenti} \end{cases} \end{equation}\]

Questo approccio, pur funzionale, produce un gradino netto nella velocità target ogni volta che il look-ahead rileva una curva. Il PID di velocità si trova così a dover inseguire un salto da \(v_{straight} = 56.94\;\text{m/s}\) a \(v_{curve} = 13.0\;\text{m/s}\) in un singolo frame, richiedendo una frenata brusca e un tratto di overshoot prima di stabilizzarsi alla velocità di curva.

Soluzione: velocity planning continuo.

Per superare questo limite, la selezione binaria è stata sostituita con una mappatura continua della curvatura in velocità target:

\[\begin{equation} v_{target} = \min\!\left(v_{straight},\; v_{ref} - (v_{ref} - v_{curve}) \cdot \text{clamp}\!\left(\frac{\kappa_{max}}{\kappa_{thresh}},\; 0,\; 1\right)\right) \end{equation}\]

dove \(v_{ref} = 57.0\;\text{m/s}\) è un parametro fisso che rappresenta la velocità massima di riferimento per il calcolo della decelerazione, indipendente da \(v_{straight}\). Questo disaccoppiamento consente di modificare la velocità target del rettilineo senza alterare la legge di decelerazione: se ad esempio si imposta \(v_{straight} = 25\;\text{m/s}\), il sistema calcola la velocità fisica corretta per ogni curvatura riferita a \(v_{ref}\) e prende il minimo tra tale valore e \(v_{straight}\), garantendo che la riduzione sia sempre coerente con la geometria del tracciato.

Il comportamento risultante è:

• \(\kappa_{max} = 0°\): \(v_{target} = v_{straight} = 56.94\;\text{m/s}\) (rettilineo);

• \(\kappa_{max} = \kappa_{thresh} = 5°\): \(v_{target} = v_{curve} = 13.0\;\text{m/s}\) (curva stretta);

• \(0° < \kappa_{max} < 5°\): \(v_{target}\) assume un valore intermedio proporzionale (es. \(\kappa_{max} = 2.5° \Rightarrow v_{target} \approx 35\;\text{m/s}\)).

Vantaggi osservati.

1. Decelerazione anticipata e graduale: il PID riceve una sequenza di target speed decrescenti invece di un singolo gradino, consentendo al freno motore di agire con più anticipo. Il veicolo arriva all’ingresso di una curva stretta già parzialmente decelerato (es. \(\approx 32\;\text{m/s}\) invece di \(56.94\;\text{m/s}\)), riducendo l’overshoot in velocità.

2. Maggiore stabilità nelle curve dolci: la riduzione della velocità in presenza di curvatura bassa diminuisce le forze centrifughe, riducendo lo slittamento laterale e il Cross-Track Error.

I test sul tracciato hanno confermato un peggioramento del tempo sul giro da \(127.8\;\text{s}\) (selezione binaria) a \(132.5\;\text{s}\) (velocity planning continuo), dovuto alla riduzione della velocità anche nelle curve dolci dove non sarebbe strettamente necessario. Il sistema risulta tuttavia più stabile, con comportamento più realistico, minore slittamento laterale e Cross-Track Error ridotto: il velocity planning continuo ottimizza la stabilità consapevolmente a scapito del tempo sul giro.

@export var V_MAX_REF: float = 56.94  # riferimento fisso, indipendente da target_speed
...
var speed_range = V_MAX_REF - curve_speed_limit          # sempre 44 m/s
var curv_norm   = clamp(max_curv_deg / curve_degree_threshold, 0.0, 1.0)
var reduced     = V_MAX_REF - speed_range * curv_norm    # velocita fisica per questa curvatura
var current_target_speed = min(target_speed, reduced)    # prende il minimo tra i due

PID di Velocità

Il controllo della velocità è affidato a un controllore PID dedicato. Il suo ruolo è garantire che la velocità del veicolo insegua con precisione il valore target calcolato dal modulo di look-ahead, sia durante l’accelerazione sul rettilineo sia durante la decelerazione in ingresso curva.

Il PID di velocità produce una risposta proporzionale all’errore: se il veicolo è molto al di sotto della velocità target accelera con forza elevata, se è leggermente sopra riduce gradualmente la forza motore senza frenare bruscamente. Un ulteriore vantaggio è la capacità di applicare freno motore: quando l’errore è negativo (velocità attuale superiore al target) la forza motore può assumere valori negativi, agendo in direzione contraria al moto esattamente come il freno motore di un veicolo reale in rilascio del gas.

L’errore di velocità è definito come:

\[\begin{equation} e_v = v_{target} - v_{actual} \end{equation}\]

e l’uscita del PID di velocità determina direttamente la forza da applicare alle ruote:

\[\begin{equation} F_{PID} = K_{P,v} \cdot e_v \;-\; K_{D,v} \cdot \frac{\Delta v_{actual}}{\Delta t} \;+\; K_{I,v} \cdot I_v \end{equation}\]

Il termine derivativo è calcolato sulla velocità misurata anziché sull’errore (derivative on measurement). Quando la velocità target cambia bruscamente, derivare l’errore produrrebbe un picco impulsivo (derivative kick) proporzionale al salto del setpoint:

\[\begin{equation} K_{D,v} \cdot \frac{\Delta e_v}{\Delta t} = K_{D,v} \cdot \frac{44\;\text{m/s}}{0.016\;\text{s}} \approx 137\,000\;\text{N} \end{equation}\]

Derivando invece la velocità reale, che varia sempre gradualmente per inerzia fisica, il termine derivativo rimane contenuto e svolge solo la funzione di smorzamento:

\[\begin{equation} d_{out} = -K_{D,v} \cdot \frac{v_{actual,k} - v_{actual,k-1}}{\Delta t} \end{equation}\]

L’anti-windup è implementato tramite integrazione condizionata:

\[\begin{equation} I_v = \begin{cases} I_{v,k-1} + e_v \cdot \Delta t & \text{se } |F_{raw}| \leq F_{max} \quad \text{(non in saturazione)} \\ I_{v,k-1} & \text{se } |F_{raw}| > F_{max} \quad \text{(in saturazione)} \end{cases} \end{equation}\]

La forza applicata al veicolo viene calcolata come:

\[\begin{equation} F = \begin{cases} \text{clamp}(F_{PID},\; -F_{max},\; +F_{max}) & \text{se } e_v \geq -\texttt{brake\_threshold} \\ \text{freno proporzionale} & \text{se } e_v < -\texttt{brake\_threshold} \end{cases} \end{equation}\]

dove brake_threshold è la soglia di frenata. Un valore positivo di \(F\) corrisponde a forza motrice, un valore negativo corrisponde al freno motore.

Ottimizzazione della Tolleranza di Frenata.

 
Il parametro brake_threshold definisce una zona morta attorno alla velocità target: finché \(e_v \geq -\texttt{brake\_threshold}\), il sistema utilizza solo il freno motore; i freni meccanici intervengono solo quando l’eccesso di velocità supera tale soglia.

Con brake_threshold \(= 0\) e un guadagno proporzionale elevato (\(K_{P,v} = 200\)), non appena la velocità supera anche di poco il target la condizione \(e_v < 0\) si verifica immediatamente, escludendo il freno motore e attivando i freni meccanici fisici. A 50 m/s con target a 13 m/s l’errore precipita a \(-37\;\text{m/s}\), causando frenate meccaniche continue che dissipano inutilmente energia cinetica e degradano le prestazioni.

Il tuning sperimentale ha prodotto i seguenti riscontri cronometrici:

• brake_threshold \(= 0.0\) m/s \(\rightarrow\) 132.5 s

• brake_threshold \(= 0.5\) m/s \(\rightarrow\) 131.6 s

• brake_threshold \(= 1.0\) m/s \(\rightarrow\) 130.7 s

• brake_threshold \(= 2.0\) m/s \(\rightarrow\) 130.3 s

L’introduzione di una tolleranza di \(2.0\;\text{m/s}\) permette al veicolo di gestire l’ingresso in curva in modo più fluido, sfruttando l’inerzia e il freno motore. Quando il target scende bruscamente, i freni meccanici intervengono con la massima forza solo per abbattere l’eccesso di velocità oltre la soglia; non appena la velocità rientra nella tolleranza, i freni a disco si disattivano e il veicolo continua decelerando per il solo effetto del freno motore. Questo approccio mantiene velocità medie leggermente superiori in fase di ingresso (\(\approx 14.5\;\text{m/s}\)), incrementando la prestazione cronometrica senza perdere stabilità.

# Engine - PID velocita con velocity planning continuo
var speed_range = V_MAX_REF - curve_speed_limit
var curv_norm   = clamp(max_curv_deg / curve_degree_threshold, 0.0, 1.0)
var reduced     = V_MAX_REF - speed_range * curv_norm
var current_target_speed = min(target_speed, reduced)

var error_v = current_target_speed - forward_speed
var p_out   = Kp_v * error_v

# Derivata sulla misura (derivative on measurement)
var d_out = -Kd_v * (forward_speed - prev_forward_speed) / delta
prev_forward_speed = forward_speed

# Anti-windup: integrazione condizionata
if not in_saturation:
    speed_integral += error_v * delta

var raw_force = p_out + Ki_v * speed_integral + d_out
current_engine = clamp(raw_force, -max_engine_force, max_engine_force)
in_saturation  = raw_force > max_engine_force or raw_force < -max_engine_force

if error_v >= -brake_threshold:
    _applica_motore(current_engine)
else:
    var brake_force = clamp(abs(error_v) * Kp_v * 0.1, 10.0, brake_power)
    _applica_freno(brake_force)

Sinergia con il Sistema 4WS

La scelta delle soglie di velocità del controllo adattivo è stata coordinata con i parametri del sistema 4WS:

• \(v_{curve} = 13\;\text{m/s} < v_{low} = 15\;\text{m/s}\): in curva il veicolo è sufficientemente lento da attivare la controfase massima del posteriore, minimizzando il raggio di curvatura.

• \(v_{straight} = 56.94\;\text{m/s} > v_{high} = 19\;\text{m/s}\): sul rettilineo il veicolo è sufficientemente veloce da attivare la fase del posteriore, massimizzando la stabilità direzionale.

Con il velocity planning continuo, la velocità target attraversa l’intera gamma \([13, 56.94]\;\text{m/s}\) durante le transizioni curva–rettilineo, percorrendo quindi anche la zona di transizione 4WS (\([15, 19]\;\text{m/s}\)). Questo non ha causato instabilità nelle prove sperimentali, poiché il veicolo attraversa questa zona rapidamente e il limite dinamico dello sterzo posteriore mantiene gli angoli contenuti.

Engine Force Modulation

L’Engine Force Modulation descrive la strategia con cui il sistema gestisce la forza motore lungo il percorso, modulandola tra il valore massimo sul rettilineo e la forza necessaria per rispettare la velocità di sicurezza in curva. L’obiettivo è duplice: massimizzare la velocità nei tratti rettilinei e garantire una decelerazione precisa e anticipata prima di ogni curva.

Forza Massima sul Rettilineo

In assenza di curvatura imminente, \(v_{target} = v_{straight}\) e il PID eroga la forza massima disponibile:

\[\begin{equation} F = \text{clamp}(F_{PID},\; -F_{max},\; +F_{max}) = F_{max} \quad \text{(rettilineo)} \end{equation}\]

Il motore opera alla piena potenza, distribuita equamente alle quattro ruote:

\[\begin{equation} F_{ruota} = \frac{F_{max}}{4} = \frac{5800}{4} = 1450\;\text{N} \end{equation}\]

Decelerazione in Ingresso Curva

Con il velocity planning continuo, la velocità target scende progressivamente man mano che la curvatura aumenta nella finestra di look-ahead. Quando la curvatura raggiunge \(\kappa_{thresh}\), \(v_{target} = v_{curve} = 13.0\;\text{m/s}\) e l’errore diventa fortemente negativo, attivando il freno motore:

\[\begin{equation} e_v = v_{curve} - v_{actual} < 0 \;\Rightarrow\; F_{PID} < 0 \end{equation}\]

Se la velocità supera il target di più di brake_threshold, intervengono anche i freni meccanici in modo proporzionale:

\[\begin{equation} F_{freno} = \text{clamp}(|e_v| \cdot K_{P,v} \cdot 0.1,\; 10,\; F_{brake}) \end{equation}\]

Accelerazione in Uscita Curva

Non appena la curvatura nella finestra di look-ahead diminuisce, \(v_{target}\) risale proporzionalmente verso \(v_{straight}\) e il motore riprende ad accelerare:

\[\begin{equation} F(t) = \text{clamp}\!\left(K_{P,v} \cdot e_v(t) + K_{I,v} \cdot I_v(t) - K_{D,v} \cdot \frac{\Delta v_{actual}(t)}{\Delta t},\; -F_{max},\; +F_{max}\right) \end{equation}\]

Ottimizzazione del Tempo sul Giro

La calibrazione è stata condotta tramite test iterativi in due fasi distinte, riportati in Tabella 3.4.

I \(\Delta t\) della Fase A sono relativi ad A0; i \(\Delta t\) della Fase B sono relativi al riferimento binario (127.8 s per B1) e al test precedente per B2–B5.

Test A1 — Abilitazione del freno motore.

Estendendo il range della forza a valori negativi, il PID può decelerare attraverso il motore prima ancora che i freni meccanici intervengano. La riduzione di 0.7 s conferma che la precisione della decelerazione in ingresso curva incide sul tempo più della velocità assoluta sul rettilineo.

Test B1 — Velocity planning continuo.

La sostituzione della selezione binaria con la mappatura continua curvatura–velocità ha prodotto un incremento di 4.7 s (da 127.8 a 132.5 s, \(+3.7\%\)). Il costo è attribuibile alla riduzione della velocità anche nelle curve dolci. Il sistema risulta tuttavia più stabile: forze centrifughe ridotte, minore slittamento laterale, Cross-Track Error migliorato. La modifica è mantenuta per criteri di stabilità e realismo del profilo di velocità.

Test B4 — Ottimizzazione brake_threshold.

Con brake_threshold \(= 0.0\), l’errore \(e_v\) negativo attiva immediatamente i freni meccanici invece del freno motore, causando micro-frenate continue che dissipano energia e peggiorano il tempo. Aumentare la soglia a \(2.0\;\text{m/s}\) recupera 2.1 s: i freni meccanici intervengono solo per abbattere gli eccessi maggiori, lasciando al freno motore il controllo delle decelerazioni graduali.

Test B5 — Calibrazione del passo veicolo.

Il termine feedforward Ackermann dipende direttamente dal valore del passo \(L\): un valore approssimato \(L = 3.0\;\text{m}\) introduce un errore sistematico nel calcolo di \(\alpha_{ff}\), sovrastimando leggermente l’angolo necessario in curva. Sostituendo il valore nominale con il passo reale della Ford Mustang 1969 (\(L = 2.819\;\text{m}\)), il feedforward geometrico risulta più preciso, riducendo il carico correttivo sul PID e abbassando il tempo di \(0.6\;\text{s}\) (da \(130.4\;\text{s}\) a \(129.8\;\text{s}\)). Il risultato conferma che la precisione del modello geometrico ha un impatto misurabile sulle prestazioni cronometriche.

Considerazione generale.

I test evidenziano tre trade-off fondamentali: la qualità del profilo di velocità target (binario vs. continuo) influenza la stabilità più del tempo; la corretta calibrazione della soglia di frenata è determinante, una soglia nulla penalizza le prestazioni attivando i freni meccanici in modo eccessivo, mentre una soglia ottimizzata sfrutta al meglio la sinergia tra freno motore e freni meccanici; infine la precisione del modello geometrico, usare il passo reale del veicolo nel feedforward Ackermann anziché un valore approssimato produce un guadagno misurabile, confermando che la fedeltà del modello fisico si traduce direttamente in prestazioni migliori.

Path Planning e Costruzione del Percorso

Estrazione dei Punti dalla Curva

Il percorso di riferimento è definito in Godot tramite un nodo Path3D. All’avvio della simulazione, la funzione _build_path() estrae i punti baked della curva e li campiona con una spaziatura regolare di 1 metro, ottenendo una sequenza di waypoint \(\mathbf{w}_0, \mathbf{w}_1, \ldots, \mathbf{w}_N\):

func _build_path():
    var points = path_node.curve.get_baked_points()
    var out: Array = []
    var accumulated = 0.0
    out.append(points[0])
    for i in range(1, points.size()):
        accumulated += points[i].distance_to(points[i-1])
        if accumulated >= 1.0:
            out.append(points[i])
            accumulated = 0.0
    waypoints = out

La spaziatura uniforme di 1 metro garantisce che il calcolo della curvatura e la selezione del rabbit point producano risultati consistenti indipendentemente dalla densità originale dei punti della curva.

Nearest Point Tracking

A ogni frame, il sistema identifica il waypoint più vicino alla posizione attuale del veicolo. Per efficienza, la ricerca è limitata a una finestra di \(\pm 5\) waypoint attorno all’indice corrente più i successivi 40:

\[\begin{equation} j^* = \arg\min_{j \in [wp\_idx - 5,\; wp\_idx + 40]} \|\mathbf{p}_{car} - \mathbf{w}_j\|^2 \end{equation}\]

Questa strategia evita la ricerca globale su tutto il percorso mantenendo robustezza in caso di piccole deviazioni.

Risultati e Analisi

Acquisizione Dati e Analisi dei Risultati

Sistema di Logging CSV

Per l’analisi quantitativa delle prestazioni, è stato implementato un sistema di acquisizione dati che salva i principali parametri di simulazione in un file CSV a ogni \(N_{log}\) frame (con \(N_{log} = 3\), corrispondente a circa 20 Hz). Le 20 colonne registrate sono riportate in Tabella 4.1.

Visualizzazione con Python/Matplotlib

I dati raccolti vengono elaborati e visualizzati tramite uno script Python che genera una figura composta da 8 grafici:

1. Traiettoria XZ colorata per velocità (mappa di calore);

2. Velocità reale vs target con area di errore evidenziata in arancione;

3. Sterzo anteriore e posteriore sovrapposti per il confronto;

4. Fase del 4WS: visualizzazione del prodotto \(\alpha_f \cdot \alpha_r\) (positivo = in fase, negativo = controfase);

5. Errore angolare PID nel tempo;

6. Cross-Track Error (CTE): deviazione laterale dal percorso;

7. PID velocità: forza motore e output PID su asse doppio;

8. Riepilogo statistico: durata, velocità media/massima, CTE medio/massimo, errore RMS angolare e di velocità, percentuale di tempo in controfase/in fase.

Metriche di Performance

• Cross-Track Error (CTE): \(CTE_k = \|\mathbf{p}_{car}^{(k)} - \mathbf{w}_{j^*}^{(k)}\|\)

• Errore angolare RMS: \(e_{\theta,RMS} = \sqrt{\frac{1}{N}\sum_{k=1}^{N} e_{\theta,k}^2}\)

• Errore di velocità RMS: \(e_{v,RMS} = \sqrt{\frac{1}{N}\sum_{k=1}^{N} e_{v,k}^2}\)

• Tempo sul giro: metrica globale di ottimizzazione del sistema.

Analisi delle Performance

Architettura del Software

Conclusioni

Il progetto ha dimostrato come sia possibile realizzare un sistema di guida autonoma funzionale all’interno di un ambiente di simulazione fisica, integrando diverse tecniche di controllo automatico.

L’algoritmo Pure Pursuit abbinato a un termine feedforward basato sul modello cinematico Ackermann ha fornito un angolo di sterzo nominale fisicamente coerente con la traiettoria richiesta; il controllore PID di feedback ha corretto gli errori residui dovuti a derive sistematiche e imprecisioni del modello. L’introduzione del sistema 4WS ha migliorato sensibilmente la manovrabilità nelle curve strette (grazie alla controfase a bassa velocità) e la stabilità direzionale ad alta velocità (grazie alla fase).

Il controllore PID di velocità ha consentito una gestione fluida e precisa delle transizioni accelerazione–decelerazione. L’abilitazione del freno motore tramite forza negativa ha prodotto una riduzione del tempo sul giro. L’evoluzione verso il velocity planning continuo ha sostituito la selezione binaria della velocità target con una mappatura proporzionale alla curvatura, migliorando la stabilità e il realismo del profilo di velocità a fronte di un incremento del tempo sul giro, accettato consapevolmente. La successiva ottimizzazione del parametro brake_threshold a \(2.0\;\text{m/s}\) ha recuperato 2.1 s, sfruttando al meglio la sinergia tra freno motore e freni meccanici. Infine, la calibrazione del passo reale del veicolo (\(L = 2.819\;\text{m}\)) nel feedforward Ackermann ha prodotto un ulteriore guadagno di 0.6 s, confermando che la fedeltà del modello geometrico si traduce direttamente in prestazioni migliori.

Il sistema di acquisizione dati ha reso possibile un’analisi delle prestazioni tramite grafici generati in Python, facilitando il processo di calibrazione dei parametri.