一种基于车路云一体化的车辆航向角计算方法及装置

1.本发明涉及智能车辆环境感知技术领域，尤其是一种基于车路云一体化的车辆航向角计算方法及装置。


背景技术：

2.无人驾驶汽车在行驶过程中,需要确定当前的航向角,并根据该航向角控制车辆的行驶方向。其中,该航向角是指大地坐标系下车辆的行驶方向与大地坐标系中的横轴的夹角,该横轴的正方向一般为正北方向。
3.相关技术中,无人驾驶汽车可以通过gnss观测信号,gnss的全称是全球导航卫星系统(global navigation satellite system)，它是泛指所有的卫星导航系统，包括全球的、区域的和增强的，如美国的gps、俄罗斯的glonass、欧洲的galileo、中国的北斗卫星导航系统，以及相关的增强系统并根据该信号确定当前的航向角。gnss卫星导航具有长期精度稳定，导航误差不随时间发散等优点，缺点在于gnss观测信号易受环境干扰和遮挡。当无人驾驶汽车周围环境中存在遮挡或受到环境干扰时,其接收到的gnss信号较弱,无法准确根据该信号确定车辆航向角。所以，随着智能驾驶技术的发展，研发一种对传感器和使用场景要求低，不完全依赖于gnss设备，不完全依赖于卫星信号好，计算航向角的稳定性好的车辆航向角的计算装置和方法是现有技术需要解决的问题。


技术实现要素：

4.本发明的首要目的在于提供一种能后稳定记录输出车辆位置和姿态，大大提高组合导航的精度，增加车辆姿态控制的应用场景的基于车路云一体化的车辆航向角计算方法。
5.为实现上述目的，本发明采用了以下技术方案：一种基于车路云一体化的车辆航向角计算方法，该方法包括下列顺序的步骤：
6.(1)通过gnss模块判断卫星信号的强弱，若判断卫星信号强，则由车辆控制器模块直接对车辆航向角进行修正，若判断卫星信号弱，则进入步骤(2)；
7.(2)由路侧摄像头模块采集车辆周围环境信息，通过路侧计算感知单元对车辆周围环境信息进行处理来判断车辆是否静止；
8.(3)若判断车辆为静止，则提取或识别航向角信息；
9.(4)若判断车辆不静止，则在车辆行驶时利用车辆轨迹对航向角进行修正；
10.(5)当车辆在行驶中停止，由路侧感知计算单元模块临时记忆当前车辆的航向角；
11.(6)当车辆下电时，路侧感知计算单元就通过第一5g通信终端模块将当前的车辆航向角信息存储到云端的存储模块中；
12.(7)当车辆再次上电启动时，路侧感知计算单元模块提取云端的存储模块中该车辆存储的航向角。
13.在所述步骤(1)中，利用载波噪声比来判断卫星信号的强弱，gnss模块接收的载波
噪声比的值为35至55dbhz，其中，大于40dbhz的视为卫星信号强，小于40dbhz的视为卫星信号弱。
14.在所述步骤(2)中，在路侧端，路侧摄像头模块拍摄目标车辆周围的图像，并将这些信息传送给路侧感知计算单元模块，路侧感知计算单元模块利用帧间差分法来判断车辆是否处于静止状态，帧间差分法的数学公式如下：
[0015][0016]
其中，d(x,y)为连续两帧图像之间的差分图像，i(t)和i(t-1)分别为t和t-1时刻的图像，t为差分图像二值化时选取的阈值，d(x,y)＝1表示前景，车辆是运动的，d(x,y)＝0表示背景，车辆是静止的。
[0017]
所述步骤(3)具体是指：若判断车辆为静止，由路侧感知计算单元模块判断云端的存储模块中是否有已经存储的航向角信息，如果有已经存储的航向角信息，则提取该航向角信息作为车辆的初步航向角；如果云端的存储模块中未存储航向角信息，则将位于目标车辆前后的车道中心线上的两点与高精地图中的信息进行对比得到两点经纬度坐标，通过两点的经纬度坐标计算得出车辆静止状态下的航向角信息；高精地图显示不同的车道中各个点的经纬度坐标，路侧摄像头模块拍摄车辆在道路上的图像，随后路侧感知计算单元模块利用yolov3算法识别出图像中的目标车辆并标记出其矩形识别框，矩形识别框与目标车辆所在地面是垂直的关系，路侧感知计算单元模块将矩形识别框的下边缘中心点与高精地图中的信息进行对比，得到矩形识别框的下边缘中心点的经纬度坐标，根据该经纬度坐标与各车道的经纬度范围分别进行比较，判断出目标车辆具体位于哪一个车道中；车辆摄像头模块拍摄该车道上的图像，车辆视觉处理单元模块通过图像的灰度化、canny边缘检测和霍夫变换检测到图像中的车道中心线，将位于车辆前方最近处的车道中心线上的点和位于车辆后方最近处的车道中心线上的点与高精地图中的信息进行对比，得到车辆前方最近处的车道中心线上的点，即a点的经纬度坐标(aj，aw)，以及车辆后方最近处的车道中心线上的点，即b点的经纬度坐标(bj，bw)，根据极坐标法计算a、b两点所连成的直线与正北方向的夹角，先将a、b两点的经纬度坐标转化成直角坐标，然后根据平面法向量的定理求得a、b两点所连直线与正北方向的夹角，将该夹角作为车辆在静止状态下的航向角，根据极坐标法计算a、b两点所连成的直线与正北方向的夹角θ1，计算公式如下：
[0018]
x＝sin(bj-aj)
×
cos(bw)
[0019]
y＝cos(aw)
×
sin(bw)-sin(aw)
×
cos(bw)
×
cos(bj-aj)
[0020]
θ1＝mod(atan2(x,y),360)
[0021]
式中，aj表示a点的经度，aw表示a点的纬度，bj表示b点的经度，bw表示b点的纬度，x，y为定义的临时变量，atan2表示直角坐标转化成极坐标的函数，mod表示求余函数，θ1表示a、b两点所连成的直线与正北方向的夹角；利用a、b两点所连成直线与正北方向的夹角θ1来表示车辆在静止状态下的航向角信息。
[0022]
所述步骤(4)具体是指：若判断车辆不静止，路侧摄像头模块拍摄车辆的行驶图像，路侧感知计算单元模块通过yolov3算法识别出图像中的目标车辆并标记出其矩形识别框，将矩形识别框的下边缘中心点与高精地图中的信息作对比得到该点的经纬度坐标，通过坐标计算得出目标车辆的当前航向角；物联网模块利用模糊控制方法，先根据目标车辆
的实际行驶轨迹和预设行驶轨迹之间的偏差建立路径误差模型，以横向偏差和航向偏差作为模糊控制的输入得到目标车辆航向角的修正值，并将目标车辆航向角的修正值与目标车辆的当前航向角进行加权处理，得到修正后的目标车辆航向角；
[0023]
在车辆处于行驶状态时，利用车辆轨迹对目标车辆航向角进行修正，首先通过路侧摄像头模块拍摄某一时刻的车辆在道路上的图像，随后路侧感知计算单元模块利用yolov3算法识别出图像中的目标车辆并标记出其矩形识别框，矩形识别框与目标车辆所在地面是垂直的关系，路侧感知计算单元模块将矩形识别框的下边缘中心点，即c点与高精地图中的信息进行对比，得到c点的经纬度坐标为(cj，cw)，经过时间t后，路侧摄像头模块再次拍摄车辆在道路上的图像，经过处理后得到新的中心点，即d点的经纬度坐标为(dj，dw)；根据极坐标法计算c、d两点所连成的直线与正北方向的夹角θ2，以夹角θ2作为目标车辆的当前航向角，计算公式如下：
[0024]
x＝sin(dj-cj)
×
cos(dw)
[0025]
y＝cos(cw)
×
sin(dw)-sin(cw)
×
cos(dw)
×
cos(dj-cj)
[0026]
θ2＝mod(atan2(x,y),360)
[0027]
式中，cj表示c点的经度，cw表示c点的纬度，dj表示d点的经度，dw表示d点的纬度，x，y为定义的临时变量，atan2表示直角坐标转化成极坐标的函数，mod表示求余函数，θ2表示c、d两点所连成的直线与正北方向的夹角；
[0028]
物联网模块采用模糊控制方法，先根据目标车辆的实际行驶轨迹和预设行驶轨迹之间的偏差建立路径误差模型，根据路径误差模型得到对应的横向偏差和航向偏差，以横向偏差和航向偏差作为模糊控制的输入量，航向角的修正值作为模糊控制的输出量，规定车辆位于车道中心线右侧横向偏差为正，左侧为负；航向偏差顺时针为正，逆时针为负，航向角往右修正为正，往左修正为负，横向偏差的基本论域为[-20cm，20cm]，航向偏差的基本论域为[-15
°
，15
°
]，航向角的修正值的基本论域为[-10
°
，10
°
]，通过模糊控制得到航向角的修正值，利用航向角的修正值对目标车辆的当前航向角进行修正，将航向角修正值与目标车辆的当前航向角进行加权处理，得到修正后的目标车辆航向角，计算公式如下：
[0029]
θ3＝qθ2+rδθ
[0030]
式中，θ2为目标车辆的当前航向角，δθ为通过模糊控制得到的航向角的修正值，q为目标车辆当前航向角的权重系数，r为航向角的修正值的权重系数，θ3为修正后的目标车辆航向角。
[0031]
本发明的另一目的在于提供一种基于车路云一体化的车辆航向角计算方法的装置，包括：
[0032]
gnss模块，用于在卫星信号强的情况下获取车辆的航向角信息；
[0033]
路侧摄像头模块，用于拍摄目标车辆在道路上的图像；
[0034]
路侧感知计算单元模块，用于在车辆静止未开始行驶时提取该车辆上次行驶后停止下电时刻的航向角信息，作为初步航向角信息；
[0035]
车辆摄像头模块，用于拍摄目标车辆周围的图像；
[0036]
车辆视觉处理单元模块，用于处理车辆摄像头模块采集的视觉信息并提取相应的目标特征；
[0037]
物联网模块，用于整合路侧端，汽车端和云端的信息，并发送相应的控制命令给车
辆控制器模块；
[0038]
第一5g通讯终端模块，用于路端传输信息；
[0039]
第二5g通讯终端模块，用于车端传输信息；
[0040]
存储模块，用于存储车辆的航向角信息和高精地图信息；
[0041]
车辆控制器模块，用于修正车辆的航向角信息；
[0042]
所述路侧摄像头模块的输出端与路侧感知计算单元模块的输入端相连，路侧感知计算单元模块与第一5g通信终端模块双向通讯，第一5g通信终端模块分别与云端的存储模块、第二5g通信终端模块双向通讯；车辆摄像头模块的输出端与车辆视觉处理单元模块的输入端相连，车辆视觉处理单元模块的输出端与物联网模块的输入端相连，物联网模块的输出端、gnss模块的输出端均与车辆控制器模块的输入端相连，第二5g通信终端模块与物联网模块双向通讯。
[0043]
由上述技术方案可知，本发明的有益效果为：第一，本发明利用了不同航向角计算方法在不同场景下的优势,极大地提高了航向角的计算准确度,并可以避免单一一种航向角计算方法的局限性所带来的航向角计算准确度低的问题；第二，本发明根据高精地图中的经纬度坐标计算车辆的航向角，利用模糊控制的方法得到航向角的修正值并对航向角进行修正，有利于避免车辆行驶过程中发生碰撞。
附图说明
[0044]
图1为本发明的装置组成框图；
[0045]
图2为本发明的方法流程图。
具体实施方式
[0046]
如图2所示，一种基于车路云一体化的车辆航向角计算方法，该方法包括下列顺序的步骤：
[0047]
(1)通过gnss模块判断卫星信号的强弱，若判断卫星信号强，则由车辆控制器模块直接对车辆航向角进行修正，若判断卫星信号弱，则进入步骤(2)；
[0048]
(2)由路侧摄像头模块采集车辆周围环境信息，通过路侧计算感知单元对车辆周围环境信息进行处理来判断车辆是否静止；
[0049]
(3)若判断车辆为静止，则提取或识别航向角信息；
[0050]
(4)若判断车辆不静止，则在车辆行驶时利用车辆轨迹对航向角进行修正；
[0051]
(5)当车辆在行驶中停止，由路侧感知计算单元模块临时记忆当前车辆的航向角；
[0052]
(6)当车辆下电时，路侧感知计算单元就通过第一5g通信终端模块将当前的车辆航向角信息存储到云端的存储模块中；
[0053]
(7)当车辆再次上电启动时，路侧感知计算单元模块提取云端的存储模块中该车辆存储的航向角。
[0054]
在所述步骤(1)中，利用载波噪声比来判断卫星信号的强弱，gnss模块接收的载波噪声比的值为35至55dbhz，其中，大于40dbhz的视为卫星信号强，小于40dbhz的视为卫星信号弱。
[0055]
在所述步骤(2)中，在路侧端，路侧摄像头模块拍摄目标车辆周围的图像，并将这
些信息传送给路侧感知计算单元模块，路侧感知计算单元模块利用帧间差分法来判断车辆是否处于静止状态，帧间差分法的数学公式如下：
[0056][0057]
其中，d(x,y)为连续两帧图像之间的差分图像，i(t)和i(t-1)分别为t和t-1时刻的图像，t为差分图像二值化时选取的阈值，d(x,y)＝1表示前景，车辆是运动的，d(x,y)＝0表示背景，车辆是静止的。
[0058]
所述步骤(3)具体是指：若判断车辆为静止，由路侧感知计算单元模块判断云端的存储模块中是否有已经存储的航向角信息，如果有已经存储的航向角信息，则提取该航向角信息作为车辆的初步航向角；如果云端的存储模块中未存储航向角信息，则将位于目标车辆前后的车道中心线上的两点与高精地图中的信息进行对比得到两点经纬度坐标，通过两点的经纬度坐标计算得出车辆静止状态下的航向角信息；高精地图显示不同的车道中各个点的经纬度坐标，路侧摄像头模块拍摄车辆在道路上的图像，随后路侧感知计算单元模块利用yolov3算法识别出图像中的目标车辆并标记出其矩形识别框，矩形识别框与目标车辆所在地面是垂直的关系，路侧感知计算单元模块将矩形识别框的下边缘中心点与高精地图中的信息进行对比，得到矩形识别框的下边缘中心点的经纬度坐标，根据该经纬度坐标与各车道的经纬度范围分别进行比较，判断出目标车辆具体位于哪一个车道中；车辆摄像头模块拍摄该车道上的图像，车辆视觉处理单元模块通过图像的灰度化、canny边缘检测和霍夫变换检测到图像中的车道中心线，将位于车辆前方最近处的车道中心线上的点和位于车辆后方最近处的车道中心线上的点与高精地图中的信息进行对比，得到车辆前方最近处的车道中心线上的点，即a点的经纬度坐标(aj，aw)，以及车辆后方最近处的车道中心线上的点，即b点的经纬度坐标(bj，bw)，根据极坐标法计算a、b两点所连成的直线与正北方向的夹角，先将a、b两点的经纬度坐标转化成直角坐标，然后根据平面法向量的定理求得a、b两点所连直线与正北方向的夹角，将该夹角作为车辆在静止状态下的航向角，根据极坐标法计算a、b两点所连成的直线与正北方向的夹角θ1，计算公式如下：
[0059]
x＝sin(bj-aj)
×
cos(bw)
[0060]
y＝cos(aw)
×
sin(bw)-sin(aw)
×
cos(bw)
×
cos(bj-aj)
[0061]
θ1＝mod(atan2(x,y),360)
[0062]
式中，aj表示a点的经度，aw表示a点的纬度，bj表示b点的经度，bw表示b点的纬度，x，y为定义的临时变量，atan2表示直角坐标转化成极坐标的函数，mod表示求余函数，θ1表示a、b两点所连成的直线与正北方向的夹角；利用a、b两点所连成直线与正北方向的夹角θ1来表示车辆在静止状态下的航向角信息。
[0063]
所述步骤(4)具体是指：若判断车辆不静止，路侧摄像头模块拍摄车辆的行驶图像，路侧感知计算单元模块通过yolov3算法识别出图像中的目标车辆并标记出其矩形识别框，将矩形识别框的下边缘中心点与高精地图中的信息作对比得到该点的经纬度坐标，通过坐标计算得出目标车辆的当前航向角；物联网模块利用模糊控制方法，先根据目标车辆的实际行驶轨迹和预设行驶轨迹之间的偏差建立路径误差模型，以横向偏差和航向偏差作为模糊控制的输入得到目标车辆航向角的修正值，并将目标车辆航向角的修正值与目标车辆的当前航向角进行加权处理，得到修正后的目标车辆航向角；
[0064]
在车辆处于行驶状态时，利用车辆轨迹对目标车辆航向角进行修正，首先通过路侧摄像头模块拍摄某一时刻的车辆在道路上的图像，随后路侧感知计算单元模块利用yolov3算法识别出图像中的目标车辆并标记出其矩形识别框，矩形识别框与目标车辆所在地面是垂直的关系，路侧感知计算单元模块将矩形识别框的下边缘中心点，即c点与高精地图中的信息进行对比，得到c点的经纬度坐标为(cj，cw)，经过时间t后，路侧摄像头模块再次拍摄车辆在道路上的图像，经过处理后得到新的中心点，即d点的经纬度坐标为(dj，dw)；根据极坐标法计算c、d两点所连成的直线与正北方向的夹角θ2，以夹角θ2作为目标车辆的当前航向角，计算公式如下：
[0065]
x＝sin(dj-cj)
×
cos(dw)
[0066]
y＝cos(cw)
×
sin(dw)-sin(cw)
×
cos(dw)
×
cos(dj-cj)
[0067]
θ2＝mod(atan2(x,y),360)
[0068]
式中，cj表示c点的经度，cw表示c点的纬度，dj表示d点的经度，dw表示d点的纬度，x，y为定义的临时变量，atan2表示直角坐标转化成极坐标的函数，mod表示求余函数，θ2表示c、d两点所连成的直线与正北方向的夹角；
[0069]
物联网模块采用模糊控制方法，先根据目标车辆的实际行驶轨迹和预设行驶轨迹之间的偏差建立路径误差模型，根据路径误差模型得到对应的横向偏差和航向偏差，以横向偏差和航向偏差作为模糊控制的输入量，航向角的修正值作为模糊控制的输出量，规定车辆位于车道中心线右侧横向偏差为正，左侧为负；航向偏差顺时针为正，逆时针为负，航向角往右修正为正，往左修正为负，横向偏差的基本论域为[-20cm，20cm]，航向偏差的基本论域为[-15
°
，15
°
]，航向角的修正值的基本论域为[-10
°
，10
°
]，通过模糊控制得到航向角的修正值，利用航向角的修正值对目标车辆的当前航向角进行修正，将航向角修正值与目标车辆的当前航向角进行加权处理，得到修正后的目标车辆航向角，计算公式如下：
[0070]
θ3＝qθ2+rδθ
[0071]
式中，θ2为目标车辆的当前航向角，δθ为通过模糊控制得到的航向角的修正值，q为目标车辆当前航向角的权重系数，r为航向角的修正值的权重系数，θ3为修正后的目标车辆航向角。
[0072]
如图1所示，本装置包括：
[0073]
gnss模块，用于在卫星信号强的情况下获取车辆的航向角信息；
[0074]
路侧摄像头模块，用于拍摄目标车辆在道路上的图像；
[0075]
路侧感知计算单元模块，用于在车辆静止未开始行驶时提取该车辆上次行驶后停止下电时刻的航向角信息，作为初步航向角信息；
[0076]
车辆摄像头模块，用于拍摄目标车辆周围的图像；
[0077]
车辆视觉处理单元模块，用于处理车辆摄像头模块采集的视觉信息并提取相应的目标特征；
[0078]
物联网模块，用于整合路侧端，汽车端和云端的信息，并发送相应的控制命令给车辆控制器模块；
[0079]
第一5g通讯终端模块，用于路端传输信息；
[0080]
第二5g通讯终端模块，用于车端传输信息；
[0081]
存储模块，用于存储车辆的航向角信息和高精地图信息；
[0082]
车辆控制器模块，用于修正车辆的航向角信息；
[0083]
所述路侧摄像头模块的输出端与路侧感知计算单元模块的输入端相连，路侧感知计算单元模块与第一5g通信终端模块双向通讯，第一5g通信终端模块分别与云端的存储模块、第二5g通信终端模块双向通讯；车辆摄像头模块的输出端与车辆视觉处理单元模块的输入端相连，车辆视觉处理单元模块的输出端与物联网模块的输入端相连，物联网模块的输出端、gnss模块的输出端均与车辆控制器模块的输入端相连，第二5g通信终端模块与物联网模块双向通讯。
[0084]
综上所述，本发明利用了不同航向角计算方法在不同场景下的优势,极大地提高了航向角的计算准确度,并可以避免单一一种航向角计算方法的局限性所带来的航向角计算准确度低的问题；本发明根据高精地图中的经纬度坐标计算车辆的航向角，利用模糊控制的方法得到航向角的修正值并对航向角进行修正，有利于避免车辆行驶过程中发生碰撞。

技术特征：
1.一种基于车路云一体化的车辆航向角计算方法，其特征在于：该方法包括下列顺序的步骤：(1)通过gnss模块判断卫星信号的强弱，若判断卫星信号强，则由车辆控制器模块直接对车辆航向角进行修正，若判断卫星信号弱，则进入步骤(2)；(2)由路侧摄像头模块采集车辆周围环境信息，通过路侧计算感知单元对车辆周围环境信息进行处理来判断车辆是否静止；(3)若判断车辆为静止，则提取或识别航向角信息；(4)若判断车辆不静止，则在车辆行驶时利用车辆轨迹对航向角进行修正；(5)当车辆在行驶中停止，由路侧感知计算单元模块临时记忆当前车辆的航向角；(6)当车辆下电时，路侧感知计算单元就通过第一5g通信终端模块将当前的车辆航向角信息存储到云端的存储模块中；(7)当车辆再次上电启动时，路侧感知计算单元模块提取云端的存储模块中该车辆存储的航向角。2.根据权利要求1所述的基于车路云一体化的车辆航向角计算方法，其特征在于：在所述步骤(1)中，利用载波噪声比来判断卫星信号的强弱，gnss模块接收的载波噪声比的值为35至55dbhz，其中，大于40dbhz的视为卫星信号强，小于40dbhz的视为卫星信号弱。3.根据权利要求1所述的基于车路云一体化的车辆航向角计算方法，其特征在于：在所述步骤(2)中，在路侧端，路侧摄像头模块拍摄目标车辆周围的图像，并将这些信息传送给路侧感知计算单元模块，路侧感知计算单元模块利用帧间差分法来判断车辆是否处于静止状态，帧间差分法的数学公式如下：其中，d(x,y)为连续两帧图像之间的差分图像，i(t)和i(t-1)分别为t和t-1时刻的图像，t为差分图像二值化时选取的阈值，d(x,y)＝1表示前景，车辆是运动的，d(x,y)＝0表示背景，车辆是静止的。4.根据权利要求1所述的基于车路云一体化的车辆航向角计算方法，其特征在于：所述步骤(3)具体是指：若判断车辆为静止，由路侧感知计算单元模块判断云端的存储模块中是否有已经存储的航向角信息，如果有已经存储的航向角信息，则提取该航向角信息作为车辆的初步航向角；如果云端的存储模块中未存储航向角信息，则将位于目标车辆前后的车道中心线上的两点与高精地图中的信息进行对比得到两点经纬度坐标，通过两点的经纬度坐标计算得出车辆静止状态下的航向角信息；高精地图显示不同的车道中各个点的经纬度坐标，路侧摄像头模块拍摄车辆在道路上的图像，随后路侧感知计算单元模块利用yolov3算法识别出图像中的目标车辆并标记出其矩形识别框，矩形识别框与目标车辆所在地面是垂直的关系，路侧感知计算单元模块将矩形识别框的下边缘中心点与高精地图中的信息进行对比，得到矩形识别框的下边缘中心点的经纬度坐标，根据该经纬度坐标与各车道的经纬度范围分别进行比较，判断出目标车辆具体位于哪一个车道中；车辆摄像头模块拍摄该车道上的图像，车辆视觉处理单元模块通过图像的灰度化、canny边缘检测和霍夫变换检测到图像中的车道中心线，将位于车辆前方最近处的车道中心线上的点和位于车辆后方最近
处的车道中心线上的点与高精地图中的信息进行对比，得到车辆前方最近处的车道中心线上的点，即a点的经纬度坐标(a
j
，a
w
)，以及车辆后方最近处的车道中心线上的点，即b点的经纬度坐标(b
j
，b
w
)，根据极坐标法计算a、b两点所连成的直线与正北方向的夹角，先将a、b两点的经纬度坐标转化成直角坐标，然后根据平面法向量的定理求得a、b两点所连直线与正北方向的夹角，将该夹角作为车辆在静止状态下的航向角，根据极坐标法计算a、b两点所连成的直线与正北方向的夹角θ1，计算公式如下：x＝sin(bj-aj)
×
cos(bw)y＝cos(aw)
×
sin(bw)-sin(aw)
×
cos(bw)
×
cos(bj-aj)θ1＝mod(atan2(x,y),360)式中，a
j
表示a点的经度，a
w
表示a点的纬度，b
j
表示b点的经度，b
w
表示b点的纬度，x，y为定义的临时变量，atan2表示直角坐标转化成极坐标的函数，mod表示求余函数，θ1表示a、b两点所连成的直线与正北方向的夹角；利用a、b两点所连成直线与正北方向的夹角θ1来表示车辆在静止状态下的航向角信息。5.根据权利要求1所述的基于车路云一体化的车辆航向角计算方法，其特征在于：所述步骤(4)具体是指：若判断车辆不静止，路侧摄像头模块拍摄车辆的行驶图像，路侧感知计算单元模块通过yolov3算法识别出图像中的目标车辆并标记出其矩形识别框，将矩形识别框的下边缘中心点与高精地图中的信息作对比得到该点的经纬度坐标，通过坐标计算得出目标车辆的当前航向角；物联网模块利用模糊控制方法，先根据目标车辆的实际行驶轨迹和预设行驶轨迹之间的偏差建立路径误差模型，以横向偏差和航向偏差作为模糊控制的输入得到目标车辆航向角的修正值，并将目标车辆航向角的修正值与目标车辆的当前航向角进行加权处理，得到修正后的目标车辆航向角；在车辆处于行驶状态时，利用车辆轨迹对目标车辆航向角进行修正，首先通过路侧摄像头模块拍摄某一时刻的车辆在道路上的图像，随后路侧感知计算单元模块利用yolov3算法识别出图像中的目标车辆并标记出其矩形识别框，矩形识别框与目标车辆所在地面是垂直的关系，路侧感知计算单元模块将矩形识别框的下边缘中心点，即c点与高精地图中的信息进行对比，得到c点的经纬度坐标为(c
j
，c
w
)，经过时间t后，路侧摄像头模块再次拍摄车辆在道路上的图像，经过处理后得到新的中心点，即d点的经纬度坐标为(d
j
，d
w
)；根据极坐标法计算c、d两点所连成的直线与正北方向的夹角θ2，以夹角θ2作为目标车辆的当前航向角，计算公式如下：x＝sin(dj-cj)
×
cos(dw)y＝cos(cw)
×
sin(dw)-sin(cw)
×
cos(dw)
×
cos(dj-cj)θ2＝mod(atan2(x,y),360)式中，c
j
表示c点的经度，c
w
表示c点的纬度，d
j
表示d点的经度，d
w
表示d点的纬度，x，y为定义的临时变量，atan2表示直角坐标转化成极坐标的函数，mod表示求余函数，θ2表示c、d两点所连成的直线与正北方向的夹角；物联网模块采用模糊控制方法，先根据目标车辆的实际行驶轨迹和预设行驶轨迹之间的偏差建立路径误差模型，根据路径误差模型得到对应的横向偏差和航向偏差，以横向偏差和航向偏差作为模糊控制的输入量，航向角的修正值作为模糊控制的输出量，规定车辆位于车道中心线右侧横向偏差为正，左侧为负；航向偏差顺时针为正，逆时针为负，航向角
往右修正为正，往左修正为负，横向偏差的基本论域为[-20cm，20cm]，航向偏差的基本论域为[-15
°
，15
°
]，航向角的修正值的基本论域为[-10
°
，10
°
]，通过模糊控制得到航向角的修正值，利用航向角的修正值对目标车辆的当前航向角进行修正，将航向角修正值与目标车辆的当前航向角进行加权处理，得到修正后的目标车辆航向角，计算公式如下：θ3＝qθ2+rδθ式中，θ2为目标车辆的当前航向角，δθ为通过模糊控制得到的航向角的修正值，q为目标车辆当前航向角的权重系数，r为航向角的修正值的权重系数，θ3为修正后的目标车辆航向角。6.实施权利要求1至5中任一项所述的基于车路云一体化的车辆航向角计算方法的装置，其特征在于：包括：gnss模块，用于在卫星信号强的情况下获取车辆的航向角信息；路侧摄像头模块，用于拍摄目标车辆在道路上的图像；路侧感知计算单元模块，用于在车辆静止未开始行驶时提取该车辆上次行驶后停止下电时刻的航向角信息，作为初步航向角信息；车辆摄像头模块，用于拍摄目标车辆周围的图像；车辆视觉处理单元模块，用于处理车辆摄像头模块采集的视觉信息并提取相应的目标特征；物联网模块，用于整合路侧端，汽车端和云端的信息，并发送相应的控制命令给车辆控制器模块；第一5g通讯终端模块，用于路端传输信息；第二5g通讯终端模块，用于车端传输信息；存储模块，用于存储车辆的航向角信息和高精地图信息；车辆控制器模块，用于修正车辆的航向角信息；所述路侧摄像头模块的输出端与路侧感知计算单元模块的输入端相连，路侧感知计算单元模块与第一5g通信终端模块双向通讯，第一5g通信终端模块分别与云端的存储模块、第二5g通信终端模块双向通讯；车辆摄像头模块的输出端与车辆视觉处理单元模块的输入端相连，车辆视觉处理单元模块的输出端与物联网模块的输入端相连，物联网模块的输出端、gnss模块的输出端均与车辆控制器模块的输入端相连，第二5g通信终端模块与物联网模块双向通讯。

技术总结
本发明涉及一种基于车路云一体化的车辆航向角计算方法，包括：判断卫星信号的强弱，若判断卫星信号强，直接对车辆航向角进行修正，否则，则进入步骤(2)；采集车辆周围环境信息，判断车辆是否静止；若判断车辆为静止，则提取或识别航向角信息；否则，在车辆行驶时利用车辆轨迹对航向角进行修正；当车辆在行驶中停止，临时记忆当前车辆的航向角；当车辆下电时，将当前的车辆航向角信息存储到云端的存储模块中；当车辆再次上电启动时，提取云端的存储模块中该车辆存储的航向角。本发明还公开了一种基于车路云一体化的车辆航向角计算系统。本发明避免单一一种航向角计算的局限性所带来的航向角计算准确度低的问题，避免车辆行驶过程中发生碰撞。程中发生碰撞。程中发生碰撞。


技术研发人员：韩超 周骏 陆小磊 丁子心
受保护的技术使用者：安徽工程大学
技术研发日：2023.03.31
技术公布日：2023/7/4