低成本MEMS惯导系统的捷联惯导解算MATLAB仿真

一、姿态角转换为四元数
二、四元数转换为姿态角
三、反对称阵
四、位置更新
五、姿态更新
六、程序及数据
- 主程序：
- 子程序：
- 数据及完整程序

之前将高成本的捷联惯导忽略地球自转、圆锥曲线运动以及划桨运动等化简为可适用低成本的捷联惯导MATLAB程序，但是其中的子程序都是用的严老师的代码，想自己锻炼一下自己的代码能力，所以对低成本的捷联惯导进行重新编写！！

一、姿态角转换为四元数

对于单位四元数的三角函数表示，有：

Q=q0+qv=cos⁡ϕ2+usin⁡ϕ2{\bf{Q}} = {q_0} + {{\bf{q}}_v} = \cos \frac{\phi }{2} + {\bf{u}}\sin \frac{\phi }{2}Q=q0+qv=cos2ϕ+usin2ϕ
（1）航向角ψ\psiψ，运载体纵轴在当地水平面上的投影线与当地地理北向的夹角，常取北偏东为正，即若从空中俯视运载体，地理北向顺时针旋转至纵轴水平投影线的角度，角度范围为0～360° ；
（2）俯仰角θ\thetaθ，运载体纵轴与其水平投影线之间的夹角，当运载体抬头时角度定义为正，角度范围-90°～90°；
（3）横滚角γ\gammaγ，运载体立轴与纵轴所在铅垂面之间的夹角，当运载体向右倾斜时角度定义为正，角度范围-180°～180°。
虽然航向角ψ\psiψ习惯上常定义为北偏东为正，但是当定义导航坐标系为“东-北-天”地理坐标系时，航向角在绕天向轴转动时不符合右手规则。为了符合右手规则和推导公式简洁对称，除非特别说明，本节将航向角定义为北偏西为正，且取值范围为 (−π，π](-\pi，\pi](−π，π]，这是在后续阅读相关公式时需要特别注意的。当然，如果要将相关公式应用于北偏东的航向角，只需再增加一个简单的航向角转换过程即可。
在“东-北-天312”欧拉角定义下，由欧拉角求解四元数的公式为：

程序验证：

%***************************************
% 将欧拉角转为四元数：att2que(att)
% input：att=[pitch;roll;yaw];           unit：rad
% output：qnb=[q0;q1;q2;q3];             q0为实部
%***************************************
theta = 0; gamma = 0; psi = 90*pi/180;
att = [theta;gamma;psi];
s1 = sin(theta/2); s2 = sin(gamma/2); s3 = sin(psi/2);
c1 = cos(theta/2); c2 = cos(gamma/2); c3 = cos(psi/2);
qnb= [c3*c1*c2 - s3*s1*s2;c3*s1*c2 - s3*c1*s2;s3*s1*c2 - c3*c1*s2;s3*c1*c2 - c3*s1*s2;]

结果：

>> exe0.7071000.7071

改写为子程序备用：

function att2que qnb = (att);
att = [theta;gamma;psi];
s1 = sin(theta/2); s2 = sin(gamma/2); s3 = sin(psi/2);
c1 = cos(theta/2); c2 = cos(gamma/2); c3 = cos(psi/2);
qnb= [c3*c1*c2 - s3*s1*s2;c3*s1*c2 - s3*c1*s2;s3*s1*c2 - c3*c1*s2;s3*c1*c2 - c3*s1*s2;];

二、四元数转换为姿态角

由四元数直接求解欧拉角并不容易。实际上，可通过姿态阵作为中间过渡量，先由四元数计算姿态阵，再由姿态阵计算欧拉角，综合一起后其结果为：

function att = que2att(qnb)
%***************************************
% 四元数转换为姿态角：que2att(qnb)
% input：qnb=[q0;q1;q2;q3];             q0为实部
% output：att=[pitch;roll;yaw];           unit：rad
%***************************************
att(1) = asin(2*(qnb(3)*qnb(4) + qnb(1)*qnb(2)));
if abs(2*(qnb(3)*qnb(4) + qnb(1)*qnb(2)) <= 0.999999att(2) = -atan2*(2*(qnb(2)*qnb(4) - qnb(1)*qnb(3))),qnb(1)^2 - qnb(2)^2 - qnb(3)^2 + qnb(4)^2);att(3) = -atan2*(2*(qnb(2)*qnb(3) - qnb(1)*qnb(4))),qnb(1)^2 - qnb(2)^2 + qnb(3)^2 - qnb(4)^2);
elseatt(2) = -atan2*(2*(qnb(2)*qnb(4) + qnb(1)*qnb(3))),qnb(1)^2 + qnb(2)^2 - qnb(3)^2 - qnb(4)^2);att(3) = 0;
end

三、反对称阵

引入三维向量的反对称阵概念后，两向量之间叉乘运算可等价表示为前一向量的反对称阵与后一向量之间的矩阵乘法运算，亦即：

funcion ssm = skew(v)
%***************************************
% 求三维向量的反对称阵（skew symmetric matrix）：skew(v)
% input：v = [v_x;v_y;v_z];
% output：ssm = [ssm_x;ssm_y;ssm_z];
%***************************************
ssm = [0,-v(3),v(2);v(3),0,-v(1);-v(2),v(1),0];

四、位置更新

f = 1/298.257223563;
R_e = 6.378136998405e6;
e = sqrt(2f-f^2);
R_N = R_e/(1-e^2*(sin(pos(1)))^2)^0.5;
R_M = R_N*(1-e^2)/(1-e^2*(sin(pos(1)))^2);
R_Mh = R_M + pos(3);
R_Nh = R_N + pos(3);
M_pv = [0,1/R_Mh,0;sec(pos(1))/R_Nh,0,0;0,0,1];vn1 = (vn2+vn1)/2;
pos = pos + M_pv*vn1*nts;

五、姿态更新

wie = 7.2921151467e-5;
wnie = [0,wie*cos(pos(1)),wie*sin(pos(1))]';
wnen = vn.*[-1/(R_M + pos(3));1/(R_N + pos(3));tan(pos(1))/(R_N + pos(3))];
wnin = wnie + wnen;
% 姿态更新
q_bb = [cos(norm(delta_theta_m)/2);(delta_theta_m/norm(delta_theta_m))'*sin((norm(delta_theta_m)/2))];
qnb = qmul(rv2q(-wnin*nts), qmul(qnb, q_bb));
qnb = qnormlz(qnb);

六、程序及数据

主程序：

clc;
clear;
imu = load ('imu.mat');                                %imu数据： avp=[wm, vm, tt(2:end)]; gx(rad)  gy  gz ax (m/s)  ay  az time
trj = load ('trj.mat');                                %trj=[att, vn, pos]; att(度) vn(米每秒) pos(米)
deg2rad = pi/180;
att = [0;0;90]*deg2rad; vn = [0;0;0]; pos = [[34;108]*deg2rad;100];
qnb = att2que(att);
n = 1;t = 0;ts = 0.01;len = length(imu.avp);avps = zeros(len,10);
for k = 1:n:lent = t + ts;wm = imu.avp(k,1:3);vm = imu.avp(k,4:6);[qnb,vn,pos] = my_insupdate(qnb,vn,pos,wm,vm,ts);cnb = que2att(qnb);cnb(3) = mod(cnb(3),2*pi);avps(k,:) = [cnb';vn;pos;t]';
end%姿态
tt=length(avps);
tf=length(trj.trj);
figure(1);
subplot(321);
plot(1:tt,(avps(1:tt,1:2)/deg2rad),1:tf,(trj.trj(1:tf,1:2)/deg2rad),'--'); title('俯仰角和横滚角');xlabel('d');ylabel('\theta,\gamma(\circ)');
legend('\it\theta','\it\gamma','\bf\theta','\bf\gamma');grid on;
subplot(322);
plot(1:tt,(avps(1:tt,3)/deg2rad),1:tf,(trj.trj(1:tf,3)/deg2rad),'--'); title('偏航角');xlabel('d');ylabel('\Phi(\circ)');
legend('\it\Phi','\bf\Phi') ;grid on;
subplot(323);
plot(1:tt,(avps(1:tt,4:5)),1:tf,(trj.trj(1:tf,4:5)),'--'); title('速度');xlabel('d');ylabel('Vel/(m.s^{-1})');
legend('\itv\rm_E','\itv\rm_N','\bfv\rm_E','\bfv\rm_N');grid on;
subplot(324);
plot(1:tt,(avps(1:tt,6)),1:tf,(trj.trj(1:tf,6)),'--'); title('速度');xlabel('d');ylabel('Vel/(m.s^{-1})');
legend('\itv\rm_U','\bfv\rm_U');grid on;
subplot(325);
plot(1:tt,deltapos(avps(1:tt,7:9)),1:tf,deltapos(trj.trj(1:tf,7:9)),'--'); title('位置');xlabel('d');ylabel('\DeltaPos / m');
legend('\Delta\itL', '\Delta\it\lambda', '\Delta\ith','\Delta\bfL', '\Delta\bf\lambda', '\Delta\bfh');grid on;

子程序：

function [qnb,vn2,pos]=my_insupdate(qnb,vn1,pos,delta_theta_m,delta_v_m,ts)
n=1;
nts=n*ts;
g0=9.780325333434361;                                                  % 单位;m/s^2
sl = sin(pos(1));                                                      % pos(1)=L
sl2 = sl*sl;  sl4 = sl2*sl2;
gLh = g0*(1+5.27094e-3*sl2+2.32718e-5*sl4)-3.086e-6*pos(3);
gn = [0;0;-gLh];f = 1/298.257223563;
R_e = 6.378136998405e6;
e = sqrt(2*f-f^2);
R_N = R_e/(1-e^2*sl2)^0.5;
R_M = R_N*(1-e^2)/(1-e^2*sl2);
R_Mh = R_M + pos(3);
R_Nh = R_N + pos(3);
M_pv = [0,1/R_Mh,0;sec(pos(1))/R_Nh,0,0;0,0,1];wie = 7.2921151467e-5;
wnie = [0,wie*cos(pos(1)),wie*sin(pos(1))]';
wnen = vn1.*[-1/(R_M + pos(3));1/(R_N + pos(3));tan(pos(1))/(R_N + pos(3))];
wnin = wnie + wnen;%速度更新
vsf =q2mat(qnb)*(delta_v_m + 1/2*cross(delta_theta_m,delta_v_m))';      % avp2(4:6) = avp1(4:6) + vsf + gn*Tm;
vn2 = vn1 + vsf + gn*nts;
%位置更新
vn1 = (vn2+vn1)/2;
pos = pos + M_pv*vn1*nts;
% 姿态更新
q_bb = [cos(norm(delta_theta_m)/2);(delta_theta_m/norm(delta_theta_m))'*sin((norm(delta_theta_m)/2))];
qnb = qmul(rv2q(-wnin*nts), qmul(qnb, q_bb));
qnb = qnormlz(qnb);
end

数据及完整程序

https://download.csdn.net/download/qq_38364548/87380265

低成本MEMS惯导系统的捷联惯导解算MATLAB仿真相关推荐

捷联惯导系统学习2.5（等效旋转矢量微分方程的泰勒级数解）
在高精度的捷联惯导系统中,陀螺仪姿态的解算往往是通过采集一定时间内的角增量信息, 计算角增量信息计算出等效旋转矢量,在通过等效旋转矢量递推余弦阵或者四元数,完成姿态更新. 等效旋转矢量微分方程的泰勒级 ...
捷联惯导基础知识解析之五（低成本姿态航向参考系统）
陀螺仪精度0.1°/s:加速度计精度5mg:主要指零偏重复性! 一.简化的惯导算法 1.姿态更新陀螺仪输出直接进行积分,得到角增量: 2.速度更新: 在导航系下,完整的速度微分方程,如下: 其中,在 ...
捷联惯导系统学习4.1(惯导数值更新算法)
1 常用坐标系的定义 (1)地心惯性坐标系(i 系,inertial frame) 用oixiyizio_ix_iy_iz_ioixiyizi表示,原点以地球为中心, 原点oio_ioi在地 ...
捷联惯导总结--初始对准，位置标定，INS姿态更新，GPS/INS组合
惯导及组合导航回顾 2018.09.16 今天和17系的同学一起把惯导的流程捋了一遍,为了加深自己的记忆,这里在前面把心得大致列出来. 我们这里只考虑捷联式惯导及松组合首先拿到惯性传感器(加速度计 ...
组合导航算法（一）之捷联惯导更新及组合模式
捷联惯导基本算法惯性导航技术于20世纪50年代最初开始投入使用,可分为物理平台与模拟平台.物理平台就是平台式惯性导航系统(PINS).模拟平台又称捷联式惯性导航系统(SINS),它以计算机为平台,随 ...
基于matlab的捷联惯导算法设计及仿真,基于 Matlab 的捷联惯导算法设计及仿真1doc.doc...
基于 Matlab 的捷联惯导算法设计及仿真1doc 基于 Matlab 的捷联惯导算法设计及仿真1 严恭敏西北工业大学航海学院,西安 (710072) E-mail:yangongmin@163. ...
捷联惯导基础知识解析之四（粗/精对准和GPS/IMU和GPS/里程计组合导航）
初始对准(粗.精对准)/组合导航一.捷联惯导粗对准目的:寻找.确定参考导航坐标系:结果表现形式:得到姿态矩阵(进而可以求出欧拉角.四元数等) 前提:在导航坐标系(比如:东北天)下的重力矢量.地球旋 ...
基于四元素法的捷联惯导姿态更新算法
摘要本文主要介绍了机载捷联惯导系统常用的姿态更新算法--四元素法,并重点介绍了利用四元素法进行姿态更新的一般过程. 关键词:四元素法,连贯导,姿态 1 引言 ...
【滤波跟踪】基于matlab捷联惯导仿真【含Matlab源码 1935期】
⛄一.获取代码方式获取代码方式1: 完整代码已上传我的资源:[滤波跟踪]基于matlab捷联惯导仿真[含Matlab源码 1935期] 点击上面蓝色字体,直接付费下载,即可. 获取代码方式2: 付费 ...

低成本MEMS惯导系统的捷联惯导解算MATLAB仿真

低成本MEMS惯导系统的捷联惯导解算MATLAB仿真

一、姿态角转换为四元数

二、四元数转换为姿态角

三、反对称阵

四、位置更新

五、姿态更新

六、程序及数据

主程序：

子程序：

数据及完整程序

低成本MEMS惯导系统的捷联惯导解算MATLAB仿真相关推荐

最新文章

热门文章