CN108646277A - 基于抗差自适应与扩展卡尔曼滤波融合的北斗导航方法 - Google Patents

Abstract

本发明的基于抗差自适应与扩展卡尔曼滤波融合的北斗导航方法包括a).获取定位数据；b).建立含有粗差的状态观测方程；c).建立状态估计；d).求取观测值方差阵；e).获取自适应滤波解；f).估计误差方差矩阵；g).抗差自适应滤。本发明的有益效果是：本发明的基于抗差自适应与扩展卡尔曼滤波融合的北斗导航方法，明显优于最小二乘法、卡尔曼滤波法，相对于扩展卡尔曼滤波法也有一定程度的提高，误差精度在5cm左右，满足农机自动驾驶的精度要求，可结合高性能控制器，有效控制农机的转向和速度，实现农机自主定位、自动航迹跟踪、自动行驶的目标。

Description

基于抗差自适应与扩展卡尔曼滤波融合的北斗导航方法

技术领域

本发明涉及一种北斗动态导航方法，更具体的说，尤其涉及一种基于抗差自适应与扩展卡尔曼滤波融合的北斗导航方法。

背景技术

农机的导航控制技术是实现农业现代化的基本要求，PVT信息(位置信息、速度信息、时间信息)的解算是农机北斗导航定位解算中最为核心的部分。国内外对于农机导航定位都有一定的研究，美国伊利诺伊大学基于RTK-GPS技术研发了车辆自动导航控制系统；中国的罗锡文、宋正河等专家对基于DGPS的农机自动驾驶技术的硬件和软件系统进行了研究，建立农机作业时的运动学和动力学模型，利用Kalman滤波等算法提高定位精确度。目前，PVT解算最基础的方法就是最小二乘法，但得到的结果不够精确误差很大，而且有些紊乱。相比较最小二乘法，卡尔曼滤波法和扩展卡尔曼滤波法精度明显提高，但还不能满足农机这类精度要求极高的行业，针对粗差的影响提出了一种可有效降低粗差影响的抗差自适应与扩展卡尔曼滤波相融合的算法，着重针对北斗动态导航方法进行深入优化，来满足农机导航的高精度要求。

北斗系统定位原理的主要是根据三球交汇定位原理，这种原理被广泛应用于北斗导航卫星定位系统，如图1所示，给出了北斗系统卫星定位原理示意图，首先设定已得到三个卫星在空间中的位置和待测北斗定位接收机到这三个卫星的距离，再根据上述方法结合伪距定位，这样我们就可以得知，以这三个卫星为中心，以待测定位接收机到卫星的距离为半径的三个球的交点上，然后再根据工程经验非常简单就可以排除另外一个点，这个点是不与地球面重合的。所剩下的那个点就是想要知道的待测北斗定位接收机的真实位置，北斗导航定位系统进行定位解算就是以这个原理为基础。因为每颗卫星都有它的时延，在进行定位解算时，这个也需要作为一个未知变量参与方程组的求解，所以依据惯例，需要多知道一颗卫星的空间位置信息，也就是至少四颗才能进一步完成定位解算。

发明内容

本发明为了克服上述技术问题的缺点，提供了一种基于抗差自适应与扩展卡尔曼滤波融合的北斗导航方法。

本发明的基于抗差自适应与扩展卡尔曼滤波融合的北斗导航方法，通过以下步骤来实现：

a).获取定位数据，农机利用装载的北斗定位模块获取t时间段内的定位数据(A,B,H)，设采样周期为T，样本容量为n；集合A、B、H中的数据均通过以下步骤进行处理；

b).建立含有粗差的状态观测方程，由于获取的定位数据中含有粗差，会使得状态估计受到干扰，建立如公式(1)所示的含有粗差的状态观测方程：

式中，为m维状态矢量Z_k的估计，X_k是系统的n维状态矢量，H_k是m×n维观测矩阵；G_k为粗差干扰矩阵，由元素0和1组成，M_k为粗差向量，V_k是m维系统的观测噪声矢量；含有粗差影响的预测残差为：

式中，是观测噪声矢量V_k的估计，是状态一步预测值X_k,k-1的估计，为k时刻的观测值；

由公式(1)和(2)可得到一个新的滤波状态估计模型：

式中，K_k为增益矩阵；

可见，在有粗差的情况下，状态估计滤波值的大小是通过增益矩阵K_k的改变而改变的，通过调节增益矩阵K_k的大小来减小或消除粗差对状态的影响；

c).建立状态估计，建立如公式(4)所示的状态估计：

状态进一步预测为：

Φ_k,k-1是系统的n×n维状态转移矩阵；增益矩阵K_k通过如下公式进行求取：

上式中：k₀，k₁是抗差参数，k₀一般取2.5～3.5，k₁一般取3.5～4.5，其中：

上式中：i，j分别代表状态向量和观测向量的维数；v_k，r_j，σ_j分别代表观测向量的预测残差、多余观测分量和量测标准差；其中：

d).求取观测值方差阵，利用公式(9)和公式(10)来求取观测值方差阵：

根据所求得到s_j同k₀，k₁的大小比较，利用公式(6)来得到对应K_ij的值，以便于消除粗差对于状态估计的影响；

e).获取自适应滤波解，建立如公式(11)所示的自适应抗差滤波函数：

上述式中：ρ为连续非减凸函数，α_k(0＜α_k≤1)为自适应因子，P_i为X_i的方差，为X_k的方差；在取得极值min后，建立如公式(12)所示的关于状态向量的自适应滤波解：

公式(12)的等价的表示为：

等价增益矩阵可表示为：

f).估计误差方差矩阵可表示为：

采用两段函数来表示自适应因子：

上述式中，c为常量。可以构建模型误差的判别统计量为：

其中tr为表示矩阵的迹；

g).利用抗差自适应滤波法将得到：

集合A、B、H中的数据分别通过上述步骤的处理后，即可得到其当前k时刻的状态估计值并比较估计值与测量值的大小，如果两者之差小于设定阈值，则采用测量值作为k时刻实际值；如果两者之差大于设定阈值，则采用估计值作为k时刻实际值，以避免系统误差、干扰信号对测量结果造成的不利影响。

本发明的有益效果是：本发明的基于抗差自适应与扩展卡尔曼滤波融合的北斗导航方法，明显优于最小二乘法、卡尔曼滤波法，相对于扩展卡尔曼滤波法也有一定程度的提高，误差精度在5cm左右，满足农机自动驾驶的精度要求，可结合高性能控制器，有效控制农机的转向和速度，实现农机自主定位、自动航迹跟踪、自动行驶的目标。

附图说明

图1为北斗系统卫星定位原理示意图；

图2为采用最小二乘法的仿真实验结果；

图3为采用卡尔曼滤波算法的仿真实验结果；

图4为采用本发明的基于抗差自适应与扩展卡尔曼滤波融合算法的仿真实验结果。

具体实施方式

下面结合附图与实施例对本发明作进一步说明。

通过以下步骤来实现：

a).获取定位数据，农机利用装载的北斗定位模块获取t时间段内的定位数据(A,B,H)，设采样周期为T，样本容量为n；集合A、B、H中的数据均通过以下步骤进行处理；

b).建立含有粗差的状态观测方程，由于获取的定位数据中含有粗差，会使得状态估计受到干扰，建立如公式(1)所示的含有粗差的状态观测方程：

式中，为m维状态矢量Z_k的估计，X_k是系统的n维状态矢量，H_k是m×n维观测矩阵；G_k为粗差干扰矩阵，由元素0和1组成，M_k为粗差向量，V_k是m维系统的观测噪声矢量；含有粗差影响的预测残差为：

式中，是观测噪声矢量V_k的估计，是状态一步预测值X_k,k-1的估计，为k时刻的观测值；

由公式(1)和(2)可得到一个新的滤波状态估计模型：

式中，K_k为增益矩阵；

可见，在有粗差的情况下，状态估计滤波值的大小是通过增益矩阵K_k的改变而改变的，通过调节增益矩阵K_k的大小来减小或消除粗差对状态的影响；

c).建立状态估计，建立如公式(4)所示的状态估计：

状态进一步预测为：

Φ_k,k-1是系统的n×n维状态转移矩阵；增益矩阵K_k通过如下公式进行求取：

上式中：k₀，k₁是抗差参数，k₀一般取2.5～3.5，k₁一般取3.5～4.5，其中：

上式中：i，j分别代表状态向量和观测向量的维数；v_k，r_j，σ_j分别代表观测向量的预测残差、多余观测分量和量测标准差；其中：

d).求取观测值方差阵，利用公式(9)和公式(10)来求取观测值方差阵：

根据所求得到s_j同k₀，k₁的大小比较，利用公式(6)来得到对应K_ij的值，以便于消除粗差对于状态估计的影响；

e).获取自适应滤波解，建立如公式(11)所示的自适应抗差滤波函数：

上述式中：ρ为连续非减凸函数，α_k(0＜α_k≤1)为自适应因子，P_i为X_i的方差，为X_k的方差；在取得极值min后，建立如公式(12)所示的关于状态向量的自适应滤波解：

公式(12)的等价的表示为：

等价增益矩阵可表示为：

f).估计误差方差矩阵可表示为：

采用两段函数来表示自适应因子：

上述式中，c为常量。可以构建模型误差的判别统计量为：

其中tr为表示矩阵的迹；

g).利用抗差自适应滤波法将得到：

集合A、B、H中的数据分别通过上述步骤的处理后，即可得到其当前k时刻的状态估计值并比较估计值与测量值的大小，如果两者之差小于设定阈值，则采用测量值作为k时刻实际值；如果两者之差大于设定阈值，则采用估计值作为k时刻实际值，以避免系统误差、干扰信号对测量结果造成的不利影响。

最小二乘法：由于PVT解算模型一般是非线性模型，而最小二乘法通常只适用于线性模型。因此，需要用泰勒级数展开的方法将非线性转化为线性模型^[8]。我们设为用户的粗略定位，用泰勒级数将其转化为线性模型。然后设(x_u,y_u,z_u)为用户的准确定位。用泰勒级数伪距表达式的粗略定位，未知的偏移量(Δx,Δy,Δz)就可以通过已知坐标和线性函数来表示，用如下方程组表示此偏移量：

以如下表达式来表示伪距：

通过接近的位置和时偏估计伪距便可以粗略的求出来：

前文假定由两部分组成用户的真实定位，同理可得，北斗接收机的时偏也看作是由两部分组成的，如下式所示：

其中表示粗略位置，表示粗略时偏，Δx_u表示增量定位，Δt_u表示增量时偏，因此有：

式子(1.5)可以围绕近似点和相关联的接收机时偏的预测值用泰勒级数展开为：

前文描述过，消除非线性项才能使用最小二重迭代法。式(1.6)中只对一阶偏导数进行了保留，而对其他高阶项进行了省略。分别计算如下四个偏导数：

其中：

在式(1.6)中代入式(1.3)和式(1.7)，得到如下：

这样，式(1.2)对于未知数Δx_u，Δy_u，Δz_u，Δt_u的线性化就解决了。将式(1.9)中的变量进行简单的调整，将得到：

将上述复杂式用以下新的未知量来表示：

式(1.11)中的a_xj是在x方向上的由用户的大体定的位置指向卫星j的矢量cos值，a_xj是在y方向上的由用户的大体定的位置指向卫星j的矢量cos值，a_xj是在z方向上的通过用户的大体定的位置指向卫星j的矢量cos值。

因此式(1.10)可以写成：

Δρ_j＝a_xjΔx_u+a_yjΔy_u+a_zjΔz_u-cΔt_u (1.12)

现在有四个未知的变量Δx_u，Δy_u，Δz_u，Δt_u，可以利用四颗卫星的伪距式联立成如下方程组，来求得未知量：

上述的方程可以用如下矩阵来表示：

Δρ＝HΔx (1.14)

其中：

式(1.13)的解为：

ΔX＝H^-1Δρ (1.16)未知量就可以通过以上公式来求解，而用户的真实定位x_u，y_u，z_u，和接收机时偏t_u也可以利用式(1.4)来求解。

在实际进行测量工作时，一般都能搜到四颗以上的卫星，可以通过测量更多的卫星，来提高测量精度，减小测量误差。

卡尔曼滤波法：卡尔曼滤波法(Kalman)也是一种线性的估计算法，这点如同最小二乘法，但相比于前者卡尔曼滤波法在去噪效果和算法效率都有了提升。卡尔曼滤波法是在运行中不断的预测和修正数据，而且不需要对所观测到的数据进行大量的存储，每当测量到新的观测值，它仅仅通过前一个估计值和最新的观测数据来对当前的信号值进行预估，很适合实时数据的处理[10]。卡尔曼滤波法是用状态方程和观测方程来实现的。

不考虑控制作用，设系统的方程为：

上述式中：X_k是系统的n维状态矢量；Z_k是系统的m维状态矢量；W_k-1是p维系统的过程噪声矢量；V_k是m维系统的观测噪声矢量；Φ_k,k-1是系统的n×n维状态转移矩阵；Γ_k,k-1是n×p为噪声输入矩阵；H_k是m×n维观测矩阵。

系统的过程噪声和观测噪声都是零均值的白噪声序列，假定如下^[11]：

上述式中：Q_k是系统的过程噪声W_k的p×p维对称非负定方差矩阵；R_k是系统观测噪声V_k的m×m维对称正定方差矩阵；δ_kj是Kronecker函数：

若各参数均满足上述方程的约束，并且已得到k-1时刻X_k1的最优状态估计则X_k的估计可通过如下卡尔曼滤波方程求解：

状态一步预测：

状态估计：

滤波增益矩阵：

一步预测误差方差矩阵：

估计误差方差矩阵：

式(1.22)进一步写成：

式(1.24)进一步写成：

P_k＝[I-K_kH_k]P_k,k-1 (1.26)

只要选择一个合适的初值和P₀，以保证Kalman滤波算法迭代的收敛性，再根据k时刻的观测值Z_k，就能得到k时刻的最优状态估计值以此来减小误差所带来的影响。

利用MATLAB仿真软件分别对最小二乘法、卡尔曼滤波法和本发明的基于抗差自适应与扩展卡尔曼滤波融合算法进行仿真，其中横坐标为观测数据的采样时间，单位为min，纵坐标为定位误差值，单位为m，仿真结果分别如图2、3、4所示。

可见，最小二乘法的定位结果的绝对误差为9.9cm，采用卡尔曼滤波法的定位结果的绝对误差为7.8cm，采用扩展卡尔曼滤波法的定位结果的绝对误差为7.2cm，采用抗差自适应扩展卡尔曼滤波法的定位结果的绝对误差为5.2cm。

Claims (1)

1.一种基于抗差自适应与扩展卡尔曼滤波融合的北斗导航方法，其特征在于，通过以下步骤来实现：

a).获取定位数据，农机利用装载的北斗定位模块获取t时间段内的定位数据(A,B,H)，设采样周期为T，样本容量为n；集合A、B、H中的数据均通过以下步骤进行处理；

b).建立含有粗差的状态观测方程，由于获取的定位数据中含有粗差，会使得状态估计受到干扰，建立如公式(1)所示的含有粗差的状态观测方程：

式中，为m维状态矢量Z_k的估计，X_k是系统的n维状态矢量，H_k是m×n维观测矩阵；G_k为粗差干扰矩阵，由元素0和1组成，M_k为粗差向量，V_k是m维系统的观测噪声矢量；含有粗差影响的预测残差为：

式中，是观测噪声矢量V_k的估计，是状态一步预测值X_k,k-1的估计，为k时刻的观测值；

由公式(1)和(2)可得到一个新的滤波状态估计模型：

式中，K_k为增益矩阵；

可见，在有粗差的情况下，状态估计滤波值的大小是通过增益矩阵K_k的改变而改变的，通过调节增益矩阵K_k的大小来减小或消除粗差对状态的影响；

c).建立状态估计，建立如公式(4)所示的状态估计：

状态进一步预测为：

Φ_k,k-1是系统的n×n维状态转移矩阵；增益矩阵K_k通过如下公式进行求取：

上式中：k₀，k₁是抗差参数，k₀一般取2.5～3.5，k₁一般取3.5～4.5，其中：

上式中：i，j分别代表状态向量和观测向量的维数；v_k，r_j，σ_j分别代表观测向量的预测残差、多余观测分量和量测标准差；其中：

d).求取观测值方差阵，利用公式(9)和公式(10)来求取观测值方差阵：

根据所求得到s_j同k₀，k₁的大小比较，利用公式(6)来得到对应K_ij的值，以便于消除粗差对于状态估计的影响；

e).获取自适应滤波解，建立如公式(11)所示的自适应抗差滤波函数：

上述式中：ρ为连续非减凸函数，α_k(0＜α_k≤1)为自适应因子，P_i为X_i的方差，为X_k的方差；在取得极值min后，建立如公式(12)所示的关于状态向量的自适应滤波解：

公式(12)的等价的表示为：

等价增益矩阵可表示为：

f).估计误差方差矩阵可表示为：

采用两段函数来表示自适应因子：

上述式中，c为常量。可以构建模型误差的判别统计量为：

其中tr为表示矩阵的迹；

g).利用抗差自适应滤波法将得到：

集合A、B、H中的数据分别通过上述步骤的处理后，即可得到其当前k时刻的状态估计值并比较估计值与测量值的大小，如果两者之差小于设定阈值，则采用测量值作为k时刻实际值；如果两者之差大于设定阈值，则采用估计值作为k时刻实际值，以避免系统误差、干扰信号对测量结果造成的不利影响。