技术摘要：
本发明涉及一种基于样条‑rrt和速度障碍的无人机避障方法，在无人机传感范围内建立随机搜索树，以当前位置和速度为起始节点，以全局路径与传感范围的交点位置为目标位置，在速度可行域内采样得到可行速度，然后生成一个新的节点。对生成的边进行碰撞检测后，更新树，重  全部
背景技术：
在实际应用中，自主完成救援搜索、农业、测绘等任务的无人机越来越受到人们的 关注。在复杂环境下航行时，为了避免静态和动态障碍，固定翼飞机在空气动力学的约束 下，在线再生一条平滑连续的路径至关重要。 对于机器人的路径规划，提出一种基于采样的RRT(快速探索随机树)方法及其变 体，能够有效地提供固定翼飞机通过静态障碍物的全局路径规划。RRT方法是创建一个从起 始点到目标点的可能操作构建的树，所有的障碍物在基于RRT的方法中都被视为静态障碍 物，因此在每个控制时间步长中，机器人会重新生成一个新的无碰撞路径或修改在之前生 长的现有树。导致机器人需要等待更长的时间才能通过RRT方法得到计算结果。
技术实现要素：
本发明为克服上述现有技术中机器人无碰撞路径规划的计算效率低问题，提供一 种基于样条-rrt和速度障碍的无人机避障方法，该避障方法结合了样条-RRT算法和速度障 碍(VO)方法，在局部区域生长一棵随机树，同时使用速度障碍(VO)方法扩展树的边缘并拒 绝不可用的节点，提高了树木生长的效率和以及树木生长更加平稳，即提高路径规划的效 率和路线的稳定性。 为解决上述技术问题，本发明采用的技术方案是：一种基于样条-rrt和速度障碍 的无人机避障方法，包括如下步骤： 步骤一：以初始状态xstart为根节点初始化搜索树，其中状态表示节点在树中的位 置和速度； 步骤二：随机树每条边的代价被分成两部分，其中包括节点的能量消耗代价cost 以及时间消耗代价t； 步骤三：随机树生长的每一步，随机树的父节点被随机选择； 步骤四：获取新的子节点，子节点的的位置可以根据父节点的状态和新速度计算 获得，子节点的速度可以通过将新速度vA,new绕轴eaxis旋转θr角获得。 步骤五：使用三次贝塞尔曲线直接定义父节点和新子节点之间的边，而不需要进 行后处理； 步骤六：在动态障碍物和静态障碍物的场景中，判断新生成的边是否与障碍物发 生碰撞； 步骤七：当一个新节点被添加到树中时，计算新节点的能量消耗代价和时间消耗 代价； 步骤八：重复步骤一至步骤七，直到新节点落入目标区域； 5 CN 111580548 A 说　明　书 2/7 页 步骤九：使用回溯在随机树中找到可行路径。 在包含多个静态障碍物和动态障碍物的动态环境中，无人机对静态环境的知识是 先验已知的，利用地图的先验知识，通过样条-RRT算法离线生成全局路径。当无人机沿着全 局路径飞行时，当遇到障碍物在无人机前方移动，无人机可以利用有限感知距离的机载传 感器检测动态障碍物。当飞机探测到其感应范围内的动态障碍物时，利用上述所提出的方 法在传感范围内建立随机搜索树，以当前位置和速度为起始节点xstart，以全局路径与传感 范围的交点位置为目标位置xgoal。由于无人机的速度是变化的，所以无人机相对于动态障 碍物的速度可行域也会发生变化。在速度可行域内采样得到可行速度，然后生成一个新的 节点。对生成的边进行碰撞检测后，更新树，重复这个过程，直到随机树生长到达目标位置 xgoal。利用回溯法在随机树中获得局部路径，无人机沿着局部路径穿过动态障碍物后，继续 沿着原全局路径飞行，直到目标区域。 优选的，在所述步骤二中，根节点的能量消耗代价和时间消耗代价都初始化为0。 优选的，在所述步骤四中，具体的流程为： S4.1：采用速度障碍算法计算子节点的速度，随机树的单个边将被转换为基于VO 的场景，无人机相对于动态障碍物的速度障碍(VO)区域定义如下： 其中，D(p,r)表示以位置p为中心；以长度r为半径的圆盘；τ是时间窗，即无人机和 障碍物在τ时间内不会发生碰撞； 表示无人机相对于障碍物O在时间窗τ内会发生碰撞 的速度集合；pO|A表示无人机A和障碍物O之间的相对距离；v表示固定翼飞机的速度；t表示 时间；rAO表示无人机A和障碍物O的总半径之和； S4.2：飞机和障碍物之间的相对速度在VO区域内,飞机将在时间间隔τ内与障碍物 发生碰撞，采用最优互反避碰算法，获得无碰撞速度集合，定义如下： 其中， 表示固定翼飞机A相对于障碍物O在时间窗τ内不会发生碰撞的速度 集合； 为飞机当前速度；u表示的相对速度离开VO区域的最小速度变化，n是外法向量； 系数λ是一种责任系数,确定飞机应当承担多少责任，由于障碍物是非合作式,所以λ＝1； S4.3：计算无人机的运动学约束SA，无碰撞速度集表达式为： 其中， 是无人机A相对于障碍物O的在运动学约束SA和无碰撞速度集合 以及最大速度 约束下的可行相对速度集合；vO是障碍物O的速度； 考虑飞行轨迹角角度作为固定翼飞机的运动学约束，能够保证无人机飞行轨迹的 平稳性。 S4.4:新的相对速度 由基于速度域的采样函数得出，具体为： 6 CN 111580548 A 说　明　书 3/7 页 其中，R是一个随机数，在[0，1]区间内采样；α是一个参数来确定选择最优速度的 概率； S4.5：新速度vA，new由以下公式计算： 其中，vO是障碍物的速度；新速度vA，new保证固定翼飞机从父节点的位置飞到子节 点的位置的直线路径是无碰撞的，并且满足固定翼飞机的运动学约束； S4.6：父节点和子节点之间的边是一个三次贝塞尔曲线，无人机从父节点到子节 点的时间消耗为τ，子节点的位置可以根据父节点的状态和新速度计算获得，具体公式为： Nodenew.p←Nodeparent.p τ*vA，new 其中，Nodenew表示新节点；Nodenew·p表示新节点的位置；Nodeparent表示父节点； Nodeparent·p表示父节点的位置；τ表示时间窗；vA，new表示无人机A生成的新速度； 子节点的速度可以通过将新速度vA，new绕轴eaxis旋转θr角获得； 其中，cos-1表示反cos函数；vA，cur是父节点的速度；vA，new是子节点的速度；·表示 点乘；|vA，cur|表示vA，cur的模；×表示叉乘；θr是父节点的速度vA，cur和生成新速度vA，new之间 的夹角；eaxis是父节点的速度vA，cur和生成新速度vA，new所在平面的法向量。 优选的，α的具体定义为： α＝dist(pcur-pgoal)/dist(pstart-pgoal)， 其中，Pcur、pgoal和Pstart分别表示飞机当前的位置、目标位置和初始位置；dist是求 位置之间欧式距离的函数； 随着当前位置与目标位置距离的减小，选择最优速度的概率增大。计算得到的最 优速度 最接近最优速度 用于引导固定翼飞机飞向目标位置。因此，选择最优 速度的概率越大，随机树在位置域中向目标位置生长的速度越快。 R的值大于α时，新的相对速度 由子函数Uniform在工作空间 中随机均 匀采样获得；R的值小于或等于α时，相对速度 选择由子函数Optimal生成的与期望速度 最接近的最优相对速度 由以下公式定义： 7 CN 111580548 A 说　明　书 4/7 页 其中， 是函数生成的无人机A相对于障碍物0的最优相对速度； 是固 定翼飞机A相对于障碍物0的期望速度。 优选的，在所述步骤五中，贝塞尔曲线B(s)的公式如下： B(s)＝(1-s)3p 3s(1-s)2p 3s2p m1 (1-s)p 3m2 s pc. 其中，s是一个实数，在0和1之间变化；pp和pc分别是父节点和子节点的位置；Pm1和 pm2分别为两个控制点； 两个控制点Pm1和Pm2的公式具体为： pm1＝pp vp*|pc-pp|/3， pm2＝pc-vc*|pc-pp|/3. 其中，pp为父节点的位置；vp为父节点的速度；pc为子节点的位置pc、vc为子节点的 速度。 优选的，在所述步骤六中，判断障碍物碰撞的方法为： 贝塞尔曲线B(s)包含在由控制点组成的凸包中，凸包根据无人机的半径扩展成凸 多面体； 利用分离轴定理来判断无人机凸多面体与障碍物凸多面体之间是否存在交集，来 进行障碍物碰撞检测。 与现有技术相比，本发明的有益效果是：当动态障碍物在无人机的感知范围内时， 通过重建无碰撞路径后并按照无碰撞路径通过动态障碍物后，恢复原设定的避障路线继续 行驶，使得路径重规划只需要执行一次而无需不断重新规划路径，提高路径规划的效率和 路线的稳定性。 附图说明 图1是本发明的一种基于样条-rrt和速度障碍的无人机避障方法的流程图。