최소 제곱 맵 합류

여기에 많은 배경이 있습니다. 질문의 맨 아래로 스크롤하십시오.

SLAM이 선형 최소 제곱 문제 에서 얼마나 멀리 떨어져 있는지에 설명 된지도 결합 알고리즘을 시도하고 있습니다 . 구체적으로는, 식 (36). 내가 작성한 코드는 항상 랜드 마크 위치에 대한 두 번째 맵의 값을 취하는 것 같습니다. 내 질문은 텍스트를 올바르게 이해하고 있거나 일종의 오류를 일으키는 것입니다. 수식을 이해하고 코드가이를 구현하는 방법을 설명하면서 수식을 설명하려고합니다. 두 개의 로컬 맵에 합류하는 간단한 경우를 시도하고 있습니다.

논문 (36)에서 두 개의 로컬 맵을 결합하면 다음 을 최소화 하는 상태 벡터 을 찾는다고 말합니다 . $X_{join,rel}$

\sum_{제이 = 1}^{케이} (\hat{{엑스}_{제이}^{엘}} - H_{제이, 아르 자형 이자형 엘} ({엑스}_{제이 영형 나는 엔, 아르 자형 이자형 엘}))^{티} (피_{제이}^{엘})^{- 1} (\hat{{엑스}_{제이}^{엘}} - H_{제이, 아르 자형 이자형 엘} ({엑스}_{제이 영형 나는 엔, 아르 자형 이자형 엘}))

$\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}))$

두 개의 로컬 맵 및 대해 확장되었습니다 . $\hat{X_1^L}$ $\hat{X_2^L}$

(\hat{{엑스}_{1}^{엘}} - H_{제이, 아르 자형 이자형 엘} ({엑스}_{제이 영형 나는 엔, 아르 자형 이자형 엘}))^{티} (피_{1}^{엘})^{- 1} (\hat{{엑스}_{1}^{엘}} - H_{제이, 아르 자형 이자형 엘} ({엑스}_{제이 영형 나는 엔, 아르 자형 이자형 엘})) + (\hat{{엑스}_{2}^{엘}} - H_{제이, 아르 자형 이자형 엘} ({엑스}_{제이 영형 나는 엔, 아르 자형 이자형 엘}))^{티} (피_{2}^{엘})^{- 1} (\hat{{엑스}_{2}^{엘}} - H_{제이, 아르 자형 이자형 엘} ({엑스}_{제이 영형 나는 엔, 아르 자형 이자형 엘}))

$(\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}))$

내가 이해하는 것처럼 서브맵은 글로벌 맵에 대한 통합 관측치로 볼 수 있으므로 는 서브맵과 관련된 노이즈입니다 (서브맵을 만들 때 사용한 EKF에서 프로세스 노이즈가 아니라). 다를 수 있습니다). $P^L_j$

벡터 은 첫 번째 맵의 포즈, 두 번째 맵의 포즈 및 두 맵의 랜드 마크 합집합입니다. $X_{join,rel}$

함수 은 다음과 같습니다. $H_{j,rel}$

[\begin{matrix} {엑스}_{{아르 자형}_{제이 이자형}}^{{아르 자형}_{(제이 - 1) 이자형}} \\ ϕ_{{아르 자형}_{제이 이자형}}^{{아르 자형}_{(제이 - 1) 이자형}} \\ 아르 자형 (ϕ_{{아르 자형}_{(제이 - 1) 이자형}}^{{아르 자형}_{{미디엄}_{제이 1} 이자형}}) ({엑스}_{{에프}_{제이 1}}^{{아르 자형}_{{미디엄}_{제이 1} 이자형}} - {엑스}_{{아르 자형}_{(제이 - 1) 이자형}}^{{아르 자형}_{{미디엄}_{제이 1} 이자형}}) \\ . \\ . \\ . \\ 아르 자형 (ϕ_{{아르 자형}_{(제이 - 1) 이자형}}^{{아르 자형}_{{미디엄}_{제이 엘} 이자형}}) ({엑스}_{{에프}_{제이 엘}}^{{아르 자형}_{{미디엄}_{제이 엘} 이자형}} - {엑스}_{{아르 자형}_{(제이 - 1) 이자형}}^{{아르 자형}_{{미디엄}_{제이 엘} 이자형}}) \\ {엑스}_{{에프}_{제이 (엘 + 1)}}^{{아르 자형}_{제이 - 1 이자형}} \\ . \\ . \\ . \\ {엑스}_{{에프}_{제이 엔}}^{{아르 자형}_{제이 - 1 이자형}} \end{matrix}]

$\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}$

아래의 평가가 정확하다고 확신하지 않습니다.

처음 두 요소는 이전 맵의 참조 프레임에서 로봇의 포즈입니다. 예를 들어, 맵 1의 경우 포즈는 에서 초기 프레임에 있습니다 . 지도 2의 경우지도 1의 프레임에 있습니다. $t_0$

다음 요소 그룹은 맵 1과 맵 2에 공통 인 요소이며 맵 1의 참조 프레임으로 변환됩니다.

마지막 행은 첫 번째 맵의 프레임에서 맵 2에 고유 한 기능입니다.

내 matlab 구현은 다음과 같습니다.

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

위에서 설명한 피트니스 기능의 최소값을 찾기 위해 최적화 도구 상자 를 사용하고 있습니다. 피트니스 기능 자체는 매우 간단합니다. 함수 H는 위에서 설명한 벡터 H를 반환합니다.

결과는 다음과 같습니다. 두 벡터에서 join_maps를 실행하면

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)

출력은 다음과 같습니다.

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

질문:

내 프로그램은 맵 2가 맵 결합 기능의 최소값임을 나타냅니다. 최소값은 맵 1과 맵 2 사이에 있어야합니다. 매트릭스 H에 문제가 있다고 확신합니다.

slam

— 뭉크
소스

이것은 올바르게 작동하는 것으로 보이며 훨씬 간단한 솔루션입니다.

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

fminunc에 대한 설명과 더 잘 맞도록 출력을 변경했습니다.

map_1 및 map_2의 출력은

이 경우 처음 두 포즈가 동일하므로 두 맵이 동일한 참조 프레임을 공유하므로 H (X)를 호출 할 필요가 없습니다. 함수 H는 상태 추정값을 서브맵의 참조 프레임으로 변환합니다.

— 뭉크
소스