[發明專利]一種基于激光雷達的室內相對定位方法

在審

• 申請號: 202010876496.4
• 申請日: 2020-08-25
• 公開（公告）號: CN112099039A
• 公開（公告）日: 2020-12-18
• 發明（設計）人: 周軍;桑豪;程承;盧青
• 申請（專利權）人: 西北工業大學
• 主分類號: G01S17/42
• 分類號: G01S17/42;G01S17/88;G01S7/48;G01C21/20;G06F17/11;G06F30/20
• 代理公司: 西北工業大學專利中心 61204
• 代理人: 王鮮凱

1.一種基于激光雷達的室內相對定位方法，其特征在于步驟如下：

步驟1：在方形室內環境下，記相鄰兩面墻分別為A、B，將激光雷達隨機放置于室內某點，通電后打開激光雷達，采集得到多幀包含三維坐標信息和反射回波強度的激光雷達點云數據；

步驟2：基于隨機采樣一致性算法，對每一幀激光雷達點云數據進行第一次擬合處理，得到每幀擬合墻面A的平面方程，進而得到激光雷達到墻面A的距離；處理過程為：

1)、從激光雷達點云數據中統計反射回波強度值出現的頻率，選擇頻率最高的j組強度值，記為I₁,I₂,I₃,...,I_j，對這些回波強度值進行直通濾波，保留與這些回波強度值對應的點云數據，作為采樣的總樣本集U₁；

2)、從第一步中獲得的樣本中隨機選擇3個點云數據作為子集，根據選擇的子集數據，在平面方程一次項系數滿足歸一化條件的情況下，利用待定系數法得到這三個點所在的平面方程a₁x+b₁y+c₁z+d₁＝0；

3)、設置誤差容限ε₁，定義一致集Ω₁＝Φ為空集，依次求取所有樣本點到平面a₁x+b₁y+c₁z+d₁＝0的距離，記樣本點A_i(x_i,y_i,z_i)到平面a₁x+b₁y+c₁z+d₁＝0的距離值為L_i＝|a₁x_i+b₁y_i+c₁z_i+d₁|，如果距離L_i滿足條件：L_i＜ε₁，則將樣本點A_i標記為局內點，并將其歸入一致集Ω₁；一致集Ω₁的元素個數記做k，一致集Ω的模型參數即為a₁,b₁,c₁,d₁；

4)、重新隨機選擇3個點云數據作為新的子集，同樣在滿足歸一化條件的情況下，用待定系數法求取出新的子集的平面方程a₁'x+b₁'y+c₁'z+d₁'＝0，根據選擇的新子集平面方程a₁'x+b₁'y+c₁'z+d₁'＝0，在同樣誤差容限ε₁的限定下，再次依次求取所有樣本點到平面a₁'x+b₁'y+c₁'z+d₁'＝0的距離L_i'，取出滿足條件L_i'＜ε₁的點，并將這些點歸入新的一致集Ω₁'，該一致集的元素個數記做k'，判斷k'是否大于k，如果是，則將平面方程的系數a₁,b₁,c₁,d₁更新為a₁',b₁',c₁',d₁'，同時將一致集Ω₁更新為Ω₁'，一致集大小k更新為k'；如果不是，則平面方程系數a₁,b₁,c₁,d₁、一致集Ω₁和一致集的大小k均保持不變；

5)、設置迭代次數上限N₁，重復第4步，隨機采樣N₁次，輸出N₁次采樣后的一致集Ω₁，以及該一致集的模型參數a₁,b₁,c₁,d₁，則激光雷達距離墻面A的距離即為|d₁|；

步驟3：從總樣本集U₁中的數據點中，剔除掉步驟2輸出的一致集Ω₁中的數據點，得到的數據點作為新的樣本集，記做U₂，對樣本集U₂重復步驟2的隨機采樣一致性算法，設置新的誤差容限ε₂和迭代次數上限N₂，輸出此時的一致集Ω₂，以及該一致集的模型參數a₂,b₂,c₂,d₂，得到激光雷達距離墻面B的距離為|d₂|；

步驟4：根據步驟2和步驟3得到的結果，以相鄰墻面法向量方向建立坐標系，使得室內環境保持在坐標系第一象限；得到每一幀激光雷達點云數據處理得到的二維位置坐標信息(x_j,y_j)，其中x_j表示第j幀點云數據輸出的距離|d₁|，y_j表示第j幀點云數據輸出的距離|d₂|。