[發明專利]一種基于LeGO-LOAM的分步式幀間位姿估計算法

有效

• 申請號: 202110317982.7
• 申請日: 2021-03-25
• 公開（公告）號: CN112907610B
• 公開（公告）日: 2022-04-29
• 發明（設計）人: 潘樹國;章輝;高旺;趙濤;譚涌
• 申請（專利權）人: 東南大學
• 主分類號: G06T7/13
• 分類號: G06T7/13;G06T7/20;G06K9/62;G06V10/764
• 代理公司: 南京眾聯專利代理有限公司 32206
• 代理人: 蔣昱

1.一種基于LeGO-LOAM的分步式幀間位姿估計算法，其特征在于，具體方法如下：

(1)讀入激光點云數據；

系統通過激光雷達獲取周圍環境的點云信息，并將點云信息進行處理，將激光束編號，從仰角最小的激光束開始編號，作為點云的縱坐標，激光的水平編號，從旋角為0的點開始編號，作為其橫坐標，對點云進行編號，最后記錄點云的深度信息、水平旋角和仰角；

(2)對點云進行分類；

點云分類通過兩步完成，第一步通過地面點計算以及公式(1)的原理完成地面點的標記；

θ＝atan2(ΔZ-ΔX)

ΔX＝R_r-1cosα-R_rcosβ

ΔZ＝R_r-1sinα-R_rsinβ (1)

公式(1)中R_r-1表示仰角為α的激光束的長度，R_r表示仰角為β的激光束的長度，當θ大于閾值時，則該點標記為非地面點，否則標記為地面點，計入地面點集合

完成地面點標記后，進行點云集群的劃分，其原理點云集群劃分和公式(2)所示；

公式(2)中β表示點云中最臨近的兩個點到激光雷達中心連線的夾角，θ為兩個點中深度更大的點到激光雷達的連線和兩點之間連線的夾角，d₁為兩點中深度更大的點的深度，d₂為另一個點的深度，若θ大于閾值這說明兩個點不為一個集群，否則兩個點為一個集群，在完成所有點的集群劃分后，對點的集群進行標記并剔除小集群點；

(3)提取地面特征點和邊緣特征點；

首先計算每個點的曲率值，公式如下式(3)所示；

式中|S|表示點集中點的個數，||r_i||表示點i的深度，通過式(3)得到點云中每個點的曲率值，在非地面點中，點的曲率越大則表明此處點云深度變化幅度越大，并計入邊緣特征點集合

若特征點集中于一個區域，可能會出現某一時刻該區域不可觀測導致的位姿跟蹤丟失，造成位姿估計出現較大的誤差，所以對點云進行分散采樣，將每束點云平均劃分為六個區域，在六個區域中各選取2個曲率最大的邊緣特征點計入集合F_e，或四個曲率值最小的地面特征點計入集合F_p；

(4)分步幀間位姿計算：

針對無人車平臺，將幀間位姿分為六個部分，三個旋轉角度：rx俯仰角，ry偏航角和rz翻滾角，三個平移量：tx左右方向上的位移，ty豎直方向上的位移和tz前后方向上的位移；

在求取幀間位姿時，設相鄰幀間，由后一幀到前一幀的旋轉矩陣為R，平移向量為-t，并且將后一幀中的邊緣特征點F_e和地面特征點F_p按照下述公式(4)投影至前一幀中，式中代表k時刻雷達坐標系下的點i的坐標，而表示k+1時刻雷達坐標系下點i的坐標；

式(4)中R和t可用(r_x，r_y，r_z，t_x，t_y，t_z)表示，如下式(5)可見：

相鄰幀間僅匹配具有相同的集群標志的點，并采用LOAM算法中提出的“點到線”和“點到面”的匹配方案，該匹配方案的誤差由下式(6)給出；

式中表示邊緣特征點，和表示k時刻中與匹配的兩個邊緣特征點的坐標，表示地面特征點，和表示k時刻中與匹配的地面點的坐標，d_ξ表示邊緣點匹配中的“點到線”的距離，d_φ表示地面點匹配中的“點到面”的距離；

針對無人車平臺位姿變化集中于水平面上的運動，將幀間位姿分為兩步求解，第一步為通過地面特征點求解得到(rx，rz，ty)，其最小二乘原理如下式(7)所示；

通過式(7)求解得到(rx，rz，ty)之后，將(rx，rz，ty)代入第二步，相對于LeGO-LOAM算法的僅通過邊緣特征點求解(ry，tx，tz)，通過邊緣特征點求解(rx，ry，rz，tx，ty，tz)，其原理如下式(8)；

通過上述步驟完成分布式幀間六自由度位姿估計。