一種利用gps與加速度計(jì)計(jì)算飛行器姿態(tài)角的方法
【專利摘要】一種利用GPS與加速度計(jì)計(jì)算飛行器姿態(tài)角的方法，根據(jù)GPS測得的飛行器導(dǎo)航坐標(biāo)系下的飛行速度Vn計(jì)算導(dǎo)航坐標(biāo)系下飛行速度的微分信號(hào)并根據(jù)公式計(jì)算獲得導(dǎo)航坐標(biāo)系下的比力fn，其中，為導(dǎo)航坐標(biāo)系下哥式加速度，為導(dǎo)航坐標(biāo)系下對(duì)地向心加速度，gn為導(dǎo)航坐標(biāo)系下重力加速度；利用加速度計(jì)測得體坐標(biāo)系下比力fb，求解轉(zhuǎn)換關(guān)系式獲得俯仰角偏航角滾轉(zhuǎn)角γ，其中，為導(dǎo)航坐標(biāo)系轉(zhuǎn)換到體坐標(biāo)系的姿態(tài)轉(zhuǎn)換矩陣；本發(fā)明使飛行器無需陀螺就能獲得姿態(tài)信息實(shí)現(xiàn)姿態(tài)控制并完成導(dǎo)航飛行，也能用于安裝陀螺的飛行器上以提升飛行器故障余度。
【專利說明】
一種利用GPS與加速度計(jì)計(jì)算飛行器姿態(tài)角的方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及一種計(jì)算飛行器姿態(tài)角的方法。
【背景技術(shù)】
[0002] 飛行器所攜帶慣導(dǎo)組件均包含陀螺和加速度計(jì)，加速度計(jì)用來測量飛行器的加速 度，陀螺用來測量飛行器的角運(yùn)動(dòng)。加速度和角速度用于解算飛行器六個(gè)自由度，是慣性導(dǎo) 航的基本條件。
[0003] 現(xiàn)有技術(shù)獲取飛行器角運(yùn)動(dòng)的方式局限于使用陀螺進(jìn)行量測。其缺陷在于：陀螺 的成本相對(duì)加速度計(jì)較高，且易損壞。在開發(fā)低成本飛行器的過程中，陀螺的價(jià)格在整個(gè)飛 行器制造的成本中占了很大的比重，是開發(fā)者必須面對(duì)的問題；
[0004] 現(xiàn)有飛行器非常依賴慣性組件提供姿態(tài)信息，陀螺還在控制角運(yùn)動(dòng)的伺服回路中 用作控制環(huán)節(jié)。在飛行過程中，若陀螺發(fā)生故障時(shí)沒有備用陀螺以持續(xù)提供姿態(tài)信息，則飛 行器無法繼續(xù)導(dǎo)航，并且陀螺參與的姿態(tài)控制回路斷開，飛行器無法繼續(xù)保持姿態(tài)，會(huì)發(fā)生 重大飛行事故；
[0005] 陀螺由于其漂移的問題其輸出量的誤差會(huì)隨時(shí)間增長而增長。在沒有母慣導(dǎo)進(jìn)行 傳遞對(duì)準(zhǔn)的情況下，成本較低的MEMS陀螺1分鐘產(chǎn)生的誤差便不滿足姿態(tài)精度要求，而精度 較高的陀螺又較昂貴。

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

[0006] 本發(fā)明所解決的技術(shù)問題是:克服現(xiàn)有技術(shù)的不足，提供一種利用GPS與加速度計(jì) 計(jì)算飛行器姿態(tài)角的方法，使飛行器無需陀螺就能獲得姿態(tài)信息，實(shí)現(xiàn)姿態(tài)控制并完成導(dǎo) 航飛行，也能用于安裝陀螺的飛行器上以提升飛行器故障余度，防止陀螺發(fā)生故障時(shí)發(fā)生 飛行事故。
[0007] 本發(fā)明所采用的技術(shù)方案是:一種利用GPS與加速度計(jì)計(jì)算飛行器姿態(tài)角的方法， 根據(jù)GPS測得的飛行器導(dǎo)航坐標(biāo)系下的飛行速度Vn計(jì)算導(dǎo)航坐標(biāo)系下飛行速度的微分信號(hào) 0，并根據(jù)公式/?Β + X ΓΧ X FM -gH,計(jì)算獲得導(dǎo)航坐標(biāo)系下的比力fn，其中，gn 為導(dǎo)航坐標(biāo)系下重力加速度，堿為地球坐標(biāo)系相對(duì)與地心慣性坐標(biāo)系的旋轉(zhuǎn)角速率在導(dǎo)航 坐標(biāo)系下的投影，政：為導(dǎo)航坐標(biāo)系相對(duì)于地球坐標(biāo)系的旋轉(zhuǎn)角速率在導(dǎo)航坐標(biāo)系下的投 影;利用加速度計(jì)測得體坐標(biāo)系下比力滬，求解轉(zhuǎn)換關(guān)系式/ 6=α?Γ，獲得俯仰角#、偏航 角ρ、滾轉(zhuǎn)角γ，其中，Ct為導(dǎo)航坐標(biāo)系轉(zhuǎn)換到體坐標(biāo)系的姿態(tài)轉(zhuǎn)換矩陣；
[0008] 導(dǎo)航坐標(biāo)系為北天東坐標(biāo)系，坐標(biāo)原點(diǎn)0n位于飛行器起飛點(diǎn)，0nxn軸指向正北方 向，〇 nyn軸指向天，〇nzn軸按右手定則確定;體坐標(biāo)系的原點(diǎn)〇 b位于飛行器的質(zhì)心，〇bxb軸在 飛行器對(duì)稱平面內(nèi)，平行于機(jī)身軸向前，〇 byb軸在對(duì)稱平面內(nèi)垂直于〇bxb軸，指向天，〇 bzb垂 直對(duì)稱平面指向右;地心慣性坐標(biāo)系的原點(diǎn)位于地心，X軸指向春分點(diǎn)，z軸沿地球自轉(zhuǎn)軸，y 軸與X與Z軸構(gòu)成右手系;地球坐標(biāo)系的原點(diǎn)位于地心，X軸穿過本初子午線與赤道的交點(diǎn)，Z 軸穿過地球北極點(diǎn)，y軸穿過東經(jīng)90°子午線與赤道的交點(diǎn)，坐標(biāo)系與地球固連。
[0009] 所述飛行速度的微分信號(hào)f"獲取方法如下：
[0010] 采用四階龍格-庫塔法計(jì)算如下微分方程，積分步長h = 0.01，調(diào)整參數(shù)R與參數(shù)δ， 得到Χ2:
[0012]
，S = 0.00005R;輸入信號(hào)v(t)為導(dǎo)航坐標(biāo)系 下的飛行速度Vn，X1為輸入信號(hào)v(t)的跟蹤信號(hào)，X2為導(dǎo)航坐標(biāo)系下的飛行速度的微分信號(hào) ￥\
[0013] 所述的參數(shù)R在輸入信號(hào)v(t)不含噪聲時(shí)，取值范圍為10~70;在輸入信號(hào)v(t)含 有1 %的噪聲時(shí)，取值范圍為2.5~5。
[0014]所述轉(zhuǎn)換關(guān)系式/=ctr采用牛頓迭代法求解。
[0015] 本發(fā)明與現(xiàn)有技術(shù)相比的優(yōu)點(diǎn)在于：
[0016] (1)本發(fā)明的方法能夠利用GPS與加速度計(jì)計(jì)算飛行器姿態(tài)角，當(dāng)被用作主控環(huán)節(jié) 時(shí)，在飛行器設(shè)計(jì)中可以取消其中的陀螺部件，有效降低飛行器生產(chǎn)成本，提高控制的精確 度。
[0017] (2)本發(fā)明的方法用作余度飛行器上的故障應(yīng)急方案，則可以在飛行器安裝的陀 螺發(fā)生故障時(shí)提供姿態(tài)信息，以保持飛行器姿態(tài)穩(wěn)定，增大其故障余度，防止飛行事故的發(fā) 生。
[0018] (3)本發(fā)明的方法計(jì)算得出的姿態(tài)信息不存在漂移問題，誤差主要來自GPS誤差， 不會(huì)隨時(shí)間增長而增長，并且通過跟蹤-微分器解決微分信號(hào)中噪聲過大的問題，不僅可減 小姿態(tài)噪聲還可以濾除GPS輸出速度信號(hào)中的噪聲，提高了測量精度。
【附圖說明】
[0019] 圖1為飛行器飛行方案示意圖；
[0020] 圖2為本發(fā)明的姿態(tài)角計(jì)算方法流程圖。
【具體實(shí)施方式】
[0021] 圖1為飛行器飛行方案示意圖，首先說明本方案中提到的兩種坐標(biāo)系。
[0022]導(dǎo)航坐標(biāo)系采用北天東坐標(biāo)系，坐標(biāo)原點(diǎn)0n位于飛行器起飛點(diǎn)；0nxn軸指向正北方 向;〇nyn軸指向天;〇nzn軸按右手定則確定，既正東方向。
[0023] 體坐標(biāo)系是固連于飛行器并隨其運(yùn)動(dòng)的一種動(dòng)坐標(biāo)系。其原點(diǎn)爐位于飛行器的質(zhì) 心;0bxb軸在飛行器對(duì)稱平面內(nèi)，平行于機(jī)身軸向前;0 byb軸亦在對(duì)稱平面內(nèi)垂直于0bxb軸， 指向天;0 bzb垂直對(duì)稱平面指向右。
[0024] 本文中所計(jì)算的姿態(tài)角便是體坐標(biāo)系與導(dǎo)航坐標(biāo)系之間的旋轉(zhuǎn)角度。
[0025]飛行器在飛行過程中利用GPS接收機(jī)接收GPS衛(wèi)星信號(hào)，GPS實(shí)時(shí)提供飛行器位置 信息與速度信息￥"均為導(dǎo)航坐標(biāo)系下的值;利用飛行器上的加速度計(jì)得到體坐標(biāo)系下的實(shí) 時(shí)比力信息爐。￥"與爐為提取導(dǎo)航坐標(biāo)系與體坐標(biāo)系之間姿態(tài)轉(zhuǎn)換矩陣的輸入。
[0026]如圖2,一種利用GPS與加速度計(jì)計(jì)算飛行器姿態(tài)角的方法，包括步驟如下：
[0027] (1)由GPS得出飛行器飛行速度Vn，并通過非線性跟蹤-微分器計(jì)算出速度的近似 微分信號(hào)r,并通過比力方程去除有害加速度獲得導(dǎo)航坐標(biāo)系下的比力Γ;
[0028]通過二階非線性跟蹤-微分器計(jì)算出速度的近似微分信號(hào)P的方法，其方程為

[0030] 其中，參數(shù)R越大，跟蹤效果越好，但是R過大會(huì)給微分信號(hào)增加高頻噪聲，具體R與 S的取值根據(jù)GPS提供的速度信號(hào)質(zhì)量而定。
[0031] 一般遵循以下取值原則：
[0032] (1)在輸入不含噪聲時(shí)，R的取值在10~70;
[0033] (2)在輸入含有1%的噪聲時(shí)，微分信號(hào)震蕩加大，在保證跟蹤的前提下，R要盡可 能小，取值在2.5~5;
[0034] (3)參數(shù)δ的取值對(duì)微分信號(hào)有極大影響，要保證微分信號(hào)盡可能不受δ的影響，δ 與R之間的關(guān)系是R增大時(shí)S應(yīng)增大相應(yīng)的倍數(shù)，在此二階非線性跟蹤-微分器中令S = 0.00005R；
[0035] 根據(jù)￥"的信號(hào)質(zhì)量設(shè)計(jì)二階非線性跟蹤-微分器中R與δ值的大小后，將入作為 輸入信號(hào)v(t)代入公式中，輸出信號(hào)X1為輸入v(t)的跟蹤信號(hào)，可以認(rèn)為是處理之后的輸 入信號(hào)，采用四階龍格-庫塔法解此微分方程，積分步長取h = 0.01，調(diào)整R與δ的大小得到兩 個(gè)光滑可用的信號(hào)，的跟蹤信號(hào)，Χ2為v(t)的近似微分信號(hào)既導(dǎo)航坐標(biāo)系下的飛行 速度的微分信號(hào)戶。
[0036] (2)通過比力方程去除有害加速度獲得導(dǎo)航坐標(biāo)系下的比力的方法:該比力方程 為r =/" -(2〇>: +6〇xjr+gn，是比力方程導(dǎo)航坐標(biāo)系下的形式，其中所有的量都取導(dǎo)航 坐標(biāo)系下的值，包括哥式加速度2?〇: X Fn、對(duì)地向心加速度臧X ?/Β以及重力加速度8"，試為 地球坐標(biāo)系相對(duì)與地心慣性坐標(biāo)系的旋轉(zhuǎn)角速率在導(dǎo)航坐標(biāo)系下的投影，執(zhí)丨為導(dǎo)航坐標(biāo)系 相對(duì)于地球坐標(biāo)系的旋轉(zhuǎn)角速率在導(dǎo)航坐標(biāo)系下的投影;在與8"均為已知值的情 況下導(dǎo)航坐標(biāo)系下的比力?？梢酝ㄟ^轉(zhuǎn)換后的公式/" +(2< )χΓ Β -gB:實(shí)時(shí)求出。 地心慣性坐標(biāo)系的原點(diǎn)位于地心，X軸指向春分點(diǎn)，z軸沿地球自轉(zhuǎn)軸，y軸與X與z軸構(gòu)成右 手系；地球坐標(biāo)系的原點(diǎn)位于地心，X軸穿過本初子午線與赤道的交點(diǎn)，z軸穿過地球北極 點(diǎn)，y軸穿過東經(jīng)90°子午線與赤道的交點(diǎn)，坐標(biāo)系與地球固連。
[0037] (3)由加速度計(jì)獲得體坐標(biāo)系下的比力fb;利用姿態(tài)轉(zhuǎn)換矩陣與姿態(tài)角的關(guān)系及 兩個(gè)坐標(biāo)系下比力的轉(zhuǎn)換關(guān)系得出姿態(tài)角與兩坐標(biāo)系下比力的關(guān)系式，并采用牛頓迭代法 求解出姿態(tài)角。
[0038]姿態(tài)轉(zhuǎn)換矩陣C〖為：
[0040]體坐標(biāo)系下的比力值fb與導(dǎo)航坐標(biāo)系下的比力值fn存在如下轉(zhuǎn)換關(guān)系/ ， 展開可得
[0042]既可得到姿態(tài)角與兩坐標(biāo)系下比力的關(guān)系式：
[0044]其中，廣、，、f、f、/,% f均已知，姿態(tài)角4、爐、γ為未知數(shù)，采用牛 頓迭代法解此方程既可得到姿態(tài)角。其中，義、/〗、<為滬沿體坐標(biāo)系三軸的分量， C、尺為Γ沿導(dǎo)航坐標(biāo)系三軸的分量，姿態(tài)角包括:俯仰角J、偏航角識(shí)、滾轉(zhuǎn)角γ。 [0045]本發(fā)明說明書中未詳細(xì)說明的內(nèi)容屬于本領(lǐng)域技術(shù)人員公知常識(shí)。
【主權(quán)項(xiàng)】
1. 一種利用GPS與加速度計(jì)計(jì)算飛行器姿態(tài)角的方法，其特征在于:根據(jù)GPS測得的飛 行器導(dǎo)航坐標(biāo)系下的飛行速度Vn計(jì)算導(dǎo)航坐標(biāo)系下飛行速度的微分信號(hào)并根據(jù)公式 /Β + 2供:χΓΧ χΓΗ -gH，計(jì)算獲得導(dǎo)航坐標(biāo)系下的比力Γ，其中，gn為導(dǎo)航坐標(biāo)系 下重力加速度，試為地球坐標(biāo)系相對(duì)與地心慣性坐標(biāo)系的旋轉(zhuǎn)角速率在導(dǎo)航坐標(biāo)系下的投 影，?：：為導(dǎo)航坐標(biāo)系相對(duì)于地球坐標(biāo)系的旋轉(zhuǎn)角速率在導(dǎo)航坐標(biāo)系下的投影;利用加速度 計(jì)測得體坐標(biāo)系下比力fb，求解轉(zhuǎn)換關(guān)系式/ ? =C$/B,獲得俯仰角偏航角P、滾轉(zhuǎn)角γ， 其中，g為導(dǎo)航坐標(biāo)系轉(zhuǎn)換到體坐標(biāo)系的姿態(tài)轉(zhuǎn)換矩陣； 導(dǎo)航坐標(biāo)系為北天東坐標(biāo)系，坐標(biāo)原點(diǎn)〇n位于飛行器起飛點(diǎn)，〇ηχη軸指向正北方向，〇 nyn 軸指向天，〇nzn軸按右手定則確定;體坐標(biāo)系的原點(diǎn)〇b位于飛行器的質(zhì)心，〇 bxb軸在飛行器對(duì) 稱平面內(nèi)，平行于機(jī)身軸向前，〇byb軸在對(duì)稱平面內(nèi)垂直于〇 bxb軸，指向天，〇bzb垂直對(duì)稱平 面指向右;地心慣性坐標(biāo)系的原點(diǎn)位于地心，X軸指向春分點(diǎn)，z軸沿地球自轉(zhuǎn)軸，y軸與X與z 軸構(gòu)成右手系;地球坐標(biāo)系的原點(diǎn)位于地心，X軸穿過本初子午線與赤道的交點(diǎn)，z軸穿過地 球北極點(diǎn)，y軸穿過東經(jīng)90°子午線與赤道的交點(diǎn)，坐標(biāo)系與地球固連。2. 根據(jù)權(quán)利要求1所述的一種利用GPS與加速度計(jì)計(jì)算飛行器姿態(tài)角的方法，其特征在 于:所述飛行速度的微分信號(hào)r獲取方法如下： 采用四階龍格-庫塔法計(jì)算如下微分方程，積分步長h = 0.01，調(diào)整參數(shù)R與參數(shù)δ，得到 Χ2： I ' '2Κ其中， ，S = 0.00005R;輸入信號(hào)v(t)為導(dǎo)航坐標(biāo)系下的 ) 飛行速度Vn，X1為輸入信號(hào)v(t)的跟蹤信號(hào)，X2為導(dǎo)航坐標(biāo)系下的飛行速度的微分信號(hào)戶。3. 根據(jù)權(quán)利要求2所述的一種利用GPS與加速度計(jì)計(jì)算飛行器姿態(tài)角的方法，其特征在 于:所述的參數(shù)R在輸入信號(hào)v(t)不含噪聲時(shí)，取值范圍為10~70;在輸入信號(hào)v(t)含有1 % 的噪聲時(shí)，取值范圍為2.5~5。4. 根據(jù)權(quán)利要求1或2所述的一種利用GPS與加速度計(jì)計(jì)算飛行器姿態(tài)角的方法，其特 征在于:所述轉(zhuǎn)換關(guān)系式/6=Gr采用牛頓迭代法求解。
【文檔編號(hào)】G01C21/20GK105865455SQ201610403806
【公開日】2016年8月17日
【申請(qǐng)日】2016年6月8日
【發(fā)明人】舒勝, 胡強(qiáng), 宋璟, 蘇浩秦
【申請(qǐng)人】中國航天空氣動(dòng)力技術(shù)研究院