CN104316058B - Interacting multiple model adopted WSN-INS combined navigation method for mobile robot - Google Patents
Interacting multiple model adopted WSN-INS combined navigation method for mobile robot Download PDFInfo
- Publication number
- CN104316058B CN104316058B CN201410614772.4A CN201410614772A CN104316058B CN 104316058 B CN104316058 B CN 104316058B CN 201410614772 A CN201410614772 A CN 201410614772A CN 104316058 B CN104316058 B CN 104316058B
- Authority
- CN
- China
- Prior art keywords
- mobile robot
- error
- ins
- directions
- model
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Feedback Control In General (AREA)
Abstract
The invention belongs to the field of multi-sensor data fusion, and discloses an interacting multiple model adopted WSN-INS combined navigation method for a mobile robot. The interacting multiple model adopted WSN-INS combined navigation method is characterized in that two error models are used to perform modeling on the movement of the indoor mobile robot, and optimum estimation to the navigation parameter error of the mobile robot is obtained through interaction fusion of two groups of filters; the two groups of filters respectively perform modeling on the movement of the mobile robot in the state of a constant speed and in the state of uniform acceleration, the first group of filters take the position error and the speed error of the mobile robot in the two directions as state variables of the system; the second group of filters take the position error, the speed error and the acceleration error of the mobile robot in the two directions as state variables of the system; the two groups of filters take the position error, obtained through measuring resolving of the INS and WSN, of the mobile robot in the two directions as the observed quantity. According to the method, through fusion of smoothing results of the two groups of filters, the accuracy in the navigation parameter of the mobile robot is obviously improved.
Description
Technical field
The present invention relates to a kind of moving machine using Interactive Multiple-Model (interacting multiple model, imm)
Device people's wsn/ins Combinated navigation method, belongs to Fusion field.
Background technology
Global positioning system (global positioning systems, gps) and inertial navigation system (inertial
Navigation system, ins) it is one of most widely used navigation system at present.Wherein gps can provide accurately,
There is the navigation information of continual and steady navigation accuracy, but indoors, the intensive urban district of skyscraper, mine, the environment such as tunnel
Under, gps signal losing lock is it is impossible to be positioned.Although ins have entirely autonomous, movable information comprehensively, in short-term, high-precision advantage,
Although independent navigation can be realized, error accumulates in time, will lead to navigation accuracy degradation under the conditions of running during long boat.
Therefore, ins can only be short-term compensatory to the compensation of gps navigation information, and presently the most conventional gps/ins integrated navigation system
Navigation accuracy depend on the navigation accuracy of gps, in the case of the long-time losing lock of gps, integrated navigation system cannot provide height
The navigation information of precision.
In recent years, wireless sensor network (wireless sensors network, wsn) is with its low cost, low-power consumption
Show very big potentiality with the feature of low system complexity in short distance positioning field.Wsn is in no gps signal area, that is,
When so-called " blind area ", such as under the environment such as the intensive urban district of interior, skyscraper, mine, tunnel, unknown node positioning provides
May.But because the communication technology that wsn adopts is usually short-distance wireless communication technology (as zigbee, wifi etc.), if therefore
Want to complete the target following positioning of distance, need substantial amounts of network node jointly to complete, which increase the network burden of wsn.
In addition, wsn can only provide position and velocity information it is impossible to provide comprehensive movable information.
In order to obtain navigation information stable for a long time under gps long losing lock environment, many scholars propose to position wsn
Technology is incorporated in the ins system of low cost, builds wsn/ins integrated navigation system, such as Southeast China University y.xu although this group
Conjunction mode solves the problems, such as distance target following and navigator high cost under the closed environment of underground well, but by
In existing low cost ins technology so that information (such as course angle, the acceleration information) accuracy that obtains of ins systematic survey is big
Big reduction, along with the error accumulation phenomenon of ins system itself is so that inexpensive ins technology is difficult to provide stable navigation to believe
Breath.
Content of the invention
Goal of the invention: in order to solve the situation of the cumulative error that inexpensive ins occurs, the present invention proposes a kind of employing and hands over
Mutually the mobile robot wsn/ins Combinated navigation method of multi-model (imm), comes to indoor moving by using two kinds of motion models
The motion of robot is modeled, and merges the filter result of two groups of wave filter, to improve the precision of Mobile Robotics Navigation parameter.
Technical scheme: the present invention is to solve its technical problem to adopt the following technical scheme that
A kind of mobile robot wsn/ins Combinated navigation method of employing Interactive Multiple-Model, comprises the following steps:
(1) to mobile robot at the uniform velocity with even accelerate two kinds in the case of ins resolution error be modeled, and then obtain
Error model under two states, is designated as model 1 and model 2 respectively;
(2) ins and wsn measurement is resolved the mobile robot obtaining position in the two directions to make the difference, difference conduct
The observation vector of Data Fusion Filtering device is input in imm wave filter, and imm wave filter passes through model 1 and model 2 to mobile machine
The state of people is estimated respectively, and the navigational parameter error of the mobile robot that two kinds of error models are estimated is weighted melting
Close to obtain the optimal estimation of mobile robot navigational parameter error under current motion state, finally output current time moves
The optimum navigation calculation error of robot is estimated, and the navigational parameter that ins is resolved compensates, and obtains current time moving machine
The optimum navigational parameter of device people is estimated.
Wherein, model 1 is ins resolution error model in the case of at the uniform velocity for the mobile robot, and its state equation is:
In formula, δ xk、δykIt is respectively k moment mobile robot site error in the two directions, δ vx,k、δvy,kRespectively
For k moment mobile robot velocity error in the two directions, δ xk+1、δyk+1It is respectively k+1 moment mobile robot two
Site error on individual direction, δ vx,k+1、δvy,k+1It is respectively k+1 moment mobile robot velocity error in the two directions,
T is the filtering cycle of wave filter, w=[ω1ω2]tIt is zero-mean, the gaussian random sequence for q for the variance matrix, and suppose at two
Acceleration noise on coordinate direction is separate and has identical variance
The observational equation of model 1 is:
In formula,It is respectively k moment ins and resolve the mobile robot obtaining position in the two directions,It is respectively k moment wsn and resolve the mobile robot obtaining position in the two directions, v=[νx,kνy,k]t
It is the white noise of r for zero-mean, covariance matrix, and uncorrelated to w, It is respectively both direction
On observation noise variance;
Model 2 is ins resolution error model in the case of even acceleration for the mobile robot, and its state equation is:
In formula, δ ax,k、δay,kIt is respectively k moment mobile robot acceleration error in the two directions, δ ax,k+1、δ
ay,k+1It is respectively k+1 moment mobile robot acceleration error in the two directions;
The observational equation of model 2 is:
Coordinate system described in above-mentioned steps may be defined as: with the barycenter of mobile robot as initial point, to point to locality
East orientation direction is x-axis, to point to local north orientation direction as y-axis, to point to local sky to direction as z-axis.
Beneficial effects of the present invention are as follows:
1st, the positioning of low precision and orientation in ground urban transportation, long and narrow tunnel, small intelligent robot etc. can be met
Require.
2nd, in order to solve the situation of the cumulative error that inexpensive ins occurs, the present invention proposes one kind and adopts interactive multimode
The mobile robot wsn/ins Combinated navigation method of type, by the Kalman filter using two parallel computations come preferably
Solve the problems, such as parameter and model uncertainty.The present invention is modeled to the error of mobile robot using two kinds of models, with
Coupling error model under different motion state for the mobile robot, the navigation ginseng of the mobile robot that two models are estimated
Number error is weighted merging to obtain the optimal estimation of mobile robot navigational parameter under current motion state, and finally output is worked as
The optimum navigation calculation error of front moment mobile robot is estimated, and the navigational parameter that ins is resolved compensates, and obtains current
The optimum navigational parameter of moment mobile robot is estimated.The method is passed through to merge the filter result of two groups of wave filter, makes moving machine
The precision of device people's navigational parameter is significantly improved.
Brief description
Fig. 1 is that the system for the mobile robot wsn/ins Combinated navigation method using Interactive Multiple-Model (imm) is illustrated
Figure.
Fig. 2 is the schematic diagram for the mobile robot wsn/ins Combinated navigation method using Interactive Multiple-Model (imm).
Fig. 3 is the system flow for the mobile robot wsn/ins Combinated navigation method using Interactive Multiple-Model (imm)
Figure.
Specific embodiment
Below in conjunction with the accompanying drawings the invention is described in further details.
As shown in figure 1, a kind of system of the interactive multimode mode filter for wsn/ins integrated navigation, including wsn system
System, ins navigation module, central data processing module composition.Above-mentioned module is separately mounted to reference to (rn) node section and unknown
(bn) two parts of node.Wherein, reference mode part (includes supersonic sounding mould by the wireless network receiver module of wsn system
Block and time synchronized module) composition;The wireless network main control module of wsn system, ins navigation module and central data processing module
It is commonly mounted in unknown node.
As shown in Fig. 2 ins and wsn measurement is resolved the mobile robot obtaining (east orientation and north in the two directions
To) position makes the difference, difference is input in imm wave filter as the observation vector of Data Fusion Filtering device, and imm wave filter passes through mould
Type 1 and model 2 are estimated respectively to the state of mobile robot, the Mobile Robotics Navigation that two kinds of error models are estimated
Parameter error is merged to obtain the optimal estimation of mobile robot navigational parameter error under current motion state.Finally defeated
The optimum navigation calculation error going out current time mobile robot is estimated, and the navigational parameter that ins is resolved compensates, and obtains
The optimum navigational parameter of current time mobile robot is estimated.
Wherein, model 1 is ins resolution error model in the case of at the uniform velocity for the mobile robot, and its state equation is:
In formula, δ xk、δykIt is respectively site error on east orientation and north orientation for the k moment mobile robot, δ vx,k、δvy,kPoint
Not Wei velocity error on east orientation and north orientation for the k moment mobile robot, t be wave filter filtering cycle, w=[ω1ω2]t
It is zero-mean, the gaussian random sequence for q for the variance matrix, and suppose that the acceleration noise on two coordinate directions is separate simultaneously
There is identical variance
The observational equation of model 1 is:
In formula,It is respectively k moment ins and resolve position on east orientation and north orientation for the mobile robot obtaining,It is respectively k moment wsn and resolve position on east orientation and north orientation for the mobile robot obtaining, v=[νx,k
νy,k]tIt is the white noise of r for zero-mean, covariance matrix, and uncorrelated to w;
Model 2 is ins resolution error model in the case of even acceleration for the mobile robot, and its state equation is:
In formula, δ ax,k、δay,kIt is respectively acceleration error on east orientation and north orientation for the k moment mobile robot;
The observational equation of model 2 is:
A kind of working-flow of the interactive multimode mode filter for wsn/ins integrated navigation is as shown in Figure 3: is moving
During mobile robot motion, on the one hand by self-contained inertial sensor, the angular velocity recording and acceleration information enter
Row attitude, speed and location updating;On the other hand, mobile robot passes through self-contained ultrasonic sensor with the fixed cycle
T measures the distance of itself and ultrasonic nodal point known to surrounding, is resolved using least-squares algorithm and obtains the current of mobile robot
Position;Ins and wsn measurement is resolved the mobile robot obtaining (east orientation and north orientation) position in the two directions make the difference, poor
Value is input in imm wave filter as the observation vector of Data Fusion Filtering device, and imm wave filter output current time moves machine
The optimum navigation calculation error of people is estimated, and the navigational parameter that ins is resolved compensates, and obtains current time mobile robot
Optimum navigational parameter estimate.
Claims (3)
1. a kind of mobile robot wsn/ins Combinated navigation method of employing Interactive Multiple-Model imm, is characterized in that including following step
Rapid:
(1) to mobile robot at the uniform velocity with even accelerate two kinds in the case of ins resolution error be modeled, and then obtain two kinds
Error model under state, is designated as model 1 and model 2 respectively;
(2) ins and wsn measurement is resolved position on east orientation and north orientation both direction for the mobile robot obtaining to make the difference, poor
Value is input in imm wave filter as the observation vector of Data Fusion Filtering device, and imm wave filter passes through model 1 and model 2 to shifting
The state of mobile robot is estimated respectively, and the navigational parameter error of the mobile robot that two kinds of error models are estimated is carried out
Weighted Fusion to obtain the optimal estimation of mobile robot navigational parameter error under current motion state, when finally exporting current
The optimum navigation calculation error carving mobile robot is estimated, and the navigational parameter that ins is resolved compensates, and obtains current time
The optimum navigational parameter of mobile robot is estimated.
2. the mobile robot wsn/ins Combinated navigation method of employing Interactive Multiple-Model according to claim 1, its feature
It is that described model 1 is ins resolution error model in the case of at the uniform velocity for the mobile robot, and its state equation is:
In formula 1, δ xk、δykIt is respectively k moment mobile robot site error in the two directions, δ vx,k、δvy,kIt is respectively k
Moment mobile robot velocity error in the two directions, δ xk+1、δyk+1It is respectively k+1 moment mobile robot two sides
Site error upwards, δ vx,k+1、δvy,k+1It is respectively k+1 moment mobile robot velocity error in the two directions, t is
The filtering cycle of wave filter, w=[ω1ω2]tIt is zero-mean, the gaussian random sequence for q for the variance matrix, and suppose two sides
Acceleration noise upwards is separate and has identical variance
The observational equation of model 1 is:
In formula 2,It is respectively k moment ins and resolve the mobile robot obtaining position in the two directions,It is respectively k moment wsn and resolve the mobile robot obtaining position in the two directions, v=[νx,kνy,k]t
It is the white noise of r for zero-mean, covariance matrix, and uncorrelated to w, It is respectively both direction
On observation noise variance.
3. the mobile robot wsn/ins Combinated navigation method of employing Interactive Multiple-Model according to claim 1, its feature
It is that described model 2 is ins resolution error model in the case of even acceleration for the mobile robot, and its state equation is:
In formula 3, δ xk、δykIt is respectively k moment mobile robot site error in the two directions, δ vx,k、δvy,kIt is respectively k
Moment mobile robot velocity error in the two directions, δ ax,k、δay,kIt is respectively k moment mobile robot in both direction
On acceleration error, δ xk+1、δyk+1It is respectively k+1 moment mobile robot site error in the two directions, δ vx,k+1、δ
vy,k+1It is respectively k+1 moment mobile robot velocity error in the two directions, δ ax,k+1、δay,k+1The respectively k+1 moment moves
Mobile robot acceleration error in the two directions, t is the filtering cycle of wave filter, w=[ω1ω2]tIt is zero-mean, side
Difference battle array is the gaussian random sequence of q, and supposition acceleration noise in the two directions is separate and has identical variance
The observational equation of model 2 is:
In formula 4,It is respectively k moment ins and resolve the mobile robot obtaining position in the two directions,It is respectively k moment wsn and resolve the mobile robot obtaining position in the two directions, v=[νx,kνy,k]t
It is the white noise of r for zero-mean, covariance matrix, and uncorrelated to w, It is respectively both direction
On observation noise variance.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201410614772.4A CN104316058B (en) | 2014-11-04 | 2014-11-04 | Interacting multiple model adopted WSN-INS combined navigation method for mobile robot |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201410614772.4A CN104316058B (en) | 2014-11-04 | 2014-11-04 | Interacting multiple model adopted WSN-INS combined navigation method for mobile robot |
Publications (2)
Publication Number | Publication Date |
---|---|
CN104316058A CN104316058A (en) | 2015-01-28 |
CN104316058B true CN104316058B (en) | 2017-01-18 |
Family
ID=52371322
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201410614772.4A Active CN104316058B (en) | 2014-11-04 | 2014-11-04 | Interacting multiple model adopted WSN-INS combined navigation method for mobile robot |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN104316058B (en) |
Families Citing this family (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104597216B (en) * | 2015-02-15 | 2016-03-02 | 东南大学 | For the removable objectionable impurities detection and location system and method for indoor and outdoors |
CN106886788B (en) * | 2015-12-11 | 2020-02-18 | 上海交通大学 | Single-target simulation track tracking difficulty detection method based on multi-index decision |
CN107966142A (en) * | 2017-11-14 | 2018-04-27 | 济南大学 | A kind of adaptive UFIR data fusion methods of indoor pedestrian based on multiwindow |
WO2019218291A1 (en) * | 2018-05-17 | 2019-11-21 | 罗伯特·博世有限公司 | Method and device used for filtering positioning data |
CN109655060B (en) * | 2019-02-19 | 2021-07-06 | 济南大学 | INS/UWB integrated navigation algorithm and system based on KF/FIR and LS-SVM fusion |
CN110849369B (en) * | 2019-10-29 | 2022-03-29 | 苏宁云计算有限公司 | Robot tracking method, device, equipment and computer readable storage medium |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102494684A (en) * | 2011-11-11 | 2012-06-13 | 东南大学 | Navigation information zero tight combination method based on WSN (Wireless Sensors Network)/MINS (Micro Inertial Navigation System) |
CN102494685A (en) * | 2011-11-14 | 2012-06-13 | 东南大学 | H infinity information fusion method for WSN (Wireless Sensors Network)/INS (Inertial Navigation System) high-precision real-time combination and navigation |
CN102636166A (en) * | 2012-05-02 | 2012-08-15 | 东南大学 | Course angle-based WSN/INS integrated navigation system and method |
CN102692223A (en) * | 2012-06-27 | 2012-09-26 | 东南大学 | Control method of multilevel non-linear filters for wireless sensor network (WSN)/inertial navigation system (INS) combination navigation |
CN103148855A (en) * | 2013-02-27 | 2013-06-12 | 东南大学 | INS (inertial navigation system)-assisted wireless indoor mobile robot positioning method |
-
2014
- 2014-11-04 CN CN201410614772.4A patent/CN104316058B/en active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102494684A (en) * | 2011-11-11 | 2012-06-13 | 东南大学 | Navigation information zero tight combination method based on WSN (Wireless Sensors Network)/MINS (Micro Inertial Navigation System) |
CN102494685A (en) * | 2011-11-14 | 2012-06-13 | 东南大学 | H infinity information fusion method for WSN (Wireless Sensors Network)/INS (Inertial Navigation System) high-precision real-time combination and navigation |
CN102636166A (en) * | 2012-05-02 | 2012-08-15 | 东南大学 | Course angle-based WSN/INS integrated navigation system and method |
CN102692223A (en) * | 2012-06-27 | 2012-09-26 | 东南大学 | Control method of multilevel non-linear filters for wireless sensor network (WSN)/inertial navigation system (INS) combination navigation |
CN103148855A (en) * | 2013-02-27 | 2013-06-12 | 东南大学 | INS (inertial navigation system)-assisted wireless indoor mobile robot positioning method |
Non-Patent Citations (1)
Title |
---|
基于无线传感器网络的移动目标导航方法;梁华为;《传感技术学报》;20070731;第20卷(第7期);1620-1624 * |
Also Published As
Publication number | Publication date |
---|---|
CN104316058A (en) | 2015-01-28 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN104316058B (en) | Interacting multiple model adopted WSN-INS combined navigation method for mobile robot | |
CN105628026B (en) | A kind of positioning and orientation method and system of mobile object | |
CN105509738B (en) | Vehicle positioning orientation method based on inertial navigation/Doppler radar combination | |
Georgy et al. | Modeling the stochastic drift of a MEMS-based gyroscope in gyro/odometer/GPS integrated navigation | |
Fan et al. | Data fusion for indoor mobile robot positioning based on tightly coupled INS/UWB | |
CN102692223B (en) | Control method of multilevel non-linear filters for wireless sensor network (WSN)/inertial navigation system (INS) combination navigation | |
Xiong et al. | G-VIDO: A vehicle dynamics and intermittent GNSS-aided visual-inertial state estimator for autonomous driving | |
CN102494684B (en) | Navigation information zero tight combination method based on WSN (Wireless Sensors Network)/MINS (Micro Inertial Navigation System) | |
CN110702091B (en) | High-precision positioning method for moving robot along subway rail | |
CN103776446B (en) | A kind of pedestrian's independent navigation computation based on double MEMS-IMU | |
CN105509739A (en) | Tightly coupled INS/UWB integrated navigation system and method adopting fixed-interval CRTS smoothing | |
CN102636166B (en) | Course angle-based WSN/INS integrated navigation system and method | |
CN105928518A (en) | Indoor pedestrian UWB/INS tightly combined navigation system and method adopting pseudo range and position information | |
CN103822633A (en) | Low-cost attitude estimation method based on second-order measurement update | |
Jung et al. | Monocular visual-inertial-wheel odometry using low-grade IMU in urban areas | |
CN106052684A (en) | Mobile robot IMU/UWB/code disc loose combination navigation system and method adopting multi-mode description | |
CN108362288A (en) | Polarized light S L AM method based on unscented Kalman filtering | |
CN104251702A (en) | Pedestrian navigation method based on relative pose measurement | |
CN108387236A (en) | Polarized light S L AM method based on extended Kalman filtering | |
Tang et al. | OdoNet: Untethered speed aiding for vehicle navigation without hardware wheeled odometer | |
CN103968844A (en) | Large ellipse maneuverable spacecraft autonomous navigation method based on low-orbit platform tracking measurement | |
Zhou et al. | DeepVIP: Deep learning-based vehicle indoor positioning using smartphones | |
CN104374389B (en) | A kind of IMU/WSN Combinated navigation methods towards indoor mobile robot | |
Wang et al. | Pedestrian motion tracking by using inertial sensors on the smartphone | |
CN109387198A (en) | A kind of inertia based on sequential detection/visual odometry Combinated navigation method |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
C14 | Grant of patent or utility model | ||
GR01 | Patent grant |