寫在前面
本文是基于全球衛星導航體統（GNSS）結合慣導(IMU）在無人駕駛中的應用。計劃寫成一個系列，力求通俗易懂，盡量少用公式，大白話解釋問題。撰寫過程中參考學習了一系列的書籍、論文、部落格文章，對前人的工作表示感謝。
概述：
定位的目的是讓自動駕駛汽車找到自身确切位置的方法，可以說，定位導航技術是整個自動駕駛技術的核心！在日常生活中，我們一直使用谷歌、百度、高德or其他地圖 來确定自己的位置，其實質就是使用了衛星導航技術，手機終端接收衛星導航電文進行解算，計算出你此刻的位置。但衛星導航定位精度誤差在10米左右，這對于自動駕駛是緻命的，完全不能滿足自動駕駛的需求。如果駕駛的周圍環境複雜，過橋洞，接收不了導航信号，遮蔽嚴重，如高樓、山脈，會産生多徑效應，導航的精度可能會更差，50米開外了。是以無人駕駛系統不能依賴衛星導航系統，是以需要借助其他方式方法來提高車輛在地圖上的位置精度，這就要配合高精地圖，融合慣導和視覺等技術了，是以，無人駕駛系統中的導航定位分系統是技術交叉很厲害的一部分。
一、導航定位技術
什麼是導航定位？導航定位就是利用電、磁、光、力學等科學原理與方法，通過測量與空中飛機、海上艦船、大洋裡的潛艇、陸地上的車輛、人流等運動物體每時每刻位置有關的參數，進而實作對運動體的定位，并正确地從出發點沿着預定的路線，安全、準确、經濟地引導到目的地，這種技術就叫導航技術（百度百科解釋）。曾經聽一個報告，一位大專家說什麼是導航？他說這是一個哲學問題：“我從哪裡來？将要到何方？”，作為一個俗人，我不能給出這麼富有詩意的表達，我的了解導航定位就是：定位是導航的第一步，導航是定位的一個連續過程，導航涉及路徑規劃和決策引導。是以，能不能導航，關鍵看定位怎麼樣，核心名額就是定位精度。導航技術分為無線電導航、衛星導航、天文導航、慣性導航、地形輔助導航等，這裡主要談衛星導航和慣性導航以及雷射雷達、視覺導航。
二、衛星導航
1、定位原理
需要說明的是衛星導航技術依然是無人駕駛導航定位的主要手段，簡單說一下全球導航衛星系統（GNSS），GNSS包括美國的GPS系統，中國北鬥（BDS），俄羅斯的格洛納斯（GLONASS），還有不争氣的歐盟伽利略（GALILEO），主流使用美國的GPS，其實作在的終端都是多模多星座，用你不經意之間就使用了北鬥系統。衛星導航原理及其簡單，就是“三球交彙原理”，使用者測量出已知位置的衛星到使用者接收機之間的距離，在時間高精度同步的前提下，理論上接收機觀測到三顆以上衛星即可實作定位（其實需要大于等于4顆）。原理圖如下，關于衛星導航技術不過多的叙述，有時間專門寫一篇介紹北鬥系統的文章。
2、RTK技術
RTK（Real - time kinematic，實時動态）載波相位差分技術，是實時處理兩個測量站載波相位觀測量的差分方法，将基準站采集的載波相位發給使用者接收機，進行求差解算坐标，RTK能夠提高衛星導航的定位精度達到厘米級。基本原理是十分簡單，在一定的區域範圍内，兩個衛星導航終端接收機都距離不遠，那麼兩者的信号将具有幾乎相同的誤差，如果能精确地計算出第一個接收機的誤差，我們就可以對第二個接收機的結果進行修正。這樣做大大提高了導航地位的精度。目前，也是無人駕駛的通用技術之一。
三、慣性導航
慣性導航系統是一種不依賴于外部資訊、也不向外部輻射能量的自主式導航系統。其工作環境不僅包括空中、地面，還可以在水下。慣導的基本工作原理是以牛頓力學定律為基礎，通過測量載體在慣性參考系的加速度，将它對時間進行積分，且把它變換到導航坐标系中，就能夠得到在導航坐标系中的速度、偏航角和位置等資訊。百度百科解釋
在駕駛中想要知道車的目前位置，用速度乘以時間即可，但是這是在車是勻速且走直線的前提下。很顯然，實際情況要複雜得多。這個時候就用到了高中實體姿勢"加速度"！對加速度做積分，得到了速度，再次積分，得到了位置，道理就這麼簡單！但是問題來了，這“加速度”如何得到？為了測量加速度，就要使用“三軸加速度計”傳感器。它可以精确測量加速度。速度解決了，還有方向問題，車不是一直走直線的，是以單有加速度計不足以計算車輛的位置。這個時候就需要一個叫“陀螺儀角速度器”出場了！用陀螺儀能測量目前的角速度。解決了車輛行駛的方向的問題。加速度計和陀螺儀合起來就是是 IMU（慣性測量單元）一個解決速度，一個解決方向。IMU 頻率為1000Hz，是以 IMU 可以提供接近實時的位置資訊。而衛星導航的頻率大約10Hz,實時性差很多，說直白一點，開跑車的話，容易“丢星”。但是IMU也是有缺點的，會産生誤差積累，運動誤差會随時間增加而增加，是以IMU隻能短時定位。現在通用辦法是結合 GNSS 和 IMU 來進行來汽車定位，一方面，IMU 彌補了GNSS更新頻率較低的缺陷；另一方面，GNSS反過來糾正了 IMU 的運動誤差。這種結合就是“組合導航”。
四、LiDAR 定位
LiDAR也就是雷射雷達。是利用衛星導航信号和IMU機載雷射掃描。其所測得的資料為數字表面模型的離散點表示，資料中含有空間三維資訊和雷射強度資訊。應用分類技術在這些原始數字表面模型中移除建築物、人造物、覆寫植物等測點，即可獲得數字高程模型(Digital Elevation Model, DEM)，并同時得到地面覆寫物的高度 。百度百科解釋
LiDAR會在車輛行駛的過程中不斷收集點雲來了解周圍的環境。就可以利用這些環境資訊來定位。這裡我們可以把這個問題用一個簡化的機率問題來表示：已知t0時刻的衛星導航（GPS/BDS）資訊，t0時刻的點雲資訊，以及t1時刻無人車可能所在的三個位置：P1、P2和P3（這裡為了簡化問題，假設無人車會在這三個位置中的某一個）。求t1時刻車在這三點的機率。根據貝葉斯法則，無人車的定位問題可以簡化為如下機率公式：
右側第一項表示給定目前位置，觀測到點雲資訊的機率分布。其計算方式一般分局部估計和全局估計兩種。局部估計較簡單的做法就是通過目前時刻點雲和上一時刻點雲的比對，借助幾何推導，可以估計出無人車在目前位置的可能性。全局估計就是利用目前時刻的點雲和上面提到過的高清地圖做比對，可以得到目前車相對地圖上某一位置的可能性。在實際中一般會兩種定位方法結合使用。右側第二項表示對目前位置預測的機率分布，這裡可以簡單的用衛星導航（GPS/BDS）系統給出的位置資訊作為預測。通過計算P1、P2和P3這三個點的後驗機率，就可以估算出無人車在哪一個位置的可能性最高。通過對兩個機率分布的相乘，可以很大程度上提高無人車定位的準确度。
五、視覺定位
基于視覺的定位算法有兩大分類：一種是基于拓撲與地标的算法，另一種是基于幾何的視覺裡程計算法。
基于拓撲與地标的算法：把所有的地标組成一個拓撲圖，然後當無人車監測到某個地标時，便可以大緻推斷出自己所在的位置。基于拓撲與地标的算法相對于基于幾何的方法容易，但是要求預先建立精準的拓撲圖，比如将每個路口的标志物做成地标。
基于幾何的視覺裡程計算法：該方法計算比較複雜，但是不需要預先建立精準的拓撲圖，這種算法可以在定位的同時擴充地圖。視覺裡程計算法主要分為單目以及雙目兩種，純單目的算法的問題是無法推算出觀察到的物體的大小，是以使用者必須假設或者推算出一個初步的大小，或者通過與其它傳感器（如陀螺儀）的結合去進行準确的定位。雙目的視覺裡程計算法通過左右圖三角剖分（Triangulation）計算出特征點的深度，然後從深度資訊中推算出物體的大小。
圖 中， Ｑ 是待測距點（某比對點），其到相機的垂直距離為 Ｒ，在左右相機上形成的像點分别是 Ｑ１ 和 Ｑ２ 。 ｂ 是雙目相機的基線長度，ｆ是相機焦距， X1,X2 是 Ｑ 點在左右兩幅圖像上像點的位置差， 又稱為視差。利用相似三角形原理， 可得相機到 Ｑ 點的距離為
雙目視覺裡程計的優勢在于，能夠精确的估計運動軌迹，且具有确切的實體機關。在單目視覺裡程計中，你僅僅隻知道物體在x/y方向上移動了1個機關，而雙目視覺裡程計則可明确知道是移動了1cm。但是，對于距離很遠的物體，雙目系統則退化成為單目系統，是以，究竟使用哪種視覺裡程計方法，取決于我們的應用場景。通過視覺裡程計算法，無人車可以實時推算出自己的位置，進行自主導航。但是純視覺定位計算的一個很大問題是算法本身對光線相當敏感。在不同的光線條件下，同樣的場景不能被識别。特别在光線較弱時，圖像會有很多噪點，極大地影響了特征點的品質。在反光的路面，這種算法也很容易失效。這也是影響視覺裡程計算法在無人駕駛場景普及的一個主要原因。
至此，把無人駕駛系統中的導航定位分系統進行了一個簡單的梳理，後續将對差分RTK技術，雷射雷達，視覺定位進行展開讨論。