CN111486840A - Robot positioning method and device, robot and readable storage medium - Google Patents
Robot positioning method and device, robot and readable storage medium Download PDFInfo
- Publication number
- CN111486840A CN111486840A CN202010600072.5A CN202010600072A CN111486840A CN 111486840 A CN111486840 A CN 111486840A CN 202010600072 A CN202010600072 A CN 202010600072A CN 111486840 A CN111486840 A CN 111486840A
- Authority
- CN
- China
- Prior art keywords
- current
- pose information
- filter
- positioning
- current pose
- 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.)
- Pending
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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- 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
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
- G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
- G01S5/0257—Hybrid positioning
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
The application provides a robot positioning method, a device, a robot and a readable storage medium, wherein the method comprises the following steps: the method comprises the steps of obtaining first current pose information issued by a preset reference positioning mode and second current pose information issued by other positioning modes, enabling an input information pair formed by the first current pose information and one second current pose information to be respectively input into sub-filters of a preset federal filter, measuring current position deviation between the second current pose information and the first current pose information in the input information pair as a quantity, obtaining current local error estimation output by each sub-filter, inputting the current local error estimation into a main filter for optimal fusion, obtaining a current global estimation error, and correcting the first current pose information according to the current local error estimation. According to the method and the device, data of all positioning modes are fused through a federal filter, and a globally optimal global estimation error is output to correct the first current pose information, so that the final positioning precision is improved.
Description
Technical Field
The application relates to the technical field of robots, in particular to a robot positioning method and device, a robot and a readable storage medium.
Background
The service robot has a complex working environment, the environment changes at any time, and the service robot is influenced and interfered by the outside, so if the navigation task is to be completed in an indoor environment independently, the global position of the robot in the environment needs to be known, namely the robot needs to have the indoor independent positioning capability.
At present, there are many methods for robot positioning, mainly including GNSS (Global Navigation satellite system), laser S L AM (simultaneous localization and mapping, instant positioning and map construction), vision S L AM, odometer, IMU (Inertial Measurement Unit), WIFI, bluetooth, infrared, etc. however, when each method is used alone, there are some problems that are difficult to solve.
For this reason, a multi-mode cooperative positioning scheme is often adopted at present. However, when a plurality of positioning methods are used to perform positioning together, how to fuse data of the plurality of positioning methods to obtain a more accurate positioning result becomes a key to influence the final positioning accuracy.
Disclosure of Invention
An object of the embodiments of the present application is to provide a method and an apparatus for positioning a robot, and a readable storage medium, so as to solve the problem that the accuracy of global positioning is not high when a single positioning means is used for positioning at present.
The embodiment of the application provides a robot positioning method, which comprises the following steps: acquiring first current pose information issued by a preset reference positioning mode; acquiring second current pose information issued by at least one other positioning mode different from the reference positioning mode; inputting an input information pair formed by the first current pose information and one second current pose information into each sub-filter of a preset federal filter respectively; each of the other positioning modes corresponds to one of the sub-filters; each sub-filter estimates a state quantity by taking a current position deviation between the second current pose information and the first current pose information in the input information pair as a measurement quantity, so as to obtain a current local error estimation aiming at the state quantity; inputting the first current pose information and current local error estimation output by each sub-filter into a main filter of the federal filter for optimal fusion to obtain a current global estimation error; and correcting the first current pose information according to the current global estimation error.
In the implementation process, information fusion between each positioning mode and the reference positioning mode can be achieved through the federal filter, and therefore the global optimal global estimation error is output. And then, the first current pose information of the reference positioning mode can be corrected according to the global estimation error. Therefore, fusion of data of various positioning modes is realized, and the final positioning precision is improved.
Further, the preset reference positioning mode is an IMU positioning mode; the state quantity includes at least one of: a position error of the IMU; attitude error of the IMU; a speed error of the IMU; a gyro zero bias of the IMU; the accelerometer of the IMU is zero offset. .
Further, the other positioning modes comprise a WiFi positioning mode; the acquiring of the second current pose information issued by at least one other positioning mode different from the reference positioning mode includes: acquiring Received Signal Strength Indication (RSSI) data of a currently received WiFi signal; inputting the RSSI data into a preset positioning model to obtain second current pose information; the positioning model is obtained by training a large number of preset sampling points and RSSI data corresponding to each sampling point as sample data.
Further, the rest of the positioning modes include a positioning mode based on Odom (odometer coordinate system) data; the acquiring of the second current pose information issued by at least one other positioning mode different from the reference positioning mode includes: and carrying out velocity integral recursion calculation on the Odom data to obtain the second current pose information.
Further, after the pair of input information consisting of the first current pose information and one second current pose information is input to each sub-filter of a preset federal filter, the method further includes: each sub-filter outputs the current local covariance matrix of each other positioning mode and the reference positioning mode; before inputting the first current pose information and the current local error estimate output by each sub-filter into a main filter of the federated filter for optimal fusion, the method further comprises: and determining that all the current local covariance matrixes meet a preset first reliability requirement.
It should be understood that in the federal filter, each sub-filter as well as the main filter outputs a corresponding error estimate, along with a corresponding covariance matrix. The values of the elements on one diagonal of the covariance matrix characterize the variance. Therefore, in the embodiment of the present application, the availability of the local error estimation of this time can be determined by the difference values contained in the current local covariance matrix output by each sub-filter, so that when the local error estimation of the sub-filters meets the reliability requirement, the local error estimation is input into the main filter for optimal fusion, thereby improving the reliability of the scheme of the present application.
Further, after the main filter of the federated filter is optimally fused, a current global covariance matrix is obtained; before correcting the first current pose information according to the current global estimation error, the method further comprises: and determining that the current global covariance matrix meets a preset second reliability requirement.
As described above, in the embodiment of the present application, the availability of the global error estimate of this time may be determined by the difference values included in the current global covariance matrix output by the main filter, so that when the global error estimate of the main filter meets the requirement of reliability, the global error estimate is used to correct the first current pose information, thereby improving the reliability of the solution of the present application.
Further, the method further comprises: and when the current global covariance matrix does not meet the preset second credibility requirement, prompting according to a preset mode so as to correct the federal filter.
In the implementation process, when the reliability requirement is not met, prompt can be performed so as to correct the Federal filter, so that the reliability of the Federal filter can be effectively guaranteed, and the reliability of the scheme of the embodiment of the application can be further guaranteed.
The embodiment of the present application further provides a robot positioning device, including: the device comprises an acquisition module and a processing module; the acquisition module is used for acquiring first current pose information issued by a preset reference positioning mode and second current pose information issued by at least one other positioning mode different from the reference positioning mode; the processing module is used for inputting an input information pair formed by the first current pose information and the second current pose information into each sub-filter of a preset federal filter respectively; each of the other positioning modes corresponds to one of the sub-filters; the processing module is further configured to estimate a state quantity in each sub-filter by using a current position deviation between the second current pose information and the first current pose information in the input information pair as a measurement to obtain a current local error estimate for the state quantity; the processing module is further configured to input the first current pose information and current local error estimates output by each sub-filter into a main filter of the federal filter for optimal fusion, so as to obtain a current global estimation error; the processing module is further configured to correct the first current pose information according to the current global estimation error.
An embodiment of the present application further provides a robot, including: a processor, a memory, and a communication bus; the communication bus is used for realizing connection communication between the processor and the memory; the processor is configured to execute one or more programs stored in the memory to implement any of the above-described robot positioning methods.
Also provided in an embodiment of the present application is a readable storage medium storing one or more programs, the one or more programs being executable by one or more processors to implement the robot positioning method of any one of the above.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings that are required to be used in the embodiments of the present application will be briefly described below, it should be understood that the following drawings only illustrate some embodiments of the present application and therefore should not be considered as limiting the scope, and that those skilled in the art can also obtain other related drawings based on the drawings without inventive efforts.
Fig. 1 is a schematic flowchart of a robot positioning method according to an embodiment of the present disclosure;
fig. 2 is a schematic structural diagram of a bang filter according to an embodiment of the present application;
fig. 3 is a schematic diagram of a specific federal filter structure provided in an embodiment of the present application;
fig. 4 is a schematic structural diagram of a robot positioning device according to an embodiment of the present disclosure;
fig. 5 is a schematic structural diagram of a robot according to an embodiment of the present application.
Detailed Description
The technical solutions in the embodiments of the present application will be described below with reference to the drawings in the embodiments of the present application.
The first embodiment is as follows:
the embodiment of the application provides a robot positioning method, and as shown in fig. 1, the method comprises the following steps:
s101: and acquiring first current pose information issued by a preset reference positioning mode.
In the embodiment of the present application, the preset reference positioning mode may be an IMU positioning mode, or an Odom data-based positioning mode.
Illustratively, when an IMU positioning mode is adopted, the robot can acquire IMU data and then update a strapdown algorithm in real time, so that current pose information based on the IMU positioning mode is obtained.
It should be understood that the IMU data is generally angular velocity data, acceleration data, and the like of the robot, and after the IMU data is processed through the strapdown algorithm, information such as the velocity, the position, the posture, and the like of the robot can be obtained.
Illustratively, when a positioning mode based on the Odom data is adopted, the current pose information of the positioning mode based on the Odom data can be obtained by performing velocity integral recursion calculation on the Odom data.
S102: and acquiring second current pose information issued by at least one other positioning mode different from the reference positioning mode.
In the embodiment of the present application, the reference positioning manner and the remaining positioning manners may be set by engineers according to actual needs, and corresponding functional devices are configured on the robot, thereby ensuring the enforceability of the solution of the present application.
For example, in the embodiment of the present application, the reference positioning manner may adopt an IMU positioning manner, and the remaining positioning manners may adopt a WiFi positioning manner and/or an Odom positioning manner.
For example, in the embodiment of the present application, the reference positioning manner may also be an Odom positioning manner, and the remaining positioning manners may be a WiFi positioning manner and/or an IMU positioning manner.
In the embodiment of the application, when a WiFi positioning mode is adopted, RSSI data corresponding to each sampling point may be collected at different sampling point positions (for example, each sampling point is 1 to 2 meters apart in a map) in advance, and then, a large number of sampling points and the RSSI data corresponding to each sampling point are input into a preset model (for example, a binary model of a support vector machine algorithm, etc.) as sample data to be trained, so that a trained model is obtained.
After that, RSSI data (i.e. current RSSI data) of a WiFi signal currently received by the robot can be continuously obtained according to a preset acquisition frequency (e.g. 1Hz frequency), and the current RSSI data is input into a trained model, so that current pose information based on a WiFi positioning mode can be obtained.
It should be understood that, in the embodiment of the present application, in order to improve the positioning accuracy, while RSSI data corresponding to each sampling Point is collected, an AP (Access Point) position corresponding to each sampling Point may also be collected, and a large number of sampling points, the RSSI data corresponding to each sampling Point, and the AP position corresponding to each sampling Point are input as sample data into a preset model for training, so as to obtain a trained model. When WiFi positioning is carried out, current RSSI data of the robot and the current corresponding AP position are collected as input data and input into a trained model, and therefore current pose information based on a WiFi positioning mode is obtained.
It should be noted that, in the embodiment of the present application, the first current pose information and the second current pose information are both represented by pose information at the current time, and are only issued by using different positioning manners.
S103: and respectively inputting an input information pair formed by the first current pose information and the second current pose information into each sub-filter of a preset federal filter.
It should be understood that, in the embodiment of the present application, each of the remaining positioning manners corresponds to one sub-filter, and one sub-filter corresponds to only one of the remaining positioning manners. That is, there is a one-to-one correspondence between the remaining positioning methods and the sub-filters. That is, in the embodiment of the present application, each sub-filter only acquires the first current pose information and the second current pose information. For convenience of description, the first current pose information and the second current pose information input to the sub-filter are referred to as an input information pair in the present application.
It should be appreciated that the federal filter is a distributed filter, which adopts a block estimation structure to realize information fusion for a plurality of systems, and thus, global optimal estimation of the overall state of the system is realized. The structure of which can be seen in fig. 2.
S104: and each sub-filter estimates the state quantity by taking the current position deviation between the second current pose information and the first current pose information in the input information pair as a measurement quantity, so as to obtain the current local error estimation aiming at the state quantity.
In the embodiment of the present application, each sub-filter of the federate filter may be implemented by using a kalman filter (such as an extended kalman filter), so as to implement the locally optimal estimation.
It should be appreciated that kalman filtering seeks a set of recursive estimated algorithms with the minimum mean square error as the best criterion for the estimation. The basic idea is as follows: and updating the estimation of the state quantity by using the estimation value of the state quantity at the previous moment and the observation value (namely, measurement value) at the current moment by using a state space model of the signal and the noise to obtain the estimation value of the state quantity at the current moment.
It should be understood that, in theory, in the case where each positioning manner is completely accurate, the second current pose information and the first current pose information should correspond to the same pose. However, in the practical application process, different positioning modes often have certain deviation according to the robot pose at the same moment. Therefore, in the embodiment of the present application, the current position deviation between the second current pose information and the first current pose information in the input information pair input to each sub-filter can be calculated, and the state quantity is estimated according to the current position deviation between the second current pose information and the first current pose information in the input information pair as a measurement quantity.
It should also be understood that in the embodiments of the present application, the state quantities are some errors in the robot related to pose information.
For example, for the positioning manner of the IMU, one or more of a position error, an attitude error, a velocity error, a gyro zero offset (e.g., a three-axis constant drift error of a three-axis gyroscope), an accelerometer zero offset (e.g., an accelerometer constant drift error), and the like of the IMU may be set as the state quantities. For example, position error, attitude error, velocity error, gyro zero offset, and accelerometer zero offset in the IMU positioning mode may all be used as state quantities for error estimation.
For another example, for the positioning method based on the odometer Odom data, one or more of a position error, an attitude error, a speed error, and the like in the positioning method based on the odometer Odom data may be set as the state quantity. For example, the error estimation may be performed using all of the position error, the attitude error, and the velocity error in the positioning method based on the odometer Odom data as the state quantities.
S105: and inputting the first current pose information and the current local error estimation output by each sub-filter into a main filter of a federal filter for optimal fusion to obtain a current global estimation error.
As can be seen from the foregoing, after an input information pair formed by the first current pose information and the second current pose information is input to the sub-filter, the sub-filter estimates the state quantities at the current time between the reference positioning mode and the remaining positioning modes corresponding to the second current pose information. In the embodiment of the present application, the state quantity at the current time may be input to the main filter as a local error estimate.
It should be noted that, in the federal filter, each sub-filter outputs the current local error estimate and also outputs the current local covariance matrix corresponding to the current local error estimate. The reliability of the current local error estimate may be determined by the current local covariance matrix corresponding to each current local error estimate.
In the embodiment of the present application, before the current error state quantities and the current local error estimates are input to the main filter of the federal filter for optimal fusion, it may be determined whether all current local covariance matrices meet a preset first reliability requirement. And when all the current local covariance matrixes meet a preset first credibility requirement, inputting the current error state quantity and each current local error estimation into a main filter of a federal filter for optimal fusion. If one or some current local covariance matrixes do not meet the preset first credibility requirement, prompting can be carried out according to a preset mode so as to inform an engineer to correct the Nippon filter.
In the embodiment of the present application, the prompting method includes, but is not limited to, notifying through a preset communication method such as a short message and an email.
It should be understood that each element value on one diagonal in the covariance matrix characterizes a variance. In the embodiment of the application, whether the preset first reliability requirement is met or not can be determined according to each variance value in the current local covariance matrix. For example, it may be determined that the preset first reliability requirement is met when all variance values in the current local covariance matrix are smaller than the preset first variance threshold, otherwise, it may be determined that the preset first reliability requirement is not met.
In addition, in the embodiment of the present application, after the main filter performs the optimal fusion, a current global covariance matrix corresponding to the current global estimation error is obtained. The reliability of the current global error estimate may be determined based on the current global covariance matrix.
Similarly, in the embodiment of the present application, after obtaining the current global estimation error and the current global covariance matrix, before performing step S106, it may be determined whether the current global covariance matrix meets a preset second confidence requirement. When the current global covariance matrix meets the preset second reliability requirement, step S106 is executed. Otherwise, prompting can be performed according to a preset mode so as to inform an engineer to correct the Nippon filter.
Similarly, in the embodiment of the present application, whether the preset second confidence requirement is met may be determined according to each variance value in the current global covariance matrix. For example, it may be determined that the preset second reliability requirement is met when all variance values in the current global covariance matrix are smaller than the preset second variance threshold, otherwise, it is determined that the preset second reliability requirement is not met.
In the embodiment of the present application, each threshold may be set by an engineer according to actual needs.
It should be noted that the federal filter includes both a federal filter with reset and a federal filter without reset. The Federal filter without resetting has no information exchange among the sub-filters in the filtering process, the sub-filters carry out recursion by themselves, and the local filtering precision is low. And the global error and the current global covariance matrix output by the reset federal filter at the main filter are fed back to each sub-filter, so that the local error estimated value and the current local covariance matrix of each sub-filter are reset, the local filtering precision can be improved, and the filtering precision is higher compared with the reset federal filter. However, the local filtering of the federal filter with reset is affected by the global filter feedback, which causes a sub-filter failure to affect the otherwise good sub-filter, and thus the federal filter with reset is less fault tolerant than the federal filter without reset.
In the embodiment of the present application, an engineer may select to use a federal filter with reset or a federal filter without reset to implement the solution of the embodiment of the present application according to actual needs.
S106: and correcting the first current pose information according to the current global estimation error.
In the embodiment of the application, after the current global estimation error is obtained, the first current pose information issued by the reference positioning mode can be corrected based on the current global estimation error, so that the finally determined positioning result is more accurate.
According to the robot positioning method provided by the embodiment of the application, the positioning information between each positioning mode and the reference positioning mode is fused through the federal filter, so that the global optimal estimation error is output, and then the first current pose information of the reference positioning mode can be corrected according to the global estimation error, and a more accurate positioning result is obtained. Therefore, fusion of data of various positioning modes is realized, and the final positioning precision is improved.
Example two:
in this embodiment, on the basis of the first embodiment, the case where the IMU positioning mode is used as the reference positioning mode, and the WiFi positioning mode and the Odom data-based positioning mode are used as the remaining positioning modes is taken as an example, and the solution of the embodiment of the present application is further illustrated.
First, the Robot runs an ROS (Robot Operating System) environment.
Next, referring to fig. 3, fig. 3 is a schematic diagram of a federal filter structure in this embodiment. The robot performs current data processing of each positioning mode to obtain current pose information of the robot corresponding to each positioning mode, and inputs the current pose information of the robot corresponding to each positioning mode into corresponding sub-filters according to the structure shown in fig. 3.
The following are exemplary:
for the WiFi positioning mode, the robot can train a model in advance, and then corresponding current pose information is obtained based on the trained model.
Specifically, the robot may divide a mesh into indoor positioning areas (i.e., maps) in advance, establish sampling points according to a preset interval distance (e.g., 1 to 2 meters) in advance, and collect RSSI data acquired by the robot at each sampling point. And (3) forming sample set data by the positions of the sampling points and the RSSI data corresponding to the sampling points, inputting the sample set data into a preset model for training, and obtaining a trained model. And further, during positioning, the current RSSI data can be acquired and input into the model, so that the corresponding current pose information can be obtained.
For the IMU positioning mode, the robot can update the strapdown algorithm of the obtained IMU data in real time, so that the current pose information corresponding to the IMU positioning mode is obtained.
For the positioning mode based on the Odom data, the robot can perform speed integral recursive calculation on the Odom data so as to obtain the current pose information corresponding to the positioning mode based on the Odom data.
Then:
taking the current position error, attitude error, speed error, gyro zero offset and accelerometer zero offset in an IMU positioning mode as state quantities; and measuring the position error between the WiFi positioning mode and the IMU positioning mode, and estimating a state quantity (current local error estimation 1) and a current local covariance matrix 1 between the WiFi positioning mode and the IMU positioning mode through a sub-filter 1 (an extended Kalman filter).
Taking the current position error, attitude error, speed error, gyro zero offset and accelerometer zero offset in an IMU positioning mode as state quantities; and measuring the position error between the positioning mode based on the Odom data and the IMU positioning mode as a quantity, and estimating a state quantity (current local error estimation 2) and a current local covariance matrix 2 between the positioning mode based on the Odom data and the IMU positioning mode through a sub-filter 2 (an extended Kalman filter).
And then, inputting the current pose information of the robot corresponding to the IMU positioning mode, the current local error estimate 1, the current local covariance matrix 1, the current local error estimate 2 and the current local covariance matrix 2 into a main filter to obtain a current global estimation error and a current global covariance matrix.
And finally, correcting the current pose information corresponding to the IMU positioning mode by using the current global estimation error to obtain the corrected pose information, thereby realizing accurate positioning.
By the scheme of the embodiment of the application, the robot can be globally positioned in real time. Through the federal filter, data fusion among multiple positioning modes can be realized, so that the reliability of the obtained global positioning result is higher.
Example three:
based on the same inventive concept, the embodiment of the application also provides a robot positioning device. Referring to fig. 4, fig. 4 shows a robot positioning device 100 corresponding to the method according to the first embodiment. It should be understood that the specific functions of the robot positioning device 100 can be referred to the above description, and the detailed description is omitted here as appropriate to avoid redundancy. The robotic positioning device 100 includes at least one software functional module that can be stored in memory in the form of software or firmware or solidified in the operating system of the robotic positioning device 100. Specifically, the method comprises the following steps:
referring to fig. 4, the robot positioning device 100 includes: an acquisition module 101, and a processing module 102. Wherein:
the acquisition module 101 is configured to acquire first current pose information issued by a preset reference positioning mode and second current pose information issued by at least one other positioning mode different from the reference positioning mode;
the processing module 102 is configured to input an input information pair formed by the first current pose information and one piece of the second current pose information into each sub-filter of a preset federal filter respectively; each of the other positioning modes corresponds to one of the sub-filters;
the processing module 102 is further configured to estimate, in each sub-filter, a state quantity by using a current position deviation between the second current pose information and the first current pose information in the input information pair as a measurement to obtain a current local error estimate for the state quantity;
the processing module 102 is further configured to input the first current pose information and the current local error estimate output by each sub-filter into a main filter of the federal filter for optimal fusion, so as to obtain a current global estimation error;
the processing module 102 is further configured to correct the first current pose information according to the current global estimation error.
In the embodiment of the application, the preset reference positioning mode is an IMU positioning mode; the state quantity includes at least one of: a position error of the IMU; attitude error of the IMU; velocity error of the IMU; gyro zero bias of the IMU; the accelerometer of the IMU is zero offset.
In the embodiment of the application, the other positioning modes include a WiFi positioning mode; the obtaining module 101 is specifically configured to: acquiring Received Signal Strength Indication (RSSI) data of a currently received WiFi signal; the RSSI data is input into a preset positioning model to obtain second current pose information; the positioning model is obtained by training a large number of preset sampling points and RSSI data corresponding to each sampling point as sample data.
In the embodiment of the application, the other positioning modes comprise a positioning mode based on Odom coordinate system Odom data; the obtaining module 101 is specifically configured to: and carrying out velocity integral recursion calculation on the Odom data to obtain second current pose information.
In a possible implementation manner of the embodiment of the present application, each sub-filter may further output a current local covariance matrix of each remaining positioning manner and the reference positioning manner; the processing module 102 is further configured to determine that all current local covariance matrices meet a preset first reliability requirement before inputting the current error state quantities and the current local error estimates to a main filter of the federal filter for optimal fusion.
In a feasible implementation manner of the embodiment of the application, after the main filter of the federal filter is optimally fused, a current global covariance matrix is obtained; the processing module 102 is further configured to determine that the current global covariance matrix meets a preset second reliability requirement before correcting the first current pose information according to the current global estimation error.
In the above possible implementation manner, the processing module 102 is further configured to prompt according to a preset manner when the current global covariance matrix does not meet the preset second confidence requirement, so as to correct the joint filter.
It should be understood that, for the sake of brevity, the contents described in some embodiments are not repeated in this embodiment.
Example four:
the present embodiment provides a robot, as shown in fig. 5, which includes a processor 501, a memory 502, and a communication bus 503. Wherein:
the communication bus 503 is used to realize connection communication between the processor 501 and the memory 502.
The processor 501 is configured to execute one or more programs stored in the memory 502 to implement the robot positioning method according to the first embodiment or the second embodiment.
It will be appreciated that the arrangement shown in figure 5 is merely illustrative and that the robot may also comprise more or fewer components than shown in figure 5 or may have a different configuration than that shown in figure 5, for example there may also be components such as a display screen, speakers etc.
The present embodiment also provides a readable storage medium, such as a floppy disk, an optical disk, a hard disk, a flash Memory, a usb (secure digital Card), an MMC (Multimedia Card), etc., in which one or more programs for implementing the above steps are stored, and the one or more programs can be executed by one or more processors to implement the robot positioning method in the first embodiment. And will not be described in detail herein.
In the embodiments provided in the present application, it should be understood that the disclosed apparatus and method may be implemented in other ways. The above-described embodiments of the apparatus are merely illustrative, and for example, the division of the units is only one logical division, and there may be other divisions when actually implemented, and for example, a plurality of units or components may be combined or integrated into another system, or some features may be omitted, or not executed. In addition, the shown or discussed mutual coupling or direct coupling or communication connection may be an indirect coupling or communication connection of devices or units through some communication interfaces, and may be in an electrical, mechanical or other form.
In addition, units described as separate parts may or may not be physically separate, and parts displayed as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of the embodiment.
Furthermore, the functional modules in the embodiments of the present application may be integrated together to form an independent part, or each module may exist separately, or two or more modules may be integrated to form an independent part.
In this document, relational terms such as first and second, and the like may be used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions.
In this context, a plurality means two or more.
The above description is only an example of the present application and is not intended to limit the scope of the present application, and various modifications and changes may be made by those skilled in the art. Any modification, equivalent replacement, improvement and the like made within the spirit and principle of the present application shall be included in the protection scope of the present application.
Claims (10)
1. A robot positioning method, comprising:
acquiring first current pose information issued by a preset reference positioning mode;
acquiring second current pose information issued by at least one other positioning mode different from the reference positioning mode;
inputting an input information pair formed by the first current pose information and one second current pose information into each sub-filter of a preset federal filter respectively; each of the other positioning modes corresponds to one of the sub-filters;
each sub-filter estimates a state quantity by taking a current position deviation between the second current pose information and the first current pose information in the input information pair as a measurement quantity, so as to obtain a current local error estimation aiming at the state quantity;
inputting the first current pose information and current local error estimation output by each sub-filter into a main filter of the federal filter for optimal fusion to obtain a current global estimation error;
and correcting the first current pose information according to the current global estimation error.
2. The robot positioning method according to claim 1, wherein the preset reference positioning manner is an Inertial Measurement Unit (IMU) positioning manner;
the state quantity includes at least one of:
a position error of the IMU;
attitude error of the IMU;
a speed error of the IMU;
a gyro zero bias of the IMU;
the accelerometer of the IMU is zero offset.
3. A robot positioning method according to claim 2, wherein the remaining positioning means includes a WiFi positioning means;
the acquiring of the second current pose information issued by at least one other positioning mode different from the reference positioning mode includes:
acquiring Received Signal Strength Indication (RSSI) data of a currently received WiFi signal;
inputting the RSSI data into a preset positioning model to obtain second current pose information; the positioning model is obtained by training a large number of preset sampling points and RSSI data corresponding to each sampling point as sample data.
4. The robot positioning method according to claim 2, wherein the remaining positioning means includes a positioning means based on Odom data of an odometer coordinate system;
the acquiring of the second current pose information issued by at least one other positioning mode different from the reference positioning mode includes:
and carrying out velocity integral recursion calculation on the Odom data to obtain the second current pose information.
5. The robot positioning method according to any one of claims 1 to 4, wherein after the pair of input information composed of the first current pose information and one of the second current pose information is input into each sub-filter of a preset federal filter, respectively, the method further comprises: each sub-filter outputs the current local covariance matrix of each other positioning mode and the reference positioning mode;
before inputting the first current pose information and the current local error estimate output by each sub-filter into a main filter of the federated filter for optimal fusion, the method further comprises:
and determining that all the current local covariance matrixes meet a preset first reliability requirement.
6. A robot positioning method according to any of claims 1-4, characterized in that the main filter of the federated filter also gets the current global covariance matrix after optimal fusion;
before correcting the first current pose information according to the current global estimation error, the method further comprises:
and determining that the current global covariance matrix meets a preset second reliability requirement.
7. The robot positioning method of claim 6, further comprising:
and when the current global covariance matrix does not meet the preset second credibility requirement, prompting according to a preset mode so as to correct the federal filter.
8. A robot positioning device, comprising: the device comprises an acquisition module and a processing module;
the acquisition module is used for acquiring first current pose information issued by a preset reference positioning mode and second current pose information issued by at least one other positioning mode different from the reference positioning mode;
the processing module is used for inputting an input information pair formed by the first current pose information and the second current pose information into each sub-filter of a preset federal filter respectively; each of the other positioning modes corresponds to one of the sub-filters;
the processing module is further configured to estimate a state quantity in each sub-filter by using a current position deviation between the second current pose information and the first current pose information in the input information pair as a measurement to obtain a current local error estimate for the state quantity;
the processing module is further configured to input the first current pose information and current local error estimates output by each sub-filter into a main filter of the federal filter for optimal fusion, so as to obtain a current global estimation error;
the processing module is further configured to correct the first current pose information according to the current global estimation error.
9. A robot, comprising: a processor, a memory, and a communication bus;
the communication bus is used for realizing connection communication between the processor and the memory;
the processor is configured to execute one or more programs stored in the memory to implement the robot positioning method of any of claims 1 to 7.
10. A readable storage medium storing one or more programs, the one or more programs being executable by one or more processors to implement the robot positioning method according to any one of claims 1 to 7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010600072.5A CN111486840A (en) | 2020-06-28 | 2020-06-28 | Robot positioning method and device, robot and readable storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010600072.5A CN111486840A (en) | 2020-06-28 | 2020-06-28 | Robot positioning method and device, robot and readable storage medium |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN111486840A true CN111486840A (en) | 2020-08-04 |
Family
ID=71795794
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010600072.5A Pending CN111486840A (en) | 2020-06-28 | 2020-06-28 | Robot positioning method and device, robot and readable storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111486840A (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113295159A (en) * | 2021-05-14 | 2021-08-24 | 浙江商汤科技开发有限公司 | Positioning method and device for end cloud integration and computer readable storage medium |
| CN114111774A (en) * | 2021-12-06 | 2022-03-01 | 纵目科技（上海）股份有限公司 | Vehicle positioning method, system, device and computer readable storage medium |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104081313A (en) * | 2011-11-11 | 2014-10-01 | 高通股份有限公司 | Sensor auto-calibration |
| CN104217245A (en) * | 2014-08-27 | 2014-12-17 | 高阳 | People stream trajectory tracking and area dwell time statistics method and system based on heterogeneous network |
| CN107656301A (en) * | 2017-09-20 | 2018-02-02 | 北京航天发射技术研究所 | A kind of vehicle positioning method based on Multi-source Information Fusion |
| CN108225302A (en) * | 2017-12-27 | 2018-06-29 | 中国矿业大学 | A kind of petrochemical factory's crusing robot alignment system and method |
| US20180188383A1 (en) * | 2017-01-04 | 2018-07-05 | Qualcomm Incorporated | Position-window extension for gnss and visual-inertial-odometry (vio) fusion |
| CN109737959A (en) * | 2019-03-20 | 2019-05-10 | 哈尔滨工程大学 | A kind of polar region Multi-source Information Fusion air navigation aid based on federated filter |
| CN109849976A (en) * | 2017-11-30 | 2019-06-07 | 河南星云慧通信技术有限公司 | A kind of train positioning system based on GPS, ODO and WKNN combination |
| CN110646825A (en) * | 2019-10-22 | 2020-01-03 | 北京新能源汽车技术创新中心有限公司 | Positioning method, positioning system and automobile |
-
2020
- 2020-06-28 CN CN202010600072.5A patent/CN111486840A/en active Pending
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104081313A (en) * | 2011-11-11 | 2014-10-01 | 高通股份有限公司 | Sensor auto-calibration |
| CN104217245A (en) * | 2014-08-27 | 2014-12-17 | 高阳 | People stream trajectory tracking and area dwell time statistics method and system based on heterogeneous network |
| US20180188383A1 (en) * | 2017-01-04 | 2018-07-05 | Qualcomm Incorporated | Position-window extension for gnss and visual-inertial-odometry (vio) fusion |
| CN107656301A (en) * | 2017-09-20 | 2018-02-02 | 北京航天发射技术研究所 | A kind of vehicle positioning method based on Multi-source Information Fusion |
| CN109849976A (en) * | 2017-11-30 | 2019-06-07 | 河南星云慧通信技术有限公司 | A kind of train positioning system based on GPS, ODO and WKNN combination |
| CN108225302A (en) * | 2017-12-27 | 2018-06-29 | 中国矿业大学 | A kind of petrochemical factory's crusing robot alignment system and method |
| CN109737959A (en) * | 2019-03-20 | 2019-05-10 | 哈尔滨工程大学 | A kind of polar region Multi-source Information Fusion air navigation aid based on federated filter |
| CN110646825A (en) * | 2019-10-22 | 2020-01-03 | 北京新能源汽车技术创新中心有限公司 | Positioning method, positioning system and automobile |
Non-Patent Citations (1)
| Title |
|---|
| 尹露 等，: ""基于可信度的多源定位数据融合方法"", 《北京邮电大学学报》 * |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113295159A (en) * | 2021-05-14 | 2021-08-24 | 浙江商汤科技开发有限公司 | Positioning method and device for end cloud integration and computer readable storage medium |
| CN113295159B (en) * | 2021-05-14 | 2023-03-03 | 浙江商汤科技开发有限公司 | Positioning method and device for end cloud integration and computer readable storage medium |
| CN114111774A (en) * | 2021-12-06 | 2022-03-01 | 纵目科技（上海）股份有限公司 | Vehicle positioning method, system, device and computer readable storage medium |
| CN114111774B (en) * | 2021-12-06 | 2024-04-16 | 纵目科技（上海）股份有限公司 | Vehicle positioning method, system, equipment and computer readable storage medium |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107884800B (en) | Combined navigation data resolving method and device for observation time-lag system and navigation equipment | |
| EP2957928B1 (en) | Method for using partially occluded images for navigation and positioning | |
| CN111102978A (en) | Method and device for determining vehicle motion state and electronic equipment | |
| CN103512584A (en) | Navigation attitude information output method, device and strapdown navigation attitude reference system | |
| CN110160545B (en) | Enhanced positioning system and method for laser radar and GPS | |
| CN110440827B (en) | Parameter error calibration method and device and storage medium | |
| JP2017194460A (en) | Navigation system and method for error correction | |
| CN111486840A (en) | Robot positioning method and device, robot and readable storage medium | |
| CN114179825B (en) | Method for obtaining confidence of measurement value through multi-sensor fusion and automatic driving vehicle | |
| JP7356528B2 (en) | Map data processing method and device | |
| CN112781586A (en) | Pose data determination method and device, electronic equipment and vehicle | |
| CN112946681B (en) | Laser radar positioning method fusing combined navigation information | |
| CN112526573B (en) | Object positioning method and device, storage medium and electronic equipment | |
| CN114264301B (en) | Vehicle-mounted multi-sensor fusion positioning method, device, chip and terminal | |
| CN113252048B (en) | Navigation positioning method, navigation positioning system and computer readable storage medium | |
| CN114076959A (en) | Fault detection method, device and system | |
| CN113009816B (en) | Method and device for determining time synchronization error, storage medium and electronic device | |
| CN116399351A (en) | Vehicle position estimation method | |
| CN114915913A (en) | UWB-IMU combined indoor positioning method based on sliding window factor graph | |
| CN111121755A (en) | Multi-sensor fusion positioning method, device, equipment and storage medium | |
| CN111504311A (en) | Multi-sensor fusion real-time positioning navigation device and method | |
| JP2020107336A (en) | Method, device, and robot apparatus of improving robust property of visual inertial navigation system | |
| CN114897942B (en) | Point cloud map generation method and device and related storage medium | |
| CN114147717B (en) | Robot motion track estimation method, device, controller and storage medium | |
| CN114001730B (en) | Fusion positioning method, fusion positioning device, computer equipment and storage medium |
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 | ||
| RJ01 | Rejection of invention patent application after publication | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20200804 |