System.ArgumentOutOfRangeException: 索引和长度必须引用该字符串内的位置。 参数名: length 在 System.String.Substring(Int32 startIndex, Int32 length) 在 zhuanliShow.Bind()
【技术实现步骤摘要】
本专利技术涉及自主航行器,具体涉及状态估计方法、无人水下航行器及计算机可读存储介质。
技术介绍
1、目前,无人水下航行器在水下结构维修、海洋科学调查等民用和军用海洋作业中发挥着重要作用。随着无人平台和人工智能的快速发展,人们越来越有兴趣将无人水下航行器用于更复杂、更持久的海洋服务,因此无人水下航行器在执行任务时需要拥有长期稳定、高效的自治能力,要对自身状态要有精确的感知。目前,水下超短基线声学定位系统(ultra short base line,usbl)、多普勒测速仪(doppler velocity log,dvl)和惯性导航系统(inertial navigation system,ins)是无人水下航行器组合导航定位系统中常见的重要组成部分。由于水下环境的复杂性和传感器的特性,对无人水下航行器的状态估计具有很大的挑战。
2、ins中的惯性测量单元(inertial measurement unit,imu)在长时间的工作下会产生漂移问题,航向测量会存在积累误差,同时各个传感器的观测数据在某些突发情况下会有较大噪声和误差,因此需要对数据进行滤波处理。而常用的滤波器如均值滤波、中值滤波、卡尔曼滤波(kf)、扩展卡尔曼滤波(ekf)和粒子滤波(pf)都有各自的局限性,并不能解决现有技术所存在的问题。
技术实现思路
1、为了克服现有技术的不足,本专利技术提供一种状态估计方法、无人水下航行器及计算机可读存储介质。
2、本专利技术解决其技术问题所采用的技术方案是
3、第一方面,本专利技术实施例提供了一种状态估计方法,所述方法用于无人水下航行器,所述方法包括:
4、获取所述无人水下航行器的测量数据;
5、获取所述无人水下航行器的第一航向角数据;
6、设置自适应事件触发条件,根据所述自适应事件触发条件对所述测量数据进行筛选,得到筛选系数;
7、根据所述测量数据、所述第一航向角数据、所述筛选系数以及扩展卡尔曼滤波方法,对所述无人水下航行器进行状态估计;
8、其中,所述根据所述测量数据、所述第一航向角数据、所述筛选系数以及扩展卡尔曼滤波方法对所述无人水下航行器进行状态估计,包括:
9、根据所述无人水下航行器的位置信息、速度信息以及航向角信息构建所述无人水下航行器的状态向量;
10、根据所述状态向量建立所述无人水下航行器的状态预测方程、协方差预测方程;
11、根据所述测量数据和所述第一航向角数据构建观测方程并得到观测实际值以及观测预测值;
12、根据所述状态预测方程、所述协方差预测方程、所述观测方程以及所述筛选系数计算扩展卡尔曼增益;
13、根据所述扩展卡尔曼增益对所述状态预测方程以及所述协方差预测方程进行更新,得到所述无人水下航行器的状态估计向量;
14、所述扩展卡尔曼增益通过公式计算得到,所述为k时刻的扩展卡尔曼增益系数,为k时刻的预测协方差矩阵,h为观测矩阵,r为观测噪声矩阵,i为单位矩阵,为筛选系数,趋近于∞。
15、在一些实施例中,所述无人水下航行器包括usbl换能器、dvl,所述获取所述无人水下航行器的测量数据包括:
16、通过所述usbl换能器获取所述无人水下航行器的位置数据;
17、通过所述dvl获取所述无人水下航行器的速度数据。
18、在一些实施例中,所述无人水下航行器还包括imu,所述获取所述无人水下航行器的第一航向角数据,包括:
19、通过所述imu获取所述无人水下航行器的第二航向角数据;
20、通过所述位置数据与所述速度数据计算得到第三航向角数据;
21、对所述第二航向角数据和所述第三航向角数据进行比较,以矫正所述第二航向角数据,得到所述第一航向角数据。
22、在一些实施例中,所述方法还包括:
23、在计算扩展卡尔曼增益之前,先计算所述状态预测方程的雅可比矩阵。
24、在一些实施例中,所述状态估计向量通过公式计算得到,所述为k时刻的状态估计向量,为根据k-1时刻的状态向量推测的k时刻状态向量,为k时刻的扩展卡尔曼增益系数,为观测实际值,为观测预测值。
25、在一些实施例中,所述设置自适应事件触发条件,包括:
26、设置自适应事件的门限阈值;
27、基于所述门限阈值、所述观测预测值与所述观测实际值的残差确定筛选系数。
28、在一些实施例中,所述门限阈值通过公式计算得到,所述为最大先验门限,为观测实际值,为观测预测值,为门限阈值。
29、在一些实施例中,所述筛选系数通过公式计算得到,所述为筛选系数,为观测实际值,为观测预测值,为门限阈值。
30、第二方面,本专利技术实施例提供了一种无人水下航行器,所述无人水下航行器包括机身、传感器和控制器,所述传感器包括usbl换能器、dvl和imu,所述控制器包括:
31、至少一个处理器;以及,
32、与所述至少一个处理器通信连接的存储器;其中,
33、所述存储器存储有可被所述至少一个处理器执行的指令,所述指令被所述至少一个处理器执行,以使所述至少一个处理器能够执行如上任一实施例所述的方法。
34、第三方面,本专利技术实施例还提供了一种非易失性计算机可读存储介质,所述计算机可读存储介质存储有计算机可执行指令,当所述计算机可执行指令被无人水下航行器执行时,使所述无人水下航行器执行如上任一实施例所述的方法。
35、与现有技术相比较,本专利技术至少具有以下有益效果:
36、本专利技术提出了一种基于无人水下航行器的状态估计方法通过结合usbl换能器和dvl两个传感器的数据估算得到第三航向角数据,将第三航向角数据与imu传感器所测得的第二航向角数据对比,对第二航向角数据进行矫正,并得到第一航向角数据,以此改善航向测量的漂移问题。结合自适应事件、扩展卡尔曼滤波方法以及矫正后的第一航向角数据,对无人水下航行器进行状态估计,可实现对无人水下航行器状态的精确估计。
本文档来自技高网...【技术保护点】
1.状态估计方法,所述方法用于无人水下航行器,其特征在于,所述方法包括:
2.根据权利要求1所述的状态估计方法,其特征在于,所述无人水下航行器包括USBL换能器、DVL,所述获取所述无人水下航行器的测量数据包括:
3.根据权利要求2所述的状态估计方法,其特征在于,所述无人水下航行器还包括IMU,所述获取所述无人水下航行器的第一航向角数据,包括:
4.根据权利要求1所述的状态估计方法,其特征在于,所述方法还包括:
5.根据权利要求1所述的状态估计方法,其特征在于,所述状态估计向量通过公式计算得到,所述为k时刻的状态估计向量,为根据k-1时刻的状态向量推测的k时刻状态向量,为k时刻的扩展卡尔曼增益系数,为观测实际值,为观测预测值。
6.根据权利要求1所述的状态估计方法,其特征在于,所述设置自适应事件触发条件,包括:
7.根据权利要求6所述的状态估计方法,其特征在于,所述门限阈值通过公式计算得到,所述为最大先验门限,为观测实际值,为观测预测值,为门限阈值。
8.根据权利要求6所述的状态估计方法,其特征在
9.一种无人水下航行器,其特征在于,所述无人水下航行器包括机身、传感器和控制器,所述传感器包括USBL换能器、DVL和IMU,所述控制器包括:
10.一种非易失性计算机可读存储介质,其特征在于,所述计算机可读存储介质存储有计算机可执行指令,当所述计算机可执行指令被无人水下航行器执行时,使所述无人水下航行器执行如权利要求1-8任一项所述的方法。
...【技术特征摘要】
1.状态估计方法,所述方法用于无人水下航行器,其特征在于,所述方法包括:
2.根据权利要求1所述的状态估计方法,其特征在于,所述无人水下航行器包括usbl换能器、dvl,所述获取所述无人水下航行器的测量数据包括:
3.根据权利要求2所述的状态估计方法,其特征在于,所述无人水下航行器还包括imu,所述获取所述无人水下航行器的第一航向角数据,包括:
4.根据权利要求1所述的状态估计方法,其特征在于,所述方法还包括:
5.根据权利要求1所述的状态估计方法,其特征在于,所述状态估计向量通过公式计算得到,所述为k时刻的状态估计向量,为根据k-1时刻的状态向量推测的k时刻状态向量,为k时刻的扩展卡尔曼增益系数,为观测实际值,为观测预测值。
6.根据权利要求1所述的状态估计...
【专利技术属性】
技术研发人员:王建华,尹土兵,王惠刚,郭亚北,刘顺发,王常乐,
申请(专利权)人:中南大学,
类型:发明
国别省市:
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。