Signalverarbeitende Systeme - L10: Koordinatentransformation: Unterschied zwischen den Versionen

Aus HSHL Mechatronik
Zur Navigation springen Zur Suche springen
 
(4 dazwischenliegende Versionen desselben Benutzers werden nicht angezeigt)
Zeile 112: Zeile 112:
| <strong>Musterlösung 10.2&thinsp;</strong>
| <strong>Musterlösung 10.2&thinsp;</strong>
|-
|-
| Das Lösungsvideo finden Sie auf der Lernplattform.
| <source line lang="matlab" style="font-size:medium">%% Gegeben
disp('************************************')
disp('Gegeben:')
A_r_P = [-6.341;-0.2345;-3;1] % Punkt P im A-KOS
B_r_0A = [3;3;0;1]            % Ursprung des A-KOS im B-KOS
C_r_0B = [5;2;0;1]            % Ursprung des B-KOS im C-KOS
 
% Homogene Transformationsmatrizen
Gamma = 10; % deg
C_D_B = [cosd(Gamma) -sind(Gamma) 0 C_r_0B(1) ;sind(Gamma) cosd(Gamma) 0 C_r_0B(2); 0 0 1 C_r_0B(3); 0 0 0 C_r_0B(4)]; % TRAFO {B}->{C}
B_D_A = [cosd(Gamma) -sind(Gamma) 0 B_r_0A(1) ;sind(Gamma) cosd(Gamma) 0 B_r_0A(2); 0 0 1 B_r_0A(3); 0 0 0 B_r_0A(4)]; % TRAFO {A}->{B}
 
%% Lösung
disp('************************************')
disp('Lösung:')
 
%% {A} -> {B}
B_r_P = B_D_A*A_r_P; % Punkt P im B-KOS
 
%% {A} -> {C}
C_r_P = C_D_B*B_D_A*A_r_P % Punkt P im A-KOS
 
%% {B} -> {C}
C_r_0A = C_D_B*B_r_0A % Translationsvektor Ursprung des A-KOS im C-KOS
C_r_AP = C_r_P-C_r_0A % Ortsvektor Ursprung des A-KOS zu P im C-KOS
 
%% Probe über Inverse Transformation
A_r_P2 = inv(C_D_B*B_D_A)*C_r_P
 
%% Darstellung der Koordinatensysteme über Einheitsvektoren
e_x = [1 0 0 1]'; % Einheitsvektoren im C
e_y = [0 1 0 1]';
%% {A} -> {C}
C_e_x_A = C_D_B*B_D_A*e_x % Trafo der Einheitsvektoren
C_e_y_A = C_D_B*B_D_A*e_y
%% {B} -> {C}
C_e_x_B = C_D_B*e_x % Trafo der Einheitsvektoren
C_e_y_B = C_D_B*e_y
 
%% Ergebnisdarstellung
figure
hold on
xlabel('x-Achse')
ylabel('y-Achse')
axis equal
ylim([-0.5 7])
 
%% {C}-KOS
quiver(0,0,1,0,'LineWidth',2,'Color','black','AutoScale','off'); % x-Achse {C}
text(1.1,0,'^C x')
quiver(0,0,0,1,'LineWidth',2,'Color','black','AutoScale','off'); % y-Achse {C}
text(0,1.1,'^C y')
text(-0.1,-0.3,'\{C\}')
 
%% {B}-KOS
h4 = quiver(0,0,C_r_0B(1),C_r_0B(2),'LineWidth',1,'Color','b','AutoScale','off'); % C_r_0B
plot(C_r_0B(1),C_r_0B(2),'ko')
text(C_r_0B(1)-0.1,C_r_0B(2)-0.3,'\{B\}')
quiver(C_r_0B(1),C_r_0B(2),C_e_x_B(1)-C_r_0B(1),C_e_x_B(2)-C_r_0B(2),'LineWidth',2,'Color','k','AutoScale','off'); % C_r_0B
quiver(C_r_0B(1),C_r_0B(2),C_e_y_B(1)-C_r_0B(1),C_e_y_B(2)-C_r_0B(2),'LineWidth',2,'Color','k','AutoScale','off'); % C_r_0B
text(C_e_x_B(1),C_e_x_B(2)+0.1,'^B x')
text(C_e_y_B(1),C_e_y_B(2)+0.1,'^B y')
 
plot(C_r_P(1),C_r_P(2),'ro'); % C_r_P
text(C_r_P(1)-0.1,C_r_P(2)+0.3,'P','Color','r')
h1=quiver(0,0,C_r_P(1),C_r_P(2),'LineWidth',1,'Color','r','AutoScale','off'); % C_r_P
h2=quiver(C_r_0B(1),C_r_0B(2),C_r_P(1)-C_r_0B(1),C_r_P(2)-C_r_0B(2),'LineWidth',1,'Color','r','LineStyle','--','AutoScale','off'); % C_r_P
 
%% {A}-KOS
h5 = quiver(C_r_0B(1),C_r_0B(2),C_r_0A(1)-C_r_0B(1),C_r_0A(2)-C_r_0B(2),'LineWidth',1,'Color','b','LineStyle','--','AutoScale','off'); % Vektor nach Trafo
plot(C_r_0A(1),C_r_0A(2),'ko')
text(C_r_0A(1)-0.1,C_r_0A(2)-0.3,'\{A\}')
quiver(C_r_0A(1),C_r_0A(2),C_e_x_A(1)-C_r_0A(1),C_e_x_A(2)-C_r_0A(2),'LineWidth',2,'Color','k','AutoScale','off'); % C_r_0A
quiver(C_r_0A(1),C_r_0A(2),C_e_y_A(1)-C_r_0A(1),C_e_y_A(2)-C_r_0A(2),'LineWidth',2,'Color','k','AutoScale','off'); % C_r_0A
text(C_e_x_A(1),C_e_x_A(2)+0.1,'^A x')
text(C_e_y_A(1),C_e_y_A(2)+0.1,'^A y')
 
 
%A_r_P = inv(C_D_B)*inv(B_D_A)*C_r_P
h3 = quiver(C_r_0A(1),C_r_0A(2),C_r_AP(1),C_r_AP(2),'LineWidth',1,'Color','r','LineStyle','-.','AutoScale','off'); % A_r_P
 
legend([h3,h2,h1,h5,h4],{'$^A\vec{r}_P$','$^B\vec{r}_P$','$^C\vec{r}_P$','$^B\vec{r}_{0,A}$','$^C\vec{r}_{0,B}$'},'Interpreter','latex','Location','east')
</source>
|}
|}


Zeile 169: Zeile 251:
|}
|}
= Übung 10.5: KOS-Trafo: Homogene transformationsmatrix, Inverse Transformation =
= Übung 10.5: KOS-Trafo: Homogene transformationsmatrix, Inverse Transformation =
Mit einem LiDAR Sensor der Firma topcon wird die Fahrt eines Roboters vermessen.  
Mit einem LiDAR Sensor der Firma Topcon wird die Fahrt eines Roboters vermessen.  
* Als digitale Karte liegen die Punkte des äußeren und inneren Fahrbahnrandes vor.
* Als digitale Karte liegen die Punkte des äußeren und inneren Fahrbahnrandes vor.
* Auf der Fahrbahn steht Objekt O.  
* Auf der Fahrbahn steht Objekt O.  
Zeile 223: Zeile 305:
| <strong>Musterlösung 10.5&thinsp;</strong>
| <strong>Musterlösung 10.5&thinsp;</strong>
|-
|-
| Die Lösung wird in der Übung besprochen.
| <source line lang="matlab" style="font-size:medium">%% Fahrbahnmarkierungen laden und sichern
InnenLinie = load('InnenLinie.mat');
AussenLinie = load('AussenLinie.mat');
 
%% Gegeben
T_r_OW  = [-4.7710;4.5473;0.03;1]  % Ursprung des W-KOS im T-KOS in m
T_r_S  = [-4.1584;2.8208;0.03;1]  % Punkt der Startlinie im T-KOS in m
T_r_AMR = [-3.9137;2.7293;0.03;1]  % Startpunkt des AMR im T-KOS in m
T_r_O  = [-1.8647;-1.8491;0.03;1] % Objektposition im T-KOS in m
 
%% Annahme: A und B sind zwei Punkte auf der Startgeraden der Aussenlinie
A = [-3.32;2.94;-0.03];
B = [-1.45;-2.33;-0.03];
AB = B-A
alpha = atand(AB(2)/AB(1)) % Drehwinkel W-KOS in T-KOS in deg
 
%% Roboterpose berechnen
T_AMR(:,1)=T_r_AMR; % Startpunkt des AMR im T-KOS in m
for i=2:5
    T_AMR(:,i)=T_AMR(:,i-1)+[cosd(alpha);sind(alpha);0;0]; % Roboterposen 2, 3, 4, 5
end
%% T-KOS
figure
plot(AussenLinie.PosX,AussenLinie.PosY,'k-')
hold on
plot(InnenLinie.PosX,InnenLinie.PosY,'b-')
title('T-KOS')
xlabel('x_T in m')
ylabel('y_T in m')
 
% Startlinie
plot(T_r_S(1),T_r_S(2),'ro')
text(T_r_S(1)-0.5,T_r_S(2),'S')
line([T_r_S(1),T_r_S(1)+0.83*sind(-alpha)],[T_r_S(2),T_r_S(2)+0.83*cosd(-alpha)],'LineWidth',2,'Color','k')
 
% Objekt
plot(T_r_O(1),T_r_O(2),'r','Marker','square')
 
% Roboterpose
plot(T_AMR(1,:),T_AMR(2,:),'bo')
for i=1:5
    quiver(T_AMR(1,i),T_AMR(2,i),T_AMR(1,2)-T_AMR(1,1),T_AMR(2,2)-T_AMR(2,1),'Color','b')
end
 
axis equal
xline(0) % Ursprung markieren
yline(0)
 
%% KOS-Trafo T->W
alpha = - alpha;
W_r_0S = [cosd(alpha) -sind(alpha) 0; sind(alpha) cosd(alpha) 0;0 0 1]*-T_r_OW(1:3); % Ursprung des S-KOS im W-KOS
D = [cosd(alpha) -sind(alpha) 0 W_r_0S(1); sind(alpha) cosd(alpha) 0 W_r_0S(2);0 0 1 W_r_0S(3);0 0 0 1] % Homogene Trafo
W_r_A = D*[A;1]; % Vektor zum Punkt A im W-KOS
W_r_B = D*[B;1]; % Vektor zum Punkt A im W-KOS
 
%% Innenlinie
for i=1:length(InnenLinie.PosX)
    W_P = [InnenLinie.PosX(i);InnenLinie.PosY(i);InnenLinie.PosZ(i);1];
    W_InnenLinie(:,i)=D*W_P;
end
%% Aussenlinie
for i=1:length(AussenLinie.PosX)
    W_P = [AussenLinie.PosX(i);AussenLinie.PosY(i);AussenLinie.PosZ(i);1];
    W_AussenLinie(:,i)=D*W_P;
end
%% Startlinie
W_r_S = D * T_r_S;
W_Startlinie = [W_r_S W_r_S];
W_Startlinie(2,2)=0.83;
 
%% Objekt
W_r_O = D*T_r_O;
 
%% AMR
for i=1:5
    W_AMR(:,i) = D*T_AMR(:,i);
end
 
%% W-KOS
figure
plot(W_AussenLinie(1,:),W_AussenLinie(2,:),'k-')
hold on
plot(W_InnenLinie(1,:),W_InnenLinie(2,:),'b-')
% Startlinie
line(W_Startlinie(1,:),W_Startlinie(2,:),'LineWidth',2,'Color','k')
 
% Objekt
plot(W_r_O(1),W_r_O(2),'r','Marker','square')
 
% Roboterpose
plot(W_AMR(1,:),W_AMR(2,:),'bo')
for i=1:5
    quiver(W_AMR(1,i),W_AMR(2,i),W_AMR(1,2)-W_AMR(1,1),W_AMR(2,2)-W_AMR(2,1),'Color','b')
end
title('W-KOS')
xlabel('x_W in m')
ylabel('y_W in m')
axis equal
xline(0) % Ursprung markieren
yline(0)
 
%% KOS-Trafo W->F
F_r_O = W_r_O - W_AMR;
 
%% F-KOS
figure
hold on
quiver(0,0,0,6,'Color','blue') % xF
quiver(0,0,6,0,'Color','blue') % yF
text(0.1,5.8,'x_F','Color','blue')
text(5.8,0.1,'y_F','Color','blue')
plot(F_r_O(2,:),F_r_O(1,:),'r','Marker','square')
set(gca,'XDir', 'reverse')
title('F-KOS')
xlabel('y_F in m')
ylabel('x_F in m')
axis equal
</source>
|}
 
= Übung 10.6: Koordinatentransformation - Navigation eines Autonomen Mobilen Roboters (AMR) =
'''Grader-URL:''' [https://grader.mathworks.com/courses/165734-ss2025-bse-signalverarbeitende-systeme/assignments/471898-lektion-10-koordinatentransformationen/problems/1731492-koordinatentransformation-navigation-eines-autonomen-mobilen-roboters-amr]
{| role="presentation" class="wikitable mw-collapsible mw-collapsed"
| <strong>Musterlösung 10.6&thinsp;</strong>
|-
| <source line lang="matlab" style="font-size:medium">pRobotWorld = randi([-5 5], [2 1])  % Roboterposition in Weltkoordinaten [-5 5] m
theta = 360*rand                          % Rotation (Kurs) des Roboters in deg
pObjectWorld = randi([-5 5], [2 1])      % Objektposition in Weltkoordinaten [-5 5] m
 
s = 0.5; % m
 
hfigure = figure;
for i = 1:6
   
    pRoboterPose(:,i)=[pRobotWorld(:,1);theta];
% Calculate T
T = KOSTrafo(pRobotWorld,theta);
 
% Use the inverse of T (or backslash operator to find the homogenous
% position in the robow frame. Make sure to add a 1 to destWorld
pObject = inv(T)*[pObjectWorld; 1];
 
% Now take only the first two elements of pr since we are in 2-D
pObjectRobot(:,i) = pObject(1:2);
 
 
 
subplot 121
hold on
PlotBot(pRobotWorld,theta)
plot(pObjectWorld(1),pObjectWorld(2),'sr')
xlabel('x in m')
ylabel('y in m')
axis equal
limMax = 6;
xlim([-limMax limMax])
ylim([-limMax limMax])
quiver(0,0,5,0,'Color','blue')
quiver(0,0,0,5,'Color','blue')
text(-0.2,6,'y_W')
text(6,0,'x_W')
grid minor
 
subplot 122
hold on
plot(pObjectRobot(1,:),pObjectRobot(2,:),'sr')
if i>1
    dx = pObjectRobot(1,i)-pObjectRobot(1,i-1);
    dy = pObjectRobot(2,i)-pObjectRobot(2,i-1);
quiver(pObjectRobot(1,i-1),pObjectRobot(2,i-1),dx, dy,'Color','red')
end
xlabel('x in m')
ylabel('y in m')
axis equal
xlim([-limMax limMax])
ylim([-limMax limMax])
quiver(0,0,5,0,'Color','blue')
quiver(0,0,0,5,'Color','blue')
text(-0.2,6,'y_R')
text(6,0,'x_R')
grid minor
 
[pRobotWorld,theta] = BewegeRoboter(pRobotWorld,theta);
end
 
print(hfigure, '-djpeg', 'Ergebnis_Roboternavigation.jpg');
 
function Dwr=KOSTrafo(pRobotWorld,theta)
Dwr = [cosd(theta), -sind(theta), pRobotWorld(1); sind(theta), cosd(theta), pRobotWorld(2); 0, 0, 1];
end
 
function PlotBot(pRobotWorld,theta)
  fLaenge = 0.4;
  plot(pRobotWorld(1),pRobotWorld(2),'ob')
  line([pRobotWorld(1) pRobotWorld(1)+fLaenge*cosd(theta)],[pRobotWorld(2) pRobotWorld(2)+fLaenge*sind(theta)],'Color','blue')
end
 
function [pRobotWorld,theta] = BewegeRoboter(pRobotWorld,theta)
persistent s dTheta
if isempty(s)
    s = 0.5; % m
    dTheta = (20*rand-10); % deg
end
theta = theta+dTheta;
pRobotWorld = pRobotWorld +[s*cosd(theta);s*sind(theta)];
end
 
</source>
|}
|}
----
----
→ zurück zum Hauptartikel: [[BSE_Signalverarbeitende_Systeme|BSE Signalverarbeitende Systeme]]<br>
→ zurück zum Hauptartikel: [[BSE_Signalverarbeitende_Systeme|BSE Signalverarbeitende Systeme]]<br>
→ weiter zum Artikel: [[BSE_Signalverarbeitende_Systeme_-_SoSe25|Signalverarbeitende Systeme - SoSe25]]
→ weiter zum Artikel: [[BSE_Signalverarbeitende_Systeme_-_SoSe25|Signalverarbeitende Systeme - SoSe25]]

Aktuelle Version vom 2. Juni 2025, 11:22 Uhr

Dozent: Prof. Dr.-Ing. Schneider
Lehrveranstaltung: Signalverarbeitende Systeme
Modul Signalverarbeitende Systeme und Systems Design Engineering
Modulbezeichnung: BSE-M-2-1.06
Modulverantwortung: Prof. Ulrich Schneider
Vorlesung: Invertierter Klassenraum, Montag, 10:00 - 11:30 Uhr
Übung: Montag, 11:45 - 12:30 Uhr
Ort: Labor L3.1-E00-120

Übung 10.1: KOS-Trafo: Rotation+Translation, Inverse Transformation

Das Lösungsvideo finden Sie auf der Lernplattform. Gegeben is der Ortsvektor zum Punkt P ArP sowie der Rotationswinkel γ=50 um die Z-Achse.

  1. Führen Sie über eine homogene Translation eine Rotation um den Winkel γ und eine Translation um den Vektor T aus.
  2. Zeichnen Sie den transformierten Punkt Q und dessen Ortsvektor ArQ ein (vgl. Abb. 1).
  3. Berechnen Sie anschließend die inverse Translation ArP,2

Gegeben sind:

  • ArP=(123)
  • T(220)
  • γ=50
Abb. 1: Ergebnisdarstellung der Übung 10.1

Arbeitsergebnis: KOStrafo.m

Übung 10.2: 2-fache Koordinatentransformation

Gegeben sind:

  • ArP=(6,34110,23453): Ortsvektor zum Punkt P im KOS A
  • Br0,A=(330): Translationsvektor KOS B zum KOS A im B-KOS
  • Cr0,B=(330): Translationsvektor KOS C zum KOS B im C-KOS
  • KOS A ist gegenüber KOS B um 10° um die Z-Achse gedreht.
  • KOS B ist gegenüber KOS C um 10° um die Z-Achse gedreht.

Aufgaben:

  • Bestimmen Sie die Vektoren BrP und CrP.
  • Erstellen Sie ein Ergebnisbild gemäß Abb. 2.
Abb. 2: Ergebnisdarstellung der Übung 10.2

Arbeitsergebnis: Transformationsarithmetik.m

Übung 10.3: Transformationsarithmetik

Ein autonomes Fahrzeug erkennt mit einem hinten rechts montierten Infrarot-Sensor ein Objekt am Messpunkt M. Die Messung erfolgt im Sensorkoordinatensystem {S}. Dieses Fahrzeug besitzt ein körperfeste Koordinatensystem {K} mit dem Ursprung am Mittelpunkt des vorderen Stoßfängers. Die Position und Lage (Pose) des Fahrzeugs im Bezugskoordinatensystem {B} ist bekannt. Abb. 3 zeigt eine mögliche Situation.

Bestimmen Sie den Ortsvektor zum Objekt im Bezugskoordinatensystem BrM und zeichnen Sie mit MATLAB®die Szene (vgl. Abb. 4) in den drei Koordinatensystemen

  • Sensorkoordinatensystem {S},
  • körperfestes Koordinatensystem {K} und
  • Bezugskoordinatensystem {B} .

Hinweis: In der Fahrzeugtechnik wird die Z-Achse antiparallel zum Gravitationsvektor angenommen: 0Zg.

Abb. 3: Transformationsarithmetik
Abb. 4: Ergebnisdarstellung der Übung 10.3

Gegeben sind:

  • SrM=(0,400) m: Messwert des IR-Sensors
  • Der Winkel zwischen xS und xK beträgt 90°.
  • Br0,K=(30,20) m
  • Der Winkel zwischen xK und xB beträgt 10°.
  • Das Objekt hat eine Länge von 30 cm und eine Breite von 20 cm.
  • Das Fahrzeug hat eine Länge von 40 cm und eine Breite von 20 cm.
  • Die detektierte Objektkante ist parallel zu xB (x0||xB).
  • Der IR-Sensor erfasst die Längsseite des Objektes.

Arbeitsergebnis: TransformationsarithmetikMobileRobotik.m

Übung 10.4: Darstellung der LiDAR Messdaten

Schreiben Sie ein Skript ZeigeMessdaten, welches die gemessenen Streckendaten der Datei LiDAR.mat darstellt.

Gehen Sie in nachfolgenden Schritten vor:

  1. Laden Sie die Messdaten.
  2. Stellen Sie die Messwerte der Datei (x,y) als rote Punkte dar.
  3. Das KOS ist in Fahrtrichtung x-Positiv und die y-Achse zeigt nach links (vgl. Abb. 5).
Abb. 5: Ergebnisdarstellung der Übung 10.4

Notwendige Datei: LiDAR.mat

Nützliche Befehle: load, plot(x,y,'r.'), hold on, subplot, get, line, xlabel, ylabel

Übung 10.5: KOS-Trafo: Homogene transformationsmatrix, Inverse Transformation

Mit einem LiDAR Sensor der Firma Topcon wird die Fahrt eines Roboters vermessen.

  • Als digitale Karte liegen die Punkte des äußeren und inneren Fahrbahnrandes vor.
  • Auf der Fahrbahn steht Objekt O.
  • Der Roboter fährt geradlinig in der rechten Fahrspur vier mal je 1 m vorwärts.

Aufgaben:

  1. Zeichnen Sie die Fahrbahn, Roboterpositionen, Objekt und Startlinie im Topcon-Koordinatensystem (T-KOS, vgl. Abb. 6).
  2. Transformieren Sie Fahrbahn, Roboterpositionen, Objekt und Startlinie in das Welt-Koordinatensystem (W-KOS).
  3. Zeichnen Sie die Fahrbahn, Roboterpositionen, Objekt und Startlinie im Welt-Koordinatensystem (vgl. Abb. 7).
  4. Transformieren Sie das Objekt in das Fahrzeug-Koordinatensystem (F-KOS, vgl. Abb. 8).

Gegeben sind:

  • Tr0,W=(4,77104,54730,03) m: Ursprung des W-KOS im T-KOS
  • TrS=(4,15842,82080,03) m: Punkt der Startlinie im T-KOS
  • TrAMR=(3,91372,72930,03) m: Startpunkt des AMR im T-KOS
  • TrO=(1,86471,84910,03) m: Objektposition im T-KOS
Abb. 6: Fahrbahn im T-KOS
Abb. 7: Fahrbahn im W-KOS
Abb. 8: Objektposition im F-KOS

Notwendige Dateien:

Übung 10.6: Koordinatentransformation - Navigation eines Autonomen Mobilen Roboters (AMR)

Grader-URL: [1]


→ zurück zum Hauptartikel: BSE Signalverarbeitende Systeme
→ weiter zum Artikel: Signalverarbeitende Systeme - SoSe25