L4/L5無人車為什么需要精確的定位系統(tǒng)？

在第三期《Hello Apollo，自動駕駛公開課》上，來自百度Apollo核心算法技術(shù)的負責(zé)人萬國偉，為大家分享了《Apollo 2.0多傳感器融合定位模塊》，以下是分享的全部內(nèi)容。

Apollo 核心算法技術(shù)負責(zé)人萬國偉

L4/L5無人車為什么需要精確的定位系統(tǒng)？精確是指厘米級，也就是說10厘米以內(nèi)。如果人開車的話，會關(guān)注什么？我們會關(guān)注車道線、道路指示牌、紅綠燈這些交通標識，路肩、路邊柱子，建筑物，這些都是場景中的靜態(tài)物體，關(guān)注它們是因為人需要知道車的可行使范圍、以及需要規(guī)劃車的行駛路徑。我們還會關(guān)注一些動態(tài)的物體，比如對面開過來的摩托車。

定位系統(tǒng)可以與高精地圖配合提供靜態(tài)場景感知，可將感知得到的動態(tài)物體正確放入靜態(tài)場景，而位置和姿態(tài)用于路徑規(guī)劃和車輛控制。因此定位系統(tǒng)對于無人駕駛至關(guān)重要。

而要獲得精確的定位，也面臨很多挑戰(zhàn)，譬如：定位需要非常高精度和高魯棒性；GPS信號遇到阻隔會引起信號丟失；在復(fù)雜的城市環(huán)境中，由于建筑物和植物的存在，引起多鏡效應(yīng)導(dǎo)致定位不準；由于天氣情況，或者人為修繕，會導(dǎo)致定位精度不高。

三種定位技術(shù)

了解了定位系統(tǒng)的重要性，我們來看看定位技術(shù)的分類。

從定位方式上可分為三類。

第一類，基于信號的定位，它的代表就是GNSS，其實就是全球?qū)Ш叫l(wèi)星系統(tǒng)；

第二類，航跡推算，依靠IMU等，根據(jù)上一時刻的位置和方位推斷現(xiàn)在的位置和方位；

第三類是環(huán)境特征匹配，基于Lidar的定位，用我們觀測到的特征和數(shù)據(jù)庫里的特征和存儲的特征進行匹配，得到現(xiàn)在車的位置和姿態(tài)。

這三種定位方式各有優(yōu)劣。

第一種衛(wèi)星定位，RTK差分技術(shù)出現(xiàn)后，精度穩(wěn)步提高。差分技術(shù)，是在一個精確的已知位置（基站）上安裝GNSS監(jiān)測接收機，計算得到基站與GNSS衛(wèi)星的距離校正。差分分為位置差分和距離差分。RTK差分可以達到厘米級定位。RTK存在的問題有：基站鋪設(shè)成本較高；非常依賴衛(wèi)星數(shù)量，比如在一些橋洞和高樓大廈的環(huán)境下，可視的衛(wèi)星數(shù)量會急劇下降；容易受到電磁環(huán)境干擾。在受到遮擋時，信號丟失，沒有辦法做定位。

第二種捷聯(lián)慣性導(dǎo)航（ SINS），是利用慣性測量元件（陀螺儀、加速度計）測量得到的載體相對于慣性空間角運動和線運動參數(shù)，經(jīng)過慣性導(dǎo)航解算得到載體的速度、位置、姿態(tài)。捷聯(lián)是相對于平臺來說的。慣性測量單元（IMU）主要由陀螺儀和加速度計組成。加速度計能夠測量出載體相對于慣性空間所受的力；陀螺儀能夠測量出載體沿陀螺儀軸向的旋轉(zhuǎn)角速度或旋轉(zhuǎn)增量。IMU是慣性導(dǎo)航系統(tǒng)的基礎(chǔ)，直接決定了慣性導(dǎo)航系統(tǒng)所能夠達到的精度。

慣性導(dǎo)航（簡稱慣導(dǎo)）的基本工作原理是以牛頓力學(xué)定律為基礎(chǔ)。慣導(dǎo)系統(tǒng)的價格很高。慣導(dǎo)系統(tǒng)通過陀螺儀獲知航向和姿態(tài)角，通過加速度得到速度，進而形成導(dǎo)航坐標系；通過速度得到位移，可以得到六自由度的信息。優(yōu)點是輸出頻率非常高，短時精度高，缺點就是誤差隨著時間累積。

第三種激光定位，要預(yù)先制作定位地圖，不管是3D的還是2D地圖。激光點云定位一般預(yù)先制作地圖，然后用車上的實時點云和地圖進行匹配，來計算激光雷達的位置和姿態(tài)，再通過激光雷達與IMU之間的外參，得到IMU的位置和姿態(tài)。匹配有很多種方法，可以是基于3D點云匹配的ICP方法，也可以是我們這里給出的基于2D概率地圖的直方圖濾波器匹配定位。激光定位的優(yōu)點是在沒有GPS情況下可以工作，魯棒性比較好；缺點就是需要預(yù)先制作地圖，同時要定期更新地圖（因為環(huán)境會發(fā)生變化），雨雪天氣也會受到影響（因為Lidar被折射的比較多，收到的點云數(shù)據(jù)變少）。

基于以上三種定位技術(shù)，百度提出多傳感器融合的定位系統(tǒng)，既做到優(yōu)勢互補，也提高了穩(wěn)定性，增強了定位精度。

多傳感器融合定位

定位技術(shù)橫跨好幾個專業(yè)，包括測繪、導(dǎo)航、計算機視覺知識、以及點云處理的知識。

Apollo2.0的多傳感器融合定位模塊的框架如下圖所示：

左邊列出了定位模塊依賴的硬件以及數(shù)據(jù)，包括慣性測量單元IMU、車端天線、基站、激光雷達、以及定位地圖；

中間是GNSS定位以及激光點云定位模塊，GNSS定位輸出位置及速度信息，點云定位輸出位置及航向角信息；

右邊是融合框架，融合框架包括兩部分：慣性導(dǎo)航解算、Kalman濾波；

融合定位的結(jié)果會反過來用于GNSS定位和點云定位的預(yù)測；

融合定位的輸出是一個6-dof的位置和姿態(tài)，以及協(xié)方差矩陣。

點云定位

先介紹點云定位子模塊。

點云定位的輸入是融合定位提供的預(yù)測位置和姿態(tài)、以及激光雷達的點云。點云定位主要包含有兩個步驟：航向角優(yōu)化、(x, y)計算，分別對應(yīng)圖像對齊、SSD-HF（直方圖濾波器）兩個部分。點云定位會輸出x, y, z, yaw，z是通過定位地圖來獲取的。

關(guān)于點云定位，它有三個輸入：融合定位提供的預(yù)測姿態(tài)、Lidar點云、定位地圖。在點云子模塊中包含兩個功能：yaw優(yōu)化和XY計算。Yaw優(yōu)化用到了圖像對齊，XY計算用了濾波器，最后會輸出XY和yaw，也輸出了一個Z。

先介紹XY怎么做，百度的方法是基于直方圖濾波器，就是SSD。SSD是Sum of Squared Difference，是一種常用的度量方法，也叫差方和。

把每個激光點的反射值或者高度值和定位地圖對應(yīng)值相減，然后把差的平方加起來，每個度量值越小說明位置匹配的越好。

怎么匹配多個位置？我們用二維直方圖濾波器，將其中心放在預(yù)測姿態(tài)(x0, y0)，濾波器一般選擇21*21，那么搜索的范圍就是2.6米* 2.6米，需要計算441個位置的SSD值。

我們可以看到有的地方匹配的比較好，比較好的就是紅色值，比較差的是藍色，綠色是過渡。

在直方圖濾波器計算前，我們會對yaw做一次優(yōu)化。

yaw的優(yōu)化，即對航向角的優(yōu)化。因為融合定位的時候很容易產(chǎn)生角度的誤差，如果用低端的IMU更容易產(chǎn)生，尤其是最開始的時候誤差非常大。在航向角有1.5度誤差的時候，明顯的看出來已經(jīng)有較大偏差，為什么會有這么大的偏差？這個時候我們要做航向角的優(yōu)化，如果不做，直接做SSD直方圖分布，可以看到它會集中在一個區(qū)域，經(jīng)過平均以后，位置大概在另一區(qū)域，和真值差了一米多，那就偏了。處理方法用的是圖像處理Lucas-Kanade算法，主要是做圖像的對齊。

對于反射值和高度值，做匹配的時候怎么聯(lián)合起來？比如：左邊有一個路段，反射值和匹配值經(jīng)過平均以后，得到的位置會偏的很多，這時高度值定位非常好。用固定的方法加在一塊，仍然得不到很好的分布，所以我們采用了自適應(yīng)融合的方式，也就是說反射值和高度值分別計算直方圖分布，由直方圖分布的優(yōu)劣來決定其權(quán)重。

我再舉另一個例子，這是真實的路測例子，在掃描地圖的時候路是這樣的，后來他們重新鋪設(shè)了，定位的時候只用反射值就會有誤差。紅色的是真值，藍色是定位的結(jié)果。如下圖左邊所示，紅色藍色偏離的很厲害，這是非常危險的。調(diào)成固定權(quán)重的模式會好一點，如果采用自適應(yīng)權(quán)重，則非常吻合。

GNSS-RTK定位

下面我們介紹GNSS-RTK定位。

現(xiàn)在慣導(dǎo)芯片一般都會配一個板卡（NovAtel也賣這樣的板卡），直接集成了RTK的定位結(jié)果。為什么我們還需要自己開發(fā)GNSS-RTK呢？ 從系統(tǒng)的角度考慮，需要每個子模塊都是可控的，舉一個簡單的例子，當給出一個定位結(jié)果偏了，但給出的方差很小，也就是置信度很高。我們是沒辦法知道原因的。

簡單說一下它的原理，用GNSS天線獲得車載原始的觀測數(shù)據(jù)，通過3G、4G數(shù)據(jù)傳輸?shù)玫交镜脑加^測，先做一個基于RAIM的車輛單點定位。單點定位的時候我們只知道大概的位置，再找基站和天線的共視星，然后做單差和雙差。

單差是通過單差偽距和載波觀測值作為EKF的時間更新，雙差是構(gòu)建雙差模糊度與協(xié)方差，之后使用LAMBDA固定雙差模糊度，然后使用固定模糊度后的載波完成量測更新。最后做融合定位。

上圖中，RAIM是根據(jù)用戶接收機的多余觀測值監(jiān)測用戶定位結(jié)果的完好性，其目的是在導(dǎo)航過程中檢測出發(fā)生故障的衛(wèi)星，并保障導(dǎo)航定位精度。LAMBDA方法是指Least-squares AMBiguityDecorrelation Adjustment。

慣性導(dǎo)航及融合定位

接下來我們介紹慣性導(dǎo)航及融合定位。

我們引入幾個坐標系，第一個是慣性坐標系（ECI），z軸是地球自轉(zhuǎn)軸，x，y在赤道平面，x, y不隨地球自轉(zhuǎn)；第二個是地心地固坐標系(ECEF)，第三個是導(dǎo)航坐標系，第四個是載體坐標系。IMU的比力和角速度測量都是載體坐標系在慣性坐標系下的測量。一般情況下，都會建立一個導(dǎo)航坐標系，在導(dǎo)航坐標系下進行位置和姿態(tài)輸出。（加速度計測量的不是載體的運動加速度，而是載體相對慣性空間的絕對加速度和重力加速度之和，稱作“比力”。）

在這里我們簡化一下，只考慮載體坐標系和慣性坐標系。

我們來解釋一下導(dǎo)航解算的原理，它分下面幾步：

1. 姿態(tài)更新，對陀螺儀輸出的角速度進行積分得到姿態(tài)增量，疊加到上次的姿態(tài)上；

2. 比力坐標轉(zhuǎn)換，是從IMU載體坐標系到位置、速度求解坐標系（慣性坐標系）；

3. 速度更新，速度這一步需要考慮重力加速度的去除，得到慣性系下的加速度，通過積分得到速度；

4. 位置更新，通過積分得到位置。

在慣性導(dǎo)航中，提到了導(dǎo)航方程的每一次迭代都需要利用上一次的導(dǎo)航結(jié)果作為初始值，因此在使用慣導(dǎo)之前必須進行初始化。初始位置和初始速度信息需要外部提供，一般是GNSS。

姿態(tài)對準是指得到IMU的roll,pitch, yaw；roll, pitch的對準過程一般稱為調(diào)平：當車靜止時，加速度計測量的比力僅由重力導(dǎo)致，可以通過f=C*g來求解；對于非常高精度的IMU可通過羅經(jīng)對準的方式，車靜止，通過測量載體系中的地球自轉(zhuǎn)來確定載體的方位(yaw)。

對于車上使用的IMU，沒那么高等級，一般采用兩種方式：車直線跑起來，用從GNSS獲取的速度方向來估計航向；第二種方式是采用雙天線GNSS，通過兩個位置連線來計算航向。

介紹完GNSS定位、點云定位以及慣性導(dǎo)航解算以后，我們看這幾個模塊怎么融合到一起。這里使用了Kalman濾波器的松耦合，意思就是我們只用了位置、姿態(tài)和速度做融合。

我們使用松耦合的方式把慣性導(dǎo)航解算、GNSS定位、點云定位三個子模塊融合在一起。松耦合的數(shù)據(jù)只有位置、速度、姿態(tài)，緊耦合會包括GNSS的導(dǎo)航參數(shù)、定位中的偽距、距離變化等。

我們使用了一個誤差卡爾曼濾波器，慣性導(dǎo)航解算的結(jié)果用于kalman濾波器的時間更新，也就是預(yù)測；而GNSS、點云定位結(jié)果用于kalman濾波器的量測更新。

Kalman濾波會輸出位置、速度、姿態(tài)的誤差用來修正慣導(dǎo)模塊，IMU期間誤差用來補償IMU原始數(shù)據(jù)。

這里單獨把我們所做的Kalman濾波拿出來，如果做GNSS定位輸出和點云定位的時候，我們會發(fā)現(xiàn)GNSS定位非常快，只要有原始消息就可以快速解算，但是點云定位的時間比較長，需要幾十毫秒甚至上百毫秒，會出問題。也就是我們先收到的消息解算時間很長，才會有結(jié)果，會造成量測更新的亂序。我們解決這個問題的思路是用了兩個濾波器，一個濾波器是Filter1，主要是做時間更新，實時的對外提供服務(wù)；量測更新是Filter2，我們會把Filter1的狀態(tài)拷貝過來做量測更新，還會補齊最新時刻，用的是時間更新的方式，最后用Filter2代替Filter1，這樣就解決了時間戳亂序的問題。

參考的論文如下：

Robustand Precise Vehicle Localization based on Multi-sensor Fusion in Diverse CityScenes, ICRA, 2018.

對GNSS及慣導(dǎo)感興趣的朋友，可以讀一下這兩本書：

?《Principles of GNSS, Inertial, and Multisensor Integrated NavigationSystems(Second Edition)》

?《GNSS與慣性及多傳感器組合導(dǎo)航系統(tǒng)原理》，國防工業(yè)出版社

Apollo 2.0中定位的使用

下面介紹Apollo2.0中定位的使用。

在Apollo工程中，我們提供了一鍵式定位地圖制作，是在（Apollo-scripts/msf/simple/map/creator.sh）。提供的是簡單制圖工具，當RTK不準時，做出來的圖是模糊的。因為做圖是很復(fù)雜的，Apollo2.0沒有提供做圖的功能，但是給它一個pose準確的文件，它可以幫你做出定位地圖。

下面介紹一下使用方式。要使用Apollo2.0中的多傳感器融合定位模塊，需要將localization_type設(shè)置為MSF；MSF定位也有多種定位模式：可以在localization/conf/localization.conf文件中設(shè)置。

上圖是測試案例，首先是在中關(guān)村測試的。第一幅圖樓房比較多，所以信號遮擋比較厲害，第二幅圖有很多高的樹，對道路有遮擋，信號也特別差，這兩個都算是弱GPS環(huán)境。另一個測試是在橋洞和地下車庫的環(huán)境，橋洞剛開始有信號，之后丟失了，但是依然會跑得很好。大家可以看到GPS已經(jīng)飛掉了，我們的定位仍然很準。車庫沒有GPS，只有Lidar定位，但定位系統(tǒng)在車庫里也可以工作。

QA問答

提問環(huán)節(jié)

01Q

想咨詢您三個問題，

第一是我們都知道需要高精度定位，百度也選擇精度比較高的系統(tǒng)，你們認為L4、L5級別的自動駕駛到底需要哪個級別的傳感器？對它的精度有沒有量化的指標？

第二個問題，您剛才講到的濾波模型中，我想請教有沒有考慮車輛約束，因為我們主要運用在車上，（能否）利用轉(zhuǎn)向模型等來輔助它的解算濾波器？

第三個問題是關(guān)于功能安全，這方面你的定位系統(tǒng)有沒有考慮？

A

第一個問題，除了高端的定位系統(tǒng)，我們現(xiàn)在也在做低端的IMU融合定位，價格是萬元級，定位結(jié)果也很好。

第二個問題，你說的應(yīng)用于車輛約束的條件，我們是有所考慮的。如果某個模型需要和我們的濾波模型放到一起，我們要在數(shù)學(xué)上推導(dǎo)通了才能應(yīng)用。我們也做了簡單的實驗，主要是對橫向的約束，車行駛過程一般不可能出現(xiàn)大的橫向偏離，假設(shè)它的橫向偏離非常小，這做了約束，但是方式比較簡單，也是在濾波器的考慮范圍內(nèi)。你說的整車(橫向偏離)運動模型我們還沒有考慮，這是很好的方向。

第三個問題，關(guān)于主動安全，我們有專門的安全團隊負責(zé)。我主要負責(zé)定位系統(tǒng)，我會告訴我的上下游模塊現(xiàn)在是不是有問題，會對顯示界面提示，還會做監(jiān)管的措施。我們的定位模塊主要輸出自己的數(shù)據(jù)和狀態(tài)。

02Q

剛才說的幾種定位模式，在激光雷達加慣導(dǎo)時的濾波模型，和之前的GPS融合一樣嗎？

A

是一樣的。我介紹了松耦合的結(jié)構(gòu)，量測更新不區(qū)分GNSS和Lidar定位，完全可以做。

03Q

在點云定位中，會有定位不準的情況嗎？

A

點云定位在環(huán)境匹配的失敗，會有，但是我們會輸出點云定位的置信度，置信度比較低時，卡爾曼濾波對量測更新采信度特別低。

04Q

如果在惡劣環(huán)境中經(jīng)過幾十秒甚至幾分鐘，定位會失敗嗎？比如周邊有大車。

A

有大車的情況下，有時會出現(xiàn)定位失??；用了自適應(yīng)算法以后明顯提高很多。

05Q

Intensity會隨著激光看到的距離甚至角度有改變，不同的激光對同一個物體也有變化，我想問一下怎么做到類似情況的處理？

A

有很多方式可以處理這種事情，比如做反射值標定。據(jù)你事先采集的場景，然后把數(shù)據(jù)合到一起，用比較準的pose投到上面，每根線做標定，相當于你給0到255的顏色值都返回標定以后的反射值。但是目前我們沒有用在模塊里，因為一般沒有太大的影響，加上我們用了高度值以后，兩者融合后不會產(chǎn)生太大的影響。

06Q

剛才提到會有多徑的情況，據(jù)我了解即使在這種情況下可能GPS會給你反饋，即使發(fā)生多鏡情況還可能給你返回精度特別高的信息，比如它的協(xié)方差特別小。

A

你說的這種情況是存在的，但是它不是非常頻繁的出現(xiàn)，是偶發(fā)。這會給我們產(chǎn)生比較大的影響，雖然這個概率特別低，所以我們自己做了一套GNSS定位模塊，我們想通過自己的模塊去看為什么會發(fā)生那樣的情況，我們?nèi)フ宜挠^測量特征去剔除這樣的情況。我們不需要你時時刻刻都很準，但是你不準的時候要告訴我們，因為我們還有Lidar定位。

07Q

比如在山洞里，可能只有左右方向的約束，前后沒有，往前走一米可能還是一模一樣的特征，你們遇到走山洞情況怎么處理的？

A

橋洞我們也跑過，高度約束只能左右，前后卡的不是很好。但是我們走過的橋洞，地上都會有車道線，反射值給它前后的約束、縱向約束特別好，也可以過去，就算是沒有這些特征，慣導(dǎo)平滑也會做得特別好。