CN103808316A

CN103808316A - Indoor-flying intelligent body inertial system and laser range finder combination type navigation improving method

Info

Publication number: CN103808316A
Application number: CN201210450567.XA
Authority: CN
Inventors: 王璐; 廖粤峰; 赵自超; 许可; 李冰; 马涛
Original assignee: Harbin Hengyu Mingxiang Technology Co Ltd
Current assignee: Harbin Hengyu Mingxiang Technology Co Ltd
Priority date: 2012-11-12
Filing date: 2012-11-12
Publication date: 2014-05-21

Abstract

The invention relates to an indoor-flying intelligent body inertial system and a laser range finder combination type navigation improving method. The navigation improving method is characterized in that a sensor portion (6) comprises a laser range finder URG (1), an IMU (Inertial Measurement Unit) (7), an atmospheric pressure altimeter (4), a magnetic field intensity sensor (5), a gyroscope (2) and an accelerometer (3); a navigation information calculation portion (14) comprises a USB (Universal Serial Bus) (8), a navigation computer (9), a converting device (10), an electric collecting line (11), a bus (12) and navigation information (13). The navigation improving method comprises the steps of acquiring position information by utilizing the laser range finder (1), acquiring attitude information by utilizing an inertial navigation system, fusing the position information with the attitude information by utilizing a unscented Kalman filter (UKF) method and a linear feature extraction algorithm, and finally performing a strapdown algorithm to realize track plotting. According to the navigation improving method, the indoor autonomous navigation of a flying intelligent body can be realized, the divergence and track plotting invalidation problems of a flying intelligent body combination type navigation system are effectively solved, and the navigation accuracy is improved.

Description

Improved method for integrated navigation of indoor flying intelligent body inertial system and laser range finder

The technical field is as follows:

the invention belongs to the field of multi-sensor information fusion.

Background

The inertial navigation is a track-pushing algorithm, the specific force and the angular rate of a carrier are respectively measured through an accelerometer and a gyroscope, and the pose information of the carrier is solved by utilizing an integral algorithm. A Strap-down Inertial Navigation System (SINS) is one type of Inertial Navigation and can continuously provide pose information of a carrier. However, inertial navigation systems suffer from the disadvantage that errors accumulate rapidly over time. The Global navigation Positioning System (GPS) and the SINS have excellent complementarity, and the combined navigation using the GPS and the SINS can reduce the cost and provide a Global precise navigation capability, so the SINS/GPS combined navigation System has been widely used.

However, the quality of the GPS signal is severely deteriorated in an indoor environment, the energy, arrival time and received signal-to-noise ratio of the GPS signal are greatly deteriorated as compared with those in a general environment, and the positioning accuracy of the GPS receiver, the availability of GPS data and the overall performance are greatly degraded, which may affect the performance of the indoor navigation system.

Disclosure of Invention

In order to solve the problems, the invention discloses an improved method for integrated navigation of an indoor flying intelligent body inertial system and a laser range finder, which takes an inertial navigation system as a core and forms a set of integrated navigation system with the laser range finder to realize attitude detection and positioning under the condition of no GPS signal indoors.

The main idea of the invention is to adopt an Unscented Kalman Filter (UKF) method to realize information fusion of an inertial measurement unit and a laser range finder under the condition that GPS data cannot be received indoors. The method comprises the steps of utilizing an Inertial Measurement Unit (IMU) to recur system pose information through an Inertial navigation algorithm, and utilizing sensors such as a laser range finder, a magnetic compass and a barometric pressure gauge to restrain divergence of the system pose. The improved method can effectively solve the problems of divergence and track pushing failure of the indoor flight intelligent agent combined navigation system, enhances the stability to external disturbance and improves the precision of the navigation system.

The purpose of the invention is realized as follows:

the improved method for the combined navigation of the indoor flying intelligent body inertial system and the laser range finder is characterized by comprising the following steps:

the sensor section 6 of the flight agent integrated navigation system structure includes: the device comprises a laser range finder URG1, an IMU 7, a barometric altimeter 4, a magnetic field intensity sensor 5, a gyroscope 2 and an accelerometer 3; the navigation information calculating section 14 includes: USB bus 8, navigation computer 9, conversion device 10, electrical connection 11, bus 12, navigation information 13.

The improved method for the combined navigation of the indoor flying intelligent body inertial system and the laser range finder is characterized in that the IMU 7 inertial measurement unit comprises a gyroscope 2 and an accelerometer 3.

The improved method for the combined navigation of the indoor flying intelligent body inertial system and the laser range finder is characterized in that the navigation information calculation part 14 is used for transmitting the ambient environment information to the navigation computer 9 by the laser range finder URG1 through a USB bus 8; the other sensors transmit the acquired data to the conversion device 10 through the electric connection wire 11 to obtain a data format which can be used by the navigation computer 9; transmitted to the navigation computer 9 through a bus 12 connected with the navigation computer 9, and the navigation computer 9 calculates navigation information 13 by the combined navigation improvement method provided by the invention.

The improved method for the integrated navigation of the indoor flying intelligent body inertial system and the laser range finder is characterized in that an algorithm flow chart of the integrated navigation system of the flying intelligent body is shown in figure 2, and firstly, a state equation and an observation equation of the integrated navigation system are calculated according to the characteristics of the flying intelligent body. The unscented Kalman filtering UKF algorithm is adopted to realize time updating, measurement updating is realized according to environmental information acquired by the laser range finder, the filter is corrected according to data of the sensor part to estimate the system state, and finally, the data processing and calculation work of the system are completed by a navigation computer, and the method specifically comprises the following steps:

step 1: describing the attitude of the flying intelligent body based on quaternion, and establishing a nonlinear state equation of the system; establishing a nonlinear observation equation of the system by taking the environment linear characteristic as an observed quantity, wherein information which can be detected by a laser range finder is collectively called environment information;

step 2: the estimation of the system state is completed by a UKF filter, and before filtering, filter parameters and global map information need to be initialized;

and step 3: time updating, namely recursion of pose information of the system at the next moment according to a nonlinear state equation by data of an inertial measurement unit and the state of the system at the previous moment;

and 4, step 4: acquiring observation data of a laser range finder, and extracting the linear characteristics of surrounding obstacles;

and 5: measuring and updating, namely obtaining the estimation of the environmental straight line characteristics which are calculated by recursion pose information and appear in the laser radar visual field according to the pose information of the carrier and an observation equation;

step 6: matching the recursion straight line characteristics obtained in the step 4 with the environment straight line characteristics extracted according to the laser range finder in the step 5, and carrying out filtering updating according to other parameters obtained by the sensor part to obtain estimated system state information;

and 7: adding the newly observed straight line information into a global map, and selecting a straight line to be observed at the next moment;

and 8: the navigation computer completes the calculation and returns to the step 3 to continue the filtering calculation of the next moment to estimate new parameters.

Wherein the state quantities of the system in the step 1 are the attitude, the speed and the position of the carrier and the zero offset of the gyroscope; the observed quantity of the system is the obstacle straight line characteristic information in the visual field of the laser range finder.

Initializing a filter in the step2 needs to initialize the statistical characteristics of a system state, an error covariance matrix, system process noise and observation noise; the initial map takes an initial point as an origin point, and stores an environment straight line feature in the map as map information, wherein the environment straight line feature is a polar coordinate parameter from the origin point to a vertical point where a straight line vertical line is located.

And 3, updating time to obtain a system state, namely pose information of the carrier.

The method for fitting the straight line by using the laser range finder data in the step 4 comprises the following steps:

step 1, acquiring data of the laser range finder;

step2, obtaining a plurality of continuous barrier point clusters, connecting head and tail points into a straight line, and dividing the straight line into a plurality of local maximum points according to the distance from each point to the straight line; dividing the point clusters into a plurality of corresponding groups according to the obtained local maximum point; performing straight line fitting on each group by using a least square method; connecting the straight lines end to obtain a continuous group of continuous straight lines;

and Step 3, acquiring characteristic parameters according to the straight line fitted in Step2, and calculating polar coordinate parameters of the flying intelligent body to a vertical point where the straight line vertical line is located.

And 5, measuring and updating, namely determining the map environment to be acquired by the laser range finder according to the position of the flying intelligent body obtained by updating the time at the current moment and the observation range of the laser range finder, and obtaining the linear characteristic of the map environment in the acquisition range relative to the recursion position at the moment according to an observation equation.

The added environment straight line feature in step 7 is a straight line feature of the environment relative to the origin.

Compared with the traditional combined navigation method, the method has the following advantages:

first, the invention adopts the laser range finder as the observation device of the horizontal position, can make up for the problem of track pushing divergence caused by that the inertial integrated navigation system can not receive GPS data indoors, and has stronger practicability indoors.

Secondly, the sampling frequency of the laser range finder used in the invention is larger than the sampling frequency of the GPS, so that the dispersion of the SINS can be inhibited to the maximum extent.

Thirdly, the measuring precision of the laser range finder used by the invention is higher than that of a GPS device, so that the state estimation precision of the system can be improved; the linear feature matching is carried out under the condition of fully considering the prior information of the system, so that the reliability is higher, and the system can be suitable for more complex environments; and can inhibit the noise of the laser range finder to a certain extent.

Fourthly, aiming at the problems of traditional track positioning failure in the non-planar motion process and the problem of the binding applied to the SLAM algorithm, the invention can well solve the problems.

Drawings

FIG. 1 is a block diagram of a flight agent integrated navigation system;

FIG. 2 is a flow chart of an algorithm for a flight agent integrated navigation system;

FIG. 2 is a schematic diagram of environment information extraction;

FIG. 4 is a flow chart of a laser rangefinder data fitting straight line;

FIG. 5 Global map straight line selection schematic

In the figure: 1 laser range finder URG, 2 gyroscope, 3 accelerometer, 4 barometric altimeter, 5 magnetic field intensity sensor, 6 sensor part, 7IMU, 8USB bus, 9 navigation computer, 10 conversion device, 11 electric appliance connection line, 12 bus, 13 navigation information, 14 navigation information calculation part.

Detailed Description

The technical scheme of the invention is explained in detail in the following with the accompanying drawings:

the structure of the navigation system combining the inertial system and the laser range finder is shown in figure 1, the sensor part comprises the laser range finder, an IMU, an air pressure altimeter and a magnetic field intensity sensor, wherein an IMU inertial measurement unit comprises a gyroscope and an accelerometer, and the laser range finder transmits the ambient environment information to a navigation computer through a USB bus; the other sensors transmit the acquired data to the conversion device through the electric connection line to obtain a data format which can be used by the navigation computer; the navigation computer obtains the navigation information through calculation by the combined navigation improvement method provided by the invention.

An algorithm flow chart of an inertial system and laser range finder combined navigation system is shown in fig. 2, recursion is performed on system pose information according to collected sensor data, observed quantity information of a carrier in a recursion state is obtained, and then filtering processing is performed on the system according to environment information obtained by the laser range finder, and the method specifically comprises the following steps:

step 1, establishing a system model of the flying intelligent body based on quaternion, and calculating a state equation of the combined navigation system of the flying intelligent body, wherein the method specifically comprises the following steps:

(a) statorAttitude quaternion Q E R of sense flight intelligent agent⁴Is a four-dimensional column vector:

Q^{T} = [\begin{matrix} q_{0} & q^{T} \end{matrix}]

= {[\begin{matrix} q_{0} & q_{1} & q_{2} & q_{3} \end{matrix}]}^{T}

wherein q is [ q ]₁ q₂ q₃]^TIs a vector portion; q. q.s₀In order to be the real part of the data,

<math> <mrow> <mover> <mi>e</mi> <mo>&OverBar;</mo> </mover> <mo>=</mo> <msup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>e</mi> <mi>x</mi> </msub> </mtd> <mtd> <msub> <mi>e</mi> <mi>y</mi> </msub> </mtd> <mtd> <msub> <mi>e</mi> <mi>z</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> </mrow> </math>

is the unit vector and phi is the rotation angle.

(b) Selecting a north, east and ground coordinate system as navigationCoordinate system, wherein P ═ x y z]^TAnd V ═ V_x v_y v_z]^TRespectively the position and speed of the carrier, zero offset of the gyro, B_b＝[B_bx B_by B_bz]^TIs the projection of the magnetic field strength on a carrier coordinate system. Taking the position, speed, attitude quaternion and gyro drift as the system state quantity X ═ PvQ epsilon_ω]^TTaking the characteristic distance l and the characteristic angle theta of the environmental straight line and the magnetic field intensity B_bAnd barometric pressure gauge A_bInformation as system output Z = [ l =_i θ_i B_b A_b]^TAnd establishing a nonlinear state and a measurement equation of the system.

The system process model is as follows:

wherein,

is the attitude rate of the carrier, f_bIs the specific force acting on the carrier, v_a、v_ω、Respectively, accelerometer, gyroscope, and gyroscope zero-bias process noise, assuming that the system process noise is zero-mean white gaussian noise.

Is a strapdown matrix with the expression of

C_{b}^{n} = {(C_{n}^{b})}^{T} = (\begin{matrix} q_{1}^{2} + q_{2}^{2} - q_{3}^{2} - q_{4}^{2} & 2 (q_{2} q_{3} - q_{1} q_{4}) & 2 (q_{2} q_{4} + q_{1} q_{3}) \\ 2 (q_{2} q_{3} + q_{1} q_{4}) & q_{1}^{2} - q_{2}^{2} + q_{3}^{2} - q_{4}^{2} & 2 (q_{3} q_{4} - q_{1} q_{2}) \\ 2 (q_{2} q_{4} - q_{1} q_{3}) & 2 (q_{3} q_{4} + q_{1} q_{2}) & q_{1}^{2} - q_{2}^{2} - q_{3}^{2} + q_{4}^{2} \end{matrix}) - - - (3)

(c) The observed quantity of the system observation equation is the linear characteristic of the environment observed by the laser range finder carried on the flying intelligent body. The linear features can be obtained according to the position and the posture of the carrier and linear feature information in the global map, the corresponding relation of the linear features is the observation equation of the system, and the calculation thought and the derivation process of the system observation equation are given below.

The system observation equation needs to obtain the intersecting line from the origin of the carrier system coordinate to the xoy plane O of the carrier system coordinate system and the environment information plane S in a global map under the carrier system coordinate systemThe vertical point of (2) is a polar coordinate parameter in the carrier coordinate system. The specific idea is as follows: selecting the normal of plane O

The origin of the overload system is selected from the normal of the plane S

Intersecting the origin of the vector system; taking into account the origin of the carrier system to the intersection line

Perpendicular line ofNormal to plane O

Perpendicular to and intersecting the origin of the carrier systemFrom the normalAt one isIn the plane U, the normal line of the plane U is an intersecting lineDue to the normal to the plane S

Normal to plane U

And normal to plane OIntersect, then normal line

If the plane is located on the plane U, the equation of the plane U can be determined according to the normal equations of the plane O and the plane S; considering that all three planes S, O, U pass through the vertical point a, the coordinate of the vertical point a can be obtained according to the equation of the three planes, so as to calculate the polar coordinate parameter of the vertical point a in the carrier coordinate system. S, O, U plan view schematic figure 3, the following details the derivation process.

Knowing the system position and attitude quaternion P = [ x ]₀ y₀ z₀]^T,Q=[q₀ q₁ q₂ q₃]^TAccording to the attitude quaternion, the attitude matrix from the carrier system to the earth coordinate system can be obtained

The equation for plane S is: k is a radical of₁₁xk₁₂y1。

Obtaining the normal unit vector of the plane O according to the attitude matrix

<math> <mrow> <mover> <mi>b</mi> <mo>&RightArrow;</mo> </mover> <mo>=</mo> <msubsup> <mi>C</mi> <mi>n</mi> <mi>b</mi> </msubsup> <msup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> <mo>=</mo> <msup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>b</mi> <mn>1</mn> </msub> </mtd> <mtd> <msub> <mi>b</mi> <mn>2</mn> </msub> </mtd> <mtd> <msub> <mi>b</mi> <mn>3</mn> </msub> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> <mo>.</mo> </mrow> </math>

According to the equation of plane S, there are:

\{\begin{matrix} [\begin{matrix} x_{0} - x & y_{0} - y \end{matrix}] {[\begin{matrix} - k_{12} & k_{11} \end{matrix}]}^{T} = 0 \\ k_{11} x + k_{12} y = 1 \end{matrix} - - - (4)

wherein the first equation in the system of equations indicates that the normal vector is perpendicular to the plane S and the second equation indicates that the intersection of the normal and the plane S is on the plane.

Obtaining the S normal vector of the overload system origin

<math> <mrow> <mover> <mi>c</mi> <mo>&RightArrow;</mo> </mover> <mo>=</mo> <msup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mfrac> <mrow> <msubsup> <mi>k</mi> <mn>12</mn> <mi>x</mi> </msubsup> <msub> <mi>x</mi> <mn>0</mn> </msub> <mo>+</mo> <msub> <mi>k</mi> <mn>11</mn> </msub> <msub> <mi>k</mi> <mn>12</mn> </msub> <msub> <mi>y</mi> <mn>0</mn> </msub> <mo>-</mo> <msub> <mi>k</mi> <mn>11</mn> </msub> </mrow> <mrow> <msubsup> <mi>k</mi> <mn>11</mn> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>k</mi> <mn>12</mn> <mn>2</mn> </msubsup> </mrow> </mfrac> </mtd> <mtd> <mfrac> <mrow> <msubsup> <mi>k</mi> <mn>12</mn> <mn>2</mn> </msubsup> <msub> <mi>y</mi> <mn>0</mn> </msub> <mo>+</mo> <msub> <mi>k</mi> <mn>11</mn> </msub> <msub> <mi>k</mi> <mn>12</mn> </msub> <msub> <mi>x</mi> <mn>0</mn> </msub> <mo>-</mo> <msub> <mi>k</mi> <mn>12</mn> </msub> </mrow> <mrow> <msubsup> <mi>k</mi> <mn>11</mn> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>k</mi> <mn>12</mn> <mn>2</mn> </msubsup> </mrow> </mfrac> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> <mo>=</mo> <msup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>c</mi> <mn>1</mn> </msub> </mtd> <mtd> <msub> <mi>c</mi> <mn>2</mn> </msub> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>5</mn> <mo>)</mo> </mrow> </mrow> </math>

Normal to plane O, S

The vector, the equation of plane U satisfies:

|\begin{matrix} x - x_{0} & y - y_{0} & z - z_{0} \\ b_{1} & b_{2} & b_{3} \\ c_{1} & c_{2} & 0 \end{matrix}| = 0 - - - (6)

the equation for plane U can be found: k is a radical of₂₁xk₂₂y+k₂₃z1。

Any straight line of the plane O and the normal linePerpendicular, according to the normal to plane O

The unit vector of (a) is:

[x-x₀ y-y₀ z-z₀][b₁ b₂ b₃]^T=0（7）

the equation for plane O is obtained: k is a radical of₃₁xk₃₂y+k₃₃z1。

Wherein,

k_{3 i} = \frac{b_{i}}{b_{1} x_{0} + b_{2} y_{0} + b_{3} z_{0}}, i = 1,2,3 .

then from the equation of plane O, S, U, the coordinates of intersection a can be found as:

[\begin{matrix} x_{1} \\ y_{1} \\ z_{1} \end{matrix}] {[\begin{matrix} k_{11} & k_{12} & 0 \\ k_{21} & k_{22} & k_{23} \\ k_{31} & k_{32} & k_{33} \end{matrix}]}^{- 1} [\begin{matrix} 1 \\ 1 \\ 1 \end{matrix}] - - - (8)

the vector from the plumbing point a to the origin of the vector system is:

<math> <mrow> <mover> <mi>g</mi> <mo>&RightArrow;</mo> </mover> <mo>=</mo> <msup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>x</mi> <mn>1</mn> </msub> <mo>-</mo> <msub> <mi>x</mi> <mn>0</mn> </msub> </mtd> <mtd> <msub> <mi>y</mi> <mn>1</mn> </msub> <mo>-</mo> <msub> <mi>y</mi> <mn>0</mn> </msub> </mtd> <mtd> <msub> <mi>z</mi> <mn>1</mn> </msub> <mo>-</mo> <msub> <mi>z</mi> <mn>0</mn> </msub> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> <mo>,</mo> </mrow> </math>

obtaining an observation equation of the system:

<math> <mrow> <mi>z</mi> <mo>=</mo> <mi>h</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>l</mi> <mi>i</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>θ</mi> <mi>i</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>B</mi> <mi>b</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>A</mi> <mi>b</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mover> <mrow> <mo>|</mo> <mo>|</mo> <mi>g</mi> <mo>|</mo> <mo>|</mo> </mrow> <mo>&RightArrow;</mo> </mover> <mn>2</mn> </msub> </mtd> </mtr> <mtr> <mtd> <mi>π</mi> <mo>/</mo> <mo>-</mo> <mi>arccos</mi> <mo>[</mo> <mover> <mi>g</mi> <mo>&RightArrow;</mo> </mover> <mo>·</mo> <mover> <mi>b</mi> <mo>&RightArrow;</mo> </mover> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mover> <mrow> <mo>|</mo> <mo>|</mo> <mi>g</mi> <mo>|</mo> <mo>|</mo> </mrow> <mo>&RightArrow;</mo> </mover> <mn>2</mn> </msub> <msub> <mover> <mrow> <mo>|</mo> <mo>|</mo> <mi>b</mi> <mo>|</mo> <mo>|</mo> </mrow> <mo>&RightArrow;</mo> </mover> <mn>2</mn> </msub> <mo>)</mo> </mrow> <mo>]</mo> </mtd> </mtr> <mtr> <mtd> <msub> <mi>R</mi> <mi>be</mi> </msub> <mrow> <mo>(</mo> <mi>q</mi> <mo>)</mo> </mrow> <msub> <mi>B</mi> <mi>b</mi> </msub> </mtd> </mtr> <mtr> <mtd> <mo>-</mo> <msub> <mi>P</mi> <mi>z</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>+</mo> <mi>v</mi> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>9</mn> <mo>)</mo> </mrow> </mrow> </math>

where v is the observed noise of the system.

And 2, considering that the UKF algorithm has moderate calculation amount and high precision, estimating the system state by adopting the UKF algorithm. Initializing an unscented Kalman filter by using the unscented Kalman filter conforming to the model in the step 1, initializing global map information, and extracting linear characteristic parameters at the initial moment;

and observing a vector z according to the state quantity x selected in the step 1. Process noise w_kAnd the measurement noise v_kWhite gaussian noise of dimensions n and m, respectively, and having the following statistical properties:

selecting the state variable of the extended maintenance system as x^a＝[x^T w^T v^T]^T。

The unscented kalman filter is initialized as follows:

{\hat{x}}_{0}^{a} = E [c_{0}^{s}] = {[\begin{matrix} x_{0}^{T} & 0 & 0 \end{matrix}]}^{T}

P_{0}^{a} = E [(x_{0}^{a} - {\hat{x}}_{0}^{a}) {(x_{0}^{a} - {\hat{x}}_{0}^{a})}^{T}] = diag (P_{0}, R_{w}, R_{v}) - - - (11)

and defining the space position of the flying intelligent agent at the moment as the origin of the global map.

Step 3, updating the time of the unscented Kalman filter, and recurrently deducing the system state at the next moment;

selecting Sigma points and weights in a time updating stage:

the mean weight and the variance weight corresponding to the Sigma point are respectively:

wherein L =2n + m, alpha determines the distance between the sampling point and the mean value, beta is an adjusting parameter, and lambda is alpha²And (n + k) -n, k describes system distribution information, k is more than or equal to 0 to ensure the semi-positive nature of the variance matrix, and the invention takes alpha =1, beta =0 and k is 2.

Calculate Sigma Point

Propagation results along the sum of the nonlinear functions f (·):

the posterior mean and covariance of the propagation of the random vector along the nonlinear function f (-) are thus:

and 4, acquiring observation data of the laser range finder, extracting features, fitting a straight line, and calculating to obtain the feature parameters of the actual observation straight line so as to acquire the spatial position of the flying intelligent body. The method for extracting the characteristics comprises the following two steps: region segmentation and feature extraction. In the area division, an area division method based on Euclidean distance between data points is adopted to divide the collected data point set of the laser range finder into different areas, and the specific method is as follows:

step 1, reading data of a laser range finder at the moment of k-1 in a navigation computer, wherein the scanning range of the laser range finder is 240 degrees, and obtaining the distances from 700 obstacle points to a flying intelligent agent and the polar coordinates of each point;

step2, extracting the characteristics of the data in Step 1, wherein the method for extracting the characteristics comprises the following two steps: region segmentation and feature extraction. Aiming at the flying intelligent agent, a region division method based on Euclidean distance between data points of the laser range finder is adopted in the division of the region, and the collected data point set of the laser range finder is divided into a plurality of regions which are not connected. The specific method comprises the following steps:

as shown in FIG. 4, let region ρ_iThe first point and the last point of (1) are M respectively_s(X_s,Y_s) And M_e(X_e,Y_e). The connecting line between the two points is J, and the length of the connecting line is d_a. Region ρ_iInterior point M_kDistance to J is recorded as d_kSetting the threshold value of the distance from the point to J as d_TIf d is_k＞d_TThen, with point d_kRegion rho for a division point_iIs divided into two parts. Otherwise, do not divide and utilize ρ_iThe laser range finder data point in (1) is fitted into a straight line by using a least square method, and straight line parameters are solved. Repeating the steps until all the basic feature extraction is completed, wherein the main steps are as follows:

a. connecting start and end points M_sAnd M_eThe connecting line between the two points is J;

b. each data point M in the measurement area_iDistance to line J. Obtaining a distance value d_i＝{

d

_i1,2, … n, and obtaining the maximum value d of the first extraction_max1The corresponding point is M_max1；

c. At point M_max1Dividing the region into two parts A for dividing the point₁，A₂And d is measured in two regions respectively_max2，d_max3And is compared with a threshold value d_maxTComparison is made, e.g. d_maxT＜d_maxiThen return to step a, e.g. d_maxT＞d_maxiThen the segmentation in this region is stopped and a straight line fit is performed using the least squares method, where the threshold is chosen empirically.

And Step 3, acquiring characteristic parameters according to the fitted straight line in Step2, and calculating the spatial distance and the attitude between the straight line and the flying intelligent agent.

Step 5, measuring and updating the unscented Kalman filter in the step2, and updating the observed quantity of the system;

calculate Sigma Point { χ_i,k|k-1Propagation results of i =0, …,2n along a non-linear function h (·)

The posterior mean, the autocovariance and the cross covariance of the propagation of the random vector along the nonlinear function h (-) are obtained

Step 6, according to the observed quantity obtained by recursion calculation in the step 5, the actual observed quantity obtained in the step 4 and other parameters measured by the sensor part, filtering updating is realized, and the state of the system is estimated;

the state gain matrix, the estimated value and the estimated state error variance matrix are respectively as follows:

K_{k} = P_{{\tilde{x}}_{k | k - 1} {\tilde{z}}_{k | k - 1}} P_{{\tilde{z}}_{k | k - 1}}^{- 1} - - - (21)

P_{k} = P_{k | k - 1} - K_{k} P_{{\tilde{z}}_{k | k - 1}} K_{k}^{T} - - - (23)

and 7, according to the system state of the flying intelligent body estimated in the step 6, calculating to obtain the absolute position of a straight line in the global map, wherein the straight line is synthesized by the data of the laser range finder, adding the newly observed straight line information into the global map to update the map, and selecting the straight line to be observed at the next moment, wherein the selection method specifically comprises the following steps:

as shown in fig. 5, the dotted line indicates that the measurement angle range of the laser range finder is 0 to 240 °, the measurement distance is D, the point of the flying intelligent agent is P, Q in the system state of the flying intelligent agent at the next moment is estimated according to step 6, the attitude of the flying intelligent agent at the next moment is calculated, and the estimation value of the characteristic angle is obtained

And according to the angle value theta in the polar coordinate value of each line segment intersection point in the global map information_LExtracting the minimum value theta_L-minCalculating a minimum value point P₁Difference from the estimated value

If e₁If > 0, the line is retained, if e₁If < 0, the straight line is removed;

and 8, returning to the step 3, continuing the filtering calculation at the next moment, and estimating new parameters. And (4) the navigation computing system of the flight intelligent agent completes data processing and resolving of the integrated navigation according to the navigation system state equation and the observation equation updated in the step (4), and the algorithm process is shown in figure 5.

The navigation computing system records the current state as the initial state when computing is started, the current time is taken as 0 time, and the current position is the coordinate origin of the global map. Establishing a flight intelligent body system model according to the step 1 and the step2, adopting a UKF filtering method, realizing time updating of the filter according to the step 3, drawing a global map according to the step 4, ensuring that the flight intelligent body can accurately know the position of the flight intelligent body under the global map at each moment, and simultaneously recording relative position change information in each period. And then, realizing measurement updating of the filter according to the step 5, calculating an observation estimated value, and finally, correcting navigation information of the mobile terminal according to the step 6 and the step 7, adding a global map, and estimating the system state.

Claims

1. The improved method for the combined navigation of the indoor flying intelligent body inertial system and the laser range finder is characterized by comprising the following steps: the sensor part (6) of the flight agent integrated navigation system structure comprises: the device comprises a laser range finder URG (1), an IMU (7), an air pressure altimeter (4), a magnetic field intensity sensor (5), a gyroscope (2) and an accelerometer (3); the navigation information calculation section (14) includes: USB bus (8), navigation computer (9), switching device (10), electric connection (11), bus (12), navigation information (13).

2. The integrated navigation improvement method of the indoor flying intelligent body inertial system and the laser range finder as claimed in claim 1, wherein: the inertial measurement unit of the IMU (7) comprises a gyroscope (2) and an accelerometer (3).

3. The integrated navigation improvement method of the indoor flying intelligent body inertial system and the laser range finder as claimed in claim 1, wherein: the navigation information calculating part (14) transmits the surrounding environment information to the navigation computer (9) by the laser range finder URG (1) through a USB bus (8); the other sensors transmit the acquired data to the conversion device (10) through an electric connection line (11) to obtain a data format which can be used by the navigation computer (9); the navigation information is transmitted to the navigation computer (9) through a bus (12) connected with the navigation computer (9), and the navigation computer (9) obtains the navigation information (13) through calculation by the combined navigation improvement method provided by the invention.

4. The integrated navigation improvement method of the indoor flying intelligent body inertial system and the laser range finder as claimed in claim 1, wherein: the flow chart of the flight agent integrated navigation system algorithm is shown in fig. 2, and firstly, the state equation and the observation equation of the integrated navigation system are calculated according to the characteristics of the flight agent; the unscented Kalman filtering UKF algorithm is adopted to realize time updating, measurement updating is realized according to environmental information acquired by the laser range finder, the filter is corrected according to data of the sensor part to estimate the system state, and finally, the data processing and calculation work of the system are completed by a navigation computer, and the method specifically comprises the following steps:

(a) defining an attitude quaternion of the flying intelligent body;

(b) selecting the position, speed, attitude quaternion and gyro drift of the flying intelligent body as system state quantities, and establishing a nonlinear state and a measurement equation of the system;

(c) calculating an observation equation of the flight intelligent agent;

step2, establishing an unscented Kalman filter according with the model in the step 1, initializing the unscented Kalman filter, initializing global map information, and extracting linear characteristic parameters at the initial moment;

step 3, according to the unscented Kalman filter in the step2, time updating is realized, and the system state at the next moment is recurred;

step 4, acquiring observation data of the laser range finder, extracting features, fitting a straight line, and calculating to obtain the feature parameters of the actual observation straight line so as to acquire the spatial position of the flying intelligent agent;

step 1, reading data of the laser range finder at the moment of k-1 in a computer;

step2, extracting the characteristics of the data in Step 1, wherein the method for extracting the characteristics comprises the following two steps: region segmentation and feature extraction; aiming at the flying intelligent agent, a region division method based on Euclidean distance between laser data points is adopted in the division of the regions, and the collected laser data point set is divided into a plurality of regions which are not connected;

step 3, acquiring characteristic parameters according to the fitted straight line in Step2, and calculating the spatial distance and the attitude between the straight line and the intelligent flying body;

step 7, according to the system state of the flying intelligent body estimated in the step 6, the absolute position of a straight line which is fit by the data of the laser range finder in the global map can be calculated, newly observed straight line information is added into the global map, the update of the map is realized, and the straight line to be observed at the next moment is selected;

step 8, returning to the step 3, continuing the filtering calculation at the next moment, and estimating new parameters; and (4) the navigation computing system of the flight intelligent agent completes the data processing and resolving of the integrated navigation according to the navigation system state equation and the observation equation updated in the step (4), and the algorithm process is shown in figure 2.