类渡槽环境中无人船测距单元与惯性测量单元的融合算法制造技术

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

【技术实现步骤摘要】
类渡槽环境中无人船测距单元与惯性测量单元的融合算法
本专利技术涉及一种类渡槽环境中无人船测距单元与惯性测量单元的融合算法，用于辅助水面无人船在类渡槽环境下的定距观测检测，属于水利工程领域。
技术介绍
目前，现有的输水渡槽的安全检测只能依靠关闸停水后进行人工检查。此方法检测周期长，对下游正常供水影响大。许多已建输水工程由于无法为人工检查提供长时间的停水条件，至今未进行过检测，安全隐患大。更为先进的是采用检测机器人(水面无人船)进行检测，现有技术所公开的水面无人船为单体船造型，主要由船体、动力系统、传感器系统、控制系统、图像系统组成。动力系统包括单个或者多个推进器配合舵对船体的运动速度和运动方向进行控制；惯性测量单元(IMU)，全球定位系统(GPS)组成了传感器系统，并为控制系统自动控制船体运动提供了底层数据支持；图像系统搭载了多个摄像机，在船体运动过程当中将所需检测部分的视频、图像录制和传输。现有的水面无人船主要采用两种方式进行控制：1.手动遥控、2.自动控制。渡槽检测最重要的部分在于视频、图像的质量，为保证图像系统拍摄的视频、图像清晰，船体相对渡槽运动的相对速度要小于0.5m/s，并且船体相对于被观测的侧壁和水底距离需要稳定。由于渡槽(包括明槽和暗槽)等类似环境的水利工程中，水流速度往往可以达到3m/s甚至以上，并且由于其内部环境复杂，环境相对封闭。采用手动遥控，要求操控员在高流速下保持船体稳定的运行，并且保持与被测壁的距离一定。这对操作员的要求极高，并且不能保证运行相对速度稳定在0.5m/s以内，也无法保持与观测壁面保持相同距离。采用自动控制，现有技术所公开的水面无人船所用的是全球定位系统(GPS)和惯性单元(IMU)组合控制船体的自动运行，但是在密闭环境下GPS几乎无法接受到信号，导致数据不精确，从而自动控制系统不能正常运行，且无法达到稳定观测的效果。控制系统，根据惯性测量单元和测距单元反馈的参数控制推进器和舵调整船体的运动速度和方向。使用测距单元取代GPS，克服了类渡槽环境密闭无信号的问题，测距单元和惯性测量单元采集的参数为控制系统自动控制船体运动提供底层数据支持，尤其适合在类渡槽环境下进行定距观测检测。
技术实现思路
为解决上述问题，本专利技术的目的在于提供一种类渡槽环境中无人船测距单元与惯性测量单元的融合算法，其利用外部测距单元结合内部计算融合为精确值，从而可以在完全密闭的条件下或无GPS的环境下进行稳定的自动航行的工作。本专利技术的目的是通过以下技术方案实现的：一种类渡槽环境中无人船测距单元与惯性测量单元的融合算法，包括以下步骤：步骤1，由测距单元计算得到船体测量偏向角θ；步骤2，采用卡尔曼滤波技术由惯性测量单元构建系统状态模型：其中，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-1|k-1)为k-1时刻获取的最优结果，X(k|k-1)为利用k-1时刻状态的预测结果；且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中得到的最优预测估算值，无人船的控制系统控制推进器和舵调整船体的运动速度和方向完成渡槽检测。进一步的，所述测距单元为激光测距装置、超声波测距装置或声纳测距装置。进一步的，测距单元为激光测距装置时，在无人船的的两侧分别等间距的装配3个激光测距传感器，每侧的激光测距传感器布置如下：中间的激光测距传感器垂直于船体边沿且处于中心位置，两端的激光测距传感器分别与船体边沿的垂线呈α角。进一步的，激光测距传感器的间距为300mm，α角为30～45度。进一步的，步骤1中测距单元根据激光测距传感器读取的激光测距传感器沿其布置角度距离墙面的直线距离L及激光测距传感器垂直于墙面的距离d计算得到船体第一偏向角θ。进一步的，所述惯性测量单元包括加速度计，陀螺仪和磁罗经。本专利技术的船体采用多体船结构，并且使用测距单元和惯性测量单元采集的参数为控制系统自动控制船体运动提供底层数据支持，具有运动稳定、控制精确的优点，尤其适合在类渡槽环境下进行定距观测检测。附图说明图1为无人船一侧3个激光测距单元布置位置示意图；图2为无人船测距单元计算船体测量偏向角的示意图。具体实施方式为了使本专利技术的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本专利技术进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本专利技术，并不用于限定本专利技术。本专利技术是基于无人船测距单元与惯性测量单元的融合算法。所述测距单元可为激光测距装置、超声波测距装置或声纳测距装置。当测距单元为激光测距装置时，在无人船的的两侧分别等间距的装配3个激光测距传感器，每侧的激光测距传感器布置如图1所示：中间的激光测距传感器垂直于船体边沿且处于中心位置，两端的激光测距传感器分别与船体边沿的垂线呈α角。其中，激光测距传感器的间距根据使用的无人船的长度而定，本实施例中选用激光测距传感器的间距D为300mm。α角为30～45度，且α角的反向延长线可以覆盖船头和船尾，通过理论计算和实际检验，角度在这个范围内是为了保证激光测距传感器，或者用其他类似传感器替代时，可以避免相互干扰，本实施例中α角优选为30度。所述惯性测量单元包括加速度计，陀螺仪和磁罗经。一种类渡槽环境中无人船测距单元与惯性测量单元的融合算法，包括以下步骤：步骤1，由测距单元计算得到船体测量偏向角θ；具体的，测距单元根据激光测距传感器读取的激光测距传感器沿其布置角度距离墙面的直线距离L及激光测距传感器垂直于墙面的距离d计算得到船体第一偏向角θ。如图2所示，L1，L2，L3分别为激光测距传感器读取的沿其布置角度距离墙面的直线距离。L1为船首，L2是中间位置，L3为尾部。由此，d1，d2，d3为L1，L2，L3垂直于墙面的距离数据。这是其中一种运行情况，在此情况下无人船朝箭头方向运行，此时L1-L3≥0，在这种情况下L1距离太远，误差大，排除讨论。所以根据三角函数相关定理，d2＝L2cosθ，d3＝L3cos(θ-30°)，sinθ＝(d2-d3)/D，结合这三个式子可以得出d1＝L1cos(θ+30°)，从而测距本文档来自技高网...

【技术保护点】
一种类渡槽环境中无人船测距单元与惯性测量单元的融合算法，其特征在于，包括以下步骤：步骤1，由测距单元计算得到船体测量偏向角θ；步骤2，采用卡尔曼滤波技术由惯性测量单元构建系统状态模型：

【技术特征摘要】
1.一种类渡槽环境中无人船测距单元与惯性测量单元的融合算法，其特征在于，包括以下步骤：步骤1，由测距单元计算得到船体测量偏向角θ；步骤2，采用卡尔曼滤波技术由惯性测量单元构建系统状态模型：其中，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)...

【专利技术属性】
技术研发人员：邵俊臻，庞硕，高岱，柴雪松，
申请(专利权)人：北京优尔博特创新科技有限公司，
类型：发明
国别省市：北京,11