[发明专利]一种无人机固定半径盘旋领航方法

权利要求书

1、一种无人机固定半径盘旋领航方法，其特征在于包括如下步骤：

步骤一：计算飞机当前位置子午面内地球主曲率半径R_m以及当前位置子午面垂直平面内的地球主曲率半径R_n，然后计算飞机盘旋时的圆心位置与当前飞机位置的地理经度偏差量ΔL及地理纬度偏差量ΔB：

Rm=Ra×(1-2×f+3×f×sin2B)Rn=Ra×(1+f×sin2B)]]>

ΔL=Rturn·VdnVdn2+Vde2·Rm·cosBΔB=Rturn·VdeVdn2+Vde2·Rn]]>

上式中的参数依据WGS_84坐标系，地球长半轴R_a＝6378137.0m，地球椭偏度f＝0.003352811；Rturn为既定的盘旋半径，V_dn为飞机的北向地速，V_de为飞机的东向地速，B为飞机的地理纬度；

步骤二：根据收到的盘旋指令的不同解算相应的盘旋圆心地理经度L_C和地理纬度B_C；

根据步骤一中的ΔB和ΔL，计算盘旋圆心地理经度L_C和盘旋圆心地理纬度B_C为：

其中B为飞机的地理纬度，L为飞机的地理经度；

步骤三：根据盘旋圆心地理纬度B_C计算盘旋圆心处的地心半径B_p、地心纬度B_ECC及飞机的地心纬度B_EC：

Rp=Ra·{1+f22·[11+(1-f2)·tgBC-1]}]]>

BEC=arctg[(H+RbH+Ra)2·tgB]]]>

BECC=arctg[(H+RbH+Ra)2·tgBC]]]>

其中，地球短半轴R_b＝6356752.3m，长半轴R_a＝6378137.0m，H和B分别为飞机高度和纬度；

步骤四：计算飞机到盘旋圆心的距离D和飞机的侧偏距D_Z：

D=Δx2+Δy2,]]>其中Δx=Rp·cosBECC·ΔLΔy=Rp·(BEC-BECC)]]>

D_Z＝D-Rturn；

步骤五：计算从盘旋圆心到飞机当前位置的向量与东向的夹角α、侧偏移速度D_Zd：

α＝arctg(Δy/Δx)

步骤六：将步骤四及步骤五中得到的飞机的侧偏距D_Z及侧偏移速度D_Zd输出至侧向控制回路得到舵偏度指令，最终实现引导飞机沿着期望航线飞行。