論文導讀：多推車回收系統——兩台機器人分工搜尋、對準、疊放，用 CLF-CBF-QP 守住視野

Peijia Xie, Bingyi Xia, Anjun Hu, Ziqi Zhao, Lingxiao Meng, Zhirui Sun, Xuheng Gao, Jiankun Wang, Max Q.-H. Meng · 2024原文連結

一句話讀懂這篇論文

機場的推車可以頭尾相接疊成一長串，一次運走最有效率。這篇論文要解決的是**「把散落各處的推車一台一台收回來、疊成一串」這整個流程。兩台機器人分工：一台留在原地夾住隊伍的頭（收集者），另一台到處找推車、夾起來送回隊伍尾端塞進去（偵測者）。最難的一步是精準對準**——低成本的機械手自由度很少，沒辦法在途中調整推車姿態，所以底盤本身要開得非常準，同時還不能讓目標跑出相機的視野。

論文資訊卡

標題：Autonomous Multiple-Trolley Collection System with Nonholonomic Robots: Design, Control, and Implementation
作者：Peijia Xie、Bingyi Xia（共同一作，Xia 為通訊作者）、Anjun Hu、Ziqi Zhao、Lingxiao Meng、Zhirui Sun、Xuheng Gao、Jiankun Wang、Max Q.-H. Meng
機構：南方科技大學電子與電氣工程系（深圳市機器人感知與智能重點實驗室）
發表：arXiv 2024 年 1 月（2401.08433），正式刊於 Journal of Field Robotics 第 42 卷第 1 期（2025）
系列定位：同團隊推車系列的整合版。第一篇處理單台推車的抓取與運送，第二篇處理已疊好推車串的雙機運送，本篇補上中間缺的「依序疊放多台推車」

問題背景

為什麼要疊起來

旅客常把行李推車丟在機場各處，需要收回指定區域重複使用。全自動的機器人系統能降低人力成本、提升工作安全、改善使用者體驗。

之前的研究有的處理單一推車的安全運動規劃，有的處理推車收集順序的決策，但最有效率的做法是把多台推車疊成一串、一起運送——推車本來就設計成頭尾對齊就能疊在一起。前作讓兩台機器人一前一後運送推車串，但只適用於已經疊好的推車隊伍。

本文把「依序疊放」這個任務定義為推車回收問題（Trolley Collection Problem, TCP）。

關鍵挑戰

基於前作在機械手設計、避障、物體定位上的基礎，本文聚焦在對接機構和多推車回收時的控制精度：

為了降低成本，設計了一個低成本驅動器、自由度少的對接機構。代價是機械手在導航途中無法調整接觸點和推車姿態
所以關鍵變成：精準地把非完整機器人（差速驅動）開到感測器估計出的目標姿態。這在「接近」和「對接」兩個階段是同一個控制問題
作者把控制李雅普諾夫函數（CLF）和控制屏障函數（CBF）放進二次規劃（QP），做成一個以視覺為基礎的控制器

系統分工

shell

收集者（Collector）：留在原地，領頭並夾住推車隊伍
偵測者（Detector）：主動找推車，夾起後送回隊伍尾端

整個任務分成四個階段，以有限狀態機循環執行，直到收集到指定數量的推車：

階段	偵測者在做什麼	用什麼感測器
搜尋	用 RGB 相機找閒置推車；找到後用關鍵點量測 6D 相對姿態，當成導航目標	RGB 相機
導航	點對點導航並避障（轉換用的階段）；用 SLAM 全域定位，在地圖上規劃	LiDAR、IMU、相機
接近	用 RGB-D 相機即時量測推車相對姿態，擷取推車背板平面特徵來估計目標姿態；視覺控制器在考慮視野的前提下縮小姿態差，收斂後夾起推車；確認夾好後透過 UDP 向收集者詢問它的全域姿態當下一個導航目標	RGB-D 相機
對接	跟接近階段同樣的做法，但改用 LiDAR 點雲量測收集者身上的高反射標記；完成後回到搜尋找下一台	LiDAR

機械設計

兩台機器人結構相同：

底盤：兩個伺服馬達當驅動輪，各提供 32.75 N·m 扭矩；從動輪是兩個被動全向輪，搭配扭力樑懸吊吸收震動
可切換狀態的機械手：既有設計要嘛是夾爪（把推車和機器人鎖成剛體，適合回收），要嘛是被動關節（運送時讓推車能動態調整，適合運送），都只考慮單一任務，沒有一個能涵蓋整個流程又保證精準對接

機械手

兩指末端執行器 + 兩個關節：俯仰軸和偏擺軸
俯仰軸由直流馬達驅動，用拉線感測器量繩長換算轉角
偏擺軸由關節馬達控制，在兩種狀態間切換：對接階段鎖住（維持剛體），運送階段解鎖（自由轉動）。切換時關節馬達先把機械手轉回中心、對齊卡榫，再用線性馬達上鎖
末端執行器由兩個線性馬達控制開合；設計了半徑 5 公分的圓角，提高放下夾爪抓推車橫桿時的容錯
控制指令從迷你電腦透過虛擬 COM 序列埠直接送到嵌入式板

對接機構

兩種情況：

隊伍裡已經有推車：前面那台推車會當成引導（推車本身的可疊性），可以容忍約 8 公分的側向誤差和 10° 的方向誤差
隊伍裡還沒有推車：沒有前一台當引導，兩台機器人要精密協作。收集者夾爪尖端設計了一個弧形引導結構：偵測者推著推車往前，推車前橫桿碰到弧形引導時，反作用力會把收集者的機械手往上推；推車繼續往前，機械手靠重力落下。同時偵測者確認推車是否對準，對準後收集者驅動機械手夾住推車

控制器：CLF-CBF-QP

基礎概念

考慮非線性仿射控制系統 $\dot{x} = f(x) + g(x)u$ 。

控制李雅普諾夫函數（CLF）：一個連續可微、正定的函數 $V$ 。如果存在控制律滿足

\inf_{u\in U}\ \dot{V}(x, u) \leq -\gamma(V(x))

（ $\gamma$ 是 class-K 函數），系統就會收斂到 $V = 0$ 的點。直覺上是「保證能逼近目標」。

控制屏障函數（CBF）：用來保證一個安全集合 $C = \{x \mid h(x) \geq 0\}$ 的前向不變性——從集合內出發的軌跡永遠不會離開。如果存在控制律滿足

\sup_{u\in U}\ \dot{h}(x, u) \geq -\alpha(h(x))

（ $\alpha$ 是延伸 class-K 函數），集合就是前向不變的。直覺上是「保證不會越界」。

合在一起做成 QP：

\begin{aligned} u^* = \arg\min_{u,\delta}\ & \tfrac{1}{2}u^\top Hu + \rho\delta^2 \\ \text{s.t.}\ & \dot{V}(x, u) + \gamma(V(x)) \leq \delta \\ & \dot{h}(x, u) + \alpha(h(x)) \geq 0 \\ & u \in U \end{aligned}

$\delta$ 是放寬 CLF 的鬆弛變數，確保 QP 有解。這樣安全（CBF）是硬約束、收斂（CLF）是軟約束，兩者衝突時以安全為優先。把 $\gamma$ 和 $\alpha$ 設成正純量時，CLF 給出一個指數衰減的上界，CBF 給出一個指數衰減的下界。

相對運動

感測器校正後與機器人旋轉軸對齊，整個機器人可以描述成一個剛體。剛體的姿態用齊次轉換矩陣 $T^O_i \in SE(3)$ 表示，對時間微分得到本體速度：

\dot{T}^O_i = T^O_i[\mathcal{V}_i] = T^O_i\begin{bmatrix}[\omega_i]_\times & v_i \\ 0 & 0\end{bmatrix}

再引入另一個移動剛體 $\{j\}$ ， $\{i\}$ 相對於 $\{j\}$ 的運動是：

\dot{T}^j_i = -[\mathcal{V}_j]T^j_i + T^j_i[\mathcal{V}_i]

這讓控制器可以完全用相對量測來寫，不依賴全域定位——全域定位的誤差就不會影響對準精度。

問題定義

差速驅動機器人用獨輪車模型： $\dot{x} = v\cos\theta$ 、 $\dot{y} = v\sin\theta$ 、 $\dot{\theta} = \omega$ ，控制輸入 $u = [v, \omega]$ ，有上限 $|v| \leq v_{max}$ 、 $|\omega| \leq \omega_{max}$ 。

目標物體 $\{T\}$ 相對於機器人 $\{R\}$ 的姿態由感測器量到： $x^R_T = [x^R_T, y^R_T, \theta^R_T]$ 。評估用的穩態誤差包括法向誤差、側向誤差和方向誤差。

問題：根據機器人的局部觀測 $x^R_T$ ，解出控制輸入讓機器人沿著已知軌跡 $s(t)$ 移動，使穩態誤差漸近收斂到零。

軌跡：用五次多項式規劃，滿足邊界位置、速度、加速度條件：

x(t) = \sum_{k=0}^{5}a_kt^k, \qquad y(t) = \sum_{k=0}^{5}b_kt^k

把規劃的軌跡看成一個虛擬目標 $\{V\}$ ，帶有名目速度 $[v_V, \omega_V]$ 。機器人追蹤軌跡就是讓相對狀態 $\tilde{x} = x^R_V$ 趨近零，誤差動態為：

\dot{\tilde{x}} = \begin{bmatrix} y^R_V\,\omega_R + v_V\cos\theta^R_V - v_R \\ -x^R_V\,\omega_R + v_V\sin\theta^R_V \\ \omega_V - \omega_R \end{bmatrix}

CLF：追上虛擬目標

V(\tilde{x}) = \tfrac{1}{4}\big(\tilde{x}^\top H\tilde{x}\big)^2, \qquad \dot{V} = \big(\tilde{x}^\top H\tilde{x}\big)\tilde{x}^\top H\dot{\tilde{x}}

CLF 約束： $\dot{V} + \mu V \leq \delta$ ， $\mu > 0$ ， $\delta > 0$ 是保證可解的鬆弛變數。

CBF：不讓目標離開視野

推車的相對位置 $p^R_T$ 和機器人朝向之間的夾角 $\psi$ ，必須在相機視野內：

|\psi| = |\arctan(p^R_T)| < |\psi_{fov}|

CBF 候選函數：

h(\psi) = \tfrac{1}{2}\big(\psi_{fov}^2 - \psi^2\big)

$h \geq 0$ 就是安全集合。對時間微分：

\dot{h}(\psi, u) = -\psi\dot{\psi} = \frac{\psi}{\rho^2}\Big[(y^R_T)^2\omega_R - y^R_T v_R + (x^R_T)^2\omega_R\Big]

$x^R_T$ 、 $y^R_T$ 都直接來自機器人目前的局部觀測。CBF 約束： $\dot{h} + \lambda h \geq 0$ 。

最終的 QP

\begin{aligned} \min_{u,\delta}\ & u^\top Q_u u + c\,\delta^2 \\ \text{s.t.}\ & u \in U \\ & \dot{h}(\psi, u) + \lambda h(\psi) \geq 0 \\ & \dot{V}(\tilde{x}, u) + \mu V(\tilde{x}) \leq \delta \end{aligned}

$Q_u$ 是控制成本的正定矩陣， $c$ 是鬆弛變數的懲罰權重。速度上限 $|v| \leq v_{max}$ 、 $|\omega| \leq \omega_{max}$ ，加速度透過離散化限制 $|v - v_0| \leq a_{max}$ 、 $|\omega - \omega_0| \leq \alpha_{max}$ 。用 CasADi 建模、IPOPT 在機器人車載電腦上即時求解。

目標定位

推車定位：從粗到細

粗定位（搜尋階段，距離遠時）：

微調過的 YOLOv5 即時偵測推車，裁切出邊界框
堆疊沙漏網路（Stacked Hourglass）預測推車的 6 個關鍵點
用相機投影模型 $s_ih^c_i = K[R\ \ p]h^w_i$ 建立影像點和真實點的對應，解 PnP 問題得到相對姿態

細定位（接近階段，距離近時，看得到足夠特徵）：

對 RGB-D 點雲做直通濾波，保留 $-0.3\,\text{m} < y < 0.6\,\text{m}$ 、 $z < 2.5\,\text{m}$ 的點
推車背板是一個平面，用 PCL 的 RANSAC 擬合平面 $Ax + By + Cz + D = 0$
投影到機器人的 X-Y 平面：背板質心座標 $\hat{d} = [\hat{x}^R_T, \hat{y}^R_T]$ ，法線角 $\hat{\theta}^R_T = -\arctan(A/C)$

視野遮擋的補償：只看到部分背板時，RANSAC 分割不完整，估出的質心會偏。利用背板幾何的先驗知識補償：

用觀測到的相對距離 $\hat{\rho}$ 和正弦定理，估計視野內看到的背板長度 $\hat{l}$
背板實際長度 $l$ 是常數，用兩者的差修正質心：

\hat{d}_{cp} = \hat{d} + \tfrac{1}{2}(\hat{l} - l)\big[\cos\hat{\theta}^R_T,\ -\sin\hat{\theta}^R_T\big]

機器人定位（對接階段）

對接時，夾著的推車完全擋住了單眼相機。改用收集者身上兩條特殊的反光條當標記：

標記在 3D LiDAR 掃描中產生高強度點，用高通濾波保留強度超過 4500 的點
這些點的平均向量 = 兩車的相對位移
用 K-Means 把點分成左右兩群，各算平均 $\bar{p}_l$ 、 $\bar{p}_r$
收集者的朝向向量 $v$ 由 $(\bar{p}_l - \bar{p}_r)^\top v = 0$ 求得，投影到 2D 平面得到方位角
對接目標姿態再依隊伍長度調整：每多收一台推車，沿收集者方向多加一個偏移距離（實驗中為 0.18 m）

實作細節

模組	規格
電源	48 V 磷酸鋰鐵電池 + 電池管理系統，供應迷你電腦 19 V、LiDAR 24 V
驅動	48 V 無刷直流伺服馬達驅動底盤，直流馬達驅動機械手；迷你電腦透過 CAN Box 經 USB 控制
感測器	32 線 Ouster OS0 LiDAR（平均偏差 1 mm，內建 6 軸 IMU）；RealSense L515 相機（垂直 55°、水平 70°）
運算	收集者：i7-11657 迷你電腦；偵測者：i7-12900F + RTX 3060 迷你電腦
軟體	ROS2 Galactic，Python 與 C++

導航框架沿用前作：

定位：基於 Ouster LiDAR 的 FAST-LIO2 SLAM；初始姿態用 GICP 把 LiDAR 點雲對到事先建好的 3D 地圖點雲；移動超過預設門檻就重新定位，消除誤差
全域規劃：只做室內實驗，考慮運算效率，在 2D 成本地圖上用 A*。用 Octomap 把 3D 地圖壓成 2D，點雲只保留 −1.0～1.0 m 高度以減少地板和天花板的干擾
避障：NMPC 當局部規劃器，把到障礙物（地圖上未知的點雲）的 2D 距離寫成約束。LiDAR 和 RGB-D 點雲一起使用；夾住推車後相機被擋住，就只用 LiDAR

原型成本約 1 萬美元（作者稱為第一個以這種低成本原型在真實環境中安全穩定回收多台推車的展示）。

實驗結果一：推車定位精度

把偵測者放在相對推車的 50 個隨機位置量推車姿態，以 OptiTrack 動作捕捉為真值：

所有誤差的中位數都在 ±0.1 m 或 ±5° 以內
加上視野遮擋補償後，縱向和側向誤差的中位數更接近零，四分位距也明顯縮小
角度誤差由背板法向量估計，不受補償影響，維持一致

實驗結果二：控制方法比較

偵測者從 30 個隨機初始姿態出發，執行接近階段去收集一台靜止推車。初始位置限制在推車座標系的 $-1.0 \leq x \leq 1.0$ m、 $-2.5 \leq y \leq -1.5$ m 範圍內，確保一開始看得到推車。所有方法用同一套推車定位演算法。

成功條件：考慮機械手和推車的物理限制，機器人要抵達指定相對位置，位置誤差 < 30 mm、方向誤差 < 5°，才能讓機械手夾到推車。

方法	成功次數	縱向誤差 $e_x$ （mm）	側向誤差 $e_y$ （mm）	方向誤差 $e_\theta$ （°）
非線性控制	15/30	20.54 ± 0.77	1.37 ± 18.62	3.02 ± 18.83
MPC（前作）	21/30	5.09 ± 7.56	16.32 ± 38.07	1.88 ± 3.11
CLF-CBF-QP	30/30	2.08 ± 0.96	9.33 ± 10.51	2.12 ± 1.55

（成功試驗中的平均 ± 標準差）

CLF-CBF-QP 全部 30 次都成功。另外兩種方法通常在需要把狀態和輸入推到接近極限時失敗，尤其初始姿態和目標差很多的時候
三種方法的角度誤差都夠小，但本方法大幅降低縱向誤差，側向誤差有競爭力
本方法在側向和角度誤差的標準差最小，顯示穩健
縱向誤差和方向誤差直接影響機械手能不能夾到推車；側向誤差影響對接階段能不能成功，因為接觸點是固定的
非線性方法難以處理視野約束、對雜訊敏感；MPC 雖然較好，但在收斂到目標時常常過度反應，在受限空間中會導致目標離開視野或精度下降

困難情境：推車只出現在初始相機畫面的側邊（部分可見）。其他方法都沒成功，本方法以平滑的軌跡和控制完成，分別在 21.35 秒和 23.83 秒抵達目標——先以最高速接近，近距離時再調整姿態。過程中量到的相對距離和角度也跟真值非常吻合。

實驗結果三：真實環境展示

在 20 m × 15 m 的區域，隨機擺放 5 個未知的靜態障礙物，行人隨機穿越（偵測者靠近目標、執行接近或對接時除外）。兩台機器人從左下角並排出發，任務是安全地找到並收集 4 台散落的推車、排成一串。

推車的全域定位不包含在內；收集順序假設為已知，給定一串路徑點作為每次收集的轉換
對接時每台推車的偏移 0.18 m；進入接近階段的觸發半徑 1.0 m、對接階段 2.1 m

結果：系統成功、安全地收集了所有推車。以第一台為例：

偵測者偵測到推車方向後繞到它後方，用視覺控制精準地把推車送到目標姿態，由收集者的機械手夾住
2.3～4.2 秒間，為了避開行人降速並調整方向
9.6 秒時又減速甚至倒車，因為細定位修正了推車位置，需要重新規劃
23.3 秒開始由視覺控制器接手，速度變化相對小
夾到推車後，透過 UDP 向收集者詢問目標位置並導航過去，稍微調整方向對齊後完成對接
一次收集約 59.6 秒，其餘 3 台也都順利完成

影響與定位

這篇論文把整個推車系列串成一個完整的系統，貢獻可以整理成：

低成本的整合系統：硬體、感知、動態環境運動規劃、最佳化控制全部整合，原型約 1 萬美元
可切換狀態的機械手與對接機構：用最少的自由度同時滿足回收（剛性）和運送（自由轉動）兩種需求；沒有前一台推車引導時，用弧形引導結構完成第一台的對接
CLF-CBF-QP 視覺控制器：只用相對量測，同時保證收斂和視野約束

跟前作比較，CBF 的用途改變了：

論文	CBF 保護什麼
推車回收（2022）	避障 + 視野
多推車回收（2024）	視野（避障交給 NMPC 局部規劃器）
DA-VPC（2025）	在視覺預測控制中處理干擾下的對接

參考資料

Xie, P., Xia, B., Hu, A., Zhao, Z., Meng, L., Sun, Z., Gao, X., Wang, J., & Meng, M. Q.-H. (2024). Autonomous Multiple-Trolley Collection System with Nonholonomic Robots: Design, Control, and Implementation. arXiv:2401.08433
Ames, A. D. et al. (2019). Control Barrier Functions: Theory and Applications. European Control Conference.