丁聰，趙蕾磊

(1.中船重工鵬力(南京)智能裝備系統(tǒng)有限公司，江蘇南京 210000；2.中國航天科工南京晨光集團江蘇金陵智造研究院，江蘇南京 210000)

0 前言

航空航天等重工業(yè)中，多見艙段、機翼等具有大體積、大質(zhì)量特性的部件。若使用重載AGV搬運該類超大超重產(chǎn)品，不僅可以提高轉(zhuǎn)運效率，還可以避免現(xiàn)有吊裝轉(zhuǎn)運方式存在的掉落風(fēng)險，因此將重載AGV應(yīng)用于重載場合逐漸引起加工制造業(yè)及學(xué)者的關(guān)注[1-3]。

為使AGV具備更高的承載能力和驅(qū)動能力，重載AGV需使用多套輪系驅(qū)動。由于差速輪的雙輪結(jié)構(gòu)使得單套輪系具有更強的驅(qū)動力和承載力，與直接轉(zhuǎn)向舵輪相比，相同承載需求下使用輪系數(shù)量更少、占用空間更小，因此差速輪更適合重載場合。當(dāng)前差速驅(qū)動相關(guān)研究多集中在汽車底盤差速系統(tǒng)[4-8]、多差速驅(qū)動單元協(xié)同控制[9-17]或含有差速承載單元的輪系控制[18-19]。汽車底盤差速系統(tǒng)與AGV原理不同，不在此次討論范圍；使用單套舵輪驅(qū)動、差速輪承載的AGV由于其結(jié)構(gòu)限制，車體旋轉(zhuǎn)中心只能位于差速輪軸線上，AGV不具備全向移動能力，靈活性及循跡能力較差，也不作為此次研究的對象。而針對雙輪均具備驅(qū)動能力的多差速驅(qū)動單元控制的研究，多集中有限(一組或兩組輪系最為常見)差速輪系且差速驅(qū)動單元輪系旋轉(zhuǎn)中心均位于車體中軸線的應(yīng)用情景。如文獻[17]提出一種使用3套差速驅(qū)動單元協(xié)同控制方法。該研究基于多機器人編隊控制方法，將前輪作為領(lǐng)航單元，利用兩輪間距離差作為剛性約束條件控制AGV進行路徑跟蹤。但是，該方法基于“3套差速驅(qū)動系統(tǒng)中心均位于車體中軸線”提出了約束并基于該約束提出了跟隨單元速度控制律，輪系布局受限，不具備擴展性。AGV在實際使用時常與循跡、跟蹤等功能密不可分[20-24]。文獻[25]雖然基于常見重載AGV輪系排布介紹了差速輪系直線行進、斜行、原地轉(zhuǎn)彎等幾種典型運動形式下各輪速度分配，但僅限于簡單的純運動學(xué)分析，未考慮循跡、跟蹤等實際復(fù)雜應(yīng)用情景，實用性不高。

直接轉(zhuǎn)向舵輪具有轉(zhuǎn)向電機和驅(qū)動電機，可實現(xiàn)AGV全向移動，循跡精度高；差速輪的左、右輪各有一套驅(qū)動電機，依靠驅(qū)動速度差轉(zhuǎn)向，因此不僅需滿足同一AGV內(nèi)的剛體運動學(xué)關(guān)系，各輪系內(nèi)部也存在耦合，循跡運行也更加困難。為使得多差速輪驅(qū)動的AGV擁有與直接轉(zhuǎn)向舵輪驅(qū)動相似的良好循跡能力，同時在充分考慮糾偏需求時兼顧差速輪運動學(xué)特性、整車剛體運動學(xué)特性及良好的可擴展性，提出了多差速單元循跡協(xié)同控制方法。

1 差速輪運動特性

如圖 1所示，一套差速輪為雙輪結(jié)構(gòu)，兩輪均為驅(qū)動輪，分別配有驅(qū)動電機。差速輪不具備直接轉(zhuǎn)向能力，需通過兩輪速度差轉(zhuǎn)向。如圖 2所示，記一個差速驅(qū)動模塊中心速度vc，兩輪中心距離的一半為dw，左輪速度為vl，右輪速度為vr；輪系繞點O旋轉(zhuǎn)，轉(zhuǎn)動角速度為ω，旋轉(zhuǎn)半徑為R。一個運動周期后，輪系繞點O轉(zhuǎn)過角度Δθ1，輪系中心速度方向有變化量Δθ2。左、右兩輪速度可表示為

vl=|ω|·(R+dw)

(1)

vr=|ω|·(R-dw)

(2)

聯(lián)立以上兩式可得：

(3)

由于Δθ1=Δθ2，因此輪系轉(zhuǎn)動角速度ωd與ω相等。當(dāng)輪系逆時針旋轉(zhuǎn)時，vr>vl，ω為正值；當(dāng)輪系順時針旋轉(zhuǎn)時，ω為負值。由幾何關(guān)系易得輪系中心速度vc為

(4)

由式(1)—(4)，當(dāng)已知輪系中心速度vc及轉(zhuǎn)動角速度ω時，可得左右輪速度為

vl=vc-ω·dw

(5)

vr=vc+ω·dw

(6)

ω受驅(qū)動輪驅(qū)動能力上限限制，設(shè)驅(qū)動電機可驅(qū)動輪系最大運動速度為vmax，當(dāng)vc=0時，輪系有最大旋轉(zhuǎn)速度ωmax，此時ωmax=vmax/dw。

圖 2 差速輪速度分配

差速輪轉(zhuǎn)向后中心位置及中心速度指向發(fā)生變化。當(dāng)兩個運動周期間隔t較小時，可簡化為圖 3所示狀態(tài)：車輪中心沿圖中vc方向移動距離Δd，且有Δd=vc·t；在新的中心位置，vc指向發(fā)生變化，與上一時刻中心速度方向相比旋轉(zhuǎn)了角度Δθ，且有Δθ=ω·t。

圖 3 小周期內(nèi)差速輪姿態(tài)變化示意

2 多差速驅(qū)動單元協(xié)同控制

2.1 基于偏差狀態(tài)的控制

此次研究針對的AGV簡化模型如圖4所示，采用4套差速輪驅(qū)動的形式。矩形線框表示連接各輪系旋轉(zhuǎn)中心的車架體。建立車體坐標(biāo)系x-y，其中x軸正向指向車體前進方向。四輪分別編號1—4，各輪旋轉(zhuǎn)中心位置在車體坐標(biāo)系中可表示為式(7)所示矩陣形式。若使用更多輪系，可將該矩陣按需擴展。

(7)

如圖5所示，建立世界坐標(biāo)系X-Y，AGV循跡線設(shè)為Y軸。車體坐標(biāo)系原點在世界坐標(biāo)系中表示為(xcyc)，車體坐標(biāo)系x軸相對于X軸有姿態(tài)角θc。理想情況下，車體中心應(yīng)位于Y軸且x軸與Y軸重合，但位姿偏差不可能被完全消除，圖中所示車體中心相對軌跡線有距離偏差dc，記dc=|xc|；定義x軸與軌跡線的夾角較小值為角度偏差δc。使用vc表示中心速度，使用ω表示車體旋轉(zhuǎn)角速度，為消除位姿偏差，設(shè)計跟蹤控制律：

(8)

式中：k1為線速度系數(shù)，k1>0；k2為角速度系數(shù)，k2>0。

vc與車體x軸有夾角β，指向軌跡線。在上述線速度及角速度組合下，AGV將靠近軌跡線，同時車體姿態(tài)得到調(diào)整。

設(shè)0時刻車體中心位于軌跡線左側(cè)且有角度偏差δc，如圖6所示。按式(8)所示控制律，需為其中心規(guī)劃指向軌跡線方向的速度及可使車體逆時針旋轉(zhuǎn)的角速度，即在到達1時刻時，在坐標(biāo)系x1-y1下，各輪有圖7所示擺角θi(1)及輪系中心速度vi(1)使得車體中心速度vc(1)與x1軸有夾角β(1)，車體旋轉(zhuǎn)半徑大小為r(1)。

圖 6 0時刻車體狀態(tài)示意

設(shè)車體參考速度大小為vd。在車體坐標(biāo)系x0-y0下，取各輪中心速度vi(0)=[vd0 0]T。期望到達1時刻時，中心速度為

vc(1)=

[|vc(1)|cosβ(1) |vc(1)|sinβ(1) 0]T

(9)

并有角速度為ω(1)。記：

ω(1)=[0 0ω(1)]T

(10)

各輪速度向量可表示為

vwheel(1)=[vc(1)1×4]+[ω(1)1×4]×P=

(11)

各輪系擺角為

(12)

設(shè)ωi(0)為各輪旋轉(zhuǎn)角速度，計算方法為

(13)

viL(0)=|vi(0)|-ωi(0)·dw

(14)

viR(0)=|vi(0)|+ωi(0)·dw

(15)

將上述計算結(jié)果賦予輪系，AGV可從x0-y0狀態(tài)運動至x1-y1狀態(tài)，如圖7所示。各輪擺角變化至θi(1)，車體姿態(tài)未發(fā)生變化。

在下一運動周期，期望車體從1時刻狀態(tài)到達2時刻狀態(tài)，即在坐標(biāo)系x2-y2下，各輪有圖8所示擺角θi(2)及輪系中心速度vi(2)使得車體中心速度vc(2)與x1軸有夾角β(2)，旋轉(zhuǎn)半徑大小為r(2)。在1時刻時各輪已有夾角θi(1)，為滿足剛體運動學(xué)關(guān)系，各輪中心速度應(yīng)為vi(1)，且有：

(16)

期望到達2時刻時，中心速度為

vc(2)=[|vc(2)|cosβ(2) |vc(2)|sinβ(2) 0]T

(17)

并有角速度為ω(2)。各輪速度向量可表示為

vwheel(2)=[vc(2)1×4]+[ω(2)1×4]×P

(18)

各輪擺角為

(19)

由于各輪中心速度為vi(1)，不同于x0→x1的變化情況，在該輪系中心速度分配下，x1→x2時車體姿態(tài)會發(fā)生變化，變化量為ω(1)·t。各輪旋轉(zhuǎn)角速度應(yīng)表示為

(20)

之后，可根據(jù)中心偏距dc與姿態(tài)角θc確定預(yù)期達到的中心速度vc及角速度ω并持續(xù)進行上述調(diào)速循環(huán)。

2.2 基于驅(qū)動能力的控制

輪系驅(qū)動能力有限，因此利用兩輪速度差消除偏差的糾偏能力是有限的；同時，為保證AGV循跡時流暢運行，應(yīng)避免出現(xiàn)指令電機頻繁正反轉(zhuǎn)、系統(tǒng)帶寬不足導(dǎo)致明顯停頓的情況。所求解各輪驅(qū)動速度應(yīng)使得對任一viL、viR，滿足0≤viL≤vmax且0≤viR≤vmax。當(dāng)基于偏差狀態(tài)所得各輪驅(qū)動速度不滿足上述條件時，應(yīng)考慮限制車體旋轉(zhuǎn)中心位于車體y軸上。

在適應(yīng)值選擇方面,理論上最理想的適應(yīng)值應(yīng)該是重建誤差Er,但在實際工程中重建誤差是不可知的。這里,距離誤差el與重建誤差有較高的相關(guān)性,在一定程度上能反映重建誤差的變化,所以本文以所有重建點的距離誤差之和作為粒子群優(yōu)化方法的適應(yīng)值。

圖9所示為xc<0條件下路徑的3種規(guī)劃。圖(a)所示為θc<π/2的情況，定義圖(a)所示軌跡為L1類軌跡，該類軌跡可實現(xiàn)姿態(tài)與距離的同步糾偏。圖(b)所示為θc<π/2條件下優(yōu)先校正距離偏差的一種軌跡規(guī)劃，定義圖(b)所示軌跡為L2類軌跡，與L1類軌跡相比更適用于中心偏距超出最大允許范圍的情況。圖(c)所示為θc>π/2的情況，該種情況下應(yīng)使θc轉(zhuǎn)化至小于π/2的狀態(tài)以便于向L1或L2類軌跡過渡，定義圖(c)所示軌跡為L3類軌跡。xc>0時規(guī)劃軌跡方式類似，定義為R1、R2及R3。若使用上述兩大類軌跡對AGV進行控制，AGV將在各類路徑間來回切換，達到循跡運行的效果，如圖 10所示。

圖 10 各類路徑相互轉(zhuǎn)化

設(shè)AGV起始時刻狀態(tài)如圖6所示，同時設(shè)中心偏距dc超出距離偏差允許范圍，為車體規(guī)劃L2類軌跡。各輪中心速度vi(0)=[vd0 0]T。期望到達1時刻時，在坐標(biāo)系x1-y1下，中心速度為vc(1)且有旋轉(zhuǎn)半徑r(1)，vc(1)與x1軸平行。1時刻車體狀態(tài)變化見圖11。設(shè)未知的旋轉(zhuǎn)半徑r(1)為rlim，則：

圖1 差速輪結(jié)構(gòu)

圖4 車體坐標(biāo)系

圖5 偏差狀態(tài)

圖7 1時刻車體狀態(tài)

圖8 2時刻車體狀態(tài)變化

圖9 L類軌跡規(guī)劃

(21)

其中：ωsign為控制車體旋轉(zhuǎn)角速度方向的符號量。各輪速度展開可表示為

vwheel(1)=[vc(1)1×4]+[ω(1)1×4]×P=

(22)

上式與式(11)的區(qū)別在于，由于rlim是未知的，使用rlim表示的vix(1)、viy(1)也是未知的。各輪擺角、各輪旋轉(zhuǎn)角速度及各輪左、右兩輪速度分配可使用式(12)—(15)計算，所求得θi(1)、ωi(0)、viL(0)及viR(0)也均含有未知量rlim。

由于車體中心速度方向與x軸平行，若角速度為順時針，可確定各輪應(yīng)有擺角方向，如θ1(1)應(yīng)為負值，v1L(0)>v1R(0)。解不等式：

(23)

可得滿足上述條件的旋轉(zhuǎn)半徑rlim，將rlim賦予r(1)并代入式(11)—(15)重新進行速度分配，可得確定的viL(0)、viR(0)、θi(1)和ω(1)。

車體運行一周期后，設(shè)1時刻車體中心偏距與姿態(tài)角仍滿足L2類軌跡規(guī)劃條件。為車體規(guī)劃L2類軌跡，即在2時刻時，坐標(biāo)系下x2-y2下，中心速度為vc(2)且有旋轉(zhuǎn)半徑r(2)，vc(2)與x2軸平行。設(shè)未知的旋轉(zhuǎn)半徑r(2)為rlim，則：

(24)

后續(xù)計算方式與前述過程類似，使用式(22)—(23)求解符合條件的旋轉(zhuǎn)半徑rlim并將rlim賦予r(2)，重新進行速度分配計算，2時刻狀態(tài)如圖12所示。

圖12 驅(qū)動能力限制下，2時刻車體狀態(tài)變化

2.3 混合糾偏策略

基于偏差狀態(tài)的控制優(yōu)勢在于可穩(wěn)定跟蹤并快速消除誤差，最終達到穩(wěn)定狀態(tài)。這種控制方法下，線速度、角速度與誤差相關(guān)，糾偏效率高。但是，方向不定的線速度與角速度組合后各輪擺角方向正負不可預(yù)知；若使用不等式限制驅(qū)動速度大小和方向，各不等式取值范圍可能不存在交集。

基于驅(qū)動能力的控制由于限制了車體中心速度方向，使得在角速度方向確定時，輪系擺角方向確定，便于控制車速范圍。但也由于限制了車體中心速度方向使得循跡不夠靈活，車體中心始終在軌跡線附近擺動。

結(jié)合兩種方法的優(yōu)勢，可得基于有限糾偏能力的多差速單元循跡協(xié)同控制方法。該方法考慮輪系驅(qū)動能力和物理結(jié)構(gòu)限制，同時受到位姿偏差約束，最終可實現(xiàn)多差速單元驅(qū)動AGV的穩(wěn)定循跡運行?？刂屏鞒炭煽偨Y(jié)如圖13所示。為便于描述，將基于偏差狀態(tài)的循跡控制稱為方法一，將基于驅(qū)動能力的狀態(tài)控制成為方法二。

圖13 混合控制流程

設(shè)起步時刻計數(shù)為q，在進行輪系回歸零位步驟后，各輪擺角均為0，此時各輪中心速度也為參考速度，即vi(q)=[vd0 0]T。使用ωsign，1、ωsign，2分別表示方法一、方法二的車體角速度指標(biāo)。使用指標(biāo)量f表示速度分配使用方法，f=1時表示使用方法一，f=2時表示使用方法二；當(dāng)車體執(zhí)行回歸零位步驟時，記f=3。

當(dāng)前時刻車體有距離偏差dc、角度偏差δc和姿態(tài)角θc，由此可確定基于方法一的指標(biāo)量ωsign,1、中心速度方向角β以及基于方法二的ωsign,2。首先使用方法一進行速度一步分配，得到當(dāng)前時刻各輪左、右輪驅(qū)動速度viL(q)、viR(q)，以及下一時刻可得到的擺角θi(q+1)、各輪中心速度vi(q+1)以及車體旋轉(zhuǎn)角速度ω(q+1)，并記ωsign(q+1)=ωsign,1、f=1。若此時有任一viL(q)或viR(q)不滿足大于或等于0且小于或等于最大驅(qū)動速度的條件，應(yīng)調(diào)用方法二進行速度一步分配并獲得相關(guān)參數(shù)，同時記錄ωsign(q+1)=ωsign,2、f=2。各輪系按照驅(qū)動速度viL(q)、viR(q)運行，到達下一時刻，此時各輪有擺角θi(q+1)，各輪中心速度應(yīng)為vi(q+1)。

記q=q+1，使用與前述類似的方法，根據(jù)當(dāng)前時刻距離偏差、姿態(tài)角等參數(shù)，確定基于方法一的相關(guān)指標(biāo)量并進行輪系速度分配；若有任一輪系驅(qū)動速度不符合條件，則使用方法二進行速度分配。與速度一步分配的區(qū)別在于，此時速度分配時應(yīng)考慮車體姿態(tài)變動量及各輪中心速度與車體坐標(biāo)系夾角。若計算所得結(jié)果f=2且車體應(yīng)有角速度旋轉(zhuǎn)方向與上一時刻不同，即ωsign(q+1)·ωsign(q)=-1，這表明各輪擺角方向?qū)l(fā)生變化，為保證調(diào)姿效率、符合剛體運動條件，應(yīng)調(diào)用輪系回歸零位步驟；若不滿足上述條件，則令q=q+1并進行正常的步驟循環(huán)。

3 仿真結(jié)果

記車體結(jié)構(gòu)參數(shù)w1=0.9 m，w2=0.8 m；差速輪兩輪中心間距的一半dw=0.21 m，指令速度vd=0.5 m/s，輪系最大驅(qū)動速度vmax=2 m/s；計數(shù)時間間隔t=0.02 s。初始時刻計數(shù)q=1，車體姿態(tài)角θc(1)=86°，各輪擺角分別為θ1(1)=-14°，θ2(1)=7°，θ3(1)=10°，θ4(1)=-10°。

初始狀態(tài)可表示如圖14(a)所示，此時車體中心與軌跡線距離dc(1)=-0.017 m，與軌跡線夾角δ(1)=4°。設(shè)置中心偏距最大允許量dlim=0.02 m，角度偏差最大允許量θlim=2°。仿真步數(shù)共計100步，由不同時刻結(jié)果及圖15、圖16可看出車體姿態(tài)得到糾正并逐步靠近軌跡線，最終穩(wěn)定行駛在軌跡線上。在時刻計數(shù)q=100時，車體中心偏距穩(wěn)定在0.01 m，車體姿態(tài)角穩(wěn)定在90.04°。

圖14 不同時刻車體相對軌跡線姿態(tài)

圖15 車體中心偏距dc變化

圖16 車體姿態(tài)角θc變化

圖17為各個計數(shù)時刻f數(shù)值記錄，可看出整個糾偏流程中歸零步驟、方法一、方法二交替使用。q=1時由于各輪擺角不一致，首先進行了輪系歸零并在計數(shù)為3時達到各輪擺角均已歸零的狀態(tài)，即θi(3)=0。

圖17 速度分配指標(biāo)

回歸零位后，首先使用方法一計算q=3時刻各輪左、右輪速度，解得：v1L=-0.466 m/s，v1R=1.466 m/s；v2L=-0.654 m/s，v2R=1.654 m/s；v3L=1.466 m/s，v3R=-0.466 m/s；v4L=1.654 m/s，v4R=-0.654 m/s。不滿足每套輪系左右輪均不反轉(zhuǎn)且不超速的要求。使用方法二重新計算速度，解得：v1L=0.134 m/s，v1R=0.866 m/s；v2L=0.110 m/s，v2R=0.890 m/s；v3L=0.866 m/s，v3R=0.134 m/s；v4L=0.890 m/s，v4R=0.110 m/s。將上述數(shù)值賦予viL(3)和viR(3)，車體在第四時刻擁有擺角θ1(4)=1.998°，θ2(4)=2.131°，θ3(4)=-1.998°，θ4(4)=-2.131°，如圖18、圖19所示。由于各輪中心速度相同且指向一致，車體姿態(tài)并未發(fā)生改變，θc(4)=86°，車體中心進一步靠近軌跡線，dc(4)=-0.016 m。

圖18 各輪驅(qū)動速度

圖19 各輪擺角

整個仿真過程按照混合控制方法，對各輪系速度進行了分配。如圖18所示，除起步時刻和70時刻進行必要的輪系歸零步驟導(dǎo)致輪系反轉(zhuǎn)外，其余時刻各輪均未出現(xiàn)速度小于0或超出驅(qū)動能力極限的情況。最終在第70時刻，距離偏差及角度偏差均位于允許范圍內(nèi)，在進行了輪系歸零步驟之后，各輪擺角為0(見圖 19)并按照指令速度沿軌跡線向前行進，達到穩(wěn)定狀態(tài)。

4 結(jié)論

(1)基于偏差的控制策略可有效消除偏差并使偏差趨于穩(wěn)定；基于驅(qū)動能力的控制策略考慮實際情況中有限的糾偏能力，為循跡過程提供速度約束。

(2)本文作者提出的基于有限糾偏能力的多差速單元循跡協(xié)同控制在充分考慮差速輪運動學(xué)特性及整車剛體運動學(xué)特性的基礎(chǔ)上，將基于偏差的控制策略與基于驅(qū)動能力的控制策略相結(jié)合。

(3)該方法可有效協(xié)調(diào)位姿糾偏需求與輪系運動能力。仿真結(jié)果表明：該方法下AGV位姿偏差被逐步消除并最終達到穩(wěn)定狀態(tài)，實現(xiàn)了差速驅(qū)動AGV循跡性能的整體優(yōu)化。