CN108731670B - Inertial/visual odometer integrated navigation positioning method based on measurement model optimization - Google Patents

Abstract

Translated fromChinese

本发明公开了一种基于量测模型优化的惯性/视觉里程计组合导航定位方法，首先建立组合导航系统的状态方程，将惯性传感器误差扩展为系统状态变量，包括陀螺的随机常数漂移、陀螺仪的一阶马尔科夫过程漂移和加速度计的一阶马尔科夫过程漂移；随后，将视觉里程计作为角速度、线速度以及位置传感器以获取量测数据进而构建量测方程；最后在载体运动过程中对导航误差进行实时反馈校正，获得误差校正后的惯性导航系统导航结果。本发明方法能够在载体运动过程中有效利用视觉里程计的角速度、线速度和位置信息,实现与惯性导航的有效融合，提高组合导航系统的精度和可靠性，适用于工程应用。

The invention discloses an inertial/visual odometry combined navigation and positioning method optimized based on a measurement model. First, the state equation of the integrated navigation system is established, and the inertial sensor error is expanded into a system state variable, including the random constant drift of the gyroscope, the gyroscope, and the gyroscope. The first-order Markov process drift of the accelerometer and the first-order Markov process drift of the accelerometer; then, the visual odometry is used as the angular velocity, linear velocity and position sensor to obtain the measurement data and then construct the measurement equation; finally, in the carrier motion process Real-time feedback correction is performed on the navigation error in the system, and the navigation result of the inertial navigation system after error correction is obtained. The method of the invention can effectively utilize the angular velocity, linear velocity and position information of the visual odometer during the movement of the carrier, realize effective integration with inertial navigation, improve the precision and reliability of the integrated navigation system, and is suitable for engineering applications.

Description

Translated fromChinese

基于量测模型优化的惯性/视觉里程计组合导航定位方法Inertial/visual odometry combined navigation and positioning method based on measurement model optimization

技术领域technical field

本发明涉及一种基于量测模型优化的惯性/视觉里程计组合导航定位方法，属于惯性/视觉里程计组合导航技术领域。The invention relates to an inertial/visual odometry combined navigation and positioning method optimized based on a measurement model, and belongs to the technical field of inertial/visual odometry combined navigation.

背景技术Background technique

近年来，随着无人机、无人车和机器人等智能运动体的研制发展，对导航系统性能提出更高要求。In recent years, with the development of intelligent moving bodies such as unmanned aerial vehicles, unmanned vehicles and robots, higher requirements have been placed on the performance of navigation systems.

在现有技术下，广泛应用于智能运动体的导航系统多采用惯性器件(IMU-加速度计和陀螺仪)/卫星组合导航系统实现。但考虑到在城市、丛林、室内等卫星信号受到干扰或失效的环境，IMU/卫星组合导航系统将难以提供准确的导航信息。视觉里程计技术是一种基于相关图像序列处理的六自由度运动估计方法，由于其仅依赖于被动测量的相机，具有自主性好、隐蔽性好、结构简单、成本低等优点，必将成为未来智能运动体导航系统的重要信息单元。In the prior art, the navigation systems widely used in intelligent moving bodies are mostly implemented by inertial devices (IMU-accelerometer and gyroscope)/satellite integrated navigation system. However, considering that the satellite signals are interfered or disabled in cities, jungles, and indoor environments, it will be difficult for the IMU/satellite integrated navigation system to provide accurate navigation information. Visual odometry technology is a six-degree-of-freedom motion estimation method based on related image sequence processing. Because it only relies on passively measured cameras, it has the advantages of good autonomy, good concealment, simple structure and low cost. Important information unit of future intelligent moving body navigation system.

惯性导航系统的误差主要由惯性传感器(IMU-加速度计和陀螺仪)测量误差引起，惯性导航系统的误差随时间累积，但是其测量精度不会受到突然机动的影响，而视觉里程计由于采用递推的方式求解位姿，视觉里程计不可避免地存在累积误差，且这种累积误差和基于光电编码器的里程计类似，是空间相关而非时间相关的；并且当运动体机动过大时，会由于图像场景变化过大而造成匹配失效进而影响定位精度。视觉里程计根据对图像信息利用的不同，主要分为利用特征点进行跟踪匹配的特征点法，以LIBVISO、PTAM等开源算法为代表，和直接利用像素灰度信息匹配的直接法，以SVO、LSD-SLAM等开源算法为代表。各种代表性的算法运动估计方法不同，并且运动体的机动与输出位姿噪声的关系难以准确建模。考虑到两者的互补性，基于惯性导航和视觉里程计的组合导航受到广泛的重视，但现有的IMU和视觉里程计的组合导航大多直接使用姿态和位置组合，忽视了视觉里程计定位误差累积和与机动相关的特点。如何实现惯性导航和视觉里程计导航信息的有效融合，具有重要的研究意义。The error of the inertial navigation system is mainly caused by the measurement error of the inertial sensor (IMU-accelerometer and gyroscope). The error of the inertial navigation system accumulates with time, but its measurement accuracy will not be affected by sudden maneuvers. To solve the pose and attitude by pushing, the visual odometry inevitably has cumulative errors, and this cumulative error is similar to the odometer based on the photoelectric encoder, which is related to space rather than time; and when the moving body maneuvers too much, Due to the large change of the image scene, the matching failure will be caused and the positioning accuracy will be affected. According to the different use of image information, visual odometry is mainly divided into the feature point method that uses feature points to track and match, represented by open source algorithms such as LIBVISO and PTAM, and the direct method that directly uses pixel grayscale information for matching, such as SVO, PTAM and other open source algorithms. Open source algorithms such as LSD-SLAM are represented. Various representative algorithms have different motion estimation methods, and the relationship between the movement of the moving body and the output pose noise is difficult to accurately model. Considering the complementarity of the two, the combined navigation based on inertial navigation and visual odometry has received extensive attention, but most of the existing combined navigation of IMU and visual odometry directly use the combination of attitude and position, ignoring the positioning error of visual odometry. Cumulative and maneuver-related characteristics. How to realize the effective fusion of inertial navigation and visual odometry navigation information has important research significance.

发明内容SUMMARY OF THE INVENTION

为解决现有技术的不足，本发明的目的在于提供一种基于量测模型优化的惯性/视觉里程计组合导航定位方法，在载体运动过程中有效利用视觉里程计的角速度、线速度和位置信息，实现与惯性导航的最优融合，提高组合导航系统的精度。In order to solve the deficiencies of the prior art, the purpose of the present invention is to provide an inertial/visual odometry combined navigation and positioning method optimized based on a measurement model, which effectively utilizes the angular velocity, linear velocity and position information of the visual odometer during the movement of the carrier. , to achieve the optimal integration with inertial navigation and improve the accuracy of the integrated navigation system.

为了实现上述目标，本发明采用如下的技术方案：In order to achieve above-mentioned goal, the present invention adopts following technical scheme:

一种基于量测模型优化的惯性/视觉里程计组合导航定位方法，其特征是，An inertial/visual odometry combined navigation and positioning method based on measurement model optimization, characterized in that:

步骤1)建立组合导航系统的状态方程，将惯性传感器误差扩展为系统状态变量，包括陀螺的随机常数漂移、陀螺仪的一阶马尔科夫过程漂移和加速度计的一阶马尔科夫过程漂移；Step 1) establish the state equation of the integrated navigation system, and expand the inertial sensor error into a system state variable, including the random constant drift of the gyroscope, the first-order Markov process drift of the gyroscope and the first-order Markov process drift of the accelerometer;

步骤2)结合视觉里程计输出的相对位姿，得到相对位姿到导航系下的姿态和位置的转换关系；Step 2) Combining the relative pose output by the visual odometry, obtain the conversion relationship between the relative pose and the pose and position under the navigation system;

步骤3)选择组合导航系统的量测变量，构建惯性/视觉里程计组合卡尔曼滤波量测方程；Step 3) select the measurement variable of the integrated navigation system, and construct the inertial/visual odometry combined Kalman filter measurement equation;

步骤4)对系统状态方程和量测方程进行离散化处理，并采用卡尔曼滤波对系统状态量进行反馈校正。Step 4) Discretize the system state equation and the measurement equation, and use Kalman filter to feedback and correct the system state quantity.

前述的一种基于量测模型优化的惯性/视觉里程计组合导航定位方法，其特征是，所述步骤1)中系统的状态方程为：The aforementioned a kind of inertial/visual odometry combined navigation and positioning method optimized based on the measurement model, is characterized in that, the state equation of the system in the described step 1) is:

其中，X为状态向量，

为捷联惯导系统中的数学平台误差角，

分别为x、y、z的三轴数学平台误差角；δvⁿ为载体的速度误差，

载体在地理系下x、y、z轴上的速度误差；δpⁿ为载体的位置误差，δL、δλ、δh分别为经度、纬度、高度误差；ε^b为陀螺仪的随机常数漂移，

分别为陀螺仪x、y、z轴的随机常数漂移；

为陀螺仪的一阶马尔科夫过程漂移，

分别为陀螺仪x、y、z轴的一阶马尔科夫过程漂移；

为加速度计的一阶马尔科夫过程漂移，

为加速度计x、y、z轴的一阶马尔科夫过程漂移；

where X is the state vector,

is the mathematical platform error angle in the strapdown inertial navigation system,

are the three-axis mathematical platform error angles of x, y, and z respectively; δvⁿ is the velocity error of the carrier,

The velocity error of the carrier on the x, y, and z axes in the geographic system; δpⁿ is the position error of the carrier, δL, δλ, and δh are the longitude, latitude, and altitude errors, respectively; ε^b is the random constant drift of the gyroscope,

are the random constant drift of the gyroscope x, y, and z axes, respectively;

is the first-order Markov process drift of the gyroscope,

are the first-order Markov process drift of the gyroscope x, y, and z axes, respectively;

is the first-order Markov process drift of the accelerometer,

is the first-order Markov process drift of the x, y, and z axes of the accelerometer;

上标n表示地理系，上标b表示载体系，下标I表示惯导系统解算值，下标V表示视觉里程计解算值；The superscript n represents the geographic system, the superscript b represents the carrier system, the subscript I represents the solution value of the inertial navigation system, and the subscript V represents the solution value of visual odometry;

其中，

为地理系相对于惯性系的角速率误差，

为地理系相对于惯性系的角速率，

表示从载体系到地理系的坐标变换矩阵，fⁿ为地理系下加速度计的输出比力，

为地球系相对于惯性系的角速率误差，

为地理系相对于地球系的角速率误差，

为地球系相对于惯性系的角速率，

为地理系相对于地球系的角速率，δgⁿ为重力加速度误差，vⁿ为载体在地理系下的速度，

为载体在地理系下x轴上的速度，R_m为卯酉圈半径，R_n为子午圈半径，h为当地海拔高度，L为当地纬度，T_g和T_a分别为一阶马尔科夫过程的相关时间，w_r和w_a分别为相关的驱动白噪声。

in,

is the angular rate error of the geographic frame relative to the inertial frame,

is the angular velocity of the geographic frame relative to the inertial frame,

represents the coordinate transformation matrix from the carrier system to the geographic system, fⁿ is the output specific force of the accelerometer in the geographic system,

is the angular rate error of the Earth system relative to the inertial system,

is the angular rate error of the geographic system relative to the Earth system,

is the angular velocity of the Earth system relative to the inertial system,

is the angular velocity of the geographic system relative to the earth system, δgⁿ is the gravitational acceleration error, vⁿ is the velocity of the carrier in the geographic system,

is the speed of the carrier on the x-axis in the geographic system, R_m is the radius of the suo unitary circle, R_n is the radius of the meridian circle, h is the local altitude, L is the local latitude, T_g and T_a are the first-order Markov Correlation time of the process, w_r and w_a are the correlated driving white noise, respectively.

前述的一种基于量测模型优化的惯性/视觉里程计组合导航定位方法，其特征是，所述步骤2)中相对位姿到导航系下的姿态和位置的转换关系为：

其中，

为k帧图像时刻的载体系到地理系的坐标变换矩阵，

为相机系到载体系的坐标变换矩阵，

和

为视觉里程计测得k+1帧图像时刻相对于k帧图像时刻的旋转矩阵和平移向量，

为k帧图像时刻载体在地理系下的位置，上标T表示矩阵的转置，下标k和k+1表示对应帧时刻的物理量。The aforementioned a kind of inertial/visual odometry combined navigation and positioning method optimized based on the measurement model, is characterized in that, in the described step 2), the transformation relationship between the relative pose to the attitude and position under the navigation system is:

in,

is the coordinate transformation matrix from the carrier system to the geographic system at the time of the k-frame image,

is the coordinate transformation matrix from the camera system to the carrier system,

and

Measure the rotation matrix and translation vector of k+1 frame image time relative to k frame image time for visual odometry,

is the position of the k-frame image time carrier in the geographic system, the superscript T represents the transposition of the matrix, and the subscripts k and k+1 represent the physical quantities of the corresponding frame time.

前述的一种基于量测模型优化的惯性/视觉里程计组合导航定位方法，其特征是，所述步骤3)的量测方程为：The aforementioned inertial/visual odometry combined navigation and positioning method optimized based on the measurement model, is characterized in that, the measurement equation of the step 3) is:

其中，

为载体相对于地理系的角速度误差，δvⁿ为载体的速度误差，δpⁿ为载体的位置误差，下标I表示惯导系统解算值，下标V表示视觉里程计解算值，H为系统的量测系数矩阵，V为系统的量测噪声阵；

in,

is the angular velocity error of the carrier relative to the geographic system, δvⁿ is the velocity error of the carrier, δpⁿ is the position error of the carrier, the subscript I represents the inertial navigation system solution value, the subscript V represents the visual odometry solution value, H is the The measurement coefficient matrix of the system, V is the measurement noise matrix of the system;

其中，

表示从地理系到载体系的坐标变换矩阵，M_v和M_p分别是与速度和位置相关的系数，v_x和v_y分别为载体速度在x轴和y轴上的分量，

为

的模值，

I为单位矩阵。

in,

Represents the coordinate transformation matrix from the geographic system to the carrier system, M_v and M_p are the coefficients related to the velocity and position, respectively, v_x and v_y are the components of the carrier velocity on the x-axis and y-axis, respectively,

for

the modulo value of ,

I is the identity matrix.

前述的一种基于量测模型优化的惯性/视觉里程计组合导航定位方法，其特征是，所述步骤4)的具体过程为：A kind of inertial/visual odometry combined navigation and positioning method optimized based on the measurement model, is characterized in that, the concrete process of described step 4) is:

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

X_k＝Φ_k,k-1X_k-1+Γ_k,k-1W_k-1，Z_k＝H_kX_k+V_k，其中，X_k为t_k时刻系统状态量,X_k-1为t_k-1时刻系统状态量，Φ_k,k-1为t_k-1时刻至t_k时刻系统的状态转移矩阵，Γ_k,k-1为t_k-1时刻至t_k时刻系统的噪声驱动矩阵，W_k-1为t_k-1时刻系统的噪声矩阵，Z_k为t_k时刻系统的姿态量测矩阵,H_k为t_k时刻的姿态量测系数矩阵，V_k为t_k时刻的姿态观测量的噪声矩阵；X_k =Φ_k,k-1 X_k-1 +Γ_k,k-1 W_k-1 , Z_k =H_k X_k +V_k , where X_k is the system state quantity at time t_k , X_{k -1} is the state quantity of the system at time t_k-1 , Φ_k,k-1 is the state transition matrix of the system from time t_k-1 to time t_k , Γ_k,k-1 is time t_k-1 to time t_k The noise driving matrix of the system, W_k-1 is the noise matrix of the system at time t_k-1 , Z_k is the attitude measurement matrix of the system at time t_k , H_k is the attitude measurement coefficient matrix at time t_k , V_k is Noise matrix of attitude observations at time t_k ;

402)对离散化处理得到的公式加入控制项U_k-1，并采用闭环修正系统状态方程对惯导系统误差进行修正：

其中，B_k,k-1为t_k-1时刻至t_k时刻系统的控制项系数矩阵；402) Add the control term U_k-1 to the formula obtained by discretization, and use the closed-loop correction system state equation to correct the inertial navigation system error:

Among them, B_k,k-1 is the control term coefficient matrix of the system from time t_k-1 to time t_k ;

403)得到系统的线性化卡尔曼滤波器方程：403) Obtain the linearized Kalman filter equation of the system:

其中，

为t_k-1时刻至t_k时刻一步预测状态量；

为t_k-1时刻滤波状态估计量；

为t_k时刻滤波状态估计量；K_k为t_k时刻滤波增益矩阵；Z_k为t_k时刻的新息序列；P_k,k-1为t_k-1时刻至t_k时刻一步预测协方差矩阵；P_k-1为t_k-1时刻滤波状态估计协方差矩阵；Φ_k,k-1^T为Φ_k,k-1的转置矩阵；Γ_k-1为t_k-1时刻系统噪声系数矩阵；Q_k-1为t_k-1时刻的系统观测噪声估计协方差阵；Γ_k-1^T为Γ_k-1的转置矩阵；

为H_k的转置矩阵；R_k为t_k时刻的量测噪声估计协方差阵；P_k为t_k时刻滤波状态估计协方差矩阵；I为单位矩阵；

in,

Predict the state quantity in one step from time t_k-1 to time t_k ;

is the filter state estimator at time t_k-1 ;

is the filter state estimator at time t_k ; K_k is the filter gain matrix at time t_k ; Z_k is the innovation sequence at time t_k ; P_k,k-1 is the one-step prediction covariance from time t_k-1 to time t_k matrix; P_k-1 is the estimated covariance matrix of the filter state at time t_k-1 ; Φ_k,k-1^T is the transposed matrix of Φ_k,k-1 ; Γ_k-1 is the system noise at time t_k-1 coefficient matrix; Q_k-1 is the covariance matrix of systematic observation noise estimation at time t_k-1 ; Γ_k-1^T is the transpose matrix of Γ_k-1 ;

is the transposed matrix of H_k ; R_k is the estimated covariance matrix of the measurement noise at time t_k ; P_k is the estimated covariance matrix of the filter state at time t_k ; I is the identity matrix;

404)根据步骤403)得到的线性化卡尔曼滤波器方程估计系统导航误差值，包括姿态、位置、速度误差，并用惯性导航系统推算的导航参数减去系统导航误差值，得到基于量测模型优化的惯性/视觉里程计组合导航修正值。404) Estimate the system navigation error value according to the linearized Kalman filter equation obtained in step 403), including the attitude, position, and speed errors, and subtract the system navigation error value from the navigation parameters estimated by the inertial navigation system to obtain an optimization based on the measurement model. The inertial/visual odometry combined navigation correction value.

本发明所达到的有益效果：本发明方法将视觉里程计作为角速度、线速度以及位置传感器以获取量测数据进而构建量测方程，符合视觉里程计输出的噪声特性，保证了对视觉里程计输出信息的有效利用，具有广泛的适用性；本发明方法应用卡尔曼滤波方法对惯性/视觉里程计组合导航系统的状态量实现了最优估计，利用惯性导航和视觉里程计的互补性，有效地提高组合导航系统的精度，适合工程应用。The beneficial effects achieved by the present invention: the method of the present invention uses the visual odometry as an angular velocity, linear velocity and position sensor to obtain measurement data and then constructs a measurement equation, which conforms to the noise characteristics of the output of the visual odometer and ensures the output of the visual odometer. The effective use of information has wide applicability; the method of the present invention uses the Kalman filter method to achieve the optimal estimation of the state quantity of the inertial/visual odometry integrated navigation system, and uses the complementarity of inertial navigation and visual odometry to effectively Improve the accuracy of the integrated navigation system, suitable for engineering applications.

附图说明Description of drawings

图1是本发明的架构图；Fig. 1 is the architecture diagram of the present invention;

图2是本发明所使用的KITTI数据集车辆真实运动轨迹示意图；Fig. 2 is the KITTI data set vehicle real motion track schematic diagram used in the present invention;

图3(a)-(c)是本发明的导航结果与使用姿态位置组合的导航结果比较图，图3(a)是位置误差比较曲线，图3(b)是姿态误差比较曲线，图3(c)是速度误差比较曲线。Figure 3(a)-(c) is a comparison diagram of the navigation result of the present invention and the navigation result using the combination of attitude and position, Figure 3 (a) is the position error comparison curve, Figure 3 (b) is the attitude error comparison curve, Figure 3 (c) is the speed error comparison curve.

具体实施方式Detailed ways

下面结合附图对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。The present invention will be further described below in conjunction with the accompanying drawings. The following examples are only used to illustrate the technical solutions of the present invention more clearly, and cannot be used to limit the protection scope of the present invention.

如图1所示，本发明所述的惯基于量测模型优化的惯性/视觉里程计组合导航方法的原理是：视觉里程计模块对双目相机输出的原始图像进行畸变校正，随后对序列图像进行特征提取与立体匹配，再进行特征点的跟踪，从而估计出相对位姿。卡尔曼滤波器模块通过建立卡尔曼滤波状态方程和量测方程，利用陀螺仪和加速度计原始输出进行捷联惯导解算，当到了滤波周期后，分别进行时间更新和量测更新，实现对组合导航系统状态量的最优估计，以提高组合导航系统性能。As shown in FIG. 1 , the principle of the inertial/visual odometry combined navigation method optimized based on the inertial measurement model of the present invention is as follows: the visual odometer module performs distortion correction on the original image output by the binocular camera, and then corrects the sequence of images. Perform feature extraction and stereo matching, and then track feature points to estimate the relative pose. The Kalman filter module uses the original output of the gyroscope and accelerometer to solve the strapdown inertial navigation by establishing the Kalman filter state equation and measurement equation. The optimal estimation of the state quantity of the integrated navigation system to improve the performance of the integrated navigation system.

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

步骤1)建立惯性/视觉里程计组合导航算法的状态方程：Step 1) Establish the state equation of the inertial/visual odometry combined navigation algorithm:

首先定义坐标系：地理坐标系选取“东-北-天”为导航系(n)，载体坐标系(b)选取为“右-前-上”。需要估计的状态量如式所示：First define the coordinate system: the geographic coordinate system selects "East-North-Sky" as the navigation system (n), and the carrier coordinate system (b) is selected as "Right-Front-Up". The state quantity that needs to be estimated is shown in the formula:

式中，X为状态向量，

为捷联惯导系统中的数学平台误差角，

分别为x、y、z的三轴数学平台误差角；δvⁿ为载体的速度误差，

载体在地理系下x、y、z轴上的速度误差；δpⁿ为载体的位置误差，δL、δλ、δh分别为经度、纬度、高度误差；ε^b为陀螺仪的随机常数漂移，

分别为陀螺仪x、y、z轴的随机常数漂移；

为陀螺仪的一阶马尔科夫过程漂移，

分别为陀螺仪x、y、z轴的一阶马尔科夫过程漂移；

为加速度计的一阶马尔科夫过程漂移，

为加速度计x、y、z轴的一阶马尔科夫过程漂移。上标n表示地理系，上标b表示载体系；where X is the state vector,

is the mathematical platform error angle in the strapdown inertial navigation system,

are the three-axis mathematical platform error angles of x, y, and z respectively; δvⁿ is the velocity error of the carrier,

The velocity error of the carrier on the x, y, and z axes in the geographic system; δpⁿ is the position error of the carrier, δL, δλ, and δh are the longitude, latitude, and altitude errors, respectively; ε^b is the random constant drift of the gyroscope,

are the random constant drift of the gyroscope x, y, and z axes, respectively;

is the first-order Markov process drift of the gyroscope,

are the first-order Markov process drift of the gyroscope x, y, and z axes, respectively;

is the first-order Markov process drift of the accelerometer,

is the first-order Markov process drift of the accelerometer x, y, and z axes. The superscript n represents the geographic system, and the superscript b represents the carrier system;

IMU的测量值作为状态方程的输入变量，状态方程表示为：The measured value of the IMU is used as the input variable of the state equation, and the state equation is expressed as:

式中，

为地理系相对于惯性系的角速率误差，

为地理系相对于惯性系的角速率，

表示从载体系到地理系的坐标变换矩阵，fⁿ为地理系下加速度计的输出比力，

为地球系相对于惯性系的角速率误差，

为地理系相对于地球系的角速率误差，

为地球系相对于惯性系的角速率，

为地理系相对于地球系的角速率，δgⁿ为重力加速度误差，vⁿ为载体在地理系下的速度，

为载体在地理系下x轴上的速度，R_m为卯酉圈半径，R_n为子午圈半径，h为当地海拔高度，L为当地纬度，T_g和T_a分别为一阶马尔科夫过程的相关时间，w_r和w_a分别为相关的驱动白噪声。

In the formula,

is the angular rate error of the geographic frame relative to the inertial frame,

is the angular velocity of the geographic frame relative to the inertial frame,

represents the coordinate transformation matrix from the carrier system to the geographic system, fⁿ is the output specific force of the accelerometer in the geographic system,

is the angular rate error of the Earth system relative to the inertial system,

is the angular rate error of the geographic system relative to the Earth system,

is the angular velocity of the Earth system relative to the inertial system,

is the angular velocity of the geographic system relative to the earth system, δgⁿ is the gravitational acceleration error, vⁿ is the velocity of the carrier in the geographic system,

is the speed of the carrier on the x-axis in the geographic system, R_m is the radius of the suo unitary circle, R_n is the radius of the meridian circle, h is the local altitude, L is the local latitude, T_g and T_a are the first-order Markov Correlation time of the process, w_r and w_a are the correlated driving white noise, respectively.

步骤2)建立基于量测模型优化的惯性/视觉里程计组合导航算法的量测方程Step 2) Establish the measurement equation of the inertial/visual odometry integrated navigation algorithm optimized based on the measurement model

视觉里程计通过对序列图像信息的匹配和跟踪估算相邻图像间相机的运动，它的输出是前后两个连续采样时刻的相对位姿，用旋转矩阵

和平移向量

来表示，k与k+1表示图像的帧数。Visual odometry estimates the motion of the camera between adjacent images by matching and tracking sequence image information. Its output is the relative pose of two consecutive sampling moments before and after, using the rotation matrix

and translation vector

to represent, k and k+1 represent the number of frames of the image.

下式表示了相对位姿到导航系下的姿态和位置的转换关系。The following formula expresses the transformation relationship between the relative pose and the pose and position under the navigation system.

式中，

表示从相机系到载体系的坐标变换矩阵，一般认为

在实验前已经标定好且在一次实验中保持不变。下标k和k+1表示载体在对应帧数图像的时刻的相应物理量的大小。In the formula,

Represents the coordinate transformation matrix from the camera system to the carrier system, generally considered to be

It has been calibrated before the experiment and remains unchanged for one experiment. The subscripts k and k+1 represent the size of the corresponding physical quantity of the carrier at the moment corresponding to the frame number image.

由于基于不同解算原理和方法的视觉里程计噪声特性不同，因此不失一般性，将视觉里程计视为一个黑箱，直接观测得到的相对位姿的噪声不会累积，近似于白噪声，因此噪声体现在姿态角增量和平移向量上，即：Because the noise characteristics of visual odometry based on different calculation principles and methods are different, without loss of generality, visual odometry is regarded as a black box, and the noise of the relative pose obtained by direct observation will not accumulate, which is similar to white noise, so The noise is reflected in the attitude angle increment and translation vector, namely:

其中，Δγ，Δθ，Δφ表示从k和k+1时刻横滚角、俯仰角、航向角的变化量，

表示带误差的横滚角、俯仰角、航向角测量值，w_γ，w_θ，w_φ，w_t分别表示横滚角、俯仰角、航向角、平移向量的白噪声。Among them, Δγ, Δθ, Δφ represent the changes of roll angle, pitch angle, and heading angle from time k and k+1,

Represents the roll angle, pitch angle, and heading angle measurement values with errors, w_γ , w_θ , w_φ , w_t represent the white noise of the roll angle, pitch angle, heading angle, and translation vector, respectively.

基于以上分析的视觉里程计噪声特性，选取惯导系统和视觉里程计解算出的角速度误差的差值、线速度误差差值和位置误差差值作为组合系统的观测量，如下式所示：Based on the noise characteristics of the visual odometry analyzed above, the difference between the angular velocity error, the linear velocity error, and the position error calculated by the inertial navigation system and the visual odometry are selected as the observations of the combined system, as shown in the following formula:

其中，

表示载体相对于地理系的角速度误差，δvⁿ为载体的速度误差，δpⁿ为载体的位置误差。下标I表示惯导系统解算值，下标V表示视觉里程计解算值。H为系统的量测矩阵。in,

Represents the angular velocity error of the carrier relative to the geographic system, δvⁿ is the velocity error of the carrier, and δpⁿ is the position error of the carrier. The subscript I represents the solution value of the inertial navigation system, and the subscript V represents the solution value of the visual odometry. H is the measurement matrix of the system.

的值可以由陀螺仪输出计算得到，如式所示：

The value of can be calculated from the gyroscope output as follows:

其中，

为陀螺仪输出，

为地球自转角速率，

为载体运动引起的角速度。in,

is the gyroscope output,

is the angular rate of Earth's rotation,

is the angular velocity caused by the motion of the carrier.

因此，忽略二阶小项，

可以表示为：Therefore, ignoring the second order minor terms,

It can be expressed as:

的值可以根据欧拉角微分方程，由姿态变化角速率转换得到，如下式所示：

The value of can be converted from the angular rate of attitude change according to the Euler angle differential equation, as shown in the following formula:

其中，

为

在三轴上的分量。in,

for

components on three axes.

为系统的状态量，

可以由k到k+1时刻的视觉位置估计变换计算。根据式(5)～(7)，系统的量测矩阵H如下式所示：

is the state quantity of the system,

It can be computed from the visual position estimation transformation at time k to k+1. According to equations (5) to (7), the measurement matrix H of the system is as follows:

其中，符号×是向量叉乘的简化表示，

M_v和M_p分别是与速度和位置相关的系数，

为

的模值，

I为单位矩阵。步骤3)建立基于量测模型优化的惯性/视觉里程计组合导航卡尔曼滤波模型where the symbol × is a simplified representation of the vector cross product,

M_v and M_p are coefficients related to velocity and position, respectively,

for

the modulo value of ,

I is the identity matrix. Step 3) Establish an inertial/visual odometry combined navigation Kalman filter model optimized based on the measurement model

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

X_k＝Φ_k,k-1X_k-1+Γ_k,k-1W_k-1 (9)X_k =Φ_k,k-1 X_k-1 +Γ_k,k-1 W_k-1 (9)

Z_k＝H_kX_k+V_k (10)Z_k =H_k X_k +V_k (10)

其中，X_k为t_k时刻系统状态量,X_k-1为t_k-1时刻系统状态量，Φ_k,k-1为t_k-1时刻至t_k时刻系统的状态转移矩阵，Γ_k,k-1为t_k-1时刻至t_k时刻系统的噪声驱动矩阵，W_k-1为t_k-1时刻系统的噪声矩阵，Z_k为t_k时刻系统的姿态量测矩阵,H_k为t_k时刻的姿态量测系数矩阵，V_k为t_k时刻的姿态观测量的噪声矩阵。Among them, X_k is the state quantity of the system at time t_k , X_k-1 is the state quantity of the system at time t_k-1 , Φ_k,k-1 is the state transition matrix of the system from time t_k-1 to time t_k , Γ_{k , k-1} is the noise driving matrix of the system from time t_k-1 to time t_k , W_k-1 is the noise matrix of the system at time t_k-1 , Z_k is the attitude measurement matrix of the system at time t_k , H_k is the attitude measurement coefficient matrix at time t_k , and V_k is the noise matrix of attitude observation at time t_k .

32)对离散化处理得到的公式加入控制项U_k-1，并采用闭环修正系统状态方程对惯导系统误差进行修正：32) Add the control term U_k-1 to the formula obtained by discretization, and use the closed-loop correction system state equation to correct the inertial navigation system error:

其中，B_k,k-1为t_k-1时刻至t_k时刻系统的控制项系数矩阵。Among them, B_k,k-1 is the control term coefficient matrix of the system from time t_k-1 to time t_k .

33)得到系统的线性化卡尔曼滤波器方程：33) Obtain the linearized Kalman filter equation of the system:

其中，

为t_k-1时刻至t_k时刻一步预测状态量；

为t_k-1时刻滤波状态估计量；

为t_k时刻滤波状态估计量；K_k为t_k时刻滤波增益矩阵；Z_k为t_k时刻的新息序列；P_k,k-1为t_k-1时刻至t_k时刻一步预测协方差矩阵；P_k-1为t_k-1时刻滤波状态估计协方差矩阵；Φ_k,k-1^T为Φ_k,k-1的转置矩阵；Γ_k-1为t_k-1时刻系统噪声系数矩阵；Q_k-1为t_k-1时刻的系统观测噪声估计协方差阵；Γ_k-1^T为Γ_k-1的转置矩阵；

为H_k的转置矩阵；R_k为t_k时刻的量测噪声估计协方差阵；P_k为t_k时刻滤波状态估计协方差矩阵；I为单位矩阵。in,

Predict the state quantity in one step from time t_k-1 to time t_k ;

is the filter state estimator at time t_k-1 ;

is the filter state estimator at time t_k ; K_k is the filter gain matrix at time t_k ; Z_k is the innovation sequence at time t_k ; P_k,k-1 is the one-step prediction covariance from time t_k-1 to time t_k matrix; P_k-1 is the estimated covariance matrix of the filter state at time t_k-1 ; Φ_k,k-1^T is the transposed matrix of Φ_k,k-1 ; Γ_k-1 is the system noise at time t_k-1 coefficient matrix; Q_k-1 is the covariance matrix of systematic observation noise estimation at time t_k-1 ; Γ_k-1^T is the transpose matrix of Γ_k-1 ;

is the transposed matrix of H_k ; R_k is the estimated covariance matrix of the measurement noise at time t_k ; P_k is the estimated covariance matrix of the filter state at time t_k ; I is the identity matrix.

为了验证发明所提出的基于量测模型优化的惯性/视觉里程计组合导航方法的正确性和有效性，采用本发明方法建立模型，利用KITTI数据集进行半物理仿真验证。测试车辆运行的真实轨迹如图2所示。In order to verify the correctness and effectiveness of the inertial/visual odometry combined navigation method based on measurement model optimization proposed by the invention, the method of the invention is used to establish a model, and the KITTI data set is used for semi-physical simulation verification. The real trajectory of the test vehicle running is shown in Figure 2.

基于所选取的数据集，基于本发明所述的基于量测模型优化的惯性/视觉里程计组合导航方法进行验证，并与姿态位置直接组合的卡尔曼滤波进行比较，导航误差比较曲线如图3(a)-图3(c)所示。Based on the selected data set, the inertial/visual odometry combined navigation method based on the measurement model optimization of the present invention is verified, and compared with the Kalman filter directly combined with the attitude and position, the navigation error comparison curve is shown in Figure 3 (a)-Fig. 3(c).

图3(a)-图3(c)中实线代表本发明误差曲线，虚线代表姿态位置直接组合的卡尔曼滤波误差曲线。从图3(a)-图3(c)导航误差比较曲线可以看出，采用本发明提出的基于量测模型优化的惯性/视觉里程计组合导航方法，惯性导航系统精度相较于姿态位置直接组合的卡尔曼滤波有明显提高，具有有益的工程应用价值。The solid line in Fig. 3(a)-Fig. 3(c) represents the error curve of the present invention, and the dashed line represents the Kalman filter error curve of the direct combination of attitude and position. It can be seen from the comparison curves of the navigation errors in Fig. 3(a)-Fig. 3(c) that using the inertial/visual odometry combined navigation method based on the optimization of the measurement model proposed by the present invention, the accuracy of the inertial navigation system is directly higher than that of the attitude position. The combined Kalman filter is obviously improved and has beneficial engineering application value.

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。The above are only the preferred embodiments of the present invention. It should be pointed out that for those skilled in the art, without departing from the technical principle of the present invention, several improvements and modifications can also be made. These improvements and modifications It should also be regarded as the protection scope of the present invention.

Claims (5)

1. An inertial/visual odometer integrated navigation positioning method based on measurement model optimization is characterized in that,

step 1) establishing a state equation of the integrated navigation system, and expanding errors of the inertial sensor into system state variables, wherein the system state variables comprise random constant drift of a gyroscope, first-order Markov process drift of the gyroscope and first-order Markov process drift of an accelerometer;

step 2) obtaining a conversion relation from the relative pose to the pose and the position under the navigation system by combining the relative pose output by the visual odometer;

step 3) selecting measurement variables of the integrated navigation system, and constructing an inertia/visual odometer integrated Kalman filtering measurement equation;

step 4) discretizing a system state equation and a measurement equation, and performing feedback correction on the system state quantity by adopting Kalman filtering;

the measurement equation of the step 3) is as follows:

wherein,

error in angular velocity of the carrier with respect to the geographic system, δ vⁿAs error in velocity of the carrier, δ pⁿAnd the subscript I represents the calculation value of the inertial navigation system, the subscript V represents the calculation value of the visual mileage, H is a measurement coefficient matrix of the system, and V is a measurement noise matrix of the system.

2. The integrated navigation and positioning method for inertial/visual odometer based on metrology model optimization as claimed in claim 1, wherein the state equation of the system in step 1) is:

wherein, X is a state vector,

is a mathematical platform error angle in a strapdown inertial navigation system,

the error angles of the three-axis mathematical platform are x, y and z respectively; delta vⁿIs the error in the speed of the carrier,

speed errors of the carrier on x, y and z axes under the geographical system; δ pⁿThe position error of the carrier is delta L, delta lambda and delta h are respectively longitude, latitude and height errors; epsilon^bIs the random constant drift of the gyroscope,

random constant drifts of the gyroscope x, y and z axes respectively;

for the first order markov process drift of the gyroscope,

first order markov process drifts for gyroscope x, y, z axes respectively;

for the first order markov process drift of the accelerometer,

first order markov process drift for accelerometer x, y, z axes;

the superscript n represents a geographical system, the superscript b represents a loading system, the subscript I represents an inertial navigation system calculation value, and the subscript V represents a visual mileage calculation value;

wherein,

the angular rate error of the geographic system relative to the inertial system,

is the angular velocity of the geographic system relative to the inertial system,

representing a coordinate transformation matrix from a carrier system to a geographical system, fⁿIn order to output specific force of the accelerometer under geographic conditions,

is the angular rate error of the earth's system relative to the inertial system,

is the angular rate error of the geographic system relative to the earth system,

is the angular velocity of the earth's system relative to the inertial system,

angular velocity of the geographic system relative to the Earth's system, δ gⁿIs the gravity acceleration error, vⁿIs the speed of the carrier under the geographic system,

speed of the carrier on the x-axis under geographic system, R_mRadius of a unit of fourth quarter_nRadius of meridian, h is the local altitude, L is the local altitude, T_gAnd T_aRespectively, the correlation time, w, of the first order Markov process_rAnd w_aRespectively, associated driving white noise.

3. The integrated navigation and positioning method for the inertial/visual odometer based on the metrology model optimization as claimed in claim 1, wherein the transformation relationship from the relative pose in step 2) to the pose and position under the navigation system is:

wherein,

is a coordinate transformation matrix from the carrier to the geographic system at the time of k frames of images,

is a coordinate transformation matrix of the camera system to the carrier system,

and

the rotational matrix and translation vectors for the k +1 frame image time relative to the k frame image time are measured for the visual odometer,

for the position of the carrier under the geographical system at the moment of the k frames of images, the superscript T represents the transpose of the matrix, and the subscripts k and k +1 represent the physical quantities corresponding to the frame moments.

4. The integrated inertial/visual odometer navigation positioning method based on metrology model optimization of claim 2,

wherein,

representing a coordinate transformation matrix from a geographical system to a carrier system, M_vAnd M_pRespectively, coefficients related to speed and position, v_xAnd v_yThe components of the carrier velocity in the x-axis and y-axis respectively,

is composed of

The value of the modulus of the (c) component,

and I is an identity matrix.

5. The integrated navigation and positioning method for inertial/visual odometer based on metrology model optimization as claimed in claim 1, wherein the specific process of step 4) is:

401) discretizing a system state equation and a measurement equation:

X_k＝Φ_k,k-1X_k-1+Γ_k,k-1W_k-1，Z_k＝H_kX_k+V_kwherein X is_kIs t_kTime of day system state quantity, X_k-1Is t_k-1Time of day system state quantity, phi_k,k-1Is t_k-1Time to t_kState transition matrix of time system, gamma_k,k-1Is t_k-1Time to t_kNoise-driven matrix of time-of-day system, W_k-1Is t_k-1Noise matrix of time of day system, Z_kIs t_kAttitude measurement matrix of time system, H_kIs t_kAttitude measurement coefficient matrix at time, V_kIs t_kA noise matrix of attitude observations at a time;

402) adding a control item U to a formula obtained by discretization_k-1And correcting the inertial navigation system error by adopting a closed loop correction system state equation:

wherein, B_k,k-1Is t_k-1Time to t_kA control item coefficient matrix of the time system;

403) obtaining a linearized Kalman filter equation of the system:

wherein,

is t_k-1Time to t_kPredicting the state quantity one step at a time;

is t_k-1A time filtering state estimator;

is t_kA time filtering state estimator; k_kIs t_kA time filtering gain matrix; z_kIs t_kAn innovation sequence of moments; p_k,k-1Is t_k-1Time to t_kPredicting a covariance matrix one step at a time; p_k-1Is t_k-1Estimating a covariance matrix by a time filtering state; phi_k,k-1^TIs phi_k,k-1The transposed matrix of (2); gamma-shaped_k-1Is t_k-1A time system noise coefficient matrix; q_k-1Is t_k-1A system observation noise estimation covariance matrix at the moment; gamma-shaped_k-1^TIs gamma_k-1The transposed matrix of (2); h_k^TIs H_kThe transposed matrix of (2); r_kIs t_kEstimating a covariance matrix of measurement noise at a moment; p_kIs t_kEstimating a covariance matrix by a time filtering state; i is an identity matrix;

404) and estimating a system navigation error value including attitude, position and speed errors according to the linearized Kalman filter equation obtained in the step 403), and subtracting the system navigation error value from a navigation parameter calculated by an inertial navigation system to obtain an inertial/visual odometer combined navigation correction value optimized based on a measurement model.

Images