專利解讀： 一種深度相機與單線激光雷達融合的移動機器人避障方法
發明人：朱威 巫浩奇 洪力棟 韓慧 陳偉鋒 何德峰 浙江工業大學
該方法包括以下步驟：
1.對深度相機和單線激光雷達進行聯合標定
2.獲取同一幀深度影像和同一時刻單線激光雷達數據）
3.深度影像預處理及裁剪
4.判斷預處理後的深度影像是否需要與激光雷達進行數據融合
5.使用 貝葉斯估計法 進行柵格地圖融合
6.使用局部避障演算法進行避障
流程圖：
發明特點：彌補了使用單個深度相機只能獲得前方視野範圍內的區域而不能獲得後面和左右兩側的環境數據的缺陷，並且透過對深度影像進行預處理及裁剪，大幅減少了需要計算的深度影像數據量，有效增快了避障的數據處理速度。
詳細說明：
（1）所述步驟1中，以棋盤標定法對深度進行標定，得到深度相機的內參矩陣和外參矩陣，按照式a和式b將深度相機座標系轉換到移動機器人本體座標系：
其中，u、v分別為深度影像中各個橡塑點的行列號，cx、cy分別為深度影像中心座標，d表示深度影像中各個像素點的深度距離，fx、fy分別為深度相機水平、垂直方向的焦距，R為旋轉矩陣，T為轉換向量；xc、yc、zc分別為深度相機的三軸座標；
PS：移動機器人本體座標與單線激光雷達的座標系一致。
（2）步驟3中，對深度圖形的預處理包括：
1.將步驟2中獲得的深度影像使用雙邊濾波演算法進行處理；
2.設經過1濾波處理後的深度影像的有效像素區域為水平像素座標在[cx‑width ,cx+width]、豎直像素座標在[cy‑height ,cy+height]的矩形區域，其中cx、cy分別為深度影像中心座標，width為有效像素區域到中心座標的最大水平像素距離，height為有效像素區域到中心座標的最大豎直像素距離；
3.基於有效像素區域和預設的像素深度值範圍，對經過1濾波處理後的深度影像進行裁切，輸出裁剪後的深度影像。
4.上述3中所預設的像素深度值範圍為[10,120]
（3）步驟4中包括以下步驟：
1.將預處理後的深度影像透過深度相機內參矩陣和外參矩陣轉換成點雲數據，將點雲數據透過步驟1的聯合標定的方式轉換至機器人本體座標系中，得到座標系轉換後的點雲數據P；
2.在三維空間下，對點雲數據P使用半徑濾波器進行處理，設定點雲鄰域半徑r，對於P中的每個點雲點，統計以其為中心、半徑為r的球體內的球體內的空間相鄰點雲點的數量Num1；
若Num1≤鄰近點雲點數量閾值THnum1，則將當前點雲點消除，否則保留該點雲點，得到濾除異常空間雜訊點雲點的點雲數據M；
3.統計點雲數據M中的點雲點個數Num2，若Num2大於等於總點雲數閾值THnum2，說明深度相機檢測到障礙物，則進行步驟5，否則不進行數據融合，直接使用單線激光雷達數據生成柵格地圖並進行步驟6。
（4）r∈[0.5,5]，單位厘米；THnum1∈[2，6]；THnum2∈[2，6]
（5）步驟5包括以下：
1.將步驟4中的3生成的點雲數據M中每個點雲點都投影到移動機器人座標系平面上形成二維的偽激光數據，然後再將偽激光點數據生成的柵格地圖與單線激光雷達獲得的激光數據所生成的柵格地圖，使用經典的貝葉斯遞推公式來進行融合；
2.設定占據概率閾值，用於確定柵格地圖是否被占據；
3.以深度相機的光心為起點，掃描所有角度，以光心和掃描到的障礙物間的柵格為 \bar{O} ，表示觀測到柵格未被占據，此區域可行，以0表示觀測到柵格被占據；
4.以 E 表示障礙物真實存在， \bar{E} 表示障礙物不存在，基於式（3）和（4），得到後驗概率：
其中，P(E)表示先驗概率，P(O|E)與P(O| \bar{E} )表示傳感器的測量模型，並且
5.當遞推公式被轉換為占據柵格結構圖時，如式（5）為改進的融合公式：
其中， P_{m}^{o} 和 (1-P_{m}^{o}) 分別代表先驗地圖的柵格占據和未被占據單元的先驗概率， P^{o} 表示柵格單元條件概率， P_{S}^{o} 代表傳感器透過觀測確認柵格是否被占據的條件概率；
6.透過柵格被占據的概率大小來確定柵格地圖的生成，被占據的概率越大，生成柵格地圖的可能性越大，設定概率閾值後，若計算出的概率大於設定的閾值則可生成柵格地圖，小於設定的概率閾值則不生成。相機生成的柵格地圖與雷達生成的柵格地圖相融合後，新生成的柵格概率補充占據原有的柵格單元的概率。
（6）基於步驟（5）得到的柵格，定義單線激光雷達距離障礙物之間的距離為Distance，單位為米；定義移動機器人與深度相機鏡頭朝向所在的局部立體空間內障礙物之間的距離為3D_Distance，單位為米；定義移動機器人能夠安全移動的情況下周圍障礙物距離機器人本體最小距離為Min_Distance；
（7）根據障礙物與移動機器人之間的平面距離Distance和局部立
體空間內障礙物之間的距離3D_Distance將融合後的區域分為：
安全區域：機器人前方無障礙或是障礙物不影響移動機器人正常行走；
障礙區域：機器人前方有障礙或障礙物影響移動機器人正常行走；
碰撞區域：機器人前方有障礙且障礙物影響移動機器人正常行走。
