GPS+RTK

GPS組成

GPS分為三部分。

1. 空間星座部分：由至少24顆衛星組成（目前有30多顆在軌運行），分布在6個中地球軌道上。保證全球任何地方、任何時間至少能接收到4顆以上的衛星信號。每顆衛星不斷播發一種包含衛星星歷?（自己的精確軌道位置）、時鐘校正參數和系統狀態的無線電信號。
2. 地面監控部分：由主控站、監測站和注入站組成，分布在全球。監測站負責持續跟蹤所有可見衛星，收集它們的信號和數據，并發送給主控站；主控站負責計算每顆衛星的精確軌道（生成星歷）和時鐘誤差，并預測其未來的軌跡；注入站負責定期將主控站計算出的星歷和時鐘參數上傳給各顆衛星。
3. 用戶設備部分：就是我們手中的GPS接收機（手機、車載導航、專業設備等）(GPS衛星只發送信號不接收信號)，GPS接收器通過測量信號的飛行時間然后用光速乘以時間來計算與衛星的距離。

三邊測量(GPS原理)

在一個平面上有三個物品，分別為一棵樹一個路燈和一個房子(位置已確定)，現在我告訴你我的位置距離樹20m，這時你就會以樹為圓心畫一個半徑為20m的圓，我的位置可能出現在這個圓的任意位置；現在我又告訴你我距離路燈15m這時你又會以路燈為圓心畫一個半徑為15m的圓來進一步確定我的位置，我的位置可能出現在上面兩圓的兩個交點上。現在我又告訴你我距離房子10m，這時再以房子為圓心畫一個半徑為10m的圓，這三個圓必交于一點，這個確定的點就是我的位置。這個過程稱為三邊測量

GPS的原理類似，其核心思想是通過測量信號從衛星到接收機的傳播時間來計算距離，然后通過幾何上的“三邊測量”（類似交會定位）來計算出自己的位置。?GPS定位的根本是測量距離(速度 × 時間 = 距離)?，GPS信號是以光速傳播的無線電波，速度?c?是一個已知常數（約每秒30萬公里），而衛星在發送信號時，會打上一個非常精確的時間戳?（由衛星上的原子鐘生成）。接收機在接收到信號時，也會記錄一個接收時間。兩者的時間差就是信號的傳播時間?Δt。但是這個計算出的距離，因為各種誤差的存在，并不完全是真實的幾何距離，所以被稱為 ??“偽距”?。誤差來源通常為以下幾中：(這也就是GPS定位精度只有幾米到十幾米的原因)

1. 要精確測量?Δt，要求衛星和接收機的時鐘必須完全同步。衛星使用昂貴的原子鐘，精度極高。但我們的手機、車載接收機使用的是普通的石英鐘，精度差很多，存在鐘差。這種鐘差是可以解決的，下面介紹的第四顆衛星就是為了解決這個誤差。
2. 信號穿過大氣層時速度會變慢，路徑會彎曲。這是最大的誤差源。
3. 信號被周圍的建筑物、地面等反射后，接收機收到多個延遲的信號副本，造成干擾

GPS的三邊測量介紹如下，知道了一顆衛星的距離，意味著你位于以這顆衛星為球心、以該距離為半徑的球面上的某個地方；當知道了兩顆衛星的距離，兩個球面相交，意味著你在一個圓環上；當知道了三顆衛星的距離，三個球面相交理論上可以得到兩個點。但其中一個點通常在地球之外或不合理的位置，可以排除，從而得到一個二維的平面坐標（經度、緯度）?；當知道了四顆衛星的距離，可以確定唯一的點，第四顆衛星的關鍵作用，就是為了解算出接收機的鐘差。這樣，接收機就能在時鐘不準的情況下，依然計算出正確的位置，并同時輸出精確的時間信息。所以我們常說GPS接收機也是一個授時工具。

關于第四顆衛星的作用，這里再做詳細解釋。接收機事實上需要解一個包含四個未知數的方程，即“偽距 = 真實幾何距離 + 鐘差誤差 × 光速 (c)”，需要解的四個未知數是接收機的三維坐標 (X, Y, Z)?? 和 ?接收機的鐘差，因此，必須有4顆（或以上）衛星的觀測數據，才能解出這4個未知數，得到精確的絕對位置。那如果我們就用三顆衛星，而將鐘差誤差看作0，事實上也是可以解出接收機的三維坐標 (X, Y, Z)??的，但是現實世界中，接收機的時鐘（普通石英鐘）誤差非常大并且光速c又很大，如果你忽略它，計算出的位置 (x, y, z) 將會包含巨大的誤差，可能偏差幾公里甚至上百公里，這個結果完全不可用。?第四顆衛星的作用就在于此：它提供了第四個約束條件，使得方程組有唯一解。

RTK(實時動態定位)

普通的單點GPS定位（比如手機或車載導航）存在多種誤差源，導致其精度通常在幾米到十幾米之間。RTK技術就是為了解決此誤差而生的。

RTK系統組成：

• 基準站：這是一個固定不動的GPS接收機(也稱為基站，且不止一個)，它的精確位置（經緯高）是通過精密測量已知的。它持續不斷地觀測所有可見的GPS衛星。
• ?移動站 ：安裝在自動駕駛車輛上的GPS接收機。
• ?數據鏈路?：用于將基準站的觀測數據實時（通常是通過4G/5G移動網絡）傳輸給移動站

RTK系統流程：

1. 基準站計算誤差：??
基準站根據它已知的精確位置和接收到的衛星信號，可以反推出每顆衛星信號的綜合誤差值?（包括大氣延遲、鐘差等）。因為它自己的位置是精確已知的，所以它算出的衛星距離（偽距）與真實幾何距離的差值，就是各種誤差的總和。

?2. 數據發送：??
基準站將這些計算出的誤差校正數據通過數據鏈路實時發送給附近的移動站（如自動駕駛汽車）。

?3. 移動站進行差分校正：??
移動站接收到基準站的校正數據后，將其應用到自己的原始觀測數據上。關鍵的一步來了：RTK不僅使用普通的GPS偽距碼，更重要的是使用載波相位測量。

• ?載波相位?：GPS衛星發射的無線電信號波長很短（約19厘米）。接收機可以非常精確地測量載波信號的相位（可以想象成測量波峰和波谷的位置），其測量精度可以達到毫米級。但問題是，它只能測出不足一個整波長的小數部分，無法直接知道距離衛星有多少個整周數?（稱為“整周模糊度”）。

?4. 解算“整周模糊度”：??
RTK技術的核心算法就是快速、準確地解算出這個“整周模糊度”。一旦成功解算（稱為“固定解”），移動站就能利用極其精確的載波相位觀測值，結合基準站傳來的差分校正數據，?消除掉公共誤差?（因為兩臺接收機距離較近，它們看到的大氣誤差等非常相似）。

?5. 輸出高精度位置：??
通過這一系列復雜的計算，移動站最終能夠計算出相對于基準站的厘米級精度的相對位置。由于基準站的絕對位置是已知的，移動站自然也就能獲得厘米級的絕對地理坐標。

慣性導航系統(INS)

簡單來說，慣性導航是一種基于慣性原理的自主式導航技術。它的核心思想是：我知道初始位置，然后我通過持續測量自身的加速度和旋轉角速度，來推算出我每一刻的新位置、新速度和新姿態（朝向）。?它不依賴任何外部信號（如GPS、基站、Wi-Fi），完全自包含（Self-contained）。

慣性導航系統中最重要的”是慣性測量單元（Inertial Measurement Unit, IMU）?,注意區分概念，INS是一個完整的系統，它包含IMU并利用其數據進行計算，最終輸出導航結果，而IMU（慣性測量單元）是一個傳感器，它只負責測量數據。IMU通常包含兩類核心傳感器：

1. ?加速度計 (Accelerometer)??：?測量物體在三個軸向（X, Y, Z）?? 上的線加速度。

2. ?陀螺儀 (Gyroscope)??：?測量物體繞三個軸向的角速度?（旋轉的快慢）。

IMU的工作流程就是一個“積分”過程：??陀螺儀測得的角速度? → 積分一次 → 得到姿態角度?（俯仰、滾轉、偏航）。加速度計測得的加速度? → 扣除重力影響 → 積分一次 → 得到速度? → 再積分一次 → 得到位移。

INS優勢如下：

• GPS只能提供位置和速度?（二維或三維），但無法直接知道車輛的姿態——即車輛的俯仰角?（上下點頭）、滾轉角?（左右傾斜）和偏航角?（車頭朝向）。
• IMU的更新頻率極高（可達1000Hz），遠超GPS（通常10-20Hz）。這意味著它能提供極其連續、平滑的位置、速度和姿態變化，對于需要快速反應的車輛控制系統（如轉向、剎車）至關重要。
• 當車輛進入隧道、地下車庫、城市峽谷（高樓之間）?? 或遇到惡劣天氣時，GPS信號會中斷或嚴重退化。此時，INS可以基于信號丟失前最后已知的精確位置，繼續提供短時間的高精度定位，保證自動駕駛功能不中斷。

INS很少單獨工作。它幾乎總是與GPS、輪速計、攝像頭、激光雷達等傳感器通過算法（如卡爾曼濾波 Kalman Filter）融合在一起，形成組合導航系統。例如，當GPS信號良好時?用GPS提供的絕對位置來持續校正和校準INS，抑制其誤差累積，并精確估算出INS的誤差參數（如零偏）；?當GPS信號丟失時?，INS利用剛才被校準好的狀態，進行高精度的自主推算。由于誤差已被最大限度地抑制，它在信號丟失的幾十秒內仍能保持極高的定位精度。

• ?GPS?：提供長期穩定、絕對準確的位置，但更新慢、易中斷。
• ?INS?：提供短期精確、高頻、連續的相對位移和姿態，但誤差會隨時間累積（漂移）。

激光雷達定位

激光雷達定位的核心思想是：?通過實時掃描周圍環境并獲取高精度的三維點云數據，然后將這些數據與一份預先存在的高精度地圖進行匹配，從而計算出車輛在當前地圖中的精確位置和姿態（朝向）。它不像GPS那樣依賴衛星信號，也不像INS那樣會漂移，它是一種基于環境特征匹配的絕對定位方式。

盡管有GPS/RTK和INS的組合，但它們仍有固有缺陷：

1. ?GPS信號丟失?：在隧道、地下、城市峽谷等場景失效。
2. ?INS誤差累積?：一旦GPS長時間失效，INS的漂移會使得定位誤差變得不可接受。
3. ?精度要求極高?：自動駕駛需要厘米級的定位精度來確定自己在哪條車道的具體位置，這是GPS/RTK+INS難以始終穩定提供的。

激光雷達定位恰好彌補了這些缺陷，提供了穩定、可靠、高精度的絕對位置信息。激光雷達定位通常不是一個一步到位的過程，它遵循一個分層、逐步精細的流程，如下圖所示，其核心是 ??“預測-匹配-更新”?? 的循環，其核心技術原理如下：

1. 預測(初始位置預測)：在開始匹配之前，算法需要一個“猜測”的初始位置，此預測過程通常由GPS/RTK+INS組合系統?提供一個大致的、米級精度的位置和朝向。
2. 匹配(掃描匹配)：這是激光雷達定位的算法核心。它的目的是找到一種變換（旋轉和平移），使得當前掃描得到的實時點云與高精度地圖上的局部參考點云盡可能完美地重疊。主要算法有兩種，迭代最近點(ICP)和正態分布變換(NDT)
3. 更新(后處理與傳感器融合)：單純的掃描匹配結果可能仍有微小誤差或抖動。為了得到更穩定、可靠的位置輸出，還需要使用卡爾曼濾波（KF）或粒子濾波（PF）等算法，對多幀激光雷達的匹配結果進行平滑處理，濾掉不合理的抖動。將激光雷達定位的結果與GPS、IMU、輪速計等信息進行傳感器融合。最終輸出一個最優的、最可靠的位姿估計。

關于激光雷達定位的更詳細解釋及算法，參考以下文章：

激光雷達定位算法-CSDN博客

視覺定位

視覺定位的核心思想是：?通過攝像頭捕獲周圍的視覺環境（如車道線、交通標志、建筑物、樹木等），然后將這些視覺信息與已知的地圖或模型進行匹配，從而推斷出車輛自身的精確位置和姿態（朝向）。?它與激光雷達定位的邏輯相似（都是環境匹配），但使用的傳感器和數據形式完全不同：一個是三維幾何點云，一個是二維紋理圖像。

在激光雷達成本高昂的背景下，視覺定位提供了一種相對低成本的解決方案。現在出現的一些視覺SLAM方法還可以不依賴預先繪制的高精地圖，真正實現“即走即建圖”。

視覺定位主要分為三條路線：

1.基于語義地圖的視覺定位（主流方案）：

這是目前最可靠、與高精地圖結合最緊密的方案，其流程與激光雷達定位類似。

a. 離線建圖階段：??

• 采集車（搭載攝像頭、GPS/IMU等）預先行駛在道路上，收集大量圖像和位置數據。
• 通過算法（如SFM， Structure from Motion）從這些圖像中提取穩定的語義特征?（如交通標志的角點、路燈的頂端、建筑物輪廓等），并計算出它們在全球坐標系中的精確3D位置，生成一份視覺高精語義地圖。這份地圖存儲的不是原始圖像，而是特征的3D坐標和描述子。

?b. 在線定位階段：??

1. ?特征提取?：車輛上的攝像頭捕獲實時圖像，提取出相同的語義特征。
2. ?特征匹配?：將當前圖像中提取的2D特征點，與視覺語義地圖中存儲的3D路標點進行匹配。尋找“當前看到的這個點，對應地圖上的哪個3D點”。
3. ?PnP求解?：一旦找到足夠多的2D-3D匹配對，就可以使用PnP（Perspective-n-Point）?? 算法。PnP是一種幾何解法，通過已知的多個3D點及其在圖像上的2D投影，直接反推出相機的6自由度位姿?（位置和旋轉）。
4. ?優化?：使用集束調整（Bundle Adjustment）等算法對位姿結果進行優化，使其更精確。

2.視覺SLAM(同步定位與建圖)

SLAM更像是一個“邊走邊畫地圖”的過程。它不需要預先的高精地圖，而是由車輛自身在行駛過程中同時完成定位和局部地圖構建。

工作原理?：

• ?跟蹤（Tracking）??：通過比較連續幀圖像，估算出相機自身的運動（通過視覺里程計, VO），并初步估計出當前位姿。
• ?建圖（Mapping）??：將跟蹤過程中觀察到的新的環境特征點添加到當前維護的局部地圖中。
• ?回環檢測（Loop Closure）??：識別出曾經訪問過的地點，從而校正整個行程中累積的定位漂移。
• ?在自動駕駛中的應用?：更多用于停車場等缺乏GPS信號的封閉結構化環境，或作為城市道路駕駛中的一種輔助定位手段。純粹的視覺SLAM難以滿足高速自動駕駛對全局絕對定位的苛刻要求。

3.基于學習的端到端定位

利用深度學習模型，?直接從圖像中回歸出車輛的位姿。這種方法目前更多處于研究階段，尚未成為主流自動駕駛系統的核心定位方案。

?工作原理?：

• 使用一個龐大的數據集（圖像+對應的真實位姿標簽）來訓練一個深度神經網絡（如CNN）。
• 訓練完成后，網絡輸入一張或多張當前圖像，直接輸出車輛的估計位置（x, y, z）和姿態（俯仰、滾轉、偏航）。

?優點?是方法簡潔，無需復雜的特征匹配和幾何計算。?目前的缺點?是泛化能力差?，在一個地區訓練的網絡，到另一個外觀迥異的地區可能完全失效，除此之外目前難以達到幾何方法的精度和可靠性，容易受到光照、天氣、季節變化的影響

多傳感器融合

原因

通過上面對各傳感器實現的定位的介紹可以發現單一傳感器實現的定位存在各種問題，比如GPS信號易受遮擋（隧道、室內、城市峽谷），更新頻率低（通常10Hz），精度有限（民用級米級），無法提供姿態信息；IMU積分會導致誤差累積；LiDAR在特征稀疏的環境（如長直走廊、沙漠）中匹配困難，受惡劣天氣（濃霧、大雨）影響大，計算量大，成本高；攝像頭受光照影響大（夜晚、過曝），無法直接測距（需通過雙目或移動視差計算），計算復雜度高（特征提取、匹配），對快速運動敏感。將這些傳感器融合使用，取長補短，是必然的選擇。

常見組合方式

1. GPS/RTK + IMU（絕對定位 + 高頻遞推）

這是最經典、最基礎的組合，為其他融合系統提供了一個“絕對位置”的錨點。??

• ?GPS (Global Positioning System):??
  • ?提供：?? 低頻（1-10Hz）、全局、絕對坐標（經緯度高度）。無累積誤差。
  • ?缺陷：?? 信號易丟失（隧道、高樓間）、精度有限（民用單點定位米級）、更新慢、有噪聲。
• ?RTK (Real-Time Kinematic):??
  • ?提供：?? GPS的增強形式，通過地基或星基校正，提供厘米級的高精度絕對定位。
  • ?缺陷：?? 需要接收校正信號，初始化需要時間（收斂），在信號遮擋區域同樣會失效。
• ?IMU (Inertial Measurement Unit):??
  • ?提供：?? 極高頻率（100-1000Hz）的機體三軸加速度和三軸角速度測量。
  • ?作用：?? 通過對角速度積分得到姿態（朝向）?，對加速度積分（并去除重力影響）得到速度和位移變化。?在GPS兩次更新的間隔內，提供平滑、連續的高頻運動推算。??
  • ?致命缺陷：?? 積分導致誤差快速累積，幾分鐘內位置估計就會“飄走”（Drift）。

• ?融合的核心思想：??
  ?用GPS的絕對位置（無漂移）來持續校正IMU積分帶來的累積誤差。?? IMU在GPS失效時（如進入隧道）提供短時、高精度的航位推算（Dead Reckoning）。

2. 視覺慣性里程計 - VIO（相對定位 + 環境感知）

這是目前機器人學和AR/VR領域的核心技術，讓設備在無GPS的環境中實現精準定位。?視覺慣性里程計（VIO）?? 是一種通過融合相機圖像和慣性測量單元（IMU）數據來實時估算設備自身在三維空間中的位置（平移）?? 和姿態（旋轉）?? 的技術

• ?單目/雙目/RGB-D 相機 (Camera):??
  • ?提供：?? 豐富的圖像信息。通過追蹤特征點，利用多視角幾何原理可以計算出相機自身的運動（旋轉和平移），稱為視覺里程計 (VO)?。
  • 單目相機無法知道真實世界的尺度（就像看一張照片，你無法判斷里面的一輛車是模型還是真車），所以它最大的問題是初始化（Initialization）?。因為從單張2D圖片無法得到深度信息，系統必須通過移動相機，利用多視角幾何（如對極幾何）來三角化出特征點的深度，這個過程需要時間且不穩定。而且 scale 是未知的。? 單目相機極度依賴IMU，IMU在初始化階段就提供尺度信息和運動預測，極大幫助系統完成初始化并穩定運行。
  • 雙目相機可以通過左右圖像的視差，可以像人眼一樣直接計算深度，得到有尺度的3D點云。無需初始化過程，更加穩定可靠。系統從一開始就知道尺度，IMU的主要作用變為提供高頻輸出、輔助應對視覺失效的場景，以及進一步優化估計精度。性能通常優于單目VIO。
  • 魚眼相機有超大的視場角（FOV），意味著在快速運動時看到的特征更多，特征跟蹤的持續時間更長，系統更魯棒，主要用于需要極高魯棒性的場景，如自動駕駛、無人機高速飛行，算法需要處理魚眼相機的畸變模型。
  • 深度相機通過紅外結構光或飛行時間法（ToF）?直接測量每個像素的深度值，輸出“RGB圖 + 深度圖”。特征點的3D坐標可以直接從深度圖讀取，省去了最復雜、最耗時的“從2D到3D”的幾何計算步驟(無需三角化)且自帶尺度無需IMU來提供尺度。?IMU的作用被進一步削弱，主要用來提供高頻運動預測和補償深度相機的運動模糊?（因為深度相機通常有曝光時間，自身運動會導致深度測量誤差）。系統整體變得非常簡單、穩定和高效。
  • ?致命缺陷：?? ?尺度不確定性（單目）?；受光照、紋理影響大；快速運動易模糊；計算耗時。
• ?IMU:??
  • ?提供：?? 高頻運動測量。
  • ?作用：??
    • ?為視覺提供尺度！?? （對于單目VIO，IMU的加速度測量提供了真實的物理尺度）。
    • ?提供高頻輸出，彌補相機相對低的處理幀率（30-60Hz），實現運動預測。
    • ?幫助處理視覺失效的場景，如快速運動、圖像模糊、純色墻面。

• ?融合的核心思想：??
  ?IMU提供高頻、有尺度、但漂移的運動預測；視覺提供低頻、無尺度（單目）、但無漂移的相對位姿觀測。兩者結合，得到高頻、有尺度、無漂移的精確位姿。?

• ?典型應用：?? 無人機室內飛行、AR/VR頭盔、機器人室內導航。

3. LiDAR + IMU（高精度幾何感知 + 運動補償）

這是自動駕駛和高端機器人的標配，提供極其精確的環境幾何和自身位姿。??

• ?LiDAR (激光雷達):??
  • ?提供：?? 直接產生高精度的3D環境點云。通過點云配準算法（如ICP, NDT, Feature-based Matching）可以計算出雷達自身的運動。
  • ?缺陷：?? 在特征less的環境（長走廊、廣場）匹配困難；自身運動會使點云畸變（smearing）；計算量大；受天氣影響。
• ?IMU:??
  • ?作用：??
    • ?運動去畸變：?? 利用IMU的高頻數據，可以估計出LiDAR在一幀掃描過程中的自身運動，從而校正點云畸變，為點云匹配提供更準確的數據。
    • ?提供初始值：?? 為迭代式的點云匹配算法（如ICP）提供一個非常好的初始位姿猜測，加速收斂，避免匹配到局部極小值。
    • ?高頻輸出：?? 在LiDAR計算間隙提供運動預測。

• ?融合的核心思想：??
  ?IMU的高頻數據用于校正LiDAR的固有缺陷（運動畸變），并輔助其匹配；LiDAR提供的高精度位姿觀測用于校正IMU的漂移。??

除此之外，還有很多融合方式，這里不再贅述

方法

多傳感器融合定位的本質是一個狀態估計問題，即根據一系列帶有噪聲的觀測數據，來估計系統當前的狀態（如位置、速度、姿態等）。最主流的框架是濾波器，尤其是卡爾曼濾波（Kalman Filter）?及其衍生系列。

?卡爾曼濾波（KF）與擴展卡爾曼濾波（EKF）

• ?基本思想：?? KF在線性系統中是最優估計器。它分為兩個步驟：

  • ?預測（Prediction）：?? 根據IMU等運動傳感器的數據（系統模型），預測下一時刻系統的狀態和不確定性（協方差）。
    預測狀態 = 上一狀態 + IMU測量的運動
  • ?更新（Correction）：?? 當GPS、LiDAR、Camera等觀測傳感器數據到來時，用觀測值來修正預測值。
    最終狀態 = 預測狀態 + K * (觀測值 - 預測的觀測值)
    其中K是卡爾曼增益，根據預測和觀測的不確定性動態調整。更相信預測還是更相信觀測，由K決定。

• ?EKF（擴展卡爾曼濾波）：?? 現實世界和傳感器模型大多是非線性的。EKF通過一階泰勒展開將非線性系統線性化，使其能應用KF的框架。這是目前最常用、最經典的融合方法。

Apollo自動駕駛定位方案

Apollo的定位模塊（localization）主要負責輸出車輛在全局坐標系下的6自由度位姿，即：

• 位置?：經緯高坐標 (x,?y,?z)
• ?姿態?：橫滾角、俯仰角、航向角 (roll,?pitch,?yaw)

其目標是達到厘米級精度，以滿足自動駕駛車輛對車道級定位的苛刻要求。

核心定位模塊

1. GNSS + IMU：全局絕對定位的基礎

這是Apollo定位系統的主干和絕對位置來源。

• ?GNSS (全球導航衛星系統)??：

  • ?作用?：提供車輛的絕對地理坐標?（經緯高）。
  • ?挑戰?：普通GPS精度為米級，且信號易受遮擋（隧道、高樓）和干擾。
  • ?解決方案?：使用RTK (實時動態定位)?? 技術。通過地基增強站校正衛星信號的誤差，將定位精度從米級提升到厘米級。
  • ?缺點?：更新頻率低（通常10-20Hz），且在信號丟失時完全失效。

• ?IMU (慣性測量單元)??：

  • ?作用?：測量車輛的三軸加速度和三軸角速度。
  • ?優勢?：?高頻輸出?（可達1000Hz），?不依賴外部信號，短時間內精度極高。
  • ?致命缺點?：誤差會隨著時間累積（漂移）?，導致長時間定位不準。

• ?GNSS-INS組合導航?：

  • Apollo通過卡爾曼濾波等算法將GNSS和IMU的數據深度融合。
  • ?當GNSS信號良好時?：用GNSS提供的精確絕對位置來校正IMU的漂移，并估算出IMU的誤差參數。
  • ?當GNSS信號丟失時（如進入隧道）??：IMU利用剛才被校正后的狀態，進行短時間的高精度航位推算，彌補GNSS的空白。
  • 這種組合提供了連續、平滑且具有絕對基準的位置、速度和姿態信息。

2. LiDAR + 高精地圖：基于環境的匹配定位

這是Apollo定位系統的關鍵冗余和校正手段，尤其在城市峽谷等GNSS信號不佳的場景下至關重要。

• ?高精地圖 (HD Map)??：

  • Apollo預先采集并制作了包含車道線、路緣石、交通標志、電線桿等靜態物體精確三維坐標的地圖。

• ?激光雷達 (LiDAR)??：

  • 車輛行駛時，LiDAR不斷掃描周圍環境，生成實時3D點云。

• ?點云匹配?：

  • Apollo將實時點云與高精地圖的局部參考點云進行匹配。
  • 通過算法（如迭代最近點算法 (ICP)?? 或 ?正態分布變換 (NDT)?）計算出一個變換矩陣，使得兩者最佳對齊。
  • 這個變換矩陣就直接給出了車輛相對于高精地圖的精確位姿。

• ?作用?：

  1. ?提供絕對定位?：在無GNSS環境下，LiDAR定位可以直接提供不漂移的絕對位置。
  2. ?校正GNSS/IMU?：即使GNSS信號可用，LiDAR匹配結果也可以作為另一個觀測值輸入到濾波器中，進一步校正和優化GNSS/IMU的組合結果，使其更穩定、更精確。

3. 輪速計與車輛模型：提供額外約束

車輛自身的傳感器也參與定位融合：

• ?輪速計?：提供車輛的速度信息，可以作為IMU速度觀測的一個補充和驗證。
• ?車輛動力學模型?：提供非holonomic（非完整）約束，例如車輛不能像螃蟹一樣橫向移動。這個約束可以幫助濾掉一些不合理的位姿估計結果。

傳感器融合(卡爾曼濾波)

上述所有傳感器的數據并不會被單獨使用，而是會送入一個傳感器融合框架?（在Apollo中通常是基于卡爾曼濾波或其變種的算法）中進行最優估計。卡爾曼濾波的終極目標就是：?最優地結合這些不可靠的、帶有噪聲的傳感器數據，來得到一個比任何單一數據源都更準確、更可靠的車輛狀態估計

卡爾曼濾波是一個遞歸的算法，它交替進行兩個步驟：?預測（Predict）?? 和 ?更新（Update）?。它基于的是狀態空間模型。

1. 預測步（Predict - 基于模型預測）：根據上一時刻的最優估計，利用系統的運動模型，來預測當前時刻的系統狀態。例如，我知道車輛上一秒在A點，速度是10m/s。根據“勻速運動”模型，我預測一秒后它應該在A點前方10米處。在Apollo定位中，IMU是預測步的核心??，IMU的高頻數據提供了完美的運動模型輸入，可以不斷地預測車輛在下一個時刻的狀態（位置、速度、姿態）

• ?數學表達?：
  • x??? = F? * x???? + B? * u?
  • P?? = F? * P??? * F?? + Q?
  • x????是預測狀態?（Predicted State），例如包含預測的位置、速度。
  • P???是預測估計協方差?（Predicted Estimate Covariance），表示你對這個預測的不確定性（信心程度）?。預測步會引入過程噪聲?Q?，使得不確定性增加。

?2.更新步（Update - 基于測量校正）：當一個新的觀測值?（傳感器測量值）到來時，用它來校正預測值，從而得到一個更優的、融合后的估計。例如，我預測車輛在B點，但我的GPS告訴我它在B點前方0.5米。我應該相信誰？卡爾曼濾波會智能地融合這兩個信息。

• ?數學表達?：
  • K? = P?? * H?? * (H? * P?? * H?? + R?)?1
  • x?? = x??? + K? * (z? - H? * x???)
  • P? = (I - K? * H?) * P??
  • z??是觀測值，例如GPS測到的位置或LiDAR匹配到的位置。
  • R??是觀測噪聲協方差，表示你對這個傳感器測量的不確定性。GPS不準，R就設得大；LiDAR很準，R就設得小。
  • K??是卡爾曼增益，這是整個算法的靈魂。它決定了我們應該在多大程度上相信預測值，又在多大程度上相信新的觀測值。
• ?如果預測很準（P??很小），觀測噪聲很大（R?很大）??：那么?K?會很小。算法會更相信預測值，觀測值的影響很小。
• ?如果預測很不確定（P??很大），觀測很準（R?很小）??：那么?K?會很大。算法會更相信新的觀測值，用它大幅修正預測值。