[发明专利]一种用于长距离隧道的自动驾驶定位方法及系统

权利要求书

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对应的车辆朝向向量；

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

计算所述初始姿态距离所述车道中心线的横向距离，将所述姿态采样点序列在垂直于所述车道中心线的方向上向所述初始姿态所在一侧平移所述横向距离，得到当前时刻的车辆定位初始姿态序列；⑶对所述车辆定位初始姿态序列的每个初始搜索姿态的变量空间进行遍历，计算得到每个所述初始搜索姿态附近的最高评分值和所述最高评分值对应的最优姿态，组成评分值序列；

(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的情况下，与点云地图的匹配程度；

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

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

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

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