[发明专利]一种基于虚拟约束的自动驾驶车辆路径规划方法

权利要求书

1.一种基于虚拟约束的自动驾驶车辆路径规划方法，其特征在于，包括以下步骤：

步骤S1：获取车体坐标系下的车道中心线航向；

步骤S2：利用步骤S1中获取的车道中心线航向对车辆航向进行修正；

步骤S3：利用步骤S2修正后的车辆航向对原始路径中的路径点进行校正；

步骤S4：基于三维激光雷达栅格地图，获取自动驾驶车辆与道路的相对横向位置关系；

步骤S5：根据步骤S4获取的自动驾驶车辆与道路的相对横向位置关系生成虚拟约束，所述虚拟约束包括所在车道的边界约束和邻车道边界约束；

步骤S6：根据步骤S5生成的虚拟约束和自动驾驶系统的决策指令，生成期望路径。

2.根据权利要求1所述的方法，其特征在于，所述步骤S1包括以下步骤：

步骤S101：判断车道线检测结果是否有效，若有效，执行步骤S102；若无效，沿用上一周期车道线检测结果；

步骤S102：获取车体坐标系下的车道中心线航向。

3.根据权利要求2所述的方法，所述步骤S102包括以下步骤：

步骤S1021：计算车道中心线的位置

其中，车道线检测结果(x₁,y₁)、(x₂,y₂)、(x₃,y₃)与(x₄,y₄)为左右车道线在车体坐标系下的坐标点，(x₁,y₁)为车辆左前方近车点，(x₂,y₂)为左前方远车点，(x₃,y₃)为右前方近车点，(x₄,y₄)为右前方远车点；

(x_c1,y_c1)与(x_c2,y_c2)分别代表车道中心线的近车端与远车端；

步骤S1022：计算车道中心线的航向：

其中θ_Lane代表车道中心线在车体坐标系下的航向。

4.根据权利要求2或3所述的方法，其特征在于，步骤S2采用卡尔曼滤波器对车辆航向进行修正，包括以下步骤：

步骤S201：初始化卡尔曼滤波器；

步骤S202：根据车辆当前的转弯半径和速度预测本周期内的大地坐标系下车辆航向预测值：

其中，θ_veh-pred为大地坐标系下当前时刻车辆航向的预测值，θ_{veh-correct-old}为大地坐标系下上一周期车辆航向的修正值，v为车辆当前的速度，R为车辆当前的转弯半径，T为规划周期；

步骤S203：计算车体坐标系下原始路径航向与车道中心线航向的差值：

θ_error＝θ_road-θ_Lane

其中，θ_error为车体坐标系下原始路径航向与车道中心线航向的差值，θ_road为车体坐标系下原始路径的航向，θ_Lane为车道中心线航向；

步骤S204：计算大地坐标系下车辆的航向测量值：

θ_veh-measure＝θ_veh-gps+θ_error，

其中θ_veh-measure为大地坐标系下车辆的航向测量值，θ_veh-gps为大地坐标系下车辆的GPS航向值；

步骤S205：将步骤S202中θ_veh-pred与步骤S204中θ_veh-measure作为卡尔曼滤波器的输入得到车辆在大地坐标系下的航向修正值θ_veh-correct；

步骤S206：将θ_veh-correct赋值给θ_{veh-correct-old}并进入到下一周期。