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 PDF

Info

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
Application number
CN201410614772.4A
Other languages
Chinese (zh)
Other versions
CN104316058A (en
Inventor
陈熙源
唐建
徐元
方琳
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Southeast University
Original Assignee
Southeast University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Southeast University filed Critical Southeast University
Priority to CN201410614772.4A priority Critical patent/CN104316058B/en
Publication of CN104316058A publication Critical patent/CN104316058A/en
Application granted granted Critical
Publication of CN104316058B publication Critical patent/CN104316058B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20Instruments for performing navigational calculations
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12Navigation; 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/16Navigation; 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/165Navigation; 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

A kind of mobile robot wsn/ins Combinated navigation method of employing Interactive Multiple-Model
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 q = σ a 2 i , i = 1 0 0 1 ;
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, r = σ x 2 0 0 σ y 2 , 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.
CN201410614772.4A 2014-11-04 2014-11-04 Interacting multiple model adopted WSN-INS combined navigation method for mobile robot Active CN104316058B (en)

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)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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

Patent Citations (5)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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