在第16講《Coursera自動駕駛課程第16講：LIDAR Sensing》我們學習了自動駕駛目前常用的3D 傳感器，激光雷達，了解了激光雷達的工作原理，掌握了對點云數據的操作以及如何使用點云配準方法來進行汽車定位。

回顧一下，在本模塊中我們學習了狀態估計常用的方法、3D 幾何知識和自動駕駛中常用的傳感器。我們現在將所有這些所學工具結合起來構建一個可以在真正的自動駕駛汽車上使用的狀態估計器。一輛真正的自動駕駛汽車通常會配備許多不同類型的傳感器。 如下圖所示，配備了攝像頭、LiDAR、IMU、Radar、GPS 或 GNSS 接收器和一個車輪編碼器。 所有這些傳感器以不同的頻率提供不同類型的數據。例如，IMU 可以以 200 Hz 的頻率來發出加速度和角速度數據，而 LiDAR 每秒僅能完成 20 次完整掃描。

因此在實踐中，我們通常需要進行多傳感器融合以得到最佳估計。多傳感器融合是自動駕駛汽車最重要的技術之一。 在本講中：

• 我們將看到 IMU 和 GNSS 或 LiDAR 融合定位的例子，我們將學習如何使用 EKF 進行多傳感器融合；
• 我們將討論如何標定我們的傳感器以確保傳感器模型是準確的，從而知道所有傳感器的參考坐標是如何相互關聯的；
• 我們還會討論一些應用常遇到的問題，如當一個或多個傳感器發生故障、在實際應用中如何考慮速度和精度要求、定位失敗時如何處理、以及如何應對我們周圍正在移動和變化的環境。

文章目錄

• • • 1. State Estimation in Practice
    • • 1.1 Overview
      • 1.2 Accuracy & Speed Requirements
      • 1.3 Localization Failures & Localization Failures
    • 2. Multisensor Fusion for State Estimation
    • • 2.1 Overview
      • 2.2 Motion Model & Measurement Model
      • 2.3 EKF：IMU + GNSS + LIDAR
    • 3. Sensor Calibration - A Necessary Evil
    • • 3.1 Intrinsic Calibration
      • 3.2 Extrinsic Calibration
      • 3.3 Temporal Calibration
    • 4. Loss of One or More Sensors
    • • 4.1 Overview
      • 4.2 Redundancy is Crucial
    • 推薦閱讀

1. State Estimation in Practice

1.1 Overview

如果我們有一輛配備了許多不同傳感器的自動駕駛汽車，我們想做的是弄清楚如何結合所有這些不同的傳感器信息來獲得對車輛狀態的最佳估計。融合所有這些數據似乎是一項艱巨的任務，但實際上，我們已經擁有執行此操作的工具。在下一節，我們將討論如何使用熟悉的工具（如擴展卡爾曼濾波器）將所有傳感器數據組合成對車輛狀態的一致估計。

但是為了進行傳感器融合，我們首先需要了解我們的傳感器以及它們在車輛上的配置方式。一方面，我們的傳感器模型可能取決于車輛特頂要求或傳感器本身參數。一個很好的例子是使用車輪編碼器來測量汽車的前進速度。車輪編碼器測量車輪的角速度。但是如果我們想用它來獲得車輛的前進速度，我們還需要知道輪胎的半徑。關于車輛，我們需要了解的另一件事是每個傳感器相對于車輛參考系的姿態（位置和方向）。這是因為我們正在組合來自位于不同位置的傳感器的信息，所以我們需要知道如何轉換所有測量值，以便它們在一個共同的參考系中表示。此外，我們需要考慮傳感器測量的時間同步程度，以便我們可以準確地融合它們。 這很好理解，傳感器時間同步越準確，狀態估計就會越好。其中一部分涉及確定傳感器記錄測量值與估計器接收測量值進行處理之間的時間偏移。所有這些因素都是標定時需要考慮的，我們將在第三節中更詳細地討論。

---

1.2 Accuracy & Speed Requirements

自動駕駛汽車在道路上安全行駛的狀態估計需要多準確呢？這取決于汽車的大小、車道的寬度和交通密度。一輛汽車的寬度大約為 1.8 米，一條普通的高速公路車道寬度大約有 3 米。因此，我們的裝態估計器需要足夠好，才能將汽車定位在車道兩側 60 厘米左右的范圍內（這是假設我們確切地知道車道在哪里并且沒有其它交通參與者）。作為比較，GPS 測量準確度的通道是 1-5 米，具體取決于硬件型號、可見的衛星數量和其他因素。因此，很明顯，即使是車道保持功能，僅使用 GPS 也是不夠的。我們需要結合來自許多不同傳感器的信息。

速度呢？我們需要以多快的速度更新車輛狀態或者車輛可以對快速變化的環境或意外事件做出反應？這一切都取決于汽車在什么樣的環境中運行。想象一下，我們正在閉著眼睛駕駛汽車，并且恰好每秒睜一次眼，看看周圍的環境并進行一些調整。對于在沒有其它交通參與者的街道上行駛，也許我們會覺得這樣做相對安全。但是，如果我們開車穿過一個繁忙的城市十字路口，周圍有幾十輛其它汽車、公共汽車、騎自行車的人和行人呢？每秒鐘睜一次眼睛就變得不夠安全了。

根據經驗，自動駕駛汽車需要能夠達到15-30Hz的更新頻率。但是，實際應用時需要進行權衡。自動駕駛汽車只有這么多可用的車載計算能力，除了狀態估計之外，計算機還需要處理許多不同的操作，如控制、路徑規劃和感知。更重要的是，車載可用的計算能力總量可能會受到對計算機實際允許消耗多少功率的限制。

使用有限的計算資源進行狀態估計，對我們的算法復雜度和計算解決方案所允許的時間之間需要進行權衡。作為自動駕駛汽車工程師，我們需要決定自動駕駛汽車在這條折衷曲線上的位置。

---

1.3 Localization Failures & Localization Failures

即使我們有一個快速準確的估計算法，也會出現定位失敗的情況。這是因為：

• 我們可能有一個或多個傳感器報告錯誤數據，甚至數據可能完全無效。一個很好的例子是 GPS，它在隧道和車路中根本不起作用，并且在有很多高層建筑的城市中很難應對反射信號；
• 我們也可能在狀態估計算法本身中遇到錯誤。如果我們使用具有高度非線性傳感器模型的擴展卡爾曼濾波器，我們可能會發現估計器中固有的線性化誤差意味著我們可能會失去狀態估計的準確性，即使估計器對其輸出非常有信心；
• 或者我們的估計器本身就有很大的不確定性。回想一下卡爾曼濾波器方程，當我們通過運動模型向前傳播時（使用 IMU 進行狀態估計），我們狀態的不確定性會增加，并且只有在我們結合來自 LiDAR 或 GPS 的外部觀察時才會縮小；
• 如果我們的 LiDAR 壞了，并且我們在沒有 GPS 的情況下在隧道中行駛，我們可以依賴 IMU 和運動模型行駛多長時間呢？我們將在第四節中討論檢測和處理此類定位失敗問題的方法；

最后，我們需要考慮汽車所處的世界。在大多數情況下，我們為 LiDAR 等傳感器開發了我們的模型，假設世界是靜止且不變的。但在現實中，世界總是在不斷變化。例如，其它汽車、行人和騎自行車的人可能正在移動。一天中的燈光會發生變化，甚至世界的幾何形狀也會隨著季節而變化。自動駕駛汽車面臨的一大挑戰是找到解釋這些變化的方法，無論是通過建模還是通過找到識別和忽略違反我們假設的對象的方法。事實上，這仍然是一個非常活躍的研究領域。

因此，總結這一小節：

• 自動駕駛汽車狀態估計時通常會通過多傳感器融合來組合來自許多不同類型傳感器的信息，如 IMU、LiDAR、相機和 GPS 或 GNSS 接收器；
• 為了使傳感器融合按預期工作，我們需要通過確定傳感器模型的參數來標定傳感器。所有傳感器的相對位置和方向以及輪詢時間的任何差異。我們還需要在算法中考慮速度和準確性之間的權衡；
• 最后，我們需要考慮如何安全地處理定位失敗和不符合我們假設的情況，例如移動物體；

---

2. Multisensor Fusion for State Estimation

2.1 Overview

現在我們已經學習了定位所使用的硬件和方法，讓我們將所有內容放在一起。 在本小節中，我們將構建一個誤差狀態擴展卡爾曼（ES-EKF）濾波器，該濾波器使用 IMU、GNSS 接收器和 LiDAR 估計自動駕駛汽車的位置、速度和方向（如下圖所示）。

在傳感器融合進行狀態估計時，首先要考慮的一個重要因素是來自不同傳感器的誤差是否相關。換句話說，如果一個傳感器發生失效，另一個也可能會失效。在本節中，我們討論的三個傳感器分別使用不同的測量方法，因此三個傳感器不太可能因為相同的原因而同時失效。其次，我們應該盡可能選擇具有互補性的傳感器。 在我們的這個例子中，IMU 充當 GPS 或 GNSS 位置估計的高頻平滑器。 GNSS 可以減輕由 IMU 漂移引起的誤差，也可以使用車輪里程計消除誤差。在這里，我們選擇使用 IMU，因為它們可以提供完整的三維位置和方向信息，而車輪里程計僅能提供二維信息。最后，LiDAR 可以根據已知地圖信息提供非常準確的位置估計來補充 GNSS 信息。 相反，GNSS 可以告訴 LiDAR 大致位置從而在定位時選擇要使用的地圖。 出于 EKF 狀態估計的目的，我們可以實現所謂的松耦合估計器或緊耦合估計器。在緊耦合的 EKF 中，我們使用來自 GNSS 和 LiDAR 的原始偽距和點云測量值作為觀測值。 在一個松耦合的系統中，我們假設這些數據已經被預處理以產生一個位置估計。 盡管緊耦合的方法可以實現更準確的定位，但實施起來通常很繁瑣，并且需要大量調整。出于這個原因，我們將在這里實現一個松耦合的 EKF。

---

2.2 Motion Model & Measurement Model

下圖是我們整個狀態估計的框架，這是一個卡爾曼濾波系統框架。 我們將使用高頻的 IMU 測量作為運動模型的輸入，這將為我們提供預測狀態，每次進行 IMU 測量時都會更新（卡爾曼系統中的預測模塊）。 使用低頻的 GNSS 和 LiDAR 測量進行融合更新，同時來糾正我們的預測狀態（卡爾曼系統中的更新模塊）。

那么，我們的狀態是什么呢？為了實現狀態估計，我們將使用一個十維的狀態向量 ${\mathbf{x}}_k$ ，包括：3D 位置 ${\mathbf{p}}_k$、3D 速度 ${\mathbf{v}}_k$、 單位四元數 ${\mathbf{q}}_k$，這些四元數將表示我們的車輛相對于導航坐標系的方向，數學表示為：
$${\mathbf{x}}_k=?\begin{array}{l}{\mathbf{p}}_k\\ {\mathbf{v}}_k\\ {\mathbf{q}}_k\end{array}?\in R^{10}\text{\hspace{1em}(17.1)}$$

我們假設 IMU 在傳感器坐標系中輸出加速度和旋轉角速度，并將它們組合成單個輸入向量 ${\mathbf{u}}_k$。同樣重要的是要指出我們這里沒有討論加速度計或陀螺儀的偏差。這些通常被放入狀態向量中，進行估計，然后從我們的 IMU 測量中減去。為方便討論，這里假設 IMU 測量是無偏的，輸入量數學表示為：
$${\mathbf{u}}_k=\left[\begin{array}{c}{\mathbf{f}}_k\\ {\omega }_k\end{array}\right]\in R^6\text{\hspace{1em}(17.2)}$$

下面來介紹運動模型，運動模型輸入為 IMU 的加速度和旋轉速率。運動模型數學公式如下：
$$\begin{array}{rl}& {\mathbf{p}}_k={\mathbf{p}}_{k?1}+\Delta t{\mathbf{v}}_{k?1}+\frac{\Delta t^2}{2}\left({\mathbf{C}}_{ns}{\mathbf{f}}_{k?1}+\mathbf{g}\right)\\ & {\mathbf{v}}_k={\mathbf{v}}_{k?1}+\Delta t\left({\mathbf{C}}_{ns}{\mathbf{f}}_{k?1}?\mathbf{g}\right)\\ & {\mathbf{q}}_k=\mathbf{\Omega }\left(\mathbf{q}\left({\mathbf{\omega }}_{k?1}\Delta t\right)\right){\mathbf{q}}_{k?1}\end{array}\text{\hspace{1em}(17.3)}$$

其中，${\mathbf{C}}_{ns}$ 為從傳感器坐標 $s$ 到導航坐標 $n$ 的旋轉矩陣。
$${\mathbf{C}}_{ns}={\mathbf{C}}_{ns}\left({\mathbf{q}}_{k?1}\right)\text{\hspace{1em}(17.4)}$$
$\mathbf{\Omega }\left(\left[\begin{array}{l}{q}_w\\ {\mathbf{q}}_v\end{array}\right]\right)$ 為四元數相乘時四元數矩陣表示形式：
$$\mathbf{\Omega }\left(\left[\begin{array}{l}{q}_w\\ {\mathbf{q}}_v\end{array}\right]\right)=q_{w}1+\left[\begin{array}{cc}0& ?{\mathbf{q}}_v^T\\ {\mathbf{q}}_v& ?{\left\{{\mathbf{q}}_v\right\}}_{\times }\end{array}\right]\text{\hspace{1em}(17.5)}$$
$\mathbf{q}(\mathbf{\theta })$ 為旋轉向量與四元數的變換形式：
$$\mathbf{q}(\mathbf{\theta })=\left[\begin{array}{c}\cos \frac{|\mathbf{\theta }|}{2}\\ \frac{\mathbf{\theta }}{|\mathbf{\theta }|}\sin \frac{|\mathbf{\theta }|}{2}\end{array}\right]\text{\hspace{1em}(17.6)}$$

由于我們的運動模型不是線性的。要在我們的 EKF 中使用它，我們需要針對預測狀態的誤差項進行線性化。為此，我們將定義一個誤差狀態向量，包括位置、速度、方向的誤差項（注意這是一個九維的狀態向量），數學表示為：
$$\delta {\mathbf{x}}_k=?\begin{array}{c}\delta {\mathbf{p}}_k\\ \delta {\mathbf{v}}_k\\ \delta {?}_k\end{array}?\in R^9\text{\hspace{1em}(17.7)}$$

其中 $\delta {?}_k$ 是一個三乘一的方向誤差狀態量。此外，我們可以得到誤差狀態量的狀態轉移方程：
$$\delta {\mathbf{x}}_k={\mathbf{F}}_{k?1}\delta {\mathbf{x}}_{k?1}+{\mathbf{L}}_{k?1}{\mathbf{n}}_{k?1}\text{\hspace{1em}(17.8)}$$
其中， ${\mathbf{F}}_{k?1}$ 為狀態轉移矩陣，數學表示為：
$${\mathbf{F}}_{k?1}=?\begin{array}{ccc}1& 1\Delta t& 0\\ 0& 1& ?{\left[{\mathbf{C}}_{ns}{\mathbf{f}}_{k?1}\right]}_{\times }\Delta t\\ 0& 0& 1\end{array}?\text{\hspace{1em}(17.9)}$$
其中， $1$ 是一個 $3\times 3$ 的單位陣。IMU 測量噪聲雅可比矩陣為${\mathbf{L}}_{k?1}$，測量噪聲為 ${\mathbf{n}}_k$，數學表示為：

$$\begin{array}{rl}{\mathbf{L}}_{k?1}=?\begin{array}{ll}0& 0\\ 1& 0\\ 0& 1\end{array}?& {\mathbf{n}}_k～\mathcal{N}\left(0,{\mathbf{Q}}_k\right)～\mathcal{N}\left(0,\Delta t^2\left[\begin{array}{ll}{\sigma }_{\mathrm{a}\mathrm{c}\mathrm{c}}^2& \\ & {\sigma }_{\mathrm{g}\mathrm{y}\mathrm{r}\mathrm{o}}^2\end{array}\right]\right)\end{array}\text{\hspace{1em}(17.10)}$$

對于測量模型，我們將使用非常簡單的位置觀察加上一些高斯噪聲。測量模型數學表示為：
$$\begin{array}{rl}{\mathbf{y}}_k& =\mathbf{h}\left({\mathbf{x}}_k\right)+{\mathbf{\nu }}_k\\ & ={\mathbf{H}}_k{\mathbf{x}}_k+{\mathbf{\nu }}_k=\left[\begin{array}{lll}1& 0& 0\end{array}\right]{\mathbf{x}}_k+{\mathbf{\nu }}_k\\ & ={\mathbf{p}}_k+{\mathbf{\nu }}_k\\ {\mathbf{\nu }}_k& ～\mathcal{N}\left(0,\mathbf{R}\right)\end{array}\text{\hspace{1em}(17.11)}$$

其中，位置噪聲可以為 ${\mathbf{R}}_{\mathrm{G}\mathrm{N}\mathrm{S}\mathrm{S}}$ 或 ${\mathbf{R}}_{\mathrm{L}\mathrm{i}\mathrm{D}\mathrm{A}\mathrm{R}}$。需要注意的是，我們這里假設我們的 LiDAR 和 GNSS 將在同一坐標系中提供位置測量。

---

2.3 EKF：IMU + GNSS + LIDAR

根據上一小節介紹的運動模型和測量模型，我們現在來實現一個完整的的擴展卡爾曼濾波器。

• Update state with IMU inputs。當 IMU 測量值到來時，濾波器根據運動模型進行狀態預測，預測狀態為：${\check{\mathbf{x}}}_k=?\begin{array}{c}{\check{\mathbf{p}}}_k\\ {\check{\mathbf{v}}}_k\\ {\check{\mathbf{q}}}_k\end{array}?$。這里的狀態量可以為更新后或未更新的狀態量。
  $$\begin{array}{rl}& {\check{\mathbf{p}}}_k={\mathbf{p}}_{k?1}+\Delta t{\mathbf{v}}_{k?1}+\frac{\Delta t^2}{2}\left({\mathbf{C}}_{ns}{\mathbf{f}}_{k?1}+{\mathbf{g}}_n\right)\\ & {\check{\mathbf{v}}}_k={\mathbf{v}}_{k?1}+\Delta t\left({\mathbf{C}}_{ns}{\mathbf{f}}_{k?1}+{\mathbf{g}}_n\right)\\ & {\check{\mathbf{q}}}_k=\mathbf{\Omega }\left(\mathbf{q}\left({\mathbf{\omega }}_{k?1}\Delta t\right)\right){\mathbf{q}}_{k?1}\end{array}\text{\hspace{1em}(17.12)}$$
• Propagate uncertainty。然后是狀態不確定性（狀態協方差矩陣）更新。同樣地，狀態協方差矩陣也可以是更新或未更新的協方差矩陣。此時，如果我們沒有任何可用的 GNSS 或 LiDAR 測量，我們就一直重復步驟一和二。
  $${\check{\mathbf{P}}}_k={\mathbf{F}}_{k?1}{\mathbf{P}}_{k?1}{\mathbf{F}}_{k?1}^T+{\mathbf{L}}_{k?1}{\mathbf{Q}}_{k?1}{\mathbf{L}}_{k?1}^T\text{\hspace{1em}(17.13)}$$
• GNSS or LIDAR position available。如果有任何可用的 GNSS 或 LiDAR 測量，我們將首先計算卡爾曼增益。
  $${\mathbf{K}}_k={\check{\mathbf{P}}}_k{\mathbf{H}}_k^T{\left({\mathbf{H}}_k{\check{\mathbf{P}}}_k{\mathbf{H}}_k^T+\mathbf{R}\right)}^{?1}\text{\hspace{1em}(17.14)}$$
  然后我們計算一個誤差狀態，我們將使用它來糾正我們的預測狀態。該誤差狀態為卡爾曼增益與預測位置和觀察位置之間的差的乘積。
  $$\delta {\mathbf{x}}_k={\mathbf{K}}_k\left({\mathbf{y}}_k?{\check{\mathbf{p}}}_k\right)\text{\hspace{1em}(17.15)}$$
  然后，我們將使用我們的誤差狀態來糾正我們的預測狀態。這種糾正對于位置和速度很簡單，但是需要一些更復雜的代數來糾正四元數（這里可參考推薦閱讀中的四元數運動學一文）。
  $$\begin{array}{rl}{\hat{\mathbf{p}}}_k& ={\check{\mathbf{p}}}_k+\delta {\mathbf{p}}_k\\ {\hat{\mathbf{v}}}_k& ={\check{\mathbf{v}}}_k+\delta {\mathbf{v}}_k\\ {\hat{\mathbf{q}}}_k& =\mathbf{\Omega }(\mathbf{q}(\delta ?)){\check{\mathbf{q}}}_k\end{array}\text{\hspace{1em}(17.16)}$$
  最后，是更新狀態協方差矩陣。
  $${\hat{\mathbf{P}}}_k=\left(1?{\mathbf{K}}_k{\mathbf{H}}_k\right){\check{\mathbf{P}}}_k\text{\hspace{1em}(17.17)}$$

至此，就是一個完整的卡爾曼濾波過程。簡單總結以下這一小節：

• 我們使用松耦合的擴展卡爾曼濾波器框架將來自 IMU 的慣性測量與來自 GNSS 接收器和 LiDAR 的位置測量融合在一起進行狀態估計。
• 我們假設 GNSS 和 LiDAR 在同一坐標系中為我們提供了位置估計，在實際中通常需要一些預處理。
• 其次，我們沒有考慮加速度計和陀螺儀偏差。這簡化了我們的代數，但這不是一個現實的假設。在實際應用中，如何不考慮偏差，我們的定位誤差會越來越大。
• 接下來，我們沒有討論狀態初始化。這通常被認為是定位過程開始時的某個已知狀態。
• 最后，我們還假設我們的傳感器在空間和時間上都是對齊的。我們假設我們的傳感器在某種意義上是經過標定的，即我們不用擔心不同的時間步長，也不用擔心我們如何將多組測量值全部對齊到一個坐標系中。我們將在下一小節介紹這部分內容。

---

3. Sensor Calibration - A Necessary Evil

3.1 Intrinsic Calibration

在上一小節我們已經了解了如何結合多個傳感器數據來估計車輛狀態，現在是時候討論一個我們迄今為止一直未討論的話題了。那就是傳感器標定，這是工程師們不太喜歡談論的事情之一，但它對于正確進行狀態估計又是十分必要的。就我個人而言（這里指課程講師），傳感器標定對我來說很重要，因為我的博士研究課題就是 相機和 IMU 的標定，這也是我的學生今天繼續研究的主題。在本小中，我們將討論傳感器標定的三種主要類型，以及為什么我們在為自動駕駛汽車設計狀態估計器時需要考慮它們。要討論的三種主要標定是：

• 內參標定，求解傳感器自身參數（例如相機內參）；
• 外參標定，求解傳感器相對于汽車坐標的安裝位置和定向；
• 時間標定（也成為時間同步），處理不同傳感器之間的測量時間差。

---

讓我們先看看內參標定。在內參標定中，我們希望確定傳感器模型的固定參數，以便我們可以在擴展卡爾曼濾波器等估計器中使用它們。每個傳感器都有與之相關的參數，這些參數對于該特定傳感器是唯一的，并且通常是恒定的。

例如，我們可能在汽車的車輪上安裝了車輪編碼器，用于測量車輪轉速 $\omega$。如果我們想使用 $\omega$ 來估計車速 $v=\omega R$，我們需要知道車輪的半徑 $R$。在這個例子中，$R$ 就是傳感器模型參數，它對應編碼器所連接的車輪，對不同的車輪有不同的 $R$。另一個例子是激光雷達傳感器中掃描線的俯仰角度，著是一個固定的量，但我們需要提前知道它，以便我們能夠正確解釋每次掃描。

那么，我們如何確定這些內參呢？ 實際中可以通過以下三方面來確定內參：

• 最簡單的就是從供應商那里得到。通常，傳感器會在工廠進行標定，并附帶一份規格表，會介紹與傳感器相關的所有參數，例如激光雷達線數、不同掃描線間隔角度等；
• 但規格表并不總是足以進行真正準確的狀態估計，因為沒有兩個傳感器完全相同，并且參數的真實值會有一些變化。 另一個方法是手動測量這些參數。這對于輪胎半徑測量來說是非常簡單的，但對于像激光雷達這樣的東西來說就不是那么簡單了，因為在傳感器內部用量角器測量是不切實際的；
• 另一種方法是將傳感器內在參數作為車輛狀態的一部分進行估計（如 IMU 加速度計和陀螺儀偏差），這種方法對特定傳感器可以精確標定，并且也可以用來處理隨時間緩慢變化的參數。例如，如果不斷估計輪胎的半徑，可以幫助檢測何時會發生爆胎；

現在，因為我們在本課程中討論的估計器是通用的，所以我們已經有了進行這種自動標定的工具。要了解它是如何工作的，讓我們回到我們的汽車在一維運動的示例。 我們在后輪上安裝了一個編碼器來測量車輪的轉速。如果我們想估計車輪半徑以及位置和速度，我們需要做的就是將它添加到狀態向量中，并計算出新的運動和測量模型。此時，狀態向量如下，只是多了一個車輪半徑 $R$。
$$\mathbf{x}=?\begin{array}{l}p\\ \dot{p}\\ R\end{array}?\text{\hspace{1em}(17.18)}$$

此外，我們可以得到如下關系：
$$\begin{gathered} \mathbf{u}=\ddot{p} \\ \dot{p}=v=\omega R\text{\hspace{1em}(17.19)} \end{gathered}$$
此時運動模型為：
$${\mathbf{x}}_k=?\begin{array}{ccc}1& \Delta t& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}?{\mathbf{x}}_{k?1}+\left[\begin{array}{c}0\\ \Delta t\end{array}\right]{\mathbf{u}}_{k?1}+{\mathbf{w}}_{k?1},{\mathbf{w}}_k～\mathcal{N}(0,{\mathbf{Q}}_k)\text{\hspace{1em}(17.20)}$$

對于測量模型，我們仍然是直接通過 GPS 觀察位置，但現在我們也通過編碼器觀察車輪轉速。 因此，我們在模型中包含了額外的非線性觀察。我們可以使用擴展或無跡卡爾曼濾波器來估計車輪半徑以及車輛的位置和速度。 因此，內參標定對于使用單個傳感器進行狀態估計是必不可少的。
$${\mathbf{y}}_k=\left[\begin{array}{c}{p}_k\\ {\dot{p}}_k/R_k\end{array}\right]+{\mathbf{v}}_k,{\mathbf{v}}_k～\mathcal{N}(0,{\mathbf{R}}_k)\text{\hspace{1em}(17.21)}$$

---

3.2 Extrinsic Calibration

外參標定對于多傳感器融合十分重要。 在外參標定中，我們感興趣的是確定所有傳感器通常相對于車輛的相對姿態。就像內參標定一樣，外參標定也有不同的技術：

• 如果可以的話，可以訪問像下圖這樣精確的車輛 CAD 模型，其中所有傳感器坐標系都已經精心布置；
• 如果不行的話，可能需要嘗試手東測量。 不幸的是，這通常很難或不可能準確地完成，因為許多傳感器的坐標系原點位于傳感器本身內部，我們可能不想拆卸汽車和所有傳感器；
• 同樣地，我們也可以將外參包含在我們的狀態中來估計它們。對于任意傳感器配置，這可能會變得有點復雜，目前有很多研究在使用不同的技術來可靠地做到這一點；

---

3.3 Temporal Calibration

最后，一種實踐中經常被忽視但仍然很重要的標定是時間標定。在我們所有關于多傳感器融合的討論中，我們一直在隱含地假設我們的所有測量值都是在同一時刻捕捉到的，或者至少足夠接近給定的精度水平。但是我們如何確定兩個測量是否足夠接近以被認為是同步的呢？

一種方法是在車載計算機接收到每個測量值時為其添加時間戳，并匹配彼此最接近的測量值。例如，如果我們獲得 15 Hz 的 LiDAR 掃描和 200 Hz的 IMU 讀數，我們可能希望將每個 LiDAR 掃描與時間戳最接近的 IMU 讀數配對。但實際上，在 LiDAR 或 IMU 實際記錄觀察結果與到達計算機之間存在未知延遲。這些延遲可能是由傳感器數據傳輸到主機所需的時間或由傳感器電路執行的預處理步驟引起的，并且不同傳感器的延遲可能不同。因此，如果我們想要獲得真正準確的狀態估計，我們需要考慮我們的傳感器實際同步的程度，并且有不同的方法可以解決這個問題：

• 最簡單和最常見的做法就是假設延遲為零。仍然可以通過這種方式獲得有效的估計器，但結果可能不如通過更好的時間同步獲得的結果準確；
• 另一種常見的做法是使用硬件同步傳感器，但這通常僅適用于更昂貴的傳感器設置；
• 正如我們剛剛介紹的那樣，也可以嘗試將這些時間延遲作為車輛狀態的一部分進行估算，但這可能會變得復雜。事實上，我（這里指課程講師）博士論文的一整章都致力于解決相機和 IMU 的時間同步問題。

簡單總結下這一小節：

• 如果沒有標定，傳感器融合是不可能的。在本小節中，我們了解了內參標定，它標定我們傳感器模型的參數。
• 外參標定，它為我們提供了將傳感器測量值轉換為公共參考坐標系架所需的坐標轉換。
• 時間標定，處理同步測量以確保它們都對應于相同的車輛狀態。
• 雖然有一些標定技術可以解決所有這些問題，但標定仍然是一個非常活躍的研究領域。

---

4. Loss of One or More Sensors

4.1 Overview

在自動駕駛汽車等應用中，傳感器可能會因多種不同原因而發生故障或性能下降，例如天氣惡化、固件出現故障或電線出現松動。我們在本模塊中已經看到，即使我們所有的傳感器都正常工作，擁有多個互補傳感器對提供穩健準確的定位估計仍然是有益的。 但是，如果其中一個傳感器發生故障會發生什么？

到目前為止，我們已經討論了 GNSS、IMU 和 LiDAR，但大多數現代自動駕駛汽車還包括車輪編碼器、毫米波雷達、超聲波雷達和多個攝像頭等傳感器（如下圖所示）。

為了制造一輛安全的自動駕駛汽車，了解當一個或多個傳感器發生故障時會發生什么，以及了解維持汽車安全運行所需的最小感知是很有必要的。在本小節中，我們將討論傳感器冗余對于穩定定位的重要性，并探討定位中傳感器失敗的幾個示例。 這類問題分析的一個重要考慮因素是傳感器測量范圍和操作限制。

GNSS 接收器無法在橋下工作，并且在高層建筑之間測量精度會下降。 IMU 可能對溫度比較敏感，有時需要定期重新校準。更重要的是，對于激光雷達、超聲波雷達、攝像頭或毫米波雷達等觀察外部環境的傳感器來說，其測量范圍對汽車安全運行起著非常重要的作用。大多數汽車都有長距離、中距離和短距離感知傳感器。如果其中一個傳感器出現故障，就要立即限制汽車的運動。例如，在定位中，我們可能會在進行停車時使用到短距離傳感器（例如超聲波雷達），這樣能確保我們不會與附近的車輛發生碰撞。中距離傳感器可以幫助檢測行人和騎自行車的人以及跟隨車道。遠距離傳感器可以幫助我們檢測和預測遠處障礙物的運動。如果其中一個出現故障，我們必須采取適當的行動，以確保車內乘員或我們周圍人的安全不受影響。這意味著自動駕駛汽車工程師必須考慮執行每個步驟所需的最小允許傳感設備。

---

4.2 Redundancy is Crucial

對于這種冗余系統設計，我們可以查看以嚴格的安全標準而聞名的商業航空行業的示例。 作為冗余系統的一個例子，波音777 以三重冗余原則運行。 所有主要系統，包括這臺飛行計算機，都有兩個備份，每個都有獨立的電源。在這里，我們可以看到每臺飛行計算機都有不同的處理器，以確保架構本身的錯誤不會影響所有三臺計算機。 如果其中一臺計算機出現故障，777 會使用共識算法無縫切換到另一臺計算機。

盡管自動駕駛技術已經取得了長足的進步，但我們距離最初的一些自動駕駛挑戰（例如 2007 年 DARPA 城市挑戰賽）只有 10 年的時間。在這里，我們可以看到缺乏安全冗余是如何導致 MIT 和 Cornell 團隊相互碰撞的。如果這次碰撞以更高的速度發生，結果可能會更糟。

還有一個例子，特斯拉 Model S 幾乎直接駛入加利福尼亞高速公路上的的例子。這是一個發人深省的提醒，對于設計自動駕駛汽車的工程師來說，仔細考慮所使用的不同傳感器的不同故障模式至關重要，以確保一個故障組件不會造成悲劇。

總而言之，考慮任何一組傳感器的局限性并使用多個互補傳感器進行魯棒性定位是很重要的。

---

推薦閱讀

• 《動手學無人駕駛（6）：基于IMU和GPS數據融合的自車定位》
• 重讀經典《Quaternion kinematics for the error-state Kalman filter》

總結

以上是生活随笔為你收集整理的Coursera自动驾驶课程第17讲：An Autonomous Vehicle State Estimator的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。