极低能见度条件下的车辆偏航检测方法和辅助驾驶系统与流程

1.本发明实施例涉及智能驾驶技术领域，尤其涉及一种极低能见度条件下的车辆偏航检测方法和辅助驾驶系统。


背景技术：

2.大雾、暴风雪等恶劣天气条件下，能见度降低，极大地影响了车辆的通行效率，使交通事故风险大大增加。这时需要为车辆提供辅助驾驶功能，确保车辆不会驶出道路。
3.早期的辅助驾驶系统采用磁道钉技术实现，但由于磁道钉需要埋设在路面下，存在着安装维护困难、定位精度和灵敏度较低等问题。专利cn201936454u公开了一种车辆定位导航监控提醒和通讯系统，专利cn108202669a公开了一种基于车车通信的不良天气视觉增强行车辅助系统及其方法。二者通过车辆定位系统辅助不良天气下的车辆行驶，但其对于车辆驶离道路等风险，仍不能提供有效的检测和提示。


技术实现要素：

4.本发明实施例提供一种极低能见度条件下的车辆偏航检测方法和辅助驾驶系统，通过卫星定位技术，提升驾驶员在极端气象条件下驾驶效率和安全性。
5.第一方面，本发明实施例提供了一种极低能见度条件下的车辆偏航检测方法，包括：通过安装于车辆中心的卫星天线，实时获取车辆中心的经纬度和车头航向角；根据车辆中心的经纬度和车头航向角，实时确定车头经纬度；根据车辆中心的经纬度和车头经纬度，构建以车辆中心和车头为端点的线段；将所述线段以车辆中心为固定点旋转90
°
并延长，分别与卫星地图中的道路边缘相交于第一交点和第二交点；确定经过车头的、平行于延长线的直线，分别与所述道路边缘相交于第三交点和第四交点；根据车辆中心分别与第一交点、第二交点的距离，以及车头分别与第三交点、第四交点的距离，实时判断车辆是否具有驶离道路的风险。
6.第二方面，本发明实施例提供一种极低能见度条件下的车辆辅助驾驶系统，包括：高精度车辆定位子系统，用于通过安装于车辆中心的卫星天线，获取车辆中心的经纬度和车头航向角；车辆实时数字孪生子系统，用于根据车辆中心的经纬度和车头航向角，在三维地图中实时反馈车辆在道路中的行驶情况，并执行权利要求1-7任一所述方法以实时判断车辆是否具有驶离道路的风险。
7.本发明实施例提供的极低能见度条件下的车辆偏航检测方法，能够基于车辆当前位置信息判断车辆在道路上的位置，并提示驾驶员车辆距离道路两侧的距离。一旦发现车辆有可能驶出道路，系统会发出警报，防止车辆驶出道路，避免事故的发生。为了使检测方
法适用于车头左偏、右偏、左侧驶离、右侧驶离等各种情况，本实施例分别确定了车辆到道路两侧边缘内的四个距离，选取最小距离与安全距离进行比较；同时通过车头旋转90
°
下的特殊坐标关系确定与道路两侧边缘的交点，无需在不同的车头偏航角下旋转不同的角度重复计算，提高了检测效率和方法的通用性。
附图说明
8.为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
9.图1是本发明实施例提供的一种极低能见度条件下的车辆辅助驾驶系统的结构图；图2是本发明实施例提供的一种极低能见度条件下的车辆偏航检测方法的流程图；图3是本发明实施例提供的以车辆中心为原点建立平面坐标系的示意图；图4是本发明实施例提供的初步判断时计算设定阈值的示意图。
具体实施方式
10.为使本发明的目的、技术方案和优点更加清楚，下面将对本发明的技术方案进行清楚、完整的描述。显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所得到的所有其它实施例，都属于本发明所保护的范围。
11.在本发明的描述中，需要说明的是，术语“中心”、“上”、“下”、“左”、“右”、“竖直”、“水平”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”、“第三”仅用于描述目的，而不能理解为指示或暗示相对重要性。
12.在本发明的描述中，还需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。
13.本发明实施例提供的一种极低能见度条件下的车辆偏航检测方法。为了说明该方法，优先介绍执行该方法的车辆辅助驾驶系统。图1是本发明实施例提供的一种极低能见度条件下的车辆辅助驾驶系统的结构图，适用于极端气象条件等车辆无法通过视觉手段检测是否偏离道路的情况，示例性的，所述极端气象条件指极低能见度条件，包括：暴风雪、大雾天气等。如图1所示，该系统包括高精度车辆定位子系统和车辆实时数字孪生子系统。
14.其中，高精度车辆定位子系统用于通过安装于车辆中心的卫星天线，实时获取车辆中心的经纬度、车辆速度和车头航向角等运动学信息，并输入至车辆实时数字孪生子系
统。示例性的，高精度车辆定位系统可以运行于集成在车内的电子设备。
15.车辆实时数字孪生子系统用于基于三维地图，根据高精度车辆定位子系统输入的车辆运动学数据，在三维地图中实时反馈车辆在道路中的行驶情况，实时判断车辆是否具有驶离道路的风险并提示驾驶员。示例性的，车辆实时数字孪生子系统可以运行于安卓系统的平板电脑，将实时数字孪生子系统的安卓软件包下载或拷贝至平板电脑中安装运行即可。
16.可选的，高精度车辆定位子系统包括定位模块、通信模块和存储模块。其中，定位模块用于获取车辆经纬度、车辆速度和车头航向角等运动学信息，通信模块用于与车辆定位实时数字孪生子系统进行数据交互，存储模块用于存储车辆运动学信息与轨迹数据。
17.可选的，车辆实时数字孪生子系统，包括三维地图模块、车辆模型映射模块、二维地图模块、车辆偏航检测模块和通信模块。三维地图模块用于以三维建模形式再现现实世界中的道路。车辆模型映射模块用于将车辆在现实世界的经纬度位置信息实时孪生至三维地图模块中，在三维场景中展示车辆当前在道路中的位置以及车辆的运动情况。二维地图模块用于展示车辆的地理位置信息（经纬度位置）。车辆偏航检测模块用于实时判断车辆是否具有驶离道路的风险，并在存在风险时提示驾驶员。通信模块用于与高精度车辆定位子系统进行数据交互。高精度车辆定位子系统的通信模块与实时数字孪生子系统的通信模块之间，采用无线通信方式建立数据连接，采用tcp方式完成数据通信。
18.本实施例中提供的极低能见度条件下的车辆辅助驾驶系统，基于高精度车辆定位子系统提供的车辆定位、运动学数据，结合数字孪生技术和虚实融合技术，在逼真的三维地图场景中展示车辆当前在道路中的运行状态，能够帮助驾驶员判断当前车辆姿态、位置与速度，提升在能见度极低的极端气象条件下行车的安全性与效率。
19.基于以上辅助驾驶系统，图2是本发明实施例提供的一种极低能见度条件下的车辆偏航检测方法的流程图。该方法由车辆偏航检测模块执行，基于卫星天线获取的车辆运动学信息和地图数据，计算车辆与道路边缘的距离，判断是否有驶离道路的风险。如图2所示，具体包括如下步骤：s110、通过安装于车辆中心的卫星天线，实时获取车辆中心的经纬度和车头航向角。
20.车载卫星天线可以包括定位天线和定向天线。定位天线用于提供安装位置处的经纬度位置，即车辆中心的经纬度位置；定向天线用于提供车头相对于正北方向的夹角，即车头航向角。车辆中心的经纬度位置和车头航向角，共同作为后续处理的数据源。
21.s120、根据车辆中心的经纬度和车头航向角，实时确定车头经纬度。
22.已知车辆中心到车头的长度为lh，沿车头航向角方向取距离车辆中心lh长度的一点作为车头，就可以得到车头经纬度。
23.s130、根据车辆中心的经纬度和车头经纬度，构建以车辆中心和车头为端点的线段。
24.可选的，每采集到一个车辆中心的轨迹点后，以该轨迹点为中心，在地图对应位置处选取一个长为设定长度（例如50米）、宽为道路宽度的长方形路段区域，在区域内以车辆中心a为原点，以地面上平行于纬线的方向为横轴、平行于经线的方向为纵轴，建立平面坐标系（如图3所示），区域内任一位置到车辆中心的经度差和纬度差分别为该坐标系下的横
坐标和纵坐标。假设车头b的坐标为(x0,y0)，则线段ab可以表示为：（1）其中，r为线段ab的向量表示，i表示横轴单位向量，j表示纵轴单位向量。
25.s140、将所述线段以车辆中心为固定点旋转90
°
并延长，分别与卫星地图中的道路边缘相交于第一交点和第二交点。
26.参见图3，在上述坐标系下，将线段ab以车辆中心a为固定点旋转90
°
后，车头b旋转到b’(-y0,x0)。具体的，向量r旋转角度后可以表示为：（2）其中，表示将向量旋转角度。将代入公式（2），可知b’的坐标为(-y0,x0)。
27.根据b’的坐标(-y0,x0)即可确定延长线方程，该延长线分别与地图数据中的道路边缘相交于第一交点和第二交点。以图3为例，ab’的延长线分别与道路边缘相交于第一交点p1和第二交点p2，根据ab’的方程f(x)和道路边缘的方程g(x)，就可以确定p1和p2的经纬度。
28.s150、确定经过车头的、平行于延长线的直线，分别与所述道路边缘相交于第三交点和第四交点。
29.仍以图3为例，确定一条与p1p2平行且经过b点的直线，这条直线与道路截面边缘交点记为第三交点p3和第四交点p4，根据平行线的方程f1(x)和道路边缘的方程g(x)，就可以确定p3和p4的经纬度。
30.s160、根据车辆中心分别与第一交点、第二交点的距离，以及车头分别与第三交点、第四交点的距离，实时判断车辆是否具有驶离道路的风险。
31.由于定位天线安装于车辆正中央的位置，且定位系统提供的车头航向角是没有正负的，无法判断车头离道路边缘更近，还是车辆中心离道路边缘更近。因此，本实施例确定了第一交点、第二交点、第三交点和第四交点，根据车辆中心与第一交点的距离r1、车辆中心与第二交点的距离r2、车头与第三交点的距离r3，以及车头与第四交点的距离r4，实时判断车辆是否具有驶离道路的风险。结合图3，r1=|ap1|，r2=|ap2|，r3=|bp3|，r4=|bp4|，车头右偏，靠近右侧道路边缘。实际行驶中，车头左偏还是右偏，车辆靠近左侧道路边缘还是右侧道路边缘，均是无法直接获取的，为了兼顾各种情况，本实施例通过|ap1|、|ap2|、|bp3|和|bp4|共同完成风险判断。
32.在一具体实施方式中，在已知各点经纬度的情况下，首先，通过以下方式计算两点之间的距离：（3）其中，d
i,j
表示点i和点j之间的距离，lati和latj分别表示点i和点j的纬度，变量ai,j
与b
i,j
分别表示点i和点j之间的纬度差和经度差，6378.137为地球半径，单位为千米，因此需要除以1000转化为米。
33.采用上述方式计算得到r1、r2、r3和r4后，选取其中的最小值lm；根据所述最小值和车头航向角，确定车辆到道路边缘的最小距离。车辆到道路边缘的距离，指车身外轮廓上一点到道路边缘的距离；车辆到道路边缘的最小距离，即车身外轮廓上将要最先驶离道路的一点到道路边缘的距离。参照图3，从|ap1|、|ap2|、|bp3|和|bp4|四个距离中选取出来的最小值lm为|bp4|，过车头右侧顶点b”作x轴的平行线，与道路右侧边缘线将于点p，则车辆到道路边缘的最小距离l为车头右侧顶点b”到道路边缘的距离|b”p|：（4）其中，lw表示车辆半宽，表示车头航向角。
34.如果车辆到道路边缘的最小距离l小于车辆到道路边缘的最小安全距离l
min
，则车辆具有驶离道路的风险。示例性的，最小安全距离l
min
取0.5米，当l《l
min
米时，系统会发出警报，提醒驾驶员车辆目前有驶出道路的危险。该实施例通过选取四个距离中的最小值，将最小距离值对应的一侧作为即将驶离道路的一侧，执行后续判断操作；省去了判断车头左偏还是右偏、车辆靠近左侧边缘还是右侧边缘的复杂过程，提高了检测效率和方法的通用性。
35.在上述实施例的基础上，为了进一步提高检测效率，还可以对s140和s150中各交点的计算时机进行改进，无需在每次确定第一交点和第二交点后，都继续确定第三交点和第四交点，而是在确定第一交点和第二交点后，先计算车辆中心与第一交点的距离r1，以及车辆中心与第二交点的距离r2，选取r1和r2中的较小距离与设定阈值作比较；如果该较小距离小于设定阈值l
t
，则确定经过车头的、平行于所述延长线的直线，计算第三交点和第四交点；否则，不再计算平行线、第三交点和第四交点，结束针对当前轨迹点的处理，进入针对下一轨迹点的计算。该实施例中，首先通过r1和r2中的较小距离初步判断车辆是否具有驶离道路的风险，为风险预测提供初步参考；当r1和r2中的较小距离小到一定程度（小于上述设定阈值l
t
）时，再通过平行线确定第三交点和第三交点，进行第二次精准判断。
36.可以看出，如果所述设定阈值l
t
取值过大，会使整个算法趋近于对于每个轨迹点都确定四个交点，无法实现加快检测的目的；如果所述设定阈值l
t
取值过小，会导致车身外轮廓已经到达道路边缘时，第二次精确判断还没有开启。因此，本实施例提供一种设定阈值l
t
的确定方式，由车辆到道路边缘的最小安全距离、车辆宽度，以及安全行驶允许的最大车头航向角共同求解阈值大小。可选的，首先获取车辆的最大车头航向角，即驾驶员将方向盘打满时车身的最大偏航角度，该角度与具体车型有关，通常不超过40
°
，可以将40
°
作为安全行驶允许的最大车头航向角。然后，结合图4计算车头航向角=、车辆到道路边缘的最小距离|b”p|=最小安全距离l
min
的情况下，r1和r2中的较小距离|ap2|的大小。具体的，过b”作平行于y轴的直线，与ap2相交于点a’；过a’作平行于x轴的直线，与道路右侧边缘相交于点p’；将ap2与车辆右侧的交点记为a”，其余符号的含义参见图3，则有：（5）将、和
代入公式（5），计算得到的|ap2|即为所述设定阈值l
t
：（6）车头偏航角越大，车头越先于车辆中心抵达道路边缘。通过公式（6）计算的阈值l
t
，能够为车头先于车辆中心的偏转提供足够的距离，当r1和r2中的较小距离距离达到l
t
时，最大偏航角下的车头与道路边缘的距离刚好等于安全距离l
min
；最大偏航角出现的情况极少，正常行驶中的偏航角均小于车头最大偏航角，因此该阈值既能充分保证车辆到道路边缘的安全距离，又不过早开启第二阶段的判断，使安全性和检测效率得到平衡。值得一提的是，尽管该阈值是基于图3所示的例子（车头右偏，车辆靠近右侧道路边缘）计算得到的，但由于没有正负，无论车头左偏或右偏、车辆靠近左侧道路边缘还是右侧道路边缘，均可以使用公式（6）来计算阈值l
t
，适用于车辆的所有运动状态。
37.本实施例提供的极低能见度条件下的车辆偏航检测方法，能够基于车辆当前位置信息判断车辆在道路上的位置，并提示驾驶员车辆距离道路两侧的距离。一旦发现车辆有可能驶出道路，系统会发出警报，防止车辆驶出道路，避免事故的发生。为了使检测方法适用于车头左偏、右偏、左侧驶离、右侧驶离等各种情况，本实施例分别确定了车辆到道路两侧边缘内的四个距离，选取最小距离与安全距离进行比较，提高了方法的通用性。为了进一步提高检测效率，本实施例通过车头旋转90
°
下的特殊坐标关系确定与道路两侧边缘的交点，无需在不同的车头偏航角下旋转不同的角度重复计算；并通过第一交点和第二交点进行初步判断，满足阈值条件后再计算第三交点和第四交点进行二次判断，与针对每个轨迹点都计算四个交点相比，减少了检测计算量。同时，本实施例根据安全行驶允许的车头最大偏航角和车辆到道路边缘的最小安全距离，为初步判断确定了合理的阈值条件，实现了安全性和检测效率之间的平衡。
38.最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案。

技术特征：
1.一种极低能见度条件下的车辆偏航检测方法，其特征在于，包括：通过安装于车辆中心的卫星天线，实时获取车辆中心的经纬度和车头航向角；根据车辆中心的经纬度和车头航向角，实时确定车头经纬度；根据车辆中心的经纬度和车头经纬度，构建以车辆中心和车头为端点的线段；将所述线段以车辆中心为固定点旋转90
°
并延长，分别与卫星地图中的道路边缘相交于第一交点和第二交点；确定经过车头的、平行于延长线的直线，分别与所述道路边缘相交于第三交点和第四交点；根据车辆中心分别与第一交点、第二交点的距离，以及车头分别与第三交点、第四交点的距离，实时判断车辆是否具有驶离道路的风险。2.根据权利要求1所述的方法，其特征在于，所述极低能见度条件包括：暴风雪、大雾天气。3.根据权利要求1所述的方法，其特征在于，所述根据车辆中心的经纬度和车头航向角，实时确定车头经纬度，包括：获取车辆中心到车头的长度l
h
；沿车头航向角方向取距离车辆中心l
h
长度的一点作为车头，得到车头经纬度。4.根据权利要求1所述的方法，其特征在于，所述将所述线段以车辆中心为固定点旋转90
°
并延长，包括：在以车辆中心为原点，以地面上平行于纬线的方向为横轴、平行于经线的方向为纵轴的坐标系中，确定车头坐标(x0,y0)；确定所述线段以车辆中心为固定点旋转90
°
后的车头坐标(-y0,x0)；根据旋转90
°
后的车头坐标(-y0,x0)确定延长线方程。5.根据权利要求1所述的方法，其特征在于，所述确定经过车头的、平行于延长线的直线，包括：确定车辆中心分别与第一交点和第二交点的距离中较小的距离；如果所述较小的距离小于设定阈值，确定经过车头的、平行于延长线的直线，其中，所述设定阈值由车辆到道路边缘的最小安全距离、车辆宽度，以及安全行驶允许的最大车头航向角共同决定。6.根据权利要求5所述的方法，其特征在于，所述设定阈值l
t
根据以下公式确定：；其中，l
w
表示车辆半宽，l
h
表示车辆中心到车头的长度，表示安全行驶允许的最大车头航向角，l
min
表示车辆到道路边缘的最小安全距离。7.根据权利要求1所述的方法，其特征在于，所述根据车辆中心分别与第一交点、第二交点的距离，以及车头分别与第三交点、第四交点的距离，判断车辆是否具有驶离道路的风险，包括：确定车辆中心分别与第一交点、第二交点的距离，以及车头分别与第三交点、第四交点
的距离中的最小值；根据所述最小值和车头航向角，确定车辆到道路边缘的最小距离；如果所述最小距离小于车辆到道路边缘的最小安全距离，判断车辆具有驶离道路的风险。8.一种极低能见度条件下的车辆辅助驾驶系统，其特征在于，包括：高精度车辆定位子系统，用于通过安装于车辆中心的卫星天线，获取车辆中心的经纬度和车头航向角；车辆实时数字孪生子系统，用于根据车辆中心的经纬度和车头航向角，在三维地图中实时反馈车辆在道路中的行驶情况，并执行权利要求1-7任一所述方法以实时判断车辆是否具有驶离道路的风险。9.根据权利要求8所述的系统，其特征在于，所述高精度车辆定位子系统包括：定位模块，用于通过安装于车辆中心的卫星天线，获取车辆中心的经纬度、车辆速度和车头航向角；通信模块，用于与所述车辆定位实时数字孪生子系统进行数据交互；存储模块，用于存储车辆中心的经纬度、车辆速度、车头航向角和轨迹数据。10.根据权利要求8所述的系统，其特征在于，所述车辆实时数字孪生子系统包括：三维地图模块，用于以三维建模形式再现道路信息；车辆模型映射模块，用于将车辆的经纬度实时孪生至三维地图模块中，实时展示车辆在道路中的位置和运动情况；二维地图模块，用于展示车辆的经纬度位置；车辆偏航检测模块，用于执行权利要求1-7任一所述方法以实时判断车辆是否具有驶离道路的风险；通信模块，用于与所述高精度车辆定位子系统进行数据交互。

技术总结
本发明实施例公开了一种极低能见度条件下的车辆偏航检测方法和辅助驾驶系统。其中，偏航检测方法包括：通过安装于车辆中心的卫星天线，实时获取车辆中心的经纬度和车头航向角；根据车辆中心的经纬度和车头航向角，实时确定车头经纬度；根据车辆中心的经纬度和车头经纬度，构建以车辆中心和车头为端点的线段；将所述线段以车辆中心为固定点旋转90


技术研发人员：李振华 刘砚玥 王守玲 郭瑞 魏戈兵
受保护的技术使用者：交通运输部公路科学研究所
技术研发日：2023.06.28
技术公布日：2023/8/1