基于危險斥力場的車間AGV 掛接路徑規劃算法研究

郭泉成，梁家強，李 塬，徐潤坤

（廣州鐵路職業技術學院機車車輛學院，廣州 510430）

自動導航車輛牽引車要實現物料的運輸，通常需要通過車鉤等裝置掛接料車。在研究車間AGV 物流車的導航算法中，避障路徑規劃一直是關注的焦點，并且是實現車間安全物流運輸的前提條件之一[1]。路徑規劃可以定義為滿足避障和邊界等約束條件下，尋找從起點到終點的達到某種最優性能指標的路徑。解決這類問題，最優控制算法是一種重要的方法。該算法的基本思想是將實際物理問題轉化為數學模型，并利用優化問題的數學框架進行求解。根據障礙物建模的不同方法，最優控制算法可以分為柵格圖法、幾何刻畫法和人工勢場法[2]。柵格圖法將環境信息分解成具有屬性信息的柵格，其本質是建立一個多像素的地圖，可以描述環境中的車道線、靜態障礙物和動態障礙物等元素[3]。然而，柵格太稀疏會導致求解路徑連續性差，柵格太密集則需要較大的存儲空間，求解過程中搜索時間較長，降低算法效率[4]。幾何刻畫法基于簡單的幾何圖形邊界約束來描述車輛、障礙物和道路之間的位置關系，常常使用直線、雙曲線、圓和橢圓等函數表征幾何圖形邊界。一種適應性較強的基于P 準則的幾何刻畫法可以近似大多數封閉圖形，但其函數參數包含高次冪和高次開方，使用該方法對障礙物進行建模可能導致更困難的數值求解過程[2]。

而人工勢場法將道路、靜態障礙物和動態障礙物抽象為具有引力或斥力的勢能場，并以勢能最小為性能目標來規劃最優路徑[5]。這種方法在1985 年便由Khatib 提出[6]，經過不斷完善，人工勢場法在路徑規劃研究中比較成熟，并具有實時性的特點[7]。借鑒人工勢場法的基本思想，類比萬有引力定律，采用危險斥力場對車間AGV 與料車掛接工況中的道路和障礙物進行建模，并進一步使用高斯偽譜法來求解最優路徑。通過仿真實驗，驗證了基于危險斥力場的車間AGV 掛接路徑規劃算法能夠規劃出一條安全無碰撞的路徑。

1 車間AGV 物流牽引車動力學建模

車間內部各工位具有高度規則和結構化的特點，所有工位均采用顏色鮮明的道路線分割功能區。檢修車間AGV 牽引車運轉與車間內部，一個常見的典型工況就是從工位的道路區域，經過指定的進出口，進入某檢修工位，掛接好檢修工位需要運輸的料車，運輸至指定地點。

將以上工況抽象建模，考慮以下場景，如圖1 所示，檢修工位處于檢修車間矩形框范圍內，四周布置規整的車間道路線，在右上方留出一個供物料和檢修人員進出的門口。其中1 為AGV 牽引車，2 為料車位置，3 為AGV車鉤公口，4 為料車車鉤母口。AGV 牽引車掛接路徑規劃即需要從起點運行到車口公口和母口對齊狀態，為掛接做好準備。AGV 牽引車與料車掛接完成之后，可拖動行駛至目標位置。

圖1 車間AGV 掛接工況示意圖

選擇阿克曼轉向AGV 作為車輛模型。因為在自動導航牽引車掛接過程中，為了實現更平穩的車鉤掛接，需要保持運行的平穩性，并且速度要求較慢。因此，可以近似認為車輛在工作過程中處于線性區域，沒有側傾和俯仰運動等其他自由度的動力學特性。在牽引車慢速掛接過程的假設條件下，使用線性二自由度動力學模型對AGV進行建模[8]，如圖2 所示。該模型的數學表達式為

圖2 AGV 動力學模型

式中：x，y，θ 是狀態變量，分別表示后輪心橫坐標、后輪心縱坐標、航向角；δ 是控制變量，為轉向角度。

2 危險斥力場建模

在車輛行駛過程中，必須避免碰撞，以確保安全地從起點到達終點。在車間AGV 掛接過程中，車間道路線是一類不能侵占、不能越過的障礙物。此外，在進入檢修場地后，檢修工位上放置了各種檢修工裝設備，并有工作人員在進行操作，因此檢修工位也是不可侵占的障礙物。

參照萬有引力公式的假設，假設危險斥力的大小與2 個質點之間的距離的平方成反比。對于檢修工位而言，以其幾何中心作為質點的坐標，并可以用以下公式來描述其對車間AGV 的斥力

式中：Fa為斥力，Ka為斥力系數，（x，y）為車輛后輪輪心坐標，（xa，ya）為檢修工作幾何中心。通過這個斥力模型，可以計算出車間AGV 在行駛過程中與檢修工位之間的斥力大小。

而對于車間道路線線段，AGV 與其的危險斥力可以看成AGV 質點與道路線上離散的每個質點之間的斥力之和，如下式所示

式中：Fb為道路線對AGV 的總斥力，Kb為斥力系數，（x，y）為車輛后輪輪心坐標，（xbi，ybi）為道路線上第i個點的坐標，n為離散處理的點的個數。

通過以上的斥力模型，能夠計算車間AGV 與檢修工位以及車間道路線之間的斥力大小，從而在路徑規劃過程中避免與這些障礙物發生碰撞，確保車間AGV 在掛接過程中的安全行駛，并遵循道路線和檢修工位的限制。

3 最優控制問題建模與求解

3.1 最優控制問題建模

在基于危險斥力場的AGV 掛接路徑規劃問題中，目標是找到一條滿足約束條件的最安全路徑，即危險斥力最小的快速路徑，可以將這個問題看作一個最優控制問題。最優控制問題的目標是確定優化性能指標的狀態變量和控制變量，同時滿足系統的物理和動態約束[9]。在數學上，一個最優控制問題可以用Bolza 形式來表示，形式如下。

優化目標，即最小化性能指標

性能函數由兩部分組成，其中Mayer 項φ，拉格朗日項g。

滿足動力學約束，即微分方程

和邊界條件，即初值和末值約束

以及滿足路徑約束，包含狀態變量和控制變量的路徑約束

式中：x（t）∈Rn是狀態變量，u（t）∈Rn是控制變量，同時時間t∈[t0，tf]。

最小化性能指標定義為“最安全”的快速路徑。“最安全”通過整個過程中的危險斥力積分最小表征，“快速”則使用t作為Mayer 項，性能指標兼顧了安全性和快速性。用以下式子描述

式中：Fa和Fb分別由式（2）和式（3）給出。k為時間系數，調整k值可以調整性能指標中安全性和快速性兩者的權重，以達到均衡狀態。

動力學約束即為AGV 線性二自由度動力學模型，見式（1）。

邊界條件主要有起點邊界、過程邊界和終點邊界。

在本工況中，根據設定坐標值，狀態變量初值約束為

狀態變量末值約束為

狀態變量路徑約束為

考慮AGV 最大輪胎角度為45°，控制變量路徑約束為

綜上所述，基于危險斥力場的車間AGV 掛接路徑規劃問題建模為

3.2 高斯偽譜法求解最優控制問題

接下來使用高斯偽譜法對最優控制問題進行數值求解，求解的流程如圖3 所示。首先，需要對優化區間進行變換，因為高斯偽譜法的插值區間通常在-1 至1。因此，需要對時間進行區間變換，將其映射到-1 至1 的范圍內。

圖3 高斯偽譜法數值求解流程

然后，使用拉格朗日基函數對優化目標和所有約束條件進行插值，對問題進行離散近似處理。通過這一步驟，將微分狀態方程轉化為代數方程，從而將離散化后的最優控制問題轉化為一個整體求解的非線性規劃問題。在求解過程中，可以借助成熟的序列二次規劃法（SNOPT）求解器，對離散化后的問題進行求解。該求解器能夠有效地求解非線性規劃問題，從而得到最優解。

高速偽譜法的具體求解過程可參照參考文獻[10]。通過應用高斯偽譜法，能夠得到最優控制問題的數值解，從而得到最優的AGV 掛接路徑規劃方案。該解確定了滿足系統的所有約束條件的變量和控制變量，同時讓性能指標達到最優化狀態。

4 結果仿真分析

對車間AGV 掛接路徑規劃進行了求解和仿真實驗分析。在配置為Intel i5，主頻為2.4 GHz，內存4 G 的Windows10 系統下，利用MATLAB 求解的時間為3.15 s，求解時精度設置為0.01 m。實驗得到的主要參數，隨時間變化的曲線，過程仿真如圖4、圖5 所示，其中各物理量的意義如下。

圖4 AGV 掛接過程仿真示意圖

圖5 各狀態變量和控制變量的時間變化曲線

（x，y）為車輛后輪軸中心在世界坐標系下的橫縱坐標值；ν 為車輛縱向速度；θ 為車輛航向角（車輛車頭方向與x軸的夾角，逆時針為正）；δ 為前輪輪胎轉角；以上參數中，δ 是控制變量x，y，θ 是狀態變量。

通過觀察圖4 中的求解結果，可以看到規劃路徑成功地實現了從起點到終點的行駛任務，并在終點處與料車對齊，為下一步的車鉤掛接做好了準備。該規劃路徑巧妙地避開了道路線和檢修工位等障礙物，從而確保了AGV 的安全行駛。

通過觀察速度的時間變化曲線，可以了解AGV 車輛倒車行駛的過程，曲線顯示AGV 經歷了一個勻加速到勻減速的過程，并在總共用時7.6 s 內完成了該過程。通過觀察航向角的時間變化曲線，可知AGV 經歷了初始狀態的0 角度到末值狀態的0 角度變化，最終狀態的0 角度即水平位置，剛好與料車的水平狀態對齊，這是進一步掛接，實現車鉤公口母口對接的重要條件。

這進一步驗證了基于危險斥力場的車間AGV 掛接路徑規劃算法的有效性。通過應用危險斥力場和最優控制問題的數學模型，并借助高斯偽譜法進行求解，能夠求出實現AGV 在車間環境中的安全導航和順利掛接的路徑。

5 結論

借鑒人工勢場法和萬有引力定律，使用危險斥力場對車間自動導航小車掛接工況中的道路和障礙物建立模型；考慮到AGV 慢速行駛的特點，選擇線性二自由度動力學模型對車輛建模，進一步分析AGV 掛接過程中的約束條件，采用危險斥力場積分最小的“最安全”及兼顧快速的性能指標作為優化目標，建立最優控制問題數學模型。進一步使用高斯偽譜法進行求解，求解結果及仿真分析表明基于危險斥力場的車間AGV 掛接路徑規劃算法能夠規劃出一條安全無碰撞的路徑。算法具有良好的性能和效果，其能夠充分考慮車輛與障礙物之間的安全距離，避免碰撞風險，同時保證了路徑的快速性。算法能夠應對復雜的車間環境，并在實際應用中具有一定的可行性和實用性。

研究還存在一些局限性和不足之處，需要進一步地改進和完善。危險斥力場模型的選擇和參數設置對最終的路徑規劃結果具有一定的影響，需要進一步研究和優化參數選擇的方法。在未來的研究中，可以進一步拓展基于危險斥力場的車間AGV 掛接路徑規劃算法的應用范圍，深入探索和改進該算法，以滿足不斷變化的需求和挑戰。