度V。
[0052]
将灸^从大到小排序，选取灸2>最大值是对应的两个目标方位角，Db和 ，对应的
，即目标集合IT1， T2，，，，Tb，Tl}方位角差值最大两目标所在射线夹角角平分线的方 位角。
[0053] 轻型无人机t2时刻方向角为^，俯仰角为碎，以轻型无人机t2时刻位置为原点 〇3,建立空间直角坐标系〇3x3y3z 3, χ3轴与X漸平行，y 3轴与y漸平行，z 3轴与z淑重合。 在o3x3y3平面做射线〇 3A，O3A与X3轴夹角为γ i。轻型无人机在O3Az3平面向ο 3A方向进行 机动，Z3轴方向速度保持不变，高度上机动Rtl, Rtl为常量，根据轻型无人机约束条件确定。机 动时长为h，机动后时刻为t3。
[0054] 对目标Τη(?€?? )，机动前在摄像设备坐标系目标位置为（KTnt2，HTnt2，dTnt2)，机 动后目标位置为（KTnt3, HTnt3，dTnt3)，机动前目标像位置为（kTnt2, hTnt2，f )，像长为lTnt2，机动 后目标像位置为（kTnt3, hTnt3, f )，像长为lTnt3。
[0055] 如果?I #开或巧# 2几或乃# 0，则有： 如果〇 <η < X，则向y3轴正向机动R 〇,机动前有：
同理，机动后有： 假设目标在h时长内目标在y 4由方向匀速运动，在摄像机坐标系则有：
如果π < K < 2π，则向y3轴负向机动R。，则有：
[0056] 如果F1 = 2JT或矜=0，则向X3轴正向机动R 0。机动前在摄像机坐标系有：
同理，机动后有：
假设目标在h时长内在X 4由方向匀速运动，在摄像机坐标系则有：
如果？I = V，则向X3轴负向机动R。，则有：
[0057] 继续对目标视频采样，t4时刻，计算目标到达O1Xiyi平面的时间t Tn为：
如& >〇，设％>〇的目标数量为c。计算目标到达O1X1Y1平面时，目标的尤为：
目标到达^仏平面时，目标的为：
目标到达O1X1Y1平面时，目标在o ^y1平面的方位角的集合：
目标到达O1X1Y1平面距原点〇 i的距离：
[0058] 步骤三：判断目标是否危险接近微型无人机，如危险接近给出机动规避方法。
[0059] 1、建立空间直角坐标系。轻型无人机t4时刻方向角为科，俯仰角为砵，以轻型无 人机t4时刻位置为原点O4，建立空间直角坐标系o4x4y4z 4, X4轴与X i轴平行，y 4轴与y 4由 平行，Z4轴与z 4自重合。
[0060] 2、设定规避规则。规避基本原则：目标到达O1Xiyi平面时与轻型无人机距离 仝/?，轻型无人机保持运动状态；如目标到达O1Xiyi平面时·%</?，则进行 规避，t4时刻开始规避，完成规避后时刻为t5,规避后目标到达O1Xiyi平面时与光心的距 离·％^^ >/?，R为定常距离值。R的确定要考虑飞行空域目标的数量和机型，机动过程 中轻型无人机在空间直角坐标系内Z4轴方向速度分量保持不变，规避后恢复规避前方向角 %和俯仰角4。
[0061] 3.判断是否需要规避。目标到达O1Xiyi平面时与轻型无人机距离Si?， 轻型无人机保持运动状态；如目标到达O1Xiyi平面时</?，则进行规避。
[0062] 4.机动规避方法。假设c个目标同时到达O1Xiyi平面，在O 1Xiyi平面依次计算目 标集合{Tl，T2，，，，Tc，T1}中一个目标方位角的边即射线逆时针旋转到下一个 目标的方位角边即射线旋转的角度k2j。
[0063]
将是从大到小排序，选取灸/'最大值对应的两个目标方位角；"Dijk和 ?Vgi%，对应K
，即目标集合it 1， T2，，，，Tc，Tl}方位角差值最大两目标所在射线O1^2ijk夹角角平分线的方 位角。
[0064] 4. 1目标为单目标。在空间直角坐标系o4x4y4平面内做射线O4A, 〇，与义4轴夹 角为M，轻型无人机在平面〇,24向ο 4A方向机动进行规避，规避距离在O4A上投影距离为 R ~ s^n °
[0065] 4. 2如目标为多目标。
[0066] 如
，在空间直角坐标系〇4x4y 4平面内做射线〇 4A, O4A与乂4轴夹角为 ?1，轻型无人机在平面O4Az4向ο 4A方向机动进行规避；
如 ，则规避距离在O4A上投影距离为 如 ，则规避距离在O4A上投影距离为
[0067] 如
，在空间直角坐标系〇4x4y 4平面内做射线O4A, 〇，与14轴夹角为 ?!，轻型无人机在平面O4Az4向ο 4A方向机动进行规避； 如，则规避距离在〇4A上投影距离为R1: 从机动后的点向射线做垂线，在直角二角形内有：
如5JWli > jVgijk，则规避距离在O4A上投影距离为R2: 从机动后的点向射线做垂线，在直角三角形内有：
[0068] 如
，在空间直角坐标系〇4x4y 4平面内做射线O4A, 〇，与乂4轴夹角为 >1，轻型无人机在平面O4Az4向ο 4A方向机动进行规避； 如，则规避距离在O4A上投影距离为R3: 从机动后的点向射线延长线做垂线，在直角三角形内有：
如jWji ，则规避距离在Q4A上投影距离为R4: 从机动后的点向射线C^jrgijk延长线做垂线，在直角三角形内有：
【主权项】
1. 一种轻型无人机自主感知与规避方法，其特征在于，对目标集合{目标I，目标2,，，， 目标1}，在空间直角坐标系内给出沿方位角差值最大两个空中目标所在射线夹角的角平 分线和规避前轻型无人机航向确定的平面规避的方法，规避方向为角平分线方向，规避过 程中规避前轻型无人机航向方向速度分量保持不变，规避后空中目标到达光心所在法平面 时，空中目标与光心的最小距离为最小安全距离。2. 根据权利要求1所述的一种轻型无人机自主感知与规避方法，其特征在于，对目标 集合{目标1，目标2,，，，目标1}，在空间直角坐标系内通过沿方位角差值最大两个空中目 标所在射线夹角的角平分线和机动前航向确定的平面机动的方法，确定空中目标长度，机 动方向为角平分线方向，机动过程中机动前前轻型无人机航向方向速度分量保持不变，机 动距离在角平分线投影为常值。3. 根据权利要求1所述的一种轻型无人机自主感知与规避方法，其特征在于，在空间 直角坐标系内一次规避视场内所有可能危险接近的空中目标。
【专利摘要】本发明公开了一种轻型无人机自主感知与规避方法，属于航路规划领域，特别是涉及一种轻型无人机超视距飞行规避空中目标方法。首先通过前视单目定焦摄像机采集视频图像，然后计算像在像直角坐标系内的长度、高度和位置；在摄像机坐标系内推导目标位置和运动趋势；在空间直角坐标系内沿方位角差值最大两个目标所在射线夹角的角平分线和机动前航向确定的平面机动，确定目标长度；在摄像机坐标系内计算具体目标位置和运动趋势；最后如危险接近在空间直角坐标系内沿方位角差值最大两个目标所在射线夹角的角平分线和机动前航向确定的平面规避，规避后目标与光心最小距离为最小安全距离。本发明的优点在于实现自主感知，一次规避所有目标，机动距离短。
【IPC分类】G05D1/10
【公开号】CN104914877
【申请号】CN201510403797
【发明人】不公告发明人
【申请人】沈阳飞羽航空科技有限公司
【公开日】2015年9月16日
【申请日】2015年7月13日