CN112650306A - 一种基于动力学rrt*的无人机运动规划方法 - Google Patents

Abstract

本发明公开了一种基于动力学RRT*的无人机运动规划方法，所述方法包括以下步骤：步骤1，检测获得环境信息；步骤2，建立无人机运动规划模型；步骤3，获得无人机避障规划运动路径。本发明公开的基于动力学RRT*的无人机运动规划方法，无需对搜索区域进行几何划分，搜索空间的覆盖率高，搜索的范围广，可以尽可能的探索未知区域，增强了无人机运动规划的目的性和灵活性，能够使无人机快速地规划出符合动力学约束的安全可行的渐进最优路径，解决多自由度无人机在复杂环境下和动态环境中的运动规划。

Description

一种基于动力学RRT*的无人机运动规划方法

技术领域

本发明属于无人机避障技术领域，具体涉及一种基于动力学RRT*的无人机运动规划方法。

背景技术

随着不同种类、型号、用途的无人机的快速更新与发展，无人机为了更好地完成人类交赋的各种任务并最终实现自主飞行，需要具备多种能力，如场景感知、避障、优化计算、信息传递、多等级辅助驾驶等。其中，运动规划能力作为无人机需要具备的基础能力之一，在无人机自主飞行***模块中起到承上启下的作用。运动规划问题被定义为在启动状态和目标状态之间找到无人机动态可行且符合约束的轨迹，而不与环境中的障碍物发生碰撞。

现有技术中，基于采样的RRT(快速探索随机树)方法及其变体(RRT*)能够有效地提供固定翼飞机通过静态障碍物的全局路径规划。但是，RRT方法是创建一个从起点到目标点的可能操作构建的树，所有的障碍物在基于RRT的方法中都被视为静态障碍物，因此在每个控制时间步长中，无人机会重新生成一个新的无碰撞路径或修改在之前生长的现有树，导致获得计算结果的时间较长。RRT方法在概率上是完整的，即当样本数接近无穷大时，该方法找到解的概率接近1，但RRT方法收敛到最优解的概率为0。此外，RRT方法在扩展过程中没有考虑各节点间的路径代价，不能保证规划结果的最优性，其只适用于具有简单动力学的***，无法适用于多自由度无人机在复杂环境下和动态环境中的运动规划。

因此，有必要提供一种无人机运动规划方法，能够在复杂多变的环境中使无人机快速地规划出符合动力学约束的安全可行的渐进最优路径，使无人机安全完成任务、降低损耗。

发明内容

为了克服上述问题，本发明人进行了锐意研究，将无人机的运动/动力学方程线性化为***状态方程，在运动约束下，引入最优控制，定义考虑了能量损耗和完成时间的代价函数，设计了一个固定最终状态-自由最终时间的规划器，精确和最优地连接任意***的任何状态节点对和任意维数状态空间中的可控线性动力学。本发明通过多次迭代的优化策略构建了可行的避障渐进最优轨迹，解决了多自由度无人机在复杂环境下和动态环境中的运动规划，从而完成了本发明。

具体来说，本发明的目的在于提供以下方面：

第一方面，提供一种基于动力学RRT*的无人机运动规划方法，所述方法包括以下步骤：

步骤1，检测获得环境信息；

步骤2，建立无人机运动规划模型；

步骤3，获得无人机避障规划运动路径。

其中，步骤3包括以下子步骤：

步骤3-1，初始化随机树，在无障碍物空间进行采样；

步骤3-2，遍历寻找最近节点，更新父节点，进行随机树重新布线；

步骤3-3，获得无人机避障规划运动路径。

其中，步骤3-1中，建立无障碍物全状态空间，在其中进行采样，

所述无障碍物全状态空间如下式(一)所示：

其中，x[t]∈X表示无人机的***状态，X＝Rⁿ是无人机的状态空间；

u[t]∈U表示无人机的控制输入，U＝R^m是无人机的控制输入空间；

A∈R^n×n和B∈R^n×m是常数和给定的。

其中，步骤3-2中，通过下式(二)寻找最近节点：

其中，c[π]表示轨迹的代价；τ表示轨迹的到达时间或持续时间；u(t)表示轨迹上无人机的控制输入；R∈R^m×m是正定且给定的。

其中，所述轨迹上无人机的最优控制输入通过下式(三)获得：

其中，τ表示给定一个固定的到达时间，0≤t≤τ；

x₀,x₁表示两个状态，其中x[0]＝x₀,x[τ]＝x₁；

G(t)表示加权可控制性Gramian矩阵。

其中，所述轨迹上无人机的最佳到达时间通过下式(四)获得：

其中，

其中，步骤3-2中，在获得最近节点后，通过下式(六)进行碰撞检测的判断：

judge(M,N,P)＝((y-y₁)(x₂-x₁))＞(y₂-y₁)(x-x₁)(六)

其中，M，N，P表示M＝(x₁,y₁)，N＝(x₂,y₂)，P＝(x,y)。

其中，所述基于动力学RRT*的无人机运动规划方法还包括步骤4，对获得的无人机避障规划运动路径进行避障检测，以保证无人机有效飞行和避障。

第二方面，提供一种计算机可读存储介质，存储有基于动力学RRT*的无人机运动规划程序，所述程序被处理器执行时，使得处理器执行所述基于动力学RRT*的无人机运动规划方法的步骤。

第三方面，提供一种计算机设备，包括存储器和处理器，所述存储器存储有基于动力学RRT*的无人机运动规划程序，所述程序被处理器执行时，使得处理器执行所述基于动力学RRT*的无人机运动规划方法的步骤。

本发明所具有的有益效果包括：

(1)本发明提供的基于动力学RRT*的无人机运动规划方法，无需对搜索区域进行几何划分，搜索空间的覆盖率高，搜索的范围广，可以尽可能的探索未知区域；通过多次迭代的优化策略构建了避障最优轨迹的渐进求解框架，减小了计算量，能以较快的速度生成安全的无人机飞行轨迹；

(2)本发明提供的基于动力学RRT*的无人机运动规划方法，提出了考虑到无人机能量损耗和完成时间的代价函数，增强了无人机运动规划的目的性和灵活性；

(3)本发明提供的基于动力学RRT*的无人机运动规划方法，设计了一个固定最终状态-自由最终时间的规划器，使该方法适用于线性微分约束无人机，生成可行的无人机轨迹，保证了任何具有可控线性动力学的***在任何维度的状态空间中的渐进最优性。

附图说明

图1示出根据本发明一种优选实施方式的基于动力学RRT*的无人机运动规划方法的流程图；

图2示出本发明实施例1所述方法的无人机的运动轨迹；

图3示出现有技术中的基于RRT*避障规划方法的无人机的运动轨迹。

具体实施方式

下面通过优选实施方式和实施例对本发明进一步详细说明。通过这些说明，本发明的特点和优点将变得更为清楚明确。

在这里专用的词“示例性”意为“用作例子、实施例或说明性”。这里作为“示例性”所说明的任何实施例不必解释为优于或好于其它实施例。

本发明人发现，传统的RRT方法依赖于以直线轨迹连接任何一对节点表示可行的路径，但是对于无人机动力学***，由于***的微分约束，节点对之间的直线连接通常不是有效的轨迹。因此，本发明对传统RRT方法进行了改进，提出一种基于动力学RRT*的无人机运动规划方法，通过多次迭代的优化策略构建了可行的避障渐进最优轨迹，以解决多自由度无人机在复杂环境下和动态环境中的运动规划。

本发明的第一方面，提供了一种基于动力学RRT*的无人机运动规划方法，所述方法包括以下步骤：

步骤1，检测获得环境信息；

步骤2，建立无人机运动规划模型；

步骤3，获得无人机避障规划运动路径。

以下进一步详细描述所述方法：

步骤1，检测获得环境信息。

在本发明中，所述环境信息通过设置在无人机上的探测装置检测获得，所述检测装置可以为双目摄像头、激光雷达、光电吊舱等。

其中，通过检测获得环境中的障碍物信息，即获得障碍物地图。

步骤2，建立无人机运动规划模型。

其中，所述建立无人机运动规划模型指的是：对无人机进行建模，具体是指通过地面坐标系E(X,Y,Z)和机体坐标系B(X,Y,Z)结合牛顿定律推导建立无人机的运动学模型和动力学模型。

步骤3，获得无人机避障规划运动路径。

其中，如图1所示，步骤3包括以下子步骤：

步骤3-1，初始化随机树，在无障碍物空间进行采样。

其中，由无人机起始状态初始化随机树，具体地，迭代次数为1时，以无人机的起始状态为起始状态点在自由状态空间中来初始化随机树。

在本发明中，所述无人机的起始状态为无人机的位置和速度，例如，在本发明中，设定无人机的起始状态的位置坐标为(1,1)，速度为(0,0)。

根据本发明一种优选的实施方式，建立无障碍物全状态空间，在其中进行采样，

其中，所述无障碍物全状态空间如下式(一)所示：

其中，x[t]∈X表示无人机的***状态，X＝Rⁿ是无人机的状态空间；

u[t]∈U表示无人机的控制输入，U＝R^m是无人机的控制输入空间；

A∈R^n×n和B∈R^n×m是常数和给定的。

在本发明中，由于式(一)所述的状态空间方程是根据运动学模型和动力学模型线性化得到的，因此A∈R^n×n和B∈R^n×m是常数和给定的，其取值是根据无人机模型而定的，建模不同或模型假设不同，取值会随之改变。

其中，所述全状态空间包括无人机的位置信息、速度信息和到达时间信息。

在本发明中，经过采样，在全状态空间内产生一个新的随机点。

步骤3-2，遍历寻找最近节点，更新父节点，进行随机树重新布线。

其中，经过上述步骤产生随机点后，遍历随机树中已经存在的相邻节点，计算每一个节点与随机点之间的距离，找出距离此随机点最近的节点。

根据本发明一种优选的实施方式，通过下式(二)寻找最近节点：

其中，c[π]表示轨迹的代价；τ表示轨迹的到达时间或持续时间；u(t)表示轨迹上无人机的控制输入；R∈R^m×m是正定且给定的。

其中，所述正定定义是设M是n阶方阵，如果对任何非零向量z，都有z^TMz＞0(z^T表示z的转置)，就称M为正定矩阵。R是一个m阶方阵，它是正定的条件是当且仅当对于所有的非零实系数向量z，都有z^TRz＞0，其中z^T表示z的转置；对应于上式(二)中，表明u(t)^TRu(t)＞0。

在本发明中，定义最近节点时，考虑无人机的运动约束引入最优控制，如果从一个节点转移到另一个节点的状态代价很小，则表明两个状态节点邻近，本发明中优选定义考虑了能量损耗和完成时间的代价函数，如上式(二)所示，相对于现有技术中仅考虑距离的代价函数，本发明中的代价函数，可调谐权衡轨迹的持续时间和扩展的控制工作，增强了无人机运动规划的目的性和灵活性。

在进一步优选的实施方式中，所述轨迹上无人机的最优控制输入(即最优的u(t))通过下式(三)获得：

其中，τ表示给定一个固定的到达时间，0≤t≤τ；

x₀,x₁表示两个状态，其中x[0]＝x₀,x[τ]＝x₁；

G(t)表示加权可控制性Gramian矩阵。

在本发明中，通过上式(三)定义的固定最终状态-自由最终时间的规划控制器，可以判断轨迹上无人机的控制输入是否为最优。

在更进一步优选的实施方式中，所述轨迹上无人机的最佳到达时间τ^*(即最优的τ)通过下式(四)获得：

其中，

在本发明中，通过上式(三)和(四)获得轨迹上无人机最优的控制输入和最佳到达时间，使得从一个状态节点转移到另一个状态节点的状态代价很小，无人机的能量损耗最低。

优选地，所述无人机的最优轨迹通过下式(五)获得：

其中，0＜t＜τ^*。

根据本发明一种优选的实施方式，在获得最近节点后，优选通过下式(六)进行碰撞检测的判断：

judge(M,N,P)＝((y-y₁)(x₂-x₁))＞(y₂-y₁)(x-x₁)(六)

其中，M，N，P表示M＝(x₁,y₁)，N＝(x₂,y₂)，P＝(x,y)。

在进行障碍物碰撞检测时，障碍物中，长方形的碰撞机制相对复杂，具体碰撞机制为在扩展随机树的过程中，新的随机点x_rand与最近节点x_near连接的轨迹不能与长方形障碍物的任何一边相交，即将长方形障碍物碰撞检测问题转化为轨迹与矩形相交的问题。

具体地，对于矩形的一条边设定一个布尔值bool_i，当bool_i＝1时，表示发生碰撞，当bool_i＝0时，表示不发生碰撞。因此，式(六)中judge函数是一个布尔函数，当等式右边为真时，judge＝1，反之judg＝0。

在进一步优选的实施方式中，通过下式(七)进行判断：

bool_i＝

(judge(x_near,Vertex₁,Vertex₂)≠judge(x_new,Vertex₁,Vertex₂))

and

(judge(x_near,x_new,Vertex₁)≠judge(x_near,x_new,Vertex₂)) (七)

其中，Vertex_i表示矩形障碍物某一条边的两个定点。

在本发明中，经过碰撞检测，如果发生碰撞，则重复进行步骤3-1和步骤3-2；如果不发生碰撞，则采样产生的随机状态点x_rand为合法新的状态节点x_new，其对应的最近节点作为新节点的父节点进行更新，进而删除原树枝，完成随机树的重新布线。

其中，对于随机树上的某些路径代价c[x_near,x_new]＜r的邻居节点，选择其作为x_new的备选父节点，其中r是邻居半径。依次计算(c[x_near]+c[x_near,x_new])并选取最小值对应的x_near|_min作为x_new的父节点。

在本发明中，邻居节点指的是落在以节点为中心，通信半径所能覆盖的面积内的节点，即能够与该节点直接通信可连接的全部节点称为该节点的邻居节点。邻居半径即为通信半径。

对于已有状态点x_near′，在满足避障条件下，如果c[x_new,x_near′]＜r且c[x_new]+c[x_new,x_near′]＜c[x_near′]，则让x_new成为x_nea′的新父节点，同时删除原树枝，完成随机树T重新布线。

本发明中通过随机树的重新布线，使得生成新节点后的随机树减少冗余通路，减小路径代价，让其中任何一对节点都可以通过直线轨迹进行最佳连接。

步骤3-3，获得无人机避障规划运动路径。

在本发明中，优选地，重复步骤3-1～3-2，直至新的节点中包含有无人机的目标点x_goal为止。

更优选地，通过回溯法在随机树中获得无人机从起始状态到目标状态的避障规划路径。

根据本发明一种优选的实施方式，在步骤3获得无人机的避障规划运动路径之后，还包括步骤4，对获得的避障规划运动路径进行避障检测，以保证无人机有效飞行和避障。

本发明提供的基于动力学RRT*的无人机运动规划方法，将无人机的运动/动力学方程线性化为***状态方程，在运动约束下，引入最优控制，定义考虑了能量损耗和完成时间的代价函数，设计了一个固定最终状态-自由最终时间的规划器，精确和最优地连接任意***的任何状态节点对和任意维数状态空间中的可控线性动力学，使本发明的运动规划方法返回的规划结果在时间花费上大大降低，提升了规划速度，且更适合作为无人机飞行轨迹，与常规RRT*方法相比，能够使无人机快速地规划出符合动力学约束的安全可行的渐进最优路径。

本发明还提供了一种计算机可读存储介质，存储有基于动力学RRT*的无人机运动规划程序，所述程序被处理器执行时，使得处理器执行所述基于动力学RRT*的无人机运动规划方法的步骤。

本发明中所述的基于动力学RRT*的无人机运动规划方法可借助软件加必需的通用硬件平台的方式来实现，所述软件存储在计算机可读存储介质(包括ROM/RAM、磁碟、光盘)中，包括若干指令用以使得一台终端设备(可以是手机、计算机、服务器、网络设备等)执行本发明所述方法。

本发明还提供了一种计算机设备，包括存储器和处理器，所述存储器存储有基于动力学RRT*的无人机运动规划程序，所述程序被处理器执行时，使得处理器执行所述基于动力学RRT*的无人机运动规划方法的步骤。

实施例

以下通过具体实例进一步描述本发明，不过这些实例仅仅是范例性的，并不对本发明的保护范围构成任何限制。

实施例1

在本实施例中，忽略无人机的垂直机动时，认为无人机的避障是在恒定的高度上进行的，无人机相当于是在二维平面内移动，因此可以简化为2D模型，设定无人机的起始位置坐标为(1,1)，目标位置坐标为(700,700)，设置扩展步长为30米，在飞行路径中设置两个矩形障碍物。

通过动力学RRT*对无人机进行避障运动规划：

(1)设迭代次数i＝1，以无人机的起始状态为起始状态点x_star在自由状态空间中来初始化随机树T。

(2)在第i次迭代时，通过在自由状态空间进行采样，得到新的随机状态点x_rand。在随机树T上已经存在的相邻节点间找到一个父节点x，通过下式规划控制器获得对应的代价函数值c[x]：

(3)通过下式进行碰撞检测的判断，如果发生碰撞，那么将重新进行步骤(2)；反之，设x_rand为合法随机状态点x_new：

judge(M,N,P)＝((y-y₁)(x₂-x₁))＞(y₂-y₁)(x-x₁)

bool_i＝

(judge(x_near,Vertex₁,Vertex₂)≠judge(x_new,Vertex₁,Vertex₂))

and

(judge(x_near,x_new,Vertex₁)≠judge(x_near,x_new,Vertex₂))

其中，矩形障碍物的每条边的判断方法是相同的。

(4)对于随机树T上的某些路径代价c[x_near,x_new]＜r的邻居节点，选择其作为x_new的备选父节点，其中r是邻居半径。依次计算(c[x_near]+c[x_near,x_new])并选取最小值对应的x_near|_min作为x_new的父节点。

(5)对于已有状态点x_near′，在满足第三步避障条件下，如果c[x_new,x_near′]＜r且c[x_new]+c[x_new,x_near′]＜c[x_near′]，则让x_new成为x_near'的新父节点，同时删除原树枝，完成随机树T重新布线。

第六步，重复上述步骤，直到新的节点中包含有无人机的目标点x_goal为止。通过回溯法可以在随机树T中获得从起始状态x_star到目标状态x_goal的避障规划路径。

实验例

实验例1

将实施例1中所述基于动力学RRT*避障规划方法的运动轨迹与现有技术中的基于RRT*避障规划方法的运动轨迹进行比较，结果分别如图2和3所示。

其中，现有技术中的基于RRT*避障规划方法的步骤如文献“林依凡，陈彦杰，何炳蔚，黄益斌，王耀南.无碰撞检测RRT*的移动机器人运动规划方法.仪器仪表学报.”所示，具体如下：

步骤1：算法初始化。设定路径规划的起始位置、目标位置和总迭代次数，设置当前迭代次数等于0；

步骤2：随机均匀采样，采样点以x_rand表示，并将当前迭代次数i加1；

步骤3：找到搜索树G中离x_rand距离最近的节点x_nearest；

步骤4：在x_nearest朝向x_rand方向上加相应增量得新节点x_new；

步骤5：找到以x_new为中心，R_near为半径范围内G上所有节点，组成集合X_near；

步骤6：计算X_near的碰撞风险评估函数Gauss(x_near,x_new)，作为代价函数的分量；

步骤7：计算X_near的代价函数，得到x_new的最小代价函数值及其父节点，将其中的最小值(以min(c')表示)设为x_new的代价函数值，并将该值对应的节点设置为x_new的父节点(以x_min表示)；

步骤8：更新X_near的父节点及代价函数值；

步骤9：算法终止，当x_new和目标点x_goal的距离小于目标偏差容忍度R_find，算法规划出可行路径，算法终止，输出搜索树图及可行路径。

由图2和3可知，本发明实施例1所提供的方法随机生成新的状态节点x_rand，和临近节点进行碰撞检测，使得生成的随机树不会扩展到障碍物内，能稳定完成避障任务，确保所规划的路径是安全的，并且在满足概率完备(随着采样点的增多，找到路径的概率为1)的情况下也具备渐进最优性(当算法的迭代次数接近无穷大时，包含最优解的概率接近1)。

因为本发明实施例所述方法将无人机的运动/动力学方程线性化为***状态方程，在运动约束下，引入最优控制，定义考虑了能量损耗和完成时间的代价函数，设计了一个固定最终状态-自由最终时间的规划器，精确和最优地连接任意***的任何状态节点对和任意维数状态空间中的可控线性动力学，使本发明的运动规划方法返回的规划结果在时间花费上大大降低，提升了规划速度，且更适合作为无人机飞行轨迹，通过与常规RRT*算法对比，证明了利用动力学RRT*方法进行无人机避障运动规划的可行性与优越性。

以上结合具体实施方式和范例性实例对本发明进行了详细说明，不过这些说明并不能理解为对本发明的限制。本领域技术人员理解，在不偏离本发明精神和范围的情况下，可以对本发明技术方案及其实施方式进行多种等价替换、修饰或改进，这些均落入本发明的范围内。

Claims (10)

1.一种基于动力学RRT*的无人机运动规划方法，其特征在于，所述方法包括以下步骤：

步骤1，检测获得环境信息；

步骤2，建立无人机运动规划模型；

步骤3，获得无人机避障规划运动路径。

2.根据权利要求1所述的方法，其特征在于，步骤3包括以下子步骤：

步骤3-1，初始化随机树，在无障碍物空间进行采样；

步骤3-2，遍历寻找最近节点，更新父节点，进行随机树重新布线；

步骤3-3，获得无人机避障规划运动路径。

3.根据权利要求2所述的方法，其特征在于，步骤3-1中，建立无障碍物全状态空间，在其中进行采样，

所述无障碍物全状态空间如下式(一)所示：

其中，x[t]∈X表示无人机的***状态，X＝Rⁿ是无人机的状态空间；

u[t]∈U表示无人机的控制输入，U＝R^m是无人机的控制输入空间；

A∈R^n×n和B∈R^n×m是常数和给定的。

4.根据权利要求2所述的方法，其特征在于，步骤3-2中，通过下式(二)寻找最近节点：

其中，c[π]表示轨迹的代价；τ表示轨迹的到达时间或持续时间；u(t)表示轨迹上无人机的控制输入；R∈R^m×m是正定且给定的。

5.根据权利要求4所述的方法，其特征在于，所述轨迹上无人机的最优控制输入通过下式(三)获得：

其中，τ表示给定一个固定的到达时间，0≤t≤τ；

x₀,x₁表示两个状态，其中x[0]＝x₀,x[τ]＝x₁；

G(t)表示加权可控制性Gramian矩阵。

6.根据权利要求4所述的方法，其特征在于，所述轨迹上无人机的最佳到达时间通过下式(四)获得：

其中，

7.根据权利要求2所述的方法，其特征在于，步骤3-2中，在获得最近节点后，通过下式(六)进行碰撞检测的判断：

judge(M,N,P)＝((y-y₁)(x₂-x₁))＞(y₂-y₁)(x-x₁) (六)

其中，M，N，P表示M＝(x₁,y₁)，N＝(x₂,y₂)，P＝(x,y)。

8.根据权利要求1所述的方法，其特征在于，所述基于动力学RRT*的无人机运动规划方法还包括步骤4，对获得的无人机避障规划运动路径进行避障检测，以保证无人机有效飞行和避障。

9.一种计算机可读存储介质，其特征在于，存储有基于动力学RRT*的无人机运动规划程序，所述程序被处理器执行时，使得处理器执行权利要求1至8之一所述基于动力学RRT*的无人机运动规划方法的步骤。

10.一种计算机设备，其特征在于，包括存储器和处理器，所述存储器存储有基于动力学RRT*的无人机运动规划程序，所述程序被处理器执行时，使得处理器执行权利要求1至8之一所述基于动力学RRT*的无人机运动规划方法的步骤。

Images