CN105928515B - 一种无人机导航*** - Google Patents
一种无人机导航*** Download PDFInfo
- Publication number
- CN105928515B CN105928515B CN201610242663.3A CN201610242663A CN105928515B CN 105928515 B CN105928515 B CN 105928515B CN 201610242663 A CN201610242663 A CN 201610242663A CN 105928515 B CN105928515 B CN 105928515B
- Authority
- CN
- China
- Prior art keywords
- navigation system
- unmanned aerial
- aerial vehicle
- inertial navigation
- error
- 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.)
- Active
Links
- 239000011159 matrix material Substances 0.000 claims description 33
- 230000001133 acceleration Effects 0.000 claims description 15
- 238000000034 method Methods 0.000 claims description 9
- 238000004364 calculation method Methods 0.000 claims description 3
- 238000001914 filtration Methods 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 3
- 238000005259 measurement Methods 0.000 claims description 3
- 238000005070 sampling Methods 0.000 claims description 3
- 230000009897 systematic effect Effects 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- 230000001186 cumulative effect Effects 0.000 abstract description 2
- 230000007547 defect Effects 0.000 description 10
- 238000010586 diagram Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008054 signal transmission Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
- G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种能够准确反映无人机的姿态和位置信息的无人机导航***。该无人机导航***包括INS惯性导航***模块、磁力计、GPS导航***模块、控制中心,利用GPS导航长时间具有高的定位精度的优点来弥补INS惯性导航累计误差随时间的增加而发散的缺点;利用INS惯性导航不受外界干扰、输出的导航信息连续的特点弥补GPS易受干扰和输出频率有限的缺点,并且为了解决由惯性导航计算出的偏航角无法找到真北,以及漂移较大的情况,本***利用磁力计计算出的偏航角来校正,获得地理真北方向和稳定的偏航角,另外,此***成本较低,***稳定性较强,能够输出比较满意的导航定位信息,从而能够准确反映无人机的姿态和位置信息。适合在导航技术领域推广应用。
Description
技术领域
本发明涉及导航技术领域,尤其是一种无人机导航***。
背景技术
在导航技术方面,目前应用得最多,最成熟的导航方式有INS惯性导航和卫星导航。GPS卫星导航的优点是具有全球性、全天候、长时间定位精度高的特点,但缺点是信号易受干扰和遮挡,在强电磁环境下和有高楼遮挡时,信号质量变差,并且其输出频率有限,一般为1—10Hz,输出不连续,在需要快速更新信息的场合,如机动性和实时性要求较高的无人机***上,GPS卫星导航的缺点便凸显出来。而INS惯性导航***是一种全自主式的导航方式,因此具有很强的隐蔽性和抗干扰的能力,并且输出信息连续,短时间内定位精度高。但由于MEMS-INS器件自身的特点,陀螺仪和加速度计有初始零偏、随机漂移等误差,随着时间的累计作用,其误差越来越大,长时间定位精度较差,最终无法准确反映无人机的姿态和位置信息。
发明内容
本发明所要解决的技术问题是提供一种能够准确反映无人机的姿态和位置信息的无人机导航***。
本发明解决其技术问题所采用的技术方案为:该无人机导航***,包括INS惯性导航***模块、磁力计、GPS导航***模块、控制中心,所述INS惯性导航***模块包括陀螺仪和加速度计,所述陀螺仪、加速度计、磁力计、GPS导航***模块分别与控制中心信号连接,所述陀螺仪用于测量无人机的角速度参数并将其测得的角速度参数传递给控制中心,所述磁力计用于测量无人机的偏航角并将其测得的偏航角参数传递给控制中心,所述加速度计用于测量无人机的加速度并将其测得的加速度参数传递给控制中心,所述GPS导航***模块用于测量无人机在GPS导航***中的位置速度信息,所述控制中心根据接收到的角速度参数、偏航角参数、加速度参数计算出无人机在INS惯性导航***中的位置速度信息,控制中心将无人机在GPS导航***中的位置速度信息与无人机在GPS导航***中的位置速度信息进行Kalman滤波并进行导航误差估计,然后利用导航误差估计修正无人机在INS惯性导航***中的位置速度信息从而得到最终的导航信息。
进一步的是,所述陀螺仪、加速度计、磁力计、分别与控制中心通过I2C总线连接。
进一步的是,所述GPS导航***模块与控制中心通过USART串口连接。
进一步的是,所述控制中心采用如下方法计算得到最终的导航信息,其具体计算方法如下所述:
A、将陀螺仪测得的无人机角速度参数代入四元数微分方程求解得到四元数q0,q1,q2,q3;其中为陀螺仪在无人机自身坐标系下的测得的三个轴的角速度信息;
所述四元数微分方程为:
B、将步骤A中求解的q0,q1,q2,q3代入下式求解得到姿态矩阵
根据下述与方向余弦的关系式
计算得出无人机的INS惯性导航模块姿态角θ、γ、
C、利用磁力计测得的偏航角替换步骤B计算得到的偏航角
D、将加速度计测得的加速度参数fb和步骤B中求解得到姿态矩阵代入下述微分方程中求解得到无人机在INS惯性导航坐标系下的东、北、天三个方向上的速度信息vN vEvU,所述微分方程为:
其中,vn=[vN vE vU]T分别为INS惯性导航坐标系中东、北、天方向上的速度投影到惯性导航坐标系中的速度,为地球自转角速度投影到惯性导航坐标系中的地球自转角速度,为无人机绕INS惯性导航坐标系各轴向的转动角速率投影到惯性导航坐标系中的转动角速率,gn为重力加速度投影到惯性导航坐标系中的重力加速度;
E、将步骤D计算得出的vN vE vU分别代入下式求解得出无人机在INS惯性导航***中的位置信息,其中L为纬度,λ为经度,h为高度,
h=h(0)+∫vUdt,其中L(0)表示无人机初始位置的纬度值,λ(0)表示无人机初始位置的经度值,h(0)表示无人机距离地球表面的初始高度。RM表示地球子午圈上的曲率半径,RN表示纬度圈上的曲率半径;
F、建立状态方程和观测方程Z(t)=H(t)XI(t)+V(t),
XI(t)表示INS惯性导航***在t时刻的误差状态,它是一个15维的向量,如下所示:
XI(t)=[δvx δv yδvz φx φy φz δL δλ δh εx εy εz ▽x ▽y ▽z],δvx,δvy,δvz为INS惯性导航***沿东、北、天方向上的速度误差;φx,φy,φz为无人机的姿态角误差;δL,δλ,δh分别代表无人机所在纬度、经度和高度误差;εx,εy,εz分别代表陀螺仪的随机漂移;▽x,▽y,▽z分别为加速度计的随机漂移,其中是一个15×15的矩阵;其中FN(t)对应于δvx,δvy,δvz,φx,φy,φz,δL,δλ,δh这9个参数的INS惯性导航***矩阵,其非零元素如下:
F(1,5)=-fz F(1,6)=fy
F(2,4)=fz F(2,6)=-fx
F(3,4)=-fy F(3,5)=fx F(3,7)=-2ωievx sinL
F(5,7)=-ωie sinL
F(9,3)=1
FS(t)为δvx,δvy,δvz,φx,φy,φz,δL,δλ,δh这9个参数与陀螺仪及加速度计漂移之间的变换矩阵,其维数是9×6,
FM(t)为εx,εy,εz,▽x,▽y,▽z与陀螺仪及加速度计漂移对应的INS惯性导航***矩阵,是一个维数为6×6的对角线矩阵,表示如下:
FM(t)=diag[-1/Tgx -1/Tgy -1/Tgz -1/Tax -1/Tay -1/Taz];其中,Tgx表示陀螺仪x轴的误差模型的时间常数,Tgy表示陀螺仪y轴的误差模型的时间常数,Tgz表示陀螺仪z轴的误差模型的时间常数,Tax表示加速度计x轴误差模型的时间常数,Tay表示加速度计y轴误差模型的时间常数,Taz表示加速度计z轴误差模型的时间常数;
GI(t)=diag[1 1 1......1 1]15×15;
WI(t)是一个15维的向量,如下所示:
WI(t)=[a1 a2 a3 a4 a5 a6 a7 a8 a9 a10 a11 a12 a13 a14 a15],
a1 a2 a3 a4 a5 a6 a7 a8 a9 a10 a11 a12 a13 a14 a15表示***过程噪声序列;
Z(t)为无人机在INS惯性导航***中的位置速度信息与无人机在GPS导航***中的位置速度信息的差值,是一个6维向量,
Z(t)=[δvx+Nvx δvy+Nvy δvz+Nvz (RM+h)δL+Ny (RM+h)cosLδλ+Nx δh+Nh]T,其中,Nvx表示GPS导航***在x方向上的速度误差,Nvy表示GPS导航***在y方向上的速度误差,Nvz表示GPS导航***在z方向上的速度误差,Nx表示GPS导航***在x方向上的位置误差,Ny表示GPS导航***在y方向上的位置误差,Nh表示GPS导航***在z方向上的位置误差;
其中
Vv(t)=[Nvx Nvy Nvz]T
Vp(t)=[Nx Ny Nz]T
G、将上述得到的连续状态方程离散化后得到Xk=Φk,k-1Xk-1+Wk-1,其中
将上述得到的连续观测方程Z(t)=H(t)XI(t)+V(t)离散化后得到Zk=HkXk+Vk;
其中I是单位矩阵,F是INS惯性导航***的状态转移矩阵,Δt是离散化后INS惯性导航***的采样时间;
H、将无人机在INS惯性导航***中的位置速度信息与无人机在GPS导航***中的位置速度信息作差得到Z(t)在k时刻的观测信息z;
I、计算k时刻INS惯性导航***状态的最优估计值 其中, 为在k-1时刻,INS惯性导航***状态的最优估计值,Qk-1是INS惯性导航***的噪声矩阵,其大小是由INS惯性导航元件的性能决定, Rk是***测量噪声的方差阵,其大小是由GPS接收机的性能决定;
J、将计算得到的值与无人机在INS惯性导航***中的位置速度信息作差得到最优的导航参数;
K、重复步骤H-J,得到连续的无人机导航信息。
本发明的有益效果:该无人机导航***包括INS惯性导航***模块、磁力计、GPS导航***模块、控制中心,通过利用INS惯性导航和GPS导航组合导航的方法解决了单一的GPS导航技术易受干扰和遮挡,短时定位精度不高,输出频率有限并且输出不连续的缺点;同时也解决了单一的INS惯性导航参数累计误差越来越大,长时间定位精度发散的缺点,利用GPS导航长时间具有高的定位精度的优点来弥补INS惯性导航累计误差随时间的增加而发散的缺点;利用INS惯性导航不受外界干扰、输出的导航信息连续的特点弥补GPS易受干扰和输出频率有限的缺点,并且为了解决由惯性导航计算出的偏航角无法找到真北,以及漂移较大的情况,本***利用磁力计计算出的偏航角来校正,获得地理真北方向和稳定的偏航角,另外,此***成本较低,***稳定性较强,能够输出比较满意的导航定位信息,从而能够准确反映无人机的姿态和位置信息。
附图说明
图1为本发明所述无人机导航***计算得出的纬度误差值;
图2为本发明所述无人机导航***计算得出的纬度误差方差值;
图3为本发明所述无人机导航***计算得出的经度误差值;
图4为本发明所述无人机导航***计算得出的经度误差方差值;
图5为本发明所述无人机导航***计算得出的高度误差值;
图6为本发明所述无人机导航***计算得出的高度误差方差值;
图7为本发明所述无人机导航***计算得出的东向速度误差值;
图8为本发明所述无人机导航***计算得出的东向速度误差方差值;
图9为本发明所述无人机导航***计算得出的北向速度误差值;
图10为本发明所述无人机导航***计算得出的北向速度误差方差值;
图11为本发明所述无人机导航***计算得出的天向速度误差值;
图12为本发明所述无人机导航***计算得出的天向速度误差方差值。
具体实施方式
本发明所述的无人机导航***,包括INS惯性导航***模块、磁力计、GPS导航***模块、控制中心,所述INS惯性导航***模块包括陀螺仪和加速度计,所述陀螺仪、加速度计、磁力计、GPS导航***模块分别与控制中心信号连接,所述陀螺仪用于测量无人机的角速度参数并将其测得的角速度参数传递给控制中心,所述磁力计用于测量无人机的偏航角并将其测得的偏航角参数传递给控制中心,所述加速度计用于测量无人机的加速度并将其测得的加速度参数传递给控制中心,所述GPS导航***模块用于测量无人机在GPS导航***中的位置速度信息,所述控制中心根据接收到的角速度参数、偏航角参数、加速度参数计算出无人机在INS惯性导航***中的位置速度信息,控制中心将无人机在GPS导航***中的位置速度信息与无人机在GPS导航***中的位置速度信息进行Kalman滤波并进行导航误差估计,然后利用导航误差估计修正无人机在INS惯性导航***中的位置速度信息从而得到最终的导航信息。该无人机导航***包括INS惯性导航***模块、磁力计、GPS导航***模块、控制中心,通过利用INS惯性导航和GPS导航组合导航的方法解决了单一的GPS导航技术易受干扰和遮挡,短时定位精度不高,输出频率有限并且输出不连续的缺点;同时也解决了单一的INS惯性导航参数累计误差越来越大,长时间定位精度发散的缺点,利用GPS导航长时间具有高的定位精度的优点来弥补INS惯性导航累计误差随时间的增加而发散的缺点;利用INS惯性导航不受外界干扰、输出的导航信息连续的特点弥补GPS易受干扰和输出频率有限的缺点,并且为了解决由惯性导航计算出的偏航角无法找到真北,以及漂移较大的情况,本***利用磁力计计算出的偏航角来校正,获得地理真北方向和稳定的偏航角,另外,此***成本较低,能够输出比较满意的导航定位信息,从而能够准确反映无人机的姿态和位置信息。
在上述实施方式中,为了保证信号传递的时效性与准确性,所述陀螺仪、加速度计、磁力计、分别与控制中心通过I2C总线连接。所述GPS导航***模块与控制中心通过USART串口连接。
所述控制中心采用如下方法计算得到最终的导航信息,其具体计算方法如下所述:
A、将陀螺仪测得的无人机角速度参数代入四元数微分方程求解得到四元数q0,q1,q2,q3;其中为陀螺仪在无人机自身坐标系下的测得的三个轴的角速度信息;
所述四元数微分方程为:
B、将步骤A中求解的q0,q1,q2,q3代入下式求解得到姿态矩阵
根据下述与方向余弦的关系式
计算得出无人机的INS惯性导航模块姿态角θ、γ、
C、利用磁力计测得的偏航角替换步骤B计算得到的偏航角
D、将加速度计测得的加速度参数fb和步骤B中求解得到姿态矩阵代入下述微分方程中求解得到无人机在INS惯性导航坐标系下的东、北、天三个方向上的速度信息vN vEvU,所述微分方程为:
其中,vn=[vN vE vU]T分别为INS惯性导航坐标系中东、北、天方向上的速度投影到惯性导航坐标系中的速度,为地球自转角速度投影到惯性导航坐标系中的地球自转角速度,为无人机绕INS惯性导航坐标系各轴向的转动角速率投影到惯性导航坐标系中的转动角速率,gn为重力加速度投影到惯性导航坐标系中的重力加速度;
E、将步骤D计算得出的vN vE vU分别代入下式求解得出无人机在INS惯性导航***中的位置信息,其中L为纬度,λ为经度,h为高度,
h=h(0)+∫vUdt,其中L(0)表示无人机初始位置的纬度值,λ(0)表示无人机初始位置的经度值,h(0)表示无人机距离地球表面的初始高度。RM表示地球子午圈上的曲率半径,RN表示纬度圈上的曲率半径;
F、建立状态方程和观测方程Z(t)=H(t)XI(t)+V(t),
XI(t)表示INS惯性导航***在t时刻的误差状态,它是一个15维的向量,如下所示:
XI(t)=[δvx δvy δvz φx φy φz δL δλ δh εx εy εz ▽x ▽y ▽z],δvx,δvy,δvz为INS惯性导航***沿东、北、天方向上的速度误差;φx,φy,φz为无人机的姿态角误差;δL,δλ,δh分别代表无人机所在纬度、经度和高度误差;εx,εy,εz分别代表陀螺仪的随机漂移;▽x,▽y,▽z分别为加速度计的随机漂移,其中是一个15×15的矩阵;其中FN(t)对应于δvx,δvy,δvz,φx,φy,φz,δL,δλ,δh这9个参数的INS惯性导航***矩阵,其非零元素如下:
F(1,5)=-fz F(1,6)=fy
F(2,4)=fz F(2,6)=-fx
F(3,4)=-fy F(3,5)=fx F(3,7)=-2ωievx sinL
F(5,7)=-ωie sinL
F(9,3)=1
FS(t)为δvx,δvy,δvz,φx,φy,φz,δL,δλ,δh这9个参数与陀螺仪及加速度计漂移之间的变换矩阵,其维数是9×6,
FM(t)为εx,εy,εz,▽x,▽y,▽z与陀螺仪及加速度计漂移对应的INS惯性导航***矩阵,是一个维数为6×6的对角线矩阵,表示如下:
FM(t)=diag[-1/Tgx -1/Tgy -1/Tgz -1/Tax -1/Tay -1/Taz];其中,Tgx表示陀螺仪x轴的误差模型的时间常数,Tgy表示陀螺仪y轴的误差模型的时间常数,Tgz表示陀螺仪z轴的误差模型的时间常数,Tax表示加速度计x轴误差模型的时间常数,Tay表示加速度计y轴误差模型的时间常数,Taz表示加速度计z轴误差模型的时间常数;
GI(t)=diag[1 1 1......1 1]15×15;
WI(t)是一个15维的向量,如下所示:
WI(t)=[a1 a2 a3 a4 a5 a6 a7 a8 a9 a10 a11 a12 a13 a14 a15],
a1 a2 a3 a4 a5 a6 a7 a8 a9 a10 a11 a12 a13 a14 a15表示***过程噪声序列;
Z(t)为无人机在INS惯性导航***中的位置速度信息与无人机在GPS导航***中的位置速度信息的差值,是一个6维向量,
Z(t)=[δvx+Nvx δvy+Nvy δvz+Nvz (RM+h)δL+Ny (RM+h)cosLδλ+Nx δh+Nh]T,其中,Nvx表示GPS导航***在x方向上的速度误差,Nvy表示GPS导航***在y方向上的速度误差,Nvz表示GPS导航***在z方向上的速度误差,Nx表示GPS导航***在x方向上的位置误差,Ny表示GPS导航***在y方向上的位置误差,Nh表示GPS导航***在z方向上的位置误差;
其中
Vv(t)=[Nvx Nvy Nvz]T
Vp(t)=[Nx Ny Nz]T
G、将上述得到的连续状态方程离散化后得到Xk=Φk,k-1Xk-1+Wk-1,其中
将上述得到的连续观测方程Z(t)=H(t)XI(t)+V(t)离散化后得到Zk=HkXk+Vk;
其中I是单位矩阵,F是INS惯性导航***的状态转移矩阵,Δt是离散化后INS惯性导航***的采样时间;
H、将无人机在INS惯性导航***中的位置速度信息与无人机在GPS导航***中的位置速度信息作差得到Z(t)在k时刻的观测信息z;
I、计算k时刻INS惯性导航***状态的最优估计值 其中, 为在k-1时刻,INS惯性导航***状态的最优估计值,Qk-1是INS惯性导航***的噪声矩阵,其大小是由INS惯性导航元件的性能决定, Rk是***测量噪声的方差阵,其大小是由GPS接收机的性能决定;
J、将计算得到的值与无人机在INS惯性导航***中的位置速度信息作差得到最优的导航参数;
K、重复步骤H-J,得到连续的无人机导航信息。
利用上述方法计算得出的导航信息,能够准确反映无人机的姿态和位置信息,可以实现较好的定位导航精度,图1为本发明所述无人机导航***计算得出的纬度误差值;图2为本发明所述无人机导航***计算得出的纬度误差方差值;图3为本发明所述无人机导航***计算得出的经度误差值;图4为本发明所述无人机导航***计算得出的经度误差方差值;图5为本发明所述无人机导航***计算得出的高度误差值;图6为本发明所述无人机导航***计算得出的高度误差方差值;图7为本发明所述无人机导航***计算得出的东向速度误差值;图8为本发明所述无人机导航***计算得出的东向速度误差方差值;图9为本发明所述无人机导航***计算得出的北向速度误差值;图10为本发明所述无人机导航***计算得出的北向速度误差方差值;图11为本发明所述无人机导航***计算得出的天向速度误差值;图12为本发明所述无人机导航***计算得出的天向速度误差方差值;
从上述测试结果图可以看出,本发明所述无人机导航***得出的经度、纬度、高度的误差方差均能快速收敛至比较小的数值;对位置、速度等导航信息也能实现滤平滑作用,不会产生大的跳变,***的稳定性较强。
Claims (3)
1.一种无人机导航***,其特征在于:包括INS惯性导航***模块、磁力计、GPS导航***模块、控制中心,所述INS惯性导航***模块包括陀螺仪和加速度计,所述陀螺仪、加速度计、磁力计、GPS导航***模块分别与控制中心信号连接,所述陀螺仪用于测量无人机的角速度参数并将其测得的角速度参数传递给控制中心,所述磁力计用于测量无人机的偏航角并将其测得的偏航角参数传递给控制中心,所述加速度计用于测量无人机的加速度并将其测得的加速度参数传递给控制中心,所述GPS导航***模块用于测量无人机在GPS导航***中的位置速度信息,所述控制中心根据接收到的角速度参数、偏航角参数、加速度参数计算出无人机在INS惯性导航***中的位置速度信息,控制中心将无人机在GPS导航***中的位置速度信息与无人机在GPS导航***中的位置速度信息进行Kalman滤波并进行导航误差估计,然后利用导航误差估计修正无人机在INS惯性导航***中的位置速度信息从而得到最终的导航信息;
所述控制中心采用如下方法计算得到最终的导航信息,其具体计算方法如下所述:
A、将陀螺仪测得的无人机角速度参数代入四元数微分方程求解得到四元数q0,q1,q2,q3;其中为陀螺仪在无人机自身坐标系下的测得的三个轴的角速度信息;
所述四元数微分方程为:
B、将步骤A中求解的q0,q1,q2,q3代入下式求解得到姿态矩阵
根据下述与方向余弦的关系式
计算得出无人机的INS惯性导航模块姿态角θ、γ、
C、利用磁力计测得的偏航角替换步骤B计算得到的偏航角
D、将加速度计测得的加速度参数fb和步骤B中求解得到姿态矩阵代入下述微分方程中求解得到无人机在INS惯性导航坐标系下的东、北、天三个方向上的速度信息vN vE vU,所述微分方程为:
其中,vn=[vN vE vU]T分别为INS惯性导航坐标系中东、北、天方向上的速度投影到惯性导航坐标系中的速度,为地球自转角速度投影到惯性导航坐标系中的地球自转角速度,为无人机绕INS惯性导航坐标系各轴向的转动角速率投影到惯性导航坐标系中的转动角速率,gn为重力加速度投影到惯性导航坐标系中的重力加速度;
E、将步骤D计算得出的vN vE vU分别代入下式求解得出无人机在INS惯性导航***中的位置信息,其中L为纬度,λ为经度,h为高度,
h=h(0)+∫vUdt,其中L(0)表示无人机初始位置的纬度值,λ(0)表示无人机初始位置的经度值,h(0)表示无人机距离地球表面的初始高度,RM表示地球子午圈上的曲率半径,RN表示纬度圈上的曲率半径;
F、建立状态方程和观测方程Z(t)=H(t)XI(t)+V(t),
XI(t)表示INS惯性导航***在t时刻的误差状态,它是一个15维的向量,如下所示:
δvx,δvy,δvz为INS惯性导航***沿东、北、天方向上的速度误差;φx,φy,φz为无人机的姿态角误差;δL,δλ,δh分别代表无人机所在纬度、经度和高度误差;εx,εy,εz分别代表陀螺仪的随机漂移;分别为加速度计的随机漂移,其中是一个15×15的矩阵;其中FN(t)对应于δvx,δvy,δvz,φx,φy,φz,δL,δλ,δh这9个参数的INS惯性导航***矩阵,其非零元素如下:
F(3,4)=-fy F(3,5)=fx F(3,7)=-2ωievx sinL
FS(t)为δvx,δvy,δvz,φx,φy,φz,δL,δλ,δh这9个参数与陀螺仪及加速度计漂移之间的变换矩阵,其维数是9×6,
FM(t)为εx,εy,εz,与陀螺仪及加速度计漂移对应的INS惯性导航***矩阵,是一个维数为6×6的对角线矩阵,表示如下:
FM(t)=diag[-1/Tgx -1/Tgy -1/Tgz -1/Tax -1/Tay -1/Taz];其中,Tgx表示陀螺仪x轴的误差模型的时间常数,Tgy表示陀螺仪y轴的误差模型的时间常数,Tgz表示陀螺仪z轴的误差模型的时间常数,Tax表示加速度计x轴误差模型的时间常数,Tay表示加速度计y轴误差模型的时间常数,Taz表示加速度计z轴误差模型的时间常数;
GI(t)=diag[1 1 1......1 1]15×15;
WI(t)是一个15维的向量,如下所示:
WI(t)=[a1 a2 a3 a4 a5 a6 a7 a8 a9 a10 a11 a12 a13 a14 a15],
a1 a2 a3 a4 a5 a6 a7 a8 a9 a10 a11 a12 a13 a14 a15表示***过程噪声序列;
Z(t)为无人机在INS惯性导航***中的位置速度信息与无人机在GPS导航***中的位置速度信息的差值,是一个6维向量,
Z(t)=[δvx+Nvx δvy+Nvy δvz+Nvz (RM+h)δL+Ny (RM+h)cosLδλ+Nx δh+Nh]T,其中,Nvx表示GPS导航***在x方向上的速度误差,Nvy表示GPS导航***在y方向上的速度误差,Nvz表示GPS导航***在z方向上的速度误差,Nx表示GPS导航***在x方向上的位置误差,Ny表示GPS导航***在y方向上的位置误差,Nh表示GPS导航***在z方向上的位置误差;
其中
Vv(t)=[Nvx Nvy Nvz]T
Vp(t)=[Nx Ny Nz]T
G、将上述得到的连续状态方程离散化后得到Xk=Φk,k-1Xk-1+Wk-1,其中
将上述得到的连续观测方程Z(t)=H(t)XI(t)+V(t)离散化后得到Zk=HkXk+Vk;
其中I是单位矩阵,F是INS惯性导航***的状态转移矩阵,Δt是离散化后INS惯性导航***的采样时间;
H、将无人机在INS惯性导航***中的位置速度信息与无人机在GPS导航***中的位置速度信息作差得到Z(t)在k时刻的观测信息z;
I、计算k时刻INS惯性导航***状态的最优估计值其中, 为在k-1时刻,INS惯性导航***状态的最优估计值,Qk-1是INS惯性导航***的噪声矩阵,其大小是由INS惯性导航元件的性能决定, Rk是***测量噪声的方差阵,其大小是由GPS接收机的性能决定;
J、将计算得到的值与无人机在INS惯性导航***中的位置速度信息作差得到最优的导航参数;
K、重复步骤H-J,得到连续的无人机导航信息。
2.如权利要求1所述的无人机导航***,其特征在于:所述陀螺仪、加速度计、磁力计、分别与控制中心通过I2C总线连接。
3.如权利要求2所述的无人机导航***,其特征在于:所述GPS导航***模块与控制中心通过USART串口连接。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610242663.3A CN105928515B (zh) | 2016-04-19 | 2016-04-19 | 一种无人机导航*** |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610242663.3A CN105928515B (zh) | 2016-04-19 | 2016-04-19 | 一种无人机导航*** |
Publications (2)
Publication Number | Publication Date |
---|---|
CN105928515A CN105928515A (zh) | 2016-09-07 |
CN105928515B true CN105928515B (zh) | 2019-03-29 |
Family
ID=56838325
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201610242663.3A Active CN105928515B (zh) | 2016-04-19 | 2016-04-19 | 一种无人机导航*** |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN105928515B (zh) |
Families Citing this family (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106980133A (zh) * | 2017-01-18 | 2017-07-25 | 中国南方电网有限责任公司超高压输电公司广州局 | 利用神经网络算法补偿和修正的gps ins组合导航方法及*** |
CN107783155A (zh) * | 2017-10-19 | 2018-03-09 | 杨锐 | 一种低成本无人机抗诱捕导航*** |
CN107861135B (zh) * | 2017-10-26 | 2021-06-11 | 国家电网公司 | 一种面向电力巡检的无人机卫星导航欺骗检测方法 |
CN109656260A (zh) * | 2018-12-03 | 2019-04-19 | 北京采立播科技有限公司 | 一种无人机地理信息数据采集*** |
CN110032201A (zh) * | 2019-04-19 | 2019-07-19 | 成都飞机工业(集团)有限责任公司 | 一种基于卡尔曼滤波的imu机载视觉姿态融合的方法 |
CN110471027A (zh) * | 2019-07-23 | 2019-11-19 | 湖南交工智能技术有限公司 | 无人机盲区环境下检测的导航方法 |
CN110440805B (zh) * | 2019-08-09 | 2021-09-21 | 深圳市道通智能航空技术股份有限公司 | 一种偏航角的融合方法、装置及飞行器 |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101000245A (zh) * | 2007-01-10 | 2007-07-18 | 北京航空航天大学 | 一种sins/gps/磁罗盘组合导航***的数据融合方法 |
JP4941199B2 (ja) * | 2007-09-25 | 2012-05-30 | ヤマハ株式会社 | ナビゲーション装置 |
CN105021183A (zh) * | 2015-07-05 | 2015-11-04 | 电子科技大学 | 多旋翼飞行器gps和ins低成本组合导航*** |
-
2016
- 2016-04-19 CN CN201610242663.3A patent/CN105928515B/zh active Active
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101000245A (zh) * | 2007-01-10 | 2007-07-18 | 北京航空航天大学 | 一种sins/gps/磁罗盘组合导航***的数据融合方法 |
JP4941199B2 (ja) * | 2007-09-25 | 2012-05-30 | ヤマハ株式会社 | ナビゲーション装置 |
CN105021183A (zh) * | 2015-07-05 | 2015-11-04 | 电子科技大学 | 多旋翼飞行器gps和ins低成本组合导航*** |
Non-Patent Citations (2)
Title |
---|
"Information Fusion Distributed Navigation for UAVs Formation Flight";ZHEN Ziyang;《Proceedings of 2014 IEEE Chinese Guidance, Navigation and Control Conference》;20140810;正文第34-37页 |
"一种基于INS /GPS的无人机组合导航控制***的设计";何清华;《飞航导弹》;20071231(第2期);正文第1520-1525页 |
Also Published As
Publication number | Publication date |
---|---|
CN105928515A (zh) | 2016-09-07 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN105928515B (zh) | 一种无人机导航*** | |
CN107655476B (zh) | 基于多信息融合补偿的行人高精度足部导航方法 | |
CN107588769B (zh) | 一种车载捷联惯导、里程计及高程计组合导航方法 | |
CN110780326A (zh) | 一种车载组合导航***和定位方法 | |
CN103245360B (zh) | 晃动基座下的舰载机旋转式捷联惯导***自对准方法 | |
CN105043415B (zh) | 基于四元数模型的惯性系自对准方法 | |
CN100516775C (zh) | 一种捷联惯性导航***初始姿态确定方法 | |
CN110926468B (zh) | 基于传递对准的动中通天线多平台航姿确定方法 | |
CN108594283B (zh) | Gnss/mems惯性组合导航***的自由安装方法 | |
CN102176041B (zh) | 一种基于gnss/sins组合的车辆导航监控*** | |
CN103389092B (zh) | 一种系留飞艇姿态测量装置及测量方法 | |
JP5602070B2 (ja) | 位置標定装置、位置標定装置の位置標定方法および位置標定プログラム | |
CN111426320B (zh) | 一种基于图像匹配/惯导/里程计的车辆自主导航方法 | |
CN105929836B (zh) | 用于四旋翼飞行器的控制方法 | |
CN104880189B (zh) | 一种动中通天线低成本跟踪抗干扰方法 | |
CN112432642B (zh) | 一种重力灯塔与惯性导航融合定位方法及*** | |
CN112066985B (zh) | 一种组合导航***初始化方法、装置、介质及电子设备 | |
CN105928519B (zh) | 基于ins惯性导航与gps导航以及磁力计的导航算法 | |
CN105910623B (zh) | 利用磁强计辅助gnss/mins紧组合***进行航向校正的方法 | |
CN111121766A (zh) | 一种基于星光矢量的天文与惯性组合导航方法 | |
CN110849360B (zh) | 面向多机协同编队飞行的分布式相对导航方法 | |
CN108151765B (zh) | 一种在线实时估计补偿磁强计误差的定位测姿方法 | |
CN103278165A (zh) | 基于剩磁标定的磁测及星光备份的自主导航方法 | |
CN103994766A (zh) | 一种抗gps失效固定翼无人机定向方法 | |
US20170016729A1 (en) | Method and device for determining navigation data |
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 | ||
PE01 | Entry into force of the registration of the contract for pledge of patent right | ||
PE01 | Entry into force of the registration of the contract for pledge of patent right |
Denomination of invention: A UAV navigation system Effective date of registration: 20220125 Granted publication date: 20190329 Pledgee: Bank of China Limited by Share Ltd. Chengdu hi tech Industrial Development Zone sub branch Pledgor: CHENGDU EBIT AUTOMATION EQUIPMENT Co.,Ltd. Registration number: Y2022510000024 |