[发明专利]基于鲁棒自适应无迹卡尔曼滤波的无人艇组合导航方法

说明书

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

技术领域

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

背景技术

当前船舶导航还是离不开人的参与，一般的船舶驾驶台虽有卫星导航、电子罗盘、电子海图等辅助，但难免会出现一些错误。因此无人水面艇(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的非线性状态估计：