[發明專利]一種相機與慣性測量單元在線初始化與標定方法及系統

有效

• 申請號: 202010197372.3
• 申請日: 2020-03-19
• 公開（公告）號: CN111429524B
• 公開（公告）日: 2023-04-18
• 發明（設計）人: 王賀升;沈玥伶
• 申請（專利權）人: 上海交通大學
• 主分類號: G06T7/80
• 分類號: G06T7/80;G06T7/70;G01C25/00
• 代理公司: 上海段和段律師事務所 31334
• 代理人: 李佳俊;郭國中

1.一種相機與慣性測量單元在線初始化與標定方法，其特征在于，包括：

步驟M1：純視覺初始化，利用相機的觀測量得到初始關鍵幀的位姿并構建初始地圖；

步驟M2：視覺與慣性聯合標定與初始化，基于初始關鍵幀的位姿以及初始地圖完成慣性測量單元角速度零漂值的初始化與相機和慣性測量單元相對旋轉角和平移量的標定；

步驟M3：相機與慣性測量單元融合的同步定位與建圖的過程中，對慣性測量單元角速度零漂值的初始化與相機和慣性測量單元相對旋轉角和平移量的標定進行異常檢測與更新；

所述步驟M2包括：

步驟M2.1：基于信息量對相機與慣性測量單元的觀測量進行篩選，篩選出最大化總信息量的預設數量的觀測量；利用篩選后得到的相機與慣性測量單元的觀測量進行慣性測量單元角速度零漂值的初始化和相機與慣性測量單元相對旋轉角的標定，直到判斷相機與慣性測量單元的相對旋轉角收斂；

步驟M2.2：假設慣性測量單元加速度的零漂值為零，基于信息量對相機與慣性測量單元的觀測量進行篩選，保留最大化總信息量的固定數量的觀測量，對尺度因子、重力向量和相機與慣性測量單元的相對平移量進行粗略估計；

步驟M2.3：利用粗略估計的重力向量的方向，并引入重力大小作為新的約束，基于信息量對相機與慣性測量單元的觀測量進行篩選，保留最大化總信息量的固定數量的觀測量，對尺度因子、重力向量、相機與慣性測量單元相對平移量以及慣性測量單元中加速度計的零漂值進行精細化估計；當尺度因子和相機與慣性測量單元的相對平移量都收斂后，視覺與慣性聯合初始化和標定完成；

所述信息量為相機與慣性測量單元的觀測量中包括相機與慣性測量單元相對旋轉角信息量；

所述步驟M2.1包括：

步驟M2.1.1：利用慣性測量單元的觀測量得到相鄰兩幀之間慣性測量單元的相對旋轉角；

通過對慣性測量單元的角速度進行積分得到相鄰兩幀之間慣性測量單元的相對旋轉角

其中，ΔR_i,i+1表示慣性測量單元角速度積分值；表示慣性測量單元相對于角速度零漂值的雅可比矩陣；b_g表示慣性測量單元角速度零漂值；

步驟M2.1.2：利用相機的觀測量得到相鄰兩幀之間慣性測量單元的相對旋轉角；

通過視覺同步定位與建圖得到相鄰兩幀的相機在世界坐標系下的位姿利用相機與慣性測量單元的相對旋轉角得到慣性測量單元的相對旋轉角

其中，R_cb表示相機與慣性測量單元的相對旋轉角；

步驟M2.1.3：最小化利用慣性測量單元的觀測量得到相鄰兩幀之間慣性測量單元的相對旋轉角和利用相機的觀測量得到相鄰兩幀之間慣性測量單元的相對旋轉角的誤差得到慣性測量單元角速度零漂值的最優估計，即得到慣性測量單元角速度零漂值的初始化；

通過最小化上述兩相對旋轉角誤差得到慣性測量單元角速度零漂值的最優估計

其中，N表示關鍵幀個數，R2表示利用相機觀測量得到的慣性測量單元的相對旋轉角，R1表示利用慣性測量單元角速度積分得到的慣性測量單元的相對旋轉角，b_i表示第i個關鍵幀的慣性測量單元，b_i+1表示第i+1個關鍵幀的慣性測量單元，b_g表示慣性測量單元角速度零漂值；

步驟M2.1.4：對任意相鄰兩關鍵幀，觀測量所包含的關于相機與慣性測量單元相對旋轉角的信息量度量為：

其中，Q_L(q)表示四元數q對應的左乘矩陣；Q_R(q)表示四元數q對應的右乘矩陣；q_bi,bi+1表示第i幀與第i+1幀慣性測量單元的相對旋轉角對應的四元數；q_ci,ci+1表示第i幀與第i+1幀相機的相對旋轉角對應的四元數；

設置信息量窗口大小為N_win，如果關鍵幀總數小于N_win，則將最新的關鍵幀加入到序列中用于初始化和標定；如果關鍵幀總數大于N_win時，則將當前的信息量度量與已有序列中的信息量度量進行比較，剔除信息量度量最小的觀測量；

步驟M2.1.5：利用篩選后的觀測量構建以相機和慣性測量單元相對旋轉角為未知量的方程組，利用最小二乘法求解方程組，得到以四元數形式表示的相機與慣性測量單元相對旋轉角的最優估計即相機與慣性測量單元相對旋轉角的標定；

對于任意相鄰兩關鍵幀，相機和慣性測量單元相對旋轉角為未知量，滿足如下方程組：

(Q_L(q_bi,bi+1)-Q_R(q_ci,ci+1))·q_bc＝0?(5)

其中，Q_L表示四元數的左乘矩陣，q_bi，bi+1表示第i幀和第i+1幀下慣性測量單元相對旋轉角的四元數表示，Q_R表示四元數的右乘矩陣，q_ci,ci+1表示第i幀和第i+1幀下相機相對旋轉角的四元數表示，q_bc表示相機與慣性測量單元的相對旋轉角的四元數表示；

將(Q_L(q_bi,bi+1)-Q_R(q_ci,ci+1))表示為Q_i,i+1，利用篩選后的觀測量Q_i,i+1組成觀測矩陣Q，建立方程：

Q·q_bc＝0?(6)

利用最小二乘法求解相機與慣性測量單元的相對旋轉角的最優估計

步驟M2.1.6：通過判斷最近N_winc個相機與慣性測量單元相對旋轉角對應的歐拉角的偏航角、俯仰角和橫滾角的標準差σ_yaw、σ_pitch、σ_roll是否小于閾值來確定視覺慣性旋轉角是否標定完成；當標定未完成，即相機與慣性測量單元的相對旋轉角未收斂，則等待新的關鍵幀重復執行步驟M2.1.1至步驟M2.1.5，直至視覺慣性旋轉角標定完成。