[發明專利]激光定位初始化方法

有效

• 申請號: 201910306885.0
• 申請日: 2019-04-17
• 公開（公告）號: CN110031825B
• 公開（公告）日: 2021-03-16
• 發明（設計）人: 顧浩;黃友;張國龍;張放;李曉飛;張德兆;王肖;霍舒豪
• 申請（專利權）人: 北京智行者科技有限公司
• 主分類號: G01S7/497
• 分類號: G01S7/497;G01S17/06
• 代理公司: 北京慧誠智道知識產權代理事務所(特殊普通合伙) 11539
• 代理人: 李楠

1.一種激光定位初始化方法，其特征在于，所述方法包括：

獲取多個點云序列的拼接幀點云信息；所述多個點云序列按照預設第一距離依次排列；每個所述點云序列包括獲取時間依次遞增的第一幀點云、第二幀點云和第三幀點云，所述第一幀點云、第二幀點云和所述第三幀點云拼接得到拼接幀點云；所述拼接幀點云信息包括拼接幀點云的局部位置坐標、拼接幀點云中各點云的位置、反射率和高度；

對所述拼接幀點云進行采樣，得到多個采樣點云；

根據所述采樣點云中各點云的位置，確定點云分布中心；

根據所述點云分布中心，對所述采樣點云進行主成分分析，生成主成分分析特征矩陣；

根據所述主成分分析特征矩陣，構建特征坐標系；

根據所述點云分布中心和所述特征坐標系，對所述采樣點云進行劃分，生成多個象限，在每個所述象限內以三維直方圖存儲相應采樣點云的反射率和/或高度；

根據所述拼接幀點云的局部位置坐標，為包括多個三維直方圖的所述采樣點云命名，生成所述采樣點云的特征文件；多個所述拼接幀的采樣點云的特征文件構成特征文件集；

實時獲取當前幀點云信息；所述當前幀點云信息包括當前幀點云的位置坐標、當前幀點云中各點云的位置、反射率和高度；

對所述當前幀點云進行采樣，得到多個當前采樣點云；

根據所述當前采樣點云中各點云的位置，確定當前點云分布中心；

根據所述當前點云分布中心，對所述當前采樣點云進行主成分分析，生成當前主成分分析特征矩陣；

根據所述當前主成分分析特征矩陣及矩陣的旋轉特性，構建四個當前特征坐標系；

根據所述當前點云分布中心和所述四個當前特征坐標系，對所述當前采樣點云進行劃分，在每個當前特征坐標系下，生成多個當前象限，在每個所述當前象限內以實時三維直方圖存儲相應當前采樣點云的反射率和/或高度；

根據預設的搜索中心與多個所述拼接幀點云的局部位置坐標的距離，獲取所述距離在預設第二距離內的第一數量個拼接幀點云對應的第一數量個特征文件；

將所述實時三維直方圖與所述第一數量個特征文件中的三維直方圖進行第一匹配計算，得到第一匹配結果；

根據第一匹配結果中的分值，確定第二數量個特征文件；所述第二數量小于所述第一數量；

根據所述第二數量個特征文件的局部位置坐標和所述主成分分析特征矩陣以及所述當前主成分分析特征矩陣，計算當前幀點云的第二數量個實時位置；

在多個方位上，對所述第二數量個特征文件進行平移，確定第二數量個特征文件中每個特征文件在每個方位上平移后的位置；

將所述第二數量個實時位置與每個特征文件在每個方位上平移后的位置進行第二匹配計算，得到第二匹配結果；

當所述第二匹配結果中的分值大于預設閾值時，確定完成激光定位的初始化；

其中，所述根據所述第二數量個特征文件的局部位置坐標和所述主成分分析特征矩陣以及所述當前主成分分析特征矩陣，計算當前幀點云的第二數量個實時位置，具體包括：

將所述第二數量個特征文件中每個特征文件的局部位置坐標和所述主成分分析特征矩陣、所述當前主成分分析特征矩陣的倒數相乘，得到與該特征文件對應的當前幀點云的一個實時位置。

2.根據權利要求1所述的方法，其特征在于，所述第一幀點云、第二幀點云和所述第三幀點云拼接得到拼接幀點云，具體包括：

計算第一幀點云相對于第二幀點云的第一相對位姿變換；

計算第三幀點云相對于第二幀點云的第二相對位姿變換；

根據所述第一相對位姿變換、所述第二相對位姿變換和所述第二幀點云的位姿，疊加得到拼接幀點云。

3.根據權利要求1所述的方法，其特征在于，所述根據所述采樣點云中各點云的位置，確定點云分布中心，具體包括：

獲取所述采樣點云中點云的個數；

將所述采樣點云中各點云的x坐標、y坐標和z坐標分別相加，得到x坐標相加結果、y坐標相加結果和z坐標相加結果；

將所述x坐標相加結果、y坐標相加結果和z坐標相加結果分別除以點云的個數，得到點云分布中心。