一種基于陀螺和傾角儀的載體航姿測(cè)量方法
【專利摘要】本發(fā)明公開(kāi)了一種基于陀螺和傾角儀的載體航姿測(cè)量方法，其包括以下幾個(gè)步驟：S1根據(jù)陀螺實(shí)測(cè)的載體角速率ω(t)及外界直接提供的載體初始航向，計(jì)算獲得所述載體航向在載體系下的投影φ(t)，其中，t為時(shí)間變量；S2根據(jù)公式C3(t)＝C1(t)*C2(t)計(jì)算獲得載體的姿態(tài)矩陣C3(t)，其中，C1(t)為傾角儀姿態(tài)旋轉(zhuǎn)矩陣；C2(t)為所述φ(t)的旋轉(zhuǎn)矩陣；S3根據(jù)所述載體的姿態(tài)矩陣C3(t)計(jì)算獲得載體當(dāng)前的航向，采用傾角儀測(cè)量獲得載體當(dāng)前的俯仰角以及載體當(dāng)前橫滾角，所述載體當(dāng)前的航向、所述載體當(dāng)前的俯仰角以及所述載體當(dāng)前橫滾角共同表征載體當(dāng)前航姿。本發(fā)明方法精度高、成本低。
【專利說(shuō)明】
一種基于陀螺和傾角儀的載體航姿測(cè)量方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明屬于慣性導(dǎo)航技術(shù)領(lǐng)域，更具體地，涉及一種低動(dòng)態(tài)高精度航姿測(cè)量方法。
【背景技術(shù)】
[0002] 在航姿測(cè)量系統(tǒng)中，姿態(tài)是反映載體運(yùn)動(dòng)的一個(gè)重要參數(shù)?，F(xiàn)有的航姿測(cè)量方案 一般采用三軸陀螺或采用陀螺、加表、磁強(qiáng)計(jì)組合等方式。以上方式存在如下缺點(diǎn)：（1)采用 三軸陀螺方案可以保證測(cè)量精度，但是其成本較高；（2)采用陀螺、加表、磁強(qiáng)計(jì)組合方案其 成本低但是無(wú)法保證高精度測(cè)量。
[0003] 因此，需要開(kāi)發(fā)一種航姿測(cè)量方法，要求其能在保證高精度測(cè)量的同時(shí)降低系統(tǒng) 成本。

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

[0004] 針對(duì)現(xiàn)有技術(shù)的以上缺陷或改進(jìn)需求，本發(fā)明提供了一種基于陀螺和傾角儀的載 體航姿測(cè)量方法，其目的在于，僅僅采用陀螺和傾角儀這樣現(xiàn)有的低成本系統(tǒng)，利用傾角儀 旋轉(zhuǎn)矩陣對(duì)載體在載體系下的投影進(jìn)行姿態(tài)分解，從而獲得高精度的航姿，本發(fā)明方法是 一種高精度、低成本的載體航姿測(cè)量方法。
[0005] 為實(shí)現(xiàn)上述目的，按照本發(fā)明的一個(gè)方面，提供了一種基于陀螺和傾角儀的載體 航姿測(cè)量方法，所述陀螺和所述傾角儀均設(shè)置在載體上，其包括如下步驟：
[0006] S1:根據(jù)陀螺實(shí)測(cè)的載體角速率ω (t)及外界直接提供的載體初始航向，計(jì)算獲得 所述載體航向在載體系下的投影Φ (t)，其中，t為時(shí)間變量；
[0007] S2:根據(jù)公式計(jì)算獲得載體的姿態(tài)矩陣C3(t)，其中，CKt)為傾 角儀姿態(tài)旋轉(zhuǎn)矩陣;C2(t)為所述(Ht)的旋轉(zhuǎn)矩陣，t為時(shí)間變量；
[0008] S3:根據(jù)所述載體的姿態(tài)矩陣C3(t)計(jì)算獲得載體當(dāng)前的航向，采用傾角儀測(cè)量獲 得載體當(dāng)前的俯仰角以及載體當(dāng)前橫滾角，所述載體當(dāng)前的航向、所述載體當(dāng)前的俯仰角 以及所述載體當(dāng)前橫滾角共同表征載體當(dāng)前航姿。
[0009] 進(jìn)一步的，所述步驟S1具體包括如下子步驟：
[0010] S11:選擇東北天當(dāng)?shù)氐乩碜鴺?biāo)系作為導(dǎo)航坐標(biāo)系，采集陀螺實(shí)測(cè)的載體的角速率 w(t)，其中，t為時(shí)間變量；
[0011]
，將地球自轉(zhuǎn)角速ω ie在東北天當(dāng)?shù)氐乩碜?標(biāo)系下的投影轉(zhuǎn)換為在載體坐標(biāo)系下的地球自轉(zhuǎn)角速率投影
[0012] 其中，ω1(3為地球自轉(zhuǎn)角速率，為載體姿態(tài)矩陣<^(〇的轉(zhuǎn)置矩陣，載體姿態(tài)矩 陣G⑴初始值根據(jù)外界直接提供的載體初始航向以及傾角儀測(cè)量獲得的載體俯仰角和載 體橫滾角獲得，L為當(dāng)?shù)氐乩頍樁?，€:〗(〇中b為載體坐標(biāo)系，Ct⑴中η為導(dǎo)航坐標(biāo)系，導(dǎo)航坐 標(biāo)系即為東北天當(dāng)?shù)氐乩碜鴺?biāo)系；
[0013] S13:根據(jù)公式州)=#/-丨)+ ? 十算獲得所述載體航向在載體系下的投 影Φ⑴，
[0014] 其中，⑴為載體坐標(biāo)系下的地球自轉(zhuǎn)角速率投影<(〇的天向分量，t為時(shí)間變 量，當(dāng)t = 0時(shí)，所述載體航向在載體系下的初始投影為Φ (0)。
[0015] 進(jìn)一步的，步驟S2具體包括如下子步驟：
[0016]
計(jì)算獲得所述載體在載體系下的投 影Φ (t)的旋轉(zhuǎn)矩陣C2 (t)，其中，Φ (t)為所述載體在載體系下的投影，
[0017]
計(jì)算獲得傾角 儀姿態(tài)旋轉(zhuǎn)矩陣&(0，
[0018] 其中，0(t)為傾角儀測(cè)量獲得的載體俯仰角；γ (t)為傾角儀測(cè)量獲得的載體橫滾 角；
[0019] S23:根據(jù)公式(^⑴二&⑴仏⑴獲得載體的姿態(tài)矩陣C3(t)。
[0020] 進(jìn)一步的，步驟S3具體包括如下子步驟：
[0021]
計(jì)算獲得載體當(dāng)前的航向yaw(t);
[0022]其中，C3(t)(l，2)為載體的姿態(tài)矩陣C3(t)的第一行第二項(xiàng)，C 3(t)(2,2)為載體的 姿態(tài)矩陣C3(t)的第二行第二項(xiàng)；
[0023] S32:采用傾角儀測(cè)量獲得載體當(dāng)前的俯仰角0(t)以及載體當(dāng)前橫滾角y(t)，所 述載體當(dāng)前的航向yaw(t)、所述載體當(dāng)前的俯仰角0(t)以及所述載體當(dāng)前橫滾角γ (t)共 同表征載體當(dāng)前航姿。
[0024] 總體而言，通過(guò)本發(fā)明所構(gòu)思的以上技術(shù)方案與現(xiàn)有技術(shù)相比，能夠取得下列有 益效果：
[0025] 本發(fā)明提供的基于陀螺和傾角儀的航姿測(cè)量方法，以單軸陀螺和雙軸傾角儀為基 本數(shù)據(jù)測(cè)量裝置，利用傾角儀旋轉(zhuǎn)矩陣對(duì)載體在載體系下的投影進(jìn)行姿態(tài)分解，能獲得高 精度的航向解算，是一種采用低成本系統(tǒng)進(jìn)行高精度航姿測(cè)量的方法。本發(fā)明簡(jiǎn)單巧妙，符 合實(shí)際需求，進(jìn)能夠大規(guī)模推廣使用。
【附圖說(shuō)明】
[0026] 圖1是本發(fā)明實(shí)施例提供的一種基于陀螺和傾角儀的載體航姿測(cè)量方法流程圖。
【具體實(shí)施方式】
[0027] 為了使本發(fā)明的目的、技術(shù)方案及優(yōu)點(diǎn)更加清楚明白，以下結(jié)合附圖及實(shí)施例，對(duì) 本發(fā)明進(jìn)行進(jìn)一步詳細(xì)說(shuō)明。應(yīng)當(dāng)理解，此處所描述的具體實(shí)施例僅僅用以解釋本發(fā)明，并 不用于限定本發(fā)明。此外，下面所描述的本發(fā)明各個(gè)實(shí)施方式中所涉及到的技術(shù)特征只要 彼此之間未構(gòu)成沖突就可以相互組合。
[0028] 本發(fā)明提供的低動(dòng)態(tài)高精度航姿測(cè)量方法，以單軸陀螺和雙軸傾角儀為基本數(shù)據(jù) 傳感器，利用傾角儀旋轉(zhuǎn)矩陣對(duì)載體航向在載體系下的投影進(jìn)行姿態(tài)分解，能獲得高精度 的航向解算，是一種采用低成本系統(tǒng)進(jìn)行高精度航姿測(cè)量的方法。
[0029] 如圖1所示，本發(fā)明實(shí)施例提供的一種基于陀螺和傾角儀的載體航姿測(cè)量方法，其 包括以下幾個(gè)步驟：
[0030] S1:根據(jù)陀螺實(shí)測(cè)的載體角速率ω (t)及外界直接提供的載體初始航向，計(jì)算獲得 所述載體在載體系下的投影Φ (t )，其中，t為時(shí)間變量；
[0031] S2:根據(jù)公式計(jì)算獲得載體的姿態(tài)矩陣C3(t)，其中，&(t)為傾 角儀姿態(tài)旋轉(zhuǎn)矩陣;C2(t)為所述(Ht)的旋轉(zhuǎn)矩陣，t為時(shí)間變量；
[0032] S3:根據(jù)所述載體的姿態(tài)矩陣C3(t)計(jì)算獲得載體當(dāng)前的航向，采用傾角儀測(cè)量獲 得載體當(dāng)前的俯仰角以及載體當(dāng)前橫滾角，所述載體當(dāng)前的航向、所述載體當(dāng)前的俯仰角 以及所述載體當(dāng)前橫滾角共同表征載體當(dāng)前航姿。
[0033]作為本發(fā)明的一個(gè)實(shí)施例，步驟1中的具體步驟為：
[0034] S11:選擇東北天當(dāng)?shù)氐乩碜鴺?biāo)系作為導(dǎo)航坐標(biāo)系，采集陀螺實(shí)測(cè)的載體的角速率 w(t)，其中，t為時(shí)間變暈：
[0035]
，將地球自轉(zhuǎn)角速《ie在東北天當(dāng)?shù)氐乩碜?標(biāo)系下的投影轉(zhuǎn)換為在載體坐標(biāo)系下的地球自轉(zhuǎn)角速率投影<(〇，
[0036] 其中，coie3為地球自轉(zhuǎn)角速率，<⑴為載體姿態(tài)矩陣⑴的轉(zhuǎn)置矩陣，載體姿態(tài)矩 陣CTW初始值根據(jù)外界直接提供的載體初始航向以及傾角儀測(cè)量獲得的載體俯仰角和載 體橫滾角獲得，L為當(dāng)?shù)氐乩頍樁?，?〇中b為載體坐標(biāo)系，⑴中η為導(dǎo)航坐標(biāo)系，導(dǎo)航坐 標(biāo)系即為東北天當(dāng)?shù)氐乩碜鴺?biāo)系；
[0037] S13:根據(jù)公式外)=# -+ 的計(jì)算獲得所述載體航向在載體系下的投 影Φ⑴，
[0038] 其中為載體坐標(biāo)系下的地球自轉(zhuǎn)角速率投影<(〇的天向分量，t為時(shí)間變 量，當(dāng)t = 0時(shí)，所述載體在載體系下的初始投影為Φ (0)。
[0039] 作為本發(fā)明的又一個(gè)實(shí)施例，本發(fā)明的步驟S2具體為：
[0040]
計(jì)算獲得所述載體在載體系下的投 影Φ (t)的旋轉(zhuǎn)矩陣C2 (t)，其中，Φ (t)為所述載體在載體系下的投影，
[0041]
計(jì)算獲得傾角 儀姿態(tài)旋轉(zhuǎn)矩陣&(0，
[0042] 其中，0(t)為傾角儀測(cè)量獲得的載體俯仰角；γ (t)為傾角儀測(cè)量獲得的載體橫滾 角；
[0043] S23:根據(jù)公式C3 (t) = & (t)*C2 (t)獲得載體的姿態(tài)矩陣C3 (t)。
[0044] 作為本發(fā)明的又一個(gè)實(shí)施例，本發(fā)明的步驟S3具體為：
[0045]
_計(jì)算獲得載體當(dāng)前的航向yaw(t);
[0046] 其中，C3(t)(l，2)為載體的姿態(tài)矩陣C3(t)的第一行第二項(xiàng)，C3(t)(2,2)為載體的 姿態(tài)矩陣C 3(t)的第二行第二項(xiàng)；
[0047] S32:采用傾角儀測(cè)量獲得載體當(dāng)前的俯仰角0(t)以及載體當(dāng)前橫滾角y(t)，所 述載體當(dāng)前的航向yaw(t)、所述載體當(dāng)前的俯仰角0(t)以及所述載體當(dāng)前橫滾角γ (t)共 同表征載體當(dāng)前航姿。
[0048] 本發(fā)明提供的基于陀螺和傾角儀的航姿測(cè)量方法，是一種低動(dòng)態(tài)高精度航姿測(cè)量 方法，以單軸陀螺和雙軸傾角儀為基本數(shù)據(jù)傳感器，利用傾角儀旋轉(zhuǎn)矩陣對(duì)載體在載體系 下的投影進(jìn)行姿態(tài)分解，從而獲得高精度的航向解算，對(duì)航姿測(cè)量領(lǐng)域的低動(dòng)態(tài)測(cè)量有較 大貢獻(xiàn)。本發(fā)明簡(jiǎn)單巧妙，符合實(shí)際需求，實(shí)用性強(qiáng)，進(jìn)步顯著，能夠大規(guī)模推廣使用。
[0049]本領(lǐng)域的技術(shù)人員容易理解，以上所述僅為本發(fā)明的較佳實(shí)施例而已，并不用以 限制本發(fā)明，凡在本發(fā)明的精神和原則之內(nèi)所作的任何修改、等同替換和改進(jìn)等，均應(yīng)包含 在本發(fā)明的保護(hù)范圍之內(nèi)。
【主權(quán)項(xiàng)】
1. 一種基于巧螺和傾角儀的載體航姿測(cè)量方法，所述巧螺和所述傾角儀均設(shè)置在載體 上，其特征在于，其包括如下步驟： S1:根據(jù)巧螺實(shí)測(cè)的載體角速率w(t)及外界直接提供的載體初始航向，計(jì)算獲得載體 航向在載體系下的投影Φ (t )，其中，t為時(shí)間變量； S2:根據(jù)公式C3(t)=Ci(t)*C2(t)計(jì)算獲得載體的姿態(tài)矩陣C3(t)，其中，Ci(t)為傾角儀 姿態(tài)旋轉(zhuǎn)矩陣;C2(t)為所述Φα)的旋轉(zhuǎn)矩陣，t為時(shí)間變量； S3:根據(jù)所述載體的姿態(tài)矩陣C3(t)計(jì)算獲得載體當(dāng)前的航向，采用傾角儀測(cè)量獲得載 體當(dāng)前的俯仰角W及載體當(dāng)前橫滾角，所述載體當(dāng)前的航向、所述載體當(dāng)前的俯仰角W及 所述載體當(dāng)前橫滾角共同表征載體當(dāng)前航姿。2. 如權(quán)利要求1所述的一種基于巧螺和傾角儀的載體航姿測(cè)量方法，其特征在于，所述 步驟S1具體包括如下子步驟： S11:選擇東北天當(dāng)?shù)氐乩碜鴺?biāo)系作為導(dǎo)航坐標(biāo)系，采集巧螺實(shí)測(cè)的載體的角速率ω (t)，其中，t為時(shí)間變量； S12:根據(jù)公式，將地球自轉(zhuǎn)角速ω 16在東北天當(dāng)?shù)氐乩碜鴺?biāo)系 下的投影轉(zhuǎn)換為在載體坐標(biāo)系下的地球自轉(zhuǎn)角速率投影巧t(〇， 其中，ω 16為地球自轉(zhuǎn)角速率，C(〇為載體姿態(tài)矩陣C價(jià)的轉(zhuǎn)置矩陣，載體姿態(tài)矩陣 q'(o初始值根據(jù)外界直接提供的載體初始航向W及傾角儀測(cè)量獲得的載體俯仰角和載體 橫滾角獲得，L為當(dāng)?shù)氐乩砭喍?，Ci貨中b為載體坐標(biāo)系，CtW中η為導(dǎo)航坐標(biāo)系，導(dǎo)航坐標(biāo) 系即為東北天當(dāng)?shù)氐乩碜鴺?biāo)系； S13:根據(jù)公式抑〇 = 9如-1) +似（〇-始(0計(jì)算獲得所述載體航向在載體系下的投影Φ (t)， 其中，0右的為載體坐標(biāo)系下的地球自轉(zhuǎn)角速率投影的天向分量，t為時(shí)間變量，當(dāng) t = 0時(shí)，所述載體在載體系下的初始投影為Φ (0)。3. 如權(quán)利要求2所述的一種基于巧螺和傾角儀的載體航姿測(cè)量方法，其特征在于，步驟 S2具體包括如下子步驟： S21:根據(jù)公式計(jì)算獲得所述載體在載體系下的投影Φ (t)的旋轉(zhuǎn)矩陣C2 (t)，其中，Φ (t)為所述載體在載體系下的投影， S22:根據(jù)公??計(jì)算獲得傾角儀姿態(tài) 旋轉(zhuǎn)矩陣Ci(t)， 其中，e(t)為傾角儀測(cè)量獲得的載體俯仰角；丫（t)為傾角儀測(cè)量獲得的載體橫滾角； S23:根據(jù)公式C3(t)=Ci(t)*C2(t)獲得載體的姿態(tài)矩陣C3(t)。4.如權(quán)利要求3所述的一種基于巧螺和傾角儀的載體航姿測(cè)量方法，其特征在于，步驟 S3具體包括如下子步驟： S31:根據(jù)公計(jì)算獲得載體當(dāng)前的航向yaw (t); 其中，C3(t)(l，2)為載體的姿態(tài)矩陣C3(t)的第一行第二項(xiàng)，C3(t)(2,2)為載體的姿態(tài) 矩陣C3(t)的第二行第二項(xiàng)； S32:采用傾角儀測(cè)量獲得載體當(dāng)前的俯仰角0(t)W及載體當(dāng)前橫滾角丫（t)，所述載 體當(dāng)前的航向yaw(t)、所述載體當(dāng)前的俯仰角0(t) W及所述載體當(dāng)前橫滾角丫（t)共同表 征載體當(dāng)前航姿。
【文檔編號(hào)】G01C21/16GK106092098SQ201610727023
【公開(kāi)日】2016年11月9日
【申請(qǐng)日】2016年8月25日
【發(fā)明人】崔蒞杭
【申請(qǐng)人】湖北三江航天紅峰控制有限公司