陸宇豪1，劉義亭2,3，郁漢琪4，余 偉5

（1.南京工程學(xué)院 研究生院，南京 211167；2.南京工程學(xué)院 自動化學(xué)院，南京 211167；3.東南大學(xué) 信息科學(xué)與工程學(xué)院，南京 210096；4.南京工程學(xué)院 工業(yè)中心，南京 211167；5.華齡和平南京智能科技有限公司，南京 210000）

0 引言

路徑規(guī)劃技術(shù)作為農(nóng)業(yè)機(jī)器人領(lǐng)域的重要技術(shù)，其目的在于尋找源與目標(biāo)之間的最佳路徑，并要求路徑安全無碰撞、算法收斂速度快[1]。目前，路徑規(guī)劃算法種類較多，主要有基于圖搜索的A星（A-star）算法和基于采樣的快速搜索隨機(jī)樹（Rapidly Exploring Random Trees，RRT）算法。

其中，以A星為代表的基于圖搜索的路徑規(guī)劃算法主要是通過設(shè)定合適的啟發(fā)函數(shù)指導(dǎo)柵格點(diǎn)的搜索，從而獲取最優(yōu)路徑[2]。此類算法在小地圖環(huán)境下響應(yīng)極快，且生成路徑最短。但在農(nóng)業(yè)大場景下，A星算法的計算量會迅速上升，搜索效率降低，且實(shí)時性差。

以RRT為代表的基于采樣的路徑規(guī)劃算法主要通過對空間點(diǎn)進(jìn)行隨機(jī)采樣，將初始點(diǎn)作為隨機(jī)樹的根節(jié)點(diǎn)，在約束條件下擴(kuò)展隨機(jī)樹并最終形成一條連通路徑[3]。此類方法無需對地圖進(jìn)行預(yù)處理，適用于大型平面或高維空間的路徑搜索，搜索速度較快，但在沒有適當(dāng)采樣指導(dǎo)的情況下搜索較為盲目，性能不穩(wěn)定且耗時較長，獲取的路徑也并非最優(yōu)[4]。

針對上述A星算法與RRT算法的優(yōu)缺點(diǎn)，Jonathan D. Gammell提出了一種將圖搜索和隨機(jī)采樣思想相結(jié)合的批量通知樹（Batch Informed Trees，BIT）算法[5]。該算法能夠在獲得移動成本最優(yōu)路徑的同時避免大空間環(huán)境下搜索效率低、實(shí)時性差的問題[6,7]，但并未考慮行車的安全問題，且生成的路徑折角較多，并不符合機(jī)器人的實(shí)際運(yùn)動需求。因此，在BIT算法的基礎(chǔ)上增加障礙物膨脹與曲線優(yōu)化，進(jìn)而提升機(jī)器人移動的安全性和穩(wěn)定性。

1 BIT算法原理

1.1 BIT算法基本原理

與傳統(tǒng)的基于圖搜索的A星算法與基于隨機(jī)采樣的RRT算法均不同，BIT算法結(jié)合兩者優(yōu)勢，其首先對自由空間進(jìn)行隨機(jī)采樣，利用采樣點(diǎn)、起點(diǎn)與終點(diǎn)構(gòu)建起隱式隨機(jī)幾何圖（Random Geometric Graph，RGG），隨后通過啟發(fā)式搜索擴(kuò)展搜索樹，在成功找到路徑后，縮小采樣范圍。與RRT相比，其構(gòu)造樹的過程不直接對邊進(jìn)行碰撞檢測，而是只有當(dāng)邊線為隊列最佳邊時，才對其進(jìn)行碰撞檢測，由此減少非必要的運(yùn)算[8]，同時在每次批量采樣前，對搜索樹進(jìn)行修剪，刪去無助于提高路徑成本的節(jié)點(diǎn)與邊線，進(jìn)而極大提升算法的運(yùn)算速度和收斂速度。其主體的偽代碼見表1，其主要包括批量采樣、擴(kuò)展及修剪樹模塊等，詳細(xì)步驟可見參考文獻(xiàn)[5]。

1.2 數(shù)學(xué)約定

為便于算法講解，對Alg1中的相關(guān)參數(shù)進(jìn)行數(shù)學(xué)約定。

Alg1.Line1:SettingUpPlanning()為初始化函數(shù)，該函數(shù)所涉及的數(shù)學(xué)約定，QE為有序連線集合，其序列第一個元素表示代價值最小的邊及其代價值。QV為有序節(jié)點(diǎn)集合，其序列第一個元素表示代價值最小的節(jié)點(diǎn)及其代價值。E為樹模型中的連線集合，E集合中的每個元素用(v,w)表示，且v,w∈V。V為樹模型中的頂點(diǎn)集合。φ為空集；xstart表示起點(diǎn)坐標(biāo)；xgoal表示終點(diǎn)坐標(biāo)；Xsamples為采樣點(diǎn)集合；r為隱式RGG算子。

Alg1.Line4:gT(xgoal)為根據(jù)隨機(jī)樹搜索路徑到達(dá)目標(biāo)點(diǎn)所需的代價。

Alg1.Line5:m為每次采樣點(diǎn)的個數(shù)，表示集合的并操作。

Alg1.Line6:Vold為舊頂點(diǎn)集合。

Alg1.Line7:Vnum表示V集合元素的個數(shù)，Xsamplesnum表示Xsamples集合元素的個數(shù)。

Alg1.Line10:vm、xm表示邊隊列中代價值最小邊對應(yīng)的兩個頂點(diǎn)。

Alg1.Line13-22:g'(v)為v點(diǎn)與起始點(diǎn)的歐拉距離；c'(v,x)為v點(diǎn)與x點(diǎn)之間的歐拉距離；h'(x)為x點(diǎn)與終點(diǎn)的歐拉距離；c(vm,xm)為vm點(diǎn)與xm點(diǎn)之間的實(shí)際距離（包含碰撞檢測）。

1.3 BIT算法程序?qū)崿F(xiàn)

BIT算法偽代碼見表1。

表1 BIT算法偽代碼Table 1 BIT Algorithm pseudo code

首先對算法進(jìn)行初始化操作，將QE、QV、E置空，起始點(diǎn)xstart插入V，目標(biāo)點(diǎn)xgoal加入采樣點(diǎn)集合Xsamples，并設(shè)置r=∞（Alg.1,Line1）。

隨后進(jìn)入循環(huán)，若QV和QE為空，則進(jìn)入修剪樹(Alg.1，Line4)和批量采樣(Alg.1，Line5)步驟。

修剪與批量采樣完成后，將當(dāng)前頂點(diǎn)集合加入Vold中進(jìn)行記錄，并將頂點(diǎn)集V加入QV頂點(diǎn)隊列進(jìn)行排序，隊列最前端元素為代價值最小的頂點(diǎn)及其代價(Alg1.Line6)。

之后，計算r算子(Alg1.Line7)，其可通過式（1）計算獲得：

式（1）中：λ()——一個集合的勒貝格測度。

Xf '——{x∈X│f '(x)≤cbest}。

n——求解空間的維度。

ζn——n維單位球的勒貝格測度。

q——Vnum+Xsamplesnum。

Vnum——V集合元素的個數(shù)。

Xsamplesnum——Xsamples集合元素的個數(shù)。

η——自適應(yīng)調(diào)整參數(shù)，η≥1。

bestVertexValue與bestEdgeValue分 別 返 回QE、QV隊列中的最小代價值及其對應(yīng)的頂點(diǎn)和邊，只要滿足循環(huán)條件，則持續(xù)對頂點(diǎn)隊列中代價值最小的點(diǎn)進(jìn)行擴(kuò)展，隨后提取邊隊列中的最優(yōu)邊(vm,xm)，將其從QE隊列中刪除(Alg1.Line8-11)。

CheckValues(vm,xm)用于檢驗(yàn)獲得的(vm,xm)邊是否滿足gT(vm)+c'(vm,xm)+h'(xm)＜gT(xgoal)、g'(vm)+c(vm,xm)+h'(xm)＜gT(xgoal)、gT(vm)+c(vm,xm)＜gT(xgoal)3個擴(kuò)展條件，若均滿足則檢查點(diǎn)xm是否已在樹中，若樹中已存在點(diǎn)xm，則將E中所有與xm相連的邊(v,xm)去除(Alg1. Line14)；若xm不在樹中，則從采樣點(diǎn)集合Xsamples中刪除點(diǎn)xm，并將點(diǎn)xm加入V、QV中(Alg1.Line16-Line17)。隨后將邊(vm,xm)加入集合E，遍歷QE中以xm為終點(diǎn)的邊，若滿足gT(v)+c'(v,xm)≥gT(xm)，則將(v,xm)從QE中刪除(Alg1.Line18-19)。

若不滿足Alg1.Line12，則表示所有的采樣點(diǎn)都不可能對當(dāng)前解進(jìn)行優(yōu)化，則將QE、QV置空，隨后進(jìn)入Alg1.Line2重新采樣計算(Alg1.21)。

在滿足結(jié)束要求后，返回求解出的搜索樹T(V,E)（Alg1.Line22-23）。

1.4 修剪樹

修剪樹模塊的函數(shù)為Prune(gT(xgoal))。gT(xgoal)為根據(jù)搜索樹路徑到達(dá)目標(biāo)點(diǎn)的實(shí)際代價。該步驟旨在去除搜索樹中對優(yōu)化解無用的點(diǎn)和邊，以提升搜索效率，減少搜索的盲目性，其主要為剔除采樣點(diǎn)集合、樹頂點(diǎn)集合和樹連線頂點(diǎn)集合中從起點(diǎn)經(jīng)過該點(diǎn)到達(dá)目標(biāo)點(diǎn)所需預(yù)估代價大于當(dāng)前最優(yōu)路徑代價的點(diǎn)。

1.5 擴(kuò)展

擴(kuò)展模塊的函數(shù)為expandVertex(v)，表示對v頂點(diǎn)進(jìn)行擴(kuò)展操作。其首先將bestInVertexValue()函數(shù)所獲得的最小代價值點(diǎn)v從隊列QV中取出，后計算采樣點(diǎn)中與點(diǎn)v的歐拉距離小于r的臨近點(diǎn)集合。遍歷臨近點(diǎn)集合，若此臨近點(diǎn)x與點(diǎn)v的連線能夠滿足g'(v)+c'(v,x)+h'(x)＜gT(xgoal)，則將兩點(diǎn)連線所形成的邊插入隊列QE。

1.6 批量采樣

Sample(m,gT(xgoal))為批量采樣功能函數(shù)，其在未找到可行路徑狀態(tài)下在全地圖自由空間進(jìn)行批量采樣，在通過搜索樹成功尋找到路徑后，則縮小采樣范圍，轉(zhuǎn)變?yōu)樵跈E圓區(qū)域的自由空間內(nèi)進(jìn)行采樣。其橢圓采樣空間的計算方法如圖1。

圖1 橢圓采樣空間Fig.1 Elliptical sampling space

長軸為cbest,為當(dāng)前到達(dá)目標(biāo)點(diǎn)的最優(yōu)路徑長度，短軸的計算方法為，cmin為起始點(diǎn)與目標(biāo)點(diǎn)間的歐拉距離，在實(shí)際運(yùn)算中，可先在標(biāo)準(zhǔn)圓中進(jìn)行采樣，并對采樣點(diǎn)進(jìn)行矩陣運(yùn)算，使其平移旋轉(zhuǎn)至橢圓區(qū)域。

此方法能夠在尋找到初始解后有效地提升算法收斂速度。

2 Bezier曲線優(yōu)化

在路徑規(guī)劃過程中，通常會產(chǎn)生一條具有多個銳角或拐角的路線，這種路徑不適合實(shí)際作業(yè)中攜帶大量物品的移動機(jī)器人[9]。因此，為了使搬運(yùn)機(jī)器人能夠更加穩(wěn)定地完成其任務(wù)，需要對生成的路徑進(jìn)行平滑處理。

Bezier曲線是一種通過較少控制點(diǎn)便能生成復(fù)雜平滑路徑的方法，目前被廣泛應(yīng)用于汽車車體工業(yè)設(shè)計、計算機(jī)輔助設(shè)計、路徑規(guī)劃等領(lǐng)域。以二階貝塞爾曲線為例，其生成過程如圖2。

為便于理解，圖2選取5個特殊比例來求解其對應(yīng)的貝塞爾曲線點(diǎn)，在實(shí)際運(yùn)算中，其選取線段長度與對應(yīng)邊長度的比例為t(t∈[0,1])，可以取無窮多個，從而形成整條貝塞爾曲線。

圖2 二階貝塞爾曲線生成過程Fig.2 Generation process of third-order Bessel curve

如圖2所示，Pi(i=0,1,2)為形成貝塞爾曲線所需要的控制點(diǎn)，Li(i=0,1,…,3)為線段P0P1上的點(diǎn)，Ki(i=0,1,…,3)為線段P1P2上的點(diǎn)，Ti(i=0,1,…,3)為貝塞爾曲線上的點(diǎn)，其幾何關(guān)系滿足：

擴(kuò)展到一般情況，其數(shù)學(xué)表達(dá)式如式（2）、式（3）表示：

式中：n——擬合點(diǎn)個數(shù)。

t——線性插值比例，t∈[0,1]。

i——i∈{0,1,…,n}。

由式（2）、式（3）可得，一階Bezier曲線的表達(dá)式為：

二階Bezier曲線為：

三階Bezier曲線為：

式中：Pi——第i個點(diǎn)。

k階Bezier曲線需要k+1個點(diǎn)進(jìn)行求解。在實(shí)際運(yùn)用中，隨著Bezier曲線階數(shù)的增加，其運(yùn)算復(fù)雜度呈幾何倍增長，而三階Bezier曲線能夠保證機(jī)器人運(yùn)行時其速度、加速度均為連續(xù)的同時，又不會嚴(yán)重影響運(yùn)算速度，符合機(jī)器人的運(yùn)動控制要求，因此采用三階Bezier曲線對點(diǎn)進(jìn)行擬合。

將三階Bezier曲線進(jìn)行擬合優(yōu)化,則優(yōu)化后的曲線點(diǎn)為：

式（7）中：xi——第i點(diǎn)的x坐標(biāo)。

yi——第i點(diǎn)的y坐標(biāo)。

x、y表示Bezier曲線的擬合點(diǎn)坐標(biāo)。

三階貝塞爾曲線優(yōu)化與BIT結(jié)合的具體算法步驟見表2。

表2 Bezier曲線優(yōu)化偽代碼Table 2 Bezier curve optimization pseudo code

3 仿真實(shí)驗(yàn)與結(jié)論

在Intel i7-6700HQ筆記本電腦上使用Python 3.9，并在Anaconda3環(huán)境下對原始BIT算法、障礙物膨脹及其Bezier曲線優(yōu)化后的算法進(jìn)行仿真實(shí)驗(yàn)。

本文路徑規(guī)劃的障礙物仿真環(huán)境如圖3，其為幾何地圖，總尺寸為240m×240m，地圖中心為原點(diǎn)坐標(biāo)(0,0)，xstart=(-95,-95)，xgoal=(60,85)，每批次采樣點(diǎn)個數(shù)200個，r算子η系數(shù)為1.1，障礙物膨脹度為5m。本文以圓形機(jī)器人為仿真模型，其半徑為R，使機(jī)器人以1m/s的速度嚴(yán)格沿生成路徑行駛。通常情況下，障礙物的膨脹度應(yīng)略大于機(jī)器人的半徑R，以此保證機(jī)器人在沿路徑行駛的過程中不會發(fā)生碰撞。

其中，圖3左下方為起點(diǎn)xstart,右上方為目標(biāo)點(diǎn)xgoal，地圖中的圓形障礙物以(x,y,r)形式進(jìn)行表示。其中，x、y為圓心坐標(biāo)，r為半徑。

圖3 障礙物分布Fig.3 Obstacle distribution

圖4為BIT未經(jīng)過障礙物膨脹和曲線優(yōu)化，運(yùn)行迭代10000次后生成的路徑，其生成路徑與障礙物距離過近，多個障礙物與路徑相切，因此在實(shí)際環(huán)境中無法滿足機(jī)器人安全行駛的需求。

圖4 原算法路徑圖Fig.4 Original algorithm path diagram

因此，對障礙物進(jìn)行膨脹操作，膨脹后的障礙物地圖如圖5。

圖5 膨脹地圖Fig.5 Expansion map

圖6為對地圖障礙物向外膨脹5m后，算法運(yùn)行迭代10000次后生成的路徑。由圖6可知，其求解出的路徑距離障礙物均大于5m，雖然能夠滿足機(jī)器人運(yùn)行的安全性，但仍存在一定折角。

圖6中的路徑坐標(biāo)見表3。

圖6 膨脹操作后的路徑圖Fig.6 Path map after expansion operation

取表3中每行兩點(diǎn)坐標(biāo)形成連線，不考慮點(diǎn)(60.00，85.00)，計算兩點(diǎn)連線的斜率，并繪制成折線圖如圖7。

表3 路徑點(diǎn)坐標(biāo)Table 3 Path point coordinates

由圖7可知，其波動較大，在機(jī)器人實(shí)際運(yùn)行過程中，會表現(xiàn)為運(yùn)行至折角處速度發(fā)生突變并快速轉(zhuǎn)向，不利于機(jī)器人在真實(shí)搬運(yùn)環(huán)境下的平穩(wěn)運(yùn)行。

圖7 斜率變化折線圖Fig.7 Line chart of slope change

增加障礙物膨脹后的算法迭代過程如圖8，其橫坐標(biāo)為時間，單位：秒（s），縱坐標(biāo)為路徑代價。算法在0.09678s后成功尋找到最初路徑，并快速收斂，在2.037s后尋找到次優(yōu)路徑，與最優(yōu)路徑259.1的代價值相差僅為4，并最終在9.57s時收斂至最優(yōu)。

圖8 增加障礙物膨脹后的迭代過程Fig.8 Iteration process after adding obstacle expansion

圖9為本文在圖6基礎(chǔ)上加入Bezier曲線優(yōu)化后得到的路徑，其路徑代價值為258.88，與原先最優(yōu)路徑相比，其代價值減少了0.22，路徑更加平滑，更適合機(jī)器人的平穩(wěn)運(yùn)行。

圖9 Bezier曲線優(yōu)化后的路徑Fig.9 Optimized path of Bezier curve

仿真實(shí)驗(yàn)結(jié)果表明，經(jīng)過障礙物膨脹和曲線優(yōu)化后的BIT算法在復(fù)雜環(huán)境和大場景范圍下仍能夠快速求解出路徑并迅速收斂，并能在減小路徑的代價值的同時使得機(jī)器人的運(yùn)行更加平穩(wěn)，能夠滿足農(nóng)業(yè)大場景下的路徑規(guī)劃要求。