摘"要：軌跡跟蹤控制是自主導(dǎo)引車（AGV）的重要組成部分。為提高軌跡跟蹤的快速性和穩(wěn)定性，根據(jù)牛頓歐拉方程建立AGV的二自由度動(dòng)力學(xué)方程，據(jù)此建立誤差模型，設(shè)計(jì)基于AGV動(dòng)力學(xué)模型的線性時(shí)變模型預(yù)測(cè)控制算法，建立以狀態(tài)增量和控制增量為變量的優(yōu)化函數(shù)，對(duì)其添加狀態(tài)量約束、控制量約束和控制增量約束，同時(shí)增加軟約束動(dòng)態(tài)，通過仿真對(duì)設(shè)計(jì)的控制器進(jìn)行分析驗(yàn)證，結(jié)果證明了控制器的有效性。

關(guān)鍵詞：自動(dòng)導(dǎo)引車；軌跡跟蹤控制；模型預(yù)測(cè)控制

中圖分類號(hào)：U461""文獻(xiàn)標(biāo)志碼：B""文章編號(hào)：1671-5276（2024）02-0243-04

Research on Lateral Trajectory Tracking of Four-wheel Drive AGV

WANG Yujie1，CHEN Yuan2

（1. School of Mechanical Engineering，Nanjing University of Science and Technology，Nanjing 210094，China；

2. Jiangsu Jinling Institute of Intelligent Manufacturing Co.， Ltd.， Nanjing 210006，China）

Abstract：Track tracking control is an important part of automated guided vehicle （AGV）. In order to improve the rapidity and stability of trajectory tracking， a two degree of freedom dynamic equation of AGV is established according to Newton Euler equation， and an error model is established accordingly. A linear time-varying model predictive control algorithm based on AGV dynamic model is designed， and an optimization function with state increment and control increment as variables is established. The constraints of state quantity， control quantity and control increment as well as soft constraint dynamics are added. The simulation results verify the effectiveness of the controller.

Keywords：automated guided vehicle；trajectory tracking control；model predictive control

0"引言

自動(dòng)導(dǎo)引車（automated guided vehicle，AGV）是一種無人操作的自動(dòng)化搬運(yùn)機(jī)器人，已被廣泛應(yīng)用于倉儲(chǔ)運(yùn)輸、醫(yī)療、軍工等多個(gè)領(lǐng)域［1］。由于傳統(tǒng)工業(yè)朝著自動(dòng)化、智能化方向發(fā)展，因而對(duì)AGV的需求量也隨之增長［2］。因此，對(duì)AGV的軌跡跟蹤控制進(jìn)行研究對(duì)AGV的自主導(dǎo)航具有很重要的意義。

目前國內(nèi)外學(xué)者對(duì)AGV的軌跡跟蹤控制進(jìn)行了大量的研究。楊李朋等［3］提出了改進(jìn)的模糊PID方法，在模糊PID控制的基礎(chǔ)上， 設(shè)計(jì)出單純形法目標(biāo)函數(shù)自動(dòng)尋優(yōu)與模糊PID控制相結(jié)合。朱時(shí)杰等［4］設(shè)計(jì)了基于線性自抗擾控制（LADRC）的軌跡跟蹤器，解決易陷入局部最優(yōu)的問題。然而，現(xiàn)在常用的控制方法在進(jìn)行預(yù)測(cè)跟蹤時(shí)，存在因計(jì)算量龐大難以滿足實(shí)時(shí)控制要求的問題，同時(shí)因?yàn)樵趦?yōu)化求解過程中未對(duì)變量進(jìn)行合理約束，無法穩(wěn)定精確地跟蹤。因而需針對(duì)上述問題進(jìn)行研究。

本文提出了基于AGV動(dòng)力學(xué)模型的線性時(shí)變模型預(yù)測(cè)控制算法。首先，建立了AGV動(dòng)力學(xué)模型，然后在此基礎(chǔ)上建立了AGV軌跡跟蹤誤差模型，同時(shí)采用泰勒公式進(jìn)行線性化處理，采用向前歐拉法進(jìn)行離散化，建立以狀態(tài)增量和控制增量為變量的優(yōu)化函數(shù)，將其轉(zhuǎn)化為二次規(guī)劃問題。對(duì)優(yōu)化函數(shù)添加狀態(tài)量約束、控制量約束和控制增量約束，在AGV精準(zhǔn)跟蹤參考軌跡的基礎(chǔ)上提高了軌跡跟蹤的穩(wěn)定性。

1"車輛動(dòng)力學(xué)模型

本文研究的AGV采用八輪四驅(qū)動(dòng)結(jié)構(gòu)設(shè)計(jì)，前后輪實(shí)現(xiàn)獨(dú)立驅(qū)動(dòng)轉(zhuǎn)向，沿車體軸線方向均勻?qū)ΨQ分布4個(gè)萬向輪，提供支撐和平衡作用。基于該平臺(tái)，為提高控制算法的實(shí)時(shí)性和穩(wěn)定性，在較為準(zhǔn)確地描述車輛動(dòng)力學(xué)過程的基礎(chǔ)上，對(duì)車輛模型進(jìn)行適當(dāng)簡化，因此建立二自由度動(dòng)力學(xué)模型，如圖1所示。其中，XOY是以大地坐標(biāo)系為參考的全局坐標(biāo)系，xO′y是以AGV自身軸線為參考的車輛坐標(biāo)系，車輛坐標(biāo)系原點(diǎn)在AGV質(zhì)心處，x軸為車身軸線方向，y軸與x軸垂直，vx和vy是車輛坐標(biāo)系中質(zhì)心處的縱橫速度，φ 是橫擺角，m是車輛質(zhì)量，Iz是質(zhì)心繞z軸的轉(zhuǎn)動(dòng)慣量。

根據(jù)牛頓第二定律建立車輛動(dòng)力學(xué)模型：

mx··=mφ·+2（Fl， fcosδf－Fc， fsinδf）+2（Fl，rcosδr－Fc， fsinδr）

my··=－mx·φ·+2（Fl， fcosδf+

Fc， fsinδf）+2（Fl，rcosδr+Fc，rsinδr）

Izφ··=2lf（Fl， fcosδf+Fc， fsinδf）－

2lr（Fl，rcosδr+Fc，rsinδr）

X·=x·cosφ－y·sinφ

Y·=x·sinφ－y·cosφ（1）

在小角度假設(shè)條件下，三角函數(shù)滿足：

cosθ≈1，sinθ≈θ，tanθ≈θ

式中計(jì)算時(shí)的車輪轉(zhuǎn)角、側(cè)偏角均為小角度。

基于小角度假設(shè)，前/后輪輪胎側(cè)偏角可表示為

αf=δf－y·+lfφ·vx

αr=lrφ·－y·vx（2）

假設(shè)低滑移率情況，輪胎側(cè)向力和輪胎縱向力可以表達(dá)為如下線性形式。

Fc， f=Cc， fαf，F(xiàn)c，r=Cc，rαr

Fl， f=Cl， fsf，F(xiàn)l，r=Cl，rsr（3）

式中：Cc， f和Cc，r分別為車輛前/后輪側(cè)偏剛度；αf和αr分別為前/后輪側(cè)偏角；Cl， f和Cl，r分別為前/后輪縱向剛度；sf和sr分別為前/后輪滑移率。

將式（1）表示成如下狀態(tài)方程的形式

ξ·=f（ξ，u）（4）

式中：系統(tǒng)狀態(tài)量ξ=x·，y·，φ，φ·，X，YT；控制量u=δf。

2"線性時(shí)變模型預(yù)測(cè)控制器設(shè)計(jì)

根據(jù)上述車輛動(dòng)力學(xué)方程，提出線性時(shí)變模型預(yù)測(cè)控制器，該控制器由預(yù)測(cè)模型、在線優(yōu)化和反饋校正組成。相較于非線性模型預(yù)測(cè)控制器，本文對(duì)原有預(yù)測(cè)模型進(jìn)行線性化和離散化處理，同時(shí)增加狀態(tài)增量約束，大大提高了控制的實(shí)時(shí)性和穩(wěn)定性。

2.1"軌跡跟蹤誤差模型

圖1所示的車輛動(dòng)力學(xué)模型為非線性系統(tǒng)，將其進(jìn)行線性化處理。對(duì)于給定的期望參考軌跡，滿足式（5），用ref表示參考量，期望軌跡的狀態(tài)量和控制量滿足下式：

ξ·ref=f（ξref，uref）（5）

對(duì)式（4）在期望參考軌跡處進(jìn)行泰勒級(jí)數(shù)一階展開，得到線性化方程：

ξ∧·=A（t）ξ∧（t）+B（t）u∧（t）（6）

但由于經(jīng)線性化后的式（6）是連續(xù)方程，不能直接用于MPC，因此需對(duì)該方程采用向前歐拉法進(jìn)行離散化［5］處理，得到離散化狀態(tài)空間方程式（7）：

ξ∧（k+1）=Ak，t（k）ξ∧（k）+Bk，t（k）u∧（k）+dk，t（7）

為避免出現(xiàn)控制量突變和在計(jì)算中出現(xiàn)無可行解的情況，在這里構(gòu)建新的狀態(tài)向量χ（k|t），帶入式（7），得到包含Δu（k）新的狀態(tài)空間表達(dá)式：

χ（k+1|t）=A～χ（k|t）+B～Δu（k|t）+d（k|t）（8）

在車輛對(duì)參考軌跡進(jìn)行軌跡跟蹤時(shí)，軌跡跟蹤精度和車輛橫向穩(wěn)定性都是很重要的。因此在軌跡跟蹤中將橫擺角和橫向位置作為輸出量，得到預(yù)測(cè)輸出方程：

η（k|t）=C～χ（k|t）（9）

根據(jù)模型預(yù)測(cè)控制設(shè)計(jì)思路將狀態(tài)量和控制量帶入離散預(yù)測(cè)輸出方程中，可得到未來一段時(shí)間的輸出量，推導(dǎo)如下：

k+1時(shí)刻的預(yù)測(cè)方程為

η（k+1）=C～χ（k+1）（10）

將式（8）帶入式（10）中，得η（k+1）=C～Aχ（k）+C～Bχ（k）+C～d（k）

以此類推，可得到在k，k+Np時(shí)刻內(nèi)的預(yù)測(cè)方程，將推導(dǎo)得到的預(yù)測(cè)方程以矩陣的形式表達(dá)，可以得到系統(tǒng)的離散預(yù)測(cè)輸出表達(dá)式：

Y（k）=Ψχ（k）+ΘΔU（k）+ΩD（k）（11）

式中ΔU（k）=［Δu（k）"Δu（k+1）"…"Δu（k+Nc）］T

基于上式，可以求解系統(tǒng)當(dāng)前的狀態(tài)量χ（k）和控制增量ΔU（k），以獲得預(yù)測(cè)時(shí)域Np內(nèi)的狀態(tài)量和輸出量，從而實(shí)現(xiàn)預(yù)測(cè)功能。

2.2"目標(biāo)優(yōu)化函數(shù)

在軌跡跟蹤過程中，為了使車輛平穩(wěn)精確地跟蹤期望軌跡，需得到最優(yōu)控制量，而在控制時(shí)域內(nèi)的最優(yōu)控制量只能通過求解目標(biāo)函數(shù)得到。若在設(shè)定目標(biāo)函數(shù)時(shí)只考慮對(duì)控制量進(jìn)行約束，不能控制控制量的跳變，這樣會(huì)在跟蹤時(shí)車身不穩(wěn)，因而應(yīng)將控制增量作為優(yōu)化函數(shù)狀態(tài)量。同時(shí)，由于車輛動(dòng)力學(xué)模型的復(fù)雜度較高，易出現(xiàn)沒有最優(yōu)解的情況，因而在目標(biāo)函數(shù)中加入松弛因子。為了方便計(jì)算，本文將目標(biāo)優(yōu)化函數(shù)設(shè)定為如下二次規(guī)劃形式：

J（ξ∧（k），u（k－1），Δu（k））=

∑Npi=1y（k+ik）－yref（k+ik）Q2+∑Nc－1i=1Δu（k+ik）R2+ρε2（12）

式中：Np表示預(yù)測(cè)時(shí)域；Nc表示控制時(shí)域；Q和R分別是輸出量和控制量變化的加權(quán)矩陣；ρ是權(quán)重因子；ε是松弛因子；y（k+ik）是控制預(yù)測(cè)輸出值；yref（k+ik）是參考預(yù)測(cè)輸出值；Δu（k+ik）是控制增量。

在實(shí)際的系統(tǒng)中，考慮到車輛執(zhí)行機(jī)構(gòu)有一定的輸出范圍，需對(duì)系統(tǒng)的狀態(tài)量、控制量添加約束條件：

ξmin（k）≤ξ（k）≤ξmax（k）（13）

umin（k）≤u（k）≤umax（k）（14）

為了保證車輛能夠平穩(wěn)跟蹤期望軌跡，需使控制量穩(wěn)定變化，本文增加對(duì)控制增量進(jìn)行約束：

Δumin（k）≤Δu（k）≤Δumax（k）（15）

為了方便預(yù)測(cè)時(shí)域里狀態(tài)量和輸出量的求解，將式（11）帶入式（12）目標(biāo)優(yōu)化函數(shù)中，定義Yref（k）為參考輸出，可以得到如下優(yōu)化方程表達(dá)式：

J=［Y（k）－Yref（k）］TQ［Y（k）－Yref（k）］+

ΔU（k）TRΔU（k）+ρε2（16）

為簡化計(jì)算，將其化為標(biāo)準(zhǔn)二次型形式：

J=12［ΔU（k）T，ε］TH（k）［ΔU（k）T，ε］+G（k）［ΔU（k）T，ε］（17）

由于式（13）—式（15）的約束方程中，約束對(duì)象不統(tǒng)一，為將約束條件的變量統(tǒng)一，可將約束轉(zhuǎn)化為如下形式：

Ymin（k）－ε≤Y（k）≤Ymax（k）+ε

Umin（k）≤AΔU（k）+U（k）≤Umax（k）（18）

在每個(gè)周期完成優(yōu)化求解后，可以得到Np內(nèi)的控制輸入增量和松弛因子。將控制增量序列里的第一個(gè)控制增量作用于系統(tǒng)，獲得當(dāng)前的最優(yōu)控制量：

u（k|k）=u（k－1|k）+Δu*（k|k）（19）

為了保證車輛能夠平穩(wěn)跟蹤期望軌跡，需使控制量穩(wěn)定變化，本文對(duì)控制增量添加約束：

ΔUmin（k）≤ΔU（k）≤Umax（k）（20）

3"仿真分析

為了驗(yàn)證算法的有效性，對(duì)軌跡跟蹤控制進(jìn)行了仿真實(shí)驗(yàn)。在AGV實(shí)際運(yùn)行過程中， Np=20，Nc=5，ρ=1 000。

未對(duì)控制增量進(jìn)行約束時(shí)的仿真結(jié)果如圖2—圖4所示。從圖中可以看出在跟蹤的過程中，實(shí)際軌跡偏離了目標(biāo)參考軌跡，因?yàn)樵谀繕?biāo)優(yōu)化函數(shù)優(yōu)化求解時(shí)，出現(xiàn)了控制量的突變，因而軌跡跟蹤的曲線會(huì)產(chǎn)生凸起躍變，AGV在跟蹤過程中會(huì)產(chǎn)生較頻繁晃動(dòng)，跟蹤效果不好。

對(duì)控制增量進(jìn)行約束后的仿真結(jié)果如圖5—圖7所示。從圖中可以看出，當(dāng)對(duì)控制增量進(jìn)行限制后，改進(jìn)后的軌跡跟蹤算法能平穩(wěn)精準(zhǔn)地跟蹤參考軌跡，有效地改善了跟蹤效果。

為驗(yàn)證該控制算法對(duì)行駛速度具有良好的魯棒性，將不同車速下的仿真結(jié)果進(jìn)行對(duì)比分析控制效果，如圖8、圖9所示（本刊黑白印刷，相關(guān)疑問咨詢作者）。從圖中可以看出當(dāng)AGV以5m/s、10m/s、15m/s的速度對(duì)參考軌跡都能進(jìn)行較好地跟蹤，只是在彎道部分與參考軌跡會(huì)有些許偏差，但最后都能將偏差收斂為0，驗(yàn)證了該控制器在不同速度下都有良好的跟蹤效果。

4"結(jié)語

本文以AGV二自由度動(dòng)力學(xué)模型為基礎(chǔ)，建立模型的狀態(tài)空間方程，在模型預(yù)測(cè)控制的框架下建立了軌跡跟蹤誤差模型，構(gòu)造了二次規(guī)劃方程形式的目標(biāo)優(yōu)化函數(shù)。根據(jù)AGV機(jī)械特性限制，在文獻(xiàn)控制算法的基礎(chǔ)上，對(duì)控制增量進(jìn)行約束，有效地改進(jìn)了軌跡跟蹤性能，使AGV能夠精準(zhǔn)穩(wěn)定地跟蹤參考軌跡。最后，通過仿真對(duì)該控制器進(jìn)行了驗(yàn)證。為將來軌跡跟蹤控制研究提供了參考。

參考文獻(xiàn)：

［1］ 王浩吉，楊永帥，趙彥微. 重載AGV的應(yīng)用現(xiàn)狀及發(fā)展趨勢(shì)［J］. 機(jī)器人技術(shù)與應(yīng)用，2019（5）：20-24.

［2］ 張滬松，樓佩煌，朱立群，等. 基于組合趨近律的差速AGV高精度路徑跟蹤技術(shù)研究［J］. 機(jī)械制造與自動(dòng)化，2019，48（6）：186-190.

［3］ 楊李朋，張文豐，王昊. 基于改進(jìn)模糊PID的AGV軌跡跟蹤研究［J］. 工業(yè)控制計(jì)算機(jī)，2017，30（8）：117-119.

［4］ 朱時(shí)杰，郭青. 基于線性自抗擾的AGV軌跡跟蹤控制器設(shè)計(jì)及參數(shù)優(yōu)化［J］. 北京化工大學(xué)學(xué)報(bào)（自然科學(xué)版），2017，44（4）：95-100.

［5］ 劉豹，唐萬生. 現(xiàn)代控制理論［M］. 3版. 北京：機(jī)械工業(yè)出版社，2006.

收稿日期：20220922