一种智能交通物理仿真平台及用于该智能交通物理仿真平台
的定位方法
技术领域
[0001] 本
发明涉及一种用于智能交通物理仿真平台的智能小车及定位方法，属于交通模拟仿真方法领域。
背景技术
[0002] 为了缓解日益增加的交通拥堵对城市路网造成的压
力，车车-车路联网环境下的智能交通概念得到了广泛的关注，它是将先进的通信网络方法、
传感器方法、计算机方法和控制方法等有机地结合在一起的交通管理方法，包括车车协同、车路协同等各个方面的内容。由于直接将新型的智能交通方法运用到实际交通中存在安全
风险，基于道路沙盘与智能小车的智能交通仿真平台具有十分重要的意义。本发明设计一种利用沙盘等比例模拟实际交通道路及附属设施，使用智能小车等比例模拟实际车辆，进行各种新型智能交通方法的实验研究与仿真展示。
[0003] 现有的智能交通仿真与展示一般分为两大类型：纯
软件仿真和交通沙盘物理平台。纯软件仿真系统主要有：VISSIM、PARAMICS、TSIS等，其中车辆的运行依赖于跟车模型和道路变换模型，系统也提供2D或3D的动画以便于观察仿真结果及研究交通路网性能。这类软件中基于数学模型模拟实际车辆，其中忽略了许多方法细节，与实际车辆之间存在一定偏差。物理仿真平台使用模拟真实道路的交通沙盘。但现有的在沙盘上行驶的仿真车辆智能化程度较低，表现为行驶速度恒定、运动轨迹固定等
缺陷，因此只能用来演示预先设计好的事件，例如起步、停车，难以应用于更复杂的方法研究。
[0004] 智能交通仿真平台的另一个难点在于对智能小车的定位，传统的定位方法包括GPS、
无线传感器网络定位、视频定位等。GPS定位方法在室外环境下使用具有良好的效果，但由于遮挡等原因导致
精度下降无法应用于室内环境，无线传感器网络定位方法作为室内定位方法越来越受到开发人员的重视，但是现阶段的无线传感器网络定位方法还无法实现室内的精确定位。现阶段唯一能做到室内精确定位的是视频定位方法，由于道路沙盘为双层结构，视频定位方法无法解决上下层道路之间存在遮挡问题，无法应用于本平台。
[0005]
射频识别(Radio Frequency Identification，RFID)方法是20世纪80年代发展起来的一种新兴自动识别方法，系统可以由一个
读卡器和若干个标签组成，每个无源标签都拥有一个唯一的UID用于识别。目前，交通领域对RFID的研究主要集中在
电子车牌的应用上，将有源防拆电子标签置于车内，利用读卡器基站群对电子标签进行
数据采集，其目的在于有效解决城市交通信息化建设的
瓶颈问题。
[0006] 本发明的目的在于设计一种用于智能交通物理仿真平台的新型智能小车，和一种基于RFID方法的室内智能小车的实时定位方法。
发明内容
[0007] 本发明的目的在于提出了一种新型的智能小车，采用
合金车身，根据真实车辆以1:16的比例微缩而成，与真实车辆相同，使用前轮转向，
后轮驱动。智能小车上装载的内传感器有
编码器、
加速度计、
陀螺仪，外传感器有
超声波模
块、激光测距模块。
[0008] 本发明还提出了微型惯性导航与RFID方法结合的智能小车定位与导航方法。与射频识别方法在电子车牌应用上不同，在实现智能小车定位与导航时，将读卡器置于车内，无源电子标签铺设在路面上，每一个无源电子标签对应于全局
坐标系中的一个坐标，当智能小车从无源电子标签上方驶过时，读卡器识别出无源电子标签的UID，并确定出与之对应的
位姿信息用来对微型惯性
导航系统进行
位置校正。
[0009] 该仿真平台可以用于车车-车路网络环境下智能车辆协作的实验研究与仿真展示。
[0010] 为实现上述目的，本发明采用的方法方案为一种用于智能交通物理仿真平台的智能小车及定位方法，本发明的整体系统结构如下。
[0011] 所述用于智能交通物理仿真平台的智能小车，该结构包括沙盘9与智能小车10；
[0012] 沙盘9包括上部沙盘9.1和下部沙盘9.2两部分；所述上部沙盘9.1设置有智能小车移动的环形车道，用以模拟城市快速路。
[0013] 所述下部沙盘9.2设置有十字路口，用以模拟城市交通路口。
[0014] 上部沙盘9.1与下部沙盘9.2通过两条对称的坡道连接。上部沙盘9.1与下部沙盘9.2的两层道路全
覆盖有视频网络，用以实时监控智能小车的
信号。
[0015] 所述上部沙盘9.1与下部沙盘9.2的路面中内嵌上遵循IOS14443协议的圆形RFID无源标签9.3，用以辅助定位并修正惯性导航的累积误差。
[0016] 智能小车10包括主控制单元1、
电池2，
电机3，编码器4，
舵机5，RFID读卡器6，激光测距模块7，
超声波测距模块组8。超声波测距模块组8包括超声波测距模块a8.1、超声波测距模块b8.2、超声波测距模块c8.3；所述主控制单元1包括
微处理器1.1、电机驱动1.2、
液晶显示1.3、惯导模块1.4、网络通讯模块1.5；
[0017] 主控制单元1设置在智能小车的中间处；
[0018] 电池2设置在主控制单元1的一端，为主控制单元1进行供电；
[0019] 电机3设置在智能小车的后
车轮驱动轴处；
[0020] 测量里程与速度的编码器4选用欧姆龙500线编码器，编码器4与电机3通过
齿轮连接。
[0021] 舵机5用以控制仿真小车在横向运动时的转向。使用该舵机5可以使智能小车顺利通过任意弯道。
[0022] RFID读卡器6遵循IOS14443协议，安装在智能小车的车头前方，通过RS-232串口与微处理器1.1相连接。
[0023] 激光测距模块7安装在智能小车的前端。
[0024] 超声波测距模块组8与微处理器1.1相连。在智能小车上共使用了3个超声波模块，即超声波测距模块a8.1、超声波测距模块b8.2、超声波测距模块c8.3，其中超声波测距模块a8.1、超声波测距模块c8.3分别设置在智能小车的左侧和右侧，超声波测距模块b8.2设置在智能小车的移动前方，超声波测距模块组8用以分别测量智能小车的前方、左侧、右侧距离。KS103S的I2C总线通信速率为50～100kbit/s，主控芯片的I2C通讯速率需要通过设置I2C Frequency Divider register使得通讯速率在50～100kbit/s之间。具体公式为：
[0025] I2C baud rate＝bus speed(Hz)/(mul×SCL divider) (1)
[0026] 微处理器1.1采用32位飞思卡尔
单片机，型号为MK60DN512ZVLQ。
[0027] 电机驱动1.2由两个BTS7960
芯片组成，BTS7960具有静态
电流低、
过热关机、过压，欠压保护等优点，非常适合作为微型车辆的电机驱动。
[0028] 液晶显示1.3为OLED屏幕。
[0029] 惯性导航模块1.4由ENC-03陀螺仪和MMA7361模拟加速度计组成。
[0030] 通讯模块1.5使用网蜂公司开发的Zigbee模块。
[0031] 一种用于智能交通物理仿真平台的定位方法，该方法包括下述流程，
[0032] S1智能小车定位与导航
[0033] S1.1二维平台下的惯性导航
[0034] 首先建立导航坐标系，以道路沙盘的左上
角为坐标原点，垂直方向为X轴，
水平方向为Y轴，竖直方向为Z轴。在建立导航坐标系的同时建立车载坐标系，以智能小车行进的方向为X轴，横向为Y轴，竖直方向为Z轴。惯性导航系统采用二维平面的捷联式惯性导航系统，由陀螺仪测量智能小车绕Z轴的
角速度ωZ，加速度计测量出X轴加速度aX，Y轴加速度aY。分别对这三个变量进行积分得到θZ，VX，VY。仿真车辆在导航坐标系下的位置由下列公式可以得出：
[0035]
[0036]
[0037] S1.2基于RFID的位置校正
[0038] 陀螺仪在使用中会产生累积误差。本发明通过RFID方法对θZ进行校正。
[0039] S1.2.1不使用RFID进行导航，当智能小车行驶到直道时，根据读取到的RIFD标签的UID查表获得此时刻车辆的位姿直接重置(X,Y,θZ)。
[0040] S1.2.2基于RFID的智能小车导航使用RFID进行导航时，在车道内智能小车行驶到每一排RFID标签时会读到其中一个RFID标签的UID，根据UID得到t时刻车辆的(Xt,Yt)坐标。再根据t-1时刻获取的坐标可以计算出智能小车的偏角 并根据此角度和t时刻(Xt,Yt)坐标重置(X,Y,θZ)。
[0041] 只要标签在读卡器的读卡范围内，车辆就能读取到标签信息，所以读取到的坐标信息只是车辆真实坐标的近似值。用带有不确定性的两组坐标计算车辆偏角 无法反映车辆真实的偏角。使用RFID方法导航时，如果车辆沿X方向行进，就将 近似为：
[0042] θ'Z＝KKd(Yt-Yt-1) (4)
[0043] θadjust＝-θ'Z (5)
[0044] 如果t时刻和t-1时刻的Y相同
[0045] θ'Z＝0 (6)
[0046] θadjust＝-Kd(Yt-Ycentral) (7)
[0047] 其中K为增益系数，Kd为方向系数，取值为[-1,1]，θadjust为智能小车舵机输入角度，智能小车在直道调整方向时，当车辆调整了一个合适的角度后就要让轮胎回正，为了模拟实际车辆的这一动作，利用电子陀螺仪实现了转角反馈，实现了车辆轮胎回正的效果。
[0048] 将计算出的偏角θadjust与电子陀螺仪检测的车身横摆角作差，输入到执行机构转化为前轮偏转角，并实时测量车身横摆角作为反馈。当车身横摆角与计算出的偏角完全抵消时，将两者清零准备下一次的角度调整，如果车辆进入弯道，则关闭此角度反馈。
[0049] 使用RFID方法在弯道进行导航，利用无线通讯设备可以获得仿真车辆在行驶中的里程和舵机转角。
[0050] 随着行驶里程的增加舵机角度的变化，行驶里程S与舵机角度θadjust的函数关系如下
[0051]
[0052] 函数的输入为行驶里程，与速度变量无关，所以只要车辆不出现
转向不足与
转向过度，那么速度无论如何变化，按照此函数获得舵机角度始终能顺利通过与函数相匹配的弯道。车辆在行驶中会偶尔漏读RFID标签，为了避免这种情况，根据弯道RFID标签摆放，当仿真车辆行驶到S0排时触发转向
算法并将
里程计清零，当仿真车辆行驶到S1，S2时也会触发转向算法，并分别将里程计设置为12cm，24cm。只要读取到S0，S1，S2其中一排的信息就可以触发转向算法，大大减小了因为漏读RFID标签而引起的不能顺利转弯的问题。又因为仿真车辆从直道驶向弯道时并不总是以垂直的角度进入弯道，会用微小的角度偏差，但是转向算法默认车辆是以垂直角度进入弯道的，所以要在弯道中利用RFID标签对
转向角度做出校正，确保仿真车辆顺利转弯。
[0053] 使用RFID在弯道进行导航的关键步骤为：
[0054] 1)预处理阶段使用
挡板让仿真车辆利用超声波测距拐弯并采集从拐弯开始到拐弯结束的行驶里程S与舵机角度θadjust的关系图。
[0055] 2)根据行驶里程S与舵机角度θadjust的关系图确定该弯道的行驶里程S与舵机角度θadjust函数。
[0056] 3)根据行驶里程S与舵机角度θadjust函数确定S0、S1、S2排RFID的位置，并设置RFID标签对转向角度的校正函数。
[0057] S2智能小车测距部分
[0058] S2.1 KS103S超声波模块测距
[0059] 在智能小车不使用RFID导航时，需要使用KS103S超声波模块测量左侧距离或右侧距离进行导航。在进行车车协同实验，例如车辆编队实验时，智能小车还需要使用KS103S超声波模块测量本车与前车的距离。因为三个超声波模块挂在一条I2C总线上，将三个超声波模块的I2C地址分别设置为：0X75、0X76、0X77。针对不同的测量范围KS103S超声波模块有不同的探测指令，根据具体需要，智能小车使用100cm量程的探测指令0X0A进行距离探测。
[0060] S2.2 GP3D03-ZZ4激光测距模块测距
[0061] 智能小车在道路沙盘上行驶中，使用超声波模块向前探测距离，但是其探测区域为
探头前方的矩形区域，探测前方障碍物或车辆时容易受到路侧设施的影响。因此，智能小车上不仅使用超声波模块探测距离，还使用激光测距模块探测前方距离。激光测距模块最快以10ms一次的速度，通过RS-232串口向控
制芯片发送一次输出值，然而输出值与探测距离不为线性关系。本发明使用BP神经网络训练得到输出值与探测距离的对应关系。
[0062] 使用BP神经网络训练的关键步骤为：
[0063] 1)初始化BP神经网络，选用
输入层-隐含层-
输出层典型3层BP结构，输入
节点设置为1个，
输出节点设置为1个，隐含层节点设置为8个。
[0064] 2)采集激光测距模块输出值与探测距离的对应关系，并将激光测距模块输出值作为BP神经网络的输入值，探测距离作为BP神经网络的输出值。
[0065] 3)开始进行BP神经网络训练，设置训练完成条件为误差小于等于0.1cm。
[0066] 在训练完成时保存BP神经网络的权值与
阈值。
[0067] 将训练好的BP神经网络下载到控制芯片内，当激光测距模块通过RS-232串口传回的数据就立即输入到训练好的BP神经网络中得到探测距离值。
[0068] S3无线通讯网络部分
[0069] 作为实验用的交通仿真平台，在平台上行驶的智能小车的运行参数如车辆瞬时速度、车身位姿等需要通过无线通讯网络被实时监控。此外，在进行车车协同、车路协同实验中也需要无线通讯网络使得车与车之间、车与路侧设施之间可以信息共享。考虑到无线通讯网络中的任意两个节点之间都有可能发生通讯，本发明使用Zigbee模块组成无线通讯网络。
[0070] Zigbee可以工作在三种模式下：协调器、路由器、终端。每一个由zigbee节点组成的无线网络中必须有一个协调器，它控制着其他节点的入网。基于zigbee协议的网络拓扑结构有三种：星形网络，树形网络和网状网络。
[0071] 网关(Getway)工作在协调器(coordinator)模式通过RS-232串口转USB连接到电脑上，节点1-5(no.1-no.5)工作在路由器模式通过RS-232串口连接到仿真车辆的控制芯片上。采集智能小车运行参数，车与车之间、车与路侧设施之间信息共享均采用点播方式传输信息，使用点播方式传输信息时关键在于如何获得对方节点的短地址。
[0072] Zigbee网络短地址管理的关键步骤为：
[0073] 1)在组网阶段节点加入网络成功时发送自身ID(1-5)到协调器。
[0074] 2)协调器在接受到的数据包中提取出ID号和短地址并存储到短地址列表中。
[0075] 3)协调器判断如果列表中的节点数大于2个则发送短地址列表到每个节点。
[0076] 4)协调器判断如果有新节点加入网络则更新所有节点中的短地址列表。
[0077] S4扩展部分
[0078] 真实环境下的无人驾驶
汽车一般具有视觉系统，这些视觉系统大多是为了检测交通标志、识别车道线等应用。为了在本交通仿真平台上验证
嵌入式系统上的视频处理方法，并开发基于视觉方法的导航系统，需要在仿真车辆上安装有彩色摄像头与视频处理单元。
[0079] QT因为优良的跨平台特性，逐渐受到越来越多开发人员的青睐，一次开发便可以在Windows、Linux两种平台上编译运行，而Linux
操作系统在实现嵌入式视频处理又提供了高效的管理。在Linux系统上运行QT的程序十分方便，并且在QT上可以使用OpenCV提供的丰富的视频
图像处理的函数，所以本发明为后续开发Linux系统下的视频处理程序提供
硬件支持。视频处理单元选择为三星的S3C6410芯片，视频处理单元与主控芯片之间通过RS-232串口连接。摄像头选为200万
像素高清OV2643，支持在嵌入式设备上使用，同时还有丰富的linux平台应用程序源码。
[0080] 与
现有技术相比，本发明具有如下有益效果。
[0081] 1、实时性好。现有的平台在定位时智能小车多将定位信息传输至电脑，再对定位信息进行集中式处理，然后由电脑将控制信息发送回智能小车，本平台使用智能小车搭载的嵌入式系统对本车的定位信息处理后直接对本车进行控制，节省了通讯时间。
[0082] 2、适应性强。在室内条件下，对于无线定位方法而言，无线信号易受到室内障碍物的干扰，存在多径效应，导致定位精度下降，甚至获取的定位信息无法采信；对于视频定位方法来说，复杂的沙盘环境，例如下层车道会受到高架的遮挡而产生定位盲区。本发明方法能够有效适应各种复杂环境。
[0083] 3、定位精度高。与无线定位方法相比，本发明方法能够有效的对智能小车进行精确定位。
[0084] 4、成本低廉，与视频定位所使用的高清摄像头上千元的成本相比，本发明使用的RFID读卡器与无源RFID标签十分廉价。
附图说明
[0085] 图1(a)交通沙盘下层平面图。
[0086] 图1(b)交通沙盘上层平面图。
[0087] 图1(c)智能车结构图。
[0088] 图2直道RFID标签摆放方式。
[0089] 图3弯道RFID标签摆放方式。
[0090] 图4十字路口RFID摆放方式。
[0091] 图5仿真车辆硬件结构图。
[0092] 图6角度反馈。
[0093] 图7(a)舵机转向角度与时间关系图。
[0094] 图7(b)行驶里程与时间关系图。
[0096] 图9 Zigbee无线通讯网络结构图。
[0097] 图10视觉系统硬件连接图。
[0098] 图11为主程序流程图。
[0099] 图12为PID增量程序图。
[0100] 图13为RFID流程图。
[0101] 图14为无线控制流程图。
[0102] 图15为超声波程序流程图。
[0103] 图中：10、智能小车，9、沙盘，1、主控制单元，2、电池，3、电机，4、编码器，5、舵机，6、RFID读卡器，7、激光测距模块，8、超声波测距模块组，8.1、超声波测距模块a，8.2、超声波测距模块b，8.3、超声波测距模块c，1.1、微处理器，1.2、电机驱动，1.3、液晶显示，1.4、惯导模块，1.5、网络通讯模块，9.1、上部沙盘，9.2、下部沙盘。
[0104] 具体实施方法
[0105] 如图1-15所示，一种用于智能交通物理仿真平台的智能小车及定位方法，本发明的整体系统结构如下。
[0106] 所述用于智能交通物理仿真平台的智能小车，该平台包括沙盘9与智能小车10；沙盘9包括上部沙盘9.1和下部沙盘9.2两部分；所述上部沙盘9.1设置有智能小车移动的环形车道，用以模拟城市快速路。
[0107] 所述下部沙盘9.2设置有十字路口，用以模拟城市交通路口。
[0108] 上部沙盘9.1与下部沙盘9.2通过两条对称的坡道连接。上部沙盘9.1与下部沙盘9.2的两层道路全覆盖有视频网络，用以实时监控智能小车的信号。
[0109] 所述上部沙盘9.1与下部沙盘9.2的路面中内嵌上遵循IOS14443协议的圆形RFID无源标签9.3，用以辅助定位并修正惯性导航的累积误差。
[0110] 使用RFID进行导航时，RFID标签摆放策略分为直道摆放策略与弯道摆放策略。直道摆放策略需要根据两个原则。第一，在直道行驶时，当舵机输出为0°，不能保证智能小车走的是一条直线。所示直道RFID标签的摆放要考虑如何抵消掉这部分偏角，使车辆始终在车道内行驶。第二，当智能小车从弯道驶出进入直道时偏角有可能过大。需要在直线部分行驶时具有一定的鲁棒性。
[0111] 智能小车在沙盘上行驶时，根据行驶路况不同使用两种导航模式；即如果行驶的道路两边任意一侧有
围栏或者隔离带，那么智能小车使用超声波测距导航模式，并沿围栏或者隔离带行驶；如果围栏或者隔离带与仿真车辆隔了一条道路，如下层十字路口双向三车道的中间车道，继续使用超声波测距沿围栏或者隔离带行驶会受到两侧车辆的影响，在这种情况下，智能小车使用RFID导航模式。
[0112] 不使用RFID进行导航时，RFID标签的功能是辅助定位并修正惯性导航，其摆放可以适当稀疏。在弯道时使用惯性导航进行定位，待智能小车从弯道驶出再根据RFID的信息对仿真车辆位姿进行校正。
[0113] 智能小车10包括主控制单元1、电池2，电机3，编码器4，舵机5，RFID读卡器6，激光测距模块7，超声波测距模块组8。超声波测距模块组8包括超声波测距模块a8.1、超声波测距模块b8.2、超声波测距模块c8.3；所述主控制单元1包括微处理器1.1、电机驱动1.2、液晶显示1.3、惯导模块1.4、网络通讯模块1.5；
[0114] 主控制单元1设置在智能小车的中间处；
[0115] 电池2设置在主控制单元1的一端，为主控制单元1进行供电；
[0116] 电机3设置在智能小车的后车轮驱动轴处；
[0117] 测量里程与速度的编码器4选用欧姆龙500线编码器，编码器4与电机3通过齿轮连接，电机
定子转动会带动编码器的定子转动，编码器定子转动一圈产生500个计数脉冲以供测量，通过控制芯片对此脉冲计数，可以计算出里程与速度。
[0118] 舵机5用以控制仿真小车在横向运动时的转向。使用该舵机5可以使智能小车顺利通过任意弯道。
[0119] RFID读卡器6遵循IOS14443协议，安装在智能小车的车头前方，通过RS-232串口与微处理器1.1相连接。
[0120] 激光测距模块7安装在智能小车的前端；激光测距模块7采用的型号为GP3D03-ZZ4，探测范围在5cm～300cm之间，并且在10cm内有着超高精度，可以检测到极为微小的距离变化，输出数据与探测距离为二次函数关系。在智能小车上使用一个GP3D03-ZZ4激光测距模块，用来测量前方距离，GP3D03-ZZ4与微处理器1.1通过RS-232串口相连接。
[0121] 超声波测距模块组8使用KS103S高精度探测模块，最大探测范围在1cm～1000cm之间，输出为探测时间，可使用I2C
接口与微处理器1.1相连。在智能小车上共使用了3个KS103S超声波模块，即超声波测距模块a8.1、超声波测距模块b8.2、超声波测距模块c8.3，其中超声波测距模块a8.1、超声波测距模块c8.3分别设置在智能小车的左侧和右侧，超声波测距模块b8.2设置在智能小车的移动前方，超声波测距模块组8用以分别测量智能小车的前方、左侧、右侧距离，3个KS103S模块一起挂在一条I2C总线上与微处理器1.1连接。KS103S的I2C总线通信速率为50～100kbit/s，主控芯片的I2C通讯速率需要通过设置I2C FrequencyDividerregister使得通讯速率在50～100kbit/s之间。具体公式为：
[0122] I2C baud rate＝bus speed(Hz)/(mul×SCL divider) (1)
[0123] 微处理器1.1采用32位飞思卡尔单片机，型号为MK60DN512ZVLQ。电机驱动1.2由两个BTS7960芯片组成，BTS7960具有静态电流低、过热关机、过压，欠压保护等优点，非常适合作为微型车辆的电机驱动。
[0124] 液晶显示1.3为OLED屏幕。
[0125] 惯性导航模块1.4由ENC-03陀螺仪和MMA7361模拟加速度计组成。其中ENC-03为微型压电陀螺仪，根据科氏力原理，输出一个与角速度成正比的模拟
电压信号，可测量的最大角速度为±300[deg/s]，当角速度为零时
输出电压为1.35V。MMA7361为高微型电容式加速度传感器，其采用了信号调理、单级低通
滤波器和
温度补偿方法，输出为三轴加速度的模拟量，量程选择为0～6g。惯性导航模块与控制芯片通过AD模数转化连接，惯性导航模块1.4安装在智能小车的几何中心。
[0126] 通讯模块1.5使用网蜂公司开发的Zigbee模块，该模块使用CC2530芯片，CC2530使用增强型的8051CPU，具有片内可编程闪存，使用Z-Stack协议栈可以快速开发出符合此交通仿真平台的无线通讯网络。
[0127] 一种用于智能交通物理仿真平台的定位方法，该方法包括下述流程，
[0128] S1智能小车定位与导航
[0129] S1.1二维平台下的惯性导航
[0130] 首先建立导航坐标系，以道路沙盘的左上角为坐标原点，垂直方向为X轴，水平方向为Y轴，竖直方向为Z轴。在建立导航坐标系的同时建立车载坐标系，以智能小车行进的方向为X轴，横向为Y轴，竖直方向为Z轴。惯性导航系统采用二维平面的捷联式惯性导航系统，由陀螺仪测量智能小车绕Z轴的角速度ωZ，加速度计测量出X轴加速度aX，Y轴加速度aY。分别对这三个变量进行积分得到θZ，VX，VY。仿真车辆在导航坐标系下的位置由下列公式可以得出：
[0131]
[0132]
[0133] S1.2基于RFID的位置校正
[0134] 陀螺仪在使用中会产生累积误差。本发明通过RFID方法对θZ进行校正。
[0135] S1.2.1不使用RFID进行导航，当智能小车行驶到直道时，根据读取到的RIFD标签的UID查表获得此时刻车辆的位姿直接重置(X,Y,θZ)。
[0136] S1.2.2基于RFID的智能小车导航使用RFID进行导航时，在车道内智能小车行驶到每一排RFID标签时会读到其中一个RFID标签的UID，根据UID得到t时刻车辆的(Xt,Yt)坐标。再根据t-1时刻获取的坐标可以计算出智能小车的偏角 并根据此角度和t时刻(Xt,Yt)坐标重置(X,Y,θZ)。
[0137] 只要标签在读卡器的读卡范围内，车辆就能读取到标签信息，所以读取到的坐标信息只是车辆真实坐标的近似值。用带有不确定性的两组坐标计算车辆偏角 无法反映车辆真实的偏角。使用RFID方法导航时，如果车辆沿X方向行进，就将 近似为：
[0138] θ'Z＝KKd(Yt-Yt-1) (4)
[0139] θadjust＝-θ'Z (5)
[0140] 如果t时刻和t-1时刻的Y相同
[0141] θ'Z＝0 (6)
[0142] θadjust＝-Kd(Yt-Ycentral) (7)
[0143] 其中K为增益系数，Kd为方向系数，取值为[-1,1]，θadjust为智能小车舵机输入角度，智能小车在直道调整方向时，当车辆调整了一个合适的角度后就要让轮胎回正，为了模拟实际车辆的这一动作，利用电子陀螺仪实现了转角反馈，实现了车辆轮胎回正的效果。
[0144] 将计算出的偏角θadjust与电子陀螺仪检测的车身横摆角作差，输入到执行机构转化为前轮偏转角，并实时测量车身横摆角作为反馈。当车身横摆角与计算出的偏角完全抵消时，将两者清零准备下一次的角度调整，如果车辆进入弯道，则关闭此角度反馈。
[0145] 使用RFID方法在弯道进行导航，利用无线通讯设备可以获得仿真车辆在行驶中的里程和舵机转角。在上层单向两车道快速路中，当车辆顺利通过弯道时，仿真车辆行驶里程和舵机转角信息如图7所示：
[0146] 图7(a)曲线表示舵机转向角度与时间关系，图7(b)表示行驶里程与时间关系，每当进入弯道之前利用RFID将里程计清零。从图中可以看到随着行驶里程的增加舵机角度的变化，得到行驶里程S与舵机角度θadjust的函数
[0147]
[0148] 函数的输入为行驶里程，与速度变量无关，所以只要车辆不出现转向不足与转向过度，那么速度无论如何变化，按照此函数获得舵机角度始终能顺利通过与函数相匹配的弯道。车辆在行驶中会偶尔漏读RFID标签，为了避免这种情况，根据弯道RFID标签摆放示意图，当仿真车辆行驶到S0排时触发转向算法并将里程计清零，当仿真车辆行驶到S1，S2时也会触发转向算法，并分别将里程计设置为12cm，24cm。只要读取到S0，S1，S2其中一排的信息就可以触发转向算法，大大减小了因为漏读RFID标签而引起的不能顺利转弯的问题。又因为仿真车辆从直道驶向弯道时并不总是以垂直的角度进入弯道，会用微小的角度偏差，但是转向算法默认车辆是以垂直角度进入弯道的，所以要在弯道中利用RFID标签对转向角度做出校正，确保仿真车辆顺利转弯。
[0149] 使用RFID在弯道进行导航的关键步骤为：
[0150] 1)预处理阶段使用挡板让仿真车辆利用超声波测距拐弯并采集从拐弯开始到拐弯结束的行驶里程S与舵机角度θadjust的关系图。
[0151] 2)根据行驶里程S与舵机角度θadjust的关系图确定该弯道的行驶里程S与舵机角度θadjust函数。
[0152] 3)根据行驶里程S与舵机角度θadjust函数确定S0、S1、S2排RFID的位置，并设置RFID标签对转向角度的校正函数。
[0153] S2智能小车测距部分
[0154] S2.1 KS103S超声波模块测距
[0155] 在智能小车不使用RFID导航时，需要使用KS103S超声波模块测量左侧距离或右侧距离进行导航。在进行车车协同实验，例如车辆编队实验时，智能小车还需要使用KS103S超声波模块测量本车与前车的距离。因为三个超声波模块挂在一条I2C总线上，将三个超声波模块的I2C地址分别设置为：0X75、0X76、0X77。针对不同的测量范围KS103S超声波模块有不同的探测指令，根据具体需要，智能小车使用100cm量程的探测指令0X0A进行距离探测，测距程序流程如图8所示。
[0156] S2.2 GP3D03-ZZ4激光测距模块测距
[0157] 智能小车在道路沙盘上行驶中，使用超声波模块向前探测距离，但是其探测区域为探头前方的矩形区域，探测前方障碍物或车辆时容易受到路侧设施的影响。因此，智能小车上不仅使用超声波模块探测距离，还使用激光测距模块探测前方距离。激光测距模块最快以10ms一次的速度，通过RS-232串口向控制芯片发送一次输出值，然而输出值与探测距离不为线性关系。本发明使用BP神经网络训练得到输出值与探测距离的对应关系。
[0158] 使用BP神经网络训练的关键步骤为：
[0159] 1)初始化BP神经网络，选用输入层-隐含层-输出层典型3层BP结构，输入节点设置为1个，输出节点设置为1个，隐含层节点设置为8个。
[0160] 2)采集激光测距模块输出值与探测距离的对应关系，并将激光测距模块输出值作为BP神经网络的输入值，探测距离作为BP神经网络的输出值。
[0161] 3)开始进行BP神经网络训练，设置训练完成条件为误差小于等于0.1cm。
[0162] 在训练完成时保存BP神经网络的权值与阈值。
[0163] 将训练好的BP神经网络下载到控制芯片内，当激光测距模块通过RS-232串口传回的数据就立即输入到训练好的BP神经网络中得到探测距离值。
[0164] S3无线通讯网络部分
[0165] 作为实验用的交通仿真平台，在平台上行驶的智能小车的运行参数如车辆瞬时速度、车身位姿等需要通过无线通讯网络被实时监控。此外，在进行车车协同、车路协同实验中也需要无线通讯网络使得车与车之间、车与路侧设施之间可以信息共享。考虑到无线通讯网络中的任意两个节点之间都有可能发生通讯，本发明使用Zigbee模块组成无线通讯网络。
[0166] Zigbee可以工作在三种模式下：协调器、路由器、终端。每一个由zigbee节点组成的无线网络中必须有一个协调器，它控制着其他节点的入网。基于zigbee协议的网络拓扑结构有三种：星形网络，树形网络和网状网络。
[0167] 在网状拓扑结构下使用Zigbee模块组成无线通讯网络如图9所示。
[0168] 网关(Getway)工作在协调器(coordinator)模式通过RS-232串口转USB连接到电脑上，节点1-5(no.1-no.5)工作在路由器模式通过RS-232串口连接到仿真车辆的控制芯片上。采集智能小车运行参数，车与车之间、车与路侧设施之间信息共享均采用点播方式传输信息，使用点播方式传输信息时关键在于如何获得对方节点的短地址。
[0169] Zigbee网络短地址管理的关键步骤为：
[0170] 1)在组网阶段节点加入网络成功时发送自身ID(1-5)到协调器。
[0171] 2)协调器在接受到的数据包中提取出ID号和短地址并存储到短地址列表中。
[0172] 3)协调器判断如果列表中的节点数大于2个则发送短地址列表到每个节点。
[0173] 4)协调器判断如果有新节点加入网络则更新所有节点中的短地址列表。
[0174] S4扩展部分
[0175] 真实环境下的无人驾驶汽车一般具有视觉系统，这些视觉系统大多是为了检测交通标志、识别车道线等应用。为了在本交通仿真平台上验证嵌入式系统上的视频处理方法，并开发基于视觉方法的导航系统，需要在仿真车辆上安装有彩色摄像头与视频处理单元。
[0176] QT因为优良的跨平台特性，逐渐受到越来越多开发人员的青睐，一次开发便可以在Windows、Linux两种平台上编译运行，而Linux操作系统在实现嵌入式视频处理又提供了高效的管理。在Linux系统上运行QT的程序十分方便，并且在QT上可以使用OpenCV提供的丰富的视频图像处理的函数，所以本发明为后续开发Linux系统下的视频处理程序提供硬件支持。视频处理单元选择为三星的S3C6410芯片，视频处理单元与主控芯片之间通过RS-232串口连接。摄像头选为200万像素高清OV2643，支持在嵌入式设备上使用，同时还有丰富的linux平台应用程序源码。
