[发明专利]一种基于碰撞检测的动态障碍物避让方法

权利要求书

1.一种基于碰撞检测的动态障碍物避让方法，其特征在于，包括：

初规划步骤：在车辆行驶过程中对车辆的行驶路径和速度进行初步规划，得到车辆的期望路径、期望速度数据，构造车辆期望轨迹；

碰撞检测步骤：基于接收的动态障碍物预测轨迹和所述车辆期望轨迹，进行碰撞检测，判断车辆是否会与障碍物发生碰撞；

重规划步骤：当判断会发生碰撞后，对车辆的行驶速度和/或路径进行重新规划，得到车辆无碰撞行驶速度和/或路径数据，重新构造车辆期望轨迹，避让动态障碍物；

所述初规划步骤中行驶路径的初步规划为根据车辆当前状态，生成一条从车辆当前位置到参考线的行驶曲线，具体包括：

1)给定车辆当前车辆状态q_init(x_init,y_init,θ_init,κ_init)、车辆终点状态q_end(x_end,y_end,θ_end,κ_end)；其中x_init,y_init为车辆当前的x，y坐标，θ_init为车辆当前航向角，κ_init为车辆当前轨迹曲率；其中x_end,y_end为车辆终点位于参考线上的x，y坐标，θ_end为车辆终点航向角，κ_end为车辆终点轨迹曲率；

2)将路径规划问题转化为解

为路径平滑目标函数，

s.t.q＝q_init，s＝0；

q＝q_end，s＝s_f；

0≤s_f≤s_max；s_max为曲线弧长的上界；

函数κ(p)的参数a为已知的车辆当前状态所对应的曲率，q为车辆状态；通过求解得到函数κ(p)的参数b、c、d、s_f；

3)通过公式：使用不同的弧长s进行采样，得到路径规划的路径点集数据。

2.根据权利要求1所述的动态障碍物避让方法，其特征在于，所述碰撞检测为分层碰撞检测，包括二维空间碰撞检测和三维时间空间碰撞检测。

3.根据权利要求2所述的动态障碍物避让方法，其特征在于，

所述碰撞检测中的二维空间碰撞检测，根据车辆期望轨迹的路径数据和动态障碍物预测轨迹的路径数据，计算车辆与障碍物的最近距离，若最近距离小于碰撞距离阈值，则进行三维时间空间碰撞检测；否则，判断不会发生碰撞。

4.根据权利要求2所述的动态障碍物避让方法，其特征在于，所述碰撞检测中的三维时间空间碰撞检测包括：

构造三维时空轨迹；三维时空的x轴和y轴为路径点的横、纵坐标，z轴为到达每个路径点的时间；在三维时空中，根据动态障碍物的几何信息、预测轨迹数据以及车辆的几何信息、期望轨迹数据，得到动态障碍物预测轨迹和车辆期望轨迹的点阵序列；

粗略检测；根据动态障碍物预测轨迹和车辆期望轨迹的点阵序列，进行三维重构，形成以三维轨迹点为边界的动态障碍物三维轨迹凸体和车辆三维轨迹凸体，对两个三维轨迹凸体之间的冲突区域进行检测，得到粗略的发生碰撞的轨迹点；

精细检测；锁定冲突区域，进行精细阶段的碰撞检测，得到精确的发生碰撞的轨迹点。

5.根据权利要求4所述的动态障碍物避让方法，其特征在于，所述重规划步骤中，当判断会与动态障碍物轨迹发生碰撞时，

1)首先启动速度重规划，进行纵向时间层面的状态空间采样，基于同一条路径形成多条不同时间序列的轨迹，与动态障碍物进行碰撞检测，得到车辆无碰撞速度数据，控制车辆避让动态障碍物；

2)若全部轨迹都发生碰撞，则此时切换成路径重规划，进行横向空间层面的状态空间采样，形成多条基于不同路径的轨迹，并与动态障碍物进行碰撞检测，得到车辆无碰撞行进路径和速度数据，控制车辆避让动态障碍物；

3)若全部轨迹都发生碰撞，则计算当前所处轨迹与动态障碍物碰撞的最近距离，基于当前所处轨迹减速停车。