【技术实现步骤摘要】
本专利技术涉及水下机器人控制技术，尤其涉及一种基于多传感器数据融合的水下机器人状态估计方法。
技术介绍
1、水下机器人是水下代替或辅助人工进行作业的智能装备，在海底地形测绘，海洋资源勘探以及水下搜救等各种场景中发挥着巨大的作用。水下机器人在水下进行作业离不开对环境和自身状态的精确感知，也即对自身搭载的姿态、速度等传感器的精度有很高的要求。
2、对于大型的水下机器人，可以直接选用体积质量相对大但精度高的传感器，但是对于微小型水下机器人来说，受限于体积和成本，往往只能采用mems惯性传感器、深度计和小型多普勒计程仪来组成一套经济的位姿传感系统。其中，mems惯性传感器能提供水下机器人的姿态角和加速度数据，深度计提供深度数据，而多普勒计程仪提供水下机器人的速度数据。这些传感器一般精度不高、噪声较大，再加上复杂水下环境造成的干扰，会对水下机器人的作业产生很大的影响，严重时甚至会导致水下机器人的意外失控。
3、现有研究提出，使用一些简单的均值滤波或低通滤波算法可以对每一个单一的传感器数据进行滤波处理，以降低传感器噪声和杂波的干扰。但这些算法只能处理单一的传感器数据，当传感器本身出现故障时就会直接失去作用。
4、因此，提出一种适用于水下机器人的多传感器融合估计方法，是具有现实需求的。
技术实现思路
1、本专利技术要解决的技术问题是，克服现有技术中的不足，提供一种基于多传感器数据融合的水下机器人状态估计方法。
2、为解决技术问题，本专利技术的解决方
3、提供一种基于多传感器数据融合的水下机器人状态估计方法，包括以下步骤：
4、(1)根据水下机器人的运动模型和对应的传感器类型及数量，建立状态转移方程和观测方程；
5、(2)读取当前水下机器人的姿态角数据，对状态转移矩阵进行更新；
6、(3)利用状态转移矩阵对当前状态进行预测，得到状态预测值
7、(4)利用协方差矩阵进行预测，得到协方差矩阵预测值
8、(5)基于cholesky分解和三角矩阵求逆算法，求解卡尔曼增益矩阵k
9、(6)读取当前水下机器人的加速度、速度和深度数据，进行状态更新，并计算状态估计值
10、(7)对协方差矩阵进行更新，计算当前的协方差矩阵估计值
11、(8)循环执行步骤(2)至(7)，获取水下机器人的实时状态估计值。
12、该实时状态估计值将用于水下机器人的姿态控制，即，基于目标动作状态值与实时状态估计值之间的差值，根据预设动作策略对水下机器人的姿态进行调整。
13、与现有技术相比，本专利技术的有益效果是：
14、(1)本专利技术基于卡尔曼滤波算法，结合水下机器人的运动模型和对应的传感器观测模型，实现了多传感器数据的融合；基于该估计方法，能够有效滤除传感器噪声和环境噪声的干扰，提高传感器的数据质量。
15、(2)本专利技术在估计方法的实现过程中，使用了矩阵cholesky分解和三角阵求逆的技巧，能够有效降低整个算法的计算复杂度，从而提高了算法的工程实用性。
本文档来自技高网...【技术保护点】
1.一种基于多传感器数据融合的水下机器人状态估计方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的方法，其特征在于，所述步骤(1)具体包括：
3.根据权利要求1所述的方法，其特征在于，所述步骤(2)具体包括：
4.根据权利要求1所述的方法，其特征在于，所述步骤(3)具体包括：
5.根据权利要求1所述的方法，其特征在于，所述步骤(4)具体包括：
6.根据权利要求1所述的方法，其特征在于，所述步骤(5)具体包括：
7.根据权利要求1所述的方法，其特征在于，所述步骤(6)具体包括：
8.根据权利要求1所述的方法，其特征在于，所述步骤(7)具体包括：
【技术特征摘要】
1.一种基于多传感器数据融合的水下机器人状态估计方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的方法，其特征在于，所述步骤(1)具体包括：
3.根据权利要求1所述的方法，其特征在于，所述步骤(2)具体包括：
4.根据权利要求1所述的方法，其特征在于，所述步骤(3)具体包括：...
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。