[發明專利]一種魯棒的車載多傳感器外參標定方法

有效

• 申請號: 202011291238.6
• 申請日: 2020-11-18
• 公開（公告）號: CN112414431B
• 公開（公告）日: 2023-05-12
• 發明（設計）人: 顧遠凌
• 申請（專利權）人: 的盧技術有限公司
• 主分類號: G01C25/00
• 分類號: G01C25/00;G01S7/497
• 代理公司: 南京經緯專利商標代理有限公司 32200
• 代理人: 曹蕓

1.一種魯棒的車載多傳感器外參標定方法，其特征在于，包括如下步驟：

步驟1：分別獲取傳感器時間對齊的位姿序列；根據不同的情況，分別選擇不同的方式獲取位姿序列，具體情況如下：

(1)對于單目或雙目相機，利用任意視覺SLAM算法獲取位姿序

列；

(2)對于激光雷達，利用任意激光SLAM算法獲取位姿序列；

(3)對于IMU傳感器，通過積分獲取位姿序列；

步驟2：確定滑動窗口，利用窗口中的位姿序列估計傳感器外參，具體步驟為：

S201、根據硬件計算能力以及實際使用的需要選擇不同的窗口大小，A、B分別代表兩傳感器的位姿，時間對齊的位姿序列記為：

ζ_A＝{A₀A₁…?A_n}

ζ_B＝{B₀B₁…?B_n}

其中，ζ_A為傳感器A的位姿序列，A₀為滑動窗口中傳感器A的第一幀位姿，A₁為滑動窗口中傳感器A的第二幀位姿，A_n為滑動窗口中傳感器A的第n幀位姿，ζ_B為傳感器B的位姿序列，B₀為滑動窗口中傳感器B的第一幀位姿，B₁為滑動窗口中傳感器B的第二幀位姿，B_n為滑動窗口中傳感器B的第n幀位姿；

S202、

其中：R_A為傳感器A某一幀的旋轉矩陣，t_A為傳感器A某一幀的平移向量，R_B為傳感器B某一幀的旋轉矩陣，t_B為傳感器B某一幀的平移向量；

S203、代價方程為：

其中，h()表示代價方程，A_τ為傳感器A的第τ幀位姿，

B_τ為傳感器B的第τ幀位姿，τ為滑動窗口中遍歷的每一幀；

其中：R為傳感器A相對于B的旋轉矩陣，t為傳感器A相對于B的平移向量；

S204、以單位矩陣或之前標定過的外參作為初始值，利用Levenberg-Marquardt法，對代價方程進行迭代優化；利用優化得到的外參估計，對位姿序列中的每一對位姿分別計算誤差：

E_τ＝A_τX-XB_τ

其中：E_τ為第τ幀位姿的誤差；

S205、剔除誤差大于閾值的位姿對，重新對代價方程進行優化；

重復這個過程直到所有位姿對都滿足誤差要求，或者達到最大迭代次數，即可得到A、B兩個傳感器的外參X；如果有更多的傳感器，也可同理進行優化，得到多傳感器的外參估計；

步驟3：移動窗口，重新估計外參，并與之前估計的外參融合；

步驟4：重復步驟3，得到隨時間修正的穩定的標定參數。