[發明專利]一種基于2D激光雷達測距的折線提取極大似然估計的方法

有效

• 申請號: 201910368726.3
• 申請日: 2019-05-05
• 公開（公告）號: CN110109134B
• 公開（公告）日: 2022-11-25
• 發明（設計）人: 匡兵;田春月;陳鳳冉;孫毛毛;曾憲鋒
• 申請（專利權）人: 桂林電子科技大學
• 主分類號: G01S17/08
• 分類號: G01S17/08;G01S17/88;G01S7/48
• 代理公司: 桂林市華杰專利商標事務所有限責任公司 45112
• 代理人: 楊雪梅

1.一種基于2D激光雷達測距的折線提取極大似然估計的方法，其特征在于，包括如下步驟：

1)在ROS平臺中建立工作區間，在工作區間內安裝robortsence3D激光雷達驅動并與激光雷達的wifi連接，在避免強光照射下，使用3D激光雷達對周圍環境進行掃描，進行3D激光雷達數據采集，得到原始的3D點云數據包，并儲存在rosbag中，并建立三維點云地圖；

2)對步驟1)得到的原始3D點云數據進行數據轉換，轉換成二維激光數據，從而便于環境地圖的特征提取和優化；

3)根據獲得的二維激光數據，建立基于機器人的運動模型，通過平面上機器人的坐標和航向角推算從而獲得點云數據的相對位姿信息，在任意t時刻和激光雷達的觀測模型相結合，完成機器人定位，然后通過最小二乘法的優化算法建立靜態SLAM下的環境初始地圖得到二維點云地圖；

4)在環境初始地圖的基礎上，對二維激光掃描點進行最大似然估計，同時通過折線提取和折線優化兩部分，尋找掃描測量可能性最大化的一組多段線，得到極大似然估計下的概率最大的環境地圖；

步驟4)中，所述的概率最大的環境地圖，具體是：首先連接環境初始地圖邊緣的相鄰數據點集，經選定合適的閾值匹配后，根據其坐標數據確定一組初始化多邊形，之后迭代刪除在測量概率方面產生最小誤差的頂點直到達到最大似然估計中給定的閾值，從而得到一組頂點位置掃描端點已知的位置折線，根據已提取折線建立相應的條件概率模型，通過求解出非線性方程組的近似解，得到使掃描概率最大化的幾何多邊形位置，從而得到極大似然估計下的概率最大的環境地圖；

步驟4)中，所述的折線提取和折線優化，具體是通過連接所有掃描端點構造初始化多邊形結構，然后在初始化多邊形的基礎上迭代地移除在測量概率方面引起最小誤差的頂點，計算點云到多邊形邊的距離，從而提出無用的點云數據，若得到的距離均小于設定的閾值d_max，則得到初始多邊形頂點的位置與掃描端點的子集的位置一致的一組折線，其中：

P(z|L)＝N(r(z)，r(z，l)，∑) (1)

公式(1)中，在一組多段線中，P(z|L)為單射線激光的半徑分布模型，其中定義單線激光的測量為模型z＝{x,y}，它兩個雙元素的列向量組成，分別為射線的起始點x和終點y，確定多邊形段線的集合L＝{z_i}，其中z為多邊形的頂點，相對于多段線l的坐標系指定，在掃描面建立線的概率服從高斯分布，r為測量射線的半徑，∑為徑向噪聲；

公式(1)中，測量的射線半徑為r＝||y-x||，推廣到整個掃描的測量概率為：

通過建立的條件概率模型，求得到的概率最大化；

設L^*為多邊形的最大頂點數J_max，用于折線提取，從而將問題轉換成已知多邊形線段集合L，求解最大頂點個數的問題，如公式(3)所示，通過迭代方法找出合適的線段L^*的集合，此時頂點可達到最多的J_max，從而確定幾何多邊形的范圍，L^*的表達式如下：

使用最大似然估計移除頂點的過程如下：

公式(4)中，p(z|L\m_j)代表條件測量的概率密度函數，目的是建圖時在一組多邊形線L上移除一組不需要的頂點m_j，c^*為給定地圖的掃描測量概率的最小值，函數d(z,L)表示了射線端點之間的距離，通過對射線端點之間的距離與射線地圖的交點進行比較分析，求得射線端點與d(z_k,L\m_j)移除頂點m_j之后的距離差值，從而得到誤差變量e_j的測量誤差概率，找到擁有最小概率的幾何環境地圖。

2.根據權利要求1所述的一種基于2D激光雷達測距的折線提取極大似然估計的方法，其特征在于，步驟1)中，所述的3D激光雷達數據采集，是在Ros框架中調用robortsence3D激光雷達的點云數據包，為環境初始地圖的建立做準備。