CN107860388B - Multi-robot collaborative navigation positioning algorithm based on hybrid topological structure - Google Patents
Multi-robot collaborative navigation positioning algorithm based on hybrid topological structure Download PDFInfo
- Publication number
- CN107860388B CN107860388B CN201711026836.9A CN201711026836A CN107860388B CN 107860388 B CN107860388 B CN 107860388B CN 201711026836 A CN201711026836 A CN 201711026836A CN 107860388 B CN107860388 B CN 107860388B
- Authority
- CN
- China
- Prior art keywords
- robot
- observability
- mobile
- mobile robot
- topological structure
- 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
Images
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
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
Abstract
A multi-robot collaborative navigation positioning algorithm based on a hybrid topological structure is characterized by comprising the following steps: the method comprises the steps of building an MMRS working environment, and measuring the position of a fixed road marking point, the initial position of a mobile robot and attitude information in the working environment; establishing an MMRS collaborative navigation nonlinear system equation comprising a state equation and an observation equation; the mobile robot starts to work, and time updating is carried out on the MMRS collaborative navigation system according to the EKF filtering frame; the mobile robot respectively observes the fixed road sign points and other robots in the working environment in real time to obtain observation information; carrying out measurement updating on the system by utilizing the hybrid topological structure; and updating the pose information of the mobile robot in the MMRS to finish the MMRS high-precision collaborative navigation positioning process. The method can realize high-efficiency utilization of observation information by utilizing the hybrid topological structure, and can simultaneously solve the problems of nonlinearity and uncertainty in the MMRS cooperative navigation, thereby improving the cooperative navigation positioning precision of the MMRS.
Description
Technical Field
The invention relates to the field of robots, in particular to a hybrid topology structure-based multi-robot collaborative navigation positioning algorithm.
Background
Since the first mobile robot came out in 1960, the development of mobile robots has received much attention. The mobile robot has the advantages of autonomy, independence, small size and the like, and is widely applied to various fields such as indoor, outdoor, space, water surface, underwater and the like. With the improvement of task complexity, a single robot cannot meet the requirements more and more, and multiple robots can complete various complex tasks, improve efficiency, have higher precision and better redundancy, so that a multi-mobile robot system gradually becomes a research hotspot in the robot field.
The collaborative navigation of the multi-mobile robot system is the premise and guarantee that multiple robots can complete tasks smoothly and efficiently, and the essence of the collaborative navigation is filtering estimation. Therefore, the observability of the multi-mobile-robot system is analyzed, and the research on the high-precision collaborative navigation positioning algorithm is developed on the basis, so that the method has important theoretical and practical significance.
Currently, multi-mobile robotic systems are generally approximated as linear systems when performing observability analysis of the system, but non-linearities exist in the actual system, especially in robot motion situations. Approximating a non-linear system to a linear system would reduce the accuracy of the filtering algorithm and even cause the filtering to diverge. In the aspect of observability analysis of a nonlinear system, the most common method is a Relative Position Measurement map (RPMG) observability analysis method based on a lie derivative, the method adopts an Extended Kalman Filter (EKF) to solve the nonlinear problem of the system, and the RPMG method is used to analyze the observability of the system. However, in an actual multi-mobile robot system, due to the movement of the robots, mutual shielding among the robots, and measurement limitations of the mounted sensors, the system has a large uncertainty, and the observability matrix in the RPMG method changes in real time, so that if the method is still used, a large error is introduced, and the navigation positioning accuracy of the multi-mobile robot system is reduced.
Disclosure of Invention
The invention aims to provide a multi-robot collaborative navigation positioning algorithm based on a mixed topological structure, which can simultaneously solve the problems of nonlinearity and uncertainty in a multi-mobile robot system, has higher utilization rate of observation information among mobile robots and better navigation positioning precision.
The purpose of the invention is realized by the following steps:
step 1: building a multi-mobile robot system working environment, and measuring the position of a fixed landmark point, the initial position and the attitude information of a mobile robot in the working environment;
step 2: establishing a multi-mobile-robot system collaborative navigation nonlinear system equation comprising a state equation and an observation equation;
and step 3: the mobile robot starts to work, and time updating is carried out on the whole multi-mobile-robot system collaborative navigation system according to the EKF filtering frame;
and 4, step 4: the mobile robot respectively observes the fixed road sign points and other robots in the working environment in real time to obtain observation information;
and 5: carrying out measurement updating on the system by utilizing the hybrid topological structure;
step 6: updating the pose information of the mobile robots in the multi-mobile robot system,
and 7: and (6) repeating the step 3 to the step 6 to finish the high-precision collaborative navigation positioning of the multi-mobile robot system.
Further, the updating of the system measurement using the hybrid topology includes the following sub-steps: firstly, analyzing the observability of a multi-mobile-robot system by using an RPMG observability analysis method based on a plum derivative to obtain a subsystem with complete observability; and then transmitting the observability by using the mixed topological structure to obtain a new sub-topological structure with complete observability, and finally measuring and updating the sub-topological structure.
The invention has the following beneficial effects:
the invention provides a hybrid topology structure-based collaborative navigation positioning algorithm for a multi-mobile robot system, which is characterized in that the observability analysis of the system is completed by using a PRMG under an EKF framework, on the basis, the hybrid topology structure is used for completing the high-efficiency utilization of observation information, and the problems of nonlinearity and uncertainty in the collaborative navigation of the multi-mobile robot system can be simultaneously solved, so that the collaborative navigation positioning accuracy of the multi-mobile robot system is improved. Because the EKF is used, the problem of nonlinearity of the multi-mobile-robot system can be effectively solved, and the positioning precision of the system is improved; the hybrid topological structure is utilized to transmit observability, an observable sub-topological structure is constructed, the information utilization rate of the observed quantity can be effectively improved, and the accuracy of collaborative navigation positioning is further improved.
Drawings
FIG. 1 is a flow chart of the present invention;
FIG. 2 is a plot of the positioning error of the robot 1 using the algorithm of the present invention;
fig. 3 is a plot of the positioning error of the robot 1 using a conventional algorithm;
FIG. 4 is a plot of the positioning error of the robot 2 using the algorithm of the present invention;
FIG. 5 is a plot of the positioning error of the robot 2 using a conventional algorithm;
FIG. 6 is a plot of the positioning error of the robot 3 using the algorithm of the present invention;
fig. 7 is a plot of the positioning error of the robot 3 using a conventional algorithm;
FIG. 8 is a plot of the positioning error of the robot 4 using the algorithm of the present invention;
fig. 9 is a plot of the positioning error of the robot 4 using a conventional algorithm;
FIG. 10 is a plot of the positioning error of robot 5 using the algorithm of the present invention;
fig. 11 is a positioning error curve of the robot 5 using a conventional algorithm.
Detailed Description
The present invention will be described in detail with reference to specific embodiments.
The invention relates to a hybrid topology structure-based multi-robot collaborative navigation positioning algorithm, which is combined with an algorithm flow block diagram shown in figure 1, and the specific implementation mode is as follows:
step 1: firstly, according to the working tasks of the multi-mobile robot system, a reasonable working environment is established, fixed road marking points in the surrounding environment of the multi-mobile robot system are determined, and information such as the positions of the fixed road marking points in the working environment and the initial positions and postures of the mobile robots are measured;
step 2: constructing a state vector comprising positions and postures of multiple robots:
X＝[X1X2… Xn]T
wherein Xi＝[xi,yi,θi]TThe pose of the ith robot is expressed according to a robot kinematic equation as follows:
and step 3: constructing a measurement vector Z ═ Z including distance and azimuth anglesRRZRL]TInvolving mutual observation Z between the robot and the robotRRAnd observation Z between the robot and the fixed waypointsRLThen Z isRRAnd ZRLCan be expressed as:
wherein [ x ]i,yi,θi]TAnd [ x ]j,yj,θj]TIs the pose of the ith robot and the jth robot, (x)l,yl) Is the location of the landmark point l.
On the basis, nonlinear and uncertain factors of collaborative navigation of the multi-mobile robot system are comprehensively considered, and a nonlinear system equation of the multi-mobile robot system is established according to the dynamic principle of the mobile robot:
wherein f (-) is the nonlinear state transition equation of the system, w (k) is the noise matrix of the system, w (k) -N (0, Q (k)); h (-) is the nonlinear observation equation of the system, η (k) is the observation noise matrix of the system, and has η (k) -N (0, R (k)).
And 4, step 4: each mobile robot starts to work, and the state of each robot in the whole collaborative navigation system is updated in time according to the EKF filtering frame;
calculating Jacobian matrixes of a state transition matrix and an observation matrix according to a system equation, wherein the Jacobian matrixes are F (k | k +1) and H (k +1), and predicting a system state and a state variance matrix in one step according to an EKF filtering step, and the Jacobian matrixes are X (k | k +1) and P (k | k + 1):
and 5: each mobile robot respectively observes a fixed road sign point and other robots in the working environment in real time to obtain observation information Z (k + 1);
step 6: the observability of the system is analyzed by using the observability measurement information to find a completely observable subsystem, the observability of the subsystem is transmitted by using the mixed topology structure to determine a completely observable sub-topology structure, and the sub-topology structure is measured and updated by using the mixed topology structure on the basis of analyzing the observability of the system:
and 7: and updating the pose information of each robot in the multi-mobile robot system to complete the high-precision collaborative navigation and positioning process of the multi-mobile robot system.
The effect of the invention is verified by the following method:
the efficacy of the invention was verified using actual data from Autonomous Space Robotics Lab, university of toronto. The test is carried out in an indoor environment, 5 mobile robots are totally arranged, each mobile robot is marked with an identity identification bar code, a monocular vision camera is carried for distance measurement and azimuth angle measurement, and the robots have a basic obstacle avoidance function; furthermore, the indoor environment is marked with 15 fixed waypoints of known location. After the test is started, 5 robots move autonomously to perform collaborative navigation. The test data is analyzed by using the invention, the traditional cooperative navigation positioning algorithm based on RPMG and EKF is used as a comparison algorithm, and the test results are respectively shown in fig. 2-11.
As can be seen from fig. 2 to 11, by using the method of the present invention, the positions of 5 mobile robots can be effectively estimated, and the estimation curves are basically within ± 3 σ; compared with the traditional multi-mobile-robot co-location algorithm, the method has higher estimation precision and smaller +/-3 sigma interval. In conclusion, the method provided by the invention has more accurate estimation precision, and can effectively improve the navigation and positioning capability of a multi-mobile-robot system.
Claims (2)
1. A multi-robot collaborative navigation positioning algorithm based on a hybrid topological structure is characterized by comprising the following steps:
step 1: building a multi-mobile robot system working environment, and measuring the position of a fixed landmark point, the initial position and the attitude information of a mobile robot in the working environment;
step 2: establishing a multi-mobile-robot system collaborative navigation nonlinear system equation comprising a state equation and an observation equation;
and step 3: the mobile robot starts to work, and time updating is carried out on the multi-mobile-robot system collaborative navigation system according to the EKF filtering frame;
and 4, step 4: the mobile robot respectively observes the fixed road sign points and other robots in the working environment in real time to obtain observation information;
and 5: carrying out measurement updating on the system by utilizing the hybrid topological structure;
step 6: updating the pose information of the mobile robots in the multi-mobile robot system;
and 7: repeating the step 3 to the step 6 to complete the high-precision collaborative navigation positioning of the multi-mobile robot system;
the measurement updating of the system by using the hybrid topology structure specifically comprises the following steps: the observability of the system is analyzed by using the observability measurement information to find a completely observable subsystem, the observability of the subsystem is transmitted by using the mixed topology structure to determine a completely observable sub-topology structure, and the sub-topology structure is measured and updated by using the mixed topology structure on the basis of analyzing the observability of the system:
2. the hybrid topology based multi-robot collaborative navigation positioning algorithm according to claim 1, wherein the step 5 of updating system measurements using the hybrid topology comprises the following sub-steps: firstly, analyzing the observability of a multi-mobile-robot system by using an RPMG observability analysis method based on a plum derivative to obtain a subsystem with complete observability; and then transmitting the observability by using the mixed topological structure to obtain a new sub-topological structure with complete observability, and finally measuring and updating the new sub-topological structure.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711026836.9A CN107860388B (en) | 2017-10-27 | 2017-10-27 | Multi-robot collaborative navigation positioning algorithm based on hybrid topological structure |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711026836.9A CN107860388B (en) | 2017-10-27 | 2017-10-27 | Multi-robot collaborative navigation positioning algorithm based on hybrid topological structure |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN107860388A CN107860388A (en) | 2018-03-30 |
| CN107860388B true CN107860388B (en) | 2020-10-27 |
Family
ID=61697400
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201711026836.9A Active CN107860388B (en) | 2017-10-27 | 2017-10-27 | Multi-robot collaborative navigation positioning algorithm based on hybrid topological structure |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107860388B (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108664024B (en) * | 2018-05-08 | 2020-09-25 | 清华大学 | Motion planning and cooperative positioning method and device for unmanned vehicle network formation |
| CN111928851B (en) * | 2020-04-17 | 2023-12-12 | 中国科学院沈阳自动化研究所 | TMA technology-based multi-autonomous underwater robot cluster collaborative navigation method |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101661098A (en) * | 2009-09-10 | 2010-03-03 | 上海交通大学 | Multi-robot automatic locating system for robot restaurant |
| CN101998503A (en) * | 2009-08-12 | 2011-03-30 | 中国科学院沈阳自动化研究所 | Mixed topological structured wireless sensor network-oriented two-grade packet aggregation method |
| CN104330083A (en) * | 2014-10-27 | 2015-02-04 | 南京理工大学 | Multi-robot cooperative positioning algorithm based on square root unscented kalman filter |
| CN106482736A (en) * | 2016-07-11 | 2017-03-08 | 安徽工程大学 | A kind of multirobot colocated algorithm based on square root volume Kalman filtering |
-
2017
- 2017-10-27 CN CN201711026836.9A patent/CN107860388B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101998503A (en) * | 2009-08-12 | 2011-03-30 | 中国科学院沈阳自动化研究所 | Mixed topological structured wireless sensor network-oriented two-grade packet aggregation method |
| CN101661098A (en) * | 2009-09-10 | 2010-03-03 | 上海交通大学 | Multi-robot automatic locating system for robot restaurant |
| CN104330083A (en) * | 2014-10-27 | 2015-02-04 | 南京理工大学 | Multi-robot cooperative positioning algorithm based on square root unscented kalman filter |
| CN106482736A (en) * | 2016-07-11 | 2017-03-08 | 安徽工程大学 | A kind of multirobot colocated algorithm based on square root volume Kalman filtering |
Non-Patent Citations (1)
| Title |
|---|
| Performance Analysis of Multirobot Cooperative Localization;Anastasios etal.;《IEEE TRANSACTIONS ON ROBOTICS》;20060807;第22卷(第4期);第666-681页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN107860388A (en) | 2018-03-30 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111968262B (en) | Semantic intelligent substation inspection operation robot navigation system and method | |
| CN109916393B (en) | Multi-grid-value navigation method based on robot pose and application thereof | |
| CN103412565B (en) | A kind of robot localization method with the quick estimated capacity of global position | |
| CN111982114B (en) | Rescue robot for estimating three-dimensional pose by adopting IMU data fusion | |
| CN104330083A (en) | Multi-robot cooperative positioning algorithm based on square root unscented kalman filter | |
| CN110749895B (en) | Laser radar point cloud data-based positioning method | |
| CN107860388B (en) | Multi-robot collaborative navigation positioning algorithm based on hybrid topological structure | |
| CN106323291A (en) | Multi-robot cooperative location algorithm based on square root cubature Kalman filtering | |
| Li et al. | An overview of the simultaneous localization and mapping on mobile robot | |
| CN113763549A (en) | Method, device and storage medium for simultaneous positioning and mapping by fusing laser radar and IMU | |
| Knuth et al. | Collaborative 3D localization of robots from relative pose measurements using gradient descent on manifolds | |
| CN107843259B (en) | Multi-robot collaborative navigation positioning method based on VCKF | |
| CN110967017A (en) | Cooperative positioning method for rigid body cooperative transportation of double mobile robots | |
| Jing et al. | A localization algorithm for mobile robots in RFID system | |
| Zhang et al. | Formations for localization of robot networks | |
| CN115847491A (en) | Space on-orbit maintenance robot target autonomous measurement method | |
| CN111121818A (en) | Calibration method for camera and two-dimensional code in unmanned vehicle | |
| Wang et al. | The SLAM algorithm of mobile robot with omnidirectional vision based on EKF | |
| Hu et al. | Three-dimensional mapping based on SIFT and RANSAC for mobile robot | |
| Lee et al. | Cooperative swarm localization and mapping with inter-agent ranging | |
| Zhang et al. | An improved scan matching algorithm in SLAM | |
| Housein et al. | Extended kalman filter sensor fusion in practice for mobile robot localization | |
| Raufelder | Modeling Analysis of Attitude Perception of Engineering Manipulator Supporting Wireless Communication and Internet of Things | |
| Ai et al. | Design of an indoor surveying and mapping robot based on SLAM technology | |
| Alghuraid et al. | Development of ultra-wide-band indoor positioning system designed to control the position of a grid confined robotic platform |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |