CN105371840A - Method for combined navigation of inertia/visual odometer/laser radar - Google Patents

Abstract

The invention belongs to navigation methods and particularly relates to a method for combined navigation of an inertia/visual odometer/laser radar. The method comprises (1) state model establishment, (2) visual odometer speed measurement based on characteristic information, (3) establishment of a measurement equation and obtaining of a measurement value, (4) Kalman filtering and (5) system error correction. The method has the advantages that a machine vision autonomous navigation technology is used, a monocular camera can measure a vector speed under the conditions of a known distance through the difference of a front frame image and a rear frame image, the laser radar can accurately measure the distance of an observation point and then measure a vector speed, navigation is performed by utilizing combination of the speed obtained through measurement and an inertial reference speed, and high-accuracy navigation is performed finally under the conditions of no outside reference information.

Description

Inertial/visual odometer/laser radar combined navigation method

Technical Field

The invention belongs to a navigation method, and particularly relates to an inertia/visual odometer/laser radar combined navigation method.

Background

With the continuous enhancement of the long-time and long-distance navigation capability of the aircraft, the requirements on the precision and the autonomy of a navigation system are also continuously improved. Pure inertial navigation systems do not fully meet the requirements of practical applications due to their inherent disadvantage of navigation errors that accumulate over time. There are two types of approaches to solving this problem: firstly, the accuracy of the inertial navigation system is improved. The precision of the inertia device is improved mainly by adopting new materials, new processes and new technologies, or a novel high-precision inertia device is developed. However, a lot of manpower and financial resources are required, and the improvement of the precision of the inertial device is limited. And secondly, adopting a combined navigation technology. The method mainly uses some additional navigation information sources outside the inertial system to improve the accuracy of the inertial system, and improves the navigation accuracy through software technology. However, there are many available combination information, if all the information is combined, although the precision can reach or even exceed the requirement, the calculation amount is huge, and the information cannot be used in practice at all, and if only partial information combination is selected, the type of the selected information, the sequence of the combination and the specific combination mode all have great influence on the precision of the result. In the prior art, a perfect combination mode does not exist, which not only considers the calculated amount, but also reduces the combined content to the maximum extent.

Disclosure of Invention

Aiming at the defects of the prior art, the invention provides a combined navigation method of an inertia/visual odometer/laser radar.

The invention is realized by the following steps: an inertial/visual odometer/lidar integrated navigation method, comprising the steps of:

(1) establishing a state model of the inertial/visual odometer/laser radar combined navigation system as follows

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)$

In the formula: x (t) is the system state vector; w (t) is system white noise; the coefficient matrices f (t) and g (t) are solved according to an error equation,

X(t)＝[V_n，V_u，V_e，L，h，λ，φ_n，φ_u，φ_e，▽_x，▽_y，▽_z，_x，_y，_z，V_{n_ov}，V_{e_OV}]

V_n,V_u,V_erespectively representing the speed errors of the strapdown inertial navigation system in the north direction, the sky direction and the east direction;

l, h and lambda respectively represent latitude errors, altitude errors and longitude errors of the strapdown inertial navigation system;

φ_n,φ_u,φ_erespectively representing misalignment angles in the north direction, the sky direction and the east direction in a navigation coordinate system of the strapdown inertial navigation system;

▽_x，▽_y，▽_zrespectively representing the zero offset of the accelerometer in X, Y, Z three directions in a carrier coordinate system of the strapdown inertial navigation system;

_x,_y,_zrespectively representing the gyro drift of X, Y, Z three directions in a carrier coordinate system of the strapdown inertial navigation system;

V_{n_ov},V_{e_OV}respectively representing the north and east speed errors of the visual odometer;

(2) visual odometer speed based on feature information

a) Selecting a suitable matching area

Selecting the geometric center position of the image as a matching area;

b) extracting image feature information

Intercepting the matching area in the image of the current frame and the image of the previous frame, extracting SIFT feature points of the intercepted images,

c) feature point matching

Matching the feature points in the current frame image and the previous frame image by using the K-D tree rapid feature point matching to obtain a series of matching pairs,

d) selecting the same feature point to calculate the speed

Calculating the speed of the feature point by dividing different distances obtained on the matching pair by the laser radar by time;

e) outputting the current frame velocity

Each feature point can calculate a speed, and the speeds are processed and then output;

(3) establishment of measurement equation and acquisition of measurement value

The kalman filter measurement equation is of the form:

Z＝HX+V

the measurement value Z is the difference value of the speeds respectively given by the inertial navigation system and the visual odometer, and actually is the difference value of the errors of the two:

$Z=\left[\begin{array}{c}{V}_{n\_imu}-V_{n\_OV}\\ V_{e\_imu}-V_{e\_OV}\end{array}\right]=\left[\begin{array}{c}{\mathrm{\δV}}_n-{\mathrm{\δV}}_{n\_OV}\\ {\mathrm{\δV}}_e-{\mathrm{\δV}}_{e\_OV}\end{array}\right]+V$

where V is the measurement noise, considered as white noise,

the H-matrix is derived from the above formula as follows:

$H=\left[\begin{array}{ccccccccccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& -1& 0\\ 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& -1\end{array}\right]$

(4) kalman filtering

According to a system equation and a measurement equation of the inertial/visual odometer/laser radar combined navigation, a state one-step transition matrix when a Kalman filtering period comes is calculated, and the calculation formula is as follows:

${\mathrm{\Φ}}_{k,k-1}=I+T_n{F}_{T_n}+T_n{F}_{2T_n}+......+T_n{F}_{{\mathrm{NT}}_n}=I+\underset{i=1}{\overset{N}{\Σ}}{T}_n{F}_{{\mathrm{iT}}_n}$

in the formula:

T_nfor navigation period, NT_nIn order to be a kalman filtering cycle,is a system matrix of the ith navigation period in a Kalman filtering period, I is a unit matrix,

state one-step prediction

${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}$

State estimation

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k\[Z_k-H_k{\hat{X}}_{k,k-1}\]$

Filter gain matrix

$K_k=P_{k,k-1}{H}_k^T{\[H_k{P}_{k,k-1}{H}_k^T+R_k\]}^{-1}$

One-step prediction error variance matrix

$P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k,k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k,k-1}^T$

Estimation error variance matrix

P_k＝[I-K_kH_k]P_k,k-1

Wherein,in order to predict the value of the one-step state,estimate the matrix for the state, phi_k,k-1For a state one-step transition matrix, H_kFor measuring the matrix, Z_kMeasurement of quantitative value, K_kFor filtering the gain matrix, R_kFor observing noise arrays, P_k,k-1For one-step prediction of error variance matrix, P_kIn order to estimate the error variance matrix,_k,k-1for system noise driven arrays, Q_k-1A system noise array;

a series of error values can be calculated by using the equation in the step (1) and the step;

(5) correcting systematic errors

And (4) correcting the system output value by using the error value calculated in the step (4).

The combined navigation method of inertial/visual odometer/lidar as described above, wherein the processing in step e) of step (2) is RANSAC method or averaging.

The invention has the following effects: the method uses the autonomous navigation technology of machine vision, and the monocular camera can measure the speed of the carrier under the condition of known distance through the difference of front and back frame images; the laser radar can accurately measure the distance to an observation point, then measure the speed of the carrier, and finally realize high-precision navigation under the condition of no external reference information input by utilizing the speed obtained by measurement and the inertial navigation speed for combined navigation.

Detailed Description

An inertial/visual odometer/lidar integrated navigation method, comprising the steps of:

(1) establishing a state model of the inertial/visual odometer/laser radar combined navigation system as follows

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)$

In the formula: x (t) is the system state vector; w (t) is system white noise; the coefficient matrices F (t) and G (t) are derived from the error equation.

X(t)＝[V_n,V_u,V_e,L,h,λ,φ_n,φ_u,φ_e,▽_x,▽_y,▽_z,_x,_y,_z,V_{n_ov},V_{e_OV}]

V_n,V_u,V_eRespectively representing the speed errors of the strapdown inertial navigation system in the north direction, the sky direction and the east direction;

l, h and lambda respectively represent latitude errors, altitude errors and longitude errors of the strapdown inertial navigation system;

φ_n,φ_u,φ_erespectively representing misalignment angles in the north direction, the sky direction and the east direction in a navigation coordinate system of the strapdown inertial navigation system;

▽_x,▽_y,▽_zrespectively representing the zero offset of the accelerometer in X, Y, Z three directions in a carrier coordinate system of the strapdown inertial navigation system;

_x,_y,_zrespectively representing the gyro drift of X, Y, Z three directions in a carrier coordinate system of the strapdown inertial navigation system;

V_{n_ov},V_{e_OV}respectively representing the north and east speed errors of the visual odometer;

(2) visual odometer speed based on feature information

a) Selecting a suitable matching area

In order to improve the speed measurement range of the visual odometer, a proper position needs to be selected as a matching area by combining the speed and posture information output by inertial navigation. The geometric center of the image is generally selected as a matching region, and the size of the region is 30 × 30 pixels.

b) Extracting image feature information

And intercepting the matching area in the image of the current frame and the image of the previous frame, and extracting SIFT feature points of the intercepted image.

c) Feature point matching

Setting a proper threshold value, being suitable for the fast characteristic point matching of the K-D tree, and matching the characteristic points in the current frame image and the previous frame image to obtain a series of matching pairs.

d) Selecting the same feature point to calculate the speed

As can be known from the velocity calculation formula of the visual odometer, the distance and the velocity are calculated for the same point, so that the same feature point needs to be found in the current frame image and the previous frame image. And obtaining the distance between the feature point and the camera by using a laser radar, and then calculating the relative speed between the feature point and the camera according to the information of the feature point in the matching pair of the current frame image and the previous frame image.

e) Outputting the current frame velocity

Each feature point can calculate a speed, the speeds are processed for a certain time (such as RANSAC method or average), and finally, the speed measurement result of the visual odometer is output.

(3) Establishment of measurement equation and acquisition of measurement value

The kalman filter measurement equation is of the form:

Z＝HX+V

the measurement value Z is the difference value of the speeds respectively given by the inertial navigation system and the visual odometer, and actually is the difference value of the errors of the two:

$Z=\left[\begin{array}{c}{V}_{n\_imu}-V_{n\_OV}\\ V_{e\_imu}-V_{e\_OV}\end{array}\right]=\left[\begin{array}{c}{\mathrm{\δV}}_n-{\mathrm{\δV}}_{n\_OV}\\ {\mathrm{\δV}}_e-{\mathrm{\δV}}_{e\_OV}\end{array}\right]+V$

where V is the measurement noise, considered white noise.

The H-matrix is derived from the above formula as follows:

$H=\left[\begin{array}{ccccccccccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& -1& 0\\ 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& -1\end{array}\right]$

(4) kalman filtering

According to a system equation and a measurement equation of the inertial/visual odometer/laser radar combined navigation, a state one-step transition matrix when a Kalman filtering period comes is calculated, and the calculation formula is as follows:

${\mathrm{\Φ}}_{k,k-1}=I+T_n{F}_{T_n}+T_n{F}_{2T_n}+......+T_n{F}_{{\mathrm{NT}}_n}=I+\underset{i=1}{\overset{N}{\Σ}}{T}_n{F}_{{\mathrm{iT}}_n}$

in the formula:

T_nfor navigation period, NT_nIn order to be a kalman filtering cycle,in a Kalman filtering cycleAnd I is a unit matrix.

State one-step prediction

${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}$

State estimation

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k\[Z_k-H_k{\hat{X}}_{k,k-1}\]$

Filter gain matrix

$K_k=P_{k,k-1}{H}_k^T{\[H_k{P}_{k,k-1}{H}_k^T+R_k\]}^{-1}$

One-step prediction error variance matrix

$P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k,k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k,k-1}^T$

Estimation error variance matrix

P_k＝[I-K_kH_k]P_k,k-1

Wherein,in order to predict the value of the one-step state,estimate the matrix for the state, phi_k,k-1For a state one-step transition matrix, H_kFor measuring the matrix, Z_kMeasurement of quantitative value, K_kFor filtering the gain matrix, R_kFor observing noise arrays, P_k,k-1For one-step prediction of error variance matrix, P_kIn order to estimate the error variance matrix,_k,k-1for system noise driven arrays, Q_k-1Is a system noise matrix.

(5) Correcting systematic errors

And (4) correcting the system output value by using the error value calculated in the step (4).

Claims (2)

1. An inertial/visual odometer/lidar integrated navigation method, comprising the steps of:

(1) establishing a state model of the inertial/visual odometer/laser radar combined navigation system as follows

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)$

In the formula: x (t) is the system state vector; w (t) is system white noise; the coefficient matrices f (t) and g (t) are solved according to an error equation,

$X\left(t\right)=\[{\mathrm{\δV}}_n,{\mathrm{\δV}}_u,{\mathrm{\δV}}_e,\mathrm{\δ}L,\mathrm{\δ}h,\mathrm{\δ}\mathrm{\λ},{\mathrm{\φ}}_n,{\mathrm{\φ}}_u,{\mathrm{\φ}}_e,{\▿}_x,{\▿}_y,{\▿}_z,{\mathrm{\ϵ}}_x,{\mathrm{\ϵ}}_y,{\mathrm{\ϵ}}_z,{\mathrm{\δV}}_{n\_ov},{\mathrm{\δV}}_{e\_OV}\]$

V_n,V_u,V_erespectively representing the speed errors of the strapdown inertial navigation system in the north direction, the sky direction and the east direction;

l, h and lambda respectively represent latitude errors, altitude errors and longitude errors of the strapdown inertial navigation system;

φ_n,φ_u,φ_erespectively representing misalignment angles in the north direction, the sky direction and the east direction in a navigation coordinate system of the strapdown inertial navigation system;

respectively representing the zero offset of the accelerometer in X, Y, Z three directions in a carrier coordinate system of the strapdown inertial navigation system;

_x,_y,_zrespectively representing the gyro drift of X, Y, Z three directions in a carrier coordinate system of the strapdown inertial navigation system;

V_{n_ov},V_{e_OV}respectively representing the north and east speed errors of the visual odometer;

(2) visual odometer speed based on feature information

a) Selecting a suitable matching area

Selecting the geometric center position of the image as a matching area;

b) extracting image feature information

Intercepting the matching area in the image of the current frame and the image of the previous frame, extracting SIFT feature points of the intercepted images,

c) feature point matching

Matching the feature points in the current frame image and the previous frame image by using the K-D tree rapid feature point matching to obtain a series of matching pairs,

d) selecting the same feature point to calculate the speed

Calculating the speed of the feature point by dividing different distances obtained on the matching pair by the laser radar by time;

e) outputting the current frame velocity

Each feature point can calculate a speed, and the speeds are processed and then output;

(3) establishment of measurement equation and acquisition of measurement value

The kalman filter measurement equation is of the form:

Z＝HX+V

the measurement value Z is the difference value of the speeds respectively given by the inertial navigation system and the visual odometer, and actually is the difference value of the errors of the two:

$Z=\left[\begin{array}{c}{V}_{n\_imu}-V_{n\_OV}\\ V_{e\_imu}-V_{e\_OV}\end{array}\right]=\left[\begin{array}{c}{\mathrm{\δV}}_n-{\mathrm{\δV}}_{n\_OV}\\ {\mathrm{\δV}}_e-{\mathrm{\δV}}_{e\_OV}\end{array}\right]+V$

where V is the measurement noise, considered as white noise,

the H-matrix is derived from the above formula as follows:

$H=\left[\begin{array}{ccccccccccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& -1& 0\\ 0& 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& -1\end{array}\right]$

(4) kalman filtering

According to a system equation and a measurement equation of the inertial/visual odometer/laser radar combined navigation, a state one-step transition matrix when a Kalman filtering period comes is calculated, and the calculation formula is as follows:

${\mathrm{\Φ}}_{k,k-1}=I+T_n{F}_{T_n}+T_n{F}_{2T_n}+......+T_n{F}_{{\mathrm{NT}}_n}=I+\underset{i=1}{\overset{N}{\Σ}}{T}_n{F}_{{\mathrm{iT}}_n}$

in the formula:

T_nfor navigation period, NT_nIn order to be a kalman filtering cycle,is a system matrix of the ith navigation period in a Kalman filtering period, I is a unit matrix,

state one-step prediction

${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}$

State estimation

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k\[Z_k-H_k{\hat{X}}_{k,k-1}\]$

Filter gain matrix

$K_k=P_{k,k-1}{H}_k^T{\[H_k{P}_{k,k-1}{H}_k^T+R_k\]}^{-1}$

One-step prediction error variance matrix

$P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k,k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k,k-1}^T$

Estimation error variance matrix

P_k＝[I-K_kH_k]P_k,k-1

Wherein,in order to predict the value of the one-step state,estimate the matrix for the state, phi_k,k-1For a state one-step transition matrix, H_kFor measuring the matrix, Z_kIn order to measure the quantity of the sample,K_kfor filtering the gain matrix, R_kFor observing noise arrays, P_k,k-1For one-step prediction of error variance matrix, P_kIn order to estimate the error variance matrix,_k,k-1for system noise driven arrays, Q_k-1A system noise array;

a series of error values can be calculated by using the equation in the step (1) and the step;

(5) correcting systematic errors

And (4) correcting the system output value by using the error value calculated in the step (4).

2. The integrated inertial/visual odometer/lidar navigation method of claim 1, wherein: the processing in step e) of step (2) is RANSAC method or averaging. .