機(jī)動(dòng)車駕駛?cè)藞?chǎng)地駕駛技能考試系統(tǒng)算法設(shè)計(jì)

馬麗娟

（西安外事學(xué)院 工學(xué)院，陜西 西安 710077）

目前，機(jī)動(dòng)車駕駛?cè)藞?chǎng)地駕駛技能[1]（科目二）考試全部采用計(jì)算機(jī)自動(dòng)評(píng)判方式，不再有考官打分。自動(dòng)評(píng)判是根據(jù)考生當(dāng)前的駕駛狀況、車輛運(yùn)行情況，結(jié)合公安部的123號(hào)令的評(píng)判標(biāo)準(zhǔn)，進(jìn)行自動(dòng)扣分?，F(xiàn)有的技術(shù)是基于雙差分GPS進(jìn)行車輛定位[2]，并結(jié)合車載信號(hào)進(jìn)行綜合評(píng)定，都是采用車載工控機(jī)進(jìn)行軟件算法的實(shí)現(xiàn)，一般要對(duì)車輛周邊進(jìn)行32點(diǎn)以上的尺寸量取，并要對(duì)場(chǎng)地項(xiàng)目進(jìn)行區(qū)域劃分，判斷車周邊各點(diǎn)是否在合理區(qū)域內(nèi)，實(shí)施成本高、運(yùn)算效率低、實(shí)時(shí)性差、施工周期長(zhǎng)。為此，提出了一種GPS坐標(biāo)的計(jì)算和比對(duì)算法，可以準(zhǔn)確、方便地實(shí)現(xiàn)GPS坐標(biāo)的比對(duì)，獲得車輛的違規(guī)信息，且易于在微控制器中實(shí)現(xiàn)。

1 評(píng)判要求

機(jī)動(dòng)車駕駛?cè)藞?chǎng)地駕駛技能考試要求對(duì)轉(zhuǎn)向燈正確使用、車輛運(yùn)行中不能熄火、必須使用安全帶，且項(xiàng)目考試過(guò)程中不能中途停車，更多的評(píng)判要求是不能車身出線、壓線、后溜等?，F(xiàn)有的GPS方案下，計(jì)算機(jī)必須不斷的對(duì)GPS坐標(biāo)進(jìn)行讀取、對(duì)車輛各點(diǎn)坐標(biāo)進(jìn)行計(jì)算，并將車輛坐標(biāo)與事先保存的場(chǎng)地坐標(biāo)進(jìn)行比對(duì)，然后獲得車輛的準(zhǔn)確位置信息，從而判別該車輛是否壓線、車身出線等。其主程序流程如圖1所示。

其中，初始化除完成微控制器的配置外，還要讀取預(yù)先存儲(chǔ)在永久存儲(chǔ)器中的車輛尺寸數(shù)據(jù)、場(chǎng)地坐標(biāo)數(shù)據(jù)等。

2 車輪壓線及車身出線判斷方法

現(xiàn)有的基于RTK技術(shù)的雙天線GPS設(shè)備，其差分定位精度為10 mm，有兩種數(shù)據(jù)輸出格式，都針對(duì)主天線（設(shè)置與車輛后方）[3]。一種是基于經(jīng)緯度的格式，另一種是基于東向、北向距離的格式[4]。為了比較方便地比較GPS坐標(biāo)，以下均采用東向、北向距離的格式 。

設(shè)獲取的東向距離為GPS.x（單位mm），北向距離為GPS.y（單位mm），車輛與北向的夾角為 Φ（單位rad）。

2.1 典型的車輛尺寸度量方法

以小型車為例，兩個(gè)GPS天線GNSS1和GNSS2安裝于車輛左右的中心，其中GNSS2為主天線，安裝于車輛后部，GNSS1為從天線，安裝于車輛前部。為方便比較坐標(biāo)，取圖2所示的各點(diǎn)尺寸，這里取的車輛尺寸單位為mm。

2.2 車輛各特征點(diǎn)坐標(biāo)計(jì)算

1）首先根據(jù)車輛尺寸計(jì)算各特征點(diǎn)與GPS主天線組成的直線相對(duì)于車輛縱向中心的角度 Φ1、Φ2、Φ3、Φ4（如圖 2）。

根據(jù)平面幾何的知識(shí)，可得到以上各角度的計(jì)算公式如下：

圖1 主程序流程圖Fig.1 Flow chart of main program

圖2 車輛天線安裝與尺寸測(cè)量方法Fig.2 Method of vehicle antenna installation and measurement

φ1=arcsin（C/2/F）；φ2=arcsin（C/2/G）；

φ3=arcsin（A/2/H）；φ4=arcsin（B/2/K）；

2）計(jì)算各特征點(diǎn)坐標(biāo)

以下以C語(yǔ)言形式給出圖2所示車輛各特征點(diǎn)的坐標(biāo)計(jì)算方法：

//----------車輛前后左右4個(gè)端點(diǎn)坐標(biāo)----------

car.xy[0][0]=GPS.x+F*sin（Φ - Φ1）；//左前端 X 坐標(biāo)

car.xy[0][1]=GPS.y+F*cos（Φ - Φ1）； //左前端 Y 坐標(biāo)

car.xy[1][0]=GPS.x+F*sin（Φ1+Φ）；//右前端 X 坐標(biāo)

car.xy[1][1]=GPS.y+F*cos（Φ1+ Φ）； //右前端 Y 坐標(biāo)

car.xy[2][0]=GPS.x-G*sin（Φ2+ Φ）；//右后端 X 坐標(biāo)

car.xy[2][1]=GPS.y-G*cos（Φ2+ Φ）；//右后端 Y 坐標(biāo)

car.xy[3][0]=GPS.x+G*sin（Φ2- Φ）； //右后端 X 坐標(biāo)

car.xy[3][1]=GPS.y╞ G*cos（Φ2-Φ）；//右后端 Y 坐標(biāo)

//----------車輛前后保險(xiǎn)杠坐標(biāo)------------

car.xy[4][0]=GPS.x+D*sin（Φ）； //前杠 X 坐標(biāo)

car.xy[4][1]=GPS.y+D*cos（Φ）； //前杠 Y 坐標(biāo)

car.xy[5][0]=GPS.x-E*sin（Φ）； //后杠 X 坐標(biāo)

car.xy[5][1]=GPS.y-E*cos（Φ）； //后杠 Y 坐標(biāo)

//---------車輛前后左右4個(gè)車輪坐標(biāo) ----------

car.xy[6][0]=GPS.x+H*sin（Φ - Φ3）；//左前輪 X 坐標(biāo)

car.xy[6][1]=GPS.y+H*cos（Φ - Φ3）； //左前輪 Y 坐標(biāo)

car.xy[7][0]=GPS.x+H*sin（Φ3+ Φ）；//右前輪 X 坐標(biāo)

car.xy[7][1]=GPS.y+H*cos（Φ3+ Φ）；//右前輪 Y 坐標(biāo)

car.xy[8][0]=GPS.x-K*sin（Φ4+ Φ）；//左后輪 X 坐標(biāo)

car.xy[8][1]=GPS.y-K*cos（Φ4+ Φ）；//左后輪 Y 坐標(biāo)

car.xy[9][0]=GPS.x+K*sin（Φ4- Φ）；//右后輪 X 坐標(biāo)

car.xy[9][1]=GPS.y-K*cos（Φ4- Φ）；//右后輪 Y 坐標(biāo)

2.3 車輛速度、前進(jìn)、后退、停止等判斷

1）車輛速度計(jì)算

設(shè)GPS終端發(fā)送坐標(biāo)的頻率為5 Hz，則每隔200 ms發(fā)送一次坐標(biāo)數(shù)據(jù)[5]。根據(jù)相鄰兩次發(fā)送的坐標(biāo)，計(jì)算出其相對(duì)距離，并結(jié)合5 Hz的頻率來(lái)計(jì)算瞬時(shí)速度。

計(jì)算給定坐標(biāo)的兩點(diǎn)（設(shè)為 x1，y1和 x2，y2）距離公式：

則瞬時(shí)速度v=d*0.018，單位km/H。

2）前進(jìn)、后退、停止判斷

若連續(xù)3次瞬時(shí)速度v小于0.2 km/h，則可認(rèn)為車輛處于停止?fàn)顟B(tài)。根據(jù)當(dāng)前坐標(biāo)（x1，y1）與上一次坐標(biāo)（x2，y2），利用三角函數(shù)即可計(jì)算出車輛行駛的角度angle：

* 在 y2!=y1 時(shí)，angle=atan（x2-x1/y2-y1）； 并要根據(jù)具體象限調(diào)整；

* 在 y2=y1 時(shí)，angle=90°或 270°。

若車輛行駛角度angle（與北向夾角）與GPS兩天線與北向的角度Φ的差小于90°，則車輛為前進(jìn)，否則車輛為后退。

2.4 車身出線判斷

車身任一特征點(diǎn)出線，則認(rèn)為車身出線。處理的方法是判斷車身兩特征點(diǎn)相連組成的線段是否與道路邊線（或庫(kù)位線等）相交，若相交，則認(rèn)為車身出線[6]。

分兩步確定兩條線段是否相交：

1）快速排斥算法

設(shè)以線段P1P2為對(duì)角線的矩形為R，以線段Q1Q2為對(duì)角線的矩形為T，如R和T不相交，顯然兩線段不會(huì)相交；

2）跨立算

如果兩線段相交，則兩線段必然相互跨立對(duì)方，則矢量（P1-Q1）和（P2-Q1）位于矢量（Q2-Q1）的兩側(cè)，即

（P1-Q1）×（Q2-Q1）*（P2-Q1）×（Q2-Q1）<0

上式可改寫成：（P1-Q1）×（Q2-Q1）*（Q2-Q1）×（P2-Q1）>0

60年前，開(kāi)磷拓荒者披荊斬棘，人拉肩扛，在荒無(wú)人煙的洋水河畔修公路、建礦山、搭帳篷、筑家園，先后建成全國(guó)聞名的清潔文明礦山和花園式礦山。一時(shí)間，行業(yè)掀起了一股“學(xué)吉化，趕開(kāi)磷”的熱潮，令開(kāi)磷人倍感自豪和驕傲。經(jīng)過(guò)30年的艱苦奮斗，開(kāi)磷先后完成了一期工程馬路坪礦段和二期工程用沙壩礦段的建設(shè)與開(kāi)發(fā)；到20世紀(jì)80年代末，形成了150萬(wàn)噸/年磷礦石生產(chǎn)能力，成為全國(guó)重要的磷礦石生產(chǎn)基地。至此，開(kāi)磷人完成了自己的第一次創(chuàng)業(yè)。

當(dāng)（P1-Q1）×（Q2-Q1）=0 時(shí)，說(shuō)明（P1-Q1）和（Q2-Q1）共線，但是因?yàn)橐呀?jīng)通過(guò)快速排斥算法，所以P1一定在線段Q1Q2上；

同理，（Q2-Q1）×（P2-Q1）=0 說(shuō)明 P2 一定在線段 Q1Q2上。所以判斷P1P2跨立Q1Q2的依據(jù)是：

（P1-Q1）×（Q2-Q1）*（Q2-Q1）×（P2-Q1）≥0

同理判斷Q1Q2跨立P1P2的依據(jù)是：

（Q1-P1）×（P2-P1）*（P2-P1）×（Q2-P1）≥0

至此已經(jīng)完全解決判斷線段是否相交的問(wèn)題。

基于上述算法，下面的程序段用來(lái)判斷給定的4個(gè)點(diǎn)組成的兩個(gè)線段是否相交：

typedef struct

{float x，y；}point；

#define zero（x） （（（x）>0?（x）:-（x））

//計(jì)算交叉乘積（P1-P0）x（P2-P0）

float xmult（point p1，point p2，point p0）

{

return （p1.x-p0.x）*（p2.y-p0.y）-（p2.x-p0.x）*（p1.y-p0.y）；

}

//判點(diǎn)是否在線段上，包括端點(diǎn)

int dot_online_in（point p，point l1，point l2）

{

return zero（xmult（p，l1，l2））&& （l1.x-p.x）* （l2.x-p.x）

}

//判兩點(diǎn)在線段同側(cè)，點(diǎn)在線段上返回0

int same_side（point p1，point p2，point l1，point l2）

{

return xmult（l1，p1，l2）*xmult（l1，p2，l2）>eps；

}

//判三點(diǎn)共線

int dots_inline（point p1，point p2，point p3）

{

return zero（xmult（p1，p2，p3））；

}

//判兩線段相交，包括端點(diǎn)和部分重合

int intersect_in（point u1，point u2，point v1，point v2） //返回值非0，則有交點(diǎn)

{

if（!dots_inline（u1，u2，v1）||!dots_inline（u1，u2，v2））

return!same_side（u1，u2，v1，v2）&&!same_side（v1，v2，u1，u2）；

return dot_online_in（u1，v1，v2）||dot_online_in（u2，v1，v2）||dot_online_in（v1，u1，u2）||dot_online_in（v2，u1，u2）；

}

同理，判斷車輪壓線，就判斷兩個(gè)前輪組成的線段及兩個(gè)后輪組成的線段與道路邊線是否相交即可。

3 結(jié)束語(yǔ)

基于線段相交判斷的機(jī)動(dòng)車駕駛?cè)丝荚囋u(píng)判算法，避免了常規(guī)劃分場(chǎng)地區(qū)域、判斷車周邊大量點(diǎn)是否在區(qū)域內(nèi)的運(yùn)算耗時(shí)、車輛測(cè)量復(fù)雜的缺陷，運(yùn)算效率高，易于在微控制器中實(shí)現(xiàn)，成本低、實(shí)時(shí)性好，特別適用于駕考車輛及駕校訓(xùn)練系統(tǒng)中。

[1]GA1026-2012.機(jī)動(dòng)車駕駛?cè)丝荚噧?nèi)容和方法[S].2013.

[2]左淑紅，左鳳鳴，鄭麗娜.相對(duì)差分GPS精度分析[J].測(cè)繪與空間地理信息，2007，30（2）：42-45.ZUO Shu-hong，ZUO Feng-ming，ZHENG Li-na.Precision analysis of relative difference GPS[J].Geomatics&Spatial in Formation Technology，2007，30（2）：42-45.

[3]趙峰.RTK GPS定位技術(shù)在駕駛員道路考試中的應(yīng)用[J].現(xiàn)代電子技術(shù)，2012， 35（15）：37-39.ZHAO Feng.Application of RTK GPSpositioning technology in road test[J].Modern Electronics Technique，2012，35（15）：37-39.

[4]段曙光.GPS-RTK技術(shù)在地籍測(cè)量中的應(yīng)用[J].測(cè)繪與空間地理信息，2010，33（1）:107-109.DUAN Shu-guang.Application of GPS RTK in cadastral survey[J].Geomatics& Spatial Information Technology，2010，33（1）:107-109.

[5]潘樹(shù)國(guó)，王慶，趙毅.GPS差分定位數(shù)據(jù)預(yù)處理關(guān)鍵技術(shù)的研究與實(shí)現(xiàn)[J].測(cè)控技術(shù)，2006，25（9）：25-28.PAN Shu-guo，WANG Qing，ZHAO Yi.Research and realization of key data pretreatment technology for GPSdifferential positioning[J].Measurement& Control Technology，2006，25（9）：25-28.

[6]陸文昌，畢世高.車輛監(jiān)控系統(tǒng)中車載GPS定位終端的設(shè)計(jì)[J].通信技術(shù)，2010，7（43）：201-203.LU Wen-chang，BI Shi-gao.Design on GPS terminal vehicle montioring system[J].Communications Technology，2010，7（43）：201-203.