Movatterモバイル変換

[0]ホーム
URL:

CN106767797B - inertial/GPS combined navigation method based on dual quaternion - Google Patents

inertial/GPS combined navigation method based on dual quaternionDownload PDF

Info

Publication number
CN106767797B
CN106767797BCN201710178159.6ACN201710178159ACN106767797BCN 106767797 BCN106767797 BCN 106767797BCN 201710178159 ACN201710178159 ACN 201710178159ACN 106767797 BCN106767797 BCN 106767797B
Authority
CN
China
Prior art keywords
quaternion
matrix
dual
inertial
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
Application number
CN201710178159.6A
Other languages
Chinese (zh)
Other versions
CN106767797A (en
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.)
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing University of Aeronautics and Astronautics
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 Nanjing University of Aeronautics and AstronauticsfiledCriticalNanjing University of Aeronautics and Astronautics
Priority to CN201710178159.6ApriorityCriticalpatent/CN106767797B/en
Publication of CN106767797ApublicationCriticalpatent/CN106767797A/en
Application grantedgrantedCritical
Publication of CN106767797BpublicationCriticalpatent/CN106767797B/en
Activelegal-statusCriticalCurrent
Anticipated expirationlegal-statusCritical

Links

Images

Classifications

Landscapes

Abstract

Translated fromChinese

本发明公开了一种基于对偶四元数的惯性/GPS组合导航方法，属于飞行器组合导航技术领域。该方法首先建立陀螺和加速度计误差模型，并将陀螺及加速度计误差扩展为系统状态变量，构建基于对偶四元数的惯性/GPS组合卡尔曼滤波状态方程；随后，结合GPS的量测信息及对偶四元数捷联惯导算法的计算值，构建基于对偶四元数的惯性/GPS组合卡尔曼滤波量测方程；最后，对系统状态方程和量测方程进行离散化处理，采用卡尔曼滤波进行闭环估计并对惯导算法中的推力速度对偶四元数、引力速度对偶四元数、位置对偶四元数进行修正，从而得到载体的导航信息。本方法能够有效利用GPS的输出信息对惯导解算误差进行修正，提高惯性导航系统性能，适用于工程应用。

Figure 201710178159

The invention discloses an inertial/GPS combined navigation method based on dual quaternion, and belongs to the technical field of aircraft combined navigation. The method firstly establishes the gyro and accelerometer error models, expands the gyro and accelerometer errors into system state variables, and constructs the inertial/GPS combined Kalman filter state equation based on dual quaternion; The calculated value of the dual quaternion SINS algorithm is used to construct the inertial/GPS combined Kalman filter measurement equation based on dual quaternion; finally, the system state equation and measurement equation are discretized, and Kalman filter is used. Carry out closed-loop estimation and correct the thrust-velocity dual quaternion, gravitational-velocity dual quaternion, and position dual quaternion in the inertial navigation algorithm, so as to obtain the navigation information of the carrier. The method can effectively use the GPS output information to correct the inertial navigation solution error, improve the performance of the inertial navigation system, and is suitable for engineering applications.

Figure 201710178159

Description

Translated fromChinese
一种基于对偶四元数的惯性/GPS组合导航方法An Inertial/GPS Integrated Navigation Method Based on Dual Quaternion

技术领域technical field

本发明涉及一种基于对偶四元数的惯性/GPS组合导航方法，属于飞行器组合导航技术领域。The invention relates to an inertial/GPS combined navigation method based on dual quaternion, and belongs to the technical field of aircraft combined navigation.

背景技术Background technique

近年来，随着高超声速飞行器等高动态飞行器的研制发展，对导航系统性能提出了更高的要求。惯性导航系统具有短时精度高、输出连续以及完全自主等突出优点，但其误差会随时间累积，需要其他导航手段加以辅助。In recent years, with the development of highly dynamic aircraft such as hypersonic aircraft, higher requirements have been placed on the performance of the navigation system. The inertial navigation system has outstanding advantages such as high short-term accuracy, continuous output, and complete autonomy, but its errors will accumulate over time, requiring other navigation methods to assist.

GPS全球定位系统是一种高精度的全球三维实时卫星系统，其导航定位的全球性和高精度使其成为一种先进的导航设备。但是GPS全球定位系统也存在一些不足之处，主要是GPS在受到遮挡的情况下容易信号丢失，且容易受到人为控制和干扰，因此主要作为一种辅助导航设备使用。惯性/GPS组合克服了各自缺点，取长补短，使组合后的导航精度高于两个系统单独工作的精度。The GPS global positioning system is a high-precision global three-dimensional real-time satellite system. The globality and high precision of its navigation and positioning make it an advanced navigation device. However, GPS global positioning system also has some shortcomings, mainly because GPS is easy to lose signal when it is blocked, and it is easy to be controlled and interfered by humans, so it is mainly used as an auxiliary navigation device. The inertial/GPS combination overcomes their respective shortcomings and learns from each other, so that the combined navigation accuracy is higher than the accuracy of the two systems working alone.

对偶四元数捷联惯性导航算法将载体的旋转和平移统一考虑，以最简洁的形式表示一般的刚体运动，在高动态环境下，具有比传统捷联惯导算法更高的精度，更能满足高动态飞行器对高精度导航性能的要求。传统的惯性导航算法误差模型通常是基于数学平台失准角的线性误差方程，但是该模型仅适用于平台失准角为小量的情况，当载体的姿态角误差较大时，在组合导航算法中使用该模型会使滤波精度大大降低，收敛时间变长甚至会导致发散。基于对偶四元数的捷联惯性导航算法可以直接利用对偶四元数建立线性误差模型，即使在大失准角的情况也同样能具有较快的收敛速度，较好的滤波精度，从而提高了组合导航系统性能。因此，研究基于对偶四元数的惯性/GPS组合导航算法具有重要的研究意义。The dual quaternion strapdown inertial navigation algorithm considers the rotation and translation of the carrier uniformly, and represents the general rigid body motion in the most concise form. It meets the requirements of high-dynamic aircraft for high-precision navigation performance. The traditional inertial navigation algorithm error model is usually based on the linear error equation of the mathematical platform misalignment angle, but this model is only suitable for the case where the platform misalignment angle is small. When the attitude angle error of the carrier is large, the integrated navigation algorithm is used. Using this model in the filter will greatly reduce the filtering accuracy, and the convergence time will be longer and even lead to divergence. The strapdown inertial navigation algorithm based on the dual quaternion can directly use the dual quaternion to establish a linear error model. Even in the case of a large misalignment angle, it can also have a faster convergence speed and better filtering accuracy. Integrated Navigation System Performance. Therefore, it is of great significance to study the inertial/GPS integrated navigation algorithm based on dual quaternion.

发明内容SUMMARY OF THE INVENTION

本发明提出了一种基于对偶四元数的惯性/GPS组合导航方法，在飞行器动态飞行过程中有效利用GPS提供的速度位置信息，对惯性导航解算参数误差进行修正，显著提高惯性导航系统精度。The invention proposes an inertial/GPS combined navigation method based on dual quaternion, which effectively utilizes the speed and position information provided by GPS in the dynamic flight process of the aircraft to correct the error of the inertial navigation solution parameters and significantly improve the precision of the inertial navigation system. .

本发明为解决其技术问题采用如下技术方案：The present invention adopts following technical scheme for solving its technical problem:

一种基于对偶四元数的惯性/GPS组合导航方法，包括如下步骤：A dual quaternion-based inertial/GPS combined navigation method, comprising the following steps:

步骤1，建立陀螺、加速度计误差模型，所述陀螺误差包括常值漂移误差、一阶马尔科夫过程随机噪声以及白噪声随机误差，所述加速度计误差为一阶马尔科夫过程随机噪声；Step 1, establishing an error model of a gyro and an accelerometer, the gyro error includes a constant drift error, a first-order Markov process random noise and a white noise random error, and the accelerometer error is a first-order Markov process random noise;

步骤2，在步骤1对陀螺和加速度计误差建模的基础上，将步骤1所述的陀螺常值漂移误差、陀螺一阶马尔科夫过程随机噪声、加速度计一阶马尔科夫过程随机噪声扩展为系统状态变量，构建基于对偶四元数的惯性/GPS组合卡尔曼滤波状态方程；Step 2: On the basis of the error modeling of the gyro and the accelerometer in step 1, the constant drift error of the gyro, the first-order Markov process random noise of the gyro, and the first-order Markov process random noise of the accelerometer described in step 1 are combined. Expanded to system state variables, constructing inertial/GPS combined Kalman filter state equation based on dual quaternion;

步骤3，将GPS输出的地理系速度、地球系位置测量误差建模为白噪声，并将其测得的地理系速度转化为惯性系速度，结合由对偶四元数捷联惯导算法计算得到的惯性系速度及地球系位置，构建基于对偶四元数的惯性/GPS组合卡尔曼滤波量测方程；Step 3: Model the geographic system speed and the earth system position measurement error output by the GPS as white noise, and convert the measured geographic system speed into the inertial system speed, which is calculated by the dual quaternion strapdown inertial navigation algorithm. The inertial system velocity and the earth system position based on the inertial system velocity and the earth system position, construct the inertial/GPS combined Kalman filter measurement equation based on dual quaternion;

步骤4，对系统状态方程和量测方程进行离散化处理，并采用卡尔曼滤波对状态量进行闭环估计，利用估计所得的对偶四元数误差对惯导算法中的推力对偶四元数、引力对偶四元数、位置对偶四元数进行修正，从而得到载体的速度、位置、姿态等导航信息。Step 4: Discretize the state equation and measurement equation of the system, and use Kalman filter to perform closed-loop estimation of the state quantity, and use the estimated dual quaternion error to calculate the thrust dual quaternion and gravity in the inertial navigation algorithm. The dual quaternion and the position dual quaternion are corrected to obtain the navigation information such as the speed, position and attitude of the carrier.

步骤1所述陀螺和加速度计误差模型为：The error model of the gyro and accelerometer described in step 1 is:

Figure BDA0001252914680000021
Figure BDA0001252914680000021

Figure BDA0001252914680000022
Figure BDA0001252914680000022

其中，

Figure BDA0001252914680000031
为陀螺误差，εb为陀螺常值漂移误差，εr为陀螺一阶马尔科夫过程随机噪声，ωg为白噪声；δfB为加速度计误差，
Figure BDA0001252914680000032
为加速度计一阶马尔科夫过程随机噪声；in,
Figure BDA0001252914680000031
is the gyro error, εb is the gyro constant drift error, εr is the first-order Markov process random noise of the gyro, ωg is white noise; δfB is the accelerometer error,
Figure BDA0001252914680000032
is the first-order Markov process random noise of the accelerometer;

对上式中的εb、εr、

Figure BDA0001252914680000033
进行求导后可得到以下数学表达式：For εb , εr ,
Figure BDA0001252914680000033
After derivation, the following mathematical expression can be obtained:

Figure BDA0001252914680000034
Figure BDA0001252914680000034

Figure BDA0001252914680000035
Figure BDA0001252914680000035

Figure BDA0001252914680000036
Figure BDA0001252914680000036

其中，

Figure BDA0001252914680000037
为εb的一阶导数；
Figure BDA0001252914680000038
为εr的一阶导数；
Figure BDA0001252914680000039
为
Figure BDA00012529146800000310
的一阶导数；Tg为陀螺一阶马尔科夫过程相关时间，ωr为陀螺一阶马尔科夫过程驱动白噪声；Ta为加速度计一阶马尔科夫过程相关时间，ωa为加速度计一阶马尔科夫过程驱动白噪声。in,
Figure BDA0001252914680000037
is the first derivative of εb ;
Figure BDA0001252914680000038
is the first derivative ofεr ;
Figure BDA0001252914680000039
for
Figure BDA00012529146800000310
Tg is the first-order Markov process correlation time of the gyro, ωr is the first-order Markov process driving white noise of the gyro; Ta is the first-order Markov process correlation time of the accelerometer, and ωa is the acceleration Calculate the first-order Markov process-driven white noise.

步骤2所述基于对偶四元数的惯性/GPS组合卡尔曼滤波状态方程为：The state equation of inertia/GPS combined Kalman filter based on dual quaternion described in step 2 is:

Figure BDA00012529146800000311
Figure BDA00012529146800000311

其中X∈R28×1为系统状态量，F∈R28×28为系统矩阵，G∈R28×12为噪声系数矩阵，w∈R12×1为系统噪声向量，

Figure BDA00012529146800000312
为X的一阶导数，各矩阵分别表示为：where X∈R28×1 is the system state quantity, F∈R28×28 is the system matrix, G∈R28×12 is the noise coefficient matrix, w∈R12×1 is the system noise vector,
Figure BDA00012529146800000312
is the first derivative of X, and each matrix is expressed as:

Figure BDA00012529146800000313
Figure BDA00012529146800000313

Figure BDA0001252914680000041
Figure BDA0001252914680000041

Figure BDA0001252914680000042
Figure BDA0001252914680000042

系统矩阵F和噪声系数矩阵G中，若将四元数q写成q＝[q0 q1 q2 q3]T的形式，我们定义矩阵

Figure BDA0001252914680000043
为q在四元数乘法中的前乘矩阵，
Figure BDA0001252914680000044
为q在四元数乘法中的后乘矩阵，其具体可表示为：In the system matrix F and the noise coefficient matrix G, if the quaternion q is written in the form of q=[q0 q1 q2 q3 ]T , we define the matrix
Figure BDA0001252914680000043
is the premultiplied matrix of q in the quaternion multiplication,
Figure BDA0001252914680000044
is the post-multiplication matrix of q in the quaternion multiplication, which can be specifically expressed as:

Figure BDA0001252914680000045
Figure BDA0001252914680000045

系统矩阵F和噪声系数矩阵G中0均为四阶零矩阵，I4为四阶单位矩阵，各变量均为四元数，其中三维向量表示为标量部分为0的四元数。δqIT为推力速度对偶四元数误差的实数部分，δq′IT为推力速度对偶四元数误差的对偶部分，δq′IG为引力速度对偶四元数误差的对偶部分，δq′IU为位置对偶四元数误差的对偶部分；

Figure BDA0001252914680000046
为陀螺输出信息，
Figure BDA0001252914680000047
为
Figure BDA0001252914680000048
在四元数乘法中的后乘矩阵；qIT为推力速度对偶四元数的实数部分，
Figure BDA0001252914680000049
为qIT在四元数乘法中的前乘矩阵；q′IT为推力对偶四元数的对偶部分，
Figure BDA0001252914680000051
为q′IT在四元数乘法中的前乘矩阵；
Figure BDA0001252914680000052
为地球自转角速度，
Figure BDA0001252914680000053
为
Figure BDA0001252914680000054
在四元数乘法中的后乘矩阵；qIU为位置对偶四元数的实数部分，
Figure BDA0001252914680000055
为其在四元数乘法中的前乘矩阵，
Figure BDA0001252914680000056
为其在四元数乘法中的后乘矩阵；q*IT为qIT的共轭四元数，
Figure BDA0001252914680000057
为其在四元数乘法中的前乘矩阵，
Figure BDA0001252914680000058
为其在四元数乘法中的后乘矩阵；In the system matrix F and the noise coefficient matrix G, 0 is a fourth-order zero matrix, I4 is a fourth-order unit matrix, and each variable is a quaternion, and the three-dimensional vector is expressed as a quaternion whose scalar part is 0. δqIT is the real part of the thrust-velocity dual quaternion error, δq′IT is the dual part of the thrust-velocity dual quaternion error, δq′IG is the dual part of the gravitational-velocity dual quaternion error, δq′IU is the position dual the dual part of the quaternion error;
Figure BDA0001252914680000046
output information for the gyroscope,
Figure BDA0001252914680000047
for
Figure BDA0001252914680000048
Post-multiplication matrix in quaternion multiplication; qIT is the real part of the thrust-velocity dual quaternion,
Figure BDA0001252914680000049
is the premultiplied matrix of qIT in the quaternion multiplication; q′IT is the dual part of the thrust dual quaternion,
Figure BDA0001252914680000051
is the premultiplied matrix of q′IT in the quaternion multiplication;
Figure BDA0001252914680000052
is the angular velocity of the Earth's rotation,
Figure BDA0001252914680000053
for
Figure BDA0001252914680000054
Postmultiplication matrix in quaternion multiplication; qIU is the real part of the positional dual quaternion,
Figure BDA0001252914680000055
is its premultiplied matrix in quaternion multiplication,
Figure BDA0001252914680000056
is its post-multiplication matrix in quaternion multiplication; q*IT is the conjugate quaternion of qIT ,
Figure BDA0001252914680000057
is its premultiplied matrix in quaternion multiplication,
Figure BDA0001252914680000058
is its postmultiplied matrix in quaternion multiplication;

系统矩阵F和噪声系数矩阵G中M1、M2、M3可分别表示为：M1 , M2 , and M3 in the system matrix F and the noise coefficient matrix G can be expressed as:

Figure BDA0001252914680000059
Figure BDA0001252914680000059

步骤3所述的GPS输出的地理系速度、地球系位置测量误差模型为：The geographic system speed of the GPS output described in step 3, the earth system position measurement error model is:

VG＝Vn+δV，VG =Vn +δV,

RG＝Re+δR，RG =Re +δR,

其中，VG为GPS输出的地理系速度，Vn为载体真实的地理系速度，δV为GPS的速度测量误差，将其建模为白噪声；RG为GPS测得的地球系位置，Re为载体真实的地球系位置，δR为GPS的位置测量误差，将其建模为白噪声；Among them, VG is the geographic system speed output by GPS, Vn is the real geographic system speed of the carrier, δV is the GPS speed measurement error, which is modeled as white noise; RG is the earth system position measured by GPS, Re is the real earth system position of the carrier, δR is the GPS position measurement error, which is modeled as white noise;

利用如下公式将VG转换到惯性系：Convert VG to the inertial frame using the following formula:

Figure BDA00012529146800000510
Figure BDA00012529146800000510

其中，VGI为VG转换到惯性系下的值，

Figure BDA00012529146800000513
为地球系到惯性系的转换矩阵，ωie为四元数
Figure BDA00012529146800000511
的矢量部分，
Figure BDA00012529146800000512
为地理系到地球系的转换矩阵；Among them, VGI is the value converted from VG to the inertial frame,
Figure BDA00012529146800000513
is the transformation matrix from the earth system to the inertial system, ωie is the quaternion
Figure BDA00012529146800000511
The vector part of ,
Figure BDA00012529146800000512
is the conversion matrix from the geographic system to the earth system;

由此可建立步骤3所述基于对偶四元数的惯性/GPS组合卡尔曼滤波量测方程：Thus, the dual quaternion-based inertial/GPS combined Kalman filter measurement equation described in step 3 can be established:

Z＝HX+v，Z=HX+v,

其中，

Figure BDA0001252914680000061
为量测向量，VI为对偶四元数惯导算法计算所得惯性系速度，RI为对偶四元数惯导算法计算所得地球系位置；H为量测系数矩阵，X为系统状态量，
Figure BDA0001252914680000062
为系统量测噪声阵；in,
Figure BDA0001252914680000061
is the measurement vector, VI is the inertial system velocity calculated by the dual quaternion inertial navigation algorithm, RI is the earth system position calculated by the dual quaternion inertial navigation algorithm; H is the measurement coefficient matrix, X is the system state quantity,
Figure BDA0001252914680000062
Measure the noise array for the system;

量测系数矩阵H的具体表达式为：The specific expression of the measurement coefficient matrix H is:

Figure BDA0001252914680000063
Figure BDA0001252914680000063

其中，qIG为引力速度对偶四元数的实数部分，

Figure BDA0001252914680000064
为qIG的共轭四元数，
Figure BDA0001252914680000065
为q*IG在四元数乘法中的后乘矩阵；qIU为位置对偶四元数的实数部分，*IU为qIU的共轭四元数，
Figure BDA0001252914680000066
为q*IU在四元数乘法中的前乘矩阵，
Figure BDA0001252914680000067
为q′IT在四元数乘法中的前乘矩阵。where qIG is the real part of the gravitational-velocity dual quaternion,
Figure BDA0001252914680000064
is the conjugate quaternion of qIG ,
Figure BDA0001252914680000065
is the post-multiplication matrix of q*IG in the quaternion multiplication; qIU is the real part of the positional dual quaternion,*IU is the conjugate quaternion of qIU ,
Figure BDA0001252914680000066
is the premultiplied matrix of q*IU in quaternion multiplication,
Figure BDA0001252914680000067
is the premultiplied matrix of q′IT in the quaternion multiplication.

所述步骤4的具体过程为：The specific process of step 4 is:

(401)将系统状态方程和量测方程离散化处理：(401) Discretize the system state equation and measurement equation:

Xk＝Φk,k-1Xk-1+Γk,k-1Wk-1，Xk =Φk,k-1 Xk-1 +Γk,k-1 Wk-1 ,

Zk＝HkXk+Vk，Zk =Hk Xk +Vk ,

其中，Xk为tk时刻系统状态量,Xk-1为tk-1时刻系统状态量，Φk,k-1为tk-1时刻至tk时刻系统的状态转移矩阵，Γk,k-1为tk-1时刻至tk时刻系统的噪声驱动矩阵，Wk-1为tk-1时刻系统的噪声矩阵，Zk为tk时刻系统的量测矩阵,Hk为tk时刻的量测系数矩阵，Vk为tk时刻的观测量噪声矩阵；Among them, Xk is the state quantity of the system at time tk , Xk-1 is the state quantity of the system at time tk-1 , Φk,k-1 is the state transition matrix of the system from time tk-1 to time tk , Γk , k-1 is the noise driving matrix of the system from time tk-1 to time tk , Wk-1 is the noise matrix of the system at time tk-1 , Zk is the measurement matrix of the system at time tk , Hk is The measurement coefficient matrix at time tk , and Vk is the noise matrix of observation at time tk ;

(402)采用卡尔曼滤波对状态量进行闭环估计：(402) Use Kalman filter to perform closed-loop estimation on the state quantity:

Figure BDA0001252914680000068
Figure BDA0001252914680000068

Figure BDA0001252914680000069
Figure BDA0001252914680000069

Figure BDA00012529146800000610
Figure BDA00012529146800000610

Figure BDA00012529146800000611
Figure BDA00012529146800000611

Figure BDA00012529146800000612
Figure BDA00012529146800000612

其中，

Figure BDA00012529146800000613
是状态量Xk-1的一步预测估计值,Pk-1为tk-1时刻滤波状态估计协方差矩阵,Qk-1为tk-1时刻系统噪声协方差矩阵,
Figure BDA0001252914680000071
为Φk,k-1的转置，
Figure BDA0001252914680000072
为Γk,k-1的转置，Pk,k-1为tk-1时刻到tk时刻的状态一步预测协方差矩阵,Rk为tk时刻的量测噪声协方差矩阵,Kk为tk时刻滤波增益矩阵,
Figure BDA0001252914680000073
为Hk的转置，
Figure BDA0001252914680000074
为状态量Xk的卡尔曼滤波估值，I为单位矩阵，
Figure BDA0001252914680000075
为Kk的转置，Pk为tk时刻滤波状态估计协方差矩阵；in,
Figure BDA00012529146800000613
is the one-step prediction estimate value of the state quantity Xk-1 , Pk-1 is the estimated covariance matrix of the filter state at time tk-1 , Qk-1 is the covariance matrix of the system noise at time tk-1 ,
Figure BDA0001252914680000071
is the transpose of Φk,k-1 ,
Figure BDA0001252914680000072
is the transpose of Γk,k-1 , Pk,k-1 is the state one-step prediction covariance matrix from time tk-1 to time tk , Rk is the measurement noise covariance matrix at time tk , Kk is the filter gain matrix at time tk ,
Figure BDA0001252914680000073
is the transpose of Hk ,
Figure BDA0001252914680000074
is the Kalman filter estimate of the state quantity Xk , I is the identity matrix,
Figure BDA0001252914680000075
is the transpose of Kk , and Pk is the estimated covariance matrix of the filtering state at time tk ;

(403)在(402)得到各状态量估计值后，利用估计所得的对偶四元数误差对惯导算法中的推力对偶四元数、引力对偶四元数、位置对偶四元数进行修正，修正模型为：(403) After obtaining the estimated value of each state quantity in (402), use the estimated dual quaternion error to correct the thrust dual quaternion, gravity dual quaternion, and position dual quaternion in the inertial navigation algorithm, The corrected model is:

Figure BDA0001252914680000076
Figure BDA0001252914680000076

Figure BDA0001252914680000077
Figure BDA0001252914680000077

Figure BDA0001252914680000078
Figure BDA0001252914680000078

Figure BDA0001252914680000079
Figure BDA0001252914680000079

上式中，

Figure BDA00012529146800000710
为δqIT的估计值，
Figure BDA00012529146800000711
为修正后的推力速度对偶四元数的实数部分；
Figure BDA00012529146800000712
为δq′IT的估计值，
Figure BDA00012529146800000713
为修正后的推力速度对偶四元数的对偶部分；
Figure BDA00012529146800000714
为δq′IG的估计值，
Figure BDA00012529146800000715
为修正后的引力速度对偶四元数的对偶部分；
Figure BDA00012529146800000716
为δq′IU的估计值，
Figure BDA00012529146800000717
为修正后的位置对偶四元数的对偶部分。In the above formula,
Figure BDA00012529146800000710
is the estimated value of δqIT ,
Figure BDA00012529146800000711
is the real part of the corrected thrust-velocity dual quaternion;
Figure BDA00012529146800000712
is the estimated value of δq′IT ,
Figure BDA00012529146800000713
is the dual part of the corrected thrust-velocity dual quaternion;
Figure BDA00012529146800000714
is the estimated value of δq′IG ,
Figure BDA00012529146800000715
is the dual part of the corrected gravitational velocity dual quaternion;
Figure BDA00012529146800000716
is the estimated value of δq′IU ,
Figure BDA00012529146800000717
is the dual part of the corrected positional dual quaternion.

本发明的有益效果如下：The beneficial effects of the present invention are as follows:

1、基于对偶四元数的惯性/GPS算法可以直接利用对偶四元数建立线性误差模型，使得该组合导航系统即使在大失准角的情况下也同样能具有较快的滤波收敛速度和较好的滤波精度，从而提高了组合导航系统性能，适合工程应用。1. The dual quaternion-based inertial/GPS algorithm can directly use the dual quaternion to establish a linear error model, so that the integrated navigation system can also have a faster filter convergence speed and a relatively high degree of misalignment even in the case of a large misalignment angle. Good filtering accuracy, thereby improving the performance of the integrated navigation system, suitable for engineering applications.

2、本发明方法给出了组合前GPS输出信息的转换方法，并给出了坐标转化后的系统量测噪声模型，该模型可以有效提高对偶四元数惯性/GPS组合导航精度，适合工程应用。2. The method of the present invention provides a conversion method of GPS output information before the combination, and provides a system measurement noise model after coordinate conversion. This model can effectively improve the dual quaternion inertia/GPS combined navigation accuracy and is suitable for engineering applications. .

附图说明Description of drawings

图1是本发明基于对偶四元数的惯性/GPS组合导航方法的架构图。FIG. 1 is an architecture diagram of an inertial/GPS integrated navigation method based on dual quaternions of the present invention.

具体实施方式Detailed ways

下面结合附图对本发明创造做进一步详细说明。The present invention will be further described in detail below with reference to the accompanying drawings.

如图1所示，本发明所述基于对偶四元数的惯性/GPS组合导航方法的原理是：GPS输出地理系速度及地球系位置信息，将其输出的地理系速度经坐标转换为惯性系速度。同时，对偶四元数捷联惯导算法也可以相应得到载体在惯性系下的速度信息及地球系下的位置信息。利用卡尔曼滤波器融合GPS和对偶四元数捷联惯导算法的地球系位置、惯性系速度，在线估计得到对偶四元数误差参数，利用该信息对对偶四元数进行补偿修正，以此提高惯性导航系统性能。As shown in Figure 1, the principle of the inertial/GPS combined navigation method based on dual quaternion of the present invention is: GPS outputs geographic system speed and earth system position information, and the output geographic system speed is converted into inertial system through coordinates speed. At the same time, the dual quaternion strapdown inertial navigation algorithm can also correspondingly obtain the velocity information of the carrier in the inertial frame and the position information in the earth frame. The Kalman filter is used to fuse the earth system position and inertial system velocity of GPS and dual quaternion SINS algorithm, and the dual quaternion error parameter is estimated online, and the dual quaternion is compensated and corrected by using this information. Improve inertial navigation system performance.

本发明的具体实施方式如下：The specific embodiments of the present invention are as follows:

1、建立陀螺、加速度计误差模型1. Establish the error model of gyroscope and accelerometer

陀螺误差包括常值漂移误差、一阶马尔科夫过程随机噪声及白噪声误差，加速度计误差建模为一阶马尔科夫过程随机噪声，具体误差模型如下：The gyro error includes constant drift error, first-order Markov process random noise and white noise error. The accelerometer error is modeled as first-order Markov process random noise. The specific error model is as follows:

Figure BDA0001252914680000081
Figure BDA0001252914680000081

Figure BDA0001252914680000082
Figure BDA0001252914680000082

其中，

Figure BDA0001252914680000083
为陀螺误差，εb为陀螺常值漂移误差，εr为陀螺一阶马尔科夫过程随机噪声，ωg为白噪声；δfB为加速度计误差，
Figure BDA0001252914680000084
为加速度计一阶马尔科夫过程随机噪声；in,
Figure BDA0001252914680000083
is the gyro error, εb is the gyro constant drift error, εr is the first-order Markov process random noise of the gyro, ωg is white noise; δfB is the accelerometer error,
Figure BDA0001252914680000084
is the first-order Markov process random noise of the accelerometer;

对上式中的εb、εr、

Figure BDA0001252914680000085
进行求导后可得到以下数学表达式：For εb , εr ,
Figure BDA0001252914680000085
After derivation, the following mathematical expression can be obtained:

Figure BDA0001252914680000086
Figure BDA0001252914680000086

Figure BDA0001252914680000087
Figure BDA0001252914680000087

Figure BDA0001252914680000088
Figure BDA0001252914680000088

其中，

Figure BDA0001252914680000091
为εb的一阶导数；
Figure BDA0001252914680000092
为εr的一阶导数；
Figure BDA0001252914680000093
为
Figure BDA0001252914680000094
的一阶导数；Tg为陀螺一阶马尔科夫过程相关时间，ωr为陀螺一阶马尔科夫过程驱动白噪声；Ta为加速度计一阶马尔科夫过程相关时间，ωa为加速度计一阶马尔科夫过程驱动白噪声。in,
Figure BDA0001252914680000091
is the first derivative of εb ;
Figure BDA0001252914680000092
is the first derivative ofεr ;
Figure BDA0001252914680000093
for
Figure BDA0001252914680000094
Tg is the first-order Markov process correlation time of the gyro, ωr is the first-order Markov process driving white noise of the gyro; Ta is the first-order Markov process correlation time of the accelerometer, and ωa is the acceleration Calculate the first-order Markov process-driven white noise.

2、建立基于陀螺、加速度计误差模型的卡尔曼滤波状态方程2. Establish the Kalman filter state equation based on the gyro and accelerometer error models

状态方程如下所示：The equation of state looks like this:

Figure BDA0001252914680000095
Figure BDA0001252914680000095

其中X∈R28×1为系统状态量，F∈R28×28为系统矩阵，G∈R28×12为噪声系数矩阵，w∈R12×1为系统噪声向量，

Figure BDA0001252914680000096
为X的一阶导数，各矩阵分别表示为：where X∈R28×1 is the system state quantity, F∈R28×28 is the system matrix, G∈R28×12 is the noise coefficient matrix, w∈R12×1 is the system noise vector,
Figure BDA0001252914680000096
is the first derivative of X, and each matrix is expressed as:

Figure BDA0001252914680000097
Figure BDA0001252914680000097

Figure BDA0001252914680000098
Figure BDA0001252914680000098

Figure BDA0001252914680000099
Figure BDA0001252914680000099

Figure BDA0001252914680000101
Figure BDA0001252914680000101

系统矩阵F和噪声系数矩阵G中，若将四元数q写成q＝[q0 q1 q2 q3]T的形式，我们定义矩阵

Figure BDA0001252914680000102
为q在四元数乘法中的前乘矩阵，
Figure BDA0001252914680000103
为q在四元数乘法中的后乘矩阵，其具体可表示为：In the system matrix F and the noise coefficient matrix G, if the quaternion q is written in the form of q=[q0 q1 q2 q3 ]T , we define the matrix
Figure BDA0001252914680000102
is the premultiplied matrix of q in the quaternion multiplication,
Figure BDA0001252914680000103
is the post-multiplication matrix of q in the quaternion multiplication, which can be specifically expressed as:

Figure BDA0001252914680000104
Figure BDA0001252914680000104

Figure BDA0001252914680000105
Figure BDA0001252914680000105

式(7)～(10)中0均为四阶零矩阵，I4为四阶单位矩阵，各变量均为四元数，其中三维向量表示为标量部分为0的四元数。δqIT为推力速度对偶四元数误差的实数部分，δq′IT为推力速度对偶四元数误差的对偶部分，δq′IG为引力速度对偶四元数误差的对偶部分，δq′IU为位置对偶四元数误差的对偶部分；

Figure BDA0001252914680000106
为陀螺输出信息，
Figure BDA0001252914680000107
为
Figure BDA0001252914680000108
在四元数乘法中的后乘矩阵；qIT为推力速度对偶四元数的实数部分，
Figure BDA0001252914680000109
为qIT在四元数乘法中的前乘矩阵；q′IT为推力对偶四元数的对偶部分，
Figure BDA00012529146800001010
为q′IT在四元数乘法中的前乘矩阵；
Figure BDA00012529146800001011
为地球自转角速度，
Figure BDA00012529146800001012
为
Figure BDA00012529146800001013
在四元数乘法中的后乘矩阵；qIU为位置对偶四元数的实数部分，
Figure BDA00012529146800001014
为其在四元数乘法中的前乘矩阵，
Figure BDA00012529146800001015
为其在四元数乘法中的后乘矩阵；q*IT为qIT的共轭四元数，
Figure BDA0001252914680000111
为其在四元数乘法中的前乘矩阵，
Figure BDA0001252914680000112
为其在四元数乘法中的后乘矩阵；In equations (7) to (10), 0 is a fourth-order zero matrix, I4 is a fourth-order unit matrix, and each variable is a quaternion, where the three-dimensional vector is represented as a quaternion whose scalar part is 0. δqIT is the real part of the thrust-velocity dual quaternion error, δq′IT is the dual part of the thrust-velocity dual quaternion error, δq′IG is the dual part of the gravitational-velocity dual quaternion error, δq′IU is the position dual the dual part of the quaternion error;
Figure BDA0001252914680000106
output information for the gyroscope,
Figure BDA0001252914680000107
for
Figure BDA0001252914680000108
Post-multiplication matrix in quaternion multiplication; qIT is the real part of the thrust-velocity dual quaternion,
Figure BDA0001252914680000109
is the premultiplied matrix of qIT in the quaternion multiplication; q′IT is the dual part of the thrust dual quaternion,
Figure BDA00012529146800001010
is the premultiplied matrix of q′IT in the quaternion multiplication;
Figure BDA00012529146800001011
is the angular velocity of the Earth's rotation,
Figure BDA00012529146800001012
for
Figure BDA00012529146800001013
Postmultiplication matrix in quaternion multiplication; qIU is the real part of the positional dual quaternion,
Figure BDA00012529146800001014
is its premultiplied matrix in quaternion multiplication,
Figure BDA00012529146800001015
is its post-multiplication matrix in quaternion multiplication; q*IT is the conjugate quaternion of qIT ,
Figure BDA0001252914680000111
is its premultiplied matrix in quaternion multiplication,
Figure BDA0001252914680000112
is its postmultiplied matrix in quaternion multiplication;

式(9)和式(10)中M1、M2、M3可分别表示为：M1 , M2 , and M3 in formula (9) and formula (10) can be expressed as:

Figure BDA0001252914680000113
Figure BDA0001252914680000113

Figure BDA0001252914680000114
Figure BDA0001252914680000114

Figure BDA0001252914680000115
Figure BDA0001252914680000115

3、建立基于GPS测量误差模型的卡尔曼滤波量测方程3. Establish a Kalman filter measurement equation based on the GPS measurement error model

(3.1)GPS测量误差模型(3.1) GPS measurement error model

GPS测量误差模型为：The GPS measurement error model is:

VG＝Vn+δV (16)VG =Vn +δV (16)

RG＝Re+δR (17)RG =Re +δR (17)

其中，VG为GPS输出的地理系速度，Vn为载体真实的地理系速度，δV为GPS的速度测量误差，将其建模为白噪声；RG为GPS测得的地球系位置，Re为载体真实的地球系位置，δR为GPS的位置测量误差，将其建模为白噪声；Among them, VG is the geographic system speed output by GPS, Vn is the real geographic system speed of the carrier, δV is the GPS speed measurement error, which is modeled as white noise; RG is the earth system position measured by GPS, Re is the real earth system position of the carrier, δR is the GPS position measurement error, which is modeled as white noise;

利用如下公式将VG转换到惯性系：Convert VG to the inertial frame using the following formula:

Figure BDA0001252914680000116
Figure BDA0001252914680000116

其中，VGI为VG转换到惯性系下的值，

Figure BDA0001252914680000121
为地球系到惯性系的转换矩阵，ωie为四元数
Figure BDA0001252914680000122
的矢量部分，
Figure BDA0001252914680000123
为地理系到地球系的转换矩阵；Among them, VGI is the value converted from VG to the inertial frame,
Figure BDA0001252914680000121
is the transformation matrix from the earth system to the inertial system, ωie is the quaternion
Figure BDA0001252914680000122
The vector part of ,
Figure BDA0001252914680000123
is the conversion matrix from the geographic system to the earth system;

(3.2)建立卡尔曼滤波测量方程(3.2) Establish Kalman filter measurement equation

量测方程如下所示：The measurement equation is as follows:

Z＝HX+v (19)Z=HX+v (19)

其中，

Figure BDA0001252914680000124
为量测向量，VI为对偶四元数惯导算法计算所得惯性系速度，RI为对偶四元数惯导算法计算所得地球系位置；H为量测系数矩阵，X为系统状态量，
Figure BDA0001252914680000125
为系统量测噪声阵；in,
Figure BDA0001252914680000124
is the measurement vector, VI is the inertial system velocity calculated by the dual quaternion inertial navigation algorithm, RI is the earth system position calculated by the dual quaternion inertial navigation algorithm; H is the measurement coefficient matrix, X is the system state quantity,
Figure BDA0001252914680000125
Measure the noise array for the system;

量测系数矩阵H的具体表达式为：The specific expression of the measurement coefficient matrix H is:

Figure BDA0001252914680000126
Figure BDA0001252914680000126

其中，qIG为引力速度对偶四元数的实数部分，q*IG为qIG的共轭四元数，

Figure BDA0001252914680000127
为q*IG在四元数乘法中的后乘矩阵；qIU为位置对偶四元数的实数部分，q*IU为qIU的共轭四元数，
Figure BDA0001252914680000128
为q*IU在四元数乘法中的前乘矩阵，
Figure BDA0001252914680000129
为q′IT在四元数乘法中的前乘矩阵。where qIG is the real part of the gravitational velocity dual quaternion, q*IG is the conjugate quaternion of qIG ,
Figure BDA0001252914680000127
is the post-multiplication matrix of q*IG in the quaternion multiplication; qIU is the real part of the positional dual quaternion, q*IU is the conjugate quaternion of qIU ,
Figure BDA0001252914680000128
is the premultiplied matrix of q*IU in quaternion multiplication,
Figure BDA0001252914680000129
is the premultiplied matrix of q′IT in the quaternion multiplication.

4、对偶四元数误差在线估计与补偿修正4. Dual quaternion error online estimation and compensation correction

(4.1)系统状态方程和量测方程离散化处理(4.1) Discretization of system state equation and measurement equation

将系统状态方程和量测方程进行离散化处理：Discretize the system state equation and measurement equation:

Xk＝Φk,k-1Xk-1+Γk,k-1Wk-1 (21)Xk =Φk,k-1 Xk-1 +Γk,k-1 Wk-1 (21)

Zk＝HkXk+Vk (22)Zk =Hk Xk +Vk (22)

其中，Xk为tk时刻系统状态量,Xk-1为tk-1时刻系统状态量，Φk,k-1为tk-1时刻至tk时刻系统的状态转移矩阵，Γk,k-1为tk-1时刻至tk时刻系统的噪声驱动矩阵，Wk-1为tk-1时刻系统的噪声矩阵，Zk为tk时刻系统的量测矩阵,Hk为tk时刻的量测系数矩阵，Vk为tk时刻的观测量噪声矩阵；Among them, Xk is the state quantity of the system at time tk , Xk-1 is the state quantity of the system at time tk-1 , Φk,k-1 is the state transition matrix of the system from time tk-1 to time tk , Γk , k-1 is the noise driving matrix of the system from time tk-1 to time tk , Wk-1 is the noise matrix of the system at time tk-1 , Zk is the measurement matrix of the system at time tk , Hk is The measurement coefficient matrix at time tk , and Vk is the noise matrix of observation at time tk ;

(4.2)卡尔曼滤波闭环估计(4.2) Kalman filter closed-loop estimation

采用卡尔曼滤波对状态量进行闭环估计：The closed-loop estimation of the state quantity is carried out using Kalman filter:

Figure BDA0001252914680000131
Figure BDA0001252914680000131

Figure BDA0001252914680000132
Figure BDA0001252914680000132

Figure BDA0001252914680000133
Figure BDA0001252914680000133

Figure BDA0001252914680000134
Figure BDA0001252914680000134

Figure BDA0001252914680000135
Figure BDA0001252914680000135

其中，

Figure BDA0001252914680000136
是状态量Xk-1的一步预测估计值,Pk-1为tk-1时刻滤波状态估计协方差矩阵,Qk-1为tk-1时刻系统噪声协方差矩阵,
Figure BDA0001252914680000137
为Φk,k-1的转置，
Figure BDA0001252914680000138
为Γk,k-1的转置，Pk,k-1为tk-1时刻到tk时刻的状态一步预测协方差矩阵,Rk为tk时刻的量测噪声协方差矩阵,Kk为tk时刻滤波增益矩阵,
Figure BDA0001252914680000139
为Hk的转置，
Figure BDA00012529146800001310
为状态量Xk的卡尔曼滤波估值，I为单位矩阵，
Figure BDA00012529146800001311
为Kk的转置，Pk为tk时刻滤波状态估计协方差矩阵；in,
Figure BDA0001252914680000136
is the one-step prediction estimate value of the state quantity Xk-1 , Pk-1 is the estimated covariance matrix of the filter state at time tk-1 , Qk-1 is the covariance matrix of the system noise at time tk-1 ,
Figure BDA0001252914680000137
is the transpose of Φk,k-1 ,
Figure BDA0001252914680000138
is the transpose of Γk,k-1 , Pk,k-1 is the state one-step prediction covariance matrix from time tk-1 to time tk , Rk is the measurement noise covariance matrix at time tk , Kk is the filter gain matrix at time tk ,
Figure BDA0001252914680000139
is the transpose of Hk ,
Figure BDA00012529146800001310
is the Kalman filter estimate of the state quantity Xk , I is the identity matrix,
Figure BDA00012529146800001311
is the transpose of Kk , and Pk is the estimated covariance matrix of the filtering state at time tk ;

(4.3)对偶四元数误差在线补偿修正(4.3) Online compensation and correction of dual quaternion errors

由(4.2)得到各状态量估计值后，利用估计所得的对偶四元数误差对惯导算法中的推力对偶四元数、引力对偶四元数、位置对偶四元数进行修正，修正模型为：After obtaining the estimated value of each state quantity from (4.2), use the estimated dual quaternion error to correct the thrust dual quaternion, gravity dual quaternion, and position dual quaternion in the inertial navigation algorithm. The correction model is: :

Figure BDA00012529146800001312
Figure BDA00012529146800001312

Figure BDA00012529146800001313
Figure BDA00012529146800001313

Figure BDA00012529146800001314
Figure BDA00012529146800001314

Figure BDA00012529146800001315
Figure BDA00012529146800001315

上式中，

Figure BDA00012529146800001316
为δqIT的估计值，
Figure BDA00012529146800001317
为修正后的推力速度对偶四元数的实数部分；
Figure BDA00012529146800001318
为δq′IT的估计值，
Figure BDA00012529146800001319
为修正后的推力速度对偶四元数的对偶部分；
Figure BDA00012529146800001320
为δq′IG的估计值，
Figure BDA00012529146800001321
为修正后的引力速度对偶四元数的对偶部分；
Figure BDA00012529146800001322
为δq′IU的估计值，
Figure BDA00012529146800001323
为修正后的位置对偶四元数的对偶部分。In the above formula,
Figure BDA00012529146800001316
is the estimated value of δqIT ,
Figure BDA00012529146800001317
is the real part of the corrected thrust-velocity dual quaternion;
Figure BDA00012529146800001318
is the estimated value of δq′IT ,
Figure BDA00012529146800001319
is the dual part of the corrected thrust-velocity dual quaternion;
Figure BDA00012529146800001320
is the estimated value of δq′IG ,
Figure BDA00012529146800001321
is the dual part of the corrected gravitational velocity dual quaternion;
Figure BDA00012529146800001322
is the estimated value of δq′IU ,
Figure BDA00012529146800001323
is the dual part of the corrected positional dual quaternion.

Claims (5)

Translated fromChinese
1.一种基于对偶四元数的惯性/GPS组合导航方法，其特征在于，包括如下步骤：1. an inertial/GPS combined navigation method based on dual quaternion, is characterized in that, comprises the steps:步骤1，建立陀螺、加速度计误差模型，所述陀螺误差包括常值漂移误差、一阶马尔科夫过程随机噪声以及白噪声随机误差，所述加速度计误差为一阶马尔科夫过程随机噪声；Step 1, establishing an error model of a gyro and an accelerometer, the gyro error includes a constant drift error, a first-order Markov process random noise and a white noise random error, and the accelerometer error is a first-order Markov process random noise;步骤2，在步骤1对陀螺和加速度计误差建模的基础上，将步骤1所述的陀螺常值漂移误差、陀螺一阶马尔科夫过程随机噪声、加速度计一阶马尔科夫过程随机噪声扩展为系统状态变量，构建基于对偶四元数的惯性/GPS组合卡尔曼滤波状态方程；Step 2: On the basis of the error modeling of the gyro and the accelerometer in step 1, the constant drift error of the gyro, the first-order Markov process random noise of the gyro, and the first-order Markov process random noise of the accelerometer described in step 1 are combined. Expanded to system state variables, constructing inertial/GPS combined Kalman filter state equation based on dual quaternion;步骤3，将GPS输出的地理系速度、地球系位置测量误差建模为白噪声，并将其测得的地理系速度转化为惯性系速度，结合由对偶四元数捷联惯导算法计算得到的惯性系速度及地球系位置，构建基于对偶四元数的惯性/GPS组合卡尔曼滤波量测方程；Step 3: Model the geographic system speed and the earth system position measurement error output by the GPS as white noise, and convert the measured geographic system speed into the inertial system speed, which is calculated by the dual quaternion strapdown inertial navigation algorithm. The inertial system velocity and the earth system position based on the inertial system velocity and the earth system position, construct the inertial/GPS combined Kalman filter measurement equation based on dual quaternion;步骤4，对系统状态方程和量测方程进行离散化处理，并采用卡尔曼滤波对状态量进行闭环估计，利用估计所得的对偶四元数误差对惯导算法中的推力对偶四元数、引力对偶四元数、位置对偶四元数进行修正，从而得到载体的速度、位置、姿态等导航信息。Step 4: Discretize the state equation and measurement equation of the system, and use Kalman filter to perform closed-loop estimation of the state quantity, and use the estimated dual quaternion error to calculate the thrust dual quaternion and gravity in the inertial navigation algorithm. The dual quaternion and the position dual quaternion are corrected to obtain the navigation information such as the speed, position and attitude of the carrier.2.根据权利要求1所述的一种基于对偶四元数的惯性/GPS组合导航方法，其特征在于，步骤1所述陀螺和加速度计误差模型为：2. a kind of inertia/GPS combined navigation method based on dual quaternion according to claim 1, is characterized in that, the described gyro and accelerometer error model of step 1 are:
Figure FDA0002253368750000011
Figure FDA0002253368750000011
δfB＝▽a，δfB =▽a ,其中，
Figure FDA0002253368750000012
为陀螺误差，εb为陀螺常值漂移误差，εr为陀螺一阶马尔科夫过程随机噪声，ωg为白噪声；δfB为加速度计误差，▽a为加速度计一阶马尔科夫过程随机噪声；in,
Figure FDA0002253368750000012
is the gyro error, εb is the gyro constant drift error, εr is the gyro first-order Markov process random noise, ωg is white noise; δfB is the accelerometer error, ▽a is the accelerometer first-order Markov process random noise;对上式中的εb、εr、▽a进行求导后可得到以下数学表达式：After derivation of εb , εr , and ▽a in the above formula, the following mathematical expressions can be obtained:
Figure FDA0002253368750000013
Figure FDA0002253368750000013
Figure FDA0002253368750000021
Figure FDA0002253368750000021
Figure FDA0002253368750000022
Figure FDA0002253368750000022
其中，
Figure FDA0002253368750000023
为εb的一阶导数；
Figure FDA0002253368750000024
为εr的一阶导数；
Figure FDA0002253368750000025
为▽a的一阶导数；Tg为陀螺一阶马尔科夫过程相关时间，ωr为陀螺一阶马尔科夫过程驱动白噪声；Ta为加速度计一阶马尔科夫过程相关时间，ωa为加速度计一阶马尔科夫过程驱动白噪声。in,
Figure FDA0002253368750000023
is the first derivative of εb ;
Figure FDA0002253368750000024
is the first derivative ofεr ;
Figure FDA0002253368750000025
is the first derivative of ▽a ; Tg is the first-order Markov process correlation time of the gyro, ωr is the first-order Markov process driving white noise of the gyro; Ta is the first-order Markov process correlation time of the accelerometer, ωa is the white noise driven by the first-order Markov process of the accelerometer.3.根据权利要求2所述的一种基于对偶四元数的惯性/GPS组合导航方法，其特征在于，步骤2所述基于对偶四元数的惯性/GPS组合卡尔曼滤波状态方程为：3. a kind of inertia/GPS combined navigation method based on dual quaternion according to claim 2, is characterized in that, described in step 2, the inertia/GPS combined Kalman filter state equation based on dual quaternion is:
Figure FDA0002253368750000026
Figure FDA0002253368750000026
其中X∈R28×1为系统状态量，F∈R28×28为系统矩阵，G∈R28×12为噪声系数矩阵，w∈R12×1为系统噪声向量，
Figure FDA0002253368750000027
为X的一阶导数，各矩阵分别表示为：where X∈R28×1 is the system state quantity, F∈R28×28 is the system matrix, G∈R28×12 is the noise coefficient matrix, w∈R12×1 is the system noise vector,
Figure FDA0002253368750000027
is the first derivative of X, and each matrix is expressed as:
Figure FDA0002253368750000028
Figure FDA0002253368750000028
Figure FDA0002253368750000029
Figure FDA0002253368750000029
Figure FDA0002253368750000031
Figure FDA0002253368750000031
系统矩阵F和噪声系数矩阵G中，若将四元数q写成q＝[q0 q1 q2 q3]T的形式，我们定义矩阵
Figure FDA0002253368750000032
为q在四元数乘法中的前乘矩阵，
Figure FDA0002253368750000033
为q在四元数乘法中的后乘矩阵，其具体可表示为：In the system matrix F and the noise coefficient matrix G, if the quaternion q is written in the form of q=[q0 q1 q2 q3 ]T , we define the matrix
Figure FDA0002253368750000032
is the premultiplied matrix of q in the quaternion multiplication,
Figure FDA0002253368750000033
is the post-multiplication matrix of q in the quaternion multiplication, which can be specifically expressed as:
Figure FDA0002253368750000034
Figure FDA0002253368750000034
系统矩阵F和噪声系数矩阵G中0均为四阶零矩阵，I4为四阶单位矩阵，各变量均为四元数，其中三维向量表示为标量部分为0的四元数；δqIT为推力速度对偶四元数误差的实数部分，δq′IT为推力速度对偶四元数误差的对偶部分，δq′IG为引力速度对偶四元数误差的对偶部分，δq′IU为位置对偶四元数误差的对偶部分；
Figure FDA0002253368750000035
为陀螺输出信息，
Figure FDA0002253368750000036
为
Figure FDA0002253368750000037
在四元数乘法中的后乘矩阵；qIT为推力速度对偶四元数的实数部分，
Figure FDA0002253368750000038
为qIT在四元数乘法中的前乘矩阵；q′IT为推力对偶四元数的对偶部分，
Figure FDA0002253368750000039
为q′IT在四元数乘法中的前乘矩阵；
Figure FDA00022533687500000310
为地球自转角速度，
Figure FDA00022533687500000311
为
Figure FDA00022533687500000312
在四元数乘法中的后乘矩阵；qIU为位置对偶四元数的实数部分，
Figure FDA00022533687500000313
为其在四元数乘法中的前乘矩阵，
Figure FDA00022533687500000314
为其在四元数乘法中的后乘矩阵；q*IT为qIT的共轭四元数，
Figure FDA00022533687500000315
为其在四元数乘法中的前乘矩阵，
Figure FDA00022533687500000316
为其在四元数乘法中的后乘矩阵；In the system matrix F and the noise coefficient matrix G, 0 is a fourth-order zero matrix, I4 is a fourth-order unit matrix, each variable is a quaternion, and the three-dimensional vector is expressed as a quaternion whose scalar part is 0; δqIT is Real part of thrust-velocity dual quaternion error, δq′IT is the dual part of thrust-velocity dual quaternion error, δq′IG is the dual part of gravitational-velocity dual quaternion error, δq′IU is position dual quaternion the dual part of the error;
Figure FDA0002253368750000035
output information for the gyroscope,
Figure FDA0002253368750000036
for
Figure FDA0002253368750000037
Post-multiplication matrix in quaternion multiplication; qIT is the real part of the thrust-velocity dual quaternion,
Figure FDA0002253368750000038
is the premultiplied matrix of qIT in the quaternion multiplication; q′IT is the dual part of the thrust dual quaternion,
Figure FDA0002253368750000039
is the premultiplied matrix of q′IT in the quaternion multiplication;
Figure FDA00022533687500000310
is the angular velocity of the Earth's rotation,
Figure FDA00022533687500000311
for
Figure FDA00022533687500000312
Postmultiplication matrix in quaternion multiplication; qIU is the real part of the positional dual quaternion,
Figure FDA00022533687500000313
is its premultiplied matrix in quaternion multiplication,
Figure FDA00022533687500000314
is its post-multiplication matrix in quaternion multiplication; q*IT is the conjugate quaternion of qIT ,
Figure FDA00022533687500000315
is its premultiplied matrix in quaternion multiplication,
Figure FDA00022533687500000316
is its postmultiplied matrix in quaternion multiplication;系统矩阵F和噪声系数矩阵G中M1、M2、M3可分别表示为：M1 , M2 , and M3 in the system matrix F and the noise coefficient matrix G can be expressed as:
Figure FDA0002253368750000041
Figure FDA0002253368750000041
4.根据权利要求3所述一种基于对偶四元数的惯性/GPS组合导航方法，其特征在于，步骤3所述的GPS输出的地理系速度、地球系位置测量误差模型为：4. a kind of inertia/GPS combined navigation method based on dual quaternion according to claim 3, is characterized in that, the geographic system speed of the GPS output described in step 3, the Earth system position measurement error model are:VG＝Vn+δV，VG =Vn +δV,RG＝Re+δR，RG =Re +δR,其中，VG为GPS输出的地理系速度，Vn为载体真实的地理系速度，δV为GPS的速度测量误差，将其建模为白噪声；RG为GPS测得的地球系位置，Re为载体真实的地球系位置，δR为GPS的位置测量误差，将其建模为白噪声；Among them, VG is the geographic system speed output by GPS, Vn is the real geographic system speed of the carrier, δV is the GPS speed measurement error, which is modeled as white noise; RG is the earth system position measured by GPS, Re is the real earth system position of the carrier, δR is the GPS position measurement error, which is modeled as white noise;利用如下公式将VG转换到惯性系：Convert VG to the inertial frame using the following formula:
Figure FDA0002253368750000042
Figure FDA0002253368750000042
其中，VGI为VG转换到惯性系下的值，
Figure FDA0002253368750000043
为地球系到惯性系的转换矩阵，ωie为四元数
Figure FDA0002253368750000044
的矢量部分，
Figure FDA0002253368750000045
为地理系到地球系的转换矩阵；Among them, VGI is the value converted from VG to the inertial frame,
Figure FDA0002253368750000043
is the transformation matrix from the earth system to the inertial system, ωie is the quaternion
Figure FDA0002253368750000044
The vector part of ,
Figure FDA0002253368750000045
is the conversion matrix from the geographic system to the earth system;由此可建立步骤3所述基于对偶四元数的惯性/GPS组合卡尔曼滤波量测方程：Thus, the dual quaternion-based inertial/GPS combined Kalman filter measurement equation described in step 3 can be established:Z＝HX+v，Z=HX+v,其中，
Figure FDA0002253368750000046
为量测向量，VI为对偶四元数惯导算法计算所得惯性系速度，RI为对偶四元数惯导算法计算所得地球系位置；H为量测系数矩阵，X为系统状态量，
Figure FDA0002253368750000047
为系统量测噪声阵；in,
Figure FDA0002253368750000046
is the measurement vector, VI is the inertial system velocity calculated by the dual quaternion inertial navigation algorithm, RI is the earth system position calculated by the dual quaternion inertial navigation algorithm; H is the measurement coefficient matrix, X is the system state quantity,
Figure FDA0002253368750000047
Measure the noise array for the system;量测系数矩阵H的具体表达式为：The specific expression of the measurement coefficient matrix H is:
Figure FDA0002253368750000048
Figure FDA0002253368750000048
其中，qIG为引力速度对偶四元数的实数部分，q*IG为qIG的共轭四元数，
Figure FDA0002253368750000051
为q*IG在四元数乘法中的后乘矩阵；qIU为位置对偶四元数的实数部分，q*IU为qIU的共轭四元数，
Figure FDA0002253368750000052
为q*IU在四元数乘法中的前乘矩阵，
Figure FDA0002253368750000053
为q′IT在四元数乘法中的前乘矩阵。where qIG is the real part of the gravitational velocity dual quaternion, q*IG is the conjugate quaternion of qIG ,
Figure FDA0002253368750000051
is the post-multiplication matrix of q*IG in the quaternion multiplication; qIU is the real part of the positional dual quaternion, q*IU is the conjugate quaternion of qIU ,
Figure FDA0002253368750000052
is the premultiplied matrix of q*IU in quaternion multiplication,
Figure FDA0002253368750000053
is the premultiplied matrix of q′IT in the quaternion multiplication.5.根据权利要求3所述一种基于对偶四元数的惯性/GPS组合导航方法，其特征在于，所述步骤4的具体过程为：5. a kind of inertial/GPS combined navigation method based on dual quaternion according to claim 3, is characterized in that, the concrete process of described step 4 is:(401)将系统状态方程和量测方程离散化处理：(401) Discretize the system state equation and measurement equation:Xk＝Φk,k-1Xk-1+Γk,k-1Wk-1，Xk =Φk,k-1 Xk-1 +Γk,k-1 Wk-1 ,Zk＝HkXk+Vk，Zk =Hk Xk +Vk ,其中，Xk为tk时刻系统状态量,Xk-1为tk-1时刻系统状态量，Φk,k-1为tk-1时刻至tk时刻系统的状态转移矩阵，Γk,k-1为tk-1时刻至tk时刻系统的噪声驱动矩阵，Wk-1为tk-1时刻系统的噪声矩阵，Zk为tk时刻系统的量测矩阵,Hk为tk时刻的量测系数矩阵，Vk为tk时刻的观测量噪声矩阵；Among them, Xk is the state quantity of the system at time tk , Xk-1 is the state quantity of the system at time tk-1 , Φk,k-1 is the state transition matrix of the system from time tk-1 to time tk , Γk , k-1 is the noise driving matrix of the system from time tk-1 to time tk , Wk-1 is the noise matrix of the system at time tk-1 , Zk is the measurement matrix of the system at time tk , Hk is The measurement coefficient matrix at time tk , and Vk is the noise matrix of observation at time tk ;(402)采用卡尔曼滤波对状态量进行闭环估计：(402) Use Kalman filter to perform closed-loop estimation on the state quantity:
Figure FDA0002253368750000054
Figure FDA0002253368750000054
Figure FDA0002253368750000055
Figure FDA0002253368750000055
Figure FDA0002253368750000056
Figure FDA0002253368750000056
Figure FDA0002253368750000057
Figure FDA0002253368750000057
Figure FDA0002253368750000058
Figure FDA0002253368750000058
其中，
Figure FDA0002253368750000059
是状态量Xk-1的一步预测估计值,Pk-1为tk-1时刻滤波状态估计协方差矩阵,Qk-1为tk-1时刻系统噪声协方差矩阵,
Figure FDA00022533687500000510
为Φk,k-1的转置，
Figure FDA00022533687500000511
为Γk,k-1的转置，Pk,k-1为tk-1时刻到tk时刻的状态一步预测协方差矩阵,Rk为tk时刻的量测噪声协方差矩阵,Kk为tk时刻滤波增益矩阵,
Figure FDA00022533687500000512
为Hk的转置，
Figure FDA00022533687500000513
为状态量Xk的卡尔曼滤波估值，I为单位矩阵，
Figure FDA00022533687500000514
为Kk的转置，Pk为tk时刻滤波状态估计协方差矩阵；in,
Figure FDA0002253368750000059
is the one-step prediction estimate value of the state quantity Xk-1 , Pk-1 is the estimated covariance matrix of the filter state at time tk-1 , Qk-1 is the covariance matrix of the system noise at time tk-1 ,
Figure FDA00022533687500000510
is the transpose of Φk,k-1 ,
Figure FDA00022533687500000511
is the transpose of Γk,k-1 , Pk,k-1 is the state one-step prediction covariance matrix from time tk-1 to time tk , Rk is the measurement noise covariance matrix at time tk , Kk is the filter gain matrix at time tk ,
Figure FDA00022533687500000512
is the transpose of Hk ,
Figure FDA00022533687500000513
is the Kalman filter estimate of the state quantity Xk , I is the identity matrix,
Figure FDA00022533687500000514
is the transpose of Kk , and Pk is the estimated covariance matrix of the filtering state at time tk ;(403)在(402)得到各状态量估计值后，利用估计所得的对偶四元数误差对惯导算法中的推力对偶四元数、引力对偶四元数、位置对偶四元数进行修正，修正模型为：(403) After obtaining the estimated value of each state quantity in (402), use the estimated dual quaternion error to correct the thrust dual quaternion, gravity dual quaternion, and position dual quaternion in the inertial navigation algorithm, The corrected model is:
Figure FDA0002253368750000061
Figure FDA0002253368750000061
Figure FDA0002253368750000062
Figure FDA0002253368750000062
Figure FDA0002253368750000063
Figure FDA0002253368750000063
Figure FDA0002253368750000064
Figure FDA0002253368750000064
上式中，
Figure FDA0002253368750000065
为δqIT的估计值，
Figure FDA0002253368750000066
为修正后的推力速度对偶四元数的实数部分；
Figure FDA0002253368750000067
为δq′IT的估计值，
Figure FDA0002253368750000068
为修正后的推力速度对偶四元数的对偶部分；
Figure FDA0002253368750000069
为δq′IG的估计值，
Figure FDA00022533687500000610
为修正后的引力速度对偶四元数的对偶部分；
Figure FDA00022533687500000611
为δq′IU的估计值，
Figure FDA00022533687500000612
为修正后的位置对偶四元数的对偶部分。In the above formula,
Figure FDA0002253368750000065
is the estimated value of δqIT ,
Figure FDA0002253368750000066
is the real part of the corrected thrust-velocity dual quaternion;
Figure FDA0002253368750000067
is the estimated value of δq′IT ,
Figure FDA0002253368750000068
is the dual part of the corrected thrust-velocity dual quaternion;
Figure FDA0002253368750000069
is the estimated value of δq′IG ,
Figure FDA00022533687500000610
is the dual part of the corrected gravitational velocity dual quaternion;
Figure FDA00022533687500000611
is the estimated value of δq′IU ,
Figure FDA00022533687500000612
is the dual part of the corrected positional dual quaternion.
CN201710178159.6A2017-03-232017-03-23inertial/GPS combined navigation method based on dual quaternionActiveCN106767797B (en)

Priority Applications (1)

Application NumberPriority DateFiling DateTitle
CN201710178159.6ACN106767797B (en)2017-03-232017-03-23inertial/GPS combined navigation method based on dual quaternion

Applications Claiming Priority (1)

Application NumberPriority DateFiling DateTitle
CN201710178159.6ACN106767797B (en)2017-03-232017-03-23inertial/GPS combined navigation method based on dual quaternion

Publications (2)

Publication NumberPublication Date
CN106767797A CN106767797A (en)2017-05-31
CN106767797Btrue CN106767797B (en)2020-03-17

Family

ID=58966597

Family Applications (1)

Application NumberTitlePriority DateFiling Date
CN201710178159.6AActiveCN106767797B (en)2017-03-232017-03-23inertial/GPS combined navigation method based on dual quaternion

Country Status (1)

CountryLink
CN (1)CN106767797B (en)

Families Citing this family (12)

* Cited by examiner, † Cited by third party
Publication numberPriority datePublication dateAssigneeTitle
CN108387918A (en)*2018-01-182018-08-10和芯星通（上海）科技有限公司A kind of pedestrian navigation method and cloud system server, storage medium, electronic equipment
CN108827301A (en)*2018-04-162018-11-16南京航空航天大学A kind of improvement error quaternion Kalman filtering robot pose calculation method
CN109470251A (en)*2018-12-212019-03-15陕西航天时代导航设备有限公司A kind of partial feedback filtering method in integrated navigation system
CN110285804B (en)*2019-06-262022-06-17南京航空航天大学Vehicle collaborative navigation method based on relative motion model constraint
CN111351482B (en)*2020-03-192023-08-18南京理工大学Multi-rotor aircraft combined navigation method based on error state Kalman filtering
CN111982126B (en)*2020-08-312023-03-17郑州轻工业大学Design method of full-source BeiDou/SINS elastic state observer model
CN113051757B (en)*2021-03-232022-08-09中国人民解放军海军工程大学Strapdown inertial navigation generalized PSI angle error model construction method
CN113091754B (en)*2021-03-302023-02-28北京航空航天大学 A Method for Unified Spacecraft Pose Estimation and Inertial Parameter Determination
CN117555036B (en)*2023-11-172024-07-09中国地质大学（北京）Inertial stabilized platform aviation gravity signal extraction method and device
CN117606474B (en)*2024-01-242024-03-29北京神导科技股份有限公司Inertial astronomical combined navigation time synchronization method based on quaternion second-order interpolation
CN119085703A (en)*2024-08-052024-12-06中国矿业大学 A UWB/INS integrated navigation method and system based on quaternion modeling
CN120010360B (en)*2025-04-172025-06-24深圳大学 A method, device, equipment and medium for position control of a planar motor

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication numberPriority datePublication dateAssigneeTitle
CN104181574B (en)*2013-05-252016-08-10成都国星通信有限公司A kind of SINS/GLONASS integrated navigation filtering system and method
CN103700082B (en)*2013-12-232016-09-07南京航空航天大学 Image Mosaic Method Based on Relative Orientation of Dual Quaternions
CN105512391B (en)*2015-12-042018-09-25上海新跃仪表厂More star appearance rail dynamic modeling methods based on dual quaterion and its verification system
CN106052716B (en)*2016-05-252019-04-05南京航空航天大学Gyro error online calibration method based on starlight information auxiliary under inertial system

Also Published As

Publication numberPublication date
CN106767797A (en)2017-05-31

Similar Documents

PublicationPublication DateTitle
CN106767797B (en)inertial/GPS combined navigation method based on dual quaternion
CN108731670B (en)Inertial/visual odometer integrated navigation positioning method based on measurement model optimization
CN110207697B (en)Inertial navigation resolving method based on angular accelerometer/gyroscope/accelerometer
CN103424114B (en)A kind of full combined method of vision guided navigation/inertial navigation
CN104655131B (en)Inertial navigation Initial Alignment Method based on ISTSSRCKF
CN106289246B (en)A kind of flexible link arm measure method based on position and orientation measurement system
CN102519470B (en)Multi-level embedded integrated navigation system and navigation method
CN105371844B (en)A kind of inertial navigation system initial method based on inertia/astronomical mutual assistance
CN103245359B (en)A kind of inertial sensor fixed error real-time calibration method in inertial navigation system
CN104034329B (en)The air navigation aid of the many integrated navigations processing means under employing launching inertial system
CN107289942B (en) A relative navigation system and method for formation flight
CN105806363B (en)The underwater large misalignment angle alignment methods of SINS/DVL based on SRQKF
CN104697520B (en)Integrated gyro free strap down inertial navigation system and gps system Combinated navigation method
CN104236586B (en)Moving base transfer alignment method based on measurement of misalignment angle
CN111351482A (en) Integrated Navigation Method for Multi-rotor Aircraft Based on Error State Kalman Filter
CN107036598A (en)Dual quaterion inertia/celestial combined navigation method based on gyro error amendment
CN104697526A (en)Strapdown inertial navitation system and control method for agricultural machines
CN106979780A (en)A kind of unmanned vehicle real-time attitude measuring method
CN107728182A (en)Flexible more base line measurement method and apparatus based on camera auxiliary
CN109163735A (en)A kind of positive-positive backtracking Initial Alignment Method of swaying base
JP2012173190A (en)Positioning system and positioning method
CN108344413B (en)Underwater glider navigation system and low-precision and high-precision conversion method thereof
CN111189442A (en)Multi-source navigation information state prediction method of unmanned aerial vehicle based on CEPF
CN110095117A (en)A kind of air navigation aid that gyro free inertia measurement system is combined with GPS
CN113091754A (en)Non-cooperative spacecraft pose integrated estimation and inertial parameter determination method

Legal Events

DateCodeTitleDescription
PB01Publication
PB01Publication
SE01Entry into force of request for substantive examination
SE01Entry into force of request for substantive examination
GR01Patent grant
GR01Patent grant
[8]ページ先頭
©2009-2025 Movatter.jp