在上一講《Coursera自動駕駛課程第14講：Linear and Nonlinear Kalman Filters》 我們學(xué)習(xí)了卡爾曼濾波相關(guān)知識，包括：線性卡爾曼濾波（KF）、擴展卡爾曼濾波（EKF）、誤差卡爾曼濾波（ES-EKF）、無損卡爾曼濾波（UKF）。

本講我們學(xué)習(xí)自動駕駛定位中常用的兩種傳感器：慣性測量單元（IMU） 和全球?qū)Ш叫l(wèi)星系統(tǒng)（GNSS）。

B站視頻鏈接：

• 3D Geometry and Reference Frames (Video)
• The Inertial Measurement Unit (Video)
• Global Navigation Satellite System (Video)

---

1. 3D Geometry and Reference Frames

1.1 Introduction

回顧一下中學(xué)所學(xué)知識，向量是具有大小和方向的矢量。在不同的坐標(biāo)系下，同一個向量可以使用不同的坐標(biāo)來表示。如下圖所示的向量 $r$，在兩個坐標(biāo)系 ${\stackrel{\mathcal{F}}{\to }}_a$ ，${\stackrel{\mathcal{F}}{\to }}_b$ 下將會有兩個不同的表示 ${\mathbf{r}}_a$ 和 ${\mathbf{r}}_b$。

很顯然，向量${\mathbf{r}}_b$ 可由向量 ${\mathbf{r}}_a$ 經(jīng)過旋轉(zhuǎn)變換而來，即可以找到一個旋轉(zhuǎn)矩陣 ${\mathbf{C}}_{ba}$，滿足 ：
$${\mathbf{r}}_b={\mathbf{C}}_{ba}{\mathbf{r}}_a$$

旋轉(zhuǎn)矩陣 ${\mathbf{C}}_{ba}$ 表示向量從 $a$ 坐標(biāo)系向 $b$ 坐標(biāo)系進行變換。

然而在現(xiàn)實世界中，兩個坐標(biāo)系的原點經(jīng)常不是同一個原點，這時候還需要進行平移變換處理，如下圖所示。慣性坐標(biāo)系為 ${\stackrel{\mathcal{F}}{\to }}_i$，車輛坐標(biāo)系為 ${\stackrel{\mathcal{F}}{\to }}_v$。現(xiàn)在我們用 ${\mathbf{r}}_i^{pi}$ 表示點 $P$ 在慣性坐標(biāo)系的位置，${\mathbf{r}}_i^{vi}$ 表示車輛坐標(biāo)系在慣性坐標(biāo)系下的位置，${\mathbf{C}}_{iv}$ 表示車輛坐標(biāo)系到慣性坐標(biāo)系的旋轉(zhuǎn)變換矩陣，${\mathbf{r}}_v^{pv}$ 表示點 $P$ 在車輛坐標(biāo)系下的位置。則 ${\mathbf{r}}_i^{pi}$ 可以寫成：
$${\mathbf{r}}_i^{pi}={\mathbf{r}}_i^{vi}+{\mathbf{C}}_{iv}{\mathbf{r}}_v^{pv}$$

---

1.2 Rotation Representation

旋轉(zhuǎn)變換可以有很多方式來表示。 最常見的是使用旋轉(zhuǎn)矩陣。 它通常也被稱為方向余弦矩陣。 旋轉(zhuǎn)矩陣的一個重要的性質(zhì)就是 $R^{?1}=R^T$。

第二種表示方法就是使用單位四元數(shù)。 四元數(shù)本身就是一個有趣的數(shù)學(xué)主題，但在本課程中，知道單位四元數(shù)可以表示為單位長度的四維向量就足夠了，這里有兩個需要注意的東西，一是旋轉(zhuǎn)軸 $\mathbf{u}$，二是旋轉(zhuǎn)角度 $?$。 為什么要使用四元數(shù)？ 因為它不受奇點的影響，與旋轉(zhuǎn)矩陣相比它只需要四個參數(shù)而不是九個。

第三種表示方法就是使用歐拉角。歐拉角之所以有吸引力，原因在于它是一種直觀簡約的表示，只需要知道繞每個軸轉(zhuǎn)動的角度就可以得到最終的旋轉(zhuǎn)變換。

在自動駕駛中會選擇哪一種旋轉(zhuǎn)表示方式呢？ 這要看具體情況。 每種表達方式都有優(yōu)缺點。旋轉(zhuǎn)矩陣可以表示任何旋轉(zhuǎn)，但需要九個參數(shù)并有六個約束。單位四元數(shù)也可用于表示任何旋轉(zhuǎn)，但它也有一個約束。 最后，歐拉角不受約束、直觀地可視化并且僅需要三個參數(shù)，但受奇點影響。

---

1.3 Reference Frames

現(xiàn)在讓我們看一下車輛定位時將使用到的重要參考坐標(biāo)系。 第一個坐標(biāo)系是地心慣性坐標(biāo)系或 ECIF。該坐標(biāo)系的原點位于地球中心，$\mathbf{z}$ 軸指向正北，$\mathbf{x}$ 軸和 $\mathbf{y}$ 軸相對于非常遙遠(yuǎn)的恒星是固定的。 這意味著雖然地球繞 $\mathbf{z}$ 軸旋轉(zhuǎn)，但 $\mathbf{x}$ 和 $\mathbf{y}$ 軸不會移動。

接下來，以地球為中心的（ECEF）與 ECIF 類似，只是它的 $\mathbf{x}$ 軸與本初子午線對齊并與地球一起旋轉(zhuǎn)。$\mathbf{y}$ 軸由右手定則確定。ECEF坐標(biāo)系是定位中經(jīng)常用到的一個坐標(biāo)系，主要是與GNSS傳感器有關(guān)。

盡管ECEF和ECIF在我們討論衛(wèi)星和飛機上的慣性傳感時很有用，但對于汽車應(yīng)用，我們通常希望使用相對于地面固定的框架。為此，我們將使用我們所說的導(dǎo)航坐標(biāo)系。 一個常見的導(dǎo)航坐標(biāo)系是選擇某個位置作為起點并且坐標(biāo)系與北、東和向下反向?qū)R的導(dǎo)航坐標(biāo)系（NED坐標(biāo)）。 最后，我們還經(jīng)常需要考慮剛性連接到傳感器的傳感器坐標(biāo)系，例如 LIDAR、GPS接收器或慣性測量單元。傳感器坐標(biāo)系通常與車輛坐標(biāo)系不同，后者可以放置在車輛上任何方便的地方，例如在質(zhì)量中心。 對于定位，我們通常會忽略車輛和傳感器坐標(biāo)系之間的區(qū)別，并假設(shè)如果我們可以跟蹤傳感器，我們應(yīng)該能夠在適當(dāng)校準(zhǔn)的情況下跟蹤車輛上的任何點，這是我們稍后研究的主題。

---

2. The Inertial Measurement Unit

2.1 Introduction

慣性測量單元或IMU測量物體在慣性空間中的運動。如今，幾乎所有智能手機中都配備了IMU。它們經(jīng)常用于健康跟蹤的計步，以及最近用作增強現(xiàn)實設(shè)備。

盡管它們在今天無處不在，但能夠準(zhǔn)確跟蹤運動物體運動的傳感器的開發(fā)是20世紀(jì)的一項重大成就。 IMU早在GPS之前就為跨洋飛行提供了幫助，并且作為機載制導(dǎo)、導(dǎo)航和控制系統(tǒng)的一部分對阿波羅任務(wù)至關(guān)重要。阿波羅航天器依靠IMU來準(zhǔn)確跟蹤飛行器在漫長的月球航行中的位置和方向。

在現(xiàn)代自動駕駛汽車中，IMU扮演著非常相似的角色。在來自其他傳感器的導(dǎo)航信息不可用或不可靠時來進行導(dǎo)航計算。那么什么是IMU呢？

一般來說，慣性測量單元是一個復(fù)合傳感器套件，它結(jié)合了三軸陀螺儀和三軸加速度計來跟蹤剛體的外部自由運動。一些IMU還包含磁力計或指南針，以幫助跟蹤方向。IMU有多種形狀和形式。現(xiàn)代智能手機中的傳感器相對便宜，批量購買時的成本通常不到幾美元。它們重量輕，需要的功率相對較小，但會產(chǎn)生相當(dāng)嘈雜的測量結(jié)果。而更昂貴的IMU使用更復(fù)雜的組件并具有更準(zhǔn)確的校準(zhǔn)模型，可以消除溫度波動等的影響。

---

下面簡單介紹下陀螺儀。陀螺儀這個詞本身來自希臘詞。歷史上，陀螺儀是一個旋轉(zhuǎn)的圓盤，它的角動量會阻止方向的變化。在 19 世紀(jì)末和 20 世紀(jì)初，工程師們意識到這種設(shè)備可以用作海洋和航空導(dǎo)航的方向參考。

在現(xiàn)代陀螺儀中，旋轉(zhuǎn)輪通常被微機電系統(tǒng)取代，該系統(tǒng)由一個小的硅音叉組成，該系統(tǒng)根據(jù)施加的旋轉(zhuǎn)或方向變化來改變其共振特性。這種傳感器要便宜得多，而且體積也很小。然而，它們會產(chǎn)生嘈雜的測量結(jié)果并且對溫度的波動很敏感。此外，它們測量旋轉(zhuǎn)速率，而不是直接測量方向，因此必須對輸出信號進行數(shù)值積分以確定方向變化。這個過程會在最終的方向估計中引入額外的誤差。

---

加速度計測量沿單個軸的加速度。更便宜的基于MEMS的加速度計使用微型懸臂梁，并附有檢測質(zhì)量。當(dāng)傳感器加速時，光束發(fā)生偏轉(zhuǎn)。可以通過電容電路測量這種偏轉(zhuǎn)，并將其轉(zhuǎn)換為加速度值。

然而，出于導(dǎo)航目的，我們更關(guān)心的是相對于某個固定參考系的加速度。為了計算這個加速度，我們需要使用重力場中加速度計的基本方程。
$$\mathbf{f}+\mathbf{g}={\ddot{\mathbf{r}}}_i$$

---

2.2 IMU Measurement Models

首先介紹陀螺儀測量模型，模型方程如下：
$$\omega (t)={\omega }_s(t)+{\mathbf{b}}_{\mathrm{g}\mathrm{y}\mathrm{r}\mathrm{o}}(t)+{\mathbf{n}}_{\mathrm{g}\mathrm{y}\mathrm{r}\mathrm{o}}(t)$$

其中 ${\omega }_s(t)$ 是陀螺儀測量值， 測量模型中包含一個緩慢變化的偏置項 ${\mathbf{b}}_{\mathrm{g}\mathrm{y}\mathrm{r}\mathrm{o}}(t)$ 和一個白高斯加性噪聲項 ${\mathbf{n}}_{\mathrm{g}\mathrm{y}\mathrm{r}\mathrm{o}}(t)$ 來模擬傳感器誤差。盡管陀螺儀確實測量地球的自轉(zhuǎn)，但對于我們只關(guān)心短時間運動的應(yīng)用，忽略這一點通常是安全的。

加速度計測量模型將具有類似的噪聲和偏置項。但是現(xiàn)在，我們不再像使用旋轉(zhuǎn)角速度那樣直接測量身體加速度，而是需要使用重力場中加速度計的基本方程來消除重力的影響。
$$\mathbf{a}(t)={\mathbf{C}}_{sn}(t)\left({\ddot{\mathbf{r}}}_n^{sn}(t)?{\mathbf{g}}_n\right)+{\mathbf{b}}_{\mathrm{a}\mathrm{c}\mathrm{c}\mathrm{e}\mathrm{l}}(t)+{\mathbf{n}}_{\mathrm{a}\mathrm{c}\mathrm{c}\mathrm{e}\mathrm{l}}(t)$$

可以看到，為了得到準(zhǔn)確的加速度，我們首先要得到準(zhǔn)確的方向矩陣 ${\mathbf{C}}_{sn}(t)$，而這又需要我們得到準(zhǔn)確的角速度。

最后，讓我們討論一下IMU模型的一些限制。

• 首先，準(zhǔn)確的方向估計對于準(zhǔn)確的位置估計至關(guān)重要。 當(dāng)我們將測得的比力轉(zhuǎn)換為加速度時，我們必須確保重力方向是正確的。 否則，即使方向上的一個小錯誤也會讓我們認(rèn)為我們正在加速，而實際上我們并沒有。
• 其次，我們推導(dǎo)出的兩個模型都忽略了地球自轉(zhuǎn)的影響。 對于更遠(yuǎn)距離的導(dǎo)航（飛機或?qū)棇?dǎo)航時不能忽略），這實際上很重要。
• 最后，我們推導(dǎo)出的模型適用于捷聯(lián)IMU。 這些IMU是物理固定在車輛上，并且沒有在萬向節(jié)上安裝旋轉(zhuǎn)輪。 盡管后者可能更準(zhǔn)確，但由于體積大且成本高，它們很少用于汽車應(yīng)用。

---

3. Global Navigation Satellite System

3.1 Introduction

與上一節(jié)介紹的IMU一樣，幾乎每部現(xiàn)代智能手機都至少有一種類型的GNSS接收器。盡管我們現(xiàn)在認(rèn)為它們是理所當(dāng)然的，但第一個現(xiàn)代全球定位衛(wèi)星系GPS 是在 1980 年代為軍事用途而建造的。

到 1995 年第二版系統(tǒng)全面運行時，GPS 已免費向公眾開放。部分原因是大韓航空 007 號航班在 1983 年墜毀。007 號航班是一架 747 客機，從紐約飛往首爾，在阿拉斯加安克雷奇加油。當(dāng)它從安克雷奇飛往首爾時，它偏離了計劃的飛行路線，在蘇聯(lián)領(lǐng)空停留了幾個小時后被一架戰(zhàn)斗機擊落。

為了穿越北太平洋，007 航班依靠慣性導(dǎo)航系統(tǒng)進行引導(dǎo)。飛行員未能正確初始化系統(tǒng)，錯誤地將飛機保持在特定的磁航向。反過來，這架飛機偏離了計劃航線 300 多公里。在 007 航班被擊落后不久，羅納德·里根總統(tǒng)發(fā)布了一項指令，允許美國全球定位系統(tǒng)在完全開發(fā)后免費向公眾開放。

---

GPS系統(tǒng)由位于六個軌道平面的 24-32 顆衛(wèi)星組成。衛(wèi)星會定期退役和更換。高度約為 20,000 公里，軌道周期不到 12 小時。系統(tǒng)的設(shè)計使得至少四顆衛(wèi)星在地球上的任何表面點始終可見。每顆衛(wèi)星以兩種頻率進行廣播，一種用于民用，一種用于軍用。每個廣播信號都包含一個偽隨機碼，用于識別衛(wèi)星位置和信號傳輸時間。

GPS背后的基本原理是到達時間測距。接收器通過將自己的內(nèi)部時鐘與傳輸時間的時鐘進行比較來計算每個可見衛(wèi)星的距離。使用電磁信號以光速傳播的知識將時間差轉(zhuǎn)換為距離。為了計算 3D 位置，測距方程需要至少四顆可見衛(wèi)星。如果已知高度并且只需要 2D 位置，則只需要三顆衛(wèi)星。

---

3.2 Trilateration

對于每顆衛(wèi)星，偽距測量方程為：
$${\rho }^{(i)}=c\left(t_r?t_s\right)=\sqrt{{\left({\mathbf{p}}^{(i)}?\mathbf{r}\right)}^T\left({\mathbf{p}}^{(i)}?\mathbf{r}\right)}+c\Delta t_r+c\Delta t_a^{(i)}+{\eta }^{(i)}$$

各物理量含義如下：

請注意，這與三角測量不同，三角測量基于角度測量計算位置。 該模型考慮了接收器時鐘誤差、大氣傳播延遲和測量噪聲。術(shù)語偽距是指距離信息被上述誤差源破壞的事實。 每個偽距測量在 2D 中定義一個圓或在 3D 中定義一個球體。 如果我們正好有四顆衛(wèi)星，我們可以明確地求解接收器位置和接收器時鐘誤差。

如果我們有四個以上，我們可以使用最小二乘法找到假設(shè)高斯噪聲的最大似然位置。

---

3.3 Error Sources and Improvements

GPS 存在多種誤差源：

• 首先，電離層中的帶電離子可以將信號延遲。
• 周圍的地形和建筑物可能會引起反射，從而增加信號在到達接收器之前傳播的距離，這些被稱為多徑誤差。
• 時鐘同步或衛(wèi)星位置信息中的任何小錯誤都可能導(dǎo)致災(zāi)難性后果。即使是 1 微秒的計時誤差也會導(dǎo)致 300 米的重大位置誤差。星歷數(shù)據(jù)和衛(wèi)星時鐘都會經(jīng)常更新和重新校準(zhǔn)，但校準(zhǔn)可能會過時。
• 最后，可見衛(wèi)星的幾何配置也會導(dǎo)致定位精度的變化。這被稱為精度的幾何稀釋。為了獲得更高的精度，最好采用衛(wèi)星遍布天空的配置。

幸運的是，對于某些應(yīng)用程序，我們可以通過以各種方式增強系統(tǒng)來提高GNSS精度。

• 差分GPS 可以通過使用一個或多個固定基站更準(zhǔn)確的已知位置來校正接收機定位估計。修正以不同的頻率廣播到移動車輛中的 GNSS 接收器。
• 在某些情況下，實時動態(tài)或RTK GPS 利用載波相位信息將定位精度提高到兩厘米。

盡管這兩種技術(shù)都可以顯著提高 GPS 的精度，但它們的實施成本通常非常高。正如我們在之前的視頻中提到的，慣性傳感器對于導(dǎo)航非常有用。然而，隨著時間的推移，它們會漂移或累積無限的誤差。相比之下，GPS 系統(tǒng)提供有界誤差定位更新。配備 GPS 的自動駕駛汽車將始終保持有保證的定位精度水平，除非 GPS 接收器出現(xiàn)故障或丟失至少四顆衛(wèi)星的軌道。

總結(jié)

以上是生活随笔為你收集整理的Coursera自动驾驶课程第15讲：GNSS and INS Sensing for Pose Estimation的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

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