CN103969671A - 一种基于非线性跟踪微分器的杆臂误差解算方法 - Google Patents

Abstract

本发明属于惯性导航系统和卫星导航系技术领域，具体涉及一种在SINS/卫星导航系统重力测量中基于非线性跟踪微分器的杆臂误差解算方法。本发明包括：得到在载体坐标系下的比力；得到地理系相对地球系的速度矢量；得到地理坐标系到惯性系角速度；得载体在导航解算后的载体相对于地理坐标系的角速度；得到两系统在载体系中的杆臂距离；得到在地理系下的表达式以及与角速度的关系；得到斜对称矩阵的微分值；解算出杆臂误差值。本发明避免了传统的杆臂误差直接处理引入噪声和精度不理想问题，本发明中构造的非线性微分跟踪器时，只需适当调整参数，构造合理的非线性函数就可以得到高品质的跟踪微分器，得到较高精度的输出结果。

Description

一种基于非线性跟踪微分器的杆臂误差解算方法

技术领域

本发明属于惯性导航系统和卫星导航系技术领域，具体涉及一种在SINS/卫星导航系统重力测量中基于非线性跟踪微分器的杆臂误差解算方法。

背景技术

SINS/卫星导航系统在重力测量中，在舰船和飞机上都可以搭载SINS和卫星导航系统接收机；由于重力测量的幅度广阔，受地形限制小，测量速度快周期短，系统搭载快捷等优点，受到越来越广泛的探究。

目前，在实际应用中，安装在载体上的SISN中心和卫星导航系统(GPS或北斗)接收机中心有一定的距离，产生了杆臂效应误差。这种杆臂误差会造成系统的位置、速度和加速度误差，且这些误差对测量精度产生较大的影响，使测量的数据效果不明显。因此，通常要对存在的杆臂效应误差进行解算和修正。通常对杆臂距离引起的加速度误差在解算和修正中需要计算角速度的微分值角加速度。跟踪微分器具有提取跟踪信号，并且微分信号的功能，由跟踪微分器得到的微分信号是输入信号广义倒数的一种光滑逼近。相应的仿真表明，适当构造非线性函数能得到品质较高的跟踪微分器，此外在跟踪微分器中加入二阶低通滤波器能有效处理噪声，得到比较精确的输出结果。因此，可以选用非线性跟踪微分器对角速度信号进行跟踪和微分，得到角加速度输出，以便进行杆臂效应解算和修正。

发明内容

本发明的目的在于提供一种可为SINS/卫星导航系统在重力测量中，由SINS和卫导接收机中心不重合产生的杆臂误差提供较精确的解算修正提高精度的基于非线性跟踪微分器的杆臂误差解算方法。

本发明包括以下几个步骤：

(1)利用SINS的加速度计采集载体比力信息，得到在载体坐标系下的比力f^b；

(2)根据惯导系统解算方程，已知载体坐标系到地理坐标系的余弦矩阵可以得到地理系相对地球系的速度矢量

(3)利用初始经纬度信息，确定地理系到地球系的角速度将地球的自转角速度投影到地理坐标下为求和得到地理坐标系到惯性系角速度

(4)利用SINS陀螺仪采集载体的角速度信息以及角速度在余弦矩阵作用下可得到求差可得载体在导航解算后的载体相对于地理坐标系的角速度

(5)利用SINS采集的比力信息和余弦矩阵确定SINS在载体坐标系中的位置信息；利用卫星导航系统接收机采集到的卫星导航系统的信号确定接收机在载体系中的位置信息，位置信息与位置信息作差得到两系统在载体系中的杆臂距离r^b；

(6)对载体系中的杆臂距离r^b进行解算和微分得到在地理坐标系下的距离误差Δl，速度误差Δv和加速度误差Δa,得到在地理系下的表达式以及与角速度的关系；

(7)根据载体相对于地理坐标系的角速度的斜对称矩阵信息，利用跟踪微分器以角速度为非线性跟踪器的输入，则输出为角加速度信息，得到斜对称矩阵的微分值

(8)根据载体相对于地理坐标系的距离误差，速度误差和加速度杆误差的模型表达式，利用跟踪微分器得到的信息，解算出杆臂误差值。

比力f^b

$f^b={\left[\begin{array}{ccc}{f}_x^b& f_y^b& f_z^b\end{array}\right]}^T.$

步骤(2)中，载体系到地理系的转换矩阵定义为坐标系先绕z轴转动偏航角ψ，再绕x轴转动俯仰角θ，最后绕y轴转动滚动角γ得到，具体表达式为：

$C_b^t=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}-\sin \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\γ}\cos \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& -\sin \mathrm{\γ}\cos \mathrm{\θ}\\ -\cos \mathrm{\θ}\sin \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\θ}\\ \sin \mathrm{\γ}\cos \mathrm{\ψ}+\cos \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right];$

地理坐标系下比力f^t为进行去噪、积分可得到地理系表示的速度导航解算系与地理坐标系重合。

在步骤(3)中，将地理坐标系下的速度转换为曲度阵

将地球自转角速度w_ie也投影到地理系得：两者求和得到载体在地理系相对于惯性系的角速度

其中K表示载体偏北向的航向，表示纬度，R表示地球半径。

在步骤(4)中，通过载体在地理系相对于惯性系的角速度转换矩阵的作用，可得到地理系相对惯性系的角速度在载体系中的投影其中又根据步骤(3)的SINS陀螺仪采集到得角速度得到载体相对于地理系在载体系的角速度投影其中 $w_{\mathrm{tb}}^b=w_{\mathrm{ib}}^b-w_{\mathrm{it}}^b.$

在步骤(5)中，通过确定SINS和卫星导航系统\接收机分别在载体系中的位置坐标矢量为和差为r^b，记r^b＝[r_x r_y r_z]^T,r_x，r_y，r_z分别表示卫星导航系统接收机和SINS位置差在载体坐标系三个坐标轴方向上的投影。

在步骤(6)中，根据两系统的位置差r^b，对r^b进行导航解算且微分可得在地理系下中的到距离误差Δl、速度误差Δv和加速度误差Δa,

$\mathrm{\Δl}=r^t=C_b^t{r}^b$

$\mathrm{\Δv}=\stackrel{\·}{r^t}=C_b^t[w_{\mathrm{tb}}^b\×]r^b,$

$\mathrm{\Δa}=\stackrel{\·\·}{r^t}=C_b^t{[w_{\mathrm{tb}}^b\×]}^2{r}^b+C_b^t\left[\stackrel{\·}{w_{\mathrm{tb}}^b\×}\right]r^b$

其中，即是步骤(5)中得到的，表示的斜对称矩阵，若记 $w_{\mathrm{tb}}^b={\left[\begin{array}{ccc}{w}_x& w_y& w_z\end{array}\right]}^T,$ 则

$[w_{\mathrm{tb}}^b\×]=\left[\begin{array}{ccc}0& -w_z& w_y\\ w_z& 0& -w_x\\ w_y& w_x& 0\end{array}\right],$

是的微分。

在步骤(7)中，利用跟踪微分器跟踪提取并微分信号的功能，让非线性跟踪微分器作用于经过滤波后可以得到精度较高的信息，然后进行导航解算。

本发明的有益效果在于：

本发明提供的杆臂误差解算方法适用于SINS和卫星导航系统重力测量领域。本发明方法直观有效，对重力测量结果修正补偿提供了有效方法，对测量精度有直接影响。本发明避免了传统的杆臂误差直接处理引入噪声和精度不理想问题，本发明中构造的非线性微分跟踪器时，只需适当调整参数，构造合理的非线性函数就可以得到高品质的跟踪微分器，得到较高精度的输出结果；同时加入低通滤波器后能进一步去噪得到高精度的微分结果，从而得到较高精度的杆臂误差解算修正值。

附图说明

图1是本发明的方法流程图；

图2是本发明提供的惯导解算详细示图；

图3是本发明提供的SINS和卫星导航接收机在载体坐标系中的位置简要图示；

图4是本发明提供的正弦曲线在简便跟踪微分器下仿真示意图。

具体实施方式

下面将结合附图和实施例对本发明作进一步的详细说明。

本发明利用SINS加速度计和陀螺分别测量载体比力信息和载体角速度信息，由导航解算方程得到载体系相对于地理系在载体系中的投影角速度根据卫星导航系统(GPS或北斗)计算其接收机在载体系中的位置利用SINS系统计算其在载体系中的位置对接收机和SINS在载体系中位置的差值进行微分得到速度和加速度表达式，由陀螺仪采集角速率信号计算旋转矢量更新四元数Q，完成的更新；同时由跟踪微分器对含的信号进行处理，完成对杆臂误差的解算。

步骤一：利用SINS的加速度计采集载体比力信息，得到在载体坐标系(b系)下的比力f^b；

步骤二：根据惯导系统解算方程，在已知载体系到地理坐标系t系的余弦矩阵条件下，可以得到地理系相对地球的速度矢量

步骤三：利用提供初始经纬度信息，确定地理系到地球系(e系)的角速度将地球的自转角速度投影到地理坐标下得两者求和得到地理坐标系到惯性系(i系)角速度

步骤四：利用SINS的陀螺仪采集载体的角速度信息利用步骤三获取的角速度在余弦矩阵作用下可得到两者求差可得载体在导航解算后的载体相对于地理坐标系的角速度

步骤五：利用SINS采集的比力信息和余弦矩阵可以确定SINS在载体坐标系中的位置信息；利用卫星导航系统(GPS或者北斗)接收机采集到的卫星导航系统的信号可以确定接收机在载体系中的位置信息，此信息与作差得到两系统在载体系中的杆臂距离，记为r^b；

步骤六：在导航解算中，对载体系中的杆臂距离r^b进行解算和微分得到在地理坐标系下的距离误差Δl，速度误差Δv和加速度误差Δa。得到了它们在地理系下的模型以及与角度度的关系；

步骤七：根据载体相对于地理坐标系的角速度的斜对称矩阵信息，利用非线性跟踪微分器提取跟踪信号并微分信号的功能，把角速度信息作为非线性跟踪器的出入，然后输出角加速度值，得到斜对称矩阵的微分值

步骤八：根据导航解算得到的由杆臂效应得到的距离误差，速度误差和加速度误差的模型表达式，利用跟踪微分器滤波后得到的信息，最终可以解算出杆臂误差值。

本发明公开了一种基于非线性跟踪微分器的杆臂误差解算方法，该方法包括：在卫星导航系统/捷联惯导系统(SINS)重力测量中，利用SINS的加速度计测量比力，根据惯导系统解算方程，在已知方向余弦矩阵的情况下，可以得到地理系(e系)相对地球坐标系(t系)在地理坐标下的速度投影同时利用载体的高度和纬度信息可以得到地理系相对于地球系的角速率此外利用地球自转的角速率信息在地理系下的投影再利用SINS的陀螺仪提供的角速度信息(其中，i表示惯性系，b表示载体坐标系)，可以得到载体相对于地理系的角度率在载体系中的投影以及其斜对称矩阵利用跟踪微分器提取信号和微分信号的特点，跟踪提取信号以及微分此信号得到值；利用SINS的位置和卫星导航系统(GPS和北斗)接收机的位置差值关系在载体坐标系下的投影r^b，然后根据杆臂模型可以得到SINS和卫星导航系统的距离误差Δl，速度误差Δv和加速度误差Δa值，以进行杆臂解算修正。

具体如图所示包括以下几个步骤：

步骤一：利用SINS的加速度计采集载体比力信息，得到在载体坐标系(b系)下的比力f^b。

具体为，搭载在载体上的SINS包含有三轴加速度，根据牛顿第二定律，加速度计采集到得是载体相对于惯性系的比力 $f^b={\left[\begin{array}{ccc}{f}_x^b& f_y^b& f_z^b\end{array}\right]}^T.$

步骤二：根据惯导系统解算方程，在已知载体系到地理坐标系t系的余弦矩阵可以得到地理系相对地球系的速度矢量

具体为，无论是对于平台式还是捷联式惯导系统，解算坐标系均为地理坐标系，即导航解算系与t系重合。载体系到地理系的转换矩阵定义为坐标系先绕z轴转动ψ(偏航角)角度，再绕x轴转动θ(俯仰角)角度，最后绕y轴转动γ(滚动角)角度得到，具体表达式为

$C_b^t=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}-\sin \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\γ}\cos \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& -\sin \mathrm{\γ}\cos \mathrm{\θ}\\ -\cos \mathrm{\θ}\sin \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\θ}\\ \sin \mathrm{\γ}\cos \mathrm{\ψ}+\cos \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right]$ 从上面的表达式可以看出，转换矩阵的计算需要用到初始以及更新的偏航角ψ、俯仰角θ和滚动角γ这些姿态角。地理坐标系下比力f^t为得到了在地理坐标系下的比力值后，然后去除有害加速度然后再积分运算，可以得到地

理系相对于地球在地理系下的速度投影值

步骤三：利用提供初始经纬度信息，确定地理系到地球系(e系)的角速度将地球的自转角速率投影到地理坐标下两者求和得到地理坐标系到惯性系(i系)角速度

具体为，利用上一步得到的然后进行积分运算可得位置信息即经度信息λ和纬度信息此外利用可得到地理系相对于地球系的旋转角速度在地理上的投影

上式中R是地球半径，K是载体航向。另外，地球自转角速度w_ie是常值，投影到地理坐标系下为

两式相加，得地理系相对于惯性系在地理系下的角速度投影

步骤四：利用SINS的陀螺仪采集载体的角速度信息利用步骤三获取的角速率在余弦矩阵作用下可得到两者求差可得载体在导航解算后的载体相对于地理坐标系的角速度

具体为，通过SINS的陀螺采集到得角速度信息可以得到载体相对于惯性系的角速度地理系相对于惯性系的在载体下的投影角速度为 因此，得到载体系相对于导航解算系在载体系上的投影角速度为(惯导解算的简要示意图见图2)。

步骤五：利用SINS采集的比力信息和余弦矩阵可以确定SINS在载体坐标系中的位置信息；利用卫星导航系统(GPS或者北斗)接收机采集到的卫星导航系统的信号可以确定接收机在载体系中的位置信息，此信息与作差得到两系统在载体系中的杆臂距离，记为r^b。

具体为，设卫星导航系统接收机在载体坐标系中的位置矢量为SINS在载体坐标系中的位置矢量为两者作差的杆臂距离为SINS和卫导接收机在载体坐标系中的位置简要示意图见图3。

步骤六：在导航解算中，对载体系中的杆臂距离r^b进行解算和微分得到在地理坐标系下的距离误差Δl，速度误差Δv和加速度误差Δa。得到了它们在地理系下的模型以及与角度度的关系。

具体为，对载体系中的杆臂距离进行导航解算，得到地理系下的距离误差r^t，然后对r^t进行一次微分和二次微分分别得到速度误差Δv和加速度误差Δa，

$\mathrm{\Δl}=r^t=C_b^t{r}^b$

$\mathrm{\Δv}=\stackrel{\·}{r^t}=C_b^t[w_{\mathrm{tb}}^b\×]r^b$

$\mathrm{\Δa}=\stackrel{\·\·}{r^t}=C_b^t{[w_{\mathrm{tb}}^b\×]}^2{r}^b+C_b^t\left[\stackrel{\·}{w_{\mathrm{tb}}^b\×}\right]r^b$

通过上式可以知道速度误差与导航解算系在载体系上的投影角速度的斜对称矩阵有关，加速度误差Δa还与的斜对称矩阵的微分有关，即与角加速度有关。表示的斜对称矩阵，若记 $w_{\mathrm{tb}}^b={\left[\begin{array}{ccc}{w}_x& w_y& w_z\end{array}\right]}^T,$ 则

$[w_{\mathrm{tb}}^b\×]=\left[\begin{array}{ccc}0& -w_z& w_y\\ w_z& 0& -w_x\\ w_y& w_x& 0\end{array}\right]$

若对角速度直接求微分，则会引入噪声，降低计算精度，对误差的修正意义不大。

步骤七：根据载体相对于地理坐标系的角速度的斜对称矩阵信息，利用跟踪微分器提取信号并微分此信号的功能，得到斜对称矩阵的微分值

具体为，若系统

$\left\{\begin{array}{c}\stackrel{\·}{z_1}=z_2\\ \stackrel{\·}{z_2}=f(z_1,z_2)\end{array}\right.$

的解满足z_i→0(i＝1,2；t→∞)，则对任意有界可积的函数v(t)和任意常数T＞0,系统

$\left\{\begin{array}{c}\stackrel{\·}{x_1}=x_2\\ \stackrel{\·}{x_2}=R^{2}f(x_1-v,x_2/R)\end{array}\right.$

的解x₁(t)满足：

$\underset{R\→\∞}{\lim }{\∫}_0^T|x_1\left(t\right)-v\left(t\right)|\mathrm{dt}=0$

上述定理表明跟踪因子R的逐渐增大，系统的解x₁(t)能在任意有限时间内充分逼近输入信号v(t)，而x₁(t)的倒数x₂(t)可以作为输入信号v(t)的微分信号。同样的，可以把中的有用项作为跟踪器的微分，经过跟踪微分器后可以得到设角速度 $w_{\mathrm{tb}}^b={\left[\begin{array}{ccc}{w}_x& w_y& w_z\end{array}\right]}^T$ 的估计量为x₁(k)，角加速度的估计量记为x₂(k)；因此有 $x_1\left(k\right)=\widehat{w_{\mathrm{tb}}^b},x_2\left(k\right)=\stackrel{^\·}{w_{\mathrm{tb}}^b},$ 跟踪微分器的方程为：

$\left\{\begin{array}{c}{x}_1(k+1)=x_1\left(k\right)+\mathrm{\τ}{x}_2\left(k\right)\\ x_2(k+1)=x_2\left(k\right)+\mathrm{\τ}\·K\·F[x_1\left(k\right)-v\left(k\right),x_2\left(k\right)/\sqrt{K}]\end{array}\right.$

其中，v(k)为跟踪微分器输入，即 $v\left(k\right)=w_{\mathrm{tb}}^b={\left[\begin{array}{ccc}{w}_x& w_y& w_z\end{array}\right]}^T;$ τ为采样间隔；K为系统待定参数，K＞0；F(x,y)＝sgn(x+|y|y/2)。但实际中，为防止高频振动和使系统收敛速度快，可以在非线性跟踪微分器中引入二阶低通滤波器。图4示意了简单的正弦曲线在简单跟踪微分器作用下的仿真图。根据非线性跟踪微分器处理得到后，可以精确的对角速度误差进行解算和修正。

Claims (8)

1.一种基于非线性跟踪微分器的杆臂误差解算方法，其特征在于：

(1)利用SINS的加速度计采集载体比力信息，得到在载体坐标系下的比力f^b；

(2)根据惯导系统解算方程，已知载体坐标系到地理坐标系的余弦矩阵可以得到地理系相对地球系的速度矢量

(3)利用初始经纬度信息，确定地理系到地球系的角速度将地球的自转角速度投影到地理坐标下为求和得到地理坐标系到惯性系角速度

(4)利用SINS陀螺仪采集载体的角速度信息以及角速度在余弦矩阵作用下可得到求差可得载体在导航解算后的载体相对于地理坐标系的角速度

(5)利用SINS采集的比力信息和余弦矩阵确定SINS在载体坐标系中的位置信息；利用卫星导航系统接收机采集到的卫星导航系统的信号确定接收机在载体系中的位置信息，位置信息与位置信息作差得到两系统在载体系中的杆臂距离r^b；

(6)对载体系中的杆臂距离r^b进行解算和微分得到在地理坐标系下的距离误差Δl，速度误差Δv和加速度误差Δa,得到在地理系下的表达式以及与角速度的关系；

(7)根据载体相对于地理坐标系的角速度的斜对称矩阵信息，利用跟踪微分器以角速度为非线性跟踪器的输入，则输出为角加速度信息，得到斜对称矩阵的微分值

(8)根据载体相对于地理坐标系的距离误差，速度误差和加速度杆误差的模型表达式，利用跟踪微分器得到的信息，解算出杆臂误差值。

2.根据权利要求1所述的一种基于非线性跟踪微分器的杆臂误差解算方法，其特征在于：所述的比力f^b

3.根据权利要求1所述的一种基于非线性跟踪微分器的杆臂误差解算方法，其特征在于：所述步骤(2)中，载体系到地理系的转换矩阵定义为坐标系先绕z轴转动偏航角ψ，再绕x轴转动俯仰角θ，最后绕y轴转动滚动角γ得到，具体表达式为：

地理坐标系下比力f^t为进行去噪、积分可得到地理系表示的速度导航解算系与地理坐标系重合。

4.根据权利要求1所述的一种基于非线性跟踪微分器的杆臂误差解算方法，其特征在于：在步骤(3)中，将地理坐标系下的速度转换为曲度阵

将地球自转角速度w_ie也投影到地理系得：两者求和得到载体在地理系相对于惯性系的角速度

其中K表示载体偏北向的航向，表示纬度，R表示地球半径。

5.根据权利要求1所述的一种基于非线性跟踪微分器的杆臂误差解算方法，其特征在于：在步骤(4)中，通过载体在地理系相对于惯性系的角速度转换矩阵的作用，可得到地理系相对惯性系的角速度在载体系中的投影其中又根据步骤(3)的SINS陀螺仪采集到得角速度得到载体相对于地理系在载体系的角速度投影其中

6.根据权利要求1所述的一种基于非线性跟踪微分器的杆臂误差解算方法，其特征在于： 在步骤(5)中，通过确定SINS和卫星导航系统\接收机分别在载体系中的位置坐标矢量为和差为r^b，记r^b＝[r_x r_y r_z]^T,r_x，r_y，r_z分别表示卫星导航系统接收机和SINS位置差在载体坐标系三个坐标轴方向上的投影。

7.根据权利要求1所述的一种基于非线性跟踪微分器的杆臂误差解算方法，其特征在于：在步骤(6)中，根据两系统的位置差r^b，对r^b进行导航解算且微分可得在地理系下中的到距离误差Δl、速度误差Δv和加速度误差Δa,

其中，即是步骤(5)中得到的，表示的斜对称矩阵，若记 则

是的微分。

8.根据权利要求1所述的一种基于非线性跟踪微分器的杆臂误差解算方法，其特征在于：在步骤(7)中，利用跟踪微分器跟踪提取并微分信号的功能，让非线性跟踪微分器作用于 经过滤波后可以得到精度较高的信息，然后进行导航解算。