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

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

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