[发明专利]一种基于路径点偏移的局部路径规划方法

权利要求书

1.一种基于路径点偏移的局部路径规划方法，其特征在于，包括如下步骤：

S1：获取期望路径信息、当前车辆位姿和障碍物信息；

S2：根据S1获取的各信息，进行初始期望路径点偏移绕过障碍物；

S3：根据S2的偏移点，计算偏移后的路径点航向角增量和方位角增量；

S4：车辆跟踪初始期望路径计算出的航向角和方位角，在此基础上加上S3中的航向角增量和方位角增量；

S5：将S4的航向角和方位角输出给控制层对车辆进行控制；

所述S1中的障碍物包括静态障碍物和动态障碍物，关于动态障碍物位置的判断，需要根据运动预测来判断是否阻碍初始期望路径，若动态障碍物阻碍初始期望路径，则将动态障碍物预测到的位置作为当前静态障碍物；

所述S2中的路径点偏移分为四个阶段：

第一阶段：车辆逐渐靠近障碍物过程，即avoid_lengthx1；avoid_length为避障累计距离；x1为雷达检测到障碍物后，车辆与障碍物之间的距离；若avoid_lengthx1则跳转到第二阶段；

第二阶段：车辆与障碍物之间的距离超过x1，但雷达仍能够检测到障碍物，即avoid_lengthx1，并且雷达与障碍物之间的横向距离不等于0；当雷达与障碍物之间的横向距离等于0时，跳转到第三阶段；

第三阶段：若雷达传感器有盲区时，障碍物进入到雷达的盲区时，需要保持方向盘不变，避免没有绕过障碍物就开始转弯；若雷达传感器没有盲区则不需要此阶段，直接跳到第四阶段；

第四阶段：经历完第三阶段后，回到初始期望路径；

相应于所述S2的四个阶段，所述S3中航向角增量angle_h和方位角增量angle_p计算也分为四个阶段：

第一阶段：车辆避障已经行驶距离为x处的横向偏移距离：height＝A*cos(k*x)-A，式中A＝avoid_width/2，k＝π/x1，avoid_width表示避障过程中车辆与初始期望路径最远横向距离；当x＝x1时，height＝avoid_width；根据此函数就可以确定行驶距离为x处的斜率为：slope＝-A*k*sin(k*x)，pre_dis为预瞄距离，此阶段angle_h和angle_p：

第二阶段：此阶段仍能够检测到障碍物，保持方向盘转角不变行驶，并将跳出此阶段的行驶距离存为model 2_length，此阶段angle_h和angle_p：

第三阶段：障碍物在雷达盲区中，保持方向盘转角不变行驶，并将跳出此阶段的行驶距离存为model 3_length，此阶段angle_h和angle_p：

第四阶段：x处的横向偏移距离：height＝A*cos(k*(x-model 2_length)+π)-A，其中A＝avoid_width/2，k＝π/x1；根据此函数可以确定行驶距离为x处的斜率为：slope＝-A*k*sin(k*(x-model 2_length)+π)；pre_dis为预瞄距离；此阶段angle_h和angle_p：

2.如权利要求1所述的一种基于路径点偏移的局部路径规划方法，其特征在于，避障过程中车辆与初始期望路径最远横向距离：avoid_width＝dis_ob+vehicle_width/2+Δl；式中dis_ob为障碍物边界与车辆初始期望路径的最小距离，vehicle_width为车辆宽度，Δl为阈值。