基于移动机器人的V2X室内仿真平台

马小陆,胡佳伟

(安徽工业大学 电气信息学院,安徽 马鞍山 243002)

车联网技术的出现,为降低交通事故发生频率提供了新的解决方案。V2X作为车联网的核心技术[1],基于车到车(Vehicle to Vehicle,V2V)、车到人(Vehicle to Pedestrian,V2P)、车到基础设施(Vehicle to Infrastructure,V2I)和车到网络(Vehicle to Network,V2N)等之间的互联通信,可以共享车辆之间的信息,扩大车辆的感知范围,及时做出预警等,能够为用户提供安全、舒适、高效的驾驶感受。V2X技术一般先采用软件仿真平台模拟V2X场景进行测试,再进行实车测试验证。胡艳等[2]利用 Microsoft Visual Studio C++构建了城市典型的 LOS场景,模拟端到端的数据传输系统仿真平台。王晶晶等[3]基于 PreScan、CarSim 和 MATLAB 联合仿真平台,搭建了行人主动避撞场景。 Wang等[4]提出了一种结合虚拟和真实环境的end-to-end 仿真测试系统。虽然上述仿真平台都能够对各自的研究课题进行相关测试验证,但参数都是预先设定的,具有一定局限性,无法保证与实车测试一致。V2X实车测试中存在场景搭建复杂、测试成本高、场地易受干扰和测试效率低等问题,且在复杂场景下测试,危险系数也会增加[5-6]。

针对上述问题,本文提出一种基于移动机器人的V2X室内仿真平台。该仿真平台使用移动机器人替代车辆和GNSS,将V2X设备通过以太网口与移动机器人相连,并将移动机器人在map坐标系中的坐标、偏航角和速度等信息转换为车辆对应的GNSS经纬度和方向角等信息,然后发送至V2X设备,终端显示移动机器人的实时位置和安全预警信息。

1 总体架构设计

1.1 基于车辆的V2X系统

基于车辆的V2X系统总体架构如图1所示。

图1 基于车辆的V2X系统总体架构

基于车辆的V2X系统主要包括输入端、V2X设备端和输出端。V2X作为系统核心单元,通过CAN和UART从输入端获取本车的状态信息以及经纬度、速度和方向角等信息,记为本车BSM信息;通过LTE-V获取他车BSM信息。数据处理单元依据两车的位置关系等信息进行算法处理,将安全预警信息展示在显示终端。同时,V2X设备通过WIFI将本车和他车BMS信息发送至显示终端,并通过LTE-V将本车BSM信息发送至他车。

1.2 基于移动机器人的V2X系统

基于移动机器人的V2X系统总体架构如图2所示。从图2可以看出,图1中的车辆和GNSS模块被移动机器人替代,且通信接口由CAN和UART 2个接口统一为1个网口;图1中使用车辆和GNSS可以分别获取车辆状态以及车辆经纬度、方向角、速度等信息,而图2则是通过移动机器人间接获取上述信息。

图2 基于移动机器人的V2X系统总体架构

2 map坐标系信息转换

V2X运行过程中需要实时获取车辆的经纬度、方向角和速度等关键信息。而移动机器人的位置和方向角信息则是以map坐标系中的x-y坐标形式和偏航角形式存在。因此,实现移动机器人同时代替车辆和GNSS的关键是将移动机器人map坐标系信息进行转换。

2.1 x-y坐标转换

已知A点经度为lon1、纬度为lat1,B点经度为lon2、纬度为lat2,均由弧度表示。使用半正矢公式计算A、B两点的距离[7],即：

d=R·Δσ

(1)

(2)

式(1)和(2)中,d为两点的距离;R为地球半径;Δσ为两点到球心直线之间的夹角。

设A和B两点经纬度坐标分别为(λ1,φ1)和(λ2,φ2),则角度转换弧度公式为：

lon1-lon2=π(λ1-λ2)/180

(3)

lat1-lat2=π(φ1-φ2)/180

(4)

联立公式(1)～(4),即可求出A和B两点间的距离。经纬度差值对应关系见表1。

表1 经纬度差值对应关系

假设机器人所在的map坐标系中,其x-y坐标值范围为x(-a,a)和y(-b,b)(如图3所示),其中a和b均为double类型数据。X轴方向两顶点坐标相距数值为2a,设定两顶点之间有n个单位长度,定义每个单位长度为1。经过数据比对,取表1中的第2组数据,设定坐标度数差值为0.000 1,即每个单位长度为0.000 1°,则两顶点坐标相距度数为0.000 2a。假定X坐标是经度,Y坐标是纬度,则X轴方向两顶点坐标相距20am。Y轴方向两顶点坐标参照X轴方向进行类推,则Y轴方向两顶点坐标相距22bm。故机器人在map中的矩形线路可转换为V2X地图中所使用的标准地图,即20a×22b的矩形路线。综上,本文取坐标数值的前4位即可满足要求,具体转换公式如下：

图3 map坐标系及坐标点示意图

lon=(107base_lon+103x)/107

(5)

lat=(107base_lat+103y)/107

(6)

式(5)和(6)中,base_lon和base_lat均表示基值,即V2X测试场景中所设定区域的任意1个GNSS经纬度坐标点。x-y坐标转换GNSS经纬度效果示意图如图4所示。

图4 x-y坐标转换GNSS经纬度效果示意图

2.2 偏航角转换

V2X所需要的方向角信息是从GNSS数据中解析出的车辆行驶方向与正北(North)方向的夹角,并且顺时针方向从0至359.999°递增。机器人的偏航角绕Z轴旋转,运动方向与X正半轴重合时的偏航角为0;与X负半轴重合时为π rad;与Y正半轴重合时为(π/2) rad;与Y负半轴重合为-(π/2) rad。为了将移动机器人的偏航角转换为车辆的方向角,需要将机器人的偏航角坐标映射到方向角坐标上。设定偏航角坐标系的X和Y轴分别映射至方向角坐标系中的E和N方向。偏航角和方向角坐标映射关系如图5所示。

图5 偏航角和方向角坐标映射关系

将偏航角坐标系中XY坐标按照象限分类,进行数学推导。在第二象限时：

(7)

在第一、三、四象限时：

(8)

公式(7)～(8)中,Heading为方向角度数值;Yaw为偏航角弧度值。

3 V2X前向碰撞场景设计

3.1 前向碰撞原理

基于V2X中的车车(V2V)通信,当主车(HV)在车道上行驶,与在前方同一车道的前车(RV)存在追尾碰撞危险时,前向碰撞预警算法会通过预警方式提醒主车驾驶员避免追尾事故的发生。车辆在行驶过程中,主车一方面获取自身的经纬度、方向角和速度等BSM信息,另一方面通过V2X设备内置的LTE-V通信模块与周围装有同样设备的其他车辆共享各自的BSM信息。主车根据接收到的他车BSM信息,首先计算两车方向角差值,判断出两车相对行驶方向,计算公式为：

(9)

式(9)中,Dir_H和Dir_R分别为本车和他车的行驶方向角;AngleDif为RV到HV的顺时针方向角角度差值。根据AngleDif的值,即可判断出RV相对于HV的行驶方向。两车相对行驶方向关系见表2,其中,本文默认α初值为10°。

表2 两车相对行驶方向关系

其次,根据两车的经纬度,计算出RV相对于HV的方位角,根据HV方向角与方位角的差值(θ)即可判断出两车的相对方位关系。

方位角是指以HV为坐标系,从HV的正北方向定义为0°起,顺时针旋转与目标方向线的夹角。方位角计算示意图如图6所示。

图6 方位角计算示意图

设地球的球心为O,A、B两点分别表示HV和RV所在的位置。A、B两点经纬度坐标分别为(lon_a,lat_a),(lon_b,lat_b)。其中,C点与A点处于同纬度,与B点处于同经度,C点经纬度坐标为(lon_a,lat_b)。地球长半轴为src,短半轴为srj,则OB和OC的分别为：

OB=srj+(src-srj)(90-lat_a)/90

(10)

OC=OBcos(lat_aπ/180)

(11)

根据弧度计算公式,dx和dy分别为：

dx=OC·β·π/180

(12)

dy=OB·α·π/180

(13)

α=lat_b-lat_a

(14)

β=lon_b-lon_a

(15)

根据反正切函数关系式,代入dx和dy,近似计算出方位角azi_angle为：

azi_angle=arctan(dx/dy)

(16)

再根据方位角和HV行驶方向角的值,计算出HV的行驶方向和RV相对于HV的方位角之间的夹角(θ),公式为：

θ=DirAngle_H-azi_angle

(17)

根据θ的大小,即可判断出RV与HV的相对方位。

最后,先根据公式(1)计算两车距离(D),再结合θ算出两车的横向距离Lat_D=D·sinθ,进而判断两车的相对位置。

车辆相对方位关系见表3。两车车道关系见表4。两车横向距离计算示意图如图7所示。表4中,sa_limit和ad_limit均是设置的阈值,参考我国公路的车道宽度和汽车车身标准等因素,默认初值分别为1.5和4.8 m。

表3 车辆相对方位关系

表4 两车车道关系

图7 两车横向距离计算示意图

综上,根据上述三部分算法处理,即可得出RV相对于HV的位置。如两车同向行驶,RV相对于HV的方位是左前方或右前方,且两车处于相同车道,则RV处于HV正前方。

当筛选出处于HV正前方车RV时,根据两车的速度和加速度信息,计算两车的碰撞时间(t),并根据t划分安全等级为0、1、2(见表5)。当安全等级为0或1时,前向碰撞预警算法将发出预警信息。前向碰撞算法程序流程图如图8所示。

表5 安全等级划分

图8 前向碰撞场景算法程序流程图

若主车与前车加速度接近,但主车速度大于前车,则两车存在碰撞风险,碰撞时间为：

t=s/(vh-vr)

(18)

若主车与前车加速度差值较大,则：

s=v0t+at2/2

(19)

在具体应用中,v0=vh-vr,a=ah-ar。若方程(19)有解,设解为t1、t2。若t1、t2均大于0,则取最小值为碰撞时间;若t1、t2其中一个大于0,另一个不大于0,则取大于0的值为碰撞时间;若t1,t2均不大于0,则不会碰撞。

3.2 场景设计

分别使用移动机器人Robot1和Robot2替代RV和HV,模拟车辆道路行驶。机器人运行过程中,ROS自身会对外广播map至base_link的TF变换和发布/cmd_vel话题。机器人内置的数据处理模块通过监听TF变换和订阅/cmd_vel话题的方式获取2台机器人的坐标、偏航角和速度等信息,然后将数据进行滤波和校准处理,转换为GNSS数据类型,发送至各自V2X设备。V2X设备获取到2台机器人的相关数据信息后,执行前向碰撞应用场景算法。若存在前向碰撞风险,则会发出预警信息至显示终端。V2X前向碰撞场景原理框图如图9所示。

图9 V2X前向碰撞场景原理框图

机器人内置数据处理模块程序流程如图10所示。程序运行时,执行步骤如下：① 分别订阅移动机器人平台robot1/cmd_vel和robot2/cmd_vel话题,定义监听器及存放坐标变换的变量;②创建socket服务通信,等待客户端连接;③通过监听map和base_link之间的坐标转换,实时获取机器人在map中的坐标和偏航角,通过订阅/cmd_vel话题,获取速度;④对获取到的数据进行滤波、校准处理;⑤经处理后,将机器人坐标值转换为GPS经纬度,将偏航角转换为方向角;⑥将处理过的数据按字节以整数类型存放入BUF缓冲中,等待发送;⑦使用TCP/IP发送BUF中的数据至V2X设备。

图10 机器人内置数据处理模块程序流程

4 测试验证

4.1 测试平台

为了验证该仿真平台的可行性,搭建了V2X前向碰撞场景硬件在环测试平台。测试平台拓扑图如图11所示。

图11 测试平台拓扑图

该测试平台包括基于ROS的室内移动机器人仿真平台、1台交换机、2台V2X设备以及显示终端。其中,交换机用来构建实验平台局域网络,基于ROS的室内移动机器人仿真平台作为服务器,V2X设备作为客户端。服务器发送2台机器人的实时位置、方向角和速度信息, V2X-1接收Robot1的信息,V2X-2接收Robot2的信息,并通过LTE-V进行信息交互,同时V2X-2设备会通过WIFI将2台机器人的位置信息上传至显示终端。

4.2 测试方案

在Ubuntu中运行基于ROS的室内移动机器人仿真环境如图12所示。在该工作空间目录下,创建机器人运行控制功能包,为每个机器人设置多个目标点,控制机器人运动。当前方机器人Robot1速度降低或停止时,后方机器人Robot2会存在前向碰撞风险,以此来模拟追尾场景。另外创建数据处理模块的功能包,用于获取2台移动机器人的坐标、速度、偏航角等数据,处理后将数据转换为 GNSS数据类型,然后发送至V2X设备。

图12 基于ROS的室内移动机器人仿真环境

测试平台的具体步骤如下：①运行ROS仿真环境,为机器人进行自主导航做准备;②运行机器人内置的数据处理模块软件,建立socket服务器,并实时获取机器人的坐标、速度、偏航角等信息,经处理后通过TCP/IP发送至V2X;③运行V2X中的相关程序,建立与机器人仿真平台socket服务器的连接,接收机器人仿真平台发送的数据;④运行显示终端程序,通过WIFI接收V2X终端发送的机器人BSM信息,显示终端将会在路测地图上显示出机器人位置;⑤运行机器人运动控制功能包,使2台机器人一前一后进行多目标点巡航,模拟一个碰撞场景;⑥2台V2X设备通过内置LTE-V模块进行信息交互,接收对方发送的其他机器人BSM信息,当Robot1在Robot2正前方,Robot2速度大于Robot1,且计算两者碰撞时间小于5 s时,Robot2会触发前向碰撞预警,随着碰撞时间缩短至3 s以内时,会触发严重前向碰撞风险;⑦预警信息通过V2X内置的WIFI模块发送至Robot2的显示终端。

4.3 测试结果

设定Robot1先移动,Robot2进行追随,为验证V2X室内仿真平台的有效性和普遍适用性,分别进行两组实验。机器人的运动轨迹如图13所示。由图13可知,两机器人初始位置分别在Robot1和Robot2处,均可在实时监控条件下完成顺时针和逆时针方向移动。

(a) 顺时针 (b) 逆时针

1)第1组实验为顺时针方向移动。2台机器人不同时刻的位置如图14所示。由前向碰撞原理可知,图14(a)中两机器人相对距离较大,则此时可判定安全等级为2。图14(b)为两机器人接近各自目标点的位置图。当Robot1停止至目标点, Robot2运行到位置1处时,存在前向碰撞的风险,即预警算法检测到预计碰撞时间在5 s内,则可判定安全等级为1;当Robot2运行到位置2处时,认为存在前向碰撞严重风险,即预警算法检测到预计碰撞时间小于等于3 s,则可判定安全等级为0。

(a) 时刻a (b) 时刻b

两机器人向各自目标点前进过程中,第1组实验Robot2的终端显示画面如图15所示。图15(a)显示两车方向角均大约为270°,即处于同一车道,但相对距离较大,此时可判定安全等级为2;图15(b)显示两车方向角均大约为0,但相对距离较近,终端显示画面中,前车颜色变为橘黄色并闪烁,且伴有报警声,此时可判定安全等级为1;图15(c)显示两车方向角仍大约为0°,然而接近碰撞距离,终端显示画面中,前车颜色变为红色并闪烁,且伴有报警声,此时可判定安全等级为0。

(a) 安全等级为2 (b) 安全等级为1 (c) 安全等级为0

移动机器人偏航角转换后的方向角及线速度变化如图16所示。由图16(a)和(b)可知,Robot1在120 s左右到达了目标点,Robot2在150 s左右达到了目标点。Robot1和Robot2分别在33 s和60 s进行第1次变道,此时方向角大约由90°变为180°左右,而后,于55 s和85 s分别进行第2次变道,此时方向角大约由180°变为270°左右,最后1次变道分别于100 s和132 s开始,此时方向角在0°左右徘徊。由图16(c)和16(d)可知, Robot1和Robot2分别在7 s和16 s开始运动,速度大约保持为7 km/h,当机器人变道时,速度为0。值得注意的是,Robot1于120 s停止,则此时Robot2运动过程中,需根据前向碰撞原理判定有无碰撞风险,并将碰撞风险预警在终端显示画面中予以显示。

(a) Robot1方向角 (b) Robot2方向角 (c) Robot1线速度 (d) Robot2线速度

2)第2组实验为逆时针方向移动。两机器人向各自目标点前进过程中,第2组实验Robot2的终端显示画面如图17所示。与第1组实验分析类似,根据两机器人相对距离判定图17(a)中两车安全等级为2,图17(b)两车安全等级为1,图17(c)两车安全等级为0。

(a) 安全等级为2 (b) 安全等级为1 (c) 安全等级为0

移动机器人逆时针运动偏航角转换后的方向角及线速度变化如图18所示。由图18(a)和18(b)可知,Robot1在115 s左右到达了目标点,Robot2在133 s左右到达了目标点。Robot1和Robot2分别大约在33 s和55 s进行第1次变道,此时方向角大约由90°变为0°(或359.999°)左右,而后于53 s和80 s分别进行第2次变道,此时方向角大约由0°(或359.999°)变为270°左右。由图18(c)和18(d)可知, Robot1和Robot2分别大约在4 s和5 s开始运动,速度仍大约保持在7 km/h,当机器人变道时,此时速度为0。值得注意的是,Robot1于95 s左右停止,而Robot2仍未到达目标点,那么在运动过程中,会根据前向碰撞原理判定有无碰撞风险,并将碰撞风险预警在终端显示画面中予以显示。

(a) Robot1方向角 (b) Robot2方向角 (c) Robot1线速度 (d) Robot2线速度

5 结论

提出基于移动机器人的V2X室内实验平台,搭建了基于移动机器人的V2X前向碰撞应用场景的硬件在环仿真测试平台并进行实验验证。本方案效率高,操作相对简便,测试结果也完全符合预期,能够躲避软件仿真测试和实车测试过程中的弊端,证实了该V2X室内实验平台是一种可行有效的V2X仿真平台方案,同时也为V2X技术进一步的发展拓展了思路。