【技术实现步骤摘要】
本专利技术涉及自主水下机器人(auv)同步定位与地图创建领域，具体来说是一种基于引力场优化的二阶中心差分滤波auv-slam算法，属于机器人导航。
技术介绍
1、自主式水下机器人(autonomous underwater vehicle，简称auv)同遥控式水下机器人(remotely operated vehicle，简称rov)相比更加灵活，因此自主式水下机器人越来越受到各国海洋和军事部门的重视。
2、目前海洋考察的范围越来越大，从近岸发展到远洋，从海面上发展到海水中，从浅海领域发展到深海空间，这就对auv提出更高的要求。auv适于在近海、浅水海区或者狭窄海域执行特种任务，所以非常适合发展成为智能化的水下装备。auv在和平时期，可执行海底测绘、资源勘探、管道维护、海洋勘察等任务；在战争时期可执行敌方目标的探测与定位、战场的侦査和监视等任务。因此，auv是具有十分重要的研究意义。
3、智能水下机器人要实现自治，即不依赖于人的自主识别和分辨能力，首先要具备独立的视觉系统和自定位系统，智能水下机器人的视觉系统主要依靠“声视觉”。作为完全自治的水下机器人，声视觉系统就是它的耳目，通过声视觉系统，机器人才能获取水下的环境信息，为其运动和水下作业提供引导。与传统的声呐系统不同，声视觉系统不仅要有声图像和声信息的获取能力，而且应该具备对图像和信息的处理、特征提取以及分类和识别的功能。同时定位与地图创建(simultaneous localization and mapping，slam)是机器人
研究热点结
4、由于水下机器人slam大多要处理的是非线性系统，其运动方程和观测方程都是带有高斯噪声的非线性模型。所以很多时候采用扩展卡尔曼滤波来估计机器人的运动状态和环境信息，然而，此方法实时性较差，计算量极大，并且计算结果的精度不高。所以将粒子滤波算法应用到非线性滤波中，为slam的研究提供了新的思路，将粒子滤波与raoblackwellization算法相结合，提出fastslam算法，由于降维处理，使算法的复杂度大大降低，从而能够有效地解决slam问题。fastslam算法性能相比于ekf-slam算法大有提升，但是仍存在一些问题，例如粒子耗尽问题，会导致粒子多样性损失和估计精度下降，此外，粒子的提议分布也会对估计精度有影响。针对这些问题，提出提升slam算法性能的策略，对提高智能水下机器人基于声呐系统的智能化水平和促进slam技术的发展具有重要意义。
技术实现思路
1、本专利技术的目的在于提供一种基于引力场优化的二阶中心差分滤波auv-slam算法，该算法基于sterling插值公式，只利用有限点的函数值，而不是对函数求导，从而取得更简单的逼近公式。采用二阶中心差分滤波方法并融合最新观测数据来产生重要性分布函数，对其性能进行验证。针对粒子退化问题以及样本贫化等现象，采用引力场优化对粒子分布进行调整，使得粒子集能够更快速地朝着真实的机器人位姿状态逼近并使其更快收敛。
2、本专利技术的目的通过以下技术方案予以实现：
3、一种基于引力场优化的二阶中心差分滤波auv-slam算法，该方法包含下列步骤：
4、(1)初始化：获取auv初始位姿信息；
5、(2)预测：(2.1)根据auv的运动预测方程传感器观测方程，基于多元函数二阶中心插值近似公式(scdf)，计算每个粒子的一阶均差向量和二阶均差向量，从而获得auv的位姿估计；(2.2)根据求得的位姿估计，获得auv在t时刻的状态预测值；
6、(3)采样：使用卡尔曼滤波算法对auv进行状态更新，产生建议分布函数。并从建议分布函数中采样n个粒子，通过权值计算公式计算每个粒子的权值。通过引力场优化算法优化粒子集，在优化过程中通过计算每个粒子所受的斥力和引力得出粒子所受合力，根据所受合力移动粒子，得到优化后的粒子集。最后重新计算权值并且做归一化处理；
7、(4)重采样：当有效粒子数小于给定阈值时，则对粒子集进行重采样获得新的粒子集，否则就不进行重采样；
8、(5)地图位姿更新：根据采样优化后输出的粒子状态关联的环境观测信息，采用卡尔曼滤波对路标位置进行估计，更新当前时刻地图信息和位姿信息；
9、(6)按照以上步骤迭代，进行auv的同时定位与地图构建。
10、进一步地，本专利技术的目的还可以通过一下技术措施实现：
11、前述基于引力场优化的二阶中心差分滤波auv-slam算法，步骤(1)具体包括：
12、通过全球定位系统获取auv的初始经纬度信息，也可以通过超短基线usbl与基站之间的相对位置获取auv位置信息；通过多普勒计程仪和陀螺仪获取auv的初始速度和姿态信息，包括三轴速度、航偏角信息、横滚角信息、俯仰角信息。
13、前述基于引力场优化的二阶中心差分滤波auv-slam算法，步骤(2)具体包括：
14、根据auv的运动预测方程传感器观测方程，基于多元函数二阶中心插值近似公式算法，计算每个粒子的一阶均差向量和二阶均差向量。
15、步骤(2.1.1)所述auv运动预测方程和传感器观测方程分别为：
16、xt＝f(xt-1,ut,wt) (1)
17、yt＝h(xt,vt) (2)
18、式中，xt、yt分别为t时刻系统的状态向量和观测向量，xt-1为t-1时刻系统的状态向量，ut为t时刻的运动控制，f(·)、h(·)分别为状态和观测的非线性函数，wt为满足均值为零，协方差为qt的过程噪声，vt为满足均值为零，协方差为rt的观测噪声；
19、步骤(2.1.2)采用cholesky分解法可以得到：
20、
21、
22、q为过程噪声协方差矩阵r为观测噪声协方差矩阵，和分别为预测协方差和估计协方差，sv与sw分别为过程噪声协方差矩阵和观测噪声协方差矩阵的因子，预测协方差的因子和估计协方差的因子用于调整协方差矩阵中的元素，以反映状态预测和观测的不确定性，在滤波过程中不断的被更新和修正；
23、步骤(2.1.3)设x∈rn为n维矢量，则y＝y(x)在处用stirling插值公式展开为：
24、
25、式中和分别表示一阶差分和二阶差分算子；
26、步骤(2.1.4)基于多元函数二阶中心插值近似公式算法，计算每个粒子的一阶均差向量：
27、
28、
29、
30、
<本文档来自技高网...【技术保护点】
1.一种基于引力场优化的二阶中心差分滤波AUV-SLAM算法，其特征在于，该方法包含下列步骤：
2.如权利要求1所述的基于引力场优化的二阶中心差分滤波AUV-SLAM算法，其特征在于所述步骤(1)中，通过全球定位系统获取AUV的初始经纬度信息，也可以通过超短基线USBL与基站之间的相对位置获取AUV位置信息；通过多普勒计程仪和陀螺仪获取AUV的初始速度和姿态信息，包括三轴速度、航偏角信息、横滚角信息、俯仰角信息。
3.如权利要求1所述的基于引力场优化的二阶中心差分滤波AUV-SLAM算法，其特征在于所述步骤(2.1)中，根据AUV的运动预测方程传感器观测方程，基于多元函数二阶中心插值近似公式算法，计算每个粒子的一阶均差向量和二阶均差向量，具体方法如下：
4.如权利要求1所述的基于引力场优化的二阶中心差分滤波AUV-SLAM算法，其特征在于，所述步骤(2.2)中，根据求得的位姿估计，获得AUV在t时刻的状态预测值具体方法如下；
5.如权利要求1所述的基于引力场优化的二阶中心差分滤波AUV-SLAM算法，其特征在于，所述步骤(3)使用卡
6.如权利要求5所述的基于引力场优化的二阶中心差分滤波AUV-SLAM算法，其中使用卡尔曼滤波算法对AUV进行状态更新，产生建议分布函数，并通过引力场优化算法优化粒子集中，为了达到最优调整效果公式(24)中的移动权重M取值为0.0618，公式(25)中的排斥系数f最大值fmax为0.3。
...【技术特征摘要】
1.一种基于引力场优化的二阶中心差分滤波auv-slam算法，其特征在于，该方法包含下列步骤：
2.如权利要求1所述的基于引力场优化的二阶中心差分滤波auv-slam算法，其特征在于所述步骤(1)中，通过全球定位系统获取auv的初始经纬度信息，也可以通过超短基线usbl与基站之间的相对位置获取auv位置信息；通过多普勒计程仪和陀螺仪获取auv的初始速度和姿态信息，包括三轴速度、航偏角信息、横滚角信息、俯仰角信息。
3.如权利要求1所述的基于引力场优化的二阶中心差分滤波auv-slam算法，其特征在于所述步骤(2.1)中，根据auv的运动预测方程传感器观测方程，基于多元函数二阶中心插值近似公式算法，计算每个粒子的一阶均差向量和二阶均差向量，具体方法如下：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。