CN114296059A - Laser radar point cloud distortion correction method for large-scale rescue equipment - Google Patents

Laser radar point cloud distortion correction method for large-scale rescue equipment Download PDF

Info

Publication number
CN114296059A
CN114296059A CN202111555737.6A CN202111555737A CN114296059A CN 114296059 A CN114296059 A CN 114296059A CN 202111555737 A CN202111555737 A CN 202111555737A CN 114296059 A CN114296059 A CN 114296059A
Authority
CN
China
Prior art keywords
laser
laser radar
coordinate system
angle
ith
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
Application number
CN202111555737.6A
Other languages
Chinese (zh)
Inventor
李旭
韦坤
周晓晶
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Southeast University
Original Assignee
Southeast University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Southeast University filed Critical Southeast University
Priority to CN202111555737.6A priority Critical patent/CN114296059A/en
Publication of CN114296059A publication Critical patent/CN114296059A/en
Pending legal-status Critical Current

Links

Images

Landscapes

  • Radar Systems Or Details Thereof (AREA)

Abstract

The invention provides a laser radar point cloud distortion correction method facing large-scale rescue equipment. The method gives consideration to the rotating angle and the angular speed of the laser radar, and ensures the accuracy of the rotation transformation matrix; the operation of large-scale rescue equipment for rescue after disaster is analyzed, the position change of the laser radar is divided into two types, and the adaptability of the use scene of the translation transformation matrix is improved; and the Kalman filtering is utilized to reduce the noise of the kinematic parameters of large rescue equipment in the rotation translation transformation matrix, so that the accuracy of the laser radar point cloud distortion correction is improved.

Description

Laser radar point cloud distortion correction method for large-scale rescue equipment
Technical Field
The invention relates to a laser radar point cloud distortion correction method, in particular to a laser radar point cloud distortion correction method for large-scale rescue equipment, and belongs to the field of post-disaster rescue.
Background
China is one of the most serious countries in the world suffering from geological disasters, including earthquakes, debris flows, landslides and the like, and has the characteristics of high occurrence frequency, high damage degree and the like. After a geological disaster occurs, rescue teams need to carry out emergency rescue work by means of engineering machinery. When the existing large-scale rescue equipment mainly based on human operation is used for rescuing, a large amount of rescue personnel are needed on site, so that the outstanding problems of no guarantee on site safety, low rescue efficiency and the like are caused. In order to overcome the problems, the intelligence degree of large-scale rescue equipment needs to be improved, so that a plurality of sensors including laser radars, cameras, millimeter wave radars and the like are added to the equipment.
The laser radar has a wide coverage range and can be suitable for various severe environments; the anti-interference capability is strong, and the interference of radio waves in the surrounding environment can be effectively avoided; the method has high ranging precision, and can accurately obtain the position information of the target in the surrounding environment. When the laser radar collects the point clouds of the surrounding environment, a certain pose difference exists between the coordinate systems of the first point and the last point in each frame of point cloud due to the motion or steering of the carrier. That is, each point in the same frame of point cloud is not in the same coordinate system, which results in point cloud distortion.
The principle of the laser radar point cloud distortion correction is as follows: all laser points in a frame are transformed to the coordinate system of the first laser point using a series of coordinate transformations. Aiming at unmanned vehicles, the Leizhiya et al propose a laser radar point cloud distortion removal method by using information of an inertial navigation system and a wheel speed sensor (patent application number: 201811479464. X). The method can remove errors generated by course angle change and plane motion of the laser radar carrier, but neglects the influence of the pitch angle and the roll angle of the carrier. Yangwu et al comprehensively consider three attitude angles and position changes of the hull for unmanned ships to correct the distortion of the lidar point cloud (patent application No.: 201911222073.4).
The method is characterized in that a crawler excavator is most commonly used in large-scale rescue equipment, an upper rotary table and a travelling mechanism of the equipment can work independently, and the method is introduced by taking the excavator as an example. When the equipment runs, the equipment cannot do rotation action, and the position change of the laser radar and the position change of the equipment are kept consistent; when the equipment performs the rotation operation, the position of the equipment is kept unchanged, and the laser radar performs circular motion. However, in the existing method, the position change of the laser radar and the position change of the carrier of the laser radar are the same, and the scene that the position change of the laser radar and the position change of the carrier of the laser radar are different is not considered. When large-scale rescue equipment works and runs on a rugged road, the pitch angle, the roll angle and the course angle of the laser radar installation position can be continuously changed, and a certain error exists in the measurement through the sensor. However, the existing method does not consider how to guarantee the accuracy of the three angle measurement.
Disclosure of Invention
Aiming at the problems, the invention provides a laser radar point cloud distortion correction method facing large-scale rescue equipment, which considers the rotating angle and angular speed of a laser radar and ensures the accuracy of a rotating transformation matrix; the operation of large-scale rescue equipment for rescue after disaster is analyzed, the position change of the laser radar is divided into two types, and the adaptability of the use scene of the translation transformation matrix is improved; and the Kalman filtering is utilized to reduce the noise of the kinematic parameters of large rescue equipment in the rotation translation transformation matrix, so that the accuracy of the laser radar point cloud distortion correction is improved.
The invention provides a laser radar point cloud distortion correction method for large-scale rescue equipment, which comprises the following specific steps of:
the method comprises the following steps: determining a rotation transformation matrix of the point cloud by using three attitude angle changes at the laser radar;
the laser radar is fixed at the top of the equipment cockpit, a coordinate system meets the right-hand criterion, the positive direction of a Z axis points to the right upper side of the cockpit, the positive direction of a Y axis points to the right front of the cockpit, and the positive direction of an X axis points to the right of the cockpit;
in a frame of point cloud, the coordinate system of the ith laser point needs to rotate around the X, Y and Z axes in turn by axi,ayi,aziThe degree can be kept to be the same as and parallel to the coordinate system of the 1 st laser point, the rotation direction meets the right-hand rule and is recorded as a positive value, and otherwise, the rotation direction is recorded as a negative value;
Figure BDA0003418589940000021
Figure BDA0003418589940000022
the pitch angle, the roll angle, the course angle and the angle increasing direction of the laser radar corresponding to the ith laser point respectivelyThe right-hand rule is satisfied;
Figure BDA0003418589940000023
the angular velocities corresponding to three angles at the jth laser spot, j being 1,2, …, i-1; f is the interval time of two adjacent laser points in the same frame of point cloud;
the coordinate system of the ith laser spot is rotated around the X-axis by axiDegree, rotation transformation matrix of
Figure BDA0003418589940000024
The coordinate system of the ith laser spot is rotated around the Y axis by ayiDegree, rotation transformation matrix of
Figure BDA0003418589940000025
The coordinate system of the ith laser spot is rotated around the Z axis by aziDegree, rotation transformation matrix of
Figure BDA0003418589940000026
The rotation transformation matrix F for the ith laser spot is:
Figure BDA0003418589940000027
step two: determining a translation transformation matrix of the point cloud by using the position change of the laser radar;
when the equipment runs, a plane rectangular coordinate system is established by taking the laser radar position corresponding to the 1 st laser point as an origin
Figure BDA0003418589940000028
The positive direction of (b) points to the east-ward direction,
Figure BDA00034185899400000213
the positive direction of the light source points to the positive north direction; in that
Figure BDA0003418589940000029
In the coordinate system, the position coordinate of the laser radar corresponding to the ith laser point is
Figure BDA00034185899400000210
sjThe speed of the equipment at the jth laser point; bjIs the included angle between the equipment driving direction and the east-righting direction at the jth laser point,
Figure BDA00034185899400000211
the heading angle at the center of the walking mechanism is equipped;
Figure BDA00034185899400000212
coordinate system rotates counterclockwise
Figure BDA0003418589940000031
Obtaining the XY coordinate system of the 1 st laser point, and in the XY coordinate system of the 1 st laser point, the translation transformation matrix of the ith laser point is
Figure BDA0003418589940000032
When the equipment executes the rotation operation, a plane rectangular coordinate system is established by taking the rotation center of the upper rotary table as an origin
Figure BDA0003418589940000033
The positive direction of (b) points to the east-ward direction,
Figure BDA0003418589940000034
the positive direction of the light source points to the positive north direction; in that
Figure BDA0003418589940000035
In the coordinate system, the position coordinate of the laser radar corresponding to the 1 st laser point is
Figure BDA0003418589940000036
The position coordinate of the laser radar corresponding to the ith laser point is
Figure BDA0003418589940000037
d is the distance from the laser radar to the rotation center; e.g. of the typeiIs at the same time
Figure BDA0003418589940000038
The polar angle of the lidar position corresponding to the ith laser point in the coordinate system,
Figure BDA0003418589940000039
in that
Figure BDA00034185899400000310
In the coordinate system, the position coordinate of the laser radar corresponding to the ith laser point is
Figure BDA00034185899400000311
In the XY coordinate system of the 1 st laser spot, the translation transformation matrix of the ith laser spot is
Figure BDA00034185899400000312
The translation transformation matrix for the ith laser spot is:
Figure BDA00034185899400000313
step three: improving accuracy of laser radar point cloud distortion correction by using Kalman filtering;
for the rescue process of equipment, the state quantity is taken as
Figure BDA00034185899400000314
Wherein the content of the first and second substances,
Figure BDA00034185899400000315
respectively a pitch angle, a roll angle and a course angle at the laser radar,
Figure BDA00034185899400000316
respectively a pitch angle speed, a roll angle speed and a course angle speed at the laser radar,
Figure BDA00034185899400000317
respectively the course angle and the course at the center of the equipment travelling mechanismAn angular velocity; taking the pitch angle, the roll angle and the course angle output by the inertial sensor at the laser radar and the course angle output by the inertial sensor at the center of the equipment travelling mechanism as observed quantities, namely
Figure BDA00034185899400000318
Wherein the content of the first and second substances,
Figure BDA00034185899400000319
respectively a pitch angle, a roll angle and a course angle output by an inertial sensor at the laser radar,
Figure BDA00034185899400000320
the course angle output by an inertial sensor at the center of the equipment travelling mechanism;
the state equation and the observation equation of the discretized Kalman filtering are as follows:
Figure BDA00034185899400000321
in the formula, t represents a discretization time; a represents a state transition matrix and a state transition matrix,
Figure BDA0003418589940000041
c represents a discrete period; h denotes an observation matrix which is,
Figure BDA0003418589940000042
q and R are independent zero mean system white noise and observation white noise vectors, the system noise covariance matrix corresponding to Q is Q, and the observation noise covariance matrix corresponding to R is R;
the recursion process comprises time updating and measurement updating, the first two steps of the next recursion process are time updating, and the remaining three steps are measurement updating;
and (3) time updating:
Figure BDA0003418589940000043
Figure BDA0003418589940000044
and (3) measurement updating:
Figure BDA0003418589940000045
Figure BDA0003418589940000046
Figure BDA0003418589940000047
the rotation transformation matrix after the optimization of the Kalman filtering state quantity is
Figure BDA0003418589940000048
The translation transformation matrix is
Figure BDA0003418589940000049
Defining the original coordinates of the ith laser point in one frame of point cloud data of the laser radar as
Figure BDA00034185899400000410
The coordinates of the ith laser spot after distortion correction are
Figure BDA00034185899400000411
The calculation formula is as follows;
Figure BDA00034185899400000412
has the advantages that:
1. the invention gives consideration to the rotating angle and the angular speed of the laser radar coordinate system, thereby ensuring the accuracy of the rotation transformation matrix;
2. according to the method, the operation of rescue after a disaster of large rescue equipment is analyzed, the position change of the laser radar is divided into two types, and the adaptability of the use scene of the translation transformation matrix is improved;
3. according to the method, the Kalman filtering is utilized to reduce the noise of the kinematic parameters of large rescue equipment in the rotational translation transformation matrix, so that the accuracy of the laser radar point cloud distortion correction is improved.
Drawings
FIG. 1 is a general design scheme diagram of a laser radar point cloud distortion correction method for large-scale rescue equipment;
fig. 2 is a schematic diagram of the rotation direction of the lidar coordinate system.
Detailed Description
The technical solutions provided by the present invention will be described in detail below with reference to specific examples, and it should be understood that the following detailed description is only illustrative and not intended to limit the scope of the present invention.
The invention provides a laser radar point cloud distortion correction method for large-scale rescue equipment. The method comprises the steps of firstly determining a rotation transformation matrix of point cloud by using three attitude angle changes at a laser radar, then determining a translation transformation matrix of the point cloud by using position changes at the laser radar, and finally improving accuracy of distortion correction of the point cloud of the laser radar by using Kalman filtering. The method gives consideration to the rotating angle and the angular speed of the laser radar, and ensures the accuracy of the rotation transformation matrix; the operation of large-scale rescue equipment for rescue after disaster is analyzed, the position change of the laser radar is divided into two types, and the adaptability of the use scene of the translation transformation matrix is improved; and the Kalman filtering is utilized to reduce the noise of the kinematic parameters of large rescue equipment in the rotation translation transformation matrix, so that the accuracy of the laser radar point cloud distortion correction is improved.
The overall design scheme is shown in fig. 1, and the specific steps comprise:
the method comprises the following steps: determining a rotation transformation matrix of the point cloud by using three attitude angle changes at the laser radar;
the laser radar is fixed at the top of the equipment cockpit, the coordinate system meets the right-hand criterion, the positive direction of the Z axis points to the position right above the cockpit, the positive direction of the Y axis points to the position right ahead of the cockpit, and the positive direction of the X axis points to the position right of the cockpit. The laser radar has a plurality of lasers in the vertical direction, and emits a plurality of laser beams at the same time. The point cloud correction method of each laser beam is the same, so the invention introduces the point cloud correction method of a single laser beam.
In a frame of point cloud, the coordinate system of the ith laser point needs to rotate around the X, Y and Z axes in turn by axi,ayi,aziThe degree can be kept to be the same as and parallel to the coordinate axis of the 1 st laser point, the rotation direction meets the right-hand rule and is recorded as a positive value, and the rotation direction meets the right-hand rule and is recorded as a negative value, as shown in fig. 2.
Figure BDA0003418589940000051
Figure BDA0003418589940000052
The pitch angle, the roll angle and the course angle of the laser radar corresponding to the ith laser point respectively, the angle increasing direction meets the right-hand rule, and the course angle is defined as the included angle between the laser radar and the true north direction;
Figure BDA0003418589940000053
the angular velocities corresponding to three angles at the jth laser spot, j being 1,2, …, i-1; f is the interval time of two adjacent laser points in the same frame point cloud, and is obtained through a product manual of a laser radar.
The coordinate system of the ith laser spot is rotated around the X-axis by axiDegree, rotation transformation matrix of
Figure BDA0003418589940000054
The coordinate system of the ith laser spot is rotated around the Y axis by ayiDegree, rotation transformation matrix of
Figure BDA0003418589940000055
The coordinate system of the ith laser spot is rotated around the Z axis by aziDegree, rotation transformation matrix of
Figure BDA0003418589940000061
Thus, the rotation transformation matrix F for the ith laser spot is:
Figure BDA0003418589940000062
step two: determining a translation transformation matrix of the point cloud by using the position change of the laser radar;
when the on-site rescue is carried out after a disaster, the large-scale rescue equipment can be regarded as planar motion within the time of one circle of rotation of the laser radar, namely the position of the laser radar is regarded to be kept unchanged in the vertical direction. When the equipment runs, the position change of the laser radar and the position change of the equipment are kept consistent because the equipment cannot do a rotary action; when the equipment performs the rotation operation, the position of the equipment is kept unchanged, and the laser radar performs circular motion.
When the equipment runs, a plane rectangular coordinate system is established by taking the laser radar position corresponding to the 1 st laser point as an origin
Figure BDA0003418589940000063
The positive direction of (b) points to the east-ward direction,
Figure BDA0003418589940000064
the positive direction of (c) points to the north direction. In that
Figure BDA0003418589940000065
In the coordinate system, the position coordinate of the laser radar corresponding to the ith laser point is
Figure BDA0003418589940000066
sjObtaining the speed of equipment at the jth laser point through information conversion of a walking motor; bjIs the included angle between the equipment driving direction and the east-righting direction at the jth laser point,
Figure BDA0003418589940000067
the heading angle at the center of the walking mechanism is equipped.
Figure BDA0003418589940000068
Counter-clockwise coordinate systemRotate
Figure BDA00034185899400000610
Obtaining the XY coordinate system of the 1 st laser point, and in the XY coordinate system of the 1 st laser point, the translation transformation matrix of the ith laser point is
Figure BDA00034185899400000611
When the equipment executes the rotation operation, a plane rectangular coordinate system is established by taking the rotation center of the upper rotary table as an origin
Figure BDA00034185899400000612
The positive direction of (b) points to the east-ward direction,
Figure BDA00034185899400000613
the positive direction of (c) points to the north direction. In that
Figure BDA00034185899400000614
In the coordinate system, the position coordinate of the laser radar corresponding to the 1 st laser point is
Figure BDA00034185899400000615
The position coordinate of the laser radar corresponding to the ith laser point is
Figure BDA00034185899400000616
d is the distance from the laser radar to the rotation center, and is obtained by static measurement in advance; e.g. of the typeiIs at the same time
Figure BDA00034185899400000617
The polar angle of the lidar position corresponding to the ith laser point in the coordinate system,
Figure BDA00034185899400000618
in that
Figure BDA00034185899400000619
In the coordinate system, the position coordinate of the laser radar corresponding to the ith laser point is
Figure BDA00034185899400000620
In the XY coordinate system of the 1 st laser spot, the translation transformation matrix of the ith laser spot is
Figure BDA00034185899400000621
Thus, the translation transformation matrix for the ith laser spot is:
Figure BDA0003418589940000071
step three: method for improving accuracy of laser radar point cloud distortion correction by utilizing Kalman filtering
Parameters in the rotation transformation matrix F and the translation transformation matrix B relate to three-axis angles and angular speeds at the laser radar and a course angle at the center of a walking mechanism, and the large-scale rescue equipment is considered to be in a dynamic state during rescue after disasters, so that data obtained by an inertial sensor has large random errors, and the accuracy of laser radar point cloud distortion correction is further influenced. The accuracy of the above parameters is thus improved by means of kalman filtering.
For the rescue process of equipment, the state quantity is taken as
Figure BDA0003418589940000072
Wherein the content of the first and second substances,
Figure BDA0003418589940000073
respectively a pitch angle, a roll angle and a course angle at the laser radar,
Figure BDA0003418589940000074
respectively a pitch angle speed, a roll angle speed and a course angle speed at the laser radar,
Figure BDA0003418589940000075
respectively is a course angle and a course angular velocity at the center of the equipment walking mechanism. The pitch angle, the roll angle and the course angle output by the inertial sensor at the laser radar and the inertial transmission at the center of the equipment travelling mechanismHeading angle of sensor output as observed quantity, i.e.
Figure BDA0003418589940000076
Wherein the content of the first and second substances,
Figure BDA0003418589940000077
respectively a pitch angle, a roll angle and a course angle output by an inertial sensor at the laser radar,
Figure BDA0003418589940000078
the heading angle output by the inertial sensor at the center of the walking mechanism is provided.
The state equation and the observation equation of the discretized Kalman filtering are as follows:
Figure BDA0003418589940000079
in the formula, t represents a discretization time; a represents a state transition matrix and a state transition matrix,
Figure BDA00034185899400000710
c represents a discrete period; h represents an observation matrix, and the parameters of the state quantities include the parameters of the observed quantities, so that
Figure BDA00034185899400000711
Figure BDA00034185899400000712
Q and R are independent zero mean system white noise and observation white noise vectors, the system noise covariance matrix corresponding to Q is Q, and the observation noise covariance matrix corresponding to R is R.
The recursion process comprises time updating and measurement updating, the first two steps of the next recursion process are time updating, and the remaining three steps are measurement updating;
and (3) time updating:
Figure BDA0003418589940000081
Figure BDA0003418589940000082
and (3) measurement updating:
Figure BDA0003418589940000083
Figure BDA0003418589940000084
Figure BDA0003418589940000085
the rotation transformation matrix after the optimization of the Kalman filtering state quantity is
Figure BDA0003418589940000086
The translation transformation matrix is
Figure BDA0003418589940000087
Defining the original coordinates of the ith laser point in one frame of point cloud data of the laser radar as
Figure BDA0003418589940000088
The coordinates of the ith laser spot after distortion correction are
Figure BDA0003418589940000089
The calculation formula is as follows;
Figure BDA00034185899400000810

Claims (1)

1. a laser radar point cloud distortion correction method for large-scale rescue equipment is characterized by comprising the following specific steps:
the method comprises the following steps: determining a rotation transformation matrix of the point cloud by using three attitude angle changes at the laser radar;
the laser radar is fixed at the top of the equipment cockpit, a coordinate system meets the right-hand criterion, the positive direction of a Z axis points to the right upper side of the cockpit, the positive direction of a Y axis points to the right front of the cockpit, and the positive direction of an X axis points to the right of the cockpit;
in a frame of point cloud, the coordinate system of the ith laser point needs to rotate around the X, Y and Z axes in turn by axi,ayi,aziThe degree can be kept to be the same as and parallel to the coordinate system of the 1 st laser point, the rotation direction meets the right-hand rule and is recorded as a positive value, and otherwise, the rotation direction is recorded as a negative value;
Figure FDA0003418589930000011
Figure FDA0003418589930000012
Figure FDA0003418589930000013
the pitch angle, the roll angle and the course angle of the laser radar corresponding to the ith laser point are respectively, and the angle increasing direction meets the right-hand rule;
Figure FDA0003418589930000014
the angular velocities corresponding to three angles at the jth laser spot, j being 1,2, …, i-1; f is the interval time of two adjacent laser points in the same frame of point cloud;
the coordinate system of the ith laser spot is rotated around the X-axis by axiDegree, rotation transformation matrix of
Figure FDA0003418589930000015
The coordinate system of the ith laser spot is rotated around the Y axis by ayiDegree, rotation transformation matrix of
Figure FDA0003418589930000016
Seating of the ith laser spotThe mark being rotated about the Z-axis by aziDegree, rotation transformation matrix of
Figure FDA0003418589930000017
The rotation transformation matrix F for the ith laser spot is:
Figure FDA0003418589930000018
step two: determining a translation transformation matrix of the point cloud by using the position change of the laser radar;
when the equipment runs, a plane rectangular coordinate system is established by taking the laser radar position corresponding to the 1 st laser point as an origin
Figure FDA0003418589930000019
Figure FDA00034185899300000110
The positive direction of (b) points to the east-ward direction,
Figure FDA00034185899300000111
the positive direction of the light source points to the positive north direction; in that
Figure FDA00034185899300000112
In the coordinate system, the position coordinate of the laser radar corresponding to the ith laser point is
Figure FDA00034185899300000113
sjThe speed of the equipment at the jth laser point; bjIs the included angle between the equipment driving direction and the east-righting direction at the jth laser point,
Figure FDA00034185899300000114
Figure FDA00034185899300000115
for equipping with travelling mechanismsA heading angle at the center;
Figure FDA00034185899300000116
coordinate system rotates counterclockwise
Figure FDA00034185899300000117
Obtaining the XY coordinate system of the 1 st laser point, and in the XY coordinate system of the 1 st laser point, the translation transformation matrix of the ith laser point is
Figure FDA0003418589930000021
When the equipment executes the rotation operation, a plane rectangular coordinate system is established by taking the rotation center of the upper rotary table as an origin
Figure FDA0003418589930000022
Figure FDA0003418589930000023
The positive direction of (b) points to the east-ward direction,
Figure FDA0003418589930000024
the positive direction of the light source points to the positive north direction; in that
Figure FDA0003418589930000025
In the coordinate system, the position coordinate of the laser radar corresponding to the 1 st laser point is
Figure FDA0003418589930000026
The position coordinate of the laser radar corresponding to the ith laser point is
Figure FDA0003418589930000027
d is the distance from the laser radar to the rotation center; e.g. of the typeiIs at the same time
Figure FDA0003418589930000028
Laser radar position corresponding to ith laser point in coordinate systemThe polar angle is arranged at the position of the magnetic pole,
Figure FDA0003418589930000029
in that
Figure FDA00034185899300000210
In the coordinate system, the position coordinate of the laser radar corresponding to the ith laser point is
Figure FDA00034185899300000211
In the XY coordinate system of the 1 st laser spot, the translation transformation matrix of the ith laser spot is
Figure FDA00034185899300000212
The translation transformation matrix for the ith laser spot is:
Figure FDA00034185899300000213
step three: improving accuracy of laser radar point cloud distortion correction by using Kalman filtering;
for the rescue process of equipment, the state quantity is taken as
Figure FDA00034185899300000214
Wherein the content of the first and second substances,
Figure FDA00034185899300000215
respectively a pitch angle, a roll angle and a course angle at the laser radar,
Figure FDA00034185899300000216
respectively a pitch angle speed, a roll angle speed and a course angle speed at the laser radar,
Figure FDA00034185899300000217
respectively setting a course angle and a course angular speed at the center of the equipment travelling mechanism; by laser radarThe pitch angle, the roll angle and the course angle output by the inertial sensor at the center of the equipment travelling mechanism are observed quantities, namely
Figure FDA00034185899300000218
Wherein the content of the first and second substances,
Figure FDA00034185899300000219
respectively a pitch angle, a roll angle and a course angle output by an inertial sensor at the laser radar,
Figure FDA00034185899300000220
the course angle output by an inertial sensor at the center of the equipment travelling mechanism;
the state equation and the observation equation of the discretized Kalman filtering are as follows:
Figure FDA00034185899300000221
in the formula, t represents a discretization time; a represents a state transition matrix and a state transition matrix,
Figure FDA0003418589930000031
c represents a discrete period; h denotes an observation matrix which is,
Figure FDA0003418589930000032
q and R are independent zero mean system white noise and observation white noise vectors, the system noise covariance matrix corresponding to Q is Q, and the observation noise covariance matrix corresponding to R is R;
the recursion process comprises time updating and measurement updating, the first two steps of the next recursion process are time updating, and the remaining three steps are measurement updating;
and (3) time updating:
Figure FDA0003418589930000033
Figure FDA0003418589930000034
and (3) measurement updating:
Figure FDA0003418589930000035
Figure FDA0003418589930000036
Figure FDA0003418589930000037
the rotation transformation matrix after the optimization of the Kalman filtering state quantity is
Figure FDA0003418589930000038
The translation transformation matrix is
Figure FDA0003418589930000039
Defining the original coordinates of the ith laser point in one frame of point cloud data of the laser radar as
Figure FDA00034185899300000310
The coordinates of the ith laser spot after distortion correction are
Figure FDA00034185899300000311
The calculation formula is as follows;
Figure FDA00034185899300000312
CN202111555737.6A 2021-12-17 2021-12-17 Laser radar point cloud distortion correction method for large-scale rescue equipment Pending CN114296059A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111555737.6A CN114296059A (en) 2021-12-17 2021-12-17 Laser radar point cloud distortion correction method for large-scale rescue equipment

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111555737.6A CN114296059A (en) 2021-12-17 2021-12-17 Laser radar point cloud distortion correction method for large-scale rescue equipment

Publications (1)

Publication Number Publication Date
CN114296059A true CN114296059A (en) 2022-04-08

Family

ID=80968488

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111555737.6A Pending CN114296059A (en) 2021-12-17 2021-12-17 Laser radar point cloud distortion correction method for large-scale rescue equipment

Country Status (1)

Country Link
CN (1) CN114296059A (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115079128A (en) * 2022-08-23 2022-09-20 深圳市欢创科技有限公司 Method and device for distortion removal of laser radar point cloud data and robot

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115079128A (en) * 2022-08-23 2022-09-20 深圳市欢创科技有限公司 Method and device for distortion removal of laser radar point cloud data and robot
CN115079128B (en) * 2022-08-23 2022-12-09 深圳市欢创科技有限公司 Method and device for distortion removal of laser radar point cloud data and robot

Similar Documents

Publication Publication Date Title
CN110631593B (en) Multi-sensor fusion positioning method for automatic driving scene
US11243081B2 (en) Slam assisted INS
CN109946730B (en) Ultra-wideband-based high-reliability fusion positioning method for vehicles under cooperation of vehicle and road
CN110208842A (en) Vehicle high-precision locating method under a kind of car networking environment
EP0936519B1 (en) Integrated vehicle positioning and navigation system, apparatus and method
CN107132563B (en) Combined navigation method combining odometer and dual-antenna differential GNSS
CN111338342B (en) Automatic tracking driving control system and method for wheel type engineering machinery
CN113311436B (en) Method for correcting wind measurement of motion attitude of laser wind measuring radar on mobile platform
CN104535061A (en) Navigation system based on multi-sensor data fusion
CN111829512A (en) AUV navigation positioning method and system based on multi-sensor data fusion
CN112034479A (en) Positioning method and system applied to intelligent inspection unmanned aerial vehicle under coal mine
CN111982114A (en) Rescue robot for estimating three-dimensional pose by adopting IMU data fusion
Dill et al. Seamless indoor-outdoor navigation for unmanned multi-sensor aerial platforms
CN115993825A (en) Unmanned vehicle cluster control system based on air-ground cooperation
CN114296059A (en) Laser radar point cloud distortion correction method for large-scale rescue equipment
CN111308457A (en) Method, system and storage medium for north finding of pulse Doppler radar
CN116728410A (en) Robot absolute positioning precision error compensation method under narrow working environment
RU195749U1 (en) Intelligent vision system for an unmanned aerial vehicle for solving navigation problems, building a three-dimensional map of the surrounding space and obstacles, and autonomous patrolling
CN206540555U (en) Front-wheel angle measuring system based on double GNSS antennas and single shaft MEMS gyro
CN115031726A (en) Data fusion navigation positioning method
CN113581320A (en) Autonomous three-dimensional surveying and mapping unmanned vehicle for mine and surveying and mapping method
CN113670318B (en) Co-location method and location system
Zhao et al. Cooperative localization based on robust GPS and Radar fusion for multiple aerial vehicles
Yang et al. AGV robot for laser-SLAM based method testing in automated container terminal
Ray et al. GPS and sonar based area mapping and navigation by mobile robots

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