人工勢場法在航道測量中的應(yīng)用

史艷霞，賈貴璽，董宏超

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

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

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

1.1 改進(jìn)人工勢場法

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

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

斥力在X，Y軸的分量為

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

合力在X，Y軸的分量為

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

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

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

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

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

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

1.3 航跡保持

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2 硬件系統(tǒng)

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

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

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

2.2 裝置選型

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

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

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

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

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

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

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

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

3 軟件系統(tǒng)

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

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

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

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

系統(tǒng)中S＝114，當(dāng)?shù)貐^(qū)不同時，需提前輸入經(jī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é)果表明：軟件程序計算得到的二維坐標(biāo)與商業(yè)軟件得到的坐標(biāo)值匹配精確.因此，后續(xù)應(yīng)用此程序時，需要將GPS系統(tǒng)測得的坐標(biāo)先轉(zhuǎn)化為二維平面坐標(biāo).

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

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

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

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

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

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

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

5 結(jié)束語

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

［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ī)械工程與自動化，2009（156）：194－197.

［3］朱大奇，顏明重.移動機(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］劉春陽，程億強(qiáng)，柳長安.基于改進(jìn)勢場法的移動機(jī)器人避障路徑規(guī)劃［J］.東南大學(xué)學(xué)報：自然科學(xué)版，2009，39（增刊1）：116－120.

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

［8］胡小平，謝珂，左富勇.基于改進(jìn)人工勢場法的機(jī)械手避障規(guī)劃［J］.測控技術(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ā)展評述［J］.華僑大學(xué)學(xué)報：自然科學(xué)版，2004，25（1）：1－4.

［12］詹月林.船舶航跡間接多模態(tài)控制方法研究［J］.工業(yè)儀表與自動化裝置，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］.長沙：國防科技大學(xué)出版社，1998：25－40.

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