惯性导航的初始对准和初始校准

1 初始对准

  捷联惯导系统在载体导航之前需要先进行初始对准，以确定载体初始的姿态、速度和位置信息。只有获取了载体的姿态、速度、位置的初始值，并以初始值为基础通过力学编排才能计算出未来各时刻的姿态、速度和位置信息。因此载体所有的定位信息都是在初始状态下不断力学编排得到的。
  惯导系统初始对准就是用来确定参考坐标系和导航坐标系的一个过程。常见的初始对准技术有静基座粗对准、静基座静对准和动基座对准等算法。由于静基座粗对准算法简单、容易实现，本文将介绍静基座粗对准的算法。

1.1 理论推导

  载体静止时，可以通过加速度计输出来计算水平姿态角，但是由于陀螺仪精度太低，不能完成偏航角的自主对准，需要使用外部辅助（磁力计、gps）来实现。

  载体静止时，其线运动及其导数均为零，比力方程：

  可简化为：

  将式(2)移项化简后可得：

  其中，


  则：

  化简后，可得：

  式（7）中fx、fy、fz分别是加速度计x、y、z轴测量的比力，利用fx、fy、fz可以反解出俯仰角和横滚角，即：


  在实际应用中，为了减少加速度计测量噪声和外界晃动干扰加速度的影响，常常使用一小段时间内的平均比力进行计算初始俯仰角与横滚角：


  式(10)、式(11)中fx、fy、fz分别表示在初始静止时段的三轴比力均值。
  加速度计只能计算初始俯仰角和初始横滚角，我们需要使用磁力计或其他方式计算得到初始偏航角，当我们获取完姿态角后，我们可以使用公式(4)计算初始方向余弦矩阵。初始的速度我们可设置为0，初始的位置可由外部测量得到，至此初始对准完成。

1.2 代码实现

function [pitch,roll] = imu_att0(imu,dt)
%*************************************************************%
  % @brief  惯导初始对准
  % @param  imu：静止时刻imu数据
  % @param  dt：惯导采样间隔
  % @retval pitch：俯仰角
  % @retval roll：横滚角
%*************************************************************%
    len = length(imu);%获取初始对准imu数据长度

    gcc_sum1 = sum(imu(:,1)); gcc_sum2 = sum(imu(:,2)); gcc_sum3 = sum(imu(:,3));%计算各轴速度增量的和
    gcc1 = gcc_sum1/len; gcc2 = gcc_sum2/len; gcc3 = gcc_sum3/len;%取平均
    fx = gcc1/dt; fy = gcc2/dt; fz = gcc3/dt;%由速度增量获取比力

    pitch = atan(fy/sqrt(fx^2 + fz^2));%计算俯仰角
    roll = atan(-fx/fz);%计算偏航角
end

1. 计算传入IMU的数据长度，为后续计算平均比力做准备。
2. 计算平均比力。
3. 利用公式(10)、公式(11)计算俯仰角和偏航角。

1.3 实验验证

• 生成一个初始俯仰角、横滚角和偏航角分别为45°、10°和0°的轨迹。
  
  
  
• 调用函数imu_att0(imu,ts)
  

可以看到俯仰角和横滚角与生成的一致。

2 初始校准

2.1 理论推导

  在惯性导航力学编排中，由于传感器常值零偏的存在，惯导系统的姿态、速度、位置误差也会不断增大。常值零偏即传感器生产出来就一直固定不变的零偏值，对于高精度惯性器件来说，该误差在出厂时就被补偿掉了，因此不会标注该参数。但是对于低精度传感器，则不可能做逐个的标定与补偿，所以我们需要消除这部分误差。本文采用的方法为在初始启动过程中利用几秒钟的静态数据求平均即可扣掉大部分零偏误差，再用卡尔曼滤波估计残余零偏的方式抑制零偏误差对结果的影响。
  在静止情况下，精度较高的陀螺仪可以感受到地球自转的角速率（7.29210×10^-5rad/s）,但是陀螺仪的常值零偏往往在10 ^-3rad/s的量级以上，因此在消除陀螺仪的常值零偏时，可以不考虑地球自转角速率。
  加速度计在静止时可以测量到重力加速度在各轴的投影，所以在零输入情况下，加速度计各轴输出的比力包含常值零偏、白噪声以及重力加速度分量，由于重力加速度分量取值较大，不可忽略。因此在计算常值零偏时需要先利用方向余弦矩阵计算重力加速度在各轴的投影。

  陀螺仪常值零偏计算公式为：

  加速度计常值零偏计算公式为：

2.2 代码实现

function result = imu_ConstantBias(imu,Cnb0,g,dt)
%*************************************************************%
  % @brief  惯导初始校准
  % @param  imu：静止时刻imu数据
  % @param  Cnb0：当前时刻方向余弦矩阵
  % @param  g：当地重力加速度值
  % @param  dt：惯导采样间隔
  % @retval result：陀螺仪和加速度计常值零偏
%*************************************************************%
    glvs;   %设置全局变量
    len = length(imu);%%获取初始校准imu数据长度

    acc_sum1 = sum(imu(:,1)); acc_sum2 = sum(imu(:,2)); acc_sum3 = sum(imu(:,3));%计算各轴角度增量的和
    acc1 = acc_sum1/len; acc2 = acc_sum2/len; acc3 = acc_sum3/len;%取平均
    eb1 = acc1/dt; eb2 = acc2/dt; eb3 = acc3/dt;%由角度增量获取角速率，该角速率即为常值零偏

    imu(:,4:6) = imu(:,4:6) - (Cnb0'*[0;0;g])'*dt;%去除重力加速度分量
    gcc_sum1 = sum(imu(:,4)); gcc_sum2 = sum(imu(:,5)); gcc_sum3 = sum(imu(:,6));%计算各轴速度增量的和
    gcc1 = gcc_sum1/len; gcc2 = gcc_sum2/len; gcc3 = gcc_sum3/len;%取平均
    db1 = gcc1/dt; db2 = gcc2/dt; db3 = gcc3/dt;%由速度增量获取比力，该比力即为常值零偏

    result = [eb1;eb2;eb3;db1;db2;db3];
end

1. 定义全局变量，计算静止状态下IMU的数据长度。
2. 利用陀螺仪零偏公式求出陀螺仪零偏。
3. 利用加速度计零偏公式求出加速度计零偏。

2.3 实验验证

• 使用如下函数设置IMU噪声：

imuerr = imuerrset(420, 1000, 0.001, 5);%imu误差设置

陀螺仪常值零偏：

加速度计常值零偏：

• 调用函数imu_ConstantBias(imu,Cnb0,g,dt)

利用估计出来的常值零偏可扣掉大部分零偏误差，后续可以用卡尔曼滤波估计残余零偏的方式抑制零偏误差对结果的影响。