2D 激光雷达扫描匹配的方法

1.Beam Model

 Beam Model我将它叫做测量光束模型。个人理解，它是一种完全的物理模型，只针对激光发出的测量光束建模。将一次测量误差分解为四个误差。

phhit，测量本身产生的误差，符合高斯分布。

phxx，由于存在运动物体产生的误差。

...

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.线特征

目前应用线段进行匹配的试验始终不理想：因为线对应容易产生错误，而且累积误差似乎也很明显！