王銘楓 李云伍 徐俊杰

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

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

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

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

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

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

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

1.1 問(wèn)題描述

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

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

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

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

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

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

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

2.1 基本A*算法

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

3 算法仿真試驗(yàn)

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

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

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

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

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

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

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

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

4 總結(jié)

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

參考文獻(xiàn)：

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

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

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

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

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

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

[7]張仕超，尚 慧，修維寧，等. 農(nóng)村田間道路工程對(duì)局地土地利用景觀格局的影響[J]. 西南大學(xué)學(xué)報(bào)（自然科學(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]劉 浩，鮑遠(yuǎn)律. A*算法在矢量地圖最優(yōu)路徑搜索中的應(yīng)用[J]. 計(jì)算機(jī)仿真，2008，25（4）：253-257.

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

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

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

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

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