在第16講《Coursera自動(dòng)駕駛課程第16講：LIDAR Sensing》我們學(xué)習(xí)了自動(dòng)駕駛目前常用的3D 傳感器，激光雷達(dá)，了解了激光雷達(dá)的工作原理，掌握了對(duì)點(diǎn)云數(shù)據(jù)的操作以及如何使用點(diǎn)云配準(zhǔn)方法來(lái)進(jìn)行汽車定位。

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

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

• 我們將看到 IMU 和 GNSS 或 LiDAR 融合定位的例子，我們將學(xué)習(xí)如何使用 EKF 進(jìn)行多傳感器融合；
• 我們將討論如何標(biāo)定我們的傳感器以確保傳感器模型是準(zhǔn)確的，從而知道所有傳感器的參考坐標(biāo)是如何相互關(guān)聯(lián)的；
• 我們還會(huì)討論一些應(yīng)用常遇到的問(wèn)題，如當(dāng)一個(gè)或多個(gè)傳感器發(fā)生故障、在實(shí)際應(yīng)用中如何考慮速度和精度要求、定位失敗時(shí)如何處理、以及如何應(yīng)對(duì)我們周圍正在移動(dòng)和變化的環(huán)境。

文章目錄

• • • 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

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

但是為了進(jìn)行傳感器融合，我們首先需要了解我們的傳感器以及它們?cè)谲囕v上的配置方式。一方面，我們的傳感器模型可能取決于車輛特頂要求或傳感器本身參數(shù)。一個(gè)很好的例子是使用車輪編碼器來(lái)測(cè)量汽車的前進(jìn)速度。車輪編碼器測(cè)量車輪的角速度。但是如果我們想用它來(lái)獲得車輛的前進(jìn)速度，我們還需要知道輪胎的半徑。關(guān)于車輛，我們需要了解的另一件事是每個(gè)傳感器相對(duì)于車輛參考系的姿態(tài)（位置和方向）。這是因?yàn)槲覀冋诮M合來(lái)自位于不同位置的傳感器的信息，所以我們需要知道如何轉(zhuǎn)換所有測(cè)量值，以便它們?cè)谝粋€(gè)共同的參考系中表示。此外，我們需要考慮傳感器測(cè)量的時(shí)間同步程度，以便我們可以準(zhǔn)確地融合它們。 這很好理解，傳感器時(shí)間同步越準(zhǔn)確，狀態(tài)估計(jì)就會(huì)越好。其中一部分涉及確定傳感器記錄測(cè)量值與估計(jì)器接收測(cè)量值進(jìn)行處理之間的時(shí)間偏移。所有這些因素都是標(biāo)定時(shí)需要考慮的，我們將在第三節(jié)中更詳細(xì)地討論。

---

1.2 Accuracy & Speed Requirements

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

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

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

使用有限的計(jì)算資源進(jìn)行狀態(tài)估計(jì)，對(duì)我們的算法復(fù)雜度和計(jì)算解決方案所允許的時(shí)間之間需要進(jìn)行權(quán)衡。作為自動(dòng)駕駛汽車工程師，我們需要決定自動(dòng)駕駛汽車在這條折衷曲線上的位置。

---

1.3 Localization Failures & Localization Failures

即使我們有一個(gè)快速準(zhǔn)確的估計(jì)算法，也會(huì)出現(xiàn)定位失敗的情況。這是因?yàn)?#xff1a;

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

最后，我們需要考慮汽車所處的世界。在大多數(shù)情況下，我們?yōu)?LiDAR 等傳感器開發(fā)了我們的模型，假設(shè)世界是靜止且不變的。但在現(xiàn)實(shí)中，世界總是在不斷變化。例如，其它汽車、行人和騎自行車的人可能正在移動(dòng)。一天中的燈光會(huì)發(fā)生變化，甚至世界的幾何形狀也會(huì)隨著季節(jié)而變化。自動(dòng)駕駛汽車面臨的一大挑戰(zhàn)是找到解釋這些變化的方法，無(wú)論是通過(guò)建模還是通過(guò)找到識(shí)別和忽略違反我們假設(shè)的對(duì)象的方法。事實(shí)上，這仍然是一個(gè)非常活躍的研究領(lǐng)域。

因此，總結(jié)這一小節(jié)：

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

---

2. Multisensor Fusion for State Estimation

2.1 Overview

現(xiàn)在我們已經(jīng)學(xué)習(xí)了定位所使用的硬件和方法，讓我們將所有內(nèi)容放在一起。 在本小節(jié)中，我們將構(gòu)建一個(gè)誤差狀態(tài)擴(kuò)展卡爾曼（ES-EKF）濾波器，該濾波器使用 IMU、GNSS 接收器和 LiDAR 估計(jì)自動(dòng)駕駛汽車的位置、速度和方向（如下圖所示）。

在傳感器融合進(jìn)行狀態(tài)估計(jì)時(shí)，首先要考慮的一個(gè)重要因素是來(lái)自不同傳感器的誤差是否相關(guān)。換句話說(shuō)，如果一個(gè)傳感器發(fā)生失效，另一個(gè)也可能會(huì)失效。在本節(jié)中，我們討論的三個(gè)傳感器分別使用不同的測(cè)量方法，因此三個(gè)傳感器不太可能因?yàn)橄嗤脑蚨瑫r(shí)失效。其次，我們應(yīng)該盡可能選擇具有互補(bǔ)性的傳感器。 在我們的這個(gè)例子中，IMU 充當(dāng) GPS 或 GNSS 位置估計(jì)的高頻平滑器。 GNSS 可以減輕由 IMU 漂移引起的誤差，也可以使用車輪里程計(jì)消除誤差。在這里，我們選擇使用 IMU，因?yàn)樗鼈兛梢蕴峁┩暾娜S位置和方向信息，而車輪里程計(jì)僅能提供二維信息。最后，LiDAR 可以根據(jù)已知地圖信息提供非常準(zhǔn)確的位置估計(jì)來(lái)補(bǔ)充 GNSS 信息。 相反，GNSS 可以告訴 LiDAR 大致位置從而在定位時(shí)選擇要使用的地圖。 出于 EKF 狀態(tài)估計(jì)的目的，我們可以實(shí)現(xiàn)所謂的松耦合估計(jì)器或緊耦合估計(jì)器。在緊耦合的 EKF 中，我們使用來(lái)自 GNSS 和 LiDAR 的原始偽距和點(diǎn)云測(cè)量值作為觀測(cè)值。 在一個(gè)松耦合的系統(tǒng)中，我們假設(shè)這些數(shù)據(jù)已經(jīng)被預(yù)處理以產(chǎn)生一個(gè)位置估計(jì)。 盡管緊耦合的方法可以實(shí)現(xiàn)更準(zhǔn)確的定位，但實(shí)施起來(lái)通常很繁瑣，并且需要大量調(diào)整。出于這個(gè)原因，我們將在這里實(shí)現(xiàn)一個(gè)松耦合的 EKF。

---

2.2 Motion Model & Measurement Model

下圖是我們整個(gè)狀態(tài)估計(jì)的框架，這是一個(gè)卡爾曼濾波系統(tǒng)框架。 我們將使用高頻的 IMU 測(cè)量作為運(yùn)動(dòng)模型的輸入，這將為我們提供預(yù)測(cè)狀態(tài)，每次進(jìn)行 IMU 測(cè)量時(shí)都會(huì)更新（卡爾曼系統(tǒng)中的預(yù)測(cè)模塊）。 使用低頻的 GNSS 和 LiDAR 測(cè)量進(jìn)行融合更新，同時(shí)來(lái)糾正我們的預(yù)測(cè)狀態(tài)（卡爾曼系統(tǒng)中的更新模塊）。

那么，我們的狀態(tài)是什么呢？為了實(shí)現(xiàn)狀態(tài)估計(jì)，我們將使用一個(gè)十維的狀態(tài)向量 ${\mathbf{x}}_k$ ，包括：3D 位置 ${\mathbf{p}}_k$、3D 速度 ${\mathbf{v}}_k$、 單位四元數(shù) ${\mathbf{q}}_k$，這些四元數(shù)將表示我們的車輛相對(duì)于導(dǎo)航坐標(biāo)系的方向，數(shù)學(xué)表示為：
$${\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)}$$

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

下面來(lái)介紹運(yùn)動(dòng)模型，運(yùn)動(dòng)模型輸入為 IMU 的加速度和旋轉(zhuǎn)速率。運(yùn)動(dòng)模型數(shù)學(xué)公式如下：
$$\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}$ 為從傳感器坐標(biāo) $s$ 到導(dǎo)航坐標(biāo) $n$ 的旋轉(zhuǎ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)$ 為四元數(shù)相乘時(shí)四元數(shù)矩陣表示形式：
$$\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 })$ 為旋轉(zhuǎn)向量與四元數(shù)的變換形式：
$$\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)}$$

由于我們的運(yùn)動(dòng)模型不是線性的。要在我們的 EKF 中使用它，我們需要針對(duì)預(yù)測(cè)狀態(tài)的誤差項(xiàng)進(jìn)行線性化。為此，我們將定義一個(gè)誤差狀態(tài)向量，包括位置、速度、方向的誤差項(xiàng)（注意這是一個(gè)九維的狀態(tài)向量），數(shù)學(xué)表示為：
$$\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$ 是一個(gè)三乘一的方向誤差狀態(tài)量。此外，我們可以得到誤差狀態(tài)量的狀態(tài)轉(zhuǎn)移方程：
$$\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}$ 為狀態(tài)轉(zhuǎn)移矩陣，數(shù)學(xué)表示為：
$${\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$ 是一個(gè) $3\times 3$ 的單位陣。IMU 測(cè)量噪聲雅可比矩陣為${\mathbf{L}}_{k?1}$，測(cè)量噪聲為 ${\mathbf{n}}_k$，數(shù)學(xué)表示為：

$$\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)}$$

對(duì)于測(cè)量模型，我們將使用非常簡(jiǎn)單的位置觀察加上一些高斯噪聲。測(cè)量模型數(shù)學(xué)表示為：
$$\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}}$。需要注意的是，我們這里假設(shè)我們的 LiDAR 和 GNSS 將在同一坐標(biāo)系中提供位置測(cè)量。

---

2.3 EKF：IMU + GNSS + LIDAR

根據(jù)上一小節(jié)介紹的運(yùn)動(dòng)模型和測(cè)量模型，我們現(xiàn)在來(lái)實(shí)現(xiàn)一個(gè)完整的的擴(kuò)展卡爾曼濾波器。

• Update state with IMU inputs。當(dāng) IMU 測(cè)量值到來(lái)時(shí)，濾波器根據(jù)運(yùn)動(dòng)模型進(jìn)行狀態(tài)預(yù)測(cè)，預(yù)測(cè)狀態(tài)為：${\check{\mathbf{x}}}_k=?\begin{array}{c}{\check{\mathbf{p}}}_k\\ {\check{\mathbf{v}}}_k\\ {\check{\mathbf{q}}}_k\end{array}?$。這里的狀態(tài)量可以為更新后或未更新的狀態(tài)量。
  $$\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。然后是狀態(tài)不確定性（狀態(tài)協(xié)方差矩陣）更新。同樣地，狀態(tài)協(xié)方差矩陣也可以是更新或未更新的協(xié)方差矩陣。此時(shí)，如果我們沒有任何可用的 GNSS 或 LiDAR 測(cè)量，我們就一直重復(fù)步驟一和二。
  $${\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 測(cè)量，我們將首先計(jì)算卡爾曼增益。
  $${\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)}$$
  然后我們計(jì)算一個(gè)誤差狀態(tài)，我們將使用它來(lái)糾正我們的預(yù)測(cè)狀態(tài)。該誤差狀態(tài)為卡爾曼增益與預(yù)測(cè)位置和觀察位置之間的差的乘積。
  $$\delta {\mathbf{x}}_k={\mathbf{K}}_k\left({\mathbf{y}}_k?{\check{\mathbf{p}}}_k\right)\text{\hspace{1em}(17.15)}$$
  然后，我們將使用我們的誤差狀態(tài)來(lái)糾正我們的預(yù)測(cè)狀態(tài)。這種糾正對(duì)于位置和速度很簡(jiǎn)單，但是需要一些更復(fù)雜的代數(shù)來(lái)糾正四元數(shù)（這里可參考推薦閱讀中的四元數(shù)運(yùn)動(dòng)學(xué)一文）。
  $$\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)}$$
  最后，是更新狀態(tài)協(xié)方差矩陣。
  $${\hat{\mathbf{P}}}_k=\left(1?{\mathbf{K}}_k{\mathbf{H}}_k\right){\check{\mathbf{P}}}_k\text{\hspace{1em}(17.17)}$$

至此，就是一個(gè)完整的卡爾曼濾波過(guò)程。簡(jiǎn)單總結(jié)以下這一小節(jié)：

• 我們使用松耦合的擴(kuò)展卡爾曼濾波器框架將來(lái)自 IMU 的慣性測(cè)量與來(lái)自 GNSS 接收器和 LiDAR 的位置測(cè)量融合在一起進(jìn)行狀態(tài)估計(jì)。
• 我們假設(shè) GNSS 和 LiDAR 在同一坐標(biāo)系中為我們提供了位置估計(jì)，在實(shí)際中通常需要一些預(yù)處理。
• 其次，我們沒有考慮加速度計(jì)和陀螺儀偏差。這簡(jiǎn)化了我們的代數(shù)，但這不是一個(gè)現(xiàn)實(shí)的假設(shè)。在實(shí)際應(yīng)用中，如何不考慮偏差，我們的定位誤差會(huì)越來(lái)越大。
• 接下來(lái)，我們沒有討論狀態(tài)初始化。這通常被認(rèn)為是定位過(guò)程開始時(shí)的某個(gè)已知狀態(tài)。
• 最后，我們還假設(shè)我們的傳感器在空間和時(shí)間上都是對(duì)齊的。我們假設(shè)我們的傳感器在某種意義上是經(jīng)過(guò)標(biāo)定的，即我們不用擔(dān)心不同的時(shí)間步長(zhǎng)，也不用擔(dān)心我們?nèi)绾螌⒍嘟M測(cè)量值全部對(duì)齊到一個(gè)坐標(biāo)系中。我們將在下一小節(jié)介紹這部分內(nèi)容。

---

3. Sensor Calibration - A Necessary Evil

3.1 Intrinsic Calibration

在上一小節(jié)我們已經(jīng)了解了如何結(jié)合多個(gè)傳感器數(shù)據(jù)來(lái)估計(jì)車輛狀態(tài)，現(xiàn)在是時(shí)候討論一個(gè)我們迄今為止一直未討論的話題了。那就是傳感器標(biāo)定，這是工程師們不太喜歡談?wù)摰氖虑橹?#xff0c;但它對(duì)于正確進(jìn)行狀態(tài)估計(jì)又是十分必要的。就我個(gè)人而言（這里指課程講師），傳感器標(biāo)定對(duì)我來(lái)說(shuō)很重要，因?yàn)槲业牟┦垦芯空n題就是 相機(jī)和 IMU 的標(biāo)定，這也是我的學(xué)生今天繼續(xù)研究的主題。在本小中，我們將討論傳感器標(biāo)定的三種主要類型，以及為什么我們?cè)跒樽詣?dòng)駕駛汽車設(shè)計(jì)狀態(tài)估計(jì)器時(shí)需要考慮它們。要討論的三種主要標(biāo)定是：

• 內(nèi)參標(biāo)定，求解傳感器自身參數(shù)（例如相機(jī)內(nèi)參）；
• 外參標(biāo)定，求解傳感器相對(duì)于汽車坐標(biāo)的安裝位置和定向；
• 時(shí)間標(biāo)定（也成為時(shí)間同步），處理不同傳感器之間的測(cè)量時(shí)間差。

---

讓我們先看看內(nèi)參標(biāo)定。在內(nèi)參標(biāo)定中，我們希望確定傳感器模型的固定參數(shù)，以便我們可以在擴(kuò)展卡爾曼濾波器等估計(jì)器中使用它們。每個(gè)傳感器都有與之相關(guān)的參數(shù)，這些參數(shù)對(duì)于該特定傳感器是唯一的，并且通常是恒定的。

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

那么，我們?nèi)绾未_定這些內(nèi)參呢？ 實(shí)際中可以通過(guò)以下三方面來(lái)確定內(nèi)參：

• 最簡(jiǎn)單的就是從供應(yīng)商那里得到。通常，傳感器會(huì)在工廠進(jìn)行標(biāo)定，并附帶一份規(guī)格表，會(huì)介紹與傳感器相關(guān)的所有參數(shù)，例如激光雷達(dá)線數(shù)、不同掃描線間隔角度等；
• 但規(guī)格表并不總是足以進(jìn)行真正準(zhǔn)確的狀態(tài)估計(jì)，因?yàn)闆]有兩個(gè)傳感器完全相同，并且參數(shù)的真實(shí)值會(huì)有一些變化。 另一個(gè)方法是手動(dòng)測(cè)量這些參數(shù)。這對(duì)于輪胎半徑測(cè)量來(lái)說(shuō)是非常簡(jiǎn)單的，但對(duì)于像激光雷達(dá)這樣的東西來(lái)說(shuō)就不是那么簡(jiǎn)單了，因?yàn)樵趥鞲衅鲀?nèi)部用量角器測(cè)量是不切實(shí)際的；
• 另一種方法是將傳感器內(nèi)在參數(shù)作為車輛狀態(tài)的一部分進(jìn)行估計(jì)（如 IMU 加速度計(jì)和陀螺儀偏差），這種方法對(duì)特定傳感器可以精確標(biāo)定，并且也可以用來(lái)處理隨時(shí)間緩慢變化的參數(shù)。例如，如果不斷估計(jì)輪胎的半徑，可以幫助檢測(cè)何時(shí)會(huì)發(fā)生爆胎；

現(xiàn)在，因?yàn)槲覀冊(cè)诒菊n程中討論的估計(jì)器是通用的，所以我們已經(jīng)有了進(jìn)行這種自動(dòng)標(biāo)定的工具。要了解它是如何工作的，讓我們回到我們的汽車在一維運(yùn)動(dòng)的示例。 我們?cè)诤筝喩习惭b了一個(gè)編碼器來(lái)測(cè)量車輪的轉(zhuǎn)速。如果我們想估計(jì)車輪半徑以及位置和速度，我們需要做的就是將它添加到狀態(tài)向量中，并計(jì)算出新的運(yùn)動(dòng)和測(cè)量模型。此時(shí)，狀態(tài)向量如下，只是多了一個(gè)車輪半徑 $R$。
$$\mathbf{x}=?\begin{array}{l}p\\ \dot{p}\\ R\end{array}?\text{\hspace{1em}(17.18)}$$

此外，我們可以得到如下關(guān)系：
$$\begin{gathered} \mathbf{u}=\ddot{p} \\ \dot{p}=v=\omega R\text{\hspace{1em}(17.19)} \end{gathered}$$
此時(shí)運(yùn)動(dòng)模型為：
$${\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)}$$

對(duì)于測(cè)量模型，我們?nèi)匀皇侵苯油ㄟ^(guò) GPS 觀察位置，但現(xiàn)在我們也通過(guò)編碼器觀察車輪轉(zhuǎn)速。 因此，我們?cè)谀Ｐ椭邪祟~外的非線性觀察。我們可以使用擴(kuò)展或無(wú)跡卡爾曼濾波器來(lái)估計(jì)車輪半徑以及車輛的位置和速度。 因此，內(nèi)參標(biāo)定對(duì)于使用單個(gè)傳感器進(jìn)行狀態(tài)估計(jì)是必不可少的。
$${\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

外參標(biāo)定對(duì)于多傳感器融合十分重要。 在外參標(biāo)定中，我們感興趣的是確定所有傳感器通常相對(duì)于車輛的相對(duì)姿態(tài)。就像內(nèi)參標(biāo)定一樣，外參標(biāo)定也有不同的技術(shù)：

• 如果可以的話，可以訪問(wèn)像下圖這樣精確的車輛 CAD 模型，其中所有傳感器坐標(biāo)系都已經(jīng)精心布置；
• 如果不行的話，可能需要嘗試手東測(cè)量。 不幸的是，這通常很難或不可能準(zhǔn)確地完成，因?yàn)樵S多傳感器的坐標(biāo)系原點(diǎn)位于傳感器本身內(nèi)部，我們可能不想拆卸汽車和所有傳感器；
• 同樣地，我們也可以將外參包含在我們的狀態(tài)中來(lái)估計(jì)它們。對(duì)于任意傳感器配置，這可能會(huì)變得有點(diǎn)復(fù)雜，目前有很多研究在使用不同的技術(shù)來(lái)可靠地做到這一點(diǎn)；

---

3.3 Temporal Calibration

最后，一種實(shí)踐中經(jīng)常被忽視但仍然很重要的標(biāo)定是時(shí)間標(biāo)定。在我們所有關(guān)于多傳感器融合的討論中，我們一直在隱含地假設(shè)我們的所有測(cè)量值都是在同一時(shí)刻捕捉到的，或者至少足夠接近給定的精度水平。但是我們?nèi)绾未_定兩個(gè)測(cè)量是否足夠接近以被認(rèn)為是同步的呢？

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

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

簡(jiǎn)單總結(jié)下這一小節(jié)：

• 如果沒有標(biāo)定，傳感器融合是不可能的。在本小節(jié)中，我們了解了內(nèi)參標(biāo)定，它標(biāo)定我們傳感器模型的參數(shù)。
• 外參標(biāo)定，它為我們提供了將傳感器測(cè)量值轉(zhuǎn)換為公共參考坐標(biāo)系架所需的坐標(biāo)轉(zhuǎn)換。
• 時(shí)間標(biāo)定，處理同步測(cè)量以確保它們都對(duì)應(yīng)于相同的車輛狀態(tài)。
• 雖然有一些標(biāo)定技術(shù)可以解決所有這些問(wèn)題，但標(biāo)定仍然是一個(gè)非常活躍的研究領(lǐng)域。

---

4. Loss of One or More Sensors

4.1 Overview

在自動(dòng)駕駛汽車等應(yīng)用中，傳感器可能會(huì)因多種不同原因而發(fā)生故障或性能下降，例如天氣惡化、固件出現(xiàn)故障或電線出現(xiàn)松動(dòng)。我們?cè)诒灸K中已經(jīng)看到，即使我們所有的傳感器都正常工作，擁有多個(gè)互補(bǔ)傳感器對(duì)提供穩(wěn)健準(zhǔn)確的定位估計(jì)仍然是有益的。 但是，如果其中一個(gè)傳感器發(fā)生故障會(huì)發(fā)生什么？

到目前為止，我們已經(jīng)討論了 GNSS、IMU 和 LiDAR，但大多數(shù)現(xiàn)代自動(dòng)駕駛汽車還包括車輪編碼器、毫米波雷達(dá)、超聲波雷達(dá)和多個(gè)攝像頭等傳感器（如下圖所示）。

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

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

---

4.2 Redundancy is Crucial

對(duì)于這種冗余系統(tǒng)設(shè)計(jì)，我們可以查看以嚴(yán)格的安全標(biāo)準(zhǔn)而聞名的商業(yè)航空行業(yè)的示例。 作為冗余系統(tǒng)的一個(gè)例子，波音777 以三重冗余原則運(yùn)行。 所有主要系統(tǒng)，包括這臺(tái)飛行計(jì)算機(jī)，都有兩個(gè)備份，每個(gè)都有獨(dú)立的電源。在這里，我們可以看到每臺(tái)飛行計(jì)算機(jī)都有不同的處理器，以確保架構(gòu)本身的錯(cuò)誤不會(huì)影響所有三臺(tái)計(jì)算機(jī)。 如果其中一臺(tái)計(jì)算機(jī)出現(xiàn)故障，777 會(huì)使用共識(shí)算法無(wú)縫切換到另一臺(tái)計(jì)算機(jī)。

盡管自動(dòng)駕駛技術(shù)已經(jīng)取得了長(zhǎng)足的進(jìn)步，但我們距離最初的一些自動(dòng)駕駛挑戰(zhàn)（例如 2007 年 DARPA 城市挑戰(zhàn)賽）只有 10 年的時(shí)間。在這里，我們可以看到缺乏安全冗余是如何導(dǎo)致 MIT 和 Cornell 團(tuán)隊(duì)相互碰撞的。如果這次碰撞以更高的速度發(fā)生，結(jié)果可能會(huì)更糟。

還有一個(gè)例子，特斯拉 Model S 幾乎直接駛?cè)爰永Ｄ醽喐咚俟飞系牡睦印＿@是一個(gè)發(fā)人深省的提醒，對(duì)于設(shè)計(jì)自動(dòng)駕駛汽車的工程師來(lái)說(shuō)，仔細(xì)考慮所使用的不同傳感器的不同故障模式至關(guān)重要，以確保一個(gè)故障組件不會(huì)造成悲劇。

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

---

推薦閱讀

• 《動(dòng)手學(xué)無(wú)人駕駛（6）：基于IMU和GPS數(shù)據(jù)融合的自車定位》
• 重讀經(jīng)典《Quaternion kinematics for the error-state Kalman filter》

總結(jié)

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

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。