Signalverarbeitende Systeme - L10: Koordinatentransformation: Unterschied zwischen den Versionen

Aus HSHL Mechatronik
Zur Navigation springen Zur Suche springen
 
(2 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 342: Zeile 424:
</source>
</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