Karte der kleinsten Quadrate verbinden

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:$X_{join,rel}$

$$\sum_{j=1}^{k}(\hat{X_j^L} - H_{j,rel}(X_{join,rel}))^T(P_j^L)^{-1}(\hat{X_j^L} - H_{j,rel}(X_{join,rel}))$$

Erweitert für zwei lokale Karten und ^ X L 2 Ich habe:$\hat{X_1^L}$$\hat{X_2^L}$

$$(\hat{X_1^L} - H_{j,rel}(X_{join,rel}))^T(P_1^L)^{-1}(\hat{X_1^L} - H_{j,rel}(X_{join,rel})) + (\hat{X_2^L} - H_{j,rel}(X_{join,rel}))^T(P_2^L)^{-1}(\hat{X_2^L} - H_{j,rel}(X_{join,rel}))$$

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).$P^L_j$

Der Vektor ist die Pose aus der ersten Karte, die Pose aus der zweiten Karte und die Vereinigung der Landmarken in beiden Karten.$X_{join,rel}$

Die Funktion ist:$H_{j,rel}$

$$\begin{bmatrix} X_{r_{je}}^{r_{(j-1)e}}\\ \phi_{r_{je}}^{r_{(j-1)e}}\\ R(\phi_{r_{(j-1)e}}^{r_{m_{j1}e}}) (X^{r_{m_{j1}e}}_{f_{j1}} - X^{r_{m_{j1}e}}_{r_{(j-1)e}})\\.\\.\\.\\ R(\phi_{r_{(j-1)e}}^{r_{m_{jl}e}}) (X^{r_{m_{jl}e}}_{f_{jl}} - X^{r_{m_{jl}e}}_{r_{(j-1)e}})\\ X_{f_{j(l+1)}}^{r_{j-1e}}\\ .\\.\\.\\ X_{f_{jn}}^{r_{j-1e}} \end{bmatrix}$$

Ich bin nicht davon überzeugt, dass meine Einschätzung unten richtig ist:

$t_0$

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?

Dies scheint richtig zu funktionieren und ist eine viel einfachere Lösung:

function [X, FVAL, EXITFLAG, OUTPUT, GRAD] = join_maps(m1, m2)
    p = [m1(1:3);m2(1:3)];
    x1 = [p;m1(4:end)];
    x2 = [p;m2(4:end)];
    guess_0 = zeros(size(x1,1),1);
    q = @(x)x'*eye(length(x))*x;
    fit = @(x)q(x1-x)+q(x2-x);
    [X,FVAL,EXITFLAG,OUTPUT,GRAD] = fminunc(fit ,guess_0);
end

Ich habe die Ausgabe geändert, um sie besser an die Beschreibung für fminunc anzupassen.

Die Ausgabe mit map_1 und map_2 ist

X =
 3.7054
 1.0577
-1.9404
 3.7054
 1.0577
-1.9404
 2.4353
-1.1101
 81.0000

In diesem Fall muss H (X) nicht aufgerufen werden, da die ersten beiden Posen identisch sind und die beiden Karten denselben Referenzrahmen haben. Die Funktion H transformiert nur die Zustandsschätzung in den Referenzrahmen der Submap.