Hier gibt es viel Hintergrundwissen. Scrollen Sie für die Frage nach unten
Ich probiere den Kartenverbindungsalgorithmus aus, der unter Wie weit ist SLAM von einem linearen Problem der kleinsten Quadrate entfernt beschrieben ist . speziell Formel (36). Der Code, den ich geschrieben habe, scheint immer die Werte der zweiten Karte für Orientierungspunkte zu verwenden. Meine Frage ist, verstehe ich den Text richtig oder mache ich einen Fehler. Ich werde versuchen, die Formeln so zu erklären, wie ich sie verstehe, und zeigen, wie mein Code dies implementiert. Ich versuche den einfachen Fall, nur zwei lokale Karten zu verbinden.
Aus dem Papier (36) , sagt zwei Karten Beitritt ist das Finden der ein Zustandsvektor Das minimiert:
Erweitert für zwei lokale Karten und ^ X L 2 Ich habe:
So wie ich es verstehe, kann eine Submap als integrierte Beobachtung für eine globale Karte angesehen werden, sodass das mit der Submap verbundene Rauschen ist (im Gegensatz zu dem Prozessrauschen in der EKF, die ich zur Erstellung der Submap verwendet habe, die möglicherweise oder darf nicht anders sein).
Der Vektor ist die Pose aus der ersten Karte, die Pose aus der zweiten Karte und die Vereinigung der Landmarken in beiden Karten.
Die Funktion ist:
Ich bin nicht davon überzeugt, dass meine Einschätzung unten richtig ist:
Die nächste Gruppe von Elementen sind diejenigen, die Karte 1 und Karte 2 gemeinsam haben und die in den Referenzrahmen von Karte 1 transformiert werden.
Die letzten Zeilen sind die für Karte 2 einzigartigen Merkmale im Rahmen der ersten Karte.
Meine Matlab-Implementierung lautet wie folgt:
function [G, fval, output, exitflag] = join_maps(m1, m2)
x = [m2(1:3);m2];
[G,fval,exitflag,output] = fminunc(@(x) fitness(x, m1, m2), x, options);
end
function G = fitness(X, m1, m2)
m1_f = m1(6:3:end);
m2_f = m2(6:3:end);
common = intersect(m1_f, m2_f);
P = eye(size(m1, 1)) * .002;
r = X(1:2);
a = X(3);
X_join = (m1 - H(X, common));
Y_join = (m2 - H(X, common));
G = (X_join' * inv(P) * X_join) + (Y_join' * inv(P) * Y_join);
end
function H_j = H(X, com)
a0 = X(3);
H_j = zeros(size(X(4:end)));
H_j(1:3) = X(4:6);
Y = X(1:2);
len = length(X(7:end));
for i = 7:3:len
id = X(i + 2);
if find(com == id)
H_j(i:i+1) = R(a0) * (X(i:i+1) - Y);
H_j(i+2) = id;
else % new lmk
H_j(i:i+2) = X(i:i+2);
end
end
end
function A = R(a)
A = [cos(a) -sin(a);
sin(a) cos(a)];
end
Ich verwende die Optimierungs-Toolbox , um das Minimum der oben beschriebenen Fitnessfunktion zu finden. Die Fitnessfunktion selbst ist meiner Meinung nach ziemlich einfach. Die Funktion H gibt den oben beschriebenen Vektor H zurück.
Das Ergebnis ist: Wenn ich join_maps auf den beiden Vektoren ausführe
map_1 = [3.7054;1.0577;-1.9404; %robot x, y, angle
2.5305;-1.0739;81.0000]; % landmark x, y, id
map_2 = [3.7054;1.0577;-1.9404;
2.3402;-1.1463;81.0000]; % note the slightly different x,y
[G,fv,output,exitflag] = join_maps(map_1, map_2)
Die Ausgabe ist:
Warning: Gradient must be provided for trust-region algorithm;
using line-search algorithm instead.
> In fminunc at 341
In join_maps at 7
Local minimum found.
Optimization completed because the size of the gradient is less than
the default value of the function tolerance.
<stopping criteria details>
Local minimum possible.
fminunc stopped because it cannot decrease the objective function
along the current search direction.
<stopping criteria details>
G =
3.7054
1.0577
-1.9404
3.7054
1.0577
-1.9404
2.3402
-1.1463
81.0000
fv =
1.3136e+07
output =
iterations: 1
funcCount: 520
stepsize: 1.0491e-16
firstorderopt: 1.6200e+05
algorithm: 'medium-scale: Quasi-Newton line search'
message: [1x362 char]
exitflag =
5
Die Frage:
Mein Programm gibt Karte 2 ist das Minimum der Kartenverbindungsfunktion. Es scheint, dass das Minimum irgendwo zwischen Karte 1 und Karte 2 liegen sollte. Ich bin mir ziemlich sicher, dass das Problem bei der Matrix H liegt. Was mache ich falsch?