[SLAM]2D激光掃描匹配方法

1.Beam Model

 Beam Model我將它叫作測量光束模型。我的理解，它是一種徹底的物理模型，只針對激光發出的測量光束建模。將一次測量偏差分解爲四個偏差。

$ph_{hit}$，測量自己產生的偏差，符合高斯分佈。

$ph_{xx}$，因爲存在運動物體產生的偏差。

...

2.Likehood field

似然場模型，和測量光束模型相比，考慮了地圖的因素。再也不是對激光的掃描線物理建模，而是考慮測量到的物體的因素。

似然比模型自己是一個傳感器觀測模型，之因此能夠實現掃描匹配，是經過劃分柵格，步進的方式求的最大的Score，將此做爲最佳的位姿。

        for k=1:size(zt,1)
            if zt(k,2)>0
                d = -grid_dim/2;
            else
                d = grid_dim/2;
            end
            phi = pi_to_pi(zt(k,2) + x(3));
            if zt(k,1) ~= Z_max
                ppx = [x(1),x(1) + zt(k,1)*cos(phi) + d];
                ppy = [x(2),x(2) + zt(k,1)*sin(phi) + d];
                end_points = [end_points;ppx(2),ppy(2)];
                
                wm = likelihood_field_range_finder_model(X(j,:)',xsensor,...
                           zt(k,:)',nearest_wall, grid_dim, std_hit,Z_weights,Z_max);
                W(j) = W(j) * wm;
            else
                dist = Z_max + std_hit*randn(1);
                ppx = [x(1),x(1) + dist*cos(phi) + d];
                ppy = [x(2),x(2) + dist*sin(phi) + d];
                missed_points = [missed_points;ppx(2),ppy(2)];                
            end
            set(handle_sensor_ray(k),'XData', ppx, 'YData', ppy)
        end

　　

function q = likelihood_field_range_finder_model(X,x_sensor,zt,N,dim,std_hit,Zw,z_max)
% retorna probabilidad de medida range finder :)
% X col, zt col, xsen col
[n,m] = size(N);

% Robot global position and orientation
theta = X(3);

% Beam global angle
theta_sen = zt(2);
phi = pi_to_pi(theta + theta_sen);

%Tranf matrix in case sensor has relative position respecto to robot's CG
rotS = [cos(theta),-sin(theta);sin(theta),cos(theta)];

% Prob. distros parameters
sigmaR = std_hit;
zhit  = Zw(1);
zrand = Zw(2);
zmax  = Zw(3);

% Actual algo
q = 1;
if zt(1) ~= z_max
    % get global pos of end point of measument
    xz = X(1:2) + rotS*x_sensor + zt(1)*[cos(phi);
                                         sin(phi)];
    xi = floor(xz(1)/dim) + 1;
    yi = floor(xz(2)/dim) + 1;
    
    % if end point doesn't lay inside map: unknown
    if xi<1 || xi>n || yi<1 || yi>m
        q = 1.0/z_max; % all measurements equally likely, uniform in range [0-zmax]
        return
    end
    
    dist2 = N(xi,yi);
    gd = gauss_1D(0,sigmaR,dist2);
    q = zhit*gd + zrand/zmax;
end

end

　　

3.Correlation based sensor models相關分析模型

XX提出了一種用相關函數表達馬爾科夫過程的掃描匹配方法。

 

互相關方法Cross-Correlation，另外相關分析在進行匹配時也能夠應用，好比對角度直方圖進行互相關分析，計算變換矩陣。

參考文獻：A Map Based On Laser scans without geometric interpretation

circular Cross-Correlation的Matlab實現

 1 % Computes the circular cross-correlation between two sequences
 2 %
 3 % a,b             the two sequences
 4 % normalize       if true, normalize in [0,1]
 5 %
 6 function c = circularCrossCorrelation(a,b,normalize)
 7 
 8 for k=1:length(a)
 9     c(k)=a*b';
10     b=[b(end),b(1:end-1)]; % circular shift
11 end
12 
13 if normalize
14     minimum = min(c);
15     maximum = max(c);
16     c = (c - minimum) / (maximum-minimum);
17 end

 

4.MCL

蒙特卡洛方法

5.AngleHistogram

角度直方圖

6.ICP/PLICP/MBICP/IDL

屬於ICP系列，經典ICP方法，點到線距離ICP，

7.NDT

正態分佈變換

8.pIC

結合機率的方法

9.線特徵

目前應用線段進行匹配的試驗始終不理想：由於線對應容易產生錯誤，並且累積偏差彷佛也很明顯！