基于A*算法的丘陵山區田間路網路徑規劃

王銘楓 李云伍 徐俊杰

摘要：丘陵山區田間道路起伏不平，路網結構復雜，針對農業機械在丘陵山區田間道路上自動行駛的路徑規劃問題，提出一種基于轉向約束和能耗最優的A*路徑規劃算法。在采用載波相位差分（real-time kinematic，簡稱RTK）全球導航衛星系統（global navigation satellite system，簡稱GNSS）精確采集及存儲田間道路路徑信息的基礎上，針對道路坡度起伏大的問題，建立起伏道路的能耗計算函數，對A*算法估價函數進行重新設計;同時，針對部分路口轉向困難的問題，引入路口轉向曲率進行約束判斷。實地仿真結果表明，該算法能夠規劃出一條滿足農業機械幾何轉向約束的能耗最優全局路徑，驗證了改進A*算法的有效性和實用性。

關鍵詞：丘陵山區;田間路網;能耗最優;A*算法;載波相位差分全球導航衛星系統（RTK-GNSS）

中圖分類號： S126 ?文獻標志碼： A ?文章編號：1002-1302（2019）07-0232-05

路徑規劃即根據全局約束目標如距離最短、時間最少、能耗最低等找到一條最優路徑，是實現農業機械自動化作業的關鍵技術之一[1-2]。農業機械或其他車輛在田間道路上自動行駛時，首先應在起點與目的地之間規劃出一條合理的行駛路徑。丘陵山區田間道路起伏不平、狹窄彎曲，路網結構多變，在其上自動行駛的路徑規劃應充分考慮丘陵山區田間道路的這些典型特征。

在農業應用領域，路徑規劃研究多應用于田地內作物行局部路徑生成、地頭轉向最優以及全區域路徑規劃等問題[3]。苑嚴偉等為了提高蘋果采摘機器人的采摘效率，將信息素自適應更新的改進蟻群算法應用于采摘機器人的路徑規劃中[4];張文等從生成路徑的平滑設計、碰撞檢測和算法實時性出發，提出一種基于曲線路徑最短的方向A*算法，以解決復雜環境下的溫室機器人路徑規劃問題[5];劉剛等針對農田平整問題，提出以無效作業狀態最少、轉向操作與重復行走最少為條件的全球導航衛星系統（global navigation satellite system，簡稱GNSS）農田平整全局路徑規劃方法，用于生成農田的土地平整路徑[6]。

目前，針對田間道路拓撲網絡的路徑規劃問題的研究較少。丘陵山區田間道路路徑規劃問題不同于上述田地內路徑規劃和傳統的車輛最短路徑規劃問題，兼顧能效與安全成為最主要的約束目標。在按最短路徑規劃得到的路徑中，往往忽略路口轉向限制以及未考慮地形陡升陡降所造成的能效問題。因此，根據田間道路起伏不平以及路口彎曲率大的特征，提出一種基于轉向約束和行駛能耗最低的改進A*算法，設計了啟發式能耗估價函數，將農業機械在實際起伏道路條件下的能耗作為實際代價，同時對路口轉向曲率進行判斷篩選，最終規劃化出一條切實可行的全局路徑。

1 田間路網采集與存儲

1.1 問題描述

隨著國家對丘陵山區坡耕地的治理以及田間道路體系的完善，各地普遍建立了1.2～2.0 m寬的田間便道和3.5～5.5 m 寬的機耕道路[7]。圖1為重慶丘陵山區某果園的田間道路環境衛星圖，地圖網絡中包括田間路口、居民點、倉庫等節點，以及由田塊與田塊、田塊與居民點倉庫之間縱橫交錯的水泥便道所構成的路徑段。這些田間道路網絡為農業機械轉移和運輸的自動化和智能化作業提供了有利條件。但由于這些田間道路沒有建立起路網地圖，要實現農業機械在田間道路上的自主行駛，應首先通過GNSS采集獲取道路坐標信息，建立道路模型，在此基礎上再利用路徑規劃算法，規劃出農業機械在這些道路上行駛的合理路徑。

1.2 地圖信息采集及預處理

丘陵山區田間道路狹窄曲折、陡升陡降，道路基礎設施差，須要實地精確采集道路網絡信息以建立地圖。本研究通過安裝在田間道路搬運車上的載波相位差分（real-time kinematic，RTK）-GNSS系統采集田間道路的坐標信息，即平面坐標系下的東向、北向及天向坐標（x，y，z），采集流程如圖2所示。串口接收RTK-GNSS接收機數據報文，并對GNSS信號錯誤點采用3次B樣條插值擬合，經坐標轉換后對平面坐標進行存儲，同時對路口節點附近采樣點進行去重復處理，得到平滑可靠的GNSS軌跡地圖;最后將獲取的地圖坐標數據以路段為對象，儲存該路段的路徑點坐標并計算路徑長度、路段能耗及路口轉向曲率等數據。

對圖1所示的田間路網實地采集并提取道路網絡結果如圖3所示，該環境中包括居民點8、22，田間倉庫25、26以及其他若干田塊路口。

1.3 路網地圖存儲及實現

在田間道路的路徑規劃中，須要對采集的三維點坐標進行存儲，并提取道路的拓撲網絡結構。因此，針對3類地圖構成元素即路口節點、路段和網絡關系分別進行描述：（1）針對路口節點的存儲，采用一維數組的結構，儲存節點編號、三維坐標以及關聯路徑段數。（2）路段信息與節點存儲結構相同，由多個采樣點一維數組組合為二維數組，儲存路段編號、路段內所有采樣點坐標以及路段起止節點編號。（3）對于地圖中拓撲網絡關系，本研究將考慮起伏道路的能耗，相同道路不同行駛方向能耗往往不一致，為了便于對地圖進行存儲和搜索，采用鄰接矩陣存儲地圖數據[8]。在該結構中，建立鄰接矩陣Q，矩陣Q中行列數為道路節點個數，其中，第u行第v列元素用二元數組[dist（u，v），E（u，v）]表示，若節點u、v相鄰，dist（u，v）、E（u，v）分別設置為對應路徑段的路徑長度及能耗，若節點u、v不相鄰，則將dist（u，v）、E（u，v）設置為無窮。以上信息可通過離線計算并存儲，規劃時直接調用。

2 基于改進A*算法的路徑規劃

2.1 基本A*算法

2.4.2 算法流程 同基本A*算法一致，在算法搜索過程中須要不斷更新Open表和Close表2個列表，Open表為待擴展列表，Close表為已擴展節點列表;u為當前擴展節點，v為與u相連節點，f、g、h含義同式（1）、式（2）。算法輸入為起點和終點坐標，輸出為全局路徑坐標點序列，其基本流程如下：（1）輸入起點和終點坐標，選擇距離最近的節點作為規劃的起點s和終點e;（2）初始化，將所有節點f、g值置為無窮;建立Open表與Close表，并將起點納入Open表并作為當前節點u，更新g（u）=0，然后將節點u移入Close表;（3）根據式（4）、式（5）對節點u的臨近節點轉向曲率進行判斷，篩選曲率符合轉向約束的節點v，計算f（v）、g（v）、h（v），并將其加入Open表中;（4）若v已在Open表中，計算節點v實際代價g（v），若經過節點u的代價小于原值，則更新g（v），并替換u為其父節點，否則不作改變;（5）從Open表中選取具有最小f值的節點作為擴展節點，更新該節點為u，并將其納入Close表中;（6）檢查所選節點u是否為終點e，若是，則表示最優路徑已經找到，轉至步驟（7）;否則返回步驟（3）繼續搜索;若Open表為空，則路徑不存在，結束搜索;（7）從終點開始，沿父節點層層回溯得到最優路徑，按節點順序組合預存GNSS采樣點，輸出完整的全局路徑坐標點序列。

3 算法仿真試驗

3.1 試驗環境及條件

為了驗證提出的A*算法在田間道路網絡中的規劃應用效果，以搬運車在田間路網中自主行駛為目標，在MATLAB環境中進行路徑規劃仿真試驗。選擇如圖3所示的田間道路網絡作為規劃對象，共計36個節點、54個路徑段。在該地圖中，存在較大坡度起伏的路段以及路口彎曲度較大的路口，能夠較好地體現丘陵山地田間道路的特征。同時采用Dijkstra算法進行規劃，僅以路徑長度作為約束條件，忽略道路起伏及轉向約束信息，以比較路徑長度最短時的車輛能耗及道路特征與改進A*算法的差異。同時，為了對比道路起伏情況，定義道路累計高程變化[12]H（n′，n），如式（14）所示，式中各參數意義同式（3）、式（10）。搬運車仿真參數見表1。

3.2 仿真結果與分析

仿真設置多組不同起點和終點的路徑規劃場景，規劃結果如表2所示。

由表2可知，按路徑最短和能耗最少為目標均能生成全局路徑，且在部分規劃場景中規劃結果相同。同時，由于不同道路起伏情況不一致，當路徑長度相近時，其路徑能耗往往差異較大。如規劃路徑1→21與17→23，基于Dijkstra算法的路徑長度相差僅約11 m，但總能耗分別為5.19、20.16 W·h，這是由于1→21為下坡路段，能耗主要由滾動阻力產生，而17→23為起伏路段，重力勢能轉化為摩擦熱較大。

以路口34到路口11規劃為例分析不同算法規劃的差異，圖6-a、圖6-b分別為改進A*與Dijkstra算法路徑規劃結果，圖7為2個算法所規劃道路的海拔高度變化對比。

由表2可知，按照Dijkstra算法規劃的道路，其路徑長度最短為548.09 m，但此時道路起伏較大（圖7），累計高程變化達81.34 m，此時對應能耗為34.16 W·h。同時，道路中存在曲率較大而無法通行的路口，如13-12-11路口12處計算曲率為0.476，大于根據式（5）計算的搬運車最小轉彎半徑對應曲率。

改進A*算法能耗最優路徑長度為556.12 m，略大于最短路徑長度，最優能耗為28.08 W·h，通過圖7可以出，此時道路起伏相對平緩，累計高程變化為57.04 m。從仿真結果中也可看出，并非路徑長度越短的能耗就越低，根據能耗進行最優路徑選擇更為合理，搬運車在此路徑下兼具能效和安全性能。

4 總結

利用RTK-GNSS采集路網信息， 以道路節點、路段和路網關系為對象存儲道路信息，建立起地圖存儲模型，實現丘陵山區田間道路網絡的存儲。對路口節點引入曲率進行判定，提前過濾轉向曲率較大路口，同時建立農機在起伏道路行駛的能耗函數，并設計A*算法能耗估價函數，提出一種基于能耗最優的丘陵山區田間道路路徑規劃方法。結果表明，相對最短路徑尋優，本研究算法規劃路徑更為合理，對丘陵山區田間道路路徑規劃具有一定的參考和實用意義。

參考文獻：

[1]董 勝，袁朝輝，谷 超，等. 基于多學科技術融合的智能農機控制平臺研究綜述[J]. 農業工程學報，2017，33（8）：1-11.

[2]孟志軍，劉 卉，王 華，等. 農田作業機械路徑優化方法[J]. 農業機械學報，2012，43（6）：147-152.

[3]苗玉彬，王明軍. 農業車輛導航系統中路徑規劃策略的研究進展[J]. 農機化研究，2011，33（5）：12-15.

[4]苑嚴偉，張小超，胡小安. 蘋果采摘路徑規劃最優化算法與仿真實現[J]. 農業工程學報，2009，25（4）：141-144.

[5]張 文，劉 勇，張超凡，等. 基于方向A*算法的溫室機器人實時路徑規劃[J]. 農業機械學報，2017，48（7）：22-28.

[6]劉 剛，康 熙，夏友祥，等. 基于GNSS農田平整全局路徑規劃方法與試驗[J]. 農業機械學報，2018，49（5）：27-33.

[7]張仕超，尚 慧，修維寧，等. 農村田間道路工程對局地土地利用景觀格局的影響[J]. 西南大學學報（自然科學版），2010，32（11）：89-97.

[8]Ma B，Feng Y，Jia X，et al. Vehicle routing in urban areas based on the OCW-Dijkstra algorithm[J]. Iet Intelligent Transport Systems，2016，10（7）：495-502.

[9]劉 浩，鮑遠律. A*算法在矢量地圖最優路徑搜索中的應用[J]. 計算機仿真，2008，25（4）：253-257.

[10]孫紅偉，李云伍，王小娟，等. 田間道路無人駕駛搬運車自動循跡行駛控制策略[J]. 江蘇農業科學，2018，46（7）：215-218.

[11]喻德生，程 程. 基于離散曲率的三次均勻B樣條的局部光順算法[J]. 浙江大學學報（理學版），2011，38（5）：511-517.

[12]宋 青，李曉磊，李 猛. 基于OpenStreetMap的城市自行車網建模與多判據路徑規劃[J]. 交通運輸系統工程與信息，2017，17（3）：143-149.

[13]吳偉斌，張 成，洪添勝，等. 基于模糊PID的山地果園運輸機動力穩定系統的設計與試驗[J]. 湖南農業大學學報（自然科學版），2017，43（4）：443-450.

[14]顧 青，豆風鉛，馬 飛. 基于改進A*算法的電動車能耗最優路徑規劃[J]. 農業機械學報，2015，46（12）：316-322.趙俊偉，黃顯雷，尹昌斌. PPP模式下養豬場戶對糞污處理社會化服務的需求分析——以河南省為例[J]. 江蘇農業科學，2019，47（7）：297-302.