小型水面機器人裝置及自適應水流優化導航方法

2023-07-04 10:07:26

專利名稱：小型水面機器人裝置及自適應水流優化導航方法
技術領域：
本發明涉及水面機器人裝置及其導航方法，尤其是一種帶有自動控制裝置的小型水面機器人裝置，以及小型水面機器人自適應水流優化導航方法。
背景技術：
水面機器人也稱作無人船，即為無人操控的智能的水面平臺，可搭載各種監控監 測勘探設備進行不同應用。其應用目前正逐步得到推廣。對於沒有人操控而需要完全自主 行駛的水面機器人來說，導航方法至關重要。導航算法是指當輸入給無人船目的地後，水面 機器人根據自身所在位置，駛向目的地。期間要經過數學運算，給自身的螺旋槳、船舵(若 是雙螺旋槳船體，則是調整兩槳的差速)發出指令，不斷校正，最終到達目的地的方法。目前尚沒有專門針對小型水面機器人(長度1米左右)的優化導航方法。小型水 面機器人體積小、重量輕，航行時受水流影響大，往往難以走出預定的路線。目前的導航方 法都是每時每顆保持船頭對準目的地。當水面機器人航行在水面時，會受到水流影響。而 在目前的導航算法中，由於水流在水面機器人行駛時不方便用傳感器準確測量，水流的影 響一般都是靠實時校正船的方向來克服。這樣，船從起始點到終點的路徑會是一條曲線，曲 線的曲率主要取決於水流速度、船速以及校正頻率。一般對於小型無人船而言，由於最大速 度和船本身質量的限制，這個曲率一般都比較大。從而增加了不必要的能耗，並延長了到達 目的地的時間。水面波動較大時，若小型水面機器人速度過快轉角過大還會出現翻船的危 險。若遇到障礙時，還有可能發生碰撞情況。

發明內容
本發明的目的是提供一種帶有自動控制裝置的小型水面機器人裝置，以及小型水 面機器人自適應水流優化導航方法。本發明的目的是通過採用以下技術方案來實現的小型水面機器人裝置，包括船體和設在船體上的驅動裝置和控制裝置，所述控制 裝置包括水流速傳感器、導航裝置和中央控制器；所述導航裝置包括GPS衛星定位傳感器、電子羅盤和慣性導航模塊；所述導航裝置與中央控制器電路連接；所述控制裝置與驅動裝置電路連接，並實現對船體的自動導航。作為本發明的優選技術方案，所述慣性導航模塊包括三軸陀螺儀或三軸加速度傳 感器。作為本發明的優選技術方案，所述控制裝置還包括雷達和雷射測距傳感器。作為本發明的優選技術方案，所述驅動裝置包括電池和驅動電機。小型水面機器人自適應水流優化導航方法，該方法包括以下步驟(1)小型水面機器人通過中央控制器接收GPS定位傳感器的信號，得到其絕對坐 標；
(2)中央控制器接收慣性導航模塊的信號，得到小型水面機器人的加速度、角速 度，並對所得加速度或角速度積分，獲得實時的速度及角度；(3)中央控制器接收雷射測距傳感器或雷達的數據，判斷與前方障礙物之間距 罔；(4)中央控制器接收電子羅盤的數據，獲得前進的方向；(5)中央控制器接收水流速傳感器的數據，獲得水流的速度。(6)中央控制器將上述所得數據進行處理後，通過驅動裝置和轉向裝置控制小型 水面機器人的航向。本發明的有益效果是相對於現有技術，本發明小型水面機器人的體積小、重量 輕，採用電力驅動，自身無汙染排放，相比大型船，不僅節約了能耗，節省了人力，而且成本 較低。本發明自適應水流導航優化方法，可以自動適應水流的影響，優化行駛的路徑，提 高效率，節約能源，有效增加運行時間；同時防翻，防撞，避免了水上交通事故的發生；保證 了小型水上機器人及其所載儀器儀表的安全。本發明加入了對水流預測元素的方程建立起 來的導航算法，經過實踐證明，小型水面機器人的行駛軌跡近似直線，大大減少了不必要的 能耗，並且節約了時間，同時可以防翻，避障，達到穩定運行的目的。


圖1是本發明的結構示意圖；圖2是本發明小型水面機器人轉彎示意圖；圖3是本發明At時間內小型水面機器人角速度ω與位移d關係圖；圖4是本發明t時刻朝向ht示意圖。圖中1.船體，2.中央控制器，3. GPS衛星定位傳感器，4.慣性導航模塊，5.雷達， 6，雷射測距傳感器，7.水流速傳感器，8.驅動電機，9.電池，10.電子羅盤。
具體實施例方式下面結合附圖與具體實施例對本發明作進一步說明如圖1至圖4所示，小型水面機器人裝置，包括船體1和設在船體1上的驅動裝置 和控制裝置，所述控制裝置包括水流速傳感器7、導航裝置和中央控制器2 ；所述導航裝置 包括GPS衛星定位傳感器3、電子羅盤10和慣性導航模塊4 ；所述導航裝置與中央控制器2 電路連接；所述控制裝置與驅動裝置電路連接，並實現對船體1的自動導航。本實施例中，所述慣性導航模塊4包括三軸陀螺儀或三軸加速度傳感器。所述控 制裝置還包括雷達5和雷射測距傳感器6。所述驅動裝置包括電池9和驅動電機8。小型水面機器人自適應水流優化導航方法，該方法包括以下步驟(1)小型水面機器人通過中央控制器接收GPS定位傳感器的信號，得到其絕對坐 標；(2)中央控制器接收慣性導航模塊的信號，得到小型水面機器人的加速度、角速 度，並對所得加速度或角速度積分，獲得實時的速度及角度；(3)中央控制器接收雷射測距傳感器或雷達的數據，判斷與前方障礙物之間距罔；(4)中央控制器接收電子羅盤的數據，獲得前進的方向；(5)中央控制器接收水流速傳感器的數據，獲得水流的速度。(6)中央控制器將上述所得數據進行處理後，通過驅動裝置和轉向裝置控制小型 水面機器人的航向。本發明的小型水面機器人包括以下幾種能力。自適應水流導航當小型水面機器人的中央控制器以某種方式收到目的地的地理坐標以後，會從 GPS接收器中讀取當前的經緯度坐標，並將其轉化為橫縱坐標的原點。小型水面機器人接下 來會從電子指南針讀取自身的朝向，並通過計算，控制驅動裝置和轉向裝置。在行駛過程中，每隔一定的時間間隔，中央控制器會從GPS接收器讀取當前經緯 度坐標，並轉化到以起始點為原點的橫縱坐標系，得到當前k時刻的位置坐標Xk，Yk ；同時中 央控制器從電子指南針讀取當前k時刻的朝向hk。這樣，中央控制器便可計算出此k時刻 小型水面機器人朝向的偏角。通過小型水面機器人的數學模型，可以計算出在下一個時間 間隔k+1時刻之前，動力裝置和轉向裝置的指令。該數學模型基於通過靜水實驗得到的一些靜態方程。如，轉彎半徑r、角速度ω與 螺旋槳轉速V、船舵角度θ的關係，或轉彎半徑r、角速度ω與螺旋槳轉速vl、ν2的關係 (雙螺旋槳)。數學表達式為，
_2] □ =或 l··)在t時刻，位置為(Xt，Yt)，朝向為ht。假設控制命令不改變，經過一個很短的時 間At後，位置可以計算為(Xc__ s V-) = Cxt + d K I+ dx ccsj - h. j)d為小型水面機器人在At時間內走過的位移。d的大小與圓周運動的半徑有關， 即與螺旋槳轉速和舵角的大小有關。我們可以通過靜水試驗得到以下函數的對應值d = d (r)我們便可以基於此進一步推導出小型水面機器人的控制模型，使之能精確地到達 目的地。對於單螺旋槳小型水面機器人來說，其三自由度非線性靜態數學模型如下
/ TxK-Il rvk]\[xk-l -+ d(rk) x sin( k/2 - h,^)]
-k = f(xk-i-uk)-^'k = f| Yk-i - sk= Vl - dO'k) x cos(wk/2 - hk_t) | + wk
I-hK-I J Lo』/hk_! - Mk』其中，rk =[l C] [^] = [l Ok 卿.)=
[i； ] := [O l]gf ekl)wk N(0，Qk)，xk是k時刻的真實狀態，且是從(k-Ι)時刻的狀態得來。Uk表示 (k-Ι)到k時刻的控制命令。GPS接收器和電子羅盤的噪聲可以用白高斯噪音模型。我們實現了一個卡爾曼濾波器來減少此類噪聲。非線性系統的測量模型為zk = h (xk) +mk ；mk N (0，Rk)卡爾曼濾波器分兩個階段。在第一階段(預測階段)，我們用以下方程預測下一狀 態和預測誤差協方差
|%-i + d(i*k) x- Siniak/2 - hk_t)]Xk k_! = FkXk.! k_! - Bk-Iuk_4= Y^1 + d:rk) K cos(cuk/2 — H^1J
1- ^k JPklH = FkPHlHF1^QH在第二階段(更新階段)，GPS和電子羅盤數據更新，卡爾曼增益、真實狀態和真實 誤差協方差也同步更新Kk = Pk丨HHJ (HkPklHHkkRk)-1Xk|k = xk 丨 H+Kk (Zk-HkXk 丨 Η)Pklk= (I-KkHk) Fk丨η在實際中，小型水面機器人由於自身重量輕，體積小，對水流的影響非常敏感。我 們在數學模型中加入了一個水流差量來抵消水流對預測模型的影響。我們假設相鄰兩個狀 態k-Ι和k時刻的水流方向和速度是不變的。這個假設非常有實際意義，也簡化我們的計 算量。我們對預測狀態XklH進行了調整，加入了上一狀態的水流Ζη-ΧΗ^。Xklk = XkiH+Zk-fXH |k_2+Kk (zk"h (Xwh+Zh—Xh |k_2))經過嚴格的數學推到可以證明，上式是收斂的。說明以上方程式可以起到對水流 作用的抵消作用。與傳統方法實時保持船頭對準目的地不同的是，本算法不校準船頭的方向，而是 根據上一狀態的水流情況，計算出理想的in,並通過反函數g-1求出PM或者|^;:1，使得在k時刻，小型水面機器人的位置儘可能的保持在目的地與起始點的連線 上，而不是k-Ι時刻的位置與目的地的連線。這樣，便大幅提高了小型水面機器人的行駛效 率，使其行駛路徑非常接近直線，從而節省了電量與時間。防翻能力每隔一定的時間間隔，中央控制器會從三周陀螺儀讀取角速度數據，通過積分算 出小型水面機器人橫搖與縱搖的角度。風浪過大時，如果小型水面機器人的速度過快，或者 轉角過大，都會有翻得危險。定義一個安全的橫搖與縱搖的臨界角度，若任意一個當前角度 超過安全值，則意味著小型水面機器人有翻的危險。此時，中央控制器將通過對動力裝置減 速和轉向裝置減小轉向角等方式，使小型水面機器人脫離險境。當遇到電磁幹擾，如果電子羅盤發生跳變，小型水面機器人會對陀螺儀檢測的角 速度對時間做積分，計算出其轉動的角度，累加到電子羅盤跳變前的角度上。這樣船在任何 時候都不會失去方向感。當電子羅盤又能在較長時間內與中央處理器所計算的角度吻合， 則認為電磁幹擾消失。當小型水面機器人GPS接收器失去信號，中央處理器會對加速度傳感器的數據對 時間做積分，可以監測到小型水面機器人在空間三個方向上速度的改變。把這個速度的改 變值與失去信號時記錄的速度疊加，中央處理器可以 準確的計算出小型水面機器人坐標的 改變。
避障能力每隔一定的時間間隔，中央控制器會從雷達接收數據，計算可得較遠處的障礙物的位置，再經過計算可知障礙物是否移動。若不移動，中央控制器會重新規劃路徑，繞開障 礙物；若為移動障礙物，中央控制器會繼續前進。繼續前行後，障礙物或已駛出，不會導致相 撞；或已接近小型水面機器人，而此時由於距離太近雷達已無法識別。中央控制會通過雷射 測距傳感器確認障礙物已無相撞風險或放出指令，停止動力裝置及轉向裝置，等待障礙物 罔開。
權利要求
一種小型水面機器人裝置，包括船體和設在船體上的驅動裝置和控制裝置，其特徵是所述控制裝置包括水流速傳感器、導航裝置和中央控制器；所述導航裝置包括GPS衛星定位傳感器、電子羅盤和慣性導航模塊；所述導航裝置與中央控制器電路連接；所述控制裝置與驅動裝置電路連接，並實現對船體的自動導航。
2.根據權利要求1所述的小型水面機器人裝置，其特徵是所述慣性導航模塊包括三 軸陀螺儀或三軸加速度傳感器。
3.根據權利要求1所述的小型水面機器人裝置，其特徵是所述控制裝置還包括雷達 和雷射測距傳感器。
4.根據權利要求1所述的小型水面機器人裝置，其特徵是所述驅動裝置包括電池和 驅動電機。
5.一種小型水面機器人自適應水流優化導航方法，其特徵是該方法包括以下步驟(1)小型水面機器人通過中央控制器接收GPS定位傳感器的信號，得到其絕對坐標；(2)中央控制器接收慣性導航模塊的信號，得到小型水面機器人的加速度、角速度，並 對所得加速度或角速度積分，獲得實時的速度及角度；(3)中央控制器接收雷射測距傳感器或雷達的數據，判斷與前方障礙物之間距離；(4)中央控制器接收電子羅盤的數據，獲得前進的方向；(5)中央控制器接收水流速傳感器的數據，獲得水流的速度。(6)中央控制器將上述所得數據進行處理後，通過驅動裝置和轉向裝置控制小型水面 機器人的航向。
全文摘要
本發明涉及小型水面機器人裝置及自適應水流優化導航方法，機器人裝置包括船體和驅動裝置、控制裝置，控制裝置包括水流速傳感器、導航裝置和中央控制器；導航裝置包括GPS傳感器、電子羅盤和慣性導航模塊，實現自動導航。自適應水流優化導航方法包括(1)接收GPS信號，得到絕對坐標；(2)獲得實時速度及角度；(3)判斷與障礙物的距離；(4)獲得前進方向；(5)獲得水流速度。(6)控制航向。本發明體積小、重量輕，不僅節約能耗，節省人力，而且成本低、便於使用。本發明的導航方法可優化行駛路徑，節約能源，同時防翻，防撞，避障，達到穩定運行的目的。
文檔編號G05D1/02GK101833335SQ201010167730
公開日2010年9月15日 申請日期2010年5月10日 優先權日2010年5月10日
發明者張雲飛, 張祚, 成亮, 王銘鈺 申請人:珠海雲洲智能科技有限公司