一种全姿态捷联惯导系统的非线性初始对准方法技术方案
A nonlinear initial alignment method for sins
The invention discloses an attitude of strapdown inertial navigation system nonlinear initial alignment method, which comprises the following steps: coarse alignment of strapdown inertial navigation system strapdown inertial navigation system; switching double Euler angle direct method of nonlinear filter initialization, update time and measurement update and inverse Euler angle of direct method. The invention combines the dual Euler method and direct method to solve the singularity problem of single Euler angle of the direct method can be applied to the carrier maneuver; system equation with velocity differential equation and Euler differential equation based model than the indirect method of one order approximation is more accurate; filtering process is also realized navigation solution, filtering output for the navigation parameters, to avoid a lot of repeated calculation of mechanical calibration equation, the algorithm is more simple than the indirect method; compared to the number of four yuan direct method, does not need to modify the filtering step, the algorithm is simple.
【技术实现步骤摘要】
本专利技术涉及一种全姿态捷联惯导系统的非线性初始对准方法，用于确定捷联惯性导航系统的初始姿态，属于导航、制导与控制
技术介绍
捷联惯性导航系统在进入导航工作前均需进行初始对准过程，其主要任务是初始姿态的确定，即确定初始数学平台。初始对准一般需要经过粗对准和精对准两个阶段，粗对准阶段，通过陀螺仪测得的角速度信息和加速度计测得的比力信息获得粗略的初始姿态信息。精对准阶段一般采用最优估计滤波方法。根据所估计的系统状态的不同，卡尔曼滤波在应用时有两种方法：直接法和间接法。间接法中采用捷联惯性导航系统的误差模型，以导航参数的误差量作为状态量，卡尔曼滤波器经过计算，获得各导航参数误差量的最优估计值。在非线性滤波器研究不够深入的早期，均采用线性滤波器，需要对误差模型作一阶线性近似，会损失模型精度。直接法滤波中,以导航参数作为状态,卡尔曼滤波器经过计算,得到导航参数的最优估计值，其系统方程以惯性导航系统的力学编排方程为主，无需近似，系统方程是非线性的，随着非线性滤波技术的发展，直接法的研究成为热点，在直接法中常用四元数微分方程和欧拉角微分方程。四元数包含四个参数，存在模值为1的非线性约束关系，无论是线性滤波器还是非线性滤波器，都要求状态量之间是不相关的，因此用四元数作为状态量时，需要对滤波算法做相应处理，如可采用二次映射或者虚拟观测量等方法，增加了复杂度。载体姿态角是按照欧拉角的概念定义的，姿态角就是一组欧拉角，欧拉角法概念直观、容易理解。欧拉角法通过求解欧拉角微分方程直接计算航向角、纵摇角和横摇角，三个参数之间没有约束关系，用欧拉角作为直接法的状态量时，滤波...
【技术保护点】
一种全姿态捷联惯导系统的非线性初始对准方法，其特征在于，该方法包括以下步骤：步骤1、捷联惯性导航系统粗对准：根据惯性器件陀螺仪测得的角速度信息和加速度计测得的比力信息，使用基于惯性系重力加速度的凝固解析粗对准算法，求得粗略的以正欧拉角表示的初始纵摇角θ0、横摇角γ0和航向角ψ0；步骤2、根据所述步骤1得到的初始纵摇角θ0，设置正/反欧拉角直接法的运行标志Flag：如果θ0∈[‑45°,+45°]，则Flag＝0，采用正欧拉角直接法；如果θ0∈[‑90°,‑45°)∪(45°,+90°]，则Flag＝1，采用反欧拉角直接法；步骤3、正/反欧拉角直接法非线性滤波器的初始化：根据标志Flag，在滤波开始时刻t0，利用粗对准的结果初始化滤波器的系统状态向量及其方差矩阵P0；其中系统状态向量根据Flag标志设定：如果Flag＝0，则采用正欧拉角直接法的状态向量，将粗对准得到的正欧拉角直接赋给系统状态向量如果Flag＝1，则采用反欧拉角直接法的状态向量，先将粗对准获得的正欧拉角转换为反欧拉角，然后将得到的反欧拉角赋给系统状态向量步骤4、每当获得新的陀螺和加速度计测量值，就进行正/反欧拉角直接法非线...
【技术特征摘要】
1.一种全姿态捷联惯导系统的非线性初始对准方法，其特征在于，该方法包括以下步骤：步骤1、捷联惯性导航系统粗对准：根据惯性器件陀螺仪测得的角速度信息和加速度计测得的比力信息，使用基于惯性系重力加速度的凝固解析粗对准算法，求得粗略的以正欧拉角表示的初始纵摇角θ0、横摇角γ0和航向角ψ0；步骤2、根据所述步骤1得到的初始纵摇角θ0，设置正/反欧拉角直接法的运行标志Flag：如果θ0∈[-45°,+45°]，则Flag＝0，采用正欧拉角直接法；如果θ0∈[-90°,-45°)∪(45°,+90°]，则Flag＝1，采用反欧拉角直接法；步骤3、正/反欧拉角直接法非线性滤波器的初始化：根据标志Flag，在滤波开始时刻t0，利用粗对准的结果初始化滤波器的系统状态向量及其方差矩阵P0；其中系统状态向量根据Flag标志设定：如果Flag＝0，则采用正欧拉角直接法的状态向量，将粗对准得到的正欧拉角直接赋给系统状态向量如果Flag＝1，则采用反欧拉角直接法的状态向量，先将粗对准获得的正欧拉角转换为反欧拉角，然后将得到的反欧拉角赋给系统状态向量步骤4、每当获得新的陀螺和加速度计测量值，就进行正/反欧拉角直接法非线性滤波器的时间更新：根据标志Flag，利用陀螺和加速度计的输出以及正/反欧拉角直接法的系统方程，进行非线性滤波器的时间更新，得到tk时刻系统状态向量的一步预测值及其方差矩阵的一步预测值Pk|k-1，如果Flag＝0，则采用正欧拉角直接法的系统方程进行非线性滤波器的时间更新；如果Flag＝1，则采用反欧拉角直接法的系统方程进行非线性滤波器的时间更新；未获得新的速度观测值时，时间更新后，将赋给tk时刻系统状态向量的估计值将Pk|k-1赋给tk时刻系统状态向量方差矩阵的估计值Pk|k，进入步骤6；若获得了新的速度观测值，则时间更新后，进入步骤5；步骤5、根据标志Flag，利用载体系速度的输出以及正/反欧拉角直接法的量测方程，进行非线性滤波器的量测更新，得到更新后的和Pk|k：如果Flag＝0，则采用正欧拉角直接法的量测方程进行非线性滤波器的量测更新；如果Flag＝1，则采用反欧拉角直接法的量测方程进行非线性滤波器的量测更新；步骤6、如果Flag＝0，则直接输出中的正欧拉角和速度分量，如果Flag＝1，则先将中的反欧拉角转换为正欧拉角，然后输出该正欧拉角和中的速度分量；每次输出后，判断对准时间是否截止，如是，则结束本方法流程，否则进入步骤7；步骤7、根据标志Flag，以及以正欧拉角表示的纵摇角θ的大小，每次量测更新后，进行正/反欧拉角直接法的切换，重置标志Flag：如果Flag＝0，且θ∈[-45°,+45°]，则Flag保持不变，返回步骤4，进行正欧拉角直接法的下一次非线性滤波的时间更新；如果Flag＝0，且θ∈[-90°,-45°)∪(45°,+90°]，则令Flag＝1，将中的正欧拉角转换为反欧拉角，然后返回步骤4，进行反欧拉角直接法的下一次非线性滤波的时间更新；如果Flag＝1，且θ∈[-45°,+45°]，则令Flag＝0，将中的反欧拉角转换为正欧拉角，然后返回步骤4，进行正欧拉角直接法的下一次非线性滤波的时间更新；如果Flag＝1，且θ∈[-90°,-45°)∪(45°,+90°]，则Flag保持不变，返回步骤4，进行反欧拉角直接法的下一次非线性滤波的时间更新。2.根据权利要求1所述的一种全姿态捷联惯导系统的非线性初始对准方法，其特征在于，所述的步骤4中正/反欧拉角直接法的系统方程具体为：正欧拉角直接法的系统方程为： x · p = F p ( x p ) + G p ( x p ) w ]]>式中，xp为正欧拉角直接法的状态向量，w为正欧拉角直接法的系统噪声向量，Fp(xp)为正欧拉角直接法的非线性状态转移函数矩阵，Gp(xp)为正欧拉角直接法的系统噪声输入函数矩阵，分别根据下式确定： x p = [ v e a s t , v n o r t h , θ , γ , ψ , ▿ a x , ▿ a y , ϵ g x , ϵ g y , ϵ g z ] T ]]>w＝[wax,way,wgx,wgy,wgz,0,0,0,0,0]T F p ( x p ) = f ~ i b z b ( cos ψ sin γ + cos γ sin ψ sin θ ) - ( cos ψ cos γ - sin ψ sin θ sin γ ) ( ▿ a x - f ~ i b x b ) + v n o r t h ( 2 ω i e sin L + v e a s t tan L r N ) + cos θ sin ψ ( ▿ a y - f ~ i b y b ) f ~ i b z b ( sin ψ sin γ - cos γ cos ψ sin θ ) - ( cos γ sin ψ + cos ψ sin θ sin γ ) ( ▿ a x - f ~ i b x b ) - v e a s t ( 2 ω i e sin L + v e a s t tan L r N ) - cos ψ cos θ ( ▿ a y - f ~ i b y b ) ω ~ i b x b cos γ + ω ~ i b z b sin γ - ϵ g x cos γ - ϵ g z sin γ + v n o r t h cos ψ r M - ω i e cos L sin ψ - v e a s t r N sin ψ ω ~ i b y b - ϵ g y - ω ~ i b z b cos γ tan θ + ω ~ i b x b sin γ tan θ + ϵ g z cos γ tan θ - ϵ g x sin γ tan θ - v n o r t h sin ψ r M cos θ - ω i e cos L cos ψ cos θ - v e a s t cos ψ r N cos θ ω ~ i b z b cos γ cos θ - v e a s t r N tan L - ω i e sin L - ω ~ i b x b sin γ cos θ - ϵ g z cos γ cos θ + ϵ g x sin γ cos θ + v n o r t h r M sin ψ tan θ + ω i e cos L cos ψ tan θ + v e a s t r N cos ψ tan θ 0 5 × 1 ]]>Gp(xp)为10行10列的矩阵，其中， G p 1 ( x p ) = cos ψ cos γ - sin ψ sin θ sin γ - cos θ sin ψ cos γ sin ψ + cos ψ sin θ sin γ cos ψ cos θ ]]> G p 2 ( x p ) = c o s γ 0 s i n γ t a n θ sin γ 1 - c o s γ t a n θ - s i n γ cos θ 0 c o s γ c o s θ ]]>veast为东向速度，vnorth为北向速度，θ、γ和ψ分别为正欧拉角组合的纵摇角、横摇角和航向角；分别为载体系中x、y向加速度计测量的随机常值偏置，εgx、εgy和εgz分别为载体系中x、y、z向陀螺仪测量的随机常值漂移；wax、way分别为载体系中x、y向加速度计测量的随机噪声误差，wgx、wgy和wgz分别为载体系中x、y、z向陀螺仪测量的随机噪声误差；和分别为载体系x、y、z向加速度计的测量值，ωie为地球自转角速率，L为当地地理纬度，rM为子午圈曲率半径，rN为卯酉圈曲率半径；和为载体系x、y、z向陀螺仪测量值；0i×j表示元素为0的i行j列矩阵；反欧拉角直接法的系统方程为： x · r = F r ( x r ) + G r ( x r ) w ]]>式中，反欧拉角直接法的系统噪声向量w与正欧拉角直接法的相同；xr为反欧拉角直接法的状态向量，Fr(xr)为反欧拉角直接法的非线性状态转移函数矩阵，Gr(xr)为反欧拉角直接法的系统噪声输入函数矩阵，分别根据下式确定： x r = [ v e a s t , v n o r t h , θ r , γ r , ψ r , ▿ a x , ▿ a y , ϵ g x , ϵ g y , ϵ g z ] T ]]> F r ( x r ) = f ~ i b z b ( cosψ r sinθ r + cosψ r cosθ r sinγ r ) - ( cosθ r sinψ r - cosψ r sinθ r sinγ r ) ( ▿ a y - f ~ i b y b ) + v n o r t h ( 2 ω i e sin L + v e a s t tan L r N ...
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。