CN104914877A - 一种轻型无人机自主感知与规避方法 - Google Patents

Abstract

本发明公开了一种轻型无人机自主感知与规避方法，属于航路规划领域，特别是涉及一种轻型无人机超视距飞行规避空中目标方法。首先通过前视单目定焦摄像机采集视频图像，然后计算像在像直角坐标系内的长度、高度和位置；在摄像机坐标系内推导目标位置和运动趋势；在空间直角坐标系内沿方位角差值最大两个目标所在射线夹角的角平分线和机动前航向确定的平面机动，确定目标长度；在摄像机坐标系内计算具体目标位置和运动趋势；最后如危险接近在空间直角坐标系内沿方位角差值最大两个目标所在射线夹角的角平分线和机动前航向确定的平面规避，规避后目标与光心最小距离为最小安全距离。本发明的优点在于实现自主感知，一次规避所有目标，机动距离短。

Description

一种轻型无人机自主感知与规避方法

技术领域

本发明属于航路规划领域，特别是涉及一种轻型无人机在隔离空域超视距飞行自主感知与规避方法。

背景技术

根据《民用无人驾驶航空器系统驾驶员管理暂行规定》，在融合空域中运行的大型无人机必须安装无人机感知与规避系统，使大型无人机具备发现与避让的能力，轻型无人机没有强制规定安装无人机感知与规避系统。轻型无人机，是指空机质量大于7 千克，但小于等于116 千克的无人机，且全马力平飞中，校正空速小于100 千米/小时，升限小于3000 米。轻型无人机使用广泛，需要具备感知与规避能力，在隔离空域作业飞行时与无人机、遥控飞艇可能发生危险接近或相撞。

轻型无人机成本低，采用传统的空中交通警告和防撞系统、广播式自动相关监视等合作型感知规避方法并不适用。采用光学成像系统、雷达探测系统等非合作型自主感知规避方法成为可能，采用雷达探测系统的方法，因轻型无人机成本和载荷的限制，很难实施。采用光学成像系统的感知与规避方法虽受到气象条件的限制，但成本低、便于实施。目前已有基于双目或多目摄像设备的视觉检测无人机防撞系统，通过数据链实时警告地面控制站操作手，通过手动操作防止碰撞，由于无人机与地面站之间的数据传输存在一定的滞后性，即使在发现将要发生碰撞的情况下，无人机操控员指令无人机规避碰撞的能力也会受到一定的限制，没有实现自主感知与规避，其机载设备体积大、重量重、成本高、数据处理复杂，也很难直接应用于轻型无人机。

轻型无人机在隔离空域超视距作业飞行时，空中相撞主要危险来至飞行前向，考虑到轻型无人机载荷、成本等约束条件，实现一种轻型无人机前向自主感知与规避方法。

发明内容

针对上述存在的技术不足，本发明所要解决的技术问题是提供一种轻型无人机自主感知与规避方法。因为空中相撞主要危险来至飞行前向，通过前视单目定焦摄像机获取视频图像，基于视觉感知技术，提出一种自主感知与规避方法。能够在轻型无人机超视距飞行时，解决轻型无人机飞行前向与无人机、飞艇危险接近，发生空中相撞的问题，确保轻型无人机与空中目标（以下简称目标）保持一定的安全飞行间隔。

为了实现上述目的，本发明所采用的技术方案为一种基于前视单目定焦摄像机的轻型无人机自主感知与规避方法。首先通过轻型无人机前视单目定焦摄像机采集视频图像，然后利用现有技术通过视频处理识别发现目标，确定目标数量；分别计算目标的像在像直角坐标系内的长度、高度和位置关系；在空间直角坐标系通过小范围机动，确定目标长度；在摄像设备坐标系内确定目标与摄像机相对位置关系、目标运动趋势，最后根据摄像设备坐标系内目标位置和运动趋势判断轻型无人机是否危险接近目标，如危险接近在空间直角坐标系内给出规避方法。具体步骤如下。

步骤一、通过轻型无人机前视单目定焦摄像机采集视频图像。摄像机焦距为f，水平视场角为α，垂直视场角为β。

将摄像机做如下标定，以光心o₁为原点构建摄像机坐标系o₁x₁y₁z₁，光轴为z₁轴，位于轻型无人机的机体左右对称平面内或平行于左右对称平面，轻型无人机定速巡航时z₁轴与地面平行，光轴在机体左右对称平面内的投影与机体轴线夹角为θ₁，z₁轴指向机体前方；x₁轴与z₁轴垂直，指向机体右方；y₁轴垂直x₁轴和z₁轴，指向机体上方。光轴延长线与像平面交点为o；以o点为原点，构建像平面直角坐标系oxy， x轴平行与x₁轴，指向相同；y轴平行于y₁轴，指向相同；oxy平面与o₁x₁y₁平面平行，两平面间距离为f。

步骤二、利用现有技术通过视频处理识别发现目标，确定目标数量，并分别计算目标的像在像直角坐标系内的轮廓，计算出像的长度、高度和像的位置关系，在摄像机坐标系内计算目标与轻型无人机相对位置关系和目标运动趋势。

1、利用现有技术通过视频处理识别发现目标，确定目标数量a，将目标逐按自然序列逐一编批，批号依次为1、2、…、n，目标集合为{T1，T2，，，，Ta}。并分别计算目标的像在像直角坐标系内的轮廓，计算轮廓上像素点坐标(x，y)，找到x值最大的点A，A点坐标为（x_A，y_A）和最小的点B，B点坐标为（x_B，y_B），找到y值最大的点C，C点坐标为（x_C，y_C）和最小的点D，D点坐标为（x_D，y_D）。像长l= x_A-x_B，像高m= y_C-y_D。

像到y轴的距离k定义为：当x_A>0且x_B>0，k= x_B。当x_A<0且x_B<0，k= -x_A。当x_A>0且x_B<0，k=0。当x_A=0或x_B=0，k=0。像到x轴的距离h定义为：当y_C>0且y_D>0，k= y_D。当y_C<0且y_D<0，k= -y_C。当y_C>0且y_D<0，h=0。当y_C=0或y_D=0，h=0。在摄像机坐标系内将目标的像位置描述为（k，h，f）。

2、在摄像机坐标系内描述目标位置。定义目标所在法平面（以下简称法平面）为与目标相切、垂直于光轴的平面，且光心o₁到平面的距离d最小的平面，d反映了目标与摄像机水平纵向间隔，光轴与法平面的交点为o₂，以o₂为原点构建法平面直角坐标系o₂x₂y₂，法平面直角坐标系x₂轴平行与摄像机坐标系x₁轴，指向相同；y₂轴平行于y₁轴，指向相同。在摄像机坐标系内Ao₁延长线与法平面交点为E，E点坐标为(x_1E，y_1E，d)，Bo₁延长线与法平面交点为F，F点坐标为(x_1F，y_1F，d)，Co₁延长线与法平面交点为G，G点坐标为(x_1G，y_1G，d)，Do₁延长线与法平面交点为I，I点坐标为(x_1I，y_1I，d)。

目标长度w定义为：w=x_1F-x_1E，目标高度g定义为：g= y_1I-y_1G。在法平面直角坐标系目标到y₂轴的距离K定义为：当x_1F>0且x_1E>0，k= x_1E。当x_1F<0且x_1E<0，k= -x_1F。当x_1F>0且x_1E<0，K=0。当x_1F=0或x_1E=0，K=0。在法平面直角坐标系目标到x₂轴的距离H定义为：当y_1I>0且y_1G>0，H= y_1G。当y_1I<0且y_1G<0，H= -y_1I。当y_1I>0且y_1G<0，H=0。当y_1I=0或y_1G=0，H=0。在摄像机坐标系内将目标位置描述为（K，H，d）。

3、在摄像机坐标系内计算目标与轻型无人机相对位置关系和目标运动趋势。为简化计算，以目标与光心o₁位置关系，代替目标与轻型无人机位置关系。目标集合为{T1，T2，，，，Tn},                                                。假设K_Tn不等于零，H_Tn不等于零，D_Tn不等于零。将目标和像在o₁x₁z₁平面内投影有：

；

；

；

将目标和像在o₁y₁z₁平面投影有：

；

。

3.1 在摄像机坐标系内推导目标位置关系和运动趋势。求解目标相对光心运动趋势，要对目标视频连续采样，用上述方法表示目标在摄像机坐标系内位置前提为目标w不变，要求目标在运动过程中不能超出摄像机水平视场或目标从盲区进入视场要完全进入视场，不然就无法准确反映出目标运动趋势。目标在视场内运动时由于目标运动姿态改变带来的w变化，由于两次采样时间间隔很短，w变化忽略不计。

当目标与摄像机水平视场角两边同时相切时，即满视场时有：

；

当目标与摄像机垂直视场角两边同时相切时，即满视场时有：

；

当目标单边与水平视场角相切时有：

如，

；

如，

；

当目标单边与垂直视场角相切时有：

如，

；

如，

；

对目标Tn()，在t1时刻目标到y₂轴距离为K_Tnt1，到x₂轴距离为H_Tnt1，到o₁距离为d_Tnt1，t2时刻目标到y₂轴距离为K_Tnt2，到x₂轴距离为H_Tnt2，到o₁距离为d_Tnt2。，为一小量，时间内目标长度w_Tn没有变化。

如d_Tnt2小于d_Tnt1，目标与摄像机水平纵向间隔在减小，目标到达o₁x₁y₁平面的时间t_Tn为：

()

设的目标数量为b。目标到达o₁x₁y₁平面时，目标的为：

，()

，

目标到达o₁x₁y₁平面时，目标的为：

，()

，()

目标到达o₁x₁y₁平面时，与o₁x₁y₁平面交点为，定义射线与x₁轴的夹角为目标在o₁x₁y₁平面的方位角，的集合为：

()

定义目标到达o₁x₁y₁平面距原点o₁的距离：

()

对目标视频持续采样，跟踪运动趋势，如跟踪过程中出视场，则认为目标Tn()保持运动趋势，不再采样计算。

对目标Tn()，如d_Tnt2大于d_Tnt1，目标中心与摄像设备水平纵向间隔变大；

对目标Tn()，如d_Tnt2等于d_Tnt1，目标中心与摄像设备水平纵向间隔不变；

对目标Tn()，如d_Tnt2小于d_Tnt1，目标中心与摄像设备水平纵向间隔变小。

3.2 确定目标长度和具体位置关系运动趋势。假设b个目标同时到达o₁x₁y₁平面，在o₁x₁y₁平面依次计算目标集合{T1，T2，，，，Tb，T1}中一个目标方位角的边即射线逆时针旋转到下一个目标的方位角边即射线旋转的角度k₂ ^j。

(，)

将从大到小排序，选取最大值对应的两个目标方位角和，对应的。设，即目标集合{T1，T2，，，，Tb，T1}方位角差值最大两目标所在射线和夹角角平分线的方位角。

轻型无人机t2时刻方向角为，俯仰角为，以轻型无人机t2时刻位置为原点o₃，建立空间直角坐标系o₃x₃y₃z₃, x₃轴与x₁轴平行，y₃轴与y₁轴平行，z₃轴与z₁轴重合。在o₃x₃y₃平面做射线o₃A，o₃A与x₃轴夹角为γ₁。轻型无人机在o₃Az₃平面向o₃A方向进行机动，z₃轴方向速度保持不变，机动距离在o₃A投影为R₀，R₀为常量，根据轻型无人机约束条件确定。机动时长为t₀，机动后时刻为t3。

对目标Tn()，机动前在摄像设备坐标系目标位置为（K_Tnt2, H_Tnt2，d_Tnt2），机动后目标位置为（K_Tnt3, H_Tnt3，d_Tnt3），机动前目标像位置为（k_Tnt2, h_Tnt2，f），像长为l_Tnt2，机动后目标像位置为（k_Tnt3, h_Tnt3，f），像长为l_Tnt3。

如果或或，则有：

如果，则向y₃轴正向机动R₀，机动前有：

；

；

同理，机动后有：

假设目标在t₀时长内目标在y₁轴方向匀速运动，在摄像机坐标系则有：

 ；

；

如果，则向y₃轴负向机动R₀，则有：

。

如果或，则向x₃轴正向机动R₀。机动前在摄像机坐标系有：

；

；

同理，机动后有：

假设目标在t₀时长内在x₁轴方向匀速运动，在摄像机坐标系则有：

 ；；

；

如果，则向x₃轴负向机动R₀，则有：

。

继续对目标视频采样，t4时刻，计算目标到达o₁x₁y₁平面的时间t_Tn为：

 ()

如，设的目标数量为c。计算目标到达o₁x₁y₁平面时，目标的为：

 ()

目标到达o₁x₁y₁平面时，目标的为：

 ()

目标到达o₁x₁y₁平面时，目标在o₁x₁y₁平面的方位角的集合：

()

目标到达o₁x₁y₁平面距原点o₁的距离：

 ()。

步骤三：判断目标是否危险接近微型无人机，如危险接近给出机动规避方法。

1、建立空间直角坐标系。轻型无人机t4时刻方向角为，俯仰角为，以轻型无人机t4时刻位置为原点o₄，建立空间直角坐标系o₄x₄y₄z₄, x₄轴与x₁轴平行，y₄轴与y₁轴平行，z₄轴与z₁轴重合。

2、设定规避规则。规避基本原则：目标到达o₁x₁y₁平面时与轻型无人机距离，轻型无人机保持运动状态；如目标到达o₁x₁y₁平面时，则进行规避，t4时刻开始规避，完成规避后时刻为t5，规避后目标到达o₁x₁y₁平面时与光心的距离，R为定常距离值。R的确定要考虑飞行空域目标的数量和机型，机动过程中轻型无人机在空间直角坐标系内z₄轴方向速度分量保持不变，规避后恢复规避前方向角和俯仰角。

3.判断是否需要规避。目标到达o₁x₁y₁平面时与轻型无人机距离，轻型无人机保持运动状态；如目标到达o₁x₁y₁平面时，则进行规避。

4.机动规避方法。假设c个目标同时到达o₁x₁y₁平面，在o₁x₁y₁平面依次计算目标集合{T1，T2，，，，Tc，T1}中一个目标方位角的边即射线逆时针旋转到下一个目标的方位角边即射线旋转的角度k₂ ^j。

(，)

将从大到小排序，选取最大值对应的两个目标方位角和，对应的。设，即目标集合{T1，T2，，，，Tc，T1}方位角差值最大两目标所在射线和夹角角平分线的方位角。

4.1目标为单目标。在空间直角坐标系o₄x₄y₄平面内做射线o₄A, o₄A与x₄轴夹角为，轻型无人机在平面o₄Az₄向o₄A方向机动进行规避,规避距离在o₄A上投影距离为。

4.2如目标为多目标。

如，在空间直角坐标系o₄x₄y₄平面内做射线o₄A, o₄A与x₄轴夹角为，轻型无人机在平面o₄Az₄向o₄A方向机动进行规避；

如，则规避距离在o₄A上投影距离为；

如，则规避距离在o₄A上投影距离为。

如，在空间直角坐标系o₄x₄y₄平面内做射线o₄A, o₄A与x₄轴夹角为，轻型无人机在平面o₄Az₄向o₄A方向机动进行规避；

如，则规避距离在o₄A上投影距离为R₁：

从机动后的点向射线做垂线，在直角三角形内有：

；

如，则规避距离在o₄A上投影距离为R₂：

从机动后的点向射线做垂线，在直角三角形内有：

。

如，在空间直角坐标系o₄x₄y₄平面内做射线o₄A, o₄A与x₄轴夹角为，轻型无人机在平面o₄Az₄向o₄A方向机动进行规避；

如，则规避距离在o₄A上投影距离为R₃：

从机动后的点向射线延长线做垂线，在直角三角形内有：

；

如，则规避距离在o₄A上投影距离为R₄：

从机动后的点向射线延长线做垂线，在直角三角形内有：

。

本发明的优点在于通过单目定焦摄像机获得空中目标视频，感知方法简单。在空间直角坐标系通过小范围机动，确定目标长度、具体位置关系和运动趋势，通过视频处理实现自主感知。以目标到光心所在法平面时目标与光心的方位角和到光心的距离为基础，制定了规避方法：对单目标，在空间直角坐标系沿目标和空间直角坐标系原点连线的延长线与机动前航向确定的平面机动，规避后目标到达光心所在法平面时，目标与轻型无人机的距离为最小安全距离；对多目标规避，找到方位角差最大的两个目标，确定目标所在射线夹角的角平分线，在空间直角坐标系内采用沿着角平分线与机动前航向确定的平面机动规避。一次规避视场内所有目标，规避次数少；规避后目标到达光心所在法平面时，轻型无人机与最近的目标的距离为最小安全距离，机动距离短。

附图说明

图1 是本发明摄像设备坐标系示意图；

图2 是本发明目标像在像直角坐标系位置示意图；

图3 是本发明目标水平视场满视场示意图。

具体实施方式

首先通过轻型无人机前视单目定焦摄像机采集视频图像，然后利用现有技术通过视频处理识别发现目标，确定目标数量；分别计算目标的像在像直角坐标系内的长度、高度和位置关系；在空间直角坐标系通过小范围机动，确定目标长度；在摄像设备坐标系内确定目标与摄像机相对位置关系、目标运动趋势，最后根据摄像设备坐标系内目标位置和运动趋势判断轻型无人机是否危险接近目标，如危险接近在空间直角坐标系内给出规避方法。具体步骤如下。

步骤一、通过轻型无人机前视单目定焦摄像机采集视频图像。摄像机焦距为f，水平视场角为α，垂直视场角为β。如飞行空域目标较少，可选用焦距较大，水平视场角较小的镜头，可尽早发现目标；如飞行空域目标较多，可选用焦距较小，水平视场角较大的镜头，可增大感知目标数量，减小盲区。

如图1所示，将摄像机做如下标定，以光心o₁为原点构建摄像机坐标系o₁x₁y₁z₁，光轴为z₁轴，位于轻型无人机的机体左右对称平面内或平行于左右对称平面，轻型无人机定速巡航时z₁轴与地面平行，光轴在机体左右对称平面内的投影与机体轴线夹角为θ₁，z₁轴指向机体前方；x₁轴与z₁轴垂直，指向机体右方；y₁轴垂直x₁轴和z₁轴，指向机体上方。光轴延长线与像平面交点为o；以o点为原点，构建像平面直角坐标系oxy， x轴平行与x₁轴，指向相同；y轴平行于y₁轴，指向相同；oxy平面与o₁x₁y₁平面平行，两平面间距离为f。

步骤二、利用现有技术通过视频处理识别发现目标，确定目标数量，并分别计算目标的像在像直角坐标系内的轮廓，计算出像的长度、高度和像的位置关系，在摄像机坐标系内计算目标与轻型无人机相对位置关系和目标运动趋势。

1、利用现有技术通过视频处理识别发现目标，确定目标数量a，将目标逐按自然序列逐一编批，批号依次为1、2、…、n，目标集合为{T1，T2，，，，Ta}。并分别计算目标的像在像直角坐标系内的轮廓，计算轮廓上像素点坐标(x，y)，找到x值最大的点A，A点坐标为（x_A，y_A）和最小的点B，B点坐标为（x_B，y_B），找到y值最大的点C，C点坐标为（x_C，y_C）和最小的点D，D点坐标为（x_D，y_D）。像长l= x_A-x_B，像高m= y_C-y_D。

如图2所示，像到y轴的距离k定义为：当x_A>0且x_B>0，k= x_B。当x_A<0且x_B<0，k= -x_A。当x_A>0且x_B<0，k=0。当x_A=0或x_B=0，k=0。像到x轴的距离h定义为：当y_C>0且y_D>0，k= y_D。当y_C<0且y_D<0，k= -y_C。当y_C>0且y_D<0，h=0。当y_C=0或y_D=0，h=0。在摄像机坐标系内将目标的像位置描述为（k，h，f）。

2、在摄像机坐标系内描述目标位置。定义目标所在法平面（以下简称法平面）为与目标相切、垂直于光轴的平面，且光心o₁到平面的距离d最小的平面，d反映了目标与摄像机水平纵向间隔，光轴与法平面的交点为o₂，以o₂为原点构建法平面直角坐标系o₂x₂y₂，法平面直角坐标系x₂轴平行与摄像机坐标系x₁轴，指向相同；y₂轴平行于y₁轴，指向相同。在摄像机坐标系内Ao₁延长线与法平面交点为E，E点坐标为(x_1E，y_1E，d)，Bo₁延长线与法平面交点为F，F点坐标为(x_1F，y_1F，d)，Co₁延长线与法平面交点为G，G点坐标为(x_1G，y_1G，d)，Do₁延长线与法平面交点为I，I点坐标为(x_1I，y_1I，d)。

目标长度w定义为：w=x_1F-x_1E，目标高度g定义为：g= y_1I-y_1G。在法平面直角坐标系目标到y₂轴的距离K定义为：当x_1F>0且x_1E>0，k= x_1E。当x_1F<0且x_1E<0，k= -x_1F。当x_1F>0且x_1E<0，K=0。当x_1F=0或x_1E=0，K=0。在法平面直角坐标系目标到x₂轴的距离H定义为：当y_1I>0且y_1G>0，H= y_1G。当y_1I<0且y_1G<0，H= -y_1I。当y_1I>0且y_1G<0，H=0。当y_1I=0或y_1G=0，H=0。在摄像机坐标系内将目标位置描述为（K，H，d）。

3、在摄像机坐标系内计算目标与轻型无人机相对位置关系和目标运动趋势。为简化计算，以目标与光心o₁位置关系，代替目标与轻型无人机位置关系。

目标集合为{T1，T2，，，，Tn},。假设K_Tn不等于零，H_Tn不等于零，D_Tn不等于零。将目标和像在o₁x₁z₁平面内投影有：

；

；

；

将目标和像在o₁y₁z₁平面投影有：

；

。

3.1 在摄像机坐标系内推导目标位置关系和运动趋势。求解目标相对光心运动趋势，要对目标视频连续采样，用上述方法表示目标在摄像机坐标系内位置前提为目标w不变，要求目标在运动过程中不能超出摄像机水平视场或目标从盲区进入视场要完全进入视场，不然就无法准确反映出目标运动趋势。目标在视场内运动时由于目标运动姿态改变带来的w变化，由于两次采样时间间隔很短，w变化忽略不计。

如图3所示，当目标与摄像机水平视场角两边同时相切时，即满视场时有：

；

当目标与摄像机垂直视场角两边同时相切时，即满视场时有：

；

当目标单边与水平视场角相切时有：

如，

；

如，

；

当目标单边与垂直视场角相切时有：

如，

；

如，

；

对目标Tn()，在t1时刻目标到y₂轴距离为K_Tnt1，到x₂轴距离为H_Tnt1，到o₁距离为d_Tnt1，t2时刻目标到y₂轴距离为K_Tnt2，到x₂轴距离为H_Tnt2，到o₁距离为d_Tnt2。，为一小量，时间内目标长度w_Tn没有变化。

如d_Tnt2小于d_Tnt1，目标与摄像机水平纵向间隔在减小，目标到达o₁x₁y₁平面的时间t_Tn为：

()

设的目标数量为b。目标到达o₁x₁y₁平面时，目标的为：

，()

，

目标到达o₁x₁y₁平面时，目标的为：

，()

，()

目标到达o₁x₁y₁平面时，与o₁x₁y₁平面交点为，定义射线与x₁轴的夹角为目标在o₁x₁y₁平面的方位角，的集合为：

()

定义目标到达o₁x₁y₁平面距原点o₁的距离：

()

对目标视频持续采样，跟踪运动趋势，如跟踪过程中出视场，则认为目标Tn()保持运动趋势，不再采样计算。

对目标Tn()，如d_Tnt2大于d_Tnt1，目标中心与摄像设备水平纵向间隔变大；

对目标Tn()，如d_Tnt2等于d_Tnt1，目标中心与摄像设备水平纵向间隔不变；

对目标Tn()，如d_Tnt2小于d_Tnt1，目标中心与摄像设备水平纵向间隔变小。

3.2 确定目标长度和具体位置关系运动趋势。

假设b个目标同时到达o₁x₁y₁平面，在o₁x₁y₁平面依次计算目标集合{T1，T2，，，，Tb，T1}中一个目标方位角的边即射线逆时针旋转到下一个目标的方位角边即射线旋转的角度k₂ ^j。

(，)

将从大到小排序，选取最大值对应的两个目标方位角和，对应的。设，即目标集合{T1，T2，，，，Tb，T1}方位角差值最大两目标所在射线和夹角角平分线的方位角。

轻型无人机t2时刻方向角为，俯仰角为，以轻型无人机t2时刻位置为原点o₃，建立空间直角坐标系o₃x₃y₃z₃, x₃轴与x₁轴平行，y₃轴与y₁轴平行，z₃轴与z₁轴重合。在o₃x₃y₃平面做射线o₃A，o₃A与x₃轴夹角为γ₁。轻型无人机在o₃Az₃平面向o₃A方向进行机动，z₃轴方向速度保持不变，高度上机动R₀，R₀为常量，根据轻型无人机约束条件确定。机动时长为t₀，机动后时刻为t3。

对目标Tn()，机动前在摄像设备坐标系目标位置为（K_Tnt2, H_Tnt2，d_Tnt2），机动后目标位置为（K_Tnt3, H_Tnt3，d_Tnt3），机动前目标像位置为（k_Tnt2, h_Tnt2，f），像长为l_Tnt2，机动后目标像位置为（k_Tnt3, h_Tnt3，f），像长为l_Tnt3。

如果或或，则有：

如果，则向y₃轴正向机动R₀，机动前有：

；

；

同理，机动后有：

假设目标在t₀时长内目标在y₁轴方向匀速运动，在摄像机坐标系则有：

 ；

；

如果，则向y₃轴负向机动R₀，则有：

。

如果或，则向x₃轴正向机动R₀。机动前在摄像机坐标系有：

；

；

同理，机动后有：

假设目标在t₀时长内在x₁轴方向匀速运动，在摄像机坐标系则有：

 ；

；

如果，则向x₃轴负向机动R₀，则有：

。

继续对目标视频采样，t4时刻，计算目标到达o₁x₁y₁平面的时间t_Tn为：

 ()

如，设的目标数量为c。计算目标到达o₁x₁y₁平面时，目标的为：

 ()

目标到达o₁x₁y₁平面时，目标的为：

 ()

目标到达o₁x₁y₁平面时，目标在o₁x₁y₁平面的方位角的集合：

()

目标到达o₁x₁y₁平面距原点o₁的距离：

 ()。

步骤三：判断目标是否危险接近微型无人机，如危险接近给出机动规避方法。

1、建立空间直角坐标系。轻型无人机t4时刻方向角为，俯仰角为，以轻型无人机t4时刻位置为原点o₄，建立空间直角坐标系o₄x₄y₄z₄, x₄轴与x₁轴平行，y₄轴与y₁轴平行，z₄轴与z₁轴重合。

2、设定规避规则。规避基本原则：目标到达o₁x₁y₁平面时与轻型无人机距离，轻型无人机保持运动状态；如目标到达o₁x₁y₁平面时，则进行规避，t4时刻开始规避，完成规避后时刻为t5，规避后目标到达o₁x₁y₁平面时与光心的距离，R为定常距离值。R的确定要考虑飞行空域目标的数量和机型，机动过程中轻型无人机在空间直角坐标系内z₄轴方向速度分量保持不变，规避后恢复规避前方向角和俯仰角。

3.判断是否需要规避。目标到达o₁x₁y₁平面时与轻型无人机距离，轻型无人机保持运动状态；如目标到达o₁x₁y₁平面时，则进行规避。

4.机动规避方法。假设c个目标同时到达o₁x₁y₁平面，在o₁x₁y₁平面依次计算目标集合{T1，T2，，，，Tc，T1}中一个目标方位角的边即射线逆时针旋转到下一个目标的方位角边即射线旋转的角度k₂ ^j。

(，)

将从大到小排序，选取最大值对应的两个目标方位角和，对应的。设，即目标集合{T1，T2，，，，Tc，T1}方位角差值最大两目标所在射线和夹角角平分线的方位角。

4.1目标为单目标。在空间直角坐标系o₄x₄y₄平面内做射线o₄A, o₄A与x₄轴夹角为，轻型无人机在平面o₄Az₄向o₄A方向机动进行规避,规避距离在o₄A上投影距离为。

4.2如目标为多目标。

如，在空间直角坐标系o₄x₄y₄平面内做射线o₄A, o₄A与x₄轴夹角为，轻型无人机在平面o₄Az₄向o₄A方向机动进行规避；

如，则规避距离在o₄A上投影距离为；

如，则规避距离在o₄A上投影距离为。

如，在空间直角坐标系o₄x₄y₄平面内做射线o₄A, o₄A与x₄轴夹角为，轻型无人机在平面o₄Az₄向o₄A方向机动进行规避；

如，则规避距离在o₄A上投影距离为R₁：

从机动后的点向射线做垂线，在直角三角形内有：

；

如，则规避距离在o₄A上投影距离为R₂：

从机动后的点向射线做垂线，在直角三角形内有：

。

如，在空间直角坐标系o₄x₄y₄平面内做射线o₄A, o₄A与x₄轴夹角为，轻型无人机在平面o₄Az₄向o₄A方向机动进行规避；

如，则规避距离在o₄A上投影距离为R₃：

从机动后的点向射线延长线做垂线，在直角三角形内有：

；

如，则规避距离在o₄A上投影距离为R₄：

从机动后的点向射线延长线做垂线，在直角三角形内有：

。

Claims (3)

1.一种轻型无人机自主感知与规避方法，其特征在于，对目标集合{目标1，目标2，，，，目标1}，在空间直角坐标系内给出沿方位角差值最大两个空中目标所在射线夹角的角平分线和规避前轻型无人机航向确定的平面规避的方法，规避方向为角平分线方向，规避过程中规避前轻型无人机航向方向速度分量保持不变，规避后空中目标到达光心所在法平面时，空中目标与光心的最小距离为最小安全距离。

2.根据权利要求1所述的一种轻型无人机自主感知与规避方法，其特征在于，对目标集合{目标1，目标2，，，，目标1}，在空间直角坐标系内通过沿方位角差值最大两个空中目标所在射线夹角的角平分线和机动前航向确定的平面机动的方法，确定空中目标长度，机动方向为角平分线方向，机动过程中机动前前轻型无人机航向方向速度分量保持不变，机动距离在角平分线投影为常值。

3.根据权利要求1所述的一种轻型无人机自主感知与规避方法，其特征在于，在空间直角坐标系内一次规避视场内所有可能危险接近的空中目标。