基于FMSRUPF算法的AUV对接回收自主导航方法与流程

本发明涉及一种基于fmsrupf算法的auv对接回收自主导航方法，属于机器人技术领域。

背景技术：

auv(autonomousunderwatervehicle无缆自主水下机器人)，这类水下机器人通过自身携带的能源设备提供动力，具有下潜深度深、活动范围广、隐蔽性高等特点，但是由于自身能源限制，auv作业时间有限，这就需要及时回收机器人进行能源补给和维修。在实际回收过程中，复杂的水下环境和水下回收的高精度的控制要求又使得auv回收困难重重，怎样精确节能的回收auv更是成为当前水下机器人研究的重中之重。

在回收过程中，导航问题仍然是面临的主要技术挑战之一。导航系统必须为水下运载器提供远距离、长时间范围内的精确姿态、速度及位置信息。精确的导航能力是水下运载器有效应用和安全回收的一个关键技术，但由于水下机器人质量轻、惯性小，对噪声响应灵敏，加上有限的测量传感器，实现水下运载器的精确导航是一项复杂而艰巨的任务。水声定位系统在水下的特殊环境中具有独特的优势，水声信号在水中传播速度快且相对的能量衰减率小，水声定位系统可以提供三维的位置信息，定位精度较高，动态性能较好。

usbl(ultra-shortbaseline超短基线定位系统)利用声学定位技术实现对水下目标的定位和跟踪。该导航系统定位精度较高，机动范围较大，可以适应深海复杂的作业环境。与其他基于声学定位的基线系统相比，usbl基线尺寸较小(一般在几厘米至十几厘米的量级)、灵活性高、操作简单、成本较低，因此在水下作业和海洋开发等工程领域得到了广泛应用，但是usbl由于易受海洋环境的影响，经常会出现数据跳动或数据丢失现象，连续性差，在某些需要高精度导航定位的场合受到较大的限制。

sins(strapdowninertialnavigationsystems捷联惯性导航系统)利用陀螺仪和加速度计测量载体相对于惯性空间的角速度和加速度信息，通过解算可求出载体的三维速度、位置和姿态信息。sins不需要任何外来信息，也不向外辐射信息，仅依靠系统自身就能在全球范围内实现全天候、全天时、自主、隐蔽、连续的定位和定向。该系统已被广泛地应用于航海、航空、航天、交通等各个领域。但是，随着系统工作时间的延长，sins系统的导航误差会随之积累增长，此时就需要利用外部传感器的观测信息通过滤波算法来修正补偿sins系统，以抑制其随时间积累的误差。

dvl(dopplervelocitylog多普勒计程仪)是广泛应用于水下及水面航行器组合系统的速度测量装置，具有实时输出载体三维速度与航程能力的声学导航定位系统。dvl系统利用安装在载体上的超声换能器向海底发射超声波，并根据多普勒效应原理测量载体速度，具有抗干扰能力强、反应快、隐蔽性好、速度测量精确和稳定性高等优点。但是当dvl的波束超出了有效射程范围，测速精度会严重下降，且dvl的测速精度还受到水温、水压、地形和盐度等外界因素的影响。

由于sins、usbl、和dvl都存在各自的优缺点，采用先进的信息融合技术将sins、usbl和dvl者有机结合起来，互相取长补短，构成新一代水下运载器sins/usbl/dvl自主组合导航系统。该组合导航系统既能克服纯惯性导航系统误差积累问题，又能适用于水下导航定位的特殊环境，为水下载体和目标提供高精度的导航信息，是一种精度高、可靠性好、容错性强的水下导航系统，在军用和民用领域具有广阔的应用前景。因此，对水下运载器sins/usbl/dvl组合导航系统的研究具有重要的理论意义和工程应用价值。

在auv回收组合导航系统模型和噪声统计特性精确已知时，kalman滤波是最优估计，精度高，但是在实际应用中，只能建立近似的数学模型并且很难获得精确的噪声统计特性，若系统模型精确性差或者系统噪声为非高斯白噪声时，kalman滤波精度下降甚至出现滤波发散，难以实现系统高精度导航跟踪定位功能。申请号为“201010559361.1”的专利文献公开了“一种小型水下机器人组合导航定位方法”，其所采用的无迹卡尔曼滤波算法一致性较差，使得auv在自主导航时容易发散；申请号为“201410156978.7”的专利文献公开了“基于sins/dvl/gps的auv组合导航系统”，其将kalman滤波和h无穷滤波算法相结合，但是状态估计精度不高，容易出现发散等问题。

针对这样的问题，在研究衰减记忆滤波和平方根滤波的基础上，以无迹粒子滤波为框架，提出一种fmsrupf(衰减记忆平方根无迹粒子滤波)算法。该算法利用衰减因子减小历史信息对滤波的影响，增强当前量测信息在滤波计算中的作用，然后用协方差矩阵的平方根阵代替协方差矩阵进行滤波解算，保证协方差矩阵的对称性和非负定性，最后用系统组合粒子重采样方法对重采样过程进行改进，提高滤波算法数值稳定性和解算精度。

技术实现要素：

本发明的目的在于提供一种基于fmsrupf算法的auv对接回收自主导航方法，针对auv对接回收基于sins/usbl/dvl的自主导航，采用基于fmsrupf算法的滤波方法，即衰减记忆平方根无迹粒子滤波算法，并对其重采样部分采用系统组合粒子重采样方法，对历史信息和协方差进行改进，提高滤波算法的精度和稳定性。

本发明的目的通过以下技术方案予以实现：

一种基于fmsrupf算法的auv对接回收自主导航方法，包括以下步骤：

1)初始化：auv在水面通过传感器获得初始位姿信息；

2)状态更新：根据输入auv的最新控制量和各种传感器观测量，及auv运动状态模型方程和量测方程采用fmsrupf算法对每个粒子进行时间更新和量测值更新；

3)权值更新：根据每个粒子关联最新观测信息，对每个粒子进行权值更新并归一化权值；

4)重采样：若粒子集的有效粒子数小于设定的阈值，则采用系统组合粒子重采样方法对粒子集进行重采样；

5)状态估计更新：根据重采样生成的新粒子得到auv的状态估计，并按照以上步骤迭代循环继续进行auv对接回收自主导航。

本发明的目的还可以通过以下技术措施来进一步实现：

前述基于fmsrupf算法的auv对接回收自主导航方法，步骤1)所述初始位姿信息从全球定位系统gps、捷联惯性导航系统sins、多普勒计程仪dvl、超短基线定位系统usbl、测深仪这些传感器获得；其中，全球定位系统gps采集到auv的经度信息和纬度信息中的任一种或任几种；捷联惯性导航系统sins采集到auv的三维速度、位置信息和姿态角信息中的任一种或任几种，位置信息包括纬度、经度和高度信息，姿态角信息包括航向角、俯仰角和横滚角信息；超短基线定位系统usbl采集到回收装置在auv的基阵坐标系内相对于三轴x轴、y轴和z轴的角度信息和分别投影到三轴x轴、y轴和z轴上的距离信息中的任一种或任几种；多普勒计程仪dvl采集到的auv速度信息为前进速度信息、横向速度信息、垂直速度信息中的任一种或任几种；测深仪采集到auv的深度信息。

前述基于fmsrupf算法的auv对接回收自主导航方法，步骤2)所述auv运动状态模型方程和量测方程由下式描述：

xk＝f(xk-1,wk-1)(1)

yk＝h(xk,vk)(2)

式中，xk、yk分别为k时刻系统的状态向量和观测向量，xk-1为k-1时刻系统的状态向量，f(·)、h(·)分别为状态和观测的非线性函数，wk-1、vk为具有零均值和协方差分别为qk-1和rk的过程噪声和观测噪声，qk-1和rk分别为噪声wk-1和vk的协方差矩阵。

前述基于fmsrupf算法的auv对接回收自主导航方法，步骤2)所述采用fmsrupf算法对每个粒子进行时间更新和量测值更新，包括以下步骤：

1)数据采集：

通过传感器系统获取auv在水下的位置信息，采集auv的初始位姿信息；

2)滤波定位：

2.1)定义状态初始条件

当k＝0时，假定抽样出的每个粒子用表示，其中，表示初始时刻第i个粒子，p(x0)表示初始时刻概率密度函数，表示初始时刻第i个粒子的估计误差协方差，e[·]表示估计误差协方差，为初始时刻协方差矩阵，t为矩阵转置符号，表示初始时刻第i个分解因子，表示第i个粒子的初始权值，chol{·}表示矩阵cholesky分解，n为粒子个数；

2.2)当k＝1,2,…,n，用fmsrupf算法更新粒子；

①计算每个粒子的sigma点和权值

其中，λ为尺度参数，表示初始sigma点，表示第j个sigma点，ωj为其权值；和分别为初始sigma点均值和方差的权值，和分别对应第j个sigma点均值和方差的权值，α表示sigma点围绕均值扩散程度，决定了sigma点集到均值点的距离，是一个非负的权系数，它可以用于合并随机变量分布的先验信息；

②构造衰减因子αk；

构造αk采用一步算法，其解析式为：

式中，tr(·)为矩阵求迹符号，max{}为取其中最大的数值，n为粒子数，mk和nk的表达式分别为：

其中，hk表示观测函数对x求偏导，fk表示状态函数对x求偏导，vk＝yk-h(xk)，yk为观测值，h(xk)为预测值，pk为状态协方差矩阵，qk和rk分别为噪声wk和vk的协方差矩阵；上述αk的求解过程中，当vk增大，由上式(7)计算所得的nk增大，αk也相应增大；

③时间更新：

式中，表示fmsrupf算法的预测sigma点，表示k-1时刻的粒子的sigma点，ωk-1表示k-1时刻sigma点的权值，表示第i个粒子预测状态估计误差协方差，σ为求和符号，表示第i个预测分解因子，和分别表示fmsrupf算法的第j个和第1到2l个预测sigma点，l表示sigma点的个数，qr{·}表示矩阵qr分解；

无迹粒子滤波的预测协方差阵为

fmsrupf算法的预测协方差阵为

利用衰减因子αk对进行调节；

其中，cholupdate{·}为更新因子，为的平方根矩阵；

其中，为更新的系统状态估计值；

④测量更新：

根据无迹变换中sigma点采样策略，通过非线性测量函数h(·)传播为即

k时刻的预测量测向量为

用于预测观测的第i个分解因子为

用于预测观测的第i个更新分解因子为

加权新sigma点集用于预测观测的协方差矩阵为

状态测量的协方差矩阵为

k时刻粒子的增益矩阵为

其中，表示每个粒子的预测sigma点，表示第j个sigma点的预测量测值，表示第i个粒子预测量测误差协方差，表示第j个粒子的预测sigma点，第j个sigma点和第1至2n个sigma点通过非线性测量函数h(·)传播分别为和表示状态测量的预测协方差矩阵，表示每个粒子的更新sigma点，为k时刻第i个粒子的量测值，为k时刻第i个分解因子，为k时刻协方差矩阵。

前述基于fmsrupf算法的auv对接回收自主导航方法，步骤4)所述系统组合粒子重采样方法采用以下步骤进行计算：

1)进行重采样判定：

根据有效粒子数neff与设定的有效粒子数阈值ng相比较，如果neff＜ng则进行重采样，否则不进行，有效粒子数由式(25)计算；

其中，为归一化粒子权值；

2)进行粒子优化组合：

根据有效粒子数设定两个权值的门限值ωh，ωl，且0＜ωl＜ωh，将每个粒子的权值与门限值一一比较之后优化组合为两部分，一部分为权值小于ωl的粒子和权值大于ωh的粒子，这些粒子都是不稳定粒子，另一部分为的权值适中的粒子，仅对不稳定粒子进行系统重采样后，再与未进行重采样的粒子组成新的粒子集，从而增加粒子的多样性。

与现有技术相比，本发明的有益效果是：

1.本发明采用的fmsrupf算法相对于其他无迹粒子滤波算法来说，在外界干扰强、环境尺度大的情况下对auv对接回收自主导航的鲁棒性、稳定性和准确性都更好；

2.所采用的fmsrupf算法是在已有算法的基础上进行改进，首次在upf算法中融入衰减记忆滤波和平方根滤波算法，其中衰减记忆滤波算法即通过衰减因子限制卡尔曼滤波器的记忆长度，减小历史信息对滤波的影响，增强当前量测信息在滤波计算中的作用，保证了算法预测过程的粒子采样效率，提高了滤波精度；

3.所采用的结合平方根滤波的目的在于用协方差矩阵的平方根阵代替协方差阵进行迭代运算，保证了协方差矩阵的非负定性，避免了产生滤波发散的一个发散源；

4.所采用的系统组合粒子重采样方法，能够减少重采样的时间并且增加粒子的多样性，从而提高auv使用fmsrupf算法进行对接回收自主导航的精确性。

附图说明

图1是本发明auv回收对接流程框图；

图2是本发明超短基线水下定位原理图；

图3是本发明fmsrupf算法流程图。

具体实施方式

下面结合附图和具体实施例对本发明作进一步说明。

如图1所示，直线归位阶段1，这个阶段是指回收装置5用超短基线定位到auv自主航行器6开始，到达直线跟踪点2，从而进入直线跟踪阶段3的跟踪过程；利用超短基线提供的相对位置、姿态信息来调整auv与回收装置的位置，使得在消耗较短对接中轴线距离的情况下，将auv航行至中轴线7上，使auv的姿态与中轴线一致，从而进入艏向调整阶段4，利于进一步实时对接。

如图2所示，usbl(ultra-shortbaseline超短基线定位系统)发射基阵和接收基阵固定在换能器中组成声基阵12安装在auv自主航行器10上，声单元之间的相互位置已精确测定且构成声基阵坐标系11，应答器/响应器13安装在回收装置9上；该系统采用相位差或相位比较法，通过测量声单元的相位差，得到应答器/响应器在声基阵坐标系中的方位，利用声波在水中传播的时间计算声基阵与水下载体之间的相对距离8，从而确定水下载体的相对位置。

如图3所示是本发明auv对接回收自主导航使用的基于fmsrupf算法流程图；fmsrupf算法包含下列步骤：

步骤1：auv在水面通过传感器获得初始位姿信息；所述初始位姿信息主要从全球定位系统gps、捷联惯性导航系统sins、多普勒计程仪dvl、超短基线定位系统usbl、测深仪这些传感器获得；其中，全球定位系统gps采集到auv的经度信息和纬度信息中的任一种或任几种；捷联惯性导航系统sins采集到auv的三维速度、位置信息和姿态角信息中的任一种或任几种，位置信息包括纬度、经度和高度信息，姿态角信息包括航向角、俯仰角和横滚角信息；超短基线定位系统usbl采集到回收装置在auv的基阵坐标系内相对于三轴x轴、y轴和z轴的角度信息和分别投影到三轴x轴、y轴和z轴上的距离信息中的任一种或任几种；多普勒计程仪dvl采集到的auv速度信息为前进速度信息、横向速度信息、垂直速度信息中的任一种或任几种；测深仪采集到auv的深度信息。

步骤2：根据输入auv的最新控制量和各种传感器观测量，及auv运动状态模型方程和量测方程采用fmsrupf算法对每个粒子进行时间更新和量测值更新，所述auv运动状态模型方程和量测方程由下式描述：

xk＝f(xk-1,wk-1)(1)

yk＝h(xk,vk)(2)

式中，xk、yk分别为k时刻系统的状态向量和观测向量，xk-1为k-1时刻系统的状态向量，f(·)、h(·)分别为状态和观测的非线性函数，wk-1、vk为具有零均值和协方差分别为qk-1和rk的过程噪声和观测噪声，qk-1和rk分别为噪声wk-1和vk的协方差矩阵；

所采用的fmsrupf算法相对于其他无迹粒子滤波算法来说，在外界干扰强、环境尺度大的情况下对auv对接回收自主导航的鲁棒性、稳定性和准确性都更好；

步骤2所述采用fmsrupf算法对每个粒子进行时间更新和量测值更新，包括以下步骤：

1)数据采集：

通过传感器系统获取auv在水下的位置信息，采集auv的初始位姿信息；

2)滤波定位：

2.1)定义状态初始条件

当k＝0时，假定抽样出的每个粒子用表示，其中，表示初始时刻第i个粒子，p(x0)表示初始时刻概率密度函数，表示初始时刻第i个粒子的估计误差协方差，e[·]表示估计误差协方差，为初始时刻协方差矩阵，t为矩阵转置符号，表示初始时刻第i个分解因子，表示第i个粒子的初始权值，chol{·}表示矩阵cholesky分解，n为粒子个数；

2.2)当k＝1,2,…,n，用fmsrupf算法更新粒子；

①计算每个粒子的sigma点和权值

其中，λ为尺度参数，表示初始sigma点，表示第j个sigma点，ωj为其权值；和分别为初始sigma点均值和方差的权值，和分别对应第j个sigma点均值和方差的权值，α表示sigma点围绕均值扩散程度，决定了sigma点集到均值点的距离，是一个非负的权系数，它可以用于合并随机变量分布的先验信息；

②构造衰减因子αk：由于衰减因子的确定方法不是唯一的；一些学者按滤波器收敛判据选择衰减因子，要求理论预计误差大于实际误差；另一些学者在自适应算法计算中用“记忆长度”决定衰减因子，当估计误差较大时选用较大的衰减因子，反之则选择较小的衰减因子；所以，提出了一种新的衰减滤波算法，推导出两类衰减因子，第一类是用最速下降法确定衰减因子，计算时需要经过多次迭代，且公式复杂，计算过程较长，不适合工程实际应用；第二类衰减因子采用一步算法，其解析式为：

式中，tr(·)为矩阵求迹符号，max{}为取其中最大的数值，n为粒子数，mk和nk的表达式分别为：

其中，hk表示观测函数对x求偏导，fk表示状态函数对x求偏导，vk＝yk-h(xk)，yk为观测值，h(xk)为预测值，pk为状态协方差矩阵，qk和rk分别为噪声wk和vk的协方差矩阵；上述αk的求解过程中，当vk增大，由上式(7)计算所得的nk增大，αk也相应增大；

③时间更新：

式中，表示fmsrupf算法的预测sigma点，表示k-1时刻的粒子的sigma点，ωk-1表示k-1时刻sigma点的权值，表示第i个粒子预测状态估计误差协方差，σ为求和符号，表示第i个预测分解因子，和分别表示fmsrupf算法的第j个和第1到2l个预测sigma点，l表示sigma点的个数，qr{·}表示矩阵qr分解；

无迹粒子滤波的预测协方差阵为

fmsrupf算法的预测协方差阵为

利用衰减因子αk对进行调节；

其中，cholupdate{·}为更新因子，为的平方根矩阵；

其中，为更新的系统状态估计值；

④测量更新：

根据无迹变换中sigma点采样策略，通过非线性测量函数h(·)传播为即

k时刻的预测量测向量为

用于预测观测的第i个分解因子为

用于预测观测的第i个更新分解因子为

加权新sigma点集用于预测观测的协方差矩阵为

状态测量的协方差矩阵为

k时刻粒子的增益矩阵为

其中，表示每个粒子的预测sigma点，表示第j个sigma点的预测量测值，表示第i个粒子预测量测误差协方差，表示第j个粒子的预测sigma点，第j个sigma点和第1至2n个sigma点通过非线性测量函数h(·)传播分别为和表示状态测量的预测协方差矩阵，表示每个粒子的更新sigma点，为k时刻第i个粒子的量测值，为k时刻第i个分解因子，为k时刻协方差矩阵；

该步骤中使用了线性代数中的qr分解、cholesky分解和cholesky因子更新，以协方差平方根阵代替协方差阵参加迭代运算，从而增强了状态协方差矩阵更新过程中的数值稳定性，保证了协方差矩阵的非负定性；同时，利用衰减因子αk调节量测值的权重，减小历史信息对滤波的影响，增强当前量测值信息在滤波计算中的作用；所采用的fmsrupf算法是在已有算法的基础上进行改进，首次在upf算法中融入衰减记忆滤波和平方根滤波算法，其中衰减记忆滤波算法即通过衰减因子限制卡尔曼滤波器的记忆长度，减小历史信息对滤波的影响，增强当前量测信息在滤波计算中的作用，保证了算法预测过程的粒子采样效率，提高了滤波精度；并且结合平方根滤波用协方差矩阵的平方根阵代替协方差阵进行迭代运算，保证了协方差矩阵的非负定性，避免了产生滤波发散的一个发散源。

步骤3：根据每个粒子关联最新观测信息，对每个粒子进行权值更新并归一化权值，所述对每个粒子进行权值更新并归一化权值采用以下步骤进行计算：

1)更新权值：

2)归一化权值

其中，为k-1时刻第i个粒子的权值，p(·)为后验概率密度函数，q(·)为重要性概率密度函数，为k时刻第i个粒子的状态估计，为k-1时刻第i个粒子的状态估计，为初始时刻到k-1时刻第i个粒子的状态估计，y0:k为初始时刻到k时刻粒子的量测值。

步骤4：若粒子集的有效粒子数小于设定的阈值，则采用系统组合粒子重采样方法对粒子集进行重采样，所述系统组合粒子重采样方法采用以下步骤进行计算：

1)进行重采样判定：

根据有效粒子数neff与设定的有效粒子数阈值ng相比较，如果neff＜ng则进行重采样，否则不进行，有效粒子数由式(27)计算；

其中，为归一化粒子权值；

2)进行粒子优化组合：

根据有效粒子数设定两个权值的门限值ωh，ωl，且0＜ωl＜ωh，将每个粒子的权值与门限值一一比较之后优化组合为两部分，一部分为权值小于ωl的粒子和权值大于ωh的粒子，这些粒子都是不稳定粒子，另一部分为的权值适中的粒子，仅对不稳定粒子进行系统重采样后，再与未进行重采样的粒子组成新的粒子集，从而增加粒子的多样性；

所采用的系统组合粒子重采样方法，能够减少重采样的时间并且增加粒子的多样性，从而提高auv使用fmsrupf算法进行对接回收自主导航的精确性。

步骤5：根据重采样生成的新粒子得到auv的状态估计，并按照以上步骤迭代循环继续进行auv对接回收自主导航。

除上述实施例外，本发明还可以有其他实施方式，凡采用等同替换或等效变换形成的技术方案，均落在本发明要求的保护范围内。