适用于无人机的无碰撞路径规划方法、系统及介质
阅读说明：本技术 适用于无人机的无碰撞路径规划方法、系统及介质 (Collision-free path planning method, system and medium suitable for unmanned aerial vehicle ) 是由 丁烨 陈永学 董伟 于 2020-11-27 设计创作，主要内容包括：本发明提供了一种无人机无碰撞路径规划方法,通过对输入路径点使用B样条曲线进行插值得到初步飞行路径；使用球体和平面得到无人机和障碍物近似模型,在路径曲线离散点位检测碰撞情况,并获取碰撞区域的曲线参数区间；根据参数区间建立新的路径点,重新进行B样条曲线插值并重新进行碰撞检测,反复迭代直到得到最终的无人机无碰撞路径。本发明可解决给定任务空间飞行路径点时,连续的无碰撞路径规划问题,并具有极高的计算效率,具有重要的理论和现实意义,适用于无人机在复杂环境中执行任务时进行无碰撞的路径规划,提高安全性和飞行效率。(The invention provides a collision-free path planning method for an unmanned aerial vehicle, which is characterized in that a preliminary flight path is obtained by interpolating input path points by using a B spline curve; obtaining an unmanned aerial vehicle and obstacle approximate model by using the sphere and the plane, detecting a collision condition at discrete point positions of a path curve, and obtaining a curve parameter interval of a collision area; and establishing new path points according to the parameter interval, performing B spline curve interpolation again, performing collision detection again, and repeating iteration until a final collision-free path of the unmanned aerial vehicle is obtained. The method can solve the problem of continuous non-collision path planning when a task space flight path point is given, has extremely high calculation efficiency, has important theoretical and practical significance, is suitable for non-collision path planning when the unmanned aerial vehicle executes a task in a complex environment, and improves the safety and the flight efficiency.)
技术领域
本发明涉及无人机避障技术领域，具体地，涉及一种适用于无人机的无碰撞路径规划方法、系统及计算机可读介质。
背景技术
轨迹规划是无人机自主巡航中最重要的任务之一，该问题普遍通过路径生成和速度规划两个步骤求解。为满足任务要求，路径生成需要得到一条通过给定路径点的几何曲线，同时需要无人机在该曲线上不与环境中的障碍物发生碰撞。为有利于速度规划时对无人机的跃度进行约束，一般需要路径曲线连续。常用的路径生成算法有人工势场法，启发式搜索方法以及几何方法等。
以A*算法为代表的启发式搜索方法计算时间长，效率较低；人工势场法计算量相对较小，但易陷入局部最优；几何方法通过对环境进行几何建模，大大缩短碰撞路径的生成时间。
现有几何方法中，一些方法在模型构建时较复杂，对规划效率产生不利影响，同时很多方法在满足通过给定路径点的情况下无法保证曲线的光滑性。
经过检索，专利文献CN111580563A公开了一种基于种子搜索的无人机自主避障飞行方法，该方法针对复杂多变的城市建筑环境，该现有技术首先在任务空间中均匀播撒若干种子，然后以终点为目标设置无人机的飞行方向，启动无人机飞行；无人机在飞行过程中，实时探测前方是否有障碍物，如果探测到前方有障碍物，则根据一定规则搜索无人机附近的种子，然后以该种子为目标重新设置无人机飞行方向，待无人机飞到该种子位置时，再重新以终点位目标设置飞行方向，继续飞行探测，如此反复，直至无人机距离终点的位置小于设置的阈值，从而完成飞行任务。但是该现有技术的不足之处在于模型构建仍然较复杂，规划效率不高。
因此，亟需研发一种逻辑清晰、构型简单并且规划效率高的无人机避障方法。
发明内容
针对现有技术中的缺陷，本发明的目的是提供一种适用于无人机的无碰撞路径规划方法、系统及计算机可读介质，能够生成通过给定路径点的连续的无碰撞路径。本发明适用于无人机在复杂环境中执行任务时进行无碰撞的路径规划，提高安全性和飞行效率。
根据本发明提供的一种适用于无人机的无碰撞路径规划方法，其特征在于，包括如下步骤：
步骤1：对给定路径点进行B样条曲线插值获取路径曲线；
步骤2：根据获取的路径曲线进行无人机碰撞检测；
步骤3：对碰撞检测结果中存在碰撞的路径曲线构建新的路径点，对碰撞检测结果中不存在碰撞的路径曲线直接输出无碰撞路径；
步骤4：对新的路径点重新进行B样条曲线插值并返回步骤2。
优选地，步骤1中B样条曲线插值使用以下公式计算给定路径点的参数值：
其中，pi,i∈{0,1,2,3,…,N}表示给定的第i+1个路径点坐标；
为对应于pi点的B样条曲线参数值。
使用以下公式计算5次B样条曲线的节点矢量：
通过求解以下线性方程组解得B样条曲线的控制点：
其中，Ni,5(u),i＝0,1,...,N为5次B样条曲线的基函数，从而得到通过输入路径点的5次B样条曲线S(u),u∈[0,1]。将B样条插值迭代次数Nint设置为0。
优选地，步骤2包括：
根据无人机的三维模型，得到能完全包裹该无人机的最小半径球体，使用该球体作为无人机的近似代替，以下称为无人机球。
将任务空间中的障碍物进行分类，将墙面、较大的立方体等障碍物用有向平面代替，以下称为障碍面，将回转体、较小的立方体以及一些其他不规则的障碍物用一个或者一系列球体近似代替，以下称为障碍球。代替过程需保证障碍物完全位于障碍面一侧或障碍球将障碍物完全覆盖。
以参数步长du对路径曲线S(u)进行离散，得到一系列离散路径点S(ui)。在每个离散路径点位计算无人机球与每个障碍面和障碍球之间的距离，如果无人机球与任一障碍面或障碍球发生碰撞则对该离散点进行标记。
最终输出M段发生碰撞的曲线参数区间[us,i,ue,i]，i∈{1,2,3,…,M}，其中us,i为第i段碰撞区间的首参数值，ue,i为第i段碰撞区间的末参数值。
优选地，步骤3根据步骤2获得的碰撞情况进行分类处理。
优选地，步骤3包括如下步骤：
步骤3.1：若不存在碰撞区间，则输出无碰撞路径曲线；
步骤3.2：若存在M段发生碰撞的曲线参数区间，则根据每段参数区间[us,i,ue,i]，i∈{1,2,3,…,M}，生成一个新的路径曲线插值点pm,i，并根据参数值与步骤1或步骤4所得路径插值点参数值之间的大小关系，将pm,i加入到路径插值点列表的合适位置。
优选地，步骤3.2根据B样条插值迭代次数，其中一种情况生成插值点pm,i，
步骤3.2.1:若B样条插值迭代次数Nint小于或等于某设定阈值，则令其中，du为对曲线进行离散化时所取的参数步长；
令其中，S(u)表示B样条曲线在参数为u时的坐标，设置k＝1为初始比例；
对处于pm,i处的无人机模型进行碰撞检测，若发生碰撞，则令
同时，适当增大比例k：k＝k+dk，其中，dk＞0为预设的比例更新步长；
重新计算pm,i，并反复迭代直到处于pm,i处的无人机模型与环境不发生碰撞为止。
优选地，步骤3.2根据B样条插值迭代次数，另一种情况生成插值点pm,i，
步骤3.2.2：若B样条插值迭代次数Nint大于某设定阈值，则：
令其中，prand为随机生成的单位向量，kr为比例系数，初始设置为kr＝2r，r为模拟无人机的球体的半径；
对处于pm,i处的无人机模型进行碰撞检测，若发生碰撞，则重新生成pm,i并适当增大kr，反复迭代直到处于pm,i处的无人机模型与环境不发生碰撞为止。
优选地，步骤4包括：
采用与步骤1所述相同的插值方法，将步骤3所得插值点列表中的路径点使用5次B样条曲线进行插值，得到新的路径曲线S(u),u∈[0,1]，更新B样条插值迭代次数：Nint＝Nint+1，返回步骤2进行无人机的碰撞检测。
根据本发明提供的一种适用于无人机的无碰撞路径规划系统，包括：
模块M1：对给定路径点进行B样条曲线插值获取路径曲线；
模块M2：根据获取的路径曲线进行无人机碰撞检测；
模块M3：对碰撞检测结果中存在碰撞的路径曲线构建新的路径点，对碰撞检测结果中不存在碰撞的路径曲线直接输出无碰撞路径；
模块M4：对新的路径点重新进行B样条曲线插值后输入模块M2。
根据本发明提供的一种计算机可读介质，其存储有可由适用于无人机的无碰撞路径规划设备执行的计算机程序，当计算机程序在适用于无人机的无碰撞路径规划设备上运行时，使得适用于无人机的无碰撞路径规划设备执行上述任一项的适用于无人机的无碰撞路径规划方法。
与现有技术相比，本发明具有如下的有益效果：
1、本发明通过获取碰撞区域的曲线参数区间，根据参数区间建立新的路径点，重新进行B样条曲线插值并重新进行碰撞检测，反复迭代直到得到最终的无人机无碰撞路径，解决了给定任务空间飞行路径点时，连续的无碰撞路径规划问题，并具有极高的计算效率，具有重要的理论和现实意义。
2、本发明适用于无人机在复杂环境中执行任务时进行无碰撞的路径规划，提高安全性和飞行效率。
3、本发明的模型构建简单，并且有助于提高规划效率，在满足通过给定路径点的同时能够保证曲线的光滑性。
附图说明
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
图1为本发明的流程示意图；
图2为本发明中的四旋翼无人机与对应的最小外接球示意图；
图3为本发明中的任务空间与所规划的飞行路径图。
具体实施方式
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
如图1所示，根据本发明提供的无人机无碰撞路径规划方法，包括如下步骤：
步骤1：对给定路径点进行B样条曲线插值获取初步路径，步骤1中的B样条曲线插值包括，读取输入的N+1个无人机路径点pi,i∈{0,1,2,3,…,N}；
使用以下公式计算给定路径点的参数值：
使用以下公式计算5次B样条曲线的节点矢量：
通过求解以下线性方程组解得B样条曲线的控制点：
其中，Ni,5(u),i＝0,1,...,N为5次B样条曲线的基函数，从而得到通过输入路径点的5次B样条曲线S(u),u∈[0,1]。将B样条插值迭代次数Nint设置为0。
步骤2：进行无人机碰撞检测，也就是根据无人机的三维模型，得到能完全包裹该无人机的最小半径球体，使用该球体作为无人机的近似代替，以下称为无人机球。
将任务空间中的障碍物进行分类，将墙面、较大的立方体等障碍物用有向平面代替，以下称为障碍面，将回转体、较小的立方体以及一些其他不规则的障碍物用一个或者一系列球体近似代替，以下称为障碍球。代替过程需保证障碍物完全位于障碍面一侧或障碍球将障碍物完全覆盖。
以参数步长du对路径曲线S(u)进行离散，得到一系列离散路径点S(ui)。在每个离散路径点位计算无人机球与每个障碍面和障碍球之间的距离，如果无人机球与任一障碍面或障碍球发生碰撞则对该离散点进行标记。
最终输出M段发生碰撞的曲线参数区间[us,i,ue,i]，i∈{1,2,3,…,M}，其中us,i为第i段碰撞区间的首参数值，ue,i为第i段碰撞区间的末参数值。。
步骤3：根据碰撞情况加入新的路径点或输出无碰撞路径，根据步骤2所得碰撞曲线参数区间，分两种情况进行处理：
情况1：若不存在碰撞区间，即M＝0，则输出无碰撞路径曲线S(u),u∈[0,1]。
情况2：若存在发生碰撞的曲线参数区间，即M＞0，则根据每段参数区间[us,i,ue,i]，i∈{1,2,3,…,M}，生成一个新的路径曲线插值点pm,i，并根据参数值与步骤1或步骤4所得路径插值点参数值之间的大小关系，将pm,i加入到路径插值点列表的合适位置。
情况2中根据B样条插值迭代次数Nint，又分两种情况生成插值点pm,i：
情况2.1：若B样条插值迭代次数Nint小于或等于某设定阈值，则令
其中，du为步骤2中对曲线进行离散化时所取的参数步长。
令
其中，S(u)表示B样条曲线在参数为u时的坐标，
设置k＝1为初始比例。
对处于pm,i处的无人机球进行碰撞检测，若发生碰撞，则更新
同时，适当增大比例k：
k＝k+dk
其中dk＞0为预设的比例更新步长。
重新计算pm,i，并反复迭代直到处于pm,i处的无人机球与环境不发生碰撞为止。
情况2.2：若B样条插值迭代次数Nint大于某设定阈值，则
令
其中，prand为随机生成的单位向量，kr为比例系数，初始设置为kr＝2r，r为无人机球的半径。
对处于pm,i处的无人机模型进行碰撞检测，若发生碰撞，则重新生成pm,i并适当增大kr，反复迭代直到处于pm,i处的无人机模型与环境不发生碰撞为止。
步骤4：重新进行B样条曲线插值并返回步骤2，采用与步骤1所述相同的插值方法，将步骤3所得插值点列表中的路径点使用5次B样条曲线进行插值，得到新的路径曲线S(u),u∈[0,1]，更新B样条插值迭代次数：Nint＝Nint+1。
实施例：
如图1所示，本实施例的具体流程包括：对给定路径点进行B样条曲线插值获取路径曲线；根据获取的路径曲线进行无人机碰撞检测；对碰撞检测结果中存在碰撞的路径曲线构建新的路径点，对碰撞检测结果中不存在碰撞的路径曲线直接输出无碰撞路径；对新的路径点重新进行B样条曲线插值并返回步骤2。
在以下实施例中，如图2-图3所示，图3中8个相同大小的小球为给定的8个路径点处的无人机球，两个平面与三个较大的球为代表障碍物的障碍面与障碍球，实线表示第一次插值所得路径曲线，虚线为最终得到的无碰撞飞行路径曲线。给定如图3中8个小球所示的路径点，坐标如下:
x(m)
y(m)
z(m)
0
0
0
1
3
15
8
-9
18
10
21
5
15
30
9
22
27
3
25
20
21
30
14
12
使用本发明提出的方法，进行无碰撞路径规划。具体步骤如下：
第一步，获取初步路径
读取输入的8个无人机路径点
使用以下公式计算8个路径点处的参数值：
得
使用以下公式计算得到5次B样条曲线的节点矢量：
通过求解以下线性方程组解得B样条曲线的控制点：
其中，
Ni,5(u),i＝0,1,...,N为5次B样条曲线的基函数。
得到控制点坐标：
从而得到5次B样条曲线S(u),u∈[0,1]，如图3中实线所示。
设置B样条插值迭代次数Nint＝0。
第二步，碰撞检测
根据无人机的三维模型，设置无人机球为四旋翼无人机的最小外接球，如2所示，半径为1。
根据任务空间障碍物，设置障碍面为z＝-1和z＝15两个平面，设置障碍球球心坐标分别为[10,10,3],[0,7,10],[1,6,12]，半径分别为5,2,2。障碍面与障碍球如图3所示。
以参数步长du＝0.01对路径曲线S(u)进行离散，得到一系列离散路径点S(ui)。在每个离散路径点位计算无人机球与每个障碍面和障碍球之间的距离，如果无人机球与任一障碍面或障碍球发生碰撞则对该离散点进行标记。
最终输出4段发生碰撞的曲线参数区间：
[0.01,0.05],[0.11,0.12],[0.39,0.45],[0.89,0.97]
第三步，生成新的路径插值点
由于上述检测存在碰撞曲线参数区间，则对每段参数区间[us,i,ue,i]，i∈{1,2,3,4}，需要生成一个新的路径曲线插值点pm,i，并根据参数值与上述所得路径插值点参数值之间的大小关系，将pm,i加入到路径插值点列表的合适位置。
由于Nint＝0，则按如下方法生成pm,i：
以第一个参数区间[0.01,0.05]为例，令
设置k＝1为初始比例，令
对处于pm,1处的无人机球进行碰撞检测，检测结果为不发生碰撞。由于处于与之间，故将pm,1加入到无人机路径插值点p0与p1之间。
第四步，采用与第一步相同的插值方法，将所得新的插值点列表中的路径点使用5次B样条曲线进行插值，得到新的路径曲线S(u),u∈[0,1]。更新B样条插值迭代次数：
Nint＝Nint+1＝1。
返回第二步进行无人机的碰撞检测。
通过多次迭代，最终得到无人机无碰撞的路径曲线S(u),u∈[0,1]，如图3中虚线所示。
本发明可解决给定任务空间飞行路径点时，连续的无碰撞路径规划问题，并具有极高的计算效率，具有重要的理论和现实意义。
本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统及其各个装置、模块、单元以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统及其各个装置、模块、单元以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同功能。所以，本发明提供的系统及其各项装置、模块、单元可以被认为是一种硬件部件，而对其内包括的用于实现各种功能的装置、模块、单元也可以视为硬件部件内的结构；也可以将用于实现各种功能的装置、模块、单元视为既可以是实现方法的软件模块又可以是硬件部件内的结构。
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。
- 上一篇：一种医用注射器针头装配设备
- 下一篇：一种空间指向测量敏感器动态精度提升方法