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

合集 - SLAM(55)

1.个人资源索引2018-03-032.SLAM学习笔记（1）基本概念2015-08-063.SLAM学习笔记（2）SLAM算法2015-08-214.[ROS]2 尝试编译OrbSLAM2016-01-185.[ROS]1 小乌龟2016-01-186.SLAM最近的工作2015-11-057.SLAM学习笔记（3）相关概念2015-09-168.[SLAM]2D激光线特征提取2016-07-169.[SLAM] GMapping SLAM源码阅读（草稿）2016-07-0110.[QGLViewer]First Demo2016-06-2911.SLAM数据集2016-06-23

12.[SLAM]2D激光扫描匹配方法2016-06-15

13.[g2o]一个备忘2016-04-2114.[ROS] slam_gmapping2016-01-2415.[ROS]3 Linux编程练习2016-01-2016.Linux学习和ROS安装（1）2017-12-1917.小豆包的学习之旅：机器人定位2017-05-0218.小豆包的学习之旅：传感器观测模型2017-04-2119.Kinect2.0相机标定2017-04-1020.小豆包的学习之旅：里程计运动模型2017-04-0321.小豆包的学习之旅：入门篇2017-04-0222.Kinect2.0点云数据获取2017-03-3023.[硬件]Robot运动控制2017-03-2224.[硬件]Urg_viewer数据读取2017-03-1425.[硬件]三维点云数据获取2017-03-0426.Kinect2.0获取数据2017-02-1527.ndt histogram_direction2016-09-0828.rplidar & hector slam without odometry2016-08-2629.rplidar测试2016-08-1230.[原创]NDT方法在SLAM中的应用2016-08-0831.[SLAM]Karto SLAM算法学习（草稿）2016-07-2232.ROS学习备忘2018-07-1833.[ROS]激光驱动安装2018-07-1634.OrbSLAM2采集点云数据2018-03-2635.Cartographer源码阅读(8)：imu_tracker2018-03-2136.[硬件]点云数据采集22018-03-1937.Cartographer源码阅读(9)：图优化的前端——闭环检测2018-03-0838.Cartographer源码阅读(7)：轨迹推算和位姿推算的原理2018-03-0839.Ethzasl MSF源码阅读(3)：MSF_Core和PoseMeasurement2018-03-0740.Ethzasl MSF源码阅读(2)：百川汇海2018-03-0641.Ethzasl MSF源码阅读(1)：程序入口和主题订阅2018-03-0442.Cartographer源码阅读(6)：LocalTrajectoryBuilder和PoseExtrapolator2018-02-2543.Cartographer源码阅读(5)：PoseGraph位姿图2018-02-2444.Cartographer源码阅读(4)：Node和MapBuilder对象22018-01-1045.Cartographer源码阅读(3)：程序逻辑结构2017-12-2946.Cartographer源码阅读(2)：Node和MapBuilder对象2017-12-2847.Cartographer源码阅读(1)：程序入口2017-12-2848.实时Cartographer测试(1) - rplidar2017-12-2149.Cartographer安装2017-12-2050.ROS安装（2）2017-12-2051.[ROS]一些传感器数据读取融合问题的思考2017-05-0652.小豆包的学习之旅：占用概率栅格地图和cost-map2017-05-0253.小豆包的学习之旅：开发记录2017-04-2954.[概述]移动机器人自主探索2017-04-1855.MRPT编译2017-04-14

收起

1.Beam Model

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

$ph_{hit}$，测量本身产生的误差，符合高斯分布。

$ph_{xx}$，由于存在运动物体产生的误差。

...

2.Likehood field

似然场模型，和测量光束模型相比，考虑了地图的因素。不再是对激光的扫描线物理建模，而是考虑测量到的物体的因素。

似然比模型本身是一个传感器观测模型，之所以可以实现扫描匹配，是通过划分栅格，步进的方式求的最大的Score，将此作为最佳的位姿。

?

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23

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

　　

?

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42

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实现

% Computes the circular cross-correlation between two sequences
%
% a,b             the two sequences
% normalize       if true, normalize in [0,1]
%
function c = circularCrossCorrelation(a,b,normalize)

for k=1:length(a)
    c(k)=a*b';
    b=[b(end),b(1:end-1)]; % circular shift
end

if normalize
    minimum = min(c);
    maximum = max(c);
    c = (c - minimum) / (maximum-minimum);
end

 

4.MCL

蒙特卡洛方法

5.AngleHistogram

角度直方图

6.ICP/PLICP/MBICP/IDL

属于ICP系列，经典ICP方法，点到线距离ICP，

7.NDT

正态分布变换

8.pIC

结合概率的方法

9.线特征

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