航姿系统矢量场传感器校正的双内积法*

李 翔,宋百麒,王勇军,2

(1.桂林电子科技大学电子工程与自动化学院,桂林 541004;2.桂林航天工业学院无人机遥测重点实验室,桂林 541004)

航姿系统广泛用于无人飞行器、车辆及船只的导航定位,可提供航向角、俯仰角、横滚角等数据。目前,航姿系统中普遍采用三轴磁强计和三轴加速度计,这两种传感器均属于矢量场传感器,分别通过测量地磁场和重力矢量获取姿态信息,是简便可靠的无源导航手段。但磁强计和加速度计易受多种因素干扰,如零偏、灵敏度(标度因数)、非正交误差、软硬磁干扰等。每当使用环境发生变化时,往往需要重新进行校正[1-2]。

三轴矢量场传感器的校正方法从原理上可分为与姿态相关的校正方法和与姿态无关的校正方法。由于磁强计和加速度计的输出直接取决于其载体的三维姿态,因此可借助精确的航姿基准实现二者的校正,此即与姿态相关的校正方法,又称多位置法[3]。但在实际应用中,为减少对外部设备的依赖并简化操作步骤,普遍采用与姿态无关的校正方法,又称椭球拟合法[4-8]。此类方法假定同一地点的地磁场强度和重力加速度在短时间内保持为常数,其主要优点是校正过程中仅用到传感器自身读数,无需外部设备或基准,便于实施现场校正;其缺点是会导致数学上的欠定问题[9-10]以及校正后的磁强计存在非对准误差[11-18],需要作进一步校正。

针对上述问题,本文提出一种基于多内积的校正方法。该方法既利用地磁场和重力矢量自身的模值不变性,又利用传感器转动时地磁场和重力矢量在旋转轴上的投影不变性,可实现三轴磁强计和加速度计的完全校正。在航姿模块上对该方法及其他常用校正方法进行了对比实验,验证了该方法的有效性。

1 校正原理

1.1 误差模型

三轴加速度计和磁强计的误差模型如式(1)所示,其中:u=(u1u2u3)T为载体坐标系中的被测矢量(对于磁强计为地磁场矢量,对于加速度计为重力矢量),v=(v1v2v3)T为传感器输出。

v=Ku+b+ε

(1)

式中,矢量ε、b和矩阵K描述传感器的各种误差来源:①零偏:传感器各轴的零偏依次由矢量b的3个分量表示;②灵敏度(标度系数):传感器各轴的灵敏度分别由矩阵K的3个对角元表示;③非正交及非对准误差:由矩阵K的6个非对角元表示;④磁强计受到来自载体和外界的磁干扰:分为硬磁干扰和软磁干扰两部分,分别包含于b和K中。⑤传感器噪声及高阶误差项:包含于ε中。

对于目前广泛应用的低成本磁强计和加速度计,校正通常主要针对常值误差及线性误差,即式(1)中的K与b,而视ε为小量并忽略之。记L=K-1,则误差补偿可由式(2)实现。

u=L(v-b)

(2)

1.2 椭球拟合法

航姿系统及其载体作原地转动时,载体坐标系中地磁场和重力矢量的方向发生变化,但矢量模值保持不变,这一性质可由式(3)描述。

(3)

式中:u0表示被测矢量u的模(即地磁场强度或重力加速度)。将式(2)代入式(3)即可得到关于L与b的方程,进而可根据传感器原始数据利用各种非线性拟合或优化算法求解L与b. 由于式(3)表示的是一个标量,故基于式(3)的校正方法也称为标量校正法(scalar calibration)。另一方面,采用式(2)进行误差补偿的前提是式(1)中的误差矩阵K可逆(即L=K-1存在)。此时,将式(2)代入式(3)所得关于u的二次型必为正定二次型,其对应的几何图形为一椭球面。因此,凡基于式(3)的校正方法,不论其是否显式地求解椭球的中心与主轴等参数,数学上均等价于对椭球面进行拟合,故此类方法又统称为椭球拟合法。

椭球拟合法的几何意义是将关于矢量v的椭球面变换为关于矢量u的球面。由该变换的数学特性可知,椭球拟合法的优点是:能实现矢量模值误差的最小化,以及传感器各轴间的正交化。而其主要缺点是:式(3)的二次型最多只能提供9个独立参数,而式(1)及式(2)的误差模型含12个独立参数,从而造成数学上的欠定问题。其根源在于式(3)完全没有涉及矢量的指向,因而缺失的3个参数对应的正是3维空间中的3个转动自由度。因此,椭球拟合法无法保证校正后传感器的三轴与载体坐标系的三轴对应重合,即存在非对准误差。这一非对准误差可用一个正交矩阵R表示,即从载体坐标系到传感器坐标系的三维旋转变换。

1.3 点积不变法

航姿系统及其载体原地转动时,重力矢量与地磁场矢量夹角不变,故二者的点积是一个常数。记重力矢量为g=(g1g2g3)T,地磁场矢量为h=(h1h2h3)T,二者夹角为ζ,则上述原理可由式(4)描述。

gTh=g0h0cosζ

(4)

式中:g0和h0分别为本地重力加速度及地磁场强度。若加速度计已经由椭球拟合法校正,则利用加速度计读数及式(4)可对三轴磁强计进行校正,此即三轴磁强计校正的点积不变法[11-12]。

相对而言,加速度计较易实现与载体坐标系的对准,这是由于其读数直接与倾角(俯仰角与横滚角)有关。又由于加速度计不存在软硬磁干扰等问题,受环境变化影响相对较小,因此若借助校正良好的加速度计作为基准,点积不变法可实现磁强计的完全校正[10-13]。

点积不变法可以直接将式(2)代入式(4)求解L与b。但也可先由椭球拟合法得到L与b,再由点积不变法求解非对准误差(即矩阵R),此时式(4)变为式(5)。此即磁强计的二步校正法[13-15]。

gTRL(v-b)=g0h0cosζ

(5)

注意矩阵R应当是一个表示三维旋转的正交矩阵,但若直接采用式(5)而不对R作任何约束,则易导致R失去正交性,而使校正效果变差[12-13]。

1.4 绕轴旋转法

当传感器原地旋转时,地磁场或重力矢量在旋转轴上的投影也可构造出类似式(4)的点积不变量。设沿旋转轴的单位矢量为w,其与被测矢量u的夹角为γ,则有

wTu=u0cosγ

(6)

如以载体坐标系的坐标轴作为旋转轴,或者旋转轴在载体坐标系下的指向为已知,则式(6)可用于求解传感器非对准误差,且不依赖于加速度计。此方法与点积不变法原理相同,但通常不单独使用,而是与椭球拟合法相结合,构成二步法或多步法[16-18]。

2 双内积法

2.1 基于双内积的校正原理

若同时利用式(3)所述的模值不变性及式(6)所述的点积不变性,便可将椭球拟合法、点积不变法和绕轴旋转法各自的优点相结合,实现三轴矢量场传感器的完全校正。由于式(3)及式(6)均属于向量的内积,故此法可称为“双内积法”。

仍采用式(2)中的误差模型,并假定共有r组测量数据,包括传感器测量值{v1,v2,…,vr}以及沿旋转轴的单位矢量{w1,w2,…,wr}。引入式(7)所示的代价函数e:

(7)

这样,三轴矢量场传感器的校正便转化为求解令‖e‖2=eTe最小化的L与b,二者全部元素共计12个参数。

理论上,式(7)中的u0应等于本地的重力加速度(对于加速度计)或者地磁场强度(对于磁强计),但其具体数值并不影响姿态解算,故在精确值未知时亦可用近似值代替。

另一方面,重力矢量或地磁场矢量与旋转轴的夹角γ如不能精确确定,则需将λ=cosγ也列为待估计参数。如数据采集过程中有n个不同的旋转轴取向,则待估计参数将有(12+n)个。

2.2 非线性优化算法实现

根据式(7)令‖e‖2=eTe最小化从而求解L与b,是一个非线性优化问题,可采用最速下降法、共轭梯度法、高斯-牛顿法、Levenberg-Marquardt法(简称L-M法)等算法求解。首先将待估计参数排列成向量形式(此处假定有且仅有一个待估计的λ=cosγ):

x=(l11l33b1b2b3λ)T

(8)

式中,lij表示矩阵L第i行第j列的元素,bi表示b的第i个元素。以采用高斯-牛顿法为例,求解过程如下:

①初始化:令迭代次数k=0,Lk=I3×3(3阶单位阵),bk=03×1(3维零向量),λk=1。

②按式(8)构造待估计参数向量xk。

③根据式(7)计算代价函数ek,注意ek为2r维列向量。

④计算雅可比矩阵Jk=∂ek/∂xk,参见式(9)～式(11),其中:ek,s为ek的第s个元素,αs,i表示矢量Lk(vs-bk)的第i个元素,βs,j表示矢量(vs-bk)的第j个元素,ws,i为ws的第i个元素,vs和ws分别是第s组磁强计读数及对应的旋转轴。

(9)

(10)

(11)

⑤根据式(12)计算xk+1,进而由xk+1得到Lk+1,bk+1和λk+1。

(12)

⑥根据式(7)计算ek+1.

⑦若‖ek+1‖2小于给定阈值,或迭代次数k达到上限,则结束;否则k=k+1,转第4步。

3 实验验证

3.1 实验条件

在基于MPU9250的航姿模块上验证本文所述校正方法,并与其他方法进行对比。MPU9250内含三轴磁强计、三轴加速度计和三轴陀螺仪。该航姿模块如图1所示,其坐标系是依外壳的几何外形来定义,已在图1中标明。

图1 实验所用航姿模块及其坐标系定义

采用角度分辨率为0.1°的三自由度手动转台提供三维姿态基准,用于检验校正前后航姿模块的角度误差。

实验中,将航姿模块的外壳表面紧贴于竖直墙面上,通过绕轴旋转的方式采集传感器原始数据,具体步骤为:①外壳侧面靠墙,使y轴水平指北,并绕该轴旋转一圈;②外壳底面靠墙,使z轴水平指北,并绕该轴旋转一圈;③外壳顶面靠墙,使z轴水平指南,并绕该轴旋转一圈。

旋转过程需保持平稳缓慢,以避免干扰加速度计读数。注意以上3个步骤中,沿转轴的单位矢量w在载体坐标系中依次表示为(0 1 0)T,(0 0 1)T,(0 0 -1)T。

3.2 加速度计校正实验

图2为按3.1节所述步骤采集的加速度计数据,分别采用以下3种方法对加速度计进行校正:①只用椭球拟合法校正;②先用椭球拟合法校正,再用绕轴旋转法修正非对准误差;③采用本文所述方法即双内积法。

图2 加速度计采样数据(单位:m/s2)

图3 校正前后俯仰角误差

实验中,采用上述方法先后对5个航姿模块中的加速度计进行了校正,并利用三轴转台检验了校正前后的俯仰角和横滚角误差(包括平均误差和均方根误差),结果分别如图3和图4所示。实验结果中,各姿态角的均方根误差反映误差的波动幅度,各姿态角的平均误差则主要来自加速度计各轴的非对准误差。由图3和图4可见,双内积法可使这两项指标均得到明显改善,且效果优于其他校正方法。

图4 校正前后横滚角误差

3.3 磁强计校正实验

图5为按3.1节所述步骤采集的磁强计数据。先对各航姿模块的加速度计进行校正,再分别采用如下4种方法对其磁强计进行校正:①椭球拟合法;②点积不变法;③先用椭球拟合法校正,再用绕轴旋转法修正非对准误差;④双内积法。

图5 磁强计采样数据(单位:μT)

利用三轴转台检验校正前后的航向角误差(包括平均误差及均方根误差),结果如图6所示。

图6 校正前后航向角误差

由图6可见,除椭球拟合法校正后误差较大外,其余校正方法效果较为接近,但仍以双内积法精度为最高。

3.4 讨论

在对三轴加速度计和磁强计进行误差校正和补偿时,采样数据的充分性是影响校正效果的重要因素。在实际应用场合,传感器数据采集过程可能会受到航姿系统安装条件及载体运动约束,导致采样数据集中分布在较窄的范围内,这将使校正算法的误差增大甚至不能求解。为模拟此类情况,设计了另一个磁强计校正实验,其数据采集步骤如下:①航姿模块置于水平面上并旋转一圈;②航姿模块置于向北倾斜约15°的平面上,并旋转一圈;③航姿模块置于向南倾斜约15°的平面上,并旋转一圈。

图7 磁强计采样数据(单位:μT)

以上三个步骤中,旋转轴均为图1中的z轴,用以模拟载体只能在水平面或小倾角的斜面上运动的情况。采集到的磁强计数据如图7所示。与图5相比,可见图7中数据点的分布范围明显较窄,这将不利于校正算法的求解。

分别采用3.3节中所述各方法对磁强计进行校正,并验证校正前后的航向角误差。航向角的均方根误差和平均误差分别如表1和表2所示。

表1 航向角均方根误差 单位:(°)

表2 航向角平均误差 单位:(°)

观察表1和表2,首先可以看到这两个表均未给出绕轴旋转法的结果。其原因是图8所示采样数据不足以求解式(5)中的非对准误差矩阵R。实际上,图7中的数据均是在绕航姿模块z轴旋转的过程中采集的,从数学角度而言,矩阵R中包含的绕z轴的非对准误差无法与采样过程中绕z轴的旋转操作相区分,从而导致非对准误差矩阵R无法准确求解。

图8 5#航姿模块校正前后航向角误差曲线

其次值得注意的是,点积不变法校正后,航向角平均误差很大,但均方根误差不大。其原因同样是由于绕z轴的非对准误差无法求出,使得校正后航向角出现较大固定偏差。在图8给出的5#航姿模块的航向角误差曲线中可清楚地看到,经过点积不变法校正后,航向角误差曲线的波动幅度虽然减小,但却存在大约30°的固定偏差。相比之下,双内积法始终具有良好的校正效果。虽然双内积法在形式上相当于将椭球拟合法+绕轴旋转法的两步校正合并为一步完成,但由于双内积法求解过程中同时利用了两个不变性(矢量场强度不变及其在旋转轴上投影不变)的约束,改善了算法的鲁棒性,同时也降低了对数据采集过程的要求。上述实验仅利用航姿模块的外壳以及平整的墙面、台面即可完成对加速度计与磁强计的校正,简便易行。

4 结论

本文提出了用于校正航姿系统中三轴磁强计与加速度计的双内积法,该方法结合了常用的椭球拟合法、点积不变法和绕轴旋转法的优点,能解决传感器的非对准误差,且校正过程操作简便,校正效果良好、可靠,能明显改善航姿系统精度。