[發明專利]基于平面信息的視覺和激光定位建圖方法

有效

• 申請號: 202011065887.4
• 申請日: 2020-09-30
• 公開（公告）號: CN112197773B
• 公開（公告）日: 2022-11-11
• 發明（設計）人: 王慶;馮悠揚;嚴超;許九靖;劉鵬飛;張波
• 申請（專利權）人: 江蘇集萃未來城市應用技術研究所有限公司
• 主分類號: G01C21/20
• 分類號: G01C21/20;G06T7/11;G06T7/187;G06T7/521;G06T7/73;G06T7/80;G06T7/90;G06K9/62
• 代理公司: 蘇州久元知識產權代理事務所(普通合伙) 32446
• 代理人: 潘宏偉

1.一種基于平面信息的視覺和激光定位建圖方法，其特征在于，包括以下步驟：

A.通過激光傳感器、視覺傳感器進行數據采集并進行數據同步；

B.根據公式(1)計算當前時刻所有點曲率，

c＝180*acos(|norm(P(i-5,j,k)-P(i,j,k))·norm((P(i+5,j,k)-P(i,j,k)))|)/π(1)

其中，P(i,j,k)為當前點三維坐標，P(i-5,j,k)為當前點左側第五個相鄰點的三維坐標，P(i+5,j,k)為當前點右側第五個相鄰點的三維坐標，norm函數表示將輸入的向量進行單位化；符號“·”表示將兩個向量點乘，將整個點云分為有曲率點和無曲率點，然后進行區域增長，將平面點、無曲率點與平面點融合；

C.已知i時刻的平面點集合和當前時刻j的平面點集合其中i時刻一共存在n個平面，j時刻一共存在m個平面，將i時刻下的坐標系移動到j時刻坐標系下的位姿矩陣為[R_j,i t_j,i]，已知在平面上的點滿足Ax+By+Cz+D＝0約束方程,A、B、C、D為同一類平面點使用PANSAC算法擬合出的平面參數，根據公式(3)將j時刻下的所有平面參數變換到i時刻坐標系下得到(A^i(j),B^i(j),C^i(j),D^i(j))，通過公式(4)計算與i時刻平面的兩個參數Δr、Δd，當夾角Δr小于30度并且距離Δd小于0.5米，則將i時刻平面加入到j時刻匹配的候選平面中；

其中i時刻第k個平面和j時刻下第l個平面對應空間中一個平面，和分別為在兩個坐標系下擬合得到的平面參數；上標i表示此平面坐標為i時刻下的平面參數，括號中的j表示此平面參數由j時刻下的平面參數變換得到，

其中，Δr用于衡量兩個待匹配平面的法線方向是否一致；Δd用于衡量i時刻坐標系下坐標原點到平面的距離是否一致；

根據公式(6)構造位姿變換矩陣R_p，然后根據位姿變換矩陣R_p將i時刻平面的凸/凹包點投影到j時刻的平面

na向量為j時刻的法向量等于(0，0，1)，nb為j時刻某個平面的法向量，θ為兩個平面的所成角度，nb向量與na向量所成的夾角即為θ，和為方向歸一化之后向量；

D.根據圖像的單應性矩陣性質并與空間三維平面點相融合形成稠密地圖。

2.根據權利要求1所述的基于平面信息的視覺和激光定位建圖方法，其特征在于，所述步驟B中，先通過第一次區域增長將空間點分為有效平面點，無效平面點，無曲率點和角點，然后進行第二次增長，所述第二次增長為遍歷空間中所有的無曲率點和角點，判斷其周圍是否存在有效平面點，當存在則判斷到有效平面的距離是否小于5cm，如果小于5cm則向其他的無曲率點和角點增長，直到停止。

3.根據權利要求1所述的基于平面信息的視覺和激光定位建圖方法，其特征在于，所述步驟C中，當存在多個候選平面時，通過位姿變換矩陣R_p進行投影，然后通過多邊形覆蓋度的方法來選擇凸/凹包重合率最大的平面作為最優的候選平面。