[發(fā)明專利]IMU與剛體的位姿融合方法、裝置、設(shè)備及存儲(chǔ)介質(zhì)

在審

• 申請(qǐng)?zhí)枺?/td>: 202111208097.1
• 申請(qǐng)日: 2020-04-30
• 公開(kāi)（公告）號(hào): CN113984051A
• 公開(kāi)（公告）日: 2022-01-28
• 發(fā)明（設(shè)計(jì)）人: 吳迪云;許秋子
• 申請(qǐng)（專利權(quán)）人: 深圳市瑞立視多媒體科技有限公司
• 主分類號(hào): G01C21/18
• 分類號(hào): G01C21/18;G01P15/18;G01C9/00
• 代理公司: 深圳市育科知識(shí)產(chǎn)權(quán)代理有限公司 44509
• 代理人: 賈鵬

本發(fā)明涉及測(cè)量領(lǐng)域，公開(kāi)了一種IMU與剛體的位姿融合方法、裝置、設(shè)備及存儲(chǔ)介質(zhì)，用于解決在測(cè)量剛體的位姿數(shù)據(jù)時(shí)，剛體的測(cè)量位姿數(shù)據(jù)與真實(shí)位姿數(shù)據(jù)之間誤差大的問(wèn)題。IMU與剛體的位姿融合方法包括：確定慣性測(cè)量單元IMU與剛體之間的位置關(guān)系；當(dāng)剛體攜帶IMU運(yùn)動(dòng)時(shí)，獲取IMU的測(cè)量姿態(tài)數(shù)據(jù)，并通過(guò)IMU的測(cè)量姿態(tài)數(shù)據(jù)計(jì)算得到IMU的待融合姿態(tài)數(shù)據(jù)：在預(yù)置的世界坐標(biāo)系中，獲取剛體的測(cè)量姿態(tài)數(shù)據(jù)，并根據(jù)IMU的測(cè)量姿態(tài)數(shù)據(jù)、剛體的測(cè)量姿態(tài)數(shù)據(jù)以及IMU與剛體之間的位置關(guān)系確定剛體的候選姿態(tài)數(shù)據(jù)；利用融合算法將IMU的待融合姿態(tài)數(shù)據(jù)與剛體的候選姿態(tài)數(shù)據(jù)融合，得到剛體的融合位姿數(shù)據(jù)。

技術(shù)領(lǐng)域

本發(fā)明涉及測(cè)量領(lǐng)域，尤其涉及一種IMU與剛體的位姿融合方法、裝置、設(shè)備及存儲(chǔ)介質(zhì)。

背景技術(shù)

慣性測(cè)量單元(Inertial measurement unit，IMU)，測(cè)量物體三軸姿態(tài)角(或角速率)以及加速度的裝置。一般的，一個(gè)IMU內(nèi)設(shè)置有三個(gè)軸向的陀螺儀和三個(gè)方向的加速度計(jì)，分別用來(lái)測(cè)量剛體在三維空間中的角速度和加速度，并以此計(jì)算出剛體的姿態(tài)，因此，IMU的使用范圍非常廣，從宇航，到汽車及消費(fèi)電子領(lǐng)域中都有IMU的身影。此外，IMU具有高頻率數(shù)據(jù)采集能力，例如：亞德諾半導(dǎo)體(ADI)ADIS16系列的采集頻率能夠達(dá)到200Hz，部分型號(hào)甚至達(dá)到400Hz采集頻率。在有些應(yīng)用場(chǎng)景下需要測(cè)量剛體高速運(yùn)動(dòng)時(shí)的位姿數(shù)據(jù)，此時(shí)可以用光學(xué)定位裝置和IMU融合方式。

但在具體的實(shí)際應(yīng)用場(chǎng)景下，當(dāng)剛體高速運(yùn)動(dòng)時(shí)如果某些攝像裝置沒(méi)有捕捉到剛體的位置或僅僅捕捉到剛體的偏移位置，這樣得到剛體的測(cè)量位姿數(shù)據(jù)與真實(shí)位姿數(shù)據(jù)之間的誤差偏大，無(wú)法得到精確的剛體數(shù)據(jù)。

發(fā)明內(nèi)容

本發(fā)明的主要目的在于解決在測(cè)量剛體的位姿數(shù)據(jù)時(shí)，剛體的測(cè)量位姿數(shù)據(jù)與真實(shí)位姿數(shù)據(jù)之間誤差大的問(wèn)題。

本發(fā)明第一方面提供了一種IMU與剛體的位姿融合方法，包括：確定慣性測(cè)量單元IMU與剛體之間的位置關(guān)系；當(dāng)所述剛體攜帶所述IMU運(yùn)動(dòng)時(shí)，獲取所述IMU的測(cè)量姿態(tài)數(shù)據(jù)，并通過(guò)所述IMU的測(cè)量姿態(tài)數(shù)據(jù)計(jì)算得到所述IMU的待融合姿態(tài)數(shù)據(jù)；在預(yù)置的世界坐標(biāo)系中，獲取剛體的測(cè)量姿態(tài)數(shù)據(jù)，并根據(jù)所述IMU的測(cè)量姿態(tài)數(shù)據(jù)、所述剛體的測(cè)量姿態(tài)數(shù)據(jù)以及所述IMU與剛體之間的位置關(guān)系確定剛體的候選姿態(tài)數(shù)據(jù)；利用融合算法將所述IMU的待融合姿態(tài)數(shù)據(jù)與所述剛體的候選姿態(tài)數(shù)據(jù)融合，得到剛體的融合位姿數(shù)據(jù)。

可選的，在本發(fā)明第一方面的第一種實(shí)現(xiàn)方式中，所述當(dāng)所述剛體攜帶所述IMU運(yùn)動(dòng)時(shí)，獲取所述IMU的測(cè)量姿態(tài)數(shù)據(jù)，并通過(guò)所述IMU的測(cè)量姿態(tài)數(shù)據(jù)計(jì)算得到所述IMU的待融合姿態(tài)數(shù)據(jù)包括：獲取IMU的標(biāo)準(zhǔn)姿態(tài)數(shù)據(jù)，所述標(biāo)準(zhǔn)位姿態(tài)據(jù)包括標(biāo)準(zhǔn)加速度偏置、標(biāo)準(zhǔn)角速度偏置、標(biāo)準(zhǔn)加速度噪聲及標(biāo)準(zhǔn)角速度噪聲；當(dāng)所述剛體攜帶所述IMU運(yùn)動(dòng)時(shí)，獲取所述IMU的測(cè)量姿態(tài)數(shù)據(jù)，并根據(jù)所述IMU的測(cè)量姿態(tài)數(shù)據(jù)以及所述標(biāo)準(zhǔn)姿態(tài)數(shù)據(jù)確定待融合姿態(tài)數(shù)據(jù)，其中，所述待融合姿態(tài)數(shù)據(jù)包括待融合加速度以及待融合角速度。