CN112432644B - 基于鲁棒自适应无迹卡尔曼滤波的无人艇组合导航方法 - Google Patents

Abstract

本发明公开一种基于鲁棒自适应无迹卡尔曼滤波的无人艇组合导航方法。本发明在实现传统UKF的基础下，采用在线故障诊断检测机制判断是否需要更新当前噪声协方差，分别采用基于新息的方法和基于残差的方法计算测量的当前噪声协方差的估计值，然后利用加权因子将最后一个噪声协方差矩阵与估计值相结合，作为新的噪声协方差矩阵。当系统对先验测量噪声估计出现了偏差或在运行过程中传感器发生故障造成实际噪声逐渐增大时，本发明相比于传统UKF可以提供更为精确的位置和航向角估计。

Description

基于鲁棒自适应无迹卡尔曼滤波的无人艇组合导航方法

技术领域

本发明涉及一种无人艇导航定位方法，具体涉及一种基于鲁棒自适应无迹卡尔曼滤波的水面无人艇组合导航方法

背景技术

当前船舶导航还是离不开人的参与，一般的船舶驾驶台虽有卫星导航、电子罗盘、电子海图等辅助，但难免会出现一些错误。因此无人水面艇(USV)成为了许多研究机构和公司研究的方向，USV是用于湖泊、运河、港口甚至公海的机器人，拥有体积小、隐藏能力好、机动性高、价格低等特点。USV中的导航系统无需人机交互即可获得实时导航数据，独立设备工作的传感器会受到环境干扰和设备限制造成的信号丢失和不确定性的影响，不能提供可靠的导航数据。根据多个传感器组成传感器网络就可以很好的解决这一问题，如INS、GPS、激光、雷达、双目视觉等，根据每个传感器提供的原始数据，将这些传感器集成为互补设备，利用数据融合算法估计出合理准确的位置、速度、角度等导航信息。

Kalman滤波(KF)作为线性随机系统的最优估计器，在数据融合算法中得到了广泛的应用。然而，对于USV导航来说，海洋环境是不确定的，也是复杂的。各方面都可能造成位置偏移，特别是环境影响，潮流、风和波浪都是引起无人水面艇漂移的最重要因素。在这种情况下，USV的航行轨迹是复杂的，在实际中不能简单地描述为在直线或曲线上运行。此外，多传感器集成可能会增加系统的非线性，这超出了传统KF的能力。因此，为了处理非线性系统产生了EKF(Extended Kalman Filter)和UKF(Unscented Kalman Filter)滤波器。尽管EKF在组合导航上得到了广泛的应用，但它依然存在自身无法克服的理论局限性:①要求非线性系统状态函数和量测函数必须是连续可微的，从而限制了EKF的应用范围；②对非线性函数的一阶线性化近似精度偏低，特别是当系统具有强非线性时，EKF估计精度严重下降，甚至发散；③需要计算非线性函数的雅克比矩阵，容易造成EKF稳定性差和计算发散。而UKF就能克服上述EKF的缺陷，能够以较高的精度和较快的计算速度处理非线性高斯系统的滤波问题。

但UKF作为Kalman滤波器的一个变种，需要对系统噪声特性有准确的先验知识。在这其中过程噪声协方差Q和测量噪声协方差R是最重要的，因为它们直接调节预测值和测量，会对系统状态估计产生影响从而导致性能下降。所以Q和R的调整是卡尔曼滤波器发展的关键。

而自适应卡尔曼滤波就是在利用观测数据进行滤波的同时，实时地对未知的或不确定的系统模型、参数和噪声的统计特征进行适当的估计和修正，从而自适应调整Q和R。目前，关于自适应卡尔曼滤波主要基于某一种自适应理论，如多模型的自适应估计、协方差缩放、基于创新的自适应估计，很多基于窗口或比例因子，需要储存和平滑前一个时间步长的相关参数，还需要计算每个时间步长的比例因子，这样就大大增加了控制器的计算负担。

发明内容

本发明针对实际海洋环境中的USV鲁棒导航中噪声协方差不确定的非线性状态估计问题，提供了一种鲁棒自适应无迹卡尔曼滤波(RAUKF)的导航定位方法。

针对USV在复杂环境下，传统UKF算法无法在噪声协方差不确定的情况下进行非线性系统状态估计。本发明提供了RAUKF的多传感器数据融合算法，该算法利用故障检测机制判断是否需要更新噪声协方差矩阵。RAUKF将根据当前理论估计值和先前值的加权组合调整Q和R，以减少传感器测量噪声的变化对系统的影响。

本发明包括以下步骤：

步骤1：基于UKF的非线性状态估计：

将USV模型化为状态空间形式，用离散系统状态向量x来描述USV运动的非线性系统：

x(k)＝f(x(k-1),w(k-1)) (1)

z(k)＝Hx(k)+v(k) (2)

其中f()为非线性状态转移矩阵，w(k)是过程噪声，H为测量矩阵，v(k)为测量噪声，k为当前时刻，k-1为上一时刻。假设它们都是满足正态概率分布p(w)～N(0,Q)和p(v)～N(0,R)的高斯白噪声。

选取横向位置p_x，纵向位置p_y，艏摇角ψ，前进速度u，横移速度v，艏摇角速度r，作为状态向量：

x＝[p_x,p_y,ψ,u,v,r]^T (3)

输出的测量向量为：

通过USV动力学方程得到状态空间型的非线性USV离散系统：

其中m为无人艇质量，X_u、Y_v、N_r为流体动力的速度导数，

为流体动力的加速度导数，I_z为惯性矩，X_P1、X_P2为两个螺旋桨的转速，d_p为两个螺旋桨的横向距离。

测量矩阵H为：

其中前两行由GPS获得位置信息，第三行由电子罗盘获得航向角信息，最后3行由IMU中的加速度和陀螺仪获得速度信息，T为采样间隔。

然后运行传统UKF算法计算出状态后验估计

测量误差协方差矩阵

卡尔曼系数K_k,误差协方差矩阵的后验估计

步骤2：利用加权因子λ自适应地调整过程噪声协方差矩阵Q：

定义加权因子λ以平衡后的噪声协方差值和当前估计。加权因子λ设置为下限λ₀∈(0,1)，用来确保更新强度，系统过程噪声协方差矩阵Q更新为：

其中K_k为卡尔曼增益，

是一个自由度为s的χ²分布，s为μ_k的维度。创新序列μ_k定义为

z_k为传感器测量向量，

为测量估计向量。

为对应的故障检测阈值。a(a>0)是依赖于实际环境的调整参数，较大的a意味着使用默认λ₀的概率较高。λ₀和a参数的调整决定了协方差更新对新数据的敏感程度。

步骤3：利用加权因子δ自适应地调整测量噪声协方差R：

和对Q自适应调整类似，使用δ设下限δ₀∈(0,1)的加权因子更新R

其中

为基于残差向量的测量噪声协方差估计，b(b>0)也是依赖于实际环境的调整参数，b和δ₀的选择与a和λ₀的选择相同。

步骤4：修正估计：

一旦过程噪声协方差矩阵Q和测量噪声协方差R被更新，当前状态估计可以用新的过程噪声协方差矩阵Q、测量噪声协方差R、状态后验估计

和误差协方差矩阵的后验估计

来修正，以获得更精确的状态估计，修正估计过程和传统UKF算法一致。

本发明的有益效果：本发明利用统计函数判断当前噪声协方差是否需要更新的在线故障检测机制，不仅大大提高了USV导航数据的精度，而且显著减少了计算负担，特别适合用于小型USV中计算能力有限的嵌入式设备。

附图说明

图1是卡尔曼滤波算法图；

图2是传感器正常情况下USV位置和航向的均方根误差图；

图3是传感器正常情况下电子罗盘测量融合航向图；

图4是传感器发生故障情况下USV位置和航向的均方根误差图；

图5是传感器发生故障情况下电子罗盘测量融合航向图。

具体实施方式

本发明包括以下步骤：

步骤1：基于传统UKF的非线性状态估计：

无迹卡尔曼滤波器是传统KF的一个改进，它的基本原理与传统的KF相同。一般KF算法被分为两个阶段，第一个阶段为预测，因为它利用系统演化预测模型根据前一个状态生成一个先验的系统状态估计。第二个阶段为更新，通过修正先前的先验估计，将系统中的测量值考虑在内，从而产生后验状态估计。这两阶段过程始于一个初始估计状态，在一个循环中重复递归直到滤波结束，算法流程如图1所示。

将USV模型化为状态空间形式，用离散系统状态向量x来描述USV运动的非线性系统：

x(k)＝f(x(k-1),w(k-1)) (10)

z(k)＝Hx(k)+v(k) (11)

其中f()为非线性状态转移矩阵，w(k)是过程噪声，H为测量矩阵，v(k)为测量噪声，k为当前时刻，k-1为上一时刻。假设它们都是满足正态概率分布p(w)～N(0,Q)和p(v)～N(0,R)的高斯白噪声。

选取横向位置p_x，纵向位置p_y，艏摇角ψ，前进速度u，横移速度v，艏摇角速度r，作为状态向量：

x＝[p_x,p_y,ψ,u,v,r]^T (12)

输出的测量向量为：

通过USV动力学方程得到状态空间型的非线性USV离散系统：

其中m为无人艇质量，X_u、Y_v、N_r为流体动力的速度导数，

为流体动力的加速度导数，I_z为惯性矩，X_P1、X_P2为两个螺旋桨的转速，d_p为两个螺旋桨的横向距离，

为前进速度的导数，

为横移速度的导数。

根据GPS、电子罗盘、IMU和状态向量的转换关系得到系数矩阵H：

其中前两行由GPS获得位置信息，第三行由电子罗盘获得航向角信息，最后3行由IMU中的加速度和陀螺仪获得速度信息，T为采样间隔。

然后运行传统UKF算法计算出

K_k,

分别为状态后验估计，测量误差协方差矩阵，卡尔曼系数和误差协方差矩阵的后验估计。

步骤2：Q自适应调整：

根据当前理论估计值和最后一次数据的加权组合自适应地调整Q和R。由于UKF在正常情况下工作良好，因此采用故障检测机制来判断是否需要修正噪声协方差矩阵。该算法采用的故障检测机制使用以下统计功能来检测故障：

其中

是一个自由度为s的χ²分布，s为μ_k的维度。创新序列μ_k定义为

为假设想检测一个可靠性等级为1-σ的故障，其中σ是选择的参数，然后有一个阈值

通过

的

分布决定

因此，利用预先选择的σ值，从式中定义一个对应的故障检测阈值

使得系统故障可以检测到1-σ的可靠性等级。

创新序列

作为新量测的结果，是滤波器自适应最相关的信息，可以用来估计噪声协方差，过程噪声可以表示为

w_k-1＝x_k-f(x_k-1) (15)

过程噪声协方差矩阵Q_k-1的估计可以估计为:

其中cov()为协方差，E()为期望，K_k为卡尔曼增益。

为了实现上述方程，

通常通过使用窗口方法平均

随时间的变化来近似。本发明没有采用移动窗口方法，而是利用加权因子λ来自适应地调整Q，以平衡最后的噪声协方差值和当前估计。加权因子λ设置为下限λ₀∈(0,1)，以确保更新强度，并且如果λ超过预设阈值

将随着

的升高而增加。因此系统处理噪声协方差矩阵更新为：

其中K_k为卡尔曼增益，

是一个自由度为s的χ²分布，s为μ_k的维度。创新序列μ_k定义为

z_k为传感器测量向量，

为测量估计向量。

为对应的故障检测阈值。a(a>0)是依赖于实际环境的调整参数，较大的a意味着使用默认λ₀的概率较高。λ₀和a参数的调整决定了协方差更新对新数据的敏感程度。

步骤3：R自适应调整：

基于时间步长k的测量噪声协方差矩阵R也可以采用基于创新的方法进行估计，具体如下:

其中

为了得到正定矩阵

本发明采用了基于残差的方法。可以导出时间步k处的测量噪声为v_k＝z_k-h(x_k)。然后，在时间步k处的剩余向量由

此外，基于残差向量k的测量噪声协方差的估计如下

其中

和对Q自适应调整类似，使用δ设下限δ₀∈(0,1)的加权因子更新R_k

其中b(b>0)也是依赖于实际环境的调整参数，b和δ₀的选择与a和λ₀的选择相同。

步骤4：修正估计：

一旦过程噪声协方差矩阵Q和测量噪声协方差R被更新，当前状态估计可以用新的R、Q、估计的状态x和估计的误差协方差P来修正，以获得更精确的状态估计。修正过程如下所述：

1.计算新的预测误差协方差矩阵P_k和互协方差矩阵P_xz，将获得的状态x视为时间步k的预测状态：

2.评估更新后的创新协方差矩阵P和卡尔曼增益K：

3.修正当前状态x及其误差协方差矩阵P的估计

综上所示，本发明使用所提出的RAUKF算法在不同的环境下进行了模拟实验，如图2,3,4,5所示。实验结果表明，当USV中的传感器对先验测量噪声估计出现了偏差或在运行过程中传感器发生故障造成实际噪声逐渐增大时，本发明相比于传统UKF都可以提供更为精确的位置和航向角估计。因此本发明提出的鲁棒自适应UKF方法可以为USV组合导航定位提供一种有效的方法。

Claims (1)

1.基于鲁棒自适应无迹卡尔曼滤波的无人艇组合导航方法，其特征在于该方法包括以下步骤：

步骤1：基于UKF的非线性状态估计：

将USV模型化为状态空间形式，用离散系统状态向量x来描述USV运动的非线性系统：

x(k)＝f(x(k-1),w(k-1)) (1)

z(k)＝Hx(k)+v(k) (2)

其中f()为非线性状态转移矩阵，w(k)是过程噪声，H为测量矩阵，v(k)为测量噪声，k为当前时刻，k-1为上一时刻；

选取横向位置p_x，纵向位置p_y，艏摇角ψ，前进速度u，横移速度v，艏摇角速度r，并令：

x＝[p_x,p_y,ψ,u,v,r]^T (3)

输出的测量向量为：

通过USV动力学方程得到状态空间型的非线性USV离散系统：

其中m为无人艇质量，X_u、Y_v、N_r为流体动力的速度导数，

为流体动力的加速度导数，I_z为惯性矩，X_P1、X_P2为两个螺旋桨的转速，d_p为两个螺旋桨的横向距离；

测量矩阵H为：

其中前两行由GPS获得位置信息，第三行由电子罗盘获得航向角信息，最后3行由IMU中的加速度和陀螺仪获得速度信息，T为采样间隔；

然后采用UKF算法计算出状态后验估计

测量误差协方差矩阵

卡尔曼系数K_k,误差协方差矩阵的后验估计

步骤2：利用加权因子λ自适应地调整过程噪声协方差矩阵Q：

定义加权因子λ以平衡后的噪声协方差值和当前估计；加权因子λ设置为下限λ₀∈(0,1)，用来确保更新强度，系统过程噪声协方差矩阵Q更新为：

其中K_k为卡尔曼增益，

是一个自由度为s的χ²分布，s为μ_k的维度；创新序列μ_k定义为

z_k为传感器测量向量，

为测量估计向量；

为对应的故障检测阈值；a是依赖于实际环境的调整参数；λ₀和a参数的调整决定了协方差更新对新数据的敏感程度；

步骤3：利用加权因子δ自适应地调整测量噪声协方差R：

和对Q自适应调整类似，使用δ设下限δ₀∈(0,1)的加权因子更新R

其中

为基于残差向量的测量噪声协方差估计，b是依赖于实际环境的调整参数；

步骤4：修正估计：

一旦过程噪声协方差矩阵Q和测量噪声协方差R被更新，当前状态估计就用新的过程噪声协方差矩阵Q、测量噪声协方差R、状态后验估计

和误差协方差矩阵的后验估计

来修正，以获得更精确的状态估计。

Images