本发明涉及无人机的，尤其涉及一种无人机飞行状态判断方法及系统。
背景技术：
1、无人机广泛应用于航空摄像、快递运输、农药喷洒等多个领域，并发挥着重要的作用。无人机通过预先设定或实时接收用户指令，进行飞行控制，主要包括：按照指令进行起飞返航、飞行指定位置、稳定飞行姿态。
2、无人机飞行状态受用户指令控制，现有的无人机飞行状态判断的研究集中于采用算法或模型提高目标状态估计精度，从而进行飞行状态判断，较少结合用户指令序列进行分析，也没有将用户指令序列与无人机飞行状态进行一一匹配，从用户指令的角度判断无人机飞行状态。因此，本发明提供一种无人机飞行状态判断方法，通过判断当前飞行状态是否与用户指令预期相符，实现用户指令的有效执行，提升无人机的控制性能。
技术实现思路
1、有鉴于此，本发明提供一种无人机飞行状态判断方法及系统，目的在于：通过滤波估计提升无人机的飞行参数预测精度，建立用户指令与无人机飞行状态变化量的对应关系以研究用户指令执行情况，通过比较无人机的实际飞行状态变化量和理想飞行状态变化量，判断无人机的飞行状态是否满足指令要求，从而实现无人机的飞行状态直观有效判断。
2、实现上述目的，本发明提供的一种无人机飞行状态判断方法，包括以下步骤：
3、s1：利用无人机相对地面雷达的距离、方位角和高度参数，构建无人机的测量向量；
4、s2：采用卡尔曼滤波，对无人机的测量向量进行滤波，预测无人机的真实飞行状态；
5、s3：通过预测的无人机真实飞行状态，计算无人机的偏航角、俯仰角和滚转角，构建无人机的六维飞行状态向量；
6、s4：构建用户指令序列，计算无人机执行第i条指令后，实际飞行状态变化量；
7、s5：计算无人机执行第i条指令后的理想飞行状态变化量，并与步骤s4输出的实际飞行状态变化量进行比较，评估无人机飞行状态。
8、作为本发明的进一步改进方法：
9、可选地，所述步骤s1中，利用无人机相对地面雷达的距离、方位角和高度参数，包括：
10、a1：地面雷达接收到无人机k时刻的回波信号ech，将回波信号ech分解为不同尺度的小波系数：
11、
12、其中，wvc(achi,b)表示ech的小波系数，achi为尺度参数，b为平移参数，ψ*为小波函数的复共轭；
13、对小波系数wvc(achi,b)进行阈值处理，去除含有噪声的高频系数，得到只含有目标的小波系数wsig(achi,b)；
14、利用wsig(achi,b)，通过信号重构，得到去噪后的无人机回波信号dec；
15、a2：选定无人机参考叶片，建立相位补偿因子
16、phc＝exp(j·amp·cos(ang·m+inp))
17、其中，phc表示参考叶片相位补偿因子，exp表示指数函数，j表示虚数符号，amp表示幅度系数向量，ang表示角速度系数向量；
18、对无人机回波信号进行相位补偿，得到补偿后的回波信号：
19、dec′＝dec·phc
20、其中，dec′表示补偿后的回波信号；
21、a3：对dec′进行二维傅里叶变换，得到距离-多普勒图像rd，对rd进行一维恒虚警检测，得到k时刻无人机相对于地面雷达的距离rangk；
22、a4：对dec′进行三维傅里叶变换，得到不同方向的空间频域响应，采用极值检测方法，得到k时刻的无人机相对于地面雷达的方位角azik；
23、a5：计算k时刻的无人机相对于地面的高度highk：
24、highk＝imw·gsd·fr/(sw·100)
25、其中，imw表示无人机拍摄图像宽度，gsd表示地面分辨率，fr表示无人机摄像机真实焦距，sw表示无人机摄像机真实宽度。
26、可选地，所述步骤s1中，构建无人机的测量向量，包括：
27、s101：以地面雷达为坐标原点，无人机前进方向为x轴方向，水平面上与无人机前进方向垂直的方向为y轴方向，竖直方向为z轴方向，计算无人机在k时刻的位置：
28、
29、
30、meazk＝highk
31、其中，meaxk表示k时刻无人机在x轴的测量位置，meayk表示k时刻无人机在y轴的测量位置，meazk表示k时刻无人机在z轴的测量位置；
32、s102：构建无人机的测量向量
33、measurek＝[meaxk,meayk,meazk]
34、其中，measure表示无人机的测量向量。
35、可选地，所述步骤s2中，采用卡尔曼滤波，预测无人机的真实飞行参数，包括：
36、s201：建立无人机在k时刻的真实状态向量：
37、statek＝[poxk,poyk,pozk,vxk,vyk,vzk,accexk,acceyk,accezk]
38、其中，statek表示无人机在k时刻的真实状态向量，poxk表示k时刻无人机在x轴的真实位置，poyk表示k时刻无人机在y轴的真实位置，pozk表示k时刻无人机在z轴的真实位置，vxk表示k时刻无人机在x轴的真实速度，vyk表示k时刻无人机在y轴的真实速度，vzk表示k时刻无人机在z轴的真实速度，accexk表示k时刻无人机在x轴的真实加速度，acceyk表示k时刻无人机在y轴的真实加速度，accezk表示k时刻无人机在z轴的真实加速度；
39、s202：预测无人机的真实飞行状态
40、statek+1k＝f·statek
41、pk+1k＝fpkft+qk
42、其中，statek+1k表示无人机在(k+1)时刻的真实状态向量预测向量，f表示无人机运动的状态转移矩阵，ft表示矩阵f转置，pk表示k时刻的协方差矩阵值，pk+1|k表示(k+1)时刻的协方差矩阵预测值，qk表示k时刻过程噪声协方差矩阵；
43、s203：计算卡尔曼增益
44、kalzk+1＝pk+1|kht(hpk+1|kht+rk+1)-1
45、其中kalzk+1表示(k+1)时刻的卡尔曼增益，rk+1表示观测噪声协方差，(hpk+1|kht+rk+1)-1表示对hpk+1|kht+rk+1求逆矩阵；
46、s204：无人机在(k+1)时刻的真实状态statek+1更新为：
47、statek+1＝statek+1k+kalzk+1(measurek+1-h·statek+1k)
48、statek+1＝[poxk+1,poyk+1,pozk+1,vxk+1,vyk+1,vzk+1,accexk+1,acceyk+1,accezk+1]
49、其中，poxk+1表示(k+1)时刻无人机在x轴的真实位置，poyk+1表示(k+1)时刻无人机在y轴的真实位置，pozk+1表示(k+1)时刻无人机在z轴的真实位置，vxk+1表示(k+1)时刻无人机在x轴的真实速度，vyk+1表示(k+1)时刻无人机在y轴的真实速度，vzk+1表示(k+1)时刻无人机在z轴的真实速度，accexk+1表示(k+1)时刻无人机在x轴的真实加速度，acceyk+1表示(k+1)时刻无人机在y轴的真实加速度，accezk+1表示(k+1)时刻无人机在z轴的真实加速度，measurek+1表示k+1时刻无人机的测量向量。
50、可选地，所述步骤s3中，利用预测的无人机真实飞行参数，构建无人机的飞行状态向量，包括：
51、s301：计算无人机在k时刻的偏航角yawk，俯仰角pitchk，滚转角rollk：
52、yawk＝arctan(accezk/sqrt(accexk2+accezk2))
53、pitchk＝arctan(accexk/sqrt(acceyk2+accezk2))
54、rollk＝arctan(acceyk/sqrt(accexk2+accezk2))
55、s302：构建无人机在k时刻的六维飞行状态向量uavk：
56、uavk＝[poxk,poyk,pozk,yawk,pitchk,rollk]。
57、可选地，所述步骤s4中，构建用户指令序列，计算无人机执行第i条指令后，实际飞行状态变化量，包括：
58、s401：构建用户指令序列：
59、useri＝[throi,yaw_vi,pitch_vi,roll_vi,ti]
60、其中，i表示用户指令序列号，useri表示用户指令，throi表示油门速度，yaw_vi表示偏航角旋转速度，pitch_vi表示俯仰角旋转速度，roll_vi表示滚转角旋转速度，ti为用户发送第i条指令的时间；
61、s402：确定用户指令序列useri对应的无人机初始飞行状态向量和最终飞行状态向量：
62、
63、
64、其中，initi表示用户发送第i条指令时无人机的初始状态，endi表示执行第i条指令后无人机的最终状态，c表示光速，rangti表示用户发送第i条指令时刻无人机相对于地面雷达的距离，rangti+1表示用户发送第i+1条指令时刻无人机相对于地面雷达的距离；
65、s403：计算initi到endi的无人机实际飞行状态变化量change：
66、change＝[δpox,δpoy,δpoz,δyaw,δpitch,δroll]
67、其中，δpox表示无人机在x轴的位置变化，δpoy表示无人机在y轴的位置变化，δpoz表示无人机在z轴的位置变化，δyaw表示无人机偏航角变化，δpitch表示无人机俯仰角变化，δroll表示无人机滚转角变化。
68、可选地，所述步骤s5中，计算无人机执行第i条指令后的理想飞行状态变化量，并与步骤s4输出的实际飞行状态变化量进行比较，评估无人机飞行状态，包括：
69、s501：无人机执行第i条指令后的理想飞行状态变化表示为：
70、ideali＝[pox′,poy′,poz′,yaw′,pitch′,roll′]
71、pox′＝throi·(ti+1-ti)
72、poy′＝throi·(ti+1-ti)·tan(yawi·(ti+1-ti))
73、poz′＝throi·(ti+1-ti)·sin(pitchi·(ti+1-ti))
74、yaw′＝yawi·(ti+1-ti)
75、pitch′＝pitchi·(ti+1-ti)
76、roll′＝rolli·(ti+1-ti)
77、其中，ideali表示无人机执行第i条指令后的理想状态变化，pox′表示无人机执行第i条指令后在x轴的理想位置变化，poy′表示无人机执行第i条指令后在y轴的理想位置变化，poz′表示表示无人机执行第i条指令后在z轴的理想位置变化，yaw′表示无人机执行第i条指令后的理想偏航角变化，pitch′表示无人机执行第i条指令后的理想俯仰角变化，roll′表示无人机执行第i条指令后的理想滚转角变化；
78、s502：对ideali与change中的元素进行逐项比较，评估无人机飞行状态：
79、
80、
81、其中，posdeter用于评估无人机飞行的位置状态，posdeter＝1表示无人机飞行的位置状态正确，posdeter＝0表示无人机飞行的位置状态不正确，δd表示无人机位置偏离的门限阈值，freedeter用于评估无人机飞行的自由度状态，freedeter＝1表示无人机飞行的自由度状态正确，freedeter＝0表示无人机飞行的自由度状态不正确，δθ表示无人机自由度偏离的门限阈值。
82、本发明还提供了一种无人机飞行状态判断系统，其特征在于，包括：
83、无人机测量向量模块：构建无人机测量向量；
84、无人机飞行状态滤波模块：采用卡尔曼滤波，预测无人机的真实飞行状态；
85、无人机飞行状态向量模块：构建无人机的六维飞行状态向量；
86、无人机飞行状态变化量计算模块：构建用户指令序列，计算无人机执行指令后实际飞行状态变化量；
87、无人机飞行状态评估模块：计算无人机执行指令后理想的飞行状态变化量，与实际飞行状态变化量比较，评估无人机飞行状态。
88、有益效果
89、本发明通过卡尔曼滤波估计，实现无人机真实飞行参数精确预测；建立用户指令与无人机飞行状态变化量的对应关系，有利于直观展示无人机飞行状态的变化，以及定量比较无人机的实际飞行状态变化量和理想飞行状态变化量，有效地实现了无人机飞行状态的判断。
90、本发明对无人机的回波信号进行去噪、相位补偿的信号处理操作，有利于得到准确的回波信号，提取无人机的准确距离、方位角、高度信息，构建精准的测量向量；采用卡尔曼滤波，充分利用无人机的位置、速度、加速度参量之间的关系进行估计，有利于得到准确的滤波结果。
91、本发明将用户指令进行序列化，有利于统一用户指令格式；计算无人机用户指令的理想飞行状态变化，为无人机的实际飞行状态变化提供了参考，逐项比较的方式有利于综合全面评估无人机的飞行状态，对无人机进行风险控制。
1.一种无人机飞行状态判断方法，其特征在于，所述方法包括：
2.根据权利要求1所述的无人机飞行状态判断方法，其特征在于，所述步骤s1包括：
3.如权利要求2所述的无人机飞行状态判断方法，其特征在于，所述步骤s2包括：
4.如权利要求3所述的无人机飞行状态判断方法，其特征在于，所述步骤s3包括：
5.如权利要求4所述的一种无人机飞行状态判断方法，其特征在于，所述步骤s4包括：
6.如权利要求5所述的无人机飞行状态判断方法，其特征在于，所述步骤s5包括：
7.一种无人机飞行状态判断系统，其特征在于，包括：
