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

---

本文譯自：

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

---

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

1 引言

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

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

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

2 牽引車-拖車系統軌跡規劃問題構建

牽引-拖車車輛軌跡規劃問題可轉化為一個考慮運動約束、避碰約束和邊界條件下的以最小化任務完成時間和最大避障距離為目標的最優控制問題。

（一）車輛運動學

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

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

圖1 以有2個拖車的系統為例的牽引車-拖車系統模型

拖車的運動學原理可描述為：

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

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

（二）避障約束

假設每臺牽引車/拖車都是矩形的，環境中的每一個障礙物都被表示為一個凸多邊形(凹多邊形可以解耦為多個凸多邊形)。在t∈[0,tf]過程中，每一輛拖拉機/拖車和每一個多邊形障礙物之間的碰撞都應該避免。設第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之外的條件可通過“三角形面積準則”不等式來描述：

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

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

（三）邊界條件

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

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

（四）目標函數

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

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

（五）最優控制問題

綜上，整個優化控制問題可以構建如下：

式（8）中的未知量包括

3 最優控制問題的數值解法

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

4 NLP的簡化解法

（一）動機

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

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

（二）自適應同倫熱啟動方法

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

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

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

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

圖3 同倫障礙物的示意圖

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

算法1 自適應同倫熱啟動算法

5 仿真結果和討論

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

表1 關于模型和方法的參數設置

（一）算法1的效率

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

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

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

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

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

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

（二）與其他規劃方法的對比

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

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

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

參考文獻