基于多傳感器的波浪能滑翔器導航策略研究*

史 健，龔 威，齊占峰，賈立娟，張 森，秦玉峰

（1.天津城建大學 計算機與信息工程學院，天津 300384;2.天津城建大學控制與機械工程學院，天津 300384;3.國家海洋技術中心 深海室，天津 300112）

0 引言

波浪能滑翔器是一種多功能的海洋自主運動平臺，其主要包括2方面結構:舟行上浮體和水下滑翔，舟行上浮體搭載北斗、電子羅盤、GPS以及波高儀和流量計等傳感器采集導航信息;同時搭載多種科學儀器，采集海洋數據，完成海洋監測任務［1］。水下滑翔機主要結構為舵機，根據不同位置角度的偏差值，實現打舵轉彎，從而控制波浪能滑翔器朝預設位置運動。近十年來，美國相繼開發了多型波浪能滑翔器作為海洋觀測平臺，并投入了海洋水文資料探測和軍事應用，水下航行器觀測平臺包括高機動性的自主水下機器人（AUV）和低機動性水下滑翔器都得到較好的應用，而我國在海洋自主監測方面的研究才剛剛起步，國家海洋技術中心聯合清華大學，天津城建大學等多所高校和科研單位致力于國家公益性行業科研基金專項“全球氣候變化海洋CO2自航監測平臺研發與應用示范”的研究與開發，取得了重大突破［2］。

1 平臺嵌入式控制與導航系統

1.1 基本結構

平臺嵌入式控制與導航系統包括平臺嵌入式控制器、平臺航行與傳感系統、能源管理系統、通信設備等，具體系統結構連接示意圖如圖1所示。由于滑翔器工作環境復雜且惡劣，所以，器件均需選擇低功耗、高可靠性的工業級芯片，降低系統功耗，提高系統的可靠性。LPC2478具有較高主頻和同步動態隨機存取器SRAM，具有速度高、能耗低的特點。因此，采用以LPC2478為微處理器的Mini ARM嵌入式核心板，中央處理器采用ARM7TDMI芯片。同時，為提高任務調度的實時性，預裝μC/OS-II操作系統;預留外設接口連接導航設備、傳感器和通信系統等，如圖2所示。

圖1 嵌入式平臺導航系統Fig 1 Embedded platform navigation system

圖2 嵌入式控制器Fig 2 Embedded controller

1.2 導航傳感系統組成

1.2.1 傳感器

綜合考慮導航傳感器的精度和波浪能自航行平臺對導航精度的要求，同時考慮到舵機轉向的機動性和方向矯正的能耗，設計了基于電子羅盤和GPS的組合導航系統;同時選用波高儀和流量計作為輔助導航傳感器，確保導航精度。

1.2.2 執行控制器

波浪能滑翔器在海上運動，只依賴海洋中的波浪提供動力，不依賴外部動力系統。因此，只需在水下滑行機尾部安裝尾舵機構進行控制，LPC2478采集GPS的經緯度和電子羅盤的航向角數據，經數據處理后注入導航算法，與預設航向比較得到航向角偏差值，控制舵機打舵偏角，從而實現了波浪能滑翔器在海上朝預設位置自主運行。

2 導航算法的分析與實現

波浪能自航行平臺載有GPS和電子羅盤，利用當前位置的經緯度和航向角作為輸入，通過高斯大地線算法計算出2個坐標點的距離長度和航向角偏差值，通過PID算法控制舵機打舵角度，使波浪能滑行器到達預設位置坐標，從而實現視向跟蹤，如圖3所示;到達預設位置坐標半徑內50 m后，在設定時間內保持波浪能滑翔器原地45°轉圈，采集該區域內的海洋數據，完成海洋監測任務，實現虛擬錨泊，如圖3所示。

圖3 導航算法圖解Fig 3 Navigation algorithm illustration

2.1 視向跟蹤算法

波浪能滑翔器在海洋上運行是按照預設軌跡進行移動，通過實時采集GPS和電子羅盤的數據，由高斯大地線算法計算當前位置與預設路徑的距離長度和偏移角度，從而調整滑翔器舵機的打舵角度，使其朝預設位置移動。為了保證波浪能滑翔器移動的精度和準確性，同時考慮到GPS和電子羅盤的精度要求，預設軌跡點的密度應保持適中，一般以500 m為單位設定一處軌跡點。

2.2 高斯大地線算法

通過GPS可獲得波浪能滑翔器在海面上的經度和緯度基于高斯大地線算法，以經緯度作為輸入參量，計算出波浪能滑翔器當前位置與預設位置的距離長度，作為波浪能滑翔器航行的距離;同時計算出預設路徑的航向角，實現對波浪能滑翔器的角度校正。高斯大地線算法是基于高斯平均引數來進行大地主題反解的迭代算法［3］，如圖4所示。

圖4 導航算法流程圖Fig 4 Flow chart of navigation algorithm

令

則高斯平均引數正解公式可以寫成下面的形式

由上式可得

2.3 PID 算法

模擬PID控制器的系統原理如圖5所示，其控制規律如公式（3）描述。

圖5 PID控制器Fig 5 PID controller

由于ARM平臺LPC2478是通過軟件的算法執行PID控制，因此不能用模擬方式進行連續控制，只能根據采樣時刻的偏差值來計算輸出的速度。所以，必須將模擬PID控制器進行離散化和增量式處理。在連續時間t內進行K次連續采樣，每次采樣周期為T，則得到模擬PID控制器的控制規律的離散變換如下所示［4］:

其中，uk為第k次采樣時刻輸出的舵機打舵角度;ek為第k次采樣時刻輸入的航向角偏差，其值為電子羅盤測量得到的當前航向角rk與高斯大地線算法計算得到的預設航向角Amk的差值，即ek=Amk-rk;KP為比例系數，TI為積分系數，TD為微分系數，令 Δuk=uk-uk-1，Δek=ekek-1，則離散化后的增量式算法表述為

在上述PID控制算法中，當有較大幅度的擾動或大幅度改變給定值時，由于此時有較大的偏差，以及系統有慣性和滯后，故在積分項的作用下，往往會產生較大的超調量和長時間的波動。為此可以采用積分分離措施，將航向角偏差值設一個上限emax，若|ek|＜|emax|，采用 PID控制;若|ek|≥|emax|，采用PD控制，具體算法描述為

其中，k=2，3，4，…。

綜合高斯大地線算法和PID算法可構建出波浪能滑翔器導航算法的數學控制模型為

其中，Δuk為第k次輸出的舵機打舵角度;Amk為在第m個位置的第k次由高斯大地線算法計算得到的預設方位角;rk為第k次由電子羅盤測量得到的當前波浪能滑翔器的方位角;T為采樣周期;KP，TD，TI分別為PID控制算法的比例系數，微分系數和積分系數。其在導航系統中的應用流程圖如圖6所示。

3 導航系統的實現流程

波浪能滑翔器在海洋上運動，嵌入式控制系統（包括ARM—LPC2478控制器和多路傳感器，可參照表1描述），實時采集導航數據，獲得波浪能滑翔器當前的經緯度，航向角數據以及用于輔助導航的波浪高度和排水流量數據。由于在實際采集的導航數據中含有噪聲雜質，干擾導航精度，因此，利用卡爾曼濾波算法對原始數據進行降噪處理［5，6］;同時，考慮到導航數據的采集存在一定誤差，對原始數據進行整合，對多次采樣的數據做平均處理。對濾波與整合后的經緯度和航向角注入高斯大地線算法計算出當前位置與下一預設位置的距離長度和預設航向角角度值。通過采集海洋中波浪的高度和滑翔器的排水流量［7］，計算得到波浪能滑翔器的當前的運行速度，從而得到波浪能滑翔器從當前位置達到下一預設坐標位置的航行距離和航行時間。預設航向角與電子羅盤采集的實時航向角計算差值，得到的航向角偏差值作為輸入量注入到PID控制算法中，輸出舵機控制器的打舵角度，從而實現波浪能滑翔器按照預設軌跡運行。當當前位置與預設位置的距離長度小于設定值時，保持波浪能滑翔器在所設定海域內的45°轉角，采集海洋數據，完成海洋監測任務。其具體流程圖如圖7、圖8所示。

圖6 PID算法的應用Fig 6 Application of PID algorithm

圖7 導航流程圖Fig 7 Navigation flow chart

圖8 流程圖詳解Fig 8 Detailed annotation of flow chart

4 實驗結果與分析

對波浪能滑翔器在青島進行實地海試，使用上位機軟件顯示波浪能滑翔器的航跡圖。圖9為第一次海試時，未使用傳感器，采用遙控打舵的方式控制平臺運行的航跡圖;圖10為第二次海試采用多傳感器控制系統，并使用高斯大地線算法和PID算法混合算法得到的平臺自運行航跡圖。其中設置采樣間隔為5 min，設置預設軌跡位置的間隔為500 m，預設軌跡形狀為直接三角形，當時實際海況為2～3級，平均有效航速1.05 m/s。由實際海試結果得出采用多傳感器并多算法混合的系統得到的運行軌跡與預設軌跡符合度更高，基本達到了實驗要求。驗證了波浪能滑翔器導航系統的可行性。圖11、圖12為波浪能滑翔器自航行過程中通過氣象傳感器采集到的溫度與風向數據。

圖9 遙控打舵方式得到的海試航跡圖Fig 9 Tracking map by remote control rudder way

圖10 多傳感器與混合算法方式得到的海試航跡圖Fig 10 Tracking map by mode of multi-sensor and mixing algorithms

圖11 溫度數據Fig 11 Temperature datas

圖12 風向數據Fig 12 Wind direction datas

5 結束語

本文設計了基于嵌入式ARM平臺LPC2478的波浪能滑翔器的導航系統，在硬件上，選用多路高精度傳感器:GPS和電子羅盤作為主傳感器，波高儀和流速儀作為輔助導航傳感器，既提高了導航的精度，又保證了導航系統的低功耗和高可靠性;軟件設計中，采用高斯大地線算法和PID控制算法相結合的算法，建構出一種新的導航算法，通過實際應用，驗證了算法的可行性和有效性;同時在任務調度中，移植μC/OS-II操作系統，實現了對多路傳感器不同任務的合理調度，保證了系統的實時性。整個系統中，嵌入式控制系統的合理搭建和多路傳感器的同時使用，以及多種算法的有效結合，提高了波浪能滑翔器在海洋運行中的導航精度，降低了導航誤差。

［1］Graver J G，Underwater gliders:Dynamics，control and design［D］.New Jersey:Princeton University，2003.

［2］Kraus N，Bingham B.Estimation of wave glider dynamics for precise positioning［C］//Oceans'11，MTS/IEEE KONA，2011.

［3］趙長勝.高斯平均引數計算大地坐標主題反解的迭代算法［J］.測繪通報，2004（10）:11-12.

［4］馮 城，葉文華.基于ARM處理器的數字PID伺服電機調速系統［J］.電工電氣，2010（3）:47-48.

［5］Mahmoudian Nina.Efficient motion planning and control for underwater gliders［D］.Blacksburg，Virginia，USA:Virginia Polytechnic Institute and State University，2009.

［6］Daniel T，Manley J，Trenaman N.The wave glider:Enabling a new approach to persistent ocean observation and research［J］.Ocean Dynamics，2011，61:1509-1520.

［7］Manley J，Willcox S.The wave glider:A new concept for deploying ocean instrumentation［J］.IEEE Instrumentation ＆ Measurement Magazine，2010，13:8-13.