人工勢場法在航道測量中的應用

史艷霞，賈貴璽，董宏超

（1.天津中德職業技術學院 電氣與能源學院，天津300350；2.天津大學 智能電網實驗室，天津300072）

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

1 控制系統實現

1.1 改進人工勢場法

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

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

斥力在X，Y軸的分量為

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

合力在X，Y軸的分量為

設θ為合力與X軸的夾角；v為測量船運行速度.則測量船位置為

1.2 航向尋找

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

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

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

上述方法針對測量線是直線的軌跡，人工勢場法平滑且安全，并跟隨既定路徑.如果所測航線是彎曲形狀，人工勢場函數下測量船將按照最短路徑原則走直線找到目標點，所走路徑將是一條折線，與既定曲線路徑相差很大，如圖5所示.為改善這種不足，將人工勢場法與PID 控制［10－11］相結合，設計了新的導航算法，如圖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 人工勢場導航算法跟蹤曲線航跡仿真圖Fig.5 Diagram of the ship with APF in curve track

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

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

式（5）中：為預定航向角度；為當前航向度；為航向偏差.

將測量船下一個到達的位置定義為B（x2，y2），可得此時的目標點D（X2，Y2）為

初始值均設定Δφ0，ΔL0為0，有

下一步船舶預定到達的位置坐標為

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

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

1.4 測量船控制系統結構

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

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

2 硬件系統

2.1 系統整體組成

測量船的主控系統包括工控機、GPS、測繪模塊（主要包括水深儀、姿態儀等）和數據采集模塊等.通過船載工控機處理采集的數據，以實現測量船的定位及導航.工控機經由RS232串口將控制指令傳遞給執行模塊，實現對測量船轉向以及船速的控制，而且在緊急情況下能發送報警信號.

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

2.2 裝置選型

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

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

伺服電機選擇直流無刷電機，采用直流6V 電源對伺服電機進行供電，產生100～200Hz的脈寬調制（PWM）信號，并通過調整信號的占空比，實現對舵機轉動角度的調整.直流無刷電動機動態模型［13］，如圖8所示.

圖8 直流無刷電機動態模型Fig.8 Dynamic model of brushless DC motor

由動態模型可得伺服電機傳遞函數為

伺服電機輸出電流與PWM 輸出信號的關系為

伺服電機轉速與輸出電流的關系為

系統中的舵機轉速、角度反饋檢測裝置為紅外脈沖計數器.

3 軟件系統

3.1 GPS定位實現

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

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

得到轉換模型后，可根據此模型編寫響應的軟件程序，并與商業軟件的結果進行比對，設計的程序滿足精度要求.

系統中S＝114，當地區不同時，需提前輸入經線弧長.將緯度轉換成弧度，表示為3 634.785 6，轉換成度分秒的格式，表示為36：34：47.136.同理，將經度表示成弧度，形式為11 356.348 9，轉化成度分秒的格式，表示為113：56：20.934.仿真結果表明：軟件程序計算得到的二維坐標與商業軟件得到的坐標值匹配精確.因此，后續應用此程序時，需要將GPS系統測得的坐標先轉化為二維平面坐標.

3.2 上位機實現

通過上位機設計圖形用戶界面，實現用戶按照需求選擇操作功能.上位機界面的設計流程，如圖9所示.調用VC 6.0程序的可以拆分窗口類CSplitterend對以上功能進行開發［15］.

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

4 試驗結果

通過試驗水域中的放樣點經緯度坐標確定放樣點的數目，測量人員可預先設定測量船的運行軌跡.將測量航線的起點和終點數據實時存儲到數據庫系統中，并通過電臺傳輸由自動導航算法得到的測量船的目標位置.

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

圖10 測量試驗上位機界面Fig.10 Interface of upper computer in testing

5 結束語

將人工場勢法與PID 控制算法相結合，提出一種新型的導航算法，實現測量船運行航道的自動調整.將此新型導航算法進行試驗，證明了算法的有效性.研究表明：此算法滿足高精度、完全自主的要求，可廣泛地應用于內陸航道及水庫的測量工程中.

［1］WEI Jiang，FANG 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］.機械工程與自動化，2009（156）：194－197.

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

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

［8］胡小平，謝珂，左富勇.基于改進人工勢場法的機械手避障規劃［J］.測控技術，2012，31（10）：109－116.

［9］于紅斌.足球機器人系統的決策推理研究［D］.西安：西北工業大學，2005：87－91.

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

［11］王永初.智能控制理論與系統的發展評述［J］.華僑大學學報：自然科學版，2004，25（1）：1－4.

［12］詹月林.船舶航跡間接多模態控制方法研究［J］.工業儀表與自動化裝置，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組合導航定位及其應用［M］.長沙：國防科技大學出版社，1998：25－40.

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