本发明涉及测绘领域，具体涉及一种多轴无人机高度更新方法。
背景技术：
MEMS(Micro-Electro-Mechanical System，微机电系统)加速度计和气压计是可以测量无人机相对高度的器件。加速度计和气压计测量高度，优缺点共存：加速度计存在长时间积分，高度短期测量准确(忽略噪声干扰)，长期测量不精确；气压计精度达到10cm(气压计输出的数据直接转化为高度的话会有几十厘米的浮动，直接用于高度控制极不稳定)短期测量不精确，长期测量精确。此外，无人机由于电池电量过高，会出现定高上升速度过高，出现炸机，因此也要解决定高上升速度过快问题。
技术实现要素：
本发明提供了一种多轴无人机高度更新方法，解决加速度计存在长时间积分，高度短期测量准确(忽略噪声干扰)，长期测量不精确；气压计精度达到10cm(气压计输出的数据直接转化为高度的话会有几十厘米的浮动，直接用于高度控制极不稳定)，实现了长期测量更加精确。
本发明的技术方案为：
一种多轴无人机高度更新方法，包括如下步骤：
步骤S1：判断无人机的气压计数据是否更新，如果更新，执行S2；如果没有更新，执行S3；
步骤S2:通过气压计获取当前飞行高度，进行高度更新，之后进行当前气压计修正系数计算；
步骤S3：判断加速度计数据是否更新，如果更新，执行S4；如果没有更新，执行S6；
步骤S4：将机体坐标系修正后的加速度转换为地理坐标系下的加速度；
步骤S5：加速度偏移向量校正；偏移向量转移到机体坐标系；
步骤S6：加速度推算高度和速度信息；利用气压计校准系数进行修正高度和速度信息；
步骤S7：返回执行S1判断气压计数据是否更新。
上述的多轴无人机高度更新方法，其中，所述无人机为四轴无人机。
上述的多轴无人机高度更新方法，其中，在步骤S2中，通过气压计获取当前飞行高度的公式为：
公式一中，Hb、pb和Tb分别为相应大气层的重力势高度、大气压力以及大气层大气温度的下限值；β为温度垂直变化率；R为空气气体常数；gn为自由落体加速度；ph为当前所测得的大气静压。
将起飞点时气压计所测得的高度作为基准高度，与当前气压高度计测量值做差，得到定位高度h测高方程公式：
hat＝h+b+ε1 公式二；
公式二中，h为无人机的真实定位高度，b为环境影响所造成的误差，ε1为量测噪声；
当前气压计修正系数计算方法：
corr_bar＝-H-z_est0 公式3；
式中，corr_bar为气压计修正系数，z_est0为加速度计Z轴高度。
上述的多轴无人机高度更新方法，其中，在步骤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为积分时间间隔。
上述的多轴无人机高度更新方法，其中，在步骤S6中，
设捷联式加速度计沿无人机的纵轴、横轴和垂直轴方向与飞机固联，加速度计放置在无人机的重心处，忽略角运动对加速度计输出的影响，则垂向加速度计的量测方程为：
aacc＝ag+ε3 公式7；
公式7中，aacc为垂向加速度，ag为真实的垂向加速度，ε3为加速度计对高度的量测噪声；
飞机相对于起飞地面的飞行高度hacc、垂向速度vacc和垂向加速度aacc之间的关系表示为：
本发明将加速度计短期测量精确与气压计长期测量不精确的特点结合在一起，保证了无人机上升过程中高度长期的精确稳定性。
附图说明
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明及其特征、外形和优点将会变得更明显。在全部附图中相同的标记指示相同的部分。并未刻意按照比例绘制附图，重点在于示出本发明的主旨。
图1为本发明提供的一种多轴无人机高度更新方法的流程图。
具体实施方式
在下文的描述中，给出了大量具体的细节以便提供对本发明更为彻底的理解。然而，对于本领域技术人员而言显而易见的是，本发明可以无需一个或多个这些细节而得以实施。在其他的例子中，为了避免与本发明发生混淆，对于本领域公知的一些技术特征未进行描述。
为了彻底理解本发明，将在下列的描述中提出详细的步骤以及详细的结构，以便阐释本发明的技术方案。本发明的较佳实施例详细描述如下，然而除了这些详细描述外，本发明还可以具有其他实施方式。
针对现有技术中无人机定高存在的问题，本发明提供了提高小四轴定高精度方法。
本发明技术方案为，利用互补滤波算法，可以实现加速度计和气压计的优势结合，实现无人机定高长时间稳定性和准确性；根据电池电量，改变无人机上升的速度，保证上升的稳定性。
在无人机定高中，所说的高度是地理坐标系下的相对高度，所获取的加速度计Z轴加速度信息是相对于当前机体坐标系下的，因此需要进行机体坐标系和地理坐标系加速度的实时转换，下面简单的介绍进行高度求解的方法：
在定高过程中，需要的速度和位置信息是由地理坐标系下的加速度通过一次积分，二次积分获得，而地理坐标系下的加速度是通过机体坐标系测量的加速度减去机体偏移向量，再转换到地理坐标系求得的。在加速度计和气压计数据融合的过程中，气压计在数据更新时，计算出加速度偏移量校正系数，对地理坐标系下的加速度偏移量进行修正。
由于加速度计XY轴所在平面与水平面上存在倾斜角度，因此Z轴输出的数据是重力在Z轴当前角度的一个分量，是不准确的，所以需要对Z轴的加速度计进行倾角补偿，这样得到的才是垂直方向上的当前重力。
本发明提供了一种多轴无人机高度更新方法，参照图1所示，包括如下步骤：
步骤S1：判断无人机的气压计数据是否更新，如果更新，执行S2；如果没有更新，执行S3。
步骤S2：通过气压计获取当前飞行高度，进行高度更新，之后进行当前气压计修正系数计算。具体的，通过气压计获取当前飞行高度的公式为：
公式一中，Hb、pb和Tb分别为相应大气层的重力势高度、大气压力以及大气层大气温度的下限值；β为温度垂直变化率；R为空气气体常数；gn为自由落体加速度；ph为当前所测得的大气静压。
将起飞点时气压计所测得的高度作为基准高度，与当前气压高度计测量值做差，得到定位高度h测高方程公式：
hat＝h+b+ε1 公式二；
公式二中，h为无人机的真实定位高度，b为环境影响所造成的误差，ε1为量测噪声；
当前气压计修正系数计算方法：
corr_bar＝-H-z_est0 公式3；
公式3中，corr_bar为气压计修正系数，z_est0为加速度计Z轴高度。
步骤S3：判断加速度计数据是否更新，如果更新，执行S4；如果没有更新，执行S6。
步骤S4：将机体坐标系修正后的加速度转换为地理坐标系下的加速度。
步骤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为积分时间间隔。
步骤S6：加速度推算高度和速度信息；利用气压计校准系数进行修正高度和速度信息。具体的，设捷联式加速度计沿无人机的纵轴、横轴和垂直轴方向与飞机固联，加速度计放置在无人机的重心处，忽略角运动对加速度计输出的影响，则垂向加速度计的量测方程为：
aacc＝ag+ε3 公式7；
公式7中，aacc为垂向加速度，ag为真实的垂向加速度，ε3为加速度计对高度的量测噪声；
飞机相对于起飞地面的飞行高度hacc、垂向速度vacc和垂向加速度aacc之间的关系表示为：
步骤S7：返回执行S1判断气压计数据是否更新。
本发明利用互补滤波算法，可以实现加速度计和气压计的优势结合，实现无人机定高长时间稳定性和准确性；根据电池电量，改变无人机上升的速度，保证上升的稳定性。
以上对本发明的较佳实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，其中未尽详细描述的设备和结构应该理解为用本领域中的普通方式予以实施；任何熟悉本领域的技术人员，在不脱离本发明技术方案范围情况下，都可利用上述揭示的方法和技术内容对本发明技术方案做出许多可能的变动和修饰，或修改为等同变化的等效实施例，这并不影响本发明的实质内容。因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所做的任何简单修改、等同变化及修饰，均仍属于本发明技术方案保护的范围内。