CN106767791A - A kind of inertia/visual combination air navigation aid using the CKF based on particle group optimizing - Google Patents

Abstract

The invention discloses a kind of inertia/visual combination air navigation aid using the CKF based on particle group optimizing, key step includes：Step one：When visual signal is effective, the camera carried using mobile robot gathers dynamic video, and the speed of camera is determined by image characteristics extraction and arest neighbors matching method；Step 2：The course angle that is measured according to inertial navigation system and the speed with reference to camera calculate the speed of mobile robot, use CKF to estimate speed, the acceleration of mobile robot；Step 3：Using CKF according to the measuring value and state value at system each moment and selected filtered parameter value, updated by the time and update the estimate for obtaining system mode with measurement；Step 4：According to target function value and filtered parameter value that each moment is current, optimizing is carried out to filtering parameter Q and R using particle cluster algorithm, the correction value that will be obtained as CKF |input paramete, until obtaining optimal state estimation.

Description

Inertial/visual combined navigation method for CKF based on particle swarm optimization

Technical Field

The invention relates to the field of inertia/vision integrated navigation, in particular to an inertia/vision integrated navigation method based on particle swarm optimization for CKF.

Background

In recent years, with the rapid development of computer technology, electronic technology, communication technology, advanced control, and artificial intelligence, research and application relating to mobile robot technology have made great progress. The intelligent mobile robot is used as a complex integrated system integrating a plurality of characteristics such as environment perception, dynamic decision, real-time behavior control and execution, and the like, and is more and more generally applied in the fields of military, civil and scientific research, industrial production and the like at present to replace human beings to execute some works which need to be developed under severe conditions or dangerous conditions. Positioning and navigation are the first prerequisites for indoor mobile robots to complete tasks, and are gradually becoming research hotspots in the field. However, in a series of complex indoor environments such as weak external radio signals and strong electromagnetic interference, the accuracy, real-time performance and robustness of the indoor robot for acquiring the navigation information are greatly affected. How to acquire effective navigation information in an indoor environment so as to meet the requirement of a high-precision navigation intelligent mobile robot and avoid the influence of the external environment to the greatest extent has very important scientific theoretical significance and engineering application value. The process of guiding a navigation carrier from a starting point to a destination is called navigation. Navigation has a variety of technical approaches, such as radio navigation, astronomical navigation, satellite navigation, infrared navigation, inertial navigation, visual navigation, and the like. The inertial navigation uses an accelerometer and a gyroscope to calculate a course and deduce a current position and a next destination, and is strong in autonomy, not easy to be interfered and the current main navigation method. However, the Inertial Navigation System (INS) cannot fully meet the requirements of practical applications because of its inherent accumulation of Navigation errors, the reduction of Navigation accuracy with the increase of time, and the high cost of equipment. In addition, the visual navigation adopts imaging equipment to shoot images, and relevant technologies such as machine vision and the like are used for identifying paths, so that automatic navigation is realized. The visual navigation has the best guidance flexibility in theory due to the wide application range, and the development is very rapid in recent years. However, the visual navigation method mainly has the disadvantages of poor precision of image matching, error in positioning of image points in a target image, calibration error of a measurement system and limited spatial resolution of an imaging system, and the application of the visual navigation method is also limited.

Therefore, the visual navigation and the inertial navigation are used as two autonomous navigation modes and have good complementarity. The combination of the two can solve the navigation problem in the GPS-free environment, and has important significance for application requirements of indoor navigation, robot path planning, unmanned plane autonomous landing, automatic vehicle navigation, underground traffic navigation positioning, mine operation safety and the like. Generally, a kalman filter is used to perform data fusion between the visual navigation information and the navigation information of the inertial navigation system. Applying kalman filters for data fusion requires solving two problems: and selecting a system noise covariance matrix and a measurement noise covariance matrix. The former influences the filtering performance and parameter estimation precision of a KF algorithm, and increases the uncertainty of the system; improper value of the latter can affect the correction speed of the filter, so that the filtering process is unstable and even diverged.

The particle swarm optimization algorithm is a global optimization algorithm for seeking a group optimal solution through collective cooperation in the process of simulating the predation behavior of the bird swarm. Each particle (a candidate solution of the d-dimensional solution space) adjusts the flight trajectory to approach the optimal point according to own and group experiences. Through continuous learning and updating, the particles move to the individual optimal position and the global optimal position in an accelerated manner, and finally the global optimal solution is output. The particle swarm algorithm simulates the biological evolution process and can randomly search to obtain the global optimal parameters.

Disclosure of Invention

The invention provides an inertial/visual integrated navigation method based on particle swarm optimization for CKF (CKF), aiming at solving the problem that a visual navigation system cannot provide high-precision navigation for a long time when a mobile robot moves in a weak light or no light environment, so that the inertial visual integrated navigation system is seriously degraded in precision.

The technical scheme of the invention is as follows: an inertial/visual combined navigation method for CKF based on particle swarm optimization comprises the following steps:

step 1: when the visual signal is effective, acquiring a dynamic image by using a camera carried by the mobile robot, and determining the speed of the camera by using an image feature extraction and nearest neighbor matching method; respectively extracting SURF characteristic points in two adjacent image frames in the video by adopting a SURF algorithm, recording position coordinates of the characteristic points in an image coordinate system, and simultaneously matching the SURF characteristic points on the two image frames according to a nearest neighbor matching method to determine the speed V of the camera on the horizontal plane_x、V_y；

Step 2: the speed V of the camera on the horizontal plane obtained according to the step 1_x、V_yCombining course angle measured by inertial navigation systemCalculating to obtain the east speed of the mobile robotAnd north velocityThe formula is as follows:

and step 3: using CKF to move the speed of the robotAnd course angle variation omega obtained by inertial navigation system measurement_zCarrying out data fusion;

the CKF obtains the velocity and acceleration estimation of the mobile robot in the east direction and the north direction through time updating and measurement updating according to the measurement value and the state value of each moment of the system and the selected filter parameter valueLet us note that the estimates of east velocity and acceleration are respectivelyThe north-bound velocity and acceleration are estimated as

(1) With the east and north speeds and accelerations of the mobile robot as state variables, the system equation of the filter is as follows:

wherein,

in the formula, V_E,k，V_N,k，Acc_E,k，Acc_N,kRespectively representing the east speed, the north speed, the east acceleration and the north acceleration of the mobile robot at the moment k, V_E,k+1，V_N,k+1，Acc_E,k+1，Acc_N,k+1Respectively represents the east speed, the north speed, the east acceleration and the north acceleration of the mobile robot at the moment of k +1, and T is the sampling time, namely the number, of two adjacent frames of picturesAccording to the sampling period of the sample,in order to be a system random noise,q is a system noise covariance matrix; initial values of state quantities in the system equation are experimental data obtained by actual measurement, and are generally all 0;

(2) the filter's observation equation is shown below:

in the formula, v_3×1For observation of the equation random noise, v_3×1N (0, R), R is an observed noise covariance matrix;

and 4, step 4: according to the current objective function value and the current filter parameter value at each moment, the particle swarm algorithm is used for filtering the filter parameter Q_i,R_iOptimizing, and taking the obtained correction value as an input parameter of the CKF until an optimal state estimation value is obtained;

in the particle swarm optimization, each particle, namely a candidate solution of a d-dimensional solution space, is adjusted to be close to an optimal point according to own and group experience; the particle x updates its velocity and position through iterations:

in the formula: g is the number of iterations,andthe flight position and the flight speed of the particle x in the G generation are respectively;the best position searched so far for particle x;the best location searched so far for the entire population; w is the inertial weight, c₁、c₂Is a learning factor, r₁And r₂Is a random number between 0 and 1;

through continuous learning and updating, the particles move to the individual optimal position and the global optimal position in an accelerated manner, and finally, a global optimal solution is output; and taking the obtained correction value as an input parameter of the CKF until an optimal state estimation value is obtained.

Further, the step 1 specifically includes:

(1) the vertical distance from the projection center of the vehicle-mounted camera to the ground is Z_RThe normalized focal lengths of the horizontal axis and the vertical axis on the imaging plane of the camera are respectivelyf is the focal length of the camera, dx and dy are the size of unit pixel on the horizontal axis and the vertical axis of the imaging plane respectively, and the coordinate (c) of the point of the main optical axis₁,c₂) The normalized mapping relation of the pinhole camera model is as follows:

in the formula (X)_R，Y_R，Z_R) The coordinate of a certain point on the real ground in the camera coordinate system is shown, and the projection of the certain point on the image coordinate system is the SURF characteristic point;

(2) consider a lens distortion coefficient of k₁,k₂,k₃,k₄,k₅]Including the radial distortion coefficient and the tangential distortion coefficient, the corresponding points on the image coordinate system can be expressed as:

whereinTangential distortion vectorThe final coordinates of the corresponding pixel points are:

suppose a lens distortion coefficient [ k ]₁,k₂,k₃,k₄,k₅]Negligible, all zero, then:

differentiating the above formula with respect to time:

in the formula, V_x，V_y，V_zAre respectively coordinate points X_R、Y_R、Z_RThe derivative with respect to time represents the instantaneous speed of the camera in the direction of the three axes of the camera coordinate system x, y, z; v. of_x、v_yRespectively as coordinate points x_c、y_cWith respect to timeThe derivative of (a) represents the instantaneous speed of the SURF feature point in the directions of two axes of the image coordinate system x and y, and the formula is substituted into the formula:

if ground level is taken into account Z_RIs constant and the derivative with respect to time is 0, i.e. V_ZWhen 0, then:

matching SURF characteristic points on the two frames of images according to a nearest neighbor matching method to obtain the transverse displacement change delta x and the longitudinal displacement change delta y of the SURF characteristic points on the images, wherein the method comprises the following steps:

in the formula, T is the sampling time of two adjacent frames of pictures, that is, the sampling period of data.

Further, the initial values of the state quantities in the system equation in the step 3 are experimental data obtained by actual measurement, and are usually all 0; q is the system noise covariance matrix, noted

Q＝diag(q₁₁，q₂₂，q₃₃，q₄₄)

R is an observation noise covariance matrix and is recorded as R ═ R₁₁，r₂₂，r₃₃)

Will Q_i,R_iAs particles, the diagonal elements of the system noise matrix are made equal, i.e.

q₁₁＝q₂₂＝q₃₃＝q₄₄，r₁₁＝r₂₂＝r₃₃。

Further, in the step 4, according to the current objective function value and the current filtering parameter value at each moment, the particle swarm algorithm is used for the filtering parameter Q_i,R_iOptimizing, and taking the obtained correction value as an input parameter of the CKF until an optimal state estimation value is obtained, wherein the method specifically comprises the following steps:

estimating east and north velocities and accelerations of a mobile robotEast and north speeds and accelerations y measured by inertial navigation system_kThe mean square error of (a) is taken as an objective function, and when the objective function value is minimum, the optimal estimation is considered to be achieved:

in the formula, Q_i,R_iAs a noisy input parameter to the Kalman filter, f (Q)_i,R_i) Is Q_i,R_iThe mean square error of the measured value and the estimated value for the parameter,for the estimated value, M is the estimated length, y_kThe measured data is obtained.

Further, in the particle swarm optimization of step 4, the particle number is 100, the predetermined iteration number is 200, and the particle number is empirically preset

c₁＝c₂＝1.9

Using particle swarm algorithm to filter parameter Q_i,R_iOptimizing, and using the obtained correction value as input parameter of CKF until obtaining optimal valueA state estimate.

The invention has the advantages that:

(1) the east and north speeds of the indoor mobile robot are obtained through vision, only the vehicle-mounted camera is needed, other external equipment is not needed, and the independence is strong.

(2) The visual navigation information and the navigation information of the inertial navigation system are subjected to data fusion by adopting the CKF, so that the system error can be effectively compensated, and the obtained navigation information has higher precision than that of the information obtained by any visual and inertial single navigation method.

(3) And optimizing the filtering parameters by a particle swarm algorithm, and taking the obtained correction value as an input parameter of the CKF until an optimal state estimation value is obtained. The problems of poor CKF state estimation effect and low navigation precision caused by the fact that inaccurate noise statistics increase the uncertainty of a navigation model are solved. Therefore, the navigation speed error of the inertial navigation system is effectively compensated after the visual information is unlocked, and the navigation precision of the inertial visual integrated navigation is improved.

Drawings

FIG. 1 is a block flow diagram of the present invention.

FIG. 2 is a flow chart of a particle swarm optimization noise matrix in the invention.

Detailed Description

The invention will be described in further detail with reference to the following drawings:

the invention relates to an inertial/visual integrated navigation method adopting CKF (Cubature Kalman filters) based on particle swarm optimization, the flow is shown in figure 1, and the method comprises the following steps:

step 1: in the visual senseWhen the signals are effective, a camera carried by the mobile robot is used for collecting dynamic videos, and the speed of the camera is determined through image feature extraction and a nearest neighbor matching method; respectively extracting SURF (Speeded Up robust features) feature points in two adjacent image frames in a video by adopting a SURF algorithm, recording position coordinates of the feature points in an image coordinate system, and matching the SURF feature points on the two image frames according to a nearest neighbor matching method to determine the speed V of the camera on a horizontal plane_x、V_y；

The camera speed acquisition process comprises the following steps:

(1) the vertical distance from the projection center of the vehicle-mounted camera to the ground is Z_RThe normalized focal lengths of the horizontal axis and the vertical axis on the imaging plane of the camera are respectivelyf is the focal length of the camera, dx and dy are the size of unit pixel on the horizontal axis and the vertical axis of the imaging plane respectively, and the coordinate (c) of the point of the main optical axis₁,c₂) The normalized mapping relation of the pinhole camera model is as follows:

in the formula (X)_R，Y_R，Z_R) Is the coordinate of a certain point on the real ground in the camera coordinate system. The projection of the specific point on the image coordinate system is the SURF characteristic point.

(2) Consider a lens distortion coefficient of k₁,k₂,k₃,k₄,k₅]Including the radial distortion coefficient and the tangential distortion coefficient, the corresponding points on the image coordinate system can be expressed as:

whereinTangential distortion vectorThe final coordinates of the corresponding pixel points are:

suppose a lens distortion coefficient [ k ]₁,k₂,k₃,k₄,k₅]Negligible, all zero, then:

differentiating the above formula with respect to time:

in the formula, V_x，V_y，V_zAre respectively coordinate points X_R、Y_R、Z_RThe derivative of time represents the instantaneous speed of the camera in the directions of three axes of a camera coordinate system x, y and z; v. of_x、v_yRespectively as coordinate points x_c、y_cRegarding the derivative of time, representing the instantaneous speed of the SURF feature point in the directions of the two axes of the image coordinate system x and y, the formula is substituted into the formula:

if ground level is taken into account Z_RIs constant and the derivative with respect to time is 0, i.e. V_ZWhen 0, then:

matching SURF characteristic points on the two frames of images according to a nearest neighbor matching method to obtain the transverse displacement change delta x and the longitudinal displacement change delta y of the SURF characteristic points on the images, wherein the method comprises the following steps:

in the formula, T is the sampling time of two adjacent frames of pictures, that is, the sampling period of data.

Step 2: the speed V of the camera on the horizontal plane obtained according to the step 1_x，V_yCombining course angle measured by inertial navigation systemCalculating to obtain the east speed of the mobile robotAnd north velocityThe calculation formula is as follows:

and step 3: using CKF to move the speed of the robotAnd course angle variation omega obtained by inertial navigation system measurement_zAnd carrying out data fusion. The CKF updates the data by time according to the measured value and the state value of each moment of the system and the selected filtering parameter valueEstimating the east and north velocities and accelerations of the mobile robot from the metrology updatesLet us note that the estimates of east velocity and acceleration are respectivelyThe north-bound velocity and acceleration are estimated as

(1) With the east and north speeds and accelerations of the mobile robot as state variables, the system equation of the filter is as follows:

wherein,

in the formula, V_E,k，V_N,k，Acc_E,k，Acc_N,kRespectively representing the east speed, the north speed, the east acceleration and the north acceleration of the mobile robot at the moment k, V_E,k+1，V_N,k+1，Acc_E,k+1，Acc_N,k+1Respectively represents the east speed, the north speed, the east acceleration and the north acceleration of the mobile robot at the moment k +1, T is the sampling time of two adjacent frames of pictures, namely the sampling period of data,in order to be a system random noise,q is the system noise covariance matrix. And state quantities in system equationsThe initial values are experimental data obtained by actual measurement, and are generally all 0;

(2) the filter's observation equation is shown below:

in the formula, v_3×1Random noise for the observation equation. v. of_3×1N (0, R), R is the observed noise covariance matrix.

And 4, step 4: according to the current objective function value and the current filter parameter value at each moment, the particle swarm algorithm is used for filtering the filter parameter Q_i,R_iOptimizing, and taking the obtained correction value as an input parameter of the CKF until an optimal state estimation value is obtained.

In the particle swarm optimization, each particle (a candidate solution in a d-dimensional solution space) is adjusted to approach to an optimal point according to own and group experience. The particle x updates its velocity and position through iterations:

in the formula: g is the number of iterations,andthe flight position and the flight speed of the particle x in the G generation are respectively;the best position searched so far for particle x;the best location searched so far for the entire population; w is the inertial weight, c₁、c₂Is a learning factor, r₁And r₂Is a random number between 0 and 1.

The beneficial effects of the invention are illustrated as follows:

initial values of state quantities in a system equation are experimental data obtained by actual measurement, and are generally all 0; q is the system noise covariance matrix, noted

Q＝diag(q₁₁，q₂₂，q₃₃，q₄₄)

And R is an observation noise covariance matrix. Is expressed as R ═ R₁₁，r₂₂，r₃₃)

In the present invention, Q is_i,R_iAs particles, to simplify the optimization calculation, the diagonal elements of the system noise matrix are made equal, i.e.

q₁₁＝q₂₂＝q₃₃＝q₄₄，r₁₁＝r₂₂＝r₃₃，

Estimating east and north velocities and accelerations of a mobile robotEast and north speeds and accelerations y measured by inertial navigation system_kThe mean square error of (a) is taken as an objective function, and the optimal estimation is considered to be achieved when the objective function value is the minimum.

In the formula, Q_i,R_iAs a noisy input parameter to the Kalman filter, f (Q)_i,R_i) Is Q_i,R_iThe mean square error of the measured value and the estimated value for the parameter,for the estimated value, M is the estimated length, y_kThe measured data is obtained.

In the particle swarm optimization, the number of particles is 100, the preset iteration number is 200, and the particle swarm optimization is set empirically

c₁＝c₂＝1.9

The invention provides an inertial/visual combined navigation method based on particle swarm optimization for CKF, which solves the problems of poor CKF state estimation effect and low navigation precision caused by inaccurate noise statistics and uncertainty of a navigation model. The method mainly comprises the following steps: the method comprises the following steps: when the visual signals are effective, a camera carried by the mobile robot is used for acquiring dynamic videos, and the speed of the camera is determined through image feature extraction and a nearest neighbor matching method; step two: calculating the speed of the mobile robot according to the course angle measured by the inertial navigation system and the speed of the camera, and estimating the speed and the acceleration of the mobile robot by using CKF; step three: utilizing the CKF to obtain an estimated value of the system state through time updating and measurement updating according to the measurement value and the state value of each moment of the system and the selected filtering parameter value; step four: and optimizing the filter parameters Q and R by using a particle swarm algorithm according to the current objective function value and the current filter parameter value at each moment, and taking the obtained correction value as an input parameter of the CKF until the optimal state estimation value is obtained. According to the method, the particle swarm algorithm is adopted to optimize the Kalman filter noise matrix, so that the problems of poor CKF state estimation effect and low navigation precision caused by the fact that inaccurate noise statistics increases the uncertainty of a navigation model are solved, and the optimal estimation of the speed of the mobile robot is realized.

Claims (5)

1. An inertial/visual combined navigation method for CKF based on particle swarm optimization is characterized by comprising the following steps:

step 1: when the visual signal is effective, acquiring a dynamic image by using a camera carried by the mobile robot, and determining the speed of the camera by using an image feature extraction and nearest neighbor matching method; respectively extracting SURF characteristic points in two adjacent image frames in the video by adopting a SURF algorithm, recording position coordinates of the characteristic points in an image coordinate system, and simultaneously matching the SURF characteristic points on the two image frames according to a nearest neighbor matching method to obtain the image quality indexDetermining the velocity V of a camera in a horizontal plane_x、V_y；

Step 2: the speed V of the camera on the horizontal plane obtained according to the step 1_x、V_yCombining course angle measured by inertial navigation systemCalculating to obtain the east speed of the mobile robotAnd north velocityThe formula is as follows:

and step 3: using CKF to move the speed of the robotAnd course angle variation omega obtained by inertial navigation system measurement_zCarrying out data fusion;

the CKF obtains the velocity and acceleration estimation of the mobile robot in the east direction and the north direction through time updating and measurement updating according to the measurement value and the state value of each moment of the system and the selected filter parameter valueLet us note that the estimates of east velocity and acceleration are respectivelyThe north-bound velocity and acceleration are estimated as

(1) With the east and north speeds and accelerations of the mobile robot as state variables, the system equation of the filter is as follows:

${\hat{w}}_{k+1}=\left[\begin{array}{cccc}1& 0& T& 0\\ 0& 1& 0& T\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]{\hat{w}}_k+{\mathrm{\ω}}_{4\×1}^{*}$

wherein,

${\hat{w}}_{k+1}=\left[\begin{array}{c}{V}_{E,k+1}\\ V_{N,k+1}\\ {\mathrm{Acc}}_{E,k+1}\\ {\mathrm{Acc}}_{N,k+1}\end{array}\right],{\hat{w}}_k=\left[\begin{array}{c}{V}_{E,k}\\ V_{N,k}\\ {\mathrm{Acc}}_{E,k}\\ {\mathrm{Acc}}_{N,k}\end{array}\right]$

in the formula, V_E,k，V_N,k，Acc_E,k，Acc_N,kRespectively representing the east speed, the north speed, the east acceleration and the north acceleration of the mobile robot at the moment k, V_E,k+1，V_N,k+1，Acc_E,k+1，Acc_N,k+1Respectively represents the east speed, the north speed, the east acceleration and the north acceleration of the mobile robot at the moment k +1, T is the sampling time of two adjacent frames of pictures, namely the sampling period of data,in order to be a system random noise,q is a system noise covariance matrix; initial values of state quantities in the system equation are experimental data obtained by actual measurement, and are generally all 0;

(2) the filter's observation equation is shown below:

$\left[\begin{array}{c}\widetilde{V_E}\\ \widetilde{V_N}\\ {\mathrm{\ω}}_Z\end{array}\right]=\left[\begin{array}{c}{V}_{E,k}\\ V_{N,k}\\ \frac{V_{N,k}{\mathrm{Acc}}_{E,k}-V_{E,k}{\mathrm{Acc}}_{N,k}}{V_{E,k}^2+V_{V,k}^2}\end{array}\right]+v_{3\×1}$

in the formula, v_3×1For observation of the equation random noise, v_3×1N (0, R), R is an observed noise covariance matrix;

and 4, step 4: according to the current objective function value and the current filter parameter value at each moment, the particle swarm algorithm is used for filtering the filter parameter Q_i,R_iOptimizing, and taking the obtained correction value as an input parameter of the CKF until an optimal state estimation value is obtained;

in the particle swarm optimization, each particle, namely a candidate solution of a d-dimensional solution space, is adjusted to be close to an optimal point according to own and group experience; the particle x updates its velocity and position through iterations:

$v_{xd}^{(G+1)}={\mathrm{\ωv}}_{xd}^{\left(G\right)}+c_1{r}_1(p_{best}^{\left(G\right)}-X_{xd}^{\left(G\right)})+c_2{r}_2(g_{best}^{\left(G\right)}-X_{xd}^{\left(G\right)})$

$X_{xd}^{(G+1)}=X_{xd}^{\left(G\right)}+v_{xd}^{(G+1)}$

in the formula: g is the number of iterations,andthe flight position and the flight speed of the particle x in the G generation are respectively;the best position searched so far for particle x;the best location searched so far for the entire population; w is the inertial weight, c₁、c₂Is a learning factor, r₁And r₂Is a random number between 0 and 1;

through continuous learning and updating, the particles move to the individual optimal position and the global optimal position in an accelerated manner, and finally, a global optimal solution is output; and taking the obtained correction value as an input parameter of the CKF until an optimal state estimation value is obtained.

2. The method for inertial/visual combined navigation with CKF based on particle swarm optimization according to claim 1, wherein the step 1 specifically comprises:

(1) the vertical distance from the projection center of the vehicle-mounted camera to the ground is Z_RThe normalized focal lengths of the horizontal axis and the vertical axis on the imaging plane of the camera are respectivelyf is the focal length of the camera, dx and dy are the size of unit pixel on the horizontal axis and the vertical axis of the imaging plane respectively, and the coordinate (c) of the point of the main optical axis₁,c₂) Normalized mapping of pinhole camera modelsThe relationship is as follows:

$\left[\begin{array}{c}{x}_n\\ y_n\end{array}\right]=\left[\begin{array}{c}\frac{X_R}{Z_R}\\ \frac{Y_R}{Z_R}\end{array}\right]$

in the formula (X)_R，Y_R，Z_R) The coordinate of a certain point on the real ground in the camera coordinate system is shown, and the projection of the certain point on the image coordinate system is the SURF characteristic point;

(2) consider a lens distortion coefficient of k₁,k₂,k₃,k₄,k₅]Including the radial distortion coefficient and the tangential distortion coefficient, the corresponding points on the image coordinate system can be expressed as:

$\left[\begin{array}{c}{x}_d\\ y_d\end{array}\right]=(1+k_1{r}^2+k_2{r}^4+k_5{r}^6)\left[\begin{array}{c}{x}_n\\ y_n\end{array}\right]+t_d$

whereinTangential distortion vectorThe final coordinates of the corresponding pixel points are:

$\left[\begin{array}{c}{x}_c\\ y_c\end{array}\right]=\left[\begin{array}{cc}{f}_1& 0\\ 0& f_2\end{array}\right]\left[\begin{array}{c}{x}_d\\ y_d\end{array}\right]+\left[\begin{array}{c}{c}_1\\ c_2\end{array}\right]$

suppose a lens distortion coefficient [ k ]₁,k₂,k₃,k₄,k₅]Negligible, all zero, then:

$\left[\begin{array}{c}{x}_c\\ y_c\end{array}\right]=\left[\begin{array}{c}{f}_1\frac{X_R}{Z_R}\\ f_2\frac{y_R}{Z_R}\end{array}\right]\left[\begin{array}{c}{x}_d\\ y_d\end{array}\right]+\left[\begin{array}{c}{c}_1\\ c_2\end{array}\right]$

differentiating the above formula with respect to time:

$\left[\begin{array}{c}{V}_x\\ V_y\end{array}\right]=\left[\begin{array}{c}\frac{Z_R{v}_x}{f_1}+\frac{X_R{V}_Z}{Z_R}\\ \frac{Z_R{v}_y}{f_2}+\frac{Y_R{V}_Z}{Z_R}\end{array}\right]$

in the formula, V_x，V_y，V_zAre respectively coordinate points X_R、Y_R、Z_RThe derivative with respect to time represents the instantaneous speed of the camera in the direction of the three axes of the camera coordinate system x, y, z; v. of_x、v_yRespectively as coordinate points x_c、y_cRegarding the derivative of time, representing the instantaneous speed of the SURF feature point in the directions of the two axes of the image coordinate system x and y, the formula is substituted into the formula:

$\left[\begin{array}{c}{V}_x\\ V_y\end{array}\right]=\left[\begin{array}{c}\frac{Z_R{v}_x}{f_1}+\frac{1}{f_1}(x_c-c_1)V_z\\ \frac{Z_R{v}_y}{f_2}+\frac{1}{f_2}(y_c-c_2)V_z\end{array}\right]$

if ground level is taken into account Z_RIs constant and the derivative with respect to time is 0, i.e. V_ZWhen 0, then:

$\left[\begin{array}{c}{V}_x\\ V_y\end{array}\right]=\left[\begin{array}{c}\frac{Z_R{v}_x}{f_1}\\ \frac{Z_R{v}_y}{f_2}\end{array}\right]$

matching SURF characteristic points on the two frames of images according to a nearest neighbor matching method to obtain the transverse displacement change delta x and the longitudinal displacement change delta y of the SURF characteristic points on the images, wherein the method comprises the following steps:

$\left[\begin{array}{c}{V}_x\\ V_y\end{array}\right]=\left[\begin{array}{c}\frac{Z_R\mathrm{\Δ}x}{f_{1}T}\\ \frac{Z_R\mathrm{\Δ}y}{f_{2}T}\end{array}\right]$

in the formula, T is the sampling time of two adjacent frames of pictures, that is, the sampling period of data.

3. The inertial/visual combined navigation method using particle swarm optimization-based CKF according to claim 1, wherein the initial values of the state quantities in the system equation of step 3 are experimental data obtained by actual measurement, and are usually all 0; q is the system noise covariance matrix, noted

Q＝diag(q₁₁，q₂₂，q₃₃，q₄₄)

R is an observation noise covariance matrix and is recorded as R ═ R₁₁，r₂₂，r₃₃)

Will Q_i,R_iAs particles, the diagonal elements of the system noise matrix are made equal, i.e.

q₁₁＝q₂₂＝q₃₃＝q₄₄，r₁₁＝r₂₂＝r₃₃。

4. The method of integrated inertial/visual navigation with particle swarm optimization-based CKF according to claim 1, wherein the current objective function value and filter parameters at each time point in step 4 are used as the basis of the current objective function value and filter parameters at each time pointNumerical value of filtering parameter Q by particle swarm optimization_i,R_iOptimizing, and taking the obtained correction value as an input parameter of the CKF until an optimal state estimation value is obtained, wherein the method specifically comprises the following steps:

estimating east and north velocities and accelerations of a mobile robotEast and north speeds and accelerations y measured by inertial navigation system_kThe mean square error of (a) is taken as an objective function, and when the objective function value is minimum, the optimal estimation is considered to be achieved:

$f(Q_i,R_i)=\frac{1}{M}{\mathrm{\Σ}}_{k=1}^M{(y_k-\hat{w}(Q_i,R_i\left)\right)}^2$

in the formula, Q_i,R_iAs a noisy input parameter to the Kalman filter, f (Q)_i,R_i) Is Q_i,R_iThe mean square error of the measured value and the estimated value for the parameter,to estimateThe value of M is the estimated length, y_kThe measured data is obtained.

5. The inertial/visual combined navigation method with CKF based on particle swarm optimization according to claim 1, wherein in the particle swarm algorithm of step 4, the number of particles is 100, the predetermined number of iterations is 200, and the number is set empirically

c₁＝c₂＝1.9

$\begin{array}{cc}{Q}_{best}=\left[\begin{array}{cccc}0.02& 0& 0& 0\\ 0& 0.02& 0& 0\\ 0& 0& 0.02& 0\\ 0& 0& 0& 0.02\end{array}\right]& R_{best}=\left[\begin{array}{ccc}0.1& 0& 0\\ 0& 0.1& 0\\ 0& 0& 0.1\end{array}\right]\end{array}$

Using particle swarm algorithm to filter parameter Q_i,R_iOptimizing, and taking the obtained correction value as an input parameter of the CKF until an optimal state estimation value is obtained.