SINS/GNSS组合导航：捷联惯导静基座下初始对准（一）粗对准（Matlab）

SINS初始对准要测定系统的姿态变换矩阵，分为粗对准和精对准两个部分，粗短准阶段利用重力和地球自转量粗略计算姿态矩阵；在精对准阶段，不仅依靠前面的姿态矩阵测量值及重力和地球自转量，还要依据惯性器件的输出值和观测量，用合适的滤波方法，对系统的误差量进行估计，修正姿态矩阵，计算出精确的姿态变换矩阵。

静态基座下解析粗对准

粗对准的目的是快速的得到一个粗略的姿态矩阵，即载体系b到导航系e的姿态矩阵，为精对准做准备，因此缩短对准时间是粗对准的重要目的。粗对准时，忽略干扰信息的影响，认为载体处于理想的静止状态，把地球自转角速度和重力矢量信息作为粗对准时刻陀螺仪和加速度计的信息，根据公式推导计算出姿态矩阵。

静基座状态下，经度纬度确定，由重力计算公式可以精确的求出当地的重力加速度，同时可以求出精确的地球自转角速率，他们在地理坐标系的投影可以表示为：

$g^e=\begin{bmatrix} g^e_x\\ g^e_y \\ g^e_z \end{bmatrix}=\begin{bmatrix} 0\\ 0\\ g \end{bmatrix} w_{ie}^e=\begin{bmatrix} w^e_{iex}\\ w^e_{iey} \\ w^e_{iez} \end{bmatrix}=\begin{bmatrix} 0\\ w_{ie}cosL\\ w_{ie}sinL \end{bmatrix}$ （1）

构造新的向量 $r=g\times w_{ie}$ ，得到 $r^e=g^e\times w_{ie}^e$ ，并且b系表示如下：

$\\ g^b=C_e^bg^e\\ w_{ie}^b=C_e^bw_{ie}^e\\ r^b=C_e^br^e$ （2）

$C_e^b$ 是正交矩阵，满足 $(C_e^b)^T=(C_e^b)^{-1}=C_b^e$ ，将（7）转置可以得到：

$\\ (g^b)^T=(g^e)^TC_b^e\\ (w_{ie}^b)^T=(w_{ie}^e)^TC_b^e\\ (r^b)^T=(r^e)^TC_b^e$ （3）

写成矩阵的形式：

$\begin{bmatrix} (g^b)^T\\ (w_{ie}^b)^T \\ (r^b)^T \end{bmatrix}=\begin{bmatrix} (g^e)^T\\ (w_{ie}^e)^T \\ (r^e)^T \end{bmatrix}C_b^e$ （4）

取逆分离 $C_b^e$ 得到：

$C_b^e=\begin{bmatrix} (g^e)^T\\ (w_{ie}^e)^T \\ (r^e)^T \end{bmatrix}^{-1}\begin{bmatrix} (g^b)^T\\ (w_{ie}^b)^T \\ (r^b)^T \end{bmatrix}$ （5）

将（6）取逆矩阵，并与（7）代入（10）即可得到：

$C_b^e=\begin{bmatrix} (g^e)^T\\ (w_{ie}^e)^T \\ (r^e)^T \end{bmatrix}^{-1}\begin{bmatrix} (g^b)^T\\ (w_{ie}^b)^T \\ (r^b)^T \end{bmatrix}=\begin{bmatrix} 0 & 0 &\frac{1}{gw_{ie}cosL} \\ \frac{tanL}{g} & \frac{1}{w_{ie}cosL} & 0\\ -\frac{1}{g}& 0 & 0 \end{bmatrix}\begin{bmatrix} (g^b)^T\\ (w_{ie}^b)^T \\ (r^b)^T \end{bmatrix}$ （7）

$g^b=[g^b_x,g^b_y,g^b_z]^T$ 是陀螺仪的输出， $w_{ie}^b=[w^e_{iex},w^e_{iey} \,w^e_{iez}]$ 是加速度计的输出， $r^b$ 根据这两个信息叉乘得到。 $g$ 为地球重力加速度， $w_{ie}$ 为地球自转角速度的。

通过上述过程，粗对准实现了由加速度计和陀螺输出到姿态矩阵的转换，得到粗略的对准矩阵，代码如下：

%INS读初值
dataPath = '../Data/';
fidOut = fopen([dataPath,'INS.txt'],'r');
initOut = fscanf(fidOut,'%e',[13,1]);
lambda = degree2radian(initOut(2));
L = degree2radian(initOut(3));
H = initOut(4);
g = G_LH(L,H);
[RM,RN] = R_M_N(L);
Cne = C_N_E(lambda,L);%IMU读数据用于粗对准（对数据做平均消减随机误差的影响）
fidIn = fopen([dataPath,'IMU.txt'],'r');
Wbib = [];
Fb = [];
for i = 1:2500imu = fscanf(fidIn,'%e',[7,1]);Wbib = [Wbib [imu(5);imu(6);imu(7)]]; %#ok<*AGROW>Fb = [Fb [imu(2);imu(3);imu(4)]];
end
Wbib = sum(Wbib,2)./size(Wbib,2);
Fb = sum(Fb,2)./size(Fb,2);%[超参]由元器件特性、安装方式、系统结构等预先确定
Fb = -Fb;%粗对准
Vn_T_inv = [0 0 1/(g*Weie(3)*cos(L));tan(L)/g 1/(Weie(3)*cos(L)) 0;-1/g 0 0];
Vb_T = [Fb(1) Fb(2) Fb(3);Wbib(1) Wbib(2) Wbib(3);...Fb(2)*Wbib(3) - Fb(3)*Wbib(2) Fb(3)*Wbib(1) - Fb(1)*Wbib(3) Fb(1)*Wbib(2) - Fb(2)*Wbib(1)];
Tnb = Vn_T_inv*Vb_T;%Tnb = orth(Tnb);%Tnb = Normalization(Tnb,1);%Tnb = Orthogonalization_Schmidt(Tnb);
[psi,theta,gamma] = AnttitudeAngle_Tnb(Tnb);
Tnb = T_N_B(psi,theta,gamma);%结果输出
disp('粗对准结果：');
disp(['ψ：',num2str(radian2degree(psi)),'°']);
disp(['θ：',num2str(radian2degree(theta)),'°']);
disp(['γ：',num2str(radian2degree(gamma)),'°']);

SINS/GNSS组合导航：捷联惯导静基座下初始对准（一）粗对准（Matlab）相关推荐

捷联惯导基础知识解析之四（粗/精对准和GPS/IMU和GPS/里程计组合导航）
初始对准(粗.精对准)/组合导航一.捷联惯导粗对准目的:寻找.确定参考导航坐标系:结果表现形式:得到姿态矩阵(进而可以求出欧拉角.四元数等) 前提:在导航坐标系(比如:东北天)下的重力矢量.地球旋 ...
基于matlab的捷联惯导算法设计及仿真,基于 Matlab 的捷联惯导算法设计及仿真1doc.doc...
基于 Matlab 的捷联惯导算法设计及仿真1doc 基于 Matlab 的捷联惯导算法设计及仿真1 严恭敏西北工业大学航海学院,西安 (710072) E-mail:yangongmin@163. ...
组合导航算法（一）之捷联惯导更新及组合模式
捷联惯导基本算法惯性导航技术于20世纪50年代最初开始投入使用,可分为物理平台与模拟平台.物理平台就是平台式惯性导航系统(PINS).模拟平台又称捷联式惯性导航系统(SINS),它以计算机为平台,随 ...
捷联惯导算法与组合导航原理学习——四元数和姿态阵转换（二）
四元数和姿态阵转换学习资料参考: [1] 严恭敏,翁浚. 捷联惯导算法与组合导航原理[M]. 西安: 西北工业大学出版社, 2019.8. Quaternion.h #pragma once #in ...
捷联惯导算法与组合导航原理学习——等效旋转矢量和姿态阵转换（一）
等效旋转矢量和姿态阵转换学习资料参考: [1] 严恭敏,翁浚. 捷联惯导算法与组合导航原理[M]. 西安: 西北工业大学出版社, 2019.8. EquRotationVec.h #pragma o ...
捷联惯导总结--初始对准，位置标定，INS姿态更新，GPS/INS组合
惯导及组合导航回顾 2018.09.16 今天和17系的同学一起把惯导的流程捋了一遍,为了加深自己的记忆,这里在前面把心得大致列出来. 我们这里只考虑捷联式惯导及松组合首先拿到惯性传感器(加速度计 ...
捷联惯导-坐标系-观测值补偿-对准-编排-时间更新-测量更新
1 坐标系捷联惯导涉及到多种坐标系. 1.1 坐标系定义惯性坐标系(i):以地球质心为原点,ziz_izi指向地球自转轴,xix_ixi轴位于赤道面内,指向空间任意点:yiy_iyi轴构成右 ...
捷联惯导知识点之非直角坐标系到直角坐标系
1.从非直角坐标系到直角坐标系的矩阵变换 2.以正交三角分解为例: 根据矩阵的 QR 分解理论,非奇异阵 Cba 总可以分解为单位正交阵 CbB和上三角阵CBa 之乘积的形式, 即在偏差角为小量情形 ...
基于PSINS工具箱的卡尔曼滤波与SINS/GNSS组合导航
文章目录卡尔曼滤波与SINS/GNSS组合导航典型的SINS/GNSS组合滤波 POS处理卡尔曼滤波与SINS/GNSS组合导航用于Kalman滤波的函数有: psinstypedef(nnm ...

SINS/GNSS组合导航：捷联惯导静基座下初始对准（一）粗对准（Matlab）

SINS/GNSS组合导航：捷联惯导静基座下初始对准（一）粗对准（Matlab）相关推荐

最新文章

热门文章

SINS/GNSS组合导航：捷联惯导静基座下初始对准 （一）粗对准（Matlab）

SINS/GNSS组合导航：捷联惯导静基座下初始对准 （一）粗对准（Matlab）相关推荐

最新文章

热门文章

SINS/GNSS组合导航：捷联惯导静基座下初始对准（一）粗对准（Matlab）

SINS/GNSS组合导航：捷联惯导静基座下初始对准（一）粗对准（Matlab）相关推荐