CN117824627A - Differential robot geomagnetic auxiliary positioning system based on particle filtering - Google Patents
Differential robot geomagnetic auxiliary positioning system based on particle filtering Download PDFInfo
- Publication number
- CN117824627A CN117824627A CN202311867591.8A CN202311867591A CN117824627A CN 117824627 A CN117824627 A CN 117824627A CN 202311867591 A CN202311867591 A CN 202311867591A CN 117824627 A CN117824627 A CN 117824627A
- Authority
- CN
- China
- Prior art keywords
- positioning
- geomagnetic
- particle
- robot
- data
- 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.)
- Granted
Links
- 239000002245 particle Substances 0.000 title claims abstract description 107
- 238000001914 filtration Methods 0.000 title claims abstract description 26
- 230000005358 geomagnetic field Effects 0.000 claims abstract description 39
- 238000006073 displacement reaction Methods 0.000 claims abstract description 22
- 238000012544 monitoring process Methods 0.000 claims abstract description 4
- 238000012937 correction Methods 0.000 claims description 29
- 238000000034 method Methods 0.000 claims description 18
- 230000005540 biological transmission Effects 0.000 claims description 5
- 238000012952 Resampling Methods 0.000 claims description 4
- 238000004364 calculation method Methods 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 5
- 238000005259 measurement Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000005070 sampling Methods 0.000 description 2
- 238000009825 accumulation Methods 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000012886 linear function Methods 0.000 description 1
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/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
- G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Life Sciences & Earth Sciences (AREA)
- Environmental & Geological Engineering (AREA)
- General Life Sciences & Earth Sciences (AREA)
- Geology (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention provides a particle filtering-based geomagnetic auxiliary positioning system of a differential robot, relates to the field of autonomous navigation of intelligent robots, and aims to solve the problems that when the existing geomagnetic/odometer positioning system based on particle filtering works continuously, mismatching and even filtering divergence can be caused due to overlarge geomagnetic data noise. Comprising the following steps: the geomagnetic field acquisition module is used for acquiring geomagnetic field intensity data and sending the acquired data to the positioning module; the mileage acquisition module is used for acquiring the data of the angle and displacement rotated by the driving wheel of the robot and transmitting the data to the positioning module; the positioning module is used for resolving geomagnetic field intensity data and angle and displacement data rotated by the driving wheel of the robot so as to perform real-time positioning, and storing a positioning result in the storage module and sending the positioning result to the upper computer; the storage module is used for storing geomagnetic field intensity data and positioning result data and sending a geomagnetic reference picture to the positioning module; the upper computer is used for monitoring the positioning result in real time.
Description
Technical Field
The invention relates to the technical field of autonomous navigation of intelligent robots, in particular to a differential robot geomagnetic auxiliary positioning system based on particle filtering.
Background
Mobile robots are currently used in various fields of social development, such as factories, hospitals, families, exhibition halls, etc. The robot senses the surrounding environment and the state of the robot through the sensor, and in the environment with the obstacle, the robot autonomously plans the motion track according to a certain behavior constraint condition, so that collision-free autonomous motion from the initial position to the target position is realized.
The method for quickly and accurately realizing global positioning has a particularly important meaning for improving the autonomy and the flexibility of the mobile robot. Wheel odometers are a method for measuring the distance and direction traveled by a moving vehicle, using encoders to measure the angle of rotation of the wheels, and combining geometric parameters of the vehicle to calculate the displacement and direction change of the vehicle. By accumulating the rotation angle of each wheel, the overall displacement and direction of the vehicle can be obtained. Wheel odometers are a low-cost, real-time positioning method, however, wheel odometers suffer from error accumulation, such as wheel slip, deformation, and uneven ground, which can affect the measurement results. Therefore, in practical applications, wheel odometers often need to be fused with other sensors to improve positioning accuracy and robustness.
Geomagnetic aided navigation is a technology for navigation by using the information of the earth magnetic field, and the principle is that the positioning and navigation of a robot or a vehicle are realized by identifying and matching geomagnetic field characteristics of different areas. Compared with other positioning navigation technologies, the geomagnetic aided navigation has the advantages of low cost, full area coverage, strong anti-interference capability, no accumulated error and the like. According to geomagnetic and odometer data, the robot is positioned based on particle filtering, so that the robot has a good application prospect, but when the robot continuously works, the problem that mismatching and even filtering divergence can be caused due to overlarge geomagnetic data noise exists, and the final positioning result is not ideal.
Disclosure of Invention
The invention aims to solve the technical problems that:
when the existing geomagnetic/odometer positioning system based on particle filtering continuously works, the problem of mismatching and even filtering divergence can be caused due to overlarge geomagnetic data noise, so that the positioning result is not ideal.
The invention adopts the technical scheme for solving the technical problems:
the invention provides a differential robot geomagnetic auxiliary positioning system based on particle filtering, which comprises: the system comprises a geomagnetic field acquisition module, a mileage acquisition module, a positioning module, a storage module and an upper computer;
the geomagnetic field acquisition module is used for acquiring geomagnetic field intensity data and sending the acquired data to the positioning module;
the mileage acquisition module is used for acquiring the data of the angle and displacement rotated by the driving wheel of the robot and transmitting the acquired data to the positioning module;
the positioning module is used for resolving geomagnetic field intensity data and angle and displacement data rotated by the driving wheel of the robot so as to perform real-time positioning, and storing a positioning result in the storage module and sending the positioning result to the upper computer;
the storage module is used for storing geomagnetic field intensity data and positioning result data, forming a geomagnetic reference map based on the geomagnetic field intensity data and sending the geomagnetic reference map to the positioning module;
the upper computer is used for monitoring the positioning result in real time.
Further, the geomagnetic field acquisition module comprises a triaxial magnetometer which is installed on the robot.
Further, the mileage acquisition module comprises two wheel type mileage meters which are respectively arranged on the left and right driving wheels of the robot.
Further, the positioning system also comprises a data transmission module for transmitting the positioning result of the positioning module to the upper computer.
Further, the positioning module is configured with a preliminary positioning model and a positioning error correction model, the preliminary positioning model is based on a particle filtering algorithm, a pre-obtained starting point and a pre-obtained starting point yaw angle theta are used as input, a particle set p and a particle weight set are initialized, acquired geomagnetic field intensity values f, angle offsets a and displacements s are input, N particles are randomly generated, a distribution area of the particles is in a limited range of a sector area, a particle corresponding position is calculated by a robot motion model, a particle weight is calculated, a preliminary positioning result is obtained, the positioning error correction model is based on an MAGCOM algorithm, a matching range L, a starting correction distance Dstart and a matching sequence length pmax are used as input, and a matching result is obtained according to the magnetic field intensity of all points on a track p_scanlist and the mean square error of the magnetic intensity acquired by a geomagnetic field acquisition module of the corresponding point; and (3) fusing the matching results of the first step and the second step to realize the correction of the error of the preliminary positioning result.
Further, the function implementation process of the robot motion model is as follows:
where xi and yi are the abscissas of the corresponding positions of the ith particle pi and x0 and y0 are the abscissas of the starting points.
Further, the function implementation process of the preliminary positioning model is as follows:
initializing a particle set p and a particle weight set aiming at a starting point and a starting point yaw angle, adding random numbers on the basis of a and s to randomly generate N particles aiming at a geomagnetic field strength value f, an angle offset a and a displacement s, wherein the angle offset and the displacement corresponding to each particle pi are ai and si respectively, and the distribution area of the particles is within a fan-shaped area limiting range; calculating the corresponding positions of particles by using a robot motion model, reversely reading a geomagnetic reference graph through the positions of the particles, obtaining a geomagnetic intensity value fi corresponding to each particle, calculating the position weights wi of each particle through ai, si and fi, resampling the particles, and calculating the particle weights to obtain a preliminary positioning result.
Further, the range of the sector area is: in the polar coordinate system, the distance range is (s- (k4×s+l2), s+ (k3×s+l1)), and the angle range is (s- (k1×a+Φ1), s+ (k2×a+Φ2)); wherein, k1, k2, k3, k4, l1, l2, phi 1 and phi 2 are constants and are adjusted according to the actual application scene.
Further, the method for calculating the particle weight wi is as follows:
wherein λ1, λ2, and λ3 are constants.
Further, the implementation process of the positioning error correction model is as follows:
aiming at a matching range L, a starting correction distance Dstart and a matching sequence length pmax; adding a positioning result obtained by the preliminary positioning model into a sequence plist, if the length of the plist is larger than pmax and the distance between the last correction point p_last and p_rst is larger than Dstart, taking a grid point p_rst' closest to the p_rst on a geomagnetic chart as a starting point, taking L grid points around the grid point p_rst as points p_scan to be searched, translating the track plist to each point to be searched to generate a track p_scan to be searched, and acquiring a corrected matching result according to the magnetic field intensity of all points on the track p_scan and the mean square error of the acquired magnetic intensity of a geomagnetic field acquisition module of the corresponding point, thereby realizing correction of error particle offset of the preliminary positioning result.
Compared with the prior art, the invention has the beneficial effects that:
a differential robot geomagnetic auxiliary positioning system based on particle filtering is characterized in that an odometer and geomagnetic data are fused, a particle distribution area is within a fan-shaped area limiting range through a particle filtering algorithm, and the problem of divergence caused by overlarge geomagnetic data noise when traditional particle filtering caused by system nonlinearity continuously works is solved. And the error correction model based on the MAGCOM algorithm is combined to correct the positioning result, so that the possibility of particle filtering divergence is further reduced, and the positioning precision is effectively improved.
The positioning system provided by the invention can effectively correct the accumulated error of the odometer so as to realize long-distance, low-cost and high-precision real-time positioning and navigation.
Drawings
Fig. 1 is a schematic structural diagram of a geomagnetic aided positioning system of a differential robot based on particle filtering in an embodiment of the present invention;
FIG. 2 is a schematic diagram of a conventional particle-filter particle distribution region according to an embodiment of the present invention;
FIG. 3 is a schematic view of a particle distribution confinement region in an embodiment of the invention;
FIG. 4 is a schematic diagram of odometer error in an embodiment of the invention;
FIG. 5 is a flow chart of particle filtering in an embodiment of the invention;
FIG. 6 is a flow chart of MAGCOM correction in an embodiment of the present invention;
FIG. 7 is a schematic diagram of the MAGCOM correction process in the embodiment of the present invention;
FIG. 8 is a functional block diagram of a positioning system in an embodiment of the invention;
FIG. 9 is a graph showing the alignment trace results of the methods according to the embodiments of the present invention.
Detailed Description
In the description of the present invention, it should be noted that the terms "first," "second," and "third" mentioned in the embodiments of the present invention are used for descriptive purposes only and are not to be construed as indicating or implying relative importance or implicitly indicating the number of technical features indicated. Thus, a feature defining "a first", "a second", or a third "may explicitly or implicitly include one or more such feature.
In order that the above objects, features and advantages of the invention will be readily understood, a more particular description of the invention will be rendered by reference to specific embodiments thereof which are illustrated in the appended drawings.
The specific embodiment I is as follows: as shown in fig. 1, the present invention provides a differential robot geomagnetic aided positioning system based on particle filtering, the positioning system comprising: the system comprises a geomagnetic field acquisition module, a mileage acquisition module, a positioning module, a storage module and an upper computer;
the geomagnetic field acquisition module is used for acquiring geomagnetic field intensity data and sending the acquired data to the positioning module;
the mileage acquisition module is used for acquiring the data of the angle and displacement rotated by the driving wheel of the robot and transmitting the acquired data to the positioning module;
the positioning module is used for resolving geomagnetic field intensity data and angle and displacement data rotated by the driving wheel of the robot so as to perform real-time positioning, and storing a positioning result in the storage module and sending the positioning result to the upper computer;
the storage module is used for storing geomagnetic field intensity data and positioning result data, forming a geomagnetic reference map based on the geomagnetic field intensity data and sending the geomagnetic reference map to the positioning module;
the upper computer is used for monitoring the positioning result in real time.
And a specific embodiment II: the geomagnetic field acquisition module comprises a triaxial magnetometer which is arranged on the robot. The other embodiments are the same as those of the first embodiment.
And a third specific embodiment: the mileage acquisition module comprises two wheel type mileage meters which are respectively arranged on the left and right driving wheels of the robot. This embodiment is otherwise identical to the second embodiment.
And a specific embodiment IV: the positioning system also comprises a data transmission module which is used for transmitting the positioning result of the positioning module to the upper computer. The other embodiments are the same as those of the first embodiment.
Fifth embodiment: the positioning module is configured with a preliminary positioning model and a positioning error correction model, the preliminary positioning model is based on a particle filtering algorithm, a pre-obtained starting point and a pre-obtained starting point yaw angle theta are used as input, a particle set p and a particle weight set are initialized, collected geomagnetic field intensity values f, angle offsets a and displacements s are input, N particles are randomly generated, a distribution area of the particles is within a limited range of a fan-shaped area, a robot motion model is used for calculating the corresponding positions of the particles, the particle weights are calculated, a preliminary positioning result is obtained, the positioning error correction model is based on an MAGCOM algorithm, a matching range L, a starting correction distance Dstart and a matching sequence length pmax are used as input, and a matching result is obtained according to the magnetic field intensity of all points on a track p_scanlist and the mean square error of the collected magnetic intensity of a geomagnetic field collection module of the corresponding point; and (3) fusing the matching results of the first step and the second step to realize the correction of the error of the preliminary positioning result. The other embodiments are the same as those of the first embodiment.
The conventional particle filter distribution area is shown in fig. 2. Wherein O is a starting point, B is a sampling point at the moment before the starting point, x0 and y0 are abscissas of the starting point, A is a predicted position of the odometer, and particles are distributed in a rectangular area with an abscissa range of (x-l, x+l) and an ordinate range of (y-l, y+l). l is a constant and needs to be adjusted according to the actual application scene. However, geomagnetic data has large errors which are difficult to predict, and continuous operation during real-time positioning may introduce additional errors and even cause particle filter divergence. Conventional particle filtering is therefore often used for single pass correction after long distance motion. To solve this problem, as shown in fig. 3 (a), the distribution area of the particles of the present invention is within the range of the limitation of the sector area. Wherein O is a starting point, B is a sampling point at the moment before the starting point, x0 and y0 are the abscissas and ordinates of the starting point, A is the predicted position of the odometer, a is the angular offset of the odometer measurement, and s is the displacement of the odometer measurement. And (3) taking O as a pole, taking a transverse axis OX of a ground coordinate system as a polar axis to establish a polar coordinate system, taking an angle of OA as a yaw angle theta, limiting a distribution area of particles to the polar coordinate, taking a distance range as (s-l, s+l), taking a fan-shaped area with an angle range as (theta-phi, theta+phi), and taking l and phi as constants, wherein the adjustment is needed according to actual application scenes.
Specific embodiment six: the function implementation process of the robot motion model comprises the following steps:
where xi and yi are the abscissas of the corresponding positions of the ith particle pi and x0 and y0 are the abscissas of the starting points. This embodiment is otherwise identical to embodiment five.
Specific embodiment seven: as shown in fig. 5, the function implementation process of the preliminary positioning model is as follows:
initializing a particle set p and a particle weight set aiming at a starting point and a starting point yaw angle, adding random numbers on the basis of a and s to randomly generate N particles aiming at a geomagnetic field strength value f, an angle offset a and a displacement s, wherein the angle offset and the displacement corresponding to each particle pi are ai and si respectively, and the distribution area of the particles is within a fan-shaped area limiting range; calculating the corresponding positions of particles by using a robot motion model, reversely reading a geomagnetic reference graph through the positions of the particles, obtaining a geomagnetic intensity value fi corresponding to each particle, calculating the position weights wi of each particle through ai, si and fi, resampling the particles, and calculating the particle weights to obtain a preliminary positioning result. This embodiment is otherwise identical to embodiment five.
Specific embodiment eight: in the polar coordinate system, the distance range is (s- (k4×s+l2), s+ (k3×s+l1)), and the angle range is (s- (k1×a+Φ1), s+ (k2×a+Φ2)); wherein, k1, k2, k3, k4, l1, l2, phi 1 and phi 2 are constants and are adjusted according to the actual application scene. This embodiment is otherwise identical to embodiment seven.
Unsuitable choice of the i, phi parameters may make the restriction too stringent, the particle distribution difficult to cover the real state, or the restriction too relaxed, the particle filtering easy to diverge. The invention compares and analyzes the multi-group odometer track with the real track, and proposes more accurate limitation. To prevent additional errors due to mismatch in the update frequency of the sensor, an average of 10 consecutive points was taken for analysis. As shown in fig. 4, the data is sorted from small to large for 600 points, and it can be seen that the error of the odometer data from the real data is not constant, but is close to a linear function of the odometer data. As shown in fig. 3 (b), the distribution area of the particles is limited to a sector area with a distance range of (s- (k4×s+l2), s+ (k3×s+l1), and an angle range of (s- (k1×a+Φ1), s+ (k2×a+Φ2)), and k1, k2, k3, k4, l1, l2, Φ1, Φ2 are constants, which are adjusted according to the actual application scene. In one implementation, k1=0.5, k2=0, Φ1=0, Φ2=0, k3=1, k4=0.5, l1=0, l2= -50 are taken.
Embodiment nine: the calculation method of the particle weight wi is as follows:
wherein λ1, λ2, and λ3 are constants. This embodiment is otherwise identical to embodiment seven.
The calculation method of the angle offset a and the displacement s in the present embodiment is as follows:
wherein nL and nR are the angles through which the left and right wheels rotate, respectively, l c And r are the wheel spacing and wheel radius of the robot, respectively.
Specific embodiment ten: as shown in fig. 6 and 7, the implementation process of the positioning error correction model is as follows:
aiming at a matching range L, a starting correction distance Dstart and a matching sequence length pmax; adding a positioning result obtained by the preliminary positioning model into a sequence plist, if the length of the plist is larger than pmax and the distance between the last correction point p_last and p_rst is larger than Dstart, taking a grid point p_rst' closest to the p_rst on a geomagnetic chart as a starting point, taking L grid points around the grid point p_rst as points to be searched p_scan, translating the track plist to each point to be searched to generate a track p_scan to be searched, and obtaining a corrected matching result according to the magnetic field intensity of all points on the track p_scan and the mean square error of the acquired magnetic intensity of a geomagnetic field acquisition module of the corresponding point, wherein the end point is set as p_match. The abscissa from p_rst to p_match is x_offset, y_offset, respectively; resampling of all particles p ensures that the weights of all particles differ little. 50% of the particles are randomly picked up in p, and the position (xp, yp) is added with the matching result offset and the random component, namely (xp+x_offset+random x, yp+y_offset+random). random and random are random numbers distributed evenly, the lower limit is 0, and the upper limit is the width of each grid; when the particles are filtered next time, the particles added with offset are calculated by taking the new position as a starting point, the unchanged particles continue to be calculated by taking the original position as the starting point, and the correction of the initial positioning result error particle offset is realized. This embodiment is otherwise identical to embodiment five.
Example 1
In order to verify the accuracy of the method of the present invention, the following examples are used to describe the method of the present invention in detail.
Experimental scenario: the positioning system is applied to the wheeled differential robot and is compared with a real track.
As shown in FIG. 8, the positioning system is based on an STM32 chip, in terms of data transmission, the geomagnetic field intensity data acquired by the triaxial magnetometer is transmitted to the STM32 chip through a serial port, the angle and displacement data transmitted by the wheel type odometer acquisition robot driving wheel are transmitted to the STM32 chip through the serial port, and the real-time positioning result of the STM32 chip is stored in an SD card on the one hand and is transmitted to an upper computer through the 4G data transmission module on the other hand.
In this embodiment, a map of 5m×6m is used to control the robot to move 16.805m in a curved random trajectory.
Experimental environment: CPU:12th Gen Intel (R) Core (TM) i5-1240P 1.70GHz,16G RAM,Windows11.
The odometer trajectory, the conventional zone limitation, the positioning trajectory using only the modified zone limitation, the modified zone limitation + MAGCOM correction were compared with the real trajectory, the resulting path is shown in fig. 9 and the results are shown in table 1.
TABLE 1
As can be seen from table 1, the filtering using the conventional region limitation has diverged after long-distance operation, and positioning cannot be completed. The modified zone limitation reduces the average error to some extent, but may introduce a larger maximum error. The system of the invention has better results than other methods in terms of average error and maximum error; under long-distance work, the accumulated error of the odometer can be reduced, and the positioning accuracy is improved.
Although the present disclosure is disclosed above, the scope of the present disclosure is not limited thereto. Various changes and modifications may be made by one skilled in the art without departing from the spirit and scope of the disclosure, and such changes and modifications would be within the scope of the disclosure.
Claims (10)
1. Differential robot geomagnetic auxiliary positioning system based on particle filtering, which is characterized in that the positioning system comprises: the system comprises a geomagnetic field acquisition module, a mileage acquisition module, a positioning module, a storage module and an upper computer;
the geomagnetic field acquisition module is used for acquiring geomagnetic field intensity data and sending the acquired data to the positioning module;
the mileage acquisition module is used for acquiring the data of the angle and displacement rotated by the driving wheel of the robot and transmitting the acquired data to the positioning module;
the positioning module is used for resolving geomagnetic field intensity data and angle and displacement data rotated by the driving wheel of the robot so as to perform real-time positioning, and storing a positioning result in the storage module and sending the positioning result to the upper computer;
the storage module is used for storing geomagnetic field intensity data and positioning result data, forming a geomagnetic reference map based on the geomagnetic field intensity data and sending the geomagnetic reference map to the positioning module;
the upper computer is used for monitoring the positioning result in real time.
2. The particle filter based differential robotic geomagnetic aided positioning system of claim 1, wherein the geomagnetic field acquisition module includes a triaxial magnetometer, the triaxial magnetometer being mounted on the robot.
3. The particle filter based differential robot geomagnetic aided positioning system of claim 2, wherein the mileage acquisition module includes two wheel type odometers, and the two wheel type odometers are respectively installed on left and right driving wheels of the robot.
4. The particle filter-based differential robot geomagnetic aided positioning system of claim 1, wherein the positioning system further comprises a data transmission module for transmitting the positioning result of the positioning module to an upper computer.
5. The differential robot geomagnetic auxiliary positioning system based on particle filtering according to claim 1, wherein the positioning module is configured with a preliminary positioning model and a positioning error correction model, the preliminary positioning model is based on a particle filtering algorithm, a particle set p and a particle weight set are initialized by taking a pre-obtained starting point and starting point yaw angle theta as input, acquired geomagnetic field intensity values f, angle offsets a and displacements s are input, N particles are randomly generated, a distribution area of the particles is within a limited range of a sector area, a robot motion model is used for calculating particle corresponding positions, a particle weight is calculated, a preliminary positioning result is obtained by calculating the particle weights, the positioning error correction model is based on an MAGCOM algorithm, a matching range L, a starting correction distance Dstart and a matching sequence length pmax are taken as input, and a matching result is obtained according to the magnetic field intensity of all points on a track p_scanlist and mean square errors of magnetic field intensities acquired by a geomagnetic field acquisition module at corresponding points; and (3) fusing the matching results of the first step and the second step to realize the correction of the error of the preliminary positioning result.
6. The particle filter-based differential robot geomagnetic aided positioning system of claim 5, wherein the function implementation process of the robot motion model is as follows:
where xi and yi are the abscissas of the corresponding positions of the ith particle pi and x0 and y0 are the abscissas of the starting points.
7. The particle filter-based geomagnetic aided positioning system of a differential robot of claim 5, wherein the function implementation process of the preliminary positioning model is as follows:
initializing a particle set p and a particle weight set aiming at a starting point and a starting point yaw angle, adding random numbers on the basis of a and s to randomly generate N particles aiming at a geomagnetic field strength value f, an angle offset a and a displacement s, wherein the angle offset and the displacement corresponding to each particle pi are ai and si respectively, and the distribution area of the particles is within a fan-shaped area limiting range; calculating the corresponding positions of particles by using a robot motion model, reversely reading a geomagnetic reference graph through the positions of the particles, obtaining a geomagnetic intensity value fi corresponding to each particle, calculating the position weights wi of each particle through ai, si and fi, resampling the particles, and calculating the particle weights to obtain a preliminary positioning result.
8. The particle filter based differential robotic geomagnetic aided positioning system of claim 7, wherein the range of the sector area is: in the polar coordinate system, the distance range is (s- (k4×s+l2), s+ (k3×s+l1)), and the angle range is (s- (k1×a+Φ1), s+ (k2×a+Φ2)); wherein, k1, k2, k3, k4, l1, l2, phi 1 and phi 2 are constants and are adjusted according to the actual application scene.
9. The particle filter-based differential robot geomagnetic aided positioning system of claim 7, wherein the calculation method of the particle weight wi is as follows:
wherein lambda is 1 、λ 2 、λ 3 Is constant.
10. The particle filter-based differential robot geomagnetic aided positioning system of claim 5, wherein the implementation process of the positioning error correction model is as follows:
aiming at a matching range L, a starting correction distance Dstart and a matching sequence length pmax; adding a positioning result obtained by the preliminary positioning model into a sequence plist, if the length of the plist is larger than pmax and the distance between the last correction point p_last and p_rst is larger than Dstart, taking a grid point p_rst' closest to the p_rst on a geomagnetic chart as a starting point, taking L grid points around the grid point p_rst as points p_scan to be searched, translating the track plist to each point to be searched to generate a track p_scan to be searched, and acquiring a corrected matching result according to the magnetic field intensity of all points on the track p_scan and the mean square error of the acquired magnetic intensity of a geomagnetic field acquisition module of the corresponding point, thereby realizing correction of error particle offset of the preliminary positioning result.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311867591.8A CN117824627B (en) | 2023-12-29 | 2023-12-29 | Differential robot geomagnetic auxiliary positioning system based on particle filtering |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311867591.8A CN117824627B (en) | 2023-12-29 | 2023-12-29 | Differential robot geomagnetic auxiliary positioning system based on particle filtering |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117824627A true CN117824627A (en) | 2024-04-05 |
| CN117824627B CN117824627B (en) | 2024-08-13 |
Family
ID=90518552
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311867591.8A Active CN117824627B (en) | 2023-12-29 | 2023-12-29 | Differential robot geomagnetic auxiliary positioning system based on particle filtering |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117824627B (en) |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6454036B1 (en) * | 2000-05-15 | 2002-09-24 | ′Bots, Inc. | Autonomous vehicle navigation system and method |
| KR20120105616A (en) * | 2011-03-16 | 2012-09-26 | (주)프라이전트 | Indoor tracking device and method for ugv using inertial sensor |
| CN106441302A (en) * | 2016-09-23 | 2017-02-22 | 上海交通大学 | Indoor localization method for large open type area |
| CN107504971A (en) * | 2017-07-05 | 2017-12-22 | 桂林电子科技大学 | A kind of indoor orientation method and system based on PDR and earth magnetism |
| CN107576325A (en) * | 2017-08-25 | 2018-01-12 | 北京麦钉艾特科技有限公司 | A kind of indoor positioning terminal for merging visual odometry and Magnetic Sensor |
| CN207540557U (en) * | 2017-12-13 | 2018-06-26 | 华中科技大学 | A kind of device pinpoint in short-term for AGV trolleies |
| CN110849349A (en) * | 2019-10-18 | 2020-02-28 | 浙江天尚元科技有限公司 | Fusion positioning method based on magnetic sensor and wheel type odometer |
| CN111964667A (en) * | 2020-07-03 | 2020-11-20 | 杭州电子科技大学 | geomagnetic-INS (inertial navigation System) integrated navigation method based on particle filter algorithm |
-
2023
- 2023-12-29 CN CN202311867591.8A patent/CN117824627B/en active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6454036B1 (en) * | 2000-05-15 | 2002-09-24 | ′Bots, Inc. | Autonomous vehicle navigation system and method |
| KR20120105616A (en) * | 2011-03-16 | 2012-09-26 | (주)프라이전트 | Indoor tracking device and method for ugv using inertial sensor |
| CN106441302A (en) * | 2016-09-23 | 2017-02-22 | 上海交通大学 | Indoor localization method for large open type area |
| CN107504971A (en) * | 2017-07-05 | 2017-12-22 | 桂林电子科技大学 | A kind of indoor orientation method and system based on PDR and earth magnetism |
| CN107576325A (en) * | 2017-08-25 | 2018-01-12 | 北京麦钉艾特科技有限公司 | A kind of indoor positioning terminal for merging visual odometry and Magnetic Sensor |
| CN207540557U (en) * | 2017-12-13 | 2018-06-26 | 华中科技大学 | A kind of device pinpoint in short-term for AGV trolleies |
| CN110849349A (en) * | 2019-10-18 | 2020-02-28 | 浙江天尚元科技有限公司 | Fusion positioning method based on magnetic sensor and wheel type odometer |
| CN111964667A (en) * | 2020-07-03 | 2020-11-20 | 杭州电子科技大学 | geomagnetic-INS (inertial navigation System) integrated navigation method based on particle filter algorithm |
Non-Patent Citations (3)
| Title |
|---|
| BENJAMIN SIEBLER; OLIVER HEIRICH; STEPHAN SAND: "Train Localization with Particle Filter and Magnetic Field Measurements", 2018 21ST INTERNATIONAL CONFERENCE ON INFORMATION FUSION (FUSION), 6 September 2018 (2018-09-06), pages 1715 - 1719 * |
| SEUNG-MOK LEE: "A Performance Comparison of Geomagnetic Field-Based Vector Field SLAM Approaches", RITA 2018. 6TH INTERNATIONAL CONFERENCE ON ROBOT INTELLIGENCE TECHNOLOGY AND APPLICATIONS, 16 June 2019 (2019-06-16), pages 213 - 218 * |
| 张文杰: "基于多传感器室内移动机器人自主定位方法的研究", 中国优秀硕士学位论文全文数据库 信息科技辑, no. 2, 15 December 2011 (2011-12-15) * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117824627B (en) | 2024-08-13 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109885049B (en) | Automatic mapping and path matching method for laser-guided AGV (automatic guided vehicle) based on dead reckoning | |
| CN109916431B (en) | Wheel encoder calibration algorithm for four-wheel mobile robot | |
| CN106289327A (en) | A kind of difference moves robot speedometer scaling method | |
| CN107957725B (en) | High-precision automatic guided vehicle positioning and orienting device and method based on single magnetic nail | |
| US20210240192A1 (en) | Estimation method and estimator for sideslip angle of straight-line navigation of agricultural machinery | |
| CN205066775U (en) | High accuracy movement track detection device | |
| CN202929482U (en) | Automatic trailing and positioning vehicle system based on induction wireless technology | |
| CN102865862A (en) | Non-horizontalization free established station for total station and project measurement method of non-horizontalization free established station | |
| CN110515381B (en) | Multi-sensor fusion algorithm for positioning robot | |
| CN113703446B (en) | Guide vehicle navigation method and dispatch system based on magnetic nails | |
| CN110068334A (en) | A kind of high-precision locating method of magnetic navigation AGV | |
| CN105066988A (en) | Magnetic strip deviation angle measurement method based on high precision magnetic navigation sensor | |
| JPH02206716A (en) | Running azimuth detection device for vehicle | |
| CN103294920B (en) | A kind of part Cylindricity error evaluation based on Minimum Area | |
| CN117824627B (en) | Differential robot geomagnetic auxiliary positioning system based on particle filtering | |
| CN108709560A (en) | Carrying robot high accuracy positioning air navigation aid based on straightway feature | |
| CN105203096A (en) | Rotary magnetic field rapid tracking method and system based on four-point measurement | |
| CN109141400A (en) | Magnetic navigation system and its centralized positioning algorithm for AGV trolley | |
| CN113752883A (en) | AGV material distribution car positioning system based on high-speed information communication | |
| CN106908058A (en) | A kind of method of definitely magnetic orientation array aperture | |
| CN102506891B (en) | Mileage detection method for magnetic-navigation wheeled autonomous mobile robot | |
| CN114088113B (en) | Odometer track alignment and precision evaluation method | |
| CN115342830A (en) | Calibration method, program product and calibration device for a positioning device and a odometer | |
| CN111474535B (en) | Mobile robot global positioning method based on characteristic thermodynamic diagram | |
| CN113759919B (en) | Mobile robot track tracking method and system |
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 |