初始对准及组合导航技术

2023-05-16

一、对准是确定坐标系的过程

1、初始对准

比如说：初始对准就是确定 $C_{b}^{n}$ 的过程，通过重力加速度和地球自转角速度，其中，天向通过重力加速度确定，水平面的东北向通过地球自转角速度的分量确定。

2、坐标系对准

比如说，地面车辆中，IMU任意放置，如何确定 $C_{b}^{v}$ ；可以看出，对准就是确定坐标系的过程。

3、矢量定姿

<1>双矢量定姿

矢量 $V_{1}$ 和 $V_{2}$ 不共线，因此， $V_{1}$ 、 $V_{2}$ 和 $V_{1}\times V_{2}$ 不共面，其三个矢量构成的矩阵可逆。

由于上述矢量构建的姿态矩阵未必满足正交化要求，因此，预先对解算的矢量进行正交及单位化处理。

即上述的 $C_{b}^{r}$ 自然满足单位正交化条件。

<2>多矢量定姿

三维空间中存在 $m(m>2)$ 个不共面的矢量；

<3>解析粗对准方法及其误差分析

初始对准满足条件：载体静止，即无角运动和线运动，且地理位置已知；同时，重力矢量和自转角速度的分量准确知道：

根据惯导角速度测量关系和比力方程：

由于静态环境下， $W_{en}^{n}$ 和 $\left ( 2w_{ie}^{n}+w_{en}^{n} \right )\times v_{n}$ ，非常小，直接忽略，再考虑到陀螺仪测量误差 $\delta w_{ib}^{b}$ 和加速度计测量误差 $\delta f_{sf}^{b}$ ，上式分别改写为：

其中， $w_{nb}^{b}$ 为基座晃动干扰角速度， $v^{n}$ 的导数为基座晃动干扰加速度；

因此，简化为：

近似为：

一般情况下线运动干扰相对误差小于角运动，所以常常选择( $-g^{n}$ )作为主参考矢量，可得姿态阵估计：

将地球自转和重力加速度代入后得：

上式，可以近似为，如下：

又因为：

即：

一般情况下，陀螺仪相对地球自转的测量误差大于加速度计相对地球重力的测量误差。上式表明：水平水准角的对准误差主要取决于加速度计的等效水平测量误差 $\delta f_{sf,N}^{n}$ 和 $\delta f_{sf,E}^{n}$ ，而方位失准角的对准误差主要取决于陀螺的等效东向测量误差 $\delta w_{ib,E}^{n}$ 。

<4>间接粗对准方法

定义两个重要的惯性坐标系：

(1)初始时刻载体惯性系( $b_{0}$ )，与初始对准开始瞬时的载体坐标系( $b$ 系)重合，随后相对于惯性空间无转动；

(2)初始时刻导航惯性系( $n_{0}$ )，与初始对准开始瞬时的导航坐标系( $n$ 系，即地理坐标系)重合，随后相对于惯性空间无转动。

间接初始对准方法的关键是求解 $b_{0}$ 系与 $n_{0}$ 系的方位关系，即常值矩阵 $C_{b0}^{n0}$ 。

1) 首先，重力矢量在 $n_{0}$ 系的投影为：

同时：

由于 $w_{ie}^{n}$ 为常值，即 $n$ 系相对于 $n_{0}$ 系为定轴转动，由上式可解得：

因此，重力矢量在 $n_{0}$ 系的投影为：

2)其次，加速度计的比力输出在 $b_{0}$ 系投影为：

同时：

$w_{ib}^{b}$ 为陀螺仪的测量值，姿态阵初始值为单位阵，利用姿态更新算法求得实时姿态阵 $C_{b}^{b0}$ 。这里无需对 $w_{ib}^{b}$ 的大小做任何限制，因而间接对准算法具有很强的抗角运动干扰能力。

3)最后，通过 $C_{b0}^{n0}$ 建立重力加速度与加速度计比力量测之间的关系。

将上式，左右两边同时乘以 $C_{n}^{n0}$ ，得：

即：

上式，忽略干扰项，得：

二、捷联惯导精对准

1、静基座下对准

精对准是在粗对准的基础上，进行kalman滤波，所需条件是静态下/静基座下，根据惯导系统误差传播定律，从速度误差中反推失准角误差。

简化的姿态算法/微分方程：

简化的速度算法/微分方程：

对应的姿态、速度微分方程为：

其中， $\xi ^{n}$ 和 $\triangledown ^{n}$ 为等效 $n$ 系下陀螺仪、加速度计的随机常值零偏：

同时，静基座下速度误差方程近似如下：

即将微分方程展开为：

注：静基座下天向速度误差仅用于天向加速度计零偏估计，在分析失准角时，一般忽略天向通道(天向速度和天向加速度计零偏)的影响。

将陀螺仪、加速度计随机常值零偏扩充为状态：

经过观测性分析，可知： $\triangledown _{E}$ 、 $\triangledown _{N}$ 、 $\varepsilon _{E}$ 三个状态不可观测；

因此，求出(与失准角相关)可观测状态的极限精度：

即，实际应用中，将系统扩充为7维的Kalman滤波方程。

2、双位置对准法

双位置对准方法：就是在初始对准过程中故意将捷联惯导系统转动一个角位置，这相当于改变了惯导系统的姿态阵，使惯导误差方程从定常系统变成了时变系统，有利于提高惯性传感器误差的可观测性，从而提高水平姿态角和方位角的初始对准精度。

常用的双位置方法是将惯导绕其方位轴转动180°，且转动时机一般选择在精对准时间段的中点附近。如考虑了天向速度通道量测，使惯导绕其俯仰轴或横滚轴转动180°构造的双位置也是可行的。

其中，状态量为：

三、惯性和卫星组合导航

1、空间杆臂误差

杆臂误差主要是IMU和GNSS的速度、位置不一致！姿态角不予考虑。

假设惯组相对于地心 $O_{e}$ 的矢量为 $R$ ,卫星接收机天线相位中心相对于地心的矢量为 $r$ ，天线相位中心相对于惯组的矢量为 $\delta l$ ，三者之间的矢量关系满足：

杆臂 $\delta l$ 在惯组坐标系(b系)下为常矢量，上式两边相对地球坐标系( $e$ 系)求导，可得：

即： $V_{en(GNSS)}=V_{en(INS)}+w_{eb}\times \delta l$

将上式投影到惯组导航坐标系(以惯组导航坐标系为基准，杆臂由IMU指向GNSS，原点位与IMU中心)，得到：

注：由于 $w_{ie}$ 和 $w_{en}$ 的影响很小，还可作近似 $w_{eb}^{b}\approx w_{ib}^{b}$ 或 $w_{eb}^{b}\approx w_{nb}^{b}$ ；

<1>杆臂速度误差：惯导与卫星之间的速度误差；

<2>杆臂位置误差

若记：

$\delta l_{E}$ 、 $\delta l_{N}$ 、 $\delta l_{U}$ 分别为杆臂的东向、北向、天向投影分量，则惯导与卫星天线之间的地理位置偏差近似满足如下关系

其中， $R_{Mh}$ 、 $R_{Nh}$ 分别为由惯导(或卫导)位置计算的子午圈曲率半径和卯酉圈曲率半径。

其中：

2、时间不同步误差

组合导航计算机获得两类传感器导航信息的时刻(C)往往不是传感器实际信息的采集时刻(A和B)，从传感器信息采集到组合导航计算之间存在一定的时间滞后，比如卫星接收机采集到无线电信号后，需要先进行一系列的解算、再经过通信端口发送给组合计算机。惯性和卫星两类传感器的时间滞后一般并不相同，两者之间的相对滞后记为时间不同步误差 $\delta t$ 。

因此，在组合导航信息比对时，必须对时间不同步误差进行估计或补偿。

3、状态空间模型

状态量分别为：失准角误差、速度误差、位置误差、陀螺仪零偏、加速度计零偏、杆臂误差、时间不同步误差等17维。

观测量为：INS与GNSS的速度、位置误差；

具体矩阵展开为：

四、车载惯导与里程计组合导航

1、航位推算算法

航位推算(DR)利用姿态、航向和行驶里程信息来推算载车相对于起始点的相对位置。里程仪输出的信号一般是载车在一段时间内行驶的路程增量。

假设里程仪测量的是非转向轮的信号。非转向轮始终与车体正前方保持同向。载车在正常行驶时，假设车轮紧贴地面，无打滑、滑行和弹跳，里程仪测量的是沿车体正前方向上的速度大小，前进取正而倒车取负。

建立里程仪测量坐标系（车体坐标系），简记为 $m$ 系， $oy_{m}$ 轴在和载车车轮相接触的地平面内，并且指向车体的正前方， $oz_{m}$ 轴垂直于地平面向上为正， $ox_{m}$ 轴指向右方，里程仪坐标系是一个与车体固连的”右-前-上“右手直角坐标系。

即：

其中， $v_{D}$ 为里程仪测得的前向速度大小，右和天向速度为零，可视为载车正常行驶时的速度约束条件。

2、航位推算误差分析

<1>误差方程推导：

车体坐标系( $m$ )系与惯组坐标系( $b$ )系;；假设从m系至 $b$ 系存在小量的安装误差角，即绕车体横轴 $ox_{m}$ 、纵轴 $oy_{m}$ 及竖轴 $oz_{m}$ 分别存在俯仰角 $\alpha _{\theta }$ 、滚动偏角 $\alpha _{\gamma }$ 和方位偏角 $\alpha _{\Psi }$ ，记偏差角矢量，可得变换矩阵：

注意：推导姿态误差方程时，为从 $n$ 系到 $n^{'}$ 系，即以 $n$ 系为参考系，如下：

综上可知，航位推算中，是以车体坐标系( $m$ 系)为参考坐标系。

又考虑到里程计存在刻度系数误差 $\delta K_{D}$ ，其输出速度大小与理论速度大小之间关系为：

因此，在导航坐标系中里程仪的实际速度输出应为：

其中， $\Phi _{D}$ 为航位推算的姿态失准角。

将 $C_{b}^{n}$ 和 $v_{D}^{m}$ 代入上式中的第三、第四项中，可得：

上式表明，横滚角 $\alpha _{\gamma }$ 不影响里程仪的速度测量值，对上式简化得：

其中记：

综上，航位推算速度误差方程为：

同理，将航位推算位置方程，左右两边求微分：

得到，航位推算位置误差方程：

类似于捷联惯导姿态误差方程，重写，得到航位推算姿态误差方程如下：

<2>轨迹相似性原理

航位推算轨迹与真实轨迹相似；

3、惯性与里程仪组合

<1>里程仪转向与杆臂校正

主要是转向轮测量输出转换至车体坐标系的方法。

根据车辆转向原理，载车转弯时虚拟转向轮和虚拟非转向轮的行驶轨迹是同心圆弧，并且圆弧中心在后轮 $CD$ 的延长线上，容易看出 $o_{1}$ 和 $o_{2}$ 的行驶轨迹是不一样的， $o_{1}$ 所在的圆弧半径大，而 $o_{2}$ 的稍小些。可见，前轮里程仪测量的是虚拟转向轮 $o_{1}$ 与地面接触点处的速度，大小记为 $v_{D}$ （倒车为负），方向指向轨迹的切线方向。

由上图看出，里程仪输出在车体坐标系( $m$ 系)上投影为：

其中，转弯偏转角 $\varphi$ 右偏取正，左偏取负。在满足车辆转向原理前提下，偏转角 $\varphi$ 可通过载车航向角变化率和里程仪速度求得，即：

式中， $d_{L}$ 为前后轮轴之间的距离； $r_{\Psi }$ 为航向转弯半径(后虚拟轮 $o_{2}$ 处)，满足下式：

实际上，只要将 $r_{\Psi }$ 看成带符号的数值，上述计算过程就能够正确求得带符号的偏转角 $\varphi$ 。根据物理意义， $\varphi$ 的取值必定在主值范围内，即有 $-pi/2< \varphi < pi/2$ 。

即：

捷联惯组安装至车体上，惯组测量中心 $O_{b}$ 与里程计测量点 $O_{1}$ 往往不一致，如下图：

假设里程计杆臂为： $\delta l_{D}^{b}=[dx dy 0]$ ，由于载车行驶过程中水平姿态角一般不大，因而可以忽略高度方向的杆臂影响。

<2>惯导与航位推算组合模式

在捷联惯导更新解算中已经进行了姿态更新解算，航位推算算法中可以不必再实施姿态更新，而直
接使用惯导的姿态矩阵对里程仪测量进行坐标变换，获得导航系下的航位推算速度。这时惯导解算和航位推算使用共同的姿态阵，也就具有相同的失准角误差，将惯导误差和航位推算误差合并在一起，组成如下状态向量：

其中， $\varepsilon ^{b}$ 、 $\triangledown ^{b}$ 、 $k_{D}$ 均视为随机常值向量。

假设里程仪相对于惯组的杆臂已知并进行了补偿，以惯导解算位置与航位推算位置之差构造观测
量，可得：

即，惯导与航位推算组合状态空间模型为：

其中：

结论：

在惯导与卫星组合导航中，卫星接收机提供的是导航坐标系下的速度或位置，通过加减速运动可提高方位失准角的可观测性；而在惯导/里程仪组合导航中，里程仪提供的是载体系下的速度，需借助于惯导姿态矩阵进行速度分解，因而惯导和航位推算具有共同的方位失准角误差，加减速运动无法提高方位失准角的可观测性。此外，惯导和航位推算具有相同的初始位置误差，初始位置误差也是不可观的。载车在短时间内做加减速运动，因惯导速度误差在短时间内变化很小，里程仪和惯导之间的横向、纵向和天向速度偏差分别反映了方位安装误差角、里程仪刻度系数误差和俯仰安装误差，因而加减速有利于误差 $k_{D}$ 的辩识。

惯导与里程仪之间方位安装误差角 $\sigma _{\theta }$ 和 $\alpha _{\Psi }$ 的辨识，依赖于惯导方位失准角 $\Phi _{DU}$ 的估计精度，因而只有高精度惯导系统才合适将方位安装误差角作为未知状态估计，否则最好事先测量准确并补偿。至于俯仰安装误差角，容易受载车载重变化的影响，难以保持为常值且不容易估计准确，惯导和航位推算在高度方向上都是发散的，为了提供高精度的高度信息，还需要依靠气压高度计等其它辅助设备。

<3>惯导与航位推算增量组合模式

在航位推算算法中，如果行驶路况不好，里程仪容易出现打滑或滑行故障；或者在转弯过程中难以严格满足车辆转弯原理。在这些不良行驶状态下，因建模不准确会导致航位推算误差变大，因此，在惯导/航位推算组合模式中，航位推算的精度制约了组合导航系统精度的提高。一种有效的改进措施是，实时对载车行驶状态进行判断，只在状态良好时进行组合，而在不良状态下不作组合。但是，当从不良状态恢复至状态良好时，航位推算精度依然会受到影响，为此提出惯导/航位推算增量组合方法。在该方法中，当判断状态良好时，使用良好时间段内的航位推算增量与惯导组合，航位推算增量不受不良时间段的影响；而当判断状态不好时不组合，因而降低了里程仪运行误差的影响。

惯导与航位推算增量组合模型为：

其中：

在惯导与航位推算增量组合模式下，量测本质上为速度误差，惯导的位置误差是不可直接观测的，kalman滤波组合导航能够给出位置误差估计的本质在于：通过估计出惯导的速度误差再进行积分预测处惯导位置误差。

五、低成本姿态航向参考系统

1、简化的惯导算法及误差方程

IMU典型指标：陀螺仪零偏重复性0.1°/s，加速度计精度为5mg量级。

<1>简化的姿态算法

由于无法敏感地球自转信息，因此简化的姿态算法为：

其中：

$Q_{b(m)}^{n}$ 表示 $t_{m}$ 时刻的四元数， $Q_{b(m)}^{b(m-1)}$ 是从 $t_{m-1}$ 时刻到 $t_{m}$ 时刻的姿态四元数变化， $\Delta \Theta _{m}$ 是陀螺仪在时间段 $\left [ t_{m-1},t_{m} \right ]$ 内输出的角增量且模值为 $\Delta \Theta _{m}=\left | \Delta \Theta _{m} \right |$ 。如果低精度陀螺仪采用角速率的输出采样方式，只需简单乘以采样周期 $T_{s}=t_{m}-t_{m-1}$ ，即可近似为角增量。

<2>简化的比力方程

完整的比力方程为：

当载体地速 $v< 100m/s$ 时，上式第二项 $\left ( 2w_{ie}^{n} +w_{en}^{n}\right )\times v_{en}^{n}$ 的量级大约为1mg，小于传感器本身的误差，因此可以忽略地球自转及地球曲率的影响，将速度更新方程简化为：

其中：

$v_{m}^{n}$ 为 $t_{m}$ 时刻的惯导速度， $C_{b(m-1))}^{n}$ 为与四元数 $Q_{b(m-1)}^{n}$ 对应的姿态阵， $\Delta v_{m}$ 是加速度计在时间段 $\left [ t_{m-1},t_{m} \right ]$ 内输出的比力增量，实际中也可直接采用比力输出乘以采样间隔进行近似。

<3>简化导航算法对应的误差方程：

其中， $w_{E}$ 和 $w_{\bigtriangledown }$ 分别为陀螺仪角速率白噪声和加速度计比力白噪声；

$\varepsilon _{r}^{b}=\left [ \varepsilon _{rx}^{b},\varepsilon _{ry}^{b},\varepsilon _{rz}^{b} \right ]$ 为陀螺仪一阶马尔可夫过程误差；

$\bigtriangledown _{r}^{b}=\left [ \bigtriangledown _{rx}^{b},\bigtriangledown _{ry}^{b},\bigtriangledown _{rz}^{b} \right ]$ 为加速度计一阶马尔可夫过程误差；

如下：

$\gamma _{Gi}$ 和 $\gamma _{Ai}$ 是相关时间常数， $w_{rGi}$ 和 $w_{rAi}$ 是一阶马尔可夫过程激励白噪声。

对于低精度的惯性传感器，假设其时间相关误差模型为一阶马尔可夫过程是非常实用的：

与随机常值模型相比，一阶马尔可夫模型可在长时间组合滤波后避免滤波器过度收敛现象，过度收敛会导致滤波器抗干扰性能变差；
如果惯性传感器误差中确实存在较大随机常值成分，可通过滤波器的惯性传感器误差反馈校正，消除随机常值误差的影响；
与同时建立随机常值和一阶马尔可夫过程两种模型相比，仅使用后者有利于降低建模维数和滤波计算量。

2、地磁场测量及误差方程

地磁场：地磁南极位于地理北极附近，地磁北极位于地理南极附近；磁力线从磁北极指向磁南极(即：地理南极指向地理北极)，

通常规定顺着磁力线方向为磁场的正方向。

地磁两级的连线称为磁轴；从上图看出，磁轴与地球的自转轴之间约存在11.5°的倾斜，这使得地磁北向和地理北向一般不重合，该偏差角称为磁偏角。

磁偏角：是指地球上任一处的磁北方向和正北方向之间的夹角。当地磁北向实际偏东时，地磁偏角为正，反之为负。

磁倾角：地磁倾角的数值在−90°（上）和90°（下）之间。

地磁场在北半球向下倾，在地磁北极指向正下方，并随纬度下降而逐渐向上，至“地磁赤道”处完全与地表平行（0°）。
地磁场在南半球向上倾，并且随着纬度（南纬）上升而继续向上，直到地磁南极处指向正上方。

在某一小范围内，可将地磁场矢量 $H$ 当作常矢量看待，建立磁场坐标系（ $o_{m}x_{m}y_{m}z_{m}$ 系，简记为m系），如上图看出， $oy_{m}$ 轴指向磁场方向(磁力线方向)、 $ox_{m}$ 轴在水平面内，三轴构成右手直角坐标系。在上图右边，显然有地磁在 $m$ 系的投影为 $H^{^{m}}=\left [ 0 ,H ,0 \right ]$ ，其中磁场大小记为： $H=\left [ H \right ]$ , $H$ 与水平面 $o_{n}x_{n}y_{n}$ （上图中的"北东地"或者“东北天”）之间夹角 $\eta _{x}$ 称为磁倾角，地理北向至 $H$ 的水平面投影线之间的夹角 $\eta _{z}$ 为磁偏角。

当导航坐标系/地理坐标系为东北天时，地理坐标系 $o_{n}x_{n}y_{n}z_{n}$ 绕 $oz_{n}$ 轴转动 $-\eta _{Z}$ 角度，再绕 $ox_{n}$ 轴转动 $-\eta _{X}$ 角度，即得磁场坐标系 $o_{m}x_{m}y_{m}z_{m}$ ，因此 $o_{n}x_{n}y_{n}z_{n}$ 系至 $o_{m}x_{m}y_{m}z_{m}$ 系的变换矩阵为：

实际应用中，真实的磁偏角和磁倾角很难准确获取，假设实际给出的估计值与真实值的关系如下：

由粗略值得到的磁场坐标系为 $m^{'}$ 系：

其中，记磁偏差角矢量： $\delta \eta =\left [ \delta \eta _{x},0,\eta _{z} \right ]$

由三轴磁强计的三个测量敏感轴确定的直角坐标系记为 $b_{m}$ 系，一般惯导IMU与磁传感器之间会存在小量的安装偏差角，假设磁传感器坐标系绕其三轴分别转动角度： $\gamma _{x}$ 、 $\gamma _{y}$ 、 $\gamma _{z}$ ，可得IMU坐标系，即两传感器坐标系之间的失准角为： $\gamma =\left [ \gamma _{x},\gamma _{y},\gamma _{z} \right ]$ ，则：

三轴磁强计测量的是地磁场矢量在 $b_{m}$ 系下的投影坐标，记实际输出为，现定义磁测量误差为：

对上式展开可得：

简化：

量测方程：

如果磁偏角参数 $\eta _{x}$ 和 $\eta _{z}$ 准确已知，并且IMU坐标系与三轴磁力计坐标系之间相互重合，则量测方程简化如下：

3、低成本组合导航系统模型

低精度MEMS IMU/卫星/地磁组合导航中，状态量为：惯导失准角、惯导速度误差、惯导位置误差、陀螺仪相关漂移、加速度计相关漂移、安装偏差角(磁传感器坐标系与IMU坐标系之间的失准角)、磁倾角、磁偏角；共20维。

系统状态空间模型为：

其中：

4、低成本惯导的姿态初始化

对低精度惯导系统初始化时，在运载体静止状态下可以通过加速度计输出计算水平姿态角，但是，由于陀螺精度太低，不能完成方位自主对准，而需要采用地磁测量来实现，或者在运动条件下依靠卫星导航信息进行测量来实现。

<1>利用加速度计进行水平姿态对准

在静态环境下，运载体的先运动及其导数均为0，比力方程简化为：

简化如下：

实际应用时，为了减小加速度计测量噪声和外界晃动干扰加速度的影响，常使用一小段时间内的平均比力进行计算。上式移项，同时两边左乘 $\left ( C_{b}^{n} \right )^{T}$ ，得：

其中，矢量 $g^{n}$ 展开如下：（g为当地重力加速度大小）

展开为分量形式：

上式表明，姿态阵 $C_{b}^{n}$ 的第三行向量是地垂线在载体系下的投影。从上式只能求得 $C_{b}^{n}$ 中的最后一行元素 $C_{31}$ 、 $C_{32}$ 、 $C_{33}$ ，而 $C_{b}^{n}$ 的前两行所有元素都是不确定的，在满足右手姿态矩阵条件下可任意选取。

如下为一种构造方式：

记姿态矩阵 $C_{b}^{n}$ 的三个行向量分别为： $C_{1}$ 、 $C_{2}$ 、 $C_{3}$ ，既有：

1)构造 $C_{3}=\left [ C_{31} ,C_{32},C_{33} \right ]=(f_{sf}^{b})^{T}/\left | f_{sf}^{b} \right |$

2)考虑到 $C_{3}$ 是三维单位向量，其绝对值最大的元素不小于sqrt(3)/3≈0.5，如果 $\left | C_{31} \right |> 0.5$ ，则构造 $C_{2}^{'}=\left [ C_{32},-C_{31},0 \right ]$ ，否则构造 $C_{2}^{'}=\left [ 0,C_{33},-C_{32} \right ]$ ，显然 $C_{2}^{'}$ 为非零向量且与 $C_{3}$ 正交（即 $C_{2}^{'}C_{3}^{T}=0$ ），归一化后得 $C_{2}=C_{2}^{'}/\left | C_{2}^{'} \right |$ ；

3)构造 $C_{1}=C_{2}\times C_{3}$ ；

值得指出的是，经过上述处理后给出得姿态矩阵 $C_{b}^{n}$ ，若计算其三个欧拉角，则仅有水平姿态角（俯仰角和横滚角）是载体真是姿态的反映，而方位角没有实际物理意义，可记 $C_{b}^{h}=C_{b}^{n}$ ， $h$ 系称为当地水平坐标系，其隐含方位角无效或还未确定。

<2>利用地磁测量进行方位对准

在初始对准过程中，如果地磁测量信息可用，通常可忽略小的磁偏角影响，直接以磁方位近似代替
地理方位进行方位对准。当然，若已知当地磁偏角参数，为提高方位精度应作适当的补偿。

假设经过加速度计水平对准之后，获得水平姿态矩阵 $C_{b}^{h}$ ，真实姿态阵 $C_{b}^{n}$ ，它可分解为：

其中， $C_{h}^{n}$ 是与方位有关的校正矩阵，可展开为：

--%% --%% --%% --%% --%% --%% --%% --%% --%% --%% --%% --%% --%% --%% --%% --%

下面将会看到，并不需要知道 $C_{h}^{n}$ 中符号 $\varphi$ 的含义，只需求得其正余弦值和余弦值即可；

根据地磁场测量关系，有

其中， $h^{n}=C_{m}^{n}H^{m}/H=\left [ h_{E}^{n},h_{N}^{n},h_{U}^{n} \right ]^{T}$ 为真实地磁矢量方向；

归一化地磁测量矢量在水平坐标系地投影；

将上述两个等式联立，展开并只取 $x$ 和 $y$ 分量，可得：

由上式可解得 $\varphi$ 的正弦值和余弦值：

实际上，求取姿态角和航向角，是利用了重力和地磁两个矢量在不同坐标系(n系和b系)之间的测量转换关系，如下：

<3>利用卫星导航进行方位对准

完成水平对准之后，如果卫星导航信号可用，对于固定翼飞行器或载车，其行驶速度方向一般沿载体纵轴方向（正前方），根据行驶轨迹的航迹角或卫星测量速度矢量，容易求得载体纵轴相对于地理北向的方位角，再结合 $C_{b}^{h}$ 计算获得的俯仰角和横滚角，即可求得完整的初始姿态阵，完成初始化。

对于多旋翼飞行器，比如四旋翼无人机，其飞行速度方向具有任意性，可沿能载体任意方向飞行，
不能再采用类似固定翼的方位确定方法，但可以通过在水平方向上做短时直线加速度机动来实现，基本原理叙述如下。

对比力方程作如下近似：

其中， $f_{sf}^{h}=C_{b}^{h}f_{sf}^{b}$ 为水平坐标系下的比力投影， $C_{b}^{h}$ 和 $C_{h}^{n}$ 的含义见上，且假设 $C_{b}^{h}$ 已知。

加速度机动意味着不为零，它可通过两个时刻的卫星导航速度平均变化量求得，近似计算为：

相应地，对应于载体在时间段 $\left [ t_{k-1},t_{k} \right ]$ 内的水平坐标系平均比力投影。

假设短时间 $\left [ t_{k-1},t_{k} \right ]$ 内载体方位近似保持为常值， $C_{h}^{n}$ 亦为常值，仅取x和y轴水平分量，可得：

其中：

求解可得：

上式的分母表达式显示，较大的水平加速度有利于可靠地求得 $sin\varphi$ 和 $cos\varphi$ 。不难看出，水平加速度机动的实质是在水平方向上提供了一个用于确定方位的观测量，这与地磁场的水平分量观测量作用完全一样。

5、捷联式地平仪的工作原理

应用场景：静止、飞行器悬停、匀速或低加速度动态时；

惯导比力方程和误差方程近似如下：

分析：低精度的MEMS惯导在类静止状态下，如果检测到水平加速度(即：加速度计的输出值)，一般认为是水平失准角引起的。因此，可以利用水平加速度修正失准角。

在速度平稳情况下，加速度与加速度误差含义一样，令上述两式相等，得：

低机动时，上式等号右边的 $f_{sf}^{n}$ 可近似为 $f_{sf}^{n}\approx \left [ 0,0,g \right ]^{T}$ ，将上式写成分量形式，有：

其中：

上式表明，水平加速度只提供水平失准角修正信息，而不能用于计算方位失准角 $\phi _{U}$ ，不妨假设方位修正量 $\phi _{U}=0$ ，因而有：

其中， $\left | f_{sf}^{n} \right |\approx g$ 为低机动时的模值， $e_{3}=\left [ 0,0,1 \right ]^{T}$ 为 $z$ 轴单位矢量。

将上式两边同时左乘 $C_{n}^{b}$ ，并记 $\Phi ^{b}=C_{n}^{b}\Phi$ ，可得：

其中， $C_{3}$ 为姿态阵 $C_{b}^{n}$ 的第三行向量。由上式可见，两单位矢量 $f_{sf}^{b}/\left | f_{sf}^{b} \right |$ 和 $C_{3}^{T}$ 之间的夹角即为水平失准角在 $b$ 系的投影 $\Phi ^{b}$ 。（揭露了本质）

将失准角计算值 $\Phi ^{b}$ 与陀螺仪角增量输出相结合，设计带加权失准角修正的四元数姿态更新算法，如下：

其中：

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)