基于危險(xiǎn)斥力場(chǎng)的車間AGV 掛接路徑規(guī)劃算法研究

郭泉成，梁家強(qiáng)，李 塬，徐潤坤

（廣州鐵路職業(yè)技術(shù)學(xué)院機(jī)車車輛學(xué)院，廣州 510430）

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

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

1 車間AGV 物流牽引車動(dòng)力學(xué)建模

車間內(nèi)部各工位具有高度規(guī)則和結(jié)構(gòu)化的特點(diǎn)，所有工位均采用顏色鮮明的道路線分割功能區(qū)。檢修車間AGV 牽引車運(yùn)轉(zhuǎn)與車間內(nèi)部，一個(gè)常見的典型工況就是從工位的道路區(qū)域，經(jīng)過指定的進(jìn)出口，進(jìn)入某檢修工位，掛接好檢修工位需要運(yùn)輸?shù)牧宪嚕\(yùn)輸至指定地點(diǎn)。

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

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

選擇阿克曼轉(zhuǎn)向AGV 作為車輛模型。因?yàn)樵谧詣?dòng)導(dǎo)航牽引車掛接過程中，為了實(shí)現(xiàn)更平穩(wěn)的車鉤掛接，需要保持運(yùn)行的平穩(wěn)性，并且速度要求較慢。因此，可以近似認(rèn)為車輛在工作過程中處于線性區(qū)域，沒有側(cè)傾和俯仰運(yùn)動(dòng)等其他自由度的動(dòng)力學(xué)特性。在牽引車慢速掛接過程的假設(shè)條件下，使用線性二自由度動(dòng)力學(xué)模型對(duì)AGV進(jìn)行建模[8]，如圖2 所示。該模型的數(shù)學(xué)表達(dá)式為

圖2 AGV 動(dòng)力學(xué)模型

式中：x，y，θ 是狀態(tài)變量，分別表示后輪心橫坐標(biāo)、后輪心縱坐標(biāo)、航向角；δ 是控制變量，為轉(zhuǎn)向角度。

2 危險(xiǎn)斥力場(chǎng)建模

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

參照萬有引力公式的假設(shè)，假設(shè)危險(xiǎn)斥力的大小與2 個(gè)質(zhì)點(diǎn)之間的距離的平方成反比。對(duì)于檢修工位而言，以其幾何中心作為質(zhì)點(diǎn)的坐標(biāo)，并可以用以下公式來描述其對(duì)車間AGV 的斥力

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

而對(duì)于車間道路線線段，AGV 與其的危險(xiǎn)斥力可以看成AGV 質(zhì)點(diǎn)與道路線上離散的每個(gè)質(zhì)點(diǎn)之間的斥力之和，如下式所示

式中：Fb為道路線對(duì)AGV 的總斥力，Kb為斥力系數(shù)，（x，y）為車輛后輪輪心坐標(biāo)，（xbi，ybi）為道路線上第i個(gè)點(diǎn)的坐標(biāo)，n為離散處理的點(diǎn)的個(gè)數(shù)。

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

3 最優(yōu)控制問題建模與求解

3.1 最優(yōu)控制問題建模

在基于危險(xiǎn)斥力場(chǎng)的AGV 掛接路徑規(guī)劃問題中，目標(biāo)是找到一條滿足約束條件的最安全路徑，即危險(xiǎn)斥力最小的快速路徑，可以將這個(gè)問題看作一個(gè)最優(yōu)控制問題。最優(yōu)控制問題的目標(biāo)是確定優(yōu)化性能指標(biāo)的狀態(tài)變量和控制變量，同時(shí)滿足系統(tǒng)的物理和動(dòng)態(tài)約束[9]。在數(shù)學(xué)上，一個(gè)最優(yōu)控制問題可以用Bolza 形式來表示，形式如下。

優(yōu)化目標(biāo)，即最小化性能指標(biāo)

性能函數(shù)由兩部分組成，其中Mayer 項(xiàng)φ，拉格朗日項(xiàng)g。

滿足動(dòng)力學(xué)約束，即微分方程

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

以及滿足路徑約束，包含狀態(tài)變量和控制變量的路徑約束

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

最小化性能指標(biāo)定義為“最安全”的快速路徑?！白畎踩蓖ㄟ^整個(gè)過程中的危險(xiǎn)斥力積分最小表征，“快速”則使用t作為Mayer 項(xiàng)，性能指標(biāo)兼顧了安全性和快速性。用以下式子描述

式中：Fa和Fb分別由式（2）和式（3）給出。k為時(shí)間系數(shù)，調(diào)整k值可以調(diào)整性能指標(biāo)中安全性和快速性兩者的權(quán)重，以達(dá)到均衡狀態(tài)。

動(dòng)力學(xué)約束即為AGV 線性二自由度動(dòng)力學(xué)模型，見式（1）。

邊界條件主要有起點(diǎn)邊界、過程邊界和終點(diǎn)邊界。

在本工況中，根據(jù)設(shè)定坐標(biāo)值，狀態(tài)變量初值約束為

狀態(tài)變量末值約束為

狀態(tài)變量路徑約束為

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

綜上所述，基于危險(xiǎn)斥力場(chǎng)的車間AGV 掛接路徑規(guī)劃問題建模為

3.2 高斯偽譜法求解最優(yōu)控制問題

接下來使用高斯偽譜法對(duì)最優(yōu)控制問題進(jìn)行數(shù)值求解，求解的流程如圖3 所示。首先，需要對(duì)優(yōu)化區(qū)間進(jìn)行變換，因?yàn)楦咚箓巫V法的插值區(qū)間通常在-1 至1。因此，需要對(duì)時(shí)間進(jìn)行區(qū)間變換，將其映射到-1 至1 的范圍內(nèi)。

圖3 高斯偽譜法數(shù)值求解流程

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

高速偽譜法的具體求解過程可參照參考文獻(xiàn)[10]。通過應(yīng)用高斯偽譜法，能夠得到最優(yōu)控制問題的數(shù)值解，從而得到最優(yōu)的AGV 掛接路徑規(guī)劃方案。該解確定了滿足系統(tǒng)的所有約束條件的變量和控制變量，同時(shí)讓性能指標(biāo)達(dá)到最優(yōu)化狀態(tài)。

4 結(jié)果仿真分析

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

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

圖5 各狀態(tài)變量和控制變量的時(shí)間變化曲線

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

通過觀察圖4 中的求解結(jié)果，可以看到規(guī)劃路徑成功地實(shí)現(xiàn)了從起點(diǎn)到終點(diǎn)的行駛?cè)蝿?wù)，并在終點(diǎn)處與料車對(duì)齊，為下一步的車鉤掛接做好了準(zhǔn)備。該規(guī)劃路徑巧妙地避開了道路線和檢修工位等障礙物，從而確保了AGV 的安全行駛。

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

這進(jìn)一步驗(yàn)證了基于危險(xiǎn)斥力場(chǎng)的車間AGV 掛接路徑規(guī)劃算法的有效性。通過應(yīng)用危險(xiǎn)斥力場(chǎng)和最優(yōu)控制問題的數(shù)學(xué)模型，并借助高斯偽譜法進(jìn)行求解，能夠求出實(shí)現(xiàn)AGV 在車間環(huán)境中的安全導(dǎo)航和順利掛接的路徑。

5 結(jié)論

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

研究還存在一些局限性和不足之處，需要進(jìn)一步地改進(jìn)和完善。危險(xiǎn)斥力場(chǎng)模型的選擇和參數(shù)設(shè)置對(duì)最終的路徑規(guī)劃結(jié)果具有一定的影響，需要進(jìn)一步研究和優(yōu)化參數(shù)選擇的方法。在未來的研究中，可以進(jìn)一步拓展基于危險(xiǎn)斥力場(chǎng)的車間AGV 掛接路徑規(guī)劃算法的應(yīng)用范圍，深入探索和改進(jìn)該算法，以滿足不斷變化的需求和挑戰(zhàn)。