基于轉向角變換的智能轉運叉車運動規劃研究*

霍旭坤,馬曉錄*,劉 艷,吳立輝

(1.河南工業大學 機電工程學院,河南 鄭州 450001;2.湖南迎春思博瑞智能裝備有限公司,湖南 寧鄉 410006)

0 引 言

在大中型企業、倉庫、碼頭等場所,常需要將貨物從碼垛或卸貨點轉運,并裝載到大型運輸車輛上。以往采用人工叉車時,用工成本高且工人存在安全隱患。

有企業提出了無人化、智能化轉運裝載需求。在非結構化環境下,無人駕駛運載車輛作業正在活躍地進行[1],貨車停放位姿具有不確定性,在叉車運載箱、包到達裝載區域后,需要對車身位置和航向進行調整,以對準登車橋,才能準確駛上貨車車廂并完成裝載。叉車屬于低自由度的大噸位車輛,在調整航向時應盡量避免原地打轉、頻繁啟停、倒車等復雜動作。應用運動規劃技術可使叉車僅用簡單動作就準確調整位置和航向的需求,因此,對智能轉運叉車的運動規劃進行研究很有意義。

解決智能轉運叉車的自動駕駛運動規劃問題有3個層次:(1)搜索最佳路徑;(2)搜索最佳機動動作;(3)搜索最佳軌跡。解決方式可歸納為兩種方案:(1)忽略時間,只考慮二維空間的路徑規劃;(2)考慮時間因素,在三維時空進行的運動規劃。

屈盼讓等[2]提出了基于B樣條曲線解決無人車路徑規劃問題的方法,建立了完整的分步規劃模型。呂恩利等[3]采用四次五階準均勻B樣條曲線,分前后段合并生成滿足叉車多約束條件下的避障路徑。周兵等[4]提出了一種基于非均勻B樣條曲線,且滿足多約束要求的路徑規劃方法。呂恩利等[5]提出了基于三次均勻B樣條曲線的托盤拾取路徑規劃方法。王國輝等[6]提出了一個用于自動駕駛車輛運動規劃的時空網絡模型,該網絡利用采集的時空信息產生轉向運動反應。KHESRANI S等[7]提出了一種解耦的路徑-軌跡規劃方法,引入了模糊平坦度的概念,以保證在不同不確定性條件下規劃任務的準確執行。

路徑規劃需要路徑跟蹤算法控制行車動作予以配合,不僅需要復雜的計算,還可能出現行車動作的快速變換,如急轉、急加速等情況,而這些情況在實際行車中可能會受到車輛本體機械結構的限制[8]。運動規劃是根據特定場景特點和車輛參數建立的,只需要簡單地控制動作參數即可完成軌跡跟蹤,針對性強。常見的運動規劃方法有軌跡預測[9]、泛函梯度優化[10]、多項式插值[11]等。

結合真實貨場裝載場景,為實現智能轉運叉車的自主登車,提高叉車在箱、包轉運裝載過程的靈活性和功能性,本文以具有非完整約束的智能轉運叉車為研究對象,綜合考慮各種約束條件,建立智能轉運叉車運動規劃模型,并根據車輛和裝車場景數據求解該模型運動控制參數,生成車輛行駛軌跡,驗證到達登車點時車輛航向的準確性以及控制過程的可行性。

1 智能轉運叉車作業

在半結構化箱、包裝載環境下,貨車停放在指定的裝車區域內,叉車作業的整個流程如下:

智能轉運叉車從倉庫拾取托盤,通過調度系統得知目標貨車的停放區域,經全局路徑規劃行駛到裝載區域目標貨車登車橋后方一定的距離,啟動登車橋位姿識別算法,通過傳感器進行登車橋位姿識別以及叉車自身位姿識別,根據識別到的登車橋相對位姿啟動運動規劃算法,得到智能轉運叉車行車動作控制參數,進而通過控制系統跟蹤行車,完成調整航向和位置的目標。

登車前位姿調整作業流程如圖1所示。

圖1 智能轉運叉車位姿調整作業流程

在叉車動力學研究中,叉車穩定性問題通常用橫擺角速度的變化規律來描述,而叉車軌跡保持問題通常用側滑角的變化規律來描述[12]。

針對叉車側偏角難以直接測量的問題,ZHANG Xu[13]提出了一種僅采用橫擺角速度反饋的叉車主動轉向系統設計方案,提高了叉車的轉向穩定性以及操縱穩定性。研究叉車運動規劃問題常忽略動力學和其他微分約束,主要關注車輛的轉向和位移[14]。

叉車屬于前輪驅動、后輪轉向的非完整性約束車輛,因此此處以前軸中心點為車輛坐標系原點建立右手坐標系,并將識別到的登車橋尾板位姿作為目標點,根據在二維平面內生成一條從起點到終點的可行路線軌跡,再運用逆運動學方法得出決策動作的參數。

不同位姿下的運動規劃算法生成的行車軌跡如圖2所示。

圖2 不同位姿下的運動規劃算法生成的行車軌跡

2 智能轉運叉車運動約束

登車運動規劃的目的是使得智能轉運叉車能實現自主登車。智能轉運叉車運動規劃以勻速前進和勻轉向角速度為基礎,建立分階段運動的運動模型,通過控制每個階段持續的時長、速度、角速度,確保智能轉運叉車到達目標位置且終點航向角準確[15],使該運動規劃模型生成一條可供智能轉運叉車跟蹤的行駛路徑。

2.1 智能轉運叉車運動學模型

智能轉運叉車的結構方式是后輪轉向前輪驅動,阿克曼轉向機構在實際轉向時后輪內側輪胎轉角偏大。在建立智能轉運叉車運動學模型時,假設叉車車體為剛體,忽略不計車輪形變,在后軸中心設一個等效轉向輪,在前軸中心設一個等效驅動輪;假設理想狀態下所有車輪作純滾動而無滑移,且轉向時無側滑。

以前軸為原點建立的運動學模型如圖3所示。

在全局坐標系XOY下,(x,y)為前軸軸心坐標,P為轉向中心。在前軸軸心處,根據運動學約束可得:

(1)

(2)

圖3 智能轉運叉車運動學模型1—等效驅動輪;2—等效轉向輪

(3)

式中:L—叉車的軸距,m;φ—等效后輪轉向偏角,rad;w—轉向速度,rad/s;v—前軸中心速度,m/s。

智能叉車運動學模型為:

(4)

2.2 智能轉運叉車參數約束

諾力FE4P20E型四支點前驅平衡重電動叉車基本參數如表1所示。

表1 叉車基本參數

智能叉車轉向曲率為:

(5)

2.3 運動起終點約束

在登車前,調整位姿運動路徑的起點及智能轉運叉車的后輪轉向角為零,航向角方向定義為X軸正方向,沿X軸方向左側且垂直于X軸定義為Y軸正方向,起點定義為坐標系原點;登車橋尾板中點為調整位姿運動的終點,智能轉運叉車在此處航向角應垂直于登車橋尾板,且后輪轉向角為零。

此處叉車相對登車起點的相對位姿(Δx,Δy,Δθ),如圖4所示。

圖4 登車運動起終點約束

圖4中,Δx向右偏移為正;Δx向上偏移為正;Δθ以x軸逆時針偏移為正。

登車橋按類型分有移動式和固定式兩種,按需求定制。

本文以某一普通移動式登車橋為例,該登車橋規格尺寸參數如表2所示。

表2 登車橋尺寸參數表

為避免發生翻車、碰撞登車橋等情況,通過登車橋的長度、寬度、翻板間隙、智能叉車前后輪距以及輪胎寬度的計算,可得叉車運動到終點位姿誤差范圍D≤230 mm,θ≤1.14°。

3 運動規劃模型

3.1 運動規劃的維度

在滿足安全性和穩定性的要求,智能轉運叉車運動規劃中,在行走模塊,叉車的運動輸出動作主要由輸入速度和轉向角動作控制。雖然行走軌跡可以在二維平面上,但同時也要考慮時間維度,因此,行走軌跡就是一條在二維空間和一維時間中的三維曲線。

從動力學上來說,叉車有很大的慣性;從運動學上來說,叉車又屬于非完整性系統,因此,只考慮當前位姿而沒有前瞻性動作就無法準確跟蹤規劃行車路線。運動規劃不僅包括路徑規劃,也考慮運動控制。這樣由運動規劃得出的一條在三維時空中的跟蹤路徑可以完整地描述智能叉車的運動全貌,并對該運動規劃模型進行求解,從而實現對叉車的精確運動控制。

3.2 運動控制模型

為保持車身行駛平穩,避免急轉向情況的出現,建立智能轉運叉車運動規劃模型,有必要考慮叉車運動的橫向和縱向加速度、加加速度,即前進運動的速度、加速度和轉向角的速度、加速度。

運動模型主要有恒速模型(CV)、恒加速度模型(CA)、恒角速度模型(CTR)和恒角加速度模型(CTRA)[16]。轉向角速度對于高速行駛車輛影響較大,不同的運動模型軌跡會有較大的差別,因此,要在規劃之初,根據叉車運動特性對運動模型進行合理決策。

為了簡化智能轉運叉車計算的復雜度,假設整個運動規劃過程中車輛勻速行駛,給定行駛勻速度v=1.0 m·s-1,角速度w=π/6 s-1。由于加速轉向減速轉向時間較短可忽略不計,在該模型下簡化為勻速轉向階段,且每個轉向狀態階段持續時長相同。

CC曲線(continuous curvature path)是曲率線性變化的曲線,它由最小轉彎半徑曲線轉換而來,對路徑進行平滑化處理[17]。

本文研究了常見場景下的叉車運動規律,并在CC曲線的基礎上進行了優化,提出了S型曲線的行車軌跡。此處按轉向角狀態轉換將運動模型分為9個階段,分別為:

(1)起始線型階段。轉向角為零且不變,車輛沿初始航向角直行;

(2)轉向階段。轉向角勻速增加,車輛轉向曲率增加;

(3)轉向保持階段。轉向角保持不變,車輛沿固定半徑圓弧行走;

(4)轉向回正階段。轉向角勻速減小,車輛轉向曲率逐漸減小;

(5)斜向直行階段。轉向角為零且不變,車輛沿當前航向角直行;

(6)反轉向階段。轉向角反向勻速增加,車輛轉向曲率增加;

(7)反轉向保持階段。轉向角保持不變,車輛沿固定半徑圓弧行走;

(8)反轉向回正階段。轉向角勻速減小,車輛轉向曲率逐漸減小;

(9)結尾直行階段。轉向角為零且不變,車輛沿當前航向角直行。

在運動模型的9個階段中,每個階段轉向角狀態持續時長用{T1,T2,T3,…,T9}表示,整個過程有10個時間節點,分別用{t0,t1,t2,…,t9}表示。其中：t0—開始時間;t9—結束時間。

定義第2階段轉向時左轉為正,右轉為負,轉速為w,則第4階段轉速為-w,第6階段轉速為-w,第8階段轉速為w,且在這4個階段轉向過程持續的時間相同,則轉向角表達式如下:

(6)

其中,第5階段轉向角φ5=w(t2-t1)-w(t4-t3)=wT2-wT4=0,第9階段轉向角φ9=-w(t6-t5)+w(t8-t7)=-wT6+wT8=0。

智能轉運叉車的航向角為:

(7)

式中:L—叉車軸距。

X軸方向移動的距離為:

(8)

Y軸方向移動的距離為:

(9)

運動模型中轉向角變換對應的θ、Sx、Sy變化曲線和行車軌跡如圖5所示。

圖5 轉向角變換對應的θ、Sx、Sy變化曲線和行車軌跡

可見,在指定后輪轉速和車速以后,通過控制每個階段的時長即可控制叉車的最終位姿。

3.3 運動模型求解

運動模型規劃要達到的目標是智能轉運叉車從起點運動到終點時,滿足以下3式:

(10)

(11)

(12)

在建立運動模型時,根據已知車速v,軸距L,轉速w,設定第2、4、6、8階段轉向過程持續時間T2、T4、T6、T8相等,即T2=T4=T6=T8=Tφ,可知影響Δθ變化的有T3、T7,影響Δy變化的有T3、T5、T7、T9,影響Δx變化的有T1、T3、T5、T7、T9。

由此,先求解第3、7階段持續時間T3、T7,其次求解第5、9階段持續時間T5、T9,最后求解第1階段持續時間T1。

3.3.1 T3、T7求解

根據式(10)可知,在其他7個階段航向角變化之和為零。求解T3、T7時,已知φ3=wT2,φ7=-wT6;又T2=T4=Tφ,可得:

(13)

Δy、Δθ值分別為正、負時,T3與T7大小關系如圖6所示。

(a)Δy為正，Δθ為正時，轉向角、航向角變化曲線及行車軌道

圖6中:規定Δy向y軸正方向偏移為正,反之為負;Δy為正時,車輛先左轉后右轉,反之先右轉后右轉;

規定向x軸接近為正,遠離x軸Δθ為負,可知Δθ為正時t3t7。

3.3.2T5、T9求解

求解運動規劃模型中的T5、T9時,模型中的第2-4、6-8階段的時長參數已由前文確定,當T5變化時,并不影響隨后的第6-8階段Δy6-8的變化值,舉例如圖7所示。

圖7 其他參數不變,T5=1.0/1.5時Δy6-8的變化量對比

已知總的Y軸偏移量為Δy,且Δy1=0,根據前文得Δy2-4,6-8=Δy2+Δy3+Δy4+Δy6+Δy7+Δy8,則:

Δy5,9=Δy-Δy2-4,6-8

(14)

在運動模型中,第(5,9)階段的行駛軌跡分別是兩條不同斜率的直線,可知Δy5、Δy9是和時長t相關的一階線性方程。

(15)

(16)

Δy5,9=ky5T5+ky9T9

(17)

式(17)中,T5和T9有多組解,調用MATLAB中的fmincon函數,以T5+T9取最小值為優化目標,建立方程組如下:

(18)

當Δθ為正時,ky5與ky9的值一個為正、一個為負,T5和T9總有解,且成正比。當Δθ為負時,ky5與ky9的值均為正值或負值,T5和T9的值成反比,若此時Δy5,9=|Δy|-|Δy2-4,6-8|<0就得出T5、T9為負值,與事實不符,要通過減小|Δy2-4,6-8|的值,即減小w或者Tφ的值來使T5和T9為正值。

等比例減小w或Tφ生成的軌跡如圖8所示。

圖8 等比例減小w或Tφ生成的軌跡

由圖8可知:減小w對減小|Δy2-4,6-8|值的效果不如等比例減小Tφ,且會使X軸偏移量增大,對下一步求解T1不利,故選擇通過減小Tφ減小|Δy2-4,6-8|值更合適。此處引入迭代法計算,直到T5、T9均為正解,但要注意tan(wTφ)值的變化會引起|T5-T9|值的變化,迭代時要重新計算T3、T7的值使Δθ保持不變。

3.3.3 T1求解

(19)

若Δx1=Δx-Δx2-9<0,則依舊用迭代法通過減小T5、T9或者Tφ的值,達到使Δx1>0的目的。

3.4 模型求解步驟總結

整個模型的求解過程用到了分步法、迭代法和fmincon函數。已知軸距L=1.5 m,目標Δx、Δy、Δθ,求解步驟總結如下:

(2)根據Δy的正負,確定w的正負;

(3)根據Δθ的值,確定t7、t3的值;

(4)計算Δy2-4,6-8的值,并與Δy比較。若Δy-Δy2-4,6-8<0則減小w值,并重新計算Δy2-4,6-8的值;

(5)根據Δy-Δy2-4,6-8的值,確定t5、t9的值;

(6)計算Δx2-9的值,并與Δx比較。若Δx-Δx2-9<0則適當增大w的值,但要注意不能超出最大轉向速度,然后再返回第3步;

(7)根據Δx-Δx2-9的值確定t1;

(8)代入所有控制參數,運行運動規劃模型,生成行車軌跡,驗證終點位置與航向是否在誤差范圍內。

4 仿真實例及分析

不同目標位姿(Δx,Δy,Δθ)下運動規劃模型運算后生成的運動軌跡簇,如圖9所示。

(a)Δx=5.5，Δy=2.0 (b) Δx =8.0，Δy =2.0 (c) Δx =8.0，Δy =1.0

(d) Δx =8.0，Δy =0.5 (e) Δx =8.0，Δy =0.3 (f) Δx =8.0，Δy =-0.3

圖9中,虛線是轉向角變化曲線,點劃線是航向角變化曲線,實線是叉車前軸中心行車軌跡。

對比圖9(a～b)可知:在相同Δy值情況下,隨著Δx值減小,w會有所增大,這是因為第一階段時長只改變X方向距離;后面階段完成求解而發生了后8個階段在X軸偏移量超出了總的目標Δx值,為避免T1出現負值,通過調整w來重新求解運動模型。由此可見,在極限情況下(Δx最小,Δy最大),模型在范圍內有解,即運動模型設計滿足應用需求。

對比圖9(b～d)中,Δx值不變,Δy值變化,可知隨著Δy的減小,最大轉向角wtφ在減小,即轉向階段在Y軸的偏移量超出了總的目標Δy值。為避免T3、T7出現負值,通過調整tφ來減小轉向階段的Δy值,使得模型有解,然后重新求解運動模型。

對比圖9(b,i,c,h)可知,在Δx值相等、Δy值相反時,運動模型規劃出的軌跡對稱,即對w轉向的控制很成功。

觀察圖9(d～g)生成的運動軌跡簇可知:Δθ<0時,隨著Δy的減小,在Δθ為較大值時運動規劃模型得不到行車軌跡;而Δy越小,模型無解時的最大Δθ值也越小。這時,需要調整策略,經過試驗把tφ初始值調整為0.5 s時能得出最優運動規劃結果。

調整轉向策略的最佳范圍如圖10所示。

圖10 調整轉向策略的范圍

圖10中,對0<Δy<0.5且Δθ<0,調整為先右轉;對0>Δy>-0.5且Δθ<0,調整為先左轉。

調整轉向策略后的運動模型求解出的運動軌跡如圖11所示。

(a) Δy =0.4

對比圖11(a～c)可知:在Δθ<0,Δy分別為0.4、0.3、0.2、0.1時,除Δy=0.1,Δθ=0.2外都能求出模型的解。在Δy值極小,且Δθ極小情況下,全程按直線行駛也滿足誤差范圍,添加修正函數為:

(20)

(21)

融合未調整轉向策略區域的模型算法,可以得到在目標位移和目標航向范圍內的所有運動規劃算法。至此,修正后的運動規劃算法模型在全部情況下都有解,完成了適用于本文的智能轉運叉車運動規劃算法。

在計算效率上,在Intel(R)Xeon(R)CPU 3.50 GHz的計算機中,MATLAB2020b版本環境使用tic、toc函數生成多個控制點的B樣條曲線軌跡,運行25次后得出平均運行時間為0.029 5 s。而本文的運動規劃模型在100種不同目標中求解運行總時間為1.228 159 s,平均單一目標求解運行時間為0.012 3 s,效率比前者高59%。

5 結束語

在參考了普通叉車裝車作業時運行軌跡特征,并結合叉車運動學分析的基礎上,筆者建立了基于轉向角變換的9種運行狀態組成的叉車運動規劃模型,并在MATLAB環境下對其進行了仿真驗證。

研究結果表明:

(1)該運動規劃算法模型能在前向位移5 m～8 m、側向位移±2 m、航向角±10°范圍內簡單、準確、快速地生成滿足多約束狀態下的運動規劃控制參數,平均運行時間約為0.012 3 s;

(2)模型改進了以往在固定場景下的路徑規劃算法,省去了需通過優化路徑曲線光滑度來實現最優路徑規劃的過程,減少了叉車運行控制計算量,運行時間減少了約59%。

在今后的研究中,筆者將針對智能轉運叉車目標位姿識別差錯,或供叉車行駛空間過于狹窄的情況,進行叉車進退式運動規劃研究,使叉車可以在更復雜的環境或場景內做二次運動規劃。