基于多傳感器的波浪能滑翔器導(dǎo)航策略研究*

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

（1.天津城建大學(xué) 計(jì)算機(jī)與信息工程學(xué)院，天津 300384;2.天津城建大學(xué)控制與機(jī)械工程學(xué)院，天津 300384;3.國(guó)家海洋技術(shù)中心 深海室，天津 300112）

0 引言

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

1 平臺(tái)嵌入式控制與導(dǎo)航系統(tǒng)

1.1 基本結(jié)構(gòu)

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

圖1 嵌入式平臺(tái)導(dǎo)航系統(tǒng)Fig 1 Embedded platform navigation system

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

1.2 導(dǎo)航傳感系統(tǒng)組成

1.2.1 傳感器

綜合考慮導(dǎo)航傳感器的精度和波浪能自航行平臺(tái)對(duì)導(dǎo)航精度的要求，同時(shí)考慮到舵機(jī)轉(zhuǎn)向的機(jī)動(dòng)性和方向矯正的能耗，設(shè)計(jì)了基于電子羅盤(pán)和GPS的組合導(dǎo)航系統(tǒng);同時(shí)選用波高儀和流量計(jì)作為輔助導(dǎo)航傳感器，確保導(dǎo)航精度。

1.2.2 執(zhí)行控制器

波浪能滑翔器在海上運(yùn)動(dòng)，只依賴海洋中的波浪提供動(dòng)力，不依賴外部動(dòng)力系統(tǒng)。因此，只需在水下滑行機(jī)尾部安裝尾舵機(jī)構(gòu)進(jìn)行控制，LPC2478采集GPS的經(jīng)緯度和電子羅盤(pán)的航向角數(shù)據(jù)，經(jīng)數(shù)據(jù)處理后注入導(dǎo)航算法，與預(yù)設(shè)航向比較得到航向角偏差值，控制舵機(jī)打舵偏角，從而實(shí)現(xiàn)了波浪能滑翔器在海上朝預(yù)設(shè)位置自主運(yùn)行。

2 導(dǎo)航算法的分析與實(shí)現(xiàn)

波浪能自航行平臺(tái)載有GPS和電子羅盤(pán)，利用當(dāng)前位置的經(jīng)緯度和航向角作為輸入，通過(guò)高斯大地線算法計(jì)算出2個(gè)坐標(biāo)點(diǎn)的距離長(zhǎng)度和航向角偏差值，通過(guò)PID算法控制舵機(jī)打舵角度，使波浪能滑行器到達(dá)預(yù)設(shè)位置坐標(biāo)，從而實(shí)現(xiàn)視向跟蹤，如圖3所示;到達(dá)預(yù)設(shè)位置坐標(biāo)半徑內(nèi)50 m后，在設(shè)定時(shí)間內(nèi)保持波浪能滑翔器原地45°轉(zhuǎn)圈，采集該區(qū)域內(nèi)的海洋數(shù)據(jù)，完成海洋監(jiān)測(cè)任務(wù)，實(shí)現(xiàn)虛擬錨泊，如圖3所示。

圖3 導(dǎo)航算法圖解Fig 3 Navigation algorithm illustration

2.1 視向跟蹤算法

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

2.2 高斯大地線算法

通過(guò)GPS可獲得波浪能滑翔器在海面上的經(jīng)度和緯度基于高斯大地線算法，以經(jīng)緯度作為輸入?yún)⒘浚?jì)算出波浪能滑翔器當(dāng)前位置與預(yù)設(shè)位置的距離長(zhǎng)度，作為波浪能滑翔器航行的距離;同時(shí)計(jì)算出預(yù)設(shè)路徑的航向角，實(shí)現(xiàn)對(duì)波浪能滑翔器的角度校正。高斯大地線算法是基于高斯平均引數(shù)來(lái)進(jìn)行大地主題反解的迭代算法［3］，如圖4所示。

圖4 導(dǎo)航算法流程圖Fig 4 Flow chart of navigation algorithm

令

則高斯平均引數(shù)正解公式可以寫(xiě)成下面的形式

由上式可得

2.3 PID 算法

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

圖5 PID控制器Fig 5 PID controller

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

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

在上述PID控制算法中，當(dāng)有較大幅度的擾動(dòng)或大幅度改變給定值時(shí)，由于此時(shí)有較大的偏差，以及系統(tǒng)有慣性和滯后，故在積分項(xiàng)的作用下，往往會(huì)產(chǎn)生較大的超調(diào)量和長(zhǎng)時(shí)間的波動(dòng)。為此可以采用積分分離措施，將航向角偏差值設(shè)一個(gè)上限emax，若|ek|＜|emax|，采用 PID控制;若|ek|≥|emax|，采用PD控制，具體算法描述為

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

綜合高斯大地線算法和PID算法可構(gòu)建出波浪能滑翔器導(dǎo)航算法的數(shù)學(xué)控制模型為

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

3 導(dǎo)航系統(tǒng)的實(shí)現(xiàn)流程

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

圖6 PID算法的應(yīng)用Fig 6 Application of PID algorithm

圖7 導(dǎo)航流程圖Fig 7 Navigation flow chart

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

4 實(shí)驗(yàn)結(jié)果與分析

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

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

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

圖11 溫度數(shù)據(jù)Fig 11 Temperature datas

圖12 風(fēng)向數(shù)據(jù)Fig 12 Wind direction datas

5 結(jié)束語(yǔ)

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

［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］趙長(zhǎng)勝.高斯平均引數(shù)計(jì)算大地坐標(biāo)主題反解的迭代算法［J］.測(cè)繪通報(bào)，2004（10）:11-12.

［4］馮 城，葉文華.基于ARM處理器的數(shù)字PID伺服電機(jī)調(diào)速系統(tǒng)［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.