論文導讀：DA-VPC——考慮干擾的視覺預測控制，讓推車對接更準

Yuhan Pang, Bingyi Xia, Zhe Zhang, Zhirui Sun, Peijia Xie, Bike Zhu, Wenjun Xu, Jiankun Wang · 2025原文連結

一句話讀懂這篇論文

把推車一台一台疊進隊伍，最關鍵的是「對接」這一步要非常準：第一台推車要對準領頭機器人的夾爪，後面每一台要塞進前一台的尾巴。以前靠 LiDAR 和全域定位，但機場有玻璃牆、亮面地板，LiDAR 常常不準。DA-VPC 換一個思路：在領頭機器人身上裝一組紅外線燈，跟隨機器人只用一台加了濾鏡的相機看這幾個光點，直接讓「看到的畫面」去對齊「對接完成時應該看到的畫面」。再加上一個觀測器，即時估計推著推車時被負載、坑洞、打滑帶來的干擾並補償掉。

論文資訊卡

標題：DA-VPC: Disturbance-Aware Visual Predictive Control Scheme of Docking Maneuvers for Autonomous Trolley Collection
作者：Yuhan Pang、Bingyi Xia（共同一作）、Zhe Zhang、Zhirui Sun、Peijia Xie、Bike Zhu、Wenjun Xu、Jiankun Wang（通訊作者）
機構：南方科技大學（深圳市機器人感知與智能重點實驗室）、鵬城實驗室
發表：arXiv 2509.07413（2025 年 9 月，v3 更新於 2026 年 3 月）
系列定位：推車系列中專門解決「對接控制」的一篇。硬體建立在推車推行的自適應控制那篇的雙臂移動機器人上

問題背景

任務設定

兩台移動操作機器人組成領導者—跟隨者編隊：跟隨者負責抓住並推推車，領導者停在推車隊伍的最前端，當作對接的基準。跟隨者把推車推過去跟領導者（或隊伍最後一台推車）對接，反覆進行，疊成完整的隊伍。

這個任務對穩健性和精度要求都很高：對不準、接不牢，可能造成碰撞或操作失敗。

為什麼底盤要開得準

跟隨者的雙臂控制器是根據移動底盤的速度來調整推力，讓推車沿著底盤前進方向走。換句話說，推車的姿態要靠底盤的對接動作來調整，底盤不準，推車就不準。

為什麼不用地圖和 LiDAR

實際部署的環境是動態的，預先建好的地圖維護成本高、缺乏彈性
機場有玻璃牆和拋光地板，LiDAR 定位在高反射環境中表現不好
單眼視覺成本低，直接用影像特徵引導對接，是很實際的解法

視覺伺服的挑戰

視覺伺服（Visual Servoing, VS）直接用視覺回饋引導動作，已經用在很多對接任務上。但直接用在推推車的情境有困難：

服務型機器人常用的 AprilTag 容易受光線干擾和遮擋影響，而且需要一塊夠大的專用平面安裝，才能遠距離看清楚
系統是緊密耦合的非線性控制問題：差速底盤有非完整約束、相機有有限視野，控制器必須在這些約束下確保精準收斂
有不可忽略的干擾：不平地面造成的輪子打滑、運動學模型本身的誤差、推動機構持續的擾動。這些干擾同時影響動作執行和視覺觀測，造成伺服偏差

系統設計

硬體

跟隨者：雙 SCARA 機械臂穩穩推推車，每支手臂三個關節馬達 + 一個夾爪，兩支手臂共用一個裝在導螺桿模組上的同軸底座，可以垂直調整。抓住推車把手、沿約束方向施力，底層控制沿用前作的自適應控制器，依底盤移動方向計算手臂關節扭矩
主動紅外線 LED 陣列（940 nm）裝在領導者身上當視覺標記：4 顆 3 W 高功率紅外線 LED，60° 光束角，用恆流驅動器維持穩定照度
跟隨者相機：MV-SUA133GM 工業相機，120° 視野、1280 × 1024 解析度，在鏡頭和 CMOS 感測器之間加了窄頻濾鏡，只讓 940 nm 的光通過

這個設計的好處：

每顆 LED 投影到影像上是一個獨立、菱形的光斑。因為 LED 發射角窄，光斑形狀在不同距離下都很穩定，很容易擷取成穩定的視覺特徵
用單一、人眼看不見的波長溝通，在公共環境中減少光害
因為濾鏡擋掉了可見光，相機校正改用 940 nm 光源照亮校正板

對接流程

目標狀態定義成在已對接位置拍下的參考影像，作為視覺伺服的期望影像
控制器計算跟隨者的底盤速度，讓目前影像逐漸趨近期望影像
跟隨者根據手臂關節扭矩的變化和視覺伺服誤差判斷對接是否完成；到達對接位置後把推車往前推，直到隊伍中所有推車剛性連接
對接第一台推車時，領導者的末端執行器是一個靠重力自鎖的引導結構：推車前橫桿碰到引導結構時產生垂直地面的反作用力，被動地把末端執行器抬起；推車繼續前進，末端執行器先靠重力落下，再由線性致動器鎖住推車前橫桿
對接完成後，跟隨者收回夾爪，繼續收集任務

問題建模

座標系

跟隨者座標系 ：固定在跟隨者底盤，速度指令以此表示
領導者座標系 ：描述視覺標記的空間配置
相機座標系 ：原點在相機光心，z 軸與光軸重合
像素座標系 ：原點在影像左上角
影像座標系 ：正規化影像平面座標，方便推導狀態方程

運動學

跟隨者是差速驅動，用獨輪車模型，控制輸入 $u_F = [v_F, \omega_F]$ 。領導者相對於跟隨者的速度：

\dot{x}^F_L = y^F_L\,\omega_F - v_F, \qquad \dot{y}^F_L = -x^F_L\,\omega_F, \qquad \dot{\theta}^F_L = -\omega_F

領導者身上一個特徵點 $P$ 在領導者座標系的位置是 $P^L = [X^L_P, Y^L_P, Z^L_P]$ ，轉到跟隨者座標系、再轉到相機座標系，對時間微分後得到特徵點在相機中的運動。用針孔相機模型 $Z^C_P\,p_h = KP^C$ 投影到正規化影像座標 $s = [\xi, \eta]$ ：

\xi = \frac{X^C_P}{Z^C_P}, \qquad \eta = \frac{Y^C_P}{Z^C_P} = \frac{Z^F_C - Z^L_P}{Z^C_P}

相機和特徵的高度差 $h = Z^F_C - Z^L_P \neq 0$ 已知，相機安裝偏移 $[X^F_C, Y^F_C] = [t_x, t_y]$ ，代入後得到：

\dot{s} = J_s(s)\,u_F, \qquad J_s(s) = \begin{bmatrix} \dfrac{\xi\eta}{h} & 1 + \xi^2 + \dfrac{\eta(t_x - \xi t_y)}{h} \\[2ex] \dfrac{\eta^2}{h} & \xi\eta - \dfrac{\eta^2 t_y}{h} \end{bmatrix}

這就是 IBVS 中的影像雅可比矩陣，把機器人運動映射成影像點的運動。

擴充狀態：為什麼要加朝向

如果狀態只用特徵座標 $s$ ，因為差速底盤的非完整約束，可能陷入局部極小值、產生穩態誤差——影像點看起來對齊了，但機器人其實是斜的。受 2.5D VS 啟發，把狀態擴充成影像座標 + 兩車相對朝向：

x = [s_0^\top, \ldots, s_{n-1}^\top, \theta^F_L]^\top \in \mathbb{R}^{2n+1}

$n$ 是特徵點數量。期望狀態是 $x^* = [s_0^{*\top}, \ldots, s_{n-1}^{*\top}, 0]^\top$ ，最後的 0 確保朝向對齊。控制目標是 $\lim_{t\to\infty}\lVert x(t) - x^*\rVert = 0$ 。

加上干擾

推推車時，推車重量是一個變動負載，讓跟隨者的速度明顯波動；不平的地面、摩擦變化又帶來外部干擾。把干擾加進模型：

\dot{x} = J_x(x)\,u_F + d, \qquad J_x(x) = \big[J_s(s_0)^\top, \ldots, J_s(s_{n-1})^\top, J_\theta^\top\big]^\top, \quad J_\theta = [0, -1]

集總干擾 $d = [d_s^\top, d_\theta]^\top$ 包含負載變化和地形振動造成的影像特徵干擾 $d_s$ ，以及輪子打滑、估計誤差造成的朝向干擾 $d_\theta$ 。

方法

狀態量測：從影像到狀態

門檻分割：灰階影像中亮度超過門檻的像素保留，其餘設為 0
光斑偵測：找出連通區域，用影像矩算出每個光斑的次像素級質心

M^i_{pq} = \sum_{(u,v)\in b_i}u^pv^qI_g(u,v), \qquad p_i = \Big[\frac{M^i_{10}}{M^i_{00}},\ \frac{M^i_{01}}{M^i_{00}}\Big]

找出真正的紅外線特徵：複雜光線環境中，可能有其他設備發出同波段的光。利用紅外線標記的空間配置和幾何約束，從候選點中挑出有效特徵並建立對應關係
用預測加速匹配：組合式點匹配在點很多時計算量很大，改用相機運動和特徵點光流的關係，根據上一刻的量測預測下一刻的特徵座標：

s(t_m + \Delta t_m) = s(t_m) + J_s(s(t_m))\,u_F(t_m)\,\Delta t_m

偵測到的點如果離最近的預測點夠近，就沿用上一次的匹配結果

相對朝向：利用特徵點共平面，估計目前影像和期望影像之間的單應矩陣（homography），分解後得到繞相機 y 軸的相對旋轉 $\theta^F_L$

干擾觀測器（ESO）

把總干擾當成一個額外的狀態，即時估計和補償，不需要事先知道干擾的動態：

\dot{x} = J_x(x)u_F + d, \qquad \dot{d} = h(t)

$h(t)$ 是干擾未知但有界的時間導數。觀測器：

\dot{\hat{x}} = J_x(x)u_F + \hat{d} + L_1(y - \hat{x}), \qquad \dot{\hat{d}} = L_2(y - \hat{x})

$\hat{x}$ 、 $\hat{d}$ 是狀態和干擾的估計， $y$ 是量測（假設為完整狀態）， $L_1$ 、 $L_2$ 是觀測器增益矩陣。

穩定性分析：觀測誤差 $\varepsilon = [\tilde{x}^\top, \tilde{d}^\top]^\top$ 的動態為 $\dot{\varepsilon} = A\varepsilon + Dh(t)$ ，

A = \begin{bmatrix}-L_1 & I \\ -L_2 & 0\end{bmatrix}, \qquad D = \begin{bmatrix}0 \\ I\end{bmatrix}

只要適當設計 $L_1$ 、 $L_2$ 讓 $A$ 成為 Hurwitz 矩陣（所有特徵值實部為負），就能證明觀測誤差一致終極有界：

|\varepsilon_i(t)| \leq \bar{\mu}\,a(0) + \kappa(\alpha + \beta)

初始誤差為零時，誤差上界只剩 $\kappa(\alpha + \beta)$ 。加大觀測器增益可以縮小誤差上界，但實務上要在估計精度和雜訊放大之間取得平衡。

視覺預測控制

不考慮干擾，用前向歐拉法離散化，得到名目預測模型：

\bar{x}_{k+1} = \bar{x}_k + \Delta t_c\,J_x(\bar{x}_k)\,\bar{u}_k

在預測時域 $N_p$ 內最佳化控制序列：

\begin{aligned} \min_{\bar{u}_0,\ldots,\bar{u}_{N_p-1}}\ & \lVert\bar{x}_{N_p} - x^*\rVert^2_P + \sum_{k=0}^{N_p-1}\lVert\bar{u}_k\rVert^2_R \\ \text{s.t.}\ & \bar{x}_{k+1} = f(\bar{x}_k, \bar{u}_k), \quad \bar{x}_0 = x_{init} \\ & s_{min} \leq \bar{s}_k \leq s_{max} \quad \text{（視野約束）} \\ & u_{min} \leq \bar{u}_k \leq u_{max} \quad \text{（速度上下限）} \\ & \Delta u_{min} \leq \bar{u}_{k+1} - \bar{u}_k \leq \Delta u_{max} \quad \text{（速度變化上下限）} \end{aligned}

視野約束直接寫在影像座標上——特徵點不能跑出畫面，這是 VPC 相對傳統 IBVS 的一大優勢。

干擾補償

每個控制時刻，量測目前狀態、用 ESO 估計干擾 $\hat{d}$ 、解名目 VPC 得到 $\bar{u}_0$ 。實際的系統是 $\dot{x} = J_x(\bar{u}_0 + u_C) + \hat{d}$ ，所以設計補償項 $u_C$ 在最小平方意義下抵銷干擾：

\min_{u_C}\ \lVert J_x(x)\,u_C + \hat{d}\rVert^2 \quad\Rightarrow\quad u_C = -J_x^\dagger\hat{d}

最終控制律：

u_F = \text{sat}\big(\bar{u}_0 - J_x^\dagger(x)\,\hat{d},\ u_{min},\ u_{max}\big)

$J_x^\dagger$ 是 Moore–Penrose 偽逆矩陣，飽和函數確保總控制輸入不超過物理限制。名目控制 $\bar{u}_0$ 負責達成目標，前饋補償負責主動排除干擾。

shell

演算法：DA-VPC
輸入：目前影像、期望影像、相機安裝位置、特徵安裝位置、容許誤差
初始化：系統狀態、觀測器狀態，從期望影像算出 x*
while ‖x − x*‖ > 容許誤差:
    門檻分割 + 影像矩 → 候選光斑質心
    依空間配置挑出特徵、算出正規化座標
    單應分解 → 相對朝向
    ESO 估計干擾 d̂
    解名目 VPC → ū₀
    最小平方 → 補償 u_C
    套用 u_F = sat(ū₀ + u_C)
    預測下一刻特徵位置（供匹配用）

實作細節

每台機器人一台 Intel NUC（i7-1165G7、32 GB RAM），Ubuntu 20.04 + ROS2 Galactic
控制問題用 CasADi 即時建模求解
感知 60 Hz、控制 20 Hz

評估指標：

位置誤差 $E_p$ ：終端與期望位置的法向誤差 $e_x$ 和側向誤差 $e_y$ （在領導者座標系）
狀態誤差 $E_s$ ：角度誤差 $e_\theta$ ，以及 $n$ 個特徵點在像素座標的 RMS 誤差 $e_p$
平滑度 $M_{sm}$ ：每次試驗中速度變化量的 RMS

精度要求來自機器人致動器和推車的關鍵尺寸：第一台推車直接對接領導者的致動器，側向誤差上限 6 公分；之後每台推車對接前一台，側向誤差上限 10 公分。

實驗結果一：模擬比較

領導者固定在原點，跟隨者的期望對接狀態是 [1.5 m, 0 m, 0°]，模擬參數完全對應實體硬體。加入高斯感測雜訊（特徵點 3 像素、相對朝向 π/60 rad），從特徵點可見範圍內 100 個隨機初始位置模擬：

方法	$e_x$ (cm)	$e_y$ (cm)	$e_\theta$ (°)	$e_p$ (pixel)	$M_{sm}$ v (m/s)	$M_{sm}$ ω (rad/s)
基本 IBVS	5.410	31.827	12.375	1.834	0.011	0.003
Ke 等人	2.604	0.764	4.559	44.599	0.064	0.204
本文	1.431	0.598	0.240	1.476	0.010	0.057

基本 IBVS：只是讓特徵點對齊期望值，但因為非完整約束，產生不出適當解耦的角速度指令，朝向誤差逐漸累積，最後有明顯的穩態姿態偏差（側向誤差 31.8 公分）
Ke 等人的方法：能抵達期望的終端姿態，但傾向先縮小位置誤差、再修正朝向。這種依序策略讓影像特徵大幅漂移（像素誤差 44.6），增加特徵跑出視野的風險；在平面上走出的彎曲軌跡也不利於穩定對接
本文：主動調整朝向，走出平滑的對接軌跡，同時滿足精度要求並維持平穩的控制指令

實驗結果二：干擾排除

跟隨者初始狀態 [6.2 m, 1.6 m, −45°]，在對接路徑上隨機擺放橡膠墊製造未建模的干擾。在 6 m × 6 m、配有動作捕捉系統（0.1 mm 精度）的區域，比較只有名目模型的 VPC 和 DA-VPC：

第一階段（0～11 秒）：機器人大幅轉向，加上遠距離時角度估計誤差，干擾估計和補償都出現尖峰式波動
第二階段（11～16 秒）：機器人壓過橡膠墊，觀測器成功捕捉到影像點和角度受到的明顯干擾，補償項主動抵銷
第三階段（16～22 秒）：離開干擾區，干擾估計和補償都收斂到零

整個過程中影像點的干擾逐漸增加——依針孔模型，相機越靠近特徵點，影像點的變化率越容易受干擾——但仍在可接受範圍內。

結果：DA-VPC 的側向對接誤差 0.4 公分，只用名目 VPC 是 4.6 公分。

實驗結果三：連續對接 8 台推車

跟隨者依序對接 8 台推車，每台 10 次、共 80 次，初始配置隨機，用動作捕捉記錄：

第一台：側向誤差平均 1.69 公分、角度誤差平均 0.84°，遠低於 6 公分的上限
之後的推車：隨著推車增加，期望的特徵影像越來越小（離領導者越來越遠），對接難度增加，側向和角度誤差都逐漸變大；但所有側向誤差都低於 10 公分的上限

實驗結果四：多種真實場景

除了這套視覺系統，實驗中沒有使用任何其他感測器：

黑暗戶外：AprilTag 這類非主動視覺特徵在這種環境偵測不到。刻意設定約 2 公尺的初始側向誤差來對接第一台推車；2 秒後朝向誤差先增加再收斂到零，跟模擬結果一致，證明能處理非完整約束
地下停車場：光線閃爍、地面不平，仍成功對接第二台推車
靠近陽光照射的窗戶、反光地板和玻璃隔間：從 8 公尺外開始對接第五台推車，系統穩定運作。資料顯示兩車距離遠時角度量測雜訊很大，但系統對角度波動很有韌性，證明干擾排除策略有效

限制與未來方向

紅外線方法雖然確保了目標特徵擷取的精度，但本質上限制了對外部環境的感知——濾鏡擋掉了可見光，看不到行人和障礙物，需要再擴充才能做動態避障
未來會探索以視覺為基礎的全身控制、協作推車運送系統，並用加速運算提升即時性

影響與定位

DA-VPC 的貢獻可以拆成三層：

感知：主動紅外線標記 + 窄頻濾鏡，用極低成本換來在黑暗、強光、閃爍、反光環境下都穩定的特徵
控制：擴充狀態（影像座標 + 相對朝向）解決非完整約束下的局部極小值問題；VPC 把視野約束直接寫進最佳化
穩健性：ESO 把無法建模的干擾當成狀態估計出來，再用前饋補償，有理論上的誤差界

跟系列前作比較，對接控制的演進：

論文	對接控制	量測來源	處理干擾
多推車回收（2024）	CLF-CBF-QP	RGB-D 點雲（接近）、LiDAR 反光標記（對接）	無
DA-VPC（2025）	視覺預測控制 + ESO	單眼相機 + 主動紅外線 LED	ESO 估計 + 前饋補償

參考資料

Pang, Y., Xia, B., Zhang, Z., Sun, Z., Xie, P., Zhu, B., Xu, W., & Wang, J. (2025). DA-VPC: Disturbance-Aware Visual Predictive Control Scheme of Docking Maneuvers for Autonomous Trolley Collection. arXiv:2509.07413
Malis, E., Chaumette, F., & Boudet, S. (1999). 2 1/2 D Visual Servoing. IEEE Transactions on Robotics and Automation.