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

申紅嬌,邱繼紅,徐 方

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

0 引 言

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

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

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

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

1 叉車作業路徑規劃任務

叉車作業流程圖如圖1所示。

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

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

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

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

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

2.1 叉車運動學模型

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

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

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

（1）

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

2.2 多項式螺旋線

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

（2）

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

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

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

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

（3）

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

2.3 目標點變換

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

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

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

變換原理如下:

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

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

（4）

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

（5）

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

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

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

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

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

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

（6）

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

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

（7）

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

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

（8）

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

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

（9）

2.5 目標函數優化

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

（10）

3 仿真實驗與結果分析

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

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

3.1 典型場景曲線參數求解

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

杭叉A30的基本參數如表1所示。

表1 杭叉A30基本參數

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

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

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

根據叉車起點曲率、目標點橫向距離和航向角的不同,筆者選取起點曲率為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種典型規劃場景,采用GA函數求解各個場景下的螺旋線參數。

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

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

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個場景的路徑曲率和航向角均符合起點和目標點的約束,并且變化連續;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的最小轉彎半徑倒數和最大轉向角,符合叉車的運動學約束。

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

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

4 結束語

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

研究結果表明:

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

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

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

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