CN103322999A - 一种适用于多艇导航的保留历史状态的信息滤波算法 - Google Patents
一种适用于多艇导航的保留历史状态的信息滤波算法 Download PDFInfo
- Publication number
- CN103322999A CN103322999A CN2013101954671A CN201310195467A CN103322999A CN 103322999 A CN103322999 A CN 103322999A CN 2013101954671 A CN2013101954671 A CN 2013101954671A CN 201310195467 A CN201310195467 A CN 201310195467A CN 103322999 A CN103322999 A CN 103322999A
- Authority
- CN
- China
- Prior art keywords
- dtri
- state
- ship
- information
- observation
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Landscapes
- Navigation (AREA)
Abstract
一种适用于多艇导航的保留历史状态的信息滤波算法,涉及一种协同导航技术。它实现了在减少了KF、EKF等常用滤波方法在协同导航应用的计算量的基础上,解决了多水面艇导航问题。其方法为:建立水面艇的***方程和观测方程;将非线性模型线性化;通过观测更新得到实时信息参数;进行时间更新;利用信息参数求得状态均值。本发明适用于多水面艇导航。
Description
技术领域
本发明属于导航技术领域,涉及一种适用于多艇导航的保留历史状态的信息滤波算法。
背景技术
水面艇是指在水面行驶的水面船艇。随着开发海洋认识的加深,水面艇以其机动性强、成本低等优势成为研究热点。多艇导航利用***中某些艇的高精度导航信息,通过一定的信息交换,实现艇间导航资源的共享,装备低精度导航设备的艇可以提高自身导航精度。当某些艇由于传感器或环境因素丧失独立导航能力时,协同导航可以在一定程度上恢复这些艇的导航能力。因此研究多水面艇的协同导航具有重要的理论价值和实际意义。
目前,对于多水面艇协同导航技术研究相对较少,但是协同导航技术在多无人平台上的应用并不鲜见。常用的方法是结合具体的导航传感器采用各种改进的卡尔曼滤波方法(KF)进行数据融合。相关文献指出在集中式KF算法中,以各个平台为处理节点,利用KF将各个节点的导航信息融合到某个主节点上,能够大大提高主节点的导航精度,在此基础上,将主节点的高精度信息反馈给各个节点从而提高多平台的整体导航能力。但是它在应用上有其缺陷,在观测更新时要改变所有与观测状态相关的状态的矩参数,使得计算量增大,通信复杂度较大,这将阻碍协同导航在军事领域的应用。通过研究机器人同步定位与建图(SLAM),发现带滞留状态的信息滤波可以取得比卡尔曼滤波较好的效果,并且滤波状态联合分布矩阵是稀疏的,计算量更优。相关文献借鉴SLAM提出一种将带滞留状态的增广信息滤波应用多运动平台的协同导航分散式设计方法。
发明内容
为了克服现有技术中存在的缺陷,本发明提供了一种适用于多艇导航的保留历史状态的信息滤波算法,将一种带历史状态的信息滤波应用于多艇的协同导航中。该方法将关键历史保留在联合状态的信息矩阵中,利用联合分布信息矩阵的稀疏性以及状态添加和观测更新仅改变与自身状态相关信息来降低运算复杂度。最后通过仿真,既保证了艇的协同定位的估计精度,又验证了该算法性能的优越性。
其技术方案如下:
一种适用于多艇导航的保留历史状态的信息滤波算法,包括以下步骤:
步骤一、建立多水面艇的运动方程,选取艇的纬度、经度和航向角作为状态量,首先建立平面2维坐标系x-y,记k时刻第i条艇状态为 为k时刻第i条艇在x轴的坐标,为k时刻第i条艇在y轴的坐标,为k时刻第i条艇的航向角,运动方程可表达为
步骤二、建立多水面艇的观测方程,选取艇的位置信息或者速度信息和各艇之间观测距离为量测量,
单艇观测方程可选为
k时刻选取艇间距为观测量(设i艇和j艇)
其中***噪声wi和观测噪声vi、vij均是零均值白噪声,且相互独立不相关;
步骤三、***运动方程和观测方程的线性化
对于单艇i观测,其量测方程雅可比矩阵为
对于艇i,j间观测,其量测方程雅可比矩阵为
步骤四、历史状态的保留,以及信息参数的求取
考虑***k-1时刻和k时刻的联合状态为[Xk-1Xk],信息参数为
其中,y为信息向量,Y为信息矩阵,Fk-1,Qk-1分别为k-1时刻,***线性化后的状态转移矩阵和***噪声阵.
由运动方程,k+1时刻添加状态Xk+1,得到联合状态[Xk-1XkXk+1],结合信息参数算法过程,可得信息参数为:
其中,ya为状态添加后的信息向量,Ya为状态添加后的信息矩阵,Fk,Qk分别为k时刻,***线性化后的状态转移矩阵和***噪声阵。
步骤五、***的观测更新,
通过观测zk+1来更新[Xk-1XkXk+1]的联合信息参数,观测更新后的信息参数为:
ya=[ya(1);ya(2);ya(3)+ik+1];
其中ik+1,Ik+1是观测更新贡献的信息向量更新和信息矩阵更新,
对于k+1时刻单艇观测,信息向量的更新为
信息矩阵的更新为:
其中,
对于k+1时刻平台i、j间相对观测,信息向量的更新为
其中,
观测贡献的信息矩阵更新为:
其中,
步骤六、信息滤波的时间更新,
时间更新是由多个时刻k-1,k,k+1…状态的联合概率求k时刻状态的边缘概率,在引入历史状态的信息滤波中,维持的是多个时刻k-1,k,k+1……联合状态的信息参数,而当联合状态的维数较高时,由于引入历史状态的信息滤波联合状态的信息矩阵是稀疏的,可以很容易由联合状态求得k时刻状态的边缘概率;
与现有技术相比,本发明的有益效果:
本发明提出了一种适用于多艇导航的保留历史状态的信息滤波算法,与传统信息滤波相比,该算法交换时间更新与观测更新的执行顺序。由于在进行观测更新时,滤波状态不仅含有当前时刻的状态,还含有历史时刻的状态。它拥有观测更新仅涉及观测艇和被观测艇的信息量的优势,以及将历史信息保留在联合状态时信息矩阵的稀疏性优势.
附图说明
图1是多水面艇轨迹示意图;
图2是B艇在X方向的误差曲线图;
图3是B艇在Y方向的误差曲线图;
图4是B艇航向角的误差曲线图。
具体实施方式
下面结合附图和具体实施方式对本发明的技术方案作进一步详细地说明。
一种适用于多艇导航的保留历史状态的信息滤波算法,包括以下步骤:
步骤一、建立多水面艇的运动方程,选取艇的纬度、经度和航向角作为状态量,首先建立平面2维坐标系x-y,记k时刻第i条艇状态为 为k时刻第i条艇在x轴的坐标,为k时刻第i条艇在y轴的坐标,为k时刻第i条艇的航向角,运动方程可表达为
步骤二、建立多水面艇的观测方程,选取艇的位置信息或者速度信息和各艇之间观测距离为量测量,
单艇观测方程可选为
k时刻选取艇间距为观测量(设i艇和j艇)
其中***噪声wi和观测噪声vi、vij均是零均值白噪声,且相互独立不相关;
步骤三、***运动方程和观测方程的线性化
对于单艇i观测,其量测方程雅可比矩阵为
对于艇i,j间观测,其量测方程雅可比矩阵为
其中,是线性化因子, h为非线性观测方程;
步骤四、历史状态的保留,以及信息参数的求取
考虑***k-1时刻和k时刻的联合状态为[Xk-1Xk],信息参数为
其中,y为信息向量,Y为信息矩阵,Fk-1,Qk-1分别为k-1时刻,***线性化后的状态转移矩阵和***噪声阵。
由运动方程,k+1时刻添加状态Xk+1,得到联合状态[Xk-1XkXk+1],结合信息参数算法过程,可得信息参数为:
其中,ya为状态添加后的信息向量,Ya为状态添加后的信息矩阵,Fk,Qk分别为k时刻,***线性化后的状态转移矩阵和***噪声阵。
步骤五、***的观测更新,
通过观测zk+1来更新[Xk-1XkXk+1]的联合信息参数,观测更新后的信息参数为:
ya=[ya(1);ya(2);ya(3)+ik+1];
其中ik+1,Ik+1是观测更新贡献的信息向量更新和信息矩阵更新,
对于k+1时刻单艇观测,信息向量的更新为
信息矩阵的更新为:
其中,
对于k+1时刻平台i、j间相对观测,信息向量的更新为
其中,
观测贡献的信息矩阵更新为:
其中,
步骤六、信息滤波的时间更新,
时间更新是由多个时刻k-1,k,k+1…状态的联合概率求k时刻状态的边缘概率,在引入历史状态的信息滤波中,维持的是多个时刻k-1,k,k+1……联合状态的信息参数,而当联合状态的维数较高时,由于引入历史状态的信息滤波联合状态的信息矩阵是稀疏的,可以很容易由联合状态求得k时刻状态的边缘概率;
为了说明所述方法的有益效果,在以下初始条件下对三艘水面艇的协同导航进行了仿真,仿真结果如图1、图2、图3及图4所示,并对其进行了分析比较。
初始条件:
三艘水面水面艇分别标识为A、B、C艇。仿真不考虑水流的扰动,每条艇都装备里程计、激光测距仪和一定的通信设备,A艇并给予GPS接收机,可以进行GPS观测。
离散时间间隔T:0.01s
水面艇最大移动速度:4m/s。
测量速度标准差:1m/s
航向角标准差:1
测距噪声标准差:5m
单艇观测噪声标准差:2m
仿真时间:10s
分析比较:
图1仿真了一次实验中A、B、C三条艇的参考轨迹、独自导航和协同导航的定位结果。由初始仿真设置知,独自导航时除艇A可以利用GPS信息校正里程计累积误差外,其它两艇只能利用里程计信息进行航迹推算。仿真结果表明,利用保留历史状态的信息滤波进行协同导航提高了水面艇的定位精度,并且没有GPS装置的艇也共享了导航信息从而提高了自身定位精度。
进一步进行仿真观察,图2~图4给出了B艇各状态量的误差曲线图,基于本导航算法的各状态量误差小于单独导航误差,说明该方法可以保证水面艇导航的估计精度,从而验证了算法的正确性。
以上所述,仅为本发明较佳的具体实施方式,本发明的保护范围不限于此,任何熟悉本技术领域的技术人员在本发明披露的技术范围内,可显而易见地得到的技术方案的简单变化或等效替换均落入本发明的保护范围内。
Claims (1)
1.一种适用于多艇导航的保留历史状态的信息滤波算法,其特征在于,包括以下步骤:
步骤一、建立多水面艇的运动方程选取艇的纬度、经度和航向角作为状态量,首先建立平面2维坐标系x-y,记k时刻第i条艇状态为 为k时刻第i条艇在x轴的坐标,为k时刻第i条艇在y轴的坐标,为k时刻第i条艇的航向角,运动方程可表达为
步骤二、建立多水面艇的观测方程,选取艇的位置信息或者速度信息和各艇之间观测距离为量测量,
单艇观测方程可选为
k时刻选取艇间距为观测量(设i艇和j艇)
其中***噪声wi和观测噪声vi、vij均是零均值白噪声,且相互独立不相关;
步骤三、***运动方程和观测方程的线性化
对于单艇i观测,其量测方程雅可比矩阵为
对于艇i,j间观测,其量测方程雅可比矩阵为
其中,是线性化因子, h为非线性观测方程;
步骤四、历史状态的保留,以及信息参数的求取
考虑***k-1时刻和k时刻的联合状态为[Xk-1Xk],信息参数为
其中,y为信息向量,Y为信息矩阵,Fk-1,Qk-1分别为k-1时刻,***线性化后的状态转移矩阵和***噪声阵,
由运动方程,k+1时刻添加状态Xk+1,得到联合状态[Xk-1XkXk+1],结合信息参数算法过程,可得信息参数为:
其中,ya为状态添加后的信息向量,Ya为状态添加后的信息矩阵,Fk,Qk分别为k时刻,***线性化后的状态转移矩阵和***噪声阵;
步骤五、***的观测更新,
通过观测zk+1来更新[Xk-1XkXk+1]的联合信息参数,观测更新后的信息参数为:
ya=[ya(1);ya(2);ya(3)+ik+1];
其中ik+1,Ik+1是观测更新贡献的信息向量更新和信息矩阵更新,
对于k+1时刻单艇观测,信息向量的更新为
信息矩阵的更新为:
其中,
对于k+1时刻平台i、j间相对观测,信息向量的更新为
其中,
观测贡献的信息矩阵更新为:
其中,
步骤六、信息滤波的时间更新,
时间更新是由多个时刻k-1,k,k+1…状态的联合概率求k时刻状态的边缘概率,在引入历史状态的信息滤波中,维持的是多个时刻k-1,k,k+1……联合状态的信息参数,而当联合状态的维数较高时,由于引入历史状态的信息滤波联合状态的信息矩阵是稀疏的,可以很容易由联合状态求得k时刻状态的边缘概率;
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN2013101954671A CN103322999A (zh) | 2013-05-24 | 2013-05-24 | 一种适用于多艇导航的保留历史状态的信息滤波算法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN2013101954671A CN103322999A (zh) | 2013-05-24 | 2013-05-24 | 一种适用于多艇导航的保留历史状态的信息滤波算法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN103322999A true CN103322999A (zh) | 2013-09-25 |
Family
ID=49191898
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN2013101954671A Pending CN103322999A (zh) | 2013-05-24 | 2013-05-24 | 一种适用于多艇导航的保留历史状态的信息滤波算法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN103322999A (zh) |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103712625A (zh) * | 2013-12-23 | 2014-04-09 | 哈尔滨工程大学 | 一种估计初始航向偏差滤波的方法 |
CN104034328A (zh) * | 2014-05-21 | 2014-09-10 | 哈尔滨工程大学 | 一种基于滤波方法和曲线拟合方法相结合的协同导航方法 |
CN111103566A (zh) * | 2019-11-28 | 2020-05-05 | 中国科学院上海微***与信息技术研究所 | 一种航向角确定方法、***、电子设备及存储介质 |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102353377A (zh) * | 2011-07-12 | 2012-02-15 | 北京航空航天大学 | 一种高空长航时无人机组合导航***及其导航定位方法 |
DE102012216211A1 (de) * | 2011-09-12 | 2013-03-14 | Continental Teves Ag & Co. Ohg | Verfahren zum Auswählen eines Satelliten |
CN103090864A (zh) * | 2013-02-05 | 2013-05-08 | 哈尔滨工程大学 | 一种基于通信延时补偿的多水面无人艇协同定位方法 |
-
2013
- 2013-05-24 CN CN2013101954671A patent/CN103322999A/zh active Pending
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102353377A (zh) * | 2011-07-12 | 2012-02-15 | 北京航空航天大学 | 一种高空长航时无人机组合导航***及其导航定位方法 |
DE102012216211A1 (de) * | 2011-09-12 | 2013-03-14 | Continental Teves Ag & Co. Ohg | Verfahren zum Auswählen eines Satelliten |
CN103090864A (zh) * | 2013-02-05 | 2013-05-08 | 哈尔滨工程大学 | 一种基于通信延时补偿的多水面无人艇协同定位方法 |
Non-Patent Citations (2)
Title |
---|
徐博等: "基于微惯性网络的多水面无人艇协同导航定位技术研究", 《惯性技术发展动态发展方向研讨会文集》 * |
穆华: "多运动平台协同导航的分散式算法研究", 《中国优秀博士学位论文全文数据库信息科技辑》 * |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103712625A (zh) * | 2013-12-23 | 2014-04-09 | 哈尔滨工程大学 | 一种估计初始航向偏差滤波的方法 |
CN104034328A (zh) * | 2014-05-21 | 2014-09-10 | 哈尔滨工程大学 | 一种基于滤波方法和曲线拟合方法相结合的协同导航方法 |
CN104034328B (zh) * | 2014-05-21 | 2017-03-29 | 哈尔滨工程大学 | 一种基于滤波方法和曲线拟合方法相结合的协同导航方法 |
CN111103566A (zh) * | 2019-11-28 | 2020-05-05 | 中国科学院上海微***与信息技术研究所 | 一种航向角确定方法、***、电子设备及存储介质 |
CN111103566B (zh) * | 2019-11-28 | 2023-09-05 | 中国科学院上海微***与信息技术研究所 | 一种航向角确定方法、***、电子设备及存储介质 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN104075715B (zh) | 一种结合地形和环境特征的水下导航定位方法 | |
CN109885080B (zh) | 自主控制***及自主控制方法 | |
CN106767752B (zh) | 一种基于偏振信息的组合导航方法 | |
CN102928858B (zh) | 基于改进扩展卡尔曼滤波的gnss单点动态定位方法 | |
CN105891863B (zh) | 一种基于高度约束的扩展卡尔曼滤波定位方法 | |
CN108387236B (zh) | 一种基于扩展卡尔曼滤波的偏振光slam方法 | |
CN106017467A (zh) | 一种基于多水下应答器的惯性/水声组合导航方法 | |
CN102538781A (zh) | 基于机器视觉和惯导融合的移动机器人运动姿态估计方法 | |
CN104236586B (zh) | 基于量测失准角的动基座传递对准方法 | |
CN104280025A (zh) | 基于无色卡尔曼滤波的深海机器人超短基线组合导航方法 | |
CN107966145B (zh) | 一种基于稀疏长基线紧组合的auv水下导航方法 | |
CN103968838A (zh) | 一种基于极坐标系的auv曲线运动状态下的协同定位方法 | |
CN104296780B (zh) | 一种基于重力视运动的sins自对准与纬度计算方法 | |
CN105547300A (zh) | 用于auv的全源导航***及方法 | |
CN103292813B (zh) | 一种提高水面艇编队导航精度的信息滤波方法 | |
CN111025229B (zh) | 一种水下机器人纯方位目标估计方法 | |
CN103322999A (zh) | 一种适用于多艇导航的保留历史状态的信息滤波算法 | |
CN109764876A (zh) | 无人平台的多模态融合定位方法 | |
Song et al. | FACON: A flow-aided cooperative navigation scheme | |
Song et al. | Hierarchical underwater localization in dominating background flow fields | |
CN103616026A (zh) | 一种基于h∞滤波的auv操纵模型辅助捷联惯导组合导航方法 | |
CN105241466A (zh) | 一种飞行器三维导航方法及其装置 | |
CN112945245B (zh) | 基于条件数理论在多auv协同导航***中可观测性分析方法 | |
Song et al. | A distributed localization hierarchy for an AUV swarm | |
CN104034328A (zh) | 一种基于滤波方法和曲线拟合方法相结合的协同导航方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
C02 | Deemed withdrawal of patent application after publication (patent law 2001) | ||
WD01 | Invention patent application deemed withdrawn after publication |
Application publication date: 20130925 |