[发明专利]一种无人机动态紧急避撞区的建模方法

权利要求书

1.一种无人机动态紧急避撞区的建模方法，其特征在于，包括以下几个步骤：

步骤一：根据无人机的机载感知设备获取入侵机的飞行信息，并通过冲突检测方法判断无人机与入侵机是否存在飞行冲突；

无人机的机载感知设备获取无人机的飞行速度V₀、入侵机的飞行速度V₁、无人机与入侵机的相对速度V_r，则实际飞行情况等效为无人机静止不动，入侵机以V_r飞行，间隔单位时间内，进行两次测量，获取第一次测量时，无人机与入侵机的相对距离R₁、相对方位角θ₁，相对方位角θ₁为R₁与V₀之间的夹角，然后，单位时间间隔后，获取第二次测量时，无人机与入侵机的相对距离R₂、相对方位角θ₂，相对方位角θ₂为R₂与V₀之间的夹角，通过几何关系推导可得，当无人机与入侵机到达最接近点处时，水平相对距离为：

Dmin=R1R2sin(θ1-θ2)R12+R22-2R1R2cos(θ1-θ2)---(1)]]>

设定无人机的最小安全距离D_limit，如果D_min小于飞机间最小安全距离D_limit，则无人机与入侵机间存在飞行冲突，进入步骤二；

步骤二：如果判定无人机与入侵机之间存在飞行冲突，根据入侵机的飞行航向确定无人机紧急避撞的机动方式；

①当无人机与入侵机迎面遭遇且入侵机在无人机左侧或者右侧，无人机采取以最大转弯角速率r右转机动或者左转机动完成紧急避撞任务；

②当无人机与入侵机追尾遭遇且入侵机在无人机左侧或者右侧，无人机采取以最大转弯角速率r右转机动或者左转机动完成紧急避撞任务；

③当无人机与入侵机正侧向遭遇且入侵机在无人机右侧或者左侧，无人机采取以最大转弯角速率r左转机动或者右转机动完成紧急避撞任务；

步骤三：建立无人机动态紧急避撞区模型方程；

根据步骤二得到的三种不同情形下的紧急避撞机动方式，分别建立无人机动态紧急避撞区模型方程：

①当无人机与入侵机迎面遭遇且入侵机在无人机左侧或者右侧，无人机采取以最大转弯角速率r右转机动或者左转机动，建立无人机动态紧急避撞区模型方程，具体为：

Δx=Dcosθ-V1t-V0rsin(r*t)Δy=Dsinθ+V0r-V0rcos(r*t)D[V0sinθsin(rt)-V0cosθcos(rt)-V1cosθ]+V02rsin(rt)+V12t+V0rV1sin(rt)+V0V1tcos(rt)=0(Δx)2+(Δy)2-Dlimit2=0---(3)]]>

其中：Δx为无人机与入侵机前向相对距离，Δy为无人机与入侵机侧向相对距离，V₀、V₁分别为无人机、入侵机的飞行速度，θ为紧急避撞机动开始时，入侵机与无人机的相对方位角；D为紧急避撞机动开始时，无人机与入侵机的相对距离，即为紧急避撞区的边界值；r表示无人机转弯角速率；t表示紧急避撞机动需要的时间；

h(t)为紧急避撞机动开始t时间后，无人机与入侵机的相对距离，引入约束条件，当h'(t)＝0时刻，h(t)刚好等于无人机与入侵机的最小安全距离D_limit；

通过求解模型方程（3），获得到达最小安全距离时刻t以及入侵机与无人机相对方位角为θ入侵时，紧急避撞区的边界值D；

②当无人机与入侵机追尾遭遇且入侵机在无人机左侧或者右侧，无人机采取以最大转弯角速率r右转机动或者左转机动完成紧急避撞机动，建立无人机动态紧急避撞区模型方程，具体为：

重复上述①的过程，得到紧急避撞区的模型方程：

Δx=Dcosθ-V1t-V0rsin(r*t)Δy=Dsinθ+V0r-V0rcos(r*t)D[V0sinθsin(rt)+V0cosθcos(rt)-V1cosθ]+V02rsin(rt)+V12t-V0V1rsin(rt)-V0V1tcos(rt)=0(Δx)2+(Δy)2-Dlimit2=0---(4)]]>

通过求解模型方程（4），得到到达最小安全距离时刻t以及入侵机与无人机相对方位角为θ入侵时，紧急避撞区的边界值D；

③当无人机与入侵机正侧向遭遇且入侵机在无人机右侧或者左侧，无人机采取以最大转弯角速率r左转机动或者右转机动完成紧急避撞机动，建立无人机动态紧急避撞区模型方程，具体为：

重复上述①的过程，得到紧急避撞区的模型方程：

Δx=Dcosθ-V0rsin(r*t)Δy=Dsinθ-V1t+V0r-V0rcos(r*t)D[V0cosθcos(rt)-V0sinθsin(rt)-V1sinθ]-V02rsin(rt)+V12t+V0rV1cos(rt)+V0V1tsin(rt)+V0V1r=0(Δx)2+(Δy)2-Dlimit2=0---(5)]]>

通过求解模型方程（5），得到到达最小安全距离时刻t以及入侵机与无人机相对方位角为θ入侵时，紧急避撞区的边界值D；

步骤四：对步骤三获取的无人机紧急避撞区模型方程进行求解计算，得到紧急避撞区的边界值以及避撞过程所需时间；

通过步骤三得到了无人机紧急避撞区模型方程，采用解非线性方程组方法，求取紧急避撞区的边界值D以及避撞过程所需时间t；

步骤五：将入侵机方位角依次从0~2π范围内取值，并重复采用步骤四进行求解计算，得到无人机紧急避撞区完整包络；

如果入侵机飞临紧急避撞区边界，则采取紧急避撞机动完成避撞任务。

2.根据权利要求1所述的一种无人机动态紧急避撞区的建模方法，其特征在于，所述的步骤四中，紧急避撞区模型方程的求解方法为：

①根据步骤三所得紧急避撞区模型方程组中的第三个方程：

D[V0cosθcos(rt)-V0sinθsin(rt)+V1sinθ]-V02rsin(rt)-V12t-V0rV1cos(rt)+V0V1tsin(rt)+V0V1r=0]]>

将紧急避撞区边界值D表示为关于避撞所需时间t的函数，即D＝f₁(t)；将模型方程组中的第四个方程：

(Δx)²+(Δy)²-D_limit²＝0

表示为关于D与t的函数，即y＝f₂(t，D)；

②将t₀,t₁分别代入上述两个函数，求得y₀，y₁；

③如果y₀*y₁>0，则表明在该时间区间内模型方程无解，此时令紧急避撞区边界值D＝D_limit，避撞过程所需时间t=0；如果y₀*y₁<0，则表明在该时间区间内模型方程有解，并进行下一步求解计算；

④如果|y₀|＜ξ，则表明当前t₀满足求解要求，此时令t=t₀，并求得紧急避撞区边界值D；

⑤如果|y₀|≥ξ，则令并代入上述两个函数求得y；

⑥如果y₀*y<0，则令y₁＝y,t₁=t，否则令y₀＝y,t₀=t，并返回④重复上述过程；

上述中，t₀为时间初始值，t₁为时间终值，D为紧急避撞区边界值，t为避撞过程所需时间，ξ为给定精度；设定时间t的初始值为t₀=0s，终值为无人机转向180°所需时间t₁=π/r，r为无人机最大转弯角速率。