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)==0
        per_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
end

plot(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:s
    x=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;
end

end

mean.m

function output = mean( acc )
%UNTITLED4 此处显示有关此函数的摘要
%   此处显示详细说明
[m,n]=size(acc);
sum_x=0;sum_y=0;sum_z=0;
for i=1:m
    sum_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==1
    output=input*pi/180;
end
% rad-->angle
if tag==2
    output=input*180/pi;
end

end

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