[發(fā)明專(zhuān)利]一種在發(fā)射坐標(biāo)系下的飛行器姿態(tài)對(duì)準(zhǔn)方法及系統(tǒng)

有效

• 申請(qǐng)?zhí)枺?/td>: 202011482413.X
• 申請(qǐng)日: 2020-12-16
• 公開(kāi)（公告）號(hào): CN112611394B
• 公開(kāi)（公告）日: 2022-08-16
• 發(fā)明（設(shè)計(jì)）人: 陳凱;沈付強(qiáng);周鈞;趙子祥;梁文超
• 申請(qǐng)（專(zhuān)利權(quán)）人: 西北工業(yè)大學(xué)
• 主分類(lèi)號(hào): G01C25/00
• 分類(lèi)號(hào): G01C25/00
• 代理公司: 北京高沃律師事務(wù)所 11569
• 代理人: 王愛(ài)濤

1.一種飛行器姿態(tài)對(duì)準(zhǔn)方法，其特征在于，包括：

獲取發(fā)射時(shí)刻飛行器的初始緯度、發(fā)射方位角、陀螺儀輸出角速度、加速度計(jì)的輸出和地球自轉(zhuǎn)角速度；

根據(jù)所述初始緯度和所述發(fā)射方位角，計(jì)算地心慣性坐標(biāo)系到發(fā)射坐標(biāo)系的轉(zhuǎn)換矩陣；

根據(jù)所述陀螺儀輸出角速度，計(jì)算載體坐標(biāo)系到載體慣性坐標(biāo)系的轉(zhuǎn)換矩陣；

根據(jù)所述加速度計(jì)的輸出、所述地球自轉(zhuǎn)角速度和所述載體坐標(biāo)系到載體慣性坐標(biāo)系的轉(zhuǎn)換矩陣，采用雙矢量定姿方法計(jì)算載體慣性坐標(biāo)系到地心慣性坐標(biāo)系的轉(zhuǎn)換矩陣；

將所述地心慣性坐標(biāo)系到發(fā)射坐標(biāo)系的轉(zhuǎn)換矩陣、所述載體慣性坐標(biāo)系到地心慣性坐標(biāo)系的轉(zhuǎn)換矩陣與所述載體坐標(biāo)系到載體慣性坐標(biāo)系的轉(zhuǎn)換矩陣的乘積確定為發(fā)射坐標(biāo)系下飛行器發(fā)射前的姿態(tài)矩陣。

2.根據(jù)權(quán)利要求1所述的飛行器姿態(tài)對(duì)準(zhǔn)方法，其特征在于，所述根據(jù)所述初始緯度和所述發(fā)射方位角，計(jì)算地心慣性坐標(biāo)系到發(fā)射坐標(biāo)系的轉(zhuǎn)換矩陣，具體包括：

根據(jù)所述初始緯度和所述發(fā)射方位角，采用如下公式計(jì)算發(fā)射坐標(biāo)系到地心慣性坐標(biāo)系的轉(zhuǎn)換矩陣；

對(duì)所述發(fā)射坐標(biāo)系到地心慣性坐標(biāo)系的轉(zhuǎn)換矩陣進(jìn)行正交變換，得到地心慣性坐標(biāo)系到發(fā)射坐標(biāo)系的轉(zhuǎn)換矩陣；所述地心慣性坐標(biāo)系到發(fā)射坐標(biāo)系的轉(zhuǎn)換矩陣如下：

式中，為發(fā)射坐標(biāo)系到地心慣性坐標(biāo)系的轉(zhuǎn)換矩陣，i為地心慣性坐標(biāo)系，g為發(fā)射坐標(biāo)系，為繞y軸旋轉(zhuǎn)的余弦矩陣，為繞x軸旋轉(zhuǎn)的余弦矩陣，為在發(fā)射點(diǎn)飛行器的初始緯度，為發(fā)射方位角，為地心慣性坐標(biāo)系到發(fā)射坐標(biāo)系的轉(zhuǎn)換矩陣。

3.根據(jù)權(quán)利要求2所述的飛行器姿態(tài)對(duì)準(zhǔn)方法，其特征在于，所述根據(jù)所述陀螺儀輸出角速度，計(jì)算載體坐標(biāo)系到載體慣性坐標(biāo)系的轉(zhuǎn)換矩陣，具體包括：

獲取上一時(shí)刻的姿態(tài)四元數(shù)；

根據(jù)所述陀螺儀輸出角速度采用如下公式計(jì)算載體坐標(biāo)系相對(duì)于地心慣性坐標(biāo)系的等效旋轉(zhuǎn)矢量；

根據(jù)所述等效旋轉(zhuǎn)矢量采用如下公式計(jì)算變換四元數(shù)；

根據(jù)所述上一時(shí)刻的姿態(tài)四元數(shù)和所述變換四元數(shù)，采用如下公式計(jì)算當(dāng)前時(shí)刻的姿態(tài)四元數(shù)；

將所述當(dāng)前時(shí)刻的姿態(tài)四元數(shù)轉(zhuǎn)換為載體坐標(biāo)系到載體慣性坐標(biāo)系的轉(zhuǎn)換矩陣；所述載體坐標(biāo)系到載體慣性坐標(biāo)系的轉(zhuǎn)換矩陣如下：

其中，

式中，為從時(shí)刻到時(shí)刻載體坐標(biāo)系相對(duì)于地心慣性坐標(biāo)系的等效旋轉(zhuǎn)矢量，為陀螺儀輸出角速度，t為時(shí)刻，b為載體坐標(biāo)系，為載體坐標(biāo)系從時(shí)刻到時(shí)刻的變換四元數(shù)，為載體慣性坐標(biāo)系，為載體坐標(biāo)系相對(duì)于載體慣性坐標(biāo)系在時(shí)刻的姿態(tài)四元數(shù)，為載體坐標(biāo)系相對(duì)于載體慣性坐標(biāo)系在時(shí)刻的姿態(tài)四元數(shù)，為載體坐標(biāo)系到載體慣性坐標(biāo)系的轉(zhuǎn)換矩陣，為第一元數(shù)，為第二元數(shù)，為第三元數(shù)，為第四元數(shù)。

4.根據(jù)權(quán)利要求3所述的飛行器姿態(tài)對(duì)準(zhǔn)方法，其特征在于，所述根據(jù)所述加速度計(jì)的輸出、所述地球自轉(zhuǎn)角速度和所述載體坐標(biāo)系到載體慣性坐標(biāo)系的轉(zhuǎn)換矩陣，采用雙矢量定姿方法計(jì)算載體慣性坐標(biāo)系到地心慣性坐標(biāo)系的轉(zhuǎn)換矩陣，具體包括：

根據(jù)所述加速度計(jì)的輸出和所述載體坐標(biāo)系到載體慣性坐標(biāo)系的轉(zhuǎn)換矩陣，采用如下公式計(jì)算載體慣性坐標(biāo)系下的重力矢量；

式中，為t時(shí)刻載體慣性坐標(biāo)系下的重力矢量，為t時(shí)刻加速度計(jì)的輸出；

根據(jù)所述地球自轉(zhuǎn)角速度，采用如下公式計(jì)算地心慣性坐標(biāo)系下的重力矢量；

其中，

式中，為t時(shí)刻地心慣性坐標(biāo)系下的重力矢量，為地心地固坐標(biāo)系到地心慣性坐標(biāo)系的轉(zhuǎn)換矩陣，為地理坐標(biāo)系到地心地固坐標(biāo)系的裝換矩陣，為地理坐標(biāo)系下的重力矢量，g為重力加速度，L為在飛行軌跡中飛行器的緯度，為地球自轉(zhuǎn)角速度；

根據(jù)所述載體慣性坐標(biāo)系下的重力矢量和所述地心慣性坐標(biāo)系下的重力矢量，采用雙矢量定姿方法計(jì)算載體慣性坐標(biāo)系到地心慣性坐標(biāo)系的轉(zhuǎn)換矩陣。