本發(fā)明涉及導(dǎo)航技術(shù)領(lǐng)域，具體涉及一種慣性器件和地磁傳感器整體標(biāo)定裝置及其標(biāo)定方法。

背景技術(shù)：

慣性測量單元(IMU)和地磁傳感器廣泛應(yīng)用于智能手機等電子消費品，以及無人機和無人駕駛車等機器人系統(tǒng)中。IMU是慣性導(dǎo)航的基礎(chǔ)測量裝置，由三軸陀螺和三軸加速度計組成，固聯(lián)于載體之上，實現(xiàn)對載體角速度和比力的感知，通過力學(xué)編排的微分方程，能夠?qū)崟r計算出載體的位置、速度和姿態(tài)角。但是，民用慣性器件的精度差，難以實現(xiàn)方位角的測量和對準(zhǔn)。因此，采用三軸地磁傳感器(例如磁通門傳感器和磁阻傳感器等)，感知當(dāng)?shù)卮艌鰪姸?，實現(xiàn)方位角的計算。但是慣性導(dǎo)航的精度嚴(yán)重依賴慣性器件的精度，即使與GNSS(全球?qū)Ш叫l(wèi)星系統(tǒng)，例如GPS和北斗等)接收機構(gòu)成組合導(dǎo)航系統(tǒng)，也存在室內(nèi)或城市峽谷等無GNSS信號環(huán)境。地磁傳感器的測量精度受載體的電磁環(huán)境的影響明顯，因此方位角的精度難以保證。為了提高慣導(dǎo)和方位角的解算精度，對慣性器件實現(xiàn)精確的標(biāo)定和補償是最普遍的方法。

傳統(tǒng)IMU的標(biāo)定通過三軸慣導(dǎo)測試轉(zhuǎn)臺和精密離心機實現(xiàn)。在陀螺標(biāo)定過程中，IMU的坐標(biāo)系與轉(zhuǎn)臺坐標(biāo)系重合，建立標(biāo)定模型計算陀螺的零偏，通過陀螺對多個基準(zhǔn)角速度輸入的響應(yīng)，計算刻度因數(shù)，安裝耦合系統(tǒng)和非線性度等標(biāo)定系數(shù)。同樣地，通過精密離心機能夠標(biāo)定出加速度計的零偏、刻度因數(shù)和耦合誤差等標(biāo)定系數(shù)。但是，三軸慣性轉(zhuǎn)臺和精密離心機的造價高，需要特定的隔離地基，標(biāo)定過程復(fù)雜。地磁傳感器的標(biāo)定，目前無相關(guān)標(biāo)準(zhǔn)，通常采用圓周運動補償方法，使標(biāo)定對象的航向角在0～360°范圍內(nèi)變化，對地磁傳感器進行連續(xù)采樣，根據(jù)采樣點最大和最小值，推導(dǎo)刻度系數(shù)和零位偏置。這種方法只能對地磁傳感器進行定性的補償，屬于經(jīng)驗方法，不能保證補償?shù)南禂?shù)的準(zhǔn)確性。中國專利公開號CN105180968A，公開日2015年12月23日，發(fā)明創(chuàng)造的名稱為《一種IMU/磁強計安裝失準(zhǔn)角在線濾波標(biāo)定方法》，公開了一種IMU/磁強計安裝失準(zhǔn)角在線濾波標(biāo)定方法，應(yīng)用卡爾曼濾波方法得到捷聯(lián)慣導(dǎo)系統(tǒng)IMU相對磁強計安裝失準(zhǔn)角全部誤差參數(shù)；利用六面體或其它相似的可翻轉(zhuǎn)裝置即可完成現(xiàn)場標(biāo)定試驗，克服了傳統(tǒng)實驗室標(biāo)定的不足，提高了系統(tǒng)實際使用精度。但是這種方法無法避免標(biāo)定裝置的軟磁效應(yīng)和硬磁效應(yīng)對地磁傳感器標(biāo)定誤差的影響，無法獲得較高的精度和足夠的標(biāo)定參數(shù)。

技術(shù)實現(xiàn)要素：

本發(fā)明為解決現(xiàn)有慣性測量單元標(biāo)定設(shè)備成本高，標(biāo)定方法復(fù)雜，并且現(xiàn)有地磁傳感器的補償技術(shù)由于存在誤差導(dǎo)致測量精度低等問題，提供一種慣性測量單元和地磁傳感器整體標(biāo)定裝置及標(biāo)定方法。

慣性測量單元和地磁傳感器整體標(biāo)定裝置，包括雙天線GNSS/SINS組合導(dǎo)航系統(tǒng)、標(biāo)定處理系統(tǒng)、工業(yè)相機和六面體工裝，標(biāo)定對象安裝在六面體工裝中，作為標(biāo)定六面體工裝，所述標(biāo)定六面體工裝的六個表面上分別貼有不同ID的增強現(xiàn)實合作目標(biāo)；所述雙天線GNSS/SINS組合導(dǎo)航系統(tǒng)作為基準(zhǔn)組合導(dǎo)航系統(tǒng)與工業(yè)相機固連，所述基準(zhǔn)組合導(dǎo)航系統(tǒng)中IMU和工業(yè)相機安裝于兩個GNSS接收機天線的中點位置；

所述雙天線GNSS/SINS組合導(dǎo)航系統(tǒng)測量當(dāng)?shù)氐乩砦恢煤突鶞?zhǔn)坐標(biāo)系相對于當(dāng)?shù)氐乩碜鴺?biāo)系的姿態(tài)角；所述標(biāo)定處理系統(tǒng)采集雙天線GNSS/SINS組合導(dǎo)航系統(tǒng)的測量值、標(biāo)定對象中加速度、陀螺以及地磁傳感器的測量值以及工業(yè)相機采集標(biāo)定六面體工裝表面的增強現(xiàn)實合作目標(biāo)圖像信息，所述標(biāo)定處理系統(tǒng)計算當(dāng)前增強現(xiàn)實合作目標(biāo)相對相機坐標(biāo)系的姿態(tài)角；

所述標(biāo)定處理系統(tǒng)計算基準(zhǔn)坐標(biāo)系和當(dāng)?shù)氐乩碜鴺?biāo)系的方向余弦矩陣，目標(biāo)坐標(biāo)系和相機坐標(biāo)系的方向余弦矩陣；獲得載體坐標(biāo)系相對于當(dāng)?shù)氐乩碜鴺?biāo)系的方向余弦矩陣；計算所述載體坐標(biāo)系三軸傳感器的標(biāo)稱值；將標(biāo)定對象中加速度計、陀螺和地磁傳感器標(biāo)稱值和標(biāo)定對象中加速度計、陀螺和地磁傳感器的測量值建立方程組，實現(xiàn)對慣性測量單元和地磁傳感器的整體標(biāo)定。

慣性測量單元和地磁傳感器整體標(biāo)定方法，該方法由以下步驟實現(xiàn)：

步驟一、將標(biāo)定對象安裝于標(biāo)定六面體工裝內(nèi)，所述標(biāo)定六面體工裝、雙天線GNSS/SINS組合導(dǎo)航系統(tǒng)以及工業(yè)相機處于同一平面，并且所述增強現(xiàn)實合作目標(biāo)位于工業(yè)相機的視場中心位置；

步驟二、建立標(biāo)定對象中加速度計、陀螺以及磁場傳感器的標(biāo)定模型；

步驟三、在所述工業(yè)相機視場內(nèi)以任意姿態(tài)放置標(biāo)定六面體工裝，保證至少有一個面上的增強現(xiàn)實合作目標(biāo)落在工業(yè)相機的視場中；

步驟四、所述標(biāo)定處理系統(tǒng)采集基準(zhǔn)組合導(dǎo)航系統(tǒng)輸出的當(dāng)?shù)氐乩砦恢煤突鶞?zhǔn)坐標(biāo)系相對于當(dāng)?shù)氐乩碜鴺?biāo)系的姿態(tài)角，并采集標(biāo)定對象中加速度、陀螺以及地磁傳感器的測量值；所述工業(yè)相機采集所述標(biāo)定六面體工裝表面的增強現(xiàn)實合作目標(biāo)的圖像，并將所述合作目標(biāo)的圖像傳送至標(biāo)定處理系統(tǒng)，所述標(biāo)定處理系統(tǒng)計算當(dāng)前增強現(xiàn)實合作目標(biāo)相對相機坐標(biāo)系的姿態(tài)角；

步驟五、標(biāo)定處理系統(tǒng)計算基準(zhǔn)坐標(biāo)系和當(dāng)?shù)氐乩碜鴺?biāo)系的方向余弦矩陣，目標(biāo)坐標(biāo)系和相機坐標(biāo)系的方向余弦矩陣；獲得載體坐標(biāo)系相對于當(dāng)?shù)氐乩碜鴺?biāo)系的方向余弦矩陣；

步驟六、計算所述載體坐標(biāo)系三軸傳感器的標(biāo)稱值；將標(biāo)定對象中加速度計、陀螺和地磁傳感器標(biāo)稱值和步驟四獲得的標(biāo)定對象中加速度計、陀螺和地磁傳感器的測量值帶入步驟二中的標(biāo)定模型；

步驟六、判斷所述測量值是否滿足最小測量次數(shù)限制，如果是，則執(zhí)行步驟七，如果否，返回執(zhí)行步驟三；

步驟七、將標(biāo)定對象中加速度計、陀螺和地磁傳感器的標(biāo)稱值和測量值建立方程組，實現(xiàn)對慣性測量單元和地磁傳感器的整體標(biāo)定。

本發(fā)明的有益效果：本發(fā)明中將高分辨率的長焦工業(yè)相機與雙天線GNSS/SINS組合導(dǎo)航系統(tǒng)固連在一起，基準(zhǔn)組合導(dǎo)航系統(tǒng)測量地球系位置和當(dāng)?shù)氐乩硐底藨B(tài)角，查表獲得理論地磁場強度值，計算理論比力和角速度，經(jīng)過光學(xué)基準(zhǔn)傳遞，計算標(biāo)定對象載體系上標(biāo)稱值，采集被測IMU和地磁傳感器的測量值，任意放置六面體工裝，得到多組被標(biāo)定對象標(biāo)稱值和測量值，建立方程組，根據(jù)最小二乘法，求取標(biāo)定參數(shù)，完成標(biāo)定。具體優(yōu)點如下：

一、本發(fā)明采用雙天線GNSS/SINS組合導(dǎo)航系統(tǒng)作為標(biāo)定基準(zhǔn)，克服了傳統(tǒng)IMU標(biāo)定手段采用三軸轉(zhuǎn)臺和高精度離心機設(shè)備昂貴，場地受限，標(biāo)定流程和數(shù)據(jù)處理復(fù)雜的缺點。雙天線GNSS/SINS組合導(dǎo)航系統(tǒng)測量精確的地球系位置，基準(zhǔn)系相對于當(dāng)?shù)氐乩硐档淖藨B(tài)角，獲得精確計算比力、角速度和地磁場強度理論值。

二、本發(fā)明采用高分辨率相機對增強現(xiàn)實合作目標(biāo)所呈圖像進行測量，計算標(biāo)定對象與基準(zhǔn)裝置的相對位姿，采用光學(xué)方式實現(xiàn)了基準(zhǔn)傳遞，避免了傳統(tǒng)機械方式基準(zhǔn)傳遞對空間的局限。除了有效地降低了成本，還避免了直接的機械安裝，以及標(biāo)定裝置對地磁傳感器嚴(yán)重的電磁干擾，提高了地磁傳感器標(biāo)定的準(zhǔn)確性和可信度。

三、本發(fā)明提出了能夠通過光學(xué)與機器視覺方式實現(xiàn)精密測量的標(biāo)定六面體。六面體外涂敷高精度加工的二維正方形增強現(xiàn)實合作目標(biāo)，每一個平面上的合作目標(biāo)ID各不相同，將標(biāo)定對象安裝于標(biāo)定六面體中，確定二者幾何關(guān)系，同時采集標(biāo)定對象和基準(zhǔn)標(biāo)定裝置輸出，將高分辨率相機采集的增強現(xiàn)實圖像進行解碼和測量作為基準(zhǔn)傳遞媒介，采用最小二乘法求解標(biāo)定模型方程組，計算標(biāo)定參數(shù)和標(biāo)定噪聲協(xié)方差。

附圖說明

圖1為本發(fā)明所述的慣性測量單元和地磁傳感器整體標(biāo)定裝置的機械結(jié)構(gòu)示意圖；

圖2為本發(fā)明的基準(zhǔn)組合導(dǎo)航系統(tǒng)的基準(zhǔn)坐標(biāo)系與當(dāng)?shù)氐乩碜鴺?biāo)系定義示意圖；

圖3為工業(yè)相機和相機坐標(biāo)系的定義示意圖；

圖4為標(biāo)定六面體工裝和載體坐標(biāo)系定義示意圖；

圖5為增強現(xiàn)實合作目標(biāo)和目標(biāo)坐標(biāo)系定義示意圖；

圖6為本發(fā)明所述的慣性測量單元和地磁傳感器整體標(biāo)定裝置的電路結(jié)構(gòu)示意圖；

圖7為本發(fā)明所述的慣性測量單元和地磁傳感器整體標(biāo)定方法的流程圖。

具體實施方式

具體實施方式一、結(jié)合圖1至圖6說明本實施方式，慣性測量單元和地磁傳感器整體標(biāo)定裝置，結(jié)合圖1說明本實施方式，包括一套雙天線GNSS/SINS高精度組合導(dǎo)航系統(tǒng)，一個長焦高分辨率工業(yè)相機2和一個標(biāo)定六面體工裝4。六面體工裝的各面上涂敷有不同ID的高精度的二維正方形增強現(xiàn)實合作目標(biāo)。高精度組合導(dǎo)航系統(tǒng)與高分辨率工業(yè)相機通過金屬板桿5固定，天線之間的基線長度保證1.5m以上，以保證基準(zhǔn)航向角的噪聲保持在較小水平。所述雙天線GNSS/SINS高精度組合導(dǎo)航系統(tǒng)作為地球標(biāo)稱物理量與當(dāng)?shù)氐乩硐底儞Q的基準(zhǔn)測試設(shè)備，包括兩個GNSS接收機天線(天線1和天線3)，GNSS接收機，基準(zhǔn)IMU和導(dǎo)航計算機。高精度組合導(dǎo)航系統(tǒng)作為標(biāo)定基準(zhǔn)，要求其采用的基準(zhǔn)IMU的零偏和噪聲水平至少優(yōu)于被標(biāo)定IMU的精度一個量級。組合導(dǎo)航系統(tǒng)IMU和工業(yè)相機2安裝于兩個天線的中點位置附近，二者質(zhì)心位置應(yīng)盡量集中，且要求較高的安裝精度，以保證基準(zhǔn)坐標(biāo)系與相機坐標(biāo)系的方向余弦矩陣準(zhǔn)確。工業(yè)相機采集標(biāo)定六面體工裝的合作目標(biāo)圖像，確定標(biāo)定對象在相機坐標(biāo)系中的位姿，結(jié)合機械結(jié)構(gòu)確定的基準(zhǔn)坐標(biāo)系與相機坐標(biāo)系的關(guān)系，標(biāo)定對象IMU與六面體的關(guān)系，完成由地球坐標(biāo)系到載體坐標(biāo)系的基準(zhǔn)傳遞。

結(jié)合圖2說明本實施方式，基準(zhǔn)坐標(biāo)系定義O_BX_BY_BZ_B指向基準(zhǔn)IMU的三個敏感軸方向，它與當(dāng)?shù)氐乩碜鴺?biāo)系NED的關(guān)系通過滾轉(zhuǎn)角，俯仰角和航向角三個歐拉角表示(φ,θ,ψ)^T，當(dāng)?shù)氐乩碜鴺?biāo)系三個坐標(biāo)軸分別指向北向，東向和地向。獲得基準(zhǔn)坐標(biāo)坐標(biāo)系與當(dāng)?shù)氐乩碜鴺?biāo)系的方向余弦矩陣為，

結(jié)合圖3說明本實施方式，圖3為相機坐標(biāo)系定義O_CX_CY_CZ_C。O_C為光心，O_CX_C和O_CY_C分別平行于成像平面的兩條邊，O_CZ_C為深度方向。結(jié)合圖1、圖2和圖3可確定相機坐標(biāo)系與基準(zhǔn)坐標(biāo)系的轉(zhuǎn)換方向余弦矩陣：

圖4為載體坐標(biāo)系。假設(shè)IMU/地磁傳感器測量模塊的三個敏感軸方向一致，載體坐標(biāo)系O_bX_bY_bZ_b指向標(biāo)定對象中IMU的三個敏感軸方向，標(biāo)定對象安裝于標(biāo)定六面體工裝中，因此可通過六面體定義載體坐標(biāo)系。

結(jié)合圖5，圖5為增強現(xiàn)實合作目標(biāo)的目標(biāo)坐標(biāo)系O_TX_TY_TZ_T，增強現(xiàn)實合作目標(biāo)通過內(nèi)部二維正方形表征唯一的ID，且可以表示矢量方向，因此增強現(xiàn)實合作目標(biāo)能夠定義唯一的坐標(biāo)系。所述的增強現(xiàn)實合作目標(biāo)為AprilTagTag.36h11系列中ID為16的合作目標(biāo)，將此合作目標(biāo)定義的坐標(biāo)系表示成六面體的其它五個平面分別采用不同ID的圖形，表示成這樣能夠確定六面體工裝每一個平面與載體坐標(biāo)系的關(guān)系由圖5和圖6可知，目標(biāo)坐標(biāo)系與載體坐標(biāo)系的方向余弦矩陣為：

所述的增強現(xiàn)實合作目標(biāo)的圖形可以為其他形式，例如，ARTag或QR Code等。除此之外，標(biāo)定板必須嚴(yán)格保證為正方形，并且邊長須精密測量。標(biāo)定裝置的高分辨率工業(yè)相機，通過對六面體工裝上增強現(xiàn)實合作目標(biāo)的圖像進行預(yù)處理，閾值處理，邊緣檢測，圖像分割，四邊形提取，識別出唯一確定ID的增強現(xiàn)實圖案，根據(jù)目標(biāo)、圖像和焦距三者之間的關(guān)系計算標(biāo)定對象中IMU/地磁傳感器相對于基準(zhǔn)坐標(biāo)系的位置和姿態(tài)。

結(jié)合圖6說明本實施方式，標(biāo)定處理系統(tǒng)用于采集基準(zhǔn)組合導(dǎo)航系統(tǒng)輸出的位置和姿態(tài)角；采集高分率工業(yè)相機輸出的增強現(xiàn)實合作目標(biāo)的圖像，并計算合作目標(biāo)相對于相機坐標(biāo)系的位姿；采集被測IMU輸出的比力和角速度以及地磁傳感器輸出的地磁場強度；電源根據(jù)各用電單元的電壓供電；顯示器用于與用戶交互并提示標(biāo)定進程；存儲單元記錄存儲結(jié)果。

所述的標(biāo)定處理系統(tǒng)可為DSP，ARM等嵌入式計算機，也可為工控機或PC機。各采樣接口根據(jù)選擇器件的實際接口，對標(biāo)定處理系統(tǒng)進行電路接口設(shè)計或選擇對應(yīng)的工業(yè)采集卡。

具體實施方式二、結(jié)合圖7說明本實施方式，本實施方式為具體實施方式一所述的慣性測量單元和地磁傳感器整體標(biāo)定裝置的標(biāo)定方法，其具體標(biāo)定過程如下：

一、將基準(zhǔn)導(dǎo)航系統(tǒng)與標(biāo)定六面體放置在同一水平面之上，為保證測量精度，盡量減小相機與六面體間的距離；

二、建立被測對象傳感器的標(biāo)定模型，

加速度計的模型為

其中，為標(biāo)定后比力；為加速度計原始輸出的比力；b_a為加速度計零偏，w_a表示加速度計噪聲水平；K_a為加速度計刻度因數(shù)與安裝耦合系數(shù)矩陣。

陀螺的標(biāo)定模型為

其中，為標(biāo)定后陀螺角速度；為陀螺原始輸出的角速度；b_g為陀螺零偏，w_g表示陀螺噪聲水平；K_g為陀螺刻度因數(shù)與安裝耦合系數(shù)矩陣。

地磁傳感器的標(biāo)定模型為

其中，為標(biāo)定后地磁場強度，根據(jù)當(dāng)?shù)氐乩砦恢貌楸碛嬎闱笕。粸榈卮艂鞲衅髟驾敵龅牡卮艌鰪姸?；b_h為當(dāng)前條件下地磁傳感器偏置，ω_h表示地磁傳感器噪聲水平；K_h為地磁傳感器刻度因數(shù)與安裝耦合系數(shù)矩陣。

三、以任意姿態(tài)放置標(biāo)定對象的六面體工裝，但必須保證至少一個平面的合作目標(biāo)完整的出現(xiàn)在工業(yè)相機的視場中；

四、采集基準(zhǔn)組合導(dǎo)航系統(tǒng)輸出的當(dāng)?shù)氐乩砦恢?，記?L₀ λ₀ h₀)，根據(jù)WMM(World Magnetic Model)能夠計算當(dāng)?shù)卮艌鰪姸群彤?dāng)?shù)氐乩碜鴺?biāo)系三軸磁場強度分量，記作分別表示北向，東向和地向的磁場強度。采集高分辨率工業(yè)相機測量的六面體工裝上第i個平面合作目標(biāo)的姿態(tài)角，記為其中i表示工裝六個面中的第i個平面，并計算相機坐標(biāo)系的方向余弦矩陣計算載體坐標(biāo)系相對當(dāng)?shù)氐乩碜鴺?biāo)系的方向余弦矩陣；

因此，載體坐標(biāo)系三軸傳感器的標(biāo)稱值為

五、計算載體坐標(biāo)系中標(biāo)定對象的比力和角速度的理論標(biāo)稱值。在當(dāng)?shù)氐乩碜鴺?biāo)系中，加速度計輸出的比力fⁿ和陀螺輸出的加速度的理論標(biāo)稱值分別為

fⁿ＝(0 0 1)^T

ωⁿ＝(0 0 7.292115×10^-5)^T

其中，fⁿ無量綱，ωⁿ的單位為rad/s。因此，根據(jù)當(dāng)?shù)氐乩碜鴺?biāo)系與載體坐標(biāo)系的轉(zhuǎn)換關(guān)系則可確定載體坐標(biāo)系內(nèi)加速度計和陀螺的輸出的理論標(biāo)稱值，記作和

六、采集標(biāo)定對象中加速度計原始輸出的比力陀螺原始輸出的角速度和地磁傳感器原始輸出的地磁場強度的測量值；

七、將標(biāo)定對象的標(biāo)稱值和測量值帶入步驟二中的標(biāo)定模型，即具體如下：加速度計標(biāo)定方程：

陀螺標(biāo)定方程：

地磁傳感器標(biāo)定方程：

上述方程共有九個方程，36個未知數(shù)，因此至少需要四組測量值才能求解；

八、為了使獲得較高的標(biāo)定精度，至少保證六面體工裝的每一個面至少有三個姿態(tài)角位置被工業(yè)相機采樣。這樣至少有18組量測值，充分保證標(biāo)定參數(shù)的精度。根據(jù)最小二乘法，求解各傳感器標(biāo)定參數(shù)。

加速度計標(biāo)定方程可以寫成：

陀螺標(biāo)定方程可以寫成：

地磁傳感器標(biāo)定方程可以寫成：

其中，j∈[1,N]，N表示對特定的增強現(xiàn)實合作目標(biāo)的角位置變換測量的次數(shù)。令

根據(jù)最小二乘法分別計算加速度計、陀螺和地磁場傳感器標(biāo)定系數(shù)矩陣的估計值，

加速度計、陀螺和地磁場傳感器噪聲的協(xié)方差矩陣分別為：

本實施方式實現(xiàn)了慣性測量單元與地磁傳感器整體快速標(biāo)定，以光學(xué)手段實現(xiàn)標(biāo)定對象與基準(zhǔn)裝置的基準(zhǔn)傳遞，避免了地磁傳感器與標(biāo)定設(shè)備直接接觸引起的電磁效應(yīng)，本發(fā)明操作簡單，無須專業(yè)實驗室。