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 sowie der Rotationswinkel um die Z-Achse.
- Führen Sie über eine homogene Translation eine Rotation um den Winkel und eine Translation um den Vektor aus.
- Zeichnen Sie den transformierten Punkt Q und dessen Ortsvektor ein (vgl. Abb. 1).
- Berechnen Sie anschließend die inverse Translation
Gegeben sind:

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:
- : Ortsvektor zum Punkt P im KOS A
- : Translationsvektor KOS B zum KOS A im B-KOS
- : 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 und .
- Erstellen Sie ein Ergebnisbild gemäß Abb. 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 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: .


Gegeben sind:
- m: Messwert des IR-Sensors
- Der Winkel zwischen und beträgt 90°.
- m
- Der Winkel zwischen und 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 ().
- 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:
- Laden Sie die Messdaten.
- Stellen Sie die Messwerte der Datei (x,y) als rote Punkte dar.
- Das KOS ist in Fahrtrichtung x-Positiv und die y-Achse zeigt nach links (vgl. Abb. 5).

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:
- Zeichnen Sie die Fahrbahn, Roboterpositionen, Objekt und Startlinie im Topcon-Koordinatensystem (T-KOS, vgl. Abb. 6).
- Transformieren Sie Fahrbahn, Roboterpositionen, Objekt und Startlinie in das Welt-Koordinatensystem (W-KOS).
- Zeichnen Sie die Fahrbahn, Roboterpositionen, Objekt und Startlinie im Welt-Koordinatensystem (vgl. Abb. 7).
- Transformieren Sie das Objekt in das Fahrzeug-Koordinatensystem (F-KOS, vgl. Abb. 8).
Gegeben sind:
- m: Ursprung des W-KOS im T-KOS
- m: Punkt der Startlinie im T-KOS
- m: Startpunkt des AMR im T-KOS
- m: Objektposition im T-KOS



Notwendige Dateien:
| 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
|
Grader-URL: [1]
| Musterlösung 10.6 |
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
|
→ zurück zum Hauptartikel: BSE Signalverarbeitende Systeme
→ weiter zum Artikel: Signalverarbeitende Systeme - SoSe25