[發(fā)明專利]基于單目視覺的位置姿態(tài)測量系統(tǒng)精度檢校方法和裝置

有效

• 申請?zhí)枺?/td>: 201810153912.0
• 申請日: 2018-02-22
• 公開（公告）號: CN108375382B
• 公開（公告）日: 2021-04-02
• 發(fā)明（設(shè)計(jì)）人: 朱莊生;袁學(xué)忠;劉剛;李建利;顧賓;王世博;劉艷紅
• 申請（專利權(quán)）人: 北京航空航天大學(xué)
• 主分類號: G01C25/00
• 分類號: G01C25/00
• 代理公司: 北京挺立專利事務(wù)所(普通合伙) 11265
• 代理人: 李鑫

1.基于單目視覺的位置姿態(tài)測量系統(tǒng)精度檢校方法，其特征在于，包括以下步驟：

通過相機(jī)預(yù)先從不同角度拍攝三幅靶標(biāo)的參考圖像；

將IMU與所述靶標(biāo)固連，通過所述相機(jī)獲取一張所述靶標(biāo)的實(shí)時(shí)圖像；

由所述相機(jī)的投影模型得到每個(gè)標(biāo)志點(diǎn)和對應(yīng)的像點(diǎn)之間的關(guān)系為：

式中：為第i幅圖上第j個(gè)標(biāo)志點(diǎn)像坐標(biāo)，X_j為第j個(gè)標(biāo)志點(diǎn)物坐標(biāo)，Pⁱ為第i幅圖對應(yīng)的投影矩陣；為第i幅圖中第j個(gè)像點(diǎn)和其對應(yīng)的物點(diǎn)的距離在相機(jī)光軸方向的投影，稱為射影深度；

構(gòu)建測量矩陣W：

其中，為測量坐標(biāo)系下所測數(shù)據(jù)，使用共軛梯度法迭代每個(gè)點(diǎn)的射影深度通過測量坐標(biāo)系的實(shí)測坐標(biāo)更新測量矩陣，對W陣通過SVD分解：

W＝V·diag(σ₁,…σ_N)·U^T

由P、X陣，可知，W的秩小于等于4，即σ₅＝σ₆＝…＝σ₉＝0因此有：

此時(shí)W陣可以分解為一個(gè)表示相機(jī)運(yùn)動的P陣和一個(gè)表示靶標(biāo)形狀的X陣：

式中V₁,U₁分別為V,U的前4列；分解不唯一，上式在相差任意非奇異4x4矩陣H的情況下都滿足：

W＝PHH^-1X

當(dāng)存在非奇異矩陣H滿足歐式變換條件：

P_pH＝αK[RT]

式中α是滿足矩陣方程的任意非零實(shí)數(shù)；將H改寫為H＝[H_a,H_b],H_a是H陣的前3列，H_b是H陣第四列；從歐式變換矩陣中分割出旋轉(zhuǎn)部分得：

P_pH_a＝αKR

對上式進(jìn)行轉(zhuǎn)置并令ω＝KK^T，可得：

將展開，可得3x3對稱陣，用D_ab表示該陣第a行b列的元素：

D_ab是對稱陣，將其拆為方程組，列出上三角6個(gè)元素對應(yīng)方程：

對M進(jìn)行奇異值分解：

M＝Udiag(σ₁,σ₂,σ₃,0)V^T

式中U_a為U的前3列；因H滿足下式P_pH_b＝[0,0,0]^T，帶入任意P_p解出H_b；最終解出H，完成歐式重構(gòu)：

C是P_e中前3行，前3列數(shù)據(jù)，即為所求的第一位姿數(shù)據(jù)，獲取靶標(biāo)的第一位姿數(shù)據(jù)；

標(biāo)定IMU坐標(biāo)系和靶標(biāo)坐標(biāo)系之間的固定相對位姿關(guān)系，具體包括步驟：

基于正交矢量和動態(tài)濾波的聯(lián)合標(biāo)定法，先使用正交矢量法標(biāo)定兩坐標(biāo)系姿態(tài)關(guān)系，再使用動態(tài)濾波法估計(jì)兩坐標(biāo)系位置關(guān)系，濾波狀態(tài)量為：

其中，為IMU在地理坐標(biāo)系的位置；為IMU在地理坐標(biāo)系的速度；b_a為IMU中加速度計(jì)漂移誤差；為IMU坐標(biāo)系和靶標(biāo)坐標(biāo)系之間的相對位置關(guān)系；

濾波量測量為：

其中，z為相機(jī)測得的靶標(biāo)的位置信息；

其中，由IMU直接輸出，由已標(biāo)定姿態(tài)和IMU、相機(jī)輸出姿態(tài)計(jì)算得到：

其中，由正交矢量法標(biāo)定得到；

將所述第一位姿數(shù)據(jù)轉(zhuǎn)到IMU坐標(biāo)系下，得出第二位姿數(shù)據(jù)；

將所述第二位姿數(shù)據(jù)與POS自身輸出的位置姿態(tài)數(shù)據(jù)進(jìn)行比對，檢校所述POS的測量精度。

2.基于單目視覺的位置姿態(tài)測量系統(tǒng)精度檢校裝置，用于檢測POS輸出的位姿數(shù)據(jù)的精度，其特征在于，包括靶標(biāo)、相機(jī)和檢校模塊；

所述靶標(biāo)與所述POS的IMU固連；

所述相機(jī)，固定設(shè)置，用于預(yù)先從不同角度拍攝三幅靶標(biāo)的參考圖像，并在固定后拍攝一張所述靶標(biāo)的實(shí)時(shí)圖像；

所述檢校模塊，用于：

由所述相機(jī)的投影模型得到每個(gè)標(biāo)志點(diǎn)和對應(yīng)的像點(diǎn)之間的關(guān)系為：

式中：為第i幅圖上第j個(gè)標(biāo)志點(diǎn)像坐標(biāo)，X_j為第j個(gè)標(biāo)志點(diǎn)物坐標(biāo)，Pⁱ為第i幅圖對應(yīng)的投影矩陣；為第i幅圖中第j個(gè)像點(diǎn)和其對應(yīng)的物點(diǎn)的距離在相機(jī)光軸方向的投影，稱為射影深度；

構(gòu)建測量矩陣W：

其中，為測量坐標(biāo)系下所測數(shù)據(jù)，使用共軛梯度法迭代每個(gè)點(diǎn)的射影深度通過測量坐標(biāo)系的實(shí)測坐標(biāo)更新測量矩陣，對W陣通過SVD分解：

W＝V·diag(σ₁,…σ_N)·U^T

由P、X陣，可知，W的秩小于等于4，即σ₅＝σ₆＝…＝σ₉＝0因此有：

此時(shí)W陣可以分解為一個(gè)表示相機(jī)運(yùn)動的P陣和一個(gè)表示靶標(biāo)形狀的X陣：

式中V₁,U₁分別為V,U的前4列；分解不唯一，上式在相差任意非奇異4x4矩陣H的情況下都滿足：

W＝PHH^-1X

當(dāng)存在非奇異矩陣H滿足歐式變換條件：

P_pH＝αK[RT]

式中α是滿足矩陣方程的任意非零實(shí)數(shù)；將H改寫為H＝[H_a,H_b],H_a是H陣的前3列，H_b是H陣第四列；從歐式變換矩陣中分割出旋轉(zhuǎn)部分得：

P_pH_a＝αKR

對上式進(jìn)行轉(zhuǎn)置并令ω＝KK^T，可得：

將展開，可得3x3對稱陣，用D_ab表示該陣第a行b列的元素：

D_ab是對稱陣，將其拆為方程組，列出上三角6個(gè)元素對應(yīng)方程：

對M進(jìn)行奇異值分解：

式中U_a為U的前3列；因H滿足下式P_pH_b＝[0,0,0]^T，帶入任意P_p解出H_b；最終解出H，完成歐式重構(gòu)：

C是P_e中前3行，前3列數(shù)據(jù)，即為所求的第一位姿數(shù)據(jù)，獲取靶標(biāo)的第一位姿數(shù)據(jù)；

標(biāo)定IMU坐標(biāo)系和靶標(biāo)坐標(biāo)系之間的固定相對位姿關(guān)系，將所述第一位姿數(shù)據(jù)轉(zhuǎn)到IMU坐標(biāo)系下，得出第二位姿數(shù)據(jù)；

將所述第二位姿數(shù)據(jù)與POS自身輸出的位置姿態(tài)數(shù)據(jù)進(jìn)行比對，檢校所述POS的測量精度；

所述檢校模塊，還用于：

基于正交矢量和動態(tài)濾波的聯(lián)合標(biāo)定法來標(biāo)定IMU坐標(biāo)系和靶標(biāo)坐標(biāo)系之間的固定相對位姿關(guān)系，先使用正交矢量法標(biāo)定兩坐標(biāo)系姿態(tài)關(guān)系，再使用動態(tài)濾波法估計(jì)兩坐標(biāo)系位置關(guān)系，濾波狀態(tài)量為：

其中，為IMU在地理坐標(biāo)系的位置；為IMU在地理坐標(biāo)系的速度；b_a為IMU中加速度計(jì)漂移誤差；為IMU坐標(biāo)系和靶標(biāo)坐標(biāo)系之間的相對位置關(guān)系；

濾波量測量為：

其中，z為相機(jī)測得的靶標(biāo)的位置信息；

其中，由IMU直接輸出，由已標(biāo)定姿態(tài)和IMU、相機(jī)輸出姿態(tài)計(jì)算得到：

其中，由正交矢量法標(biāo)定得到。