編者按：牽引車-拖車系統(tǒng)由于其靈活的自由度、可拆卸性、靈活的自由度等原因而得到廣泛的應(yīng)用。隨著無人駕駛技術(shù)的發(fā)展，牽引車-拖車系統(tǒng)的運動規(guī)劃問題引起了重視。傳統(tǒng)的運動規(guī)劃方法包括圖搜索算法、基于采樣的算法、最優(yōu)控制算法等等，但是由于牽引車-拖車系統(tǒng)存在欠驅(qū)動約束和非完整約束高度耦合的復(fù)雜情況，導(dǎo)致一般運用于普通車輛上的運動規(guī)劃算法無法處理該系統(tǒng)的規(guī)劃問題。本文提供了利用最優(yōu)控制的方法，將牽引車-拖車系統(tǒng)的運動規(guī)劃問題轉(zhuǎn)化為一個最優(yōu)控制問題，為了簡化數(shù)值求解過程，文中提出了一種自適應(yīng)同倫熱啟動方法，提高了求解效率和成功率，成功解決了該復(fù)雜系統(tǒng)的運動規(guī)劃問題。

本文譯自：
Trajectory Planning for a Tractor with Multiple Trailers in Extremely Narrow Environments: A Unified Approach *
文章來源：
2019 International Conference on Robotics and Automation (ICRA)
作者：
Bai Li, Youmin Zhang, Tankut Acarman, Qi Kong, Yue Zhang
原文鏈接：
https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=8793955

摘要：由于牽引-拖車系統(tǒng)的車輛運動學(xué)是由欠驅(qū)動約束和非完整約束高度耦合而構(gòu)成的，因此對牽引-拖車系統(tǒng)的軌跡規(guī)劃具有挑戰(zhàn)性。目前流行的適用于剛體車輛的基于抽樣或搜索的規(guī)劃方法無法處理牽引車-拖車系統(tǒng)的情況。這項工作旨在處理小環(huán)境中的一般n個拖車的情況。為此，提出了一個有利于精確、直觀和統(tǒng)一的最優(yōu)控制問題。為了簡化最優(yōu)控制問題的數(shù)值求解過程，提出了一種自適應(yīng)同倫熱啟動方法。與已有的序貫熱啟動策略相比，我們的方法可以自適應(yīng)地定義子問題，使相鄰子問題之間的間隙達到合適的區(qū)間來方便解算器解算。提出的自適應(yīng)同倫熱啟動方法的統(tǒng)一性和有效性已經(jīng)在幾個極狹窄的情況下進行了研究。我們的規(guī)劃方法能找到其他現(xiàn)有規(guī)劃方法無法找到的解決方案。文中也簡要討論了在線規(guī)劃的可能性。

1 引言

牽引車-拖車系統(tǒng)是指牽引車上掛有一輛或多輛無動力拖車[1]。通常，拖車車輪是不可轉(zhuǎn)向的，轉(zhuǎn)向力來自于旋轉(zhuǎn)關(guān)節(jié)，這些關(guān)節(jié)依次連接整個車輛的相鄰部件[2]。與等長剛體車輛相比，牽引拖車在狹窄/雜亂環(huán)境或不平坦地形下行駛更加靈活，因此在許多復(fù)雜的場景中得到了廣泛的應(yīng)用[3]。牽引車-拖車車輛軌跡規(guī)劃是指產(chǎn)生牽引車和拖車從初始配置到終端配置的滿意軌跡。在這里，滿意度要求解決的可行性(例如，沒有違反車輛運動學(xué)或無碰撞約束)和最優(yōu)性(即規(guī)劃軌跡預(yù)期為最優(yōu))。對于牽引-拖車車輛的軌跡規(guī)劃是具有挑戰(zhàn)性的，原因在于：（1）由于規(guī)劃模型中的欠驅(qū)動約束和非完整約束耦合[4]，使得現(xiàn)有的大部分類車規(guī)劃方法不能直接適用；（2）牽引車-拖車的動力學(xué)系統(tǒng)在向后運動時是不穩(wěn)定的。本文主要研究了牽引拖車的軌跡規(guī)劃方案。

目前流行的軌跡規(guī)劃方法分為基于圖搜索的方法、基于抽樣的方法和基于最優(yōu)控制的方法。圖搜索規(guī)劃方法首先將連續(xù)配置空間抽象為圖中的節(jié)點，然后搜索節(jié)點之間的可行鏈路，使車輛被引導(dǎo)到目的地。Dijkstra算法[5,6]，A*算法[7-9]，采用動態(tài)規(guī)劃[10]作為搜索器。與離散配置空間的基于圖形搜索的規(guī)劃方法不同，基于采樣的規(guī)劃器使用特定的狀態(tài)模式來探索連續(xù)空間。State-lattice planner[11,12]和closed-loop rapid exploring Random Tree (CL-RRT)[13]分別代表了這類方法的確定性和隨機性。無論是否進行空間離散化，基于圖搜索和基于抽樣的規(guī)劃者通常都要避免直接處理整個連續(xù)的配置空間。這種思想在處理剛體車輛時可能是有效的，但對于多體車輛則有更多的挑戰(zhàn)，因為整個系統(tǒng)的構(gòu)型/狀態(tài)是高度耦合的。一個明顯的證據(jù)是，相對于針對剛體車輛的大量出版文獻，針對牽引車-拖車車輛的圖搜索方法或采樣方法相當(dāng)少。事實上，在上述已有的研究中，很少有研究能夠處理受限環(huán)境下的一般n個拖車的情況。名義上，相關(guān)的軌跡規(guī)劃任務(wù)應(yīng)制定為最優(yōu)控制問題，其中的成本函數(shù)、車輛運動學(xué)和避碰限制是明確和客觀的描述。解析解由龐特里亞金最大化原理導(dǎo)出 [14,15]，而數(shù)值解則由非線性規(guī)劃(NLP)相關(guān)優(yōu)化器[1,12,16]和初始化策略[17]獲得。然而，它們在有復(fù)雜障礙的情況下不起作用。除上述三類外，早期的研究還包括基于規(guī)則的規(guī)劃方法，將原方案解耦成先調(diào)整航向、再沿直線[18]行走等多個步驟。這樣的規(guī)劃方法不夠聰明，仍然不適用于不規(guī)則或狹窄的情況?？偠灾?，目前還沒有一個統(tǒng)一的局部軌跡規(guī)劃器，能夠以可接受的計算能力來處理微小環(huán)境中的n個拖車的情況。

本文提出了一種局部軌跡規(guī)劃方法，該方法能夠處理狹窄/不規(guī)則環(huán)境下的一般n個拖車的情況。為此，提出了一個最優(yōu)控制問題來描述該方案，并進行了數(shù)值求解。其核心貢獻在于提出了一個序貫熱啟動計算框架，該框架簡化了數(shù)值求解過程。本文其余部分的結(jié)構(gòu)如下。第二節(jié)給出了最優(yōu)控制問題陳述。第三節(jié)簡要介紹了數(shù)值計算的基本原理。第四節(jié)提出了一種自適應(yīng)同倫熱啟動方法以促進解決方案。仿真結(jié)果、討論和結(jié)論將在第五節(jié)中給出。

2 牽引車-拖車系統(tǒng)軌跡規(guī)劃問題構(gòu)建

牽引-拖車車輛軌跡規(guī)劃問題可轉(zhuǎn)化為一個考慮運動約束、避碰約束和邊界條件下的以最小化任務(wù)完成時間和最大避障距離為目標(biāo)的最優(yōu)控制問題。

（一）車輛運動學(xué)

牽引車-拖車系統(tǒng)可描述為一個牽引車拖著（n-1）個無動力的拖車的模型。由于這類系統(tǒng)往往在狹窄環(huán)境中移動的較慢，因此可以忽略輪胎側(cè)滑的影響，自行車模型適用于牽引車的建模[1]：



公式1中，tf 表示未知的終端時間，（x1，y1）表示后軸的中點（圖1中的點P1），θ1 表示航向角，v1表示P1的速度，a1表示對應(yīng)的加速度，φ1 是前輪轉(zhuǎn)角，ω1是對應(yīng)的角速度，Lw1表示軸距，LN1表示前懸長度，LN2表示后懸長度，LB1表示牽引車寬度。


圖1 以有2個拖車的系統(tǒng)為例的牽引車-拖車系統(tǒng)模型

拖車的運動學(xué)原理可描述為：

這里，（xi，yi）表示第i個部分的輪軸中點。Θi 表示第（i-1）個拖車的航向角，Pi表示連接第i個部分和第（i-1）個部分的連接點。每兩個相鄰的連接點 Pi和Pi-1都通過長度為Lwi（i≥2）的剛性連接相連。圖1中描述了LBi，LMi和LNi的定義。邊界條件如下：



為避免涉及折刀效應(yīng)[19]，對牽引車-拖車系統(tǒng)的兩個相鄰部分之間的航向角之差進行了界定:



（二）避障約束

假設(shè)每臺牽引車/拖車都是矩形的，環(huán)境中的每一個障礙物都被表示為一個凸多邊形(凹多邊形可以解耦為多個凸多邊形)。在t∈[0,tf]過程中，每一輛拖拉機/拖車和每一個多邊形障礙物之間的碰撞都應(yīng)該避免。設(shè)第j個多邊形障礙的第k個頂點為Vjk(k=1,2，…Npolj)，將車輛第i部分的四個頂點表示為Ai、Bi、Ci和Di (圖2)。


圖2 矩形牽引車/拖車和凸多邊形障礙物的頂點示意圖

車輛第i個部分與第j個障礙物之間的避障約束為：（i）Ai、Bi、Ci和Di位于第j個凸多邊形之外；（ii）每個頂點Vjk位于矩形Ai、Bi、Ci和Di之外。一點Q位于一個凸多邊形W1W2…Wm之外的條件可通過“三角形面積準(zhǔn)則”不等式來描述：


式中，表示三角形面積，表示多邊形面積。根據(jù)公式（4），前面的約束條件（i）和（ii）可表述為：



將（5）在[0, tf]之間對每個牽引車/拖車和障礙物之間進行應(yīng)用，就能以完整的形式表達避障約束。

（三）邊界條件

在起始時刻t=0，整車狀態(tài)(包括x1(0), y1(0), v1(0), φ1(0),θ1(0),a1(0),w1(0), θ2(0)等)。在終端時刻tf，期望車穩(wěn)定地停在期望的位置。這里，穩(wěn)定性是指



若沒有式（6）的約束，車輛無法在t> tf時穩(wěn)定地停止。

（四）目標(biāo)函數(shù)

我們希望車輛在最短的時間內(nèi)完成行程，并在整個過程中與障礙物保持最大的間隙。因此，代價函數(shù)J被定義為


這里，表示時刻（xi，yi）與第k個障礙物幾何中心之間的歐氏距離。，是權(quán)重參數(shù)。

（五）最優(yōu)控制問題

綜上，整個優(yōu)化控制問題可以構(gòu)建如下：


式（8）中的未知量包括

3 最優(yōu)控制問題的數(shù)值解法

由于避碰約束(5)的復(fù)雜性，一般無法得到(8)的解析解。相反，我們期望得到數(shù)值解。通常，數(shù)值求解(8)包括兩個步驟:（i）離散化所有的狀態(tài)空間，建立一個NLP問題;（ii）求解轉(zhuǎn)換后的NLP問題。本文采用一階顯式龍格-庫塔法進行離散化，采用內(nèi)點法[21]求解NLP解。

4 NLP的簡化解法

（一）動機

簡單地應(yīng)用第三節(jié)中的方法并不能得到通用情況的最優(yōu)解，因為復(fù)雜的避免碰撞約束(5)很難由IPM或其他基于梯度的優(yōu)化器直接處理。簡化計算負(fù)擔(dān)的一種常用方法是找到一個接近最優(yōu)或甚至接近可行的初始解，從這個初始解開始求解nlp過程[17]。根據(jù)這個想法，我們可以定義一個子問題序列，使第i個NLP解總是比第（i-1）個子問題復(fù)雜，最后一個子問題是原問題，即離散化問題(8)。由于第i個子問題比（i-1）個子問題復(fù)雜，因此子問題（i-1）的最優(yōu)值是子問題i的一個接近可行的初始解。如果我們找到子問題1的最優(yōu)解，那么序列過程將在有限周期后得到原問題的最優(yōu)解。前面提到的順序過程本質(zhì)上是為了避免一次性處理所有的困難。相反，整個困難分散在子問題中，然后逐步解決。理想情況下，離散應(yīng)該保證每個子問題的遞增的難度是相同的。然而，在順序計算過程真正開始之前，幾乎不可能做到這樣的離散度。

為了解決上述問題，一個自然的想法是在順序過程中自適應(yīng)地調(diào)整子問題的分散水平。例如，當(dāng)子問題k無法解決時(即優(yōu)化過程收斂到不可行或不收斂)，則表示子問題k與k-1之間增加的難度太大，因此進一步的難度分散需要施加。這使得更多的子問題將在子問題k與k-1之間被創(chuàng)建并求解。另一方面，如果連續(xù)的子問題已經(jīng)被解決，這可能表明相鄰子問題之間的間隙非常小，那么我們將嘗試跳過序列中的一些子問題來加速計算。下一節(jié)將詳細(xì)介紹自適應(yīng)色散策略的原理。

（二）自適應(yīng)同倫熱啟動方法

自適應(yīng)同倫熱啟動方法的特點是解決一系列子問題，其中，首先縮小障礙，然后自適應(yīng)地擴大到其標(biāo)稱大小。

標(biāo)量用來調(diào)整每個障礙物的尺寸（圖3）。假設(shè)第j個多邊形障礙物擁有Npolj個頂點，即：，那么幾何中心圖片可表示為：

其中為頂點i的坐標(biāo)。一組新的頂點集合就可以用來表示：



根據(jù)公式（10），新多邊形大小就會隨著單調(diào)變化。當(dāng)圖片=1時，新的多邊形就等同于原來的障礙物；當(dāng)=0時，新的多邊形就縮小為幾何中心。如果我們在NLP問題中將每個障礙物用代替，那么我們可以通過簡單地調(diào)整，使其從0到1單調(diào)變化，以此來定義一系列不同的子問題，即從簡到繁。

圖3 同倫障礙物的示意圖

自適應(yīng)同倫熱啟動方法的目標(biāo)是找到合適的增量步驟讓從0到1變化。最開始，最簡單的子問題是即子問題0)，其中接近于0+。如果最簡單的子問題0無法解決，則取消后續(xù)程序;否則我們設(shè)置達到，然后開始順序計算。在循環(huán)1，一個新的子問題(即子問題1)以進行建立。在數(shù)值求解子問題1時，所記錄的最優(yōu)值作為初始解。如果成功地解決了子問題1，那么就實現(xiàn)了更新，，并記錄子問題1的導(dǎo)出的最優(yōu)解以供將來使用。反之，如果子問題1無法解決，那么step只需將其值降低為。這個迭代過程一直持續(xù)到時問題圓滿解決。在迭代過程中，如果成功地展開了個連續(xù)子問題，則步長增大為。當(dāng)step小于預(yù)定義的閾值時，則整個迭代計算過程終止。該方法的偽代碼如下。

算法1 自適應(yīng)同倫熱啟動算法


5 仿真結(jié)果和討論

仿真在MATLAB+AMPL平臺[22]上進行，并在2.50×2GHz運行速度為8GB RAM的i5-7200U CPU上執(zhí)行。關(guān)于車輛和算法1的常用參數(shù)見表1。

表1 關(guān)于模型和方法的參數(shù)設(shè)置


（一）算法1的效率

對三種情況進行了測試，優(yōu)化軌跡如圖4 - 6所示。案例1的場景中有一個由兩個不規(guī)則放置的障礙物構(gòu)成的瓶頸。在圖4中，整個系統(tǒng)的任何部分都沒有與障礙物發(fā)生碰撞，這說明我們的公式(5)是有效的。案例2涉及到一個雜亂的環(huán)境。觀察情形2的最優(yōu)解(如圖7所示)，可以發(fā)現(xiàn)牽引車的軌跡比每一輛拖車的軌跡復(fù)雜，拖車i的軌跡比拖車i+1的軌跡復(fù)雜。這一現(xiàn)象背后的原因已經(jīng)在[1]中進行了分析。這種現(xiàn)象也反映在案例3中，這是一個典型的車庫-停車場方案。在本小節(jié)結(jié)束之前，讓我們以案例1為例，研究一下算法1的順序計算過程。如圖8所示，在最終解決原NLP問題之前，共解決了40個子問題。在順序過程中，無論何時停止增加，出現(xiàn)問題，然后step減少。另一方面，step仍有增加的機會(見其在20 ~ 25周期的短暫增長)，這意味著擴展的相關(guān)準(zhǔn)則生效。綜上所述，算法1能夠有效地處理各種情況。


圖4 案例1中優(yōu)化的軌跡和對應(yīng)的足跡(CPU時間為334.10秒，tf =28.57秒)


圖5 案例2中優(yōu)化的軌跡和對應(yīng)的足跡(CPU時間為114.24秒，tf =32.07秒)


圖6 案例3中優(yōu)化的軌跡和對應(yīng)的足跡(CPU時間為237.91秒，tf =21.68秒)


圖7 案例2中牽引車/拖車的純軌跡（從圖5重新繪制）


圖8 算法1在案例1中的step和圖片的演變過程

（二）與其他規(guī)劃方法的對比

比較了算法1與[1]、[13]、[17]、[23]中的方法。[1]代表基本的數(shù)值優(yōu)化，不需要初始化。通過簡單地采用線性初始化，[1]中的求解器對于三種情況中的每一種都很快地收斂到不可行。這意味著初始化是有用的。[23]的方法是算法1的一個簡單版本，它將step固定為指定的值。step設(shè)置過大，相鄰子問題之間的間隙過大，失敗是不可避免的;如果步長設(shè)置得小，與算法1的步長擴大策略相比，會浪費CPU時間。綜上所述，從理論上講，[23]的性能永遠(yuǎn)不會超過算法1。參考[17]根據(jù)代價函數(shù)值的演化，構(gòu)建了類似的順序自適應(yīng)計算框架。這種策略在這里并不適用，因為成本函數(shù)值可能不是難度劃分的好標(biāo)準(zhǔn)。[13]的方法是基于采樣的方法。在一般的n拖車系統(tǒng)中，自由度遠(yuǎn)遠(yuǎn)小于狀態(tài)剖面的維數(shù)，這使得將采樣節(jié)點與運動學(xué)可行性聯(lián)系起來變得困難。我們相信，當(dāng)拖車數(shù)量增加時，增量搜索或基于采樣的方法將逐漸變得效率低下。

（三）如何衍生在線解法？

采用非線性模型預(yù)測控制(NMPC)方法，在熱啟動的幫助下進行滾動優(yōu)化，獲得在線軌跡。以案例3為例，我們發(fā)現(xiàn)在后退水平中求解無限水平開環(huán)最優(yōu)控制問題的平均CPU時間為1.29 s。對(8)進行簡化可以進一步加快計算速度。我們可以預(yù)先離線準(zhǔn)備典型軌跡，并利用它們作為在線NMPC框架中的參考解決方案。



參考文獻