CN114721433B - 一种基于无人机的无碰撞多项式轨迹生成方法 - Google Patents

Abstract

本发明公开了一种基于无人机的无碰撞多项式轨迹生成方法，包括：1)利用加入椭圆约束的改进RRT*路径搜索算法进行路径搜索，得到一系列无障碍点；2)使用梯形时间分配准则为经过一系列无障碍点的多项式轨迹分配时间；3)使用Minimum snap算法规划出满足时间分配的多项式轨迹；4)若检测到多项式轨迹发生碰撞，则利用插值法重新生成多项式轨迹，最终得到无碰撞多项式轨迹；本发明加入椭圆约束大大加快RRT*路径搜索算法向最短路径收敛的速度；使用梯形时间分配准则，提高了Minimum snap算法生成的多项式轨迹的质量；当检测到生成的多项式轨迹发生碰撞时，利用插值法重新生成多项式轨迹，保证轨迹不会与障碍物发生碰撞；本发明适用性广，具有广阔的应用前景。

Description

一种基于无人机的无碰撞多项式轨迹生成方法

技术领域

本发明涉及运动规划的技术领域，尤其是指一种基于无人机的无碰撞多项式轨迹生成方法。

背景技术

近年来，随着无人机的算法趋于成熟以及硬件算力的不断提高，无人机的应用也变得更加广泛，由此展开了对无人机运动规划的研究，主流的运动规划由路径搜索和轨迹规划两大方面组成。

路径搜索的目的是获取一条连接起点到终点的无障碍路径，其中典型的算法包括A*算法与RRT*算法；A*算法优点是生成的路径是全局最优的，但是缺点是时间复杂度较高，不适合在线搜索；RRT*算法优点是能够快速生成一条可行路径，但缺点是该可行路径收敛到全局最优路径的速度较慢。

轨迹规划的目的是将路径搜索得到的点光滑的连接起来，使得生成的轨迹满足无人机的动力学约束的同时会满足一些性能指标，比如路径最短、耗能最低等。其中典型的算法为Minimum snap算法，其优点是生成的轨迹光滑且耗能最低，但是缺点有二：其一是缺乏一种有效的轨迹时间分配策略，导致生成的轨迹容易出现“打结”，降低轨迹质量；其二是当生成的轨迹与障碍物发生碰撞时，缺乏一种有效的方法对轨迹进行重规划，重新生成不与障碍物发生碰撞的轨迹。

发明内容

本发明的目的在于克服现有技术的缺点与不足，提出了一种基于无人机的无碰撞多项式轨迹生成方法，提出了增加椭圆约束的改进RRT*路径搜索算法，大大加速可行路径收敛速度；使用梯形时间分配准则，为Minimum snap算法提供了较好的时间分配，避免Minimum snap算法生成的多项式轨迹出现“打结”现象；当检测到Minimum snap算法生成的多项式轨迹发生碰撞时，利用插值法重新生成多项式轨迹，最终得到无碰撞多项式轨迹。

为实现上述目的，本发明所提供的技术方案为：一种基于无人机的无碰撞多项式轨迹生成方法，包括以下步骤：

1)在含有障碍物的地图上，利用改进RRT*路径搜索算法进行路径搜索，得到n+1个无障碍点，n为正整数；其中，所述无障碍点是指不在障碍物内的点，所述改进RRT*路径搜索算法是在RRT*路径搜索算法的基础上加入了椭圆约束，保证生成的随机采样点约束在椭圆内部或椭圆上；

2)使用梯形时间分配准则为经过n+1个无障碍点的n条多项式轨迹分配时间；

3)使用Minimum snap算法规划出满足时间分配的n条多项式轨迹；

4)对满足时间分配的n条多项式轨迹进行碰撞检测，若检测到多项式轨迹发生碰撞，则利用插值法重新生成多项式轨迹，最终得到无碰撞多项式轨迹。

进一步，在步骤1)中，所述RRT*路径搜索算法是一种通过在地图中生成随机采样点，进而扩展快速搜索随机树，从而得到连接起点X_init与终点X_goal的可行路径的一种路径搜索算法；其中随机采样点是指随机生成的含有二维坐标信息的点；快速搜索随机树是一种含有点与边的计算机数据结构；可行路径是指连接X_init与X_goal并且不经过障碍物的折线段；

所述椭圆约束指的是在RRT*路径搜索算法中，若生成的随机采样点落在了以X_init与X_goal为两焦点，c_best为长轴的椭圆外，则重新进行采样，保证生成的随机采样点约束在椭圆内部或椭圆上，表示为：

c_best≥||X_init-X_rand||+||X_goal-X_rand||

式中，X_rand是随机采样点，||X_init-X_rand||表示X_init与X_rand的直线距离，||X_goal-X_rand||表示X_goal与X_rand的直线距离；当改进RRT*路径搜索算法找到一条更优可行路径时，即满足：

c_best≥||X_init-X₁||+||X₁-X₂||+||X₂-X₃||+...+||X_k-X_goal||

式中，X₁,X₂...X_k是通过改进RRT*路径搜索算法搜索得到的k个点，其中k是正整数；此时更新c_best，表示为：

c_best←||X_init-X₁||+||X₁-X₂||+||X₂-X₃||+...+||X_k-X_goal||

式中，←表示赋值号；

由于RRT*路径搜索算法在运行过程中会产生许多对当前路径搜索没有任何提升的点，在RRT*路径搜索算法中加入椭圆约束后能够有效地排除这些点，通过不断更新、缩小c_best，不断加速路径搜索过程向最短路径收敛；其中最短路径指的是连接X_init与X_goal并且不经过障碍物的最短折线段。

进一步，在步骤2)中，所述梯形时间分配准则是指按照如下函数分配各段轨迹的时间：

式中，S_j代表第j段轨迹两端点之间的直线距离，v_m代表无人机的最大速度，a_m代表无人机的最大加速度，T(S_j)代表输入量为S_j、输出为轨迹的分配时间。

进一步，在步骤3)中，所述Minimum snap算法指的是一种输入值为X_p和X_q，输出值为以X_p为起点、X_q为终点的多项式轨迹的算法，并满足生成的多项式轨迹是以X_p为起点、X_q为终点的所有多项式轨迹中耗能最低的；其中X_p和X_q为平面上的任意两个无障碍点，在这里使用的是7阶多项式，表示为：

f(t)＝p₀+p₁t+p₂t²+...+p₇t⁷

式中，t表示时间，p₀,p₁,...,p₇分别是多项式轨迹f(t)的各阶系数。

进一步，在步骤4)中，所述碰撞检测是指以规定的时间步长dt来对某段轨迹进行采样，得到若干个轨迹上的点，即X₀,X_dt,X_2dt...X_T，其中该段轨迹的起始时间为0，终止时间为T；当X₀,X_dt,X_2dt...X_T都不在障碍物内时，定义为无碰撞，否则定义为有碰撞；dt是一个可调参数，当dt越小，碰撞检测越慢，检测结果越准确；当dt越大，碰撞检测越快，检测结果越不准确；

插值法重新生成多项式轨迹的过程如下：

当发现第j条轨迹与障碍物发生碰撞时，需要取X_mid＝(X_j+X_j+1)/2，其中X_j,X_j+1分别是第j条轨迹的起点与终点，X_mid是以X_j与X_j+1为两端点的线段的中点，对X_j,X_mid,X_j+1这三点重新使用Minimum snap算法规划出两条多项式轨迹，若生成的两条多项式轨迹又发生碰撞，则重复递归地执行上述过程，直到所有多项式轨迹与障碍物都没有发生碰撞为止。

本发明与现有技术相比，具有如下优点与有益效果：

1、本发明加入椭圆约束大大加快RRT*路径搜索算法向最短路径收敛的速度。

2、本发明使用梯形时间分配准则，避免Minimum snap算法生成的多项式轨迹出现“打结”现象，提高了生成的多项式轨迹的质量。

3、本发明当检测到生成的多项式轨迹发生碰撞时，利用插值法重新生成多项式轨迹，保证多项式轨迹不会与障碍物发生碰撞。

4、本发明在无人机运动规划任务中能够迅速生成一条光滑、无障碍且满足耗能最低的多项式轨迹，对各种障碍物环境具有普遍适应性，具有广阔的应用前景。

附图说明

图1是本发明方法的流程示意图。

图2是采样500次生成的可行路径图，路径长度820。

图3是采样2500次生成的可行路径图，路径长度725。

图4是采样6000次生成的可行路径图，路径长度693。

图5是采样8000次生成的可行路径图，路径长度692。

图6是经Minimum snap算法规划后的多项式轨迹图。

图7是生成的多项式轨迹与障碍物发生碰撞图。

图8是第一次利用插值法重新生成的多项式轨迹图。

图9是第二次利用插值法生成的无障碍多项式轨迹图。

具体实施方式

下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。

如图1至图9所示，本实施例提供了一种基于无人机的无碰撞多项式轨迹生成方法，使用了改进RRT*路径搜索算法、梯形时间分配准则以及插值法，最终生成无碰撞多项式轨迹，包括以下步骤：

1)在含有障碍物的地图上，利用改进RRT*路径搜索算法进行路径搜索，得到n+1个无障碍点，n为正整数；其中，所述无障碍点是指不在障碍物内的点，所述改进RRT*路径搜索算法是在RRT*路径搜索算法的基础上加入了椭圆约束，保证生成的随机采样点约束在椭圆内部或椭圆上；

RRT*路径搜索算法是一种通过在地图中生成随机采样点，进而扩展快速搜索随机树，从而得到连接起点X_init与终点X_goal的可行路径的一种路径搜索算法；其中随机采样点是指随机生成的含有二维坐标信息的点；快速搜索随机树是一种含有点与边的计算机数据结构；可行路径是指连接X_init与X_goal并且不经过障碍物的折线段；

椭圆约束指的是在RRT*路径搜索算法中，若生成的随机采样点落在了以X_init与X_goal为两焦点，c_best为长轴的椭圆外，则重新进行采样，保证生成的随机采样点约束在椭圆内部或椭圆上，表示为：

c_best≥||X_init-X_rand||+||X_goal-X_rand||

式中，X_rand是随机采样点，||X_init-X_rand||表示X_init与X_rand的直线距离，||X_goal-X_rand||表示X_goal与X_rand的直线距离；当改进RRT*路径搜索算法找到一条更优可行路径时，即满足：

c_best≥||X_init-X₁||+||X₁-X₂||+||X₂-X₃||+...+||X_k-X_goal||

式中，X₁,X₂...X_k是通过改进RRT*路径搜索算法搜索得到的k个点，其中k是正整数；此时更新c_best，表示为：

c_best←||X_init-X₁||+||X₁-X₂||+||X₂-X₃||+...+||X_k-X_goal||

式中，←表示赋值号；

下面简要证明加入椭圆约束的合理性：

考虑到如下事实，若设从起点X_init到终点X_goal并在中间经过随机采样点X_rand的最短路径记为g(X_rand)，则有：

g(X_rand)＝g₁(X_rand)+g₂(X_rand) (1)

其中g₁(X_rand)为X_init到X_rand的最短路径，g₂(X_rand)为X_rand到X_goal的最短路径，并设为g₁(X_rand)的估计值，/>为g₂(X_rand)的估计值，当以两点直线距离作为估计值时，有：

则当满足如下条件：

||X_init-X_rand||+||X_goal-X_rand||＞c_best (3)

由式(1)至式(3)联立可得：

g(X_rand)＞c_best

可知当式(3)成立时，穿过X_rand的最短路径已经大于了当前已得到的最短路径c_best，也就是椭圆长轴，因此X_rand一定不在起点为X_init，终点为X_goal的最短路径上，也就将该点及时的排除了。

由于RRT*路径搜索算法在运行过程中会产生许多对当前路径搜索没有任何提升的点，根据上述推导可知，在RRT*路径搜索算法中加入椭圆约束后可以有效地排除这些点，通过不断更新、缩小c_best，不断加速路径搜索过程向最短路径收敛；其中最短路径指的是连接X_init与X_goal并且不经过障碍物的最短折线段；如图2至图6所示，在不断的采样过程中，椭圆的长轴c_best不断地被更新成当前的最短路径长度，从图2的820，到图3的725，再到图4的693，最后到图5的692，从而不断缩小随机采样点的选取范围，最终不断加速路径搜索过程向最短路径收敛。

2)使用如下梯形时间分配准则为经过n+1个无障碍点的n条多项式轨迹分配时间：

式中，S_j代表第j段轨迹两端点之间的直线距离，v_m代表无人机的最大速度，a_m代表无人机的最大加速度，T(S_j)代表输入量为S_j、输出为轨迹的分配时间；

梯形时间分配准则的含义在于：它模拟无人机从某段轨迹的起点处，从速度为0开始，以加速度为a_m进行加速，直到速度达到了v_m；之后以v_m匀速飞行一段时间后，再以加速度为-a_m进行减速，当减速为0时刚好抵达了终点处，这一过程所花费的时间就用于该段轨迹的时间分配；由于这种加速-匀速-减速运动广泛存在于物理世界中，因此用这种时间分配方式通常能够获得较为合理的分配时间；如图6所示，利用梯形时间分配准则能够得到不打结的质量较好的Minimum snap轨迹。

3)使用Minimum snap算法规划出满足时间分配的n条多项式轨迹。

Minimum snap算法指的是一种输入值为X_p和X_q，输出值为以X_p为起点、X_q为终点的多项式轨迹的算法，并满足生成的多项式轨迹是以X_p为起点、X_q为终点的所有多项式轨迹中耗能最低的；其中X_p和X_q为平面上的任意两个无障碍点，在这里使用的是7阶多项式，表示为：

f(t)＝p₀+p₁t+p₂t²+...+p₇t⁷

式中，t表示时间，p₀,p₁,...,p₇分别是多项式轨迹f(t)的各阶系数。如图6所示，利用Minimum snap算法生成了连接X_init到X_goal的光滑轨迹。

4)对n条多项式轨迹进行碰撞检测，若检测到多项式轨迹发生碰撞，则利用插值法重新生成多项式轨迹，最终得到无碰撞多项式轨迹。

碰撞检测是指以一定的时间步长dt来对某段轨迹进行采样，得到若干个轨迹上的点，即X₀,X_dt,X_2dt...X_T，其中该段轨迹的起始时间为0，终止时间为T；当X₀,X_dt,X_2dt...X_T都不在障碍物内时，定义为无碰撞，否则定义为有碰撞；dt是一个可调参数，当dt越小，碰撞检测越慢，检测结果越准确；当dt越大，碰撞检测越快，检测结果越不准确；

插值法重新生成多项式轨迹的过程如下：

当发现第j条轨迹与障碍物发生碰撞时，需要取X_mid＝(X_j+X_j+1)/2，其中X_j,X_j+1分别是第j条轨迹的起点与终点，X_mid是以X_j与X_j+1为两端点的线段的中点，对X_j,X_mid,X_j+1这三点重新使用Minimum snap算法规划出两条多项式轨迹，若生成的两条多项式轨迹又发生碰撞，则重复递归地执行上述过程，直到所有多项式轨迹与障碍物都没有发生碰撞为止。

如图7所示，浅色轨迹为利用Minimum snap算法生成的光滑多项式轨迹，深色路径为利用改进RRT*路径搜索算法生成的路径，可以看出，Minimum snap第二段多项式轨迹与障碍物发生了碰撞。

如图8所示，第一次利用插值法生成中点后，重新生成多项式轨迹，发现重新生成的多项式轨迹仍然与障碍物发生了碰撞。

如图9所示，第二次利用插值法生成新的中点后，重新生成多项式轨迹，最终所有的多项式轨迹都不与障碍物发生碰撞，最终得到无碰撞多项式轨迹。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (3)

1.一种基于无人机的无碰撞多项式轨迹生成方法，其特征在于，包括以下步骤：

1)在含有障碍物的地图上，利用改进RRT*路径搜索算法进行路径搜索，得到n+1个无障碍点，n为正整数；其中，所述无障碍点是指不在障碍物内的点，所述改进RRT*路径搜索算法是在RRT*路径搜索算法的基础上加入了椭圆约束，保证生成的随机采样点约束在椭圆内部或椭圆上；

所述RRT*路径搜索算法是一种通过在地图中生成随机采样点，进而扩展快速搜索随机树，从而得到连接起点X_init与终点X_goal的可行路径的一种路径搜索算法；其中随机采样点是指随机生成的含有二维坐标信息的点；快速搜索随机树是一种含有点与边的计算机数据结构；可行路径是指连接X_init与X_goal并且不经过障碍物的折线段；

所述椭圆约束指的是在RRT*路径搜索算法中，若生成的随机采样点落在了以X_init与X_goal为两焦点，c_best为长轴的椭圆外，则重新进行采样，保证生成的随机采样点约束在椭圆内部或椭圆上，表示为：

c_best≥||X_init-X_rand||+||X_goal-X_rand||

式中，X_rand是随机采样点，||X_init-X_rand||表示X_init与X_rand的直线距离，||X_goal-X_rand||表示X_goal与X_rand的直线距离；当改进RRT*路径搜索算法找到一条更优可行路径时，即满足：

c_best≥||X_init-X₁||+||X₁-X₂||+||X₂-X₃||+...+||X_k-X_goal||

式中，X₁,X₂...X_k是通过改进RRT*路径搜索算法搜索得到的k个点，其中k是正整数；此时更新c_best，表示为：

c_best←||X_init-X₁||+||X₁-X₂||+||X₂-X₃||+...+||X_k-X_goal||

式中，←表示赋值号；

由于RRT*路径搜索算法在运行过程中会产生许多对当前路径搜索没有任何提升的点，在RRT*路径搜索算法中加入椭圆约束后能够有效地排除这些点，通过不断更新、缩小c_best，不断加速路径搜索过程向最短路径收敛；其中最短路径指的是连接X_init与X_goal并且不经过障碍物的最短折线段；

2)使用梯形时间分配准则为经过n+1个无障碍点的n条多项式轨迹分配时间；

所述梯形时间分配准则是指按照如下函数分配各段轨迹的时间：

式中，S_j代表第j段轨迹两端点之间的直线距离，v_m代表无人机的最大速度，a_m代表无人机的最大加速度，T(S_j)代表输入量为S_j、输出为轨迹的分配时间；

3)使用Minimum snap算法规划出满足时间分配的n条多项式轨迹；

4)对满足时间分配的n条多项式轨迹进行碰撞检测，若检测到多项式轨迹发生碰撞，则利用插值法重新生成多项式轨迹，最终得到无碰撞多项式轨迹。

2.根据权利要求1所述的一种基于无人机的无碰撞多项式轨迹生成方法，其特征在于，在步骤3)中，所述Minimum snap算法指的是一种输入值为X_p和X_q，输出值为以X_p为起点、X_q为终点的多项式轨迹的算法，并满足生成的多项式轨迹是以X_p为起点、X_q为终点的所有多项式轨迹中耗能最低的；其中X_p和X_q为平面上的任意两个无障碍点，在这里使用的是7阶多项式，表示为：

f(t)＝p₀+p₁t+p₂t²+...+p₇t⁷

式中，t表示时间，p₀,p₁,...,p₇分别是多项式轨迹f(t)的各阶系数。

3.根据权利要求1所述的一种基于无人机的无碰撞多项式轨迹生成方法，其特征在于，在步骤4)中，所述碰撞检测是指以规定的时间步长dt来对某段轨迹进行采样，得到若干个轨迹上的点，即X₀,X_dt,X_2dt...X_T，其中该段轨迹的起始时间为0，终止时间为T；当X₀,X_dt,X_2dt...X_T都不在障碍物内时，定义为无碰撞，否则定义为有碰撞；dt是一个可调参数，当dt越小，碰撞检测越慢，检测结果越准确；当dt越大，碰撞检测越快，检测结果越不准确；

插值法重新生成多项式轨迹的过程如下：

当发现第j条轨迹与障碍物发生碰撞时，需要取X_mid＝(X_j+X_j+1)/2，其中X_j,X_j+1分别是第j条轨迹的起点与终点，X_mid是以X_j与X_j+1为两端点的线段的中点，对X_j,X_mid,X_j+1这三点重新使用Minimum snap算法规划出两条多项式轨迹，若生成的两条多项式轨迹又发生碰撞，则重复递归地执行上述过程，直到所有多项式轨迹与障碍物都没有发生碰撞为止。