基于SIR粒子滤波方法的低功耗地形辅助导航系统及方法与流程

本发明涉及水下航行器自主导航领域，特别涉及一种基于sir粒子滤波方法的地形辅助导航系统。

背景技术：

目前，水下运载器常用的定位导航方式有gps/ins组合定位与声学定位。但在常年有冰雪覆盖的北极地区，无法有效利用gps，并且较厚的冰层不利于声学信标的布置。单独利用惯性传感器会引起无边界的误差累积。地形辅助导航仅利用距离传感器获取高度数据，即可实现一定精度的定位。粒子滤波技术可以解决非线性系统与非高斯观测的解算问题，是地形辅助导航系统中常用的方法。在地形辅助导航中，海流与地图分辨率是影响导航精度的两大因素，应建立有效的航迹推算模型，采用合理的滤波技术，有效克服海流与低分辨率地图的影响。

此外，随着对水下信息采集要求的提高，当代水下运载器要求可以在水下长时间连续工作，因此需要考虑功耗的限制，而传统导航系统中利用多波束声呐或多普勒声速仪会增加系统功耗，减小运载器的联系运行时间。基于单波束的高度数据进行导航，增加了导航的难度。

技术实现要素：

本发明的目的在于提供一种基于sir粒子滤波方法的低功耗地形辅助导航系统，其在低分辨率地图与单波束高度数据的情况下，可有效提升导航的精度；本发明基于建立的有效运动模型，合理选择海流造成的运动误差，克服海流造成的影响；利用双线性插值技术与系统重采样技术降低低分辨率地图造成的误差；该系统基于获取的单波束高度数据更新推算当前位置，可以有效节省能源，克服海流对导航的影响，提升水下运载器在水下联系工作的能力。

为了达到上述目的，本发明通过以下技术方案实现：

一种基于粒子滤波方法的地形辅助导航系统，包含导航数据采集板块和导航数据处理板块；所述导航数据采集板块设有：用于提供目标载体的位置信息、运动信息和姿态信息的惯性导航传感器、用于提供目标载体的水下深度信息的深度计和用于提供载体距离海底的高度信息的高度计；所述导航数据处理板块设置有相互通信连接的航位推算模块和粒子滤波模块；惯性导航传感器、深度计和高度计中的一种或多种分别将各自输出的数据信息传输至所述航位推算模块和/或所述粒子滤波模块中；所述航位推算模块输出航位推算数据并经所述粒子滤波模块进行滤波，最终得到目标载体位置结果。

优选地，所述粒子滤波模块为sir粒子滤波模块。

优选地，惯性导航传感器、深度计和高度计分别与数据总线相连，所述数据总线分别与导航数据处理板块中的航位推算模块和粒子滤波模块相连；惯性导航传感器、深度计和高度计分别将各自输出的数据信息通过所述数据总线传输到航位推算模块和粒子滤波模块中。

优选地，所述惯性导航传感器设置在所述目标载体上。

优选地，所述目标载体为水下运载器。

优选地，所述惯性导航传感器输出沿水下运载器侧向、水下运载器轴向的加速度，以及航向角，记作其中，水下运载器的侧向记为x轴方向，水下运载器的轴向记为y轴方向，分别表示沿水下运载器x轴与y轴的加速度，ψ表示航向角；所述深度计输出水下运载器的深度z，所述高度计输出水下运载器距离海底的高度h。

本发明进一步提供了一种基于如上文所述的基于粒子滤波方法的地形辅助导航系统的仿真方法，该方法包含以下步骤：

步骤t1：惯性导航传感器提供目标载体的位置信息或运动信息或姿态信息，深度计提供目标载体的水下深度信息，高度计提供载体距离海底的高度信息；

步骤t2：航位推算模块接收惯性导航传感器、深度计和高度计中的一种或多种发送的数据信息，推算并输出航位推算数据；

步骤t3：粒子滤波模块将其接收到的航位推算模块的航位推算数据进行滤波，最终得到目标载体位置结果。

优选地，所述步骤t2，进一步包含：

所述惯性导航传感器输出沿水下运载器侧向、水下运载器轴向的加速度，以及航向角，记作其中，水下运载器的侧向记为x轴方向，水下运载器的轴向记为y轴方向，分别表示沿水下运载器x轴与y轴的加速度，ψ表示航向角；所述深度计输出水下运载器的深度z，所述高度计输出水下运载器距离海底的高度h。

优选地，所述步骤t2，进一步包含：

所述航位推算模块根据接收的三个参数先对加速度量进行积分运算，分别得到x轴方向、y轴方向这两个方向上的速度量(vx,vy)，并推算出水下运载器在δt时间间隔内的航位变化量，所述航位推算模块再结合上一时刻t的位置(xt,yt)，计算得到当前时刻t+δt的位置(xt+δt,yt+δt)；t时刻的深度值zt直接由深度计测量值zm给定，如下：

其中，初始时刻的位置(x0,y0,z0)是释放时刻的位置或在水面有gps信号时的位置；当gps信号时，会重置初值(x0,y0,z0)。

优选地，所述步骤t3中粒子滤波模块进行滤波，包含以下过程：

s1：在接收到所述航位推算模块传送的位置信息(x,y,z)的基础上，产生数量为n的随机噪声wi附加到位置值上，产生粒子位置(xi,yi)，其中，噪声符合正态分布

s2：根据每个粒子位置(xi,yi)，在给定的地图模型中进行插值处理，得到当前位置对应的地形高度f(xi,yi)：

其中，f(qij),(i,j＝1,2])是位置(xi,yi)四周与之下标相对应的4个点(xi,yj),(i,j＝1,2)对应的高度值；scal是地图分辨率；

s3：根据深度数据z与高度数据h，计算当前水体深度：

zw＝z+h

利用高斯浓度函数，计算当前水体深度zw相对于各个地形高度f(xi,yi)的权重wi：

并将每一个粒子共n个的权重相累加，得到粒子总权重

s4：采用系统重采样方法重新给每一个粒子分配数值相等的统一权重，根据原粒子权重选择粒子复制的数量；

s5：根据步骤s1中得到的粒子位置和步骤s3中得到粒子权重，求解水下运载器在水平方向上的位置：

s6：重复步骤s1到步骤s5，直至到达设定位置时导航结束。

与现有技术相比，本发明的有益效果在于：本发明提供的基于sir粒子滤波技术的地形辅助导航系统，利用sir粒子滤波技术评估更新载体的位置，可以有效克服水流干扰造成的误差，在低分辨率地图与单波束高度数据的情况下，可有效提升导航的精度。本发明可以适用于地图分辨率较小的地区，拓展了应用范围。

附图说明

图1为本发明的地形辅助导航系统示意图；

图2为本发明的仿真地形图(分辨率为500m，图中黑色线段为仿真路径)；

图3为本发明的不同粒子数量情况下的路径跟踪误差对比示意图；

图4为本发明的不同过程噪声情况下的跟踪误差对比示意图。

具体实施方式

使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

如图1-图4结合所示，本发明提供一种基于sir(sequentialimportanceresampling,序列重要性重采样)粒子滤波技术的地形辅助导航系统，包括导航数据采集板块1和导航数据处理板块2。所述导航数据采集板块1设有惯性导航传感器3、深度计4、高度计5和数据总线6。所述导航数据处理板块2包括相互连接的航位推算模块7和粒子滤波模块8。本发明基于sir粒子滤波技术的地形辅助导航系统，利用sir粒子滤波技术评估更新载体的位置，可以有效克服水流干扰造成的误差，并且可以适用于地图分辨率较小的地区，拓展应用范围。

如图1所示，本发明的惯性导航传感器3安装在载体(例如水下运载器)上，用于提供载体的三维位置、三维速度或加速度、三维姿态等信息；深度计4用于提供载体(例如水下运载器)的水下深度信息；高度计5用于提供载体(例如水下运载器)距离海底的高度信息。惯性导航传感器3、深度计4和高度计5分别与数据总线6相连，且数据总线6还与导航数据处理板块2相连，惯性导航传感器3、深度计4和高度计5将各自输出的数据信息通过数据总线6传输到导航数据处理板块2的航位推算模块7和粒子滤波模块8中。

示例地，航位推算模块7输出的航位推算数据经过滤波模块8的滤波操作后形成最终的位置输出，即最终得到水下运载器的位置坐标信息；所述航位推算模块7和粒子滤波模块8分别与数据总线6相连。

具体的一个示例中，惯性导航传感器3输出沿水下运载器x轴(侧向)、y轴(轴向)的加速度与航向角，记作其中，分别表示沿水下运载器x轴与y轴的加速度，ψ表示航向角；深度计4输出运载器的深度z，高度计5输出运载器距离海底的高度h。其中，三个传感器(惯性导航传感器3、深度计4和高度计5)的输出数据都是数字量。

本发明的航位推算模块7根据接收的这三个参数，首先对加速度量进行积分运算，得到两个方向上的速度量(vx,vy)，并推算出运载器在δt时间间隔内的航位变化量，航位推算模块7再结合上一时刻的位置(xt,yt)，计算得到当前时刻的位置(xt+δt,yt+δt)。t+δt时刻的深度值zt+δt直接由深度计测量值zm给定，如下：

其中，深度是由水下运载器携带的深度计(单波束声学测高仪器，其用于测量水下运载器器距离水底的距离)测量的，且测量的是当前时刻的深度值；初始时刻的位置(x0,y0,z0)是释放时刻的位置或在水面有gps信号时的位置；当gps信号时，会重置初值(x0,y0,z0)。

本发明的粒子滤波模块8根据以下步骤进行滤波：

步骤s1：在接受到航位推算模块7传送的位置信息(x,y,z)的基础上，产生数量为n的随机噪声wi附加到位置值上，产生粒子位置(xi,yi)。其中，噪声符合正态分布

步骤s2：根据每个粒子位置(xi,yi)，在给定的地图模型(目标区域的地图模型)中进行插值处理(如双线性插值)，得到当前位置对应的地形高度f(xi,yi)：

其中，f(qij),(i,j＝1,2])是位置(xi,yi)四周与之下标相对应的4个点(xi,yj),(i,j＝1,2)对应的高度值；scal是地图分辨率。

步骤s3：根据深度数据z与高度数据h，计算当前水体深度：

zw＝z+h

利用高斯浓度函数，计算当前水体深度zw相对于各个地形高度f(xi,yi)的权重wi：

并将每一个粒子共n个的权重相累加，得到粒子总权重

步骤s4：为避免粒子的局部收敛，采用系统重采样技术重新给每一个粒子分配数值相等的统一权重，根据原粒子权重选择粒子复制的数量。

步骤s5：根据步骤s1中得到的粒子位置和步骤s3中得到粒子权重，利用重心法，求解运载器在水平方向上的位置：

步骤s6：重复步骤s1到步骤s5，直至到达设定位置时导航结束。

本发明的基于sir粒子滤波技术的低功耗地形辅助导航系统，通过仿真，基于一个分辨率只有500m的地图(如图2所示的仿真地形图，分辨率为500m，图中黑色线段为仿真路径)验证本发明系统的可行性，基于单波束声呐的高度数据，在x方向与y方向各有0.1m/s的海流干扰下，产生的路径跟踪误差如图3所示，该图3为本发明的不同粒子数量情况下的路径跟踪误差对比示意图，海流干扰为x方向0.1m/，y方向0.1m/s，由图3可知，导航误差随着粒子数的增多而减小；在x方向与y方向各有0.2m/s的海流干扰下，产生的路径跟踪误差如图4所示，该图4为本发明的不同过程噪声情况下的跟踪误差对比示意图，海流干扰为x方向0.2m/s，y方向0.2m/s，由图4可知，在海流干扰较大的情况下，导航误差随着过程噪声的增大而减小。

综上所述，本发明在低分辨率地图与单波束高度数据的情况下，可有效提升导航的精度；本发明基于建立的有效运动模型，合理选择海流造成的运动误差，克服海流造成的影响；利用双线性插值技术与系统重采样技术降低低分辨率地图造成的误差；该系统基于获取的单波束高度数据更新推算当前位置，可以有效节省能源，克服海流对导航的影响，提升水下运载器在水下联系工作的能力。

尽管本发明的内容已经通过上述优选实施例作了详细介绍，但应当认识到上述的描述不应被认为是对本发明的限制。在本领域技术人员阅读了上述内容后，对于本发明的多种修改和替代都将是显而易见的。因此，本发明的保护范围应由所附的权利要求来限定。