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

权利要求书

1.一种类渡槽环境中无人船测距单元与惯性测量单元的融合算法，其特征在于，包括以下步骤：

步骤1，由测距单元计算得到船体测量偏向角θ；

步骤2，采用卡尔曼滤波技术由惯性测量单元构建系统状态模型：

其中，X(k)是k时刻的系统状态，ω_imu(k-1)是k-1时刻的角速度值，W_imu(k)T为高斯白噪声，T为采样周期；

步骤3，采用卡尔曼滤波技术由惯性测量单元构建系统测量模型：

Z(k)＝HX(k)+W_laser(k)

Z(k)是k时刻的测量值，H为测量矩阵[1 0]，W_laser(k)为k时刻测距单元的高斯白噪声；

步骤4，系统过程的协方差矩阵为q_laser和q_imu分别为惯性测量单元和测距单元的权重常数，测量过程的协方差矩阵为R，且惯性测量单元的可靠性大于测距单元；根据k-1时刻预测k时刻的状态模型得到k时刻的船体预测偏向角，其中根据k-1时刻预测k时刻的状态模型为：

X(k|k-1)＝A₁X(k-1|k-1)+B₁ω_imu(k)，

且X(k|k-1)的协方差为：P(k|k-1)＝A₁P(k-1|k-1)A₁^T+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)H^T[HP(k|k-1)H^T+R]^-1为卡尔曼增益矩阵；

步骤6，k+1,k+2…k+n时刻的最优预测估算值重复上述步骤1～5进行自回归运算得到；

步骤7，根据步骤6中得到的最优预测估算值，无人船的控制系统控制推进器和舵调整船体的运动速度和方向完成渡槽检测。

2.根据权利要求1所述的类渡槽环境中无人船测距单元与惯性测量单元的融合算法，其特征在于，测距单元为激光测距装置、超声波测距装置或声纳测距装置。

3.根据权利要求2所述的类渡槽环境中无人船测距单元与惯性测量单元的融合算法，其特征在于，测距单元为激光测距装置时，在无人船的的两侧分别等间距的装配3个激光测距传感器，每侧的激光测距传感器布置如下：

中间的激光测距传感器垂直于船体边沿且处于中心位置，两端的激光测距传感器分别与船体边沿的垂线呈α角。

4.根据权利要求3所述的类渡槽环境中无人船测距单元与惯性测量单元的融合算法，其特征在于，激光测距传感器的间距为300mm，α角为30～45度。

5.根据权利要求3所述的类渡槽环境中无人船测距单元与惯性测量单元的融合算法，其特征在于，步骤1中测距单元根据激光测距传感器读取的激光测距传感器沿其布置角度距离墙面的直线距离L及激光测距传感器垂直于墙面的距离d计算得到船体第一偏向角θ。

6.根据权利要求1所述的类渡槽环境中无人船测距单元与惯性测量单元的融合算法，其特征在于，所述惯性测量单元包括加速度计，陀螺仪和磁罗经。