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

说明书

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

技术领域

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

背景技术

农机的导航控制技术是实现农业现代化的基本要求，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为增益矩阵；