無(wú)人駕駛車(chē)GPS自主導(dǎo)航與障礙規(guī)避系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)

方海洋，趙瑞妮，涂市委

?

無(wú)人駕駛車(chē)GPS自主導(dǎo)航與障礙規(guī)避系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)

方海洋，趙瑞妮，涂市委

（長(zhǎng)安大學(xué)汽車(chē)學(xué)院，陜西 西安 710064）

提出一種基于Trimble BD982定位傳感器和ZED雙目視覺(jué)傳感器的無(wú)人車(chē)GPS自主導(dǎo)航與障礙規(guī)避系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)方案。利用RTK-GPS系統(tǒng)進(jìn)行高精度定位，當(dāng)ZED雙目相機(jī)檢測(cè)前方出現(xiàn)出現(xiàn)正負(fù)障礙物時(shí)，借助于ZED建立的三維障礙物模型對(duì)原來(lái)GPS采集的路網(wǎng)文件中的一部分路段進(jìn)行點(diǎn)位整體偏移建立局部GPS路網(wǎng)，最終將局部GPS路網(wǎng)與整體GPS路網(wǎng)融合實(shí)現(xiàn)無(wú)人駕駛車(chē)GPS自主導(dǎo)航。

BD982；雙目視覺(jué)傳感器；RTK-GPS；路網(wǎng)融合

1 概述

本文所研究的無(wú)人駕駛車(chē)是基于高精度GPS定位系統(tǒng)實(shí)現(xiàn)自主導(dǎo)航，配合ZED雙目相機(jī)進(jìn)行避障。由人工操作無(wú)人駕駛車(chē)提前進(jìn)行GPS點(diǎn)位的采集，利用NURBS曲線插值法構(gòu)建車(chē)輛運(yùn)行的二維軌跡路徑。車(chē)輛的自主駕駛運(yùn)行過(guò)程中采用NURBS曲線算法中尋找最近點(diǎn)的方法，實(shí)時(shí)尋找車(chē)輛運(yùn)行的目標(biāo)點(diǎn)，結(jié)合車(chē)輛的航向角計(jì)算出車(chē)輛所需的舵量以及適宜速度，利用中值濾波方法處理數(shù)據(jù)后，實(shí)時(shí)控制車(chē)輛線控系統(tǒng)，從而讓無(wú)人駕駛車(chē)沿著該軌跡運(yùn)行。在行進(jìn)的過(guò)程中當(dāng)ZED雙目相機(jī)檢測(cè)到前方出現(xiàn)障礙物或坑洼時(shí)，制作局部GPS路網(wǎng)，并且根據(jù)車(chē)輛航向角和路網(wǎng)信息進(jìn)行局部GPS路網(wǎng)和整體路網(wǎng)的融合。

2 高精度GPS地圖

2.1 GPS點(diǎn)位采集

本文采取載波相位差技術(shù)獲得無(wú)人車(chē)的準(zhǔn)確位置，將BD982采集的WGS-84坐標(biāo)通過(guò)高斯投影轉(zhuǎn)換為平面坐標(biāo)。

圖1 系統(tǒng)整體工作流程圖

圖2 高斯坐標(biāo)投影原理圖

高斯投影后轉(zhuǎn)換的平面坐標(biāo)系是以是以子午線與赤道的交點(diǎn)為原點(diǎn)，x軸為中央子午線的投影，y軸為赤道的投影，大地坐標(biāo)點(diǎn)（B,L）經(jīng)高斯投影轉(zhuǎn)換后的平面坐標(biāo)(x,y)如下：

式中D是指赤道至緯度B的子午線弧長(zhǎng)，N為卯酉圈曲率半徑，為橢球的長(zhǎng)軸半徑。

圖3 坐標(biāo)系轉(zhuǎn)換

通過(guò)高斯投影變換后的坐標(biāo)系x軸指向正北方，y軸指向正東方，不符合操作習(xí)慣，所以將坐標(biāo)系進(jìn)行矩陣變換如下：

2.2 路網(wǎng)制作

在本文中我們利用NURBS曲線插值法將離散的點(diǎn)位數(shù)據(jù)處理成平滑的曲線：

式中E()是m階B樣條基函數(shù)，q是權(quán)重因子，p是控制點(diǎn)?；瘮?shù)E()通常由遞推公式定義如下：

2.3 無(wú)人駕駛GPS導(dǎo)航算法

根據(jù)實(shí)際道路情況在軟件中提前設(shè)定打舵參考行n，相當(dāng)于選取距離車(chē)輛一定距離的GPS點(diǎn)位作為車(chē)輛打舵的目標(biāo)點(diǎn)。

圖4 GPS導(dǎo)航示意圖

車(chē)輛前后自身安裝了2個(gè)GPS天線，根據(jù)這兩個(gè)天線位置計(jì)算出車(chē)輛與參考行GPS點(diǎn)位的連線與地球正北的夾角為：

由和車(chē)輛的航向角可以求得打舵角度為：

式中n為倍率系數(shù)。

對(duì)打舵角度進(jìn)行中值濾波處理：

計(jì)算機(jī)將打舵角度δ發(fā)送給下位機(jī)，計(jì)算出車(chē)輛打舵的信號(hào)電壓二次多項(xiàng)式為：

無(wú)人駕駛車(chē)速度的控制采用設(shè)定一個(gè)和，反復(fù)試驗(yàn)確定車(chē)速和打舵角度△θ之間的關(guān)系如下：

3 避障控制策略

3.1 障礙物建模

為了配合GPS導(dǎo)航，需要將障礙物的相機(jī)坐標(biāo)系轉(zhuǎn)化為世界坐標(biāo)系。

式中Xw代表世界坐標(biāo)系，代cmn表旋轉(zhuǎn)矩陣，Xc代表相機(jī)坐標(biāo)系，Dx代表平移矩陣。相機(jī)坐標(biāo)系向世界坐標(biāo)系的轉(zhuǎn)換過(guò)程可視為坐標(biāo)系先繞Z軸旋轉(zhuǎn)，然后繞Y軸旋轉(zhuǎn)，最后繞X軸旋轉(zhuǎn)的過(guò)程，但最終還需要做出適當(dāng)平移達(dá)到相互統(tǒng)一。

繞Z軸、Y軸、X軸旋轉(zhuǎn)即等同于將原來(lái)的坐標(biāo)矩陣經(jīng)過(guò)三次矩陣旋轉(zhuǎn)變換，三個(gè)旋轉(zhuǎn)矩陣按照順序如下：

無(wú)人車(chē)主要行駛在平坦道路，所以可認(rèn)為車(chē)輛的俯仰角和側(cè)傾角為0，即=90°，=90°。此時(shí)車(chē)輛的航向角為，當(dāng)＜90°，=90°-，即旋轉(zhuǎn)矩陣為=321。

3.2 局部GPS路網(wǎng)重建

當(dāng)ZED雙目相機(jī)探測(cè)到前方出現(xiàn)障礙物時(shí)，本文采用兩步判斷法進(jìn)行路網(wǎng)重建。首先基于可通過(guò)性判斷，當(dāng)障礙物的一側(cè)仍然存在其他障礙物或坑洼，排除從該側(cè)避障的可能性；其次從左右側(cè)避障的路徑長(zhǎng)度來(lái)判斷，分別計(jì)算車(chē)輛與左右角點(diǎn)的距離為d，d為：

當(dāng)d-d≥3時(shí)選擇從右側(cè)避障；當(dāng)d-d＞3時(shí)選擇從左側(cè)避障。

本論文按照障礙物橫向輪廓線方向平移，將靠近車(chē)輛一側(cè)的離散點(diǎn)位用最小二乘法回歸：

假設(shè)車(chē)輛從右側(cè)避障，由二次回歸直線的斜率可以求得直線與大地坐標(biāo)系X軸正向的角度為：

計(jì)算GPS路網(wǎng)與障礙物第一個(gè)交點(diǎn)為(4,4)，根據(jù)該交點(diǎn)與右側(cè)角點(diǎn)(R,R)的距離計(jì)算向右偏移距離4。

偏移后點(diǎn)的坐標(biāo)1為：

當(dāng)車(chē)輛行駛至右側(cè)1點(diǎn)時(shí)，如果車(chē)輛的航向偏向右方，由于ZED雙目相機(jī)水平視場(chǎng)角有限，所以可能捕捉不到障礙物，因此必須預(yù)先按照障礙物右側(cè)邊緣與前側(cè)邊緣垂直的假設(shè)情形來(lái)偏移一段距離的GPS點(diǎn)位。系統(tǒng)遍歷E點(diǎn)之后50個(gè)GPS點(diǎn)位信息，并向右按照與當(dāng)前航向垂直方向?qū)ふ艺系K物右邊緣點(diǎn)坐標(biāo)，計(jì)算每個(gè)GPS點(diǎn)位所需要偏移的距離d為：

當(dāng)車(chē)輛通過(guò)1在障礙物右側(cè)的行進(jìn)過(guò)程中,當(dāng)雙目相機(jī)拍攝到障礙物的圖像時(shí)，按照上文提到的方法對(duì)障礙物建模，利用坐標(biāo)系轉(zhuǎn)換求出該段所有GPS點(diǎn)位的平移距離，然后計(jì)算出更新后的GPS點(diǎn)位。如果障礙物在路徑方向的距離很長(zhǎng)，ZED相機(jī)無(wú)法捕捉全景，所以要對(duì)障礙物的模型進(jìn)行更新?？紤]到車(chē)速和計(jì)算機(jī)性能，我們將障礙物模型個(gè)更新頻率設(shè)置為1HZ。采用算術(shù)平均值濾波方法對(duì)大量離散的GPS坐標(biāo)數(shù)據(jù)進(jìn)行濾波處理，具體做法是：選取若干個(gè)樣本框，對(duì)各個(gè)樣本框里點(diǎn)位的橫縱坐標(biāo)進(jìn)行平均后得到一個(gè)新的坐標(biāo)。單個(gè)樣本框的確定取決于點(diǎn)位的數(shù)量，當(dāng)GPS點(diǎn)位達(dá)到預(yù)設(shè)數(shù)量時(shí)就形成了一個(gè)樣本框。樣本框自身的個(gè)數(shù)由雙目相機(jī)前方的盲區(qū)區(qū)域大小確定，因?yàn)闊o(wú)法對(duì)盲區(qū)建模，所以不會(huì)有新的離散數(shù)據(jù)干擾濾波。在車(chē)輛前進(jìn)時(shí)，盲區(qū)不斷向前擴(kuò)展，系統(tǒng)根據(jù)新增的GPS點(diǎn)位生成樣本框，然后對(duì)樣本框的數(shù)據(jù)進(jìn)行算術(shù)平均值濾波處理。

在車(chē)輛前進(jìn)的過(guò)程中，當(dāng)雙目相機(jī)檢測(cè)到前方?jīng)]有障礙物時(shí)，可能障礙物右側(cè)邊緣線向左側(cè)偏移，還有可能車(chē)輛已經(jīng)通過(guò)障礙物區(qū)域。首先我們需要確定車(chē)輛是否通過(guò)障礙物，具體做法是：選取一個(gè)適宜的△，將此時(shí)車(chē)輛航向角由調(diào)整為△，使車(chē)輛向左側(cè)行駛，可以增大雙目相機(jī)左側(cè)的視場(chǎng)。將原來(lái)的坐標(biāo)系順時(shí)針旋轉(zhuǎn)，相當(dāng)于GPS點(diǎn)位逆時(shí)針旋轉(zhuǎn)，車(chē)輛航向向左側(cè)偏移。

當(dāng)車(chē)輛向左側(cè)偏移后，雙目相機(jī)檢測(cè)到障礙物邊緣后，按照上述方法繼續(xù)建模，GPS偏移。如果車(chē)輛向左偏移后仍無(wú)法檢測(cè)到障礙物，此時(shí)系統(tǒng)認(rèn)為車(chē)輛已經(jīng)通過(guò)障礙物區(qū)域。

3.3 局部GPS路網(wǎng)和全局GPS路網(wǎng)的融合

首先根據(jù)偏移距離d確定車(chē)輛在距離E點(diǎn)d時(shí)就開(kāi)始離開(kāi)全局GPS路網(wǎng)（d正相關(guān)于d）。根據(jù)距離d遍歷E點(diǎn)之前的點(diǎn)位，尋找離開(kāi)點(diǎn)F，統(tǒng)計(jì)F點(diǎn)和E點(diǎn)之間的GPS點(diǎn)位個(gè)數(shù)為n，假設(shè)第一個(gè)點(diǎn)位的偏移量為0.05m，按照等比數(shù)列設(shè)計(jì)其他點(diǎn)位的偏移量，等比數(shù)列的比例系數(shù)q為：

根據(jù)該比例系數(shù)計(jì)算每一個(gè)點(diǎn)位的偏移距離d,計(jì)算所有點(diǎn)位的偏移距離d=0.05(m-1)，然后再次使用NURBS曲線插值法構(gòu)造該段路網(wǎng)曲線，通過(guò)障礙物之后的局部GPS路網(wǎng)偏移方法與此相同。

[1] 李陸浩,面向無(wú)人駕駛汽車(chē)的車(chē)道級(jí)導(dǎo)航研究[D].吉林大學(xué).2014.

[2] 夏天,智能車(chē)速度規(guī)劃及路徑跟蹤控制方法研究[D].北京工業(yè)大學(xué).2017.

[3] 王晨,無(wú)人駕駛智能車(chē)控制與規(guī)劃系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)[D].上海交通大學(xué).2009.

Design and implementation of GPS and Binocular camera for driverless car

Fang Haiyang, Zhao Ruini, Tu Shiwei

( Automobile College of Chang’an University, Shaanxi Xi’an 710064 )

This paper presents a design and implementation of driverless car based on Trimble BD982 positioning sensor and ZED binocular vision sensor. The difference with RTK-GPS system base station for high precision positioning. When obstacles or eliminate appears in front of the road is detected by ZED binocular camera, With the help of the 3D obstacle model established by ZED, the local GPS road network has been established by the point bit overall migration of some sections of the road network files which were collected by the GPS originally. Finally we integrate the local GPS road network with the overall GPS network to realize the GPS autonomous navigation of the driverless car.

BD982; binocular vision sensor; RTK-GPS; road network fusion

B

1671-7988(2018)24-30-04

U462

B

1671-7988(2018)24-30-04

U462

方海洋，長(zhǎng)安大學(xué)汽車(chē)學(xué)院碩士研究生，研究方向：無(wú)人駕駛汽車(chē)，線控地盤(pán)改造，高精度GPS導(dǎo)航。

10.16638/j.cnki.1671-7988.2018.24.009