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

权利要求书

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、状态后验估计和误差协方差矩阵的后验估计来修正，以获得更精确的状态估计。