[发明专利]类渡槽环境中无人船测距单元与惯性测量单元的融合算法

摘要

本发明涉及一种类渡槽环境中无人船测距单元与惯性测量单元的融合算法，包括以下步骤：由测距单元计算得到船体测量偏向角；采用卡尔曼滤波技术由惯性测量单元构建系统状态模型；采用卡尔曼滤波技术由惯性测量单元构建系统测量模型；根据k‑1时刻预测k时刻的状态模型得到k时刻的船体预测偏向角；将k时刻的船体测量偏向角和船体预测偏向角结合，根据最优估算模型得到k时刻的最优预测估算值；k+1,k+2…k+n时刻的最优预测估算值重复上述步骤进行自回归运算得到；根据得到的最优预测估算值，无人船的控制系统控制推进器和舵调整船体的运动速度和方向完成渡槽检测。本发明可以控制无人船在完全密闭的条件下或无GPS的环境下进行稳定的自动航行的工作。

主权项

一种类渡槽环境中无人船测距单元与惯性测量单元的融合算法，其特征在于，包括以下步骤：步骤1，由测距单元计算得到船体测量偏向角θ；步骤2，采用卡尔曼滤波技术由惯性测量单元构建系统状态模型：X(k)=A1X(k-1)+B1ωimu(k-1)+Wimu(k)T0]]>其中，X(k)是k时刻的系统状态，ωimu(k‑1)是k‑1时刻的角速度值，Wimu(k)T为高斯白噪声，T为采样周期；步骤3，采用卡尔曼滤波技术由惯性测量单元构建系统测量模型：Z(k)＝HX(k)+Wlaser(k)Z(k)是k时刻的测量值，H为测量矩阵[10]，Wlaser(k)为k时刻测距单元的高斯白噪声；步骤4，系统过程的协方差矩阵为qlaser和qimu分别为惯性测量单元和测距单元的权重常数，测量过程的协方差矩阵为R，且惯性测量单元的可靠性大于测距单元；根据k‑1时刻预测k时刻的状态模型得到k时刻的船体预测偏向角，其中根据k‑1时刻预测k时刻的状态模型为：X(k|k‑1)＝A1X(k‑1|k‑1)+B1ωimu(k)，且X(k|k‑1)的协方差为：P(k|k‑1)＝A1P(k‑1|k‑1)A1T+Q；步骤5，将k时刻的船体测量偏向角和船体预测偏向角结合，根据最优估算模型得到k时刻的最优预测估算值，所述最优估算模型为：X(k|k)＝X(k|k‑1)+K(k)(Z(k)‑HX(k|k‑1))其中，K(k)＝P(k|k‑1)HT[HP(k|k‑1)HT+R]‑1为卡尔曼增益矩阵；步骤6，k+1,k+2…k+n时刻的最优预测估算值重复上述步骤1～5进行自回归运算得到；步骤7，根据步骤6中得到的最优预测估算值，无人船的控制系统控制推进器和舵调整船体的运动速度和方向完成渡槽检测。