一種姿態(tài)測(cè)量系統(tǒng)的精度校準(zhǔn)方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及測(cè)量技術(shù)領(lǐng)域，尤其涉及一種姿態(tài)測(cè)量系統(tǒng)的精度校準(zhǔn)方法。
【背景技術(shù)】
[0002] 姿態(tài)測(cè)量系統(tǒng)是一種可以測(cè)量物體空間姿態(tài)（俯仰角、橫滾角、航向角）的一系 列設(shè)備，在工業(yè)上的許多領(lǐng)域有著廣泛的應(yīng)用，近年來(lái)，隨著硬件成本的不斷下降，各種類(lèi) 型的姿態(tài)測(cè)量系統(tǒng)開(kāi)始走進(jìn)千家萬(wàn)戶的日常生活之中，以手機(jī)為例，目前大部分智能手機(jī) 都內(nèi)置了加速度計(jì)、陀螺儀和電子羅盤(pán)，構(gòu)成了一個(gè)簡(jiǎn)易的低成本姿態(tài)測(cè)量系統(tǒng)。然而，目 前大部分姿態(tài)測(cè)量系統(tǒng)中，由于成本的限制，系統(tǒng)內(nèi)置傳感器的精度和穩(wěn)定性都不高，從而 導(dǎo)致了整個(gè)姿態(tài)測(cè)量系統(tǒng)的測(cè)量結(jié)果不理想。在不改變系統(tǒng)硬件的情況下對(duì)傳感器進(jìn)行校 準(zhǔn)，無(wú)疑是提升整個(gè)系統(tǒng)測(cè)量精度的一種極為實(shí)用的方式。因此，研究低成本姿態(tài)測(cè)量系統(tǒng) 的校準(zhǔn)方法具有極強(qiáng)的現(xiàn)實(shí)應(yīng)用價(jià)值。
[0003] 傳感器的校準(zhǔn)與標(biāo)定一直是傳感器技術(shù)領(lǐng)域內(nèi)的一個(gè)重要課題，對(duì)于姿態(tài)測(cè)量系 統(tǒng)的校準(zhǔn)方案，國(guó)內(nèi)外學(xué)者也有大量的相關(guān)研究。孫偉、付心如等研究了利用速率轉(zhuǎn)臺(tái)進(jìn)行 MEMS慣導(dǎo)的多位置標(biāo)定方法；馬斌良等學(xué)者提出了一種在有角度基準(zhǔn)條件下基于傅里葉 變換的電子羅盤(pán)校準(zhǔn)方案；秦偉等學(xué)者研究了基于神經(jīng)網(wǎng)絡(luò)和UKF(無(wú)跡卡爾曼濾波）的姿 態(tài)測(cè)量系統(tǒng)在線標(biāo)定技術(shù)。
[0004] 目前的研究大多數(shù)集中在利用轉(zhuǎn)臺(tái)等校準(zhǔn)器械對(duì)姿態(tài)測(cè)量系統(tǒng)進(jìn)行校準(zhǔn)。然而， 校準(zhǔn)專(zhuān)用器械價(jià)格昂貴，且其中大部分器械操作復(fù)雜，導(dǎo)致生產(chǎn)成本較高；另一方面，在實(shí) 際應(yīng)用中，隨著周?chē)h(huán)境的變化和傳感器本身的老化，系統(tǒng)內(nèi)傳感器的特性會(huì)不斷變化，僅 僅依靠出廠前的校準(zhǔn)在實(shí)際應(yīng)用中難以起到很好的效果。另外，目前一部分關(guān)于非器械校 準(zhǔn)的研究中，其校準(zhǔn)方案要么太過(guò)簡(jiǎn)易，對(duì)實(shí)際測(cè)量精度的提升幫助不大；要么過(guò)于復(fù)雜， 對(duì)使用人員的相關(guān)技能有著較高的要求。

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

[0005] 鑒于目前姿態(tài)校準(zhǔn)存在的上述不足，本發(fā)明提供一種姿態(tài)測(cè)量系統(tǒng)的精度校準(zhǔn) 方法，校準(zhǔn)結(jié)果可靠、精度高、校準(zhǔn)耗時(shí)少。
[0006] 為達(dá)到上述目的，本發(fā)明的實(shí)施例采用如下技術(shù)方案：
[0007] -種姿態(tài)測(cè)量系統(tǒng)的精度校準(zhǔn)方法，所述姿態(tài)測(cè)量系統(tǒng)的精度校準(zhǔn)方法包括如下 步驟：
[0008] 通過(guò)橢球擬合模型對(duì)姿態(tài)測(cè)量系統(tǒng)的加速度計(jì)的零偏、刻度系數(shù)與軸間不正交角 進(jìn)行校準(zhǔn)；
[0009] 利用計(jì)算出的橢球參數(shù)對(duì)加速度計(jì)原始數(shù)據(jù)進(jìn)行補(bǔ)償；
[0010] 根據(jù)補(bǔ)償后的加速度計(jì)數(shù)據(jù)通過(guò)橢球擬合模型對(duì)電子羅盤(pán)進(jìn)行準(zhǔn)；
[0011] 利用計(jì)算出的橢球參數(shù)對(duì)電子羅盤(pán)原始數(shù)據(jù)進(jìn)行補(bǔ)償；
[0012] 根據(jù)補(bǔ)償后的加速度計(jì)數(shù)據(jù)與電子羅盤(pán)數(shù)據(jù)對(duì)姿態(tài)進(jìn)行解算。
[0013] 依照本發(fā)明的一個(gè)方面，所述通過(guò)橢球擬合模型對(duì)姿態(tài)測(cè)量系統(tǒng)的加速度計(jì)的零 偏、刻度系數(shù)與軸間不正交角進(jìn)行校準(zhǔn)步驟執(zhí)行前執(zhí)行以下步驟：對(duì)加速度計(jì)進(jìn)行水平校 準(zhǔn)，消除加速度計(jì)原始零偏。
[0014] 依照本發(fā)明的一個(gè)方面，所述對(duì)加速度計(jì)進(jìn)行水平校準(zhǔn)，消除加速度計(jì)原始零偏 步驟執(zhí)行后執(zhí)行以下步驟：采集加速度計(jì)在一段時(shí)間內(nèi)的三軸數(shù)據(jù)，其中，三軸數(shù)據(jù)標(biāo)記為 [八)!0為0為0]1'，零偏標(biāo)記為8=[八 )!0為0為0]1'-[008]1'。。
[0015] 依照本發(fā)明的一個(gè)方面，所述通過(guò)橢球擬合模型對(duì)姿態(tài)測(cè)量系統(tǒng)的加速度計(jì)的零 偏、刻度系數(shù)與軸間不正交角進(jìn)行校準(zhǔn)步驟包括以下步驟：
[0016] 設(shè)定擬合橢球參數(shù)向量，標(biāo)記為a =[abcdefklmn]T;
[0017] 設(shè)定校準(zhǔn)過(guò)程迭代次數(shù)為^將加速度采集的第一個(gè)值以^^^^拓展為⑴維 列向量D1，并計(jì)算矩陣S1，其中，D1標(biāo)記為 1]T，SI = D1TD1 ;
[0018] 若加速度計(jì)三軸數(shù)據(jù)的采集次數(shù)未達(dá)到η次，則用新采集的加速度計(jì)數(shù)據(jù)DK對(duì)矩 陣S進(jìn)行更新，其中，Sk= S k
[0019] 計(jì)算出矩陣S后，同時(shí)引入橢球限制矩陣C并將矩陣S分塊，其中，
[0027] 通過(guò)以下公式求得橢球擬合向量：
[0028] C! %τ) α 1= λ a i
[0029] α 2= -S4 %τα 1;
[0030] 計(jì)算出<^后，通過(guò)上述公式計(jì)算出α 2，則橢球擬合向量的所有參數(shù)全部被計(jì)算 出，其中，α = [α 1 α 2] = [abcdefklm η]τ;
[0031] 再根據(jù)實(shí)際的物理對(duì)應(yīng)關(guān)系，三軸的刻度系數(shù)kx，ky，k z、軸間不正交角 sxy，δ?ζ，δχζ及殘余零偏bx,b y,bz可以通過(guò)以下公式計(jì)算得出：

[0035] 依照本發(fā)明的一個(gè)方面，所述通過(guò)橢球擬合模型對(duì)姿態(tài)測(cè)量系統(tǒng)的加速度計(jì)的零 偏、刻度系數(shù)與軸間不正交角進(jìn)行校準(zhǔn)步驟執(zhí)行后執(zhí)行以下步驟：當(dāng)采集到新的加速度計(jì) 數(shù)據(jù)后，可以用橢球參數(shù)對(duì)新采集到的數(shù)據(jù)進(jìn)行修正。
[0036] 依照本發(fā)明的一個(gè)方面，所述根據(jù)補(bǔ)償后的加速度計(jì)數(shù)據(jù)通過(guò)橢球擬合模型對(duì)電 子羅盤(pán)進(jìn)行準(zhǔn)步驟包括如下步驟：
[0037] 設(shè)定擬合橢球參數(shù)向量，標(biāo)記為a =[abcdefklmn]T;
[0038] 采集電子羅盤(pán)在一段時(shí)間內(nèi)的三軸數(shù)據(jù)，并對(duì)采集的數(shù)據(jù)進(jìn)行運(yùn)算防溢處理，其 中，三軸數(shù)據(jù)標(biāo)記為[Hx。Ηγ。Ηζο]τ;
[0039] 設(shè)定校準(zhǔn)過(guò)程迭代次數(shù)為n，將加速度采集的第一個(gè)值[HX1 HY1 HZ1]T拓展為10維 列向量D1，并計(jì)算矩陣S1，其中，D1 標(biāo)記為Dl= [Hxl2 Hyl2 Hzl2 HxlHyl HxlHzl HylHzl Hxl Hyl Ηζι 1]T，SI = D1TD1 ;
[0040] 若電子羅盤(pán)三軸數(shù)據(jù)的采集次數(shù)未達(dá)到n次，則用新采集的電子羅盤(pán)數(shù)據(jù)DK對(duì)矩 陣S進(jìn)行更新，其中，Sk= S k
[0041] 計(jì)算出矩陣S后，同時(shí)引入橢球限制矩陣C并將矩陣S分塊，其中，
[0049] 通過(guò)以下公式求得橢球擬合向量：
[0052] 計(jì)算出α肩，通過(guò)上述公式計(jì)算出α 2，則橢球擬合向量的所有參數(shù)全部被計(jì)算 出，其中，α = [α 1 α 2] = [abcdefklm η]τ;
[0053] 再根據(jù)實(shí)際的物理對(duì)應(yīng)關(guān)系，三軸的刻度系數(shù)kx, ky, kz、軸間不正交角 Sxy，δχζ及殘余零偏bx,by,b z可以通過(guò)以下公式計(jì)算得出：
[0057] 依照本發(fā)明的一個(gè)方面，所述根據(jù)補(bǔ)償后的加速度計(jì)數(shù)據(jù)通過(guò)橢球擬合模型對(duì)電 子羅盤(pán)進(jìn)行準(zhǔn)步驟執(zhí)行后執(zhí)行以下步驟：當(dāng)采集到新的電子羅盤(pán)數(shù)據(jù)后，可以用橢球參數(shù) 對(duì)新采集到的數(shù)據(jù)進(jìn)行修正。
[0058] 依照本發(fā)明的一個(gè)方面，所述根據(jù)補(bǔ)償后的加速度計(jì)數(shù)據(jù)與電子羅盤(pán)數(shù)據(jù)對(duì)姿態(tài) 進(jìn)行解算步驟執(zhí)行后執(zhí)行以下步驟：電子羅盤(pán)在測(cè)量過(guò)程中利用測(cè)量數(shù)據(jù)對(duì)橢球參數(shù)進(jìn)行 更新，提高橢球參數(shù)的準(zhǔn)確性。
[0059] 依照本發(fā)明的一個(gè)方面，所述電子羅盤(pán)在測(cè)量過(guò)程中利用測(cè)量數(shù)據(jù)對(duì)橢球參數(shù)進(jìn) 行更新，提高橢球參數(shù)的準(zhǔn)確性步驟執(zhí)行前執(zhí)行以下步驟：通過(guò)統(tǒng)計(jì)規(guī)則的方式剔除采集 的電子羅盤(pán)數(shù)據(jù)中出現(xiàn)跳變的計(jì)算點(diǎn)。
[0060] 本發(fā)明實(shí)施的優(yōu)點(diǎn)：通過(guò)采用了橢球擬合的方式對(duì)姿態(tài)測(cè)量系統(tǒng)中的電子羅盤(pán)和 加速度進(jìn)行校準(zhǔn)，可有效地避免傳感器偶然輸出異常、跳數(shù)等問(wèn)題，使得校準(zhǔn)結(jié)果的可靠性 得到了有效的保證；對(duì)橢球擬合算法進(jìn)行了遞推化處理，使得姿態(tài)測(cè)量系統(tǒng)內(nèi)部CPU在處 理數(shù)據(jù)時(shí)不需要存儲(chǔ)傳感器采集的過(guò)往所有點(diǎn)的數(shù)據(jù)，而僅需要存儲(chǔ)之前遞推得到的矩陣 和當(dāng)次的測(cè)量結(jié)果即可，這使得在相同的硬件環(huán)境下，該橢球擬合方法可以處理更多的數(shù) 值，從而提高橢球擬合的精度。相對(duì)應(yīng)的，采用傳統(tǒng)擬合算法時(shí)，CPU為了確保運(yùn)算過(guò)程中內(nèi) 存棧不會(huì)溢出，只能根據(jù)少量的樣本點(diǎn)做擬合運(yùn)算，因而擬合精度有限；對(duì)橢球擬合算法中 大規(guī)模的矩陣運(yùn)算進(jìn)行了改進(jìn)，優(yōu)化了其中的計(jì)算流程，使得橢球擬合的計(jì)算量減少90% 以上，以Matlab程序?yàn)槔?，按照傳統(tǒng)算法，運(yùn)算一次擬合算法耗時(shí)563ms ;相對(duì)地，采用改 進(jìn)后的算法耗時(shí)僅為14ms ;采用了利用測(cè)量數(shù)據(jù)進(jìn)行校準(zhǔn)的策略，提高了校準(zhǔn)時(shí)的容錯(cuò)能 力，從而降低了初始校準(zhǔn)時(shí)的要求，減少了使用人員的操作復(fù)雜程度。
【附圖說(shuō)明】
[0061] 為了更清楚地說(shuō)明本發(fā)明實(shí)施例中的技術(shù)方案，下面將對(duì)實(shí)施例中所需要使用 的附圖作簡(jiǎn)單地介紹，顯而易見(jiàn)地，下面描述中的附圖僅僅是本發(fā)明的一些實(shí)施例，對(duì)于本 領(lǐng)域普通技術(shù)人員來(lái)講，在不付出創(chuàng)造性勞動(dòng)的前提下，還可以根據(jù)這些附圖獲得其他的 附圖。
[0062] 圖1為本發(fā)明所述的一種姿態(tài)測(cè)量系統(tǒng)的精度校準(zhǔn)方法的實(shí)施例1的方法流程 圖；
[0063] 圖2為本發(fā)明所述的一種姿態(tài)測(cè)量系統(tǒng)的精度校準(zhǔn)方法的實(shí)施例2的方法流程 圖；
[0064] 圖3為本發(fā)明所述的一種姿態(tài)測(cè)量系統(tǒng)的精度校準(zhǔn)方法的實(shí)施例3的方法流程 圖；
[0065]圖4為本發(fā)明所述的一種姿態(tài)測(cè)量系統(tǒng)的精度校準(zhǔn)方法的實(shí)施例4的方法流程 圖；
[0066]圖5為本發(fā)明所述的一種姿態(tài)測(cè)量系統(tǒng)的精度校準(zhǔn)方法的實(shí)施例5的方法流程 圖；
[0067]圖6為本發(fā)明所述的一種姿態(tài)測(cè)量系統(tǒng)的精度校準(zhǔn)方法的實(shí)施例6的方法流程 圖；
[0068]圖7為本發(fā)明所述的一種姿態(tài)測(cè)量系統(tǒng)的精度校準(zhǔn)方法的實(shí)施例7的方法流程 圖。
【具體實(shí)施方式】
[0069] 下面將結(jié)合本發(fā)明實(shí)施例中的附圖，對(duì)本發(fā)明實(shí)施例中的技術(shù)方案進(jìn)行清楚、完 整地描述，顯然，所描述的實(shí)施例僅僅是本發(fā)明一部分實(shí)施例，而不是全部的實(shí)施例?；?本發(fā)明中的實(shí)施例，本領(lǐng)域普通技術(shù)人員在沒(méi)有做出創(chuàng)造性勞動(dòng)前提下所獲得的所有其他 實(shí)施例，都屬于本發(fā)明保護(hù)的范圍。
[0070] 實(shí)施例1 :
[0071] 如圖1所示，一種姿態(tài)測(cè)量系統(tǒng)的精度校準(zhǔn)方法，所述姿態(tài)測(cè)量系統(tǒng)的精度校準(zhǔn) 方法包括如下步驟：
[0072] 步驟S1 :通過(guò)橢球擬合模型對(duì)姿態(tài)測(cè)量系統(tǒng)的加速度計(jì)的零偏、刻度系數(shù)與軸間 不正交角進(jìn)行校準(zhǔn)；
[0073] 所述步驟S1 :通過(guò)橢球擬合模型對(duì)姿態(tài)測(cè)量系統(tǒng)的加速度計(jì)的零偏、刻度系數(shù)與 軸間不正交角進(jìn)行校準(zhǔn)步驟包括以下步驟：
[0074] 設(shè)定擬合橢球參數(shù)向量，標(biāo)記為a =[abcdefklmn]T;
[0075] 設(shè)定校準(zhǔn)過(guò)程迭代次數(shù)為^將加速度采集的第一個(gè)值以^^^^拓展為⑴維 列向量D1，并計(jì)算矩陣S1，其中，D1 標(biāo)記為Dl= [Axl2 Ayl2 Azl2 AxlAyl AxlAzl AylAzl Axl Ayl Azl1]T，SI = D1TD1 ;
[0076] 若加速度計(jì)三軸數(shù)據(jù)的采集次數(shù)未達(dá)到n次，則用新采集的加速度計(jì)數(shù)據(jù)DK對(duì)矩 陣S進(jìn)行更新，其中，Sk= S k
[0077] 計(jì)算出矩陣S后，同時(shí)引入橢球限制矩陣C并將矩陣S分塊，其中，
[0078]
[0080] 將矩陣S分塊：
[0085] 通過(guò)以下公式求得橢球擬合向量：
[0088] 計(jì)算出α肩，通過(guò)上述公式計(jì)算出α 2，則橢球擬合向量的所有參數(shù)全部被計(jì)算 出，其中，α = [α 1 α 2] = [abcdefklm η]τ;
[0089] 再根據(jù)實(shí)際的物理對(duì)應(yīng)關(guān)系，三軸的刻度系數(shù)kx, ky, kz、軸間不正交角 S