IMU静态初始粗对准计算姿态角

初始对准的目的是确定惯性导航系统各坐标轴相对于参考坐标系指向过程。在捷联导航系统中，姿态信息可以以方向余弦矩阵形式表示，角度对准的目的就是去欸的那个方向余弦矩阵，方向余弦矩阵定义了惯性导航敏感轴与参考坐标系各轴的关系。在静止的捷联惯性系统下，装在运载体上的加速度计和陀螺仪测量出克服重力所需要比力的各分量和地球角速度分量，分别用和表示。在精确已知当地的重力加速度g和经纬度信息后，可以由加速度计和陀螺仪输出数据，初始确定方向余弦矩阵。

静止状态下，在当地地理坐标系中重力矢量和地球角速度矢量分别为和，其中，，表示地球的自转角速度，Φ表示当地的纬度。和是确定方向余弦矩阵的其中两个分量，因此引出第三分量定义为，表示重力矢量和加速度矢量的叉乘积。

在当地坐标系和载体坐标系下这三个分量之间的关系满足以下：

其中方向余弦矩阵满足正交性，即，因此可得方向余弦矩阵为：

由方向余弦矩阵可以得出初始姿态角为：

航向角：

，()

俯仰角：

，()

横滚角：

，()

以下是通过matlab进行静态IMU初始粗对准计算姿态角：

static_rough_align.m

% 读取文件
file=fopen('Static_Data.txt');
data=textscan(file,'%n%n%n%n%n%n%n');
gy_X=cell2mat(data(1,2))/0.005;gy_Y=cell2mat(data(1,3))/0.005;gy_Z=cell2mat(data(1,4))/0.005;%陀螺仪数据
acc_X=cell2mat(data(1,5))/0.005;acc_Y=cell2mat(data(1,6))/0.005;acc_Z=cell2mat(data(1,7))/0.005;%加速度数据
%卡尔曼滤波
acc_filter=KalmanFilter(0,0,-9.8,acc_X,acc_Y,acc_Z);
gy_filter=KalmanFilter(1e-4,1e-4,3e-5,gy_X,gy_Y,gy_Z);%整个静态数据计算姿态角
acc_mean=mean(acc_filter);
gy_mean=mean(gy_filter);
lat=30.4042231878;
static_all_attitude=attitudeAngleCal(acc_mean,gy_mean,AngleAndRad(lat,1));% 每个历元计算姿态角for i=1:size(acc_X)epoch_acc=[acc_filter(i,1);acc_filter(i,2);acc_filter(i,3)];epoch_gy=[gy_filter(i,1);gy_filter(i,2);gy_filter(i,3)];per_epoch_attitude(i,:)=attitudeAngleCal(epoch_acc,epoch_gy,AngleAndRad(lat,1));end%每秒计算姿态角
acc_sum=[0,0,0];
gy_sum=[0,0,0];
tag=1;
for i=1:size(acc_X)acc_sum=acc_sum+acc_filter(i,:);gy_sum=gy_sum+gy_filter(i,:);if mod(i,200)==0per_sec_acc=acc_sum/200;acc_sum=[0,0,0];per_sec_gy=gy_sum/200;gy_sum=[0,0,0];%每秒输出姿态角信息per_sec_attitude(tag,:)=attitudeAngleCal(per_sec_acc',per_sec_gy',AngleAndRad(lat,1));tag=tag+1;end
endplot(per_sec_attitude(:,1));
xlabel('秒（s）','fontsize',18);
ylabel('Yaw角度（°）','fontsize',18);

attitudeAngleCal.m

function attitude = attitudeAngleCal( acc,gy ,lat)
%UNTITLED2 此处显示有关此函数的摘要
%   此处显示详细说明
g=9.7936174; %m/s2
we=7.292115e-5;%地球自转角速度
kappa_n=[0;0;-g];%n系下的重力矢量
we_n=[we*cos(lat);0;-we*sin(lat)];%n系下地球角速度矢量
v_n=cross(kappa_n,we_n);%n系下的叉乘积
v_b=cross(acc,gy);%b系下的叉乘积
matrix_b=[acc,gy,v_b];
matrix_n=[kappa_n,we_n,v_n];
%方向余弦矩阵(n-b)
direction_cosine_matrix=matrix_b/matrix_n;
% a=direction_cosine_matrix';
% b=inv(direction_cosine_matrix);
%方向余弦矩阵（b-n）
direction_cosine_matrix=direction_cosine_matrix';
yaw_angle=atan2(direction_cosine_matrix(2,1),direction_cosine_matrix(1,1));
pitch_angle=atan2(-direction_cosine_matrix(3,1),sqrt(direction_cosine_matrix(3,2)^2+direction_cosine_matrix(3,3)^2));
roll_angle=atan2(direction_cosine_matrix(3,2),direction_cosine_matrix(3,3));% c=a-b;attitude(1,1)=AngleAndRad(yaw_angle,2);
attitude(1,2)=AngleAndRad(pitch_angle,2);
attitude(1,3)=AngleAndRad(roll_angle,2);% attitude(2,1)=c(1,1);attitude(2,2)=c(1,2);attitude(2,3)=c(1,3);
% attitude(3,1)=c(2,1);attitude(3,2)=c(2,2);attitude(3,3)=c(2,3);
% attitude(4,1)=c(3,1);attitude(4,2)=c(3,2);attitude(4,3)=c(3,3);
end

KalmanFilter.m

function output = KalmanFilter(initx,inity,initz,acc_X,acc_Y,acc_Z)
%UNTITLED2 此处显示有关此函数的摘要
%   此处显示详细说明
s=size(acc_X);
% k-1时刻的方差
D_x=1;%初始化
D_y=1;
D_z=1;
% k-1时刻的状态向量
X=initx;%初始化
Y=inity;
Z=initz;
% 协方差
w=0.0001;
% 测量噪声
e=0.1;
output(1,1)=X;
output(1,2)=Y;
output(1,3)=Z;
n=2;
for i=2:sx=X;%一步预测d_x=D_x+w;%预测方差k_x=d_x/(d_x+e);%增益系数X=x+k_x*(acc_X(i)-x);D_x=(1-k_x)*d_x;output(n,1)=X;y=Y;%一步预测d_y=D_y+w;%预测方差k_y=d_y/(d_y+e);%增益系数Y=y+k_y*(acc_Y(i)-y);D_y=(1-k_y)*d_y;output(n,2)=Y;z=Z;%一步预测d_z=D_z+w;%预测方差k_z=d_z/(d_z+e);%增益系数Z=z+k_z*(acc_Z(i)-z);D_z=(1-k_z)*d_z;output(n,3)=Z;n=n+1;
endend

mean.m

function output = mean( acc )
%UNTITLED4 此处显示有关此函数的摘要
%   此处显示详细说明
[m,n]=size(acc);
sum_x=0;sum_y=0;sum_z=0;
for i=1:msum_x=sum_x+acc(i,1);sum_y=sum_y+acc(i,2);sum_z=sum_z+acc(i,3);
end
output(1,1)=sum_x/m;
output(2,1)=sum_y/m;
output(3,1)=sum_z/m;end

AngleAndRad.m

function output = AngleAndRad( input,tag )
%UNTITLED4 此处显示有关此函数的摘要
%   此处显示详细说明
% angle-->rad
if tag==1output=input*pi/180;
end
% rad-->angle
if tag==2output=input*180/pi;
endend

每秒平均输出的姿态角输出曲线图如下所示：

IMU静态初始粗对准计算姿态角相关推荐

imu初始对准matlab,IMU静态初始粗对准计算姿态角
初始对准的目的是确定惯性导航系统各坐标轴相对于参考坐标系指向过程.在捷联导航系统中,姿态信息可以以方向余弦矩阵形式表示,角度对准的目的就是去欸的那个方向余弦矩阵 ,方向余弦矩阵定义了惯性导航敏感轴与参 ...
imu初始对准的姿态角解算注意事项
众所周知,在b系下的一个向量要投影到n系,需要其在b系下的坐标,乘上方向余弦矩阵Cnb.根据此原理,通过分别找到两个坐标系下对应的的三个不平行矢量,即可求解出该矩阵.在武大牛小骥的ppt中介绍了姿态阵 ...
四元数解算姿态角解析
本文来自:链接一.概述无人机求解姿态角有多种算法,但由于各种算法的自身限制及计算机计算速度的限制,所以我们需要选择一个较佳的求解算法,下面我们先来看看几种求解姿态角的算法: 1. 欧拉角法: 欧拉 ...
关于无人机四元数解算姿态角解析你知道吗？
原文链接http://www.elecfans.com/d/705815.html 一.概述无人机求解姿态角有多种算法,但由于各种算法的自身限制及计算机计算速度的限制,所以我们需要选择一个较佳的求解 ...
IMU之磁力计校准地磁场计算磁航向
背景知识: 导航坐标系:东-北-天载体坐标系:右-前-上欧拉角定义:3-1-2旋转,(航向角-俯仰角-滚转角): 航向角北偏西为正,范围[-pi pi]: 俯仰角, 运载体抬头时角度定义为正,角度 ...
磁力计如何用来计算姿态（2）
上一篇磁力计如何用来计算姿态(1)介绍了磁强计算姿态角的原理. 本篇介绍, 在无人飞行器上常用的加速度计+磁强计的定姿方法. 静止状态抑或悬停状态:利用加速度计算横滚角(roll)和俯仰 ...
加速度计、磁力计求初始姿态角
1.几个基本概念. 地理坐标系:北东地.北为X轴.东为Y轴.地为Z轴. 机体坐标系:见下图. 姿态角:我的理解是,roll角是Zb在ZOY平面与Z轴的夹角.pitch角是Zb在ZOX平面与Z轴的夹角. ...
IMU内参标定以及初始化（9轴IMU，比6轴多三个姿态角信息）
IMU内参标定以及初始化(绕8字) 一.IMU内参标定 1.6轴(角速度+线加速度)信息初始化(标定噪声和bias) 2.三轴姿态信息初始化(绕8子) 二.IMU模块ROS配置注意事项: 因为三个方 ...
SINS/GNSS组合导航：捷联惯导静基座下初始对准（一）粗对准（Matlab）
SINS初始对准要测定系统的姿态变换矩阵,分为粗对准和精对准两个部分,粗短准阶段利用重力和地球自转量粗略计算姿态矩阵:在精对准阶段,不仅依靠前面的姿态矩阵测量值及重力和地球自转量,还要依据惯性器件的输 ...

IMU静态初始粗对准计算姿态角

IMU静态初始粗对准计算姿态角相关推荐

最新文章

热门文章