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

1 初始对准

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

1.1 理论推导

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

  载体静止时，其线运动及其导数均为零，比力方程：
$${\dot{v}}_{en}^n={\mathbf{C}}_b^n{f}_{\mathrm{sf}}^b-(2{\mathbf{\omega }}_{ie}^n+{\mathbf{\omega }}_{en}^n)\times {\mathbf{v}}_{en}^n+{\mathbf{g}}^n$$
  可简化为：
$$0={\mathbf{C}}_b^n{f}_{\mathrm{sf}}^b+{\mathbf{g}}^n$$
  将上式移项化简后可得：
$$f_{\mathrm{sf}}^b=-{\left({\mathbf{C}}_b^n\right)}^{\mathrm{T}}{\mathbf{g}}^n$$
  其中，
$${\mathbf{C}}_b^n=\left[\begin{array}{ccc}{c}_{\psi }{c}_{\gamma }-s_{\psi }{s}_{\theta }{s}_{\gamma }& -s_{\psi }{c}_{\theta }& c_{\psi }{s}_{\gamma }+s_{\psi }{s}_{\theta }{s}_{\gamma }\\ s_{\psi }{c}_{\gamma }+c_{\psi }{s}_{\theta }{s}_{\gamma }& c_{\psi }{c}_{\theta }& s_{\psi }{s}_{\gamma }-c_{\psi }{s}_{\theta }{c}_{\gamma }\\ -c_{\theta }{s}_{\gamma }& s_{\theta }& c_{\theta }{c}_{\gamma }\end{array}\right]$$

$$g^n=[\begin{array}{ccc}0& 0& -g\end{array}{]}^{\mathrm{T}}$$
  则：
$$f^b=\left[\begin{array}{ccc}{c}_{\psi }{c}_{\gamma }-s_{\psi }{s}_{\theta }{s}_{\gamma }& -s_{\psi }{c}_{\theta }& c_{\psi }{s}_{\gamma }+s_{\psi }{s}_{\theta }{s}_{\gamma }\\ s_{\psi }{c}_{\gamma }+c_{\psi }{s}_{\theta }{s}_{\gamma }& c_{\psi }{c}_{\theta }& s_{\psi }{s}_{\gamma }-c_{\psi }{s}_{\theta }{c}_{\gamma }\\ -c_{\theta }{s}_{\gamma }& s_{\theta }& c_{\theta }{c}_{\gamma }\end{array}\right]\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]$$

  化简后，可得：
$$\left[\begin{array}{c}{f}_x\\ f_y\\ f_z\end{array}\right]=\left[\begin{array}{c}-g{c}_{\theta }{s}_{\gamma }\\ g{s}_{\theta }\\ g{c}_{\theta }{c}_{\gamma }\end{array}\right]$$

  上式fx、fy、fz分别是加速度计x、y、z轴测量的比力，利用fx、fy、fz可以反解出俯仰角和横滚角，即：
$$\theta =\arctan (f_y/\sqrt{f_x^2+f_z^2})$$
  由于俯仰角取值范围为[-90°，90°]，与反正弦的主值区间一致，可以直接求取真值。
$$\gamma =\arctan (-f_x/f_z)$$

  由于横滚角的取值范围为[-180°，180°]，而反正弦的主值范围为[-90°，90°],需要对算出的横滚角取真值。

$x^b$	$z^b$	$\gamma$
+	-	$-180^{\circ }+|\gamma |$
-	+	$|\gamma |$
-	-	$180^{\circ }-|\gamma |$
+	+	$-|\gamma |$

  在实际应用中，为了减少加速度计测量噪声和外界晃动干扰加速度的影响，常常使用一小段时间内的平均比力进行计算初始俯仰角与横滚角：
$${\theta }_0=\arctan (\overline{f_y}/\sqrt{{\overline{f_x}}^2+{\overline{f_z}}^2})$$
$${\gamma }_0=\arctan (-\overline{f_x}/\overline{f_z})$$
  上式${\bar{f}}_x$、${\bar{f}}_y$、${\bar{f}}_z$分别表示在初始静止时段的三轴比力均值。
  加速度计只能计算初始俯仰角和初始横滚角，我们需要使用磁力计或其他方式计算得到初始偏航角，本文介绍利用两点的GPS坐标计算偏航角。

  由两点的gps经度差可以计算东向的位置差：
$$dx=d\lambda (R_N+h)\cos \phi$$

  由两点的gps纬度差可以计算北向的位置差：
$$dy=d\phi (R_M+h)$$
  可得偏航角为：
$$\psi =-arctan2(dx,dy)$$

1.2 代码实现

1.2.1 计算俯仰角和横滚角

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);%计算横滚角
    roll = abs(roll);
    
    if fx>=0 && fz <=0
        roll = -pi + roll;
    elseif fx<=0 && fz>=0
        roll = roll;
    elseif fx<=0 && fz<=0
        roll = pi - roll;
    elseif fx>=0 && fz>=0
        roll = -roll;
    end
    
    pitch = rad2deg(pitch);
    roll  = rad2deg(roll);
end 

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

1.2.2 计算偏航角

function [yaw0] = att_yaw0(pos1,pos2)
%*************************************************************%
  % @brief  惯导初始对准(获取初始偏航角)
  % @param  pos1：坐标点1
  % @param  pos2：坐标点2
  % @retval yaw0：偏航角
%*************************************************************%
    [pos1_RMh,pos1_clRNh] = RMRN(pos1);

    dlat = pos2(1)-pos1(1);
    dlon = pos2(2)-pos1(2);
    
    yaw_x = dlon*pos1_clRNh;
    yaw_y = dlat*pos1_RMh;
    
    yaw0 = -atan2(yaw_x,yaw_y);
    yaw0 = rad2deg(yaw0);
end

1.3 实验验证

• 生成一个初始俯仰角、横滚角和偏航角分别为60°、120°和30°的轨迹。
  

• 调用函数imu_att0(imu,ts)
  
  可以看到俯仰角和横滚角与设定的一致。

• 调用函数att_yaw0(pos1,pos2)
  
  可以看到偏航角与设定的一致。

2 初始校准

2.1 理论推导

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

  陀螺仪常值零偏计算公式为：
$${\mathfrak{m}}_{{}_{GYRO}}=\overline{D_{{}_{GYRO}}}-0$$
  其中，$m_{{}_{GYRO}}$表示陀螺仪的常值零偏，$\overline{D_{{}_{GYRO}}}$表示陀螺仪静止情况下输出的平均值。

  加速度计常值零偏计算公式为：
$$m_{{}_{ACC}}=\overline{D_{{}_{ACC}}}-C_{n0}^b\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]$$
  其中，$m_{{}_{ACC}}$表示加速度计的常值零偏，$\overline{D_{{}_{ACC}}}$表示加速度计静止情况下输出的均值。

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)

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