多轴无人机高度更新方法与流程

技术特征：

1.一种多轴无人机高度更新方法，其特征在于，包括如下步骤：

步骤S1：判断无人机的气压计数据是否更新，如果更新，执行S2；如果没有更新，执行S3；

步骤S2:通过气压计获取当前飞行高度，进行高度更新，之后进行当前气压计修正系数计算；

步骤S3：判断加速度计数据是否更新，如果更新，执行S4；如果没有更新，执行S6；

步骤S4：将机体坐标系修正后的加速度转换为地理坐标系下的加速度；

步骤S5：加速度偏移向量校正；偏移向量转移到机体坐标系；

步骤S6：加速度推算高度和速度信息；利用气压计校准系数进行修正高度和速度信息；

步骤S7：返回执行S1判断气压计数据是否更新。

2.如权利要求1所述的多轴无人机高度更新方法，其特征在于，所述无人机为四轴无人机。

3.如权利要求1所述的多轴无人机高度更新方法，其特征在于，在步骤S2中，通过气压计获取当前飞行高度的公式为：

公式一中，Hb、pb和Tb分别为相应大气层的重力势高度、大气压力以及大气层大气温度的下限值；β为温度垂直变化率；R为空气气体常数；g_n为自由落体加速度；p_h为当前所测得的大气静压。

将起飞点时气压计所测得的高度作为基准高度，与当前气压高度计测量值做差，得到定位高度h测高方程公式：

h_at＝h+b+ε₁ 公式二；

公式二中，h为无人机的真实定位高度，b为环境影响所造成的误差，ε₁为量测噪声；

当前气压计修正系数计算方法：

corr_bar＝-H-z_est0 公式3；

式中，corr_bar为气压计修正系数，z_est0为加速度计Z轴高度。

4.如权利要求1所述的多轴无人机高度更新方法，其特征在于，在步骤S5中，加速度偏移向量校正的公式为：

accel_bias＝accel_bias-corr_bar*para*para 公式4；

公式4中，accel_bias为校正后的加速度偏移向量，para为加速度计Z轴校正系数；

偏移向量转移到机体坐标系的步骤包括：

将地理坐标系利用四元素转换为载体坐标系时，其方向余弦矩阵为

将偏移向量转换到机体坐标系公式：

acc_bias＝acc_bias+Rot_matrix[x][y]*accel_bias*para1*dt 公式6；

公式6中，acc_bias为机体坐标系下的偏移向量，Rot_matrix[x][y]为对应的公式5的旋转矩阵，para1为偏移向量积分修正系数，dt为积分时间间隔。

5.如权利要求1所述的多轴无人机高度更新方法，其特征在于，在步骤S6中，

设捷联式加速度计沿无人机的纵轴、横轴和垂直轴方向与飞机固联，加速度计放置在无人机的重心处，忽略角运动对加速度计输出的影响，则垂向加速度计的量测方程为：

a_acc＝a_g+ε₃ 公式7；

公式7中，a_acc为垂向加速度，a_g为真实的垂向加速度，ε₃为加速度计对高度的量测噪声；

飞机相对于起飞地面的飞行高度h_acc、垂向速度v_acc和垂向加速度a_acc之间的关系表示为：