本發明屬于水下機器人領域，具體地說，涉及一種水下無人船航行控制方法及系統。

背景技術：

水下機器人也稱無人水下潛水器，它是一種可以在水下代替人完成某種任務的裝置，在外形上更像一艘微小型潛艇，水下機器人的自身形態是依據水下工作要求來設計的，水下機器人是將人工智能、探測識別信息融合、智能控制、系統集成等多方面的技術集中應用于同一水下載體上，在沒有人工控制，或者人工進行半自動控制下，完成地質、地形等的探測。

目前的水下機器人能夠實現水下航拍和地形、地質的探測等，民用方面的應用還很有限，目前除了作為娛樂用途的無人船之外，用于釣魚的無人船在民用市場的需求越來越大，因此對于釣魚無人船提出了越來越高的要求。

另外針對水下無人船如何根據獲取的姿態數據通過三個推進器來完成對水下無人船航行姿態的調整，成為目前亟待解決的技術問題。

有鑒于此特提出本發明。

技術實現要素：

本發明要解決的技術問題在于克服現有技術的不足，提供一種水下無人船航行控制方法及系統，能夠根據獲取的姿態數據，用戶通過手動或水下無人船自動完成航行姿態的調整。

為解決上述技術問題，本發明采用技術方案的基本構思是：

本發明的第一方面提出了一種水下無人船航行控制方法，步驟包括，

s1，實時獲取水下無人船的姿態數據；

s2，根據姿態數據確定當前航行方向并與目標航行方向進行比較，確定偏移量；

s3，根據偏移量計算驅動模塊的調整數據，并根據調整數據調整水下無人船的航行姿態。優選地，所述姿態數據包括：

陀螺儀檢測的平衡數據、加速度計檢測的加速度數據、磁強計檢測的方位數據。

優選地，所述步驟s2具體包括：

s21，根據平衡數據和加速度數據確定水下無人船的俯仰數據和滾轉數據；

s22，將俯仰數據、滾轉數據和方位數據進行結合確定水下無人船的當前航行方向；

s23，將當前航行方向與目標航行方向進行比較確定偏移量，當偏移量超過設定偏移閾值時啟動驅動模塊調整水下無人船的航行方向，優選地，根據無人船自身的航行狀態確定目標航行方向，或者根據用戶發出的航行命令確定目標航行方向。

優選地，所述步驟s23具體包括：

sa1，計算俯仰數據偏移平衡位置的俯仰偏移量；

sa2，當俯仰偏移量超出設定俯仰偏移閾值時，啟動驅動模塊中的垂直推進器，將水下無人船調整到平衡位置。

優選地，所述步驟s23具體包括：

sb1，計算滾轉數據偏移平衡位置的滾轉偏移量；

sb2，當滾轉偏移量超出設定滾轉偏移閾值時，利用驅動模塊中的垂直推進器和水平推進器將水下無人船調整到平衡位置。

優選地，所述步驟s23具體包括：

sc1,計算方位數據與目標方位的方位偏移量；

sc2，當方位偏移量超出設定方位偏移閾值時，啟動驅動模塊中的水平推進器，將水下無人船調整到目標方位。

本發明的第二方面提出了一種水下無人船航行控制系統，使用上述第一方面所述的水下無人船航行控制方法，包括，控制器、驅動模塊和姿態獲取模塊，所述控制器與驅動模塊和姿態獲取模塊相連，所述控制器根據接收到的航行命令和/或姿態獲取模塊獲取的姿態數據，利用驅動模塊調整水下無人船的航行姿態。

優選地，所述姿態獲取模塊設置在電路板上，包括，陀螺儀、加速度計和磁強計，所述姿態數據包括，所述陀螺儀檢測的平衡數據、所述加速度計檢測的水下無人船的加速度數據和磁強計檢測的方位數據；

所述電路板上設有至少兩個磁強計，所述至少兩個磁強計重合堆疊放置或以電路板為對稱平面對稱設置在電路板兩側。

優選地，所述驅動模塊包括，設置在水下無人船重心前方的垂直推進器、分別設置在水下無人船尾部兩側的水平推進器，其中，垂直推進器調整水下無人船的垂直方向的運動，兩個水平推進器控制水下無人船的前進、后退和轉彎。

優選地，還包括智能跟隨模塊，所述智能跟隨模塊與所述控制器相連，所述控制器根據對移動目標的跟隨命令獲取移動目標的位置信息，并利用智能跟隨模塊控制驅動模塊對移動目標進行跟隨；

優選地，所述控制器獲取移動目標與水下無人船的當前距離，并利用智能跟隨模塊控制驅動模塊保持當前距離對移動目標進行跟蹤。

采用上述技術方案后，本發明與現有技術相比具有以下有益效果。

通過本發明上述技術方案能夠讓用戶獲知水下無人船的航行姿態，并根據該航行姿態控制水下無人船的航行，以及水下無人船根據該航行姿態進行自動調整和糾正，方便了用戶的使用。

用戶就可以利用遙控器或手機控制水下無人船進行加速前行，用戶也可以根據自己的實際需要控制無人船來完成轉彎、掉頭、上升、下潛、旋轉機身等航行姿態的改變功能。

能夠通過控制終端上的顯示屏中顯示的水下無人船的模型看到水下無人船在水下航行的樣子，并且可以將該模型與東南西北四個方位進行配合，這樣用戶就能夠直觀看見水下無人船在水中的航行方向和所處的姿態，進而能夠提高用戶的體驗。

另外，當用戶想要觀察某個魚或魚群的生活狀態時，或者想要跟蹤一些其他水下移動目標時，就可以利用智能跟隨模塊對移動目標進行跟隨了，進而方便了用戶的使用。

通過多個磁強計對檢測的方位信息進行校準，然后將校準結果作為磁強計檢測的最終方位信息結果，并且由于電路板厚度較小，多個磁強計檢測的方位信息的偏差較小，進而使通過多個磁強計進行校準后得到的最終方位信息更加準確。

下面結合附圖對本發明的具體實施方式作進一步詳細的描述。

附圖說明

附圖作為本發明的一部分，用來提供對本發明的進一步的理解，本發明的示意性實施例及其說明用于解釋本發明，但不構成對本發明的不當限定。顯然，下面描述中的附圖僅僅是一些實施例，對于本領域普通技術人員來說，在不付出創造性勞動的前提下，還可以根據這些附圖獲得其他附圖。在附圖中：

圖1是本發明的實施例一的無人船航行控制方法的流程圖；

圖2是本發明的步驟s2的具體展開流程圖；

圖3是本發明的步驟s23的一個具體展開流程圖；

圖4是本發明的步驟s23的另一個具體展開流程圖；

圖5是本發明的步驟s23的再一個具體展開流程圖；

圖6是本發明的一個實施例的無人船航行控制系統的結構框圖；

圖7是本發明的另一個實施例的無人船航行控制系統的結構框圖；

圖8是本發明的再一個實施例的無人船航行控制系統的結構框圖。

需要說明的是，這些附圖和文字描述并不旨在以任何方式限制本發明的構思范圍，而是通過參考特定實施例為本領域技術人員說明本發明的概念。

具體實施方式

為使本發明實施例的目的、技術方案和優點更加清楚，下面將結合本發明實施例中的附圖，對實施例中的技術方案進行清楚、完整地描述，以下實施例用于說明本發明，但不用來限制本發明的范圍。

在本發明的描述中，需要說明的是，術語“上”、“下”、“前”、“后”、“左”、“右”、“豎直”、“內”、“外”等指示的方位或位置關系為基于附圖所示的方位或位置關系，僅是為了便于描述本發明和簡化描述，而不是指示或暗示所指的裝置或元件必須具有特定的方位、以特定的方位構造和操作，因此不能理解為對本發明的限制。

在本發明的描述中，需要說明的是，除非另有明確的規定和限定，術語“安裝”、“相連”、“連接”應做廣義理解，例如，可以是固定連接，也可以是可拆卸連接，或一體地連接；可以是機械連接，也可以是電連接；可以是直接相連，也可以通過中間媒介間接相連。對于本領域的普通技術人員而言，可以具體情況理解上述術語在本發明中的具體含義。

下述實施例中，將水下水下無人船設計成平均密度與周圍水域的密度相近似，水下無人船內設有密封腔，該密封腔具有防水效果，能夠保護密封腔內的各個用電模塊不會浸水，進而保證水下無人船的正常工作，并且通過該密封腔與水下無人船外殼體及各個部件之間的配合，來使水下無人船達到與周圍水域密度相近似，進而在水域中實現零浮力的效果，通過零浮力的水下無人船能夠更好的調整航行方向和航行姿態，另外，在沒有動力驅動的情況下水下無人船能夠靜止懸停在水中。

實施例一

如圖1-5所示，本發明提出了一種水下無人船航行控制方法，步驟包括，

s1，實時獲取水下無人船的姿態數據；

s2，根據姿態數據確定當前航行方向并與目標航行方向進行比較，確定偏移量；

s3，根據偏移量計算驅動模塊的調整數據，并根據調整數據調整水下無人船的航行姿態。用戶能夠利用手機或遙控器控制驅動模塊驅動無人船進行集魚、和/或尋魚、和/或釣魚、和/或觀察水下魚群/水下環境，利用水下無人船上的姿態獲取模塊將獲取的姿態數據發送給控制器，控制器可以將該姿態數據進行計算處理后利用通信模塊實時反饋到水上控制端，水上控制端上設有顯示屏可以將水下無人船的航行姿態實時顯示給用戶，以供用戶根據該航行姿態控制水下無人船的水下航行；

用戶利用遙控器或者具有遙控水下無人船功能的移動終端來控制水下無人船的航行方向、旋轉角度或者下潛深度等，例如，用戶在顯示屏上觀察到水下無人船還沒有達到用戶想要進行釣魚的目的地，顯示屏上顯示水下無人船的前行速度比較慢，用戶就可以利用遙控器或手機控制水下無人船進行加速前行，用戶也可以根據自己的實際需要控制無人船來完成轉彎、掉頭、上升、下潛、旋轉機身等航行姿態的改變功能。

另外，水下無人船在航行過程中，如果出現航道偏移等現象，水下無人船能夠自動糾正，調整到正確的航道上來。

優選地，所述姿態數據包括：

陀螺儀檢測的平衡數據、加速度計檢測的加速度數據、磁強計檢測的方位數據。

如圖2所示，所述步驟s2具體包括：

s21，根據平衡數據和加速度數據確定水下無人船的俯仰數據和滾轉數據；

s22，將俯仰數據、滾轉數據和方位數據進行結合確定水下無人船的當前航行方向；

s23，將當前航行方向與目標航行方向進行比較確定偏移量，當偏移量超過設定偏移閾值時啟動驅動模塊調整水下無人船的航行方向，優選地，根據無人船自身的航行狀態確定目標航行方向，或者根據用戶發出的航行命令確定目標航行方向。

通過陀螺儀得到旋轉角速度數據，通過加速度計得到加速度數據，(其中，測量的角速度數據和加速度數據均在無人船本體坐標系下)

由于當無人船無運動加速度時，在參考坐標系下恒受豎直向下的重力加速度，根據重力加速度在本體坐標系的分量(加速度計測得)可以估算無人船相對于參考坐標系的俯仰角和滾轉角。

其中，陀螺儀可以測量無人船俯仰角和滾轉角的變化，俯仰角和滾轉角由角度的估計值(加速度計算得到)和角度在一個周期內的變化值(陀螺儀測量)，通過卡爾曼濾波，得到角度的最小方差估計，即最優估計。

對所述陀螺儀進行零偏校正，利用零偏校正后的陀螺儀獲取平衡數據，利用所述加速度計檢測的加速度數據計算俯仰速度&滾轉角速度，并將平衡數據和俯仰速度&滾轉角速度進行結合確定水下無人船的俯仰姿態和滾轉姿態。

在上述技術方案中，由于陀螺儀會受到水下無人船上的各個結構或組件的影響，或者其他情況，陀螺儀檢測的平衡數據會有偏差，因此需要首先將陀螺儀進行零偏校正，進而保證陀螺儀的檢測精度，零偏校正完成后，陀螺儀就會獲取相應的平衡數據；

然后將該平衡數據與加速度計檢測的俯仰速度&滾轉速度進行結合，確定水下無人船當前的俯仰姿態(即，水下無人船偏移水平面的姿態)和滾轉姿態(即，水下無人船偏移)，例如，能夠確定水下無人船在向前、后、左、右、上、下六個方位中的偏移航行姿態。

對所述磁強計進行零偏校正和橢圓校正，利用校正后的磁強計獲取方位數據，所述控制器將方位數據、平衡數據和俯仰速度&滾轉角速度進行結合確定水下無人船的當前航行方向。

在上述技術方案中，受環境因素和磁強計自身因素的影響，磁強計常存在較大的航向角誤差，為了保證磁強計的精度，首先要對磁強計進行零偏校正和橢圓校正，然后再利用校正后的磁強計獲取水下無人船的方位數據(即，獲取水下無人船在東、南、西、北四個方向中所處的方位)，并將該方位數據與上述方案中利用陀螺儀和加速度計獲得的俯仰速度&滾轉角速度進行結合，能夠進一步確定出水下無人船的當前航行方向。

所述加速度計進行姿態補償，之后將獲取的加速度數據去除重力項得到去重加速度數據，對獲取的三軸的去重加速度數據進行積分，確定出水下無人船的當前航行速度。

無人船在某固定空間受到一恒強度的磁場(大致指向北方)，當無人船姿態發生變化時，該磁場在無人船參考坐標系的分量發生改變，導致磁強計的測量值發生改變，由于制造工藝或周圍磁場的干擾問題，首先需要對磁強計進行標定，對數據進行校準。之后刨除俯仰角和滾轉角對測量值的影響(即，姿態補償)，最后計算得到無人船偏航角的估計值。

結合陀螺儀測量的俯仰角速度的變化，利用卡爾曼濾波得到偏航角的最優值，以上為導航數據處理部分，利用多慣性器件的數據融合得到無人船的姿態數據。

陀螺儀檢測的平衡數據是水下無人船參考坐標系與水下無人船本體坐標系的旋轉矩陣，所述加速度計的測量值是基于水下無人船本體坐標系的，將本體坐標系分為三個坐標軸即x軸、y軸、z軸，測量值本身就是三軸的；

然后，利用該加速度計獲取相應的三軸加速度數據，姿態補償就是把三軸加速度數據轉換到參考坐標系中，由于獲取的加速度數據中會有重力加速度，因此，需要將重力加速度進行去除，最后對經過姿態補償和去除重力項之后獲得的三軸的加速度值分別進行積分，就知道水下無人船在三個方向上的當前航行速度。

如圖3所示，所述步驟s23具體包括：

sa1，計算俯仰數據偏移平衡位置的俯仰偏移量；

sa2，當俯仰偏移量超出設定俯仰偏移閾值時，啟動驅動模塊中的垂直推進器，將水下無人船調整到平衡位置。

如圖4所示，所述步驟s23具體包括：

sb1，計算滾轉數據偏移平衡位置的滾轉偏移量；

sb2，當滾轉偏移量超出設定滾轉偏移閾值時，利用驅動模塊中的垂直推進器和水平推進器將水下無人船調整到平衡位置。

如圖5所示，所述步驟s23具體包括：

sc1,計算方位數據與目標方位的方位偏移量；

sc2，當方位偏移量超出設定方位偏移閾值時，啟動驅動模塊中的水平推進器，將水下無人船調整到目標方位。

航向控制：由航向的目標值(用戶給定)和無人船的實際航向值(導航數據)得到航向的偏差值(即俯仰偏移量、滾轉偏移量、方位偏移量)，利用增量式pid算法計算得到無人船的自旋糾正轉矩，最后映射為推進器的推力值(電機轉速)。

實施例二

如圖6所示，本實施例提出了一種水下無人船航行控制系統，包括，控制器1、驅動模塊2和姿態獲取模塊3，所述控制器1與驅動模塊2和姿態獲取模塊3相連，所述控制器1根據接收到的航行命令和/或姿態獲取模塊3獲取的姿態數據，利用驅動模塊2調整水下無人船的航行姿態。

所述姿態獲取模塊3設置在電路板上，包括，陀螺儀31、加速度計32和磁強計33，所述姿態數據包括，所述陀螺儀31檢測的平衡數據、所述加速度計32檢測的水下無人船的加速度數據和磁強計33檢測的方位數據；

所述電路板上設有至少兩個磁強計33，所述至少兩個磁強計33重合堆疊放置或以電路板為對稱平面對稱設置在電路板兩側。

這樣通過兩個磁強計33對檢測的方位信息進行校準，然后將校準結果作為磁強計33檢測的最終方位信息結果，并且由于電路板厚度較小，兩個磁強計33檢測的方位信息的偏差較小，進而使通過兩個磁強計33進行校準后得到的最終方位信息更加準確。并且由于兩個磁強計33只是在垂直方向上有偏差這樣控制器在進行校準計算時只要針對垂直方向進行相應計算校準就可以，能夠減少控制器的計算量，進而加快了計算速率，并且還能夠提高水下無人船方位檢測的準確性。

并且，還可以將兩個磁強計33整合成一體，進而使兩個磁強計33檢測的偏差進一步減小，使整個校準算法能夠更加準確，這樣就能提高水下無人船的工作性能。

也可以在電路板上設置兩個以上的磁強計33，這樣就可以利用多個磁強計33的相互校準來使水下無人船方位檢測的準確性得到更加有效的提高。

另外，在電路板上還可以設置兩個加速度計32，這樣就可以利用兩個加速度計32進行互補校正，這樣經過校正后得到的加速度值能夠更加準確，進而提高水下無人船的工作性能。

所述驅動模塊2包括，設置在水下無人船重心前方的垂直推進器、分別設置在水下無人船尾部兩側的水平推進器，其中，垂直推進器調整水下無人船的垂直方向的運動，兩個水平推進器控制水下無人船的前進、后退和轉彎。

實施例三

如圖7所示，該水下無人船航行控制系統還包括智能跟隨模塊4，所述智能跟隨模塊4與所述控制器1相連，所述控制器1根據對移動目標的跟隨命令獲取移動目標的位置信息，并利用智能跟隨模塊4控制驅動模塊2對移動目標進行跟隨；

所述控制器1獲取移動目標與水下無人船的當前距離，并利用智能跟隨模塊4控制驅動模塊2保持當前距離對移動目標進行跟蹤。在上述技術方案中，當用戶通過遙控器或者手機或其他控制端向無人船發出對某移動目標(可以是魚、人、水下潛艇或者其他能夠移動的物體)的跟隨命令后，首先獲取移動目標的位置信息，對移動目標進行鎖定，然后啟動智能跟隨模塊4，控制水下無人船對移動目標進行跟隨，其中，移動目標還可以是魚群。這樣，當用戶想要觀察某個魚或魚群的生活狀態時，或者想要跟蹤一些其他水下移動目標時，就可以利用該智能跟隨模塊4對移動目標進行跟隨了，進而方便了用戶的使用。

所述控制器1獲取移動目標與水下無人船的當前距離，并利用智能跟隨模塊4保持當前距離對移動目標進行跟蹤；所述智能跟隨模塊4接收到一鍵跟隨命令后，對距離水下無人船最近的移動生物進行跟隨。

實施例四

如圖8所示，在上述實施例的方案的基礎上，所述水下無人船航行控制系統還包括與控制器1相連的全球定位模塊5能夠實時獲取水下無人船的位置信息。

該全球定位模塊5為gps模塊或北斗模塊，這樣能夠實時獲知該水下無人船的位置，這樣能夠對水下無人船進行跟蹤定位，也能方便用戶尋找水下無人船，給用戶帶來便利。

實施例五

水下無人船航行控制系統還包括與控制器相連的通信模塊，所述控制器將接收的姿態數據通過通信模塊發送至水上控制端，并利用水上控制端上的顯示屏將姿態數據進行顯示，在所述顯示屏上顯示水下無人船的模型，并將姿態數據通過所述水下無人船的模型進行直觀展示。

姿態獲取模塊能夠獲取水下無人船當前的航行姿態，比如，航行速度(各個軸向、各個方向的航行速度)，水下無人船的當前姿態(是處于水平或傾斜或俯仰等姿態)，并將這些航行姿態實時反饋給控制器，控制器將這些姿態數據進行處理后利用通信模塊發送給水上控制端(可以是手機、平板、筆記本、電腦或者帶有顯示屏的遙控器等)，當用戶啟動對水下無人船的控制軟件后，控制終端上的顯示屏中就會顯示出水下無人船的模型，水下無人船在水下航行的姿態可以通過該模型直觀的展現給用戶，用戶能夠通過模型看到水下無人船在水下航行的樣子，并且可以將該模型與東南西北四個方位進行配合，這樣用戶就能夠直觀看見水下無人船在水中的航行方向和所處的姿態，進而能夠提高用戶的體驗。

所述控制器將當前航行方向和當前航行速度通過通信模塊傳送至水上控制端；優選地，水上控制端發送的航行命令通過通信模塊發送至控制器，所述控制器將當前航行方向、當前航行速度與航行命令進行結合控制驅動模塊調整水下無人船的航行姿態。在上述技術方案中，可以將水下無人船的航行速度反饋至水上控制端，可以通過數字或者圖形的方式展現出來，這樣用戶就可以將航行速度與上述的水下無人船在水中的航行方向和所處的姿態進行結合，進而使用戶能夠做出更加準確的控制命令，控制水下無人船的航行姿態。

以上所述僅是本發明的較佳實施例而已，并非對本發明作任何形式上的限制，雖然本發明已以較佳實施例揭露如上，然而并非用以限定本發明，任何熟悉本專利的技術人員在不脫離本發明技術方案范圍內，當可利用上述提示的技術內容做出些許更動或修飾為等同變化的等效實施例，但凡是未脫離本發明技術方案的內容，依據本發明的技術實質對以上實施例所作的任何簡單修改、等同變化與修飾，均仍屬于本發明方案的范圍內。