自動(dòng)落布車軌跡跟蹤控制算法

王玉　沈丹峰　王榮軍　李耀杰　李靖宇

摘要： 針對自動(dòng)落布車在紡織車間里的換道軌跡規(guī)劃和跟蹤控制問題，文章提出基于B樣條的快速高效且避障的換道拐彎軌跡規(guī)劃，并設(shè)計(jì)模型預(yù)測控制的落布車軌跡跟蹤控制器。以自主設(shè)計(jì)的雙舵輪自動(dòng)落布車為研究對象，利用B樣條曲線規(guī)劃紡織車間里落布車換道軌跡，分析車身結(jié)構(gòu)并建立其運(yùn)動(dòng)學(xué)模型，求得落布車在紡織車間里的定位算法及考慮車體的運(yùn)動(dòng)性能和環(huán)境約束，使用模型預(yù)測控制（MPC）減少復(fù)雜環(huán)境中的跟蹤誤差和隨機(jī)干擾。最后，通過Matlab軟件進(jìn)行仿真，驗(yàn)證了MPC在給定的可行距離和角度偏差范圍內(nèi)能夠穩(wěn)定地跟蹤參考軌跡，并與傳統(tǒng)PID控制進(jìn)行對比，最終證明采用MPC的軌跡跟蹤算法具有穩(wěn)定良好的跟蹤性能。

關(guān)鍵詞： 自動(dòng)落布車;B樣條;模型預(yù)測;PID控制;軌跡跟蹤;Matlab仿真

中圖分類號： TS103.7;TP242文獻(xiàn)標(biāo)志碼： A文章編號： 10017003（2022）04006509

引用頁碼： 041110DOI： 10.3969/j.issn.1001-7003.2022.04.010（篇序）

目前大部分織造車間都采用了手工落布的方式，由于人工落布速度慢，消耗大量人力，影響紡織車間的作業(yè)效率，因此迫切需要在落布過程中以機(jī)器代替人，采用自動(dòng)落布的方法。自動(dòng)落布車主要應(yīng)用于織造車間，將卷布成品自動(dòng)脫下，輸送至驗(yàn)布車間，減少了對勞動(dòng)力的需求，從而降低運(yùn)營成本。落布車的自主運(yùn)動(dòng)主要是指根據(jù)預(yù)定的目標(biāo)軌跡及落布車的狀態(tài)和紡織車間里的環(huán)境信息，自動(dòng)控制落布車到達(dá)指定的目標(biāo)點(diǎn)。此外，軌跡跟蹤控制要求被控對象在給定的時(shí)間內(nèi)到達(dá)指定的目標(biāo)點(diǎn)，其目的是確保車輛遵循預(yù)定路徑，選擇最合適的速度并最大限度地減少跟蹤誤差。由于其獨(dú)特的布局和生產(chǎn)方式，自動(dòng)落布車在織造車間的軌跡跟蹤控制方法亟待探討。

常用的軌跡跟蹤控制算法大多采用比例積分微分（Proportional-Integral-Derivative，PID）控制[1-2]，滑膜控制[3-4]，或神經(jīng)網(wǎng)絡(luò)控制[5-6]，但這些方法高度依賴于參數(shù)和環(huán)境，因此適應(yīng)性不強(qiáng)。Chein等[7]提出了一種軌跡跟蹤算法，用于汽車類機(jī)器人輸出速度和轉(zhuǎn)向命令，從而使跟蹤誤差和控制器工作量最小化;楊勇生等[8]提出的基于Lyapunov第二方法的自動(dòng)導(dǎo)引車軌跡跟蹤控制器，采用真實(shí)的樣車參數(shù)模擬直線和圓弧軌跡;Geng等[9]提出了一種新的容錯(cuò)模型預(yù)測控制（Model Predictive Control，MPC）算法，用于自主車輛的魯棒路徑跟蹤控制，通過構(gòu)建目標(biāo)函數(shù)，考慮前輪偏角和輪胎側(cè)偏的動(dòng)態(tài)約束，設(shè)計(jì)了車輛橫向運(yùn)動(dòng)控制的線性時(shí)變模型預(yù)測控制算法;Liu等[10]提出將自動(dòng)導(dǎo)向車（Automated Guided Vehicle，AGV）的角速度和縱向速度的誤差校正作為耦合估計(jì)誤差，將耦合估計(jì)誤差和改進(jìn)的純跟蹤算法相結(jié)合作為AGV小車的橫向控制，同時(shí)采用PID控制作為縱向控制，進(jìn)一步減小誤差干擾。MPC具有明確的基本思想和強(qiáng)大的發(fā)展?jié)摿?，它可以與各種系統(tǒng)模型、控制理論和優(yōu)化算法相結(jié)合，形成各種控制算法[11-13]。此外，落布車是由機(jī)械和電氣部件組成的，也受到物理限制，因此，必須考慮落布車的非線性特性及其與車間環(huán)境的相互作用[14-15]。

本文針對自主設(shè)計(jì)的雙舵輪自動(dòng)落布車，考慮紡織車間里復(fù)雜的道路環(huán)境，對比不同階次B樣條曲線規(guī)劃的軌跡及曲率，最終選擇7次B樣條在織機(jī)之間生成平滑的換道軌跡[16-17]，并分析了落布車的雙舵輪車體結(jié)構(gòu)，建立其運(yùn)動(dòng)模型及在車間里的定位算法，然后結(jié)合控制系統(tǒng)設(shè)計(jì)基于模型預(yù)測的自動(dòng)落布車糾偏控制器，控制器根據(jù)落布車軌跡與目標(biāo)軌跡之間的位置偏差和角度偏差，在每個(gè)采樣時(shí)刻進(jìn)行滾動(dòng)優(yōu)化，并實(shí)時(shí)調(diào)整落布車航向角和速度。最后，采用Matlab軟件仿真落布車糾偏控制過程，與傳統(tǒng)PID控制算法進(jìn)行比較，驗(yàn)證模型預(yù)測控制的實(shí)時(shí)性和魯棒性。

1自動(dòng)落布車在織機(jī)間的換道路徑規(guī)劃

由于紡織車間里獨(dú)特的織機(jī)布局和生產(chǎn)模式，導(dǎo)致落布車在織機(jī)間的換道軌跡需要更精確地規(guī)劃，而B樣條[18-19]曲線具有局部支撐性、凸包性和變差縮減性等優(yōu)點(diǎn)，可以調(diào)整控制點(diǎn)靈活地配置曲線幾何形狀，適用于落布車在狹窄的織機(jī)間換道的局部軌跡規(guī)劃。在二維空間中給定n+1個(gè)控制點(diǎn)P，P，…，P和一個(gè)節(jié)點(diǎn)向量U=[u，u，…，u]，這些控制點(diǎn)和節(jié)點(diǎn)向量用于定義樣條曲線的極限范圍，所以K階B樣條曲線的定義為：

B-spline曲線對規(guī)劃的軌跡影響很大，因?yàn)榍€的調(diào)整點(diǎn)越少，計(jì)算時(shí)間越快，但選擇得太少可能會(huì)偏離實(shí)際路線，也會(huì)導(dǎo)致落布車與織機(jī)相互碰撞的路線，因此分別用3、5、7次B樣條曲線生成的落布車換道軌跡曲線如圖1所示。

對B樣條曲線進(jìn)行不同階次特征分析，令換道過程中兩臺織機(jī)之間的距離為落布車車道寬d，本文中取值為1 m，P點(diǎn)為車道的中點(diǎn)，即為0.5d，縱向位移X設(shè)定為在落布車恒定車速換道時(shí)間下的縱向行駛距離?？傻没?次、5次和7次B樣條曲線規(guī)劃的變道軌跡曲線及對應(yīng)的曲線曲率，如圖2所示。

從圖2可以看出，雖然3次或5次B樣條曲線可以在指定的位置之間生成一條可行的平滑路徑，但在初始和終止位置處生成的軌跡的曲率值不為0，使得自動(dòng)落布車在實(shí)際行駛中難以遵循規(guī)劃的預(yù)期軌跡，且5次B樣條曲率的最大值達(dá)到4.5×10。而7次B樣條生成的軌跡更接近于車道的中點(diǎn)，基于7次B樣條曲線在起始點(diǎn)和終止點(diǎn)的曲率值能完全滿足為0的條件，雖然相同條件下7次B樣條規(guī)劃的路徑最大曲率比3次B樣條略大，但最大值2.8×10仍遠(yuǎn)小于4.5×10，即能充分滿足落布車在兩臺織機(jī)間的換道動(dòng)作。因此，采用7次B樣條曲線來規(guī)劃落布車在紡織車間的換道軌跡。

由于B樣條曲線的形狀由選取的坐標(biāo)點(diǎn)決定，因此將基于7次B樣條曲線的理想換道軌跡規(guī)劃問題轉(zhuǎn)化為數(shù)學(xué)優(yōu)化問題求9個(gè)坐標(biāo)點(diǎn)。通過優(yōu)化求解得到落布車對應(yīng)換道需求下的最優(yōu)換道軌跡，這是解決更貼近紡織車間里實(shí)際換道情況問題的關(guān)鍵，同時(shí)，也為實(shí)現(xiàn)目標(biāo)點(diǎn)的動(dòng)態(tài)搜索提供了可能。由式（3）可知，自動(dòng)落布車換道路徑曲率的連續(xù)性要求B樣條曲線是二階連續(xù)的，且規(guī)劃曲線兩端的節(jié)點(diǎn)具有重復(fù)度K，得到B樣條曲線的節(jié)點(diǎn)向量U=[0，0，0，0，0，0，0，0.25，05，0.75，1，1，1，1，1，1，1]。

因此，本文選擇圖1（c）所示的7次B樣條曲線規(guī)劃自動(dòng)落布車換道路徑，如圖3所示。P=（P，P）（i=0，1，…，8）為7次B樣條曲線的控制點(diǎn)，d為兩臺織機(jī)之間的寬度，L≥0，…，L≥0，這些均為決定控制點(diǎn)的待優(yōu)化坐標(biāo)參數(shù)。

圖3建立地面坐標(biāo)系OXY，則B樣條曲線的控制點(diǎn)P=（P，P）（i=0，1，…，8）可表示為：

2自動(dòng)落布車運(yùn)動(dòng)學(xué)模型分析

自動(dòng)落布車的運(yùn)動(dòng)系統(tǒng)是由兩個(gè)舵輪和兩個(gè)萬向輪組成，前后兩個(gè)舵輪分別安裝在落布車前進(jìn)方向軸線上。落布車主要控制舵輪在車間內(nèi)實(shí)現(xiàn)直線和轉(zhuǎn)彎換道運(yùn)動(dòng)，使其能按照預(yù)設(shè)的路線行進(jìn)，且主要是通過動(dòng)態(tài)調(diào)整落布車的兩個(gè)舵輪來實(shí)現(xiàn)運(yùn)動(dòng)。因此，在運(yùn)動(dòng)學(xué)建模分析不影響的情況下，將自動(dòng)落布車簡化為前后兩個(gè)驅(qū)動(dòng)輪的運(yùn)動(dòng)模型[20]。假設(shè)落布車車體為剛性，移動(dòng)速度較低，水平運(yùn)動(dòng)時(shí)兩個(gè)驅(qū)動(dòng)輪受力相同，則忽略輪子寬度保證驅(qū)動(dòng)輪同心轉(zhuǎn)動(dòng)。

自動(dòng)落布車的運(yùn)動(dòng)學(xué)模型如圖4所示，每臺舵輪驅(qū)動(dòng)帶動(dòng)一個(gè)驅(qū)動(dòng)輪，通過兩個(gè)舵輪的舵角不斷調(diào)節(jié)，落布車可以完成直線與曲線路徑的軌跡跟蹤任務(wù)。

圖4建立平面直角坐標(biāo)系XOY，兩個(gè)驅(qū)動(dòng)輪與地面的接觸點(diǎn)分別為O、O;f表示前輪，r表示后輪;兩輪到質(zhì)心點(diǎn)C的距離分別為a和b;設(shè)落布車車身坐標(biāo)系為xCy，前后輪的行進(jìn)速度分別為V、V，行進(jìn)速度與車身坐標(biāo)系X軸的夾角分別為α、αr;R表示轉(zhuǎn)彎半徑，φ表示落布車航向角。

2.1自動(dòng)落布車定位算法

如圖4所示，設(shè)自動(dòng)落布車的車體幾何中心為前后兩個(gè)舵輪中點(diǎn)O與O_r的連線中點(diǎn)，在落布車均勻負(fù)載的情況下，認(rèn)為落布車的車體質(zhì)心即為幾何中心，表示為C=（X，Y）。α、α分別表示為兩個(gè)舵輪的轉(zhuǎn)角角度;V、V分別表示為兩個(gè)舵輪的線速度;O點(diǎn)表示為落布車轉(zhuǎn)彎時(shí)的瞬心;R、R分別表示兩個(gè)舵輪轉(zhuǎn)彎時(shí)的軌跡半徑。因此，兩個(gè)驅(qū)動(dòng)輪的位置坐標(biāo)則可表示為：

考慮落布車的剛體運(yùn)動(dòng)特性，落布車?yán)@瞬心點(diǎn)O運(yùn)動(dòng)時(shí)，兩個(gè)驅(qū)動(dòng)輪的中心點(diǎn)及落布車質(zhì)心點(diǎn)C運(yùn)動(dòng)時(shí)的角速度是一致的，因此結(jié)合驅(qū)動(dòng)輪運(yùn)動(dòng)時(shí)的速度和航向角，根據(jù)正弦定理求得落布車的角速度為：

3自動(dòng)落布車糾偏控制

由于自動(dòng)落布車系統(tǒng)是非線性高耦合系統(tǒng)，且在紡織車間狹小的過道環(huán)境約束下及落布車車體的機(jī)動(dòng)性能限制下，很難通過直接求解運(yùn)動(dòng)學(xué)方程獲得精確解析，因此，考慮在線獲取帶約束優(yōu)化問題的數(shù)值解，簡化求解過程，滿足自動(dòng)落布車實(shí)時(shí)控制的系統(tǒng)要求。

3.1自動(dòng)落布車軌跡跟蹤控制誤差模型

假設(shè)利用B樣條規(guī)劃的換道軌跡是自動(dòng)落布車的目標(biāo)軌跡，并且給出了落布車在任意時(shí)刻的狀態(tài)量和控制量，那么就可以通過落布車實(shí)際與目標(biāo)軌跡之間的偏差來進(jìn)行跟蹤控制。為使落布車能跟蹤規(guī)劃好的軌跡運(yùn)行，可建立目標(biāo)軌跡狀態(tài)空間表達(dá)式：

模型預(yù)測控制需要在每個(gè)采樣時(shí)間求解一個(gè)優(yōu)化問題，因此需要預(yù)先設(shè)置性能指標(biāo)函數(shù)。通過控制優(yōu)化量，可以獲得最大值或最小值。性能指標(biāo)函數(shù)通常采用二次函數(shù)，正常的控制目標(biāo)是使輸出預(yù)測值盡可能接近目標(biāo)值。本文涉及的軌跡跟蹤控制屬于輸出最優(yōu)控制問題，其性能函數(shù)表示為：

式（24）顯示的是無約束優(yōu)化，通過加權(quán)矩陣Q和R可以在一定程度上抑制預(yù)測輸出和控制輸入的波動(dòng)。然而，控制變量不能被精確地約束。在實(shí)際過程中，施加控制量的幅度或其增量過大，會(huì)對系統(tǒng)造成重大影響，甚至影響系統(tǒng)的穩(wěn)定性。所以，對于式（22）需要考慮控制變量的邊界約束，即：

4仿真試驗(yàn)與結(jié)果分析

為了驗(yàn)證模型預(yù)測控制器用于自動(dòng)落布車的有效性，本文采用自主設(shè)計(jì)的自動(dòng)落布車進(jìn)行跟蹤試驗(yàn)，使用PLC搭建的控制系統(tǒng)，如圖5所示。在Matlab中建立車輛的運(yùn)動(dòng)學(xué)模型，并在Simulink中建立控制器。模型控制器是基于當(dāng)前位置模擬多個(gè)輸出，優(yōu)化器找到最優(yōu)的。在獲得最佳控制序列后，控制器將第一步應(yīng)用于落布車。然后，它使用式（22）計(jì)算下一時(shí)刻的位置，式（12）將其用作新優(yōu)化的起點(diǎn)，它在這個(gè)過程中一直重復(fù)直到跟蹤控制完成。

自動(dòng)落布車在紡織車間里的運(yùn)動(dòng)軌跡分為直線和B樣條規(guī)劃的兩種路徑，其中，落布車在車間里進(jìn)行換道軌跡時(shí)的穩(wěn)定跟蹤能力是軌跡跟蹤算法魯棒性的重要體現(xiàn)。為了驗(yàn)證在模型預(yù)測下自動(dòng)落布車的軌跡跟蹤效果，本文選取的目標(biāo)軌跡為直線路徑和B樣條規(guī)劃的曲線換道路徑，并將MPC控制器和傳統(tǒng)PID控制器分別應(yīng)用于自動(dòng)落布車，比較軌跡跟蹤效果。

4.1落布車在直線軌跡下的仿真對比試驗(yàn)

圖6和圖7分別展示了落布車在直線下，用MPC控制器和傳統(tǒng)PID控制器進(jìn)行軌跡跟蹤的對比效果。從圖6可以看出，落布車在運(yùn)行時(shí)就能快速并穩(wěn)定地進(jìn)行直線目標(biāo)軌跡的跟蹤，但PID控制器到達(dá)穩(wěn)定的時(shí)間較長且運(yùn)行過程中有很大的抖動(dòng)。由圖7可以知道，隨著被控落布車速度和航向的實(shí)時(shí)調(diào)整，距離誤差和角度誤差迅速減小，在6 s時(shí)實(shí)現(xiàn)精確跟蹤后誤差保持在0;傳統(tǒng)PID控制器則需要15 s才能趨于穩(wěn)定，穩(wěn)態(tài)誤差也相比MPC更大。綜合可以看出，MPC的軌跡跟蹤要更快更穩(wěn)定。

4.2落布車在換道軌跡下的仿真對比試驗(yàn)

圖8和圖9分別展示了在規(guī)劃的B樣條換道曲線下，用MPC和PID控制器進(jìn)行軌跡跟蹤的仿真對比效果。如圖8所示，在MPC控制器下落布車能很快地開始跟蹤目標(biāo)軌跡，即使在換道拐彎的位置有輕微的一點(diǎn)偏差，也不影響后續(xù)目標(biāo)的穩(wěn)定軌跡跟蹤，完成落布車在紡織車間里的轉(zhuǎn)彎;相對于PID控制器來說，自動(dòng)落布車在換道軌跡跟蹤時(shí)就出現(xiàn)了較大幅度的超調(diào)，會(huì)使自動(dòng)落布車在拐彎時(shí)與織機(jī)相撞，影響車間工作效率。如圖9所示，落布車運(yùn)動(dòng)一段時(shí)間后位置與角度偏差在10 s左右收斂到0，基于PID控制器的落布車需要

在20 s才能達(dá)到穩(wěn)定狀態(tài)，且穩(wěn)態(tài)誤差相較于MPC較大。因此在用B樣條規(guī)劃的軌跡下，MPC控制器相比于傳統(tǒng)的PID控制器能夠滿足落布車穩(wěn)定的跟蹤目標(biāo)軌跡，實(shí)現(xiàn)在紡織車間里的拐彎換道，且具有較高的實(shí)時(shí)性和魯棒性。

5結(jié)語

針對紡織車間里自動(dòng)落布車的跟蹤控制問題，本文先利用7次B樣條曲線規(guī)劃落布車在紡織車間里的平滑拐彎換道軌跡，再建立落布車雙舵輪的轉(zhuǎn)向運(yùn)動(dòng)模型。同時(shí)針對自動(dòng)落布車設(shè)計(jì)了基于模型預(yù)測控制的軌跡跟蹤算法，選取小車的線速度和角速度作為控制變量，利用預(yù)期與實(shí)際路徑的誤差加入邊界約束，使其能快速穩(wěn)定地完成軌跡跟蹤任務(wù)，提高工作效率。最后通過Matlab仿真驗(yàn)證MPC軌跡跟蹤算法的有效性與可行性，以直線和B樣條規(guī)劃的轉(zhuǎn)彎換道路徑為目標(biāo)軌跡，與傳統(tǒng)PID控制器進(jìn)行對比，結(jié)果表明模型預(yù)測控制算法更具有優(yōu)勢，使落布車的軌跡跟蹤能快速趨于穩(wěn)定且跟蹤精度更好。

《絲綢》官網(wǎng)下載中國知網(wǎng)下載

參考文獻(xiàn)：

[1]劉金琨. 先進(jìn)PID控制Matlab仿真[M]. 北京： 電子工業(yè)出版社， 2016.LIU Jinkun. Advanced PID Control Matlab Simulation[M]. Beijing： Electronic Industry Press， 2016.

[2]ROVIRA-MAS F， ZHANG Q. Fuzzy logic control of an electrohydraulic valve for auto-steering off-road vehicles[J]. Proceedings of the Institution of Mechanical， 2008， 222（6）： 917-934.

[3]XIA Y Q， LU K F， ZHU Z， et al. Adaptive-back-stepping sliding mode attitude control of missile systems[J]. International Journal of Robust & Nonlinear Control， 2013， 23（15）： 1699-1717.

[4]姜立標(biāo)， 楊杰. 基于滑?？刂频淖詣?dòng)泊車系統(tǒng)路徑跟蹤研究[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)， 2019， 50（2）： 356-364.JIANG Libiao， YANG Jie. Path tracking of automatic parking system based on sliding mode control[J]. Transactions of the Chinese Society for Agricultural Machinery， 2019， 50（2）： 356-364.

[5]范其明， 呂書豪. 移動(dòng)機(jī)器人的自適應(yīng)神經(jīng)網(wǎng)絡(luò)滑?？刂芠J]. 控制工程， 2017， 24（7）： 1409-1414.FAN Qiming， L Shuhao. Adaptive neural network sliding mode control of mobile robots[J]. Control Engineering of China， 2017， 24（7）： 1409-1414.

[6]JAZAR R. Mathematical theory of autodriver for autonomous vehicles[J]. Journal of Vibration & Control， 2010， 16（2）： 253-279.

[7]CHEIN F A， SCAGLIA G. Trajectory tracking controller design for unmanned vehicles： A new methodology[J]. Journal of Field Robotics， 2014， 31（6）： 861-887.

[8]楊勇生， 趙宏， 姚海慶. 基于Lyapunov第二方法的自動(dòng)導(dǎo)引車軌跡跟蹤控制器設(shè)計(jì)與仿真[J]. 上海海事大學(xué)學(xué)報(bào)， 2019， 40（2）： 73-77.YANG Yongsheng， ZHAO Hong， YAO Haiqing. Design and simulation of trajectory tracking controller for automated guided vehicles based on Lyapunov’s second method[J]. Journal of Shanghai Maritime University， 2019， 40（2）： 73-77.

[9]GENG K K， CHULIN N A， WANG Z W. Fault-tolerant model predictive control algorithm for path tracking of autonomous vehicle[J]. Sensors， 2020， 20（15）： 4245.

[10]LIU Y Q， JIN G H， LIU X， etal. An improved hybrid error control path tracking intelligent algorithm for omnidirectional AGV on ROS[J]. International Journal of Computer Applications in Technology， 2020， 64（2）： 115-125.

[11]張家旭， 楊雄， 施正堂， 等. 汽車緊急換道避障的路徑規(guī)劃與跟蹤控制[J]. 華南理工大學(xué)學(xué)報(bào)（自然科學(xué)版）， 2020， 48（9）： 86-93.ZHANG Jiaxu， YANG Xiong， SHI Zhengtang， et al. Path planning and tracking control for emergency lane change and obstacle avoidance of vehicles[J]. Journal of South China University of Technology （Natural Science Edition）， 2020， 48（9）： 86-93.

[12]丁森， 瞿文燕. 雙驅(qū)動(dòng)AGV轉(zhuǎn)彎分析與優(yōu)化[J]. 計(jì)算機(jī)光盤軟件與應(yīng)用， 2013， 16（4）： 203-204.DING Sen， QU Wenyan. Analysis and optimization of dual-drive AGV turning[J]. Computer CD Software and Applications， 2013， 16（4）： 203-204.

[13]朱曉祥， 陳乃軍， 殷邦革. AGV路徑糾偏控制器設(shè)計(jì)與分析[J]. 科技創(chuàng)新與應(yīng)用， 2017（12）： 62-63.ZHU Xiaoxiang， CHEN Naijun， YIN Bangge. Design and analysis of AGV path correction controller[J]. Technology Innovation and Application， 2017（12）： 62-63.

[14]王殿君， 關(guān)似玉， 陳亞， 等. 雙驅(qū)雙向AGV機(jī)器人運(yùn)動(dòng)學(xué)分析及仿真[J]. 制造業(yè)自動(dòng)化， 2016， 38（3）： 42-46.WANG Dianjun， GUAN Siyu， CHEN Ya， et al. Kinematic analysis and simulation of double-drive and double-direction AGV robot[J]. Manufacturing Automation， 2016， 38（3）： 42-46.

[15]劉金珠， 李穎新， 羅丹， 等. 基于智能技術(shù)打造無人化數(shù)字棉紡工廠[J]. 紡織導(dǎo)報(bào)， 2014（1）： 47-51.LIU Jinzhu， LI Yingxin， LUO Dan， et al. Building unmanned digital cotton spinning mill based on intelligent technology[J]. China Textile Leader， 2014（1）： 47-51.

[16]施法中. 計(jì)算機(jī)輔助幾何設(shè)計(jì)與非均勻有理B樣條[M]. 北京： 高等教育出版社， 2001.SHI Fazhong. Computer-Aided Geometric Design and Non-Uniform Rational B-Spline[M]. Beijing： Higher Education Press， 2001.

[17]張永華， 杜煜， 潘峰， 等. 基于三次B樣條曲線擬合的智能車軌跡跟蹤算法[J]. 計(jì)算機(jī)應(yīng)用， 2018， 38（6）： 1562-1567.ZHANG Yonghua， DU Yu， PAN Feng， et al. Intelligent vehicle path tracking algorithm based on cubic B-spline curve fitting[J]. Journal of Computer Applications， 2018， 38（6）： 1562-1567.

[18]陳成， 何玉慶， 卜春光， 等. 基于四階貝塞爾曲線的無人車可行軌跡規(guī)劃[J]. 自動(dòng)化學(xué)報(bào)， 2015， 41（3）： 486-496.CHEN Cheng， HE Yuqing， BU Chunguang， et al. Feasible trajectory generation for autonomous vehicles based on quartic Bezier curve[J]. Acta Automatica Sinica， 2015， 41（3）： 486-496.

[19]AHMED F， DEB K. Multi-objective path planning using spline representation[C]//IEEE International Conference on Robotics and Biomimetics. New York： Institute of Electricle and Electronics Engineers， 2011.

[20]齊嘉暉， 吳耀華， 汪威. 雙舵輪自動(dòng)導(dǎo)引車軌跡追蹤控制算法[J]. 科學(xué)技術(shù)與工程， 2020， 20（20）： 8252-8260.QI Jiahui， WU Yaohua， WANG Wei. Trajectory tracking control algorithm for double-steering automated guided vehicle[J]. Science Technology and Engineering， 2020， 20（20）： 8252-8260.

Automatic plaiting vehicle trajectory tracking control algorithmWANG Yu SHEN Danfeng WANG Rongjun LI Yaojie LI Jingyu（1.School of Mechanical and Electrical Engineering， Xi’an Polytechnic University， Xi’an 710048， China;

2.Qingdao Haijia Machinery Co.， Ltd.， Qingdao 266000， China）

Abstract： China’s textile industry already accounts for more than 50% of the world， and the textile and clothing market is growing at an accelerated pace， playing an important role in market forecasting， expanding exports， increasing employment and farmers’ income and promoting urbanisation， but in China， the textile industry is a highly labour-intensive and externally dependent industry. Intelligent manufacturing is one of the important means for the transformation and modernization of China’s textile industry and for the long-term dominance of China’s textile industry in the world. After 2018， the textile machinery industry has been further promoting industrial upgrading， achieving certain development in many aspects such as intelligence by means of new efficiency， low energy consumption， flexibility， automation and digitalization. The majority of existing workshop logistics systems， consisting of conveyor belts， human vehicles， etc.， are not seamlessly integrated between upstream and downstream production processes， resulting in low logistics and transport efficiency， which directly affects the efficiency of the entire production system.

At present most of weaving workshops use manual plaiting， which requires a lot of physical labour. Due to the low efficiency of manual plaiting， it is easy to miss some plaiting tasks， and there is an urgent need to replace people with machines in the process of plaiting and adopt an automated method of plaiting. An automatic plaiting vehicle applicable for the water jet loom production workshop is designed combined with the practical requirements， the hardware and software systems of the vehicle are completed， and the trajectory tracking of the vehicle in the process of movement is studied in depth to achieve precise navigation and movement control of the vehicle during its work， conveying its full cloth rolls to the storage position. To address the problem of lane changing trajectory planning and tracking control of plaiting vehicles in textile workshops， a fast and efficient B-sample-based trajectory planning with obstacle avoidance is proposed， and a model predictive control trajectory tracking controller for plaiting vehicles is designed. The B-sample curve is used to plan the lane changing trajectory of the plaiting vehicle in the textile workshop. The structure of the body is analyzed and its kinematic model is established to find out the positioning algorithm of the plaiting vehicle in the textile workshop and to reduce the tracking error and random disturbance in the complex environment by using MPC considering the motion performance of the body and environmental constraints. Finally， the simulation is carried out by Matlab software to verify that MPC is able to track the reference trajectory stably within the given feasible distance and angle deviation， and the trajectory tracking algorithm is compared with the traditional PID control. It proves that the trajectory tracking algorithm using model predictive control has stable and good tracking performance.

The automatic plaiting vehicle has been designed and developed for the transport of full fabric rolls on water jet weaving machines， but there are still many shortcomings as far as we can see. The investigated trajectory tracking of the plaiting vehicle is done in a known environment， but special situations cannot be excluded in the textile workshop， and further verification and improvement of the algorithms on the automatic vehicle are needed to optimize and improve the control system functions of the vehicle.

Key words： automatic plaiting vehicle; B-spline; model prediction; PID control; trajectory tracking; Matlab simulation