機動車駕駛?cè)藞龅伛{駛技能考試系統(tǒng)算法設計

馬麗娟

（西安外事學院 工學院，陜西 西安 710077）

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

1 評判要求

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

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

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

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

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

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

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

2.2 車輛各特征點坐標計算

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

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

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

圖2 車輛天線安裝與尺寸測量方法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）計算各特征點坐標

以下以C語言形式給出圖2所示車輛各特征點的坐標計算方法：

//----------車輛前后左右4個端點坐標----------

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

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

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

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

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

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

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

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

//----------車輛前后保險杠坐標------------

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

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

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

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

//---------車輛前后左右4個車輪坐標 ----------

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

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

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

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

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

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

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

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

2.3 車輛速度、前進、后退、停止等判斷

1）車輛速度計算

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

計算給定坐標的兩點（設為 x1，y1和 x2，y2）距離公式：

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

2）前進、后退、停止判斷

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

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

* 在 y2=y1 時，angle=90°或 270°。

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

2.4 車身出線判斷

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

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

1）快速排斥算法

設以線段P1P2為對角線的矩形為R，以線段Q1Q2為對角線的矩形為T，如R和T不相交，顯然兩線段不會相交；

2）跨立算

如果兩線段相交，則兩線段必然相互跨立對方，則矢量（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年前，開磷拓荒者披荊斬棘，人拉肩扛，在荒無人煙的洋水河畔修公路、建礦山、搭帳篷、筑家園，先后建成全國聞名的清潔文明礦山和花園式礦山。一時間，行業(yè)掀起了一股“學吉化，趕開磷”的熱潮，令開磷人倍感自豪和驕傲。經(jīng)過30年的艱苦奮斗，開磷先后完成了一期工程馬路坪礦段和二期工程用沙壩礦段的建設與開發(fā)；到20世紀80年代末，形成了150萬噸/年磷礦石生產(chǎn)能力，成為全國重要的磷礦石生產(chǎn)基地。至此，開磷人完成了自己的第一次創(chuàng)業(yè)。

當（P1-Q1）×（Q2-Q1）=0 時，說明（P1-Q1）和（Q2-Q1）共線，但是因為已經(jīng)通過快速排斥算法，所以P1一定在線段Q1Q2上；

同理，（Q2-Q1）×（P2-Q1）=0 說明 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)完全解決判斷線段是否相交的問題。

基于上述算法，下面的程序段用來判斷給定的4個點組成的兩個線段是否相交：

typedef struct

{float x，y；}point；

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

//計算交叉乘積（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）；

}

//判點是否在線段上，包括端點

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

{

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

}

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

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

{

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

}

//判三點共線

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

{

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

}

//判兩線段相交，包括端點和部分重合

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

{

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）；

}

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

3 結(jié)束語

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

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

[2]左淑紅，左鳳鳴，鄭麗娜.相對差分GPS精度分析[J].測繪與空間地理信息，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定位技術在駕駛員道路考試中的應用[J].現(xiàn)代電子技術，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技術在地籍測量中的應用[J].測繪與空間地理信息，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]潘樹國，王慶，趙毅.GPS差分定位數(shù)據(jù)預處理關鍵技術的研究與實現(xiàn)[J].測控技術，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定位終端的設計[J].通信技術，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.