一种基于最小偏度单形采样的自适应UKF组合导航方法与流程

本发明属于导航技术领域，具体涉及一种基于最小偏度单形采样的自适应ukf组合导航方法。

背景技术：

组合导航系统通过对两种或多种导航系统测量或输出信息进行综合处理从而使总系统的性能指标优于任一子导航系统。惯性导航和卫星导航都有各自的优缺点，如果采用某种手段将它们结合构成组合导航系统，那么既可以克服惯性导航误差积累的问题，又可以弥补gps导航信号受遮蔽的问题，其精度和可靠性将得到极大的提高。在组合导航中，最关键的就是数据融合，而卡尔曼滤波器是一种基于最小均方误差准则的使用最为广泛的滤波器。在实际应用中，导航系统一般都是非线性的，因此研究组合导航系统也就集中在研究非线性卡尔曼滤波在组合导航系统中的应用。

为解决非线性系统的估值问题，目前为止使用最广泛的滤波方法就是扩展卡尔曼滤波(extendkalmanfilter，ekf)，它是在kalman滤波的基础上对非线性的状态方程和观测方程进行taylor展开，只保留一阶项，舍去二阶及以上项，得到的一种近似非线性滤波方法。但当系统的非线性程度较高时，高阶项较大，这种简单的线性化处理会引入较大的高阶误差，甚至使系统发散。ekf最关键的就是jacobian矩阵的求取，这使它只能应用于具有显式表达且连续的系统。目前国内外学者针对也ekf提出了许多改进策略如：高阶截断ekf、迭代ekf等，但都没有从本质上克服这些缺点，虽然提高了ekf估计的精度，但这也增加了计算量，得不偿失。为避免以上提到的问题，学者julier和uhlmann于2002年基于“近似概率密度要比近似非线性函数容易的多”的观点，提出了一种新的基于确定性采样点计算的滤波方法，由于该算法是建立在unscented变换(unscentedtransformation，ut)的基础上的，所以称之为无迹卡尔曼滤波器(unscentedkalmanfilter，ukf)。相较于ekf，它不需要计算非线性函数的jacobian或者hessians矩阵。在ut变换中，最关键的就是确定sigma点的采样策略。sigma点的采样策略与算法的计算量和精度息息相关，一般来说，采样点个数越多，算法计算量越大。目前应用最广泛的采样策略是对称采样，但其采样点的个数会随着系统状态的维数比例增大，计算量也随之增加，较大的计算量会使系统无法满足实时性的要求。此外，随着系统维数的增大，部分sigma采样点会距中心点较远，致使相应sigma点的参考价值较小，这便是采样的非局部效应，它会导致滤波精度下降。

技术实现要素：

本发明目的在于提供设计合理、精度高且实时性好的基于无迹卡尔曼滤波组合的一种基于最小偏度单形采样的自适应ukf组合导航方法。

本发明的目的通过如下技术方案来实现：

一种基于最小偏度单形采样的自适应ukf组合导航方法，包括以下步骤：

(1)根据sins和gps的误差特性，建立起sins/gps紧组合误差模型，即状态方程和

量测方程。

步骤(1)具体包括：

(1.1)建立状态方程：

(1.1.1)选取sins系统的误差量为：东北天向的平台误差角速度误差δve,δvn,δvu，经度、纬度、高度的位置误差δl,δλ,δh，沿载体坐标系x,y,z轴的陀螺仪漂移εbx,εby,εbz、陀螺仪的一阶马氏过程εrx,εry,εrz，加速度计零偏

(1.1.2)选取gps系统的误差量为：与时钟频率误差等效的速度误差量δtru以及与时钟误差等效的距离误差量δtu；

(1.1.3)sins系统的误差量和gps系统的误差量为状态量列写状态方程得：

即

式中的状态向量x(t)为20维，是分别由xi(t)的18维，和xg(t)的2维合并成的，下标i表示sins，下标g表示gps，

其中，fi(t)、fg(t)分别为xi(t)，xg(t)的系统矩阵，bi(t)、bg(t)分别为wi(t)，wg(t)的噪声驱动阵，

其中，ωgx,ωgy,ωgz,ωrx,ωry,ωrz,ωax,ωay,ωaz分别为陀螺仪漂移、陀螺仪的一阶马氏过程以及加速度计在载体坐标系下的白噪声。

(1.2)建立量测方程：

(1.2.1)紧组合系统的伪距量测方程为：

zρ(t)＝hρ(t)x(t)+vρ(t)

式中，zp(t)为伪距量测量，zρ(t)＝[δρ1δρ2…δρn]^t，

(1.2.2)紧组合系统的伪距率量测方程为：

式中，

(1.2.3)合并伪距量测方程和伪距率量测方程并进行维数扩展，得到sins/gps导航系统紧组合模型的量测方程：

(2)选取初始滤波值并设置初始比例因子α。

步骤(2)具体包括：

滤波器的初值取为：

其中，x0为状态变量的初值，为x0的均值，p0为状态变量的初始误差协方差矩阵；

根据量测噪声(m维)和系统噪声(q维)，对n维的系统状态矩阵和协方差矩阵进行扩维，并分别设初始状态向量和初始协方差矩阵为：

其中，q是系统噪声的协方差阵，r是观测噪声的协方差阵；

比例因子α用来控制sigma点到中心点的距离，在[10^-4，1]区间上取值。

(3)采用最小偏度单形采样的方法计算sigma样本点和相应的权值并更新比例因子α＝αk|k-1。

步骤(3)具体包括：

(3.1)最小偏度单形采样：

sigma点集为：

其中，设为j维空间采样点集的第i个sigma点，不失一般性，假设pxx＝i，选择0≤w0≤1；

sigma采样点一阶、二阶权系数分别为：

其中α为比例因子，参数β用来描述x的先验分布信息，β≥0是一个非负的权函数，上标m为均值，c为协方差，下标为第几个采样点，表示pxx的平方根矩阵，可用cholesky分解取下三角计算得到；

(3.2)更新比例因子：

具体算法为：

(3.2.1)由xk|k和pkk按预先确定的最小偏度单形采样策略生成sigma点集和权值{χi,wj}；

(3.2.2)计算

(3.2.3)令

其中，di为比例修正后第i个sigma点到中心点x的距离，即l为最小偏度单形采样点的个数。

(4)将计算得到的sigma采样点通过状态函数求取sigma点集的一步预测并计算系统状态量的一步预测和协方差矩阵p(k|k-1)。

步骤(4)具体包括：

计算得到的sigma采样点通过状态函数求取sigma点集的一步预测

预测k时刻的系统状态变量及协方差：

根据一步预测值再次进行ut变换，生成新的sigma点集，代入观测方程得：

预测系统的量测均值及协方差：

计算卡尔曼增益阵：

(5)计算系统的状态更新和协方差更新p(k|k)。

步骤(5)具体包括：

计算系统的状态更新和协方差更新：

本发明的有益效果在于：

一、本发明技术相比对称采样的ukf能够缩短滤波时间，提高系统的实时性；

二、本发明相比最小偏度单形采样的ukf滤波精度高；

三、本发明的技术可以适用于任何非线性系统的滤波模型。

附图说明

图1为本发明sins/gps紧组合结构框图；

图2为本发明asukf方法流程图；

图3为本发明方法与其他方法的定位误差对比图；

图4为本发明方法与其他方法的速度误差对比图。

具体实施方式

下面结合附图对本发明的具体实施方式作进一步说明：

基于单形采样的自适应ukf组合导航新方法，其具体步骤如下：

步骤一、根据sins和gps的误差特性，建立起sins/gps紧组合误差模型，即状态方程和量测方程。

所述步骤一中，紧组合是一种sins系统和gps系统相互辅助的组合模式，组合水平较松组合高，实现也更复杂。其结构如附图1。紧组合是在伪距、伪距率级别上的耦合，状态方程是由sins和gps的误差状态方程合并而成。我们选取东北天地理坐标系(e，n，u分别表示东、北、天向)作为导航坐标系，并在此坐标系下推导sins系统的误差方程。

(1)状态方程的建立

选取sins系统的误差量为：东北天向的平台误差角速度误差δve,δvn,δvu，经度、纬度、高度的位置误差δl,δλ,δh，沿载体坐标系x,y,z轴的陀螺仪漂移εbx,εby,εbz、陀螺仪的一阶马氏过程εrx,εry,εrz，加速度计零偏而gps系统的误差状态选取了与gps系统的时钟误差相关的两个误差量：与时钟频率误差等效的速度误差量δtru以及与时钟误差等效的距离误差量δtu。将sins系统的误差量和gps系统的误差量为状态量列写状态方程得：

即

式中的状态向量x(t)为20维，是分别由xi(t)的18维，和xg(t)的2维合并成的。下标i表示sins，下标g表示gps。

其中，fi(t)、fg(t)分别为xi(t)，xg(t)的系统矩阵。bi(t)、bg(t)分别为wi(t)，wg(t)的噪声驱动阵。

其中，ωgx,ωgy,ωgz,ωrx,ωry,ωrz,ωax,ωay,ωaz分别为陀螺仪漂移、陀螺仪的一阶马氏过程以及加速度计在载体坐标系下的白噪声。

(2)量测方程的建立

紧组合的量测量并不是各子导航系统的输出信息简单的整合，它是将sins系统输出的位置和速度等信息结合得到的卫星星历信息，计算得到运载体相对于各卫星的伪距和伪距率信息，再减去gps接收设备输出的伪距和伪距率等原始信息，并把这个差值作为紧组合系统的量测值。与松组合相比，因为使用了gps系统的原始信息，增大了系统的能控能观性，使系统的可靠性更高。

紧组合系统的伪距量测方程为：

zρ(t)＝hρ(t)x(t)+vρ(t)(3)

式(3)中，zp(t)为伪距量测量，zρ(t)＝[δρ1δρ2…δρn]^t，

紧组合系统的伪距率量测方程为：

式(4)中

合并式(3)和式(4)并进行维数扩展，至此便得到了sins/gps导航系统紧组合模型的量测方程：

以上建立的sins/gps组合导航系统紧组合模型是连续模型，考虑到计算机的实现方式，必须先对连续系统做离散化处理才能应用卡尔曼滤波器对其进行估计虑波。

步骤二、选取初始滤波值并设置初始比例因子α。令和其中，x0为状态变量的初值，为x0的均值。p0为状态变量的初始误差协方差矩阵。

所述步骤二滤波器的初值取为：再根据量测噪声(m维)和系统噪声(q维)，对n维的系统状态矩阵和协方差矩阵进行扩维，并分别设初始状态向量和初始协方差矩阵为：

其中，q是系统噪声的协方差阵，r是观测噪声的协方差阵。

比例因子α用来控制sigma点到中心点的距离，它是一个较小的正数，可在[10^-4，1]区间上取值。

步骤三、采用最小偏度单形采样的方法计算sigma样本点和相应的权值并更新比例因子α＝αk|k-1。

所述步骤三中，ukf以无迹变换(unscentedtransformation，ut变换)为基础，利用确定性的sigma采样点去逼近原非线性函数的状态分布。传统ukf的ut变换采用对称采样，需要计算2n+1(n为系统的维数)个sigma点。而最小偏度单形采样只需计算n+1(考虑中心点为n+2)个采样点，可以减小计算量，减少滤波时间。又sigma点到中心点x的距离会随系统状态维数的增加而增大，这样会使高维系统在ut变换过程中产生采样的非局部效应，致使高阶误差项增大。为此，采用自适应比例因子修正算法，可有效降低采样的非局部效应问题，而且还能减小高阶误差项的影响，此方法还满足协方差矩阵的半正定性，适用于多种基本的采样策略。本发明方法流程图如附图2所示。

(1)最小偏度单形采样：

设为j维空间采样点集的第i个sigma点，不失一般性，假设pxx＝i，选择0≤w0≤1。sigma点集为：

sigma采样点一阶、二阶权系数分别为：

其中α为比例因子，系统的非线性越强α值越小。通过改变α的值调节sigma采样点到的距离，以减小ut变换的非局部效应造成的影响。参数β用来描述x的先验分布信息，β≥0是一个非负的权函数。对于高斯分布的系统，β＝2时为最优。式中上标m为均值，c为协方差，下标为第几个采样点。表示pxx的平方根矩阵，可用cholesky分解取下三角计算得到。如果pxx＝uu^t，则显然pxx必须是正定的。

(2)更新比例因子：

设di为比例修正后第i个sigma点到中心点x的距离，即l为最小偏度单形采样点的个数。具体算法为：

a)由xk|k和pk|k按预先确定的最小偏度单形采样策略生成sigma点集和权值{χi,wj}；

b)计算

c)令

在每次ut变换的过程中，用上面计算得到的自适应比例因子α取代常规的固定参数的比例因子对采样策略进行修正，便得到了自适应的比例修正ut变换。

步骤四、预测。将计算得到的sigma采样点通过状态函数求取sigma点集的一步预测并计算系统状态量的一步预测和协方差矩阵p(k|k-1)。返回步骤三，用和p(k|k-1)重新生成采样点集及权值。将得到的预测的sigma点集带入观测方程，得到观测量。计算自协方差矩阵pzz(k|k-1)和互协方差矩阵pxz(k|k-1)。

步骤四中将计算得到的sigma采样点通过状态函数求取sigma点集的一步预测

预测k时刻的系统状态变量及协方差：

根据一步预测值再次进行ut变换，生成新的sigma点集，代入观测方程得：

预测系统的量测均值及协方差：

计算卡尔曼增益阵：

步骤五、更新。计算系统的状态更新和协方差更新p(k|k)。

步骤五计算系统的状态更新和协方差更新。

实施例：

仿真所使用的轨迹是利用matlab系统导航信息的输出频率为100hz，gps的输出频率为10hz，组合导航系统的数据输出频率与sins系统同步。设陀螺仪常值漂移0.1°/h，白噪声均方差0.01°/h；加速度计常值零偏1×10^-4g，白噪声均方差5×10^-5g；gps伪距白噪声10m，伪距率白噪声0.5m/s。

分别采用对称采样ukf、单形采样ukf(sukf)和本发明asukf方法对sins/gps导航系统的紧组合模型进行matlab仿真，并对实验结果进行对比分析。实验结果如附图3和图4所示，图中黑色“—”表示ukf滤波估计误差，红色“--”表示ukf滤波估计误差，蓝色“-.”表示ukf滤波估计误差。

重复进行50次mc仿真实验，并计算三种ukf算法各类误差信息的rmse，统计这三种ukf算法的平均运行时间，结果如表1所示：

表1紧组合三种ukf方法仿真效果对比

由图3和图4的误差曲线图及表1的仿真效果统计信息可得：对比这三种滤波方法的误差曲线可以得出，单形采样ukf(记为sukf)滤波精度比对称采样ukf(记为ukf)滤波精度稍差，但用时比对称采样ukf少了大概有30％。本发明方法虽然在ut变换的过程中，计算量较固定比例参数的单形采样ut变换策略只是略微增大，大概增大了5％，但其精度却有很大的提高，尤其是其速度精度提高了将近20％，位置精度也有10％多的提升。而本发明asukf方法与常规的对称采样ukf滤波相比，在估计精度大体相同的情况下，使计算量却减小了大约25％。综合来看，本发明方法在不损失滤波精度的条件下，能够减少滤波时间，提高系统的实时性。且本发明方法方法简单，易于实现，具有较强的应用价值和推广前景。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。