[發明專利]一種基于點云融合的行車場景構建方法

有效

• 申請號: 202110755584.3
• 申請日: 2021-07-05
• 公開（公告）號: CN113379915B
• 公開（公告）日: 2022-12-23
• 發明（設計）人: 李柔儀;黃梓欣;全芷瑩;蔡健蘋;李賀;余榮;譚北海
• 申請（專利權）人: 廣東工業大學
• 主分類號: G06T17/05
• 分類號: G06T17/05;G06T3/40;G06V20/58
• 代理公司: 佛山市君創知識產權代理事務所(普通合伙) 44675
• 代理人: 杜鵬飛

1.一種基于點云融合的行車場景構建方法，其特征在于，包括以下步驟：

步驟1，對車載激光雷達在汽車運動過程中掃描收集的點云數據進行畸變補償；

步驟2，將畸變補償后的點云數據進行傳感器時間對齊，使點云數據在進行畸變補償的同時，對同一時間下的里程計數據進行線性插值計算，使里程計數據向點云數據對齊時間，得到實現傳感器對齊的機器人的位置姿態作為前端里程估計的初始預測位置姿態；

步驟3，創建一個任務隊列用來存放點云數據，并生成一個局部地圖；任務隊列中每插入一幀點云數據時，輸入該點云數據對應的機器人的初始預測位置姿態，與局部地圖進行點云匹配，對點云數據進行正態分布變換，得到目標點云的所有單位體素，代入高斯概率模型，尋找到局部地圖上與初始預測位置姿態最近似的點，得到前端里程估計的位姿預估值；

步驟4，采用基于點云的場景識別方式，將前端里程估計創建的任務隊列中的點云數據用scan Context特征圖進行描述；定義scan Context特征圖之間的距離計算公式；從scanContext特征圖中提取描述符環，將其表示為一個多維向量以進行KD樹的構建，獲得當前場景的環的最相似區域，從中選取和當前場景的環最相似的歷史環對應的scan context作為候選場景的scan context；計算任務隊列中的點云數據的scan Context和候選場景的scancontext之間的距離，滿足閾值的候選場景則被認為是閉環，則將所述位姿預估值進行后端優化；

步驟5，將機器人不同時刻通過前端里程估計得到的位姿預估值抽象定義為節點，機器人在不同位置上的觀測所產生的約束被抽象為節點之間的邊，節點的位姿信息和路標構成圖的頂點集，用邊來表示觀測數據的約束；

定義誤差函數為兩個節點之間的虛擬測量值的均值與期望的虛擬測量值的差值；通過高斯牛頓法對誤差函數進行迭代求最優解，將求得的最優解疊加到初始預測位置姿態得到新的位姿預估值，將新的位姿預估值作為下一輪迭代的初始預測位置姿態，繼續迭代直至位姿預估值誤差最小；

步驟1-步驟5在邊緣設備中進行，構建出局部地圖后再上傳到云服務器進行全局地圖拼接，最后再將拼接好的地圖回傳到邊緣設備供車輛進行定位操作。

2.根據權利要求1所述的基于點云融合的行車場景構建方法，其特征在于，所述點云數據進行畸變補償，包括：

根據讀取點云幀的首尾數據計算點云幀夾角，得到點云幀采樣的實際時間；

利用讀取進來的慣性測量單元信息獲取車輛角速度和線速度，用角速度乘以時間得到車輛旋轉角度；用線速度乘以時間得到車輛偏移位移，生成旋轉偏移矩陣；

將原始點云數據矩陣乘上旋轉偏移矩陣，得到畸變補償后的點云數據。

3.根據權利要求1所述的基于點云融合的行車場景構建方法，其特征在于，所述實現傳感器對齊的機器人的位置姿態的計算公式為：

其中，X(t_n)代表t_n點的機器人的位置姿態，X(t_o1)、X(t_o2)分別是t_o1、t_o2時刻的里程計數據。

4.根據權利要求1所述的基于點云融合的行車場景構建方法，其特征在于，點云數據的任何區域可通過環和扇形唯一索引，將點云數據用二維形式表示即得到scan Context特征圖；

記兩個點云的scan context特征圖為I^q和I^c，定義scan Context特征圖之間的距離計算公式如下：

其中，N_s代表點云中的扇形數量，分別為利用誤差函數對I^q和I^c的求解結果；

對于存在同一位置但是不同時間訪問的兩個scan context特征圖的情況，所有可能出現列平移的scan contexts特征圖的列向量之和并找到最小值作為最終的距離，列平移的最佳匹配關系n^*定義為：

最小距離定義為：

其中，N_s代表點云中的扇形數量，n代表的就是其中的一個扇形。