回到課程地圖 第 八 講

RRT、車輛運動學與感測器標定

上週的路徑規劃只保證幾何上無碰撞。這一講先回顧組態空間、佔據格網與 PRM 的限制,把 RRT 從取樣、steer 到 RRT* 講完,接著問:車子真的走得出這條路徑嗎?講者以智慧駕駛系統為例,說明非完整約束(不能橫移)、運動學自行車模型與轉向運動學(最小轉彎半徑、轉向角上限)如何限制可行運動,以及為什麼動態環境需要把路徑變成含時間的軌跡,並用代價與限制做軌跡最佳化。下半場回到感知:障礙物位置來自多感測器,要把 LiDAR 點投影到相機影像,就得做內參與外參標定;講者分享標定若只涵蓋近距離樣本、80 公尺外就會失準的親身經驗,並指出感知、標定、預測、物理模型與規劃的誤差都會向下游傳遞,預告下週要談如何系統化評估系統的安全。

課堂錄影約 172 分鐘

完整課堂錄影
LEARNING PLAN

完成本講後,你應該能

重點學習約 120–160 分鐘;若觀看完整影片約 172 分鐘(含兩次下課)
  1. 逐步說明 RRT 的取樣、最近節點、steer 與無碰撞檢查,並比較目標偏向的利弊與 RRT*
  2. 區分完整約束與非完整約束,並說明為何光靠非完整約束不能解釋「不能瞬間轉彎」
  3. 用運動學自行車模型與轉向運動學計算最小轉彎半徑,說明它對路徑可執行性的意義
  4. 區分路徑與軌跡,說明軌跡最佳化中目標(安全、舒適、效率)與限制的角色
  5. 說明 LiDAR 到相機的投影需要內參與外參標定,並解釋標定誤差與其他誤差如何沿 sense–plan–act 往後傳

建議先備:Lecture 7 的組態空間、佔用網格與 PRM、Lecture 5 的座標轉換與針孔相機模型、基本向量與導數(速度、加速度)

核心詞彙:RRT/RRT*非完整約束運動學自行車模型軌跡最佳化LiDAR–相機外參標定

尚未開始0 / 26 項核心檢核
從本講開始

答案與進度只儲存在這個瀏覽器,不會上傳;更換裝置或清除網站資料後無法同步。

快速掌握

本講核心:上週的路徑規劃只保證「幾何上無碰撞」,這一講問的是:這樣的路徑,車子真的走得出來嗎?講者先把 RRT 講完(作業二要實作的演算法),再以智慧駕駛系統為例回答:車輛有運動學限制(不能橫移、轉彎半徑有下限、方向不能瞬間改變),所以路徑還得轉成隨時間變化、滿足限制的軌跡;而規劃用的障礙物位置,又來自多感測器標定這件看似瑣碎、實際上很容易出錯的工作。最後的訊息是:感知、標定、預測、物理模型與規劃都會有誤差,而且會沿著 sense–plan–act 一路往後傳,所以「感知完美、一路順暢」的循序流程在真實世界行不通。

  1. RRT 不像 PRM 先建整張圖,而是從起點開始反覆「取樣、找最近節點、朝它走一小步、無碰撞才加入」,逐步長出一棵樹;取樣時偏向目標能更快抵達,但探索會變少,RRT* 再用直線連接來縮短路徑。
  2. 完整約束限制組態(例如粒子必須在圓上),非完整約束限制速度(車子不能直接橫移);光靠非完整約束還不能說明為什麼不能瞬間轉 90 度,還要加上運動學自行車模型與轉向運動學(最小轉彎半徑、轉向角上限)。
  3. 路徑只描述空間,軌跡同時描述空間與時間;在動態環境裡,同一條幾何路徑可能安全也可能相撞,差別只在到達時間。軌跡最佳化把安全、舒適、效率寫成代價,把動力學與物理限制寫成約束。
  4. 要知道障礙物在哪,就得把 LiDAR 與相機的資料放進同一個座標系:先做內參標定,再用棋盤格估計 LiDAR 到相機的外參(旋轉與平移);標定若只涵蓋近距離樣本,遠處的投影就會偏掉。
  5. 感知誤差、標定誤差、預測誤差、物理模型誤差與規劃失敗,都會往下游傳;下一講預告:感知、預測與物理假設被挑戰時,要怎麼系統化地評估系統仍然安全。

回顧:從重建地圖到導航的流程

看老師講解(13:40)
A Robot Navigation System: SLAM builds a map, then localization in the map
導航系統的高階流程:多張影像經 SLAM 重建出地圖(作業一);把自己放進地圖、做定位,再知道障礙物在哪裡,才能規劃不碰撞的路徑,行動後再觀測、再定位。 YouTube · 13:40

講者開場先回顧前幾週,並把上週留下的三件事重述一次:表示物理限制(要同時考慮實體代理本身和環境)、規劃有目標導向的路徑(給定起點與目標,找出從起點到目標的路徑),以及把規劃銜接到實際的物理執行——後者是這一講的重點。第 5 講的 SLAM 假設有深度感測器所以能重建 3D;第 6 講拿掉這個假設,介紹用 structure from motion(只能得到相對深度)、立體相機(用已知基線得到公制深度,但不太適合戶外)估計深度;今天要談的 LiDAR 更適合戶外。

整條導航流程是:有一堆影像,重建出地圖(也就是你們正在做的作業一),再把自己放進地圖、拍一張照片取得觀測,判斷自己在地圖中的位置(定位),同時知道障礙物在哪裡,規劃一條不會碰撞的路徑(路徑規劃),然後在環境中行動,下一階段再觀測、再定位。講者也再次釐清路徑的定義:路徑是幾何空間中的一連串點,路徑尋找只關心起始組態到目標組態之間的無碰撞路徑,通常不考慮動力學(作用在機器人上的力與所產生運動之間的關係);把這些納入的是接下來一小時的運動規劃。

回顧:組態空間、佔據格網與維度爆炸

看老師講解(19:20)
From visual observations to a navigable map pipeline
從視覺觀測到可導航的地圖:RGB 影像 → 深度估計 → 3D 點雲 → 佔據格網 → 組態空間 → 路徑規劃。 YouTube · 19:20

組態是對機器人上每一點位置的完整描述,自由度是表示組態所需的最少實數座標數,組態空間是包含所有可能組態的 n 維空間。講者沿用全向 2D 機器人的例子(只能平移、不能旋轉,自由度為 2):考慮機器人本身的大小之後,障礙物會「長胖」成組態空間中的障礙物,剩下的白色區域才是自由空間,這時就能把機器人當成組態空間裡的一個點來規劃。佔據格網則是把連續環境切成固定解析度的格子(例如 50 公分),格子裡只要有東西就標成被佔據;轉成組態空間時還要把機器人的實體大小考慮進去。

把規劃當成搜尋問題時,每個格子是一個狀態,動作是往相鄰格子走一步,每步代價為 1,轉移模型說明採取動作後會到哪個狀態;解是一串把初始狀態帶到目標狀態的動作(A* 搜尋等做法上週沒細講,這裡也略過)。但自由度一高,離散化就撐不住:2D 移動機器人每維切成 100 格是 1002,七自由度機械手臂是 1007,24 維的靈巧手就是 10024,組態數量多到無法逐一追蹤。因此要改為不窮舉,只聰明地探索組態空間的一小部分,也就是取樣式規劃。

回顧:PRM 的困難與機率完備性

看老師講解(24:50)
Challenges in PRM: narrow passage, more samples, samples near C-space obstacles
PRM 的挑戰:取樣太少時,路線圖會分裂成互不連通的兩張圖,即使路徑存在也找不到;解法是取樣更多點,或在靠近組態空間障礙物處多取樣。 YouTube · 24:50

PRM 分兩階段:先在自由空間取樣建出路線圖,再在圖上搜尋路徑。它的問題是,即使路徑存在,取樣過程也可能沒取到合適的點,讓圖分成彼此不連通的兩塊,因此找不到從起點到目標的無碰撞路徑。講者提到兩種補救:持續取樣更多點(取樣夠多就能在狹窄通道連起兩張圖),以及在靠近組態空間障礙物的地方多取樣。

這引出機率完備性:在適當假設下,若可行路徑存在,取樣數量趨近無限多時,規劃器找到解的機率趨近 1。但這個保證在實務上不夠用——如果可用的計算時間有限,就沒辦法保證一定找得到解。於是問題變成:能不能換個方式做?這就是 RRT。

重點:機率完備性只說「取樣無限多、時間無限長」時會找到;真實系統的計算時間有限,所以要看演算法在有限預算下能不能有效探索。

RRT:一步一步長出一棵樹

看老師講解(27:25)
General idea of the RRT algorithm
RRT 的核心想法:從起始組態出發,以反覆擴展搜尋樹的方式找路徑,而不是像 PRM 先建整張圖。 YouTube · 27:25
RRT pseudocode and an animation with polygon obstacles
RRT 的虛擬碼與動畫:隨機取樣 x_rand → 找最近節點 x_nearest → steer 出 x_new → 無碰撞才把點與邊加入樹。 YouTube · 29:30

RRT(快速探索隨機樹)是作業二要實作的演算法;講者說作業會要求在模擬環境裡實作 RRT,指定目標組態(例如冰箱),再用它找出從初始組態到目標的路徑。他用黑板式的步驟示範:樹一開始只有起點一個頂點、沒有邊;每一輪先隨機取樣一個點,找出樹上離它最近的節點,然後做 steer——從最近節點朝取樣點的方向走一小步(固定步長);如果這一步以及連線都無碰撞,就把新點加進頂點集合,並把連線加進樹的邊。

講者逐輪示範幾種情況:取樣點離最近節點比步長還近時,直接把它加進樹;連線會穿過障礙物時,不接取樣點,而是在這條路徑上的安全範圍內取一個點;取樣點落在障礙物裡面,就直接丟掉。反覆這樣做,樹就會一路長出去。他也強調這跟 PRM 不同:PRM 一開始就取樣許多點再連起來,RRT 則是每一步取樣、延伸一小步。

目標偏向:探索與利用的取捨

看老師講解(33:00)
How could we leverage goal information: less vs more samples picking the goal
取樣時以一定機率直接選目標:左邊很少選目標,樹均勻探索整個空間;右邊多選目標,樹更快長向目標,但涵蓋範圍較小。 YouTube · 33:00

到目前為止,每次取樣都是在組態空間裡隨機取點,其實可以利用目標的資訊:以某個機率直接取樣目標組態本身。如果很少選目標,樹基本上是在隨機探索,對整個空間涵蓋得比較完整;如果多花一些取樣在目標上,樹會比較快抵達目標,但有些區域沒被探索到,也比較難找出組態空間裡其他可能的自由空間。講者把這個取捨稱為探索(exploration)與利用(exploitation):多花時間探索,或多花時間利用目標資訊以更快到達。

重點:目標偏向不是越高越好——偏向越多,樹越快長向目標,卻越容易漏掉繞路的自由空間。

RRT 的路徑品質與 RRT*

看老師講解(34:20)
Quality of paths generated by RRT
RRT 產生的路徑離最佳解很遠,要再經過改善才真正能用。 YouTube · 34:20
RRT*: pick two nodes and shortcut with a straight line
RRT*:隨機挑兩個節點,若能用不碰撞的直線連起來就連,逐步縮短路徑;結果仍不是最佳,但更短、更有效率。 YouTube · 35:05
RRT vs RRT* demo: path length 36.95 vs 34.51
RRT 與 RRT* 的比較(網頁示範):右邊 RRT* 的路徑更短也更平滑,因為由許多不同的直線段接成。 YouTube · 35:40

RRT 因為是反覆隨機取樣再 steer,產生的路徑並不最佳,看起來彎彎曲曲,要再改善才真正能用。另一個演算法 RRT* 的做法是:隨機挑兩個節點,檢查它們中間能不能用直線連起來而不碰撞;如果可以,就把它們連起來,路徑因此縮短,解會逐步改善,接著再取樣另外兩個點重複。最後的結果仍不是最佳路徑,但比原本更短、更有效率。講者用網頁示範並排比較:左邊 RRT 比較不平滑,右邊 RRT* 看起來平滑得多。這是第二種取樣式方法,也是作業要實作的東西。

本講的問題:無碰撞的路徑一定走得出來嗎?

看老師講解(36:15)
Can a geometrically collision-free path still be unsafe or physically infeasible?
承接上週的結論:幾何上無碰撞的路徑,仍然可能不安全,或物理上不可行嗎? YouTube · 36:15
Focus of today's lecture: model, plan, connect
本講三個重點:建立物理運動的模型(運動學與非完整約束)、規劃可執行的軌跡、把感知連結到實際動作;核心問題是智慧駕駛系統如何整合感知、車輛動力學與運動規劃。 YouTube · 36:50

講者以智慧駕駛系統為例,說明找到一條幾何上無碰撞的路徑之後,可以拿它來控制車輛,但中間還有落差。三個重點是:一、建立物理運動的模型,了解車輛運動學和非完整約束如何決定可行的運動;二、規劃可執行的軌跡,把幾何路徑轉成滿足物理限制、隨時間變化的軌跡;三、把感知連結到實際動作,對齊多個感測器的觀測,建立一致的世界表示供運動規劃使用。

他接著播放 Google 2009 年自駕車早期的影片:車頂的 LiDAR 與相機主動感測周遭環境,偵測其他參與者的距離、速度與狀態,辨識紅燈或黃燈,並預測其他車輛未來的狀態;影片中的車偵測到右邊的車停下來,在路口等有車通過再重新調整位置後左轉。這是智慧駕駛系統在環境中實際行動的樣子。

為什麼車子不能照著每條無碰撞路徑走

看老師講解(40:20)
Why cannot a car follow every collision-free path? A path with a 90-degree turn
路徑 A:(0,0) → (5,0) → (5,5),由 A* 產生;幾何上若無障礙物就是合法的,但車子在 (5,0) 要瞬間急轉 90 度。 YouTube · 40:20

講者用最簡單的例子:2D 空間裡,A* 產生一條先直行、到某點再急轉 90 度向上的路徑,幾何上有效、也無碰撞。但真正的車子做不到——想像你自己開車,要瞬間轉 90 度是不可能的。我們憑常識知道,但有沒有模型能解釋為什麼?他列出三個問題:車子能不能直接橫向移動?能不能用任意小的半徑轉彎?能不能瞬間改變速度?這些就是讓車子無法照著特定路徑走的原因。

要分析這些,講者引入運動學(kinematics,描述點與物體的運動,考慮位置、速度與加速度)與動力學(kinetics,探討運動的成因:力與力矩)。這一講只談運動學,不談動力學。

完整約束:對組態的限制

看老師講解(42:12)
Kinematics vs. kinetics
運動學描述位置、速度、加速度;動力學描述運動的成因(力與力矩)。 YouTube · 42:12
Holonomic constraints definition and constraint function
完整約束是對組態的約束:用約束函式 f(q1,…,qn,t)=0 判斷某個組態是否滿足特定的幾何約束,例如把粒子限制在 z=0 的 xy 平面上。 YouTube · 43:15
Example: particle constrained to a circle
例子:粒子必須沿著半徑 R 的圓移動,約束函式為 f(x,y)=x²+y²−R²。R=5 時 (3,4) 有效,(1,2) 無效。 YouTube · 44:30

完整約束(holonomic constraint)是對組態的約束。假設粒子用 (x, y, z) 表示,自由度是 3;用 z = 0 這個約束把它限制在 xy 平面上,就只剩在平面上移動。一般形式是約束函式 f 接收組態變數 q1 到 qn,有時還帶有時間 t(約束可能隨時間改變),條件是 f = 0。再舉圓的例子:粒子被限制在半徑 5 的圓上,約束函式是 x² + y² − 25;把 (3, 4) 代入得 0,是有效組態;(1, 2) 代入不為 0,不在圓上。完整約束會直接限制組態空間,也就是系統可以出現在哪些位置。

非完整約束:對速度的限制

看老師講解(47:00)
Non-holonomic constraints on velocity: x-dot = v cos(phi), y-dot = v sin(phi)
車輛以 (x, y, φ) 參數化,速度為 ẋ = v cos φ、ẏ = v sin φ;推得的約束 ẋ sin φ − ẏ cos φ = 0 無法寫成只含組態的 f(q,t)=0。 YouTube · 47:00
Holonomic vs nonholonomic constraints comparison table
比較:完整約束限制組態(系統可以在哪裡),非完整約束限制可行速度(系統可以怎麼動),對規劃的影響分別是限制組態空間,與限制組態空間中實際可行的路徑。 YouTube · 49:00

非完整約束(nonholonomic constraint)是對速度的約束。講者把車輛用 x、y 與航向角 φ(範圍 0 到 2π)參數化,x 方向速度是 v cos φ,y 方向速度是 v sin φ;整理後得到 ẋ sin φ − ẏ cos φ = 0,意思是車子不能自由往任意方向移動,例如橫向。他舉例:車頭朝北(φ = π/2)時,x 方向速度必為零。也就是說,可控制的自由度比總自由度少——原本三個自由度,在這個約束下只剩兩個。

為什麼叫「非完整」?因為這個約束只限制瞬間的運動方向,是對速度的微分形式限制;就算積分,也得不到只跟位置有關的等價約束(無法寫成完整約束的形式)。比較起來:完整約束的例子是「點被限制在圓上」,回答「系統可以在哪裡」;非完整約束的例子是「車子不能直接橫移」,回答「系統可以怎麼動」。

非完整約束還不夠:方向可以任意快地改變嗎?

看老師講解(52:50)
Why nonholonomic constraints matter in motion planning, with the (5,0) sharp turn
回到 (0,0)→(5,0)→(5,5):在 (5,0) 朝向要從 0 變成 π/2,速度要從 (v, 0) 瞬間變成 (0, v)——必須先轉動朝向,不能直接讓 y 方向有速度。 YouTube · 52:50

運動規劃是找出機器人從起始狀態到目標狀態的運動,避開障礙物,同時滿足其他約束(例如轉向角限制)。講者回到 (0,0)→(5,0)→(5,5) 的例子:幾何上有效,但實際做不到,因為車子在非零速度下無法瞬間改變方向。朝向要從 0 改成 π/2,意味著 x 方向的速度要從 v 變成 0、y 方向的速度要從 0 變成 v;要往北走,得先把朝向轉過去。

但他特別提醒:非完整約束本身並不禁止朝向任意快速地改變,它只告訴我們不能橫向移動。要限制朝向變化的快慢,需要另一個概念:轉向運動學(steering kinematics)。接著進入下課休息。

重點:非完整約束解釋「為什麼不能橫著走」,卻解釋不了「為什麼不能瞬間轉彎」;後者需要轉向運動學與車輛模型。

座標系與車輛狀態:組態空間 vs. 狀態空間

看老師講解(1:10:50)
Vehicle states x, y, phi, v and the state vector
車輛狀態:x(t)、y(t) 為位置,φ(t) 為航向,v(t) 為速度,狀態向量 x(t) = [x, y, φ, v]T。 YouTube · 1:10:50
Lecture 7 configuration space vs Lecture 8 state space
第 7 講的組態空間描述車輛組態,只看幾何可行性、產出無碰撞路徑;第 8 講的狀態空間還包含運動狀態,要檢查物理可行性與軌跡在動力學、運動學上是否可行。 YouTube · 1:11:40

休息回來後,講者先快速回顧前面的結論,再介紹車輛的座標系:固定在地球上的慣性座標系(z 軸垂直、xy 為水平面)、固定在車輛參考點上的車輛座標系(xv 指向車頭、yv 指向側邊、zv 指向車頂),以及原點在車輛參考點、由 xv、yv 投影到水平面得到的水平座標系。車輛狀態則用 x(t)、y(t)、航向 φ(t) 與速度 v(t) 表示。

講者把第七講與第八講做了對比:第七講的組態空間(x, y, θ)只描述車輛的組態,關注幾何可行性,目標是產生無碰撞路徑;這一講的狀態空間多了速度 v,描述組態與運動狀態,要檢查的是物理可行性——也就是這條「路徑」(應該說軌跡)在動力學和運動學上是否可行。

點與剛體的運動學

看老師講解(1:13:40)
Notation: kinematics of a rigid body
剛體運動學的符號:C 為固定在剛體上的參考點,P 為任意點,ω 為角速度;rP = rC + rCP,vP = vC + ω × rCP;剛體有 6 個自由度(3 位置、3 旋轉)。 YouTube · 1:13:40

先從簡單的開始:點的運動學。位置 r(t) 是三維空間中隨時間變化的一個點,速度和加速度分別是位置的一階與二階導數。剛體則是由無限多個無窮小質點組成、彼此以剛性連接的物體(相對位置不隨時間改變);講者拿手上的杯子(不看把手)當例子:杯身所有點的相對位置都不變,一動就同時動,所以整個運動可以用任意一個參考點 C 的運動來描述。

由於剛性,剛體上任一點 P 只能繞參考點 C 旋轉:位置 rP = rC + rCP,速度 vP = vC + ω × rCP。剛體共有六個自由度——三個位置、三個旋轉。把剛體擴展到車輛時,車輛繞著某個旋轉中心轉動,車上不同的點會沿著不同的圓形軌跡運動。

運動學自行車模型與狀態空間模型

看老師講解(1:15:10)
Kinematics bicycle model: approximates the 4 wheels with 2 imaginary wheels
運動學自行車模型:用兩個假想的輪子近似四個輪子,是對複雜車輛的簡化。 YouTube · 1:15:10
State space model x-dot = f(x, u), u = [a, delta]
狀態空間模型:用輸入 u(t) = [a(t), δ(t)](加速度與轉向角)控制車輛,狀態對時間的導數 ẋ = f(x, u) 給出每個狀態的變化率。 YouTube · 1:18:00

運動學自行車模型把四個輪子簡化成前後兩個假想輪子,參數包括前輪轉向角、前輪速度、航向角、重心、後輪轉向角與旋轉中心;車輛繞 z 軸旋轉的這個角度叫 yaw(偏航)。它有幾個假設:只做平面運動(沒有 roll、沒有 pitch)、只適用於低速、輪胎不打滑;真實駕駛當然不完全符合,若要模擬打滑,需要更複雜的動力學自行車模型。運動方程式中要考慮三個速度:x 方向、y 方向和航向角的變化率;細節不必擔心,重點是它提供了車輛運動的一個有用近似。

在狀態空間模型中,車輛透過系統的輸入被控制:狀態 x 包含位置、航向與速度,控制 u(t) 由加速度 a(t) 與轉向角 δ(t) 兩部分組成;送出控制輸入,模型就輸出各狀態的變化率,把狀態變成新的狀態。

最小轉彎半徑與轉向運動學

看老師講解(1:20:20)
Why does the kinematic bicycle model matter for planning? Minimum turning radius
自行車模型給出轉彎半徑 R = L / tan(δf);軸距 L = 2.8 m、最大前輪轉向角 30° 時,最小轉彎半徑約 4.85 公尺。 YouTube · 1:20:20
Steering kinematics: heading rate and bounded steering angle
轉向運動學:航向變化率受轉向角限制(|δf| ≤ δmax),因此車子不可能瞬間把航向轉 90 度。 YouTube · 1:21:10

為什麼自行車模型對規劃重要?因為要做出轉彎,就得滿足物理約束,其中一個是最小轉彎半徑。模型給出 R = L / tan(δf),L 是軸距,δf 是前輪轉向角。講者舉例:軸距約 2.5 到 2.8 公尺(投影片寫 2.8)、最大轉向角 30 度,最小轉彎半徑約 4.85 公尺——在這個模型的假設下,後軸中點無法走半徑小於這個值的圓形路徑。

再回到轉向運動學:航向變化率有上限(轉向角被限制在最大值以內,速度也有限),所以車子不可能瞬間改變航向 90 度。這個結論我們憑常識就知道,但非完整約束、運動學自行車模型與轉向運動學一起,才說明了「為什麼」。光靠非完整約束,禁止不了朝向任意快速地改變,因此才需要引入運動學自行車模型。

作業二的範圍與平行停車

看老師講解(1:25:30)
Parallel parking: instantaneous motion constraints do not prevent reaching the configuration
平行停車:車子不能橫移,仍可藉由一連串可行的運動(轉動方向盤、倒車、再轉方向盤)進入旁邊的車位。 YouTube · 1:25:30

講者說明作業二的範圍:作業二基本上把問題當成路徑規劃,控制只當成路徑追隨——沿著 A* 或 RRT 產生的路徑走,沒有考慮剛才講的運動限制,所以你會看到示範影片裡有些動作看起來很怪(沒有考慮平滑度,也沒有考慮動作是怎麼做出來的)。如果真的要控制一台真實的移動機器人,就必須考慮速度、航向與運動學模型;這次作業只讓同學專注在路徑上,他提這些是讓大家知道。

接著他用平行停車說明一個有趣的問題:如果車子不能橫向移動,怎麼最後能開進旁邊的停車位?答案是前後來回好幾次——瞬時的運動限制不一定讓系統到不了目標,只是不能一步到位;轉向角、倒車、再轉方向盤,一連串可行的運動就能把車停進去。他建議有興趣的同學搜尋 parallel parking,看運動學自行車模型如何解釋;並提醒:停車看似簡單,要讓車輛自動平行停車其實非常困難,不要低估。

從路徑到軌跡:為什麼時間很重要

看老師講解(1:28:15)
Why does time matter? Collision-free path is not enough in a dynamic environment
動態環境中,只有無碰撞路徑不夠:路徑描述空間關係,軌跡同時描述空間與時間,避障需要推論物體在同一時間會在哪裡。 YouTube · 1:28:15

路徑是車輛要走的幾何空間中的一串點;軌跡描述的則是車輛狀態如何隨時間變化。講者用兩個情境說明差別:情境 A,車輛 A 在第 5 秒到達路口、車輛 B 在第 10 秒到達,兩者佔用路口的時間區間不重疊,可以安全通過;情境 B,兩輛車都在第 5 秒到達,可能相撞。兩種情況下它們的幾何路徑完全相同,差別只在時間點。所以在動態環境中光有無碰撞路徑是不夠的,規劃時時間非常重要。

重點:同一條幾何路徑可以安全,也可以相撞;差別在於「什麼時候到」。這就是路徑與軌跡的差別。

軌跡最佳化:目標與限制

看老師講解(1:30:30)
Trajectory optimization with safety, comfort, efficiency costs and hard constraints
軌跡最佳化:在狀態空間模型、最大轉向角、加速度上下限、不可進入被佔用區域等限制下,最小化安全、舒適與效率三種代價的加權和。 YouTube · 1:30:30

講者快速介紹軌跡最佳化:這是一個很大的最佳化問題,要在一組限制下,求出最佳的車輛狀態與對應的控制輸入,再用控制輸入去控制車輛。目標函數裡有三種代價:安全代價,鼓勵車輛和障礙物、其他用路人保持適當距離;舒適代價,避免突然的加速度與急跳度(jerk),不讓乘客感到很猛的加速和急煞;效率代價,鼓勵車輛朝目的地有合理的進展。

他特別區分目標與限制:目標是我們想達成的(安全、效率、舒適),限制則是必須遵守的——例如轉向角最大 30 度,不能要求超過;加速度有極限(例如 0 到 100 要 6 秒,不能要求 3 秒);也不能撞到其他車輛或障礙物。他推薦史丹佛的運動規劃課程與教科書,想多了解的同學可以自行閱讀。這樣就說明了如何把 A* 或 RRT 給的幾何無碰撞路徑,變成動態環境中可以控制車輛的動作。

障礙物在哪裡:多感測器與投影問題

看老師講解(1:33:10)
How does the motion planner know where obstacles are located?
運動規劃器怎麼知道障礙物在哪裡?回到感知:RGB 影像 → 深度估計 → 3D 點雲 → 佔據格網 → 組態空間 → 路徑規劃。 YouTube · 1:33:10
Obtain information from multiple sensors: the KITTI test vehicle
實驗載具用多種感測器取得資訊:Velodyne 3D 光達、立體相機、GPS;講者回到第 4 講問過的問題。 YouTube · 1:34:30
Sensor Fusion: Camera & LiDAR - how can we project 3D LiDAR data onto the image?
問題:怎麼把 3D LiDAR 資料投影到影像上,產生這樣的畫面?之後就能同時用相機與 LiDAR 的資訊偵測障礙物的位置。 YouTube · 1:34:50

運動規劃要用佔據格網與組態空間,前提是知道障礙物在哪裡;而目前智慧駕駛系統通常用多種感測器,例如 Velodyne 這類 3D 感測器掃描環境,加上立體相機,再加 GPS 幫助定位。其中一種作法是結合兩種感測器:把 LiDAR 的 3D 點投影到影像平面,或反過來把影像反投影到 3D 空間;講者先從前者開始。

他請同學想想需要哪些關鍵資訊,並帶討論:有人提出用影像建立 3D 點的場景結構,但講者指出,那樣投影回去只會看到同一張影像;這裡的 LiDAR 點是另一個感測器量到的,位置和相機不同,為什麼卻能疊上影像?有同學提到要先知道相機的外參與內參再做轉換,也有同學指出兩者都剛性固定在車上,可以事先定義轉換矩陣;講者肯定後一點,並說明這件事其實一點都不簡單。

剛性固定、看不見的座標系,以及 KITTI

看老師講解(1:55:00)
KITTI vehicle sensor setup figure with translations between sensors
車上感測器的相對位置圖(放大):標示出 Velodyne 到相機的平移(x 方向 0.27 公尺)、IMU 到 Velodyne、各相機高度等具體數字,這些都要靠標定算出來。 YouTube · 1:55:00

講者說明為什麼不簡單。第一,所謂剛性固定,是要先架一個平台,把所有感測器牢牢裝在上面,不能隨便放在桌上,一動、一碰相機,它和車子之間的相對位置就變了;要把它們當成剛體,相對運動才不會因為車子行駛而改變。第二,即使感測器都裝好了,也要知道彼此之間的轉換:用尺只能量實體之間的距離,但針孔相機模型的原點(針孔)看不見,LiDAR 的座標系原點也看不見,所以必須用別的方法找出座標系位置、算出轉換。

投影片上寫著 Velodyne 到相機的平移是 0.27 公尺,這個數字需要標定才得到。講者介紹 Andreas Geiger 的博士論文:他花很長的時間專心研究怎麼標定車輛的感測器,並提出許多方法;論文第二部分則是 KITTI 資料集,大約在 2010 到 2012 年,當時幾乎沒有人公開發布自駕相關的資料集,他建立平台、標定感測器之間所有的轉換、收集並標註資料,讓社群能研究物件偵測、追蹤、光流等感知問題。講者想強調的是:這些繁瑣的細節常藏在論文與資料集背後,不要低估它們,每一個都可能造成問題。

內參與外參標定:把 LiDAR 點接到相機像素

看老師講解(2:19:30)
Camera intrinsic calibration with extrinsic and intrinsic parameters annotated
內參標定:焦距 fx、fy(像素單位)與主點 cx、cy,構成投影矩陣 K,負責把相機座標系中的 3D 點投影到像素;從世界到相機座標則需要外參。 YouTube · 2:19:30
Camera–LiDAR extrinsic calibration: P_cam = R P_lidar + t
相機—LiDAR 外參標定:Pcam = CAMRLiDAR PLiDAR + CAMtLiDAR,接著用針孔相機模型 λ[u v 1]T = K Pcam 投影。 YouTube · 2:22:20
From LiDAR coordinates to camera pixels: three steps
三步:量得 LiDAR 座標系下的 3D 點 → 用外參轉到相機座標系 → 用相機投影矩陣投到影像平面。 YouTube · 2:22:50

休息回來後,講者先釐清上一小時的重點:現在取得障礙物位置或狀態,基本上都靠多種感測器,這裡要結合 LiDAR 與相機。難處在於:轉換矩陣包含旋轉(三軸)與平移(三軸),用尺只能量出實體之間的距離,量不到座標系;旋轉也不能假設沒有,就算盡力把所有東西放在同一平面上,也不知道每個軸有沒有微小的旋轉,近處的物體或許還好,遠處就會看到偏差。

流程分兩部分:相機內參標定(焦距與主點,決定怎麼把相機座標系中的 3D 點投影到像素),與相機—LiDAR 外參標定(求兩個座標系之間的相對旋轉與平移)。投影一個 LiDAR 點要三步:先得到 LiDAR 座標系下的量測,再用外參轉到相機座標系(仍是 3D),最後用投影矩陣投到影像平面;得到的是齊次座標,要除以第三個元素才是實際像素位置。

怎麼估計外參:棋盤格、角點與平面法向量

看老師講解(2:25:30)
Calibration pipeline: find corners, plane fitting, rotation by normals, translation estimation, refinement
外參估計流程:同時收集 3D 點與影像 → 找棋盤格角點 → 在 LiDAR 與相機中各擬合一個平面 → 用兩個平面的法向量估計旋轉 → 估計平移 → 精修。 YouTube · 2:25:30

講者只做高階說明。典型設置是用一個棋盤格:它形成一個平面,可以量出相機到平面的距離,也可以量出 LiDAR 到平面的距離。首先收集 3D 掃描資料與影像;在影像裡偵測棋盤格的角點——棋盤格黑白相間、對比大,比較容易辨識角點;找到角點後擬合出相機裡的平面。同時對 LiDAR 點做同樣的事,落在棋盤格上的點可以擬合出另一個平面。

兩個平面各有一個法向量(一個來自 LiDAR 點,一個來自相機)。要解的問題是找出一個轉換,使兩個法向量套用後互相對齊,這就得到旋轉的估計;旋轉估計完成之後再做平移估計。關鍵想法就是在 LiDAR 與相機裡都找出平面。講者鼓勵有興趣的同學閱讀原論文,並強調這個流程其實不簡單。

標定錯了會怎樣:80 公尺外的教訓

看老師講解(2:29:40)
What happens when calibration is incorrect: iterative calibration results
標定不正確時,系統可能把物體對應到錯誤的 3D 位置,讓世界表示中的障礙物位置不準,直接影響運動規劃。四張圖由左至右是未標定與經過多次標定迭代的投影結果。 YouTube · 2:29:40

如果相機與 LiDAR 標定不正確,系統可能把感興趣的物體對應到錯誤的 3D 位置,或讓世界表示中的障礙物位置不準確,這會直接影響運動規劃。講者展示迭代標定的例子:完全沒標定時投影錯位;第一次迭代旋轉看起來沒問題,只是整體偏右;第二次又往左移一些但仍不準;幾次迭代之後,LiDAR 點才對上影像中相應的物體。

他分享自己的經驗:在前一家公司,主管要他確保 LiDAR 投影到影像的結果在 80 公尺外也正確——展示的圖大約只有三公尺。他用同樣的公式與工具卻花了一個月才解決。原因是估計時需要在相機和 LiDAR 之間建立很多組配對(通常收集約 100 到 200 個資料點),而他只用特定的姿態拿棋盤格,遠距離的樣本也不夠,等於讓最佳化演算法對少數樣本過擬合(overfit):近處看起來沒問題,遠處旋轉只要差一點點就會造成很大的投影誤差。

重點:照著流程做、近處看起來也對,不代表這個轉換夠用;要回頭問「我的需求是什麼」(80 公尺外也準),再確認資料有沒有涵蓋那個條件。講者的結論是:要親自動手,才找得出資料為什麼不夠好。

誤差沿著 sense–plan–act 一路往後傳

看老師講解(2:38:20)
Errors in perception can propagate through the entire Sense-Plan-Act pipeline
感知中的誤差,會在整個 sense–plan–act 流程中一路傳遞。 YouTube · 2:38:20

講者回頭看整條流程:立體相機量測遠處物體本來就有誤差;深度估計不會準確;投影成 3D 點雲後,物體位置不一定正確,可能有誤偵測或定位錯誤;即使再用雷射掃描儀投影到影像平面,如果標定不準,一樣不準。這些小誤差一個接一個出現,並往後面階段傳遞。例如建立佔據格網,我們把它當成確定的結果(這裡有障礙物),但量測有誤差、標定也可能有誤差,格網就會不準;若假設佔據格網沒有不確定性,再轉成組態空間、做路徑與運動規劃,其實是在一個想像的空間裡工作,障礙物的位置是有誤差的。

本講整理:三個訊息

看老師講解(2:42:20)
Takeaways: physical constraints, planning in space and time, reliable action needs consistent perception
三個重點:物理限制決定可行動作,無碰撞路徑不一定能被實體 agent 執行;運動規劃要同時推論空間與時間,軌跡指定如何隨時間移動;可靠的物理行動取決於一致的感知,運動規劃只和它所依據的世界表示與假設一樣可靠。 YouTube · 2:42:20

講者歸納今天的三個重點。第一,物理限制決定物理運動:無碰撞路徑不一定能被實體 agent 執行(完整約束、非完整約束、自行車模型與轉向運動學說明了不能瞬間轉彎)。第二,在動態環境中,時間非常重要,不只是產生無碰撞路徑,還要同時考慮空間與時間。第三,sense–plan–act 系統有一個根本的問題:我們常常拿不到一致的表示,因為一致的表示要仰賴完美的感知或完美的前置工作(像是標定),而這幾乎做不到。

他因此說,不能只靠循序的流程——「假設得到完美的量測、完美的 3D、完美的佔據格網和組態空間,然後規劃路徑」,這個典範適合用來理解如何在完美的世界裡控制機器人,但在真實環境裡行不通。我們需要不同的工具組。

哪裡會出錯:下週預告與公告

看老師講解(2:43:50)
A vehicle is approaching an intersection while a pedestrian begins crossing
情境:車輛接近路口,行人開始過馬路;流程是感知 → 世界表示 → 運動規劃 → 執行,假設每一步都能完美運作。 YouTube · 2:43:50
What could go wrong? Scenarios A to E
五種可能出錯:A 感知誤差(行人被部分遮擋)、B 標定誤差(感測器觀測在幾何上錯位)、C 預測誤差(行人突然改變方向)、D 物理模型誤差(路面濕滑使可用煞車力降低)、E 規劃失敗(運算期限內找不到可行軌跡)。 YouTube · 2:45:00
How can we systematically evaluate whether an intelligent driving system remains safe?
下週的問題:當感知、預測與物理假設受到挑戰時,如何系統化地評估智慧駕駛系統仍然安全? YouTube · 2:48:00

講者以路口的情境預告下週:從感知、世界表示、運動規劃到執行,哪裡可能出錯?感知誤差(行人被部分遮擋)、標定誤差(相機與 LiDAR 定位物體失敗)、預測誤差(行人突然轉 90 度、倒退走或加速)、物理模型誤差(運動學自行車模型只適用於規劃好的運動與低速,路面濕滑時抓不到,而路面有多濕本身也很難量測),以及規劃失敗(例如要併入壅塞的車道,系統可能在運算期限內找不到可行的軌跡)。這些挑戰從最簡單的硬體標定一直到非常複雜的運動,讓 physical AI 系統很難在真實世界運作;下週的主題就是如何系統化地評估:當感知、預測與物理假設被挑戰時,系統是否仍然安全。

結尾時講者提醒:做 physical AI 系統不能只當成「在電腦上輸入、它幫你產生文字」,要考慮物理世界與動態環境;而從感知到世界表示、運動規劃、執行之間,總是有不確定性與誤差。作業很快會公布,線上同學的資訊會發布到論壇,請向協同教師與學校的助教團隊詢問,拿不到作業規格可以告訴講者協調。課程專題正在整理,因為 GPU 資源有限,團隊想先統一協調分組,再與 GPU 供應商協調,讓每個人都能取得專題所需的運算資源,請大家到論壇查看最新資訊。

複習與理解檢核

這一講把「幾何上可行」與「物理上可行」分開來談。回答時請刻意指出:哪個限制作用在組態、哪個作用在速度、哪個作用在時間;以及哪一種誤差來自感知、標定、預測、模型或規劃。

一、RRT 與取樣式規劃

  1. PRM 為什麼可能在路徑存在的情況下找不到解?兩種補救做法分別針對什麼問題?
  2. 機率完備性保證什麼、不保證什麼?為什麼有限的計算時間讓它在實務上不夠?
  3. 依序說明 RRT 一輪迭代的步驟:取樣、最近節點、steer、無碰撞檢查、加入樹。
  4. 取樣點落在障礙物裡、或最近節點到取樣點的連線穿過障礙物時,RRT 各怎麼處理?
  5. 提高取樣目標的機率有什麼好處與代價?這和探索/利用的取捨有什麼關係?
  6. RRT 的路徑為什麼通常不最佳?RRT* 用什麼做法改善,結果為何仍不是最佳解?
作答檢核
  1. 取樣沒有涵蓋關鍵位置(如狹窄通道)時,路線圖會分裂成不連通的圖;補救是取樣更多點,以及在靠近組態空間障礙物處加密取樣。
  2. 保證:可行路徑存在時,取樣數趨近無限,成功機率趨近 1;不保證有限時間內找到,實務上計算預算有限,所以仍可能失敗。
  3. 隨機取樣 x_rand→找樹上最近的節點→朝它走固定步長得到 x_new→檢查連線無碰撞→無碰撞才把 x_new 與邊加入。
  4. 落在障礙物裡就丟棄;連線穿障礙時不直接接取樣點,只在可行範圍內取一個點(steer 一小步)。
  5. 多選目標讓樹更快抵達目標,但涵蓋較少、可能漏掉繞路的自由空間;是在探索與利用之間取捨。
  6. 因為是隨機取樣再走一小步;RRT* 隨機挑兩節點,若能直線連接且無碰撞就縮短,逐步改善但仍非最佳。

二、完整約束、非完整約束與車輛運動學

  1. 為什麼 (0,0)→(5,0)→(5,5) 幾何上有效,車子卻走不出來?列出三個阻止它的物理原因。
  2. 完整約束與非完整約束分別限制什麼?各給一個例子,並說明對規劃的影響。
  3. 為什麼 ẋ sin φ − ẏ cos φ = 0 不能寫成完整約束?它讓車輛自由度從幾個變成幾個?
  4. 為什麼「非完整約束」本身不足以說明車子不能瞬間轉 90 度?還缺哪一塊?
  5. 狀態空間(第 8 講)與組態空間(第 7 講)的差別是什麼?各用來檢查哪一種可行性?
  6. 說明運動學自行車模型的假設與適用範圍;什麼情況下它會失效?
  7. 軸距 2.8 m、最大前輪轉向角 30° 時,最小轉彎半徑約多少?它對 (5,0) 的 90 度急轉彎有什麼意義?
  8. 平行停車為什麼不矛盾?「不能橫移」並不代表什麼?
作答檢核
  1. 車子不能直接橫移、不能用任意小的半徑轉彎、不能瞬間改變速度與航向;幾何路徑只考慮空間。
  2. 完整約束限制組態(點被限制在圓上),限制組態空間;非完整約束限制速度(車子不能橫移),限制組態空間中實際可行的路徑。
  3. 它是對速度的微分約束,無法積分成只含組態與時間的 f=0;三個自由度(x, y, φ)可控的只剩兩個。
  4. 它只禁止橫向移動,不限制朝向改變的快慢;還需要轉向運動學(轉向角上限)與車輛模型限制航向變化率。
  5. 組態空間只描述位置與朝向,檢查幾何可行(無碰撞);狀態空間多了速度,檢查動力學/運動學上的物理可行。
  6. 假設平面運動(無 roll/pitch)、低速、輪胎不打滑;高速、打滑、濕滑路面時要用動力學模型。
  7. 約 4.85 公尺;後軸中點不可能走半徑更小的圓,所以不能原地 90 度急轉,必須用更大的弧線與更早轉向。
  8. 瞬時的運動限制不等於到不了目標,來回倒車、轉方向盤的一連串可行動作能把組態移到目標,只是不能一步到位。

三、從路徑到軌跡

  1. 用兩輛車到達路口的例子說明為什麼路徑不夠、需要軌跡。
  2. 軌跡最佳化裡的「目標」與「限制」有什麼不同?各舉三個例子。
  3. 安全、舒適、效率三種代價分別鼓勵什麼行為?jerk 為什麼會被放進舒適代價?
  4. 作業二為什麼只做路徑追隨?這樣的結果在真實機器人上可能出現什麼問題?
作答檢核
  1. 兩種情境的幾何路徑相同,但到達時間不同:時間區間不重疊就安全,同時到達可能相撞;避障要推論同一時間物體的位置。
  2. 目標是想達成並最小化的(安全、舒適、效率);限制是必須遵守的(最大轉向角、加速度上下限、不進入被佔用區域、狀態空間模型)。
  3. 安全:與障礙及其他用路人保持距離;舒適:避免突然加速與急煞,jerk 是加速度的變化率,會讓乘客不適;效率:朝目的地有合理進展。
  4. 作業二的重點是路徑,不處理速度、航向與運動學;動作可能不平滑、無法被真實車輛或機器人執行。

四、多感測器標定與誤差傳遞

  1. 為什麼要把 LiDAR 點投影到影像上?投影一個點需要哪三步?
  2. 為什麼不能只靠用尺量感測器之間的距離?「看不見的座標系」是什麼意思?
  3. 內參與外參各包含什麼?哪一個要在 LiDAR 與相機之間估計?
  4. 棋盤格標定在影像與 LiDAR 兩邊各做什麼?如何用法向量估計旋轉?
  5. 講者為什麼花了一個月才讓 80 公尺外的投影變準?這是什麼樣的學習問題?
  6. 列出至少四種會往 sense–plan–act 下游傳遞的誤差來源,說明佔據格網為什麼不該被當成確定的事實。
作答檢核
  1. 要結合兩種感測器定位障礙物;步驟:取得 LiDAR 座標系下的點→用外參轉到相機座標系→用投影矩陣投到像素(再除以第三個元素)。
  2. 尺量的是實體,不是座標系原點;針孔原點與 LiDAR 的座標系原點都看不見,因此要靠標定算出轉換。
  3. 內參:焦距 fx、fy 與主點;外參:旋轉與平移。LiDAR 到相機的是外參。
  4. 影像找角點、擬合平面;LiDAR 找落在棋盤格上的點、擬合平面;兩個法向量旋轉對齊得到旋轉,再估平移。
  5. 樣本太少、姿態單一、遠距離資料不足,優化對少數樣本過擬合;近處看起來對,遠處微小的旋轉誤差會被放大。
  6. 深度估計、點雲定位、標定、感測器雜訊、預測、物理模型等;佔據格網含有量測與標定誤差,當成確定的會讓後面的規劃建立在想像的空間上。

五、整合情境

  1. 情境:一輛低速接駁車要在校園路口,避開突然改變方向的行人,同時併入一條壅塞車道。請指出五種可能出錯的地方(感知、標定、預測、物理模型、規劃)各會怎麼發生,並說明你會怎麼降低其中兩種風險。
  2. 某 RRT 規劃器找出了 (0,0)→(5,0)→(5,5) 的路徑,工程師直接把它交給車輛。請用本講概念診斷問題,並提出把它變成可執行軌跡的三項修改。
作答檢核
  1. 感知:行人被遮擋;標定:相機與 LiDAR 錯位;預測:行人突然轉向、倒退或加速;物理模型:濕滑路面使煞車力下降;規劃:運算期限內找不到可行軌跡。降低風險可答:多感測器冗餘與定期重新標定、用更大的安全距離與保守預測、在模型中加入路面與打滑不確定性、設定備援動作(減速、停車)與規劃失敗處理。
  2. 問題:只考慮幾何,忽略非完整約束、最小轉彎半徑與時間;修改:平滑路徑(避免急轉)、依最小轉彎半徑與轉向角上限重新規劃、把路徑轉成帶時間的軌跡並加入速度、加速度限制與安全代價。

六、最終口述挑戰

  1. 用三分鐘回答:「為什麼一條幾何上無碰撞的路徑,仍然不能直接交給車輛執行?」必須依序提到運動學約束、軌跡與時間、感知與標定誤差,並說明為什麼不能假設循序流程完美運作。
通過標準

能先指出路徑只保證幾何無碰撞;再說明非完整約束、轉向運動學與最小轉彎半徑限制可行運動,動態環境需要把路徑變成含時間的軌跡並以代價與限制做最佳化;最後說明障礙物位置來自多感測器與標定,感知、標定、預測、物理模型與規劃的誤差都會向下游傳遞,因此必須用能處理不確定性並系統化評估的工具,而不是假設感知完美。

內容整理自本專案 Lecture 8 影片及 YouTube 自動英文字幕;投影片截圖取自同一支課程影片(開頭約 6 分鐘與兩次下課時段只拍到講者鏡頭,Lecture 7 的回顧投影片除少數用以銜接的畫面外不重複)。繁中字幕已依台灣用語翻譯並通過格式驗證,專有名詞與數學符號依投影片與上下文校正,正式公開前仍建議人工複核問答與收音不清的片段。本講由每週排程自動整理上線。