Hallo Folk,
Heute direkt vom Feldtest:
Das Video ist nur eine kleine Impression. In der Halle waren es gemütliche 25 °C, draußen dagegen gefühlt deutlich über 30 °C – der Rover war entsprechend „rot glühend“ unterwegs.
Auf dem PC läuft der Mission Planner. Zu sehen ist die programmierte Route. Zusätzlich liest die Sprachausgabe einige Werte vor, die der Rover an den PC sendet. Die Datenpakete selbst kommen vollständig an, allerdings schafft die Sprachausgabe das Tempo verständlicherweise nicht ganz.
Trotzdem ist das sehr nützlich: So kann man den PC auch mal verlassen und direkt am Rover etwas prüfen oder untersuchen, ohne ständig auf den Bildschirm schauen zu müssen.
Interessant sind im Moment zwei Dinge:
1: Auf dem PC: Die Querabweichung von der Ideallinie = Track Error im Messages-Fenster
2. Die ständig arbeitenden Vorderräder des Rover
Zunächst bin ich recht glücklich das "anscheinend" ALLES funktioniert. Insbesondere die Navigation auf ausschließlich GPS-x/y-Punkten.
Dazu habe ich mal einen kleinen code-Auszug. Hier könnt ihr sehen, daß keine Winkel von einem Sensor benötigt werden.
Da mein Rover ja nicht auf der Stelle wendet, gibt es immer neue Koordinaten. Für Euch, Hinterachs-Angetriebene, geht das aber genauso gut. Ihr müßt für einen Wendevorgang einfach nur einen Punkt im 90-Grad Winkel "konstruieren" und dann den Regler rechnen lassen. Oder ihr verwendet einfach reale Wegpunkte. Bei RTK-Fix funktioniert das bei mir sehr gut.
void State::STC(int _next_target_idx){ Übergabe der Nummer n des anzusteuernden Wegpunktes X[n]
if(_next_target_idx < 1){
_next_target_idx = 1;
}
// Ideallinie für den Stanley-Controller festlegen.
// Normalfall: Linie vom vorherigen Wegpunkt X(n-1) zum aktuellen Zielwegpunkt X
.
int from_idx = _next_target_idx - 1;
int to_idx = _next_target_idx;
// Sonderfall erster echter Missionswegpunkt:
// Home wird nicht als Anfangspunkt der Sollspur verwendet.
// Statt Home -> WP1 wird die Fahrspur WP1 -> WP2 als Ideallinie genutzt.
// Richtungsvektor der Ideallinie d = X(to_idx) - X(from_idx)
float d_x = waypointlistxy[to_idx].x - waypointlistxy[from_idx].x;
float d_y = waypointlistxy[to_idx].y - waypointlistxy[from_idx].y;
// Vektor v vom Startpunkt der Ideallinie zur aktuellen Rover-Position
float v_x = rover_x_m - waypointlistxy[from_idx].x;
float v_y = rover_y_m - waypointlistxy[from_idx].y;
// Skalarprodukt v·d für die Projektion der Rover-Position auf die Ideallinie
float dot_product_v_d = v_x * d_x + v_y * d_y;
// Skalarprodukt d·d entspricht der quadrierten Länge der Ideallinie
float dot_product_d_d = d_x * d_x + d_y * d_y;
// Schutz gegen identische oder nahezu identische Wegpunkte.
// In diesem Fall ist keine sinnvolle Ideallinie definiert.
if (dot_product_d_d < 0.0001) {
lenkeinschlag_rad = 0;
lenkeinschlag_deg = 0;
live.STC_angle_rad = lenkeinschlag_rad;
return;
}
// Positionsfaktor der senkrechten Projektion auf die Ideallinie.
// scale = 0 liegt bei from_idx, scale = 1 liegt bei to_idx.
float scale = dot_product_v_d / dot_product_d_d;
// Projektionspunkt der Rover-Position auf die Ideallinie,
// bezogen auf den Startpunkt der Ideallinie.
float proj_v_on_d_x = scale * d_x;
float proj_v_on_d_y = scale * d_y;
// Querfehlervektor r von der Ideallinie zur aktuellen Rover-Position.
// Betrag von r = seitlicher Abstand des Rovers zur Ideallinie.
float r_x = v_x - proj_v_on_d_x;
float r_y = v_y - proj_v_on_d_y;
// Abstand des Rovers zum aktuellen Zielwegpunkt.
// Dieser Abstand wird hier zur Abschwächung des Stanley-Anteils in Zielnähe verwendet.
float distanceToTarget = sqrt((waypointlistxy[_next_target_idx].x - rover_x_m) * (waypointlistxy[_next_target_idx].x - rover_x_m) + (waypointlistxy[_next_target_idx].y - rover_y_m) * (waypointlistxy[_next_target_idx].y - rover_y_m));
// Betrag des Querfehlers zur Ideallinie.
float r_magnitude = sqrt(r_x * r_x + r_y * r_y);//Querfehler (Cross-Track-Error) – also den Abstand des Rovers zur Ideallinie
// Wirksame Geschwindigkeit mit Mindestwert.
// Verhindert Division durch 0 und zu hohe Lenkausschläge bei sehr kleiner Geschwindigkeit.
float v_eff = max(fabs(rover_v_mps), 0.01);
// Wirksamer Zielabstand mit Mindestwert.
// Verhindert Division durch 0 direkt am Zielwegpunkt.
float d_eff = max(distanceToTarget, 0.1);
// Headingfreie Stanley-Variante:
// Der Lenkwinkel wird nur aus Querfehler, Zielabstand und Geschwindigkeit berechnet.
//float theta = atan2(r_magnitude * stanley_dist / d_eff, v_eff / stanley_spee);
//float theta = atan2(r_magnitude * stanley_dist, v_eff / stanley_spee);
// ich nutze im Moment nur die Querabweichung, dadurch fährt der Rover immer direkt zur Ideallinie
float theta = atan2(r_magnitude * stanley_dist, 1.0);
// 2D-Kreuzprodukt zur Bestimmung, auf welcher Seite der Ideallinie der Rover steht.
float cross_product_z = d_x * r_y - d_y * r_x;
// Positive z-Komponente: Rover steht links der Sollspur.
// Da Rechtslenkung negativ ist, wird theta in diesem Fall negativ gesetzt.
// Negative z-Komponente: Rover steht rechts der Sollspur, theta bleibt positiv.
if (cross_product_z > 0) {
// Rechts lenken
theta = -theta;
}
if (isnan(theta)) {
#ifdef CONTROLLER_DEBUGGING
Serial.println("theta ist NaN!");
#endif
theta = 0;
}
cross_track_error_cm = r_magnitude * 100.0;
RunningMedianCrossTrackErrorCm.add(cross_track_error_cm);
cross_track_error_cm = RunningMedianCrossTrackErrorCm.getAverage();
if(cross_product_z > 0){
cross_track_error_cm = -cross_track_error_cm;
}
// Ergebnis der headingfreien STC-Variante übernehmen.
// Es wird kein Magnetheading, kein IMU-Heading und kein heading_error verwendet.
lenkeinschlag_rad = theta;
// Lenkwinkel auf den mechanisch zulässigen Bereich begrenzen.
lenkeinschlag_rad = constrain(lenkeinschlag_rad, -maxSteer, maxSteer);
if (isnan(lenkeinschlag_rad)) {
#ifdef CONTROLLER_DEBUGGING
Serial.println("lenkeinschlag_rad ist NaN!");
#endif
lenkeinschlag_rad = 0;
}
if (rover_v_mps < 0) {
// Rückwärtsfahrt:
// Diese headingfreie STC-Variante wird rückwärts nicht angewendet.
// Variante A: beim Zurücksetzen geradeaus fahren.
lenkeinschlag_rad = 0.0;
// Variante B: optional entgegengesetzt lenken.
//lenkeinschlag_rad = -lenkeinschlag_rad;
}
// Lenkwinkel zusätzlich in Grad bereitstellen.
lenkeinschlag_deg = PIDeg(lenkeinschlag_rad);
#ifdef CONTROLLER_DEBUGGING
Serial.print(" StanleyWinkel : ");Serial.print(PIDeg(theta)); Serial.print(" Lenkung : ");Serial.println(lenkeinschlag_deg);
#endif
// STC-Ausgabewert für Anzeige, Debugging oder Reglerauswahl bereitstellen.
live.STC_angle_rad = lenkeinschlag_rad;
}
Allerdings sieht man die "geringe" Abtastrate vom GPS (5Hz). Dadurch gibt es kein geschmeidiges Lenken.
daher ist nun mein nächster Schritt für den schönen Fall des RTK-Fix den Kalman-Filter wieder in Betrieb zu setzen.
Ich werde berichten.
Meine Ritter - schönes Basteln, Testen und Schwitzen!
Euer immer gnädiger Fürst Ruprecht
