PSINS惯性系初始对准与代码解读
PSINS惯性系初始对准与代码解读
- 0.前言
- 1.坐标系定义
- 2.传统解析式对准算法
- 3.惯性系初始对准：
- 3.1.基本原理：
- 3.2.CenC^n_eCen:地球坐标系(e)到导航坐标系(n)的转换矩阵
- 3.3.CieC^e_iCie:地球坐标系(e)到惯性系(i)的转换矩阵
- 3.4.Cbib0C^{i_{b_0}}_bCbib0:初始捷联惯导惯性系(ib0i_{b_0}ib0)到捷联惯导坐标系(b)的转换矩阵
- 3.5.Cib0iC^i_{i_{b_0}}Cib0i:惯性系(i)到初始捷联惯导惯性系(ib0i_{b_0}ib0)的转换矩阵
- 4.代码解析
- CAlign0.m
- i0fvp
码字不易，转载请注明出处
0.前言
严恭敏老师的开源代码库psins中，惯性系对准代码alingi0与《捷联惯导与组合导航》一书理论部分并没有严格对应，相应的资料也不太好找，学习中废了挺多脑筋。
本文参考“摇摆基座上基于信息的捷联惯导粗对准研究”(秦永元)，以及“严恭敏博士后研究报告”，详细解读并推导了静基座惯性系对准算法，并与PSINS代码进行对照解读。希望能对惯导初学者有所帮助，同时也欢迎各位大佬雅正。
1.坐标系定义
地心惯性坐标系（iii系）：
oxiox_ioxi轴在赤道平面内且指向春分点, ozioz_iozi轴指向地球自转方向，三轴构成右手坐标系；地球坐标系（eee系）：
oxeox_eoxe轴在赤道平面内且指向中央子午线，ozeoz_eoze轴沿地球自转方向，三轴构成右手坐标系，e系与地球固连，e系相对于i系的转动角速率即为地球自转角速率 ；导航坐标系（nnn系）:
选取“东-北-天（E−N−UE-N-UE−N−U）”坐标系为导航坐标系（nnn系）；载体坐标系（bbb系）:
定义“右-前-上（R−F−UR-F-UR−F−U）”坐标系为捷联惯导坐标系（bbb系），或称之为运载体坐标系；初始时刻惯性坐标系（i0i_0i0系）:
在初始对准起始时刻（即当t=t0=0t=t_0=0t=t0=0 时），oxi0ox_{i_0}oxi0轴在当地子午面内且平行于赤道平面，ozi0oz_{i_0}ozi0轴指向地球自转方向，三轴构成右手坐标系，初始对准开始后i0i_0i0系三轴方向相对惯性空间保持不动；初始时刻地球坐标系（e0e_0e0系）:
原点为地球中心，oxe0ox_{e_0}oxe0轴在赤道平面内且指向初始对准开始时刻的当地子午线，oze0oz_{e_0}oze0沿地球自转方向，三轴构成右手坐标系，e0e_0e0系也与地球固连，并且e0e_0e0系与i0i_0i0系之间方位关系是前者只绕后者的ozi0oz_{i_0}ozi0轴转动了ωiet{\omega}_{ie}tωiet角度；初始时刻导航坐标系（n0n_0n0系）：
把初始对准开始时刻的导航坐标系定义为n0n_0n0系，它相对地球表面固定不动，即不随捷联惯导在地球表面运动而运动；初始时刻捷联惯导惯性坐标系（ib0i_b0ib0系）:
在t0t_0t0时刻ib0i_{b_0}ib0系重合于b系，初始对准开始后ib0i_{b0}ib0 系不随捷联惯导转动，即在惯性空间中保持指向不变。
2.传统解析式对准算法
静基座下即捷联惯组静止不动时，加速度计的比力输出f~sfb\tilde{f}^b_{sf}f~sfb测量的是重力加速度矢量ggg在b系中的投影−gb-g^b−gb，陀螺角速度输出b~ibb\tilde{b}^b_{ib}b~ibb测量的是地球自转角速度矢量ωie{\omega}_{ie}ωie在b系中的投影ωieb{\omega}^b_{ie}ωieb。当对准地点的地理位置准确已知时，物理量ggg和ωie{\omega}_{ie}ωie在n系中的投影gng^ngn和 ωien{\omega}^n_{ie}ωien也是已知的。由姿态矩阵CbnC^n_bCbn可建立如下两个转换关系式:
−f~sfb=Cnbgnω~ibb=Cnbωien\begin{aligned} -\tilde{f}^b_{sf}=&C^b_ng^n \\ \tilde{\omega}^b_{ib}=&C^b_n{\omega}^n_{ie} \end{aligned}−f~sfb=ω~ibb=CnbgnCnbωien式中[00−g]T\begin{bmatrix}0&0&-g\end{bmatrix}^T[00−g]T，g为重力加速度大小；ωien=[0ωiecosLωiesinL]T{\omega}^n_{ie}=\begin{bmatrix}0&{\omega}_{ie}cosL&{\omega}_{ie}sinL\end{bmatrix}^Tωien=[0ωiecosLωiesinL]T，L是当地地理纬度，ωie{\omega}_{ie}ωie是地球自转角速度。通过公式（1）可求出得初始对准姿态矩阵CbnC^n_bCbn1。
Cbn=[(gn)T(gn×ωie)T(gn×ωie×gn)T]−1[(−f~sfb)T(−f~sfb×ω~ibb)T(f~sfb×ω~ibb×f~sfb)T](1)C^n_b= \begin{bmatrix} (g^n)^T\\ (g^n\times{\omega}_{ie})^T\\ (g^n\times{\omega}_{ie}\times g^n)^T \end{bmatrix}^{-1} \begin{bmatrix} (-\tilde{f}^b_{sf})^T\\ (-\tilde{f}^b_{sf}\times\tilde{\omega}^b_{ib})^T\\ (\tilde{f}^b_{sf}\times\tilde{\omega}^b_{ib}\times \tilde{f}^b_{sf})^T \end{bmatrix} \tag{1} Cbn=⎣⎡(gn)T(gn×ωie)T(gn×ωie×gn)T⎦⎤−1⎣⎢⎡(−f~sfb)T(−f~sfb×ω~ibb)T(f~sfb×ω~ibb×f~sfb)T⎦⎥⎤(1)
3.惯性系初始对准：
秦永元老师在论文中指出，上述对准方法仅适用于微小晃动的对准环境2 ，而惯性系对准的基本思想是，从惯性坐标系观察地球表面上某固定点的重力矢量，通过在不同时间对重力矢量的观测，进行双/多矢量定姿。该方法具备极强的抗角运动能力。非常适用于线运动不强但角运动明显的场景。
虽然原理如上所述，但PSINS的代码实现与书中1 并不完全一致，现按照PSINS代码的算法编排，推导如下：
3.1.基本原理：
将姿态矩阵作如下分解，分别求出各个变换关系然后组合。
Cbn=CenCieCib0iCbib0(2)C^n_b=C^n_eC^e_iC^i_{i_{b_0}}C^{i_{b_0}}_b\tag{2}Cbn=CenCieCib0iCbib0(2)
下面分步推导涉及的变换矩阵
3.2.CenC^n_eCen:地球坐标系(e)到导航坐标系(n)的转换矩阵
从e系到n系的转换可分为两个步骤：
1、沿Z轴逆时针旋转90+λ90+\lambda90+λ，使X轴和East轴重合。
2、沿East轴（新X轴）逆时针旋转90−φ90-\varphi90−φ ，使Z轴和UP轴重合。
以上旋转即：
[xyz]=R3[−(π/2+λ)]R1[−(π/2−φ)]\begin{bmatrix} x\\ y\\ z \end{bmatrix}=R_3[-(\pi/2+\lambda)]R_1[-(\pi/2-\varphi)] ⎣⎡xyz⎦⎤=R3[−(π/2+λ)]R1[−(π/2−φ)]
其中
R1[θ]=[1000cos(θ)sin(θ)0−sin(θ)cos(θ)];R2[θ]=[cos(θ)0sin(θ)010−sin(θ)0cos(θ)]R3[θ]=[cos(θ)sin(θ)0−sin(θ)cos(θ)0001]\begin{aligned} &R_1[\theta]= \begin{bmatrix} 1&0&0\\ 0&cos(\theta)&sin(\theta)\\ 0&-sin(\theta)&cos(\theta) \end{bmatrix}; R_2[\theta]= \begin{bmatrix} cos(\theta)&0&sin(\theta)\\ 0&1&0\\ -sin(\theta)&0&cos(\theta) \end{bmatrix}\\ &R_3[\theta]= \begin{bmatrix} cos(\theta)&sin(\theta)&0\\ -sin(\theta)&cos(\theta)&0\\ 0&0&1 \end{bmatrix} \end{aligned} R1[θ]=⎣⎡1000cos(θ)−sin(θ)0sin(θ)cos(θ)⎦⎤;R2[θ]=⎣⎡cos(θ)0−sin(θ)010sin(θ)0cos(θ)⎦⎤R3[θ]=⎣⎡cos(θ)−sin(θ)0sin(θ)cos(θ)0001⎦⎤
带入得：
R3[−(π/2+λ)]R1[−(π/2−φ)]=(−sin(λ)cos(λ)0−cos(λ)sin(φ)−sin(λ)sin(φ)cos(φ)cos(λ)cos(φ)sin(λ)cos(φ)sin(φ))(3)R_3[-(\pi/2+\lambda)]R_1[-(\pi/2-\varphi)]= \begin{pmatrix} -sin(\lambda)&cos(\lambda)&0\\ -cos(\lambda)sin(\varphi)&-sin(\lambda)sin(\varphi)&cos(\varphi)\\ cos(\lambda)cos(\varphi)&sin(\lambda)cos(\varphi)&sin(\varphi)\tag{3} \end{pmatrix} R3[−(π/2+λ)]R1[−(π/2−φ)]=⎝⎛−sin(λ)−cos(λ)sin(φ)cos(λ)cos(φ)cos(λ)−sin(λ)sin(φ)sin(λ)cos(φ)0cos(φ)sin(φ)⎠⎞(3)
通常，e系的x轴是指向n系原点所在子午线与赤道交点的，即λ=0\lambda=0λ=0代入(3)可得：
Cen=R3[−(π/2)]R1[−(π/2−φ)]=(010−sin(φ)0cos(φ)cos(φ)0sin(φ))(4)C^n_e= R_3[-(\pi/2)]R_1[-(\pi/2-\varphi)]= \begin{pmatrix} 0&1&0\\ -sin(\varphi)&0&cos(\varphi)\\ cos(\varphi)&0&sin(\varphi)\tag{4} \end{pmatrix} Cen=R3[−(π/2)]R1[−(π/2−φ)]=⎝⎛0−sin(φ)cos(φ)1000cos(φ)sin(φ)⎠⎞(4)
3.3.CieC^e_iCie:地球坐标系(e)到惯性系(i)的转换矩阵
e系相对i系是沿z轴的定轴转动,转动角度为ωie⋅t{\omega}_{ie}\cdot tωie⋅t旋转矩阵为:
Cie=[cosωie⋅tsinωie⋅t0−sinωie⋅tcosωie⋅t0001](5)C^e_i= \begin{bmatrix} cos{\omega}_{ie}\cdot t&sin{\omega}_{ie}\cdot t&0\\ -sin{\omega}_{ie}\cdot t&cos{\omega}_{ie}\cdot t&0\\ 0&0&1\tag{5} \end{bmatrix} Cie=⎣⎡cosωie⋅t−sinωie⋅t0sinωie⋅tcosωie⋅t0001⎦⎤(5)
3.4.Cbib0C^{i_{b_0}}_bCbib0:初始捷联惯导惯性系(ib0i_{b_0}ib0)到捷联惯导坐标系(b)的转换矩阵
初始时刻两个坐标系是重合的，即Cbib0(t=0)=IC_b^{i_{b_0}} (t=0)=ICbib0(t=0)=I，之后利用陀螺输出的角运动信息，通过捷联惯导姿态更新算法(下式)可以很容易实时求得矩阵Cbib0(t)C_b^{i_{b_0}} (t)Cbib0(t):
C˙tib0(t)=Cbib0(t)[ωi0bb(t)×](6)\dot C_t^{i_{b_0}}(t)=C_b^{i_{b_0}}(t)[\omega ^b_{i_0b}(t)_{\times}]\tag{6} C˙tib0(t)=Cbib0(t)[ωi0bb(t)×](6)
其中[ωi0bb(t)×][\omega ^b_{i_0b}(t)_{\times}][ωi0bb(t)×]表示由陀螺测量输出的角增量构成的反对称矩阵，可通过n子样算法近似计算等效旋转矢量后求解，即ins的姿态更新。
3.5.Cib0iC^i_{i_{b_0}}Cib0i:惯性系(i)到初始捷联惯导惯性系(ib0i_{b_0}ib0)的转换矩阵
（此处推导有区别于秦永元论文与严恭敏博士后报告论文，重点在于静基座、微小晃动）
在静止基座上，加速度计输出的比力在坐标系ib0i_{b_0}ib0的投影为：
Cbib0f~sfb=Cbib0(−gb+∇b)(7.1)C^{i_{b_0}}_b\tilde f^b_{sf}=C^{i_{b_0}}_b(-g^b+\nabla^b)\tag{7.1} Cbib0f~sfb=Cbib0(−gb+∇b)(7.1)
其中∇b=δfsfb+v˙b\nabla^b=\delta f^b_{sf}+\dot v^b∇b=δfsfb+v˙b是比力测量误差和线速度干扰。
对(7.1)在[t0,tk][t_0,t_k][t0,tk]内积分
v~b0i=∫t0tkCbib0f~sfbdt=−Ciib0∫t0tkgidt+∫t0tk∇ib0dt(7.2)\tilde v^i_{b_0}= \int_{t_0}^{t_k} C^{i_{b_0}}_b\tilde f^b_{sf}\, {\rm d}t =-C^{i_{b_0}}_i\int_{t_0}^{t_k} g^i\, {\rm d}t+ \int_{t_0}^{t_k} \nabla^{i_{b_0}}\, {\rm d}t\tag{7.2} v~b0i=∫t0tkCbib0f~sfbdt=−Ciib0∫t0tkgidt+∫t0tk∇ib0dt(7.2)
记
vb0i=−Ciib0∫t0tkgidtv^i_{b_0}=-C^{i_{b_0}}_i\int_{t_0}^{t_k} g^i\,{\rm d}tvb0i=−Ciib0∫t0tkgidt
其中
gi=CeiCne[00−g]=[−gcosLcosωie(t−t0)−gcosLsinωie(t−t0)−gsinL](7.3)g^i=C^i_eC^e_n \begin {bmatrix} 0\\ 0\\ -g \end{bmatrix}= \begin{bmatrix} -gcosLcos\omega_{ie}(t-t_0)\\ -gcosLsin\omega_{ie}(t-t_0)\\ -gsinL \end{bmatrix}\tag{7.3} gi=CeiCne⎣⎡00−g⎦⎤=⎣⎡−gcosLcosωie(t−t0)−gcosLsinωie(t−t0)−gsinL⎦⎤(7.3)
积分得
vi(tk)=[gcosLωiesinωietkgcosLωie(1−cosωietk)tkgsinL](7.4)v^i(t_k)= \begin{bmatrix} \cfrac{gcosL}{\omega_{ie}}sin\omega_{ie}t_k\\ \cfrac{gcosL}{\omega_{ie}}(1-cos\omega_{ie}t_k)\\ t_k g sinL \end{bmatrix}\tag{7.4} vi(tk)=⎣⎢⎢⎢⎢⎡ωiegcosLsinωietkωiegcosL(1−cosωietk)tkgsinL⎦⎥⎥⎥⎥⎤(7.4)
再次积分得：
Pi(tk)=[gcosLωie2(1−cosωietk)−gcosLωie2(ωietk−sinωietk)tk2gsinL2](7.5)P^i(t_k)= \begin{bmatrix} \cfrac{gcosL}{\omega^2_{ie}}(1-cos\omega_iet_k)\\ -\cfrac{gcosL}{\omega^2_{ie}}(\omega_{ie}t_k-sin\omega_{ie}t_k)\\ \cfrac{t_k^2gsinL}{2} \end{bmatrix}\tag{7.5} Pi(tk)=⎣⎢⎢⎢⎢⎢⎢⎡ωie2gcosL(1−cosωietk)−ωie2gcosL(ωietk−sinωietk)2tk2gsinL⎦⎥⎥⎥⎥⎥⎥⎤(7.5)
当线运动极小时：
v~b0i≈vb0i=Ciib0viP~b0i≈Pib0=Ciib0Pi\tilde v^i_{b_0}\approx v^i_{b_0}=C^{i_{b_0}}_i v^i\\ \tilde P^i_{b_0}\approx P^{i_{b_0}}=C^{i_{b_0}}_iP^i v~b0i≈vb0i=Ciib0viP~b0i≈Pib0=Ciib0Pi
v~b0i,P~b0i\tilde v^i_{b_0},\tilde P^i_{b_0}v~b0i,P~b0i为真实的速度和位移量，可以由INS速度、位置更新求解。
采取两个不同时间间隔的数据tk1,tk2t_{k_1},t_{k_2}tk1,tk2构建双矢量定姿方程(参考书上1P194)：
C^iib0=[[vi(tk1)]T[vi(tk2)]T[vi(tk1)]T×[vi(tk2)]T]−1⋅[[v~ib0(tk1)]T[v~ib0(tk2)]T[v~ib0(tk1)]T×[v~ib0(tk2)]T](7.6)\hat C_i^{i_{b_0}}= \begin{bmatrix} [v^i(t_{k_1})]^T\\ [v^i(t_{k_2})]^T\\ [v^i(t_{k_1})]^T\times[v^i(t_{k_2})]^T \end{bmatrix}^{-1} \cdot \begin{bmatrix} [\tilde v^{i_{b_0}}(t_{k_1})]^T\\ [\tilde v^{i_{b_0}}(t_{k_2})]^T\\ [\tilde v^{i_{b_0}}(t_{k_1})]^T\times[\tilde v^{i_{b_0}}(t_{k_2})]^T \end{bmatrix}\tag {7.6} C^iib0=⎣⎡[vi(tk1)]T[vi(tk2)]T[vi(tk1)]T×[vi(tk2)]T⎦⎤−1⋅⎣⎡[v~ib0(tk1)]T[v~ib0(tk2)]T[v~ib0(tk1)]T×[v~ib0(tk2)]T⎦⎤(7.6)
位置矢量的求解与上式原理相同。
最后将解得的(4)(5)(6)(7.6)带入(2)求得CbnC^n_bCbn，完成初始对准。
该方法对角运动有很好的抗干扰能力，在线运动不明显的时候，短时间即可达到较高精度对准。
4.代码解析
CAlign0.m
35行—计算等效旋转矢量与补偿后的速度增量
36行—计算初始比力
40行—速度更新，对应(7.6)中的 v~ib0\tilde v^{i_{b_0}}v~ib0
41行—位置更新，对应(7.6)中的 P~ib0\tilde P^{i_{b_0}}P~ib0
45行—计算惯性系观测速度位移，对应(7.*)中的 gi,vi,Pig^i,v^i,P^igi,vi,Pi 。代码解析见下文
46行—初始捷联惯导惯性系到捷联惯导坐标系(t)的转换矩阵，对应(6)
52行—对应(4)乘以(5)
47行—选取t(k1)=0.5t(k2)t_(k_1 )=0.5t_(k_2 )t(k1)=0.5t(k2)时刻，为参与定姿的矢量变量赋值
56行—对应(7.6)
57行—对应(2)完成求解
i0fvp
21行—对应(7.3)
22行—对应(7.4)
23行—对应(7.5)
码字不易，转载请注明出处
《捷联惯导算法与组合导航》（严恭敏著）第七章第一节。 ↩︎ ↩︎ ↩︎
“摇摆基座上基于信息的捷联惯导粗对准研究”(秦永元) ↩︎
PSINS惯性系初始对准与代码解读相关推荐
- 200行代码解读TDEngine背后的定时器
作者 | beyondma来源 | CSDN博客 导读:最近几周,本文作者几篇有关陶建辉老师最新的创业项目-TdEngine代码解读文章出人意料地引起了巨大的反响,原以为C语言已经是昨日黄花,不过从读 ...
- 装逼一步到位！GauGAN代码解读来了
↑↑↑关注后"星标"Datawhale 每日干货 & 每月组队学习,不错过 Datawhale干货 作者:游璐颖,福州大学,Datawhale成员 AI神笔马良 如何装逼一 ...
- Unet论文解读代码解读
论文地址:http://www.arxiv.org/pdf/1505.04597.pdf 论文解读 网络 架构: a.U-net建立在FCN的网络架构上,作者修改并扩大了这个网络框架,使其能够使用很少 ...
- Lossless Codec---APE代码解读系列(二)
APE file 一些概念 APE代码解读系列(一) APE代码解读系列(三) 1. 先要了解APE compression level APE主要有5level, 分别是: CompressionL ...
- RT-Thread 学习笔记（五）—— RTGUI代码解读
---恢复内容开始--- RT-Thread 版本:2.1.0 RTGUI相关代码解读,仅为自己学习记录,若有错误之处,请告知maoxudong0813@163.com,不胜感激! GUI流程: ma ...
- vins 解读_代码解读 | VINS 视觉前端
AI 人工智能 代码解读 | VINS 视觉前端 本文作者是计算机视觉life公众号成员蔡量力,由于格式问题部分内容显示可能有问题,更好的阅读体验,请查看原文链接:代码解读 | VINS 视觉前端 v ...
- BERT：代码解读、实体关系抽取实战
目录 前言 一.BERT的主要亮点 1. 双向Transformers 2.句子级别的应用 3.能够解决的任务 二.BERT代码解读 1. 数据预处理 1.1 InputExample类 1.2 In ...
- shfflenetv2代码解读
shufflenetv2代码解读 目录 shufflenetv2代码解读 概述 shufflenetv2网络结构图 shufflenetv2架构参数 shufflenetv2代码细节分析 概述 shu ...
- GoogLeNet代码解读
GoogLeNet代码解读 目录 GoogLeNet代码解读 概述 GooLeNet网络结构图 1)从输入到第一层inception 2)从第2层inception到第4层inception 3)从第 ...
最新文章
- python用渐变色画圆_利用python控制Autocad:pyautocad方式
- Android学习笔记(1)----播放音乐文件
- python中类方法与实例方法的区别-python中类方法，实例方法，静态方法的作用和区别...
- 游标 每天给每个用户发钱
- 14-数据库连接池和jdbc优化
- kerberos 主从安装
- 车联网发展对汽车经销商的影响
- 华为笔试题大全(史上最齐全)
- Go避免使用大堆造成的高GC开销
- 全球及中国缓控释肥行业产能规模与投资盈利能力分析报告2022版
- C语言伽罗华域乘法,伽罗瓦域上的乘法
- 我们是选择开源CRM，还是选择商业CRM?
- linux dd命令刻录u盘,Linux使用dd命令烧录启动U盘
- Java接口测试工具rap_接口文档管理工具-Postman、Swagger、RAP（转载）
- 构建红图注册到蓝图中
- 使用百度文字识别API进行图片中文字的识别
- 计算机硬盘的性能指标,对于硬盘而言它有哪些性能指标
- Python制作专属有声小说（调用百度语音合成接口）
- 设置图片跟随鼠标移动
- 什么是形式参数，什么是实际参数，它们的区别和各自的定义是什么