CN111982091A - Laser point cloud distortion correction method based on synchronous IMU - Google Patents
Laser point cloud distortion correction method based on synchronous IMU Download PDFInfo
- Publication number
- CN111982091A CN111982091A CN202010657251.2A CN202010657251A CN111982091A CN 111982091 A CN111982091 A CN 111982091A CN 202010657251 A CN202010657251 A CN 202010657251A CN 111982091 A CN111982091 A CN 111982091A
- Authority
- CN
- China
- Prior art keywords
- imu
- point cloud
- laser radar
- chip microcomputer
- single chip
- 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.)
- Withdrawn
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
Abstract
The invention relates to the technical field of laser navigation, in particular to a laser point cloud distortion correction method based on a synchronous IMU (inertial measurement Unit), which comprises the steps of collecting signals of the IMU and a laser radar through a single chip microcomputer, connecting the IMU and the laser radar through a signal line by the single chip microcomputer, calculating pose data by the IMU, synchronously outputting point cloud data by the laser radar, completing synchronization of the pose data given by the IMU and the point cloud data given by the laser radar by the single chip microcomputer, receiving the pose data and the point cloud data synchronized by the single chip microcomputer by an upper computer, and predicting a displacement and a rotation curve of the laser radar through the pose data; the single chip microcomputer synchronizes the speed measurement information given by the IMU and the point cloud data of the laser radar, and the positioning precision and the stability of laser positioning are greatly improved through twice correction of the point cloud data.
Description
Technical Field
The invention relates to the technical field of laser navigation, in particular to a laser point cloud distortion correction method based on a synchronous IMU (inertial measurement unit).
Background
The laser navigation is widely applied to industrial AGV trolleys, service robots and household consumption-level robots. The laser navigation has the characteristics of stability, high scanning precision, popularization and the like. The laser radar has the scanning characteristic that the output point cloud is generated by driving components such as a laser, a lens, a ToF chip and the like to rapidly rotate and scan by an internal motor. Due to the limitation of the principles of motor rotation speed, gyro effect and the like, the scanning period of the general laser radar is long, the time of scanning a circle is about 100ms generally, and the scanning period of the high-performance laser radar is about 30 ms. This characteristic can create potential problems for positional stability of the laser navigation. Because the AGV trolley has the requirement of high-speed movement, for example, the running speed of the AGV trolley in an unmanned warehouse can often reach 2 meters per second, the moving amplitude of the AGV in a scanning period reaches 20 centimeters, the high moving amplitude can not be treated as random noise of point cloud, and the AGV trolley can be used after a certain point cloud treatment algorithm. The point cloud distortion of the laser point cloud during the high-speed movement of the AGV and the carriage often results in inaccurate or unstable navigation positioning, so a method is needed for processing.
Some existing laser point cloud distortion correction methods utilize wheel axle encoder data for correction. And resolving the motion path of the AGV according to the wheel shaft encoder data and the trolley model by using a point cloud distortion correction algorithm, so that the motion track of the AGV in a point cloud scanning period is determined and corrected. However this approach requires the use of encoder data. The encoder is generally used as a servo motor component, is expensive, and limits the cost reduction of the laser navigation AGV. In addition, the method must have accurate trolley models conforming to the reality, such as laser installation deflection angles, relative positions of wheels, wheel diameters and the like, and the laser point cloud correction is incorrect due to inaccurate assembly errors in actual production of the parameters. Finally, the numerical value output of the encoder and the output of the laser radar are not synchronous, so that the numerical value of the encoder is possibly earlier or later than the output of the laser radar, the AGV movement track error in the scanning period is caused, and the laser point cloud correction is also incorrect.
Disclosure of Invention
The invention aims to provide a laser point cloud distortion correction method based on a synchronous IMU (inertial measurement unit), so as to solve the problems in the background technology.
In order to achieve the purpose, the invention provides the following technical scheme: the system is characterized in that the specific equipment comprises an IMU, a laser radar, a single chip microcomputer and an upper computer, and the specific steps comprise the following steps:
integrating the IMU, the laser radar and the single chip microcomputer together to enable a central line of an IMU measuring unit to be aligned with a rotation center of the laser radar;
secondly, signals of the IMU and the laser radar are collected through the single chip microcomputer, the single chip microcomputer is connected with the IMU and the laser radar through signal lines, the IMU calculates pose data, the laser radar synchronously outputs point cloud data, and the single chip microcomputer synchronizes the pose data given by the IMU and the point cloud data given by the laser radar;
thirdly, the upper computer receives the pose data and the point cloud data synchronized by the single chip microcomputer and predicts the displacement and rotation curve of the laser radar through the pose data;
and step four, verifying the displacement and rotation curves by an ICP method and correcting.
Preferably, in the second step, firstly, the laser radar and the IMU are synchronized, secondly, the single chip microcomputer continuously reads and temporarily stores the point cloud output of the laser radar, and finally, the single chip microcomputer continuously calculates the speed change data through the IMU.
Preferably, in the third step, the upper computer predicts the position of the laser radar according to the IMU data, and corrects the point cloud data.
Preferably, in the fourth step, further accurate correction is performed on the point cloud data obtained in the third step.
The invention has the technical effects and advantages that: the single chip microcomputer synchronizes the speed measurement information given by the IMU and the point cloud data of the laser radar, and the positioning precision and the stability of laser positioning are greatly improved through correcting the point cloud data for multiple times.
Drawings
FIG. 1 is a flow chart of the present invention.
FIG. 2 is a schematic diagram of the structural connection relationship of the present invention.
Detailed Description
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 only a part of the embodiments of the present invention, and not all of the embodiments. 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.
The invention provides a laser point cloud distortion correction method based on synchronous IMU (inertial measurement Unit) as shown in figures 1-2.
The method comprises the following steps: and setting the device.
The IMU, the laser radar and the single chip microcomputer are integrated together, so that the central line of the IMU is aligned with the rotation center of the laser radar. IMU and lidar need to have a synchronous output line: the synchronous output line of the IMU should change the level of the output line each time the IMU begins a measurement, the output of the lidar should be rotated to a particular angle in response to the radar: th0 angle changes the output line level. The IMU and the laser radar are simultaneously connected into the single chip microcomputer.
Step two: the singlechip calculates the pose through the IMU and outputs point cloud information fed back to the upper computer by combining with a laser radar synchronization line.
1. Laser radar and IMU synchronization: after the single chip microcomputer detects a synchronous line signal of the laser radar, the current speed change historical data Va calculated through the IMU is temporarily stored as Vc, and the angle historical data Tha is temporarily stored as Thc. Reset current velocity V (including Vx, Vy, xy direction velocity) Vx =0, Vy =0, reset current angle Thn to 0, clear Va, clear Tha.
2. The singlechip continuously reads and temporarily stores the point cloud output of the laser radar:
a) the initial temporary point cloud list Ls is empty.
b) The singlechip is continuously communicated with the laser radar, and every time the current measurement angle th and the distance d returned by the laser radar are measured, the (th, d) is used as a pair of data to be added to a temporary storage point cloud list Ls.
c) Until the laser radar point cloud outputs the measuring distance d0 at the angle of Th0, (Th0, d0) are added to Ls as a pair of data, and Ls, Vc and Thc are output to an upper computer through a network cable as a data packet.
3. The single chip microcomputer continuously calculates speed change data through the IMU.
a) When the single chip microcomputer reads data measured by one IMU, the x-direction acceleration ax, the y-direction acceleration ay and the z-direction angular acceleration ath in the data are analyzed. The time of this data measurement is intv;
b) the current angle Thn = Thn + ath intv, storing Thn in the list Thc;
c) the current x-direction velocity is Vx = Vx + (cos (Thn) ax-sin (Thn) ay) intv;
d) the current y-direction speed is Vy = Vy + (sin (Thn) × + cos (Thn) × ay) × intv;
e) the current velocity V (Vx, Vy) is stored in the list Vc.
Through the steps, the single chip microcomputer synchronizes the speed measurement information given by the IMU and the point cloud data of the laser radar. The data of the IMU is real-time, the data of the laser radar needs to be read indirectly through a network chip, certain delay exists, and the IMU calculation data and the radar data are synchronized through a method for temporarily storing the IMU calculation data.
Step three: and the upper computer predicts the displacement and rotation curve of the laser radar by using the IMU data.
The upper computer predicts the position of the laser radar according to the speed measurement information, corrects point cloud data, and records the movement speeds Vx and Vy of the tail of the last frame of point cloud, wherein the initial movement speed Vx =0 and the initial movement speed Vy = 0.
1. The upper computer reads a data packet from the single chip microcomputer, wherein the data packet comprises three lists of Ls, Vc and Thc, namely point cloud, speed historical data and angle historical data.
2. For each point (Th, D) of Ls, a temporary displacement and angle xs =0, ys =0, ths =0 is first initialized, and then a corrected point cloud Ls' is calculated with the following method:
1. converting the points output by the laser radar from polar coordinates to rectangular coordinates: x = cos (Th) × D; y = sin (Th) × D.
2. Calculating the output point (x ', y ') of the laser radar according to the temporary displacement and the angle, and storing the output point (x ', y ') into the corrected point cloud Ls ':
x’=x*cos(ths)-y*sin(ths)+xs；y’=x*sin(ths)+y*cos(ths)+ys。
3. interpolating Vc to obtain the velocity corresponding to the current point:
1, setting the current point as the ith point, wherein Ls is the total L points, and Vc is the total L2 points;
taking the first point (i/L2) and the next point in Vc, calculating the interpolated speed by the following formula:
Vx’=Vc[floor(i/L*L2)].vx* (i/L*L2-floor(i/L*L2))+ Vc[floor(i/L*L2)+1].vy* (floor(i/L*L2)+1- i/L*L2)；
Vy’=Vc[floor(i/L*L2)].vy* (i/L*L2-floor(i/L*L2))+ Vc[floor(i/L*L2)+1].vy* (floor(i/L*L2)+1- i/L*L2)。
where Vc [ i ]. vy represents the vy portion of the ith data in Vc.
4. The temporary displacement and angle are updated.
xs=xs+Vx’；ys=ys+Vy’。
5. And interpolating the Thc to obtain the corresponding angle of the current point, and updating the temporary angle.
1, setting the current point as the ith point, wherein Ls is L points, and Thc is L3 points;
taking the first point (i/L3) and the next point in Vc, calculating the interpolated speed by the following formula:
ths=Thc[floor(i/L*L3)]*(i/L*L2-floor(i/L*L3))+Thc[floor(i/L*L3)+1]*(floor(i/L*L3)+1- i/L*L3)Asdf。
3. and storing the temporary displacement xs, ys into the motion speed Vx, Vy at the end of the previous frame point cloud, and waiting for the next data packet.
Step four: the displacement and rotation curves were verified using ICP method and corrected.
The corrected point cloud Ls ' can be continuously obtained through the second method, further fine correction is carried out on the point cloud, and the parameter ' last frame point cloud end motion speed ' in the third step is prevented from dispersing:
1. a frame of the modified point cloud Ls1 is obtained as a reference frame.
2. Acquiring a frame of corrected point cloud Ls2, and performing point cloud matching on Ls1 and Ls2 by using an ICP (inductively coupled plasma) algorithm to obtain a displacement between Ls1 and Ls2 as (dx, dy, dth).
3. Let scale factor fac = (dx/Vx + dy/Vy)/2.
4. And multiplying the speed history record numerical value obtained from the singlechip in the second frame corresponding method by fac. And the second method is repeated to obtain the re-corrected point cloud Ls 2'.
5. And performing point cloud matching on Ls1 and Ls2 ' by using an ICP (inductively coupled plasma) algorithm to obtain the displacement (dx ', dy ', dth ') between Ls1 and Ls2 '.
Taking Ls2 ' as a reference frame, temporarily displacing Vx = dx ', Vy = dy ', and returning to step three.
Finally, it should be noted that: although the present invention has been described in detail with reference to the foregoing embodiments, it will be apparent to those skilled in the art that modifications may be made to the embodiments or portions thereof without departing from the spirit and scope of the invention.
Claims (4)
1. A laser point cloud distortion correction method based on a synchronous IMU is characterized in that specific equipment comprises the IMU, a laser radar, a single chip microcomputer and an upper computer, and the specific steps comprise the following steps:
integrating the IMU, the laser radar and the single chip microcomputer together to enable a central line of an IMU measuring unit to be aligned with a rotation center of the laser radar;
secondly, signals of the IMU and the laser radar are collected through the single chip microcomputer, the single chip microcomputer is connected with the IMU and the laser radar through signal lines, the IMU calculates pose data, the laser radar synchronously outputs point cloud data, and the single chip microcomputer synchronizes the pose data given by the IMU and the point cloud data given by the laser radar;
thirdly, the upper computer receives the pose data and the point cloud data synchronized by the single chip microcomputer and predicts the displacement and rotation curve of the laser radar through the pose data;
and step four, verifying the displacement and rotation curves by an ICP method and correcting.
2. The synchronous IMU-based laser point cloud distortion correction method of claim 1, wherein: in the second step;
firstly, synchronizing a laser radar and an IMU;
secondly, continuously reading and temporarily storing the point cloud output of the laser radar by the singlechip;
and finally, continuously calculating speed change data by the single chip microcomputer through the IMU.
3. The synchronous IMU-based laser point cloud distortion correction method of claim 2, wherein: and in the third step, the upper computer predicts the position of the laser radar according to the pose data given by the IMU and corrects the point cloud data.
4. The synchronous IMU-based laser point cloud distortion correction method of claim 3, wherein: and in the fourth step, further accurately correcting the point cloud data obtained in the third step.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010657251.2A CN111982091A (en) | 2020-07-09 | 2020-07-09 | Laser point cloud distortion correction method based on synchronous IMU |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010657251.2A CN111982091A (en) | 2020-07-09 | 2020-07-09 | Laser point cloud distortion correction method based on synchronous IMU |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN111982091A true CN111982091A (en) | 2020-11-24 |
Family
ID=73438592
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010657251.2A Withdrawn CN111982091A (en) | 2020-07-09 | 2020-07-09 | Laser point cloud distortion correction method based on synchronous IMU |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111982091A (en) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112781586A (en) * | 2020-12-29 | 2021-05-11 | 上海商汤临港智能科技有限公司 | Pose data determination method and device, electronic equipment and vehicle |
| CN112781594A (en) * | 2021-01-11 | 2021-05-11 | 桂林电子科技大学 | Laser radar iteration closest point improvement algorithm based on IMU coupling |
| CN113311411A (en) * | 2021-04-19 | 2021-08-27 | 杭州视熵科技有限公司 | Laser radar point cloud motion distortion correction method for mobile robot |
| CN113391300A (en) * | 2021-05-21 | 2021-09-14 | 中国矿业大学 | Laser radar three-dimensional point cloud real-time motion compensation method based on IMU |
-
2020
- 2020-07-09 CN CN202010657251.2A patent/CN111982091A/en not_active Withdrawn
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112781586A (en) * | 2020-12-29 | 2021-05-11 | 上海商汤临港智能科技有限公司 | Pose data determination method and device, electronic equipment and vehicle |
| CN112781586B (en) * | 2020-12-29 | 2022-11-04 | 上海商汤临港智能科技有限公司 | Pose data determination method and device, electronic equipment and vehicle |
| CN112781594A (en) * | 2021-01-11 | 2021-05-11 | 桂林电子科技大学 | Laser radar iteration closest point improvement algorithm based on IMU coupling |
| CN113311411A (en) * | 2021-04-19 | 2021-08-27 | 杭州视熵科技有限公司 | Laser radar point cloud motion distortion correction method for mobile robot |
| CN113391300A (en) * | 2021-05-21 | 2021-09-14 | 中国矿业大学 | Laser radar three-dimensional point cloud real-time motion compensation method based on IMU |
| CN113391300B (en) * | 2021-05-21 | 2022-02-01 | 中国矿业大学 | Laser radar three-dimensional point cloud real-time motion compensation method based on IMU |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111982091A (en) | Laser point cloud distortion correction method based on synchronous IMU | |
| CN113311411B (en) | Laser radar point cloud motion distortion correction method for mobile robot | |
| CN110702091B (en) | High-precision positioning method for moving robot along subway rail | |
| KR101222298B1 (en) | Calibrating method of odometry error of 2-wheel mobile robot | |
| CN110515381B (en) | Multi-sensor fusion algorithm for positioning robot | |
| CN108759822B (en) | Mobile robot 3D positioning system | |
| CN111060131B (en) | Laser radar-based robot accurate posture correction method and device | |
| CN112097792B (en) | Method for calibrating odometer of mobile robot by using Ackerman model | |
| CN112748421A (en) | Laser radar calibration method based on automatic driving of straight road section | |
| CN110763224A (en) | Navigation method and navigation system for automatic guided transport vehicle | |
| CN110379044B (en) | Method and device for compensating motion error | |
| CN110068334A (en) | A kind of high-precision locating method of magnetic navigation AGV | |
| CN113324542A (en) | Positioning method, device, equipment and storage medium | |
| CN112148017B (en) | Error calibration method and device, electronic equipment and storage medium | |
| CN109506657A (en) | A kind of automatic deviation correction track and correction localization method based on AGV | |
| CN115993089B (en) | PL-ICP-based online four-steering-wheel AGV internal and external parameter calibration method | |
| CN115683170B (en) | Calibration method based on radar point cloud data fusion error | |
| CN112945266A (en) | Laser navigation robot and odometer calibration method thereof | |
| CN116338719A (en) | Laser radar-inertia-vehicle fusion positioning method based on B spline function | |
| CN114475581B (en) | Automatic parking positioning method based on wheel speed pulse and IMU Kalman filtering fusion | |
| CN115319738A (en) | Control method for an arm assembly, work apparatus, processor, and storage medium | |
| CN115639814A (en) | Path following method and system of two-wheeled differential mobile robot | |
| CN114932951A (en) | Method and system for calibrating equivalent steering angle of front wheel of robot chassis | |
| CN113377104A (en) | Robot position control method and device based on differential model | |
| CN111076718B (en) | Autonomous navigation positioning method for subway train |
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 | ||
| WW01 | Invention patent application withdrawn after publication | ||
| WW01 | Invention patent application withdrawn after publication |
Application publication date: 20201124 |