非完整約束AGV運動控制系統設計及軟件實現

王偉峰，楊 斌，周 濤

（上海神添實業有限公司，上海 200438）

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

本文提出了一種非完整約束AGV的運動控制平臺設計方法。首先，建立了非完整約束AGV的數學模型；其次，簡要介紹了AGV運動平臺的系統組成及原理；最后詳細介紹了LabVIEW平臺下AGV運動控制系統的軟件實現及試驗結果。

1 非完整約束AGV及其數學模型

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

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

下面針對單舵輪結構的非完整約束AGV建立數學模型。

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

如圖1所示建立坐標系，車體轉彎圓弧對應圓心為前后輪速度垂線的交點O，存在幾何關系如下：

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

AGV后輪線速度為

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

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

因此，結合速度分解可得單舵輪結構的非完整約束AGV運動學模型為

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

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

2 運動平臺的系統組成及原理

本文中單舵輪AGV的驅動方式為前輪驅動，AGV前輪作為驅動輪同時由前進、轉向2個電機驅動；2個后輪作為從動輪，沒有動力輸出。

AGV運動控制平臺主要由運動控制器、DA轉換卡、電機驅動器、伺服電機、位姿測量傳感器以及超聲波傳感器等組成，其系統組成如圖2所示。

圖2 AGV運動控制平臺系統組成Fig.2 Composition of AGV motion control platform

AGV運動控制器是用miniPC實現的，操作系統為Windows系統，軟件開發環境為LabVIEW 2014，其主要作用是傳感器信號接收解析、控制器算法實現以及控制量數字信號輸出。DA轉換卡的作用是將控制器輸出的數字量轉換成電機驅動器可以直接識別的模擬量，其與運動控制器的通信接口為RS485串口。電機驅動器包括Curtis1232E前進電機驅動器和Curtis1220轉向電機驅動器，分別驅動AGV前輪的前進和轉向動作。

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

3 運動控制系統的軟件設計與實現

3.1 軟件結構以及前面板設計

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

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

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

規劃路徑跟蹤模塊是實現AGV自動導引的主要程序，包括路徑規劃程序和相鄰目標點位置跟蹤控制程序。傳感器信號接收處理模塊主要涉及導航雷達和超聲波傳感器的信號解算。狀態監測模塊主要采集監測AGV傳感器、驅動器上傳的各類信息。系統參數配置模塊主要設置車體尺寸、電機減速比等參數。

3.2 RRT路徑規劃程序設計

路徑規劃是指移動機器人根據某一性能指標（如距離、時間、能量等）搜索一條從起始狀態到目標狀態的最優或次優路徑[4]。快速擴展隨機樹（rapidly-exploring random trees，RRT）算法是路徑規劃算法的一種，尤其適合解決動態、復雜環境下的路徑規劃問題[5]。

若已知起始節點qstart、目標節點qend以及全局或局部地圖信息，RRT算法生長步驟具體過程如下：

（1）選擇 qstart為根節點，tree=qstart；

（2）在地圖中隨機選擇采樣點qrand；

（3）搜索當前tree上離qrand最近的樹節點qnearest，連接qrand和qnearest，以固定步長step在連線上選取新節點qnew；

（4）對qnearest與qnew連線進行障礙物碰撞檢測，若無碰撞則將qnew作為qnearest的子節點加入tree，若有碰撞則tree無擴展發生；

（5）循環步驟1到步驟4直到有節點與qend間距小于設定閾值，即可獲得從qstart到qend的一條可達路徑。

RRT路徑規劃算法中隨機樹擴散生長的核心LabVIEW代碼如圖4所示。

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

由于非完整約束AGV車體結構本身的限制，當規劃路徑步長固定時，其在實際運動過程中會受到輪子偏角范圍及車體轉彎半徑的約束。本文在上述RRT算法步驟4的基礎上加入非完整約束的兩個限定條件：

式中：αmax為AGV驅動輪的最大偏轉范圍；δ為車體轉彎半徑限制常數。

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

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

3.3 相鄰目標點位置跟蹤控制程序設計

路徑規劃算法的輸出路徑可表示為路徑曲線上一系列離散坐標點的集合。AGV的路徑跟蹤可分解為連續多個從當前點位姿到相鄰目標點位姿的伺服控制過程[6]。

AGV從當前點位姿運動到相鄰目標點位姿是靠驅動輪的轉向和前進完成的。AGV車體當前位姿、目標位姿與驅動輪偏角α及驅動輪行進位移sf之間的關系如式（8）所示，因此可以根據式（8）將AGV的相鄰點位姿分解到驅動輪的偏角α及行進位移sf，進而轉化為對驅動輪的控制，其控制系統框圖如圖6所示。

圖6 相鄰目標點位置跟蹤控制方框圖Fig.6 Block diagram of position tracking control for adjacent target points

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

圖7為相鄰目標點位置跟蹤控制的LabVIEW程序代碼，車體模型解算輸出的驅動輪轉角經系數變換直接賦值給轉角控制的寄存器變量；車體模型解算輸出的驅動輪前進距離作為位置環PID控制器的參考輸入。位置環PID的輸出經系數變換后賦值給速度控制寄存器。

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

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

4 結語

本文建立了非完整約束AGV的數學模型，將其模型約束作為路徑規劃的輸入條件，提出了一種基于模型的RRT路徑規劃算法并設計了RRT算法的LabVIEW軟件。此外，設計了非完整約束AGV的路徑跟蹤控制系統，在LabVIEW環境下進行了實現并取得了理想的跟蹤效果，證明了本文非完整約束AGV運動控制系統的設計合理性和工程實用性。