史艷霞，賈貴璽，董宏超

（1.天津中德職業(yè)技術(shù)學(xué)院 電氣與能源學(xué)院，天津300350；2.天津大學(xué) 智能電網(wǎng)實(shí)驗(yàn)室，天津300072）

航道是重要的水運(yùn)基礎(chǔ)設(shè)施，為滿足水運(yùn)工程規(guī)劃、設(shè)計(jì)和船舶安全航行的需要，必須定期對(duì)現(xiàn)行航道進(jìn)行常規(guī)性的維護(hù)測(cè)量.航道測(cè)量的傳統(tǒng)方法精度低、實(shí)時(shí)性差，不能滿足航道測(cè)量高精度定位的要求.近年來(lái)，基于全球定位系統(tǒng)（GPS）技術(shù)的自動(dòng)導(dǎo)航船不斷發(fā)展，并逐漸應(yīng)用于水深測(cè)繪工作中，很好地滿足了內(nèi)河航道的測(cè)量要求［1］.國(guó)內(nèi)外學(xué)者提出了許多理論和方法，其核心是對(duì)測(cè)量船進(jìn)行控制，從而快速、無(wú)障礙、自主地找到既定航向.目前，已知的導(dǎo)航路徑規(guī)劃算法主要有可視圖法、柵格法、自由空間法等［2－4］，但這些算法存在計(jì)算時(shí)間長(zhǎng)，精度不高，對(duì)環(huán)境變化反應(yīng)不靈敏，難于應(yīng)用于實(shí)際等缺點(diǎn).人工勢(shì)場(chǎng)法（APF）是Khatib于1986年提出的，最早應(yīng)用于移動(dòng)機(jī)器人的路徑規(guī)劃中［1］.本文提出一種將人工勢(shì)場(chǎng)法與比例－積分－微分（PID）控制相結(jié)合的新型導(dǎo)航算法.

1 控制系統(tǒng)實(shí)現(xiàn)

1.1 改進(jìn)人工勢(shì)場(chǎng)法

移動(dòng)物體（如測(cè)量船）在一個(gè)力場(chǎng)中運(yùn)動(dòng)時(shí)，存在引力極和斥力極.需要到達(dá)的目標(biāo)位置稱為引力極，運(yùn)動(dòng)路徑中存在的障礙物稱為斥力極［5－7］.求解斥力極與引力極間的人工勢(shì)能可得到運(yùn)動(dòng)路徑.在運(yùn)動(dòng)空間中，通過(guò)人工勢(shì)場(chǎng)法可以求解勢(shì)場(chǎng).引力場(chǎng)隨運(yùn)動(dòng)物體與引力極間距離的增大而增大，其方向從運(yùn)動(dòng)物體指向引力極.斥力場(chǎng)的極大值出現(xiàn)于移動(dòng)物體到達(dá)斥力極位置，斥力場(chǎng)的大小隨運(yùn)動(dòng)物體與斥力極間距離的增大而減小，其方向遠(yuǎn)離斥力極.總體勢(shì)場(chǎng)為引力場(chǎng)與斥力場(chǎng)的矢量和.

當(dāng)測(cè)量船、障礙物、目標(biāo)點(diǎn)在同一條直線上時(shí)，這種方法避障效果非常不理想，應(yīng)對(duì)人工勢(shì)場(chǎng)函數(shù)進(jìn)行改進(jìn).當(dāng)測(cè)量船、障礙物和目標(biāo)點(diǎn)在同一條直線上，且障礙物位于測(cè)量船和目標(biāo)點(diǎn)之間時(shí)，將斥力Frep與引力Fatt方向定義為與測(cè)量船相切，以保證兩力的合力方向不經(jīng)過(guò)障礙點(diǎn)，從而使測(cè)量船能夠躲避障礙物.設(shè)測(cè)量船的初始位置為（x（1），y（1）），下一步達(dá)到的位置為（x，y）.設(shè)測(cè)量船與目標(biāo)的距離為L(zhǎng)s，與X軸的夾角為θc，k為正比例增益［8］.則引力在X，Y軸的分量為

斥力在X，Y軸的分量為

式（2）中：Frep，X為斥力的一個(gè)分量，方向從障礙物指向測(cè)量船；Frep，Y為斥力的另一個(gè)分量，方向從障礙物指向目標(biāo)；θr為障礙物與測(cè)量船連線與X軸的夾角.

合力在X，Y軸的分量為

設(shè)θ為合力與X軸的夾角；v為測(cè)量船運(yùn)行速度.則測(cè)量船位置為

1.2 航向?qū)ふ?/h3>

測(cè)量船尋找航向時(shí)，一般行駛到河道中央，沿所測(cè)放樣線的垂直平分線行駛，待尋找到放樣線的中點(diǎn)，調(diào)轉(zhuǎn)舵角，沿放樣線行駛找到一端端點(diǎn).此時(shí)，可認(rèn)為完成航向找尋.上述方法中，需保證放樣線與河道垂直，但實(shí)際河道多呈彎曲狀，放樣線的設(shè)定不能保證其垂直型.鑒于放樣線的起始點(diǎn)已經(jīng)預(yù)先設(shè)定，采用人工勢(shì)場(chǎng)算法直接尋找放樣線的起點(diǎn)，從而簡(jiǎn)化過(guò)程，節(jié)約時(shí)間.算法示意圖，如圖1所示.

測(cè)量船自動(dòng)尋找航向避障仿真圖，如圖2所示.圖2中：放樣線共有3條，分別表示為A，B，C；測(cè)量船的運(yùn)動(dòng)軌跡用直線表示；障礙物用圓點(diǎn)來(lái)表示.由圖2可知：測(cè)量船在人工勢(shì)場(chǎng)算法作用的情況下，完全實(shí)現(xiàn)自主尋找航向，可避開障礙物的干擾，向目標(biāo)點(diǎn)行進(jìn).

圖1 人工勢(shì)場(chǎng)航向?qū)ふ宜惴ㄊ疽鈭DFig.1 Diagram of course finding algorithm of APF

圖2 測(cè)量船自動(dòng)尋找航向避障仿真圖Fig.2 Simulation diagram of the automatic course finding and obstacle－avoiding of the ship

1.3 航跡保持

由圖2可知：盡管測(cè)量船避開了障礙物，但它偏離了放樣線行駛.這種情況是由于傳統(tǒng)算法只對(duì)下個(gè)目標(biāo)點(diǎn)位置進(jìn)行識(shí)別，而沒有綜合考慮放樣線軌跡造成的.通過(guò)在人工勢(shì)場(chǎng)函數(shù)中加入中間目標(biāo)點(diǎn)，對(duì)傳統(tǒng)算法進(jìn)行改進(jìn)［9］.將檢測(cè)到的障礙物作為圓心坐標(biāo)，圓的半徑定義為一定的影響范圍，圓的方程與放樣線的方程相交，交點(diǎn)為下一次需要尋找的目標(biāo)點(diǎn).通過(guò)上述改進(jìn)，測(cè)量船成功躲開障礙物后，需要先找定義的目標(biāo)點(diǎn)，然后，返回到放樣線上，重新尋找放樣線端點(diǎn).通過(guò)這個(gè)過(guò)程，即可在最小偏差范圍內(nèi)保持既定的航跡.測(cè)量船跟蹤直線航跡保持示意圖，如圖3所示.通過(guò)仿真驗(yàn)證改進(jìn)后方法的有效性，如圖4所示.通過(guò)應(yīng)用改進(jìn)的算法，測(cè)量船既可以成功避開障礙物，又可以沿著既定的航線行駛，比傳統(tǒng)的算法精度更高.

上述方法針對(duì)測(cè)量線是直線的軌跡，人工勢(shì)場(chǎng)法平滑且安全，并跟隨既定路徑.如果所測(cè)航線是彎曲形狀，人工勢(shì)場(chǎng)函數(shù)下測(cè)量船將按照最短路徑原則走直線找到目標(biāo)點(diǎn)，所走路徑將是一條折線，與既定曲線路徑相差很大，如圖5所示.為改善這種不足，將人工勢(shì)場(chǎng)法與PID 控制［10－11］相結(jié)合，設(shè)計(jì)了新的導(dǎo)航算法，如圖6所示.

圖3 測(cè)量船跟蹤直線航跡保持示意圖Fig.3 Diagram of the ship in straight line track

圖4 測(cè)量船跟蹤直線航跡仿真圖Fig.4 Simulation diagram of the ship in straight line track

圖5 人工勢(shì)場(chǎng)導(dǎo)航算法跟蹤曲線航跡仿真圖Fig.5 Diagram of the ship with APF in curve track

圖6 人工勢(shì)場(chǎng)－PID 導(dǎo)航算法示意圖Fig.6 Diagram of the navigational algorithm in APF－PID

將目標(biāo)點(diǎn)橫坐標(biāo)X設(shè)為當(dāng)前測(cè)量船位置A（x1，y1）的橫坐標(biāo)，將X代入航跡段方程Y＝f（X），可得目標(biāo)點(diǎn)的坐標(biāo)C（X1，Y1）.兩點(diǎn)間的距離與航向之間的差值為

式（5）中：為預(yù)定航向角度；為當(dāng)前航向度；為航向偏差.

將測(cè)量船下一個(gè)到達(dá)的位置定義為B（x2，y2），可得此時(shí)的目標(biāo)點(diǎn)D（X2，Y2）為

初始值均設(shè)定Δφ0，ΔL0為0，有

下一步船舶預(yù)定到達(dá)的位置坐標(biāo)為

改進(jìn)的算法將人工勢(shì)場(chǎng)法與PID 控制算法相結(jié)合，可以平滑跟蹤設(shè)定的曲線航向，也可以實(shí)現(xiàn)對(duì)障礙物的自動(dòng)躲避，比其他智能算法精度更高.仿真比較示意圖，如圖7所示.

圖7 人工勢(shì)場(chǎng)－PID 導(dǎo)航算法跟蹤曲線航跡仿真圖Fig.7 Simulation diagram of the ship with APF－PID in curve track

1.4 測(cè)量船控制系統(tǒng)結(jié)構(gòu)

測(cè)量船控制系統(tǒng)的控制方法為間接控制［12］，通過(guò)加入如檢測(cè)舵機(jī)的旋轉(zhuǎn)角度與接收到指令間誤差是否為零的反饋控制裝置，保證實(shí)際值與接收指令的一致性.首先，通過(guò)新的導(dǎo)航算法求得預(yù)定的航向作為測(cè)量船將要行駛的目標(biāo)航向.然后，由陀螺儀測(cè)得此時(shí)測(cè)量船的船艏，即船頭的實(shí)際方向φr.最后，將目標(biāo)航向與實(shí)際航向之間的偏差值Δφ輸入航向控制器，并設(shè)計(jì)控制算法，控制舵機(jī)達(dá)到預(yù)定航向需要旋轉(zhuǎn)的角度δ及旋轉(zhuǎn)方向，以減小偏差值.控制方程為

式（10）中：k為控制系數(shù)，一般根據(jù)測(cè)量船掉頭時(shí)的轉(zhuǎn)彎半徑（1.5m）、船體長(zhǎng)度（1.2m）、舵機(jī)的最大偏舵角度（50°）選取，文中k＝（50／180）×（1.2／1.5）＝0.222.

2 硬件系統(tǒng)

2.1 系統(tǒng)整體組成

測(cè)量船的主控系統(tǒng)包括工控機(jī)、GPS、測(cè)繪模塊（主要包括水深儀、姿態(tài)儀等）和數(shù)據(jù)采集模塊等.通過(guò)船載工控機(jī)處理采集的數(shù)據(jù)，以實(shí)現(xiàn)測(cè)量船的定位及導(dǎo)航.工控機(jī)經(jīng)由RS232串口將控制指令傳遞給執(zhí)行模塊，實(shí)現(xiàn)對(duì)測(cè)量船轉(zhuǎn)向以及船速的控制，而且在緊急情況下能發(fā)送報(bào)警信號(hào).

無(wú)人測(cè)量船體積較小，行駛過(guò)程中容易受到環(huán)境因素的影響.因此，設(shè)計(jì)時(shí)需要考慮精度的定義問題.如果精度定義太低，無(wú)法實(shí)現(xiàn)測(cè)量船沿著期望的放樣線軌跡運(yùn)行；如果精度太高，舵機(jī)的動(dòng)作則會(huì)受到輕微擾動(dòng)的影響，導(dǎo)致設(shè)備損耗增加.因此，需要綜合考慮二者之間的平衡關(guān)系.自動(dòng)舵機(jī)控制方向是采用單獨(dú)電源供電，通過(guò)傳統(tǒng)的汽油機(jī)模式調(diào)節(jié)舵機(jī)槳葉的轉(zhuǎn)動(dòng)方向和大小，以實(shí)現(xiàn)對(duì)船體轉(zhuǎn)動(dòng)方向的控制.方向舵的動(dòng)力來(lái)源為蓄電池，自動(dòng)舵機(jī)的主題為電動(dòng)舵機(jī).此外，系統(tǒng)還包括控制器、功率放大電路、紅外脈沖反饋裝置，以及各種傳感器.

2.2 裝置選型

采用3.5馬力，最大功率為2.6kW，最大轉(zhuǎn)速為4 200～5 300r·m－1的汽油機(jī)作為動(dòng)力源.電動(dòng)舵機(jī)的選取受動(dòng)力裝置的影響，可以將鏈條作為船動(dòng)力裝置中方向舵的滑動(dòng)連接裝置.鏈條轉(zhuǎn)動(dòng)帶動(dòng)方向舵機(jī)轉(zhuǎn)動(dòng)，在鏈條的平行移動(dòng)轉(zhuǎn)化為舵機(jī)轉(zhuǎn)動(dòng)的過(guò)程中，可有效控制測(cè)量船的轉(zhuǎn)動(dòng)方向.

伺服機(jī)工作的核心部分為無(wú)核心馬，用細(xì)銅線纏繞于電機(jī)的外側(cè)，中心裝入磁塊，可完成轉(zhuǎn)速快、能耗小的電極構(gòu)建.伺服機(jī)的具體工作流程有以下4個(gè)步驟：1）上級(jí)控制器發(fā)送指令與伺服機(jī)；2）測(cè)量船轉(zhuǎn)動(dòng)方向的命令通過(guò)電路板進(jìn)行檢測(cè)，得到控制器發(fā)出的指令；3）馬達(dá)由功率放大器驅(qū)動(dòng)，馬達(dá)的輸出力矩傳至擺臂；4）傳感器實(shí)時(shí)檢測(cè)實(shí)際的轉(zhuǎn)動(dòng)角度，判斷測(cè)量船是否按照指令達(dá)到指定的位置.

伺服電機(jī)選擇直流無(wú)刷電機(jī)，采用直流6V 電源對(duì)伺服電機(jī)進(jìn)行供電，產(chǎn)生100～200Hz的脈寬調(diào)制（PWM）信號(hào)，并通過(guò)調(diào)整信號(hào)的占空比，實(shí)現(xiàn)對(duì)舵機(jī)轉(zhuǎn)動(dòng)角度的調(diào)整.直流無(wú)刷電動(dòng)機(jī)動(dòng)態(tài)模型［13］，如圖8所示.

圖8 直流無(wú)刷電機(jī)動(dòng)態(tài)模型Fig.8 Dynamic model of brushless DC motor

由動(dòng)態(tài)模型可得伺服電機(jī)傳遞函數(shù)為

伺服電機(jī)輸出電流與PWM 輸出信號(hào)的關(guān)系為

伺服電機(jī)轉(zhuǎn)速與輸出電流的關(guān)系為

系統(tǒng)中的舵機(jī)轉(zhuǎn)速、角度反饋檢測(cè)裝置為紅外脈沖計(jì)數(shù)器.

3 軟件系統(tǒng)

3.1 GPS定位實(shí)現(xiàn)

采用GPS定位技術(shù)［14］獲取測(cè)量船所在位置.在上位機(jī)界面顯示測(cè)量船位置時(shí)，需要其二維平面坐標(biāo)（x，y），而GPS測(cè)量裝置得到的坐標(biāo)為WGS－84的三維經(jīng)緯度坐標(biāo)（B，L，H），所以需要先對(duì)經(jīng)緯度坐標(biāo)進(jìn)行分離，將其轉(zhuǎn)化為能夠直接用于上位機(jī)顯示的施工平面測(cè)量坐標(biāo).轉(zhuǎn)換模型為

式（14）中：（x，y）為平面直角坐標(biāo)系的橫、縱坐標(biāo)；（L，B）為GPS返回的經(jīng)、緯度坐標(biāo)；S為從赤道到緯度坐標(biāo)B的經(jīng)線弧長(zhǎng)；N為卯酉圈曲率半徑；η2＝e′2cos2B，e′為地球的第二偏心率.

得到轉(zhuǎn)換模型后，可根據(jù)此模型編寫響應(yīng)的軟件程序，并與商業(yè)軟件的結(jié)果進(jìn)行比對(duì)，設(shè)計(jì)的程序滿足精度要求.

系統(tǒng)中S＝114，當(dāng)?shù)貐^(qū)不同時(shí)，需提前輸入經(jīng)線弧長(zhǎng).將緯度轉(zhuǎn)換成弧度，表示為3 634.785 6，轉(zhuǎn)換成度分秒的格式，表示為36：34：47.136.同理，將經(jīng)度表示成弧度，形式為11 356.348 9，轉(zhuǎn)化成度分秒的格式，表示為113：56：20.934.仿真結(jié)果表明：軟件程序計(jì)算得到的二維坐標(biāo)與商業(yè)軟件得到的坐標(biāo)值匹配精確.因此，后續(xù)應(yīng)用此程序時(shí)，需要將GPS系統(tǒng)測(cè)得的坐標(biāo)先轉(zhuǎn)化為二維平面坐標(biāo).

3.2 上位機(jī)實(shí)現(xiàn)

通過(guò)上位機(jī)設(shè)計(jì)圖形用戶界面，實(shí)現(xiàn)用戶按照需求選擇操作功能.上位機(jī)界面的設(shè)計(jì)流程，如圖9所示.調(diào)用VC 6.0程序的可以拆分窗口類CSplitterend對(duì)以上功能進(jìn)行開發(fā)［15］.

圖9 操作界面設(shè)計(jì)框圖Fig.9 Design diagram of operation interface

4 試驗(yàn)結(jié)果

通過(guò)試驗(yàn)水域中的放樣點(diǎn)經(jīng)緯度坐標(biāo)確定放樣點(diǎn)的數(shù)目，測(cè)量人員可預(yù)先設(shè)定測(cè)量船的運(yùn)行軌跡.將測(cè)量航線的起點(diǎn)和終點(diǎn)數(shù)據(jù)實(shí)時(shí)存儲(chǔ)到數(shù)據(jù)庫(kù)系統(tǒng)中，并通過(guò)電臺(tái)傳輸由自動(dòng)導(dǎo)航算法得到的測(cè)量船的目標(biāo)位置.

最終試驗(yàn)結(jié)果，如圖10所示.圖10中：直線為預(yù)設(shè)的放樣線；曲線為船體行駛軌跡.由圖10可知：在無(wú)人操作的自動(dòng)狀態(tài)下，測(cè)量船可以自動(dòng)尋找到既定航道的起點(diǎn)位置，并沿預(yù)定航向及航道進(jìn)行移動(dòng)；通過(guò)實(shí)時(shí)上傳到上位機(jī)數(shù)據(jù)，控制算法在每次測(cè)量結(jié)束后，自動(dòng)尋找下一步運(yùn)行的方向；此過(guò)程完全在無(wú)人操作的狀態(tài)下自動(dòng)完成.設(shè)計(jì)的航道間的間隔為60m，每次測(cè)量的航道長(zhǎng)度為100m.試驗(yàn)結(jié)果表明：測(cè)量船在15min內(nèi)可完成全部航道的測(cè)量.通過(guò)系統(tǒng)中友好的上位機(jī)人機(jī)接口界面，操作人員只需要通過(guò)筆記本電腦實(shí)時(shí)監(jiān)控上位機(jī)界面中顯示的測(cè)量船運(yùn)行方向及位置，提高了系統(tǒng)的可用性.

圖10 測(cè)量試驗(yàn)上位機(jī)界面Fig.10 Interface of upper computer in testing

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

將人工場(chǎng)勢(shì)法與PID 控制算法相結(jié)合，提出一種新型的導(dǎo)航算法，實(shí)現(xiàn)測(cè)量船運(yùn)行航道的自動(dòng)調(diào)整.將此新型導(dǎo)航算法進(jìn)行試驗(yàn)，證明了算法的有效性.研究表明：此算法滿足高精度、完全自主的要求，可廣泛地應(yīng)用于內(nèi)陸航道及水庫(kù)的測(cè)量工程中.

［1］WEI Jiang，F(xiàn)ANG Rui－xian，KHAN J.Performance prediction and dynamic simulation of electric ship hybrid power system［C］∥Proceedings of the IEEE Electric Ship Technologies Sympoisium.［s.l.］：IEEE Press，2007：490－497.

［2］李愛萍，李元宗.機(jī)器人路徑規(guī)劃方法的研究［J］.機(jī)械工程與自動(dòng)化，2009（156）：194－197.

［3］朱大奇，顏明重.移動(dòng)機(jī)器人路徑規(guī)劃技術(shù)綜述［J］.控制與決策，2010，25（7）：961－967.

［4］PARK M G，JEON J H，LEE M C.Obstacle avoidance for mobile robots using artificial potential field approach with simulated annealing［J］.Symposium on Industrial Electronics，2001，3（6）：1530－1535.

［5］KHATID O.Real－time obstacle avoidance for manipulators and mobile robots［J］.International Journal of Robotics Research，1986，5（1）：90－98.

［6］劉春陽(yáng)，程億強(qiáng)，柳長(zhǎng)安.基于改進(jìn)勢(shì)場(chǎng)法的移動(dòng)機(jī)器人避障路徑規(guī)劃［J］.東南大學(xué)學(xué)報(bào)：自然科學(xué)版，2009，39（增刊1）：116－120.

［7］張建英，劉暾.基于人工勢(shì)場(chǎng)法的移動(dòng)機(jī)器人最優(yōu)路徑規(guī)劃［J］.航空學(xué)報(bào)，2007（增刊1）：183－188.

［8］胡小平，謝珂，左富勇.基于改進(jìn)人工勢(shì)場(chǎng)法的機(jī)械手避障規(guī)劃［J］.測(cè)控技術(shù)，2012，31（10）：109－116.

［9］于紅斌.足球機(jī)器人系統(tǒng)的決策推理研究［D］.西安：西北工業(yè)大學(xué)，2005：87－91.

［10］陶永華.新型PID 控制及其應(yīng)用［M］.北京：機(jī)械工業(yè)出版社，1998：57－62.

［11］王永初.智能控制理論與系統(tǒng)的發(fā)展評(píng)述［J］.華僑大學(xué)學(xué)報(bào)：自然科學(xué)版，2004，25（1）：1－4.

［12］詹月林.船舶航跡間接多模態(tài)控制方法研究［J］.工業(yè)儀表與自動(dòng)化裝置，2003（6）：31－34.

［13］OOSHIMA M，TAKEUCHI C.Magnetic suspension performance of a bearingless brushless DC motor for small liquid pumps［J］.Transactions on Industry Applications，2011，47（1）：72－78.

［14］董緒榮，張守信，華仲春.GPS／INS組合導(dǎo)航定位及其應(yīng)用［M］.長(zhǎng)沙：國(guó)防科技大學(xué)出版社，1998：25－40.

［15］劉光.地理信息系統(tǒng)二次開發(fā)教程：組件篇［M］.北京：清華大學(xué)出版社，2002：18－35.