一种无人飞行器空间避障方法及相关路径在线规划方法技术

本发明专利技术提出了一种无人飞行器空间避障方法及相关路径在线规划方法，该方法是一种基于三维速度障碍锥的动态空间直接避障方法，能够给出避障的插入点及避障方向，并与航迹规划方法相结合，可以实现无人飞行器在线航迹的重新规划。避障方法主要包括以下步骤：（1）构建三维速度障碍锥，（2）运用三维速度障碍锥进行避碰判断，（3）求解避障方向，（4）求解避障点。相关路径在线规划方法是以前述避障方法求解的避障方向和避障点来作为曲线路径在线规划的初始条件进行路径在线规划的方法。

【技术实现步骤摘要】

本专利技术属于无人飞行器空间避障飞行的控制领域，尤其是涉及一种基于三维速度障碍锥和空间速度障碍球冠的无人飞行器空间避障方法以及相关的路径在线规划方法。
技术介绍
当前无人飞行器空间归避障碍的飞行有多种控制方法。主要是利用探头探测周围信息，信息包括无人飞行器与障碍物之间的相对距离、方位、视线角、视线角速度、相对速度及加速度等。控制规避障碍物(以下简称“避障”)的方式主要有两类：一类是在航路规划时，将航路上存在的障碍物以约束条件的形式加在无人飞行器飞行的性能指标中。例如：采用A*搜索算法、快速随机搜索树、遗传算法等，在进行航路的规划过程中通过数学运算，解算出不符合避障约束条件的解，并将其剔除；第二类是设计避障导引函数，如人工势场法或改进的导航函数法、导引避障法及基于跟踪误差的李雅普诺夫控制律设计法等，将避障的过程结合在规划算法之中。第一类方式主要用于对静态障碍如高山、高层建筑物等的归避；然而对于动态障碍物诸如动态的飞行物时，由于发现障碍物后能够反应的时间极短，而且飞行器一般具有机动快速的特点，这样采用第二类方式才能够更好地解决这类问题。上述第二类方式中的人工势场法，其实现避障的原则是：首先定义一个势场力函数，在该势场中势场力对障碍物产生排斥力，对无人飞行器产生吸引力，并将此原则转换成路径规划的费用函数，引导无人飞行器使用最小费用，沿安全路径到达目的地。但是，经过反复试验发现：该势场力函数在对静态障碍物进行避障时，控制的实时性比较好；但是当障碍物为运动障碍，将障碍物和无人飞行器的速度加入到势场函数构造中时，侧避障效果不佳，往往在计算运动体路径时产生无效路径；而且，由于运动障碍物的作用，无人飞行器即使根据实际飞行路径来看与障碍物二者之间并不会碰撞，但根据势场函数的计算,仍然会产生势场力,使无人飞行器在飞行途中会偏离原来的运动轨迹；另外，有时还易陷入局部极小，使无人飞行器的飞行路径混乱。Hyo-SangShin等基于微分几何学原理，对无人飞行器飞行过程中的控制方法，应用避障的速度圆锥法进行碰撞检测，采用导引律的思想进行碰撞冲突消解，给出了无人飞行器航向角及速度的变化规律。但是，由于微分几何学中给出的是平面的几何关系，所以解出的控制飞行规律的方程是非连续的导引控制规律，并且引入了符号函数，其规划路径易产生颤振。F.Belkhouche[16]针对三维静态避障，采用了平面降维思想，利用避障圆锥法实现了避障，并用扩展比例导引律进行避障导引，在导引律设计参数中考虑了飞行器的飞行速度、角速度等约束限制，可产生非直线导引航迹。但是,由于采用的是平面避障思想,所以避障中产生的非直线导引航迹并不是三维空间的非直线导引的最短距离航迹。综上所述：目前空间避障方法还是基于平面降维避障的思想，而不是三维空间的直接避障，这样就会导致在避障平面上飞行角度改变较大、飞行航迹偏离原航迹较远等问题。
技术实现思路
针对上述提出的问题，有必要提出一种新的空间直接避障法，该方法可以较好的实现三维空间直接避障的问题。本专利技术以此为目的提出了一种三维速度障碍锥的动态空间直接避障方法，该方法能够给出避障的插入点及避障方向；并与航迹规划方法相结合，可以实现无人飞行器在线航迹的重新规划。本专利技术提出的避障方法具体包括以下步骤：步骤S1，构建三维速度障碍锥；根据无人飞行器在三维空间中的当前时刻位置坐标Pu＝(xu,yu,zu)和探测到的障碍物O的位置坐标Po＝(xo,yo,zo)，以Pu作为三维速度障碍锥的顶点，并将障碍物点膨化为以Po为球心、半径为R的障碍球PO，然后过Pu点作障碍球的切线，则所有以Pu为顶点障碍球的切线形成的锥面为三维速度障碍锥；步骤S2，运用构建三维速度障碍锥进行避碰判断；根据无人飞行器和障碍物的速度矢量大小和方向，求解无人飞行器相对于障碍物O的相对速度矢量vuo的大小和方向；然后求解相对速度矢量vuo与无人飞行器和障碍物的位置矢量之间的夹角大小α；判断障碍锥的半顶角α0与α的关系，若α<α0，需要对障碍物进行避碰，则继续进行步骤S3；若α>α0，不需要对障碍物进行避碰，则退出；步骤S3，求解避障方向；步骤S4，求解避障点。优选的，步骤S3中通过建立空间速度障碍球冠求解避障方向，具体包括以下步骤：步骤S31，建立相对碰撞区RCC和绝对碰撞区ACC；根据建立相对碰撞区；其中luo为vuo方向上的射线；将RCC沿障碍物O速度矢量vo平移||vo||，得到绝对碰撞区ACC，即ACC＝RCC⊕vo，其中⊕表示闵科夫斯基矢量和；步骤S32，建立空间速度障碍球冠；做以无人飞行器位置点Pu为球心、||vu||为半径的速度球Su，然后取速度球Su与ACC相交且位于ACC内部的球面为空间速度障碍球冠Go＝(r,ψo,θo)，其中r为球心Pu和球冠中心M之间所形成向量的长度，ψo和θo为球心Pu和球冠中心M之间所形成向量的方向角，记此球面为球冠大圆；步骤S33，根据空间速度矢量几何关系，解出球冠的参数；步骤S34，确定避障方向；避障方向为速度矢量vu根据避障后无人飞行器的飞行方向需要在空间以某一旋转角旋转至对应的期望速度矢量的方向。优选的，步骤S33具体为：步骤S331，根据三维空间向量vuo和作平面PuPoT，则平面PuPoT与三维速度障碍锥交线为圆锥体的母线l3、l′3，母线l3、l′3与障碍球的切点为Q、Q′；将相对速度矢量vuo旋转至母线l3方向上，得到期望相对速度矢量的方向，对应的旋转角度为最小角度δ，母线l′3为无人飞行器避障过程中速度矢量偏转角度最大位置；步骤S332，确定母线l3上的一个单位向量q，从而可以确定相对速度矢量vuo旋转到母线l3方向上转过的偏航角Δψuo和俯仰角Δθuo；然后由单位向量q求得期望相对速度矢量的大小及无人飞行器期望速度矢量由此可确定无人飞行器在完成对障碍物的避障任务所需转过的最小角度及最大角度；步骤S333，将无人飞行器对障碍物O进行避障的速度矢量抽象到三维空间中得到矢量关系，其中B、C点为两个临界边l3和l′3上的临界点。M为线段BC的二分点，根据矢量三角形的加法原则可得出及矢量A点即为Pu点，由此可将障碍物O的球冠Go表示为优选的，步骤S34具体为：在进行避障时，无人飞行器选择临界曲线上的点进行避障，对于单个障碍，三维避障锥为临界曲线；设在球冠Go1临界曲线上任意一点K的位置坐标为(xk,yk,zk)，根据几何位置关系和确定任意点K的三个坐标分量xk，yk，z本文档来自技高网...

【技术保护点】
一种无人飞行器空间避障方法，其特征在于，包括以下步骤：步骤S1，构建三维速度障碍锥；根据无人飞行器在三维空间中的当前时刻位置坐标Pu＝(xu,yu,zu)和探测到的障碍物O的位置坐标Po＝(xo,yo,zo)，以Pu作为三维速度障碍锥的顶点，并将障碍物点膨化为以Po为球心、半径为R的障碍球PO，然后过Pu点作障碍球的切线，则所有以Pu为顶点障碍球的切线形成的锥面为三维速度障碍锥；步骤S2，运用构建的三维速度障碍锥进行避碰判断；根据无人飞行器和障碍物的速度矢量大小和方向，求解无人飞行器相对于障碍物O的相对速度矢量vuo的大小和方向；然后求解相对速度矢量vuo与无人飞行器和障碍物的位置矢量之间的夹角大小α；判断障碍锥的半顶角α0与α的关系，若α<α0，需要对障碍物进行避碰，则继续进行步骤S3；若α>α0，不需要对障碍物进行避碰，则退出；步骤S3，求解避障方向；步骤S4，求解避障点。

【技术特征摘要】
1.一种无人飞行器空间避障方法，其特征在于，包括以下步骤：
步骤S1，构建三维速度障碍锥；
根据无人飞行器在三维空间中的当前时刻位置坐标Pu＝(xu,yu,zu)和探测到的障碍物O
的位置坐标Po＝(xo,yo,zo)，以Pu作为三维速度障碍锥的顶点，并将障碍物点膨化为以Po为
球心、半径为R的障碍球PO，然后过Pu点作障碍球的切线，则所有以Pu为顶点障碍球的切线形
成的锥面为三维速度障碍锥；
步骤S2，运用构建的三维速度障碍锥进行避碰判断；
根据无人飞行器和障碍物的速度矢量大小和方向，求解无人飞行器相对于障碍物O的
相对速度矢量vuo的大小和方向；然后求解相对速度矢量vuo与无人飞行器和障碍物的位置
矢量之间的夹角大小α；判断障碍锥的半顶角α0与α的关系，若α<α0，需要对障碍物进行
避碰，则继续进行步骤S3；若α>α0，不需要对障碍物进行避碰，则退出；
步骤S3，求解避障方向；
步骤S4，求解避障点。
2.如权利要求1所述的一种无人飞行器空间避障方法，其特征在于，步骤S3中通过建立
空间速度障碍球冠求解避障方向，具体包括以下步骤：
步骤S31，建立相对碰撞区RCC和绝对碰撞区ACC；
根据建立相对碰撞区；其中luo为vuo方向上的射线；
将RCC沿障碍物O速度矢量vo平移||vo||，得到绝对碰撞区ACC，即其
中表示闵科夫斯基矢量和；
步骤S32，建立空间速度障碍球冠；
做以无人飞行器位置点Pu为球心、||vu||为半径的速度球Su，然后取速度球Su与ACC相交
且位于ACC内部的球面为空间速度障碍球冠Go＝(r,ψo,θo)，其中r为球心Pu和球冠中心M之
间所形成向量的长度，ψo和θo为球心Pu和球冠中心M之间所形成向量的方向角；
步骤S33，根据空间速度矢量几何关系，解出球冠的参数；
步骤S34，确定避障方向；
避障方向为速度矢量vu根据避障后无人飞行器的飞行方向需要在空间以某一旋转角旋
转至对应的期望速度矢量的方向。
3.如权利要求2所述的一种无人飞行器空间避障方法，其特征在于，步骤S33具体为：
步骤S331，根据三维空间向量vuo和作平面PuPoT，则平面PuPoT与三维速度障碍锥交
线为圆锥体的母线l3、l′3，母线l3、l′3与障碍球的切点为Q、Q′；将相对速度矢量vuo旋转至母
线l3方向上，得到期望相对速度矢量的方向，对应的旋转角度为最小角度δ，母线l′3为无
人飞行器避障过程中速度矢量偏转角度最大位置；
步骤S332，确定母线l3上的一个单位向量q，从而可以确定相对速度矢量vuo旋转到母线
l3方向上转过的偏航角Δψuo和俯仰角Δθuo；然后由单位向量q求得期望相对速度矢量的
大小及无人飞行器期望速度矢量由此可确定无人飞行器在完成对障碍物的避障任务所
需转过的最小角度及最大角度；
步骤S333，将...

【专利技术属性】
技术研发人员：张毅，杨秀霞，周硙硙，
申请(专利权)人：中国人民解放军海军航空工程学院，
类型：发明
国别省市：山东;37