本發(fā)明涉及一種類渡槽環(huán)境中無人船測距單元與慣性測量單元的融合算法，用于輔助水面無人船在類渡槽環(huán)境下的定距觀測檢測，屬于水利工程領(lǐng)域。

背景技術(shù)：

目前，現(xiàn)有的輸水渡槽的安全檢測只能依靠關(guān)閘停水后進行人工檢查。此方法檢測周期長，對下游正常供水影響大。許多已建輸水工程由于無法為人工檢查提供長時間的停水條件，至今未進行過檢測，安全隱患大。

更為先進的是采用檢測機器人(水面無人船)進行檢測，現(xiàn)有技術(shù)所公開的水面無人船為單體船造型，主要由船體、動力系統(tǒng)、傳感器系統(tǒng)、控制系統(tǒng)、圖像系統(tǒng)組成。動力系統(tǒng)包括單個或者多個推進器配合舵對船體的運動速度和運動方向進行控制；慣性測量單元(IMU)，全球定位系統(tǒng)(GPS)組成了傳感器系統(tǒng)，并為控制系統(tǒng)自動控制船體運動提供了底層數(shù)據(jù)支持；圖像系統(tǒng)搭載了多個攝像機，在船體運動過程當(dāng)中將所需檢測部分的視頻、圖像錄制和傳輸。

現(xiàn)有的水面無人船主要采用兩種方式進行控制：1.手動遙控、2.自動控制。

渡槽檢測最重要的部分在于視頻、圖像的質(zhì)量，為保證圖像系統(tǒng)拍攝的視頻、圖像清晰，船體相對渡槽運動的相對速度要小于0.5m/s，并且船體相對于被觀測的側(cè)壁和水底距離需要穩(wěn)定。由于渡槽(包括明槽和暗槽)等類似環(huán)境的水利工程中，水流速度往往可以達到3m/s甚至以上，并且由于其內(nèi)部環(huán)境復(fù)雜，環(huán)境相對封閉。采用手動遙控，要求操控員在高流速下保持船體穩(wěn)定的運行，并且保持與被測壁的距離一定。這對操作員的要求極高，并且不能保證運行相對速度穩(wěn)定在0.5m/s以內(nèi)，也無法保持與觀測壁面保持相同距離。采用自動控制，現(xiàn)有技術(shù)所公開的水面無人船所用的是全球定位系統(tǒng)(GPS)和慣性單元(IMU)組合控制船體的自動運行，但是在密閉環(huán)境下GPS幾乎無法接受到信號，導(dǎo)致數(shù)據(jù)不精確，從而自動控制系統(tǒng)不能正常運行，且無法達到穩(wěn)定觀測的效果?？刂葡到y(tǒng)，根據(jù)慣性測量單元和測距單元反饋的參數(shù)控制推進器和舵調(diào)整船體的運動速度和方向。使用測距單元取代GPS，克服了類渡槽環(huán)境密閉無信號的問題，測距單元和慣性測量單元采集的參數(shù)為控制系統(tǒng)自動控制船體運動提供底層數(shù)據(jù)支持，尤其適合在類渡槽環(huán)境下進行定距觀測檢測。

技術(shù)實現(xiàn)要素：

為解決上述問題，本發(fā)明的目的在于提供一種類渡槽環(huán)境中無人船測距單元與慣性測量單元的融合算法，其利用外部測距單元結(jié)合內(nèi)部計算融合為精確值，從而可以在完全密閉的條件下或無GPS的環(huán)境下進行穩(wěn)定的自動航行的工作。

本發(fā)明的目的是通過以下技術(shù)方案實現(xiàn)的：

一種類渡槽環(huán)境中無人船測距單元與慣性測量單元的融合算法，包括以下步驟：

步驟1，由測距單元計算得到船體測量偏向角θ；

步驟2，采用卡爾曼濾波技術(shù)由慣性測量單元構(gòu)建系統(tǒng)狀態(tài)模型：

其中，X(k)是k時刻的系統(tǒng)狀態(tài)，ω_imu(k-1)是k-1時刻的角速度值，W_imu(k)T為高斯白噪聲，T為采樣周期；

步驟3，采用卡爾曼濾波技術(shù)由慣性測量單元構(gòu)建系統(tǒng)測量模型：

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

Z(k)是k時刻的測量值，H為測量矩陣[1 0]，W_laser(k)為k時刻測距單元的高斯白噪聲；

步驟4，系統(tǒng)過程的協(xié)方差矩陣為q_laser和q_imu分別為慣性測量單元和測距單元的權(quán)重常數(shù)，測量過程的協(xié)方差矩陣為R，且慣性測量單元的可靠性大于測距單元；根據(jù)k-1時刻預(yù)測k時刻的狀態(tài)模型得到k時刻的船體預(yù)測偏向角，其中根據(jù)k-1時刻預(yù)測k時刻的狀態(tài)模型為：

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

其中，X(k-1|k-1)為k-1時刻獲取的最優(yōu)結(jié)果，X(k|k-1)為利用k-1時刻狀態(tài)的預(yù)測結(jié)果；

且X(k|k-1)的協(xié)方差為：P(k|k-1)＝A₁P(k-1|k-1)A₁^T+Q；

步驟5，將k時刻的船體測量偏向角和船體預(yù)測偏向角結(jié)合，根據(jù)最優(yōu)估算模型得到k時刻的最優(yōu)預(yù)測估算值，所述最優(yōu)估算模型為：

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時刻的最優(yōu)預(yù)測估算值重復(fù)上述步驟1～5進行自回歸運算得到；

步驟7，根據(jù)步驟6中得到的最優(yōu)預(yù)測估算值，無人船的控制系統(tǒng)控制推進器和舵調(diào)整船體的運動速度和方向完成渡槽檢測。

進一步的，所述測距單元為激光測距裝置、超聲波測距裝置或聲納測距裝置。

進一步的，測距單元為激光測距裝置時，在無人船的的兩側(cè)分別等間距的裝配3個激光測距傳感器，每側(cè)的激光測距傳感器布置如下：

中間的激光測距傳感器垂直于船體邊沿且處于中心位置，兩端的激光測距傳感器分別與船體邊沿的垂線呈α角。

進一步的，激光測距傳感器的間距為300mm，α角為30～45度。

進一步的，步驟1中測距單元根據(jù)激光測距傳感器讀取的激光測距傳感器沿其布置角度距離墻面的直線距離L及激光測距傳感器垂直于墻面的距離d計算得到船體第一偏向角θ。

進一步的，所述慣性測量單元包括加速度計，陀螺儀和磁羅經(jīng)。

本發(fā)明的船體采用多體船結(jié)構(gòu)，并且使用測距單元和慣性測量單元采集的參數(shù)為控制系統(tǒng)自動控制船體運動提供底層數(shù)據(jù)支持，具有運動穩(wěn)定、控制精確的優(yōu)點，尤其適合在類渡槽環(huán)境下進行定距觀測檢測。

附圖說明

圖1為無人船一側(cè)3個激光測距單元布置位置示意圖；

圖2為無人船測距單元計算船體測量偏向角的示意圖。

具體實施方式

為了使本發(fā)明的目的、技術(shù)方案及優(yōu)點更加清楚明白，以下結(jié)合附圖及實施例，對本發(fā)明進行進一步詳細(xì)說明。應(yīng)當(dāng)理解，此處所描述的具體實施例僅用以解釋本發(fā)明，并不用于限定本發(fā)明。

本發(fā)明是基于無人船測距單元與慣性測量單元的融合算法。所述測距單元可為激光測距裝置、超聲波測距裝置或聲納測距裝置。當(dāng)測距單元為激光測距裝置時，在無人船的的兩側(cè)分別等間距的裝配3個激光測距傳感器，每側(cè)的激光測距傳感器布置如圖1所示：中間的激光測距傳感器垂直于船體邊沿且處于中心位置，兩端的激光測距傳感器分別與船體邊沿的垂線呈α角。其中，激光測距傳感器的間距根據(jù)使用的無人船的長度而定，本實施例中選用激光測距傳感器的間距D為300mm。α角為30～45度，且α角的反向延長線可以覆蓋船頭和船尾，通過理論計算和實際檢驗，角度在這個范圍內(nèi)是為了保證激光測距傳感器，或者用其他類似傳感器替代時，可以避免相互干擾，本實施例中α角優(yōu)選為30度。所述慣性測量單元包括加速度計，陀螺儀和磁羅經(jīng)。

一種類渡槽環(huán)境中無人船測距單元與慣性測量單元的融合算法，包括以下步驟：

步驟1，由測距單元計算得到船體測量偏向角θ；

具體的，測距單元根據(jù)激光測距傳感器讀取的激光測距傳感器沿其布置角度距離墻面的直線距離L及激光測距傳感器垂直于墻面的距離d計算得到船體第一偏向角θ。

如圖2所示，L₁，L₂，L₃分別為激光測距傳感器讀取的沿其布置角度距離墻面的直線距離。L₁為船首，L₂是中間位置，L₃為尾部。由此，d₁，d₂，d₃為L₁，L₂，L₃垂直于墻面的距離數(shù)據(jù)。

這是其中一種運行情況，在此情況下無人船朝箭頭方向運行，此時L₁-L₃≥0，在這種情況下L₁距離太遠，誤差大，排除討論。所以根據(jù)三角函數(shù)相關(guān)定理，d₂＝L₂cosθ，d₃＝L₃cos(θ-30°)，sinθ＝(d₂-d₃)/D，結(jié)合這三個式子可以得出d₁＝L₁cos(θ+30°)，從而測距單元計算得到船體測量偏向角θ。

步驟2，利用卡爾曼濾波算法，由慣性測量單元(IMU)可以計算處預(yù)測偏向角φ：

其中，c為偏移誤差常數(shù)，ω_imu為角速度值，W_imu為慣性測量單元的高斯白噪聲，A為狀態(tài)矩陣B為控制輸入矩陣

W_laser為測距單元的高斯白噪聲；

采用卡爾曼濾波技術(shù)由慣性測量單元構(gòu)建系統(tǒng)狀態(tài)模型：

其中，X(k)是k時刻的系統(tǒng)狀態(tài)，ω_imu(k-1)是k-1時刻的角速度值，W_imu(k)T為高斯白噪聲，T為采樣周期；

步驟3，采用卡爾曼濾波技術(shù)由慣性測量單元構(gòu)建系統(tǒng)測量模型：

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

Z(k)是k時刻的測量值，H為測量矩陣[1 0]，W_laser(k)為k時刻測距單元的高斯白噪聲；

步驟4，系統(tǒng)過程的協(xié)方差矩陣為q_laser和q_imu分別為慣性測量單元和測距單元的權(quán)重常數(shù)，測量過程的協(xié)方差矩陣為R，且慣性測量單元的可靠性大于測距單元；根據(jù)k-1時刻預(yù)測k時刻的狀態(tài)模型得到k時刻的船體預(yù)測偏向角，其中根據(jù)k-1時刻預(yù)測k時刻的狀態(tài)模型為：

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

且X(k|k-1)的協(xié)方差為：P(k|k-1)＝A₁P(k-1|k-1)A₁^T+Q；

步驟5，將k時刻的船體測量偏向角和船體預(yù)測偏向角結(jié)合，根據(jù)最優(yōu)估算模型得到k時刻的最優(yōu)預(yù)測估算值，所述最優(yōu)估算模型為：

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時刻的最優(yōu)預(yù)測估算值重復(fù)上述步驟1～5進行自回歸運算得到；

步驟7，根據(jù)步驟6中得到的最優(yōu)預(yù)測估算值，無人船的控制系統(tǒng)控制推進器和舵調(diào)整船體的運動速度和方向完成渡槽檢測。

以上所述僅為本發(fā)明的優(yōu)選實施例而已，并不用于限制本發(fā)明，對于本領(lǐng)域的技術(shù)人員來說，本發(fā)明可以有各種更改和變化。凡在本發(fā)明的精神和原則之內(nèi)，所作的任何修改、等同替換、改進等，均應(yīng)包含在本發(fā)明的保護范圍之內(nèi)。