CN114296096A - Robot positioning method, system and terminal - Google Patents
Robot positioning method, system and terminal Download PDFInfo
- Publication number
- CN114296096A CN114296096A CN202111590975.0A CN202111590975A CN114296096A CN 114296096 A CN114296096 A CN 114296096A CN 202111590975 A CN202111590975 A CN 202111590975A CN 114296096 A CN114296096 A CN 114296096A
- Authority
- CN
- China
- Prior art keywords
- positioning result
- robot
- positioning
- map
- frequency
- 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
- 238000000034 method Methods 0.000 title claims abstract description 73
- 238000005259 measurement Methods 0.000 claims abstract description 46
- 238000012545 processing Methods 0.000 claims abstract description 11
- 239000011159 matrix material Substances 0.000 claims description 27
- 238000004422 calculation algorithm Methods 0.000 claims description 24
- 230000009466 transformation Effects 0.000 claims description 24
- 230000015654 memory Effects 0.000 claims description 16
- 238000004891 communication Methods 0.000 claims description 11
- 230000008569 process Effects 0.000 claims description 6
- 238000012937 correction Methods 0.000 claims description 3
- 230000004807 localization Effects 0.000 claims description 2
- 239000000126 substance Substances 0.000 claims description 2
- 230000001133 acceleration Effects 0.000 description 8
- 238000004364 calculation method Methods 0.000 description 6
- 238000010586 diagram Methods 0.000 description 6
- 230000007613 environmental effect Effects 0.000 description 6
- 238000013519 translation Methods 0.000 description 6
- 230000008859 change Effects 0.000 description 5
- 230000006870 function Effects 0.000 description 5
- 230000009286 beneficial effect Effects 0.000 description 3
- 238000010276 construction Methods 0.000 description 3
- 230000004927 fusion Effects 0.000 description 3
- 238000013507 mapping Methods 0.000 description 3
- 238000005457 optimization Methods 0.000 description 3
- 230000002159 abnormal effect Effects 0.000 description 2
- 238000006073 displacement reaction Methods 0.000 description 2
- 238000004590 computer program Methods 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000010295 mobile communication Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Traffic Control Systems (AREA)
- Navigation (AREA)
Abstract
The application discloses a robot positioning method, a system and a terminal, wherein the robot positioning method comprises the following steps: acquiring laser radar data, constructing a front-end local map according to the laser radar data, and outputting a first positioning result of the robot under the front-end local map at a first frequency; acquiring a prior global map, and matching the prior global map according to laser radar data to obtain a rear end positioning result of the robot under the prior global map; correcting the first positioning result through the rear end positioning result at a second frequency so as to obtain and output a second positioning result; and processing the second positioning result according to the acquired data of the inertial measurement unit, thereby acquiring and outputting a third positioning result at a third frequency. The method and the device perform positioning based on the local map, on one hand, the error of local positioning is corrected through global positioning, the positioning accuracy is improved, on the other hand, interpolation is performed on an output result, the output frequency is improved, and the higher control requirement is met.
Description
Technical Field
The present invention relates to the field of robot positioning, and in particular, to a robot positioning method, system and terminal.
Background
The existing positioning scheme of the outdoor mobile robot in the park mainly depends on sensors such as a multi-line laser radar, an Inertial Measurement Unit (IMU), a wheel type odometer and a Global Positioning System (GPS), under the condition of a known prior map, the original data of the IMU and the wheel type odometer are analyzed to be used as predicted values, and the mode of providing observed values by matching 3D laser point cloud data is fused to obtain a more accurate positioning result.
The scheme can be normally used under the conditions that sensor data are not abnormal and the existing environmental information and the prior map are not obviously changed, but cannot be used when a scene with large environmental change is encountered. For example, if a priori map is generated in the case of an open parking area and then is located based on the map, then if the vehicle moves to a parking area where a large number of vehicles are parked again, the map environment changes, and the matching of the map based on the situation inevitably reduces the accuracy of location, even obtains a completely wrong result. Meanwhile, the data volume of one frame of laser is large, the speed of resolving point cloud matching is slow, and the actually output positioning frequency cannot reach the frequency required by motion control, so that abnormal planning occurs.
Therefore, the inventor finds out in the invention creation process that a method is needed to be provided, the accuracy of outdoor mobile robot positioning is improved, and the positioning frequency meeting the control requirement is output.
Disclosure of Invention
The application provides a robot positioning method, a robot positioning system and a robot positioning terminal, which can improve the accuracy of robot positioning.
The embodiment of the invention provides the following technical scheme:
in a first aspect, an embodiment of the present invention provides a robot positioning method, which is applied to a terminal, where the terminal is in communication connection with a robot, and the method includes:
acquiring laser radar data, constructing a front-end local map according to the laser radar data, and outputting a first positioning result of the robot under the front-end local map at a first frequency;
acquiring a prior global map, and matching the prior global map according to laser radar data to obtain a rear end positioning result of the robot under the prior global map;
correcting the first positioning result through the rear end positioning result at a second frequency so as to obtain and output a second positioning result;
and processing the second positioning result according to the acquired data of the inertial measurement unit, thereby acquiring and outputting a third positioning result at a third frequency.
In some embodiments, constructing a front-end local map from the lidar data, outputting a first positioning result of the robot under the front-end local map at a first frequency, comprising:
constructing a front-end local map according to the laser radar data;
and performing laser interframe matching on the laser radar data of each frame and the front-end local map to obtain a first positioning result of the robot under the front-end local map, and outputting the first positioning result of the robot under the front-end local map at a first frequency.
In some embodiments, the method further comprises:
the front-end local map is maintained at a first number of keyframes, and when a new keyframe is collected for input, the oldest keyframe is deleted from the front-end local map.
In some embodiments, matching the prior global map according to the lidar data to obtain a rear-end positioning result of the robot under the prior global map includes:
and matching the laser radar data with the prior global map through a first algorithm to obtain a rear-end positioning result of the robot under the prior global map.
In some embodiments, defining: the origin of coordinates of the front-end local map is from, the origin of coordinates of the prior global map is map, and the position of the robot is base;
correcting the first positioning result by the back-end positioning result at a second frequency to obtain and output a second positioning result, comprising:
determining a first transformation matrix according to the rear-end positioning result and the first positioning result, correcting the first positioning result through the first transformation matrix at a second frequency to enable the corrected first positioning result to be equal to the rear-end positioning result, and outputting the corrected first positioning result as an output second positioning result;
the formula for the correction process is as follows:
Tmap odom*Todom base＝Tmap base
wherein, Tmap odomIs a first transformation matrix, Todom baseFor the first positioning result, T, of the robot under the front-end local mapmap baseAnd positioning a result for the rear end of the robot under the prior global map.
In some embodiments, processing the second positioning result according to the obtained inertial measurement unit data, so as to obtain and output a third positioning result at a third frequency, includes:
acquiring data of an inertial measurement unit;
and resolving the mileage of the robot according to the acquired data of the inertia measurement unit, and interpolating a positioning result corresponding to the mileage to a second positioning result so as to acquire a third positioning result output in a third frequency.
In some embodiments, the first algorithm includes a normal distribution transformation algorithm and a closest point iteration algorithm.
In some embodiments, the first frequency is less than or equal to a data acquisition frequency of the lidar, the second frequency is less than the first frequency, and the third frequency is greater than the first frequency and less than or equal to a data acquisition frequency of the inertial measurement unit.
In a second aspect, an embodiment of the present invention provides a terminal, where the terminal includes:
at least one processor; and the number of the first and second groups,
a memory communicatively coupled to the at least one processor; wherein the content of the first and second substances,
the memory stores instructions executable by the at least one processor to enable the at least one processor to perform a robot positioning method as in the first aspect.
In a third aspect, an embodiment of the present invention provides a robot positioning system, where the system includes:
a terminal as in the first aspect;
and the robot is in communication connection with the terminal and is used for transmitting data collected by the laser radar and the inertia measurement unit carried by the robot to the terminal.
In a fourth aspect, an embodiment of the present invention provides a non-transitory computer-readable storage medium, which stores computer-executable instructions for causing a terminal to perform the robot positioning method according to the first aspect.
The embodiment of the invention has the beneficial effects that: different from the prior art, an embodiment of the present invention provides a robot positioning method, which is applied to a terminal, where the terminal is in communication connection with a robot, and the method includes: acquiring laser radar data, constructing a front-end local map according to the laser radar data, and outputting a first positioning result of the robot under the front-end local map at a first frequency; acquiring a prior global map, and matching the prior global map according to laser radar data to obtain a rear end positioning result of the robot under the prior global map; correcting the first positioning result through the rear end positioning result at a second frequency so as to obtain and output a second positioning result; and processing the second positioning result according to the acquired data of the inertial measurement unit, thereby acquiring and outputting a third positioning result at a third frequency. The method and the device perform positioning based on the local map, on one hand, the error of local positioning is corrected through global positioning, the positioning accuracy is improved, on the other hand, interpolation is performed on an output result, the output frequency is improved, and the higher control requirement is met. The method and the device perform positioning based on the local map, on the first aspect, errors of local positioning are corrected through global positioning, positioning accuracy is improved, on the second aspect, interpolation is performed on output results, output frequency is improved, and therefore higher control requirements are met.
Drawings
One or more embodiments are illustrated by way of example in the accompanying drawings, which correspond to the figures in which like reference numerals refer to similar elements and which are not to scale unless otherwise specified.
Fig. 1 is a schematic view of an application scenario of a robot positioning method according to an embodiment of the present invention;
fig. 2 is a schematic flow chart of a robot positioning method according to an embodiment of the present invention;
FIG. 3 is a detailed flowchart of step S101 in FIG. 2;
FIG. 4 is a schematic interpolation diagram of an inertial measurement unit of a robot positioning method according to an embodiment of the present invention;
fig. 5 is a detailed flowchart of a robot positioning method according to an embodiment of the present invention;
fig. 6 is a schematic structural diagram of a terminal according to an embodiment of the present invention;
fig. 7 is a schematic structural diagram of a robot positioning system according to an embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are some, but not all, embodiments of the present invention. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
Referring to fig. 1, fig. 1 is a schematic view of an application scenario of a robot positioning method according to an embodiment of the present invention;
as shown in fig. 1, the application scenario includes a robot 10 and a terminal 20, the robot 10 is in communication connection with the terminal 20, and the terminal 20 is disposed on the robot 10;
the robot 10 may be a mobile service robot. The terminal 20 is a computer device having data processing capabilities. Among them, the robot 10 is mounted with sensors and transmits sensor data to the terminal 20. Wherein, the robot is provided with a laser radar and an inertia measurement unit, the laser radar comprises a single line laser radar and a multi-line laser radar, the multi-line laser radar is mainly divided into 4 lines, 8 lines, 16 lines, 32 lines and 64 lines, the line of the laser radar is a transmitter and a receiver which are arranged in the vertical direction, through the rotation of a motor, a plurality of wire bundles can be obtained, the more the number of wires is, the more perfect the surface profile of the object is, of course, the larger the amount of data processed, the higher the requirement on hardware, some of the existing multiline laser radars have built-in instant positioning and mapping (SLAM) functions, and object information acquired by the laser radars presents a series of dispersed points with accurate angle and distance information, which are called point clouds, map construction can be performed through point cloud data acquired by a laser radar, and a 16-line laser radar is preferred in the embodiment of the invention; an inertial measurement unit is a device that measures the three-axis angular velocity (or angular rate) and acceleration of an object. Generally, an IMU includes three single-axis accelerometers and three single-axis gyroscopes, the accelerometers detect acceleration signals of an object in three independent axes of a carrier coordinate system, and the gyroscopes detect angular velocity signals of the carrier relative to a navigation coordinate system, and measure angular velocity and acceleration of the object in three-dimensional space, and then solve the attitude of the object.
In the embodiment of the present application, the application environment of the robot positioning includes but is not limited to: campus environment, school environment, open road, etc.
Referring to fig. 2, fig. 2 is a schematic flow chart of a robot positioning method according to an embodiment of the present invention;
as shown in fig. 2, the robot positioning method is applied to a terminal, and the terminal is in communication connection with a robot, and the method includes:
step S101: acquiring laser radar data, constructing a front-end local map according to the laser radar data, and outputting a first positioning result of the robot under the front-end local map at a first frequency;
further, please refer to fig. 3, fig. 3 is a detailed flowchart of step S101 in fig. 2;
as shown in fig. 3, the step S101: the method comprises the steps of obtaining laser radar data, constructing a front-end local map according to the laser radar data, and outputting a first positioning result of the robot under the front-end local map at a first frequency, wherein the method comprises the following steps:
step S1011: acquiring laser radar data, and constructing a front-end local map according to the laser radar data;
specifically, the robot is provided with a laser radar and an inertia measurement unit, and can acquire laser radar data and inertia measurement unit data and transmit the laser radar data and the inertia measurement unit data to the terminal. Specifically, the robot transmits the data collected by the laser radar to the terminal, and the terminal can automatically construct a front-end local map within a certain range of the robot according to the data collected by the laser radar and the SLAM system matched with the laser radar.
Further, the method further comprises:
the front-end local map is maintained at a first number of keyframes, and when a new keyframe is collected for input, the oldest keyframe is deleted from the front-end local map.
Specifically, positioning of a front-end laser odometer is firstly carried out to obtain an initial pose of the robot, then a front-end local map of the positioning is constructed, the coordinate origin of the front-end local map is coincident with the coordinate origin of a prior global map, the size of the front-end local map is maintained at a certain number of key frames, when a new key frame is added, the earliest key frame is deleted from the front-end local map, and therefore the front-end local map always keeps dynamic updating in a sliding window mode.
In the embodiment of the invention, the front-end local map is kept to be dynamically updated in a sliding window mode all the time, the first aspect is that the range of the point cloud data acquired by the laser radar is limited, and the second aspect is that the size of the local map is artificially controlled, so that the calculation force required by the terminal is reduced.
Step S1012: carrying out laser interframe matching on the laser radar data of each frame and the front-end local map to obtain a first positioning result of the robot under the front-end local map;
specifically, a first positioning result of the robot under the front-end local map can be obtained by the laser odometer according to two adjacent frames of laser data, and it can be understood that the front-end local map is formed by point cloud data acquired by the laser radar, performing laser inter-frame matching by the laser odometer refers to calculating a feature matching and motion relation between a reference frame and a current frame to obtain a pose of the robot, where the pose of the robot is the first positioning result, and setting the current frame as a new reference frame after calculation, where the current frame refers to laser point cloud data newly acquired by the laser radar, and the reference frame is previous frame of laser point cloud data of the current frame under the front-end local map, that is, the frame of laser point cloud data acquired before the current frame.
Step S1013: outputting a first positioning result of the robot under the front-end local map at a first frequency;
specifically, because the frequency of data acquisition by the laser radar is limited, and because the data volume of the point cloud data is large, the point cloud matching is required after the data is transmitted to the terminal, the terminal can only output the first positioning result at the first frequency, the first frequency is less than or equal to the data acquisition frequency of the laser radar, for example, the data acquisition frequency of the laser radar is 10Hz, and if the time consumed by the point cloud matching is 0, the first frequency can only be 10Hz at the maximum under an ideal condition.
It can be understood that the front end in the front-end local map is a proper noun and is not an actual object, in the embodiment of the invention, the positioning of the robot under the front-end local map is performed at the front end, the local map is constructed and updated, and the positioning of the robot under the front-end local map refers to estimating the pose change of the robot between two adjacent frames of laser through a laser odometer to obtain the positioning of the robot under the front-end local map; and receiving the pose on the front-end local map measured by the laser odometer at different moments at the rear end, and optimizing the received pose on the front-end local map according to the pose on the rear-end global prior map to obtain the track of the robot on the rear-end global prior map.
In the embodiment of the invention, the laser odometer is used for solving the pose transformation of the robot between two frames of laser point clouds by matching the two adjacent frames of laser point clouds, namely laser frame-to-frame matching, so as to obtain a first positioning result of the robot under a front-end local map and a global positioning result under a prior global map, namely a rear-end positioning result. The point cloud matching algorithm adopted by the laser odometer comprises an Iterative Closest Points (ICP), a Normal distribution transformation algorithm (NDT) and a Lidar mileage measurement and Mapping in Real-time Mapping (LOAM) algorithm based on feature Points.
The positioning is carried out through the front-end local map, a reliable positioning result can be obtained under the condition of large environmental change, and the influence of environmental change is avoided as the positioning is carried out only through the prior global map, but the front-end local map generates errors through the positioning of the laser odometer.
Step S102: acquiring a prior global map, and matching the prior global map according to laser radar data to obtain a rear end positioning result of the robot under the prior global map;
specifically, regarding the acquisition of the prior global map, global map information is acquired in advance through a laser radar of the robot, the prior global map is constructed, for example, if the prior global map is a parking lot map, the robot is controlled to go to a parking lot, map data is acquired through a sensor such as the laser radar, and the prior global map is constructed, further, when the positioning based on the prior global map is deviated due to environmental changes, for example, the prior global map is generated under the condition that the parking lot is empty, and then the positioning based on the prior global map is performed, the robot is assumed to move to the parking lot again, but when a lot of vehicles are parked in the parking lot, because the environmental data acquired by the robot is changed when no vehicle is parked, the matching accuracy based on the prior global map is low at this time, and even a large positioning deviation occurs, therefore, the invention combines the prior global map with the front-end local map to improve the accuracy of positioning.
Further, matching a prior global map according to the laser radar data to obtain a rear end positioning result of the robot under the prior global map, which comprises the following steps:
and matching the laser radar data with the prior global map through a first algorithm to obtain a rear-end positioning result of the robot under the prior global map.
Specifically, in the embodiment of the invention, the laser odometer obtains the pose transformation of the robot between two frames of laser point clouds by matching the two adjacent frames of laser point clouds (namely, laser inter-frame matching) acquired by the laser radar, so as to obtain the global positioning result of the robot under the prior global map, namely the rear-end positioning result. The point cloud matching algorithm (i.e. the first algorithm) adopted by the laser odometer mainly includes an Iterative Closest Point (ICP), a Normal distribution transformation algorithm (NDT), and a feature point-based load in Real-time (Lidar) algorithm.
In the embodiment of the invention, the ICP algorithm is taken as an example, and the basic principle of the ICP algorithm is as follows: and respectively finding the nearest point pairs (pi, qi) in the laser point cloud data P acquired by the laser radar point cloud and the point cloud data Q of the prior global map, and then calculating optimal matching parameters R and t to ensure that the error function is minimum. The error function E (R, t) may be:
wherein n is the number of the nearest point pairs (pi, qi), pi is one point in P, qi is the nearest point corresponding to pi in Q, R is a rotation matrix, and t is a translation matrix.
The ICP algorithm comprises the following steps:
the method comprises the steps of assuming that laser point cloud data to be matched obtained by a laser radar is a point cloud P to be matched, and point cloud data in a prior global map is a source point cloud Q;
(1) extracting a point set pi from a point cloud P to be matched, wherein the point set pi belongs to the P;
(2) finding out a corresponding point set qi in the source point cloud Q, wherein qi belongs to Q, and min is obtained from qi to pi;
(3) calculating a rotation matrix R and a translation matrix t so that an error function E (R, t) is minimum;
(4) carrying out rotation and translation transformation on pi by using the rotation matrix R and the translation matrix t obtained in the previous step to obtain a new corresponding point set pi '((pi' ═ Rpi + t, pi belongs to P };
(5) calculating the average distance d between pi' and the corresponding point set qi;
(6) and if d is smaller than the first numerical threshold or larger than the preset maximum iteration times, stopping iterative computation to obtain the optimal transformation matrix. Otherwise, returning to the step 2, and taking pi' as a new pi until the convergence condition is met. The first numerical threshold and the maximum iteration number can be set according to needs.
Step S103: correcting the first positioning result through the rear end positioning result at a second frequency so as to obtain and output a second positioning result;
specifically, defining the coordinate origin of the robot under the front-end local map as odom, the coordinate origin under the prior-to global map as map, and the location of the robot as base, it is obvious that two points of the coordinate origin of the robot under the front-end local map and the coordinate origin of the robot under the prior-to global map are completely coincident when positioning is started, but as the error of the laser odometer is larger and larger (the laser odometer only estimates the pose change of the robot through two frames of laser data, so there is an error), at this time, the odom deviates from the map (the origin map always keeps still), so at this time, the error from the odom to the map is corrected through global matching, that is, a first transformation matrix is determined according to the rear-end positioning result and the first positioning result, and the first positioning result is corrected through the first transformation matrix at a second frequency, the corrected first positioning result is equal to the rear end positioning result, and the corrected first positioning result is used as a second positioning result and is output;
the formula for the correction process is as follows:
Tmap odom*Todom base＝Tmap base
wherein, Tmap odomIs a first transformation matrix, Todom baseFor the first positioning result, T, of the robot under the front-end local mapmap baseFor robot backend localization under prior global mapAnd (6) obtaining the result.
Tmap baseThe first transformation matrix may be any form, but is generally 4 × 4 in three-dimensional computer graphics, and in the embodiment of the present invention, the transformation matrix of 4 × 4 is also preferred, where the transformation matrix of 4 × 4 includes a rotation matrix of 3 × 3 and a translation matrix of 3 × 1, and the first transformation matrix may compare the pose of the robot in the first positioning result with the pose of the robot in the back-end positioning result, and is determined according to the deviation between the two poses, that is, according to the deviation between the first pose of the robot in the first positioning result and the second pose of the robot in the back-end positioning result, the first transformation matrix is determined, for example, the odem is translated by (1, 0, 0) in the map coordinate system, and then the translation matrix in the first transformation matrix is [ -1; 0; 0]。
Specifically, the front end positioning T of the robot under a front end local map is obtained through the laser odometerodom baseNamely, the first positioning result and the rear end positioning T of the robot under the prior global mapmap baseNamely, a rear-end positioning result, when the laser odometer has no error, the first positioning result is the same as the rear-end positioning result, but because the origin of coordinates of the robot under the front-end local map is odom, the robot can generate offset due to the error generated by the laser odometer, the first positioning result is not equal to the rear-end positioning result, the first positioning result and the first transformation matrix are subjected to dot multiplication, the first transformation matrix and the first positioning result are subjected to dot multiplication to enable the first positioning result to be equal to the rear-end positioning result, the offset origin of coordinates of the front-end local map is superposed with the origin of coordinates of the prior global map again, the corrected first positioning result is determined as a second positioning result, and the second positioning result is output; it should be noted that the second frequency of the first result corrected by the back-end positioning result is less than the first frequency, and if the second frequency is one tenth of the first frequency, only one first positioning result is corrected every ten first positioning results are output, which is called as a fourth positioning result, that is, 10 output second positioning results, including 9 first positioning results and 1 fourth positioning result.
If the positioning is carried out only by depending on the front-end local map without adding loop and more constraint information, a larger positioning error is introduced for a long time, so that the positioning error of the front-end local map is corrected by a rear-end positioning result at intervals.
However, since the frequency of the first positioning result obtained by correcting the front-end local map with the back-end positioning result cannot be equal to the first frequency, otherwise, the positioning is completely dependent on the prior global map, the second frequency obtained by correcting the first positioning result of the front-end local map with the back-end positioning result is less than the first frequency, in the embodiment of the present invention, the first frequency is preferably 10Hz, and the second frequency is 0.1Hz, that is, every 99 first positioning results are output, the 100 th first positioning result is corrected by the back-end positioning result, and in this way, the second positioning result is output.
The laser odometer matches data acquired by a laser radar with a global prior map through a point cloud matching algorithm to obtain the position of the current robot under the prior global map, and the error generated by the front-end local map can be eliminated to a certain extent by correcting the first positioning result by using the rear-end positioning result through the method.
Step S104: processing the second positioning result according to the acquired data of the inertial measurement unit, thereby acquiring and outputting a third positioning result at a third frequency;
specifically, the robot is provided with an inertia measurement unit, can acquire data of the inertia measurement unit and transmit the data to the terminal, can calculate the mileage of the robot according to the data of the inertia measurement unit, and interpolates a positioning result corresponding to the mileage into a second positioning result, so as to obtain and output a third positioning result at a third frequency.
Specifically, the inertial measurement unit data may provide the acceleration and angular velocity of the robot in the three-axis direction under the inertial measurement unit coordinate system, so the robot mileage may be calculated, and the formula is as follows:
θ＝w*t；
v＝v0+a*t；
s＝v*t+1/2*a*t^2；
wherein w is the angular velocity in the three-axis direction, t is the time interval between two times of data acquisition by the inertial measurement unit (i.e. the data acquisition frequency of the inertial measurement unit), θ is the angle of the robot deflected on the axis corresponding to w, and the current attitude, v, of the robot is obtained0The initial velocity of the robot is a, the acceleration in the three-axis direction is a, the current velocity of the robot is v, the displacement of the robot in the inertial measurement unit coordinate system is s, and the displacement endpoint is the positioning of the robot in the inertial measurement unit coordinate system, namely the position of the robot. Therefore, the position and the attitude of the robot under the coordinate system of the inertial measurement unit are obtained through the formula, namely, the positioning result of the robot under the coordinate system of the inertial measurement unit is obtained through the mileage of the robot under the coordinate system (IMU coordinate system) of the inertial measurement unit.
Next, interpolating the result obtained by the IMU calculation between two frames of the second positioning result, and outputting the third positioning result at a third frequency, theoretically, even if the second positioning result keeps a very low frequency output, a high frequency output can be kept through the IMU interpolation, wherein the data acquisition frequency of the IMU can reach 100Hz, it can be understood that the third frequency is usually smaller than the data acquisition frequency of the IMU, in the embodiment of the present invention, the positioning output frequency of the front-end local map is preferably 10Hz, the frequency of the rear-end positioning result correcting the first positioning result is preferably 0.1Hz, and through the IMU interpolation, preferably, 8 IMU positioning results are inserted between every two frames of the second positioning result, so that the output frequency of the third positioning result reaches 100 Hz.
Further, the method further comprises:
and calculating the variation of the pose of the robot between two adjacent mileages, and performing interpolation according to the second positioning result and the variation.
Specifically, the output of the second positioning result is actually the pose of the robot, and assuming that the output is the first pose, the pose variation of the pose of the robot between two adjacent mileages is calculated, the pose variation obtained by the mileage solution is added to the first pose, so as to generate a new pose of the robot, and the new pose of the robot is interpolated between two frames of the second positioning result.
The problem of positioning output frequency can be solved by adding an IMU interpolation module, the data acquisition frequency of most of laser radars is considered to be 10Hz, meanwhile, certain time consumption is needed for point cloud calculation after data are transmitted to a terminal, therefore, the positioning frequency based on the laser radars is inevitably less than 10Hz, the positioning frequency actually meeting control needs to be kept above 20Hz, and therefore, the IMU interpolation module is added, and the control requirement of the robot can be better met.
Referring to fig. 4 again, fig. 4 is an interpolation schematic diagram of an inertial measurement unit of a robot positioning method according to an embodiment of the present invention;
as shown in fig. 4, fig. 4 includes second positioning results 41(LIDAR) and positioning results 42 of Inertial Measurement Units (IMUs), assuming that the output frequency of the second positioning results 41 is 10Hz, the positioning results 42 of 8 IMUs are interpolated between every two frames of the second positioning results 41, the output frequency of the output third positioning result reaches 100Hz, and the purpose of increasing the output frequency is to better control the robot, for example, the current speed of the robot is 10m/s, if the robot is positioned only by the output frequency of 10Hz, the robot updates positioning once every 1m advance, and has a large lag, which is not beneficial to the operation of avoiding obstacles of the robot, but after interpolation is performed by using the IMUs, positioning is output at the frequency of 100Hz, and the robot outputs positioning of the robot in turn every 10cm advance, thereby increasing the information acquisition speed, to better manipulate the robot.
Referring to fig. 5, fig. 5 is a detailed flowchart illustrating a robot positioning method according to an embodiment of the present invention;
as shown in fig. 5, the robot positioning method is applied to a terminal, and the terminal is in communication connection with a robot, and the method includes:
(1) and acquiring point cloud data of the surrounding environment of the robot through a laser radar.
(2) Constructing a local point cloud map and positioning a laser odometer at the front end;
the construction of the local point cloud map refers to the construction of a front-end local map by means of point cloud data of the surrounding environment of the robot acquired by the laser radar, the positioning of the laser odometer refers to the obtaining of a first positioning result of the robot under the front-end local map by means of the laser odometer based on the point cloud data acquired by the laser radar, and the first positioning result is output at the frequency of 10Hz, the specific steps are the same as S102, and the description is omitted.
(3) Positioning and global matching optimization of a prior global map are carried out at the back end;
the prior global map refers to a prior global map in a target area where the robot moves, the robot can be controlled to go to the target area, point cloud data are obtained through a laser radar, the prior global map is built in advance, the prior global map is located, the rear end locating result of the robot under the prior global map is obtained through a laser odometer based on the point cloud data obtained through the laser radar, the global matching optimization refers to that the first locating result of the robot under the front end local map is corrected through the rear end locating result at the frequency of 0.1Hz, the second locating result is output, the first locating result is output through the laser odometer, the corrected first locating result, namely the second locating result is output through the laser odometer, and the specific steps are not repeated as in S103.
(4) Obtaining measurement data of the IMU and carrying out IMU mileage calculation;
the IMU measurement data acquisition refers to acquiring angular velocity and acceleration of the robot in three axis directions through the IMU, the IMU mileage calculation refers to calculating the mileage of the robot in an IMU coordinate system through the angular velocity and acceleration of the robot in the three axis directions acquired through the IMU, and interpolating positioning results corresponding to the mileage into second positioning results, in the embodiment of the invention, 8 IMU positioning results are inserted between every two frames of second positioning results to achieve 100Hz frequency output, the specific steps are the same as S104, and are not described herein again.
(5) High-frequency positioning output;
in the embodiment of the present invention, the first frequency is 10Hz, the second frequency is 0.1Hz, and the third frequency is 100Hz, that is, the first positioning result of the front-end local map is corrected once every 10s by using the rear-end positioning result of the rear-end prior global map (i.e., global matching optimization), the second positioning result is output, and 2 second positioning results and 8 IMU interpolations are output every 0.1s (i.e., the third positioning result is output at a frequency of 100 Hz).
According to the method, a front-end local map is established in front-end positioning, a point cloud matching algorithm of laser radar and IMU fusion is carried out on the front-end local map, a positioning algorithm of laser point cloud matching including ICP, NDT or the combination of ICP and NDT is adopted on the basis of a prior global map in rear-end positioning, and a final positioning result is generated by front-end high-frequency positioning and rear-end low-frequency positioning phase fusion. The method and the device are not limited to the garden environment, and can normally operate on simple open roads.
Referring to fig. 6, fig. 6 is a schematic structural diagram of a terminal of a robot positioning method according to an embodiment of the present invention;
as shown in fig. 6, the terminal 60 includes: one or more processors 61 and a memory 62, with one processor 61 being an example in fig. 6.
The processor 61 and the memory 62 may be connected by a bus or other means, such as the bus connection in fig. 6.
The processor 61 is used for acquiring laser radar data, constructing a front-end local map according to the laser radar data, and outputting a first positioning result of the robot under the front-end local map at a first frequency; acquiring a prior global map, and matching the prior global map according to laser radar data to obtain a rear end positioning result of the robot under the prior global map; correcting the first positioning result through the rear end positioning result at a second frequency so as to obtain and output a second positioning result; and processing the second positioning result according to the acquired data of the inertial measurement unit, thereby acquiring and outputting a third positioning result at a third frequency. The method and the device perform positioning based on the local map, on one hand, the error of local positioning is corrected through global positioning, the positioning accuracy is improved, on the other hand, interpolation is performed on an output result, the output frequency is improved, and the higher control requirement is met. The method and the device perform positioning based on the local map, on one hand, the error of local positioning is corrected through global positioning, the positioning accuracy is improved, on the other hand, interpolation is performed on an output result, the output frequency is improved, and the higher control requirement is met.
The memory 62, which is a non-volatile computer-readable storage medium, may be used to store non-volatile software programs, non-volatile computer-executable programs, and modules, such as program instructions/modules of the robot positioning method in the embodiments of the present application. The processor 61 executes various functional applications of the terminal and data processing by running nonvolatile software programs, instructions, and modules stored in the memory 62, that is, implements the robot positioning method of the above-described method embodiment.
The memory 62 may include a storage program area and a storage data area, wherein the storage program area may store an operating system, an application program required for at least one function; the storage data area may store data created according to the use of the terminal, and the like. Further, the memory 62 may include high speed random access memory, and may also include non-volatile memory, such as at least one magnetic disk storage device, flash memory device, or other non-volatile solid state storage device. In some embodiments, the memory 62 optionally includes memory located remotely from the processor 61, and these remote memories may be connected to the controller via a network. Examples of such networks include, but are not limited to, the internet, intranets, local area networks, mobile communication networks, and combinations thereof.
One or more modules are stored in the memory 62, which when executed by the one or more processors 61, perform the robot positioning method in any of the above-described method embodiments, e.g. performing the above-described method steps S101 to S104 in fig. 2.
It should be noted that the product can execute the method provided by the embodiment of the present application, and has corresponding functional modules and beneficial effects for executing the method. For technical details that are not described in detail in this embodiment, reference may be made to the methods provided in the embodiments of the present application.
Embodiments of the present application provide a non-transitory computer-readable storage medium storing computer-executable instructions, which are executed by one or more processors, such as the processor 61 in fig. 6, to enable the one or more processors to perform the robot positioning method in any of the above method embodiments, and perform the above-described method steps S101 to S104 in fig. 2.
Referring to fig. 7, fig. 7 is a schematic structural diagram of a robot positioning system according to an embodiment of the present invention.
As shown in fig. 7, the robot positioning system 70 includes a robot 71 and a terminal 72, where the robot 71 is in communication connection with the terminal 72, the terminal 72 is disposed on the robot 71, and the robot 71 is configured to transmit data collected by a laser radar and an inertial measurement unit mounted on the robot 72 to the terminal 72.
The robot 71 is connected with a terminal 72 in communication, and the robot 71 may be a mobile service robot. Among them, the robot 71 is mounted with a laser radar for acquiring point cloud data of the environment around the outdoor mobile robot and an inertia measurement unit for acquiring acceleration and angular velocity in three axial directions of the outdoor mobile robot. In the embodiment of the present invention, the robot 71 acquires the lidar data and the inertial measurement unit data and then sends the lidar data and the inertial measurement unit data to the terminal 72.
The terminal 72 is connected to the robot 71 in communication, the terminal 72 is disposed in the robot 71, and the terminal 72 is a computer device having a data processing capability. The terminal 72 receives sensor data (including laser radar data and inertial measurement unit data) transmitted by the robot 71, generates a front-end local map, outputs a first positioning result of the robot 71 under the front-end local map at a first frequency, corrects the first positioning result of the robot 71 under the front-end local map by using a rear-end positioning result of the robot 71 under a first-check global map at a second frequency through a prior global map obtained in advance and stored in the terminal 72, outputs a second positioning result, and finally calculates mileage through inertial measurement unit data, interpolates the positioning result corresponding to the mileage between the second positioning results, and generates a third positioning result.
The method comprises the steps of constructing a front-end local map through front-end positioning, carrying out a matching algorithm of laser radar and IMU fusion on the front-end local map, adopting a positioning algorithm of laser point cloud matching including ICP, NDT or the combination of ICP and NDT on the basis of a prior global map for rear-end positioning, fusing front-end high-frequency positioning and rear-end low-frequency positioning phases to generate a final positioning result, and is a low-cost and high-efficiency positioning method. The method and the device are not limited to the garden environment, and can normally operate on simple open roads.
Finally, it should be noted that: the above examples are only intended to illustrate the technical solution of the present invention, but not to limit it; within the idea of the invention, also technical features in the above embodiments or in different embodiments may be combined, steps may be implemented in any order, and there are many other variations of the different aspects of the invention as above, which are not provided in detail for the sake of brevity; although the present invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some technical features may be equivalently replaced; and the modifications or the substitutions do not make the essence of the corresponding technical solutions depart from the scope of the technical solutions of the embodiments of the present invention. Through the above description of the embodiments, those skilled in the art will clearly understand that the embodiments may be implemented by software plus a general hardware platform, and may also be implemented by hardware. It will be understood by those skilled in the art that all or part of the processes of the methods of the embodiments described above can be implemented by a computer program that can be stored in a computer-readable storage medium and that when executed, can include processes of the embodiments of the methods described above. The storage medium may be a magnetic disk, an optical disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), or the like.
Claims (10)
1. A robot positioning method is applied to a terminal, wherein the terminal is in communication connection with a robot, and the method comprises the following steps:
acquiring laser radar data, constructing a front-end local map according to the laser radar data, and outputting a first positioning result of the robot under the front-end local map at a first frequency;
acquiring a prior global map, and matching the prior global map according to the laser radar data to obtain a rear end positioning result of the robot under the prior global map;
correcting the first positioning result through the rear end positioning result at a second frequency so as to obtain and output a second positioning result;
and processing the second positioning result according to the acquired data of the inertial measurement unit, so as to obtain and output a third positioning result at a third frequency.
2. The method of claim 1, wherein constructing a front-end local map from the lidar data and outputting a first positioning result of the robot under the front-end local map at a first frequency comprises:
constructing the front-end local map according to the laser radar data;
and performing laser interframe matching on the laser radar data and the front-end local map to obtain the first positioning result of the robot under the front-end local map, and outputting the first positioning result of the robot under the front-end local map at the first frequency.
3. The method of claim 2, further comprising:
the front-end local map is maintained at a first number of key frames, and when a new key frame is collected, the earliest key frame is deleted from the front-end local map.
4. The method of claim 1, wherein the matching the prior global map according to the lidar data to obtain a back-end localization result of the robot under the prior global map comprises:
and matching the laser radar data with the prior global map through a first algorithm to obtain the rear end positioning result of the robot under the prior global map.
5. The method according to claim 1, characterized in that: the origin of coordinates of the front-end local map is from, the origin of coordinates of the prior global map is map, and the position of the robot is base;
the correcting the first positioning result through the back end positioning result at the second frequency to obtain and output a second positioning result, including:
determining a first transformation matrix according to the rear-end positioning result and the first positioning result, correcting the first positioning result through the first transformation matrix at a second frequency to enable the corrected first positioning result to be equal to the rear-end positioning result, and taking the corrected first positioning result as the second positioning result and outputting the second positioning result;
the formula for the correction process is as follows:
Tmap odom*Todom base＝Tmap base
wherein, Tmap odomFor said first transformation matrix, Tmap baseIs the first positioning result of the robot under the front-end local map, Tmap baseIs the robot atAnd the rear end positioning result under the prior global map.
6. The method according to claim 1, wherein the processing the second positioning result according to the obtained inertial measurement unit data to obtain and output a third positioning result at a third frequency comprises:
acquiring the inertial measurement unit data;
and calculating the mileage of the robot according to the acquired data of the inertial measurement unit, and interpolating a positioning result corresponding to the mileage to the second positioning result, so as to obtain and output a third positioning result at a third frequency.
7. The method of claim 4, wherein the first algorithm comprises a normal distribution transform algorithm and a closest point iteration algorithm.
8. The method of any of claims 1-7, wherein the first frequency is less than or equal to a lidar data acquisition frequency, the second frequency is less than the first frequency, and the third frequency is greater than the first frequency and less than or equal to an inertial measurement unit data acquisition frequency.
9. A terminal, characterized in that the terminal comprises:
at least one processor; and the number of the first and second groups,
a memory communicatively coupled to the at least one processor; wherein the content of the first and second substances,
the memory stores instructions executable by the at least one processor to enable the at least one processor to perform the robot positioning method of any of claims 1-8.
10. A robot positioning system, characterized in that the system comprises:
the terminal of claim 9;
and the robot is in communication connection with the terminal and is used for transmitting data collected by the laser radar and the inertia measurement unit carried by the robot to the terminal.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111590975.0A CN114296096A (en) | 2021-12-23 | 2021-12-23 | Robot positioning method, system and terminal |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111590975.0A CN114296096A (en) | 2021-12-23 | 2021-12-23 | Robot positioning method, system and terminal |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114296096A true CN114296096A (en) | 2022-04-08 |
Family
ID=80970256
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111590975.0A Pending CN114296096A (en) | 2021-12-23 | 2021-12-23 | Robot positioning method, system and terminal |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114296096A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115727836A (en) * | 2022-11-23 | 2023-03-03 | 锐趣科技(北京)有限公司 | Fusion positioning method and system based on laser reflector and odometer |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107144285A (en) * | 2017-05-08 | 2017-09-08 | 深圳地平线机器人科技有限公司 | Posture information determines method, device and movable equipment |
| WO2018140701A1 (en) * | 2017-01-27 | 2018-08-02 | Kaarta, Inc. | Laser scanner with real-time, online ego-motion estimation |
| CN111354037A (en) * | 2018-12-21 | 2020-06-30 | 北京欣奕华科技有限公司 | Positioning method and system |
| CN113447949A (en) * | 2021-06-11 | 2021-09-28 | 天津大学 | Real-time positioning system and method based on laser radar and prior map |
-
2021
- 2021-12-23 CN CN202111590975.0A patent/CN114296096A/en active Pending
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2018140701A1 (en) * | 2017-01-27 | 2018-08-02 | Kaarta, Inc. | Laser scanner with real-time, online ego-motion estimation |
| CN107144285A (en) * | 2017-05-08 | 2017-09-08 | 深圳地平线机器人科技有限公司 | Posture information determines method, device and movable equipment |
| CN111354037A (en) * | 2018-12-21 | 2020-06-30 | 北京欣奕华科技有限公司 | Positioning method and system |
| CN113447949A (en) * | 2021-06-11 | 2021-09-28 | 天津大学 | Real-time positioning system and method based on laser radar and prior map |
Non-Patent Citations (1)
| Title |
|---|
| GENGHANG ZHUANG 等；: "A Real-Time Embedded Localization in Indoor Environment Using LiDAR Odometry", EMBEDDED SYSTEMS TECHNOLOGY, ESTC 2017, vol. 857, 27 May 2019 (2019-05-27), pages 210 - 222 * |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115727836A (en) * | 2022-11-23 | 2023-03-03 | 锐趣科技(北京)有限公司 | Fusion positioning method and system based on laser reflector and odometer |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US10268203B2 (en) | Calibration validation for autonomous vehicle operations | |
| US10024668B2 (en) | Position estimation system, position estimation method and mobile unit | |
| CN111413721B (en) | Vehicle positioning method, device, controller, intelligent vehicle and system | |
| Fakharian et al. | Adaptive Kalman filtering based navigation: An IMU/GPS integration approach | |
| US9767372B2 (en) | Target detection apparatus and target detection method | |
| US20140297093A1 (en) | Autonomous vehicle and method of estimating self position of autonomous vehicle | |
| JP2016149132A (en) | System and method for prediction in driver assist system of vehicle | |
| KR102086270B1 (en) | Control method and traveling control device of the traveling control device | |
| JP2022518911A (en) | Generate structured map data from vehicle sensors and camera arrays | |
| CN108061555B (en) | Vehicle positioning error correction method and device | |
| CN114636993A (en) | External parameter calibration method, device and equipment for laser radar and IMU | |
| KR101909953B1 (en) | Method for vehicle pose estimation using LiDAR | |
| US20200035099A1 (en) | Distributing processing resources across local and cloud-based systems with respect to autonomous navigation | |
| US20160231432A1 (en) | Method for determining a current position of a motor vehicle in a geodetic coordinate system and motor vehicle | |
| CN113587930B (en) | Indoor and outdoor navigation method and device of autonomous mobile robot based on multi-sensor fusion | |
| CN111536972A (en) | Vehicle-mounted DR navigation method based on odometer scale factor correction | |
| CN113984044A (en) | Vehicle pose acquisition method and device based on vehicle-mounted multi-perception fusion | |
| EP3819594B1 (en) | Travel assistance method and travel assistance device | |
| CN114111775A (en) | Multi-sensor fusion positioning method and device, storage medium and electronic equipment | |
| CN110243364B (en) | Unmanned aerial vehicle course determining method and device, unmanned aerial vehicle and storage medium | |
| US11579628B2 (en) | Method for localizing a vehicle | |
| CN114694111A (en) | Vehicle positioning | |
| CN114296096A (en) | Robot positioning method, system and terminal | |
| WO2022141240A1 (en) | Determining vehicle positions for autonomous driving based on monocular vision and semantic map | |
| CN116358525A (en) | Laser radar-based map building and positioning method, system and engineering vehicle |
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 |