0 引言
重力匹配技术是将惯性导航系统输出的位置信息与重力传感器实时测量获得的重力信息与所对应的预存背景图中的位置信息进行位置上的匹配,经过匹配计算得出一条与潜航器真实运动轨迹最为接近的匹配轨迹[1,2]。对于重力匹配辅助导航系统来说,匹配算法是整个系统的关键所在,匹配算法的合适与否将直接决定系统的校正效果,而迭代最近等值线法(ICCP 算法)由于其匹配精度高、计算方便,常被作为辅助系统的匹配算法来使用[3]。但由于该算法的匹配效果受重力场变化的明显程度、重力传感器的测量精度和预存重力图的精度等因素的影响,使得该算法匹配的位置存在一定的误差,因此单一的使用该算法来构建辅助导航系统并不能对惯导系统进行很好地误差修正。为此可以将卡尔曼滤波算法与 ICCP 算法进行组合处理,形成新的匹配算法,通过新的算法来构建辅助系统可以很好的对惯性导航系统进行误差校正[4-6],因此本文所研究的重力匹配算法在减小潜航器导航误差方面具有巨大的应用价值。
ICCP 算法原理是建立在两个假设上的:一是惯导提供的指示航迹接近真实轨迹,且累积误差不大;二是重力传感器无测量误差[7]。假设一可以通过不断使用ICCP算法提供充足的外部信息来限制惯导误差的积累,而假设二在工程应用中无法实现,不论是重力基准图的预测量还是航行中重力传感器的实际测量,测量误差都不可避免且不可能完全一致,因此会造成最近等值线点不一定存在或者不能确定的情况,导致匹配算法失配或误配[8]。针对于实际测量中ICCP算法存在的问题,国内学者对此进行了深入的研究。肖晶[9]等提出了一种基于概率数据关联的地磁 ICCP 算法,对某一位置的地磁场进行多次测量,将置信区间内传感器的伪测量值都设为对于位置的有效测量,融合测量值对应的结果,提升了匹配算法的定位精度和鲁棒性;陈卓[10]等提出了一种基于可信点集和轨迹的搜索方法来拒绝标量匹配的不可靠迭代点,提高了匹配的准确性;罗诗途[11]等提出一种仿射变换法,以替换传统ICCP算法中的刚性变换,并采用粗匹配和精匹配的二级匹配策略,解决了真实航迹与惯导轨迹存在误差时的匹配问题;序列匹配算法方面,曲政豪[12]利用COR、MAD、MSD等相关极值匹配算法进行了重力梯度不同分量组合的匹配定位研究。以上文献对抑制 ICCP 算法测量噪声和匹配精度方面进行了不同程度的改进,但均未考虑测量误差对 ICCP 算法匹配残差的影响。
针对传统最近等值线迭代(ICCP)算法因惯导初始误差和重力测量误差导致的匹配精度不足甚至发散的问题,本文提出一种改进的ICCP算法以提高匹配精度和可靠性。首先,分析了匹配点轨迹因匹配原点误差造成的估计路径差,引入总约束误差来将估计路径限制在INS路径附近;在此基础上,利用MSD和MAD这两个TERCOM匹配规则构建粗匹配,用得到的粗匹配路径替代后续精确匹配中的INS测量路径,再使用ICCP算法进行精匹配,以获得更高的定位精度;最后,将惯导输出位置和ICCP算法匹配位置之差作为滤波器观测向量建立卡尔曼滤波器模型,对惯导系统误差进行校正。
1 惯性/重力梯度组合导航系统原理
1.1 系统原理
惯性/重力梯度组合导航系统由重力背景图、重力传感器、匹配算法和惯性导航系统四个共同构成。需注意的是,系统中的匹配算法不仅包括了匹配时的 ICCP 算法还包括了匹配后的卡尔曼滤波算法[13]。
重力传感器实时通过由惯导系统(或组合导航系统的匹配结果)所提供的位置序列测量出潜航器目前所处位置的重力值,同时匹配系统会根据惯导系统所提供的位置序列在预存的背景图上提取出该位置序列所对应的重力值;紧接着匹配系统会将这两组不同的重力值进行匹配计算得出一条最接近于潜航器真实位置的位置序列,并计算其匹配误差;然后卡尔曼滤波器将惯导系统输出的位置误差和匹配系统输出的位置误差进行卡尔曼滤波处理得到最优的导航误差值,再将该误差值输送给惯导系统,抑制惯导系统积累误差的增长,对惯导系统的导航误差进行校正,提高其定位精度。
1.2 ICCP匹配算法
ICCP算法是假设重力传感器测量出由惯导系统提供的位置所对应的一系列重力值序列为{Xn},真实运动轨迹的坐标序列为{yn},预存背景图里的重力值序列为{gn}[15]。由于惯导系统存在累积误差,导致其输出的位置坐标与其当时所在的真实坐标之间同样存在一定的误差,这就会造成重力传感器测量出的重力值与预存背景图里对应的重力值之间存在一定误差[9]。所以为得到潜航器真实的位置信息,需要将传感器测量得到的重力值序列{Xn}与预存背景图中对应的重力值序列{gn}进行匹配,通过刚性变换,使{Xn}与{gn}之间的距离最小,得出一个最接近于{yn}的新序列,而这条新得到的重力序列所对应的轨迹就是所要求的最接近与潜航器真实轨迹的轨迹[16]。
以真实航行轨迹为目标,并且重力传感器测量得到的重力值 Xn 一定在由背景图的重力等值线上,但是我们并不清楚该点具体在什么地方。因此我们需要进行刚性变换使测量序列{Xn}与预存序列{gn}之间的欧几里得距离最小,保证测量序列与预存序列重合程度最高,即使下式距离最小
$\mathrm{M}(\mathrm{C},\mathrm{R}\mathrm{X}+\mathrm{t})=\stackrel{N}{\sum _{n=1}}{w}_{n}d({C}_{n},R{x}_{n}+t)$
式中:X为重力传感器测量出的一系列重力值集合,记为{Xn};C为背景图中重力测量值 gn 的等值线集合记为{Cn};d(Cn,xn)是指测量点到等值线的距离;wn是考虑第n个测量点重要程度的加权系数;R为旋转矩阵;t为平移向量。
一般情况下要求出距离的最小值,需要分别提取每个测量点对应的最近等值线点、寻找刚性变换T使目标函数$M={\sum }_{t=1}^{N}\Vert R{x}_{i}+t-{y}_{i}{\Vert }^{2}$的值最小,xi与yi为网格的四个顶点坐标,将{Xn}通过变换到{Rxn+t},将{Rxn+t}作为新的起始集合进行下一次迭代直到T没有明显的变化。
设航迹质心为:
$\left\{\begin{array}{l}\stackrel{~}{y}=\frac{1}{w}\stackrel{N}{\sum _{n=1}}{w}_{n}{y}_{n}\\ \stackrel{~}{x}=\frac{1}{w}\stackrel{N}{\sum _{n=1}}{w}_{n}{x}_{n}\\ w=\stackrel{N}{\sum _{n=1}}{w}_{n}\end{array}\right.$
取
w=$\left[\begin{array}{llll}{s}_{11}+{s}_{22}& 0& 0& {s}_{21}-{s}_{12}\\ 0& {s}_{11}-{s}_{22}& {s}_{12}+{s}_{22}& 0\\ 0& {s}_{12}+{s}_{21}& {s}_{22}-{s}_{11}& 0\\ {s}_{21}-{s}_{12}& 0& 0& -{s}_{11}-{s}_{22}\end{array}\right]$
其中sij 通过公式$\mathrm{s}={\sum }_{n=1}^{N}{w}_{n}({y}_{n}-\stackrel{~}{y})({x}_{n}{-\stackrel{~}{x})}^{T}$得出,通过计算可以得到矩阵w的特征值为
$\left\{\begin{array}{l}{\lambda }_{\mathrm{1,2}}=\pm \left[\right({s}_{11}+{s}_{22}{)}^{2}+({s}_{21}-{s}_{12}{)}^{2}{]}^{\frac{1}{2}}\\ {\lambda }_{\mathrm{3,4}}=\pm \left[\right({s}_{11}-{s}_{22}{)}^{2}+({s}_{12}-{s}_{21}{)}^{2}{]}^{\frac{1}{2}}\end{array}\right.$
将最大特征值记为λmax,可以得出旋转矩阵R的旋转角度为
tan$\left(\frac{\theta }{2}\right)$=$\frac{\mathrm{s}\mathrm{i}\mathrm{n}\left(\frac{\theta }{2}\right)}{\mathrm{c}\mathrm{o}\mathrm{s}\left(\frac{\theta }{2}\right)}$=$\frac{({s}_{11}+{s}_{22}-{\lambda }_{\mathrm{m}\mathrm{a}\mathrm{x}})}{({s}_{12}-{s}_{21})}$
由刚体变换原理推导得到旋转矩阵R和平移向量t的计算公式
$\left\{\begin{array}{l}\mathrm{R}=\left(\begin{array}{ll}\mathrm{c}\mathrm{o}\mathrm{s}\theta & -\mathrm{s}\mathrm{i}\mathrm{n}\theta \\ \mathrm{s}\mathrm{i}\mathrm{n}\theta & \mathrm{c}\mathrm{o}\mathrm{s}\theta \end{array}\right)\\ \mathrm{t}=\stackrel{~}{y}-R\stackrel{~}{p}\end{array}\right.$
ICCP算法的匹配过程一共可以分为六个过程[18]:运用重力传感器获得实时的重力数据、计算重力传感器测量序列的等值线图、提取其对应的最近等值线点、计算刚性变换 T 的两个参数、迭代计算求出最优轨迹、将最优轨迹传送给惯导系统用以减少导航误差。
首先,重力传感器通过匹配系统给出的轨迹信息,测量出该轨迹对应的重力序列,同时惯导系统也给出了相同时刻的轨迹信息,由于重力传感器和惯导系统的采样频率并不相同,因此需要对惯导系统给出的轨迹进行抽样,以保证两个轨迹皆为同一时刻,然后再将采样过后的轨迹在预存背景图中匹配到相应的重力值。然后对两组重力序列进行最近等值线点提取,以获得两组待匹配轨迹。下一步开始进行刚性变换的求取,计算出旋转矩阵和平移向量,再将二者带入最优目标函数,计算欧式距离,当函数值小于设定值时,即两个序列经过变换后其欧式距离会比较小(一般将这个值设置为 10-5),或者刚性变换T已经收敛,这时就将该组轨迹进行输出用以校正惯导误差。如果没有满足上述两个条件系统将会把这组变换的新轨迹作为初始序列进行下一轮迭代计算直至满足输出条件。
1.3 改进ICCP算法
由上述ICCP算法原理可知,该算法基于两个假设,即潜航器真实位置与INS测量位置接近,累积误差不大,且无重力测量误差。前者可以通过不断使用ICCP算法滤波来限制 INS 误差的积累,而后者在实际应用中无法实现,不论是重力基准图的预测量还是航行中重力传感器的实际测量[19],都不可能完全消除。
因此,通过改进ICCP算法,引入总约束误差来将估计路径限制在INS路径附近,以提高匹配精度和可靠性。首先将匹配原点也作为匹配路径中的一个点进行调整,并建立优化公式:
E=d(x1,a1)+$\stackrel{M}{\sum _{i=2}}$d(x1-xi-1,ai-ai-1)$\stackrel{M}{\sum _{i=1}}$d+$\stackrel{M}{\sum _{i=1}}$dK(xi,yi)
式中:E为总约束误差;d(x,y)为x与y之间的欧氏距离;xi为p″i的估计位置;yi为xi在等值线ci的最近轮廓点或投影;ai为INS测量位置;K为刚度系数。
由上式可知,前两项用于将估计路径限制在INS路径附近,第三项则是使估计路径接近测量值轮廓。
图4所示为航速和航向误差对算法的影响,图中bi+1是沿(ai+1,ai)的单位向量,ei+1是垂直于bi+1的单位向量。
则有更新方程:
$\left\{\begin{array}{l}{X}_{i}={X}_{i}+(\Vert {a}_{i}-{a}_{i-1}\Vert +{p}_{i})(\mathrm{c}\mathrm{o}\mathrm{s}{\theta }_{i}{b}_{i}+\mathrm{s}\mathrm{i}\mathrm{n}{\theta }_{i}{e}_{i}),i>1\\ {X}_{i}={a}_{i}+{p}_{i}(\mathrm{c}\mathrm{o}\mathrm{s}{\theta }_{1}{b}_{1}+\mathrm{s}\mathrm{i}\mathrm{n}{\theta }_{1}{e}_{1}),i=1\end{array}\right.$
若pi和θi很小,并且忽略二阶及以上的项,可将上式近似为
$\left\{\begin{array}{l}{X}_{i}{X}_{i-1}+{a}_{i}-{a}_{i-1}+{p}_{i}{b}_{i}+{\xi }_{i}{e}_{i},i>1\\ {X}_{1}{a}_{1}+{p}_{1}{b}_{1}+{\xi }_{1}{e}_{1},i=1\end{array}\right.$
其中,ξi=‖ai-ai-1‖θi且ξ1=p1θ1,将式(8)代入式(6)即有
$E=\stackrel{M}{\sum _{i=1}}({p}_{i}^{2}+{\xi }_{i}^{2})+K\stackrel{M}{\sum _{i=1}}\Vert {X}_{i}-{Y}_{i-1}{\Vert }^{2}$
通过上述改进可将ICCP算法的估计路径限制在INS路径附近,但如果INS初始误差很大,则又无法满足这一条件,并可能导致改进的ICCP算法发散,因此这里在应用ICCP之前先进行一个粗匹配,得到一个粗匹配路径,该路径比INS路径更接近于实际路径,在后面的精匹配中,用粗匹配路径代替INS路径。
引入均方差(MSD)和平均绝对差(MAD)这两项匹配规则来构建粗匹配,其值可表示为
$\left\{\begin{array}{l}{J}_{MSD}(x,y)=\frac{1}{M}\stackrel{M}{\sum _{i=1}}\left[{P}_{t}\right(i)-{P}_{m}(i)-({\overline{P}}_{t}-\overline{P}{〗}_{m}{\left)\right]}^{2}\\ {J}_{MAD}(x,y)=\frac{1}{M}\stackrel{M}{\sum _{i=1}}\left|{P}_{t}\left(i\right)-{P}_{m}\left(i\right)-({\overline{P}}_{t}-{\overline{P}}_{m})\right|\end{array}\right.$
式中:Pt(i)为估计路径中的第i个点的位置,Pm(i)为测量路径中第i个点的位置,$\overline{P}$和${\overline{P}}_{m}$是估计路径和测量路径的均值。
若获得最小JMAD(x,y)值的点和获得最小JMSD(x,y)值的点相同,则匹配图中该点处的路径视为粗略估计路径;否则,将计算最小MAD与MSD值点处的路径与重力测量路径之间的绝对差,将差值最小的路径确定为粗估计路径。
由此得到的粗匹配路径将代替都需精确匹配中的INS测量路径。
2 组合导航系统误差方程和量测方程
2.1 惯导系统误差模型
惯导系统位置误差的误差方程建立如下:
$\left\{\begin{array}{l}{J}_{MSD}(x,y)=\frac{1}{M}\stackrel{M}{\sum _{i=1}}\left[{P}_{t}\right(i)-{P}_{m}(i)-({\overline{P}}_{t}-{\overline{P}}_{m}{\left)\right]}^{2}\\ {J}_{MAD}(x,y)=\frac{1}{M}\stackrel{M}{\sum _{i=1}}\left|{P}_{t}\left(i\right)-{P}_{m}\left(i\right)-({\overline{P}}_{t}-{\overline{P}}_{m})\right|\end{array}\right.$
式中:RM为子午曲率半径,RN为卯酉曲率半径;
惯导系统速度误差的误差方程建立如下:
$\left\{\begin{array}{l}\delta {\stackrel{ ·}{V}}_{x}=\left(2{\omega }_{ie}\mathrm{c}\mathrm{o}\mathrm{s}\varphi {V}_{y}+\frac{{V}_{x}{V}_{y}}{{R}_{N}}\mathrm{s}\mathrm{e}{\mathrm{c}}^{2}\varphi \right)\delta \varphi +\frac{{V}_{y}}{{R}_{N}}\mathrm{t}\mathrm{a}\mathrm{n}\varphi \delta {V}_{x}\\ +\left(2{\omega }_{ie}\mathrm{s}\mathrm{i}\mathrm{n}\varphi +\frac{{V}_{x}}{{R}_{N}}\mathrm{t}\mathrm{a}\mathrm{n}\varphi \right)\delta {V}_{y}+{f}_{y}{\varphi }_{z}-g{\varphi }_{y}+{\mathrm{∇ }}_{x}\\ \delta {\stackrel{ ·}{V}}_{y}=-\left(2{\omega }_{ie}\mathrm{c}\mathrm{o}\mathrm{s}\varphi {V}_{x}+\frac{{V}_{x}^{2}}{{R}_{N}}\mathrm{s}\mathrm{e}{\mathrm{c}}^{2}\varphi \right)\delta \varphi \\ -\left(2{\omega }_{ie}\mathrm{s}\mathrm{i}\mathrm{n}\varphi +\frac{{V}_{x}}{{R}_{N}}\mathrm{t}\mathrm{a}\mathrm{n}\varphi \right)\delta {V}_{x}+g{\varphi }_{x}-{f}_{x}{\varphi }_{z}+{\mathrm{∇ }}_{y}\end{array}\right.$
姿态角误差的误差方程如下:
$\left\{\begin{array}{l}{\stackrel{ ·}{\varphi }}_{x}=-\frac{1}{{R}_{M}}\delta {V}_{y}+\left({\omega }_{ie}\mathrm{s}\mathrm{i}\mathrm{n}\varphi +\frac{{V}_{x}}{{R}_{N}}\mathrm{t}\mathrm{a}\mathrm{n}\varphi \right){\varphi }_{y}\\ -\left({\omega }_{ie}\mathrm{c}\mathrm{o}\mathrm{s}\varphi +\frac{{V}_{x}}{{R}_{N}}\right){\varphi }_{z}+{\epsilon }_{x}\\ {\stackrel{ ·}{\varphi }}_{y}=-{\omega }_{ie}\mathrm{s}\mathrm{i}\mathrm{n}\varphi \delta \varphi +\frac{1}{{R}_{N}}\delta {V}_{x}\\ -\left({\omega }_{ie}\mathrm{s}\mathrm{i}\mathrm{n}\varphi +\frac{{V}_{x}}{{R}_{N}}\mathrm{t}\mathrm{a}\mathrm{n}\varphi \right){\varphi }_{x}-\frac{{V}_{y}}{{R}_{M}}{\varphi }_{z}+{\epsilon }_{y}\\ {\stackrel{ ·}{\varphi }}_{z}=\left({\omega }_{ie}\mathrm{c}\mathrm{o}\mathrm{s}\varphi +\frac{{V}_{x}}{{R}_{N}}\mathrm{s}\mathrm{e}\mathrm{c}{\varphi }^{2}\right)\delta \varphi \\ +\frac{\mathrm{t}\mathrm{a}\mathrm{n}\varphi }{{R}_{N}}\delta {V}_{x}+\left({\omega }_{ie}\mathrm{c}\mathrm{o}\mathrm{s}\varphi +\frac{{V}_{x}}{{R}_{N}}\right){\varphi }_{x}+\frac{{V}_{y}}{{R}_{M}}{\varphi }_{y}+{\epsilon }_{z}\end{array}\right.$
式中:ωie为地球自转角速度;εx,εy,εz为陀螺常值漂移;fx,fy,fz为导航系下的比力值;∇x,∇y为加速度计零偏。
2.2 ICCP误差模型
ICCP的误差主要来源于四个方面,一是重力传感器的测量误差、二是重力背景图的数据误差,三则是惯导系统在为匹配系统提供位置信息时的位置误差,四是匹配算法本身存在的计算误差[20]。
前两者的误差可以认为是高斯白噪声:
$\left\{\begin{array}{l}\delta {g}_{m}~N(0,{\sigma }_{m}^{2})\\ \delta {g}_{t}~N(0,{\sigma }_{t}^{2})\end{array}\right.$
式中:δgm为预存背景图误差;δgt为重力传感器测量误差。
ICCP算法的计算误差可由马尔科夫模型计算得出,其误差协方差矩阵表示为:
QICCP=Q1P${Q}_{1}^{T}$
$\left\{\begin{array}{l}{Q}_{1}=\left[\begin{array}{ll}{C}_{11}+{C}_{22}-{C}_{33}-{C}_{44}& 2({C}_{23}-{C}_{14})\\ 2({C}_{32}-{C}_{14})& {C}_{11}-{C}_{22}+{C}_{33}-{C}_{44}\end{array}\right]\\ {C}_{ij}=\frac{-{J}_{ij}}{\sqrt{(1-{J}_{ii})(1-{J}_{jj})}}\end{array}\right.$
式中:P为观测值的权矩阵,矩阵J的元素是由算法在计算旋转矩阵的过程中的S阵和假设出的观测值误差协方差矩阵Pl计算得到。
可得到ICCP算法误差的协方差矩阵为
Q=QICCP+${\delta }_{m}^{2}$I2×2+${\delta }_{t}^{2}$I2×2
2.3 量测方程与滤波方程
根据上述对系统的分析,选取系统状态变量为X=[δϕINS,δλINS,δVx,δVy,φx,φy,φz,εx,εy,εz,∇x,∇y,δϕICCP,δλICCP]T,其中δϕINS,δλINS为惯导系统的位置误差;δϕICCP,δλICCP为ICCP算法的匹配误差,其前的项为惯导系统误差项,令W=[wgx,wgy,wgz,wrx,wry,wrz,wax,way]T为系统的过程噪声,将其对应的微分方程写成矩阵形式可得到卡尔曼滤波器的状态方程如下:
$\stackrel{ ·}{X}$=FMX+GMW
式中:FM为状态方程转移矩阵,GM为噪声系数矩阵。
FM=$\left(\begin{array}{ll}F& 0\\ 0& {I}_{2\times 2}\end{array}\right)$
GM=$\left(\begin{array}{l}G\\ 0\end{array}\right)$
其中:
F=$\left[\begin{array}{lll}{F}_{11}& {F}_{2\times 3}& {F}_{2\times 5}\\ {F}_{21}& {F}_{22}& {I}_{5\times 5}\\ \mathrm{ }& {0}_{5\times 12}& \mathrm{ }\end{array}\right]$
F11=$\left[\begin{array}{llll}0& 0& 0& \frac{1}{R}\\ \frac{{V}_{X}}{{R}_{N}}\mathrm{t}\mathrm{a}\mathrm{n}\varphi \mathrm{s}\mathrm{e}\mathrm{c}\varphi & 0& \frac{\mathrm{s}\mathrm{e}\mathrm{c}\varphi }{{R}_{N}}& 0\end{array}\right]$
F21=$\left[\begin{array}{llll}2{\omega }_{ie}\mathrm{c}\mathrm{o}\mathrm{s}\varphi {V}_{y}+\frac{{V}_{x}{V}_{y}}{{R}_{n}}\mathrm{s}\mathrm{e}{\mathrm{c}}^{2}\varphi & 0& \frac{{V}_{y}}{{R}_{N}}\mathrm{t}\mathrm{a}\mathrm{n}\varphi & 2{\omega }_{ie}\mathrm{s}\mathrm{i}\mathrm{n}\varphi +\frac{{V}_{x}}{{R}_{N}}\mathrm{t}\mathrm{a}\mathrm{n}\varphi \\ -2{\omega }_{ie}\mathrm{c}\mathrm{o}\mathrm{s}\varphi {V}_{X}-\frac{{V}_{x}^{2}}{{R}_{N}}\mathrm{s}\mathrm{e}{\mathrm{c}}^{2}\varphi & 0& -2{\omega }_{ie}\mathrm{s}\mathrm{i}\mathrm{n}\varphi -\frac{{V}_{x}}{{R}_{N}}\mathrm{t}\mathrm{a}\mathrm{n}\varphi & 0\\ 0& 0& 0& -\frac{1}{{R}_{M}}\\ -{\omega }_{ie}\mathrm{s}\mathrm{i}\mathrm{n}\varphi & 0& \frac{1}{{R}_{M}}& 0\\ {\omega }_{ie}\mathrm{c}\mathrm{o}\mathrm{s}\varphi +\frac{{V}_{x}}{{R}_{N}}\mathrm{s}\mathrm{e}\mathrm{c}{\varphi }^{2}& 0& \frac{\mathrm{t}\mathrm{a}\mathrm{n}\varphi }{R}& 0\end{array}\right]$
F22=$\left[\begin{array}{lll}0& -g& {f}_{y}\\ g& 0& -{f}_{x}\\ 0& {\omega }_{ie}\mathrm{s}\mathrm{i}\mathrm{n}\varphi +\frac{{V}_{x}}{{R}_{N}}\mathrm{t}\mathrm{a}\mathrm{n}\varphi & -{\omega }_{ie}\mathrm{c}\mathrm{o}\mathrm{s}\varphi -\frac{{V}_{x}}{{R}_{N}}\\ -{\omega }_{ie}\mathrm{s}\mathrm{i}\mathrm{n}\varphi -\frac{{V}_{x}}{{R}_{N}}\mathrm{t}\mathrm{a}\mathrm{n}\varphi & 0& -{\omega }_{ie}\mathrm{s}\mathrm{i}\mathrm{n}\varphi \\ -{\omega }_{ie}\mathrm{c}\mathrm{o}\mathrm{s}\varphi +\frac{{V}_{x}}{{R}_{N}}& \frac{{V}_{y}}{{R}_{M}}& 0\end{array}\right]$
F2×3 、F2×5 均为零矩阵,I2×2 、I5×5均为单位矩阵。系统矩阵G为
G=I12×12
将状态方程进行离散化处理可得
Xk+1=Φk+1,kXk+k+1,kWk
选取惯导系统的输出位置误差和ICCP算法输出的位置误差的差值作为系统的观测向量,即
Z=(Δλ,Δϕ)T=(λINS-λICCP,ϕINS-ϕICCP)
将其写成矩阵形式得到系统的状态方程为
Z=HX+V
其中H为观测方程的观测矩阵,V为观测噪声矩阵,即ICCP算法的误差矩阵
H=$\left[\begin{array}{lll}\begin{array}{ll}1& 0\\ 0& 1\end{array}& {0}_{13\times 2}& \begin{array}{ll}-1& 0\\ 0& -1\end{array}\end{array}\right]$
对观测方程作离散化处理有
Zk=HkXk+Vk
综上,构建ICCP滤波方程结构如下
$\left\{\begin{array}{l}{\hat{X}}_{k,k-1}={X}_{0}\\ {\hat{X}}_{k}={\hat{X}}_{k|k-1}+{K}_{k}({Z}_{k}-{H}_{k}{\hat{X}}_{k|k-1})\\ {K}_{k}=\frac{{P}_{k,k-1}{H}_{k}^{T}}{{H}_{k}{P}_{k,k-1}{H}_{k}^{T}+{R}_{k}}\\ {P}_{k,k-1}={\Phi }_{k,k-1}{P}_{k-1}{\Phi }_{k,k-1}^{T}+{\Gamma }_{k,k-1}{Q}_{k-1}{\Gamma }_{k,k-1}^{T}\\ {P}_{k}=[I-{K}_{k}{H}_{k}]{P}_{k,k-1}\end{array}\right.$
3 仿真验证与分析
为了验证算法的有效性,本文采用真实海洋重力数据的重力背景图,如图5所示,并在真实重力背景图上通过仿真得到载体的航行路线;起始位置为东经117.541°、北纬20.243°,东向和北向速度均为5m/s,仿真时间2h,重力梯度仪噪声为0.01E,陀螺常值漂移为0.001°/h,初始速度和位置误差为0,加速度计零漂为10μg,三初始角误差分别设置为0.01°、0.01°和0.15°,惯导更新周期为1s。
将使用传统ICCP算法对惯导轨迹进行匹配处理的结果与本文改进后的ICCP算法对惯导轨迹的匹配处理结果进行比较,其匹配轨迹如图6所示。
位置精度方面如图8所示,传统ICCP匹配方法下,潜航器组合导航系统位置误差为北向160m,东向120m;经过对ICCP匹配方法的改进后,在该组合导航系统的定位精度在2h的仿真结束后,北向和东向误差均小于30m,可见经过引入总约束误差和构建粗匹配后,组合导航系统的输出位置轨迹达到了最优。
4 结论
本文针对传统最近等值线迭代(ICCP)算法因惯导初始误差和重力测量误差导致的匹配精度不足甚至发散的问题,提出一种改进的ICCP算法以提高匹配精度和可靠性。首先,通过分析ICCP算法原理,分析了匹配点轨迹因匹配原点误差、INS测量误差以及重力测量误差会对ICCP算法匹配造成的影响,提出了改进ICCP算法的方法,即引入总约束误差来将估计路径限制在INS路径附近,并在此基础上,利用MAD和MSD这两个匹配规则构建粗匹配,用得到的粗匹配路径替代后续精确匹配中的INS测量路径,再使用ICCP算法进行精匹配,以获得更高的定位精度;将惯导输出位置和ICCP算法匹配位置之差作为滤波器观测向量建立卡尔曼滤波器模型,对惯导系统误差进行校正。仿真和分析结果表明,在考虑惯导初始误差和重力测量误差的情况下,改进的ICCP算法的姿态误差最大值小于0.015°,航向误差小于0.4°,位置误差最大值小于30m,导航精度较传统ICCP算法提升了70%以上,即该惯性/重力梯度组合导航系统拥有更高的定位精度。