法律状态公告日
法律状态信息
法律状态
2022-10-14
未缴年费专利权终止 IPC(主分类):G01S19/37 专利号:ZL2011103336190 申请日:20111028 授权公告日:20140903
专利权的终止
2014-09-03
授权
授权
2012-09-26
实质审查的生效 IPC(主分类):G01S19/37 申请日:20111028
实质审查的生效
2012-07-25
公开
公开
技术领域
本发明涉及卫星导航接收机位置参数解算,尤其是涉及一种基于模糊逻辑 的自适应强跟踪UKF定位滤波算法。
背景技术
卫星导航技术的飞速发展已逐渐取代了无线电导航、天文导航等传统导航 技术,而成为一种普遍采用的导航定位技术,并在精度、实时性、全天候等方 面取得了长足进步。目前,世界上多个国家和地区已经建立或者正在建设全球 卫星定位导航系统,具体包括美国建立的GPS系统、俄罗斯建立的GLONASS 系统、欧盟正在建设的“伽利略”系统和中国正在建设的“北斗二代”系统。导航 系统中的定位解算模块是根据卫星信号的量测信息采用相应算法对用户接收机 位置、速度和时钟参数进行解算,是卫星导航系统实现的关键技术之一。
在卫星导航接收机中,导航定位解算是用户接收机定位最为关键的一个环 节,采用定位算法的优劣直接导致用户定位精度的好坏。传统接收机用户定位 的算法选择为最小二乘,具体实现步骤:(1)获取跟踪环路输出伪距信息 ρ=[ρ1,ρ2LρN]T,其中N代表跟踪通道输出伪距数目;(2)用户接收机初始位 置估计X0=[x0,y0,z0]T;3)量测方程线性化,即量测方程
最小二乘算法应用于运动状态估计中,其算法简单,使用与对常值向量或 随机向量的估计,并且在对被估计量和量测误差缺乏了解的情况下仍能使用, 但是由于缺乏对系统动力学状态的考虑,估计精度并不很高,特别是对于非静 止运动状态,存在较大的估计误差。因此,有些研究学者提出了卡尔曼滤波算 法,用以加强系统动力学的估计,以提高状态量的估计精度。
用户接收机通常采用最小二乘算法(参见Fundamentals of Global Positioning System Receivers-A Software Approach,SENOND EDIRION, Canada,James B and Yen T,2005.)和卡尔曼Kalman滤波算法(参见Jwo D J and Wang S H,Adaptive Fuzzy Strong Tracking Extended Kalman Filtering for GPS Navigation,IEEE SENSORS JOURNAL.VOL.7,NO.5,MAY 2007.和Liu J and Lu M Q,An Adaptive UKF Filtering Algorithm for GPS Position Estimation,IEEE XPLORE Wireless Communications,Networking and Mobile Computing.2009. WiCom 09.5th International Conference on Issue Data:24-26Sept.2009.)进行用户 接收机位置的解算。
最小二乘算法虽然算法简单,但由于缺乏对系统动力学状态的考虑,估计 精度并不很高,特别是对于非静止运动状态,存在较大的估计误差。而Kalman 滤波算法虽能够解决定位解算中的非线性问题,但是对于先验信息要求极为严 格,同时定位精度受到载体动态性的限制。
发明内容
本发明为克服现有技术的不足,提出一种基于模糊逻辑的自适应强跟踪 UKF定位滤波算法,该方法在传统UKF算法基础上,将自适应强跟踪算法和 模糊逻辑系统结合,大大地提高了卫星导航系统定位算法的性能。
根据本发明的一个方面,提供一种基于模糊逻辑的自适应强跟踪UKF定位 滤波算法,包括以下步骤:
(1)建立滤波模型;
(2)设定滤波器初始参数;
(3)运用UKF滤波算法对状态量进行滤波;
(4)运用模糊逻辑系统求解强跟踪自适应算法中的软化因子;
(5)强跟踪自适应算法中自适应因子求解;
(6)历元时刻递加1,读取下一时刻观测,返回步骤(4),直至结束。
进一步地,步骤(1)中所述滤波模型包括:
针对低动态环境,根据接收机的位置、速度、时钟钟差和频差建立的常速 CV模型;针对高动态环境,根据接收机的位置、速度、加速度、时钟钟差和 频差建立的常加速度CA模型。
进一步地,步骤(2)中所述设定滤波器初始参数包括:
设置系统噪声矩阵Q(t)和观测噪声矩阵R;
设置状态量初始估计X0和初始状态估计误差协方差P0;
滤波时刻k清零,将自适应因子赋值1,即自适应因子λi,k=1。
进一步地,步骤(3)中所述运用UKF滤波算法对状态量X进行滤波的步 骤包括:
初始状态采样:初始状态量采样,得到初始状态sigma样本点及对应权重 因子;
一步预测状态采样:根据所获得的初始状态sigma样本点,对一步预测状 态和一步预测状态协方差矩阵进行估计;
一步预测状态采样二次采样:对获得的一步预测状态及一步预测状态协方 差矩阵进行二次采样,得到二次采样的一步预测状态sigma样本点及对应权重 因子;
输出一步预测:根据一步预测状态的二次采样输出观测量一步预测,之后 进行观测量自协方差矩阵、观测量和状态量之间互协方差矩阵估计;
卡尔曼增益求解:通过获得的观测量自协方差矩阵和观测量与状态量的互 协方差矩阵,求解卡尔曼增益矩阵,实现状态量估计更新和状态量估计协方差 更新。
进一步地,步骤(4)中所述运用模糊逻辑系统求解强跟踪自适应算法中的 软化因子的步骤包括:
获得滤波信息向量vk;
模糊逻辑系统输入:通过滤波信息向量vk,求解模糊逻辑系统输入ri,其中 模糊逻辑系统输入ri为滤波信息向量vk单步协方差矩阵的迹与单步理论信息方 差阵的迹之比;
模糊逻辑系统求解:根据模糊逻辑系统输入ri的变化规律,进行模糊逻辑系 统的模糊化过程,设定模糊规则,进行模糊推理,去模糊化过程,实现软化因 子输出。
所述模糊化过程中输入参量隶属度函数包括三角形函数,所述模型系统规 则包括一阶T-S模型、IF-THEN形式。
进一步地,步骤(5)中所述强跟踪自适应算法中自适应因子λi,k求解的步 骤包括:
由滤波信息向量vk对滤波信息真实协方差Vk进行估计求解;
求解包含滤波信息协方差Vk和观测噪声估计的自适应矩阵Nk;
由自适应矩阵Nk和信息理论协方差矩阵Mk求解自适应因子中间参数Ci,k;
通过对中间参数Ci,k进行判定来对自适应因子λi,k赋值。
本发明与现有技术相比的优点在于:
(1)与传统的UKF算法相比,对一步预测状态进行了二次采样过程,减 小了对于随机变量通过非线性系统后输出变量统计特性的估计误差,在一定程 度上提高了状态量估计的精度。
(2)将强跟踪算法与UKF算法相结合,克服了传统UKF滤波算法容易受 初始值和模型误差的缺点。
(3)强跟踪算法中的软化因子估计采用了模糊逻辑推理系统进行估计,用 单次滤波信息协方差的迹与理论信息协方差的迹之比作为模糊逻辑系统的输 入,时刻监测滤波器的工作状况,实时调整软化因子。
(4)自适应参数求解过程中,对于信息实际协方差矩阵估计时,采用了一 种新的递推估计算法,更加直接的反应器载体实际的运动状态,同时也减小了 计算量。
附图说明
图1是现行GPS定位最小二乘算法的流程图;
图2是现行UKF滤波算法的流程图;
图3是本发明实施例提供的自适应强跟踪UKF定位滤波算法的流程图;
图4是本发明实施例提供的强跟踪算法中软化因子的求解流程图;
图5是本发明实施例提供的强跟踪算法中自适应因子的求解流程图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白,以下结合附图,对 本发明进行进一步详细说明。应当理解,此处所描述的具体实施例仅仅用以解 释本发明,并不用于限定本发明。
本发明中处理的数据是卫星导航系统跟踪模块输出的测量伪距信息,同时 还包括对应跟踪时刻卫星的位置和速度参数。用户接收机的定位是在地心地固 ECEF坐标系下进行的,首先根据接收机通道卫星信号跟踪测量得到的伪距信 息,对用户接收机的位置参数和时钟钟差参数进行解算。当通道跟踪有效卫星 超过4颗,可用于接收机的实际定位解算。
下面结合附图,对本发明详细说明如下:
参见图3,该图示出了本发明实施例提供的自适应强跟踪UKF定位滤波算 法的详细流程;
S1:建立滤波模型:
在建立滤波模型之前,本发明须要获取卫星测量伪距信息,以及对应跟踪 时刻卫星在地心地固ECEF坐标系下的位置和速度参数;
首先,由卫星信号模拟器或者软件接收机处理实际数据得到卫星的测量伪 距信息,一般情况下伪距信息获得频率为1Hz,同时得到对应跟踪时刻卫星在 地心地固ECEF坐标系下的位置和速度参数。
当采用卫星信号模拟器生成卫星的测量伪距信息时(通常采用Renix文件还 原伪距生成的方式),须要首先确定使用历书的周数,设置仿真间隔和仿真总时 间(其中可以包含相应选星算法,保证定位精度)。
由于卫星信号模拟器生成数据时,只模拟了时钟误差在生成伪距中,因此 在生成的伪距中添加适量的白噪声,视为伪距中的环境噪声。
如果处理的数据是实际数据,经过软件接收机处理后,即可得到相应通道 卫星的伪距信息和卫星的位置、速度参数信息。
然后,根据用户接收机(载体)的实际运动规律选取状态估计量和系统量 测量,建立滤波模型;建立滤波模型包括建立以下模型:
建立CV模型:
对于载体运动的低动态环境,使用接收机的位置、速度和时钟参数作为状 态量X=[x,y,z,vx,vy,vz,td,fd]TECEF,其中x,y,z和vx,vy,vz分别表示ECEF坐标系下 三维的位置与速度分量;td,fd分别表示本地时钟的钟差和钟漂分量。
根据用户接收机的运动规律,建立CV模型,其中状态转移方程,如公式(1) 所示。
其中,
X(t)=[x(t)y(t)z(t)td]T (2)
上式中分别表示载体运动在x,y,z方向上和时钟频率的;κ为载体晶振漂移 系数,W(t)协方差阵
建立CA模型:
对于载体运动的高动态环境,使用接收机的位置、速度、加速度和时钟参 数作为状态量其中ax,ay,az表示三维加速度 分量。
根据用户接收机的运动规律,建立CA模型。其中状态转移方程参照公式 (1)建立,其中添加状态参量
根据选取的状态估计量,建立系统量测模型,其中用户接收机与第i颗卫 星之间的测量伪距ρi如公式(5)所示。
其中,Xi,Yi,Zi和X,Y,Z分别表示第i颗卫星和用户接收机的三维位置参量, C表示电磁波在真空中的传播速度,vi表示第i颗卫星测量伪距量测噪声。
S2:滤波器初始参数设定;
所设定的滤波器初始参数包括:系统噪声矩阵Q(t)和观测噪声矩阵R、状态 量初始估计X0和初始状态估计误差协方差P0、滤波时刻k清零,将自适应因子 赋值为1。
设置系统噪声矩阵Q(t)和观测噪声矩阵R:根据选取的状态估计量和系统量 测量建立的状态模型和观测模型,对载体实际运动规律和观测噪声强度进行先 验估计,设置系统噪声矩阵Q(t)和观测噪声矩阵R。
其中,系统噪声矩阵设置,如步骤(2)中推导,分别估计不同坐标轴方向 上的驱动噪声强度σ2。观测噪声矩阵R的设置方法依据不同卫星跟踪环路相互 独立,观测噪声彼此不相关,既而可得其观测噪声矩阵R=diag{R1,R2,L,RN},其 中表示第N颗卫星测量伪距噪声的强度。
设置状态量初始估计X0和初始状态估计误差协方差P0;本算法中用最小二 乘算法估计滤波状态初始值。
滤波时刻k清零,将自适应因子赋值为1,即λi,k=1。
S3:运用UKF滤波算法对状态量X滤波;
301:初始状态采样;
在计数器count=0时,进行初始状态采样,得到初始状态sigma样本点及 权重因子;
通过初始状态估计X0和初始状态估计协方差矩阵P0,根据对称采样策略, 进行初始状态sigma样本点采样,得到初始状态sigma样本点及相应权重因子 如公式(6)和(7)所示。
其中,状态量维数L,采样维数2L+1;λ=α2(L+κ1)-L表示采样尺度参数; α表示样本点χi在采样附近的遍布范围;κ1通常设置为0或者3-L;对于状 态量高斯分布
302:一步预测状态采样;
根据所获得的初始状态sigma样本点,对一步预测状态及一步预测状态协 方差矩阵进行估计;具体形式如公式(8)、(9)和(10)所示。
χk|k-1=f(χk-1) (8)
303:一步预测状态二次采样;
对上述步骤获得的一步预测状态和一步预测协方差矩阵进行二次采样,得 到二次采样的一步预测状态sigma样本点及权重因子;
利用最优采样理论,根据对称采样策略对一步预测状态和一步预测协 方差Pk|k-1进行二次采样,再次得到一步预测状态sigma样本点及对应权重因子, 具体公式如(11)和(12)所示。
其中,Sk|k-1表示矩阵Pk|k-1的平方根矩阵,χk|k-1的每列分别表示一个采样向量 点χi,k|k-1。
304:输出一步预测;
根据一步预测状态的二次采样输出观测量一步预测,之后进行观测量自协 方差矩阵、观测量和状态量之间互协方差矩阵估计;
由一步预测状态的二次采样,输出观测量一步预测,具体公式如公式(13) 和(14)所示。
ζk|k-1i=hk(χk|k-1i)i=0,1,...,2L (13)
其中,y=h(x)表示观测量与状态量之间的函数关系。
输出观测量一步预测后,对观测量自协方差矩阵和观测量与状态量的互协 方差矩阵进行估计,具体如公式(15)、(16)所示。
所述观测量自协方差矩阵作为滤波信息理论协方差矩阵Mk。
305:卡尔曼增益求解;
通过获得的观测量自协方差矩阵和观测量与状态量的互协方差矩阵,求解 卡尔曼Kalman增益矩阵K,从而完成状态量的更新和状态量估计协方差的更 新,具体如公式(17)~(19)所示。
S4:采用模糊逻辑系统求解强跟踪算法中的软化因子epsilon;模糊逻辑系 统是以模糊集合为基础,最早提出用于系统控制,从而解决系统建模不准确带 来的滤波器发散问题。一个完成的模糊逻辑系统由模糊化、模糊规则、模糊推 理和去模糊化四个部分构成(参见San-Tong Zhang and Xue-Ye Wei.Fuzzy Adaptive Kalman Filtering for DR/GPS IEEE.Proceedings of the second International Conference on Machine Learning and Cybemetics,xi’an,2-5Nov. 2003.b)。
强跟踪算法中的软化因子ε用于调节状态估计精度和状态跟踪能力。当接 收机运动剧烈时,ε值应该减小,以减弱状态模型不准确导致的滤波器不稳定; 反之当接收机运动平稳时,应适当增大ε,用于提高状态量的估计精度。参见 图4所示,其包如下步骤:
401:获得滤波量测信息向量;
一次滤波更新结束后,获得滤波量测信息向量vk;
402:模糊逻辑系统输入;
通过滤波量测信息向量vk,求解模糊逻辑系统输入ri;
其中,滤波量测信息向量vk单步协方差矩阵的迹与单步理论信息方差阵的 迹之比ri作为模糊逻辑系统的输入,用以衡量i时刻滤波器的工作稳定程度,如 公式(20)所示。当ri≥1时,可认为滤波器开始发散;反之ri<1时,滤波器 工作正常。
403:模糊逻辑系统求解;
根据模糊逻辑系统输入ri的变化规律,进行模糊逻辑系统的模糊化过程,设 定模糊规则,进行模糊推理,去模糊化过程,实现软化因子输出。
所述模糊化过程包括划分模糊几何、建立模型系统输入参量隶属度函数, 在模糊逻辑系统中,输入参量隶属度函数选取为三角形函数,但不局限于三角 形函数,评价模糊子集合划分为{A,B,C}={good,normal,bad}。
模糊规则采用一阶T-S模型、IF-THEN形式但不局限于此,规则如下所示:
①IFri∈good THENε等于ri+15;
②IFri∈normal THENε等于2*ri+4;
③IFri∈bad THENε等于1。
所述去模糊化过程或者模糊化输出由相应的A,B,C三个模糊子集合输出 进行加权输出。假设Pi,k,k=1,2,3为样本ri输入FLAS系统时集合k的隶属度, yi,k,k=1,2,3为样本ri输入时分属集合k的输出,可得模糊逻辑系统总的输出如公 式(21)所示。
S5:强跟踪自适应算法中自适应因子λi,k求解;
参见图5所示,强跟踪自适应算法中自适应因子λi,k求解包括以下步骤:
501:由获得的滤波信息向量vk,对滤波信息真实协方差进行估计;
在一次滤波更新结束后,在计数器k=1时,获得滤波信息向量vk;
滤波信息真实协方差矩阵估计采用了新型的递推算法,其克服了传统方法 求解过程中对历史信息取平均,而直接采用了当前历元信息,更能敏感地反应 当前观测历元动力学模型误差的现状;具体形式如公式(22)所示。
所述强跟踪算法中滤波信息向量真实协方差的估计求解步骤如下:
一次滤波更新结束后,获得滤波信息向量vk;
初始化滤波信息向量协方差,当计数器count=0时,当计数器 count≥1时,
502:通过滤波信息真实协方差求解包含信息协方差Vk和观测噪声估计的自 适应矩阵Nk,如公式(23)所示;
Nk=ηVk-εRk (23)
503:由自适应矩阵Nk和信息理论协方差矩阵Mk求解自适应强跟踪因子中 间参数Ci,k,如公式(24)所示;其中,信息理论协方差矩阵Mk即观测量自协 方差矩阵,见公式(15);
504:通过对中间参数Ci,k判定,对自适应因子λi,k赋值,如公式(25)所示。
S6:历元时刻递1,count=count+1时,读取下一时刻观测伪距,返回步骤(3) 继续执行,直至结束。
以上公开的仅为本发明的具体实施例,但本发明并非局限于此,对于本领 域的普通技术人员来说,在不脱离本发明原理的前提下,做出的变形应视为属 于本发明保护范围。
机译: 基于模糊逻辑的视频多目标跟踪方法及装置
机译: 基于控制器的模糊逻辑太阳跟踪系统
机译: 基于模糊逻辑的联系跟踪模型评估系统及方法