基于CORS定位的無人駕駛路徑跟蹤研究

高偉健，儲江偉，管湘源

（東北林業(yè)大學(xué) 交通學(xué)院，哈爾濱 150040）

無人駕駛車輛是未來的車輛發(fā)展方向。無人駕駛車不僅僅是某個領(lǐng)域的成就，還集成了機械、自動控制、計算機科學(xué)以及人工智能等眾多領(lǐng)域的先進技術(shù)。目前的無人駕駛車依靠攝像頭（單目和雙目）、激光雷達等一系列的設(shè)備進行環(huán)境感知，特別是三維激光雷達，高昂的成本難以讓大眾接受，也給普及無人駕駛車帶來極大阻礙［1-2］。因此，尋找低成本的傳感器和輔助設(shè)備的方案才是發(fā)展無人駕駛車的必由之路。

高精度定位問題一直是無人駕駛車面對的最基本的問題。現(xiàn)在的定位技術(shù)多種多樣，主要分為室內(nèi)定位［3-4］和室外定位［5］，本文中關(guān)注室外定位技術(shù)。室外定位技術(shù)主要是GPS和BDS的衛(wèi)星定位法以及現(xiàn)在大力開發(fā)的基于高精度地圖的SLAM（同時定位與地圖構(gòu)建）［5］。GPS定位是世界上最早的定位系統(tǒng)，發(fā)展至今，民用定位精度仍然停留在米級，國內(nèi)的BDS衛(wèi)星定位精度大部分也停留在米級［6］。上述2種技術(shù)定位精度不足以滿足戶外無人駕駛車的需求?；诟呔鹊貓D的SLAM可以滿足無人駕駛車要求的厘米級定位，但其高精度地圖的繪制需要花費大量的人力、物力和財力，并且需要巨大數(shù)量的詳細數(shù)據(jù)計算［7］。隨著GPS和DBS衛(wèi)星定位系統(tǒng)的不間斷發(fā)展和建設(shè)，以及CORS（continuous operating reference station）網(wǎng)的建設(shè)，現(xiàn)有的載波相位差分技術(shù)的發(fā)展，目前可以輕松實現(xiàn)厘米級的定位精度，特別是隨著接收機關(guān)鍵技術(shù)發(fā)展和基站的廣泛建設(shè)，接收機設(shè)備價格便宜，定位精度種類繁多且應(yīng)用范圍廣。

路徑跟蹤是無人駕駛車的核心技術(shù)之一，路徑跟蹤是指無人駕駛車根據(jù)預(yù)先規(guī)劃出的路徑進行安全行駛，最終到達目的地［8］。我國工程院郭孔輝院士于1982年提出了預(yù)瞄-跟蹤系統(tǒng)理論［9］，建立了駕駛員預(yù)瞄最優(yōu)曲率模型，還對不同的方向控制駕駛員模型進行了比較。目前有很多研究人員加入其中并對無人駕駛車路徑跟蹤進行更深入的研究，特別是對駕駛員模型的深入研究。這些駕駛員模型應(yīng)用模糊數(shù)學(xué)及現(xiàn)代控制理論描繪人的駕駛操作行為，能夠反映出人的部分思維能力，特別是模糊神經(jīng)網(wǎng)絡(luò)駕駛員模型的提出［10-11］，在很大程度上能夠得出人的思維能力，來更優(yōu)、更完善地對無人駕駛車進行路徑跟蹤控制。

上述的路徑跟蹤都是基于駕駛員模型和相應(yīng)算法結(jié)合達到最優(yōu)的路徑跟蹤控制，還有一種是將RTK-GPS接收器和IMU傳感器在開放的調(diào)控中自主導(dǎo)航的軌跡信息轉(zhuǎn)換成無人駕駛車的運動狀態(tài)量實現(xiàn)路徑跟蹤［12-13］。

為了更好地提高無人駕駛的定位精度以及路徑跟蹤的精度，盡量降低各種額外傳感的成本［14］，本文中提出了基于CORS網(wǎng)的GPS和DBS衛(wèi)星定位的無人駕駛車的路徑跟蹤研究。利用定位系統(tǒng)設(shè)備測量規(guī)劃軌跡，對比測量軌跡和規(guī)劃軌跡之間產(chǎn)生的誤差，把測量軌跡數(shù)據(jù)量傳輸給無人駕駛車，觀測無人駕駛車實際運行軌跡，驗證整體方法的可靠性。

1 大地坐標(biāo)與空間直角坐標(biāo)轉(zhuǎn)換

1.1 大地坐標(biāo)系的選取

大地坐標(biāo)系是在大地測量中以參考橢球面為基準(zhǔn)面建立起來的坐標(biāo)系。地面某點A的位置用大地經(jīng)度L、大地緯度B、大地高度H表示。本文中采用建設(shè)10年左右的2000國家大地坐標(biāo)系（China geodetic coordinate system 2000）［15］。

1.2 地球模型及坐標(biāo)轉(zhuǎn)換

地球是個近似的橢球體，大地平面凹凸不平整，采用CGCS2000參考橢球為第一等位的橢球，CGCS2000參考橢球的定義常數(shù)是［14］：

長半軸a＝6 378 137 m，

短半軸b＝6 356 752.314 1 m，

地球扁率f＝1／298.257 221 01，

地球的地心引力常數(shù)GM＝3.986 004 418×1014m3／s2，

地球角速度 ω＝7 292 115×10-11rad／s。

如圖1所示，CGCS2000大地坐標(biāo)是以地球質(zhì)心O為大地坐標(biāo)的原點，以長半徑為Y軸、旋轉(zhuǎn)軸為Z軸所建立起來的空間之間坐標(biāo)系?？梢詼y出點A經(jīng)度、緯度和高度（L、B、H），通過下列轉(zhuǎn)換公式可以得出（X、Y、Z）：

圖1 CGCS2000橢球體模型示意圖

2 測量方式不同的定位數(shù)據(jù)分析

采用GPS和BDS衛(wèi)星定位2種衛(wèi)星定位融合系統(tǒng)進行測量。網(wǎng)絡(luò)CORS在國內(nèi)的發(fā)展極為迅速，但其發(fā)展過程中存在標(biāo)準(zhǔn)不統(tǒng)一、缺少統(tǒng)一管理等問題。目前，由中國兵器工業(yè)集團和阿里巴巴集團共同開發(fā)，在全國范圍內(nèi)建立2 200多個地基增強站以及自主定位算法，使全國形成一張網(wǎng)，為農(nóng)業(yè)、汽車業(yè)等行業(yè)提供高精度定位及高效工作效率。本文中采用 “千尋知寸-FindCM”服務(wù)進行定位測量。數(shù)據(jù)測量整體思路是從點開始分析，再對線分析，最后實現(xiàn)整體規(guī)劃路徑分析。

2.1 測量點數(shù)據(jù)分析

基于網(wǎng)絡(luò)CORS的GPS和DBS定位服務(wù)主要還是以DBS衛(wèi)星為主，因為此定位方式得出的定位值（B、L、H）基本都是在浮動值和固定值之間隨時變化。為了克服這種變化帶來的測量誤差，對一點的測量次數(shù)在500～800左右。根據(jù)不同的時間段、不同的天氣等情況進行觀察，用均方根（root mean square，RMS）公式擬合最終此點的定位精度［16］。

均方根公式為：

式（3）能在一定程度上反映測量值的優(yōu)劣，認為RMS值越小越好。再根據(jù)定位水平精度因子（HDOP）評價2種衛(wèi)星定位融合系統(tǒng)的定位精度及定位誤差，水平精度因子數(shù)值越大代表精度越差，數(shù)值越小代表精度越高。

2.1.1 未連接CORS網(wǎng)絡(luò)測量點分析

利用上述公式對數(shù)據(jù)進行初步處理，選取RMS較小值的點進行數(shù)據(jù)對比分析，如圖2、3所示。圖2、3表示未連接CORS網(wǎng)絡(luò)定位數(shù)據(jù)。圖2中的A、B、C、D不同形狀點代表在1 d時間內(nèi)、不同時段對地面同一點的測量，可以清晰地反映出測量值與預(yù)估值之間的浮動誤差約1 m。

圖2 測量值與預(yù)估值

圖3 測量值與預(yù)估值波動

2.1.2 連接CORS網(wǎng)絡(luò)測量點分析

利用上述公式對數(shù)據(jù)進行初步處理，選取RMS較小值的點進行數(shù)據(jù)對比分析，如圖4、5所示。圖4中可以清晰地反映出測量值與預(yù)估值之間的浮動誤差基本在25 cm以內(nèi)，一些測量點的測量值與預(yù)估值間的偏差距離過大，但偏差距離大的測量點對整體測量結(jié)果影響不大。圖5中的數(shù)據(jù)反映在1 d內(nèi)、不同時間段對同一點的測量波動情況，可以充分反映出測量點與預(yù)估點之間的跳躍程度，從圖中看出并未出現(xiàn)距離突然增大點，表明其穩(wěn)定可靠。

通過試驗分析連接CORS網(wǎng)絡(luò)定位數(shù)據(jù)和未連接CORS網(wǎng)絡(luò)定位數(shù)據(jù)時的情況對比，可以清晰得出，連接CORS網(wǎng)絡(luò)定位可以提高4～5倍的定位精度，且定位設(shè)備工作穩(wěn)定。

圖4 測量值與預(yù)估值

圖5 測量值與預(yù)估值波動

圖4、5僅對測量點和預(yù)估點之間距離和波動情況做出詳細分析，并未得出具體的測量點相對預(yù)估點的具體位置和方位信息。本文中將利用二維高斯投影坐標(biāo)公式（5）來確定測量點相對于預(yù)估點的位置信息［17］：

式中：Sm為緯度B對應(yīng)的子午線弧長（m）；η2＝

利用上述公式可以繪畫出測量點和預(yù)估點之間的具體位置和方位圖，如圖6所示。

圖6 測量值與預(yù)估值方位

2.2 測量點形成測量軌跡

通過對測量點的詳細分析可以得出測量點和預(yù)估點之間的誤差距離，也可以得出測量點和預(yù)估點之間的相對位置和距離信息。通過對一條直線進行分點測量，對直線中預(yù)估點進行測量來形成測量軌跡。測量軌跡偏差分析如圖7所示。

圖7 測量軌跡偏差

圖7中負值代表測量軌跡偏差在直線左邊，正值代表測量軌跡偏差在直線右邊。從圖7中可以得出，測量軌跡單側(cè)偏差在30 cm以內(nèi)。通過多次測量對比總結(jié)規(guī)律得出更為準(zhǔn)確的偏差區(qū)間，此偏差區(qū)間作為仿真實驗路徑跟蹤研究的反饋信息，可進一步提高路徑跟蹤的精度。

3 基于運動學(xué)自行車模型的仿真控制

對于汽車運動的描述極其復(fù)雜，故采用自行車模型（bicycle model）作為一種有效的車輛運動化簡方式，應(yīng)用到仿真中。利用python語言實現(xiàn)車輛控制及點追蹤的跟蹤。

3.1 車輛運動學(xué)自行車模型

運動學(xué)自行車模型可以簡化汽車運動狀態(tài)，是最有效、簡單的方式。如圖8所示，為運動學(xué)自行車模型示意圖。

式中：（xt，yt）為車輛當(dāng)前的（x，y）坐標(biāo)；（xt+1，yt+1）為車輛下一時刻的（x，y）坐標(biāo)；vt為車輛的速度；φt為車輛當(dāng)前的偏航角；lf和lr分別為前輪和后輪到車輛重心的距離；θf為車輛前輪轉(zhuǎn)角，由于大部分車輛后輪不帶轉(zhuǎn)向功能故θr＝0。

圖8 運動學(xué)自行車模型示意圖

根據(jù)上述運動學(xué)自行車模型，利用PID控制實現(xiàn)自行車模型的橫向和縱向控制。軌跡的設(shè)定利用能夠帶有直線和弧線的400 m標(biāo)準(zhǔn)跑道。車輛預(yù)瞄點設(shè)置為提前兩點，車輛到達目標(biāo)點的半徑閾值根據(jù)測量的具體誤差進行調(diào)節(jié)，利用python中的numpy數(shù)據(jù)進行車輛運動狀態(tài)的運算。

3.2 仿真實驗數(shù)據(jù)分析

應(yīng)用PID控制實現(xiàn)自行車模型的點追蹤仿真實驗，在控制車輛速度不同的情況下分析點追蹤誤差和調(diào)節(jié)不同的目標(biāo)點的半徑閾值對跟蹤誤差的影響。車輛跟蹤的路徑信息用GPS／BDS連接CORS網(wǎng)絡(luò)進行定位測量而出，形成類似于標(biāo)準(zhǔn)400 m跑道圖形，此路徑信息是通過測量大約120個定位點而形成的。如圖9所示，為40 km／h點追蹤圖。

圖9 40 km／h點追蹤

根據(jù)圖9的40 km／h的點追蹤圖和利用均方根求出的車輛到達目標(biāo)點和規(guī)劃點的誤差值，如圖10所示。

圖10 40 km／h的點追蹤誤差

根據(jù)圖9，車輛起始點在曲線點（2，0）位置，經(jīng)過2個點之后，車輛就回歸在預(yù)定的路徑點上，證明車輛橫向PID控制器在一段時間后開始起作用，車輛在縱向PID的控制下也將車速控制在40 km／h的時速下，車輛按照規(guī)定的目標(biāo)點進行有規(guī)劃的行駛，但可以看出車輛在轉(zhuǎn)彎處時到達的點和預(yù)定的目標(biāo)點之間產(chǎn)生偏差，根據(jù)圖10可以清晰地看出半徑閾值為30 cm時，車輛在轉(zhuǎn)彎處的最大誤差值在0.85 m，而在直線的誤差值最大0.54 m，其余的直線誤差值在0.45 m左右。在半徑閾值20 cm時，車輛在轉(zhuǎn)彎處最大誤差值在0.75 m，在直線的誤差值最大值為0.48 m，直線的平均誤差值在0.4 m左右，在兩者相對比下在不同的目標(biāo)點閾值下車輛跟蹤誤差有所差別。不同的目標(biāo)點閾值設(shè)置是為了模擬CORS網(wǎng)絡(luò)的差分定位，通過仿真實驗得出在30 cm半徑閾值和20 cm半徑閾值下，半徑閾值設(shè)置的相對小能夠提高車輛跟蹤誤差精度，由于GPS定位精度的限制，目標(biāo)點半徑閾值的設(shè)置需在一個可控的范圍內(nèi)。

圖11為50 km／h點追蹤圖，從圖中可以明顯看出車輛在轉(zhuǎn)彎處產(chǎn)生的誤差相對較大，在直線線點的跟蹤誤差也會相應(yīng)增大。圖12為50 km／h點追蹤誤差圖，車輛在轉(zhuǎn)彎處的最大誤差值在1.42 m，而在直線的誤差值最大0.56 m，其余的直線誤差值在0.5 m左右，在不同的半徑閾值下車輛的跟蹤精度相對提高了7 cm。但在車輛轉(zhuǎn)彎處的橫向PID控制的控制參數(shù)相對不滿足當(dāng)前的車速設(shè)定，造成車輛跟蹤精度下降，對于PID控制的3個參數(shù)需要多次重新調(diào)節(jié)來適應(yīng)相應(yīng)的車速增加，達到相對理想的車輛跟蹤精度。

圖11 50 km／h點追蹤

圖12 50 km／h的點追蹤誤差

4 結(jié)論

基于CORS網(wǎng)絡(luò)定位的無人駕駛路徑跟蹤，通過GPS點定位精度分析到整條路徑誤差分析的思路，通過大量實驗得出CORS網(wǎng)絡(luò)定位精度能夠提升4～5倍，定位精度穩(wěn)定且定位設(shè)備工作可靠。在車輛橫向、縱向PID控制方面都具有一定的效果，使路徑跟蹤誤差控制在0.6 m，整體誤差比較合適。通過不同車速相同半徑閾值的對比，車輛行駛速度的增加導(dǎo)致車輛橫向控制難度增大，造成路徑跟蹤誤差增加，也可能速度到達一定值時PID控制失去作用。在相同車速不同的半徑閾值設(shè)定情況下，半徑閾值的減小會提高車輛跟蹤精度。

應(yīng)用CORS網(wǎng)絡(luò)定位提升了定位精度和路徑跟蹤精度，但存在一些問題，還需要進一步的理論與試驗研究。尋找出更有效果的控制算法以及采用多頻段、多通道、處理性能更高的RTK定位設(shè)備，搭建小型的無人駕駛車輛進行試驗，改進理論控制算法，提高路徑跟蹤精度。