CN105973237B - 基于实际飞行数据插值的仿真动态轨迹解析生成方法 - Google Patents
基于实际飞行数据插值的仿真动态轨迹解析生成方法 Download PDFInfo
- Publication number
- CN105973237B CN105973237B CN201610290487.0A CN201610290487A CN105973237B CN 105973237 B CN105973237 B CN 105973237B CN 201610290487 A CN201610290487 A CN 201610290487A CN 105973237 B CN105973237 B CN 105973237B
- Authority
- CN
- China
- Prior art keywords
- formula
- multinomial
- section
- represent
- attitude quaternion
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种基于实际飞行数据插值的仿真动态轨迹解析生成方法，步骤包括：对实际飞行数据进行坐标系转换得到ECI坐标系下的位置、速度、姿态；对姿态、位置、重力分别进行三次样条函数插值并计算角速率解析表达式、载体系加速度样条多项式、载体系重力样条多项式，计算扰动加速度积分增量理论值和陀螺角增量理论值、积分计算加速度增量理论值、积分计算重力积分增量理论值；计算飞行器的加速度计比力积分增量；根据加速度计比力积分增量、陀螺角增量理论值进行惯导解算得到导航飞行器的动态轨迹。本发明能够完全满足组合导航动态仿真精度要求、同样也适用于其他高精度高动态导航系统和刚体运动控制仿真中的角运动、线运动传感器信号模拟。
Description
技术领域
本发明涉及INS/GNSS组合导航技术，具体涉及一种基于实际飞行数据插值的仿真动态轨迹解析生成方法。
背景技术
利用信号模拟器进行科学研究、系统测试和性能评估是目前广泛使用的方法，具有成本低、高效、快速的特点。而在对捷联惯导系统、惯性/卫星组合导航系统的研究和实验中，也离不开轨迹发生器的研究和应用。国内外对此做了大量细致的研究，可分为以下几类：
一、基于载体运动学方程的典型轨迹拼接，其中最经典的轨迹发生器当属美国空军航空电子实验室的PROFGEN(Musick S H.,PROFGEN-A computer program forgenerating flight profiles[R]//Air Force Avionics Lab.Wright-Patterson AFBOhio,ADA034993,1977.)。PROFGEN能够支持四种基本的航迹仿真生成：垂直转弯、水平转弯、正弦航向变化和直线飞行。类似地，文献[2](Li,G.X.,Mao Y.L.,Song,C.L.,Designand Simulation of Trajectory Generator[C]//IEEE Computer Society/The 4thInternational Conference on Intelligent Human-Machine Systems andCybernetics,2012,201-205.)、文献[3](范欣，张福斌，李晓晖.一种水下航行器轨迹发生器的设计与仿真[J].鱼雷技术，2010,18(3)：214-217.)、文献[4](李海静、陈伟强.机载武器轨迹发生器设计与仿真[J].科学技术与工程，2010,10(13)：3158-3162.)将几种飞行轨迹(如直航、转弯、爬升和俯冲等)或水下航行器的运动轨迹(如直航、转弯和爬潜等)进行拼接，在此基础上产生比力、角速度信号，计算相应的姿态、速度和位置。虽然能实现一定的惯性导航和组合导航仿真测试功能，但所生成轨迹与载体线运动、角运动实际情况还存在较大差别。
二、基于载体动力学方程计算飞行仿真条件下的比力、角速度信号，文献[5]([5]李军伟，程咏梅，陈克吉，等.基于飞行仿真的捷联惯导算法测试平台[J].中国惯性技术学报，2012,20(5)：530-535.)在建立六自由度飞机非线性动力学模型基础上，实现了预定航迹多模态的飞行仿真；文献[6](陈凯、卫凤、张前程等.基于飞行力学的惯导轨迹发生器及其在半实物仿真中的应用[J].中国惯性技术学报，2014,22(4)：486-491.)基于飞行器六自由度模型进行轨迹生成，轨迹的运动由飞行控制系统实现。所生成轨迹与载体线运动、角运动实际情况的接近程度取决于动力学模型的准确度。
三、基于组合导航实际实验数据和捷联惯性导航数值解算方程，文献[7](G.M.Yan.,J.L.Wang and X.Y.～Zhou.,High-Precision Simulator for StrapdownInertial Navigation Systems Based on Real Dynamics from GNSS and IMUIntegration[C]//Springer Verlag Berlin Heidelberg/China Satellite NavigationConference(CSNC)2015,Proceedings:Volume III,Lecture Notes in ElectricalEngineering 342,(2015),789-799.)在当地水平地理坐标系下，对GPS/SINS组合导航卡尔曼滤波得到的100Hz欧拉角姿态、速度和位置输出进行插值，得到200Hz频率下离散时间点的等效转动矢量、速度和位置，通过捷联惯性导航数值解算算法的逆运算，得到角增量和加速度计比力积分增量。因此，该方法不能对捷联惯性导航数值解算算法本身进行仿真验证。由于没有角速率及其一阶导数的仿真数据，也没有对杆臂效应进行模拟。
发明内容
本发明要解决的技术问题：针对现有技术的上述问题，提供一种完全满足组合导航动态仿真精度要求、适用于其他高精度高动态导航系统和刚体运动控制仿真中的角运动、线运动传感器信号模拟的基于实际飞行数据插值的仿真动态轨迹解析生成方法。
为了解决上述技术问题，本发明采用的技术方案为：
一种基于实际飞行数据插值的仿真动态轨迹解析生成方法，步骤包括：
1)对实际飞行数据进行坐标系转换，得到ECI坐标系下的位置、速度、姿态；
2)对姿态进行三次样条函数插值并计算角速率解析表达式A，根据角速率解析表达式A计算扰动加速度积分增量理论值A1和陀螺角增量理论值A2；
3)对位置进行三次样条函数插值并计算载体系加速度样条多项式B，根据载体系加速度样条多项式B进行积分计算加速度增量理论值B1；
4)对重力进行三次样条函数插值并计算载体系重力样条多项式C，根据载体系重力样条多项式C进行积分计算重力积分增量理论值C1；
5)根据扰动加速度积分增量理论值A1、加速度增量理论值B1、重力积分增量理论值C1计算飞行器的加速度计比力积分增量D1；
6)根据加速度计比力积分增量D1、陀螺角增量理论值A2进行惯导解算得到导航飞行器的动态轨迹。
优选地，所述步骤2)的详细步骤包括：
2.1)根据姿态欧拉角计算姿态四元数；
2.2)对姿态四元数以指定时间间隔进行三次样条函数插值，得到姿态四元数各个分量的三次样条多项式；对所述三次样条多项式求导得到三次样条多项式的一阶导数，代入时间得到姿态四元数各个分量及其一阶导数的离散值；根据预设的四元数范数约束条件，对姿态四元数各个分量及其一阶导数的离散值进行修正；
2.3)根据修正后的姿态四元数各个分量及其一阶导数的离散值，将[tk,tk+1]转换到[0,T]区间，分段计算得到[0,T]区间的姿态四元数各个分量的三次样条多项式；
2.4)将转换到区间[0,T]姿态四元数各个分量的三次样条多项式利用泰勒近似展开得到角速率解析表达式A；
2.5)根据角速率解析表达式A计算扰动加速度积分增量理论值A1；
2.6)根据角速率解析表达式A计算陀螺角增量理论值A2。
优选地，所述步骤2.1)计算姿态四元数的函数表达式如式(1)所示；
式(1)中，q0,q1,q2,q3分别为姿态四元数的四个元素，γ为姿态的滚转角，为姿态的俯仰角，为姿态的偏航角；
所述步骤2.2)中，姿态四元数各个分量的三次样条多项式的函数表达式如式(2)所示，预设的四元数范数约束条件如式(3)所示，对姿态四元数各个分量进行修正的函数表达式如式(4)所示，对姿态四元数各个分量的一阶导数的离散值进行修正的函数表达式如式(5)所示；
式(2)中，为姿态四元数各个分量的三次样条多项式，ai,bi,ci,di分别为样条插值后三次样条多项式的系数，t为时间；
式(3)中，||q(t)||代表t时刻的姿态四元数q(t)的范数，q0(t),q1(t),q2(t),q3(t)分别为姿态四元数q(t)的四个元素，分别为姿态四元数q(t)的四个元素的一阶导数；
式(4)中，qi(tk)代表插值点tk时刻的姿态四元数各个分量的修正结果，代表插值点tk时刻姿态四元数的各个分量，代表插值点tk时刻姿态四元数的范数；
式(5)中，代表插值点tk时刻姿态四元数中各个分量的一阶导数的修正结果，代表插值点tk时刻姿态四元数中各个分量的一阶导数，代表插值点tk时刻姿态四元数的范数；代表插值点tk时刻姿态四元数的范数的求导结果，其函数表达式如式(6)所示；
式(6)中，分别为插值点tk时刻的姿态四元数的四个元素，分别为插值点tk时刻的姿态四元数的四个元素一阶导数；
所述步骤2.3)分段计算得到[0,T]区间的姿态四元数各个分量的三次样条多项式如式(7)所示；
式(7)中，代表转换到区间[0,T]的插值样条多项式，ai,k,bi,k,ci,k,di,k分别为转换到[0,T]区间后的三次样条多项式系数，t代表时间；转换到[0,T]区间后的三次样条多项式系数的计算函数表达式如式(8)所示；
式(8)中，T代表分段样条的分段间隔，qi(tk)代表tk时刻的姿态四元数中第i个元素的值，代表tk时刻姿态四元数中第i个元素的一阶导数值，qi(tk+1)代表tk+1时刻的姿态四元数中第i个元素的值，代表tk+1时刻姿态四元数中第i个元素的一阶导数值，符号i取值为0或1或2或3，ai,k,bi,k,ci,k,di,k分别为转换到区间[0,T]后的插值多项式系数；
所述步骤2.4)的详细步骤包括：根据式(9)所示的限制条件代入式(10)所示角速率的计算式，对姿态四元数范数的平方的倒数部分进行泰勒近似展开，得到角速率解析表达式A如式(11)所述；
式(9)中，qk(t)代表姿态四元数，代表原始数据区间为[tk,tk+1]转换到[0,T]区间后的姿态四元数，代表原始数据区间为[tk,tk+1]转换到[0,T]区间后的姿态四元数的范数，代表姿态四元数的一阶导数，代表原始数据区间为[tk,tk+1]转换到[0,T]区间后的姿态四元数的一阶导数，分别代表原始[tk,tk+1]区间转换到区间[0,T]后的姿态四元数各个元素，分别代表原始[tk,tk+1]区间转换到[0,T]区间后的姿态四元数各个元素的一阶导数，t代表时间；
式(10)中，代表角速率解析表达式A，所述角速率解析表达式A为姿态四元数的样条多项式，下标ib代表载体坐标系相对于惯性坐标系、k代表原始数据为区间[tk,tk+1]、上标b代表载体坐标系；qk(t)代表姿态四元数的样条多项式，代表姿态四元数的样条多项式的一阶导数，t代表时间；
式(11)中，代表角速率的时间函数，下标ib代表载体坐标系相对于惯性坐标系、k代表原始数据为区间[tk,tk+1]、上标b代表载体坐标系；代表原始数据区间为[tk,tk+1]转换到[0,T]区间后的姿态四元数的范数，分别代表原始[tk,tk+1]区间转换到区间[0,T]后的姿态四元数各个元素，分别代表原始[tk,tk+1]区间转换到[0,T]区间后的姿态四元数各个元素的一阶导数，t代表时间；
所述步骤2.5)包括首先根据式(12)所示的扰动加速度解析积分表达式计算扰动加速度积分理论值，然后根据指定时间间隔△T的扰动加速度积分理论值δυk(t)和δυk(t+△T)计算差值，得到指定的时间间隔△T内的扰动加速度积分增量理论值A1；
式(12)中，δυk(t)代表[0,t]时间的扰动加速度积分理论值，分别为角速率分别在x、y、z方向的解析表达式；代表x方向加速度计偏离质心的杆臂矢量中的x方向偏移量，代表y方向加速度计偏离质心的杆臂矢量中的y方向偏移量，代表z方向加速度计偏离质心的杆臂矢量中的z方向偏移量， 分别为三个方向的加速度计偏离质心的杆臂矢量且均为常值，t代表时间；
所述步骤2.6)具体是指根据式(13)计算陀螺角增量理论值A2；
式(13)中，θk(t)代表由角速率解析表达式A积分得到的陀螺角增量，代表角速率解析表达式A，所述角速率解析表达式A为姿态四元数的样条多项式，下标ib代表载体坐标系相对于惯性坐标系、k代表原始数据为区间[tk,tk+1]、上标b代表载体坐标系，△θk,j+1代表△T时间间隔内的陀螺角增量理论值A2，θk(tj)代表[0,tj]时间的陀螺角积分增量，θk(tj+△T)代表[0,tj+△T]时间的陀螺角积分增量，t代表时间。
优选地，所述步骤3)的详细步骤包括：
3.1)对ECI坐标系下位置的各个分量进行样条插值得到位置的样条多项式，对所得到的位置的样条多项式求一次导得到速度的样条多项式，分别代入时间得到位置和速度的离散值；
3.2)根据位置和速度的离散值，将[tk,tk+1]转换到[0,T]区间，分段计算得到[0,T]区间的位置各个分量的三次样条多项式，进行两次求导得到加速度各个分量的样条多项式其中k代表原始数据区间为[tk,tk+1]；
3.3)结合坐标系转换，得到载体系b中的加速度的解析表达式如式(14)所示；
式(14)中，代表载体系b中的加速度的样条多项式，代表原始数据为[tk,tk+1]区间转换到[0,T]区间的加速度的样条多项式，代表无误差惯性坐标系到载体坐标系的方向余弦矩阵，t代表时间，其中方向余弦矩阵与姿态四元数的转换关系如式(15)所示；
式(15)中，代表无误差惯性坐标系到载体坐标系的方向余弦矩阵，代表未修正时的惯性坐标系到载体坐标系的方向余弦矩阵， 分别代表未修正时的惯性坐标系到载体坐标系的方向余弦矩阵中的九个元素，代表原始数据为[tk,tk+1]转换到[0,T]区间的四元数的范数的平方，t代表时间；根据精度要求选择泰勒近似展开计算的解析多项式，得到载体系加速度样条多项式B；
3.4)根据载体系加速度样条多项式B的解析多项式进行积分计算加速度增量理论值B1。
优选地，所述步骤3.2)中分段计算得到[0,T]区间的位置各个分量的三次样条多项式如式(16)所示，两次求导得到加速度各个分量的样条多项式如式(17)所示；
式(16)和(17)中，分别代表位置各个分量,分别代表加速度各分量，ax,k,bx,k,cx,k,dx,k、ay,k,by,k,cy,k,dy,k、az,k,bz,k,cz,k,dz,k分别代表转换到区间[0,T]的样条多项式系数，t代表时间，转换到区间[0,T]的样条多项式系数之间的转换矩阵如式(18)所示；
式(18)中，T代表分段样条的分段间隔，ax,k,bx,k,cx,k,dx,k、ay,k,by,k,cy,k,dy,k、az,k,bz,k,cz,k,dz,k分别代表转换到区间[0,T]的样条多项式系数，代表原始数据tk时刻的位置的值，代表原始数据tk时刻的位置的导数值，代表原始数据tk+1时刻的位置的值，代表原始数据tk+1时刻的位置的导数值；
所述步骤3.3)得到的载体系加速度样条多项式B如式(19)所示；
式(19)中，代表选择泰勒近似展开后计算得到的载体系加速度样条多项式B，代表原始数据为[tk,tk+1]区间转换到[0,T]区间的加速度的样条多项式， 分别代表未修正时的惯性坐标系到载体坐标系的方向余弦矩阵中的九个元素，代表原始数据为[tk,tk+1]转换到[0,T]区间的四元数的范数的平方，t代表时间；其中，未修正时的惯性坐标系到载体坐标系的方向余弦矩阵中的九个元素如式(20)所示；
式(20)中，分别代表原始[tk,tk+1]区间转换到区间[0,T]后的姿态四元数各个元素的时间函数，t代表时间；
所述步骤3.4)中根据载体系加速度样条多项式B的解析多项式进行积分计算加速度增量理论值B1的函数表达式如式(21)所示；
式(21)中，代表加速度增量理论值B1，表示载体系加速度样条多项式B，t代表时间。
优选地，所述步骤4)的详细步骤包括：
4.1)对重力以指定时间间隔进行三次样条函数插值得到重力各分量的三次样条多项式，对重力各分量的三次样条多项式进行求导得到重力各分量的三次样条多项式的一阶导数，然后代入时间得到重力各个分量及其一阶导数的离散值；
4.2)根据重力各个分量及其一阶导数的离散值，将[tk,tk+1]转换到[0,T]区间，分段计算得到[0,T]区间的重力各个分量的三次样条多项式；
4.3)根据精度要求选择泰勒近似展开计算的解析多项式，得到载体系重力样条多项式C，其中代表原始数据为[tk,tk+1]转换到[0,T]区间的四元数的范数的平方，t代表时间；
4.4)根据载体系重力样条多项式C的解析多项式进行积分计算重力积分增量理论值C1。
优选地，所述步骤4.1)中重力各分量的三次样条多项式如式(22)所示；
式(22)中，代表重力在分别在x、y、z方向的分量，ax,bx,cx,dx、ay,by,cy,dy、az,bz,cz,dz分别代表样条插值后三次样条多项式的系数，t代表时间；
所述步骤4.2)中分段计算得到[0,T]区间的重力各分量的三次样条多项式如式(23)所示；
式(23)中，分别代表原数据区间为[tk,tk+1]转换到[0,T]区间后重力各分量的三次样条多项式,ax,k,bx,k,cx,k,dx,k、ay,k,by,k,cy,k,dy,k、az,k,bz,k,cz,k,dz,k代表原数据区间为[tk,tk+1]转换到[0,T]区间后的三次样条多项式的系数，t代表时间；转换到区间[0,T]的样条多项式系数之间的转换矩阵如式(24)所示；
式(24)中，T代表分段样条的分段间隔，ax,k,bx,k,cx,k,dx,k、ay,k,by,k,cy,k,dy,k、az,k,bz,k,cz,k,dz,k分别代表转换到[0,T]区间后三次样条多项式的系数，代表原始数据tk时刻的重力的值，代表原始数据tk时刻的重力的一阶导数值，代表原始数据tk+1时刻的重力的值，代表原始数据tk+1时刻的重力的一阶导数值；
所述步骤4.3)得到的载体系重力样条多项式C的函数表达式如式(25)所示；
式(25)中，代表原始数据为[tk,tk+1]区间转换到[0,T]区间的重力的样条多项式，分别代表未修正时的惯性坐标系到载体坐标系的方向余弦矩阵中的九个元素，代表原始数据为[tk,tk+1]转换到[0,T]区间的四元数的范数的平方，分别代表原始[tk,tk+1]区间转换到区间[0,T]后的姿态四元数各个元素的时间函数，t代表时间；其中，未修正时的惯性坐标系到载体坐标系的方向余弦矩阵中的九个元素如式(26)所示；
式(26)中，分别代表原始[tk,tk+1]区间转换到区间[0,T]后的姿态四元数各个元素的时间函数，t代表时间；
所述步骤4.4)根据载体系重力样条多项式C的解析多项式进行积分计算重力积分增量理论值C1的函数表达式如式(27)所示；
式(27)中，为载体系重力的积分增量理论值C1，代表载体系重力样条多项式C，t代表时间。
优选地，所述步骤5)具体是指根据式(28)所示函数表达式计算飞行器的加速度计比力积分增量D1；
式(28)中，υk(t)代表加速度计比力积分样条多项式，代表加速度增量理论值B1，代表重力积分增量理论值C1，△υk,j+1和△υk(tj+1)均代表△T时间间隔内不考虑杆臂效应等误差源的加速度计比力积分增量，υk(tj)代表[0,tj]时间的加速度计比力积分，υk(tj+△T)代表[0,tj+△T]时间的加速度计比力积分，代表飞行器的加速度计比力积分增量D1，所述飞行器的加速度计比力积分增量D1为考虑杆臂效应误差源的加速度计比力积分，δυk(tj+△T)代表[0,tj+△T]时间的扰动加速度积分增量理论值A1，δυk(tj)代表[0,tj]时间的扰动加速度积分增量理论值A1。
本发明基于实际飞行数据插值的仿真动态轨迹解析生成方法具有下述优点：本发明针对INS/GNSS组合导航仿真中，捷联惯导系统陀螺、加速度计信号高精度模拟问题，对实际飞行数据进行坐标系转换，得到ECI坐标系下的位置、速度、姿态，对姿态进行三次样条函数插值并计算角速率解析表达式、对位置进行三次样条函数插值并计算载体系加速度样条多项式、对重力进行三次样条函数插值并计算载体系重力样条多项式；根据角速率解析表达式计算扰动加速度积分增量理论值和陀螺角增量理论值，根据载体系加速度样条多项式进行积分计算加速度增量理论值，根据载体系重力样条多项式进行积分计算重力积分增量理论值；根据扰动加速度积分增量理论值进行修正加速度增量理论值，根据重力积分增量理论值、加速度增量理论值计算飞行器的加速度计比力积分增量，根据加速度计比力积分增量、陀螺角增量理论值进行惯导解算得到导航飞行器的动态轨迹符合载体运动学和动力学特性，反映杆臂效应等误差源的影响，同时与经事后处理的实测GNSS伪距、伪距率等数据特征保持一致，且本发明基于某实际无人机飞行数据，验证了所提出算法的有效性，能够完全满足组合导航动态仿真精度要求、适用于其他高精度高动态导航系统和刚体运动控制仿真中的角运动、线运动传感器信号模拟。
附图说明
图1为本发明实施例方法的基本流程示意图。
图2为发明实施例中的载体飞行轨迹。
图3为发明实施例中的载体姿态角变化曲线。
图4为发明实施例中姿态各分量的误差变化曲线。
图5为发明实施例中速度各分量的误差变化曲线。
图6为发明实施例中位置各分量的误差变化曲线。
具体实施方式
如图1所示，本实施例基于实际飞行数据插值的仿真动态轨迹解析生成方法的步骤包括：
1)对实际飞行数据进行坐标系转换，得到ECI坐标系下的位置、速度、姿态；
2)对姿态进行三次样条函数插值并计算角速率解析表达式A，根据角速率解析表达式A计算扰动加速度积分增量理论值A1和陀螺角增量理论值A2；
3)对位置进行三次样条函数插值并计算载体系加速度样条多项式B，根据载体系加速度样条多项式B进行积分计算加速度增量理论值B1；
4)对重力进行三次样条函数插值并计算载体系重力样条多项式C，根据载体系重力样条多项式C进行积分计算重力积分增量理论值C1；
5)根据扰动加速度积分增量理论值A1、加速度增量理论值B1、重力积分增量理论值C1计算飞行器的加速度计比力积分增量D1；
6)根据加速度计比力积分增量D1、陀螺角增量理论值A2进行惯导解算得到导航飞行器的动态轨迹。
本实施例步骤1)中，具体是指将无人机的实际飞行数据经事后滤波处理，再进行坐标系转换，得到ECI坐标系下与实测飞行过程中卫星伪距、伪距变化率数据特征一致的200Hz频率的位置信息和速度信息。实际飞行数据中1Hz频率的飞机GPS/INS组合导航姿态信息虽然不是载体真实姿态，但基本反映了飞机实际姿态变化的运动学特性。由于所生成的陀螺、加速度计信号是时间的分段解析函数，因此不仅可以对杆臂效应进行模拟，而且可以对不同解算频率、不同机械编排的捷联惯性导航数值解算算法进行一定的比较和验证。
本实施例中，步骤2)的详细步骤包括：
2.1)根据姿态欧拉角计算姿态四元数；
2.2)对姿态四元数以指定时间间隔进行三次样条函数插值，得到姿态四元数各个分量的三次样条多项式；对三次样条多项式求导得到三次样条多项式的一阶导数，代入时间得到姿态四元数各个分量及其一阶导数的离散值；根据预设的四元数范数约束条件，对姿态四元数各个分量及其一阶导数的离散值进行修正；
2.3)根据修正后的姿态四元数各个分量及其一阶导数的离散值，将[tk,tk+1]转换到[0,T]区间，分段计算得到[0,T]区间的姿态四元数各个分量的三次样条多项式；
2.4)将转换到区间[0,T]姿态四元数各个分量的三次样条多项式利用泰勒近似展开得到角速率解析表达式A；
2.5)根据角速率解析表达式A计算扰动加速度积分增量理论值A1；
2.6)根据角速率解析表达式A计算陀螺角增量理论值A2。
本实施例中，步骤2.1)计算姿态四元数的函数表达式如式(1)所示；
式(1)中，q0,q1,q2,q3分别为姿态四元数的四个元素，γ为姿态的滚转角，为姿态的俯仰角，为姿态的偏航角；
步骤2.2)中，姿态四元数各个分量的三次样条多项式的函数表达式如式(2)所示，预设的四元数范数约束条件如式(3)所示，对姿态四元数各个分量进行修正的函数表达式如式(4)所示。根据样条插值误差界的相关理论，得到的各个区间[0,T]的姿态四元数样条多项式在非插值点仍不满足范数为1的约束条件，因此在计算陀螺角速率、角增量时也需要对姿态四元数范数进行修正，将角速率视为标量为0的四元数，对姿态四元数各个分量的一阶导数的离散值进行修正的函数表达式如式(5)所示；
式(2)中，为姿态四元数各个分量的三次样条多项式，ai,bi,ci,di分别为样条插值后三次样条多项式的系数，t为时间；
式(3)中，||q(t)||代表t时刻的姿态四元数q(t)的范数，q0(t),q1(t),q2(t),q3(t)分别为姿态四元数q(t)的四个元素，分别为姿态四元数q(t)的四个元素的一阶导数；
式(4)中，qi(tk)代表插值点tk时刻的姿态四元数各个分量的修正结果，代表插值点tk时刻姿态四元数的各个分量，代表插值点tk时刻姿态四元数的范数；
式(5)中，代表插值点tk时刻姿态四元数中各个分量的一阶导数的修正结果，代表插值点tk时刻姿态四元数中各个分量的一阶导数，代表插值点tk时刻姿态四元数的范数；代表插值点tk时刻姿态四元数的范数的求导结果，其函数表达式如式(6)所示；
式(6)中，分别为插值点tk时刻的姿态四元数的四个元素，分别为插值点tk时刻的姿态四元数的四个元素一阶导数；
所述步骤2.3)分段计算得到[0,T]区间的姿态四元数各个分量的三次样条多项式如式(7)所示；
式(7)中，代表转换到区间[0,T]的插值样条多项式，ai,k,bi,k,ci,k,di,k分别为转换到[0,T]区间后的三次样条多项式系数，t代表时间；转换到[0,T]区间后的三次样条多项式系数的计算函数表达式如式(8)所示；
式(8)中，T代表分段样条的分段间隔，qi(tk)代表tk时刻的姿态四元数中第i个元素的值，代表tk时刻姿态四元数中第i个元素的一阶导数值，qi(tk+1)代表tk+1时刻的姿态四元数中第i个元素的值，代表tk+1时刻姿态四元数中第i个元素的一阶导数值，符号i取值为0或1或2或3，ai,k,bi,k,ci,k,di,k分别为转换到区间[0,T]后的插值多项式系数；
本实施例中，步骤2.4)的详细步骤包括：根据式(9)所示的限制条件代入式(10)所示角速率的计算式，对姿态四元数范数的平方的倒数部分进行泰勒近似展开，得到角速率解析表达式A如式(11)所述；
式(9)中，qk(t)代表姿态四元数，代表原始数据区间为[tk,tk+1]转换到[0,T]区间后的姿态四元数，代表原始数据区间为[tk,tk+1]转换到[0,T]区间后的姿态四元数的范数，代表姿态四元数的一阶导数，代表原始数据区间为[tk,tk+1]转换到[0,T]区间后的姿态四元数的一阶导数，分别代表原始[tk,tk+1]区间转换到区间[0,T]后的姿态四元数各个元素，分别代表原始[tk,tk+1]区间转换到[0,T]区间后的姿态四元数各个元素的一阶导数，t代表时间；
式(10)中，代表角速率解析表达式A，角速率解析表达式A为姿态四元数的样条多项式，下标ib代表载体坐标系相对于惯性坐标系、k代表原始数据为区间[tk,tk+1]、上标b代表载体坐标系；qk(t)代表姿态四元数的样条多项式，代表姿态四元数的样条多项式的一阶导数，t代表时间；
式(11)中，代表角速率的时间函数，下标ib代表载体坐标系相对于惯性坐标系、k代表原始数据为区间[tk,tk+1]、上标b代表载体坐标系；代表原始数据区间为[tk,tk+1]转换到[0,T]区间后的姿态四元数的范数，分别代表原始[tk,tk+1]区间转换到区间[0,T]后的姿态四元数各个元素，分别代表原始[tk,tk+1]区间转换到[0,T]区间后的姿态四元数各个元素的一阶导数，t代表时间；
对姿态四元数范数的平方的倒数部分进行泰勒近似展开时，可以根据需要选择其近似展开阶数，例如进行泰勒一阶和二阶近似展开时展开结果分别如式(11-1)和(11-2)所示。将展开结果代入式(11)，则可以得到角速率解析表达式A的函数表达式如式(11-3)或(11-4)所示。
式(11-1)中，代表原始数据区间为[tk,tk+1]转换到[0,T]区间后的姿态四元数的范数，t代表时间；
式(11-2)中，代表原始数据区间为[tk,tk+1]转换到[0,T]区间后的姿态四元数的范数，t代表时间；
式(11-3)和式(11-4)中，代表角速率解析表达式A，角速率解析表达式A为姿态四元数的样条多项式，下标ib代表载体坐标系相对于惯性坐标系、k代表原始数据为区间[tk,tk+1]、上标b代表载体坐标系；代表原始数据区间为[tk,tk+1]转换到[0,T]区间后的姿态四元数的范数的样条多项式，分别代表原始[tk,tk+1]区间转换到区间[0,T]后的姿态四元数各个元素的时间函数，分别代表原始[tk,tk+1]区间转换到[0,T]区间后的姿态四元数各个元素的时间函数的一阶导数，t代表时间；
加速度计偏离IMU质心而造成的杆臂效应或尺寸效应引起的扰动加速度，飞行器中三个方向的加速度计由于物理结构上的差异，其相对IMU质心的偏倚量各不相同，因此考虑杆臂效应或尺寸效应引起的扰动加速度，使得仿真动态轨迹解析更加准确。本实施例中，步骤2.5)包括首先根据式(12)所示的扰动加速度解析积分表达式计算扰动加速度积分理论值，然后根据指定时间间隔△T的扰动加速度积分理论值δυk(t)和δυk(t+△T)计算差值，得到指定的时间间隔△T内的扰动加速度积分增量理论值A1；
式(12)中，δυk(t)代表[0,t]时间的扰动加速度积分理论值，分别为角速率分别在x、y、z方向的解析表达式；代表x方向加速度计偏离质心的杆臂矢量中的x方向偏移量，代表y方向加速度计偏离质心的杆臂矢量中的y方向偏移量，代表z方向加速度计偏离质心的杆臂矢量中的z方向偏移量， 分别为三个方向的加速度计偏离质心的杆臂矢量且均为常值，t代表时间；
步骤2.6)具体是指根据式(13)计算陀螺角增量理论值A2；
式(13)中，θk(t)代表由角速率解析表达式A积分得到的陀螺角增量，代表角速率解析表达式A，所述角速率解析表达式A为姿态四元数的样条多项式，下标ib代表载体坐标系相对于惯性坐标系、k代表原始数据为区间[tk,tk+1]、上标b代表载体坐标系，△θk,j+1代表△T时间间隔内的陀螺角增量理论值A2，θk(tj)代表[0,tj]时间的陀螺角积分增量，θk(tj+△T)代表[0,tj+△T]时间的陀螺角积分增量，t代表时间。
对1Hz ECI坐标系下位置信息进行插值，得到位置的三次样条解析多项式，求导得到速度的二次样条解析多项式。同样，为避免时间增大带来的误差影响，用类似式角速率的方法，将[tk,tk+1]转换到[0,T]区间，分段计算得到[0,T]区间的位置各个分量的三次样条多项式。本实施例中，步骤3)的详细步骤包括：
3.1)对ECI坐标系下位置的各个分量进行样条插值得到位置的样条多项式，对所得到的位置的样条多项式求一次导得到速度的样条多项式，分别代入时间得到位置和速度的离散值；
3.2)根据位置和速度的离散值，将[tk,tk+1]转换到[0,T]区间，分段计算得到[0,T]区间的位置各个分量的三次样条多项式，进行两次求导得到加速度各个分量的样条多项式其中k代表原始数据区间为[tk,tk+1]；
3.3)结合坐标系转换，得到载体系b中的加速度的解析表达式如式(14)所示；
式(14)中，代表载体系b中的加速度的样条多项式，代表原始数据为[tk,tk+1]区间转换到[0,T]区间的加速度的样条多项式，代表无误差惯性坐标系到载体坐标系的方向余弦矩阵，t代表时间，其中方向余弦矩阵与姿态四元数的转换关系如式(15)所示；
式(15)中，代表无误差惯性坐标系到载体坐标系的方向余弦矩阵，代表未修正时的惯性坐标系到载体坐标系的方向余弦矩阵， 分别代表未修正时的惯性坐标系到载体坐标系的方向余弦矩阵中的九个元素，代表原始数据为[tk,tk+1]转换到[0,T]区间的四元数的范数的平方，t代表时间；根据精度要求选择泰勒近似展开计算的解析多项式，得到载体系加速度样条多项式B；
3.4)根据载体系加速度样条多项式B的解析多项式进行积分计算加速度增量理论值B1。
本实施例中，步骤3.2)中分段计算得到[0,T]区间的位置各个分量的三次样条多项式如式(16)所示，两次求导得到加速度各个分量的样条多项式如式(17)所示；
式(16)和(17)中，分别代表位置各个分量,分别代表加速度各分量，ax,k,bx,k,cx,k,dx,k、ay,k,by,k,cy,k,dy,k、az,k,bz,k,cz,k,dz,k分别代表转换到区间[0,T]的样条多项式系数，t代表时间，转换到区间[0,T]的样条多项式系数之间的转换矩阵如式(18)所示；
式(18)中，T代表分段样条的分段间隔，ax,k,bx,k,cx,k,dx,k、ay,k,by,k,cy,k,dy,k、az,k,bz,k,cz,k,dz,k分别代表转换到区间[0,T]的样条多项式系数，代表原始数据tk时刻的位置的值，代表原始数据tk时刻的位置的导数值，代表原始数据tk+1时刻的位置的值，代表原始数据tk+1时刻的位置的导数值；
步骤3.3)得到的载体系加速度样条多项式B如式(19)所示；
式(19)中，代表选择泰勒近似展开后计算得到的载体系加速度样条多项式B，代表原始数据为[tk,tk+1]区间转换到[0,T]区间的加速度的样条多项式， 分别代表未修正时的惯性坐标系到载体坐标系的方向余弦矩阵中的九个元素，代表原始数据为[tk,tk+1]转换到[0,T]区间的四元数的范数的平方，t代表时间；其中，未修正时的惯性坐标系到载体坐标系的方向余弦矩阵中的九个元素如式(20)所示；
式(20)中，分别代表原始[tk,tk+1]区间转换到区间[0,T]后的姿态四元数各个元素的时间函数，t代表时间；
针对步骤3.3)得到的载体系加速度样条多项式B，根据对选择的泰勒近似开展方式不同而有所不同。例如采用泰勒一阶近似展开时，具体得到的载体系加速度样条多项式B如式(19-1)所示；采用泰勒二阶近似展开时，具体得到的载体系加速度样条多项式B如式(19-2)所示；
式(19-1)为选择泰勒一阶近似展开计算的解析多项式得到的载体系加速度样条多项式B，式(19-2)为选择泰勒二阶近似展开计算的解析多项式得到的载体系加速度样条多项式B，式(19-1)和式(19-2)中，代表载体系加速度样条多项式B，代表原始数据为[tk,tk+1]区间转换到[0,T]区间的加速度的样条多项式，代表无误差惯性坐标系到载体坐标系的方向余弦矩阵， 分别代表未修正时的惯性坐标系到载体坐标系的方向余弦矩阵中的九个元素，代表原始数据为[tk,tk+1]转换到[0,T]区间的四元数的范数的平方， 分别代表原始[tk,tk+1]区间转换到区间[0,T]后的姿态四元数各个元素的时间函数，t代表时间。
步骤3.4)中根据载体系加速度样条多项式B的解析多项式进行积分计算加速度增量理论值B1的函数表达式如式(21)所示；
式(21)中，代表加速度增量理论值B1，表示载体系加速度样条多项式B，t代表时间。
同理，加速度增量理论值B1根据对选择的泰勒近似开展方式不同而有所不同。例如采用泰勒一阶近似展开时，具体得到的加速度增量理论值B1如式(21-1)所示；采用泰勒二阶近似展开时，具体得到的加速度增量理论值B1如式(21-2)所示，其中载体系加速度样条多项式B已代入式(19)；
式(21-1)和(21-2)中，代表加速度增量理论值B1，代表原始数据区间为[tk,tk+1]转换到[0,T]区间后的姿态四元数的范数的样条多项式， 分别代表未修正时的惯性坐标系到载体坐标系的方向余弦矩阵中的九个元素，代表原始数据区间为[tk,tk+1]转换到[0,T]区间的惯性坐标系下加速度的样条多项式，t代表时间。
本实施例中，步骤4)的详细步骤包括：
4.1)对重力以指定时间间隔进行三次样条函数插值得到重力各分量的三次样条多项式，对重力各分量的三次样条多项式进行求导得到重力各分量的三次样条多项式的一阶导数，然后代入时间得到重力各个分量及其一阶导数的离散值；
4.2)根据重力各个分量及其一阶导数的离散值，将[tk,tk+1]转换到[0,T]区间，分段计算得到[0,T]区间的重力各个分量的三次样条多项式；
4.3)根据精度要求选择泰勒近似展开计算的解析多项式，得到载体系重力样条多项式C，其中代表原始数据为[tk,tk+1]转换到[0,T]区间的四元数的范数的平方，t代表时间；
4.4)根据载体系重力样条多项式C的解析多项式进行积分计算重力积分增量理论值C1。
根据重力计算式，由ECI系下的位置值得到离散的重力值。根据重力数据求飞行航迹中重力各分量的三次样条多项式，同样为减小时间增大带来的误差影响，将样条多项式由[tk,tk+1]转换到[0,T]区间，分段计算得到[0,T]区间的重力各个分量的三次样条多项式。
本实施例中，步骤4.1)中重力各分量的三次样条多项式如式(22)所示；
式(22)中，代表重力在分别在x、y、z方向的分量，ax,bx,cx,dx、ay,by,cy,dy、az,bz,cz,dz分别代表样条插值后三次样条多项式的系数，t代表时间；
本实施例中，步骤4.2)中分段计算得到[0,T]区间的重力各分量的三次样条多项式如式(23)所示；
式(23)中，分别代表原数据区间为[tk,tk+1]转换到[0,T]区间后重力各分量的三次样条多项式,ax,k,bx,k,cx,k,dx,k、ay,k,by,k,cy,k,dy,k、az,k,bz,k,cz,k,dz,k代表原数据区间为[tk,tk+1]转换到[0,T]区间后的三次样条多项式的系数，t代表时间；转换到区间[0,T]的样条多项式系数之间的转换矩阵如式(24)所示；
式(24)中，T代表分段样条的分段间隔，ax,k,bx,k,cx,k,dx,k、ay,k,by,k,cy,k,dy,k、az,k,bz,k,cz,k,dz,k分别代表转换到[0,T]区间后三次样条多项式的系数，代表原始数据tk时刻的重力的值，代表原始数据tk时刻的重力的一阶导数值，代表原始数据tk+1时刻的重力的值，代表原始数据tk+1时刻的重力的一阶导数值；
本实施例中，步骤4.3)得到的载体系重力样条多项式C的函数表达式如式(25)所示；
式(25)中，代表原始数据为[tk,tk+1]区间转换到[0,T]区间的重力的样条多项式，分别代表未修正时的惯性坐标系到载体坐标系的方向余弦矩阵中的九个元素，代表原始数据为[tk,tk+1]转换到[0,T]区间的四元数的范数的平方，分别代表原始[tk,tk+1]区间转换到区间[0,T]后的姿态四元数各个元素的时间函数，t代表时间；其中，未修正时的惯性坐标系到载体坐标系的方向余弦矩阵中的九个元素如式(26)所示；
式(26)中，分别代表原始[tk,tk+1]区间转换到区间[0,T]后的姿态四元数各个元素的时间函数，t代表时间；
步骤4.3)得到的载体系重力样条多项式C根据选择泰勒展开方式不同而有所不同，例如采用一阶泰勒近似展开时得到的载体系重力样条多项式C的函数表达式如式(25-1)所示，采用二阶泰勒近似展开时得到的载体系重力样条多项式C的函数表达式如式(25-2)所示；
式(25-1)为采用选择泰勒一阶近似展开计算的解析多项式得到的载体系重力样条多项式C，式(25-2)为采用选择泰勒二阶近似展开计算的解析多项式得到的载体系重力样条多项式C；式(25-1)和式(25-2)中，代表原始数据为[tk,tk+1]区间转换到[0,T]区间的重力的样条多项式，代表无误差惯性坐标系到载体坐标系的方向余弦矩阵，分别代表未修正时的惯性坐标系到载体坐标系的方向余弦矩阵中的九个元素，代表原始数据为[tk,tk+1]转换到[0,T]区间的四元数的范数的平方，分别代表原始[tk,tk+1]区间转换到区间[0,T]后的姿态四元数各个元素的时间函数，t代表时间。
本实施例中，步骤4.4)根据载体系重力样条多项式C的解析多项式进行积分计算重力积分增量理论值C1的函数表达式如式(27)所示；
式(27)中，为载体系重力的积分增量理论值C1，代表载体系重力样条多项式C，t代表时间。
同理，步骤4.4)得到的重力积分增量理论值C1的函数表达式根据选择泰勒展开方式不同而有所不同，例如采用一阶泰勒近似展开时得到的重力积分增量理论值C1的函数表达式如式(27-1)所示，采用二阶泰勒近似展开时得到的重力积分增量理论值C1的函数表达式如式(27-2)所示，其中载体系重力样条多项式C已代入式(27)；
式(27-1)和(27-2)中，为载体系重力的积分增量理论值C1，代表从原始区间[tk,tk+1]转换到[0,T]区间的惯性坐标系下重力的样条多项式，代表原始数据区间为[tk,tk+1]转换到[0,T]区间后的四元数时间函数对应的姿态四元数的范数，t代表时间， 分别代表未修正时的惯性坐标系到载体坐标系的方向余弦矩阵中的九个元素。
本实施例中，步骤5)具体是指根据式(28)所示函数表达式计算飞行器的加速度计比力积分增量D1；
式(28)中，υk(t)代表加速度计比力积分样条多项式，代表加速度增量理论值B1，代表重力积分增量理论值C1，△υk,j+1和△υk(tj+1)均代表△T时间间隔内不考虑杆臂效应等误差源的加速度计比力积分增量，υk(tj)代表[0,tj]时间的加速度计比力积分，υk(tj+△T)代表[0,tj+△T]时间的加速度计比力积分，代表飞行器的加速度计比力积分增量D1，所述飞行器的加速度计比力积分增量D1为考虑杆臂效应误差源的加速度计比力积分，δυk(tj+△T)代表[0,tj+△T]时间的扰动加速度积分增量理论值A1，δυk(tj)代表[0,tj]时间的扰动加速度积分增量理论值A1。
本实施例中所采用的实际飞行数据长度1200s，无人机做复杂机动飞行，导航参数记录频率为200Hz，包含纬度、经度、高程和姿态欧拉角数据，以及相应的卫星伪距、伪距率信息。图2给出了载体的运动轨迹，图3给出了载体整个过程中的姿态变化，其中横坐标均为时间(单位为秒)，Heading Angle代表偏航角Pitch Angle代表俯仰角Roll Angle代表滚转角γ，上述角度的单位均为°，从图2和图3中可以看出载体运动的动态特性和航迹的复杂程度。本实施例中，算法插值解析积分区间为1s，分别计算400Hz、200Hz、100Hz的陀螺角增量和比力积分增量，采用双子样算法进行惯导解算，由等效转动矢量计算四元数时采用四阶算法，对采用不同导航解算周期的导航参数精度进行比较。
此外，四元数范数平方倒数的泰勒近似展开分别采用一阶、二阶近似时，通过比较400Hz采样的双子样姿态计算精度，比较所生成的陀螺角增量精度。不同泰勒近似展开方法时姿态欧拉角误差如表1所示。
表1：不同泰勒近似时姿态欧拉角误差表。
| 泰勒一阶近似展开(rad) | 泰勒二阶近似展开(rad) | |
| 最大滚转角误差 | 6.145360e-9 | 1.576517e-12 |
| 最大俯仰角误差 | 5.218896e-9 | 1.597999e-12 |
| 最大航向角误差 | 5.544100e-9 | 1.115552e-12 |
表1给出了分别采用式(11-1)泰勒一阶近似展开和式(11-2)泰勒二阶近似展开时，1200s时间内计算得到的姿态欧拉角最大误差值。由表格数据可以看出，采用式(11-2)的近似其精度优于采用式(11-1)的近似，采用公式(11-1)计算得到的精度也满足基本的仿真需要。
采用式(11-2)作四元数范数平方倒数的泰勒级数展开二阶近似，生成100Hz、200Hz、400Hz理想陀螺角增量和加速度计比力积分增量信号，采用双子样算法分别进行50Hz、100Hz和200Hz的无初始误差的纯惯导解算，比较相应的姿态、速度和位置误差，得到如表2所示导航参数误差数据和图4～6所示导航参数误差随时间变化曲线，其中图4为姿态各分量的误差变化曲线，图5为速度各分量的误差变化曲线，图6为位置各分量的误差变化曲线。
表2：不同解算频率时捷联惯导计算误差表。
| 姿态误差模值rad | 速度误差模值m/s | 位置误差模值m | |
| 50Hz | 6.872268e-11 | 1.167316e-3 | 5.213068e-1 |
| 100Hz | 4.311188e-12 | 2.918444e-4 | 1.303409e-1 |
| 200Hz | 1.613925e-12 | 7.297478e-5 | 3.259874e-2 |
从图4～6可以看出，导航解算频率越高，精度越高，且符合捷联惯导误差随时间积累的规律，这也就说明，本实施例所使用的方法得到的角增量和比力积分增量可以用来检验算法的性能。另一方面，表2给出的基于理想陀螺、加速度计信号的导航参数计算误差的模值，远远小于当前工程实际中由于陀螺、加速度计本身误差造成的捷联惯性导航误差，从精度的角度来看，本实施例所使用的利用实际飞行数据插值来解析生成模拟陀螺、加速度计信号，完全满足INS/GNSS组合导航动态仿真精度要求。
此外，从图4所示姿态误差随时间变化曲线可以看出，姿态误差的变化呈现不规则波动，这是由于在进行姿态插值计算理想陀螺角增量时，四元数约束插值中，四元数范数平方倒数的泰勒级数展开得到的样条多项式不能完全逼近真值导致的，另外，计算机计算过程中的截断误差对精度也有一定的影响。但相应的姿态误差已经足够小，对INS/GNSS组合导航动态仿真没有影响。
综上所述，本实施例针对INS/GNSS组合导航仿真中器件信号的高精度模拟问题，提出了基于实际飞行数据插值的动态仿真轨迹解析生成算法，通过分段样条插值，结合约束条件和泰勒近似展开得到了载体坐标系下陀螺角速率、角增量以及加速度计比力积分增量的高精度解析表达式，具有下述优点：(1)本实施例提出了基于实际飞行数据插值的动态仿真轨迹解析生成算法，生成的陀螺、加速度计信号符合载体运动学和动力学特性，在此基础上叠加陀螺、加速度计系统性误差模型以及实际静态测试随机噪声数据，可使仿真的陀螺、加速度计信号更贴近工程实际。仿真结果验证了所提出算法的可行性，陀螺、加速度计信号模拟精度完全满足组合导航动态仿真精度要求。(2)本实施例在加速度计信号仿真中，可反映杆臂效应的影响，由于基于实测飞行航迹的位置、速度进行插值，所生成仿真轨迹可同时与经事后处理的实测GNSS伪距、伪距率等数据特征保持一致，有利于组合导航系统的仿真分析。(3)本实施例所提出仿真轨迹生成算法可以有效地模拟动态环境条件下复杂航迹的陀螺、加速度计信号，以支撑INS/GNSS组合导航算法性能评估和系统仿真测试，同时也适用于其他高精度高动态导航系统和刚体运动控制仿真中的角运动、线运动传感器信号模拟。
以上所述仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。
Claims (8)
1.一种基于实际飞行数据插值的仿真动态轨迹解析生成方法，其特征在于步骤包括：
1)对实际飞行数据进行坐标系转换，得到ECI坐标系下的位置、速度、姿态；
2)对姿态进行三次样条函数插值并计算角速率解析表达式A，根据角速率解析表达式A计算扰动加速度积分增量理论值A1和陀螺角增量理论值A2；
3)对位置进行三次样条函数插值并计算载体系加速度样条多项式B，根据载体系加速度样条多项式B进行积分计算加速度增量理论值B1；
4)对重力进行三次样条函数插值并计算载体系重力样条多项式C，根据载体系重力样条多项式C进行积分计算重力积分增量理论值C1；
5)根据扰动加速度积分增量理论值A1、加速度增量理论值B1、重力积分增量理论值C1计算飞行器的加速度计比力积分增量D1；
6)根据加速度计比力积分增量D1、陀螺角增量理论值A2进行惯导解算得到导航飞行器的动态轨迹。
2.根据权利要求1所述的基于实际飞行数据插值的仿真动态轨迹解析生成方法，其特征在于，所述步骤2)的详细步骤包括：
2.1)根据姿态欧拉角计算姿态四元数；
2.2)对姿态四元数以指定时间间隔进行三次样条函数插值，得到姿态四元数各个分量的三次样条多项式；对所述三次样条多项式求导得到三次样条多项式的一阶导数，代入时间得到姿态四元数各个分量及其一阶导数的离散值；根据预设的四元数范数约束条件，对姿态四元数各个分量及其一阶导数的离散值进行修正；
2.3)根据修正后的姿态四元数各个分量及其一阶导数的离散值，将[tk,tk+1]转换到[0,T]区间，分段计算得到[0,T]区间的姿态四元数各个分量的三次样条多项式；
2.4)将转换到区间[0,T]姿态四元数各个分量的三次样条多项式利用泰勒近似展开得到角速率解析表达式A；
2.5)根据角速率解析表达式A计算扰动加速度积分增量理论值A1；
2.6)根据角速率解析表达式A计算陀螺角增量理论值A2。
3.根据权利要求2所述的基于实际飞行数据插值的仿真动态轨迹解析生成方法，其特征在于，所述步骤2.1)计算姿态四元数的函数表达式如式(1)所示；
式(1)中，q0,q1,q2,q3分别为姿态四元数的四个元素，γ为姿态的滚转角，为姿态的俯仰角，为姿态的偏航角；
所述步骤2.2)中，姿态四元数各个分量的三次样条多项式的函数表达式如式(2)所示，预设的四元数范数约束条件如式(3)所示，对姿态四元数各个分量进行修正的函数表达式如式(4)所示，对姿态四元数各个分量的一阶导数的离散值进行修正的函数表达式如式(5)所示；
式(2)中，为姿态四元数各个分量的三次样条多项式，ai,bi,ci,di分别为样条插值后三次样条多项式的系数，t为时间；
式(3)中，||q(t)||代表t时刻的姿态四元数q(t)的范数，q0(t),q1(t),q2(t),q3(t)分别为姿态四元数q(t)的四个元素，分别为姿态四元数q(t)的四个元素的一阶导数；
式(4)中，qi(tk)代表插值点tk时刻的姿态四元数各个分量的修正结果，代表插值点tk时刻姿态四元数的各个分量，代表插值点tk时刻姿态四元数的范数；
式(5)中，代表插值点tk时刻姿态四元数中各个分量的一阶导数的修正结果，代表插值点tk时刻姿态四元数中各个分量的一阶导数，代表插值点tk时刻姿态四元数的范数；代表插值点tk时刻姿态四元数的范数的求导结果，其函数表达式如式(6)所示；
式(6)中，分别为插值点tk时刻的姿态四元数的四个元素，分别为插值点tk时刻的姿态四元数的四个元素一阶导数；
所述步骤2.3)分段计算得到[0,T]区间的姿态四元数各个分量的三次样条多项式如式(7)所示；
式(7)中，代表转换到区间[0,T]的插值样条多项式，ai,k,bi,k,ci,k,di,k分别为转换到[0,T]区间后的三次样条多项式系数，t代表时间；转换到[0,T]区间后的三次样条多项式系数的计算函数表达式如式(8)所示；
式(8)中，T代表分段样条的分段间隔，qi(tk)代表tk时刻的姿态四元数中第i个元素的值，代表tk时刻姿态四元数中第i个元素的一阶导数值，qi(tk+1)代表tk+1时刻的姿态四元数中第i个元素的值，代表tk+1时刻姿态四元数中第i个元素的一阶导数值，符号i取值为0或1或2或3，ai,k,bi,k,ci,k,di,k分别为转换到区间[0,T]后的插值多项式系数；
所述步骤2.4)的详细步骤包括：根据式(9)所示的限制条件代入式(10)所示角速率的计算式，对姿态四元数范数的平方的倒数部分进行泰勒近似展开，得到角速率解析表达式A如式(11)所述；
式(9)中，qk(t)代表姿态四元数，代表原始数据区间为[tk,tk+1]转换到[0,T]区间后的姿态四元数，代表原始数据区间为[tk,tk+1]转换到[0,T]区间后的姿态四元数的范数，代表姿态四元数的一阶导数，代表原始数据区间为[tk,tk+1]转换到[0,T]区间后的姿态四元数的一阶导数，分别代表原始[tk,tk+1]区间转换到区间[0,T]后的姿态四元数各个元素，分别代表原始[tk,tk+1]区间转换到[0,T]区间后的姿态四元数各个元素的一阶导数，t代表时间；
式(10)中，代表角速率解析表达式A，所述角速率解析表达式A为姿态四元数的样条多项式，下标ib代表载体坐标系相对于惯性坐标系、k代表原始数据为区间[tk,tk+1]、上标b代表载体坐标系；qk(t)代表姿态四元数的样条多项式，代表姿态四元数的样条多项式的一阶导数，t代表时间；
式(11)中，代表角速率的时间函数，下标ib代表载体坐标系相对于惯性坐标系、k代表原始数据为区间[tk,tk+1]、上标b代表载体坐标系；代表原始数据区间为[tk,tk+1]转换到[0,T]区间后的姿态四元数的范数，分别代表原始[tk,tk+1]区间转换到区间[0,T]后的姿态四元数各个元素，分别代表原始[tk,tk+1]区间转换到[0,T]区间后的姿态四元数各个元素的一阶导数，t代表时间；
所述步骤2.5)包括首先根据式(12)所示的扰动加速度解析积分表达式计算扰动加速度积分理论值，然后根据指定时间间隔△T的扰动加速度积分理论值δυk(t)和δυk(t+△T)计算差值，得到指定的时间间隔△T内的扰动加速度积分增量理论值A1；
式(12)中，δυk(t)代表[0,t]时间的扰动加速度积分理论值，分别为角速率分别在x、y、z方向的解析表达式；代表x方向加速度计偏离质心的杆臂矢量中的x方向偏移量，代表y方向加速度计偏离质心的杆臂矢量中的y方向偏移量，代表z方向加速度计偏离质心的杆臂矢量中的z方向偏移量， 分别为三个方向的加速度计偏离质心的杆臂矢量且均为常值，t代表时间；
所述步骤2.6)具体是指根据式(13)计算陀螺角增量理论值A2；
式(13)中，θk(t)代表由角速率解析表达式A积分得到的陀螺角增量，代表角速率解析表达式A，所述角速率解析表达式A为姿态四元数的样条多项式，下标ib代表载体坐标系相对于惯性坐标系、k代表原始数据为区间[tk,tk+1]、上标b代表载体坐标系，△θk,j+1代表△T时间间隔内的陀螺角增量理论值A2，θk(tj)代表[0,tj]时间的陀螺角积分增量，θk(tj+△T)代表[0,tj+△T]时间的陀螺角积分增量，t代表时间。
4.根据权利要求1所述的基于实际飞行数据插值的仿真动态轨迹解析生成方法，其特征在于，所述步骤3)的详细步骤包括：
3.1)对ECI坐标系下位置的各个分量进行样条插值得到位置的样条多项式，对所得到的位置的样条多项式求一次导得到速度的样条多项式，分别代入时间得到位置和速度的离散值；
3.2)根据位置和速度的离散值，将[tk,tk+1]转换到[0,T]区间，分段计算得到[0,T]区间的位置各个分量的三次样条多项式，进行两次求导得到加速度各个分量的样条多项式其中k代表原始数据区间为[tk,tk+1]；
3.3)结合坐标系转换，得到载体系b中的加速度的解析表达式如式(14)所示；
式(14)中，代表载体系b中的加速度的样条多项式，代表原始数据为[tk,tk+1]区间转换到[0,T]区间的加速度的样条多项式，代表无误差惯性坐标系到载体坐标系的方向余弦矩阵，t代表时间，其中方向余弦矩阵与姿态四元数的转换关系如式(15)所示；
式(15)中，代表无误差惯性坐标系到载体坐标系的方向余弦矩阵，代表未修正时的惯性坐标系到载体坐标系的方向余弦矩阵， 分别代表未修正时的惯性坐标系到载体坐标系的方向余弦矩阵中的九个元素，代表原始数据为[tk,tk+1]转换到[0,T]区间的四元数的范数的平方，t代表时间；根据精度要求选择泰勒近似展开计算的解析多项式，得到载体系加速度样条多项式B；
3.4)根据载体系加速度样条多项式B的解析多项式进行积分计算加速度增量理论值B1。
5.根据权利要求4所述的基于实际飞行数据插值的仿真动态轨迹解析生成方法，其特征在于，所述步骤3.2)中分段计算得到[0,T]区间的位置各个分量的三次样条多项式如式(16)所示，两次求导得到加速度各个分量的样条多项式如式(17)所示；
式(16)和(17)中，分别代表位置各个分量,分别代表加速度各分量，ax,k,bx,k,cx,k,dx,k、ay,k,by,k,cy,k,dy,k、az,k,bz,k,cz,k,dz,k分别代表转换到区间[0,T]的样条多项式系数，t代表时间，转换到区间[0,T]的样条多项式系数之间的转换矩阵如式(18)所示；
式(18)中，T代表分段样条的分段间隔，ax,k,bx,k,cx,k,dx,k、ay,k,by,k,cy,k,dy,k、az,k,bz,k,cz,k,dz,k分别代表转换到区间[0,T]的样条多项式系数，代表原始数据tk时刻的位置的值，代表原始数据tk时刻的位置的导数值，代表原始数据tk+1时刻的位置的值，代表原始数据tk+1时刻的位置的导数值；
所述步骤3.3)得到的载体系加速度样条多项式B如式(19)所示；
式(19)中，代表选择泰勒近似展开后计算得到的载体系加速度样条多项式B，代表原始数据为[tk,tk+1]区间转换到[0,T]区间的加速度的样条多项式， 分别代表未修正时的惯性坐标系到载体坐标系的方向余弦矩阵中的九个元素，代表原始数据为[tk,tk+1]转换到[0,T]区间的四元数的范数的平方，t代表时间；其中，未修正时的惯性坐标系到载体坐标系的方向余弦矩阵中的九个元素如式(20)所示；
式(20)中，分别代表原始[tk,tk+1]区间转换到区间[0,T]后的姿态四元数各个元素的时间函数，t代表时间；
所述步骤3.4)中根据载体系加速度样条多项式B的解析多项式进行积分计算加速度增量理论值B1的函数表达式如式(21)所示；
式(21)中，代表加速度增量理论值B1，表示载体系加速度样条多项式B，t代表时间。
6.根据权利要求1所述的基于实际飞行数据插值的仿真动态轨迹解析生成方法，其特征在于，所述步骤4)的详细步骤包括：
4.1)对重力以指定时间间隔进行三次样条函数插值得到重力各分量的三次样条多项式，对重力各分量的三次样条多项式进行求导得到重力各分量的三次样条多项式的一阶导数，然后代入时间得到重力各个分量及其一阶导数的离散值；
4.2)根据重力各个分量及其一阶导数的离散值，将[tk,tk+1]转换到[0,T]区间，分段计算得到[0,T]区间的重力各个分量的三次样条多项式；
4.3)根据精度要求选择泰勒近似展开计算的解析多项式，得到载体系重力样条多项式C，其中代表原始数据为[tk,tk+1]转换到[0,T]区间的四元数的范数的平方，t代表时间；
4.4)根据载体系重力样条多项式C的解析多项式进行积分计算重力积分增量理论值C1。
7.根据权利要求6所述的基于实际飞行数据插值的仿真动态轨迹解析生成方法，其特征在于，所述步骤4.1)中重力各分量的三次样条多项式如式(22)所示；
式(22)中，代表重力在分别在x、y、z方向的分量，ax,bx,cx,dx、ay,by,cy,dy、az,bz,cz,dz分别代表样条插值后三次样条多项式的系数，t代表时间；
所述步骤4.2)中分段计算得到[0,T]区间的重力各分量的三次样条多项式如式(23)所示；
式(23)中，分别代表原数据区间为[tk,tk+1]转换到[0,T]区间后重力各分量的三次样条多项式,ax,k,bx,k,cx,k,dx,k、ay,k,by,k,cy,k,dy,k、az,k,bz,k,cz,k,dz,k代表原数据区间为[tk,tk+1]转换到[0,T]区间后的三次样条多项式的系数，t代表时间；转换到区间[0,T]的样条多项式系数之间的转换矩阵如式(24)所示；
式(24)中，T代表分段样条的分段间隔，ax,k,bx,k,cx,k,dx,k、ay,k,by,k,cy,k,dy,k、az,k,bz,k,cz,k,dz,k分别代表转换到[0,T]区间后三次样条多项式的系数，代表原始数据tk时刻的重力的值，代表原始数据tk时刻的重力的一阶导数值，代表原始数据tk+1时刻的重力的值，代表原始数据tk+1时刻的重力的一阶导数值；
所述步骤4.3)得到的载体系重力样条多项式C的函数表达式如式(25)所示；
式(25)中，代表原始数据为[tk,tk+1]区间转换到[0,T]区间的重力的样条多项式，分别代表未修正时的惯性坐标系到载体坐标系的方向余弦矩阵中的九个元素，代表原始数据为[tk,tk+1]转换到[0,T]区间的四元数的范数的平方，分别代表原始[tk,tk+1]区间转换到区间[0,T]后的姿态四元数各个元素的时间函数，t代表时间；其中，未修正时的惯性坐标系到载体坐标系的方向余弦矩阵中的九个元素如式(26)所示；
式(26)中，分别代表原始[tk,tk+1]区间转换到区间[0,T]后的姿态四元数各个元素的时间函数，t代表时间；
所述步骤4.4)根据载体系重力样条多项式C的解析多项式进行积分计算重力积分增量理论值C1的函数表达式如式(27)所示；
式(27)中，为载体系重力的积分增量理论值C1，代表载体系重力样条多项式C，t代表时间。
8.根据权利要求1所述的基于实际飞行数据插值的仿真动态轨迹解析生成方法，其特征在于，所述步骤5)具体是指根据式(28)所示函数表达式计算飞行器的加速度计比力积分增量D1；
式(28)中，υk(t)代表加速度计比力积分样条多项式，代表加速度增量理论值B1，代表重力积分增量理论值C1，△υk,j+1和△υk(tj+1)均代表△T时间间隔内不考虑杆臂效应等误差源的加速度计比力积分增量，υk(tj)代表[0,tj]时间的加速度计比力积分，υk(tj+△T)代表[0,tj+△T]时间的加速度计比力积分，代表飞行器的加速度计比力积分增量D1，所述飞行器的加速度计比力积分增量D1为考虑杆臂效应误差源的加速度计比力积分，δυk(tj+△T)代表[0,tj+△T]时间的扰动加速度积分增量理论值A1，δυk(tj)代表[0,tj]时间的扰动加速度积分增量理论值A1。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610290487.0A CN105973237B (zh) | 2016-05-04 | 2016-05-04 | 基于实际飞行数据插值的仿真动态轨迹解析生成方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610290487.0A CN105973237B (zh) | 2016-05-04 | 2016-05-04 | 基于实际飞行数据插值的仿真动态轨迹解析生成方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN105973237A CN105973237A (zh) | 2016-09-28 |
| CN105973237B true CN105973237B (zh) | 2018-09-07 |
Family
ID=56994433
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201610290487.0A Active CN105973237B (zh) | 2016-05-04 | 2016-05-04 | 基于实际飞行数据插值的仿真动态轨迹解析生成方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN105973237B (zh) |
Families Citing this family (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107024224A (zh) * | 2017-04-28 | 2017-08-08 | 南京信息工程大学 | 倾转旋翼飞行器的角度测量装置及测量方法 |
| CN111077800B (zh) * | 2019-12-12 | 2022-12-23 | 上海航天控制技术研究所 | 一种双星编队半实物测试系统与方法 |
| CN113720329B (zh) * | 2021-09-10 | 2024-08-06 | 北京控制工程研究所 | 一种基于杆臂效应代数约束的大动态惯性导航方法 |
| CN113806950B (zh) * | 2021-09-23 | 2024-10-29 | 中国人民解放军海军工程大学 | 基于潜器空间运动模型的捷联惯导系统仿真方法 |
| CN114323018B (zh) * | 2021-11-26 | 2024-11-01 | 中国航空无线电电子研究所 | 一种验证航空航迹融合算法软件的方法 |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102508954A (zh) * | 2011-10-21 | 2012-06-20 | 天津大学 | Gps/sins组合导航全数字仿真方法及装置 |
| CN102607591A (zh) * | 2012-02-27 | 2012-07-25 | 浙江大学 | 一种用于捷联惯导软件测试的轨迹数据生成方法 |
| CN104713559A (zh) * | 2015-02-01 | 2015-06-17 | 西北工业大学 | 一种高精度sins模拟器的设计方法 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US8670964B2 (en) * | 2009-10-15 | 2014-03-11 | American Gnc Corporation | Gyrocompass modeling and simulation system (GMSS) and method thereof |
-
2016
- 2016-05-04 CN CN201610290487.0A patent/CN105973237B/zh active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102508954A (zh) * | 2011-10-21 | 2012-06-20 | 天津大学 | Gps/sins组合导航全数字仿真方法及装置 |
| CN102607591A (zh) * | 2012-02-27 | 2012-07-25 | 浙江大学 | 一种用于捷联惯导软件测试的轨迹数据生成方法 |
| CN104713559A (zh) * | 2015-02-01 | 2015-06-17 | 西北工业大学 | 一种高精度sins模拟器的设计方法 |
Non-Patent Citations (1)
| Title |
|---|
| GNSS导航信号仿真中运动轨迹设计;李俊毅，等，;《海洋测绘》;20131125;第33卷(第6期);第26-38页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN105973237A (zh) | 2016-09-28 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN105973237B (zh) | 基于实际飞行数据插值的仿真动态轨迹解析生成方法 | |
| CN100585602C (zh) | 惯性测量系统误差模型验证试验方法 | |
| CN101424534B (zh) | 惯性/重力组合导航半实物模拟装置 | |
| CN103913181A (zh) | 一种基于参数辨识的机载分布式pos传递对准方法 | |
| CN108548542A (zh) | 一种基于大气阻力加速度测量的近地轨道确定方法 | |
| CN103925930B (zh) | 一种重力仪双轴陀螺稳定平台航向误差效应的补偿方法 | |
| CN107764261A (zh) | 一种分布式pos传递对准用模拟数据生成方法和系统 | |
| Barczyk | Nonlinear state estimation and modeling of a helicopter UAV | |
| Yang et al. | Gyro-free inertial measurement unit with unfettered accelerometer array distribution and for the object with position change in center of gravity | |
| CN116992700B (zh) | 一种物流无人机导航精度确定的方法及设备 | |
| CN104121930A (zh) | 一种基于加表耦合的mems陀螺漂移误差的补偿方法 | |
| CN110260862B (zh) | 一种基于捷联惯导系统的旋翼直升机载导航装置 | |
| CN104297525A (zh) | 基于火箭橇试验的惯性测量系统加速度计标定方法 | |
| CN105180928A (zh) | 一种基于惯性系重力特性的船载星敏感器定位方法 | |
| CN103411614A (zh) | 火星动力下降段多源信息组合导航的迭代skf方法 | |
| Sun et al. | A new inertial measurement method of ship dynamic deformation | |
| Oliveira et al. | Output error method and two step method for aerodynamic model identification | |
| CN102914319B (zh) | 一种基于先验信息的多光纤惯组贮存期静态快速检测方法 | |
| Liu et al. | A computer simulation of the influence of GGI and inertial sensors on gravity gradient aided navigation | |
| CN106248065B (zh) | 一种飞行器发射后效期时间与距离测量的方法及系统 | |
| Lu et al. | Segmented Angular Rate Joint Estimation of Inertial Sensor Arrays for UAV Navigation | |
| Komendat | Center-of-gravity estimation of an aircraft solely using traditional aircraft measurement sensors | |
| CN114577204B (zh) | 基于神经网络的捷联惯导系统抗干扰自对准方法和装置 | |
| EP4328595A1 (en) | Fluid flow estimation and navigation | |
| Abeywardena | Model-aided state estimation for quadrotor micro aerial vehicles |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |