CN104503466B - 一种微小型无人机导航装置 - Google Patents
一种微小型无人机导航装置 Download PDFInfo
- Publication number
- CN104503466B CN104503466B CN201510004820.2A CN201510004820A CN104503466B CN 104503466 B CN104503466 B CN 104503466B CN 201510004820 A CN201510004820 A CN 201510004820A CN 104503466 B CN104503466 B CN 104503466B
- Authority
- CN
- China
- Prior art keywords
- mrow
- msub
- mtd
- mtr
- navigation
- 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.)
- Expired - Fee Related
Links
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Navigation (AREA)
Abstract
本发明提出了一种微小型无人机导航装置,包括:GNSS导航模块、IMU导航模块、地磁导航模块、姿态融合及导航控制模块和外壳,姿态融合及导航控制模块用于接收多个数据,并进行数据验证以判断GNSS导航模块、IMU导航模块和地磁导航模块是否正常,如果正常,则计算无人机的第一俯仰角和第一航向角、第二俯仰角和第一横滚角,第二航向角和第二横滚角,以预设融合算法进行姿态融合,生成无人机的位置数据,判断无人机的位置数据是否位于预设安全范围内,如果否,则控制无人机调整飞行状态直至位于预设安全范围内。本发明采用低成本传感器,可以降低整个装置的生产成本,实现跨平台的导航方法,可适用不同平台的无人机。
Description
技术领域
本发明涉及无人机技术领域,特别涉及一种微小型无人机导航装置。
背景技术
无人机具有独有的低成本、低损耗、零伤亡、可重复使用和高机动等诸多优势,其使用范围已拓宽到军事、民用和科学研究三大领域。在军事上可用于侦察、监视、攻击、目标模拟、早期预警等;在民用上,可用于大地测量、气象观测、城市环境检测、地球资源勘探和森林防火等;在科学研究上,可用于大气研究,对核生化污染区的取样与监控、新技术新设备与新飞行器的试验验证等。
目前的无人机导航方法包括以下几种:地标导航法、航位推算导航法、无线电导航法、惯性导航法、图像匹配导航法、天文导航法、卫星导航法、组合导航法,存在以下缺陷:
1、现有装置体积较大,不适合微小型无人机特别是微型无人机的使用;
2、现有装置采用的传感器价格较贵,不适合微小型无人机特别是微型无人机的使用;
3、现有组合导航算法计算方法复杂,计算周期较长。
发明内容
本发明的目的旨在至少解决所述技术缺陷之一。
为此,本发明的目的在于提出一种微小型无人机导航装置。
为了实现上述目的,本发明的实施例提供一种微小型无人机导航装置,包括:GNSS导航模块,用于测量无人机的天向方向速度V天、水平方向速度V水平、正北方向速度V北和正东方向速度V东;IMU导航模块,用于测量所述无人机三维方向上的加速度值Ax、Ay和Az;地磁导航模块,用于测量所述无人机在水平面的三维方向上的磁场强度MXh、MYh、MZh和垂直面上的三维方向上的磁场强度MXv、MYv、MZv;姿态融合及导航控制模块,用于接收来自所述GNSS导航模块、IMU导航模块和地磁导航模块的多个数据,并进行数据验证以判断所述GNSS导航模块、IMU导航模块和地磁导航模块是否正常,如果正常,则根据所述天向方向速度V天、水平方向速度V水平、正北方向速度V北和正东方向速度V东计算所述无人机的第一俯仰角PGNSS和第一航向角HGNSS,根据加速度值Ax、Ay和Az计算所述无人机的第二俯仰角PIMU和第一横滚角RIMU,根据MXh、MYh、MZh、MXv、MYv、MZv计算所述无人机的第二航向角HEC和第二横滚角REC,以及对所述第一俯仰角PGNSS、第一航向角HGNSS、第二俯仰角PIMU、第一横滚角RIMU、第二航向角HEC和第二横滚角REC以预设融合算法进行姿态融合,生成所述无人机的位置数据,并判断所述无人机的位置数据是否位于预设安全范围内,如果否,则控制所述无人机调整飞行状态直至位于预设安全范围内;外壳,所述GNSS导航模块、IMU导航模块、地磁导航模块和姿态融合及导航控制模块均位于所述外壳内。
在本发明的一个实施例中,所述GNSS导航模块为GPS/GLONASS双频接收机。
在本发明的一个实施例中,所述GNSS导航模块为还包括内置天线。
在本发明的一个实施例中,所述IMU导航模块包括:加速度计传感器,用于测量所述无人机三维方向上的加速度模拟信号;第一至第三陀螺仪,用于测量所述无人机三维方向上的角速度模拟信号;模数AD转换模块,所述模数AD转换模块连接至所述加速度计传感器和第一至第三陀螺仪,用于将加速度模拟信号和角速度模拟信号转换为对应的加速度值Ax、Ay、Az和角速度值ωx、ωy、ωz。
在本发明的一个实施例中,所述地磁导航模块包括:地磁传感器,其中,
其中,MX、MY、MZ分别为X轴、Y轴和Z轴地磁传感器的输出值,MXh、MYh、MZh为地磁传感器的输出值在水平面的投影,MXv、MYv、MZv为地磁传感器的输出值在垂直面的投影。
在本发明的一个实施例中,所述姿态融合及导航控制模块计算所述无人机的第一俯仰角PGNSS=asin(V天/V水平),第一航向角HGNSS=atan(V北/V东);
所述姿态融合及导航控制模块计算所述无人机的第二俯仰角PIMU=asin(Ay/g),第一横滚角RIMU=atan2(Ax,Az),其中,g为重力加速度;
所述姿态融合及导航控制模块计算所述无人机的第二航向角HEC=atan2(-MXh,MYh)-H0,第二横滚角REC=atan2(MXv,-MZv),其中,H0为偏磁角。
在本发明的一个实施例中,所述姿态融合及导航控制模块以预设融合算法进行姿态融合,包括:P=PIMU+K×(PIMU-PGNSS),R=RIMU+K×(RIMU-REC),H=HEC+K×(HEC-HGNSS),其中,P为姿态融合后的无人机的俯仰角,R为姿态融合后的无人机的横滚角,H为姿态融合后的无人机的航向角,K为预设系数。
在本发明的一个实施例中,所述姿态融合及导航模块还用于在经过数据验证判断所述GNSS导航模块、IMU导航模块和地磁导航模块异常时,进一步对所述GNSS导航模块、IMU导航模块和地磁导航模块进行传感器可用性判断,包括如下步骤:
判断所述天向方向速度V天、水平方向速度V水平、正北方向速度V北、正东方向速度V东以及水平面的三维方向上的磁场强度MXh、MYh、MZh和垂直面上的三维方向上的磁场强度MXv、MYv、MZv、加速度值Ax、Ay、Az和角速度值ωx、ωy、ωz是否正确;
如果正确,则进行容错计算,调整所述GNSS导航模块、IMU导航模块和地磁导航模块的数据至容错范围内,然后将容错计算后的数据以预设融合算法进行姿态融合;
如果不正确,则判断GNSS导航模块、IMU导航模块和地磁导航模块均不可用,发出开伞指令以弹出降落伞。
在本发明的一个实施例中,本发明的微小型无人机导航装置还包括:RS422接口,所述RS422接口位于所述外壳上,用于与其他设备进行通信。
在本发明的一个实施例中,本发明的微小型无人机导航装置还包括:还包括:电源及控制接口,用于接入电源电压。
根据本发明实施例的微小型无人机导航装置,将GNSS导航模块、IMU导航模块和地磁导航模块进行融合,计算位置、速度、姿态的第一次组合,利用第一次组合计算出的俯仰角、横滚角与地磁进行数据融合,修正姿态角,使用航迹规划,防止无人机飞越禁飞区域,可以确保无人机执行任务时能够安全完成飞行。本发明采用低成本传感器,可以降低整个装置的生产成本,不依赖于某个传感器的、可靠的导航方法,可以实现跨平台的导航方法,可适用不同平台的无人机。并且,本发明的导航装置还提供一种失控情况下的补救措施,即加装降落伞,当系统探测到无人机失控时,打开降落伞,让其安全降落,防止伤及地面人员、设备、建筑物及无人机自身。
本发明附加的方面和优点将在下面的描述中部分给出,部分将从下面的描述中变得明显,或通过本发明的实践了解到。
附图说明
本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解,其中:
图1为根据本发明实施例的微小型无人机导航装置的结构框图;
图2为根据本发明实施例的微小型无人机导航装置的工作流程图;
图3为根据本发明实施例的微小型无人机导航装置的传感器可用性判断流程图;
图4为根据本发明实施例的微小型无人机导航装置的示意图。
具体实施方式
下面详细描述本发明的实施例,所述实施例的示例在附图中示出,其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的,旨在用于解释本发明,而不能理解为对本发明的限制。
如图1所示,本发明实施例的微小型无人机导航装置,包括:GNSS导航模块1、IMU导航模块2、地磁导航模块3、姿态融合及导航控制模块4和外壳。其中,GNSS导航模块1、IMU导航模块2、地磁导航模块3和姿态融合及导航控制模块4均位于外壳内。
首先,定义无人机的导航坐标系如下:设北天坐标系(ENU)为导航坐标系(n系),机体坐标系(b系)为OXbYbZb;其中OYb轴沿着飞行方向,OXb轴向左,OZb轴成右手定则。
具体地,GNSS导航模块1用于测量无人机的天向方向速度V天、水平方向速度V水平、正北方向速度V北和正东方向速度V东。
在本发明的一个实施例中,GNSS导航模块1为GPS/GLONASS双频接收机。例如,采用U-BLOX CAM-M8Q型号的GPS/GLONASS双频接收机,该接收机定位精度达到2.5m,且具有内置天线,从而可以在结构上节省一个天线的体积。
IMU导航模块2用于测量无人机三维方向上的加速度值Ax、Ay和Az。
具体地,IMU导航模块2包括:加速度计传感器、第一至第三陀螺仪和模数AD转换模块。其中,加速度计传感器用于测量无人机三维方向上的加速度模拟信号。第一至第三陀螺仪分别用于测量无人机三维方向上的角速度模拟信号。模数AD转换模块连接至加速度计传感器和第一至第三陀螺仪,用于将加速度模拟信号和角速度模拟信号转换为对应的加速度值Ax、Ay、Az和角速度值ωx、ωy、ωz。
在本发明的示例中,加速度计传感器可以采用型号为ADXL325的加速度计传感器。第一至第三陀螺仪可以分别采用型号为ADXRS620的陀螺仪。模数AD转换模块为高精度AD模块。
地磁导航模块3用于测量无人机在水平面的三维方向上的磁场强度MXh、MYh、MZh、垂直面上的三维方向上的磁场强度MXv、MYv、MZv。
地磁导航模块3包括:地磁传感器,其中,
其中,MX、MY、MZ分别为X轴、Y轴和Z轴地磁传感器的输出值,单位mG,MXh、MYh、MZh为地磁传感器的输出值在水平面的投影,MXv、MYv、MZv为地磁传感器的输出值在垂直面的投影,单位mG。
在本发明的一个实施例中,地磁传感器可以分别采用型号为HMC1043的三轴磁阻传感器,该型号的传感器体积较小。
姿态融合及导航控制模块4用于接收来自GNSS导航模块1、IMU导航模块2和地磁导航模块3的多个数据,并进行数据验证以判断GNSS导航模块1、IMU导航模块2和地磁导航模块3是否正常。
如果姿态融合及导航控制模块4判断GNSS导航模块1、IMU导航模块2和地磁导航模块3正常,则根据GNSS导航模块1测定的天向方向速度V天、水平方向速度V水平、正北方向速度V北和、正东方向速度V东计算无人机的第一俯仰角PGNSS和第一航向角HGNSS。
PGNSS=asin(V天/V水平), (1)
HGNSS=atan(V北/V东), (2)
姿态融合及导航控制模块4根据IMU导航模块2测量的加速度值Ax、Ay和Az计算无人机的第二俯仰角PIMU和第一横滚角RIMU。
PIMU=asin(Ay/g), (3)
RIMU=atan2(Ax,Az), (4)
其中,g为重力加速度,Ax、Ay、Az分别为X、Y、Z轴加速度计的输出值,单位m/s2。
姿态融合及导航控制模块4根据地磁导航模块3测量的MXh、MYh、MZh、MXv、MYv、MZv计算无人机的第二航向角HEC和第二横滚角REC。
HEC=atan2(-MXh,MYh)-H0, (5)
REC=atan2(MXv,-MZv), (6)
其中,H0为偏磁角,MXh、MYh、MZh为地磁传感器在水平面的投影,MXv、MYv、MZv为地磁传感器在垂直面的投影,单位mG。
其中,上述(1)~(6)可以理解为对测量的数据的第一次组合。
姿态融合及导航控制模块4对计算出的第一俯仰角PGNSS、第一航向角HGNSS、第二俯仰角PIMU、第一横滚角RIMU、第二航向角HEC和第二横滚角REC以预设融合算法进行姿态融合,生成无人机的位置数据(P,R,H)。
P=PIMU+K×(PIMU-PGNSS), (7)
R=RIMU+K×(RIMU-REC), (8)
H=HEC+K×(HEC-HGNSS), (9)
其中,P为姿态融合后的无人机的俯仰角,R为姿态融合后的无人机的横滚角,H为姿态融合后的无人机的航向角,K为预设系数,取值范围:0.4~0.9。
然后,姿态融合及导航控制模块4判断无人机的位置数据是否位于预设安全范围内,如果否,则控制无人机调整飞行状态直至位于预设安全范围内。
具体来说,预设安全范围是根据无人机飞行任务的航迹规划设置,无人机只能在该预设安全范围内飞行。即,将无人机的位置数据(P,R,H)与航迹规划的预设安全范围进行比对,超出该预设安全范围时,则根据控制算法控制无人机将自动调整航向,直至位于预设安全范围内。
如果姿态融合及导航控制模块4判断GNSS导航模块1、IMU导航模块2和地磁导航模块3异常,进一步对所述GNSS导航模块、IMU导航模块和地磁导航模块进行传感器可用性判断,包括如下步骤:首先,判断天向方向速度V天、水平方向速度V水平、正北方向速度V北和正东方向速度V东以及水平面的三维方向上的磁场强度MXh、MYh、MZh和垂直面上的三维方向上的磁场强度MXv、MYv、MZv、加速度值Ax、Ay和Az和角速度值ωx、ωy、ωz是否正确。
如果正确,则姿态融合及导航控制模块4进行容错计算,调整GNSS导航模块、IMU导航模块和地磁导航模块的数据至容错范围内,然后将容错计算后的数据以预设融合算法进行姿态融合。
在本发明的实施例中,容错计算算法如下:P=PGNSS,R=REC,H=HEC+K×(HEC-HGNSS),K为预设系数,取值范围:0.4~0.9。
如果不正确,则姿态融合及导航控制模块4判断GNSS导航模块1、IMU导航模块2和地磁导航模块3均不可用,无人机处于失控状态,进行失控报警,发出开伞指令以弹出降落伞。无人机执行任务时将不可避免的发生意外导致失控,一旦发生失控现象,本发明的微小型无人机导航装置发出弹出降落伞指令,可以最大限度地降低无人机失控所带来的地面人员、设备、建筑物以及无人机自身的损失。
图2为根据本发明实施例的微小型无人机导航装置的工作流程图。其中,下述步骤均由姿态融合及导航控制模块4执行。
步骤S1,读取传感器数据。
其中,传感器数据包括GNSS导航模块1测定的天向方向速度V天、水平方向速度V水平、正北方向速度V北、正东方向速度V东,IMU导航模块2测量的加速度值Ax、Ay、Az,以及角速度值ωx、ωy、ωz,地磁导航模块3测量的MXh、MYh、MZh、MXv、MYv、MZv。
步骤S2,数据验证。
步骤S3,判断数据是否正常,如果是,则执行步骤S4,否则执行步骤S9。
步骤S4,计算无人机的速度和位置。
步骤S5,计算加速度计姿态。
根据IMU导航模块2测量的加速度值Ax、Ay和Az计算无人机的第二俯仰角PIMU和第一横滚角RIMU。
PIMU=asin(Ay/g), (3)
RIMU=atan2(Ax,Az), (4)
步骤S6,计算GNSS姿态。
根据GNSS导航模块1测定的天向方向速度V天、水平方向速度V水平、正北方向速度V北和正东方向速度V东计算无人机的第一俯仰角PGNSS和第一航向角HGNSS。
PGNSS=asin(V天/V水平), (1)
HGNSS=atan(V北/V东), (2)
步骤S7,计算地磁姿态。
根据地磁导航模块3测量的MXh、MYh、MZh、MXv、MYv、MZv计算无人机的第二航向角HEC和第二横滚角REC。
HEC=atan2(-MXh,MYh)-H0, (5)
REC=atan2(MXv,-MZv), (6)
步骤S8,姿态融合,然后执行步骤S13。
对计算出的第一俯仰角PGNSS、第一航向角HGNSS、第二俯仰角PIMU、第一横滚角RIMU、第二航向角HEC和第二横滚角REC以预设融合算法进行姿态融合,生成无人机的位置数据(P,R,H)。
P=PIMU+K×(PIMU-PGNSS), (7)
R=RIMU+K×(RIMU-REC), (8)
H=HEC+K×(HEC-HGNSS), (9)
其中,P为姿态融合后的无人机的俯仰角,R为姿态融合后的无人机的横滚角,H为姿态融合后的无人机的航向角,K为预设系数,取值范围:0.4~0.9。
步骤S9,判断传感器的可用性。
图3为根据本发明实施例的微小型无人机导航装置的传感器可用性判断流程图。
步骤S21,读取传感器数据。
步骤S22,判断GNSS导航模块1测量的数据和地磁导航模块3测量的数据是否均正确,如果是,则执行步骤S23,否则执行步骤S25。
步骤S23,判断IMU导航模块2的陀螺仪和加速计传感器测量的数据是否均正确,如果是,则执行步骤S24,否则执行步骤S25。
步骤S24,判断GNSS导航模块1、IMU导航模块2和地磁导航模块3中的各个传感器均正常可用,执行步骤S12。
步骤S25,判断GNSS导航模块1、IMU导航模块2和地磁导航模块3中的各个传感器异常,不可用,执行步骤S11。
步骤S10,如果判断传感器可用,则执行步骤S12,否则执行步骤S11。
步骤S11,发送开伞指令。
步骤S12,容错计算,然后执行步骤S8。
容错计算算法如下:P=PGNSS,R=REC,H=HEC+K×(HEC-HGNSS),K为预设系数,取值范围:0.4~0.9。
步骤S13,判断计算得到的无人机的位置信息是否位于预设安全范围内,如果是,则返回步骤S1,否则执行步骤S14。
步骤S14,调整无人机的飞行姿态。
根据控制算法控制无人机将自动调整航向,直至位于预设安全范围内。
如图4所示,本发明实施例的微小型无人机导航装置,还包括:RS422接口6,位于外壳5上,用于与其他设备进行通信。
进一步,本发明实施例的微小型无人机导航装置,还包括:电源及控制接口7,用于接入电源电压。
在本发明的实施例中,本发明的微小型无人机导航装置的体积仅为45mm×40mm×20mm(L×W×H),该体积可适用不同的无人机平台。
根据本发明实施例的微小型无人机导航装置,将GNSS导航模块、IMU导航模块和地磁导航模块进行融合,计算位置、速度、姿态的第一次组合,利用第一次组合计算出的俯仰角、横滚角与地磁进行数据融合,修正姿态角,使用航迹规划,防止无人机飞越禁飞区域,可以确保无人机执行任务时能够安全完成飞行。本发明采用低成本传感器,可以降低整个装置的生产成本,不依赖于某个传感器的、可靠的导航方法,可以实现跨平台的导航方法,可适用不同平台的无人机。并且,本发明的导航装置还提供一种失控情况下的补救措施,即加装降落伞,当系统探测到无人机失控时,打开降落伞,让其安全降落,防止伤及地面人员、设备、建筑物及无人机自身。
在本说明书的描述中,参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中,对上述术语的示意性表述不一定指的是相同的实施例或示例。而且,描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
尽管上面已经示出和描述了本发明的实施例,可以理解的是,上述实施例是示例性的,不能理解为对本发明的限制,本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。本发明的范围由所附权利要求极其等同限定。
Claims (10)
1.一种微小型无人机导航装置,其特征在于,包括:
GNSS导航模块,用于测量无人机的天向方向速度V天、水平方向速度V水平、正北方向速度V北和正东方向速度V东;
IMU导航模块,用于测量所述无人机三维方向上的加速度值Ax、Ay和Az;
地磁导航模块,用于测量所述无人机在水平面的三维方向上的磁场强度MXh、MYh、MZh和垂直面上的三维方向上的磁场强度MXv、MYv、MZv;
姿态融合及导航控制模块,用于接收来自所述GNSS导航模块、IMU导航模块和地磁导航模块的多个数据,并进行数据验证以判断所述GNSS导航模块、IMU导航模块和地磁导航模块是否正常,如果正常,则根据所述天向方向速度V天、水平方向速度V水平、正北方向速度V北和正东方向速度V东计算所述无人机的第一俯仰角PGNSS和第一航向角HGNSS,根据加速度值Ax、Ay和Az计算所述无人机的第二俯仰角PIMU和第一横滚角RIMU,根据MXh、MYh、MZh、MXv、MYv、MZv计算所述无人机的第二航向角HEC和第二横滚角REC,以及对所述第一俯仰角PGNSS、第一航向角HGNSS、第二俯仰角PIMU、第一横滚角RIMU、第二航向角HEC和第二横滚角REC以预设融合算法进行姿态融合,生成所述无人机的位置数据,并判断所述无人机的位置数据是否位于预设安全范围内,如果否,则控制所述无人机调整飞行状态直至位于预设安全范围内;
外壳,所述GNSS导航模块、IMU导航模块、地磁导航模块和姿态融合及导航控制模块均位于所述外壳内。
2.如权利要求1所述的微小型无人机导航装置,其特征在于,所述GNSS导航模块为GPS/GLONASS双频接收机。
3.如权利要求1所述的微小型无人机导航装置,其特征在于,所述GNSS导航模块为还包括内置天线。
4.如权利要求1所述的微小型无人机导航装置,其特征在于,所述IMU导航模块包括:
加速度计传感器,用于测量所述无人机三维方向上的加速度模拟信号;
第一至第三陀螺仪,用于测量所述无人机三维方向上的角速度模拟信号;
模数AD转换模块,所述模数AD转换模块连接至所述加速度计传感器和第一至第三陀螺仪,用于将加速度模拟信号和角速度模拟信号转换为对应的加速度值Ax、Ay和Az、角速度值ωx、ωy、ωz。
5.如权利要求1所述的微小型无人机导航装置,其特征在于,所述地磁导航模块包括:地磁传感器,其中,
<mrow>
<mfenced open = "(" close = ")">
<mtable>
<mtr>
<mtd>
<msub>
<mi>M</mi>
<mrow>
<mi>X</mi>
<mi>h</mi>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>M</mi>
<mrow>
<mi>Y</mi>
<mi>h</mi>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>M</mi>
<mrow>
<mi>Z</mi>
<mi>h</mi>
</mrow>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "(" close = ")">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>cos</mi>
<mi> </mi>
<msub>
<mi>P</mi>
<mrow>
<mi>G</mi>
<mi>N</mi>
<mi>S</mi>
<mi>S</mi>
</mrow>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mi>sin</mi>
<mi> </mi>
<msub>
<mi>P</mi>
<mrow>
<mi>G</mi>
<mi>N</mi>
<mi>S</mi>
<mi>S</mi>
</mrow>
</msub>
<mi>cos</mi>
<mi> </mi>
<msub>
<mi>R</mi>
<mrow>
<mi>I</mi>
<mi>M</mi>
<mi>U</mi>
</mrow>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>sin</mi>
<mi> </mi>
<msub>
<mi>P</mi>
<mrow>
<mi>G</mi>
<mi>N</mi>
<mi>S</mi>
<mi>S</mi>
</mrow>
</msub>
<mi>sin</mi>
<mi> </mi>
<msub>
<mi>R</mi>
<mrow>
<mi>I</mi>
<mi>M</mi>
<mi>U</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>sin</mi>
<mi> </mi>
<msub>
<mi>P</mi>
<mrow>
<mi>G</mi>
<mi>N</mi>
<mi>S</mi>
<mi>S</mi>
</mrow>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>cos</mi>
<mi> </mi>
<msub>
<mi>P</mi>
<mrow>
<mi>G</mi>
<mi>N</mi>
<mi>S</mi>
<mi>S</mi>
</mrow>
</msub>
<mi>cos</mi>
<mi> </mi>
<msub>
<mi>R</mi>
<mrow>
<mi>I</mi>
<mi>M</mi>
<mi>U</mi>
</mrow>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mi>cos</mi>
<mi> </mi>
<msub>
<mi>P</mi>
<mrow>
<mi>G</mi>
<mi>N</mi>
<mi>S</mi>
<mi>S</mi>
</mrow>
</msub>
<mi>sin</mi>
<mi> </mi>
<msub>
<mi>R</mi>
<mrow>
<mi>I</mi>
<mi>M</mi>
<mi>U</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mi>sin</mi>
<mi> </mi>
<msub>
<mi>R</mi>
<mrow>
<mi>I</mi>
<mi>M</mi>
<mi>U</mi>
</mrow>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>cos</mi>
<mi> </mi>
<msub>
<mi>H</mi>
<mrow>
<mi>I</mi>
<mi>M</mi>
<mi>U</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "(" close = ")">
<mtable>
<mtr>
<mtd>
<msub>
<mi>M</mi>
<mi>X</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>M</mi>
<mi>Y</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>M</mi>
<mi>Z</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>;</mo>
</mrow>
<mrow>
<mfenced open = "(" close = ")">
<mtable>
<mtr>
<mtd>
<msub>
<mi>M</mi>
<mrow>
<mi>X</mi>
<mi>v</mi>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>M</mi>
<mrow>
<mi>Y</mi>
<mi>v</mi>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>M</mi>
<mrow>
<mi>Z</mi>
<mi>v</mi>
</mrow>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "(" close = ")">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>cos</mi>
<mi> </mi>
<msub>
<mi>P</mi>
<mrow>
<mi>G</mi>
<mi>N</mi>
<mi>S</mi>
<mi>S</mi>
</mrow>
</msub>
<mi>cos</mi>
<mi> </mi>
<msub>
<mi>H</mi>
<mrow>
<mi>G</mi>
<mi>N</mi>
<mi>S</mi>
<mi>S</mi>
</mrow>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mi>sin</mi>
<mi> </mi>
<msub>
<mi>P</mi>
<mrow>
<mi>G</mi>
<mi>N</mi>
<mi>S</mi>
<mi>S</mi>
</mrow>
</msub>
<mi>cos</mi>
<mi> </mi>
<msub>
<mi>H</mi>
<mrow>
<mi>G</mi>
<mi>N</mi>
<mi>S</mi>
<mi>S</mi>
</mrow>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>sin</mi>
<mi> </mi>
<msub>
<mi>H</mi>
<mrow>
<mi>G</mi>
<mi>N</mi>
<mi>S</mi>
<mi>S</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>sin</mi>
<mi> </mi>
<msub>
<mi>P</mi>
<mrow>
<mi>G</mi>
<mi>N</mi>
<mi>S</mi>
<mi>S</mi>
</mrow>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>cos</mi>
<mi> </mi>
<msub>
<mi>H</mi>
<mrow>
<mi>G</mi>
<mi>N</mi>
<mi>S</mi>
<mi>S</mi>
</mrow>
</msub>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<mi>cos</mi>
<mi> </mi>
<msub>
<mi>P</mi>
<mrow>
<mi>G</mi>
<mi>N</mi>
<mi>S</mi>
<mi>S</mi>
</mrow>
</msub>
<mi>cos</mi>
<mi> </mi>
<msub>
<mi>H</mi>
<mrow>
<mi>G</mi>
<mi>N</mi>
<mi>S</mi>
<mi>S</mi>
</mrow>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>sin</mi>
<mi> </mi>
<msub>
<mi>P</mi>
<mrow>
<mi>G</mi>
<mi>N</mi>
<mi>S</mi>
<mi>S</mi>
</mrow>
</msub>
<mi>sin</mi>
<mi> </mi>
<msub>
<mi>H</mi>
<mrow>
<mi>G</mi>
<mi>N</mi>
<mi>S</mi>
<mi>S</mi>
</mrow>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>cos</mi>
<mi> </mi>
<msub>
<mi>H</mi>
<mrow>
<mi>G</mi>
<mi>N</mi>
<mi>S</mi>
<mi>S</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "(" close = ")">
<mtable>
<mtr>
<mtd>
<msub>
<mi>M</mi>
<mi>X</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>M</mi>
<mi>Y</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>M</mi>
<mi>Z</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>,</mo>
</mrow>
其中,MX、MY、MZ分别为X轴、Y轴和Z轴地磁传感器的输出值,MXh、MYh、MZh为地磁传感器的输出值在水平面的投影,MXv、MYv、MZv为地磁传感器的输出值在垂直面的投影。
6.如权利要求1所述的微小型无人机导航装置,其特征在于,所述姿态融合及导航控制模块计算所述无人机的第一俯仰角PGNSS=asin(V天/V水平),第一航向角HGNSS=atan(V北/V东);
所述姿态融合及导航控制模块计算所述无人机的第二俯仰角PIMU=asin(Ay/g),第一横滚角RIMU=atan2(Ax,Az),其中,g为重力加速度;
所述姿态融合及导航控制模块计算所述无人机的第二航向角HEC=atan2(-MXh,MYh)-H0,第二横滚角REC=atan2(MXv,-MZv),其中,H0为偏磁角。
7.如权利要求6所述的微小型无人机导航装置,其特征在于,所述姿态融合及导航控制模块以预设融合算法进行姿态融合,包括:
P=PIMU+K×(PIMU-PGNSS),R=RIMU+K×(RIMU-REC),H=HEC+K×(HEC-HGNSS),
其中,P为姿态融合后的无人机的俯仰角,R为姿态融合后的无人机的横滚角,H为姿态融合后的无人机的航向角,K为预设系数。
8.如权利要求4所述的微小型无人机导航装置,其特征在于,所述姿态融合及导航模块还用于在经过数据验证判断所述GNSS导航模块、IMU导航模块和地磁导航模块异常时,进一步对所述GNSS导航模块、IMU导航模块和地磁导航模块进行传感器可用性判断,包括如下步骤:
判断所述天向方向速度V天、水平方向速度V水平、正北方向速度V北、正东方向速度V东以及水平面的三维方向上的磁场强度MXh、MYh、MZh和垂直面上的三维方向上的磁场强度MXv、MYv、MZv、加速度值Ax、Ay、Az和角速度值ωx、ωy、ωz是否正确;
如果正确,则进行容错计算,调整所述GNSS导航模块、IMU导航模块和地磁导航模块的数据至容错范围内,然后将容错计算后的数据以预设融合算法进行姿态融合;
如果不正确,则判断GNSS导航模块、IMU导航模块和地磁导航模块均不可用,发出开伞指令以弹出降落伞。
9.如权利要求1所述的微小型无人机导航装置,其特征在于,还包括:RS422接口,所述RS422接口位于所述外壳上,用于与其他设备进行通信。
10.如权利要求1所述的微小型无人机导航装置,其特征在于,还包括:电源及控制接口,用于接入电源电压。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201510004820.2A CN104503466B (zh) | 2015-01-05 | 2015-01-05 | 一种微小型无人机导航装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201510004820.2A CN104503466B (zh) | 2015-01-05 | 2015-01-05 | 一种微小型无人机导航装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN104503466A CN104503466A (zh) | 2015-04-08 |
CN104503466B true CN104503466B (zh) | 2017-09-12 |
Family
ID=52944870
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201510004820.2A Expired - Fee Related CN104503466B (zh) | 2015-01-05 | 2015-01-05 | 一种微小型无人机导航装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN104503466B (zh) |
Families Citing this family (18)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105991924A (zh) * | 2015-03-04 | 2016-10-05 | 珠海金山办公软件有限公司 | 一种拍摄辅助方法和装置 |
WO2017045141A1 (en) * | 2015-09-16 | 2017-03-23 | SZ DJI Technology Co., Ltd. | Method and apparatus for operating mobile platform |
CN105427563A (zh) * | 2015-11-27 | 2016-03-23 | 杨珊珊 | 无人飞行器的遥控装置及其遥控方法 |
CN106289241B (zh) * | 2016-07-18 | 2019-02-26 | 北京方位捷讯科技有限公司 | 利用磁场特征的定位修正方法及装置 |
CN107843231A (zh) * | 2016-09-21 | 2018-03-27 | 中国移动通信集团公司 | 一种天线方位角检测方法和装置 |
CN106708088B (zh) * | 2016-09-26 | 2021-03-05 | 北京远度互联科技有限公司 | 坐标计算方法及装置、飞行控制方法及系统、无人机 |
CN106371124A (zh) * | 2016-10-11 | 2017-02-01 | 金陵科技学院 | 一种基于协同模式的微小型无人机定位系统 |
CN106527464A (zh) * | 2016-12-06 | 2017-03-22 | 深圳市元征科技股份有限公司 | 一种无人机姿态保持方法及装置 |
CN108253966B (zh) * | 2016-12-28 | 2021-08-06 | 昊翔电能运动科技(昆山)有限公司 | 无人机飞行三维模拟显示方法 |
US11385059B2 (en) | 2017-05-26 | 2022-07-12 | Guangzhou Xaircraft Technology Co., Ltd | Method for determining heading of unmanned aerial vehicle and unmanned aerial vehicle |
CN107911793B (zh) * | 2017-10-30 | 2020-03-31 | 东南大学 | 一种无人机任意图形禁飞区识别导航系统 |
CN108490972A (zh) * | 2018-03-21 | 2018-09-04 | 深圳臻迪信息技术有限公司 | 飞行器的飞行控制方法、系统以及电子设备 |
CN108801245B (zh) * | 2018-05-29 | 2020-09-25 | 深圳市海梁科技有限公司 | 一种导航系统 |
CN109916429B (zh) * | 2018-11-23 | 2021-10-08 | 深圳市中科金朗产业研究院有限公司 | 一种微机电陀螺仪校准装置、方法及无人机导航系统 |
CN109683164A (zh) * | 2019-01-25 | 2019-04-26 | 西南科技大学 | 一种基于飞行数据的无人机坠落行为识别方法 |
CN111397575A (zh) * | 2020-04-26 | 2020-07-10 | 上海豪骋智能科技有限公司 | 一种无人驾驶汽车的车体姿态检测装置 |
CN113959438A (zh) * | 2021-10-20 | 2022-01-21 | 广东电力通信科技有限公司 | 一种基于多源数据融合的导航定位方法、存储介质 |
CN116828132A (zh) * | 2023-07-05 | 2023-09-29 | 广州磐碟塔信息科技有限公司 | 一种虚拟摄影的控制方法及其系统 |
Family Cites Families (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2003042766A (ja) * | 2001-07-30 | 2003-02-13 | Japan Aviation Electronics Industry Ltd | 方位角計測装置 |
DE10308308A1 (de) * | 2003-02-26 | 2004-09-16 | Buck Neue Technologien Gmbh | Roll-Nick-Ausgleich |
KR100565794B1 (ko) * | 2003-12-30 | 2006-03-29 | 삼성전자주식회사 | 기울기의 영향을 보상하여 방위각을 연산하는 지자기센서, 및 그 연산방법 |
JP4963429B2 (ja) * | 2007-03-06 | 2012-06-27 | 日本電信電話株式会社 | 追尾アンテナ装置、姿勢角推定方法およびそのプログラム |
CN100557540C (zh) * | 2007-12-25 | 2009-11-04 | 北京航空航天大学 | 一种基于磁航向传感器的无人机航向自动修正方法 |
CN201242443Y (zh) * | 2008-07-29 | 2009-05-20 | 宝鸡市博远信航电子科技有限责任公司 | 飞机磁罗盘校准仪 |
US20110063167A1 (en) * | 2009-09-15 | 2011-03-17 | Qualcomm Incorporated | Using magnetometer with a positioning system |
US8779971B2 (en) * | 2010-05-24 | 2014-07-15 | Robert J. Wellington | Determining spatial orientation information of a body from multiple electromagnetic signals |
CN102117072B (zh) * | 2011-03-10 | 2014-10-15 | 上海交通大学 | 具有旋翼转速检测功能的多旋翼飞行器地面测控系统 |
CN102692225B (zh) * | 2011-03-24 | 2015-03-11 | 北京理工大学 | 一种用于低成本小型无人机的姿态航向参考系统 |
CN103245963A (zh) * | 2013-05-09 | 2013-08-14 | 清华大学 | 双天线gnss/ins深组合导航方法及装置 |
-
2015
- 2015-01-05 CN CN201510004820.2A patent/CN104503466B/zh not_active Expired - Fee Related
Also Published As
Publication number | Publication date |
---|---|
CN104503466A (zh) | 2015-04-08 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN104503466B (zh) | 一种微小型无人机导航装置 | |
García Carrillo et al. | Combining stereo vision and inertial navigation system for a quad-rotor UAV | |
JP6506302B2 (ja) | モバイルプラットフォームを操作するための方法及び装置 | |
US20190243212A1 (en) | Flying camera with string assembly for localization and interaction | |
CN109900300B (zh) | 一种用于无人机的组合导航完好性监测系统 | |
CN107643762A (zh) | 自主导航的无人机系统及其导航方法 | |
CN101319902B (zh) | 一种低成本组合式定位定向装置及组合定位方法 | |
US10852364B2 (en) | Interference mitigation in magnetometers | |
US11585658B2 (en) | Virtual sensor and compass calibration | |
CN106352872B (zh) | 一种无人机自主导航系统及其导航方法 | |
CN108255190B (zh) | 基于多传感器的精确着陆方法及使用该方法的系留无人机 | |
CN103487822A (zh) | 北斗/多普勒雷达/惯性自主式组合导航系统及其方法 | |
CN102809377A (zh) | 飞行器惯性/气动模型组合导航方法 | |
KR20180064155A (ko) | 드론의 추락사고 방지 장치 및 그 제어 방법 | |
CN104199455A (zh) | 基于多旋翼飞行器的隧道巡检系统 | |
CN104635743A (zh) | 一种高速无人机超低空全程自主飞行控制系统 | |
CN202771262U (zh) | 固定翼自动导航飞行控制系统 | |
CN105242682A (zh) | 靶机目标特性测量系统 | |
JP6934116B1 (ja) | 航空機の飛行制御を行う制御装置、及び制御方法 | |
CN111679680A (zh) | 一种无人机自主着舰方法及系统 | |
CN206363163U (zh) | 一种无人机防坠毁降落系统 | |
CN103994766A (zh) | 一种抗gps失效固定翼无人机定向方法 | |
WO2002014890A2 (en) | Attitude determination using a global positioning system | |
JP2020149640A (ja) | 飛行システム及び着陸制御方法 | |
JP3852842B2 (ja) | 飛翔体の位置姿勢計測装置 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20170912 Termination date: 20200105 |
|
CF01 | Termination of patent right due to non-payment of annual fee |