机动车驾驶人场地驾驶技能考试系统算法设计

马丽娟

（西安外事学院 工学院，陕西 西安 710077）

目前，机动车驾驶人场地驾驶技能[1]（科目二）考试全部采用计算机自动评判方式，不再有考官打分。自动评判是根据考生当前的驾驶状况、车辆运行情况，结合公安部的123号令的评判标准，进行自动扣分。现有的技术是基于双差分GPS进行车辆定位[2]，并结合车载信号进行综合评定，都是采用车载工控机进行软件算法的实现，一般要对车辆周边进行32点以上的尺寸量取，并要对场地项目进行区域划分，判断车周边各点是否在合理区域内，实施成本高、运算效率低、实时性差、施工周期长。为此，提出了一种GPS坐标的计算和比对算法，可以准确、方便地实现GPS坐标的比对，获得车辆的违规信息，且易于在微控制器中实现。

1 评判要求

机动车驾驶人场地驾驶技能考试要求对转向灯正确使用、车辆运行中不能熄火、必须使用安全带，且项目考试过程中不能中途停车，更多的评判要求是不能车身出线、压线、后溜等。现有的GPS方案下，计算机必须不断的对GPS坐标进行读取、对车辆各点坐标进行计算，并将车辆坐标与事先保存的场地坐标进行比对，然后获得车辆的准确位置信息，从而判别该车辆是否压线、车身出线等。其主程序流程如图1所示。

其中，初始化除完成微控制器的配置外，还要读取预先存储在永久存储器中的车辆尺寸数据、场地坐标数据等。

2 车轮压线及车身出线判断方法

现有的基于RTK技术的双天线GPS设备，其差分定位精度为10 mm，有两种数据输出格式，都针对主天线（设置与车辆后方）[3]。一种是基于经纬度的格式，另一种是基于东向、北向距离的格式[4]。为了比较方便地比较GPS坐标，以下均采用东向、北向距离的格式 。

设获取的东向距离为GPS.x（单位mm），北向距离为GPS.y（单位mm），车辆与北向的夹角为 Φ（单位rad）。

2.1 典型的车辆尺寸度量方法

以小型车为例，两个GPS天线GNSS1和GNSS2安装于车辆左右的中心，其中GNSS2为主天线，安装于车辆后部，GNSS1为从天线，安装于车辆前部。为方便比较坐标，取图2所示的各点尺寸，这里取的车辆尺寸单位为mm。

2.2 车辆各特征点坐标计算

1）首先根据车辆尺寸计算各特征点与GPS主天线组成的直线相对于车辆纵向中心的角度 Φ1、Φ2、Φ3、Φ4（如图 2）。

根据平面几何的知识，可得到以上各角度的计算公式如下：

图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终端发送坐标的频率为5 Hz，则每隔200 ms发送一次坐标数据[5]。根据相邻两次发送的坐标，计算出其相对距离，并结合5 Hz的频率来计算瞬时速度。

计算给定坐标的两点（设为 x1，y1和 x2，y2）距离公式：

则瞬时速度v=d*0.018，单位km/H。

2）前进、后退、停止判断

若连续3次瞬时速度v小于0.2 km/h，则可认为车辆处于停止状态。根据当前坐标（x1，y1）与上一次坐标（x2，y2），利用三角函数即可计算出车辆行驶的角度angle：

* 在 y2!=y1 时，angle=atan（x2-x1/y2-y1）； 并要根据具体象限调整；

* 在 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）的两侧，即

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

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

60年前，开磷拓荒者披荆斩棘，人拉肩扛，在荒无人烟的洋水河畔修公路、建矿山、搭帐篷、筑家园，先后建成全国闻名的清洁文明矿山和花园式矿山。一时间，行业掀起了一股“学吉化，赶开磷”的热潮，令开磷人倍感自豪和骄傲。经过30年的艰苦奋斗，开磷先后完成了一期工程马路坪矿段和二期工程用沙坝矿段的建设与开发；到20世纪80年代末，形成了150万吨/年磷矿石生产能力，成为全国重要的磷矿石生产基地。至此，开磷人完成了自己的第一次创业。

当（P1-Q1）×（Q2-Q1）=0 时，说明（P1-Q1）和（Q2-Q1）共线，但是因为已经通过快速排斥算法，所以P1一定在线段Q1Q2上；

同理，（Q2-Q1）×（P2-Q1）=0 说明 P2 一定在线段 Q1Q2上。所以判断P1P2跨立Q1Q2的依据是：

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

同理判断Q1Q2跨立P1P2的依据是：

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

至此已经完全解决判断线段是否相交的问题。

基于上述算法，下面的程序段用来判断给定的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）

}

//判两点在线段同侧，点在线段上返回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 结束语

基于线段相交判断的机动车驾驶人考试评判算法，避免了常规划分场地区域、判断车周边大量点是否在区域内的运算耗时、车辆测量复杂的缺陷，运算效率高，易于在微控制器中实现，成本低、实时性好，特别适用于驾考车辆及驾校训练系统中。

[1]GA1026-2012.机动车驾驶人考试内容和方法[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].现代电子技术，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差分定位数据预处理关键技术的研究与实现[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]陆文昌，毕世高.车辆监控系统中车载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.