CN111580548B - 一种基于样条-rrt和速度障碍的无人机避障方法 - Google Patents

Abstract

本发明涉及一种基于样条‑rrt和速度障碍的无人机避障方法，在无人机传感范围内建立随机搜索树，以当前位置和速度为起始节点，以全局路径与传感范围的交点位置为目标位置，在速度可行域内采样得到可行速度，然后生成一个新的节点。对生成的边进行碰撞检测后，更新树，重复这个过程，直到随机树生长到达目标位置。利用回溯法在随机树中获得局部路径，无人机沿着局部路径穿过动态障碍物后，继续沿着原全局路径飞行，直到目标区域。通过重建无碰撞路径后并按照无碰撞路径通过动态障碍物后，恢复原设定的避障路线继续行驶，使得路径重规划只需要执行一次而无需不断重新规划路径，提高路径规划的效率和路线的稳定性。

Description

一种基于样条-rrt和速度障碍的无人机避障方法

技术领域

本发明涉及机器人路径规划领域，更具体地，涉及一种基于样条-rrt和速度障碍的无人机避障方法。

背景技术

在实际应用中，自主完成救援搜索、农业、测绘等任务的无人机越来越受到人们的关注。在复杂环境下航行时，为了避免静态和动态障碍，固定翼飞机在空气动力学的约束下，在线再生一条平滑连续的路径至关重要。

对于机器人的路径规划，提出一种基于采样的RRT(快速探索随机树)方法及其变体，能够有效地提供固定翼飞机通过静态障碍物的全局路径规划。RRT方法是创建一个从起始点到目标点的可能操作构建的树，所有的障碍物在基于RRT的方法中都被视为静态障碍物，因此在每个控制时间步长中，机器人会重新生成一个新的无碰撞路径或修改在之前生长的现有树。导致机器人需要等待更长的时间才能通过RRT方法得到计算结果。

发明内容

本发明为克服上述现有技术中机器人无碰撞路径规划的计算效率低问题，提供一种基于样条-rrt和速度障碍的无人机避障方法，该避障方法结合了样条-RRT算法和速度障碍(VO)方法，在局部区域生长一棵随机树，同时使用速度障碍(VO)方法扩展树的边缘并拒绝不可用的节点，提高了树木生长的效率和以及树木生长更加平稳，即提高路径规划的效率和路线的稳定性。

为解决上述技术问题，本发明采用的技术方案是：一种基于样条-rrt和速度障碍的无人机避障方法，包括如下步骤：

步骤一：以初始状态x_start为根节点初始化搜索树，其中状态表示节点在树中的位置和速度；

步骤二：随机树每条边的代价被分成两部分，其中包括节点的能量消耗代价cost以及时间消耗代价t；

步骤三：随机树生长的每一步，随机树的父节点被随机选择；

步骤四：获取新的子节点，子节点的的位置可以根据父节点的状态和新速度计算获得，子节点的速度可以通过将新速度v_A,new绕轴e_axis旋转θ_r角获得。

步骤五：使用三次贝塞尔曲线直接定义父节点和新子节点之间的边，而不需要进行后处理；

步骤六：在动态障碍物和静态障碍物的场景中，判断新生成的边是否与障碍物发生碰撞；

步骤七：当一个新节点被添加到树中时，计算新节点的能量消耗代价和时间消耗代价；

步骤八：重复步骤一至步骤七，直到新节点落入目标区域；

步骤九：使用回溯在随机树中找到可行路径。

在包含多个静态障碍物和动态障碍物的动态环境中，无人机对静态环境的知识是先验已知的，利用地图的先验知识，通过样条-RRT算法离线生成全局路径。当无人机沿着全局路径飞行时，当遇到障碍物在无人机前方移动，无人机可以利用有限感知距离的机载传感器检测动态障碍物。当飞机探测到其感应范围内的动态障碍物时，利用上述所提出的方法在传感范围内建立随机搜索树，以当前位置和速度为起始节点x_start，以全局路径与传感范围的交点位置为目标位置x_goal。由于无人机的速度是变化的，所以无人机相对于动态障碍物的速度可行域也会发生变化。在速度可行域内采样得到可行速度，然后生成一个新的节点。对生成的边进行碰撞检测后，更新树，重复这个过程，直到随机树生长到达目标位置x_goal。利用回溯法在随机树中获得局部路径，无人机沿着局部路径穿过动态障碍物后，继续沿着原全局路径飞行，直到目标区域。

优选的，在所述步骤二中，根节点的能量消耗代价和时间消耗代价都初始化为0。

优选的，在所述步骤四中，具体的流程为：

S4.1：采用速度障碍算法计算子节点的速度，随机树的单个边将被转换为基于VO的场景，无人机相对于动态障碍物的速度障碍(VO)区域定义如下：

其中，D(p,r)表示以位置p为中心；以长度r为半径的圆盘；τ是时间窗，即无人机和障碍物在τ时间内不会发生碰撞；

表示无人机相对于障碍物O在时间窗τ内会发生碰撞的速度集合；p_O|A表示无人机A和障碍物O之间的相对距离；v表示固定翼飞机的速度；t表示时间；r_AO表示无人机A和障碍物O的总半径之和；

S4.2：飞机和障碍物之间的相对速度在VO区域内,飞机将在时间间隔τ内与障碍物发生碰撞，采用最优互反避碰算法，获得无碰撞速度集合，定义如下：

其中，

表示固定翼飞机A相对于障碍物O在时间窗τ内不会发生碰撞的速度集合；

为飞机当前速度；u表示的相对速度离开VO区域的最小速度变化，n是外法向量；系数λ是一种责任系数,确定飞机应当承担多少责任，由于障碍物是非合作式,所以λ＝1；

S4.3：计算无人机的运动学约束S_A，无碰撞速度集表达式为：

其中，

是无人机A相对于障碍物O的在运动学约束S_A和无碰撞速度集合

以及最大速度

约束下的可行相对速度集合；v_O是障碍物O的速度；

考虑飞行轨迹角角度作为固定翼飞机的运动学约束，能够保证无人机飞行轨迹的平稳性。

S4.4:新的相对速度

由基于速度域的采样函数得出，具体为：

其中，R是一个随机数，在[0，1]区间内采样；α是一个参数来确定选择最优速度的概率；

S4.5：新速度v_A，new由以下公式计算：

其中，v_O是障碍物的速度；新速度v_A，new保证固定翼飞机从父节点的位置飞到子节点的位置的直线路径是无碰撞的，并且满足固定翼飞机的运动学约束；

S4.6：父节点和子节点之间的边是一个三次贝塞尔曲线，无人机从父节点到子节点的时间消耗为τ，子节点的位置可以根据父节点的状态和新速度计算获得，具体公式为：

Node_new·p←Node_parent·p+τ*v_A，new

其中，Node_new表示新节点；Node_new·p表示新节点的位置；Node_parent表示父节点；Node_parent·p表示父节点的位置；τ表示时间窗；v_A，new表示无人机A生成的新速度；

子节点的速度可以通过将新速度v_A，new绕轴e_axis旋转θ_r角获得；

其中，cos^-1表示反cos函数；v_A，cur是父节点的速度；v_A，new是子节点的速度；·表示点乘；|v_A，cur|表示v_A，cur的模；×表示叉乘；θ_r是父节点的速度v_A，cur和生成新速度v_A，new之间的夹角；e_axis是父节点的速度v_A，cur和生成新速度v_A，new所在平面的法向量。

优选的，α的具体定义为：

α＝dist(p_cur-p_goal)/dist(p_start-p_goal)，

其中，P_cur、p_goal和P_start分别表示飞机当前的位置、目标位置和初始位置；dist是求位置之间欧式距离的函数；

随着当前位置与目标位置距离的减小，选择最优速度的概率增大。计算得到的最优速度

最接近最优速度

用于引导固定翼飞机飞向目标位置。因此，选择最优速度的概率越大，随机树在位置域中向目标位置生长的速度越快。

R的值大于α时，新的相对速度

由子函数Uniform在工作空间

中随机均匀采样获得；R的值小于或等于α时，相对速度

选择由子函数Optimal生成的与期望速度

最接近的最优相对速度

由以下公式定义：

其中，

是函数生成的无人机A相对于障碍物0的最优相对速度；

是固定翼飞机A相对于障碍物0的期望速度。

优选的，在所述步骤五中，贝塞尔曲线B(s)的公式如下：

B(s)＝(1-s)³p_p+3s(1-s)²p_m1+3s²(1-s)p_m2+s³p_c.

其中，s是一个实数，在0和1之间变化；p_p和p_c分别是父节点和子节点的位置；P_m1和p_m2分别为两个控制点；

两个控制点P_m1和P_m2的公式具体为：

p_m1＝p_p+v_p*|p_c-p_p|/3，

p_m2＝p_c-v_c*|p_c-p_p|/3.

其中，p_p为父节点的位置；v_p为父节点的速度；p_c为子节点的位置p_c、v_c为子节点的速度。

优选的，在所述步骤六中，判断障碍物碰撞的方法为：

贝塞尔曲线B(s)包含在由控制点组成的凸包中，凸包根据无人机的半径扩展成凸多面体；

利用分离轴定理来判断无人机凸多面体与障碍物凸多面体之间是否存在交集，来进行障碍物碰撞检测。

与现有技术相比，本发明的有益效果是：当动态障碍物在无人机的感知范围内时，通过重建无碰撞路径后并按照无碰撞路径通过动态障碍物后，恢复原设定的避障路线继续行驶，使得路径重规划只需要执行一次而无需不断重新规划路径，提高路径规划的效率和路线的稳定性。

附图说明

图1是本发明的一种基于样条-rrt和速度障碍的无人机避障方法的流程图。

具体实施方式

附图仅用于示例性说明，不能理解为对本专利的限制；为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。附图中描述位置关系仅用于示例性说明，不能理解为对本专利的限制。

下面通过具体实施例，并结合附图，对本发明的技术方案作进一步的具体描述：

实施例1

如图1所示为一种基于样条-rrt和速度障碍的无人机避障方法的实施例，包括如下步骤：

步骤一：以初始状态x_start为根节点初始化搜索树，其中状态表示节点在树中的位置和速度；

步骤二：随机树每条边的代价被分成两部分，其中包括节点的能量消耗代价cost以及时间消耗代价t；根节点的能量消耗代价和时间消耗代价都初始化为0。

步骤三：随机树生长的每一步，随机树的父节点被随机选择；

步骤四：获取新的子节点，子节点的的位置可以根据父节点的状态和新速度计算获得，子节点的速度可以通过将新速度v_A,new绕轴e_axis旋转θ_r角获得；

具体流程为：

S4.1：采用速度障碍算法计算子节点的速度，随机树的单个边将被转换为基于VO的场景，无人机相对于动态障碍物的VO区域定义如下：

其中，D(p,r)表示以位置p为中心；以长度r为半径的圆盘；τ是时间窗，即无人机和障碍物在τ时间内不会发生碰撞；

表示无人机相对于障碍物O在时间窗τ内会发生碰撞的速度集合；p_O|A表示无人机A和障碍物O之间的相对距离；v表示固定翼飞机的速度；t表示时间；r_AO表示无人机A和障碍物O的总半径之和；

S4.2：飞机和障碍物之间的相对速度在VO区域内,飞机将在时间间隔τ内与障碍物发生碰撞，采用最优互反避碰算法，获得无碰撞速度集合，定义如下：

其中，

表示固定翼飞机A相对于障碍物O在时间窗τ内不会发生碰撞的速度集合；

为飞机当前速度；u表示的相对速度离开VO区域的最小速度变化，n是外法向量；系数λ是一种责任系数,确定飞机应当承担多少责任，由于障碍物是非合作式,所以λ＝1；

S4.3：计算无人机的运动学约束S_A，无碰撞速度集表达式为：

其中，

是无人机A相对于障碍物O的在运动学约束S_A和无碰撞速度集合

以及最大速度

约束下的可行相对速度集合；v_O是障碍物O的速度；

考虑飞行轨迹角角度作为固定翼飞机的运动学约束，能够保证无人机飞行轨迹的平稳性。

S4.4:新的相对速度

由基于速度域的采样函数得出，具体为：

其中，R是一个随机数，在[0，1]区间内采样；α是一个参数来确定选择最优速度的概率；

α的具体定义为：

α＝dist(p_cur-p_goal)/dist(p_start-p_goal)，

其中，p_cur、p_goal和p_start分别表示飞机当前的位置、目标位置和初始位置；dist是求位置之间欧式距离的函数；随着当前位置与目标位置距离的减小，选择最优速度的概率增大。计算得到的最优速度

最接近最优速度

用于引导固定翼飞机飞向目标位置。因此，选择最优速度的概率越大，随机树在位置域中向目标位置生长的速度越快。

R的值大于α时，新的相对速度

由子函数Uniform在工作空间

中随机均匀采样获得；R的值小于或等于α时，相对速度

选择由子函数Optimal生成的与期望速度

最接近的最优相对速度

由以下公式定义：

其中，

是函数生成的无人机A相对于障碍物0的最优相对速度；

是固定翼飞机A相对于障碍物0的期望速度。

S4.5：新速度v_A，new由以下公式计算：

其中，v_O是障碍物的速度；新速度v_A，new保证固定翼飞机从父节点的位置飞到子节点的位置的直线路径是无碰撞的，并且满足固定翼飞机的运动学约束；

S4.6：父节点和子节点之间的边是一个三次贝塞尔曲线，无人机从父节点到子节点的时间消耗为τ，子节点的位置可以根据父节点的状态和新速度计算获得，具体公式为：

Node_new·p←Node_parent·p+τ*v_A，new

其中，Node_new表示新节点；Node_new·p表示新节点的位置；Node_parent表示父节点；Node_parent·p表示父节点的位置；τ表示时间窗；v_A，new表示无人机A生成的新速度；

子节点的速度可以通过将新速度v_A，new绕轴e_axis旋转θ_r角获得；

其中，cos^-1表示反cos函数；v_A，cur是父节点的速度；v_A，new是子节点的速度；·表示点乘；|v_A，cur|表示v_A，cur的模；×表示叉乘；θ_r是父节点的速度v_A，cur和生成新速度v_A，new之间的夹角；e_axis是父节点的速度v_A，cur和生成新速度v_A，new所在平面的法向量。

步骤五：使用三次贝塞尔曲线直接定义父节点和新子节点之间的边，而不需要进行后处理；贝塞尔曲线B(s)的公式如下：

B(s)＝(1-s)³p_p+3s(1-s)²p_m1+3s²(1-s)p_m2+s³p_c.

其中，s是一个实数，在0和1之间变化；p_p和p_c分别是父节点和子节点的位置；p_m1和P_m2分别为两个控制点；

两个控制点P_m1和P_m2的公式具体为：

p_m1＝p_p+v_p*|p_c-p_p|/3，

p_m2＝p_c-v_c*|p_c-p_p|/3.

其中，p_p为父节点的位置；v_p为父节点的速度；p_c为子节点的位置p_c、v_c为子节点的速度。

步骤六：在动态障碍物和静态障碍物的场景中，判断新生成的边是否与障碍物发生碰撞；具体的方法为：

贝塞尔曲线B(s)包含在由控制点组成的凸包中，凸包根据无人机的半径扩展成凸多面体；

利用分离轴定理来判断无人机凸多面体与障碍物凸多面体之间是否存在交集，来进行障碍物碰撞检测。

步骤七：当一个新节点被添加到树中时，计算新节点的能量消耗代价和时间消耗代价；

步骤八：重复步骤一至步骤七，直到新节点落入目标区域；

步骤九：使用回溯在随机树中找到可行路径。

本发明的工作原理或工作过程：在包含多个静态障碍物和动态障碍物的动态环境中，无人机对静态环境的知识是先验已知的，利用地图的先验知识，通过样条-RRT算法离线生成全局路径。当无人机沿着全局路径飞行时，当遇到障碍物在无人机前方移动，无人机可以利用有限感知距离的机载传感器检测动态障碍物。当飞机探测到其感应范围内的动态障碍物时，利用上述所提出的方法在传感范围内建立随机搜索树，以当前位置和速度为起始节点x_start，以全局路径与传感范围的交点位置为目标位置x_goal。由于无人机的速度是变化的，所以无人机相对于动态障碍物的速度可行域也会发生变化。在速度可行域内采样得到可行速度，然后生成一个新的节点。对生成的边进行碰撞检测后，更新树，重复这个过程，直到随机树生长到达目标位置x_goal。利用回溯法在随机树中获得局部路径，无人机沿着局部路径穿过动态障碍物后，继续沿着原全局路径飞行，直到目标区域。

本实施例的有益效果：当动态障碍物在无人机的感知范围内时，通过重建无碰撞路径后并按照无无碰撞路径通过动态障碍物后，恢复原设定的避障路线继续行驶，使得路径重规划只需要执行一次而无需不断重新规划路径，提高路径规划的效率和路线的稳定性。

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (5)

1.一种基于样条-rrt和速度障碍的无人机避障方法，其特征在于，包括如下步骤：

步骤一：以初始状态x_start为根节点初始化搜索树，状态表示节点在树中的位置和速度；

步骤二：随机树每条边的代价被分成两部分，包括节点的能量消耗代价cost以及时间消耗代价t；

步骤三：随机树生长的每一步，随机树的父节点被随机选择；

步骤四：获取新的子节点，子节点的位置可以根据父节点的状态和新速度计算获得，子节点的速度可以通过将新速度v_A，new绕轴e_axis旋转θ_r角获得；具体的流程为：

S4.1：采用速度障碍算法计算子节点的速度，随机树的单个边将被转换为基于VO的场景，无人机相对于动态障碍物的速度障碍区域定义如下：

其中，D(p，r)表示以位置p为中心；以长度r为半径的圆盘；τ是时间窗，即无人机和障碍物在τ时间内不会发生碰撞；

表示无人机相对于障碍物O在时间窗τ内会发生碰撞的速度集合；p_O|A表示无人机A和障碍物O之间的相对距离；v表示固定翼飞机的速度；t表示时间；r_AO表示无人机A和障碍物O的总半径之和；

S4.2：飞机和障碍物之间的相对速度在速度障碍区域内，飞机将在时间间隔τ内与障碍物发生碰撞，采用最优互反避碰算法，获得无碰撞速度集合，定义如下：

其中，

表示固定翼飞机A相对于障碍物O在时间窗τ内不会发生碰撞的速度集合；

为飞机当前速度；u表示的相对速度离开速度障碍区域的最小速度变化，n是外法向量；系数λ是一种责任系数，确定飞机应当承担多少责任，由于障碍物是非合作式，所以λ＝1；

S4.3：计算无人机的运动学约束S_A，无碰撞速度集表达式为：

其中，

是无人机A相对于障碍物O的在运动学约束S_A和无碰撞速度集合

以及最大速度

约束下的可行相对速度集合；v_O是障碍物O的速度；

S4.4：新的相对速度

由基于速度域的采样函数得出，具体为：

其中，R是一个随机数，在[0，1]区间内采样；α是一个参数来确定选择最优速度的概率；

S4.5：新速度v_A，new由以下公式计算：

其中，v_O是障碍物的速度；新速度v_A，new保证固定翼飞机从父节点的位置飞到子节点的位置的直线路径是无碰撞的，并且满足固定翼飞机的运动学约束；

S4.6：父节点和子节点之间的边是一个三次贝塞尔曲线，无人机从父节点到子节点的时间消耗为τ，子节点的位置可以根据父节点的状态和新速度计算获得，具体公式为：

Node_new·p←Node_pparent·p+τ*v_A，new

其中，Node_new表示新节点；Node_new·P表示新节点的位置；Node_parent表示父节点；Node_parent·p表示父节点的位置；τ表示时间窗；v_A，new表示无人机A生成的新速度；

子节点的速度可以通过将新速度v_A，new绕轴e_axis旋转θ_r角获得；

其中，cos^-1表示反cos函数；v_A，cur是父节点的速度；v_A，new是子节点的速度；·表示点乘；|v_A,cur|表示v_A，cur的模；×表示叉乘；θ_r是父节点的速度v_A，cur和生成新速度v_A，new之间的夹角；e_axis是父节点的速度v_A，cur和生成新速度v_A，new所在平面的法向量；

步骤五：使用三次贝塞尔曲线直接定义父节点和新子节点之间的边，而不需要进行后处理；

步骤六：在动态障碍物和静态障碍物的场景中，判断新生成的边是否与障碍物发生碰撞；

步骤七：当一个新节点被添加到树中时，计算新节点的能量消耗代价和时间消耗代价；

步骤八：重复步骤一至步骤七，直到新节点落入目标区域；

步骤九：使用回溯在随机树中找到可行路径。

2.根据权利要求1所述的一种基于样条-rrt和速度障碍的无人机避障方法，其特征在于，在所述步骤二中，根节点的能量消耗代价和时间消耗代价都初始化为0。

3.根据权利要求1所述的一种基于样条-rrt和速度障碍的无人机避障方法，其特征在于，α的具体定义为：

α＝dist(p_cur-p_goal)/dist(p_start-p_goal)，

其中，p_cur、p_goal和p_start分别表示飞机当前的位置、目标位置和初始位置；dist是求位置之间欧式距离的函数；

R的值大于α时，新的相对速度

由子函数Uniform在工作空间

中随机均匀采样获得；R的值小于或等于α时，相对速度

选择由子函数Optimal生成的与期望速度

最接近的最优相对速度

由以下公式定义：

其中，

是函数生成的无人机A相对于障碍物0的最优相对速度；

是固定翼飞机A相对于障碍物0的期望速度。

4.根据权利要求1所述的一种基于样条-rrt和速度障碍的无人机避障方法，其特征在于，在所述步骤五中，贝塞尔曲线B(s)的公式如下：

B(s)＝(1-s)³p_p+3s(1-s)²p_m1+3s²(1-s)p_m2+s³p_c

其中，s是一个实数，在0和1之间变化；p_p和p_c分别是父节点和子节点的位置；p_m1和p_m2分别为两个控制点；

两个控制点p_m1和p_m2的公式具体为：

p_m1＝p_p+v_p*|p_c-p_p|/3，

p_m2＝p_c-v_c*|p_c-p_p|/3

其中，p_p为父节点的位置；v_p为父节点的速度；p_c为子节点的位置p_c、v_c为子节点的速度。

5.根据权利要求1所述的一种基于样条-rrt和速度障碍的无人机避障方法，其特征在于，在所述步骤六中，判断障碍物碰撞的方法为：

贝塞尔曲线B(s)包含在由控制点组成的凸包中，凸包根据无人机的半径扩展成凸多面体；

利用分离轴定理来判断无人机凸多面体与障碍物凸多面体之间是否存在交集，来进行障碍物碰撞检测。

Images