一种非结构化环境下无人车轨迹规划方法

本发明属于无人驾驶，具体涉及一种非结构化环境下无人车轨迹规划方法。

背景技术：

1、无人驾驶系统通常由感知、决策、规划和控制模块组成，其中规划模块的核心任务是生成满足车辆运动学约束且无碰撞的可行驶轨迹。规划模块中的轨迹规划方案直接反映了无人驾驶车辆的智能水平。

2、目前，在非结构化环境中常用的轨迹规划方法为传统的时空解耦规划方法，这类方法的核心思想是在静态障碍物的约束下首先进行路径规划，随后基于生成的路径再针对动态障碍物进行速度规划。在具有高度不确定性和复杂性的非结构化环境中，传统的时空解耦方法与动态障碍物的交互能力较弱，导致生成的轨迹容易陷入局部次优，从而限制了无人驾驶汽车在非结构化环境中的避障表现。

技术实现思路

1、针对上述现有技术的缺点，本发明提供一种非结构化环境下无人车轨迹规划方法。

2、为实现上述目的，本发明采取的技术方案为：一种非结构化环境下无人车轨迹规划方法，通过激光雷达来感知周围环境，使用imu为自车提供定位信息，然后经过初始轨迹搜索、构建自车在时空域内的可行驶区域和轨迹优化来得到最终轨迹，步骤如下：

3、s1、在笛卡尔坐标系下基于车辆运动学模型构建运动基元树，对运动基元树进行搜索，获取初始轨迹，步骤如下：

4、s1.1、根据车辆二自由度运动学模型可得车辆微分方程，表达式如下：

5、

6、式中，x，y分别为车辆后轴中心点坐标，其中，x为中心点的横坐标，y为中心点的纵坐标；v为车辆纵向速度；l为车辆轴距；为车身方向角；δ为前轮转角；a为车辆加速度；分别表示对车辆后轴中心点坐标、车身方向角、车辆纵向速度求导；

7、s1.2、将车辆加速度a和前轮转角δ分别作为车辆的纵向控制量和横向控制量，其中a∈[amin,amax]，δ∈[-δmin,δmax]；把车辆的纵向控制量和横向控制量离散化为a∈{a1,a2,...,an}，δ∈{δ1,δ2,...,δn}，n的取值和轨迹正相关，并向前模拟时间长度为τ；在节点扩展过程中，以固定时间间隔δt和最小扩展步长dmin进行逐步节点扩展，表达式如下：

8、

9、定义为车辆状态空间，设给定时刻t的状态为xt，对应的控制量为at和δt，则下一时刻的状态xt+1可表示为：

10、

11、式中，d为向前模拟过程中车辆的行驶距离，r为车辆的转弯半径，xt、yt为车辆在给定时刻t的后轴中心点坐标，为车辆在给定时刻t的方向角，vt为车辆在给定时刻t的纵向速度，at为车辆在第t时刻的加速度，tt为车辆在给定时刻t的时间，为车辆在第t+1时刻的方向角；

12、通过对离散化控制量{a1,a2,...,an}和{δ1,δ2,...,δn}的扩展，形成多层运动基元，即运动基元树；随后，在运动基元树上进行初始轨迹搜索，以获取初始轨迹；

13、s2、将初始轨迹离散化为状态点，并投影到xyt构型空间中，以状态点为基础构建自车在时空域内的可行驶区域；

14、状态点x1,x2,...,xn和轨迹正相关；

15、构建步骤包括：

16、s2.1、构建时空栅格地图，以描述未来一段时间内障碍物在时空中的分布信息；

17、s2.2、基于时空栅格地图与初始轨迹，构建自车在时空域内的可行驶区域，即三维时空走廊；

18、在构建时空栅格地图过程中，为简化构建流程，将障碍物(环境复杂程度)分类为静态障碍物和动态障碍物两类分别进行处理；

19、针对静态障碍物，因其位置固定不变，为提升算法运行效率，可将其视为在整个离散化时间轴t上占据相同空间的障碍物，无需进行轨迹预测；

20、针对动态障碍物，由于其位置随时间发生变化，需要进行轨迹预测；即采用二维定速模型对周围动态障碍物的运动轨迹进行预测，并将其预测轨迹沿离散化的时间轴进行投影，以生成时空维度上的占据信息，其中离散时间间隔为δt，根据周围环境的复杂度进行调整；

21、进一步的，构建自车在时空域内的可行驶区域过程具体包括如下步骤：

22、s2.2.1、生成种子点：为确保种子点无碰撞，首先基于初始轨迹的无碰撞性，将其离散化为状态点x1,x2,...,xn；将这些状态点投影到xyt构型空间中，作为走廊生成的种子点；

23、s2.2.2、立方体膨胀：此步骤为自车在时空域内的可行驶区域构建的核心环节；首先，遍历每个种子点，并在相邻两个种子点之间生成初始立方体，具体地，两个相邻种子点作为立方体的顶点，通过连接这两个种子点，形成初始求解空间；其次，在初始立方体的基础上，分别沿着x、y轴正负方向和t轴正方向五个方向进行膨胀；当立方体的膨胀边界与静态障碍物和动态障碍物的边界相交时停止膨胀；为了提高计算效率，在膨胀过程中，已包含在膨胀区域内的种子点可被合并至当前膨胀区域；每完成一个立方体的膨胀后，接着对下一个未处于膨胀区域内的最近立方体进行膨胀，依此类推，构建出三维时空栅格地图以及自车在时空域内的可行驶区域；

24、自车在时空域内的可行驶区域定义了一段适用于后续轨迹优化的凸自由空间，为优化算法提供了明确的约束条件，确保了优化过程中轨迹与障碍物的碰撞避免，提供了轨迹优化必要的时空保障；

25、s3、在自车在时空域内的可行驶区域内使用分段贝塞尔曲线对初始轨迹进行优化得到最终可行驶轨迹；步骤如下：

26、s3.1、在自车在时空域内的可行驶区域内生成一条由l+1个控制点组成的l阶贝塞尔曲线f(t)；生成贝塞尔曲线的表达式如下：

27、

28、式中，t表示时间，t∈[0,1]；pi为贝塞尔曲线的第i个控制点；为伯恩斯坦多项式(bernsteinbasis)；

29、贝塞尔曲线具有convex hull和hodograph两个性质，能够确保优化后的轨迹严格遵循约束条件，从而提升轨迹的平滑性和安全性；

30、s3.2、根据环境复杂程度对自车在时空域内的可行驶区域进行划分，提升轨迹优化效果；

31、具体的，当自车在时空域内的可行驶区域在膨胀阶段遇到障碍物时，则停止膨胀，因此，自车在时空域内的可行驶区域是一个分段的结构；

32、膨胀后的每个立方体构成了一段独立的自车在时空域内的可行驶区域区域，轨迹在时空域内根据自车在时空域内的可行驶区域的分段结构进行分段处理，这个过程确保每段自车在时空域内的可行驶区域区域内均由一条独立的分段贝塞尔曲线进行描述；通过这种方式，轨迹能够在各个局部自车在时空域内的可行驶区域区域内分别进行优化，提升轨迹优化的灵活性；

33、若s3.2所述的各分段轨迹分配的时间不同，需要根据每段轨迹分配的时间定义相应的缩放比例因子s；若s3.2所述的各分段轨迹分配的时间相同，则定义缩放比例因子s＝1；由此，一维空间μ∈{x,y}中的m段分段贝塞尔曲线可表示为：

34、

35、式中，表示第m段轨迹中的第h个控制点，h∈l；t0,t1,...,tm表示每段轨迹的起始和终止时间，t0,t1,...,tm∈t；

36、s3.3、以最小化加加速度(jerk)作为优化目标对初始轨迹进行优化，避免车辆突然加减速，提高轨迹的平滑性；定义第j段贝塞尔曲线的优化目标函数，j∈m；表达式如下：

37、

38、式中，fx(t)和fy(t)分别表示在x、y轴上的贝塞尔曲线，tj-1和tj表示第j段轨迹的起始和终止时间，tj-1和tj∈t；

39、定义fμj(t)为μ维度下的第j段贝塞尔曲线，t∈[0,1]未经缩放的分段贝塞尔曲线表示为gμj(t)，令γ＝(t-tj-1)/sj；则在μ维度下的第j段贝塞尔曲线的目标函数可表示为如下的二次型形式：

40、

41、式中，pj表示贝塞尔曲线gμj(t)的控制点集合，q为该条曲线的海森(hessian)矩阵；

42、s3.4、在贝塞尔曲线优化过程中，分别设定轨迹的安全和动力学约束、首末状态约束、连续性约束条件；

43、安全和动力学约束：基于贝塞尔曲线的convex hull和hodograph特性，确保轨迹、速度曲线以及加速度曲线均被严格约束在由控制点构成的凸包内，即：

44、

45、式中，和分别表示第j段曲线k阶导数的上下界；

46、首末状态约束：将轨迹优化的起始和终止状态分别约束在自车后轴中心点和规划目标位置，即：

47、

48、式中，和分别表示自车后轴中心点和规划目标位置的k阶导数；

49、连续性约束：为保证轨迹的平滑性，应确保每段贝塞尔曲线在连接点处的各阶导数均连续，即：

50、

51、轨迹的安全和动力学约束、首末状态约束、连续性约束条件表达式中，k的取值包括：0，1，2和3；

52、具体的，轨迹的安全和动力学约束表达式中，k取0，1和2，其中，当k＝0时对轨迹位置进行约束，当k＝1时对速度曲线进行约束，当k＝2时对加速度曲线进行约束；

53、具体的，轨迹的连续性约束表达式中，k取0，1和2；

54、具体的，轨迹的连续性约束表达式中，k取3，表示表示对加加速度(jerk)进行约束；

55、s3.5、通过引入s3.4中的线性不等式、等式约束以及二次型目标函数，轨迹优化问题可以转化为二次规划问题(quadratic program)，并采用ooqp求解器对该二次规划问题进行求解，获取优化后的轨迹。

56、本发明的有益效果

57、本发明通过在轨迹规划过程中同时考虑时间与空间维度，使得车辆能够实现横纵向的联合运动，从而更符合人类驾驶习惯。

58、本发明生成的轨迹具有更高的灵活性，能够有效增强与动态障碍物的交互能力。其次，本发明在三维时空中构建时空可通行区域，即三维时空走廊，为后续轨迹优化算法提供了明确的约束条件，确保了轨迹与障碍物的碰撞避免，使得在具有高度不确定性和复杂性的非结构化环境下规划的轨迹更加安全。

59、本发明在自车在时空域内的可行驶区域中使用分段贝塞尔曲线进行轨迹优化，在优化过程中充分考虑了车辆的安全和动力学约束、首末状态约束、连续性约束，确保在多种约束条件下生成安全、连续且平滑的轨迹。