自己動手寫全套無人駕駛算法系列（四）機器人2D SLAM
目錄:
一、概述
1.1 系列整體概述
二、傳感器層
2.1 輪式里程計
2.2 IMU
2.3 激光雷達
2.4 視覺VO
三、建圖層
3.1 靜態二值貝葉斯濾波
3.2 Ray casting算法
3.3 Beam end model算法
3.4 Likelihood range finder算法
3.5 多分辨率地圖及內存管理問題
3.5.1 ROS存儲
3.5.2 稀疏存儲
3.5.3 塊存儲
3.5.4 直接點雲存儲
四、匹配算法
4.1 Brute force（含分數計算的三種方法）
4.2 Compute 2D slice
4.3 Multiple resolution match
4.4 Fast scanmatching
4.5 ICP
4.6 NDT
4.7 NDT+ICP
4.8 插值匹配
4.9 總結
五、前端融合算法
5.1 EKF
5.2 比例融合
六、後端融合算法
6.1 EKF
6.2 UKF
6.3 PF
6.4 Graph Optimizer
一、概述
1.1 系列整體概述：
本文主要介紹無人駕駛SLAM各個環節的各個算法，主要包括4個方面：傳感器層，建圖層，匹配層和融合定位層。運動模型爲差分驅動模型，目前代碼已經做完，但因爲要對其專利保護，日後再放出，特此先作介紹，並提供交流。
歡迎大家對我的github項目提出BUG，或者需要我更新新的算法,或者需要技術合作，都可以聯繫我：
163郵箱：[email protected]
gmail:[email protected]
QQ羣：710805413
二、傳感器層
2.1 輪式里程計
輪式里程計的來源主要是編碼器或者CAN傳入PC端的小車的輪子速度和加速度信息，我們通過輪子的速度積分可以得到車輛已經行走的座標信息，或者編碼器也可以直接提供小車已經行走的座標信息，同時我們也可以得到車輛的速度信息，將其可以轉換爲twist，即成了估計車輛位姿的重要來源。但是我們要注意，因爲時間的延遲，我們必須將時間對齊固定時鐘源，比如我的代碼裏對齊的是激光雷達的時間，如果輪式早到則要按運動學模型推到相應時間，同時按v及w可以得到相應寫方差矩陣
2.2 IMU
imu可以提供3個軸的旋轉信息和3個方向的加速度信息，加速度積分兩次可以得到位移，但一般考慮精度問題不這麼做，除非你的IMU是上千上萬那種可以考慮這麼做，我們主要從IMU得到旋轉速度及旋轉角度兩個數據，同時按IMU誤差模型可以得到其協方差矩陣，可見IMU誤差模型。同樣的，imu也需要時間對齊。
2.3 激光雷達
激光雷達信息往往是一個角度+一個距離，我們通過距離×cos（角度）或sin(角度)既可以把每個激光束所表示的點的座標算出來，又因爲運動的原因，比如激光雷達的時間間隔是0.2s，一共360個點，那麼第1個點就需要做0.2/360 s的運動畸變矯正，我們知道車輛的速度和角速度，即可通過運動模型矯正激光點雲，其他點也是按同樣的方法矯正。如下：
for(U4 i = 0;i<scan_t.size();i++){
yaw = scan_t[i].th;
if(yaw < 0){
yaw += 2*TT;
}
yaw_res = end_angle - yaw;
time_res = lidar_intv*(yaw_res/all_angle);
wdt = wt*time_res;
scan_t[i].x -= ( vbw*sin(wheel_th) + vbw*sin(wheel_th+wdt) );
scan_t[i].y += ( vbw*cos(wheel_th) - vbw*cos(wheel_th+wdt) );
}
而vw的獲得主要有兩種方法：
1.輪式里程計及IMU輔助
2.雷達兩楨速度不變假設。
2.4 視覺VO
本人的代碼尚未提及，對於掃地機器人等2d小型機器人，直接激光雷達方案，或者視覺VIO以MSCKF都可以，這屬於單獨的內容了，故不作提及。
三、建圖層
本人的代碼主要提供3大見圖算法，都用到了bresenhamline，其實就是電腦中畫直線的算法，具體可見bresenham算法，其過程一般是先用建圖算法建圖再用靜態二值貝葉斯濾波融合。如下：
3.1 靜態二值貝葉斯濾波
ROS中將全局地圖與局部地圖對齊後如果，全局地圖值爲-1（未探測），即把局部地圖的值直接賦過去即可，但若全局地圖有值，那麼則要進行靜態二值貝葉斯濾波：
P(x)就是原來的全局地圖對應柵格概率0-1，P(X|Zt)就是局部地圖對應柵格概率，L0就是全局爲未探測時候覆蓋的局部地圖值，之後一步步更新就可了。效果如下圖：(讓局部地圖向着右上不斷移動，並擴張和融合到全局地圖)。
3.2 Ray casting算法
對每個激光雷達點轉換到局部地圖座標系後直接使用bresenham探測，在擊中點附近半徑小於hit_r的範圍概率置爲0.95,（ROS值是95,擴大了100倍節省內存）,其他點都置爲0.01即可。每次見圖完用靜態二值貝葉斯濾波融合即可，如下：
3.3 Beam end model算法
波束模型，使用前我們要用EM算法算出激光雷達的Zhit,Zshort,Zmax,Zrand及hit的方差和均值，short分佈的lambda：如下：
以如下代碼：
void Mapper::Static_EM_Estimation_For_Z(std::vector<R8> z_m,std::vector<R8> z_r,Mapper_Param ¶m){
U4 z_size = z_m.size(); // the size of z_m and z_r must be equal
if(z_r.size() != z_size || z_size == 0){
return ;
}
R8 z_hit = param.z_hit;
R8 z_short = param.z_short;
R8 z_max = param.z_max;
R8 z_rand = param.z_rand;
R8 z_hit_abs = z_hit*z_size;
R8 z_short_abs = z_short*z_size;
R8 sigma2 = 0,lamda = 0;
for(U4 i = 0; i< z_size; i++){
sigma2 += (z_m[i] - z_r[i])*(z_m[i] - z_r[i]);
lamda += z_m[i];
}
sigma2 = sigma2/z_hit_abs;
lamda = z_short_abs/lamda;
R8 Eta = 0;
R8 pre_sigma2 = 0,pre_lamda = 0;
R8 p_hit_i,p_short_i,p_max_i,p_rand_i,b2;
R8 e_hit_i,e_short_i,e_max_i,e_rand_i;
R8 abs_z,e_hit_all,e_short_all,e_max_all,e_rand_all;
R8 e_hit_sqr_all,e_short_z_all;
p_max_i = 0;
p_rand_i = param.min_rslo/param.range_max;
U4 times = 0;
while(fabs(pre_sigma2 - sigma2) >= 1e-3 || fabs(pre_lamda - lamda) >= 1e-3){
abs_z = 0,e_hit_all = 0,e_short_all = 0,e_max_all = 0,e_rand_all = 0;
e_hit_sqr_all = 0,e_short_z_all = 0;
pre_sigma2 = sigma2;
pre_lamda = lamda;
for(U4 i = 0; i<z_size;i++){
b2 = (z_m[i] - z_r[i])*(z_m[i] - z_r[i])/(2*sigma2);
p_hit_i = exp(-b2)/(2*TT*sigma2);
p_short_i = lamda*exp(-lamda*z_m[i]);
Eta = 1/(p_hit_i + p_short_i + p_max_i + p_rand_i);
e_hit_i = Eta*p_hit_i;
e_short_i = Eta*p_short_i;
e_max_i = Eta*p_max_i;
e_rand_i = Eta*p_rand_i;
abs_z += (e_hit_i + e_short_i + e_max_i + e_rand_i);
e_hit_all += e_hit_i;
e_short_all += e_short_i;
e_rand_all += e_rand_i;
e_hit_sqr_all += (e_hit_i*(z_m[i]-z_r[i])*(z_m[i]-z_r[i]));
e_short_z_all += (e_short_i*z_m[i]);
}
z_hit = e_hit_all/(abs_z);
z_short = e_short_all/(abs_z);
z_max = 0;
z_rand = e_rand_all/(abs_z);
sigma2 = e_hit_sqr_all/e_hit_all;
lamda = e_short_all/e_short_z_all;
times ++;
if(times >= 100){
break;
}
}
param.z_hit = z_hit;
param.z_short = z_short;
param.z_max = z_max;
param.z_rand = z_rand;
param.sigma2 = sigma2;
param.lambda = lamda;
}
最後需要融合各個分佈到如下分佈：
然後在bresenham line 上根據離終點和起點的距離算出其概率填入局部地圖即可。如下：
其中數值積分按如下代碼：
R8 Mapper::Integral_From_Function(X1 mode,R8 start,R8 end,Mapper_Param ¶m){
R8 result=0,u,sigma2,lambda;
R8 interval = param.intg_intv;
if(mode != Gaussian && mode != Exp){
return 0;
}
if(mode == Gaussian){
u = param.u;
sigma2 = param.sigma2;
for(R8 i = start;i<end;){
result += 1/sqrt(2*TT*sigma2)*exp(-(i-u)*(i-u)/(2*sigma2))*interval;
i += interval;
}
}
if(mode == Exp){
lambda = param.lambda;
start = start<0? 0:start;
result = exp(-lambda*start)-exp(-lambda*end);
}
return result;
}
這裏有個小技巧，當激光雷達不是很準的時候，主要依賴圖優化去優化，這時就可以把最高概率的點設置爲Pmax比如95,其他概率按最高概率按比例放縮，這樣的圖更具有信息，不容易被靜態二值濾掉。
3.4 Likelihood range finder算法
說白了在bresenham line上每個點，以波束模型算出的那個分佈取Phit那個分佈，用kdtree搜索其離最近點的距離，代入分佈中即得到概率，得到局部地圖同樣用SBF融合即可，容易出現穿牆問題，是gmapping的建圖算法,如下：
核心是使用如下pcl結構：
pcl::PointCloud<pcl::PointXY>::Ptr cloud (new pcl::PointCloud<pcl::PointXY>);
pcl::KdTreeFLANN<pcl::PointXY> kdtree;
效果如下：
3.5 多分辨率地圖及內存管理問題
建圖的時候需要在多分辨率建圖，這時就需要考慮內存問題，少內存必然帶來的是低速度，所以必須就內存和速度進行權衡，權衡是按如下公式計算，
假設電腦內存爲M,而一個地圖點是1字節，故地圖邊長最大柵格數G_length = sqrt(M)，設地圖最小分辨率爲rslo，那麼地圖最大邊長L爲G_length*rslo, 多分辨率下，其等比數列收斂於1/4，即最大邊長爲L/(5/4),如果你內存不夠就要用磁盤，現在固態的速度300MB-2000MB/s，速度其實是夠的，如果是機械就比較麻煩。
3.5.1 ROS存儲
存儲方式可以用ROS這種，就是一維數組，x = i%width y = i/width;
3.5.2 稀疏存儲
也可以是稀疏存儲，當地圖填充率小於1/9的時候即使用，結構體如下：
class Map_Data{
I4 x;
I4 y;
X1 val;
}
3.5.3 塊存儲
比較常用，就是分割地圖爲一個個正方形塊，可以存儲到磁盤中，使用的時候按當前歸化座標索引即可，花費只是索引時間，存儲也是一樣，最多的時候只需取4塊即可，保存的時候文件的名字可以幫助你快速索引，並且排序由系統命令完成即可。
文件名如下: 5_10.ff 我用了不同文件夾放不同分辨率的地圖，所以每個文件直接以柵格座標xy命名即可。而地圖邊長這些都是在程序定義好的，不需要再讀取。
3.5.4 直接點雲存儲
把點雲經過VOXEL filter後按時間順序直接存下來，對於圖優化的算法，這種可以在圖優化迴環結束後再快速建圖，故如果你用圖優化算法，就用這種存儲是可以的，當然也可以加塊算法，結構如下：
std::vector<std::vector<Pose_2d>> scan_set;
四、匹配算法
主要有8大算法，其他衍生算法我也會提到。
4.1、Brute force
說白就是暴力匹配，就是在當前位置附近取abc個位置探索分數最高的一個，然後把位置更新過去即可，a是x窗口大小，b是y窗口大小，c是yaw的窗口大小，很耗內存，其中分數的計算方法有兩種：1.相關性測量模型，2.插值模型：
相關性測量模型如下：
即協方差的算法，很簡單。
插值模型分爲雙線性插值和雙三次插值：
雙線性插值如下：雙線性插值，這是hector slam用的插值匹配方法。本用於圖像縮放。
雙三次插值如下：雙三次插值，這是cartographer的插值匹配方法。本用於圖像縮放。
然後我們就獲得了那個分數。相關性和插值的函數實現如下：
R8 Get_Score(nav_msgs::OccupancyGrid &local_map,nav_msgs::OccupancyGrid &global_map,X1 mode);
R8 Get_Score(Pose_2d &now_pose,std::vector<Pose_2d> &scan,nav_msgs::OccupancyGrid &global_map,X1 mode);
注：gmapping和cartographer在分數上還加入了離初始點的距離作爲分數項，越近分數越高。我的代碼沿用這種做法
4.2、Compute 2D slice
說白就是相關性模型你算分數要先算出局部地圖，xy的變化可以通過變換地圖的起點座標即可，而angle那個要重新建圖，那我直接提前算好不同角度的地圖不就可以了。如下：
for(R8 a = -angle_s;a<=angle_s;a+=angle_inc){
tmp_pose.th = pred_pose.th + a;
map_maker.Get_Local_Map(tmp_pose,scan,param.map_param);
//3.move map and search
for(R8 y = -length_y/2;y <= length_y/2;y+=rslo){
for(R8 x = -length_x/2;x <= length_x/2;x+=rslo){
//待上傳後
}
}
}
4.3、Multiple resolution match
可見hector slam，這本是hector slam的算法，先在低分辨率匹配得到粗略的結果再在高分辨率匹配，結果是次優而非最優。其匹配用的是我後面要說的插值匹配。
4.4、Fast scanmatching
這是cartographer的匹配算法，cartographer主要有4種匹配算法，CSM，real time CSM，ceres scanmatching（用ceres做的插值匹配），Fast match（Branch and bound）。real time CSM那個類似compute 2D slice，主要就是branch and bound，說白了粗分辨率的分數要高於細分辨率的分數，因爲粗分辨率類似最大采樣，細分辨率上打在概率爲0上的點，粗分率上可能就是打在障礙物的柵格上，即使是在柵格邊緣。有了這個理解，我們先對當前層結點分數算出來排序，取最大的按DFS去搜索，對於葉子節點取最大的一個分數，與我們之前存儲的best score比較（初始化爲-1），如果大於他就把best node更新爲這個葉子節點，回到上一層，如果上一層的老二比這個best score還低，就不進入DFS了，直接再退一層，就按這種遞歸方式就加速了暴力匹配的速度。參考函數如下：
//2.4 branch and bound like cartographer
Tree_node best_node;
void Branch_And_Brand_DFS(Pose_2d &pred_pose, std::vector<Pose_2d> &scan,Matcher_Param ¶m,
std::vector<nav_msgs::OccupancyGrid> &multi_map);
Pose_2d Fast_CSM_Matcher(Pose_2d &pred_pose, std::vector<Pose_2d> &scan,Matcher_Param ¶m,
std::vector<nav_msgs::OccupancyGrid> &multi_map);
4.5、ICP
就是迭代最近點，這個經典算法聽的太多了，具體見SLAM十四講，我是用PCL實現，唯一要注意的就是存儲上個點的分配方式：
std::vector<pcl::PointXYZ,Eigen::aligned_allocator<pcl::PointXYZ>> last_scan;
注：實際用的時候必須加入RANSAC，不然難以得到很好的效果。
4.6、NDT
正態分佈變換，可見正態分佈變換，類似插值匹配，不過是用高斯分佈模型。常用於粗匹配與ICP結合使用。
4.7、NDT+ICP
先NDT得到一個粗略的值，再ICP，參考如下：
Pose_2d Matcher::NDT_MIX_ICP_Matcher(Pose_2d &now_pose,Pose_2d &pred_pose, std::vector<Pose_2d> &scan,Matcher_Param ¶m){
if(first_match){
last_scan = Scan_To_Point(scan);
return pred_pose;
} else {
Pose_2d tmp_pose = NDT_Matcher(now_pose,pred_pose,scan,param);
return ICP_Matcher(now_pose,tmp_pose,scan,param);
}
}
4.8、插值匹配
主要有兩種算法：1.線性插值匹配（hector slam） 2.雙三次插值匹配（cartographer）。
其形式都類似hectorslam。
其中雙三次插值的插值函數和微分函數如下：
R8 Matcher::BiCubic(R8 x,R8 a){
R8 abs_x = fabs(x);
R8 x2 = abs_x*abs_x;
R8 x3 = x2*abs_x;
if(abs_x <= 1){
return (a+2)*x3 - (a+3)*x2 + 1;
}else if(abs_x < 2 && abs_x >1){
return a*x3 - 5*a*x2 + 8*a*abs_x - 4*a;
}else{
return 0;
}
}
R8 Matcher::D_BiCubic(R8 x,R8 a){
R8 x2 = x*x;
if(x > -2 && x < -1){
return -3*a*x2 - 10*a*x - 8*a;
} else if(x >= -1 && x < 0){
return -3*(a+2)*x2 - 2*(a+3)*x;
} else if(x >= 0 && x < 1){
return 3*(a+2)*x2 - 2*(a+3)*x;
} else if(x > 1 && x < 2){
return 3*a*x2 - 10*a*x + 8*a;
}
return 0;
}
帶入其公式eigen即可，而cartographer用ceres完成了這個步驟。損失函數即使插值之平方和，自動求導，具體見cartographer源碼。
4.9、總結
這就是匹配的算法，匹配算法至關重要，圖優化中匹配如果太差也做不起來，故值得斟酌。完整的匹配過程如下：
即重合了。
五、前端融合算法
主要用於IMU和輪子前端融合，也可以不在前端，直接緊耦合或者在後端融合
5.1、EKF
屬於高斯濾波。
之前寫過不作過多論述，主要代碼實現就是J的寫法：
JF << 1.0,1.0,-dt*v*sin(yaw),dt*cos(yaw),
0.0,1.0,dt*v*cos(yaw) ,dt*sin(yaw),
0.0,0.0,1.0 ,0.0 ,
0.0,0.0,0.0 ,1.0 ;
5.2、比例融合
這個沒啥難的，就是yaw按輪子和imu給的那個按比列分配即可。
六、後端融合算法
主要有4種，本還想加入SEIF，IF主要的優勢是不知道權重Omega給0即可，而KF協方差要給無限大，不過現在主要圖優化時代，SEIF實際用的少。其實這個我之前就寫過了，見我之前寫的，寫過我就不說了。
6.1、EKF
就上面講過，關鍵是imu，輪子，匹配的誤差模型，imu誤差模型 ，輪子誤差模型就是av×v + b×W×W，這個在概率機器人中說的很明白了。
激光雷達誤差模型一是概率機器人波束模型那樣算，二是直接按分數，歸一化後×length即可。
6.2、UKF
這個我之前也寫了，就取2n+1個點變換過去，再重新計算均值和方差即可，可見我之前寫的UKF。現實情況如果誤差模型建的好， 其誤差殘項十分符合高斯分佈，那麼UKF好於EKF，否則則不一定
6.3、PF
這個我也寫過，particle filter，粒子濾波，gmapping用的是改進版粒子濾波，其在預測粒子生成後還進行了前後左右，左轉右轉6次的CSM分數探測，取了分數最大的那個，這樣預測粒子分佈就好了很多，不過嚴重依賴里程計，說白了就是6次探測跟不不夠，有時候誤差高達10個柵格長度，那就沒戲了要麼加大粒子數，不過因爲gmapping一個粒子維持一個地圖，其內存和計算量都會變得十分大，這也導致了gmapping在數據集處理旋轉很好，但是實際運用過程中還是受不了不停旋轉，比如intel數據集下表現很好，但是你拿到實際場景就不行，效果甚至不如ICP，（說ICP過時了的可以看ICP+RANSAC+圖優化論文，scan to map是擔心scan to scan的累計誤差，但加入圖優化就得到很好的解決）最後gmapping 用Neff統計粒子分散度，如果過於分散就重採樣，但是重採樣是個敗筆，數據集中可以，比如MIT那個數據集，但是實際過程中，重採樣往往整個位姿就壞了。
然後在實現過程中，按權重採樣可以按如下實現,使用 random庫：
//之前是 W += count W，所以每個粒子佔有一個區間，隨機出來粒子看在哪個區間就取哪個粒子
std::default_random_engine e;
e.seed(time(NULL));
std::uniform_int_distribution<unsigned> nw(0, W);
for(U4 i = 0;i<param.x_set_size;i++){
I4 wp = nw(e);
R8 low_b = 0;
for(U4 j = 0;j<x_pred_set.size();j++){
if(wp<x_pred_set[j].Wc && wp >= low_b){
x_set.push_back(x_pred_set[j]);
break;
}
low_b += x_pred_set[j].Wc;
}
}
6.4、Graph Optimizer
圖優化算法是cartographer的主要核心，見圖優化，主要就是把誤差項填入邊，然後按SPA求解稀疏矩陣即可，求解的時候按如下：
Xx = Hx.colPivHouseholderQr().solve(bx);
也可用LU分解，或者Cholesky，LLT都可以，普通的邊就是當前位姿與最近距離小於dis且索引小於n的點，而回環邊就是那些不是最近索引點的距離與當前匹配位姿小於dis的點，迴環優化後地圖要重新生成，並把迴環邊去掉，如下：
void Fusion::Reconstruct_Map(Mapper &mapper,Mapper_Param &mapper_param,Fusion_Param & fusion_param){
mapper.multi_map.clear();
mapper.Init_Map(node_set[0],scan_set[0],mapper_param);
for(I4 i = 1;i<node_set.size();i++){
mapper.Get_Local_Map(node_set[i],scan_set[i],mapper_param);
mapper.Mix_Local_Map_With_SBF(mapper_param);
}
for(I4 i = 0;i<extra_edge;i++){
edge_set.pop_back();
}
}
而優化可見lsslamPDF，激光slam和視覺不同的在於，激光slam不需要用DBow3或者深度學習迴環，但不代表不可以，你用這個添加環也是可以的，cartographer中用ceres優化自動求導，當然如果你懂雅可比矩陣求導的話，爲了效率建議用Eigen。有些人覺得圖優化要優化那麼多速度肯定很慢，其實不然，如果是加入landmark的graph確實慢，但現在是Pose graph只有node，所以速度不比gmapping慢，內存也沒gmapping消耗高。其保存形式如下：
class Pose_Edge{
public:
Pose_Edge();
~Pose_Edge();
U4 x_i;
U4 x_j;
Pose_2d edge;
Pose_2d omega;
};
std::vector<Pose_Edge> edge_set;
std::vector<Pose_2d> node_set;
std::vector<std::vector<Pose_2d>> scan_set;
故其實消耗內存也很少,最終效果如下：
