Signalverarbeitende Systeme - L10: Koordinatentransformation

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 ${}^A{\overrightarrow{r}}_P$ sowie der Rotationswinkel $\gamma =50^{\circ }$ um die Z-Achse.

1. Führen Sie über eine homogene Translation eine Rotation um den Winkel $\gamma$ und eine Translation um den Vektor $\overrightarrow{T}$ aus.
2. Zeichnen Sie den transformierten Punkt Q und dessen Ortsvektor ${}^A{\overrightarrow{r}}_Q$ ein (vgl. Abb. 1).
3. Berechnen Sie anschließend die inverse Translation ${}^A{\overrightarrow{r}}_{P,2}$

Gegeben sind:

• ${}^A{\overrightarrow{r}}_P=\left(\begin{array}{c}1\\ 2\\ 3\end{array}\right)$
• $\overrightarrow{T}\left(\begin{array}{c}2\\ 2\\ 0\end{array}\right)$
• $\gamma =50^{\circ }$

Abb. 1: Ergebnisdarstellung der Übung 10.1

Arbeitsergebnis: KOStrafo.m

Musterlösung 10.1 

%% Gegeben
A_r_P = [1;2;3;1]; % Ortsvektor zum Punkt P in KOS A

% Translation
T = [2;2;0]'; % Translationsvektor
S = 1;        % Skalierungsfaktor
P = [0 0 0];  % Perspektiventrafo
Gamma = 50;   % Rotationswinkel im deg
R = [cosd(Gamma) -sind(Gamma) 0;sind(Gamma) cosd(Gamma) 0; 0 0 1];   % Rotationsmatrix

D = zeros(4,4); % Homogene Transformation
D(1:3,1:3)= R;
D(1:3, 4) = T;
D(4,4)    = S;
D(4,1:3)  = P

A_r_Q = D * A_r_P % KOS-Trafo

%% Inverse Transformation
A_r_P2 = inv(D)*A_r_Q

%% Ergebnisdarstellung
figure
hold on
quiver(0,0,1,0,'LineWidth',2,'Color','black','AutoScale','off'); % x-Achse
text(1.1,0,'X')
quiver(0,0,0,1,'LineWidth',2,'Color','black','AutoScale','off'); % y-Achse
text(0,1.1,'Y')
h1 = quiver(0,0,A_r_P(1),A_r_P(2),'LineWidth',2,'Color','r','AutoScale','off'); % Ursprungsvektor
plot(A_r_P(1),A_r_P(2),'ro')
text(A_r_P(1)+0.1,A_r_P(2),'P')
h2 = quiver(0,0,A_r_Q(1),A_r_Q(2),'LineWidth',2,'Color','b','AutoScale','off'); % Vektor nach Trafo
text(A_r_Q(1)+0.1,A_r_Q(2),'Q')
plot(A_r_Q(1),A_r_Q(2),'bo')
h3 = quiver(A_r_P(1),A_r_P(2),T(1),T(2),'LineWidth',2,'Color','y','AutoScale','off'); % Translationsvektor
axis equal
legend([h1,h2,h3],{'$^A\vec{r}_P$','$^A\vec{r}_Q$','$\vec{T}$'},'Interpreter','latex','Location','east')
text(-0.3,0,'\{A\}')
ylim([-0.3 4.5])

Übung 10.2: 2-fache Koordinatentransformation

Gegeben sind:

• ${}^A{\overrightarrow{r}}_P=\left(\begin{array}{c}-6,3411\\ -0,2345\\ -3\end{array}\right)$: Ortsvektor zum Punkt P im KOS A
• ${}^B{\overrightarrow{r}}_{0,A}=\left(\begin{array}{c}3\\ 3\\ 0\end{array}\right)$: Translationsvektor KOS B zum KOS A im B-KOS
• ${}^C{\overrightarrow{r}}_{0,B}=\left(\begin{array}{c}3\\ 3\\ 0\end{array}\right)$: 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 ${}^B{\overrightarrow{r}}_P$ und ${}^C{\overrightarrow{r}}_P$.
• Erstellen Sie ein Ergebnisbild gemäß Abb. 2.

Abb. 2: Ergebnisdarstellung der Übung 10.2

Arbeitsergebnis: Transformationsarithmetik.m

Musterlösung 10.2 

%% 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')

Ü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 ${}^B{\overrightarrow{r}}_M$ 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: ${}^0\overrightarrow{Z}\uparrow \downarrow \overrightarrow{g}$.

Abb. 3: Transformationsarithmetik

Abb. 4: Ergebnisdarstellung der Übung 10.3

Gegeben sind:

• ${}^S{\overrightarrow{r}}_M=\left(\begin{array}{c}0,4\\ 0\\ 0\end{array}\right)$ m: Messwert des IR-Sensors
• Der Winkel zwischen ${\overrightarrow{x}}_S$ und ${\overrightarrow{x}}_K$ beträgt 90°.
• ${}^B{\overrightarrow{r}}_{0,K}=\left(\begin{array}{c}3\\ -0,2\\ 0\end{array}\right)$ m
• Der Winkel zwischen ${\overrightarrow{x}}_K$ und ${\overrightarrow{x}}_B$ 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 ${\overrightarrow{x}}_B$ (${\overrightarrow{x}}_0||{\overrightarrow{x}}_B$).
• Der IR-Sensor erfasst die Längsseite des Objektes.

Arbeitsergebnis: TransformationsarithmetikMobileRobotik.m

Musterlösung 10.3  Das Lösungsvideo finden Sie auf der Lernplattform.

Ü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

Musterlösung 10.4  Die Lösung wird in der Übung besprochen.

Ü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:

• ${}^T{\overrightarrow{r}}_{0,W}=\left(\begin{array}{c}-4,7710\\ 4,5473\\ 0,03\end{array}\right)$ m: Ursprung des W-KOS im T-KOS
• ${}^T{\overrightarrow{r}}_S=\left(\begin{array}{c}-4,1584\\ 2,8208\\ 0,03\end{array}\right)$ m: Punkt der Startlinie im T-KOS
• ${}^T{\overrightarrow{r}}_{AMR}=\left(\begin{array}{c}-3,9137\\ 2,7293\\ 0,03\end{array}\right)$ m: Startpunkt des AMR im T-KOS
• ${}^T{\overrightarrow{r}}_O=\left(\begin{array}{c}-1,8647\\ -1,8491\\ 0,03\end{array}\right)$ 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:

• AussenLinie.mat
• InnenLinie.mat

Vorlage 10.5 

%% Fahrbahnmarkierungen
load('InnenLinie.mat')
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

Musterlösung 10.5 

%% 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

---

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