[發明專利]用于無人駕駛中激光點云定位的正態分布變換方法

有效

• 申請號: 202110718013.2
• 申請日: 2021-06-28
• 公開（公告）號: CN113536232B
• 公開（公告）日: 2023-03-21
• 發明（設計）人: 鄧豈;孫豪;哈亞軍;汪輝
• 申請（專利權）人: 上海科技大學
• 主分類號: G06F17/18
• 分類號: G06F17/18;G06F17/16;G06T15/00;G06T7/30;G01S7/48
• 代理公司: 上海申匯專利代理有限公司 31001
• 代理人: 翁若瑩;徐穎

1.一種用于無人駕駛中激光點云定位的正態分布變換方法，其特征在于，具體包括如下步驟：

1)OAVS數據結構的建立：

1.1)定義激光雷達前一次掃描輸入的點云為固定點云P_F，根據輸入的固定點云P_F，計算其軸對齊邊界框；

1.2)根據預設置的尺度參數R₁，將步驟1.1)獲得的軸對齊邊界框分割為一系列邊長為R₁的三維立體空間體，每個邊長為R₁的三維立體空間體定義為一個體素；

1.3)統計每個體素中點的個數，對于包含點數超過閾值Th_p的體素空間，對其再次進行分割，得到一系列邊長為R₂的三維立體空間體，R₂＜R₁，每個邊長為R₂的三維立體空間體定義為一個子體素，體素與子體素組成OAVS數據結構；

2)基于OAVS數據結構對固定點云建立正態分布信息，并對搜索操作進行優化，以提高正態分布變換算法的實時性，具體如下：

2.1)首先對固定點云P_F做分割，使用開源的點云分割神經網絡Cylinder3D提取出點云中不同的識別物類別；

2.2)對固定點云P_F建立正態分布變換信息，對于固定點云P_F對應的OAVS中每個子體素，計算其中每個類別點集的正態分布變換信息，即每個子體素中每個類別的均值和方差；

2.3)配準移動點云P_M與建立正態分布變換信息后的固定點云P_F：

對于移動點云中的每一個點執行以下操作：

2.3.1)根據位姿轉換矩陣T，將點轉換到固定點云的坐標系中,轉換后的每個點為得到配準后在固定點云的坐標系中的移動點云

2.3.2)在固定點云的子體素空間中，搜索所在的子體素o，根據所屬類別j，計算與此子體素的正態分布匹配的概率：

其中公式是矩陣的轉置矩陣乘以∑_oj矩陣的逆矩陣再乘以矩陣均值μ_oj為子體素o中類別為j的均值，∑_oj為子體素o中類別為j的方差；

2.3.3)并根據步驟2.3.2)計算的概率，得到總的匹配得分

為了求得最佳的位姿轉換矩陣T，獲得最佳的匹配得分，使用牛頓高斯迭代法來求解位姿轉換矩陣T；

2.3.4)記score函數的一階導數梯度矩陣為二階導數海瑟矩陣為H；

2.3.5)通過求解得到位姿態旋轉矩陣T的增量ΔT；

2.3.6)通過T←T+ΔT，更新位姿態旋轉矩陣T；

2.3.7)如果score函數沒有收斂到期望閾值以下，即移動點云P_M與固定點云P_F的匹配程度沒有達到期望，回到步驟2.3.1)，用步驟2.3.6)更新的位姿旋轉矩陣T重新定義位姿轉換矩陣T，重復步驟2.3.1)到2.3.6)直至score函數收斂至期望閾值以下；

2.3.8)步驟2.3.7)獲得的最終位姿轉換矩陣T即所求的移動點云相對固定點云的位姿轉換矩陣。

2.根據權利要求1所述用于無人駕駛中激光點云定位的正態分布變換方法，其特征在于，所述步驟2.2)在建立正態分布轉換信息階段，分割后的固定點云被流式地傳入FPGA加速器，進行步驟1)建立OAVS數據結構，并計算每個子體素的正態分布變換信息。

3.根據權利要求2所述用于無人駕駛中激光點云定位的正態分布變換方法，其特征在于，所述步驟2.3)配準階段，分割后的移動點云被流式地傳入FPGA加速器，以數據流和流水線的形式被高效處理，得到梯度矩陣與海瑟矩陣。