基于車體坐標系的激光雷達坐標轉(zhuǎn)換方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明設及一種坐標轉(zhuǎn)換方法，尤其設及一種基于車體坐標系的激光雷達坐標轉(zhuǎn) 換方法。
【背景技術(shù)】
[0002] 數(shù)據(jù)顯示，我國民用汽車保有量已經(jīng)達到1. 5億輛，隨之而來的是交通事故的不 斷增加，據(jù)統(tǒng)計，我國每年交通事故亡人數(shù)已經(jīng)超過了 10萬人；因此，需要利用先進的技術(shù) 來保證安全駕駛，減少交通事故的發(fā)生。
[0003] 激光雷達作為智能交通系統(tǒng)的重要組成部分，在汽車安全與自動駕駛中發(fā)揮著重 要的作用。它能夠精確測量出掃描方向物體的Ξ維坐標，具有測量精度高、采集頻率高的特 點。但是激光雷達采集的數(shù)據(jù)要經(jīng)過復雜的坐標轉(zhuǎn)換才能轉(zhuǎn)換同一個坐標系WGS-84坐標 系中，轉(zhuǎn)化過程中需要實時的激光雷達數(shù)據(jù)、慣性導航INS數(shù)據(jù)、全球定位系統(tǒng)GI^數(shù)據(jù)，然 而運些數(shù)據(jù)的獲取頻率有著很大的差距，激光雷達的頻率為100K監(jiān)甚至更高，INS頻率大 約為200監(jiān)，GPS頻率只有20監(jiān)，因此需要插值，運種高頻率的插值會帶來誤差，影響數(shù)據(jù)采 集速度和精度。
[0004] 測繪激光雷達和防撞激光雷達有著本質(zhì)的區(qū)別，測繪激光雷達要求測量區(qū)域內(nèi)數(shù) 據(jù)盡可能精確，但是對數(shù)據(jù)處理時間沒有要求；防撞激光雷達對數(shù)據(jù)的實時處理功能有著 較高的要求，其總處理時間應該達到100毫秒級別，即其掃描、解算、識別、決策總時間為 100毫秒左右。汽車載體相對于飛機載體有著高度的不確定性，飛機起飛后飛行姿態(tài)相對 比較平穩(wěn)，INS數(shù)據(jù)變化平穩(wěn)，GI^沒有遮擋能實施準確獲取數(shù)據(jù)；但是在汽車行駛過程中 路過城區(qū)、隧道等有遮擋的地方會導致GI^失效，出現(xiàn)連續(xù)的沒有GI^數(shù)據(jù)的時間，此時進 行插值或預測將會增大誤差；汽車速度的變化，路面的高低起伏，甚至汽車的變道都會導致 INS誤差的積累。防撞激光雷達的重點是掃描道路上是否有障礙物，然而道路上的車輛和行 人是隨著時間的推移位置變化的，因此將不同掃描周期的數(shù)據(jù)同時處理變的困難且沒有意 義。由于導航信息是INS經(jīng)過積分而產(chǎn)生，定位誤差隨時間推移而增大，長期精度差，且每 次使用之前需要較長的初始對準時間。傳統(tǒng)的激光雷達數(shù)據(jù)坐標轉(zhuǎn)換要經(jīng)過W下步驟：
[0005] 1、瞬時激光束坐標系轉(zhuǎn)化為激光掃描參考坐標系，根據(jù)瞬時激光與激光雷達坐標 系的夾角和激光飛行的距離，解算出激光腳點相對于激光雷達坐標系的坐標：
[0006]
[0007] 其中狂LYlZJT為激光腳點在激光掃描參考坐標系的坐標，a為瞬時激光束在X0Z 平面內(nèi)的投影與X軸的夾角；β為瞬時激光束在X0Y平面的投影與X軸的夾角。
[0008] 2、激光掃描參考坐標系轉(zhuǎn)化慣性平臺參考坐標系，測量激光雷達與慣性導航平臺 的安置夾角和位移，根據(jù)旋轉(zhuǎn)平移公式，解算激光腳點相對于慣性平臺參考坐標系的坐標； 安置完之后該坐標轉(zhuǎn)換矩陣不會變化，坐標轉(zhuǎn)換公式為：
[0009]
[0010] 其中，狂IΥιΖι)τ為激光腳點相對于慣性平臺參考坐標系的坐標化為由激光掃描 參考坐標系和慣性平臺參考坐標系的安置夾角（a、b、c)得到的旋轉(zhuǎn)矩陣；安裝后旋轉(zhuǎn)矩陣 固定；（A、ΔΥ,AZJT為激光掃描參考坐標系原點和慣性平臺參考坐標系原點之間的位 移向量，安裝后該位移向量固定。
[0011] 3、慣性平臺參考坐標系轉(zhuǎn)化為當?shù)厮絽⒖甲鴺讼担ㄌ炀€GP巧，根據(jù)慣性導航系 統(tǒng)測量的Ξ個姿態(tài)角，Η化eading方向角，繞Z軸旋轉(zhuǎn)）、P(Pitch俯仰角，繞Y軸旋轉(zhuǎn)）、 R(R〇ll側(cè)滾角，繞X軸旋轉(zhuǎn)）和安置位移解算目標點相對于GI^坐標系的坐標；該坐標轉(zhuǎn) 換矩陣由于INS測量的姿態(tài)角的不同而實時變化，但是位移向量不會變化，此時坐標轉(zhuǎn)換 公式為：
[0012]
[001引其中佑Ζ<；)τ為激光腳點相對于當?shù)厮絽⒖甲鴺讼档淖鴺?；（ΔXiΔΥιΔZi)τ為慣性平臺參考坐標系和當?shù)厮絽⒖甲鴺讼抵g的位移向量；Re為慣性導航系統(tǒng)測量 的Ξ個夾角化、R、巧組成的旋轉(zhuǎn)公式。
[0014] 4、當?shù)厮絽⒖甲鴺讼缔D(zhuǎn)化為WGS-84坐標系，根據(jù)GI^測量的締度度）、經(jīng)度 化)、高度（H)數(shù)據(jù)和WGS-84的相關(guān)常系數(shù)，解算激光腳點相對于WGS-84坐標系的坐標；該 坐標轉(zhuǎn)換矩陣由于GI^測量的經(jīng)締度和高度不同而實時變化，此時坐標轉(zhuǎn)換公式為：
[0015]
[001引其中佑4Ys4Zs4)T表示激光腳點相對于WGS-84坐標系的坐標斬為當?shù)厮阶鴺?系到大地坐標系經(jīng)締度度、L)的旋轉(zhuǎn)公示，由GPS的經(jīng)締度獲得；（ΔΧ。ΔΥ。ΔΖ<；)τ為天線 (GP巧到地屯、的位移，由GPS獲得。
[0017] 綜上所述坐標轉(zhuǎn)換公式為：
[0018]
[0019] 傳統(tǒng)的車載激光雷達數(shù)據(jù)坐標系轉(zhuǎn)換需要經(jīng)過四次轉(zhuǎn)換，且有Ξ次坐標轉(zhuǎn)換的轉(zhuǎn) 換矩陣不同，因此需要對每個激光腳點計算轉(zhuǎn)換矩陣，其計算量相當巨大。INS頻率和GPS頻率遠遠小于激光頻率，需要進行預測或內(nèi)插，帶來誤差；汽車路過城區(qū)、隧道等有遮擋的 地方會導致GI^失效，出現(xiàn)連續(xù)的沒有GI^數(shù)據(jù)的時間，此時進行插值或預測將會增大誤 差；INS定位誤差隨時間而增大，長期精度差。
[0020] 忽略周期內(nèi)INS、GPS的數(shù)據(jù)變化，采用最簡單的坐標轉(zhuǎn)換模型，將該掃描周期內(nèi) 看成一個時刻，選取該時刻內(nèi)的某一個INS數(shù)據(jù)和GI^數(shù)據(jù)，即W上坐標轉(zhuǎn)換矩陣的后Ξ矩 陣在一個掃描周期內(nèi)不變，減少了矩陣相乘的運算量。該算法雖然簡單，但是誤差太大，僅 W汽車位移誤差為例，若汽車W25m/s(90km/h)的速度行駛，激光掃描的頻率是10HZ，則一 個周期內(nèi)汽車移動2. 5m，即僅汽車運動誤差就達到2. 5m，運顯然是難W接受的。

【發(fā)明內(nèi)容】

[0021] 本發(fā)明提供了一種基于車體坐標系的激光雷達坐標轉(zhuǎn)換方法，不需要將激光腳點 數(shù)據(jù)轉(zhuǎn)化為WGS-84坐標系，而是轉(zhuǎn)化為掃描周期開始時刻汽車載體瞬時坐標系，即減少了 運算量和數(shù)據(jù)依賴，又能保持點和汽車之間的相對坐標；此方法不依賴于GI^數(shù)據(jù)，尤其是 避免了GI^失效時帶來的內(nèi)插或預測誤差。
[0022] 為實現(xiàn)上述目的，本發(fā)明所采用的技術(shù)方案是：基于車體坐標系的激光雷達坐標 轉(zhuǎn)換方法，具體步驟如下：
[0023] S1 :瞬時激光束坐標系轉(zhuǎn)化為激光掃描參考坐標系，根據(jù)瞬時激光與激光雷達坐 標系的夾角和激光飛行的距離，解算出激光腳點相對于激光雷達坐標系的坐標：
[0024]
[002引其中化為激光腳點在激光掃描參考坐標系的坐標，該激光掃描參考坐標 系為右手坐標系，a為瞬時激光束在X0Z平面內(nèi)的投影與X軸的夾角；β為瞬時激光束在 Χ0Υ平面的投影與X軸的夾角。
[0026] S2 :激光掃描參考坐標系轉(zhuǎn)化慣性平臺參考坐標系，測量激光雷達與慣性導航平 臺的安置夾角和位移，根據(jù)旋轉(zhuǎn)平移公式，解算激光腳點相對于慣性平臺參考坐標系的坐 標；本步驟中的旋轉(zhuǎn)平移公式為：
[0027]
[002引其中，化ΥιΖι)τ為激光腳點相對于慣性平臺參考坐標系的坐標斯為由激光掃描 參考坐標系和慣性平臺參考坐標系之間的安置夾角得到的旋轉(zhuǎn)矩陣；（Α、ΔΥ,AZJT為 激光掃描參考坐標系原點和慣性平臺參考坐標系原點之間的位移向量，激光雷達安裝在車 內(nèi)后旋轉(zhuǎn)矩陣和位移向量固定。
[0029]S3 :慣性平臺參考坐標系轉(zhuǎn)化為周期開始瞬時坐標系，其轉(zhuǎn)化公式為：
[0030]
[00引]其中狂。Υ。Z。)τ是相對于周期開始坐標軸n*t時刻的移動坐標；（ΔX1ΔXiΔXi)τ表示i時刻汽車行駛的位移向量。
[0032] 綜上所述得到最終坐標轉(zhuǎn)換公式：
[0033]
[0034] 進一步的，在步驟S3中位移向量是通過W下步驟得出的：
[0035]A1、在周期開始瞬時坐標系上，將汽車每個掃描周期內(nèi)的行駛軌跡進行微分，該周 期內(nèi)汽車做勻速直線運動，則汽車行駛的距離為：
[0036] 1 = V X t
[0037] 其中V為汽車的速度，t為運行時間；
[003引 A2、初始時A點經(jīng)過t時間到達B點，將AB之間的距離看成等于A到B的行駛軌 跡；此時AB向量相對于A瞬時的坐標（AXΔΥΔ幻T有W下公式；
[0039]
[0040] 其中Κλ巧旋轉(zhuǎn)公式：RΛ?=R(H)R(巧R(R)
[0044] 其中R(H)、R佩、R(巧分別是慣性平臺參考坐標系相對于周期開始瞬時坐標系的 姿態(tài)角，1為在該時間段內(nèi)汽車的位移；
[004引 A3、由此計算出B點相對于起始時刻A的坐標為狂