一种基于保角映射分析的无人飞行器空间避障方法与流程
本发明属于无人飞行器空间避障飞行的控制领域，尤其是涉及一种基于保角映射分析方法的无人飞行器空间避障方法。
背景技术：
当前，无人飞行器无论是在民用领域还是军事领域中都得到了广泛的应用。但随着无人飞行器执行任务范围的扩大和执行环境的日益复杂，如何提高无人飞行器在复杂环境中执行艰巨任务的生存能力成为无人飞行器技术研究中亟待解决的关键问题之一。为无人飞行器配备高性能的自主避障系统可实现无人飞行器在复杂环境下对动静态障碍物的避碰，而自主避障系统的核心是自主避障算法。
为了提升无人飞行器的自主避障系统性能，可提高无人飞行器在复杂动态环境中执行任务的生存能力，这就要求自主避障系统配备的自主避障算法能实现对动静态威胁障碍物的避碰。现有避障算法对复杂环境中多威胁障碍物的避碰较为复杂，且多采用降维的思想进行处理，很难得到无人飞行器对多威胁障碍物避碰的最小速度矢量偏转方向。
技术实现要素：
针对上述提出的问题，有必要提出一种新的空间避障方法，该方法能够简单、直观地处理多威胁障碍物条件下的避碰问题，并能够确定多动态威胁障碍避碰的速度矢量方向。
本发明提出的避障方法具体包括以下步骤：
步骤S1，构建三维速度障碍锥；
步骤S2，运用构建的三维速度障碍锥进行避碰判断；
步骤S3，建立相对碰撞区RCC和绝对碰撞区ACC；然后建立空间速度障碍球冠，并解出球冠的参数；
步骤S4，对空间速度障碍球冠进行保角映射，求解避障方向，具体包括以下子步骤，
步骤S41，建立映射坐标系，以为平面直角坐标系的横轴，oθ为平面直角坐标系的纵轴，建立平面方向角直角坐标系和θ分别表示为无人飞行器速度矢量vu的偏航角和俯仰角，且
步骤S42，将空间速度障碍球冠的边界保角映射为坐标系内以点为圆心的⊙Co，⊙Co的半径为γ；其中，为矢量的方向角，而PU为无人飞行器的位置，C为空间速度障碍球冠的中心点，即球冠大圆中心；γ为PU与空间速度障碍球冠的大圆边界形成圆锥的半顶角；
步骤S43，将当前时刻无人飞行器速度矢量vu的偏航角和俯仰角记为无人飞行器的速度矢量方向角则矢量与⊙Co的交点为最小避障速度矢量偏转方向；
步骤S5，求解避障点。
除了步骤S4及其子步骤外，其它步骤的具体方法可参见CN105717942A专利申请中的技术方案。
优选的，当障碍物为多个时，步骤S2要对多个障碍物进行避障判断，如果无人飞行器的速度矢量vu与障碍物Oi的速度矢量voi之间的相对速度矢量vuoi在障碍物Oi形成的障碍锥内，则需要同时对多个障碍物进行避碰，否则不需要同时对多个障碍物进行避碰。
优选的，当需要同时对多个障碍物进行避碰时，
步骤S42中需要对每个空间速度障碍球冠的边界均进行保角映射，将映射后所得各映射圆的并集记为∪⊙；
步骤S43中最小避障速度矢量偏转方向为无人飞行器的速度矢量方向角到∪⊙边界的最小距离点对应方向。
本发明的提出的基于保角映射分析的无人飞行器空间避障方法，确定了无人飞行器对威胁障碍物避碰的最小速度矢量偏转方向，简化了多威胁障碍物避碰分析。使用该方法处理多威胁障碍物条件下的避碰问题，具有简单、直观等优点。
附图说明
图1基于保角映射分析的无人飞行器空间避障流程图；
图2三维速度障碍锥示意图；
图3相对碰撞区RCC和绝对碰撞区ACC示意图；
图4空间速度障碍球冠示意图；
图5空间速度障碍球冠在速度球上的表示图；
图6映射坐标系示意图；
图7无人飞行器对单威胁障碍物避碰的仿真结果；
图8无人飞行器对多威胁障碍物避碰的仿真结果。
具体实施方式
下面结合附图进一步说明本发明的具体实施方式。
如附图1所示，本发明提出的避障方法主要包括：构建三维速度障碍锥，运用三维速度障碍锥进行避碰判断，建立空间速度障碍球冠并求解球冠参数，对空间速度障碍球进行保角映射、进而求解避障方向，以及求解避障点五个步骤。
具体包括以下步骤：
步骤S1，构建三维速度障碍锥；
利用无人飞行器(Unmanned Aerial Vehicle，UAV)自身携带的障碍感知模块，可感知周围环境中存在的动静态障碍物，并可获取障碍物的位姿信息Poseobs(PO,vo)，即障碍物位置坐标PO(xo,yo,zo)，障碍物速度矢量无人飞行器的飞行感知模块可实时地获取自身的位姿信息Poseuav(PU,vu)，即无人飞行器速度矢量无人飞行器位置坐标PU(xu,yu,zu)；设定障碍感知模块的感知距离为d0。
为简化无人飞行器对威胁障碍物的避碰，将无人飞行器简化为一点PU，并将障碍物点膨化为以Po为球心、半径为R的障碍球，然后过PU点作障碍球的切线，障碍球上所有的切点形成切线圆Tangent Circle，则所有以PU为顶点障碍球的切线l形成的锥面为三维速度障碍锥，三维速度障碍锥也称为空间碰撞锥，如图2所示。
步骤S2，运用构建的三维速度障碍锥进行避碰判断；
根据无人飞行器速度vu和障碍物速度vo的大小和方向，求解无人飞行器相对于障碍物O的相对速度矢量vuo的大小和方向；然后求解相对速度矢量vuo与无人飞行器和障碍物的位置矢量之间的夹角大小α，如图2所示；
然后求出三维速度障碍锥的半顶角α0，其中R为障碍球PO的半径，d0为无人飞行器到障碍物O的距离。
当障碍物为单个的时候，判断障碍锥的半顶角α0与α的关系，若α＜α0，需要对障碍物进行避碰，则继续进行步骤S3；若α＞α0，不需要对障碍物进行避碰，则退出。
当障碍物为多个的时候，无人飞行器需要同时对多个障碍物进行避碰的条件为：无人飞行器对每一个障碍物的避碰判断条件同时成立。
即：无人飞行器需要同时对多个障碍物进行避碰的判定条件可确定为
即无人飞行器的速度矢量vu与障碍物Oi的速度矢量voi之间的相对速度矢量vuoi都在障碍物Oi形成的障碍锥内，此时需要对多个障碍物进行避碰，否则不需要同时对多个障碍物进行避碰。
步骤S3，建立相对碰撞区RCC和绝对碰撞区ACC；然后建立空间速度障碍球冠，并解出球冠的参数；
本步骤的方法可具体参见CN105717942A专利申请中的技术方案。
建立的相对碰撞区RCC和绝对碰撞区ACC如图3所示；建立空间速度障碍球冠如图4所示。将空间速度障碍球冠表示为其中r为速度球的球心PU和球冠大圆中心C之间所形成矢量的长度，和θr为矢量的方向角，即分别与ox和oz轴之间的夹角，并用γ表示无人飞行器位置坐标点PU与球冠G大圆边界形成圆锥的半顶角，γ∈[0,π]。空间速度障碍球冠在速度球上的表示如图5所示。
根据空间速度矢量几何关系，解出球冠的参数以及γ。
步骤S4，对空间速度障碍球冠进行保角映射，求解避障方向，具体包括以下子步骤，
步骤S41，建立映射坐标系，以为平面直角坐标系的横轴，oθ为平面直角坐标系的纵轴，建立平面方向角直角坐标系和θ分别表示为无人飞行器速度矢量vu的偏航角和俯仰角，且θ∈[-π/2,π/2]；建立的映射坐标系如图6所示。
步骤S42，将空间速度障碍球冠的边界保角映射为坐标系内以点为圆心的⊙Co，⊙Co的半径为γ；
当需要同时对多个障碍物进行避碰时，在步骤S42中需要对每个空间速度障碍球冠的边界均进行保角映射，将映射后所得各映射圆的并集记为∪⊙。
步骤S43，将当前时刻无人飞行器速度矢量vu的偏航角和俯仰角记为无人飞行器的速度矢量方向角
当仅需要对单个障碍物进行避碰时，则矢量与⊙Co的交点为最小避障速度矢量偏转方向；
当需要同时对多个障碍物进行避碰时，最小避障速度矢量偏转方向为无人飞行器的速度矢量方向角到∪⊙边界的最小距离点对应方向。
步骤S5，求解避障点。
根据步骤S4中求解的最小避障速度矢量偏转方向，采用现有技术中的方法求解避障点。
实施例1：单个威胁障碍物的避碰
在动态不确定环境下，无人飞行器执行从起飞点Ps(0,0,0)至目标点Pf(600,400,125)的飞行任务。无人飞行器在飞行过程中利用自身携带的传感器装置感知到周围环境中的威胁障碍物，无人飞行器需要对威胁障碍物进行避碰。其中，单个威胁障碍物的避碰为无人飞行器避障的最简单情况，表1给出了无人飞行器对单个威胁障碍物避碰的初始化条件。
表1单个威胁障碍物避碰的初始化条件
根据表1中求解的威胁障碍物空间速度障碍球冠参数，可将威胁障碍物的空间速度障碍球冠进行保角映射，相应的最优无人飞行器避碰速度矢量方向角为Qavo(28.8,11.9)，避碰点为Pavo(216,180,41.7)。应用于PH曲线路线规划的仿真结果如图7所示，仿真结果表明提出的三维空间无人飞行器自主避障算法能够实现对单威胁障碍物的避碰。
实施例2：多个威胁障碍物的避碰
当无人飞行器同时感知到多个威胁障碍物时，表2给出了相应的无人飞行器和威胁障碍物的初始化条件以及求解的威胁障碍物空间速度障碍球冠参数。以表2中的两个威胁障碍物避碰为例，其它多威胁障碍物避碰可类似进行避碰分析。
表2多威胁障碍物避碰的初始化条件
根据表2中求解的威胁障碍物空间速度障碍球冠参数，可对威胁障碍物的空间速度障碍球冠进行保角映射，相应的最优避碰速度矢量方向角为Qavo(30.7,15.4)，避碰点为Pavo(231,202,75.1)。应用于PH曲线路线规划的仿真结果如图8所示，仿真结果表明提出的三维空间无人飞行器自主避障算法能够实现对多威胁障碍物的避碰。
仿真结果表明，本发明提出的基于保角映射分析的无人飞行器空间避障方法对提升无人飞行器在复杂动态环境中执行艰巨任务的安全性和生存能力具有重要意义。
此外，应当理解，以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
- 还没有人留言评论。精彩留言会获得点赞！