非完整約束AGV運(yùn)動控制系統(tǒng)設(shè)計(jì)及軟件實(shí)現(xiàn)

王偉峰，楊 斌，周 濤

（上海神添實(shí)業(yè)有限公司，上海 200438）

隨著制造業(yè)的高速發(fā)展，特別是“工業(yè)4.0”的概念提出以來，圍繞智能工廠、智能生產(chǎn)及智能物流三大核心主題的產(chǎn)業(yè)技術(shù)研究得到了國內(nèi)外學(xué)者的廣泛關(guān)注。自動導(dǎo)引運(yùn)輸車（automated guided vehicle，簡稱AGV）是一種無人駕駛搬運(yùn)車，它可以依照車載傳感器確定的位置信息，自動跟蹤預(yù)先規(guī)劃的行駛路線，完成一系列作業(yè)功能[1]。AGV在生產(chǎn)制造及物流方面的應(yīng)用極大地提高了企業(yè)的生產(chǎn)效率、降低了企業(yè)的生產(chǎn)成本。

本文提出了一種非完整約束AGV的運(yùn)動控制平臺設(shè)計(jì)方法。首先，建立了非完整約束AGV的數(shù)學(xué)模型；其次，簡要介紹了AGV運(yùn)動平臺的系統(tǒng)組成及原理；最后詳細(xì)介紹了LabVIEW平臺下AGV運(yùn)動控制系統(tǒng)的軟件實(shí)現(xiàn)及試驗(yàn)結(jié)果。

1 非完整約束AGV及其數(shù)學(xué)模型

非完整約束AGV作為移動機(jī)器人的一種，其運(yùn)動控制系統(tǒng)往往受到外部環(huán)境的相關(guān)約束，此類系統(tǒng)稱為受限系統(tǒng)。對于一般的受限系統(tǒng)[2]：式中：x∈Rn為系統(tǒng)狀態(tài)變量；u∈Rn為系統(tǒng)控制變量；t為時間；H（x，，t）為約束。

若存在函數(shù) G（x，t）（不為常數(shù)）使得式（3）成立，則稱系統(tǒng)是完整系統(tǒng)，對應(yīng)的約束為完整約束；反之，則稱為非完整約束。

下面針對單舵輪結(jié)構(gòu)的非完整約束AGV建立數(shù)學(xué)模型。

圖1 單舵輪AGV示意Fig.1 Single rudder wheel AGV schematic diagram

如圖1所示建立坐標(biāo)系，車體轉(zhuǎn)彎圓弧對應(yīng)圓心為前后輪速度垂線的交點(diǎn)O，存在幾何關(guān)系如下：

式中：L為AGV車體前后輪距；R為后輪所在位置的瞬時轉(zhuǎn)彎半徑；α為舵輪偏角。

AGV后輪線速度為

式中：Vr為AGV 后輪速度；Vf為前輪速度；ψ、ω 分別為車體運(yùn)動的航向角度和航向角速度。

由式（4）、式（5）可得車體航向角速度：

因此，結(jié)合速度分解可得單舵輪結(jié)構(gòu)的非完整約束AGV運(yùn)動學(xué)模型為

在微小時間周期T內(nèi)進(jìn)行積分，可得T時刻和T+1時刻的車體位姿與前輪行進(jìn)距離及偏角的遞推模型：

式中：sf為車體位姿從（x（kT），y（kT），ψ（kT））運(yùn)動到（x（（k+1）T），y（（k+1）T），ψ（（k+1）T））時的前輪行進(jìn)距離。

2 運(yùn)動平臺的系統(tǒng)組成及原理

本文中單舵輪AGV的驅(qū)動方式為前輪驅(qū)動，AGV前輪作為驅(qū)動輪同時由前進(jìn)、轉(zhuǎn)向2個電機(jī)驅(qū)動；2個后輪作為從動輪，沒有動力輸出。

AGV運(yùn)動控制平臺主要由運(yùn)動控制器、DA轉(zhuǎn)換卡、電機(jī)驅(qū)動器、伺服電機(jī)、位姿測量傳感器以及超聲波傳感器等組成，其系統(tǒng)組成如圖2所示。

圖2 AGV運(yùn)動控制平臺系統(tǒng)組成Fig.2 Composition of AGV motion control platform

AGV運(yùn)動控制器是用miniPC實(shí)現(xiàn)的，操作系統(tǒng)為Windows系統(tǒng)，軟件開發(fā)環(huán)境為LabVIEW 2014，其主要作用是傳感器信號接收解析、控制器算法實(shí)現(xiàn)以及控制量數(shù)字信號輸出。DA轉(zhuǎn)換卡的作用是將控制器輸出的數(shù)字量轉(zhuǎn)換成電機(jī)驅(qū)動器可以直接識別的模擬量，其與運(yùn)動控制器的通信接口為RS485串口。電機(jī)驅(qū)動器包括Curtis1232E前進(jìn)電機(jī)驅(qū)動器和Curtis1220轉(zhuǎn)向電機(jī)驅(qū)動器，分別驅(qū)動AGV前輪的前進(jìn)和轉(zhuǎn)向動作。

AGV的傳感器主要包括電機(jī)編碼器、室內(nèi)定位雷達(dá)以及超聲波傳感器。前進(jìn)、轉(zhuǎn)向兩路電機(jī)編碼器輸出的A、B、Z三相脈沖信號經(jīng)由雙通道編碼器采集卡采集計(jì)算得到兩路電機(jī)的行進(jìn)速度和距離，實(shí)時傳輸給運(yùn)動控制器實(shí)現(xiàn)電機(jī)轉(zhuǎn)動的閉環(huán)反饋。室內(nèi)定位雷達(dá)采用Sick公司的Nav350型導(dǎo)航雷達(dá)，可以測量AGV車體在絕對世界系內(nèi)的實(shí)時位姿（xt，yt，ψt），并傳輸給運(yùn)動控制器實(shí)現(xiàn)車體位姿的閉環(huán)反饋。超聲波傳感器的功能是通過測量與障礙物的間距實(shí)現(xiàn)防撞功能。

3 運(yùn)動控制系統(tǒng)的軟件設(shè)計(jì)與實(shí)現(xiàn)

3.1 軟件結(jié)構(gòu)以及前面板設(shè)計(jì)

本文中的AGV運(yùn)動控制軟件是在LabVIEW開發(fā)環(huán)境下實(shí)現(xiàn)的，LabVIEW是NI公司研發(fā)的一種圖形化語言開發(fā)平臺，在信號采集與控制方面具有豐富的資源支持，用戶可以根據(jù)自己的需求靈活且高效地設(shè)計(jì)測量或控制系統(tǒng)[3]。AGV運(yùn)動控制程序主要包括：遙控模塊、規(guī)劃路徑跟蹤模塊、傳感器信號接收處理模塊、狀態(tài)監(jiān)測模塊以及系統(tǒng)參數(shù)配置模塊。軟件前面板設(shè)計(jì)如圖3所示。

圖3 AGV運(yùn)動控制LabVIEW軟件前面板Fig.3 Front panel of AGV control LabVIEW software

AGV的運(yùn)動控制包括遙控模式控制和規(guī)劃路徑跟蹤控制2種模式。轉(zhuǎn)向電機(jī)驅(qū)動器Curtis1220內(nèi)部設(shè)計(jì)有角度伺服控制的位置環(huán)、速度環(huán)及電流環(huán)，前進(jìn)電機(jī)驅(qū)動器Curtis1232E內(nèi)部設(shè)計(jì)有行進(jìn)速度控制的速度環(huán)及電流環(huán)。因此，AGV的遙控模塊僅需通過前面板界面的驅(qū)動輪角度、速度控件將數(shù)字量通過DA轉(zhuǎn)換卡傳輸給驅(qū)動器即可。

規(guī)劃路徑跟蹤模塊是實(shí)現(xiàn)AGV自動導(dǎo)引的主要程序，包括路徑規(guī)劃程序和相鄰目標(biāo)點(diǎn)位置跟蹤控制程序。傳感器信號接收處理模塊主要涉及導(dǎo)航雷達(dá)和超聲波傳感器的信號解算。狀態(tài)監(jiān)測模塊主要采集監(jiān)測AGV傳感器、驅(qū)動器上傳的各類信息。系統(tǒng)參數(shù)配置模塊主要設(shè)置車體尺寸、電機(jī)減速比等參數(shù)。

3.2 RRT路徑規(guī)劃程序設(shè)計(jì)

路徑規(guī)劃是指移動機(jī)器人根據(jù)某一性能指標(biāo)（如距離、時間、能量等）搜索一條從起始狀態(tài)到目標(biāo)狀態(tài)的最優(yōu)或次優(yōu)路徑[4]?？焖贁U(kuò)展隨機(jī)樹（rapidly-exploring random trees，RRT）算法是路徑規(guī)劃算法的一種，尤其適合解決動態(tài)、復(fù)雜環(huán)境下的路徑規(guī)劃問題[5]。

若已知起始節(jié)點(diǎn)qstart、目標(biāo)節(jié)點(diǎn)qend以及全局或局部地圖信息，RRT算法生長步驟具體過程如下：

（1）選擇 qstart為根節(jié)點(diǎn)，tree=qstart；

（2）在地圖中隨機(jī)選擇采樣點(diǎn)qrand；

（3）搜索當(dāng)前tree上離qrand最近的樹節(jié)點(diǎn)qnearest，連接qrand和qnearest，以固定步長step在連線上選取新節(jié)點(diǎn)qnew；

（4）對qnearest與qnew連線進(jìn)行障礙物碰撞檢測，若無碰撞則將qnew作為qnearest的子節(jié)點(diǎn)加入tree，若有碰撞則tree無擴(kuò)展發(fā)生；

（5）循環(huán)步驟1到步驟4直到有節(jié)點(diǎn)與qend間距小于設(shè)定閾值，即可獲得從qstart到qend的一條可達(dá)路徑。

RRT路徑規(guī)劃算法中隨機(jī)樹擴(kuò)散生長的核心LabVIEW代碼如圖4所示。

圖4 隨機(jī)樹生長LabVIEW代碼Fig.4 Random tree growth LabVIEW code

由于非完整約束AGV車體結(jié)構(gòu)本身的限制，當(dāng)規(guī)劃路徑步長固定時，其在實(shí)際運(yùn)動過程中會受到輪子偏角范圍及車體轉(zhuǎn)彎半徑的約束。本文在上述RRT算法步驟4的基礎(chǔ)上加入非完整約束的兩個限定條件：

式中：αmax為AGV驅(qū)動輪的最大偏轉(zhuǎn)范圍；δ為車體轉(zhuǎn)彎半徑限制常數(shù)。

將車體模型條件加入RRT生長步驟4以約束隨機(jī)樹新節(jié)點(diǎn)的生長，可以得到適用于本文非完整約束AGV的RRT路徑規(guī)劃算法。圖5為得到RRT路徑規(guī)劃結(jié)果后調(diào)用Matlab的Plot函數(shù)繪制的路徑曲線。

圖5 模型約束RRT路徑規(guī)劃曲線Fig.5 Model constrained RRT path planning curve

3.3 相鄰目標(biāo)點(diǎn)位置跟蹤控制程序設(shè)計(jì)

路徑規(guī)劃算法的輸出路徑可表示為路徑曲線上一系列離散坐標(biāo)點(diǎn)的集合。AGV的路徑跟蹤可分解為連續(xù)多個從當(dāng)前點(diǎn)位姿到相鄰目標(biāo)點(diǎn)位姿的伺服控制過程[6]。

AGV從當(dāng)前點(diǎn)位姿運(yùn)動到相鄰目標(biāo)點(diǎn)位姿是靠驅(qū)動輪的轉(zhuǎn)向和前進(jìn)完成的。AGV車體當(dāng)前位姿、目標(biāo)位姿與驅(qū)動輪偏角α及驅(qū)動輪行進(jìn)位移sf之間的關(guān)系如式（8）所示，因此可以根據(jù)式（8）將AGV的相鄰點(diǎn)位姿分解到驅(qū)動輪的偏角α及行進(jìn)位移sf，進(jìn)而轉(zhuǎn)化為對驅(qū)動輪的控制，其控制系統(tǒng)框圖如圖6所示。

圖6 相鄰目標(biāo)點(diǎn)位置跟蹤控制方框圖Fig.6 Block diagram of position tracking control for adjacent target points

圖7 位置跟蹤控制LabVIEW代碼Fig.7 Location tracking control LabVIEW code

圖7為相鄰目標(biāo)點(diǎn)位置跟蹤控制的LabVIEW程序代碼，車體模型解算輸出的驅(qū)動輪轉(zhuǎn)角經(jīng)系數(shù)變換直接賦值給轉(zhuǎn)角控制的寄存器變量；車體模型解算輸出的驅(qū)動輪前進(jìn)距離作為位置環(huán)PID控制器的參考輸入。位置環(huán)PID的輸出經(jīng)系數(shù)變換后賦值給速度控制寄存器。

將圖5所示的路徑曲線作為跟蹤路徑進(jìn)行試驗(yàn)，路徑起點(diǎn)為（315 cm，1573 cm），路徑終點(diǎn)為（605 cm，510 cm），跟蹤路徑步長step選擇為30 cm。AGV運(yùn)動過程中的實(shí)際測量位置與理論坐標(biāo)位置曲線如圖8所示。根據(jù)路徑跟蹤曲線可知，本文設(shè)計(jì)的非完整約束AGV取得了理想的路徑跟蹤效果。

圖8 AGV路徑跟蹤效果曲線Fig.8 AGV path tracking effect curve

4 結(jié)語

本文建立了非完整約束AGV的數(shù)學(xué)模型，將其模型約束作為路徑規(guī)劃的輸入條件，提出了一種基于模型的RRT路徑規(guī)劃算法并設(shè)計(jì)了RRT算法的LabVIEW軟件。此外，設(shè)計(jì)了非完整約束AGV的路徑跟蹤控制系統(tǒng)，在LabVIEW環(huán)境下進(jìn)行了實(shí)現(xiàn)并取得了理想的跟蹤效果，證明了本文非完整約束AGV運(yùn)動控制系統(tǒng)的設(shè)計(jì)合理性和工程實(shí)用性。