[发明专利]一种基于采样和路径参数化的轨迹规划方法

权利要求书

1.一种基于采样和路径参数化的轨迹规划方法，其特征在于，包括如下步骤：

S1、利用TOPPRA算法来获得每一步路径速度平方的采样范围K_i(i＝1，...，N)；

S2、从i＝1开始到N，每一步都在K_i范围上进行采样后计算是否满足三阶约束，确保最终的轨迹不会违反各个约束；

S3、在每一步中计算可连接的情况下计算两点之间所需要的时间，最终计算整个路径下时间最短所涉及的点连成的轨迹；

S4、根据采样时间求解每一时刻系统的系统构型进行控制。

2.如权利要求1所述的基于采样和路径参数化的轨迹规划方法，其特征在于，步骤S1中，所述TOPPRA算法的具体算法流程为：

S1-1、从N-1到N-2，……到0，逆向求解控制集其中N为路径点；

S1-2、从0到1……，到N-1，正向求解每一步最大路径加速度u；

S1-3、获得每一步路径速度平方x的采样范围K_i(i＝1，...，N)。

3.如权利要求1所述的基于采样和路径参数化的轨迹规划方法，其特征在于，步骤S2中，所述从i＝1开始到N，每一步都在K_i范围上进行采样后计算是否满足三阶约束，确保最终的轨迹不会违反各个约束的算法流程为：

1)、初始化树根节点为x₀；

2)、将x₀的节点放入集合中；

3)、在路径速度平方K_i(i＝1，...，N)范围上采用Samples(K_i)函数进行采样m个点，记为放入集合中；

4)、遍历集合中的每个节点

5)、遍历集合中的每个节点

6)、如果集合中的节点与集合中的节点满足冲击约束，将添加到集合中，连接到树中的采样节点；

7)、当树的深度达到了N，通过i是否等于N来判断是否有解，该规划方法至少返回一条可行结果；若是返回多条，则通过函数计算所需的时间，以时间最短为最终的最佳结果；

其中，x0为初始时刻的路径速度平方；为在第0步的初始节点；为集合中的第j个节点；为集合中的第k个节点；为在第i步已满足冲击约束，连接到树中的采样节点。

4.如权利要求3所述的基于采样和路径参数化的轨迹规划方法，其特征在于，步骤6)中，所述集合中的第j个节点，通过函数在集合中查找代价最小的父节点。

5.如权利要求4所述的基于采样和路径参数化的轨迹规划方法，其特征在于，所述函数的算法流程，包括如下步骤：

6-1)、输入第i步的集合中的第j个采样点y，以及第i-1步的集合中的所有待连接父节点；

6-2)、利用函数计算采样点与每个父节点之间的时间，并放入Cost_list集合中；

6-3)、对Cost_list集合中的元素按照从小到大进行排序；

6-4)、利用函数计算y与Cost_list第q个元素对应的父节点是否满足冲击约束；

6-5)、当步骤6-4)第一次出现结果为满足冲击约束时，即找到了父节点，并进行连接；

6-6)、当步骤6-4)所有结果都不满足冲击约束时，则无父节点存在，抛弃该采样点。

6.如权利要求5所述的基于采样和路径参数化的轨迹规划方法，其特征在于，步骤6-2)中，所述函数是在采样点连接成功之后计算从父节点到该节点的时间，其计算公式为

其中，表示在第i步时采样点j所对应的时间；表示在第i-1步时，父节点k所对应的时间。