論文導讀：雙機器人協作運送一長串行李推車——階層式規劃 + NMPC 維持隊形

Bingyi Xia, Hao Luan, Ziqi Zhao, Xuheng Gao, Peijia Xie, Anxing Xiao, Jiankun Wang, Max Q.-H. Meng · 2023原文連結

一句話讀懂這篇論文

機場的行李推車收回來之後會疊成一長串，要運回定點。一台機器人推不動、也控制不住一長串推車在人群中轉彎。這篇論文用兩台機器人一前一後夾住整排推車：前面那台負責轉向，後面那台負責出力並維持隊形。難點在於兩台機器人都是差速驅動（不能橫移）、彼此又必須保持精確的距離，否則推車會散掉或被擠壞。作者用一套分層的規劃架構解決：全域路徑、協作運動規劃、遇到行人的行為決策，各自在不同層級處理。

論文資訊卡

標題：Collaborative Trolley Transportation System with Autonomous Nonholonomic Robots
作者：Bingyi Xia、Hao Luan（共同一作）、Ziqi Zhao、Xuheng Gao、Peijia Xie、Anxing Xiao、Jiankun Wang、Max Q.-H. Meng（後兩位為通訊作者）
機構：南方科技大學電子與電氣工程系（深圳市機器人感知與智能重點實驗室）、新加坡國立大學
發表：arXiv 2303.06624（2023），IEEE/RSJ IROS 2023
影片：youtu.be/efnPERm0Rco
系列定位：同一團隊行李推車系列的第二篇。前作是單台機器人回收推車，本篇是把收好的一串推車運送回去，之後的多推車回收系統則把回收和運送整合起來

問題背景

為什麼難

在國際機場這類擁擠環境運送一串收好的推車，有三個挑戰：

要搬運一長串可全向移動的推車——推車的輪子是萬向輪，本身沒有方向約束，會隨便滑
環境可能人很多、走道窄、轉角急
兩台機器人必須做高度協調的動作，維持推車串的完整

既有方法的不足

多機器人協作搬運在控制領域研究很多，但大多不適用：

傳統做法把它當控制問題，物體的目標軌跡事先給定，機器人只負責追蹤。這些方法假設參考路徑已知，不提供動態環境中的導航解法
很多控制演算法是為全向移動機器人設計的，沒有非完整約束（nonholonomic constraint），不適用於差速驅動機器人
集中式最佳化隨機器人數量增加擴展性差，分散式方法則不太適合高度非線性、強耦合的多機系統
跟本文情境最接近的研究：有的用虛擬領導—跟隨的編隊控制讓多台差速車搬棧板狀物體；有的研究兩台非完整機器人在沒有直接通訊下的分散式控制；有的用風險敏感隨機控制，但只算出雙機系統的單一控制輸入。這些都缺乏系統性的導航解法
最接近的一篇能整合追蹤控制和避障，但用了多個手工設計的吸引函數和大量需要精調的參數

本文的階層式架構在不同決策層級保有演算法彈性，參數調整也比較直覺實用。

系統架構

shell

感知
├─ 領導者：LiDAR + 地圖 → 全域定位；障礙物偵測
└─ 跟隨者：LiDAR + 地圖 → 全域定位；偵測領導者身上的標記 → 相對距離與方向
         │
      狀態估計（EKF 融合）
         │
規劃模組
├─ 全域路徑規劃（Hybrid A*，把整組當成一台虛擬車）
├─ 行為選擇器（有限狀態機，偵測行人時調整）
└─ 協作運動規劃（NMPC）→ 兩台機器人的速度指令

三個規劃部分對應三個核心目標：運動學可行的軌跡生成、避障、維持推車串完整。這樣拆的好處是：能即時解出一個有緊密機器人間耦合的複雜規劃與控制問題，同時仍提供精確、高度協調的動作。

機械設計

元件：兩台機器人從前作的推車回收機器人改裝而來，結構精簡、元件相同：有懸吊的差速驅動底盤、鋁合金框架、大容量電池、多種感測器（LiDAR、相機、IMU）、車載電腦、一支機械手。

機械手：由線性致動器和夾爪組成，用槓桿原理夾住推車的把手或橫桿。安裝高度依要夾的位置調整——夾爪底部要跟橫桿同高，才能以正確的水平角度夾住，同時避免運送時線性致動器承受軸向力。前面的領導者把機械手放低、夾推車前方的橫桿；後面的跟隨者把機械手放高、夾推車把手。

被動關節：為了讓兩台機器人能沿彎曲路徑運送，夾爪和機器人之間用一個被動關節連接，並讓夾爪的旋轉軸跟底盤的旋轉軸對齊。這消除了推車串施加在夾爪上的力矩，降低機器人角度誤差的影響，也減少整個系統需要的動態調整。

協作運動規劃：NMPC

這是整篇論文的核心。兩台機器人要計算控制輸入，讓整個機器人—推車組合沿著全域路徑走，同時讓推車緊貼在一起、牢牢夾住。這牽涉多個約束，所以採用非線性模型預測控制（NMPC）——它能很方便地加入各種約束，在機器人領域已有很多成功應用。

系統模型

整個組合由兩台差速驅動機器人和一串全向推車組成。領導者 L 在前方轉向，跟隨者 F 在後方提供動力並維持編隊。假設推車都夾好、數量已知，用兩台機器人的平面位置和朝向當狀態，線速度和角速度當輸入：

x = [x^L,\ y^L,\ \theta^L,\ x^F,\ y^F,\ \theta^F]^\top \in \mathbb{R}^6, \qquad u = [v^L,\ \omega^L,\ v^F,\ \omega^F]^\top \in \mathbb{R}^4

運動學是兩個串接的離散時間獨輪車模型：

x_{k+1} = f(x_k, u_k) = x_k + G(x_k)\,u_k, \qquad G(x_k) = \Delta t \begin{bmatrix} \cos\theta^L_k & 0 & 0 & 0 \\ \sin\theta^L_k & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & \cos\theta^F_k & 0 \\ 0 & 0 & \sin\theta^F_k & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}

從狀態推導出兩個關鍵量：

兩車相對距離 $r \triangleq \lVert p^L - p^F \rVert$
方位角差：每台機器人的朝向，跟「從跟隨者指向領導者的向量」之間的夾角

\phi^i \triangleq \theta^i - \arctan2(y^L - y^F,\ x^L - x^F), \quad i \in \{L, F\}

兩個補充說明：

期望的兩車距離 $l$ 是推車數量 $N$ 的函數，可以從之前回收推車的任務得知，所以在這裡是常數
推車串的中心不一定剛好在兩車連線的中點。但只要推車排得緊密、機械手被動地保持跟推車同朝向，夾爪和推車之間的滑動在實務上可以忽略，把整串推車當成一個剛體

最佳化問題

\min_{\{x,u,\epsilon\}}\ J_T(x_T) + \sum_{k=0}^{T-1} J_S(x_k, u_k, \epsilon_k)

受限於：

$x_{k+1} = f(x_k, u_k)$ ：運動學約束
$x_0 = x_{init}$ ：初始條件
$|r_k - l| \leq \epsilon_k$ ：相對距離約束，用鬆弛變數 $\epsilon_k$ 放寬，避免問題無解
$x_k \in \mathcal{X}$ ：工作空間約束
$u_k \in \mathcal{U}$ ：速度上限 $|v^i| \leq v^i_{max}$ 、 $|\omega^i| \leq \omega^i_{max}$ ，以及透過離散化的加速度上限 $|v^i_{k+1} - v^i_k| \leq a^i_{max}$ 、 $|\omega^i_{k+1} - \omega^i_k| \leq \alpha^i_{max}$

目標函數

終端成本：純追蹤（pure pursuit）

J_T(x_T) = \lVert p^L_T - p^L_{ref} \rVert^2_{P^L_T} + \lVert p^F_T - p^F_{ref} \rVert^2_{P^F_T}

參考位置從全域規劃器給的路徑點 $q^d = [x^d, y^d, \theta^d]$ 推得——路徑點代表整個組合的中心，領導者和跟隨者的參考位置分別在中心前後各 $l/2$ ：

p^L_{ref} = \Big[x^d + \tfrac{l}{2}\cos\theta^d,\ \ y^d + \tfrac{l}{2}\sin\theta^d\Big], \qquad p^F_{ref} = \Big[x^d - \tfrac{l}{2}\cos\theta^d,\ \ y^d - \tfrac{l}{2}\sin\theta^d\Big]

階段成本：維持編隊

J_S(x_k) = k_r\big(r_k^2 - l^2\big)^2 + k_\phi\,\lVert\phi^F_k\rVert^2 + \lVert u_k\rVert^2_{R_k} + w_k\,\epsilon_k^2

第一項讓兩車距離穩定在期望值
第二項讓跟隨者的朝向對齊推車串
第三項懲罰控制量
最後一項懲罰距離誤差的鬆弛量

全域路徑規劃：把整組當成一台車

系統要在國際機場這種大型場地運作，所以需要全域路徑規劃。為了保證運動學可行，把整個機器人—推車組合建模成一台像汽車的虛擬車輛，在 2D 佔據地圖中找無碰撞路徑。

規劃器的選擇很彈性，本文為了簡單採用 Hybrid A*。運動學從自行車模型改寫：前後兩台機器人分別當成虛擬車的前後「舵」：

\begin{aligned} x_{k+1} &= x_k + v\Delta t\cos(\theta_k + \beta_k) \\ y_{k+1} &= y_k + v\Delta t\sin(\theta_k + \beta_k) \\ \theta_{k+1} &= \theta_k + \frac{v\cos\beta_k\,\Delta t}{l}\big(\tan\delta^L_k - \tan\delta^F_k\big) \\ \beta_k &= \arctan\Big(\tfrac{1}{2}\big(\tan\delta^L_k + \tan\delta^F_k\big)\Big) \end{aligned}

$q = [x, y, \theta]$ 是組合上某個參考點的 2D 姿態， $v$ 是切線速度， $\delta^L$ 、 $\delta^F$ 是前後兩個「舵角」， $l$ 是兩車期望距離。為了加快搜尋，把 $v$ 固定成合理常數，在格點之間均勻取樣可行的舵角組合 $[\delta^L, \delta^F]$ 來延伸虛擬車的姿態。得到的路徑當成一串路徑點送給運動規劃模組。

行為選擇器：遇到人就停下來讓路

在擁擠的機場，機器人絕對不能撞到人。作者發現，讓拖著一長串推車的機器人主動做激烈的閃避動作，既笨拙、不安全，甚至會嚇到人。所以改用一個有限狀態機管理行為，四個模式：

模式	觸發條件	行為
導航	興趣區內沒有動態障礙物	運動規劃器正常運作
減速	有障礙物在前方擋路	平滑減速到停止
等待	速度降到接近 0	停著讓周圍的行人先過
限速導航	偵測到行人在推車組合兩側	用較低的速度上限移動，方便平滑地剎車和起步

狀態轉換依據領導者與偵測到的動態障礙物之間的相對位置。以領導者為原點的極座標 $o = (\rho, \alpha)$ ：

興趣區 $\mathcal{O}$ ： $\rho \leq R_{ROI}$ 、 $\alpha \in [\pi/3, 5\pi/3]$
前方區 $\mathcal{O}_1$ ： $\rho \leq R_{front}$ 、 $\alpha \in [2\pi/3, 4\pi/3]$
側邊區 $\mathcal{O}_2 = \mathcal{O} \setminus \mathcal{O}_1$

在限速導航中，如果側邊的人離開就切回導航；如果有人從側邊移到前方就進入減速。

狀態估計：多來源融合

維持編隊需要高精度的動作，所以可靠的狀態資訊非常重要。除了每台機器人用 LiDAR、IMU、里程計各自定位，在領導者身上加了標記，讓跟隨者能用自己的 LiDAR 量測兩車的相對姿態。

用**延伸卡爾曼濾波（EKF）**融合三個來源：兩台機器人各自的定位結果，以及相對姿態量測：

\begin{aligned} x_{k+1} &= x_k + G(x_k)u_k + v_k \\ y^L_{k+1} &= H^L x_{k+1} + w^L_{k+1}, \quad H^L = [I_3\ \ 0] \\ y^F_{k+1} &= H^F x_{k+1} + w^F_{k+1}, \quad H^F = [0\ \ I_3] \\ y^\Delta_{k+1} &= H^\Delta x_{k+1} + w^\Delta_{k+1}, \quad H^\Delta = [I_3\ \ {-I_3}] \end{aligned}

所有雜訊假設為零均值、彼此獨立的高斯分佈。因為局部運動規劃是集中式的，預測步驟跟一般 EKF 一樣；量測的更新頻率不同，所以每收到一個觀測就非同步地更新一次。作者發現這種多來源融合對運動規劃器順利運作、對兩車高度協作都非常關鍵。

實作細節

軟體：Python 實作，用 ROS Melodic 整合
硬體：每台機器人一台 Intel NUC（Core i7-1165G7、32 GB RAM），一顆 Ouster OS1 128 線 LiDAR，一顆 9 軸 IMU。兩車透過車載 WiFi 路由器組成無線 ad-hoc 網路通訊
定位：每台機器人用 AMCL 在事先以 GMapping 建好的 2D 佔據地圖上做全域定位。在領導者的旋轉中心裝一個高反射率的圓柱標記，跟隨者從 LiDAR 掃描點中擷取標記位置，當成領導者的位置
全域規劃：解析度 0.25 m × 0.25 m × 15°，虛擬車尺寸稍微放大以策安全
運動規劃：約 30 Hz，預測時域 $T = 20$ 步、每步 $\Delta t = 0.1$ 秒。用 CasADi 建模、IPOPT 求解，在一台機器人的車載電腦上即時執行

實驗結果一：路徑追蹤精度

在沒有障礙物的動作捕捉區（OptiTrack 提供精確定位）運送 3 台推車，追蹤一條由 30 個路徑點組成的參考路徑，路徑取自兩段曲率 0.44 m⁻¹ 的圓弧。

從 (0, 0) 出發，到目標區中心 (4.5, 4.2) 距離 0.3 公尺內停下
完成時間 12.2 秒，平均速度 0.491 m/s
追蹤誤差 2.77 ± 1.68 公分，最大 5.66 公分（追蹤誤差定義為兩條曲線間的歐氏距離；只為了計算誤差，參考路徑用三次內插加密）

實驗結果二：真實環境運送

窄空間

兩台機器人協作運送 5 台推車，穿越一個擺了 6 個靜態障礙物、有 3 位行人的窄空間。推車串長 1.98 公尺、重約 100 公斤——連成年人要在這麼複雜的環境手動推超過五台推車都不容易。

全域規劃器成功找到一條平滑路徑。雖然障礙物讓路徑變得蜿蜒、有好幾個轉彎接近組合的運動學極限，路上沒有行人時兩車仍以接近全速運作（跟隨者上限 0.7 m/s、領導者上限 0.6 m/s）
行人擋路時，行為選擇器啟動：機器人平滑停下、牢牢夾住推車，路清空後再繼續。在 t = 15.8 秒、36 秒、46 秒各發生一次
兩車距離誤差 1.49 ± 2.03 公分，推車串朝向與兩車朝向的差都維持在 ±45° 以內

維持推車串完整是成功運送的另一個關鍵要求，可以從兩車的相對姿態看出來：機械手不是剛性固定在推車上，相對距離或方向差變化太大就會讓推車脫落或結構損壞。

人多的空間

兩台機器人運送 8 台推車（總長 2.56 公尺），在一個有更多行人走動的大廳中。速度上限調低為跟隨者 0.58 m/s、領導者 0.5 m/s，加速度也更小。

組合成功避開所有障礙物、讓路給好幾位行人、追蹤參考路徑並抵達目標
跟隨者的速度波動，反映出它不斷調整來讓推車保持緊貼
兩車距離誤差 0.255 ± 2.36 公分（作為參考，Ouster OS1 LiDAR 的測距精度是 2.5 公分）

限制與未來方向

作者列出的未來工作：

加入社交感知的運動規劃（目前遇到人是停下讓路，而不是預測人的動向）
更大規模的戶外實地實驗
探索分散式的解法，提升穩健性和擴展性（目前的 NMPC 是集中式的，在一台機器人上解兩台的問題）

影響與定位

這篇論文的價值在於提供了一個能實際運作的完整系統，而不只是一個控制演算法：

分層處理約束：運動學可行性交給全域規劃、編隊與追蹤交給 NMPC、安全交給有限狀態機。每一層的問題都夠小，能即時求解
把多機器人當成一台虛擬車：讓全域規劃可以直接用現成的 Hybrid A*
保守的避人策略：拖著一長串推車時，停下讓路比激烈閃避更安全，這是很務實的工程判斷
用低成本標記取得相對姿態：一個高反射圓柱 + 跟隨者既有的 LiDAR，不需要額外的感測器

跟前作比較，這篇把問題從「一台機器人對準、夾起一台推車」延伸到「兩台機器人協作搬運一整排推車」，NMPC 的角色也從「對準 + 避障」變成「追蹤 + 維持編隊」。

參考資料

Xia, B., Luan, H., Zhao, Z., Gao, X., Xie, P., Xiao, A., Wang, J., & Meng, M. Q.-H. (2023). Collaborative Trolley Transportation System with Autonomous Nonholonomic Robots. arXiv:2303.06624
示範影片：youtu.be/efnPERm0Rco