基于A*算法的丘陵山區(qū)田間路網(wǎng)路徑規(guī)劃

王銘楓 李云伍 徐俊杰

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

關(guān)鍵詞：丘陵山區(qū);田間路網(wǎng);能耗最優(yōu);A*算法;載波相位差分全球?qū)Ш叫l(wèi)星系統(tǒng)（RTK-GNSS）

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

路徑規(guī)劃即根據(jù)全局約束目標(biāo)如距離最短、時間最少、能耗最低等找到一條最優(yōu)路徑，是實現(xiàn)農(nóng)業(yè)機械自動化作業(yè)的關(guān)鍵技術(shù)之一[1-2]。農(nóng)業(yè)機械或其他車輛在田間道路上自動行駛時，首先應(yīng)在起點與目的地之間規(guī)劃出一條合理的行駛路徑。丘陵山區(qū)田間道路起伏不平、狹窄彎曲，路網(wǎng)結(jié)構(gòu)多變，在其上自動行駛的路徑規(guī)劃應(yīng)充分考慮丘陵山區(qū)田間道路的這些典型特征。

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

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

1 田間路網(wǎng)采集與存儲

1.1 問題描述

隨著國家對丘陵山區(qū)坡耕地的治理以及田間道路體系的完善，各地普遍建立了1.2～2.0 m寬的田間便道和3.5～5.5 m 寬的機耕道路[7]。圖1為重慶丘陵山區(qū)某果園的田間道路環(huán)境衛(wèi)星圖，地圖網(wǎng)絡(luò)中包括田間路口、居民點、倉庫等節(jié)點，以及由田塊與田塊、田塊與居民點倉庫之間縱橫交錯的水泥便道所構(gòu)成的路徑段。這些田間道路網(wǎng)絡(luò)為農(nóng)業(yè)機械轉(zhuǎn)移和運輸?shù)淖詣踊椭悄芑鳂I(yè)提供了有利條件。但由于這些田間道路沒有建立起路網(wǎng)地圖，要實現(xiàn)農(nóng)業(yè)機械在田間道路上的自主行駛，應(yīng)首先通過GNSS采集獲取道路坐標(biāo)信息，建立道路模型，在此基礎(chǔ)上再利用路徑規(guī)劃算法，規(guī)劃出農(nóng)業(yè)機械在這些道路上行駛的合理路徑。

1.2 地圖信息采集及預(yù)處理

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

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

1.3 路網(wǎng)地圖存儲及實現(xiàn)

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

2 基于改進A*算法的路徑規(guī)劃

2.1 基本A*算法

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

3 算法仿真試驗

3.1 試驗環(huán)境及條件

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

3.2 仿真結(jié)果與分析

仿真設(shè)置多組不同起點和終點的路徑規(guī)劃場景，規(guī)劃結(jié)果如表2所示。

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

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

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

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

4 總結(jié)

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

參考文獻：

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

[2]孟志軍，劉 卉，王 華，等. 農(nóng)田作業(yè)機械路徑優(yōu)化方法[J]. 農(nóng)業(yè)機械學(xué)報，2012，43（6）：147-152.

[3]苗玉彬，王明軍. 農(nóng)業(yè)車輛導(dǎo)航系統(tǒng)中路徑規(guī)劃策略的研究進展[J]. 農(nóng)機化研究，2011，33（5）：12-15.

[4]苑嚴偉，張小超，胡小安. 蘋果采摘路徑規(guī)劃最優(yōu)化算法與仿真實現(xiàn)[J]. 農(nóng)業(yè)工程學(xué)報，2009，25（4）：141-144.

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

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

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

[8]Ma B，F(xiàn)eng 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*算法在矢量地圖最優(yōu)路徑搜索中的應(yīng)用[J]. 計算機仿真，2008，25（4）：253-257.

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

[11]喻德生，程 程. 基于離散曲率的三次均勻B樣條的局部光順?biāo)惴╗J]. 浙江大學(xué)學(xué)報（理學(xué)版），2011，38（5）：511-517.

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

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

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