CN112083433B - Laser radar distortion removal method applied to two-wheeled mobile robot - Google Patents
Laser radar distortion removal method applied to two-wheeled mobile robot Download PDFInfo
- Publication number
- CN112083433B CN112083433B CN202010704463.1A CN202010704463A CN112083433B CN 112083433 B CN112083433 B CN 112083433B CN 202010704463 A CN202010704463 A CN 202010704463A CN 112083433 B CN112083433 B CN 112083433B
- Authority
- CN
- China
- Prior art keywords
- data
- queue
- time
- array
- distortion
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- 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
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
-
- 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
- G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
-
- 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
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
- G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- General Physics & Mathematics (AREA)
- Electromagnetism (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Traffic Control Systems (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses a laser radar distortion removal method applied to a two-wheeled mobile robot, which comprises the following steps: acquiring original pose data of each laser point in a frame of laser radar; acquiring first pose data of an odometer in a first time period; acquiring acceleration data of the IMU in a second time period and angular velocity data of a Z axis of the IMU; performing first distortion removal on the original pose data by utilizing the angular velocity data of the Z axis of the IMU; compensating the first pose data of the odometer by using the acceleration data of the IMU to obtain compensation data; and carrying out second distortion removal on the data subjected to the first distortion removal by using the compensation data to obtain final distortion removal data of the laser radar. The method and the device can accurately reflect the movement condition of the laser radar and can realize the removal distortion and the state estimation decoupling of the laser radar.
Description
Technical Field
The application belongs to the technical field of robot mapping and positioning, and particularly relates to a laser radar distortion removal method applied to a two-wheeled mobile robot.
Background
The mobile robot is a new transport means and shows great advantages. Not only can lighten the labor intensity and improve the production efficiency, but also can liberate people from dangerous, severe and heavy working environments in terms of production modes. Since the last 80 s of the century, many countries have begun to study mobile robots.
The laser radar plays an important role in mapping and pose estimation of the robot in an unknown environment. But high scanning frequency and high price of multi-line laser radar greatly limit the development and application of indoor service mobile robots. And the single-wire laser radar with low scanning frequency has low price, mature technology and wide application. However, when applied to a two-wheeled mobile robot, distortion occurs when the terrain is scanned. The causes of the distortion are: the laser scanning frequency, the data of the laser spot is not obtained instantaneously; the laser frame rate is low, and the displacement of the robot in the motion process cannot be ignored; when measuring at the laser spot, the two-wheeled robot shakes.
The speedometer has very high update frequency (the serial communication can reach 200 HZ), can accurately reflect the motion condition of the robot in a period of time, and the IMU can accurately measure the acceleration of the robot in the x-axis direction and accurately reflect the shake of the two-wheeled mobile robot in the x-axis direction. However, when the data of the lidar is distorted by the odometer, there is a bumpy condition, resulting in low accuracy of the distortion removal.
Disclosure of Invention
The purpose of the application is to provide a laser radar distortion removal method applied to a two-wheeled mobile robot, which can accurately reflect the movement condition of the laser radar and can realize the laser radar distortion removal and state estimation decoupling.
In order to achieve the above purpose, the technical scheme adopted by the application is as follows:
a lidar de-distortion method applied to a two-wheeled mobile robot, the two-wheeled mobile robot being mounted with a lidar, an odometer and an IMU, the lidar de-distortion method applied to the two-wheeled mobile robot comprising:
acquiring original pose data of each laser point in a frame of laser radar, wherein the time for starting the laser point in the frame of laser radar is t s The time of the last laser spot is t e ；
Acquiring first pose data of an odometer in a first time period, wherein the starting time of the first time period is t a Ending time t b And t a ＜t s ＜t e ＜t b ；
Acquiring acceleration data of the IMU and angular velocity data of a Z axis of the IMU in a second time period, wherein the starting time of the second time period is t m Ending time t n And t m ＜t a ＜t b ＜t n ；
Performing first distortion removal on the original pose data by utilizing the angular velocity data of the Z axis of the IMU;
compensating the first pose data of the odometer by using the acceleration data of the IMU to obtain compensation data;
and carrying out second distortion removal on the data subjected to the first distortion removal by using the compensation data to obtain final distortion removal data of the laser radar.
The following provides several alternatives, but not as additional limitations to the above-described overall scheme, and only further additions or preferences, each of which may be individually combined for the above-described overall scheme, or may be combined among multiple alternatives, without technical or logical contradictions.
Preferably, the original pose data are stored in a queue Lidarlist1 according to time sequence, t s The original pose data obtained at the moment is stored at the head of a queue Lidarlist1 and is recorded as
t e The original pose data obtained at the moment is stored at the tail of a queue Lidarlist1 and is marked as +.>
Each laser spot in a frame of laser radar has a time interval delta t r Thus the length of queue Lidarlist1 is l rl 。
As a preferred alternative to this,the first pose data of the odometer acquired in the first time period are stored in a queue Odomlist1 according to time sequence, t a The first pose data collected at the moment is stored at the head of a queue Odomlist1 and is recorded as
t b The first pose data collected at the moment is stored at the tail of a queue Odomlist1 and is marked as +.>
The time interval between two adjacent first pose data is deltat o Thus queue Odomlist1 has a length of l ol 。
Preferably, the acceleration data of the IMU acquired in the second period is stored in an array of acceraray 1 according to time sequence, and the first data storage t of the array of acceraray 1 m Acceleration data acquired at moment, and last data storage t of array Accumay 1 n Acceleration data acquired at moment, wherein the time interval between two adjacent acceleration data is deltat u Array Acarray 1 has length l al ；
The acquired angular velocity data of the Z axis of the IMU in the second time period is stored in an array Warray1 according to time sequence, and the first data storage t of the array Warray1 m Angular velocity data acquired at moment, last data storage t of array Warray1 n Angular velocity data acquired at a moment, wherein the time interval between two adjacent angular velocity data is deltat u Array Warray1 has length l al And there is l ol ＞l al ＞l rl Is a relationship of (3).
Preferably, the first de-distorting the original pose data by using the angular velocity data of the Z axis of the IMU includes:
taking the array Warray1, multiplying each angular velocity data in the array Warray1 by the time interval Δt u Obtaining
Years in array Warray2, < >>
Represents the k+1st angular velocity data in the array Warray1, 0.ltoreq.k.ltoreq.l al -1)；
Updating original pose data in a queue Lidarlist1 by using an array Warray 2: taking the k+1st original pose data Lidarlist1[ k ] in the queue Lidarlist1]Azimuth angle sigma of (a) k Updated as
Will update sigma k Each data in the queue Lidarlist1 is multiplied by an R matrix on the left of a column vector formed by an x-axis coordinate, a y-axis coordinate and an azimuth angle, and is recorded as a queue Lidarlist2, so that the first distortion removal is completed, wherein the R matrix is a coordinate conversion matrix between the known mounting position of the laser radar and the mounting position of the IMU.
Preferably, the compensating the first pose data of the odometer by using the acceleration data of the IMU to obtain compensation data includes:
taking an array Acarray 1, processing data in the array Acarray 1 through linear interpolation, and storing the processed data in an array Acarray 2, wherein the length of the array Acarray 2 is l ol ；
Sequentially overlapping the first pose data in the queue Odomlist1 with corresponding elements in the array Acarray 2:
wherein, (x) i ，y i ，θ i ) For the (i+1) th first pose data in the queue Odomlist1, a i For the (i+1) th acceleration data in the array Accray 2, 0.ltoreq.i.ltoreq.l ol -1)，Δt o The time interval of two adjacent first pose data;
storing the superimposed compensation data in a queue Odomlist2, and storing the time of two adjacent compensation dataThe interval is delta t o Queue Odomlist2 has length l ol Queue head store t for queue Odomlist2 a Time-of-day compensation data, tail store t of queue Odomlist2 b Time of day compensation data.
Preferably, the performing the second distortion removal on the data after the first distortion removal by using the compensation data to obtain final distortion removal data of the laser radar includes:
let t g ＜t s ＜t p And t s The corresponding compensation data is not equal to the data after the first distortion removal at the moment, t g And t p The corresponding compensation data is equal to the data after the first distortion removal at the moment, namely
For t in queue Lidarlist2 obtained after first de-distortion g Data corresponding to time of day->
For t in queue Lidarlist2 obtained after first de-distortion p Data corresponding to time of day->
For t in queue Odomlist2 g The compensation data corresponding to the moment in time,
for t in queue Odomlist2 p Compensating data corresponding to the moment;
calculating t according to the formula s Interpolation point of time
The following are provided:
the same thing is done to calculate t e Interpolation point of time
And at t s From time to t e Calculating to obtain interpolation points at other 3 arbitrary moments between moments;
interpolation between two adjacent interpolation points of the 5 interpolation points using linear interpolation
And storing all the data generated after interpolation into a queue Lidarlist3 to be used as final de-distortion data of the laser radar, and finishing secondary de-distortion.
The laser radar distortion removal method applied to the two-wheeled mobile robot provided by the application utilizes the angular velocity data of the Z axis of the IMU to carry out first distortion removal on the original pose data. Because the IMU is sensitive to the course angle, the IMU can be adopted to carry out first distortion removal in the distortion removal process, so that the preliminary correction of data is realized, and better correction results can be obtained in the later period; the accelerometer is compensated by utilizing the acceleration data of the IMU, so that the distortion caused by position drift and jolt of the accelerometer due to the back-and-forth shaking of the robot is reduced; and finally, carrying out secondary correction on the data subjected to the first distortion removal by using the compensated data with higher accuracy after compensation, so that the distortion removal of the laser radar can be effectively completed, and the movement condition of the laser radar can be accurately reflected.
Drawings
Fig. 1 is a flowchart of a laser radar de-distortion method applied to a two-wheeled mobile robot according to the present application.
Detailed Description
The following description of the technical solutions in the embodiments of the present application will be made clearly and completely with reference to the drawings in the embodiments of the present application, and it is apparent that the described embodiments are only some embodiments of the present application, not all embodiments. All other embodiments, which can be made by one of ordinary skill in the art without undue burden from the present disclosure, are within the scope of the present disclosure.
Unless defined otherwise, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs. The terminology used herein in the description of the present application is for the purpose of describing particular embodiments only and is not intended to be limiting of the application.
In one embodiment, a method for removing distortion of a laser radar applied to a two-wheeled mobile robot is provided, which is used for performing distortion removal correction on data acquired by the laser radar, so as to facilitate the construction and positioning of the two-wheeled mobile robot (for short, robot). The two-wheeled mobile robot is provided with at least a laser radar, an odometer and an IMU, and the number of the laser radar, the odometer and the IMU is at least one. The present embodiment focuses on removing distortion from lidar data, and does not severely limit the number of lidars, odometers and IMUs.
In this embodiment, the laser radar, the odometer and the IMU are all described as one example, and if there are multiple laser radars, odometers and IMUs, one laser radar, odometer and IMU may be used as a set of data hardware to perform distortion removal, or the data average of multiple odometers or the data average of multiple IMUs may be used to perform distortion removal on laser radar data.
As shown in fig. 1, the laser radar distortion removal method applied to the two-wheeled mobile robot in the present embodiment includes the following steps:
and S1, data acquisition.
(1) Acquiring original pose data of each laser point in a frame of laser radar, wherein the time for starting the laser point in the frame of laser radar is t s The time of the last laser spot is t e 。
In this embodiment, the obtained original pose data is stored in a queue Lidarlist1 according to time sequence, t s The original pose data obtained at the moment is stored at the head of a queue Lidarlist1 and is recorded as
t e Time of day acquisitionThe original pose data is stored at the tail of a queue Lidarlist1 and is marked as +.>
Each laser spot in a frame of laser radar has a time interval delta t r Thus the length of queue Lidarlist1 is l rl . The laser transmitter rotates at a constant speed in the laser radar, and transmits laser once when rotating a small angle, and generates a frame of complete data after a certain angle is round.
It is easy to understand that, since the pose data includes x-axis coordinates, y-axis coordinates and azimuth angles, the pose data is stored in a queue form in this embodiment, but the queue storage form is not limited to a unique storage form, and for example, the pose data may be split and stored in 3 independent arrays.
(2) Acquiring first pose data of an odometer in a first time period, wherein the starting time of the first time period is t a Ending time t b And t a ＜t s ＜t e ＜t b That is, the time range of acquiring the data of the odometer covers the time of one frame of laser so as to ensure the effectiveness of the subsequent data processing.
The acquired first pose data has various storage forms, and can be a queue, an array or an index pointer. In order to reduce the storage pressure and ensure the data read-write speed, the embodiment provides a preferred storage form as follows:
the first pose data of the odometer acquired in the first time period are stored in a queue Odomlist1 according to time sequence, t a The first pose data collected at the moment is stored at the head of a queue Odomlist1 and is recorded as
t b The first pose data collected at the moment is stored at the tail of a queue Odomlist1 and is marked as +.>
The time interval between two adjacent first pose data is deltat o Thus queue Odomlist1 has a length of l ol 。
(3) Acquiring acceleration data of an IMU (Inertial measurement unit, an inertial measurement unit, which is a device for measuring three-axis attitude angle (or angular rate) and acceleration of an object and angular velocity data of a Z axis of the IMU in a second time period, wherein the starting time of the second time period is t m Ending time t n And t m ＜t a ＜t b ＜t n I.e., the time frame in which the IMU data is acquired covers the time frame in which the odometer acquires the data.
Because the IMU can acquire acceleration data and angular velocity data simultaneously, in this embodiment, acceleration data and angular velocity data at the same time are acquired simultaneously, and after the acquisition, the data are split and stored.
One preferred way provided in this embodiment for the storage of acceleration data is: the acceleration data of the IMU acquired in the second time period is stored in an array Acarray 1 according to the time sequence, and the first data of the array Acarray 1 is stored t m Acceleration data acquired at moment, and last data storage t of array Accumay 1 n Acceleration data acquired at moment, wherein the time interval between two adjacent acceleration data is deltat u Array Acarray 1 has length l al 。
For the storage of angular velocity data, a preferred manner provided by this embodiment is: the acquired angular velocity data of the Z axis of the IMU in the second time period is stored in an array Warray1 according to time sequence, and the first data storage t of the array Warray1 m Angular velocity data acquired at moment, last data storage t of array Warray1 n Angular velocity data acquired at a moment, wherein the time interval between two adjacent angular velocity data is deltat u Array Warray1 has length l al 。
The storage form of the acceleration data and the angular velocity data is not limited to the form described above, and the above is only one preferable storage method.
Taking into account the hardnessThe cost of the piece and the size of the data quantity are measured, the data output frequency of the mileage meter is larger than the data output frequency of the IMU, the data output frequency of the IMU is larger than the data output frequency of the laser radar, and the length of the data obtained in one time of distortion removal meets the requirement of l ol ＞l al ＞l rl Is a relationship of (3).
And S2, performing first distortion removal on the original pose data by utilizing the angular velocity data of the Z axis of the IMU. Because the IMU is sensitive to the course angle, the IMU can be adopted to carry out the first distortion removal in the distortion removal process, thus realizing the preliminary correction of data and being beneficial to obtaining better correction results in the later period.
The conventional distortion removal operation may be performed by using linear interpolation, but in order to ensure the first distortion removal force and avoid the problems of excessive distortion removal or insignificant distortion removal effect, the embodiment provides a preferred distortion removal method as follows:
taking the array Warray1, multiplying each angular velocity data in the array Warray1 by the time interval Δt u Obtaining
Stored in array Warray2, < >>
Represents the k+1st angular velocity data in the array Warray1, 0.ltoreq.k.ltoreq.l al -1)。
Updating original pose data in a queue Lidarlist1 by using an array Warray 2: taking the k+1st original pose data Lidarlist1[ k ] in the queue Lidarlist1]Azimuth angle sigma of (a) k Updated as
Will update sigma k Each data in the queue Lidarlist1 is multiplied by an R matrix on the left of a column vector formed by an x-axis coordinate, a y-axis coordinate and an azimuth angle, and is recorded as a queue Lidarlist2, so that the first distortion removal is completed, wherein the R matrix is a coordinate conversion matrix between the known mounting position of the laser radar and the mounting position of the IMU. And R is a positive matrix of 3*3。
In the first time of de-distortion, the azimuth angle sigma is updated with the R matrix pairs k The post queue Lidarlist1 is converted, so that the effectiveness of removing distortion is further improved, and poor distortion removing effect caused by the installation position of the component is avoided.
And S3, compensating the first pose data of the odometer by utilizing the acceleration data of the IMU to obtain compensation data. Because the speedometer can be subjected to position drift jolt caused by forward and backward shaking of the robot, in order to reduce distortion caused by the drift jolt, the IMU is added in the embodiment, and the IMU can measure the acceleration of the laser radar along with the swing of the vehicle body at all times, so that the speedometer is compensated by utilizing the acceleration data of the IMU.
The data compensation mode can be proportional compensation, simple superposition and the like. The present embodiment combines the characteristics of the IMU and the odometer to provide a preferred compensation scheme as follows:
taking an array Acarray 1, processing data in the array Acarray 1 through linear interpolation, and storing the processed data in an array Acarray 2, wherein the length of the array Acarray 2 is l ol 。
Sequentially overlapping the first pose data in the queue Odomlist1 with corresponding elements in the array Acarray 2:
wherein, (x) i ，y i ，θ i ) For the (i+1) th first pose data in the queue Odomlist1, a i For the (i+1) th acceleration data in the array Accray 2, 0.ltoreq.i.ltoreq.l ol -1)，Δt o Is the time interval of two adjacent first pose data.
The compensation data obtained after superposition is stored in a queue Odomlist2, and the time interval between two adjacent compensation data is deltat o Queue Odomlist2 has length l ol Queue head store t for queue Odomlist2 a Time-of-day compensation data, tail store t of queue Odomlist2 b Time of day compensation data.
And S4, performing second distortion removal on the data subjected to the first distortion removal by using the compensation data to obtain final distortion removal data of the laser radar.
After the first distortion removal, the data of the laser radar is partially corrected, and at the moment, the compensation data with higher accuracy after compensation is used for carrying out secondary correction, so that the distortion removal of the laser radar can be effectively completed, and the movement condition of the laser radar can be accurately reflected.
In this embodiment, the secondary distortion removal is implemented by linear interpolation, and the specific process is as follows:
let t g ＜t s ＜t p And t s The corresponding compensation data is not equal to the data after the first distortion removal at the moment, t g And t p The corresponding compensation data is equal to the data after the first distortion removal at the moment, namely
For t in queue Lidarlist2 obtained after first de-distortion g Data corresponding to time of day->
For t in queue Lidarlist2 obtained after first de-distortion p Data corresponding to time of day->
For t in queue Odomlist2 g The compensation data corresponding to the moment in time,
for t in queue Odomlist2 p And compensating data corresponding to the moment. In the data searching process, t g And t p The time is forced to find a data in the queue Odomlist2 and the queue Lidarlist2 are equal.
Calculating t according to the formula s Interpolation point of time
The following are provided:
the same thing is done to calculate t e Interpolation point of time
And at t s From time to t e And calculating interpolation points at other 3 arbitrary moments between the moments. Calculation of interpolation points +.>
For example, the calculation process of other interpolation points is the same, and in this embodiment, no description will be given.
Interpolation between two adjacent interpolation points of the 5 interpolation points using linear interpolation
And storing all the data generated after interpolation into a queue Lidarlist3 to be used as final de-distortion data of the laser radar, and finishing secondary de-distortion.
The interpolation points at 5 times were calculated at the time of the second distortion removal because the laser radar used in the present example was a strainaa 2, and the scanning frequency of the radar was 10 hz, that is, the scanning time was 0.05 seconds for one week. By linear interpolation, the frequency of the lidar is 20 hz, i.e., 5 points are inserted, which corresponds to 100 hz, so that an error of 0 can be assumed.
The linear interpolation processing method adopted in the embodiment is an existing linear interpolation method, that is, an average value is inserted before two adjacent points, time average value processing and data average value processing are performed, and for some details not mentioned in the embodiment of the linear interpolation processing method, reference may be made to the prior art, and no further limitation is made in the embodiment.
It is easily understood that, of course, the use of the linear difference method to perform the second distortion processing can increase the softness of the distorted data, avoid the occurrence of excessive mutation, and have stronger rationality of the data.
The finally obtained distortion removal data are pose data of laser points with higher accuracy, the pose data of each laser point are converted into angle and distance data, and then the angle and distance data are packed and released into laser beams, so that the laser beams can be used for robot mapping or positioning and other purposes.
It should be noted that, in this embodiment, for convenience of description, two methods of describing data with reference to time and describing data with reference to number are adopted, but since the collected data are stored in time sequence, the data at each time and the data of the number correspond to each other. For example, t a The first pose data collected at the moment is stored at the head of a queue Odomlist1 and is recorded as
The 1 st data stored in queue Odomlist1, i.e. +.>
Other things are understood.
It should be understood that, although the steps in the flowchart of fig. 1 are shown in sequence as indicated by the arrows, the steps are not necessarily performed in sequence as indicated by the arrows. The steps are not strictly limited to the order of execution unless explicitly recited herein, and the steps may be executed in other orders. Moreover, at least some of the steps in fig. 1 may include multiple sub-steps or stages that are not necessarily performed at the same time, but may be performed at different times, nor do the order in which the sub-steps or stages are performed necessarily performed in sequence, but may be performed alternately or alternately with at least a portion of other steps or sub-steps of other steps.
The technical features of the above-described embodiments may be arbitrarily combined, and all possible combinations of the technical features in the above-described embodiments are not described for brevity of description, however, as long as there is no contradiction between the combinations of the technical features, they should be considered as the scope of the description.
The above examples merely represent a few embodiments of the present application, which are described in more detail and are not to be construed as limiting the scope of the invention. It should be noted that it would be apparent to those skilled in the art that various modifications and improvements could be made without departing from the spirit of the present application, which would be within the scope of the present application. Accordingly, the scope of protection of the present application is to be determined by the claims appended hereto.
Claims (3)
1. A method for removing distortion of a laser radar applied to a two-wheeled mobile robot, wherein the two-wheeled mobile robot is provided with the laser radar, an odometer and an IMU, the method for removing distortion of the laser radar applied to the two-wheeled mobile robot comprises the following steps:
acquiring original pose data of each laser point in a frame of laser radar, wherein the time for starting the laser point in the frame of laser radar is t s The time of the last laser spot is t e ；
Acquiring first pose data of an odometer in a first time period, wherein the starting time of the first time period is t a Ending time t b And t a ＜t s ＜t e ＜t b ；
Acquiring acceleration data of the IMU and angular velocity data of a Z axis of the IMU in a second time period, wherein the starting time of the second time period is t m Ending time t n And t m ＜t a ＜t b ＜t n ；
Performing first distortion removal on the original pose data by utilizing the angular velocity data of the Z axis of the IMU;
compensating the first pose data of the odometer by using the acceleration data of the IMU to obtain compensation data;
performing second distortion removal on the data subjected to the first distortion removal by using the compensation data to obtain final distortion removal data of the laser radar;
wherein the original pose data are stored in a queue Lidarlist1 according to time sequence, t s The original pose data obtained at the moment is stored at the head of a queue Lidarlist1 and is recorded as
t e The original pose data obtained at the moment is stored at the tail of a queue Lidarlist1 and is marked as +.>
Each laser spot in a frame of laser radar has a time interval delta t r Thus the length of queue Lidarlist1 is l rl ；
Wherein, the first pose data of the odometer acquired in the first time period is stored in a queue Odomlist1 according to time sequence, t a The first pose data collected at the moment is stored at the head of a queue Odomlist1 and is recorded as
t b The first pose data collected at the moment is stored at the tail of a queue Odomlist1 and is marked as +.>
The time interval between two adjacent first pose data is deltat o Thus queue Odomlist1 has a length of l ol ；
The acceleration data of the IMU acquired in the second period of time is stored in an array accelaray 1 according to a time sequence, and the first data storage t of the array accelaray 1 m Acceleration data acquired at moment, and last data storage t of array Accumay 1 n Degree of acceleration acquired at timeAccording to the time interval delta t between two adjacent acceleration data u Array Acarray 1 has length l al ；
The acquired angular velocity data of the Z axis of the IMU in the second time period is stored in an array Warray1 according to time sequence, and the first data storage t of the array Warray1 m Angular velocity data acquired at moment, last data storage t of array Warray1 n Angular velocity data acquired at a moment, wherein the time interval between two adjacent angular velocity data is deltat u Array Warray1 has length l al And there is l ol ＞l al ＞l rl Is a relationship of (2);
the first removing distortion of the original pose data by using the angular velocity data of the Z axis of the IMU comprises the following steps:
taking the array Warray1, multiplying each angular velocity data in the array Warray1 by the time interval Δt u Obtaining
Stored in array Warray2, < >>
Represents the (K+1) th angular velocity data in the array Warray1, 0.ltoreq.k.ltoreq.l al -1)；
Updating original pose data in a queue Lidarlist1 by using an array Warray 2: taking the K+1st original pose data Lidarlist1[ K ] in the queue Lidarlist1]Azimuth angle sigma of (a) k Updated as
Will update sigma k Each data in the queue Lidarlist1 is multiplied by an R matrix on the left of a column vector formed by an x-axis coordinate, a y-axis coordinate and an azimuth angle, and is recorded as a queue Lidarlist2, so that the first distortion removal is completed, wherein the R matrix is a coordinate conversion matrix between the known mounting position of the laser radar and the mounting position of the IMU.
2. The method for removing distortion of a lidar for a two-wheeled mobile robot according to claim 1, wherein the compensating the first pose data of the odometer with the acceleration data of the IMU to obtain compensation data comprises:
taking an array Acarray 1, processing data in the array Acarray 1 through linear interpolation, and storing the processed data in an array Acarray 2, wherein the length of the array Acarray 2 is l ol ；
Sequentially overlapping the first pose data in the queue Odomlist1 with corresponding elements in the array Acarray 2:
wherein, (x) i ，y i ，θ i ) For the (i+1) th first pose data in the queue Odomlist1, a i For the (i+1) th acceleration data in the array Accray 2, 0.ltoreq.i.ltoreq.l ol -1)，Δt o The time interval of two adjacent first pose data;
the compensation data obtained after superposition is stored in a queue Odomlist2, and the time interval between two adjacent compensation data is deltat o Queue Odomlist2 has length l ol Queue head store t for queue Odomlist2 a Time-of-day compensation data, tail store t of queue Odomlist2 b Time of day compensation data.
3. The method for removing distortion of a lidar for a two-wheeled mobile robot according to claim 2, wherein the performing the second distortion removal on the first distorted data using the compensation data to obtain final distortion removal data of the lidar comprises:
let t g ＜t s ＜t p And t s The corresponding compensation data is not equal to the data after the first distortion removal at the moment, t g And t p The corresponding compensation data is equal to the data after the first distortion removal at the moment, namely
For t in queue Lidarlist2 obtained after first de-distortion g Data corresponding to time of day->
For t in queue Lidarlist2 obtained after first de-distortion p Data corresponding to time of day->
For t in queue Odomlist2 g Compensation data corresponding to the time instant->
For t in queue Odomlist2 p Compensating data corresponding to the moment;
calculating t according to the formula s Interpolation point of time
The following are provided:
the same thing is done to calculate t e Interpolation point of time
And at t s From time to t e Calculating to obtain interpolation points at other 3 arbitrary moments between moments;
between two adjacent interpolation points of the 5 interpolation points using linear interpolationInsertion into
And storing all the data generated after interpolation into a queue Lidarlist3 to be used as final de-distortion data of the laser radar, and finishing secondary de-distortion. />
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010704463.1A CN112083433B (en) | 2020-07-21 | 2020-07-21 | Laser radar distortion removal method applied to two-wheeled mobile robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010704463.1A CN112083433B (en) | 2020-07-21 | 2020-07-21 | Laser radar distortion removal method applied to two-wheeled mobile robot |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112083433A CN112083433A (en) | 2020-12-15 |
| CN112083433B true CN112083433B (en) | 2023-06-13 |
Family
ID=73735353
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010704463.1A Active CN112083433B (en) | 2020-07-21 | 2020-07-21 | Laser radar distortion removal method applied to two-wheeled mobile robot |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112083433B (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113311411B (en) * | 2021-04-19 | 2022-07-12 | 杭州视熵科技有限公司 | Laser radar point cloud motion distortion correction method for mobile robot |
| CN113219973B (en) * | 2021-05-08 | 2022-06-24 | 浙江工业大学 | Local path control method of mobile robot |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US10043076B1 (en) * | 2016-08-29 | 2018-08-07 | PerceptIn, Inc. | Visual-inertial positional awareness for autonomous and non-autonomous tracking |
| CN109709801A (en) * | 2018-12-11 | 2019-05-03 | 智灵飞（北京）科技有限公司 | A kind of indoor unmanned plane positioning system and method based on laser radar |
| CN109975792A (en) * | 2019-04-24 | 2019-07-05 | 福州大学 | Method based on Multi-sensor Fusion correction multi-line laser radar point cloud motion distortion |
| CN110243358A (en) * | 2019-04-29 | 2019-09-17 | 武汉理工大学 | The unmanned vehicle indoor and outdoor localization method and system of multi-source fusion |
| CN110879400A (en) * | 2019-11-27 | 2020-03-13 | 炬星科技（深圳）有限公司 | Method, equipment and storage medium for fusion positioning of laser radar and IMU |
| CN110888120A (en) * | 2019-12-03 | 2020-03-17 | 华南农业大学 | Method for correcting laser radar point cloud data motion distortion based on integrated navigation system |
| CN111045017A (en) * | 2019-12-20 | 2020-04-21 | 成都理工大学 | Method for constructing transformer substation map of inspection robot by fusing laser and vision |
| CN111199578A (en) * | 2019-12-31 | 2020-05-26 | 南京航空航天大学 | Unmanned aerial vehicle three-dimensional environment modeling method based on vision-assisted laser radar |
| CN111207774A (en) * | 2020-01-17 | 2020-05-29 | 山东大学 | Method and system for laser-IMU external reference calibration |
| CN111398984A (en) * | 2020-03-22 | 2020-07-10 | 华南理工大学 | Self-adaptive laser radar point cloud correction and positioning method based on sweeping robot |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107817503B (en) * | 2016-09-14 | 2018-12-21 | 北京百度网讯科技有限公司 | Motion compensation process and device applied to laser point cloud data |
-
2020
- 2020-07-21 CN CN202010704463.1A patent/CN112083433B/en active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US10043076B1 (en) * | 2016-08-29 | 2018-08-07 | PerceptIn, Inc. | Visual-inertial positional awareness for autonomous and non-autonomous tracking |
| CN109709801A (en) * | 2018-12-11 | 2019-05-03 | 智灵飞（北京）科技有限公司 | A kind of indoor unmanned plane positioning system and method based on laser radar |
| CN109975792A (en) * | 2019-04-24 | 2019-07-05 | 福州大学 | Method based on Multi-sensor Fusion correction multi-line laser radar point cloud motion distortion |
| CN110243358A (en) * | 2019-04-29 | 2019-09-17 | 武汉理工大学 | The unmanned vehicle indoor and outdoor localization method and system of multi-source fusion |
| CN110879400A (en) * | 2019-11-27 | 2020-03-13 | 炬星科技（深圳）有限公司 | Method, equipment and storage medium for fusion positioning of laser radar and IMU |
| CN110888120A (en) * | 2019-12-03 | 2020-03-17 | 华南农业大学 | Method for correcting laser radar point cloud data motion distortion based on integrated navigation system |
| CN111045017A (en) * | 2019-12-20 | 2020-04-21 | 成都理工大学 | Method for constructing transformer substation map of inspection robot by fusing laser and vision |
| CN111199578A (en) * | 2019-12-31 | 2020-05-26 | 南京航空航天大学 | Unmanned aerial vehicle three-dimensional environment modeling method based on vision-assisted laser radar |
| CN111207774A (en) * | 2020-01-17 | 2020-05-29 | 山东大学 | Method and system for laser-IMU external reference calibration |
| CN111398984A (en) * | 2020-03-22 | 2020-07-10 | 华南理工大学 | Self-adaptive laser radar point cloud correction and positioning method based on sweeping robot |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112083433A (en) | 2020-12-15 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109975792B (en) | Method for correcting point cloud motion distortion of multi-line laser radar based on multi-sensor fusion | |
| CN113311411B (en) | Laser radar point cloud motion distortion correction method for mobile robot | |
| CN112083433B (en) | Laser radar distortion removal method applied to two-wheeled mobile robot | |
| CN112697138B (en) | Bionic polarization synchronous positioning and composition method based on factor graph optimization | |
| CN112731450B (en) | Point cloud motion compensation method, device and system | |
| CN110440830B (en) | Self-alignment method of vehicle-mounted strapdown inertial navigation system under movable base | |
| CN108387236B (en) | Polarized light SLAM method based on extended Kalman filtering | |
| CN111238469B (en) | Unmanned aerial vehicle formation relative navigation method based on inertia/data chain | |
| CN110207691B (en) | Multi-unmanned vehicle collaborative navigation method based on data link ranging | |
| CN111189474A (en) | Autonomous calibration method of MARG sensor based on MEMS | |
| CN116448145A (en) | Navigation attitude determination method based on polarization vector space difference | |
| CN111854747A (en) | DVL-assisted SINS (strapdown inertial navigation system) coarse alignment method under large-mobility condition of carrier | |
| CN115824223A (en) | Indoor and outdoor seamless positioning method based on multi-source fusion | |
| CN109696179B (en) | Thermoelastic error estimation method for remote sensing satellite star sensor | |
| CN111998870B (en) | Calibration method and device of camera inertial navigation system | |
| JP4846784B2 (en) | Vehicle trajectory measuring device | |
| CN112284381A (en) | Visual inertia real-time initialization alignment method and system | |
| CN116167919A (en) | Laser point cloud data de-distortion method based on kernel ridge regression | |
| CN114543786B (en) | Wall climbing robot positioning method based on visual inertial odometer | |
| JP4517298B2 (en) | Correction coefficient calculation device and calculation program for direction sensor | |
| CN112977603B (en) | Method and device for controlling motor, agricultural machine and computer readable storage medium | |
| CN115097481A (en) | Point cloud motion compensation method and device and electronic equipment | |
| JP2022149051A (en) | Map creation device, map creation system, map creation method, and program | |
| CN114440881A (en) | Unmanned vehicle positioning method integrating multi-source sensor information | |
| CN114184190A (en) | Inertial/odometer integrated navigation system and method |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |