一种无人飞行器空间避障方法及相关路径在线规划方法
【技术领域】
[0001] 本发明属于无人飞行器空间避障飞行的控制领域，尤其是涉及一种基于三维速度 障碍锥和空间速度障碍球冠的无人飞行器空间避障方法以及相关的路径在线规划方法。
【背景技术】
[0002] 当前无人飞行器空间归避障碍的飞行有多种控制方法。主要是利用探头探测周围 信息，信息包括无人飞行器与障碍物之间的相对距离、方位、视线角、视线角速度、相对速度 及加速度等。控制规避障碍物（以下简称"避障"）的方式主要有两类:一类是在航路规划时， 将航路上存在的障碍物以约束条件的形式加在无人飞行器飞行的性能指标中。例如:采用 A1 叟索算法、快速随机搜索树、遗传算法等，在进行航路的规划过程中通过数学运算，解算 出不符合避障约束条件的解，并将其剔除;第二类是设计避障导引函数，如人工势场法或改 进的导航函数法、导引避障法及基于跟踪误差的李雅普诺夫控制律设计法等，将避障的过 程结合在规划算法之中。第一类方式主要用于对静态障碍如高山、高层建筑物等的归避;然 而对于动态障碍物诸如动态的飞行物时，由于发现障碍物后能够反应的时间极短，而且飞 行器一般具有机动快速的特点，这样采用第二类方式才能够更好地解决这类问题。
[0003] 上述第二类方式中的人工势场法，其实现避障的原则是:首先定义一个势场力函 数，在该势场中势场力对障碍物产生排斥力，对无人飞行器产生吸引力，并将此原则转换成 路径规划的费用函数，引导无人飞行器使用最小费用，沿安全路径到达目的地。但是，经过 反复试验发现:该势场力函数在对静态障碍物进行避障时，控制的实时性比较好;但是当障 碍物为运动障碍，将障碍物和无人飞行器的速度加入到势场函数构造中时，侧避障效果不 佳，往往在计算运动体路径时产生无效路径；而且，由于运动障碍物的作用，无人飞行器即 使根据实际飞行路径来看与障碍物二者之间并不会碰撞，但根据势场函数的计算，仍然会 产生势场力，使无人飞行器在飞行途中会偏离原来的运动轨迹;另外，有时还易陷入局部极 小，使无人飞行器的飞行路径混乱。
[0004] Hyo-Sang Shin等基于微分几何学原理，对无人飞行器飞行过程中的控制方法，应 用避障的速度圆锥法进行碰撞检测，采用导引律的思想进行碰撞冲突消解，给出了无人飞 行器航向角及速度的变化规律。但是，由于微分几何学中给出的是平面的几何关系，所以解 出的控制飞行规律的方程是非连续的导引控制规律，并且引入了符号函数，其规划路径易 产生颤振。
[0005] F. Belkhouche [ 16]针对三维静态避障，采用了平面降维思想，利用避障圆锥法实 现了避障，并用扩展比例导引律进行避障导引，在导引律设计参数中考虑了飞行器的飞行 速度、角速度等约束限制，可产生非直线导引航迹。但是，由于采用的是平面避障思想，所以 避障中产生的非直线导引航迹并不是三维空间的非直线导引的最短距离航迹。
[0006] 综上所述：目前空间避障方法还是基于平面降维避障的思想，而不是三维空间的 直接避障，这样就会导致在避障平面上飞行角度改变较大、飞行航迹偏离原航迹较远等问 题。
【发明内容】
[0007] 针对上述提出的问题，有必要提出一种新的空间直接避障法，该方法可以较好的 实现三维空间直接避障的问题。本发明以此为目的提出了一种三维速度障碍锥的动态空间 直接避障方法，该方法能够给出避障的插入点及避障方向；并与航迹规划方法相结合，可以 实现无人飞行器在线航迹的重新规划。
[0008] 本发明提出的避障方法具体包括以下步骤：
[0009] 步骤S1，构建三维速度障碍锥；
[0010] 根据无人飞行器在三维空间中的当前时刻位置坐标?11=(^，3^，211)和探测到的障 碍物〇的位置坐标Po= (1。，7。，2。），以Pu作为三维速度障碍锥的顶点，并将障碍物点膨化为以 P。为球心、半径为R的障碍球P0，然后过pu点作障碍球的切线，则所有以pu为顶点障碍球的切 线形成的锥面为三维速度障碍锥；
[0011] 步骤S2，运用构建三维速度障碍锥进行避碰判断；
[0012] 根据无人飞行器和障碍物的速度矢量大小和方向，求解无人飞行器相对于障碍物 〇的相对速度矢量％。的大小和方向；然后求解相对速度矢量％。与无人飞行器和障碍物的位 置矢量M之间的夹角大小a;判断障碍锥的半顶角a〇与a的关系，若cKao,需要对障碍物进 行避碰，则继续进行步骤S3;若aXto，不需要对障碍物进行避碰，则退出；
[0013] 步骤S3,求解避障方向；
[0014]步骤S4,求解避障点。
[0015] 优选的，步骤S3中通过建立空间速度障碍球冠求解避障方向，具体包括以下步骤：
[0016] 步骤S31，建立相对碰撞区RCC和绝对碰撞区ACC;
[0017]根据= {igi门尸0笑0；}建立相对碰撞区；其中luc^Vu。方向上的射线；
[0018] 将RCC沿障碍物0速度矢量V。平移| | V。| |，得到绝对碰撞区ACC，即ACC = RCC ? v。，其 中?表示闵科夫斯基矢量和；
[0019] 步骤S32,建立空间速度障碍球冠；
[0020] 做以无人飞行器位置点Pu为球心、| |Vu| |为半径的速度球Su，然后取速度球5^>与 ACC相交且位于ACC内部的球面为空间速度障碍球冠 G。=(r>。，0。），其中r为球心Pu和球冠 中心M之间所形成向量的长度，札和9。为球心Pu和球冠中心M之间所形成向量_的方 向角，记此球面为球冠大圆；
[0021] 步骤S33,根据空间速度矢量几何关系，解出球冠的参数；
[0022]步骤S34,确定避障方向；
[0023]避障方向为速度矢量Vu根据避障后无人飞行器的飞行方向需要在空间以某一旋 转角旋转至对应的期望速度矢量元的方向。
[0024]优选的，步骤S33具体为：
[0025] 步骤S331，根据三维空间向量vu。和:^作平面PJVT，则平面PUP QT与三维速度障碍 锥交线为圆锥体的母线uas，母线has与障碍球的切点为q、q、将相对速度矢量Vu。旋转 至母线13方向上，得到期望相对速度矢量I。的方向，对应的旋转角度为最小角度S，母线V 3 为无人飞行器避障过程中速度矢量偏转角度最大位置；
[0026] 步骤S332,确定母线h上的一个单位向量q，从而可以确定相对速度矢量vu。旋转到 母线13方向上转过的偏航角A也。和俯仰角A 0U。;然后由单位向量q求得期望相对速度矢量 化。的大小及无人飞行器期望速度矢量<，由此可确定无人飞行器在完成对障碍物的避障 任务所需转过的最小角度及最大角度；
[0027]步骤S333,将无人飞行器对障碍物0进行避障的速度矢量抽象到三维空间中得到 矢量关系，其中B、C点为两个临界边13和1S上的临界点。M为线段BC的二分点，根据矢量三角 形的加法原则可得出^及矢量石7，. A点即为Pu点，由此可将障碍物0的球冠 G。表示为 [0028]优选的，步骤S34具体为：
[0029]在进行避障时，无人飞行器选择临界曲线上的点进行避障，对于单个障碍，三维避 障锥为临界曲线;设在球冠 Gca临界曲线上任意一点K的位置坐标为(处，7^)，根据几何位 置关系|@| =心和^丄^^，确定任意点K的三个坐标分量xk，yk，zk之间的关系表达式， 由此计算出矢量$在三维空间中的两个方向角，即无人飞行器期望速度矢量方向角匕和 兔，以及相应的速度矢量vu旋转角度A iK和A 0U。
[0030]优选的，步骤S4具体为：
[0031] 步骤S41，根据无人飞行器的避障速度球冠确定避碰的主体障碍物；
[0032]当无人飞行器与多运动障碍有碰撞危险时，计算无人飞行器与各障碍物在速度空 间的碰撞时间，将时间最短的障碍物作为主体障碍物，然后依据单个障碍物避障插入点坐 标的计算方法确定无人飞行器对主体障碍物避障的插入点，作为多障碍物避障的插入点； [0033]步骤S42,在主体障碍物的速度矢量相反方向确定避碰点；
[0034] 采用障碍物经过调整时间t后的空间位置坐标口^^^^^^^彡来确定避碰点一等 避障的插入点确定在障碍物速度矢量V。的相反方向上，同时将障碍物的膨化半径乘上一个 比例系数k，然后根据障碍物位置坐标 ？/。（1/。，7/。，2 /。），得出避障插入点位置坐标〇11^ (Xins , y ins , Zins ) j
[0035] xinsZx'o+R'cosQ。cosit〇
[0036] yinf^y'o+R'cosQ。sini^。
[0037] zins = z/ 〇+R7 sin9〇
[0038] 其中f =kR，k为设定的常数且1。
[0039]优选的，当障碍物为多个时，
[0040] 步骤S2要对多个障碍物进行避障判断，如果无人飞行器的速度矢量^与障碍物⑴ 的速度矢量v〇i之间的相对速度矢量Vuoi均在障碍物〇i形成的障碍锥内，则需要同时对多个 障碍物进行避碰，否则不需要同时对多个障碍物进行避碰。
[0041] 对于多个障碍物同时避障时，则各球冠大圆的并集为避障临界曲线。
[0042]使用前述的无人飞行器空间