CN110986966A - 一种用于长距离隧道的自动驾驶定位方法及系统 - Google Patents

Abstract

本发明涉及一种用于长距离隧道的自动驾驶定位方法及系统，该方法包括以下步骤：⑴加载地图，并获取传感器数据；⑵沿车道线方向进行初始姿态采样，计算得到车辆定位初始姿态序列；⑶对所述车辆定位初始姿态序列进行遍历，计算得到每个所述初始搜索姿态附近的最高评分值和所述最高评分值对应的最优姿态，组成评分值序列；⑷判断所述评分值序列是否有效；⑸若判定为有效，则以所述评分值序列中评分最高的最高评分值对应的最优姿态作为最终的定位结果；若判定为无效，则以所述初始姿态附近的最优姿态作为最终的定位结果。本发明能够在GPS失效的场景中，借助高精地图，沿车道线方向进行动态范围搜索，进行准确、稳定的定位。

Description

一种用于长距离隧道的自动驾驶定位方法及系统

技术领域

本发明涉及信息处理技术领域，特别涉及一种用于长距离隧道的自动驾驶定位方法及系统。

背景技术

自动驾驶车辆需要实时提供车身在世界坐标系中的位置信息，随着卫星定位技术的成熟，在空旷道路上可以通过GPS和RTK直接获得非常精确的定位结果。而在GPS信号弱或者完全失效的情况下，一般需要事先构建对应的高精地图，然后借助高精地图提供精确的定位结果。但是在高速公路中的长距离隧道场景中，车辆很长一段时间内无法获得稳定的GPS信号，即使存在高精地图，无论是基于激光点云还是视觉特征的高精地图，由于隧道内特征信息区分度低，现有的隧道定位方案，如通过固定搜索匹配策略、粒子滤波等方式，仍然不能提供比较准确且稳定的定位结果。

由于隧道场景中的GPS信号弱或者可能完全失效，同时可能在很长一段时间内都只能做车道横向的精确定位，而无法获得可靠车道方向的纵向定位。现有的方案要么通过加入更多的传感器信息来缓解场景中可区分特征少的问题，例如，加入视觉特征地图等，但隧道中的可区分性视觉特征也非常少，难以得到可信的定位结果；或者利用滤波的方式以获得平滑的定位结果，例如融合进IMU和轮速计的定位结果等，但其无法得到和地图一致的定位结果。但是对于可区分特征少且无GPS信号的长距离隧道场景，现有方法仍然不能得到精确且稳定的定位结果。

发明内容

本发明要解决的技术问题是针对上述不足，提供一种沿车道线方向进行动态范围搜索，并借助高精度地图，保证自动驾驶车辆能够在长距离隧道场景或者相类似场景中，进行精确且稳定定位的自动驾驶定位方法及系统。

本发明是通过以下技术方案实现的：

一种用于长距离隧道的自动驾驶定位方法，该方法包括以下步骤：

⑴加载地图，并获取当前时刻的传感器数据；

⑵沿车道线方向进行自身车辆的初始姿态采样，计算得到车辆定位初始姿态序列；

⑶对所述车辆定位初始姿态序列的每个初始搜索姿态的变量空间进行遍历，计算得到每个所述初始搜索姿态附近的最高评分值和所述最高评分值对应的最优姿态，组成评分值序列；

⑷对所述评分值序列的每个所述最高评分值进行归一化处理，并判断所述评分值序列是否有效；

⑸若判定所述评分值序列为有效，则以所述评分值序列中评分最高的最高评分值对应的最优姿态作为最终的定位结果；

若判定所述评分值序列为无效，则以所述初始姿态附近的最优姿态作为最终的定位结果。

进一步的，所述的自动驾驶定位方法，所述地图包括高精点云地图和对应车道线地图。

进一步的，所述的自动驾驶定位方法，所述初始姿态根据以下公式计算：

T_t ^*＝T_t-1+D(T_t-1)*s_t-1

其中，T_t-1为t-1时刻的姿态，s_t-1为t-1时刻的轮速信息，D(T_t-1)为朝向提取函数，用以获得T_t-1对应的车辆朝向向量。

进一步的，所述的自动驾驶定位方法，步骤⑵包括：

从所述传感器数据中获取所述初始姿态所在的车道中心线，计算所述初始姿态在所述车道中心线上的投影点，得到当前时刻对应所述车道中心线上的姿态采样点序列；

计算所述初始姿态距离所述车道中心线的横向距离，将所述姿态采样点序列在垂直于所述车道中心线的方向上向所述初始姿态所在一侧平移所述横向距离，得到当前时刻的车辆定位初始姿态序列。

进一步的，所述的自动驾驶定位方法，所述归一化处理的计算公式为：

N(Score_t ^i*)＝(Score_t ^i*-Score_min)/(Score_max-Score_min)

其中，Score_t ^i*为评分值序列中的第i个最高评分值，Score_min为评分值序列中的最小值，Score_max为评分值序列中的最大值。

进一步的，所述的自动驾驶定位方法，步骤⑷包括：

若归一化后的评分值序列中，只要有一个最高评分值大于第一阈值，且其对应的最优姿态与归一化后的评分值序列中最大值所对应的最优姿态之间的距离大于第二阈值，则判定所述评分值序列为无效；否则，判定所述评分值序列为有效。

进一步的，所述的自动驾驶定位方法，步骤⑸包括：

若判定为有效的定位结果，则使用ICP算法对最终的定位结果进行优化，并将定位不可信距离设置为0。

一种用于长距离隧道的自动驾驶定位系统，该系统包括：

加载模块，用于加载地图，并获取当前时刻的传感器数据；

采样模块，用于沿车道线方向进行自身车辆的初始姿态采样，计算得到车辆定位初始姿态序列；

遍历计算模块，用于对所述车辆定位初始姿态序列的每个初始搜索姿态的变量空间进行遍历，计算得到每个所述初始搜索姿态附近的最高评分值和所述最高评分值对应的最优姿态，组成评分值序列；

判定模块，用于对所述评分值序列的每个所述最高评分值进行归一化处理，并判断所述最高评分值是否为有效的定位结果；和

输出模块，用于若判定为有效的定位结果，则以所述评分值序列中评分最高的最高评分值对应的最优姿态作为最终的定位结果；若判定为无效的定位结果，则以所述初始姿态附近的最优姿态作为最终的定位结果。

进一步的，所述的自动驾驶定位系统，该系统还包括：

优化模块，用于若判定为有效的定位结果，则使用ICP算法对所述最终的定位结果进行优化，并将定位不可信距离设置为0。

进一步的，所述的自动驾驶定位系统，所述采样模块执行以下操作：

从所述传感器数据中获取所述初始姿态所在的车道中心线，计算所述初始姿态在所述车道中心线上的投影点，得到当前时刻对应所述车道中心线上的姿态采样点序列；

计算所述初始姿态距离所述车道中心线的横向距离，将所述姿态采样点序列在垂直于所述车道中心线的方向上向所述初始姿态所在一侧平移所述横向距离，得到当前时刻的车辆定位初始姿态序列。

本发明的优点与效果是：

1.本发明提供的自动驾驶定位方法，其在沿车道线方向采样初始姿态点，而不是直接在世界坐标系下对初始姿态点进行采样，由于车辆一般都是在车道中行驶，因此，通过沿车道线方向采样初始姿态点，一方面采样的初始姿态更有可能命中真实姿态，另一方面也可以提高采样初始姿态所对应的有效范围。

2.本发明提供的自动驾驶定位方法及系统能够在GPS失效的长距离隧道场景中，借助高精地图，沿车道线方向进行动态范围搜索，进行准确、稳定的定位。

附图说明

图1示出本发明提供的自动驾驶定位方法的一实施例的流程图；

图2示出本发明提供的自动驾驶定位方法的举例实际场景的初始姿态的示意图；

图3示出图2的车辆定位初始姿态序列的示意图；

图4示出图3的初始搜索姿态的遍历计算的示意图；

图5示出本发明提供的自动驾驶定位系统的一实施例的结构示意图；

图6示出本发明提供的自动驾驶定位系统的另一实施例的结构示意图。

具体实施方式

为使本发明实施的目的、技术方案和优点更加清楚，下面结合本发明实施例中的附图，对本发明实施例中的技术方案进行更加详细的描述。所描述的实施例是本发明的一部分实施例，而不是全部的实施例。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。基于本发明中的实施例，本领域技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。下面结合附图对本发明的实施例进行详细说明：

在有些情况下存在诸如“一个或多个”、“至少”、“但不限于”或其他类似短语的扩展性单词和词组将不应当理解为在扩展性词组不存在的情况下有意或要求更窄的情形。当提及数值或范围时，术语“约”旨在包含当进行测量时可能出现的实验误差产生的值。

如本文中使用，除非另外明确地陈述，否则“可(能)操作(用于)”意指能够被使用、安装、或准备好使用或服务、可用于特定目的、和能够执行本文中描述的陈述的或期望的功能。关于系统和设备，术语“可(能)操作(用于)”意指系统和/或设备是完全地功能化和被校准的，包括用于和满足可应用的操作要求的元件，以在激活时执行所陈述的功能。

在本文中使用的术语和短语和其变体，除非另外明确地陈述，否则应当解释为开放式的，而不是限制性的。作为上述的实例：术语“包括”应当理解为“包括，但不限于”等；术语“实例”用于提供所讨论项目的示例性实例，而不是其穷尽的或限制的清单；并且形容词诸如“惯用的”、“传统的”、“常规的”、“标准的”、“已知的”和类似含义的术语不应当解释为将描述的项目限制为特定时期的项目或特定时间可获得的项目，而是应当理解为包含现在或在将来任何时候可以获得或已知的惯用的、传统的、常规的、或标准的技术。

图1示出本发明提供的自动驾驶定位方法的一实施例的流程图。该自动驾驶定位方法包括以下步骤：

⑴加载地图，该地图可以但不限于高精地图，其包括高精点云地图(也可以是视觉特征点地图)和对应车道线地图。

并获取当前时刻t下的传感器数据。具体的是，当前自身车辆坐标系下的激光点云数据P_t，并使用点云车辆检测模型和点云行人检测模型检测P_t中的其他车辆和行人部分，并去除P_t中的类别为其他车辆和行人的点。其中，点云车辆检测模型和点云行人检测模型为本领域技术人员所熟知的任何可起到上述作用的模型。

⑵沿车道线方向进行自身车辆的初始姿态采样。具体的是，如图2所示，根据上一时刻t-1下的姿态T_t-1估算当前时刻t的姿态T_t ^*。假设上一时刻点云P_t-1的定位姿态为T_t-1，根据上一时刻获取传感器数据的轮速信息s_t-1，得到当前时刻t下的初始姿态T_t ^*根据以下公式计算：

T_t ^*＝T_t-1+D(T_t-1)*s_t-1

其中，T_t-1为上一时刻t-1的姿态，s_t-1为上一时刻t-1的轮速信息，D(T_t-1)为朝向提取函数，用以获得T_t-1对应的车辆朝向向量。T_t-1中包含朝向信息，D(T_t-1)可以认为是将T_t-1中的位移置0后的变换矩阵。

如图2所示，计算当前时刻t对应车道中心线上的姿态采样点序列{T_c ⁱ}₀ ^2*Nx+1。具体的是，设定车道线方向单位搜索距离为d_x，上一时刻更新后的单位搜索数量记为N_x，从车道线地图中获取初始姿态T_t ^*所在车道中心线C，并计算T_t ^*在车道中心线C上的投影点T_c，以等距间隔d_x在车道中心线C上T_c的前后各取N_x个点，与T_c一并组成(2*N_x+1)个C上的点，即为姿态采样点序列{T_c ⁱ}₀ ^2*Nx+1。d_x设置可以根据实际的计算资源和精度要求进行选择，N_x根据每一次定位的结果进行动态调整。优选的是，d_x＝0.2～0.5cm，N_x最小值N_x ^min＝5，N_x最大值N_x ^max根据计算力而设定。

如图3所示，计算当前时刻t的车辆定位初始姿态序列{T_t ⁱ}₀ ^2*Nx+1。具体的是，计算初始位姿T_t ^*距离车道中心线C的横向距离为y₀，将姿态采样点序列{T_c ⁱ}₀ ^2*Nx+1的各姿态点在垂直于车道中心线C的方向上向T_t ^*所在一侧平移y₀，得到车辆定位初始姿态序列{T_t ⁱ}₀ ^2*Nx+1。

需要注意的是每个姿态点的朝向yaw角的计算，假设的T_t ^*朝向角为yaw^*，而对于其他某个初始搜索姿态T_t ⁱ的朝向角yawⁱ，计算其对应T_c ⁱ在车道中心线C上的切线角与T_c处切线角的差值为△yawⁱ，即yawⁱ＝yaw^*+△yawⁱ。

⑶对车辆定位初始姿态序列的每个初始搜索姿态的变量空间进行遍历。计算每个初始搜索姿态值T_t ⁱ附近姿态遍历得到的最高评分值Score_t ^i*，以及最高评分值对应的最优姿态值T_t ^i*。具体的是，如图4所示，(x_w，y_w)为世界坐标系，T_t ⁱ对应的局部坐标系为(x_i，y_i)。设定变量roll,pitch,yaw,y_local,和z的搜索步长和搜索步长数分别为[d_roll,d_pitch,d_yaw,d_y,d_z]和[N_roll,N_pitch,N_yaw,N_y,N_z]，其中，变量y_local表示T_t ⁱ对应的局部坐标系中的y_i方向，亦为垂直于车道中心线的方向。对于每个初始搜索姿态值T_t ⁱ，通过遍历所有以T_t ⁱ为初始姿态值的姿态空间，得到评分值最高的姿态值T_t ^i*为最优姿态，并记录其最高得分Score_t ^i*为最高评分值，并组成评分值序列{Score_t ^i*}₀ ^2*Nx+1，遍历算法如下：

其中Score(P_t,T)为评分函数，其值反应了当前点云P_t在姿态T的情况下，与点云地图的匹配程度，具体实现可以通过统计点云P_t中每个激光点p在姿态T的变换下，其对应的3D网格点在点云地图中的占有情况。

⑷对评分值序列的每个最高评分值进行归一化处理。具体的是，对评分值序列归一化处理的计算公式为：

N(Score_t ^i*)＝(Score_t ^i*-Score_min)/(Score_max-Score_min)

其中，Score_t ^i*为评分值序列{Score_t ^i*}₀ ^2*Nx+1中的第i个最高评分值，Score_min为评分值序列{Score_t ^i*}₀ ^2*Nx+1中的最小值，Score_max为评分值序列{Score_t ^i*}₀ ^2*Nx+1中的最大值。

对归一化的评分值序列做有效性判断，即判断最高评分值对应的定位姿态是否为有效的定位结果。具体的是，若归一化处理后的评分值序列中，只要有一个最高评分值大于第一阈值t_score，且其对应的最优姿态T_t ^i*与评分值序列中最大值Score_max所对应的最优姿态T_t ^max*之间的距离大于第二阈值t_distance，则判定评分值序列为无效；否则，判定评分值序列为有效。优选的是，第一阈值t_score＝0.6～0.8，第二阈值t_distance＝0.3～0.5。

⑸若判定评分值序列为有效，则以评分值序列{Score_t ^i*}₀ ^2*Nx+1中评分最高的最高评分值Score_max对应的最优姿态T_t ^i_max作为最终的定位结果。进一步的，以T_t ^i_max为姿态初始值，使用ICP算法(迭代最近点算法)对T_t ^i_max进行优化。当前时刻t对应的车辆姿态T_t等于在T_t ^i_max基础下点云ICP优化后的姿态，并将定位不可信距离S_failed设置为0。

若判定评分值序列为无效，则以初始姿态T_t ^*附近的最优姿态作为最终的定位结果。具体的是，当前时刻t对应的车辆姿态T_t等于T_t ^*对应的T_t ^i*，即，步骤⑴中的初始位姿T_t ^*附近，通过步骤⑶中得到的最高评分值对应的最优姿态。并更新定位不可信距离S_failed，S_failed＝S_failed+|T_t ^*-T_t-1|，|T_t ^*-T_t-1|表示从姿态T_t-1到姿态T_t ^*的欧式距离，同时更新N_x，N_x＝min(N_x ^max，2*E(S_failed)/d_x)，一般随着定位评定无效的累计，初始采样点的数量会不断增加，直到达到最大值。其中，E(S_failed)为关于S_failed的函数，其表示在仅使用轮速计的情况下，车辆运动估算S_failed距离后，与真实距离的误差值。函数E(S_failed)可以通过在GPS信号好的情况下进行统计拟合得到，可以使用一个关于S_failed的n阶的多项式进行拟合，也可以用其他形式的函数进行拟合。

图5示出本发明提供的自动驾驶定位系统的一实施例的结构示意图。该自动驾驶定位系统包括：

加载模块，用于加载地图，并获取当前时刻的传感器数据；

采样模块，用于沿车道线方向进行自身车辆的初始姿态采样，计算得到车辆定位初始姿态序列；

遍历计算模块，用于对车辆定位初始姿态序列的每个初始搜索姿态的变量空间进行遍历，计算得到每个初始搜索姿态附近的最高评分值和最高评分值对应的最优姿态，组成评分值序列；

判定模块，用于对评分值序列的每个所述最高评分值进行归一化处理，并判断最高评分值是否为有效的定位结果；和

输出模块，用于若判定为有效的定位结果，则以评分值序列中评分最高的最高评分值对应的最优姿态作为最终的定位结果；若判定为无效的定位结果，则以初始姿态附近的最优姿态作为最终的定位结果。

进一步的，采样模块执行以下操作：

从传感器数据中获取初始姿态所在的车道中心线，计算初始姿态在车道中心线上的投影点，得到当前时刻对应车道中心线上的姿态采样点序列；

计算初始姿态距离车道中心线的横向距离，将姿态采样点序列在垂直于车道中心线的方向上向初始姿态所在一侧平移横向距离，得到当前时刻的车辆定位初始姿态序列。

进一步的，图6示出本发明提供的自动驾驶定位系统的另一实施例的结构示意图。该自动驾驶定位系统还包括：

优化模块，用于若判定为有效的定位结果，则使用IPC算法对所述最终的定位结果进行优化，并将定位不可信距离设置为0。

该实施例中的自动驾驶定位系统的工作方式和过程如上述实施例中的自动驾驶定位方法相同，在此不再具体赘述。

以上实施例仅用以说明本发明的技术方案，并非用来限定本发明的实施范围。但凡在本发明的保护范围内所做的等效变化及修饰，皆应认为落入了本发明的保护范围内。

Claims (10)

1.一种用于长距离隧道的自动驾驶定位方法，其特征在于，该方法包括以下步骤：

⑴加载地图，并获取当前时刻的传感器数据；

⑵沿车道线方向进行自身车辆的初始姿态采样，计算得到车辆定位初始姿态序列；

⑶对所述车辆定位初始姿态序列的每个初始搜索姿态的变量空间进行遍历，计算得到每个所述初始搜索姿态附近的最高评分值和所述最高评分值对应的最优姿态，组成评分值序列；

⑷对所述评分值序列的每个所述最高评分值进行归一化处理，并判断所述评分值序列是否有效；

⑸若判定所述评分值序列为有效，则以所述评分值序列中评分最高的最高评分值对应的最优姿态作为最终的定位结果；

若判定所述评分值序列为无效，则以所述初始姿态附近的最优姿态作为最终的定位结果。

2.根据权利要求1所述的自动驾驶定位方法，其特征在于，所述地图包括高精点云地图和对应车道线地图。

3.根据权利要求1所述的自动驾驶定位方法，其特征在于，所述初始姿态根据以下公式计算：

T_t ^*＝T_t-1+D(T_t-1)*s_t-1

其中，T_t-1为t-1时刻的姿态，s_t-1为t-1时刻的轮速信息，D(T_t-1)为朝向提取函数，用以获得T_t-1对应的车辆朝向向量。

4.根据权利要求1所述的自动驾驶定位方法，其特征在于，步骤⑵包括：

从所述传感器数据中获取所述初始姿态所在的车道中心线，计算所述初始姿态在所述车道中心线上的投影点，得到当前时刻对应所述车道中心线上的姿态采样点序列；

计算所述初始姿态距离所述车道中心线的横向距离，将所述姿态采样点序列在垂直于所述车道中心线的方向上向所述初始姿态所在一侧平移所述横向距离，得到当前时刻的车辆定位初始姿态序列。

5.根据权利要求1所述的自动驾驶定位方法，其特征在于，所述归一化处理的计算公式为：

N(Score_t ^i*)＝(Score_t ^i*-Score_min)/(Score_max-Score_min)

其中，Score_t ^i*为评分值序列中的第i个最高评分值，Score_min为评分值序列中的最小值，Score_max为评分值序列中的最大值。

6.根据权利要求1所述的自动驾驶定位方法，其特征在于，步骤⑷包括：

若归一化后的评分值序列中，只要有一个最高评分值大于第一阈值，且其对应的最优姿态与归一化后的评分值序列中最大值所对应的最优姿态之间的距离大于第二阈值，则判定所述评分值序列为无效；否则，判定所述评分值序列为有效。

7.根据权利要求1所述的自动驾驶定位方法，其特征在于，步骤⑸包括：

若判定为有效的定位结果，则使用ICP算法对最终的定位结果进行优化，并将定位不可信距离设置为0。

8.一种用于长距离隧道的自动驾驶定位系统，其特征在于，该系统包括：

加载模块，用于加载地图，并获取当前时刻的传感器数据；

采样模块，用于沿车道线方向进行自身车辆的初始姿态采样，计算得到车辆定位初始姿态序列；

遍历计算模块，用于对所述车辆定位初始姿态序列的每个初始搜索姿态的变量空间进行遍历，计算得到每个所述初始搜索姿态附近的最高评分值和所述最高评分值对应的最优姿态，组成评分值序列；

判定模块，用于对所述评分值序列的每个所述最高评分值进行归一化处理，并判断所述最高评分值是否为有效的定位结果；和

输出模块，用于若判定为有效的定位结果，则以所述评分值序列中评分最高的最高评分值对应的最优姿态作为最终的定位结果；若判定为无效的定位结果，则以所述初始姿态附近的最优姿态作为最终的定位结果。

9.根据权利要求8所述的自动驾驶定位系统，其特征在于，该系统还包括：

优化模块，用于若判定为有效的定位结果，则使用ICP算法对所述最终的定位结果进行优化，并将定位不可信距离设置为0。

10.根据权利要求8所述的自动驾驶定位系统，其特征在于，所述采样模块执行以下操作：

从所述传感器数据中获取所述初始姿态所在的车道中心线，计算所述初始姿态在所述车道中心线上的投影点，得到当前时刻对应所述车道中心线上的姿态采样点序列；

计算所述初始姿态距离所述车道中心线的横向距离，将所述姿态采样点序列在垂直于所述车道中心线的方向上向所述初始姿态所在一侧平移所述横向距离，得到当前时刻的车辆定位初始姿态序列。

Images