基于多項式螺旋線的智能叉車路徑規(guī)劃研究*

申紅嬌,邱繼紅,徐 方

（1.中國科學院沈陽自動化研究所 機器人學國家重點實驗室,遼寧 沈陽 110016;2.中國科學院 機器人與智能制造創(chuàng)新研究院,遼寧 沈陽 110169;3.中國科學院大學,北京 100049;4.沈陽新松機器人自動化股份有限公司,遼寧 沈陽 110168）

0 引 言

通常,碼頭、工廠、倉庫等場合需要叉車完成貨物的裝卸。人工駕駛叉車具有危險性大、易疲勞、用工成本高、難以精確到達目標點的缺點。

隨著移動機器人技術的發(fā)展,智能叉車取代駕駛員逐漸成為可能[1]。路徑規(guī)劃是智能車輛的關鍵技術之一,一直以來都是叉車研究的熱點[2,3]。

王洪斌等人[4]提出了一種A*與動態(tài)窗口算法相結合的移動機器人路徑規(guī)劃算法,該算法考慮了車輛的運動學約束,但是并未考慮目標點的航向角約束。STRUB M P等人[5]研究了基于隨機采樣的路徑規(guī)劃算法,但是該算法的規(guī)劃結果具有隨機性。REEDS J A和SHEPP R A[6]針對泊車問題提出了一種圓弧與直線組合的路徑規(guī)劃算法,但是該算法的路徑曲率不連續(xù)。張家旭等人[7]利用回旋線連接半徑不同的圓弧,從而得到了曲率連續(xù)的平行泊車路徑,但是該算法的可行泊車起點區(qū)域十分有限。魏勇等人[8]研究了粒子群算法在移動機器人上的應用,但該算法并未考慮起終點的航向角和曲率約束。呂恩利等人[9]提出了一種基于三次B樣條曲線優(yōu)化的叉車托盤拾取路徑規(guī)劃算法,通過該算法可得出曲率連續(xù)的路徑;但是當終點相對于起點的橫向距離或航向角偏差較大時,存在難以求解的問題。李紅等人[10]提出了基于四次B樣條曲線的無人駕駛汽車平行泊車路徑規(guī)劃算法,該算法要求車輛的起點航向與目標車位平行,但叉車在實際作業(yè)過程中通常難以滿足該條件。曾德全等人[11]基于三次B樣條曲線設計了一種無人駕駛汽車高實時性連續(xù)避障規(guī)劃算法,規(guī)劃結果滿足起點和終點的位姿約束與車輛的運動學約束;但是該算法僅適用于車道行駛工況。吳冰等人[12]設計了基于徑向基函數(shù)（radial basis function,RBF）神經(jīng)網(wǎng)絡的平行泊車路徑規(guī)劃算法,但該算法需要大量實驗數(shù)據(jù)構建控制模型。陳鑫等人[13]針對自動泊車問題,提出了基于深度強化學習的路徑規(guī)劃算法,但是該算法的規(guī)劃結果嚴重依賴于模型訓練數(shù)據(jù)。霍旭坤等人[14]提出了一種基于轉向角變換的智能叉車運動規(guī)劃算法;但是該算法要求起點的轉向角為零,難以在實際中應用。

基于以上研究,為使叉車能精確到達目標點,筆者提出一種基于多項式螺旋線優(yōu)化的路徑規(guī)劃算法,即在使路徑滿足起點和目標點的狀態(tài)約束與叉車的運動學約束的前提下,構建基于多項式螺旋線的曲率代價最小化目標函數(shù),在MATLAB中選取典型實驗場景對算法進行驗證,并對實驗結果進行分析。

1 叉車作業(yè)路徑規(guī)劃任務

叉車作業(yè)流程圖如圖1所示。

叉車在作業(yè)時,首先會通過叉車定位系統(tǒng)獲取叉車在全局坐標系的當前位姿,通過上層調(diào)度系統(tǒng)獲取叉車的目標位姿,并將其發(fā)送給叉車;然后,考慮叉車當前位姿和目標位姿進行路徑規(guī)劃;最后,通過叉車運動控制算法完成路徑跟蹤,叉車到達目標點后進行托盤拾取。

筆者的研究重點為叉車的路徑規(guī)劃算法。筆者研究的叉車作業(yè)場景如圖2所示。

叉車工作場景中設置有若干貨物緩沖區(qū),貨物的擺放具有任意性。叉車是典型的前輪驅(qū)動、后輪轉向的非完整約束車輛,為滿足叉車的運動學約束,路徑的曲率應連續(xù),且小于叉車最小轉彎半徑的倒數(shù)。為避免叉車原地轉向,其路徑起點的曲率應滿足叉車的前輪轉角約束。為使貨叉順利插入托盤,路徑終點的曲率應為零,并且滿足目標點的位姿約束。

因此,叉車路徑規(guī)劃的任務為:規(guī)劃出一條滿足起點和目標點的位姿和曲率約束的路徑,并使路徑曲率連續(xù),且滿足叉車的運動學約束。

2 基于多項式螺旋線的路徑規(guī)劃算法

2.1 叉車運動學模型

在作業(yè)過程中,由于叉車行駛速度低,車輪與地面之間的相對滑動可以忽略,因此,其車輪運動可視為純滾動。

叉車的運動學模型如圖3所示。

由圖3可知：叉車前軸中心路徑曲率、叉車轉彎半徑、轉向輪轉角的關系如下:

（1）

式中:κ—前軸中心路徑曲率,m-1;r—前軸中心轉彎半徑,m;δ—轉向輪轉角,rad。

2.2 多項式螺旋線

多項式螺旋線的曲率為弧長的多項式函數(shù),則曲線上任意一點的曲率、角度和橫縱坐標與該點弧長的關系如下[15]:

（2）

式中:s—多項式螺旋線上任一點弧長,m;κ—多項式螺旋線上任一點曲率,m-1;θ—多項式螺旋線上任一點角度,rad;x,y—多項式螺旋線上任意一點橫縱坐標,m,m;k0,k1,k2,…—多項式螺旋線的多項式系數(shù),m-1,m-2,m-3,…。

為了方便描述,筆者將多項式螺旋線上任意一點的位姿和曲率[x（s）,y（s）,θ（s）,κ（s）]合稱為該點的狀態(tài)。

多項式螺旋線的對稱和縮放性質(zhì)如圖4所示。

圖4中,l1與l0、l2與l0的參數(shù)變換關系如下:

（3）

式中:sf0—l0的弧長,m;a0,b0,c0,…—l0的多項式系數(shù),m-1,m-2,m-3,…;sf1—l1的弧長,m;a1,b1,c1,…—l1的多項式系數(shù),m-1,m-2,m-3,…;sf2—l2的弧長,m;a2,b2,c2,…—l2的多項式系數(shù),m-1,m-2,m-3,…;p—縮放系數(shù),1。

2.3 目標點變換

由于多項式螺旋線的形狀完全由多項式系數(shù)和弧長決定,因此,基于多項式螺旋線的路徑規(guī)劃算法就是求解多項式系數(shù)和弧長的過程。

利用多項式螺旋線規(guī)劃出一條連接叉車當前點和目標點的路徑L。設L的多項式系數(shù)為[A,B,C,…],弧長為Sf,L上任意一點的狀態(tài)記為[X（S）,Y（S）,Θ（S）,K（S）],其中,S為任意一點的弧長;

利用式（3）,對螺旋線L進行變換,可將叉車到任意目標點的路徑規(guī)劃問題轉化為與第一象限四分之一單位圓上點的連接問題,從而縮小弧長的取值范圍,加快求解速度。

變換原理如下:

設變換后的新螺旋線l的多項式系數(shù)為[a,b,c,…],弧長為sf,l上任一點的狀態(tài)記為[x（s）,y（s）,θ（s）,κ（s）]。求解出l的多項式系數(shù)和弧長,然后進行逆變換,即可得到L的多項式系數(shù)和弧長。

若原目標點位于第一象限,僅需對L進行縮放,l的起點和終點應滿足的狀態(tài)約束如下:

（4）

若原目標點位于第二象限,需進行鏡像和縮放,l的起點和終點應滿足的狀態(tài)約束如下:

（5）

為方便描述,筆者將l的起點和終點的狀態(tài)約束分別記為[0,0,0,κ0]和[xf,yf,θf,κf]。

2.4 多項式螺旋線的約束條件

l應滿足起點和終點的狀態(tài)約束,并使路徑曲率符合叉車的運動學約束。

顯然,l起點處的弧長為0,根據(jù)式（2）,起點的位姿約束自然得到滿足。l終點的狀態(tài)約束構成的非線性方程組難以求解,因此,筆者采用優(yōu)化的方法進行求解。

為了使l滿足起點的曲率約束與終點的橫坐標、縱坐標、航向角和曲率約束這5個約束條件,并能夠?qū)φ麠l路徑的平滑性代價進行優(yōu)化,筆者取l的曲率表達式為四次多項式,即l的多項式系數(shù)為[a,b,c,d,e]。

為使l起點的曲率約束被滿足,令a=κ0。令l始終滿足終點的曲率和航向角約束,并利用b、c、sf表示d、e,可推導出d、e如下:

（6）

因此,只需對b、c、sf3個變量進行優(yōu)化求解,便可求得最優(yōu)路徑。

為滿足作業(yè)需求,叉車到達目標點時的距離誤差應在允許范圍內(nèi),與之對應,l的終點位置應滿足的約束如下:

（7）

式中:Δ—叉車到達目標點時允許的最大距離誤差,m。

為了滿足叉車的運動學約束,L上任意一點的曲率K（S）應小于叉車最小轉彎半徑的倒數(shù),與之對應,l上任意一點應滿足的曲率約束如下:

（8）

式中:rmin—叉車最小轉彎半徑,m。

由于終點位于單位圓上,因此l的弧長sf范圍較為固定。根據(jù)對大量規(guī)劃結果的弧長數(shù)據(jù)分布規(guī)律總結可知,目標點位于第一象限的四分之一單位圓上、目標點航向角范圍為[-π/2,π/2]時,l的弧長范圍與終點航向角的關系如下式所示:

（9）

2.5 目標函數(shù)優(yōu)化

為使l盡量平滑并縮短路徑長度,曲率最小化目標函數(shù)及其約束條件如下:

（10）

3 仿真實驗與結果分析

MATLAB遺傳算法工具箱中的GA函數(shù)用于求解多約束條件模型的優(yōu)化函數(shù)[16]。

建立叉車路徑規(guī)劃的數(shù)學模型后,以b、c、sf為優(yōu)化變量,以式（10）為目標函數(shù)和約束條件,調(diào)用GA函數(shù),即可求得多項式螺旋線的弧長和多項式系數(shù)。

3.1 典型場景曲線參數(shù)求解

此處仿真所采用的叉車型號為杭叉A30。

杭叉A30的基本參數(shù)如表1所示。

表1 杭叉A30基本參數(shù)

根據(jù)表1,可推導出路徑的曲率應小于1.063 m-1。根據(jù)作業(yè)要求,叉車到達終點時與目標點的距離誤差應小于10 cm;考慮到路徑跟蹤誤差,規(guī)定路徑的終點與目標點的距離誤差應小于5 cm。

當目標點與叉車縱向距離約為6.5 m時,便可得到目標點的位姿,因此,在仿真時,筆者令目標點相對于叉車的縱向距離固定為6 m,只考慮目標點相對于叉車當前位姿的橫向距離和航向角。

仿真場景示意圖如圖5所示。

根據(jù)叉車起點曲率、目標點橫向距離和航向角的不同,筆者選取起點曲率為0.1 m-1、0.5 m-1、-0.5 m-1,橫向距離為0.5 m、2 m、4 m,航向角為0 rad、±π/12 rad、±π/6 rad、±π/2 rad等23種典型規(guī)劃場景,采用GA函數(shù)求解各個場景下的螺旋線參數(shù)。

不同典型場景下的參數(shù)求解結果如表2所示。

表2 不同典型場景下的參數(shù)求解結果

3.2 典型場景仿真結果分析

筆者選取表2中的場景1、場景21、場景23這3個典型場景進行仿真。

場景1仿真結果如圖6所示。

場景21仿真結果如圖7所示。

場景23仿真結果如圖8所示。

由圖（6～8）知:

場景1、場景21和場景23這3個場景的路徑終點與目標點的距離誤差分別4.26 cm、3.69 cm、3.24 cm,滿足使用要求;3個場景的路徑曲率和航向角均符合起點和目標點的約束,并且變化連續(xù);3個場景的路徑最大曲率分別為0.198 1 m-1、-0.866 2 m-1、-0.917 1 m-1,最大等效轉向角分別為0.312 2 rad、-0.954 3 rad、-0.980 9 rad,均小于杭叉A30的最小轉彎半徑倒數(shù)和最大轉向角,符合叉車的運動學約束。

對比場景1、場景21的仿真結果可知:在起點狀態(tài)和縱向距離相同時,目標點相對于叉車的橫向距離和角度偏差越大,路徑的曲率越大;

對比場景21和場景23的仿真結果可知:目標點狀態(tài)相同時,起點的曲率越大,路徑越曲折。因此,應盡量減小目標點相對于叉車的橫向距離、航向角和叉車起點處的轉向角,從而降低路徑跟蹤的難度。

4 結束語

為了使智能叉車能精確到達目標點,筆者提出了一種基于多項式螺旋線的路徑規(guī)劃算法,即在使路徑滿足起點和目標點的狀態(tài)約束與叉車的運動學約束的前提下,構建了基于多項式螺旋線的曲率代價最小化目標函數(shù),在MATLAB中選取了23種典型實驗場景對算法進行了驗證,并對實驗結果進行了分析。

研究結果表明:

（1）路徑滿足起點和目標點的位姿和曲率約束,路徑的曲率、等效轉向角變化連續(xù),符合叉車的運動學約束;

（2）算法對于起點的轉向角無特殊要求,對于目標點橫向距離和航向角較大的規(guī)劃場景仍然適用;

（3）對于最小轉彎半徑較大的叉車,需再增大規(guī)劃距離,才能使路徑符合叉車的運動學約束。

目前,筆者提出的算法只考慮了叉車的前進運動。在后續(xù)的研究中,筆者將對叉車的進退式路徑進行規(guī)劃,以便使叉車能夠在更狹窄的工作場景中進行作業(yè)。