一種基于GPS的自動(dòng)航向控制系統(tǒng)＊

董沛然 張 浩 辛愛學(xué) 王 釗

（海軍704廠 青島 266109）

1 引言

現(xiàn)代的GPS接收機(jī)除了提供位置數(shù)據(jù)外，還可提供日期、時(shí)間、航向和航速等參數(shù)［1］，根據(jù)這些參數(shù)可以對(duì)船舶的航路進(jìn)行規(guī)劃，實(shí)時(shí)控制舵角變化以完成自動(dòng)航向控制。船舶在開闊水域航行時(shí)，可以預(yù)設(shè)好多個(gè)航路目標(biāo)點(diǎn)，本系統(tǒng)即可按照目標(biāo)點(diǎn)控制船舶自動(dòng)航行。

2 GPS數(shù)據(jù)讀取

2.1 GPS模塊

GPS模塊使用GARMIN的12通道GPS15L接收機(jī)，同時(shí)可以跟蹤最多12顆GPS衛(wèi)星，從而能夠快速定位，數(shù)據(jù)更新率為每秒一次，性能可以滿足導(dǎo)航的靈敏度需求和動(dòng)態(tài)需求。數(shù)據(jù)接口采用異步串行數(shù)據(jù)輸入輸出［2］，數(shù)據(jù)格式使用美國(guó)國(guó)家海洋電子協(xié)會(huì)的NMEA 0183ASCII碼接口協(xié)議［3］。

2.2 GPS數(shù)據(jù)格式

采用推薦定位信息（Recommended Minimum Specific GPS／TRANSIT Data）格式［4］，該格式定義見表1。

2.3 GPS數(shù)據(jù)處理

讀取GPRMC數(shù)據(jù)后，分別對(duì)第＜3＞項(xiàng)緯度，第＜5＞項(xiàng)經(jīng)度，第＜7＞項(xiàng)速率，第＜8＞項(xiàng)航向單獨(dú)讀取并存為字符串變量和double型變量，作為下一步流程航向控制的參數(shù)［5］。

表1 GPRMC數(shù)據(jù)格式

3 自動(dòng)舵控制

航向控制實(shí)時(shí)更新最新航向變化，從而推算目標(biāo)航向，在接口單元，建立一個(gè)獨(dú)立線程。該線程完成目標(biāo)航向計(jì)算，目標(biāo)舵角計(jì)算，打舵過(guò)程的功能。

圖1 自動(dòng)舵控制原理

在航向改變過(guò)程中，自動(dòng)舵的舵角并不只是隨航向角的偏差而變化，而是和航向差變化的速率以及航向偏差對(duì)時(shí)間的積分有關(guān)［6］。由圖1舵角與航向的關(guān)系為

其中K＊＝K·L／μ，T＊＝T·L／μ。

式中L和μ分別是船長(zhǎng)和航速，由以上公式可得，當(dāng)船長(zhǎng)，船性能不變Kp與航速平方成反比。

Kd與航速的平方成反比。

若忽略首搖運(yùn)動(dòng)的自然頻率ωn和阻尼系數(shù)ζ，舵角可以簡(jiǎn)化為

4 航向控制

4.1 目標(biāo)航向算法

如圖2所示，當(dāng)前航向Dir用實(shí)線表示，目標(biāo)航向Tar用虛線表示，R＿dir指示向右偏轉(zhuǎn)的航向，L＿dir指示向左偏轉(zhuǎn)的航向。

圖2 目標(biāo)航向算法

當(dāng)Tar＜Dir時(shí)候：

所有統(tǒng)計(jì)計(jì)算均用SASv 9.13統(tǒng)計(jì)分析軟件進(jìn)行。定量數(shù)據(jù)采用t檢驗(yàn)、ANOVA或Wilcoxon秩和檢驗(yàn)。定性數(shù)據(jù)用χ2檢驗(yàn)、Fisher精確概率、連續(xù)校正χ2檢驗(yàn)、或Wilcoxon秩和檢驗(yàn)，考慮到中心或其他因素的影響采用CMHχ2檢驗(yàn)。對(duì)于生存數(shù)據(jù)采用，Log‐rank檢驗(yàn)。主要指標(biāo)的兩兩比較，采用95%置信區(qū)間（95%CI）法，計(jì)算雙側(cè)95%CI。全部的假設(shè)檢驗(yàn)均采用雙側(cè)檢驗(yàn)，取α=0.05。

當(dāng)Tar＞Dir時(shí)候：

計(jì)算完成比較R＿dir及L＿dir大小，判斷航向改變的方向和舵角的大小。

4.2 舵角控制優(yōu)化

當(dāng)目標(biāo)航向計(jì)算完成，經(jīng)上述公式得到舵角后，舵角量應(yīng)經(jīng)過(guò)舵角限定算法，防止舵角超出舵角控制范圍。

由于舵機(jī)控制及舵的剎車?yán)匣?，造成了在達(dá)到目標(biāo)舵角時(shí)刻斷開舵機(jī)繼電器不能及時(shí)停止現(xiàn)象，而且左右舵由于慣性問題沖出的舵角大小不等，所以引入慣性系數(shù)概念，讀取舵角信息的同時(shí)計(jì)算舵角變化率，當(dāng)舵角變化速度高的時(shí)候，停舵提前量大，反之，若舵機(jī)繼電器吸合時(shí)間短，舵變化速率慢的情況下，停舵的提前量較?。?/p>

經(jīng)過(guò)一定時(shí)間的調(diào)試，當(dāng)K值達(dá)到一定的時(shí)候，可以使得舵角精度達(dá)到±0.3°之間。

4.3 航路規(guī)劃及執(zhí)行

能按計(jì)劃導(dǎo)航點(diǎn)順序航行，在計(jì)算L＿dir、R＿dir前更新目標(biāo)航向。

程序運(yùn)行時(shí)，依次設(shè)置目標(biāo)點(diǎn)，按順序記錄在內(nèi)存中，程序裝載第一個(gè)目標(biāo)點(diǎn)位置信息，并由當(dāng)前GPS經(jīng)緯度計(jì)算與目標(biāo)導(dǎo)航點(diǎn)的航向，更新目標(biāo)航向，目標(biāo)航向每秒更新一次，使船按照目標(biāo)航跡航行。當(dāng)?shù)竭_(dá)目標(biāo)點(diǎn)時(shí)，程序會(huì)自動(dòng)切換下一目標(biāo)點(diǎn)，啟動(dòng)新的航向控制［7］。

4.4 航向數(shù)據(jù)修正

如圖4所示，當(dāng)海浪較大時(shí)，會(huì)產(chǎn)生大量海浪和船首搖擺等噪音向量，這些噪音向量與真實(shí)航向的疊加，航向數(shù)據(jù)呈正弦型在真實(shí)航向周圍變化，不能如實(shí)反映航向，影響到航向保持功能。

圖3 根據(jù)當(dāng)前GPS經(jīng)緯度計(jì)算與目標(biāo)導(dǎo)航點(diǎn)的航向

圖4 海浪和船首搖擺對(duì)航向的影響

c＿PLF1（k）＝［c（k）＋c（k－1）＋ … ＋c（k－FIR＿PLF＿Len＋1）］／FIR＿PLF＿Len

FIR＿PLF＿Len為第一級(jí)FIR濾波器長(zhǎng)度，即當(dāng)前時(shí)刻輸入數(shù)據(jù)c（k）連同前面（FIR＿PLF＿Len－1）個(gè)數(shù)據(jù)求均值的數(shù)據(jù)總數(shù)。

c＿PLF1（k）為第一級(jí)濾波輸出；

再用IIR第二級(jí)低通濾波平滑：

Delta＿Crt＝c＿PLF1（k）－c＿Avg；Delta＿Crt為當(dāng)前誤差，即最新輸入值－上次紀(jì)錄平滑后取值。

Delta＿Avg＝ Delta＿Crt＊ p＋ （1－ p）＊ Delta＿Avg；對(duì)誤差進(jìn)行平滑，當(dāng)前誤差Delta＿Crt中含有大量噪聲，因此乘p＝0.2，而誤差均值Delta＿Avg中可認(rèn)為已含有較小噪聲，因此乘（1－p）＝0.8；

c＿Avg＝c＿Avg＋ Delta＿Avg；經(jīng)平滑后 Delta＿Avg可以認(rèn)為含有較小噪聲，而真正含有航向角變化需累加到角度中。

濾波流程圖如圖5所示，經(jīng)Matlab仿真輸出如圖6和圖7所示。

圖5 濾波流程圖

圖6 航向仿真圖

圖7 航向差（噪聲）仿真

橫坐標(biāo)為時(shí)間，單位（s）。波動(dòng)較大線條為航行數(shù)據(jù)，波動(dòng)較小線條是經(jīng)濾波后的航行數(shù)據(jù)［8～9］。由圖可以看出，真實(shí)航行紀(jì)錄線條很粗，每秒航向差有可能超出10°以上，對(duì)程序來(lái)說(shuō)，可能造成這一秒判斷打左舵，下一秒判斷打右舵，嚴(yán)重影響航向控制參數(shù)。數(shù)據(jù)經(jīng)過(guò)濾波器后，航向差縮減到2°以內(nèi)，數(shù)據(jù)平滑，已能符合程序要求。

5 結(jié)語(yǔ)

GPS的高精度位置信息和運(yùn)動(dòng)要素為有效地控制船舶的運(yùn)動(dòng)，使船舶航行在計(jì)劃航線上提供了可能［11］。

本文僅涉及到船舶自動(dòng)航向控制和自動(dòng)舵的控制實(shí)現(xiàn)，該系統(tǒng)在實(shí)際使用中還涉及到很多相關(guān)的領(lǐng)域，如電子海圖等，這些相關(guān)領(lǐng)域還有待今后進(jìn)一步研究。

［1］鄭道昌.船舶自動(dòng)航行系統(tǒng)的研究［J］.中國(guó)航海，2002（2）：18－21.

［2］王俊賢.船舶在局部區(qū)域內(nèi)自動(dòng)導(dǎo)航路徑識(shí)別系統(tǒng)［J］.西安電子科技大學(xué)學(xué)報(bào)，1998（5）：560－564.

［3］陳姚杰，紀(jì)業(yè)新，陳小歡.GPS在智能電動(dòng)車自動(dòng)控制系統(tǒng)中應(yīng)用［J］.中國(guó)科技信息，2010，19（56）：126－127.

［4］NMEA，0183協(xié)議.

［5］朱示立.電子海圖應(yīng)用程序設(shè)計(jì)［M］.國(guó)防工業(yè)出版社，1997：80－81.

［6］任茂東.船舶仿人智能自動(dòng)避碰控制系統(tǒng)［J］.中國(guó)航海，1994（1）：1－7.

［7］戴忠達(dá).自動(dòng)控制理論基礎(chǔ)［M］.清華大學(xué)出版社，1989：522－530.

［8］清源計(jì)算機(jī)工作室.MATLAB基礎(chǔ)及其應(yīng)用［M］.北京：機(jī)械工業(yè)出版社，2000：72－81.

［9］樓順天，于衛(wèi).基于MATLAB的系統(tǒng)分析與設(shè)計(jì)［M］.西安電子科技大學(xué)出版社，1998：90－102.

［10］陳嬌，陳瑋，陸曉野，等.基于GPSOne和航位推算的定位算法研究［J］.計(jì)算機(jī)與數(shù)字工程，2012（1）.

［11］李洪濤，許國(guó)昌，薛鴻印.GPS應(yīng)用程序設(shè)計(jì)［M］.科學(xué)出版社，1999：19－20.