本發(fā)明涉及傳感器
技術(shù)領(lǐng)域：
，具體地，涉及一種基于序貫濾波的陀螺與磁傳感器聯(lián)合標(biāo)定方法。
背景技術(shù)：
：陀螺和磁傳感器(后者又名磁力儀、磁強(qiáng)計(jì))經(jīng)常用于姿態(tài)確定或科學(xué)測量領(lǐng)域。陀螺敏感載體的角速度，磁力儀敏感環(huán)境磁場。當(dāng)磁力儀處于鐵磁材料附近時(shí)，磁力儀周圍的磁場受到扭曲，不能正確測量出磁場強(qiáng)度。磁場干擾可分為硬磁效應(yīng)和軟磁效應(yīng)兩種。硬磁效應(yīng)是由永磁鐵或電流產(chǎn)生的加性磁干擾，而軟磁效應(yīng)由軟磁材料誘導(dǎo)產(chǎn)生，處于背景磁場中的軟磁材料會(huì)誘導(dǎo)產(chǎn)生出自身的磁場，并對(duì)背景磁場的強(qiáng)度和方向產(chǎn)生扭曲。除此之外，因制造工藝不完善，磁力儀還存在標(biāo)度因數(shù)、敏感軸交叉耦合和偏置等誤差，因此，在使用磁力儀之前，必須對(duì)以上誤差進(jìn)行標(biāo)校，即進(jìn)行磁力儀內(nèi)部標(biāo)定。當(dāng)磁力儀與陀螺一起使用時(shí)，還必須進(jìn)行磁力儀外部標(biāo)定，即標(biāo)定磁力儀與陀螺之間的坐標(biāo)系失準(zhǔn)角。軟磁效應(yīng)不僅會(huì)引起磁力儀內(nèi)部參數(shù)的變化，也會(huì)導(dǎo)致磁力儀與陀螺的坐標(biāo)系失準(zhǔn)角發(fā)生改變。因此，在使用之前，磁力儀內(nèi)部標(biāo)定和外部標(biāo)定都需要進(jìn)行。目前存在的磁力儀標(biāo)定方法或要求磁力儀內(nèi)部標(biāo)定和外部標(biāo)定先后分別進(jìn)行，或要求在靜態(tài)條件下采集加速度計(jì)信息。另一方面，低成本陀螺(如MEMS陀螺)的零偏誤差較大，而且每次使用時(shí)都有所變化，若不做補(bǔ)償直接使用陀螺測量值，將影響磁力儀外部標(biāo)定的效果。技術(shù)實(shí)現(xiàn)要素：針對(duì)現(xiàn)有技術(shù)中的缺陷，本發(fā)明的目的是提供一種基于序貫濾波的陀螺與磁傳感器聯(lián)合標(biāo)定方法。根據(jù)本發(fā)明提供的基于序貫濾波的陀螺與磁傳感器聯(lián)合標(biāo)定方法，包括如下步驟：步驟1：建立聯(lián)合標(biāo)定問題的狀態(tài)空間模型；步驟2：采用序貫濾波方法估計(jì)出狀態(tài)空間模型的狀態(tài)。優(yōu)選地，所述步驟1中聯(lián)合標(biāo)定問題的狀態(tài)空間模型如下：系統(tǒng)方程：C·bi=Cbi(ωibb-ϵ-ng)×]]>ϵ·=nϵ]]>S·=0]]>h·=0]]>m·i=nmi]]>其中：陀螺/加速度計(jì)的體坐標(biāo)系表示為b系，慣性坐標(biāo)系表示為i系，即初始時(shí)刻的b系；式中：表示慣性坐標(biāo)系相對(duì)于體坐標(biāo)系的姿態(tài)矩陣，表示的時(shí)間導(dǎo)數(shù)，表示陀螺測量的b系下的角速度矢量，ε表示陀螺零偏，表示ε的時(shí)間導(dǎo)數(shù)，ng表示陀螺測量的高斯噪聲，nε表示陀螺零偏的高斯噪聲，矩陣S表示磁力儀參數(shù)矩陣，表示矩陣S的時(shí)間導(dǎo)數(shù)，向量h表示磁力儀零偏，表示h的時(shí)間導(dǎo)數(shù)，mi為i系下表示的恒定磁場向量，表示mi的時(shí)間導(dǎo)數(shù)，nmi表示因地球自轉(zhuǎn)引起的磁場向量誤差；其中：運(yùn)算a×表示由一個(gè)三維向量a＝[a1a2a3]T構(gòu)成的叉乘矩陣，具體地，a×的展開式如下：a×=0-a3a2a30-a1-a2a10;]]>觀測方程：ym=SCibmi+h+nm]]>式中：ym表示磁力儀的測量值，為姿態(tài)矩陣的轉(zhuǎn)置，即上標(biāo)T表示向量的轉(zhuǎn)置運(yùn)算，nm表示磁力儀測量值的高斯噪聲。優(yōu)選地，步驟1中的狀態(tài)空間模型的狀態(tài)包括姿態(tài)、磁力儀內(nèi)部參數(shù)、磁力儀外部參數(shù)、陀螺零偏和磁場向量；磁場向量mi的初始值包括：磁力儀的首次測量值；磁力儀參數(shù)矩陣S的初始值包括：單位矩陣或事先給定；陀螺零偏ε和磁力儀零偏h的初始值選為零；因狀態(tài)可觀性的關(guān)系，姿態(tài)的初始值確定為單位矩陣，對(duì)應(yīng)的初始方差矩陣為零。優(yōu)選地，所述步驟2包括：采用序貫濾波方法估計(jì)出狀態(tài)空間模型的狀態(tài)，所述序貫濾波方法包括：擴(kuò)展卡爾曼濾波，或者粒子濾波法；具體地，采用擴(kuò)展卡爾曼濾波時(shí)，包括如下步驟：步驟2.1：定義狀態(tài)誤差δx為估計(jì)值減去真值x，即δ表示對(duì)應(yīng)狀態(tài)的誤差，而姿態(tài)估計(jì)值與姿態(tài)真值及姿態(tài)誤差ψ的關(guān)系定義為則相應(yīng)的狀態(tài)誤差向量表示為δx≡[ψTδεTvecT(δS)δhTδmiT]T，vec(·)表示將矩陣按照列的順序拼接起來的運(yùn)算；ψT表示姿態(tài)誤差ψ的轉(zhuǎn)置，εT表示陀螺零偏向量ε的轉(zhuǎn)置，hT、miT分別表示h、mi的轉(zhuǎn)置；狀態(tài)誤差的近似線性狀態(tài)空間模型按如下給出δx·=Fδx+Gwδym=Hmδx+nm]]>其中：噪聲F=03×3-Cbi03×15018×3018×3018×15,G=-Cbi03×303×303×3I303×3012×3012×3012×303×303×3I3,]]>Hm=[-SCib(mi)×03×3(Cibmi)T⊗I3I3SCib]]]>式中：表示δx的時(shí)間導(dǎo)數(shù)，矩陣F、G、Hm分別表示系統(tǒng)矩陣、系統(tǒng)輸入矩陣、觀測矩陣；I表示單位矩陣，I的下標(biāo)表示單位矩陣的階數(shù)；0表示零矩陣，0的下標(biāo)表示零矩陣的行列數(shù)；符號(hào)表示矩陣與矩陣的Kronecker乘積；步驟2.2：不失一般性，假定當(dāng)?shù)卮艌鱿蛄康拇笮?，對(duì)擴(kuò)展卡爾曼濾波的結(jié)果進(jìn)行如下尺度調(diào)整；調(diào)整公式如下：Srs＝||mi||S，mirs＝mi/||mi||；式中：Srs表示尺度調(diào)整后的磁力儀矩陣，mirs表示尺度調(diào)整后的磁場向量，||·||表示取模運(yùn)算。與現(xiàn)有技術(shù)相比，本發(fā)明具有如下的有益效果：本發(fā)明中磁力儀與陀螺固定連接，在均勻磁場中充分變化姿態(tài)，并同步采集磁力儀與陀螺的測量值，由于磁力儀和陀螺的數(shù)據(jù)共同用于本發(fā)明的磁力儀內(nèi)部參數(shù)、磁力儀外部參數(shù)以及陀螺零偏的聯(lián)合標(biāo)定，因此零標(biāo)定效果佳?？梢詫?shí)時(shí)標(biāo)定出磁力儀內(nèi)、外部參數(shù)以及陀螺零偏，而且不受任何加速度干擾的影響，因此實(shí)施過程中不需要設(shè)備保持靜止?fàn)顟B(tài)。本發(fā)明還可用于磁傳感器與慣性測量單元(內(nèi)含陀螺和加速度計(jì))的姿態(tài)配準(zhǔn)。具體實(shí)施方式下面結(jié)合具體實(shí)施例對(duì)本發(fā)明進(jìn)行詳細(xì)說明。以下實(shí)施例將有助于本領(lǐng)域的技術(shù)人員進(jìn)一步理解本發(fā)明，但不以任何形式限制本發(fā)明。應(yīng)當(dāng)指出的是，對(duì)本領(lǐng)域的普通技術(shù)人員來說，在不脫離本發(fā)明構(gòu)思的前提下，還可以做出若干變化和改進(jìn)。這些都屬于本發(fā)明的保護(hù)范圍。根據(jù)本發(fā)明提供的基于序貫濾波的陀螺與磁傳感器聯(lián)合標(biāo)定方法，包括如下步驟：步驟1：建立聯(lián)合標(biāo)定問題的狀態(tài)空間模型；步驟2：采用序貫濾波方法估計(jì)出狀態(tài)空間模型的狀態(tài)。所述步驟1中聯(lián)合標(biāo)定問題的狀態(tài)空間模型如下：系統(tǒng)方程：C·bi=Cbi(hibb-ϵ-ng)×]]>ϵ·=nϵ]]>S·=0]]>h·=0]]>m·i=nmi]]>式中：表示慣性坐標(biāo)系相對(duì)于體坐標(biāo)系的姿態(tài)矩陣，表示的時(shí)間導(dǎo)數(shù)，表示陀螺測量的b系下的角速度矢量，ε表示陀螺零偏，表示ε的時(shí)間導(dǎo)數(shù)，ng表示陀螺測量的高斯噪聲，nε表示陀螺零偏的高斯噪聲，矩陣S表示磁力儀參數(shù)矩陣，表示矩陣S的時(shí)間導(dǎo)數(shù)，向量h表示磁力儀零偏，表示h的時(shí)間導(dǎo)數(shù)，mi為i系下表示的恒定磁場向量，表示mi的時(shí)間導(dǎo)數(shù)，nmi表示因地球自轉(zhuǎn)引起的磁場向量誤差；其中：運(yùn)算a×表示由一個(gè)三維向量a＝[a1a2a3]T構(gòu)成的叉乘矩陣，具體地，a×的展開式如下：a×=0-a3a2a30-a1-a2a10;]]>觀測方程：ym=SCibmi+h+nm]]>式中：ym表示磁力儀的測量值，為姿態(tài)矩陣的轉(zhuǎn)置，即上標(biāo)T表示向量的轉(zhuǎn)置運(yùn)算，nm表示磁力儀測量值的高斯噪聲。具體地，不是一般性，假定均勻磁場的磁場強(qiáng)度為1，即||mi||＝1。以上模型的狀態(tài)包括姿態(tài)、磁力儀內(nèi)部參數(shù)、磁力儀外部參數(shù)、陀螺零偏和磁場向量。磁場向量mi的初始值可選為磁力儀的首次測量值，磁力儀參數(shù)矩陣S的初始值可選為單位矩陣或事先給定，陀螺零偏ε和磁力儀參數(shù)矩陣h的初始值可選為零。因狀態(tài)可觀性的關(guān)系，姿態(tài)的初始值確定為單位矩陣，對(duì)應(yīng)的初始方差矩陣為零。基于以上模型，可采用序貫濾波方法(如擴(kuò)展卡爾曼濾波、粒子濾波等)估計(jì)出狀態(tài)空間模型的狀態(tài)。為了滿足||mi||＝1的狀態(tài)約束，需要對(duì)序貫濾波的結(jié)果進(jìn)行如下尺度調(diào)整：Srs＝||mi||S，mirs＝mi/||mi||，其中：Srs表示尺度調(diào)整后的磁力儀矩陣，mirs表示尺度調(diào)整后的磁場向量，||·||表示取模運(yùn)算。下面以擴(kuò)展卡爾曼濾波為例扼要闡述具體實(shí)施過程：定義狀態(tài)誤差δx為估計(jì)值減去真值x，即而姿態(tài)估計(jì)值與姿態(tài)真值及姿態(tài)誤差ψ的關(guān)系定義為則相應(yīng)的狀態(tài)誤差向量表示為δx＝[ψTδεTvecT(δS)δhTδmiT]T，上部T表示向量的轉(zhuǎn)置運(yùn)算，vec(·)表示將矩陣按照列的順序拼接起來的運(yùn)算；ψT表示姿態(tài)誤差ψ的轉(zhuǎn)置，εT表示陀螺零偏向量ε的轉(zhuǎn)置，hT、miT分別表示h、mi的裝置；狀態(tài)誤差的線性近似狀態(tài)空間模型按如下給出δx·=Fδx+Gwδym=Hmδx+nm]]>其中噪聲矩陣F=03×3-Cbi03×15018×3018×3018×15,G=-Cbi03×303×303×3I303×3012×3012×3012×303×303×3I3,]]>Hm=[-SCib(mi)×03×3(Cibmi)T⊗I3I3SCib]]]>式中：矩陣F、G、Hm分別表示系統(tǒng)矩陣、系統(tǒng)輸入矩陣和觀測矩陣；I3表示三階單位矩陣；符號(hào)表示矩陣與矩陣的Kronecker乘積；對(duì)擴(kuò)展卡爾曼濾波的結(jié)果進(jìn)行如下尺度調(diào)整：Srs＝||mi||S，mirs＝mi/||mi||。以上對(duì)本發(fā)明的具體實(shí)施例進(jìn)行了描述。需要理解的是，本發(fā)明并不局限于上述特定實(shí)施方式，本領(lǐng)域技術(shù)人員可以在權(quán)利要求的范圍內(nèi)做出各種變化或修改，這并不影響本發(fā)明的實(shí)質(zhì)內(nèi)容。在不沖突的情況下，本申請(qǐng)的實(shí)施例和實(shí)施例中的特征可以任意相互組合。當(dāng)前第1頁1 2 3