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

摘要

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

主权项

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)可得到一个新的滤波状态估计模型：式中，Kk为增益矩阵；可见，在有粗差的情况下，状态估计滤波值的大小是通过增益矩阵Kk的改变而改变的，通过调节增益矩阵Kk的大小来减小或消除粗差对状态的影响；c).建立状态估计，建立如公式(4)所示的状态估计：状态进一步预测为：Φk,k‑1是系统的n×n维状态转移矩阵；增益矩阵Kk通过如下公式进行求取：上式中：k0，k1是抗差参数，k0一般取2.5～3.5，k1一般取3.5～4.5，其中：上式中：i，j分别代表状态向量和观测向量的维数；vk，rj，σj分别代表观测向量的预测残差、多余观测分量和量测标准差；其中：d).求取观测值方差阵，利用公式(9)和公式(10)来求取观测值方差阵：根据所求得到sj同k0，k1的大小比较，利用公式(6)来得到对应Kij的值，以便于消除粗差对于状态估计的影响；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时刻实际值，以避免系统误差、干扰信号对测量结果造成的不利影响。