[發明專利]一種多傳感器聯合標定裝置及方法

有效

• 申請號: 202010881818.4
• 申請日: 2020-08-28
• 公開（公告）號: CN111735479B
• 公開（公告）日: 2021-03-23
• 發明（設計）人: 羅哉;江文松;朱志遠;趙洪楠;陳藝文;黃杰伊
• 申請（專利權）人: 中國計量大學
• 主分類號: G01C25/00
• 分類號: G01C25/00;G01S7/497;G06T7/80
• 代理公司: 杭州杭誠專利事務所有限公司 33109
• 代理人: 尉偉敏

1.一種多傳感器聯合標定方法，相應的多傳感器聯合標定裝置包括機械臂，所述機械臂上設置傳感器融合框架，傳感器融合框架上設置有激光雷達、單目相機，以及用以處理數據的計算機，其特征在于，還包括激光雷達-相機四標定板聯合標定靶，所述激光雷達-相機四標定板聯合標定靶包括1號標定板、2號標定板、3號標定板、4號標定板；1號標定板、2號標定板、3號標定板的中心位置由圓點標注，用于提供外參標定用的特征點；4號標定板上排布有黑白棋盤格圖案，用于相機的內參標定；四個標定板按照“田”字形布置，1號標定板和2號標定板并列設置，3號標定板和4號標定板并列設置在1號標定板和2號標定板的前方，3號標定板和4號標定板低于1號標定板和2號標定板；3號標定板面的法向量n₃與4號標定板面法向量n₄之間角度差大于30°；1號標定板面法向量n₁與2號標定板面法向量n₂之間角度差大于30°；

多傳感器聯合標定方法包括相機的內參標定、激光雷達-相機的外參標定；相機內參標定，計算機單元控制機械臂和相機，使得相機在不同姿態下對棋盤格標定板進行拍攝，采用張正友標定法計算出相機內參K；激光雷達-相機的外參標定步驟為：

步驟一：計算機單元調整機械臂姿態，使標定板模塊出現在激光雷達和相機視野范圍內；控制機械臂保持靜止狀態，激光雷達和相機進行數據采集；

步驟二：對激光雷達數據進行分析與處理，提取點云特征點；記錄各點云坐標信息，濾除異常點，采用點云分割方法，對點云數據進行分割，并將四個標定板的點云數據分成四個不同的組{L₁}、{L₂}、{L₃}、{L₄}，采用K-means方法提取點云聚類中心點；即為第i個標定板的中心點位置在激光雷達坐標系下的三維坐標值，將選定為用于匹配的激光特征點；

步驟三：對相機數據進行分析與處理，提取視覺特征點；記錄每個像素的灰度值，采用FAST關鍵點提取算法，檢測標定板局部像素灰度變化明顯的地方，從而提取每個標定板的中心點位置；提取1號標定板、2號標定板、3號標定板圓點中心位置，記錄其坐標值C₁、C₂、C₃；通過分析棋盤格之間的關系，求出4號標定板中心點坐標值C₄；C_i即為第i個標定板的中心點位置在相機坐標系C下的二維坐標值，將C_i選定為用于匹配的視覺特征點；

步驟四：根據激光雷達特征點以及相機特征點{C₁，C₂，C₃，C₄}，建立匹配關系：根據特征點匹配關系，建立最小化重投影誤差，構建誤差方程，根據誤差方程，進行最小二乘求解，得到最優的外參矩陣T_LC；在標定板中心位置特征點基礎上，采集標定板邊角位置作為特征點；具體步驟為：

根據定義激光雷達坐標(Xi，Yi，Zi)和單目相機坐標(Ui，Vi)，基于特征點Pi的匹配關系，建立最小化重投影誤差，構建最小二乘問題；定義激光雷達和單目相機的外參矩陣為:

其中R為旋轉矩陣，t為平移向量；則結合匹配關系和外參矩陣，可表示出誤差方程為：

其中S_i為視覺特征點的深度值，根據誤差方程，構建最小二乘優化函數，使誤差最小化，得到最優的外參矩陣：

多傳感器聯合標定裝置 還包括設置在傳感器融合框架上的慣性導航系統，聯合標定方法包括激光雷達與慣性導航系統聯合標定方法，定義機械臂初始運動時刻為t₀，機械臂運動結束時刻為t_n，t_n時刻掃描到的激光點云為P_n，t₀時刻下激光雷達坐標系為L₀，t₀時刻下慣性導航系統坐標系為I₀,t_n時刻下激光雷達坐標系為L_n，t_n時刻下慣性導航系統坐標系為I_n，激光雷達在t₀時刻到t_n時刻之間的位姿變換矩陣為T_L，慣性導航系統在t₀時刻到t_n時刻之間的位姿變換矩陣為T_I，激光雷達和慣性導航系統之間的外參矩陣為T_LI，其步驟為：

步驟一：機械臂做指定軌跡的運動，機械臂運動過程中，激光雷達和慣性導航系統進行數據采集；

步驟二：控制機械臂停止運動，對傳感器采集的數據進行處理；對于激光雷達，按照勻速運動模型對每幀激光點云進行去畸變處理；采用點云濾波算法，將點云數據中的離群點剔除；

步驟三：計算激光雷達坐標系L在初始時刻t₀的坐標系L₀與運動結束的坐標系L_n之間的位姿變換矩陣T_L，位姿變換矩陣T_L計算方法為：采用迭代最近鄰方法匹配第m幀點云和第m+1幀點云，得到第m幀點云P＝{p₁，...，p_n}與第m+1幀點云P′＝{p′₁，...p′_n}的匹配關系；第m幀到第m+1幀激光雷達的位姿變換由關系旋轉矩陣R_m和平移向量t_m組成，進一步，構建誤差方程，將誤差方程轉換為最小二乘問題用SVD方法計算R_m、t_m，根據R_m、t_m，得到第m時刻到第m+1時刻激光雷達的位姿變換矩陣為T_m＝(R_m|t_m)，將n幀激光雷達位姿變換矩陣T_m累乘，得到激光雷達從t₀時刻到t_n時刻的位姿變換矩陣T_L；

步驟四：計算慣性導航系統坐標系I在初始時刻t₀的坐標系I₀與運動結束時刻t_n的坐標系I_n之間的位姿變換矩陣T_I，位姿變換矩陣T_I計算方法為：對慣性導航系統的加速度測量數據與角速度測量數據進行積分，得到位移數據t_I與旋轉數據R_I，則t₀時刻到t_n時刻慣性導航系統的位姿變換矩陣表示為T_I＝(R_I|t_I)；

步驟五：通過T_L將t_n時刻的激光點云P_n投影到t₀時刻的L₀坐標系下，得到點云P_nL；

步驟六：通過T_I和待標定參數T_LI，將t_n時刻的激光點云P_n投影到t₀時刻的L₀坐標系下，得到P_nI；

步驟七，匹配P_nL和P_nI兩組點云，通過對齊兩組點云，對外參矩陣T_LI進行優化，采用迭代最近點方法，將P_nL和P_nI所描述的同一片點云區域配準，構建并優化最近鄰誤差T_error，根據T_error、T_L、T_I求解外參矩陣T_LI；

所述計算外參矩陣T_LI，其方法為對齊點云P_nL和點云P_nI；理論上，P_nL和P_nI描述的是同一片點云數據，它們在空間上是重合的，即：

由于外參誤差的存在，P_nL和P_nI不是完全重合的；采用迭代最近點方法，將P_nL和P_nI所描述的同一片點云區域配準，構建并優化最近鄰誤差：

根據T_error、T_L、T_I求解外參矩陣T_LI：

至此，通過上述標定方法，得到外參矩陣T_LC和T_LI；

在視覺條件充足的情況下，記錄單目相機觀測數據，采用視覺-IMU標定工具，計算出單目相機和慣性導航系統之間的外參矩陣T_CI；依據確定的三個外參矩陣T_LC、T_LI、T_CI之間的位姿一致性進行聯合驗證，對外參矩陣中的轉換參數進行調整，提高多傳感器聯合標定的精度；

參數優化，采用在線調整方法，融合激光雷達、單目相機、慣性導航系統數據，對T_LI、T_CI進行調整。

2.根據權利要求1所述的多傳感器聯合標定方法，其特征在于，所述相機的內參標定采用棋盤格標定法，計算機單元控制機械臂和相機，使得相機在不同姿態對棋盤格標定板進行拍攝；提取20到30張圖片的角點信息，采用張正友標定法計算出相機內參K。