基于Unscented FastSLAM算法的AUV自主导航方法
技术领域
[0001] 本
发明涉及一种AUV自主导航方法，尤其涉及一种基于Unscented Fast
SLAM算法的AUV自主导航方法，属于
机器人导航技术领域。
背景技术
[0002] AUV(
水下机器人)研究的重要课题即为自主导航的研究，自主导航确保机器人在已知环境中运动并执行任务，此时的先验地图是已知的。但是实际情况中的海底地图信息是未知的，因此要真正实现AUV自主导航就必须使得机器人具有同时
定位与地图构建的能
力。
[0003] 同时定位与地图构建(simultaneous localization andmapping，SLAM)可以使AUV在未知的环境中运动，并在运动中通过自身携带
传感器，如声纳、摄像机等，不断的采集周围的环境信息，实现自身
位姿的确定，同时增量式地构建全局环境地图。自Smith等提出基于
扩展卡尔曼滤波EKF的SLAM随机建图方法之后，EKF方法成为研究SLAM问题的主要方法。但随着SLAM问题研究的深入，EKF方法在实际应用中存在两个
缺陷。一是计算复杂度太高，至少与路标(环境特征)的个数的平方成正比，其平方复杂性难以满足创建大规模地图和实时性的要求；二是存在数据关联问题。
[0004] 针对EKF所存在的问题，Montemelo等首先提出了FastSLAM算法。FastSLAM将SLAM问题分解成机器人运动轨迹的估计和基于机器人位姿估计上的路标
位置估计两个过程。该算法用粒子滤波估计机器人的运动轨迹(每个粒子代表机器人的一条可能运动轨迹，利用观测信息计算每个粒子的权重，以评价每条轨迹的好坏)，而用EKF
滤波器来估计路标的位置(每个EKF滤波器对应一个相应的路标，其状态是二维的)，使其计算的复杂度降低。对于数据关联而言，在FastSLAM算法中，每个粒子的数据关联是相互独立的，当其中部分出现问题时，在重
采样阶段这部分粒子会被衰退掉而不影响机器人同时定位与地图创建的整体估计，从而可以提高数据关联的准确度。
[0005] 尽管FastSLAM很好的解决了EKF-SLAM算法所出现的复杂度和数据关联问题，但是在标准FastSLAM过程中，由于粒子滤波重采样会使具有较大权重的样本被多次选取，导致采样结果中样本多样性下降，使得粒子滤波器出现粒子贫乏问题，最终可能造成SLAM
精度的降低。同时，EKF估计高度非线性的运动模型和测量模型，可能会引起估计的发散和不一致。另一方面，EKF需要计算雅可比矩阵，具有一定复杂度。
申请号为“200910100962.3”的
专利文献公开了一种“基于改进重采样方法和粒子选取的FastSLAM算法”，但所采用的EKF算法一致性较差，使得AUV在自主导航时容易发散，并且所采用的重采样算法实时性不高；申请号为“201410156978.7”的专利文献公开了“一种基于无极FastSLAM算法和匹配优化目标定位方法及机器人”，其将粒子滤波和匹配优化定位算法相结合，但是状态估计精度不高，容易出现发散等问题。
发明内容
[0006] 本发明的目的在于提供一种基于Unscented FastSLAM算法的AUV自主导航方法，提供一种能够提高AUV自主导航精度，基于渐消自适应无味粒子滤波和自适应部分系统重采样的Unscented FastSLAM算法对采样过程中建议分布函数的产生进行改进，并且对粒子滤波中重采样的方法进行改进，通过这些改进来提高AUV自主导航精度。
[0007] 本发明的目的通过以下技术方案予以实现：
[0008] 一种基于Unscented FastSLAM算法的AUV自主导航方法，该方法包含下列步骤：
[0009] 1)初始化：AUV在水面通过传感器获得初始位姿信息；
[0010] 2)预测：根据输入AUV的最新控制量和各种传感器观测量采用无味粒子滤波UPF对AUV位姿和环境路标进行预测；
[0011] 3)采样：采用渐消自适应无味卡尔曼滤波AF-UKF来产生一种参数自适应调节的建议分布函数，并从建议分布函数中采样；
[0012] 4)地图更新：根据每个粒子关联最新观测环境信息，采用无味卡尔曼滤波UKF更新每个特征的估计；
[0013] 5)重采样：计算粒子集的有效粒子数，若有效粒子数小于设定的
阈值，则采用自适应部分系统重采样方法对粒子集进行重采样；
[0014] 6)根据重采样生成的新粒子按照以上步骤
迭代循环继续进行AUV定位和地图构建。
[0015] 本发明的目的还可以通过以下技术措施进一步实现：
[0016] 前述基于Unscented FastSLAM算法的AUV自主导航方法，其中步骤1)初始化过程包括通过全球定位系统GPS获得初始位置信息；通过多普勒计程仪DVL、
加速度计、
陀螺仪获得初始速度和
姿态信息；其中，全球定位系统GPS获取的AUV的初始位置信息为经度信息和纬度信息；多普勒计程仪DVL采集到的AUV速度信息为前进速度信息、横向速度信息、垂直速度信息中的任一种或任几种；陀螺仪采集到的AUV姿态
角信息包括航偏角信息、
横滚角信息、
俯仰角信息中的任一种或任几种。
[0017] 前述基于Unscented FastSLAM算法的AUV自主导航方法，其中步骤2)根据输入AUV的最新控制量和各种传感器观测量，及AUV运动
预测模型和传感器测量模型对每个粒子中的AUV位姿和对应路标采用无味粒子滤波UPF进行预测，所述AUV运动预测方程和传感器测量方程分别为：
[0018] xt＝f(xt-1,wt-1) (1)
[0019] zt＝h(xt,vt) (2)
[0020] 式中，xt、zt分别为t时刻系统的状态向量和观测向量，f(·)、h(·)分别为状态和观测的非线性函数，wt、vt为具有零均值和协方差分别为Qt和Rt的过程噪声和观测噪声。
[0021] 前述
基于UnscentedFastSLAM算法的AUV自主导航方法，其中步骤3)中采用渐消自适应无味粒子滤波AF-UPF来产生一种参数自适应调节的建议分布函数，并从建议分布函数中采样，渐消自适应无味粒子滤波AF-UPF通过以下步骤实现：
[0022] 1)状态增广：
[0023]
[0024]
[0025] 式中，T为矩阵转置符号， 为增广状态向量， 为先验状态向量， 为先验x轴状态， 为先验y轴状态， 为先验航向角， 为增广协方差矩阵， 为协方差矩阵；
[0026] 2)通过对称采样得到L＝2N+1个sigma点，N为估计向量的维数：
[0027]
[0028]
[0029]
[0030] 式中， 为第k个采样点，k为矩阵的第k列，λ＝α2(N+κ)-N为尺度参数，常量α决定了Sigma点在均值附近的分布，κ是次级尺度调节参数，一般设置为0；这些sigma点通过非线性函数进行传递，产生预测的sigma样本点：
[0031]
[0032]
[0033] 式中， 为系统状态预测， 为系统观测预测；
[0034] 3)对sigma样本点进行加权处理，得到输出变量的先验估计。其中系统状态的均值和协方差估计为：
[0035]
[0036]
[0037] 式中， 为系统状态均值估计， 为系统状态协方差估计，Σ为求和符号，和 分别为第k个sigma点的权值的均值和协方差；
[0038] 观测量的均值和协方差估计为：
[0039]
[0040]
[0041] 式中，为观测的均值估计， 为观测的协方差估计；
[0042] 系统状态与观测的协方差估计 为：
[0043]
[0044] UPF的增益Kt为：
[0045]
[0046] 4)利用渐消滤波思想采用一步算法计算渐消因子αt：
[0047] αt＝max{1,tr(Nt)tr(Mt)-1} (16)
[0048] 式中，tr()为求迹符号，max{}为取其中最大的数值，Nt和Mt在非线性滤波中计算公式如下：
[0049]
[0050]
[0051] 式中， 为观测值，h(xt)为预测值， 表示观测函数对x求偏导， 表示状态函数对x求偏导；
[0052] 5)将渐消滤波与UPF相结合，计算出更新后状态的均值 和方差Pti：
[0053]
[0054]
[0055] 得到建议分布函数 由渐消因子αt对权值方差Pti自适应调节。
[0056] 前述基于Unscented FastSLAM算法的AUV自主导航方法，其中步骤5)计算粒子集的有效粒子数，若有效粒子数小于设定的阈值，则采用自适应部分系统重采样方法对粒子集进行重采样，其中自适应部分系统重采样方法通过以下设计步骤实现：
[0057] 1)进行重采样判定：
[0058] 计算粒子集中粒子权值的大小并且归一化为 然后再根据有效粒子数Neff与设定的有效粒子数阈值Ng相比较，如果Neff＜Ng则进行重采样，否则不进行，有效粒子数由式(21)计算：
[0059]
[0060] 2)进行粒子优化组合：
[0061] 根据有效粒子数设定两个权值的
门限值wh，wl(0＜wl＜wh)，将每个粒子的权值与门限值一一比较之后优化组合为两部分，设 和 的粒子为权值不稳定粒子，设的粒子为权值适中的粒子，仅对权值不稳定粒子进行系统重采样后，再与未进行重采样的粒子组成新的粒子集，从而增加粒子的多样性。
[0062] 与
现有技术相比，本发明的有益效果是：
[0063] 1.本发明采用的Unscented FastSLAM算法相对于其他SLAM算法来说，在外界干扰强、环境尺度大的情况下对AUV自主导航的鲁棒性和准确性都更好；
[0064] 2.所采用的渐消自适应无味粒子滤波AF-UKF应用在FastSLAM算法中，能够融入最新环境观测值，利用渐消因子自适应的调节权值，充分提取有效信息，使得建议分布函数更加贴近后验分布函数，提高算法预测过程的粒子采样效率，保证了粒子的收敛性；
[0065] 3.所采用的自适应部分系统重采样，能够减少重采样的时间并且增加粒子的多样性，从而提高AUV使用UnscentedFastSLAM进行完全自主导航的精确性。
附图说明
[0066] 图1是应用于AUV的基于UnscentedFastSLAM算法
流程图；
[0067] 图2是自适应部分系统重采样方法的流程图。
具体实施方式
[0068] 下面结合附图和具体
实施例对本发明作进一步说明。
[0069] 如图1所示，是采用基于渐消自适应无味卡尔曼滤波结合自适应部分系统重采样方法的应用于AUV的UnscentedFastSLAM算法流程图，描述了改进的UnscentedFastSLAM的处理过程；具体步骤如下：
[0070] 步骤1：AUV在水面通过全球定位系统GPS获得初始位置信息；通过多普勒计程仪DVL、加速度计、陀螺仪这些传感器获得初始速度和姿态信息。
[0071] 其中，全球定位系统获取的AUV的初始位置信息为经度信息和纬度信息；多普勒计程仪采集到的AUV速度信息为前进速度信息、横向速度信息、垂直速度信息中的任一种或任几种；导航传感器采集到的AUV姿态角信息包括航偏角信息、横滚角信息、俯仰角信息中的任一种或任几种。
[0072] 步骤2：预测：根据输入AUV的最新控制量和各种传感器观测量，及AUV运动预测模型和传感器测量模型对每个粒子中的AUV位姿和对应路标(环境特征)采用无味粒子滤波UPF进行预测。
[0073] 以步骤1的初始位置信息、速度和姿态角信息定义AUV的状态向量、观测向量、状态模型方程和观测模型方程。它的
状态方程和测量方程可以由以下方程描述：
[0074] xt＝f(xt-1,wt-1) (1)
[0075] zt＝h(xt,vt) (2)
[0076] 式中，xt、zt分别为t时刻系统的状态向量和观测向量，f(·)、h(·)分别为状态和观测的非线性函数，wt、vt为具有零均值和协方差分别为Qt和Rt的过程噪声和观测噪声。
[0077] 步骤3：采用渐消自适应无味卡尔曼滤波AF-UKF来产生一种参数自适应调节的建议分布函数，并从建议分布函数中采样，AF-UKF的本质思想是将渐消滤波与UKF融合，渐消自适应无味粒子滤波通过以下设计步骤实现：
[0078] 1)状态增广：
[0079]
[0080]
[0081] 式中，T为矩阵转置符号， 为增广状态向量， 为先验状态向量， 为先验x轴状态， 为先验y轴状态， 为先验航向角， 为增广协方差矩阵， 为协方差矩阵。
[0082] 2)通过对称采样得到L＝2N+1个sigma点，N为估计向量的维数：
[0083]
[0084]
[0085]
[0086] 式中， 为第k个采样点，k为矩阵的第k列，λ＝α2(N+κ)-N为尺度参数，常量α(0＜α＜1)决定了Sigma点在均值附近的分布，κ是次级尺度调节参数，一般设置为0。这些sigma点通过非线性函数进行传递，产生预测的sigma样本点：
[0087]
[0088]
[0089] 式中， 为系统状态预测， 为系统观测预测。
[0090] 3)对sigma样本点进行加权处理，得到输出变量的先验估计。其中系统状态的均值和协方差估计为：
[0091]
[0092]
[0093] 式中， 为系统状态均值估计， 为系统状态协方差估计，Σ为求和符号，和 分别为第k个sigma点的权值的均值和协方差。
[0094] 观测量的均值和协方差估计为：
[0095]
[0096]
[0097] 式中，为观测的均值估计， 为观测的协方差估计。
[0098] 系统状态与观测的协方差估计 为：
[0099]
[0100] UPF的增益Kt为：
[0101]
[0102] 4)利用渐消滤波思想采用一步算法计算渐消因子αt：
[0103] αt＝max{1,tr(Nt)tr(Mt)-1} (16)
[0104] 式中，tr()为求迹符号，max{}为取其中最大的数值，Nt和Mt在非线性滤波中计算公式如下：
[0105]
[0106]
[0107] 式中， 为观测值，h(xt)为预测值， 表示观测函数对x求偏导， 表示状态函数对x求偏导。
[0108] 5)将渐消滤波与UPF相结合，计算出更新后状态的均值 和方差Pti：
[0109]
[0110]
[0111] 得到建议分布函数 由渐消因子αt对权值方差Pti自适应调节，可以充分提取有效信息，抑制异常干扰对状态模型和观测模型的不利影响，使建议分布更加接近真实后验分布。
[0112] 步骤4：根据每个粒子关联最新观测环境信息，采用无味卡尔曼滤波UKF更新每个特征的估计；
[0113] 步骤5：如图2所示，为自适应部分系统重采样方法的算法流程图。描述了改进后Unscented FastSLAM过程中粒子滤波重采样的处理过程，自适应部分系统重采样方法通过以下设计步骤实现：
[0114] 1)进行重采样判定：
[0115] 计算粒子集中粒子权值的大小并且归一化为 然后再根据有效粒子数Neff与设定的有效粒子数阈值Ng相比较，如果Neff＜Ng则进行重采样，否则不进行，有效粒子数由式(21)计算：
[0116]
[0117] 2)进行粒子优化组合：
[0118] 根据有效粒子数设定两个权值的门限值wh，wl(0＜wl＜wh)，将每个粒子的权值与门限值一一比较之后优化组合为两部分，一部分为 和 的权值较小或者较大的不稳定粒子，另一部分为 的权值适中的粒子，仅对权值不稳定的那部分粒子进行系统重采样后，再与未进行重采样的粒子组成新的粒子集，从而增加粒子的多样性。
[0119] 步骤6：根据重采样生成的新粒子按照以上步骤迭代循环继续进行AUV定位和地图构建。
[0120] 本发明对应用于AUV自主导航的Unscented FastSLAM算法做出改进，提出基于渐消自适应无味粒子滤波的建议分布，该算法在采样过程中融入渐消滤波，通过渐消因子产生可自适应调节的建议分布函数，使其更加接近后验分布函数，在提高粒子采样效率的同时，根据粒子权重大小将粒子进行分配组合后再对部分粒子进行重采样，保证了粒子的多样性，避免了粒子的贫化现象。同时采用自适应部分系统重采样方法来改进粒子滤波退化的问题，自适应调整重采样的时间，增加粒子的多样性，使得AUV使用Unscented FastSLAM算法进行自主导航的精度得到提高。
[0121] 除上述实施例外，本发明还可以有其他实施方式，凡采用等同替换或等效变换形成的技术方案，均落在本发明要求的保护范围内。
