一种基于自适应联邦卡尔曼滤波的无人船组合导航方法
专利汇可以提供一种基于自适应联邦卡尔曼滤波的无人船组合导航方法专利检索，专利查询，专利分析的服务。并且本 发明 公开了一种基于自适应联邦卡尔曼滤波的无人船组合导航方法，利用无人船组合 导航系统 进行导航，所述无人船组合导航系统包括SINS、GPS、Compass和嵌入式处理器。所述嵌入式处理器内采用自适应联邦卡尔曼滤波 算法 进行 传感器 信息融合，输出无人船的 位置 、速度和 姿态 信息。本发明应用了无人船SINS/GPS/Compass组合导航的误差模型和观测模型，减小各个子系统之间的故障干扰，提高无人船导航系统的可靠性与 稳定性 。本发明在联邦卡尔曼滤波的 基础 上，设计联邦卡尔曼滤波的子 滤波器 的信息分配因子，在保障系统的容错能 力 的前提下，能够有效抑制系统的异常扰动，减小分配原则对滤波 精度 的影响。，下面是一种基于自适应联邦卡尔曼滤波的无人船组合导航方法专利的具体信息内容。
1.一种基于自适应联邦卡尔曼滤波的无人船组合导航方法，其特征在于：利用无人船组合导航系统进行导航，所述无人船组合导航系统包括捷联惯导系统传感器即SINS、全球定位系统传感器即GPS、三维电子罗盘传感器即Compass和嵌入式处理器；所述嵌入式处理器内采用自适应联邦卡尔曼滤波算法进行传感器信息融合，输出无人船的位置、速度和姿态信息；所述自适应联邦卡尔曼滤波算法基于自适应联邦卡尔曼滤波器实现；所述自适应联邦卡尔曼滤波器包括主滤波器、SINS/GPS子滤波器、SINS/Compass子滤波器和自适应信息分配因子计算模块；所述主滤波器分别与SINS/GPS子滤波器和SINS/Compass子滤波器双向连接；所述SINS/GPS子滤波器输出位置、速度信息的状态估计值和估计误差协方差矩阵，SINS/Compass子滤波器输出姿态信息的状态估计值和估计均方误差协方差矩阵；自适应信息分配因子计算模块通过两个子滤波器的预测状态矢量的估计均方误差协方差矩阵计算出自适应信息分配因子，将归一化后的自适应信息分配因子送入主滤波器；主滤波器内进行时间更新过程和信息融合过程，将两个子滤波器的状态估计值和状态估计均方误差协方差矩阵与主滤波器的状态估计值进行信息融合，得到全局最优状态估计值，并反馈到两个子滤波器，进行子滤波器的重置；
所述组合导航方法，包括以下步骤：
A、建立无人船组合导航系统的状态方程
选择东北天地理坐标系为导航坐标系，状态方程考虑捷联惯导系统、加速度计、陀螺仪的误差因素，则所述组合导航系统的状态方程为：
其中， 为组合导航系统的状态估计值，F为组合导航系统的状态转移矩阵，X为组合导航系统的状态变量，G为组合导航系统的控制矩阵，W为组合导航系统的白噪声矩阵；
组合导航系统的状态变量X为：
式中， 分别为无人船东、北、天方向的姿态误差角；δVE、δVN、δVU分别为无
人船东、北、天方向的速度误差；δλ、δL、δh分别为无人船的纬度、经度、高度的误差；εbx、εby、εbz分别为载体系下陀螺仪东、北、天方向的常值漂移；εrx、εry、εrz分别为载体系下陀螺仪东、北、天方向的慢变漂移； 分别为载体系下加速度计东、北、天方向的常值随机
误差；
组合导航系统的白噪声矩阵W为：
W＝[ωgx,ωgy,ωgz,ωrx,ωry,ωrz,ωax,ωay,ωaz]T
其中，ωgx、ωgy、ωgz分别为陀螺仪东、北、天方向的白噪声；ωrx、ωry、ωrz分别为陀螺仪东、北、天方向的一阶马尔科夫过程驱动白噪声；ωax、ωay、ωaz为加速度计东、北、天方向的一阶马尔科夫过程驱动白噪声；
陀螺仪的误差模型为：
ε＝εb+εr+ωg
式中，εb为随机常数；εr为一阶马尔科夫过程随机噪声，εr满足下式：、
Tg为陀螺仪相关时间，ωr为陀螺仪的一阶马尔科夫过程驱动白噪声；
加速度计的误差模型为：
式中，为加速度计的误差， Ta为加速度计相关时间，ωa为加速度计
的一阶马尔科夫过程驱动白噪声， 为加速度计的速度误差；
组合导航系统的状态转移矩阵为：
式中，FN为9行9列的基本导航参数的系统阵，具体如下：
FN(2,7)＝-ωie sinL
FN(4,2)＝-fU
FN(5,1)＝fU
FN(5,3)＝-fE
FN(6,1)＝-fN
FN(6,2)＝-fE
FN(6,7)＝-2VEωie sinL
FN(9,6)＝1
其余元素为0；
其中ωie为地球自转角速率，fE、fU分别为东、天方向的地球偏扁率，RE为地球赤道半径，RM地球表面上的任一点处沿子午圈的主曲率半径；RN为地球表面上的任一点处沿卯酉圈的主曲率半径；L为经度；
FM＝diag{0，0,0,-1/Trx,-1/Try,-1/Trz,-1/Tax,-1/Tay,-1/Taz}
式中：
θ、 ψ分别为无人船的横滚角、俯仰角、航向角；
组合导航系统的控制矩阵为：
B、设计SINS/GPS子滤波器
SINS/GPS子滤波器采用位置和速度组合模式，建立SINS/GPS子滤波器的位置、速度的量测方程为：
Z1＝HX1+V1
Z1为SINS/GPS子滤波器的量测值，H1为SINS/GPS子滤波器的量测矩阵，V1为SINS/GPS子滤波器的量测噪声；
SINS的速度信息为：
GPS的速度信息为：
SINS的位置信息为：
GPS的位置信息为：
式中，vIE、vIN、vIU分别为SINS东、北、天方向的速度信息；vGE、vGN、vGU分别为GPS东、北、天方向的速度信息；λI、LI、hI分别为SINS东、北、天方向的位置信息；λG、LG、hG分别为GPS东、北、天方向的位置信息；vN、vE、vU分别为无人船东、北、天方向的速度真值；λt、Lt、ht分别为无人船纬度、经度、高度的位置真值；δvE、δvN、δvU分别为SINS东、北、天方向的速度误差；MN、ME、MU分别为GPS东、北、天方向的速度误差；δλ、δL、δh分别为SINS的纬度、经度、高度的误差，NN、NE、NU分别为GPS的纬度、经度、高度的误差；
取SINS和GPS的速度差值作为SINS/GPS子滤波器量测值，定义速度量测方程为：
式中：
ZV为SINS/GPS子滤波器速度信息的量测值，HV为SINS/GPS子滤波器速度信息的量测矩阵，VV为SINS/GPS子滤波器速度信息的量测噪声，O3×3为3行3列的零矩阵，O3×12为3行12列的零矩阵；
取SINS和GPS的位置差值作为SINS/GPS子滤波器量测值，定义位置量测方程为：
式中：
Vp＝[NN,NE,NU]T
RM＝Re(1-2f+3fsin2L)
RN＝Re(1+fsin2L)
式中：ZP为SINS/GPS子滤波器速度信息的量测值，HP为SINS/GPS子滤波器速度信息的量测矩阵，VP为SINS/GPS子滤波器速度信息的量测噪声，VP当做白噪声处理；Re为地球赤道半径；取6378137m；f为地球扁率，取1/298.257；O3×6为3行6列的零矩阵；O3×9为3行9列的零矩阵；
采用速度与位置的组合方式，建立SINS/GPS子滤波器的量测方程为：
C、设计SINS/Compass子滤波器
建立无人船组合导航SINS/Compass子滤波器的姿态的量测方程为：
Z2＝H2X+V2
式中，Z2为SINS/Compass子滤波器的量测值，H2为SINS/Compass子滤波器的量测矩阵，V2为SINS/Compass子滤波器的量测噪声；
SINS的姿态信息为：
Compass的姿态信息为：
式中， 分别为SINS东、北、天方向的姿态信息； 分别为
Compass东、北、天的姿态信息； 分别为无人船东、北、天方向的姿态真值；
分别为SINS东、北、天方向的姿态误差；δαE、δαN、δαU分别为Compass的东、北、天方向的姿态误差；
将SINS和Compass输出的姿态信息的差值作为SINS/Compass子滤波器的测量值，则观测方程为：
式中，H2＝[I3×3,03×15]，I3×3为3行3列的单位矩阵，O3×15为3行15列单位矩阵，V2＝[δαc,δβc,δγc]T是均值为零的观测白噪声；
D、实现自适应联邦卡尔曼滤波的过程
SINS/GPS子滤波器和SINS/Compass子滤波器并行运行，将SINS、GPS、Compass数据进行分散式处理，在主滤波器中进行信息融合；两个子滤波器的估计误差协方差阵及状态估计值被送入主滤波器，并与主滤波器的估计值进行信息融合得到全局最优估计；按照信息分配原则将全局最优估计反馈到子滤波器，完成一次滤波，并重置两个子滤波器的估计值；
所述自适应联邦卡尔曼滤波器的信息融合过程在主滤波器内进行，包括信息分配、时间更新、量测更新、信息融合四个步骤，具体步骤如下：
D1、信息分配
将全局的状态估计、误差方差阵、噪声信息分配给两个子滤波器和主滤波器，分配原则为：
式中，i＝1、2分别代表SINS/GPS子滤波器和SINS/Compass子滤波器，Pi,k-1为子滤波器k-1时刻的估计均方误差协方差矩阵，Pg,k-1为k-1时刻的全局最优估计均方误差协方差矩阵，Qi,k-1为子滤波器噪声k-1时刻的系统噪声方差阵，Qg,k-1为主滤波器噪声k-1时刻的系统噪声方差阵， 为子滤波器k-1时刻的状态估计， 为主滤波器k-1时刻的状态估计；
信息分配因子βi满足信息守恒原则，即
主滤波器中无信息分配，执行状态估计和协方差阵的时间更新，即Pm,k＝0；
D2、时间更新
时间更新在两个子滤波器间独立进行，分别获得两个子滤波器的状态预测矢量和一步预测均方误差协方差矩阵：
式中，i＝1、2分别代表SINS/GPS子滤波器和SINS/Compass子滤波器，Pi,k|k-1为子滤波器的一步预测均方误差协方差矩阵， 为子滤波器的状态预测矢量，Φi,k|k-1为子滤波器k-1时刻到k时刻的一步转移矩阵，Γi,k-1为噪声驱动阵；
D3、量测更新
两个子滤波器接收量测信息并独立进行量测更新过程，分别获得两个子滤波器的状态估计值、滤波增益和估计均方误差协方差矩阵，并传至主滤波器：
式中，i＝1、2分别代表SINS/GPS子滤波器和SINS/Compass子滤波器， 为子滤波器k时刻的状态估计，Ki,k为子滤波器k时刻的滤波增益，Pi,k为估计均方误差协方差矩阵，Zi,k为子滤波器k时刻的量测值，Hi,k子滤波器k时刻的量测矩阵，Ri,k子滤波器k时刻的量测噪声方差阵，I单位矩阵；
量测更新过程仅在子滤波器内进行，主滤波器内只进行时间更新过程；
其中 为主滤波器k-1时刻的状态估计， 为主滤波器k-1时刻的状态预测矢
量；
D4、信息融合
把两个子滤波器的状态估计信息和主滤波器的状态估计进行融合，从而得到全局状态估计信息：
式中，Pg,k为k时刻的全局最优估计均方误差协方差矩阵，P1,k为SINS/GPS子滤波器k时刻的估计均方误差协方差矩阵，P2,k为SINS/Compass子滤波器k时刻的估计均方误差协方差矩阵，Pm,k为主滤波器k时刻的估计均方误差协方差矩阵， 为k时刻的全局最优状态估计；
D5、计算自适应信息分配因子
最优自适应因子满足以下条件：
最优自适应因子表示如下：
几何状态矢量与预测状态矢量的偏差为：
其中，几何状态矢量 为：
式中，∑i,k为Zi,k的等效权矩阵；
其中， 为预测状态矢量，Pi,k|k-1为预测状态矢量的估计协方差矩阵， 为预测
状态矢量的理论协方差矩阵；
当估计量基于当前时刻的预测状态矢量，预测状态矢量的估计协方差矩阵如下：
最优自适应因子αk的期望值小于1，表示为：
式中：
则最优自适应因子基于预测状态矢量的估计协方差矩阵表示为：
由上式表示的自适应因子类似于由模型的预测状态与量测的估计状态之间的差异构
造的自适应因子；
自适应因子表示为预测状态矢量的形式为：
其中，c为常数， 为基于预测状态矢量 构造的统计量，其表达式为：
通过基于预测状态矢量的最优自适应因子的计算方法求得自适应联邦卡尔曼滤波器
的自适应信息分配因子为：
其中，b为常数，取0.85～1.0；
为确保自适应信息分配因子满足信息守恒定律，对自适应信息分配因子进行归一化处理；自适应信息分配因子归一化为：
式中，βi',k为归一化后的子滤波器在k时刻的自适应信息分配因子。
一种基于自适应联邦卡尔曼滤波的无人船组合导航方法
技术领域
背景技术
发明内容
分别为SINS东、北、天方向的姿态误差；δαE、δαN、δαU分别为Compass的东、北、天方向的姿态误差。
具体实施方式
EKF -1.885～1.939 -0.710～0.618 -3.857～4.010
AEKF -1.840～1.435 -0.714～0.616 -0.592～0.488
东向速度误差 北向速度误差 天向速度误差
EKF -0.693～1.201 -3.693～14.896 -0.505～0.745
AEKF -0.693～1.201 -0.559～1.226 -0.377～0.586
东向姿态误差 北向姿态误差 天向姿态误差
EKF -0.209～0.266 -0.210～0.268 -3690.023～4708.490
AEKF -0.224～0.246 -0.210～0.267 -51.710～100.228
| 标题 | 发布/更新时间 | 阅读量 |
|---|---|---|
| 一种用于水损害检测的探地雷达图谱自适应选取方法 | 2020-05-08 | 564 |
| 一种面向无人船组合导航信息融合的多尺度数据分块算法 | 2020-05-11 | 374 |
| 一种基于遗传算法的视觉惯性组合的SLAM方法 | 2020-05-16 | 526 |
| 眼部疲劳度检测系统及方法 | 2020-05-08 | 737 |
| 一种双发油动变桨距多旋翼飞行器的飞行控制方法 | 2020-05-12 | 840 |
| 性能退化产品可靠度置信区间估计数值仿真方法 | 2020-05-12 | 535 |
| 一种基于行为的室内定位和地标语义标识的方法 | 2020-05-14 | 204 |
| 一种陀螺漂移量的自适应调节方法 | 2020-05-08 | 272 |
| 空中移动目标的反制系统及其反制方法 | 2020-05-12 | 718 |
| 判断目标行人在非重叠相机视角中出现的匹配方法 | 2020-05-11 | 320 |
高效检索全球专利专利汇是专利免费检索,专利查询,专利分析-国家发明专利查询检索分析平台，是提供专利分析，专利查询，专利检索等数据服务功能的知识产权数据服务商。
我们的产品包含105个国家的1.26亿组数据，免费查、免费专利分析。
专利汇分析报告产品可以对行业情报数据进行梳理分析，涉及维度包括行业专利基本状况分析、地域分析、技术分析、发明人分析、申请人分析、专利权人分析、失效分析、核心专利分析、法律分析、研发重点分析、企业专利处境分析、技术处境分析、专利寿命分析、企业定位分析、引证分析等超过60个分析角度，系统通过AI智能系统对图表进行解读，只需1分钟，一键生成行业专利分析报告。
