CN115727842A - 一种无人机快速对准方法、系统、计算机设备及存储介质 - Google Patents

一种无人机快速对准方法、系统、计算机设备及存储介质 Download PDF

Info

Publication number
CN115727842A
CN115727842A CN202211381425.2A CN202211381425A CN115727842A CN 115727842 A CN115727842 A CN 115727842A CN 202211381425 A CN202211381425 A CN 202211381425A CN 115727842 A CN115727842 A CN 115727842A
Authority
CN
China
Prior art keywords
angle
course angle
initial
corrected
gnss
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202211381425.2A
Other languages
English (en)
Other versions
CN115727842B (zh
Inventor
张淏酥
王涛
苗建明
龚喜
任磊
郑若晗
孙兴宇
仝懿聪
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Sun Yat Sen University
Original Assignee
Sun Yat Sen University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Sun Yat Sen University filed Critical Sun Yat Sen University
Priority to CN202211381425.2A priority Critical patent/CN115727842B/zh
Publication of CN115727842A publication Critical patent/CN115727842A/zh
Application granted granted Critical
Publication of CN115727842B publication Critical patent/CN115727842B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Navigation (AREA)

Abstract

本发明提供了一种无人机快速对准方法、系统、计算机设备及存储介质，所述方法包括：将无人机在起点处上电，并判断是否存在干扰加速度，若不存在，则采用加速度计和三轴磁强计分别对水平姿态和航向角进行对准，得到初始俯仰角、初始横滚角和初始航向角；建立失准角误差模型，并基于失准角误差模型，采用预设迭代算法对初始俯仰角、初始横滚角和初始航向角进行修正，得到修正俯仰角、修正横滚角和修正航向角；当无人机直线加速运行预设时长后，通过卫星导航进行方位对准，得到GNSS航向角；将修正航向角与GNSS航向角进行加权融合，得到融合航向角。本发明的无人机INS对准不仅精度高、耗时短，而且抗干扰能力强、鲁棒性好，还具有普适性。

Description

一种无人机快速对准方法、系统、计算机设备及存储介质
技术领域
本发明涉及无人机技术领域，特别是涉及一种无人机快速对准方法、系统、计算机设备和存储介质。
背景技术
固定翼、四旋翼、六旋翼等无人机在军事侦探、资源探测、抢险救援、摄影摄像、安全监测等领域应用广泛，发挥着巨大的作用。导航控制属于无人机研制中的关键技术，其导航精度影响着执行任务的效率甚至成败，且导航系统的失效一般将造成无人机的丢失或损坏。为了降低成本，小型无人机大多采用九轴MEMS惯性导航装置(inertialnavigationsystem，INS)和卫星定位系统(global navigation satellite system，GNSS)组合的方式进行导航，对应的导航算法通常采用互补滤波算法。MEMS-INS开始导航信息的解算前都需要进行初始对准，也就是建立东北天坐标系，其对准误差将作为导航的初始误差一直影响导航的精度，对准耗时也将影响执行任务的效率。因此，一种耗时短、精度高、鲁棒性好的对准算法对于无人机至关重要。
目前，传统的无人机对准算法主要有两类：(a)先采用加速度计进行水平姿态角(俯仰角和横滚角)的对准，再利用水平姿态角进行“数学平台”的调平，最后利用三轴磁强计进行航向角(或方位角)的对准；(b)先利用算法(a)或其类似方法进行粗对准，再用卡尔曼滤波器进行精对准；尽管上述两类方法一定程度上满足了普通无人机的对准需求，但均存在着各自的应用缺陷：算法(a)虽然简单高效，对准精度可满足大部分无人机的需求，但抗干扰能力差，阵风等因素引起的干扰加速度会影响水平姿态角的对准精度从而影响航向角的精度，周围的磁干扰会影响航向角的对准精度；算法(b)的对准精度取决于卡尔曼滤波器的收敛速度，航向角的收敛一般耗时较长，即对准耗时一般较长。显然，现有两类无人机对准算法均不能适用于一些MEMS-INS的精度高和耗时短(几十秒)的对准需求。
发明内容
本发明的目的是提供一种无人机快速对准方法，通过利用加速度阈值作为判断条件来决定何时利用加速度计进行水平姿态角的对准，并基于建立的失准角误差模型采用预设迭代算法对俯仰角、横滚角和航向角进行修正，且将修正后的航向角与利用卫星导航得到GNSS航向角进行加权融合得到融合航向角，有效解决现有无人机对准的应用缺陷，实现精准快速地无人机INS对准的同时，还具有较强的抗干扰能力，以及较好的鲁棒性与普适性。
为了实现上述目的，有必要针对上述技术问题，提供了一种无人机快速对准方法、系统、计算机设备和存储介质。
第一方面，本发明实施例提供了一种无人机快速对准方法，所述方法包括以下步骤：
将无人机在起点处上电，并判断是否存在干扰加速度，若不存在，则采用加速度计和三轴磁强计分别对水平姿态和航向角进行对准，得到初始俯仰角、初始横滚角和初始航向角；
建立失准角误差模型，并基于所述失准角误差模型，采用预设迭代算法对所述初始俯仰角、初始横滚角和初始航向角进行修正，得到修正俯仰角、修正横滚角和修正航向角；
当所述无人机直线加速运行预设时长后，通过卫星导航进行方位对准，得到GNSS航向角；
将所述修正航向角与所述GNSS航向角进行加权融合，得到融合航向角。
进一步地，所述判断是否存在干扰加速度的步骤包括：
获取加速度计输出模值与当地重力大小的差值，并判断所述差值的绝对值是否小于第一加速度阈值，若否，则判定存在加速度机动，反之，则获取水平加速度模值，并判断所述水平加速度模值是否小于第二加速度阈值；
若所述水平加速度模值小于所述第二加速度阈值，则判定存在加速度机动。
进一步地，所述采用加速度计和三轴磁强计分别对水平姿态和航向角进行对准，得到初始俯仰角、初始横滚角和初始航向角的步骤包括：
采用加速度计获取b坐标系下的比力，并根据所述比力，结合对应的比力方程，得到初始俯仰角和初始横滚角；所述初始俯仰角和初始横滚角分别表示为：
Figure BDA0003926131630000031
其中，θ和γ分别表示初始俯仰角和初始横滚角；
Figure BDA0003926131630000032
和
Figure BDA0003926131630000033
分别表示比力的X轴分量、Y轴分量和Z轴分量；arctan2表示第二类反正切函数；
采用三轴磁强计获取三轴磁场强度大小，并根据所述三轴磁场强度大小、所述初始俯仰角和初始横滚角，得到所述初始航向角；所述初始航向角表示为：
Figure BDA0003926131630000034
其中，
Figure BDA0003926131630000035
和
Figure BDA0003926131630000036
分别表示b坐标系下三轴磁强计测量到的X轴、Y轴和Z轴的磁场强度大小；β表示磁偏角大小。
进一步地，所述失准角误差模型包括俯仰角误差模型、横滚角误差模型和航向角误差模型；
所述建立失准角误差模型，并基于所述失准角误差模型，采用预设迭代算法对所述初始俯仰角、初始横滚角和初始航向角进行修正，得到修正俯仰角、修正横滚角和修正航向角的步骤包括：
根据水平姿态对准中b坐标系到n坐标系的姿态转换矩阵，得到所述俯仰角误差模型和所述横滚角误差模型；
根据所述俯仰角误差模型、所述横滚角误差模型、以及所述初始航向角的计算公式，得到所述航向角误差模型；所述航向角误差模型表示为：
Figure BDA0003926131630000041
式中，
Figure BDA0003926131630000042
Figure BDA0003926131630000043
Figure BDA0003926131630000044
其中，
Figure BDA0003926131630000045
δθ和δγ分别表示航向角误差、俯仰角误差和横滚角误差；Cij(i＝1、2、3；j＝1、2、3)表示是b坐标系到n坐标系的姿态转换矩阵的第i行第j列的元素；φE和φN分别表示东向失准角和北向失准角；
Figure BDA0003926131630000046
和
Figure BDA0003926131630000047
分别表示东向加速度计零偏和北向加速度计零偏；
Figure BDA0003926131630000048
和
Figure BDA0003926131630000049
分别表示b坐标系下的X轴、Y轴和Z轴的加速度计零偏；
根据所述姿态转换矩阵和所述失准角误差模型进行迭代计算，直至达到预设迭代次数，得到俯仰角误差预测值、横滚角误差预测值和航向角误差预测值；
根据所述俯仰角误差预测值、所述横滚角误差预测值和所述航向角误差预测值分别对所述初始俯仰角、初始横滚角和初始航向角进行修正，得到所述修正俯仰角、所述修正横滚角和所述修正航向角；所述修正俯仰角、所述修正横滚角和所述修正航向角分别表示为：
Figure BDA0003926131630000051
其中，
Figure BDA0003926131630000052
和
Figure BDA0003926131630000053
分别表示修正航向角、修正俯仰角和修正横滚角。
进一步地，所述当所述无人机直线加速运行预设时长后，通过卫星导航进行方位对准，得到GNSS航向角的步骤包括：
将所述初始俯仰角和初始横滚角代入b坐标系到n坐标系的姿态转换矩阵，并将所述姿态转换矩阵中的航向角设为0，得到h水平坐标系变换矩阵；
取无人机前进方向为惯性测量单元的前向轴，并基于所述前向轴和所述h水平坐标系变换矩阵进行短时直线加速度机动分析，得到近似比力方程；所述近似比力方程表示为：
Figure BDA0003926131630000054
式中，
Figure BDA0003926131630000055
其中，
Figure BDA0003926131630000056
表示加速度机动值；
Figure BDA0003926131630000057
表示b系到h系的变换矩阵；
Figure BDA0003926131630000058
表示b坐标系下的加速度；
Figure BDA0003926131630000059
表示水平坐标系下的比力投影；
Figure BDA00039261316300000510
表示h系到n系的变换矩阵；gn表示n坐标系下的重力矢量；
Figure BDA00039261316300000511
表示依靠全球卫星定位信息得到GNSS航向角；
将所述预设时长进行等时段划分，并计算各个时段对应邻近时刻的卫星导航速度平均变化量，得到对应的加速度机动值；
Figure BDA00039261316300000512
其中，n'表示将卫星导航测量飞行速度信息时用到的导航系，该坐标系相对于真实的n坐标系存在较小的偏差，可忽略不计；
Figure BDA00039261316300000513
和
Figure BDA00039261316300000514
分别表示n坐标系和导航系n'下的第k个时间段[tk-1,tk](k＝1,2,…,N)的加速度值；
Figure BDA00039261316300000515
和
Figure BDA00039261316300000516
分别表示导航系n'下第tk和tk-1时刻的卫星导航速度；
Figure BDA0003926131630000061
和
Figure BDA0003926131630000062
分别表示n坐标系下第tk和tk-1时刻的卫星导航速度；
在各个时段将所述近似比力方程按照X轴、Y轴和Z轴进行三轴分量展开，得到对应的X轴加速度机动方程和Y轴加速度机动方程；所述X轴加速度机动方程和Y轴加速度机动方程分别表示为：
Figure BDA0003926131630000063
式中，
Figure BDA0003926131630000064
Figure BDA0003926131630000065
其中，
Figure BDA0003926131630000066
表示载体在第k个时间段内的水平坐标系平均比力投影矢量；
Figure BDA0003926131630000067
表示载体在第k个时间段内的b坐标系下的比力矢量；
Figure BDA0003926131630000068
和
Figure BDA0003926131630000069
表示n坐标系下第k个时间段加速度机动值
Figure BDA00039261316300000610
的三轴分量；
Figure BDA00039261316300000611
和
Figure BDA00039261316300000612
表示载体在第k个时间段内的水平坐标系平均比力投影矢量的三轴分量；
Figure BDA00039261316300000613
表示第k个时间段依靠全球卫星定位信息得到GNSS航向角；
将各个时段的所述加速度机动值的X轴分量和Y轴分量分别代入对应的X轴加速度机动方程和Y轴加速度机动方程，求解得到对应的GNSS航向角；
计算各个时段的GNSS航向角的平均值，得到所述GNSS航向角。
进一步地，所述将所述修正航向角与所述GNSS航向角进行加权融合，得到融合航向角的步骤包括：
根据获取的磁强计测量噪声、磁干扰、GNSS测速误差、GNSS信号丢帧数、水平加速度和天向陀螺输出值，确定修正航向角权重和GNSS航向角权重；
根据所述修正航向角权重和所述GNSS航向角权重对所述修正航向角与所述GNSS航向角进行加权融合，得到所述融合航向角；所述融合航向角表示为：
Figure BDA0003926131630000071
其中，
Figure BDA0003926131630000072
表示融合航向角；
Figure BDA0003926131630000073
和
Figure BDA0003926131630000074
分别表示修正航向角与GNSS航向角；kmag和kGNSS分别表示修正航向角权重和GNSS航向角权重。
进一步地，所述根据获取的磁强计测量噪声、磁干扰、GNSS测速误差、GNSS信号丢帧数、水平加速度和天向陀螺输出值，确定修正航向角权重和GNSS航向角权重的步骤包括：
根据所述磁强计测量噪声和所述磁干扰，分别确定对应的噪声权重和干扰权重，并将所述噪声权重和所述干扰权重相加，得到所述修正航向角权重；所述修正航向角权重表示为：
kmag＝kmag1+kmag2
其中，kmag1、kmag2和kmag分别表示噪声权重、干扰权重和修正航向角权重；
根据所述GNSS测速误差、所述GNSS信号丢帧数、所述水平加速度和天向陀螺输出值，分别确定对应的测速误差权重、丢帧数权重、加速度权重和天向陀螺值权重，并根据所述测速误差权重、所述丢帧数权重、所述加速度权重和所述天向陀螺值权重，得到所述GNSS航向角权重；所述GNSS航向角权重表示为：
kGNSS＝0.5×(kGNSS1+kGNSS2+kGNSS3+kGNSS4)
其中，kGNSS1、kGNSS2、kGNSS3、kGNSS4和kGNSS分别表示测速误差权重、丢帧数权重、加速度权重、天向陀螺值权重和GNSS航向角权重。
第二方面，本发明实施例提供了一种无人机快速对准系统，所述系统包括：
初始对准模块，用于将无人机在起点处上电，并判断是否存在干扰加速度，若不存在，则采用加速度计和三轴磁强计分别对水平姿态和航向角进行对准，得到初始俯仰角、初始横滚角和初始航向角；
对准修正模块，用于建立失准角误差模型，并基于所述失准角误差模型，采用预设迭代算法对所述初始俯仰角、初始横滚角和初始航向角进行修正，得到修正俯仰角、修正横滚角和修正航向角；
卫星对准模块，用于当所述无人机直线加速运行预设时长后，通过卫星导航进行方位对准，得到GNSS航向角；
融合模块，用于将所述修正航向角与所述GNSS航向角进行加权融合，得到融合航向角。
第三方面，本发明实施例还提供了一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述方法的步骤。
第四方面，本发明实施例还提供一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述方法的步骤。
上述本申请提供了一种无人机快速对准方法、系统、计算机设备及存储介质，通过所述方法，实现了将无人机在起点处上电，并判断是否存在干扰加速度，若不存在，则采用加速度计和三轴磁强计分别对水平姿态和航向角进行对准得到初始俯仰角、初始横滚角和初始航向角后，基于建立的失准角误差模型，采用预设迭代算法对初始俯仰角、初始横滚角和初始航向角进行修正得到修正俯仰角、修正横滚角和修正航向角，并在无人机直线加速运行预设时长后，通过卫星导航进行方位对准得到GNSS航向角，以及将修正航向角与GNSS航向角进行加权融合，得到融合航向角的技术方案。与现有技术相比，该无人机快速对准方法，通过利用加速度阈值作为判断条件来决定何时利用加速度计进行水平姿态角的对准，并基于建立的失准角误差模型采用预设迭代算法对姿态角进行修正，以及将修正后的航向角与利用卫星导航得到GNSS航向角进行加权融合得到融合航向角，不仅实现了精准快速地无人机INS对准，而且具有较强的抗干扰能力，以及较好的鲁棒性与普适性，应用价值较高。
附图说明
图1是本发明实施例中无人机快速对准方法的流程示意图；
图2是本发明实施例中无人机快速对准系统的结构示意图；
图3是本发明实施例中计算机设备的内部结构图。
具体实施方式
为了使本申请的目的、技术方案和有益效果更加清楚明白，下面结合附图及实施例，对本发明作进一步详细说明，显然，以下所描述的实施例是本发明实施例的一部分，仅用于说明本发明，但不用来限制本发明的范围。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明提供的无人机快速对准方法为MEMS-INS/GNSS融合的快速、高精度对准算法，可应用于固定翼、四旋翼、六旋翼和直升机等无人机的有效对准：固定翼无人机系统上电后在起点处完成加速度干扰检测并采用加速度计和三轴磁强计分别对水平姿态和航向角进行初始对准和修正，再朝目标点或沿目标方向进行直线加速运动，在运动足够长时间后基于卫星导航系统获取GSNN方位角，并将其与前期得到的修正航向角进行加权融合，得到精准航向角；四旋翼、六旋翼和直升机等无人机系统上电、起飞至空中某一固定点处悬停，加速度干扰检测并采用加速度计和三轴磁强计分别对水平姿态和航向角进行初始对准和修正，再朝目标点或沿目标方向进行直线加速运动，在运动足够长时间后基于卫星导航系统获取GSNN方位角，并将其与前期得到的修正航向角进行加权融合，得到精准航向角；下述实施例将对本发明的无人机快速对准方法进行详细说明。
在一个实施例中，如图1所示，提供了一种无人机快速对准方法，包括以下步骤：
S11、将无人机在起点处上电，并判断是否存在干扰加速度，若不存在，则采用加速度计和三轴磁强计分别对水平姿态和航向角进行对准，得到初始俯仰角、初始横滚角和初始航向角；其中，干扰加速度是指在利用加速度计进行水平姿态对准时会干扰重力加速度，导致东北天导航坐标系n下的重力矢量不是常矢量的干扰项，干扰加速度较大会导致水平姿态角对准失效，因此，有必要在水平姿态角对准前进行干扰加速度监测，并对干扰加速度不符合对准条件要求的情形停止无人机使用，以确保水平姿态角的对准精度；
具体的，所述判断是否存在干扰加速度的步骤包括：
获取加速度计输出模值与当地重力大小的差值，并判断所述差值的绝对值是否小于第一加速度阈值，若否，则判定存在加速度机动，反之，则获取水平加速度模值，并判断所述水平加速度模值是否小于第二加速度阈值；其中，第一加速度阈值可理解为初步检测是否存在干扰加速的预设阈值，在获取加速度计输出模值
Figure BDA0003926131630000101
(
Figure BDA0003926131630000102
为采用加速度计测量到的b坐标系下比力)后，计算其与当地重力大小的差值的绝对值
Figure BDA0003926131630000103
是否小于第一加速度阈值Δ1，即判断是否满足
Figure BDA0003926131630000104
若满足，则初步认为不存在加速度机动(不存在干扰加速度)；需要说明的是，第一加速度阈值可根据实际应用需求设定，为了降低加速度计测量噪声的影响，一般在运载体平稳运动情况下，加速度计输出模值
Figure BDA0003926131630000105
使用加速度计在一小段时间内的平均值替代瞬时值进行判断；然而，此限制条件较为宽松，为了进一步保证干扰加速度检测的精准性，本实施例优选地在满足条件
Figure BDA0003926131630000106
的前提下，对水平加速度模值
Figure BDA0003926131630000107
作进一步判断；
若所述水平加速度模值小于所述第二加速度阈值，则判定存在加速度机动；其中，第二加速度阈值Δ2也可根据实际应用需求设定，即在满足条件
Figure BDA0003926131630000108
的前提下，当fH<Δ2时，则最终判定为不存在加速度机动(不存在干扰加速度)，可放心使用加速度计测量到的比力
Figure BDA0003926131630000109
按照加速度计对准俯仰角θ和横滚角γ，三轴磁强计对准航向角
Figure BDA0003926131630000111
的对准方法求解水平姿态角；
具体的，所述采用加速度计和三轴磁强计分别对水平姿态和航向角进行对准，得到初始俯仰角、初始横滚角和初始航向角的步骤包括：
采用加速度计获取b坐标系下的比力，并根据所述比力，结合对应的比力方程，得到初始俯仰角和初始横滚角；所述初始俯仰角和初始横滚角分别表示为：
Figure BDA0003926131630000112
其中，θ和γ分别表示初始俯仰角和初始横滚角；
Figure BDA0003926131630000113
和
Figure BDA0003926131630000114
分别表示比力的X轴分量、Y轴分量和Z轴分量；arctan2表示第二类反正切函数；
上述比力方程可理解为在静态环境下，基于运载体的线运动及其导数均为零，将一般式比力方程简化得到的下述方程：
Figure BDA0003926131630000115
其中，n坐标系是东北天导航坐标系，b坐标系是右前上载体坐标系(与惯性测量单元IMU的坐标系一致)；
Figure BDA0003926131630000116
是b系到n系的姿态转换矩阵，h系与n系之间相差一个航向角，也就是b系只经过调平就得到了h系；
Figure BDA0003926131630000117
是b系下的加速度计测量到的比力、gn是n系下的重力矢量；
需要说明的是，为了减小加速度计测量噪声和外界晃动干扰加速度的影响，常常使用某一小段时间内的平均比力结合式(1)所述的比力方程进行计算得到初始俯仰角和初始横滚角，具体过程如下：
对式(1)进行移项，再两边同时左乘
Figure BDA0003926131630000118
得到式(2)：
Figure BDA0003926131630000119
式(2)展开成分量形式，即：
Figure BDA0003926131630000121
其中，
Figure BDA0003926131630000122
Cij为
Figure BDA00039261316300001212
的第i行j列元素；gn＝[0 0 -g]T；
由式(3)，可解得:
Figure BDA0003926131630000123
式(4)表明，姿态转换矩阵
Figure BDA0003926131630000124
的第三行向量是地垂线在载体系下的投影；从式(4)只能求得姿态阵
Figure BDA0003926131630000125
中的最后一行元素C31、C32和C33，包含水平姿态角信息；
Figure BDA0003926131630000126
中的前2行在满足右手姿态阵的条件下可任意选取，姿态转换矩阵
Figure BDA0003926131630000127
如下：
Figure BDA0003926131630000128
式中，
Figure BDA0003926131630000129
是航向角、θ是俯仰角、γ是横滚角；
由式(5)可知：
Figure BDA00039261316300001210
将C32、C31和C33代入，得到：
Figure BDA00039261316300001211
式(7)在当载体无加速度，比力为重力加速度矢量在X，Y，Z轴的投影才成立，即存在干扰加速度时，式(3)右端的gn列向量就变成一个含有干扰加速度的向量，结论就不成立了，当载体存在线加速度，被包含在加速度计测量的比力中，就会导致水平姿态角误差急剧增加；通过上述前置干扰加速度检测进行过滤的方法可以有效弥补水平姿态角对准易受到干扰加速度影响的缺点，实现简单快捷完成对准的同时(理论上仅需几十ms内加速度计采样输出的几帧数据就可以完成对准)，还能保证对准精度；
采用三轴磁强计获取三轴磁场强度大小，并根据所述三轴磁场强度大小、所述初始俯仰角和初始横滚角，得到所述初始航向角；所述初始航向角表示为：
Figure BDA0003926131630000131
其中，
Figure BDA0003926131630000132
和
Figure BDA0003926131630000133
分别表示b坐标系下三轴磁强计测量到的X轴、Y轴和Z轴的磁场强度大小；arctan2表示第二类反正切函数；β表示磁偏角大小，可通过现有公开的数据库获取。
上述采用三轴磁强计进行航向角对准的原理和过程如下：
地球在其外部产生一个巨大的磁场，可视为一个磁偶极子，其两极(S极和N极)分别位于地球的地理北极和地理南极附近，地磁两极的连线称为磁轴，磁轴与地球的自转轴之间约存在11.5°的倾斜，这使得地球表面各点的地磁北向和地理北向一般不重合，该偏差角称为磁偏角；比如，我国范围内磁偏角通常情况下为2～3°，最大可达10°。
地磁场是矢量场，其强度大小为0.5～0.6Gs(Gs，高斯)，通常规定顺着磁力线方向为磁场的正方向。在某一小范围区域内，可将地磁场矢量H当作常矢量看待。
通过前述采用加速度计完成水平姿态对准，得到初始俯仰角和初始横滚角后，就可以基于磁罗盘领域的基础知识，根据磁强矢量和姿态阵关系推导出相对地理北的航向角
Figure BDA0003926131630000134
的表达式(8a)；若b坐标系经过调平，有γ≈0°和θ≈0°，则式(8a)可转换为式(8b)：
Figure BDA0003926131630000135
通过上述步骤，至此就完成了三个姿态角的初始对准，原则上经过前置干扰加速度过滤得到初始俯仰角、初始横滚角和初始航向角已在一定程度上满足了短时精度要求，但为了进一步提升对准的精准度，本实施例优选的，继续通过下述建立误差修正模型并结合迭代算法对三个姿态角进行误差修正处理；
S12、建立失准角误差模型，并基于所述失准角误差模型，采用预设迭代算法对所述初始俯仰角、初始横滚角和初始航向角进行修正，得到修正俯仰角、修正横滚角和修正航向角；其中，所述失准角误差模型包括俯仰角误差模型、横滚角误差模型和航向角误差模型；
具体的，所述建立失准角误差模型，并基于所述失准角误差模型，采用预设迭代算法对所述初始俯仰角、初始横滚角和初始航向角进行修正，得到修正俯仰角、修正横滚角和修正航向角的步骤包括：
根据水平姿态对准中b坐标系到n坐标系的姿态转换矩阵，得到所述俯仰角误差模型和所述横滚角误差模型；其中，姿态转换矩阵如式(5)所示，则基于惯导领域的基础知识，由式(5)利用三角函数和差化积公式和小角度近似公式推导易得：
Figure BDA0003926131630000141
式中，φn表示n坐标系下的失准角向量；(φn×)表示向量φn构成的反对称矩阵；
Figure BDA0003926131630000142
表示
Figure BDA0003926131630000143
的微分；
则由式(9)可得，
Figure BDA0003926131630000144
根据静态对准法的理论，可以由东向和北向加速度计的零偏计算得到：
Figure BDA0003926131630000145
式中，
Figure BDA0003926131630000151
和
Figure BDA0003926131630000152
分别为东向和北向加速度计零偏，n坐标系下东、北、天向三个加速度计的零偏计算方式如下：
Figure BDA0003926131630000153
其中，
Figure BDA0003926131630000154
δθ和δγ分别表示航向角误差、俯仰角误差和横滚角误差；Cij(i＝1、2、3；j＝1、2、3)表示是b坐标系到n坐标系的姿态转换矩阵的第i行第j列的元素；φE和φN分别表示东向失准角和北向失准角；
Figure BDA0003926131630000155
和
Figure BDA0003926131630000156
分别表示东向加速度计零偏和北向加速度计零偏；
Figure BDA0003926131630000157
和
Figure BDA0003926131630000158
分别表示b坐标系下的X轴、Y轴和Z轴的加速度计零偏，是已知量；
根据所述俯仰角误差模型、所述横滚角误差模型、以及所述初始航向角的计算公式，得到所述航向角误差模型；所述航向角误差模型表示为：
Figure BDA0003926131630000159
将式(10)-(12)代入式(13)，即可得到完整的航向角误差模型；
根据所述姿态转换矩阵和所述失准角误差模型进行迭代计算，直至达到预设迭代次数，得到俯仰角误差预测值、横滚角误差预测值和航向角误差预测值；其中，迭代计算可理解为基于考虑式(5)、(10)-(13)中计算姿态角误差时需要用到姿态转换矩阵，而该矩阵中元素又是由姿态角构成的情况，将上述公式加上(14)式所示的姿态角修正更新公式构成迭代计算俯仰角误差预测值、横滚角误差预测值和航向角误差预测值的公式，以不断地修正姿态角提高精度的计算方法；假设k表示当前迭代次数，经过k次迭代计算以后得到
Figure BDA00039261316300001510
δθ(k)和δγ(k)，可以对三个姿态角
Figure BDA00039261316300001511
θ(k)和γ(k)进行修正，得到修正后的
Figure BDA00039261316300001512
θ(k+1)和γ(k+1)：
Figure BDA0003926131630000161
其中，k表示预设迭代次数；
Figure BDA0003926131630000162
δθ(k)和δγ(k)分别表示经过k次迭代得到的航向角误差预测值、俯仰角误差预测值和横滚角误差预测值；
Figure BDA0003926131630000163
θ(k)和γ(k)分别表示初始航向角、初始俯仰角和初始横滚角；
Figure BDA0003926131630000164
θ(k+1)和γ(k+1)分别表示修正航向角、修正俯仰角、和修正横滚角。需要说明的是，预设迭代次数可根据实际应用需求设定，此处不作具体限制；
上述具体迭代过程为：由已知加速度计零偏
Figure BDA0003926131630000165
开始，依次用式(5)，(12)，(11)，(10)，(13)，(14)进行计算；然后，将k增加1，进入下一个循环：依次用式(5)，(12)，(11)，(10)，(13)，(14)进行计算；循环计算足够多次，当姿态角和姿态转换矩阵的当前值相比上一次迭代值都小于设定的小量时，迭代结束，得到最终的俯仰角误差预测值δθ、横滚角误差预测值δγ和航向角误差预测值
Figure BDA0003926131630000166
根据所述俯仰角误差预测值、所述横滚角误差预测值和所述航向角误差预测值分别对所述初始俯仰角、初始横滚角和初始航向角进行修正，得到所述修正俯仰角、所述修正横滚角和所述修正航向角；所述修正俯仰角、所述修正横滚角和所述修正航向角分别表示为：
Figure BDA0003926131630000167
其中，
Figure BDA0003926131630000168
和
Figure BDA0003926131630000169
分别表示修正航向角、修正俯仰角和修正横滚角；
本实施例通过建立失准角误差模型结合预设迭代算法对初始航向角、俯仰角和横滚角进行修正的方法，较与传统姿态角对准必然能有效提升对准精度，然而由于迭代计算过程中
Figure BDA00039261316300001610
实际的变化规律复杂且受磁干扰影响，对应的修正精度仍有进一步提升的空间；
S13、当所述无人机直线加速运行预设时长后，通过卫星导航进行方位对准，得到GNSS航向角；其中，GNSS(global navigation satellite system)方位角可理解为基于卫星导航信号得到相对地理北的航向角，可通过在水平方向上做短时直线加速度机动来实现求解：对于固定翼无人机等，其行驶速度方向一般沿载体纵轴方向(正前方)，根据行驶轨迹的航迹角或卫星测量速度矢量，容易求得载体纵轴相对于地理北向的方位角；对于多旋翼飞行器，比如四旋翼或六旋翼无人机，其飞行速度方向具有任意性，可沿能载体任意方向飞行，但是前进方向一般默认为惯性测量单元的前向轴(IMU的b系的y轴为前向轴，遥控器操控前进方向就是前向轴，与b系的y轴一致)，可根据前向轴水平方向上做短时直线加速度机动来实现求解方位角；
具体的，所述当所述无人机直线加速运行预设时长后，通过卫星导航进行方位对准，得到GNSS航向角的步骤包括：
将所述初始俯仰角和初始横滚角代入b坐标系到n坐标系的姿态转换矩阵，并将所述姿态转换矩阵中的航向角设为0，得到h水平坐标系变换矩阵；即h水平坐标系变换矩阵可理解为将(5)式中θ和γ用0近似代替(因为经过了调平，横滚和俯仰角都接近0°)得到的变换矩阵；
取无人机前进方向为惯性测量单元的前向轴，并基于所述前向轴和所述h水平坐标系变换矩阵进行短时直线加速度机动分析，得到近似比力方程；其中，近似比力方程可理解为考虑到对准时无人机速度是从0开始增大有害加速度项相应的比力很小，而将有害加速度项忽略得到的近似方程，具体表示为：
Figure BDA0003926131630000171
式中，
Figure BDA0003926131630000172
Figure BDA0003926131630000173
其中，
Figure BDA0003926131630000181
表示加速度机动值；
Figure BDA0003926131630000182
表示b系到h系的变换矩阵；
Figure BDA0003926131630000183
表示b坐标系下的比力；
Figure BDA0003926131630000184
表示水平坐标系下的比力投影；
Figure BDA0003926131630000185
表示h系到n系的变换矩阵；h系是水平坐标系，
Figure BDA0003926131630000186
与
Figure BDA0003926131630000187
中包含的三个姿态角相等，除了
Figure BDA0003926131630000188
中航向角以任意值(常用0)代入，也就是h系与n系的区别就是仅相差一个准确的航向角；gn表示n坐标系下的重力矢量；
Figure BDA0003926131630000189
表示依靠全球卫星定位信息得到GNSS航向角；
将所述预设时长进行等时段划分，并计算各个时段对应邻近时刻的卫星导航速度平均变化量，得到对应的加速度机动值；其中，加速度机动值可理解为根据加速度的定义，得到的某个时段对应的加速度值，表示为：
Figure BDA00039261316300001810
其中，n'表示将卫星导航测量飞行速度信息时用到的导航系，该坐标系相对于真实的n坐标系存在较小的偏差，可忽略不计；
Figure BDA00039261316300001811
和
Figure BDA00039261316300001812
分别表示n坐标系和导航系n'下的第k个时间段[tk-1,tk](k＝1,2,…,N)的加速度值；
Figure BDA00039261316300001813
和
Figure BDA00039261316300001814
分别表示导航系n'下第tk和tk-1时刻的卫星导航速度；
Figure BDA00039261316300001815
和
Figure BDA00039261316300001816
分别表示n坐标系下第tk和tk-1时刻的卫星导航速度；
在各个时段将所述近似比力方程按照X轴、Y轴和Z轴进行三轴分量展开，得到对应的X轴加速度机动方程和Y轴加速度机动方程；其中，X轴加速度机动方程和Y轴加速度机动方程可理解为基于较短时间段内载体方位近似保持不变的考虑，将(16a)式的矢量形式展开为对应的分量形式，且略去天向分量而仅取两个水平分量得到的方程；所述X轴加速度机动方程和Y轴加速度机动方程分别表示为：
Figure BDA00039261316300001817
式中，
Figure BDA00039261316300001818
Figure BDA0003926131630000191
其中，
Figure BDA0003926131630000192
表示载体在第k个时间段内的水平坐标系平均比力投影矢量；
Figure BDA0003926131630000193
表示载体在第k个时间段内的b坐标系下的比力矢量；
Figure BDA0003926131630000194
和
Figure BDA0003926131630000195
表示n坐标系下第k个时间段加速度机动值
Figure BDA0003926131630000196
的三轴分量；
Figure BDA0003926131630000197
和
Figure BDA0003926131630000198
表示载体在第k个时间段内的水平坐标系平均比力投影矢量的三轴分量；
Figure BDA0003926131630000199
表示第k个时间段依靠全球卫星定位信息得到GNSS航向角；
将各个时段的所述加速度机动值的X轴分量和Y轴分量分别代入对应的X轴加速度机动方程和Y轴加速度机动方程，求解得到对应的GNSS航向角；具体的，GNSS航向角基于(18)式给出的X轴加速度机动方程和Y轴加速度机动方程的求解过程如下：
先将(18)式的第一个式子两边同时乘以
Figure BDA00039261316300001910
第二个同时乘以
Figure BDA00039261316300001911
然后两式相减得(19a)式中的第一个式子；把(18)式的第一个式子两边同时乘以
Figure BDA00039261316300001912
第二个同时乘以
Figure BDA00039261316300001913
然后两式相加得(19a)式中的第二个式子，如下所示：
Figure BDA00039261316300001914
式(19a)的分母表达式
Figure BDA00039261316300001915
显示，较大的水平加速度有利于可靠地求得
Figure BDA00039261316300001916
和
Figure BDA00039261316300001917
且水平加速度机动的实质是在水平方向上提供了一个用于确定方位的观测量；由方程组(19a)很容易得到：
Figure BDA00039261316300001918
再将式(19a)和(19b)右边
Figure BDA00039261316300001919
和
Figure BDA00039261316300001920
用(15)式计算得到加速度机动值的X轴分量和Y轴分量代入，即可得到各个时间段k对应的GNSS航向角
Figure BDA00039261316300001921
为了进一步提高精度，本实施例优选地，将通过上述方法求得的N个时间段内的
Figure BDA0003926131630000201
进行平均，得到所需的GNSS航向角；即计算各个时段的GNSS航向角的平均值，得到所述GNSS航向角，表示为：
Figure BDA0003926131630000202
通过上述步骤求解GNSS航向角的过程中，由于运动轨迹为直线，不同时间段之间的
Figure BDA0003926131630000203
差值非常小，通过求平均起到平滑、剔除野值等作用，可有效保证所求GNSS航向角的精准性；基于对上述(19a)式的分析可知，较大的水平加速度有利于求解
Figure BDA0003926131630000204
和
Figure BDA0003926131630000205
本实施例还引入了一个式(21)所示的各段比力的平均量
Figure BDA0003926131630000206
根据该量的大小来衡量算法的可靠性：
Figure BDA0003926131630000207
其中，
Figure BDA0003926131630000208
越大说明求得的
Figure BDA0003926131630000209
越可靠；
S14、将所述修正航向角与所述GNSS航向角进行加权融合，得到融合航向角；其中，融合航向角可理解为将基于磁罗盘信息计算得到的航向角与基于卫星导航得到的航向角融合得到具有较高精准度的航向角，且在修正航向角与GNSS航向角对应精度确定的情况下，修正航向角与GNSS航向角对应的权重值是影响融合航向角计算的关键因素；为了保证权重值选择的合理性，本实施例优选的，基于传感器精度、周围环境、直线加速运动的参数等等分别对修正航向角权重和GNSS航向角权重进行设定；
具体的，所述将所述修正航向角与所述GNSS航向角进行加权融合，得到融合航向角的步骤包括：
根据获取的磁强计测量噪声、磁干扰、GNSS测速误差、GNSS信号丢帧数、水平加速度和天向陀螺输出值，确定修正航向角权重和GNSS航向角权重；对应的，所述根据获取的磁强计测量噪声、磁干扰、GNSS测速误差、GNSS信号丢帧数、水平加速度和天向陀螺输出值，确定修正航向角权重和GNSS航向角权重的步骤包括：
根据所述磁强计测量噪声和所述磁干扰，分别确定对应的噪声权重和干扰权重，并将所述噪声权重和所述干扰权重相加，得到所述修正航向角权重；
其中，磁强计测量噪声通过查磁罗盘的出厂检测报告得到；磁干扰强度通过统计磁强计输出的异常值的多少而设定，对应的修正航向角权重的设置原则为：磁强计测量噪声δhmag越小则权重越大；磁干扰Δmag越小则权重越大(干扰大小可用三轴磁强计测量的总磁场强度与真实地球磁场强度大小之差来衡量)；所述修正航向角权重表示为：
kmag＝kmag1+kmag2
式中，kmag1、kmag2和kmag分别表示噪声权重、干扰权重和修正航向角权重；
根据所述GNSS测速误差、所述GNSS信号丢帧数、所述水平加速度和天向陀螺输出值，分别确定对应的测速误差权重、丢帧数权重、加速度权重和天向陀螺值权重，并根据所述测速误差权重、所述丢帧数权重、所述加速度权重和所述天向陀螺值权重，得到所述GNSS航向角权重；
其中，GNSS的测速误差通过GNSS的出厂报告得到；GNSS信号的丢帧量的统计方法如下：假设GNSS接收机每100ms接收一帧GNSS信号，软件判别100ms时间到了但是没有收到GNSS信号的数据，就判定为丢帧一个，对一段时间内的丢帧数量进行统计即可；水平加速度平均值由收到的加速度计输出值经过式(21)的计算得到；天向旋转角速度由收到的IMU中的天向陀螺输出值得到(INS中的导航计算板会一直实时接收IMU中的三个陀螺和三个角速度计的测量输出值)，对应的GNSS航向角权重的设置原则为：GNSS测速误差δv越小则权重越大；GNSS数据丢帧数m越少则权重越大；水平加速度平均值
Figure BDA0003926131630000211
越大则权重越大；天向陀螺的输出值
Figure BDA0003926131630000212
越小则权重越大(该输出只看幅值、不计符号，输出越小说明轨迹的直线度越高，因为侧向风等原因造成航向角变化过大可能导致
Figure BDA0003926131630000221
求解失败)；对应的GNSS航向角权重表示为：
kGNSS＝0.5×(kGNSS1+kGNSS2+kGNSS3+kGNSS4)
式中，kGNSS1、kGNSS2、kGNSS3、kGNSS4和kGNSS分别表示测速误差权重、丢帧数权重、加速度权重、天向陀螺值权重(或天向旋转角速度)和GNSS航向角权重；
需要说明的是，GNSS航向角权重中系数0.5的选择是为了保证kGNSS的最大值与kmag的最大值保持一致；其它kmag1、kmag2、kGNSS1、kGNSS2、kGNSS3和kGNSS4的选取原则如表1和表2所示，具体取值可根据实际应用需求选择，也可根据经验和大量仿真测得得到，此处不作具体限制；
表1 kmag＝kmag1+kmag2
Figure BDA0003926131630000222
Figure BDA0003926131630000231
表2 kGNSS＝0.5×(kGNSS1+kGNSS2+kGNSS3+kGNSS4)
Figure BDA0003926131630000232
Figure BDA0003926131630000241
Figure BDA0003926131630000251
表1和2中kmag和kGNSS分别由2部分和4部分求和得到，对应的最大值一致都为2。表1和2中的阈值δ1、δ2、σ1、σ2、σ3、σ4均可根据经验选择。其中δ1和σ1的选择原则是保证kmag1＝1时的
Figure BDA0003926131630000252
和kGNSS1＝1时的
Figure BDA0003926131630000253
精度几乎相同；
根据所述修正航向角权重和所述GNSS航向角权重对所述修正航向角与所述GNSS航向角进行加权融合，得到所述融合航向角；所述融合航向角表示为：
Figure BDA0003926131630000254
其中，
Figure BDA0003926131630000255
表示融合航向角；
Figure BDA0003926131630000256
和
Figure BDA0003926131630000257
分别表示修正航向角与GNSS航向角；kmag和kGNSS分别表示修正航向角权重和GNSS航向角权重。
本实施例考虑到磁强计解算的航向角和卫星导航求解的航向角均会存在相关精度干扰因素，而基于传感器精度、周围环境、直线加速运动的参数来调整对应权重值的方式来降低各自的对准误差方式，尽可能保证融合航向角的精度：当无磁干扰或磁干扰微弱而卫星定位信号遮挡严重时，该融合算法得到的航向角精度取决于磁强计解算的航向角精度(目前主流的磁罗盘解算的水平姿态角精度约0.05°、航向角精度约0.2°)；当磁干扰严重而卫星定位信号几乎无遮挡时，该融合算法得到的航向角精度取决于卫星定位解算的航向角精度；当无磁干扰或磁干扰微弱而卫星定位信号几乎无遮挡时，该融合算法得到的航向角精度取决于两者中较高的一个；因此，该融合算法具有较强的抗干扰能力且鲁棒性较好。
需要说明的是，如果磁干扰较严重或加速度计零偏未知，也可以直接将前文得到的初始航向角代入(22)式，也可以一定程度上提高对准精度；
本实施例通过将无人机在起点处上电，并判断是否存在干扰加速度，若不存在，则采用加速度计和三轴磁强计分别对水平姿态和航向角进行对准得到初始俯仰角、初始横滚角和初始航向角后，基于建立的失准角误差模型，采用预设迭代算法对初始俯仰角、初始横滚角和初始航向角进行修正得到修正俯仰角、修正横滚角和修正航向角，并在无人机直线加速运行预设时长后，通过卫星导航进行方位对准得到GNSS航向角，以及将修正航向角与GNSS航向角进行加权融合，得到融合航向角的无人机对准方法，不仅实现了精准快速地无人机INS对准，而且具有较强的抗干扰能力，以及较好的鲁棒性与普适性，应用价值较高。
在一个实施例中，如图2所示，提供了一种无人机快速对准系统，所述系统包括：
初始对准模块1，用于将无人机在起点处上电，并判断是否存在干扰加速度，若不存在，则采用加速度计和三轴磁强计分别对水平姿态和航向角进行对准，得到初始俯仰角、初始横滚角和初始航向角；
对准修正模块2，用于建立失准角误差模型，并基于所述失准角误差模型，采用预设迭代算法对所述初始俯仰角、初始横滚角和初始航向角进行修正，得到修正俯仰角、修正横滚角和修正航向角；
卫星对准模块3，用于当所述无人机直线加速运行预设时长后，通过卫星导航进行方位对准，得到GNSS航向角；
融合模块4，用于将所述修正航向角与所述GNSS航向角进行加权融合，得到融合航向角。
关于一种无人机快速对准系统的具体限定可以参见上文中对于一种无人机快速对准方法的限定，在此不再赘述。上述一种无人机快速对准系统中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。
图3示出一个实施例中计算机设备的内部结构图，该计算机设备具体可以是终端或服务器。如图3所示，该计算机设备包括通过系统总线连接的处理器、存储器、网络接口、显示器和输入装置。其中，该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统和计算机程序。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现一种无人机快速对准方法。该计算机设备的显示屏可以是液晶显示屏或者电子墨水显示屏，该计算机设备的输入装置可以是显示屏上覆盖的触摸层，也可以是计算机设备外壳上设置的按键、轨迹球或触控板，还可以是外接的键盘、触控板或鼠标等。
本领域普通技术人员可以理解，图3中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备的限定，具体的计算设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有同的部件布置。
在一个实施例中，提供了一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行计算机程序时实现上述方法的步骤。
在一个实施例中，提供了一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现上述方法的步骤。
综上，本发明实施例提供的一种无人机快速对准方法、系统、计算机设备及存储介质，其无人机快速对准方法实现了将无人机在起点处上电，并判断是否存在干扰加速度，若不存在，则采用加速度计和三轴磁强计分别对水平姿态和航向角进行对准得到初始俯仰角、初始横滚角和初始航向角后，基于建立的失准角误差模型，采用预设迭代算法对初始俯仰角、初始横滚角和初始航向角进行修正得到修正俯仰角、修正横滚角和修正航向角，并在无人机直线加速运行预设时长后，通过卫星导航进行方位对准得到GNSS航向角，以及将修正航向角与GNSS航向角进行加权融合，得到融合航向角的技术方案，该方法通过利用加速度阈值作为判断条件来决定何时利用加速度计进行水平姿态角的对准，并基于建立的失准角误差模型采用预设迭代算法对俯仰角、横滚角和航向角进行修正，且将修正后的航向角与利用卫星导航得到GNSS航向角进行加权融合得到融合航向角，有效解决现有无人机对准的应用缺陷，实现精准快速地无人机INS对准的同时，还具有较强的抗干扰能力，以及较好的鲁棒性与普适性。
本说明书中的各个实施例均采用递进的方式描述，各个实施例直接相同或相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于系统实施例而言，由于其基本相似于方法实施例，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。需要说明的是，上述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种优选实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和替换，这些改进和替换也应视为本申请的保护范围。因此，本申请专利的保护范围应以所述权利要求的保护范围为准。

Claims (10)

1.一种无人机快速对准方法，其特征在于，所述方法包括以下步骤：
将无人机在起点处上电，并判断是否存在干扰加速度，若不存在，则采用加速度计和三轴磁强计分别对水平姿态和航向角进行对准，得到初始俯仰角、初始横滚角和初始航向角；
建立失准角误差模型，并基于所述失准角误差模型，采用预设迭代算法对所述初始俯仰角、初始横滚角和初始航向角进行修正，得到修正俯仰角、修正横滚角和修正航向角；
当所述无人机直线加速运行预设时长后，通过卫星导航进行方位对准，得到GNSS航向角；
将所述修正航向角与所述GNSS航向角进行加权融合，得到融合航向角。
2.如权利要求1所述的无人机快速对准方法，其特征在于，所述判断是否存在干扰加速度的步骤包括：
获取加速度计输出模值与当地重力大小的差值，并判断所述差值的绝对值是否小于第一加速度阈值，若否，则判定存在加速度机动，反之，则获取水平加速度模值，并判断所述水平加速度模值是否小于第二加速度阈值；
若所述水平加速度模值小于所述第二加速度阈值，则判定存在加速度机动。
3.如权利要求1所述的无人机快速对准方法，其特征在于，所述采用加速度计和三轴磁强计分别对水平姿态和航向角进行对准，得到初始俯仰角、初始横滚角和初始航向角的步骤包括：
采用加速度计获取b坐标系下的比力，并根据所述比力，结合对应的比力方程，得到初始俯仰角和初始横滚角；所述初始俯仰角和初始横滚角分别表示为：
Figure FDA0003926131620000021
其中，θ和γ分别表示初始俯仰角和初始横滚角；
Figure FDA0003926131620000022
和
Figure FDA0003926131620000023
分别表示比力的X轴分量、Y轴分量和Z轴分量；arctan2表示第二类反正切函数；
采用三轴磁强计获取三轴磁场强度大小，并根据所述三轴磁场强度大小、所述初始俯仰角和初始横滚角，得到所述初始航向角；所述初始航向角表示为：
Figure FDA0003926131620000024
其中，
Figure FDA0003926131620000025
和
Figure FDA0003926131620000026
分别表示b坐标系下三轴磁强计测量到的X轴、Y轴和Z轴的磁场强度大小；β表示磁偏角大小。
4.如权利要求3所述的无人机快速对准方法，其特征在于，所述失准角误差模型包括俯仰角误差模型、横滚角误差模型和航向角误差模型；
所述建立失准角误差模型，并基于所述失准角误差模型，采用预设迭代算法对所述初始俯仰角、初始横滚角和初始航向角进行修正，得到修正俯仰角、修正横滚角和修正航向角的步骤包括：
根据水平姿态对准中b坐标系到n坐标系的姿态转换矩阵，得到所述俯仰角误差模型和所述横滚角误差模型；
根据所述俯仰角误差模型、所述横滚角误差模型、以及所述初始航向角的计算公式，得到所述航向角误差模型；所述航向角误差模型表示为：
Figure FDA0003926131620000027
式中，
Figure FDA0003926131620000031
Figure FDA0003926131620000032
Figure FDA0003926131620000033
其中，
Figure FDA0003926131620000034
δθ和δγ分别表示航向角误差、俯仰角误差和横滚角误差；Cij(i＝1,2,3；j＝1,2,3)表示是b坐标系到n坐标系的姿态转换矩阵的第i行第j列的元素；φE和φN分别表示东向失准角和北向失准角；
Figure FDA0003926131620000035
和
Figure FDA0003926131620000036
分别表示东向加速度计零偏和北向加速度计零偏；
Figure FDA0003926131620000037
和
Figure FDA0003926131620000038
分别表示b坐标系下的X轴、Y轴和Z轴的加速度计零偏；
根据所述姿态转换矩阵和所述失准角误差模型进行迭代计算，直至达到预设迭代次数，得到俯仰角误差预测值、横滚角误差预测值和航向角误差预测值；
根据所述俯仰角误差预测值、所述横滚角误差预测值和所述航向角误差预测值分别对所述初始俯仰角、初始横滚角和初始航向角进行修正，得到所述修正俯仰角、所述修正横滚角和所述修正航向角；所述修正俯仰角、所述修正横滚角和所述修正航向角分别表示为：
Figure FDA0003926131620000039
其中，
Figure FDA00039261316200000310
和
Figure FDA00039261316200000311
分别表示修正航向角、修正俯仰角和修正横滚角。
5.如权利要求1所述的无人机快速对准方法，其特征在于，所述当所述无人机直线加速运行预设时长后，通过卫星导航进行方位对准，得到GNSS航向角的步骤包括：
将所述初始俯仰角和初始横滚角代入b坐标系到n坐标系的姿态转换矩阵，并将所述姿态转换矩阵中的航向角设为0，得到h水平坐标系变换矩阵；
取无人机前进方向为惯性测量单元的前向轴，并基于所述前向轴和所述h水平坐标系变换矩阵进行短时直线加速度机动分析，得到近似比力方程；所述近似比力方程表示为：
Figure FDA0003926131620000041
式中，
Figure FDA0003926131620000042
其中，
Figure FDA0003926131620000043
表示加速度机动值；
Figure FDA0003926131620000044
表示b系到h系的变换矩阵；
Figure FDA0003926131620000045
表示b坐标系下的加速度；
Figure FDA0003926131620000046
表示水平坐标系下的比力投影；
Figure FDA0003926131620000047
表示h系到n系的变换矩阵；gn表示n坐标系下的重力矢量；
Figure FDA0003926131620000048
表示依靠全球卫星定位信息得到GNSS航向角；
将所述预设时长进行等时段划分，并计算各个时段对应邻近时刻的卫星导航速度平均变化量，得到对应的加速度机动值；
Figure FDA0003926131620000049
其中，n'表示将卫星导航测量飞行速度信息时用到的导航系，该坐标系相对于真实的n坐标系存在较小的偏差，可忽略不计；
Figure FDA00039261316200000410
和
Figure FDA00039261316200000411
分别表示n坐标系和导航系n'下的第k个时间段[tk-1,tk](k＝1,2,…,N)的加速度值；
Figure FDA00039261316200000412
和
Figure FDA00039261316200000413
分别表示导航系n'下第tk和tk-1时刻的卫星导航速度；
Figure FDA00039261316200000414
和
Figure FDA00039261316200000415
分别表示n坐标系下第tk和tk-1时刻的卫星导航速度；
在各个时段将所述近似比力方程按照X轴、Y轴和Z轴进行三轴分量展开，得到对应的X轴加速度机动方程和Y轴加速度机动方程；所述X轴加速度机动方程和Y轴加速度机动方程分别表示为：
Figure FDA00039261316200000416
式中，
Figure FDA0003926131620000051
Figure FDA0003926131620000052
其中，
Figure FDA0003926131620000053
表示载体在第k个时间段内的水平坐标系平均比力投影矢量；
Figure FDA0003926131620000054
表示载体在第k个时间段内的b坐标系下的比力矢量；
Figure FDA0003926131620000055
和
Figure FDA0003926131620000056
表示n坐标系下第k个时间段加速度机动值
Figure FDA0003926131620000057
的三轴分量；
Figure FDA0003926131620000058
和
Figure FDA0003926131620000059
表示载体在第k个时间段内的水平坐标系平均比力投影矢量的三轴分量；
Figure FDA00039261316200000510
表示第k个时间段依靠全球卫星定位信息得到GNSS航向角；
计算各个时段的GNSS航向角的平均值，得到所述GNSS航向角。
6.如权利要求1所述的无人机快速对准方法，其特征在于，所述将所述修正航向角与所述GNSS航向角进行加权融合，得到融合航向角的步骤包括：
根据获取的磁强计测量噪声、磁干扰、GNSS测速误差、GNSS信号丢帧数、水平加速度和天向陀螺输出值，确定修正航向角权重和GNSS航向角权重；
根据所述修正航向角权重和所述GNSS航向角权重对所述修正航向角与所述GNSS航向角进行加权融合，得到所述融合航向角；所述融合航向角表示为：
Figure FDA00039261316200000511
其中，
Figure FDA00039261316200000512
表示融合航向角；
Figure FDA00039261316200000513
和
Figure FDA00039261316200000514
分别表示修正航向角与GNSS航向角；kmag和kGNSS分别表示修正航向角权重和GNSS航向角权重。
7.如权利要求6所述的无人机快速对准方法，其特征在于，所述根据获取的磁强计测量噪声、磁干扰、GNSS测速误差、GNSS信号丢帧数、水平加速度和天向陀螺输出值，确定修正航向角权重和GNSS航向角权重的步骤包括：
根据所述磁强计测量噪声和所述磁干扰，分别确定对应的噪声权重和干扰权重，并将所述噪声权重和所述干扰权重相加，得到所述修正航向角权重；所述修正航向角权重表示为：
kmag＝kmag1+kmag2
其中，kmag1、kmag2和kmag分别表示噪声权重、干扰权重和修正航向角权重；
根据所述GNSS测速误差、所述GNSS信号丢帧数、所述水平加速度和天向陀螺输出值，分别确定对应的测速误差权重、丢帧数权重、加速度权重和天向陀螺值权重，并根据所述测速误差权重、所述丢帧数权重、所述加速度权重和所述天向陀螺值权重，得到所述GNSS航向角权重；所述GNSS航向角权重表示为：
kGNSS＝0.5×(kGNSS1+kGNSS2+kGNSS3+kGNSS4)
其中，kGNSS1、kGNSS2、kGNSS3、kGNSS4和kGNSS分别表示测速误差权重、丢帧数权重、加速度权重、天向陀螺值权重和GNSS航向角权重。
8.一种无人机快速对准系统，其特征在于，所述系统包括：
初始对准模块，用于将无人机在起点处上电，并判断是否存在干扰加速度，若不存在，则采用加速度计和三轴磁强计分别对水平姿态和航向角进行对准，得到初始俯仰角、初始横滚角和初始航向角；
对准修正模块，用于建立失准角误差模型，并基于所述失准角误差模型，采用预设迭代算法对所述初始俯仰角、初始横滚角和初始航向角进行修正，得到修正俯仰角、修正横滚角和修正航向角；
卫星对准模块，用于当所述无人机直线加速运行预设时长后，通过卫星导航进行方位对准，得到GNSS航向角；
融合模块，用于将所述修正航向角与所述GNSS航向角进行加权融合，得到融合航向角。
9.一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至7中任一所述方法的步骤。
10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至7中任一所述方法的步骤。
CN202211381425.2A 2022-11-04 2022-11-04 一种无人机快速对准方法、系统、计算机设备及存储介质 Active CN115727842B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211381425.2A CN115727842B (zh) 2022-11-04 2022-11-04 一种无人机快速对准方法、系统、计算机设备及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211381425.2A CN115727842B (zh) 2022-11-04 2022-11-04 一种无人机快速对准方法、系统、计算机设备及存储介质

Publications (2)

Publication Number Publication Date
CN115727842A true CN115727842A (zh) 2023-03-03
CN115727842B CN115727842B (zh) 2024-07-05

Family

ID=85294641

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211381425.2A Active CN115727842B (zh) 2022-11-04 2022-11-04 一种无人机快速对准方法、系统、计算机设备及存储介质

Country Status (1)

Country Link
CN (1) CN115727842B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117029760A (zh) * 2023-10-07 2023-11-10 深圳深海创新技术有限公司 无人机航向确定方法、电子设备及存储介质

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111024070A (zh) * 2019-12-23 2020-04-17 哈尔滨工程大学 一种基于航向自观测的惯性足绑式行人定位方法
WO2020087845A1 (zh) * 2018-10-30 2020-05-07 东南大学 基于gpr与改进的srckf的sins初始对准方法
CN113959462A (zh) * 2021-10-21 2022-01-21 北京机电工程研究所 一种基于四元数的惯性导航系统自对准方法
CN114964222A (zh) * 2022-04-15 2022-08-30 广州市中海达测绘仪器有限公司 一种车载imu姿态初始化方法、安装角估计方法及装置

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2020087845A1 (zh) * 2018-10-30 2020-05-07 东南大学 基于gpr与改进的srckf的sins初始对准方法
CN111024070A (zh) * 2019-12-23 2020-04-17 哈尔滨工程大学 一种基于航向自观测的惯性足绑式行人定位方法
CN113959462A (zh) * 2021-10-21 2022-01-21 北京机电工程研究所 一种基于四元数的惯性导航系统自对准方法
CN114964222A (zh) * 2022-04-15 2022-08-30 广州市中海达测绘仪器有限公司 一种车载imu姿态初始化方法、安装角估计方法及装置

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117029760A (zh) * 2023-10-07 2023-11-10 深圳深海创新技术有限公司 无人机航向确定方法、电子设备及存储介质
CN117029760B (zh) * 2023-10-07 2023-12-15 深圳深海创新技术有限公司 无人机航向确定方法、电子设备及存储介质

Also Published As

Publication number Publication date
CN115727842B (zh) 2024-07-05

Similar Documents

Publication Publication Date Title
CN111121773B (zh) 一种mems惯性测量组合
CN105929836B (zh) 用于四旋翼飞行器的控制方法
CN110488865B (zh) 无人机航向确定方法、装置及无人机
CN110377056B (zh) 无人机航向角初值选取方法及无人机
WO2021078002A1 (zh) 无人机航向角初值选取方法、装置及无人机
Di et al. A data fusion system for attitude estimation of low-cost miniature UAVs
CN110873563B (zh) 一种云台姿态估计方法及装置
CN103712598A (zh) 一种小型无人机姿态确定系统与确定方法
CN112066985A (zh) 一种组合导航系统初始化方法、装置、介质及电子设备
CN110736457A (zh) 一种基于北斗、gps和sins的组合导航方法
CN115727842B (zh) 一种无人机快速对准方法、系统、计算机设备及存储介质
Yang et al. Multi-sensor data fusion for UAV navigation during landing operations
Weibel et al. Small unmanned aerial system attitude estimation for flight in wind
CN110375773B (zh) Mems惯导系统姿态初始化方法
Blachuta et al. Attitude and heading reference system based on 3D complementary filter
Eldredge Improved state estimation for miniature air vehicles
CN111141285A (zh) 一种航空重力测量装置
Yang et al. Model-free integrated navigation of small fixed-wing UAVs full state estimation in wind disturbance
CN105890626A (zh) 一种抗磁干扰的方法
Oshman et al. Mini-UAV altitude estimation using an inertially stabilized payload
CN114877881A (zh) 一种无人机航向角测量数据的融合方法及融合系统
Tehrani et al. Gyroscope offset estimation using panoramic vision-based attitude estimation and Extended Kalman Filter
Marmion Airborne attitude estimation using a kalman filter
Mung et al. Target State Estimation for UAV's Target Tracking and Precision Landing Control: Algorithm and Verification System
Huang et al. Integration of MEMS inertial sensor-based GNC of a UAV

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant