CN104035067A - Mobile robot automatic positioning algorithm based on wireless sensor network - Google Patents

Abstract

The invention discloses a mobile robot automatic positioning algorithm based on a wireless sensor network and belongs to the field of mobile robot automatic positioning. The method includes the following steps that wireless sensor nodes are uniformly installed, working space is divided into a plurality of equal-sized rectangular grids, and each node knows own calibration position. When a mobile robot moves, four sensor nodes of the rectangular grids where the mobile robot is located inform the robot of the position and speed information of the nodes through wireless communication. The mobile robot performs local optimal estimation on node information firstly through local kalman filters, integrates results of all local filters through a certain weight distribution strategy by an overall filter to obtain an optimal integrated result. According to the mobile robot automatic positioning algorithm based on the wireless sensor network, integrated-feedback federated kalman filters are utilized, the local optimal estimation of each local filter is sent to the overall filter so that information fusion is performed to obtain the overall optimal fusion, and the positioning accuracy is improved.

Description

The automatic location algorithm of a kind of mobile robot based on wireless sensor network

Technical field

The invention belongs to the automatic positioning field of mobile robot, relate to the automatic location algorithm of a kind of mobile robot based on wireless sensor network.

Background technology

Along with the development of computer technology, sensing technology etc., robot also, from first carrying out operation simple, that repeat to having higher intelligence at industrial flow-line, can complete more complicated task, has moved towards movement from static.Along with robot performance is constantly perfect, mobile robot's range of application is greatly expanded, not only in industry, and agricultural, national defence, medical treatment, is widely used in the industries such as service, and in the removal of mines, track down and arrest, rescue, radiation and space field etc. are harmful well to be applied with dangerous situation.Therefore, mobile robot technology has obtained the common concern of countries in the world.Automatic positioning technology is that it studies core, is also the gordian technique that realizes real intelligent and complete autonomous.At present, the conventional localization method of mobile robot mainly contains reckoning, signal lamp location, location based on map, road sign location, location based on vision etc.The research of various localization methods is nothing but for high-precision positioning result, and this just requires robot under nobody's intervention, to analyze voluntarily the exact position of extrapolating oneself, and the position of oneself and velocity information are shown and stored.

At present, mobile robot's aspect, automatic location exists following difficulty:

(1) positioning precision is limited;

(2) impact of external environment condition on location;

(3) material cost is higher;

(4) lack complete independence;

Along with the development of wireless sensor network, how combine wireless sensor network one of heat subject becoming current scientist with mobile robot technology.Since stepping into 21 century, by wireless sensor network, localization for Mobile Robot has been emerged in large numbers to many methods, but still there are many deficiencies in the theory and the method that have proposed, need further perfect.

The automatic location algorithm of mobile robot in fact mainly solves Data Source and two problems of data processing: how obtaining positional information and it is processed, wireless sensor network provides an approach well for it.Wireless sensor network location technology has independence and reliability is high, cost is low, use is flexible, be easy to the features such as arrangement, but the data message of wireless sensor network gained more or less can be subject to the impact of the factor such as external temperature, noise in actual applications, filtering method and information fusion just play vital effect for the precision of localization for Mobile Robot so.

Summary of the invention

Automatically locate the above-mentioned technical matters of existence for solving mobile robot,, the object of the present invention is to provide the automatic location algorithm of a kind of mobile robot based on wireless sensor network.Utilizing and merge--the Federated Kalman Filtering of reaction type carries out filtering, fusion, weighting to the information of sensor node, thereby realizes the location to mobile robot.The method is obviously being better than traditional wireless sensor networks filtering method aspect the precision of localization for Mobile Robot.

For achieving the above object, the invention provides following technical scheme:

The automatic location algorithm of a kind of mobile robot based on wireless sensor network.Comprise the following steps:

Step 1: wireless sensor node is evenly installed in given space, thereby space is divided into big or small rectangular grid such as multiple grades, each node is all known the position of its demarcation, mobile robot is in traveling process, and 4 sensor nodes that close on of its place rectangular grid are informed robot by radio communication by its position;

Step 2: behind the position and velocity information of mobile robot's receiving sensor node, the information of each node is carried out to local Kalman's optimal estimation, obtain evaluated error covariance matrix simultaneously;

Step 3: adopt fusion-feedback model Federated Kalman Filter, the local optimum of each local filter is estimated to send into global filtering device, carry out information fusion, obtain global optimum and merge;

Step 4: the procedural information of global filtering device carries out information distribution according to certain allocation criteria and local filter utilizes the information distribution factor respectively each local filter to be weighted simultaneously;

Step 5: mobile robot is presented at final position on display screen, and the positional information of oneself is deposited in to database.

Useful technique effect of the present invention is: in the present invention, adopt and merge--the Federated Kalman Filter of reaction type, the local optimum of each local filter is estimated to send into global filtering device, and carry out information fusion, obtain global optimum and merge, improve positioning precision; Carry out information distribution by global filtering device according to allocation criteria and local filter and utilize the data of information distribution factor pair local filter to be weighted, effectively reduce the error effect of local Kalman filtering, and in the situation that not changing measurement noise covariance matrix, reach the object of the each subsystem evaluated error of indirect change by changing distribution factor, thereby reach global optimum's filtering, significantly improve positioning precision.

Brief description of the drawings

In order to make object of the present invention, technical scheme and beneficial effect clearer, the invention provides following accompanying drawing and describe:

Fig. 1 is the process flow diagram of automatic positioning method of the present invention

Fig. 2 is the motion track figure of mobile robot in wireless sensor network

Fig. 3 is the process flow diagram of the federal Kalman's location algorithm of mobile robot based on wireless sensor network

Embodiment

Below in conjunction with accompanying drawing, the preferred embodiments of the present invention are described in detail.

In invention, adopt and merge--the Federated Kalman Filter of reaction type, the local optimum of each local filter is estimated to send into global filtering device, carry out information fusion, obtain global optimum and merge, improve positioning precision; Carry out information distribution by global filtering device according to allocation criteria and local filter and utilize the data of information distribution factor pair local filter to be weighted, effectively reduce the error effect of local Kalman filtering, and in the situation that not changing measurement noise covariance matrix, reach the object of the each subsystem evaluated error of indirect change by changing distribution factor, thereby reach global optimum's filtering, significantly improve positioning precision.

Fig. 1 is the process flow diagram of mobile robot's automatic positioning method of the present invention.As shown in the figure, should the localization for Mobile Robot based on wireless sensor network comprise two courses of work, i.e. Data Source and data processing.The present invention adopts the automatic position fixing process of mobile robot based on wireless sensor network as follows:

(1) wireless sensor node is evenly installed in given space, thereby space is divided into big or small rectangular grid such as multiple grades, each node is known its calibration position;

(2) mobile robot is in traveling process, and 4 sensor nodes that close on of its place rectangular grid are informed robot by radio communication by its positional information;

Fig. 2 is the two-dimensional space figure of mobile robot in wireless sensor network.In two-dimensional space, in the sampling k moment, robot place rectangular grid node is numbered respectively to a _k, b _k, c _k, d _k.Node a _k, b _k, c _k, d _kby its positional information [x ^a, y ^a], [x ^b, y ^b], [x ^c, y ^c], [x ^d, y ^d] send to robot.X, y are expressed as laterally and lengthwise position.In the time that robot enters next grid, the rectangular grid node at its place is numbered respectively a _k+1, b _k+1, c _k+1, d _k+1.Point a _k+1, b _k+1, c _k+1, d _k+1again its information is sent to robot, constantly repeat, thus the continuous location of realizing robot.

(3) behind the position and velocity information of mobile robot's receiving sensor node, the information of each node is carried out to local Kalman's optimal estimation, obtain evaluated error covariance matrix simultaneously.Fig. 3 is the process flow diagram of the federal Kalman's location algorithm of mobile robot based on wireless sensor network, and concrete implementation step is as follows:

According to the measured value of different sensors, the state variable of establishing dolly is X=[x, v _x, y, v _y] ^t, wherein: x, y represent laterally and lengthwise position, v _x, v _ybe respectively horizontal and longitudinal velocity.4 local filter of diverse location information design that provide according to node a, b, c, d.State equation is

$X_{k+1}^i={\mathrm{AX}}_k^i+V_k^i---\left(1\right)$

Wherein i is a, b, c, d.Wherein: A is that state moves matrix; for process noise, meeting average is zero, and standard deviation is gaussian distribution, and remember Q ⁱfor covariance matrix;

Subscript k represents updated time.Represent the positioning information update cycle with T,

$A=\left(\begin{array}{cccc}1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right)$

The measurement equation of listing the measured value of 4 sensor nodes is

$Y_k^i={\mathrm{CX}}_k^i+W_k^i---\left(2\right)$

Wherein: C is systematic survey matrix, so C is unit matrix; for measuring noise, meeting average is zero, and standard deviation is gaussian distribution, and remember that R is the covariance matrix of W.In local filter, utilize the new breath bringing in measured value to revise one-step prediction value, obtain local optimum and be estimated as

${\hat{X}}_k^i={\hat{X}}_{k|k-1}^i+K_k^i(y_k^i-C^i{\hat{X}}_{k|k-1}^i)---\left(3\right)$

Wherein for the gain of each subcard Thalmann filter), obtain evaluated error covariance matrix P simultaneously ^a, P ^b, P ^cand P ^d.

(4) adopt fusion-feedback model Federated Kalman Filter, the local optimum of each local filter is estimated to send into global filtering device, by formula

$\begin{array}{c}X=P(P^a{X}^a+P^b{X}^b+P^c{X}^c+P^d{X}^d)\\ P={({\left(P^a\right)}^{-1}+{\left(P^b\right)}^{-1}+{\left(P^c\right)}^{-1}+{\left(P^d\right)}^{-1})}^{-1}\end{array}---\left(4\right)$

Carry out information fusion, obtain global optimum and merge;

(5) procedural information of global filtering device carries out information distribution according to certain allocation criteria and local filter, utilizes the information distribution factor respectively each local filter to be weighted simultaneously.Concrete steps are as follows:

1) determine the information distribution factor: the information distribution factor plays a part very important in federal Kalman, it has completed global filtering and has estimated the weighting that part filter is estimated, according to formula

X＝P(P ^aX ^a+P ^bX ^b+P ^cX ^c+P ^dX ^d) (5)

X＝β ^aX ^a+β ^bX ^b+β ^cX ^c+β ^dX ^d

Can obtain:

β ^a＝P·P ^a

β ^b＝P·P ^b

β ^c＝P·P ^c

β ^d＝P·P ^d (6)

Obtain information distribution factor-beta ^a, β ^b, β ^c, β ^d.

2) information distribution: in global filtering device, do not measure, only carry out time renewal, therefore by the procedural information of global filtering device according to formula

$P_k^i={\left({\mathrm{\β}}^i\right)}^{-1}{P}_k$

$Q_k^i={\left({\mathrm{\β}}^i\right)}^{-1}{Q}_k---\left(7\right)$

$X_k^i=X_k$

Shown allocation criteria and local filter are carried out information distribution.Wherein information distribution factor-beta meets information conservation theorem β ^a+ β ^b+ β ^c+ β ^d=I.

(6) mobile robot is presented at final position on display screen, and the position of oneself and speed are deposited in to database.

Finally explanation is, above preferred embodiment is only unrestricted in order to technical scheme of the present invention to be described, although the present invention is described in detail by above preferred embodiment, but those skilled in the art are to be understood that, can make various changes to it in the form and details, and not depart from the claims in the present invention book limited range.

Claims (6)

1. the automatic location algorithm of the mobile robot based on wireless sensor network; Wireless sensor node is evenly installed in given space, thereby space is divided into big or small rectangular grid such as multiple grades, each node is known its calibration position; Mobile robot is in traveling process, 4 sensor nodes of vicinity of its place rectangular grid are informed robot by radio communication by its position and velocity information, and mobile robot first by local Kalman filter respectively carries out local optimum estimation to 4 nodes give position and velocity information around; Recycling global filtering device, by certain weights allocation strategy, each local filter result is merged, obtaining global optimum merges, again the procedural information of global filtering device is carried out to information distribution according to certain allocation criteria and local filter, utilize the information distribution factor respectively each local filter to be weighted simultaneously, object information is shown and deposited; Specifically comprise the following steps:

Step 1: wireless sensor node is evenly installed in given space, thereby space is divided into big or small rectangular grid such as multiple grades, each node is all known the position of its demarcation, mobile robot is in traveling process, and 4 sensor nodes that close on of its place rectangular grid are informed robot by radio communication by its position;

Step 2: behind the position and velocity information of mobile robot's receiving sensor node, the information of each node is carried out to local Kalman's optimal estimation, obtain evaluated error covariance matrix simultaneously;

Step 3: adopt fusion-feedback model Federated Kalman Filter, the local optimum of each local filter is estimated to send into global filtering device, carry out information fusion, obtain global optimum and merge;

Step 4: the procedural information of global filtering device carries out information distribution according to certain allocation criteria and local filter utilizes the information distribution factor respectively each local filter to be weighted simultaneously;

Step 5: mobile robot is presented at final position on display screen, and the position of oneself and speed are deposited in to database.

2. the automatic location algorithm of the mobile robot based on wireless sensor network according to claim 1, is characterized in that: in step 1, in two-dimensional space, in the sampling k moment, dolly place rectangular grid node is numbered respectively to a, b, c, d; Node a, b, c, d is by its positional information [x ^a, y ^a], [x ^b, y ^b], [x ^c, y ^c], [x ^d, y ^d] send to dolly; X, y are expressed as laterally and lengthwise position.

3. the automatic location algorithm of the mobile robot based on wireless sensor network according to claim 1, is characterized in that: the subcard Thalmann filter described in step 2 carries out local Kalman's optimal estimation to the proximity sense positional information of dolly place grid; According to the measured value of different sensors, the state variable of establishing dolly is X=[x, v _x, y, v _y] ^t, wherein: x, y are expressed as laterally and lengthwise position, v _x, v _ybe respectively horizontal and longitudinal velocity; According to node a, b, c, 4 local filter of diverse location information design that d provides; Robot location's state equation is i is a, b, c, d; Wherein: A is state-transition matrix; V _kfor process noise, meeting average is zero, and standard deviation is gaussian distribution, and remember that Q is V _kcovariance matrix; Subscript k represents updated time; Represent the positioning information update cycle with T,

$A=\left(\begin{array}{cccc}1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right)$

The measurement equation of the measured value of four sensor nodes is wherein: C is systematic survey matrix, so C is unit matrix; w is for measuring noise, and meeting average is zero, and standard deviation is gaussian distribution, and remember that R is the covariance matrix of W; In local filter, utilize the new breath bringing in measured value to revise one-step prediction value, obtain local optimum and be estimated as (wherein for the gain of each subcard Thalmann filter), obtain evaluated error covariance matrix P simultaneously ^a, P ^b, P ^cand P ^d.

4. the automatic location algorithm of the mobile robot based on wireless sensor network according to claim 1, is characterized in that: the global filtering device that utilizes described in step 3 is estimated that local optimum merge with global filtering device status information, and concrete steps are:

1) local optimum of each local filter is estimated to send into global filtering device;

2) by formula $\left.\begin{array}{c}X=P(P^a{X}^a+P^b{X}^b+P^c{X}^c+P^d{X}^d)\\ P={({\left(P^a\right)}^{-1}+{\left(P^b\right)}^{-1}+{\left(P^c\right)}^{-1}+{\left(P^d\right)}^{-1})}^{-1}\end{array}\right\}$ Carry out information fusion, obtain global optimum and merge.

5. the automatic location algorithm of the mobile robot based on wireless sensor network according to claim 1, is characterized in that: in step 4, in global filtering device, do not measure, only carry out time renewal, therefore by the procedural information of senior filter according to formula $\left.\begin{array}{c}{P}_k^i={\left({\mathrm{\β}}^i\right)}^{-1}{P}_k\\ Q_k^i={\left({\mathrm{\β}}^i\right)}^{-1}{Q}_k\\ X_k^i=X_k\end{array}\right\}$ Shown allocation criteria and local filter are carried out information distribution.Wherein information distribution factor-beta meets information conservation theorem β ^a+ β ^b+ β ^c+ β ^d=I.

6. an application rights requires the automatic location algorithm of the mobile robot based on wireless sensor network described in any one in 1 to 5.