CN116068535A - Multi-laser radar external parameter self-calibration method, system, terminal equipment and storage medium - Google Patents
Multi-laser radar external parameter self-calibration method, system, terminal equipment and storage medium Download PDFInfo
- Publication number
- CN116068535A CN116068535A CN202211473733.8A CN202211473733A CN116068535A CN 116068535 A CN116068535 A CN 116068535A CN 202211473733 A CN202211473733 A CN 202211473733A CN 116068535 A CN116068535 A CN 116068535A
- Authority
- CN
- China
- Prior art keywords
- laser radar
- coordinate system
- point cloud
- angle
- cloud data
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 53
- 238000013519 translation Methods 0.000 claims abstract description 20
- 238000009434 installation Methods 0.000 claims abstract description 11
- 239000011159 matrix material Substances 0.000 claims description 49
- 238000005457 optimization Methods 0.000 claims description 40
- 230000006870 function Effects 0.000 claims description 34
- 238000004364 calculation method Methods 0.000 claims description 14
- 230000008569 process Effects 0.000 abstract description 5
- 238000010586 diagram Methods 0.000 description 7
- 238000004590 computer program Methods 0.000 description 6
- 238000005259 measurement Methods 0.000 description 5
- 230000009466 transformation Effects 0.000 description 4
- 230000007613 environmental effect Effects 0.000 description 3
- 230000003287 optical effect Effects 0.000 description 3
- 238000012545 processing Methods 0.000 description 3
- 230000011218 segmentation Effects 0.000 description 3
- 238000000605 extraction Methods 0.000 description 2
- 230000000644 propagated effect Effects 0.000 description 2
- 238000012897 Levenberg–Marquardt algorithm Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004422 calculation algorithm Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 239000000835 fiber Substances 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
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
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/497—Means for monitoring or calibrating
-
- 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
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/4802—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00 using analysis of echo signal for target characterisation; Target signature; Target cross-section
-
- 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
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/497—Means for monitoring or calibrating
- G01S7/4972—Alignment of sensor
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
- Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
- Y02A90/10—Information and communication technologies [ICT] supporting adaptation to climate change, e.g. for weather forecasting or climate simulation
Landscapes
- Engineering & Computer Science (AREA)
- Computer Networks & Wireless Communication (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Optical Radar Systems And Details Thereof (AREA)
Abstract
The invention discloses a multi-laser radar external parameter self-calibration method, a system, terminal equipment and a storage medium, wherein the method comprises the following steps: acquiring first point cloud data and second point cloud data, wherein the first point cloud data is acquired by a first laser radar, and the second point cloud data is acquired by a second laser radar; respectively establishing a coordinate system by using the centers of the first laser radar and the second laser radar; determining a reference coordinate system; calculating yaw angles from the first laser radar coordinate system and the second laser radar coordinate system to a reference coordinate system; calculating pitch angles and roll angles from the first laser radar coordinate system and the second laser radar coordinate system to a reference coordinate system; and calculating translation parameters from the second laser radar coordinate system to the first laser radar coordinate system according to the yaw angle, the pitch angle and the roll angle and factory installation settings of the knapsack multi-laser radar system. The invention solves the problems of the prior art that the calibration cost is high due to the fact that the calibration process is complex and external calibration objects are relied on when the multi-laser radar reaches the standard.
Description
Technical Field
The invention relates to a multi-laser radar external parameter self-calibration method, a multi-laser radar external parameter self-calibration system, terminal equipment and a storage medium, and belongs to the technical field of laser radars.
Background
The backpack laser radar is easy to carry and convenient to operate, and becomes a main tool for mobile laser measurement in recent years. The laser radar senses surrounding environment information by emitting laser points and collects three-dimensional information of the surrounding environment information, and has the characteristics of large detection range and strong anti-interference performance. The existing knapsack laser radar is provided with a single laser radar or a plurality of laser radars, and aims to collect point cloud information rich in surrounding environment by utilizing different view angles. Therefore, before the backpack laser radar is used, the relative positions of different laser radars need to be calibrated, namely rotation and translation parameters among the laser radars are calculated, and point clouds acquired by a plurality of laser radars are unified into the same coordinate system. The calibration accuracy of the method has an important influence on point cloud data fusion of the multi-laser radar.
The external parameter calibration of the laser radar mainly comprises manual calibration and reflecting plate calibration. Manual calibration means that a manual measurement or instrument measurement mode is adopted to measure the conversion relation of a coordinate system between the laser radars, but the method requires a precise measurement instrument, and re-measurement is needed when the backpack laser radars are unfolded each time, so that labor cost and time cost are consumed, and popularization is not achieved. The reflecting plate calibration needs a specific calibration environment, mutual calibration is realized between the laser radars through matching the reflecting plate control points, but the matching of the control points is difficult due to the different view angles of the laser radars, and in addition, the method relies on an external calibration object in the calibration process, so that the calibration cost is high and the efficiency is low. Therefore, the design of the efficient knapsack multi-laser radar external parameter calibration method has important significance.
Disclosure of Invention
In view of the above, the invention provides a multi-laser radar external parameter self-calibration method, a system, terminal equipment and a storage medium, which aim to solve the problems of the prior art that the multi-laser radar reaches the standard at fixed time, the calibration process is complex, the calibration cost is high due to the dependence on external calibration objects; the invention gets rid of environmental limitation, and realizes high-precision and high-efficiency automatic calibration aiming at the backpack rotary laser radar.
The first object of the invention is to provide a multi-laser radar external parameter self-calibration method.
The second object of the invention is to provide a multi-laser radar external parameter self-calibration system.
A third object of the present invention is to provide a terminal device.
A fourth object of the present invention is to provide a storage medium.
The first object of the present invention can be achieved by adopting the following technical scheme:
the utility model provides a many lidars external parameters self-calibration method, is applied to knapsack many lidar system, the radar system includes first lidar and second lidar, and first lidar is fixed the level and is placed, and the second lidar takes place rotatoryly with first lidar through rotatory bolt, and rotation angle is adjustable, the method includes:
Acquiring first point cloud data and second point cloud data, wherein the first point cloud data is acquired by a first laser radar, and the second point cloud data is acquired by a second laser radar;
respectively establishing a coordinate system by using the centers of the first laser radar and the second laser radar;
determining a reference coordinate system, wherein an origin is a first laser radar center, an X axis is perpendicular to a connecting line of the first laser radar center and a second laser radar center, and a Z axis is perpendicular to the ground;
according to the first point cloud data and the second point cloud data, calculating yaw angles from the first laser radar coordinate system and the second laser radar coordinate system to a reference coordinate system;
calculating pitch angles and roll angles from a first laser radar coordinate system and a second laser radar coordinate system to a reference coordinate system according to the yaw angle based on the first point cloud data and the second point cloud data;
and calculating translation parameters from the second laser radar coordinate system to the first laser radar coordinate system according to the yaw angle, the pitch angle and the roll angle and factory installation settings of the knapsack multi-laser radar system.
Further, the calculating, according to the first point cloud data and the second point cloud data, a yaw angle from the first lidar coordinate system and the second lidar coordinate system to the reference coordinate system includes:
According to the corresponding point cloud data, the corresponding wall point cloud is segmented, and the longitudinal mean value of the wall point cloud is calculated;
and calculating yaw angles from the first laser radar coordinate system and the second laser radar coordinate system to the reference coordinate system according to an angle compensation mode based on the longitudinal mean value.
Further, the calculating, based on the longitudinal mean value, a yaw angle from the first lidar coordinate system and the second lidar coordinate system to the reference coordinate system according to an angle compensation manner includes:
setting a rotation matrix containing only yaw angles, as follows:
wherein yaw1 represents a yaw angle from the first lidar coordinate system to the reference coordinate system, and yaw2 represents a yaw angle from the second lidar coordinate system to the reference coordinate system, R yaw1 Representing a rotation matrix containing only yaw angle yaw1, R yaw2 Representing a rotation matrix containing only yaw angle yaw 2;
based on the rotation matrix, counting a first variance between each point of the corresponding wall surface point cloud and the corresponding longitudinal mean value in an angle compensation mode, and further constructing a first optimization function, wherein the first optimization function has the following formula:
wherein ,
the representation is based on R yaw1 A first optimization function constructed>
The representation is based on R yaw 2, second optimization function constructed +.>
Longitudinal mean value of wall point cloud representing first laser radar,/->
Representing longitudinal mean value, X of wall surface point cloud of second laser radar i X represents any point of wall surface point cloud of first laser radar j The method comprises the steps that any point of the wall surface point cloud of the second laser radar is represented, w1 represents the wall surface point cloud number of the first laser radar, and w2 represents the wall surface point cloud number of the second laser radar;
and obtaining yaw angles from the first laser radar coordinate system and the second laser radar coordinate system to the reference coordinate system according to the nonlinear least square optimization method based on the first optimization function.
Further, the calculating, based on the first point cloud data and the second point cloud data, a pitch angle and a roll angle from the first lidar coordinate system and the second lidar coordinate system to a reference coordinate system according to the yaw angle includes:
extracting the ground point cloud of the first laser radar and the ground point cloud of the second laser radar according to the yaw angle based on the first point cloud data and the second point cloud data, and calculating a height average value;
and calculating pitch angles and roll angles from the first laser radar coordinate system and the second laser radar coordinate system to a reference coordinate system according to an angle compensation mode based on the height average value.
Further, the extracting the ground point cloud of the first lidar and the ground point cloud of the second lidar according to the yaw angle based on the first point cloud data and the second point cloud data includes:
rotating the corresponding point cloud data around a Z axis according to the corresponding yaw angle to obtain rotated first point cloud data and second point cloud data, wherein the Z axis belongs to a reference coordinate system;
fitting a plurality of planes to the rotated first point cloud data, extracting and dividing, comparing the included angle between the normal vector of the fitted planes and the Z axis, and taking the plane where the normal vector meeting the included angle threshold value is located as the ground point cloud of the first laser radar;
fitting a plurality of planes to the rotated second point cloud data, extracting and dividing, comparing the magnitude of an included angle between the normal vector of the fitted plane and the Z axis, calculating the Z coordinate mean value of the fitted plane, and taking the plane which meets the normal vector of the included angle threshold and has smaller Z coordinate mean value as the ground point cloud of the second laser radar.
Further, the calculating, based on the height average value, the pitch angle and the roll angle from the first laser radar coordinate system and the second laser radar coordinate system to the reference coordinate system according to the angle compensation mode includes:
Setting a rotation matrix containing only pitch angles and roll angles, as follows:
wherein roll1 and pitch1 are respectively the first laser radar seatRoll angle and pitch angle from standard system to reference coordinate system, roll2 and pitch2 are respectively roll angle and pitch angle from second laser radar coordinate system to reference coordinate system, R roll1 Representing a rotation matrix containing only roll angle roll1, R roll2 Representing a rotation matrix containing only roll angle roll2, R pitch1 Representing a rotation matrix containing only pitch1, R pitch2 Representing a rotation matrix containing only pitch 2;
based on the rotation matrix, a second variance between each point of the corresponding ground point cloud and the corresponding height mean value is counted in an angle compensation mode, and a second optimization function is further constructed, wherein the second optimization function is represented by the following formula:
wherein ,
the representation is based on R roll1 ，R pitch1 The first optimization function is constructed and the first optimization function,
the representation is based on R roll2 R pitch2 A second optimization function constructed>
Representing the height mean value of the ground point cloud of the first lidar>
Representing the height average value, Z, of the ground point cloud of the second laser radar i ' represents any point, Z, of the ground point cloud of the first laser radar j ' represents any point of the ground point cloud of the second laser radar, g1 represents the first laser The ground point cloud number of the radar, g1 represents the ground point cloud number of the second laser radar;
and obtaining pitch angles and roll angles from the first laser radar coordinate system and the second laser radar coordinate system to a reference coordinate system according to a nonlinear least square optimization method based on the second optimization function.
Further, calculating the translation parameter from the second lidar coordinate system to the first lidar coordinate system according to the yaw angle, the pitch angle, the roll angle, and factory installation settings of the backpack multi-lidar system, includes:
according to the yaw angle, the pitch angle and the roll angle, overlapping the first laser radar coordinate system with the reference coordinate system, enabling the second laser radar coordinate system to be parallel to the reference coordinate system, and obtaining a rotation matrix from the second laser radar coordinate system to the first laser radar coordinate system;
according to the rotation matrix from the second laser radar coordinate system to the first laser radar coordinate system, calculating a rotation angle theta of the second laser radar, namely a rotation angle theta generated by rotating the bolt;
the calculation of the translation parameters is done according to the following formula:
(tx，ty，tz) T ＝(R pitch1 R roll1 R yaw1 ) -1 (tx′，ty′，tz′) T
wherein ：
tx′＝0
ty′＝l1+l2cosθ+(m+n)sinθ
tz′＝m+n+l2sinθ-(m+n)cosθ
(tx，ty，tz) T three translation parameters from the second laser radar coordinate system to the first laser radar coordinate system are represented by three-dimensional coordinates of (a); (tx ', ty ', tz ') T Representing the coordinates of the center point of the second laser radar coordinate system under the reference coordinate system; r is R pitch Representing a rotation matrix containing only pitch 1; r is R roll1 Representing a rotation matrix containing only roll angle roll 1; r is R yaw Representing a rotation matrix containing only yaw angle yaw 1; l1 represents the distance from the center of the first laser radar base to the rotating shaft; l2 represents a second laserThe distance from the center of the radar base to the rotating shaft; n represents the radius of the rotating shaft of the rotating bolt; m represents the height of the laser radar center point from the base.
The second object of the invention can be achieved by adopting the following technical scheme:
many lidars are outer to be added from calibration system is applied to knapsack many lidar system, radar system includes first lidar and second lidar, and first lidar is fixed the level and is placed, and the second lidar takes place rotatoryly with first lidar through rotatory bolt, and rotation angle is adjustable, the system includes:
the acquisition unit is used for acquiring first point cloud data and second point cloud data, wherein the first point cloud data is acquired by a first laser radar, and the second point cloud data is acquired by a second laser radar;
the establishing unit is used for respectively establishing a coordinate system by the centers of the first laser radar and the second laser radar;
The determining unit is used for determining a reference coordinate system, wherein the origin is a first laser radar center, the X axis is perpendicular to the connecting line of the first laser radar center and the second laser radar center, and the Z axis is perpendicular to the ground;
a first calculation unit for calculating yaw angles from the first and second lidar coordinate systems to a reference coordinate system according to the first and second point cloud data;
the second calculation unit is used for calculating pitch angles and roll angles from the first laser radar coordinate system and the second laser radar coordinate system to the reference coordinate system according to the yaw angle based on the first point cloud data and the second point cloud data;
and the third calculation unit is used for calculating the translation parameters from the second laser radar coordinate system to the first laser radar coordinate system according to the yaw angle, the pitch angle and the roll angle and the factory installation setting of the knapsack multi-laser radar system.
The third object of the present invention can be achieved by adopting the following technical scheme:
the terminal equipment comprises a processor and a memory for storing a program executable by the processor, wherein the processor realizes the multi-laser radar external parameter self-calibration method when executing the program stored by the memory.
The fourth object of the present invention can be achieved by adopting the following technical scheme:
a storage medium storing a program which, when executed by a processor, implements the multi-lidar external parameter self-calibration method described above.
Compared with the prior art, the invention has the following beneficial effects:
according to the invention, the yaw angle from the second laser radar to the first laser radar is a fixed value, when the rotating bolt rotates the second laser radar each time, the ground point clouds of the first laser radar and the second laser radar are automatically extracted, the transformed pitch angle, yaw angle and three translation parameters can be calculated only by the ground point clouds, the calibration of external parameters among multiple laser radars is realized, the calibration process is simple and quick, and the calibration result precision is good. The invention gets rid of environmental limitation, only the ground gradient is required to be kept consistent, and the scenes such as rooms, roads, forests and the like all meet the calibration requirement. According to the calibration method, the backpack is unfolded and the second laser radar is rotated to start the working flow without the aid of a calibration plate and manual calibration, automatic extraction of the ground point cloud and high-precision automatic calibration of the backpack rotary laser radar are achieved, the working efficiency is improved, and the problems of high and complex calibration cost in the prior art are solved.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following description will briefly explain the drawings used in the embodiments or the description of the prior art, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and other drawings can be obtained according to the structures shown in these drawings without inventive effort for a person skilled in the art.
Fig. 1 is a specific flowchart of a multi-lidar external parameter self-calibration method according to embodiment 1 of the present invention.
Fig. 2 is a schematic flow chart of the multi-lidar external parameter self-calibration method according to embodiment 1 of the present invention.
Fig. 3 is a schematic diagram of the placement of the backpack multi-lidar when collecting data according to embodiment 1 of the present invention.
Fig. 4 is a schematic diagram of the initial acquisition result of embodiment 1 of the present invention.
Fig. 5 is an explanatory diagram of the positional relationship of multiple lidars when the rotation parameters are calculated in embodiment 1 of the present invention.
Fig. 6 is an explanatory diagram of the positional relationship of multiple lidars when resolving the translational parameters according to embodiment 1 of the present invention.
FIG. 7 is a schematic diagram of the final calibration result of example 1 of the present invention.
Fig. 8 is a block diagram of the multi-lidar external parameter self-calibration system according to embodiment 2 of the present invention.
Fig. 9 is a block diagram showing the structure of a computer device according to embodiment 3 of the present invention.
In fig. 3, L1 is a first lidar, S is a rotation bolt, and L2 is a second lidar that rotates by rotating the rotation bolt;
in fig. 4, P1 and P2 are a frame of point cloud acquired by the first laser radar and the second laser radar synchronously, respectively;
in fig. 5, l1 is the distance from the center of the first lidar base to the rotating shaft, l2 is the distance from the center of the second lidar base to the rotating shaft, n is the radius of the rotating shaft of the rotating bolt, m is the height of the lidar center point from the base, and the above are fixed values when the backpack lidar leaves the factory.
In fig. 7, P1 is a point cloud acquired by the first laser radar, and P2 is a point cloud acquired by the second laser radar, after calibration is completed, converted to a point cloud under the first laser radar coordinate system.
Detailed Description
For the purpose of making the objects, technical solutions and advantages of the embodiments of the present invention more apparent, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention, and it is apparent that the described embodiments are some embodiments of the present invention, but not all embodiments, and all other embodiments obtained by those skilled in the art without making any inventive effort based on the embodiments of the present invention are within the scope of protection of the present invention.
Example 1:
as shown in fig. 1 and 2, the present embodiment provides a multi-lidar external parameter self-calibration method applied to a backpack multi-lidar system.
As shown in fig. 3, the backpack rotary multi-lidar (backpack multi-lidar system) mainly includes a first lidar L1, a second lidar L2, and a rotary bolt S; the first laser radar is fixed to be placed horizontally, the second laser radar rotates with the first laser radar through the rotary bolt, the rotation angle is adjustable until two laser radar field angles meet the working requirement, and the second laser radar cannot block the scanning rays of the first laser radar.
The multi-laser radar external parameter self-calibration method in the embodiment comprises the following steps:
s101, acquiring first point cloud data and second point cloud data, wherein the first point cloud data is acquired by a first laser radar, and the second point cloud data is acquired by a second laser radar.
Specifically, acquiring one frame of point cloud data acquired by a first laser radar at the same time
And a frame of point cloud data acquired by a second laser radar +.>
Refer to fig. 4.
S102, respectively establishing a coordinate system by the centers of the first laser radar and the second laser radar.
In this embodiment, the first lidar is a lidar placed horizontally when the backpack is deployed, and the coordinate system is C L1 The second laser radar is a laser radar which rotates and inclines with the first laser radar when the rotary bolt is rotated, and the coordinate system is C L2 。
S103, determining a reference coordinate system, wherein the origin is a first laser radar center, and the X axis is perpendicular to a connecting line of the first laser radar center and a second laser radar center.
In this embodiment, the positive direction of the Y axis points to a straight line connecting the second laser radar center point to the first laser radar center point, the X axis is parallel to the ground and perpendicular to the Y axis, and the Z axis is perpendicular to the ground, forming a right-hand coordinate system.
S104, according to the first point cloud data and the second point cloud data, yaw angles from the first laser radar coordinate system and the second laser radar coordinate system to the reference coordinate system are calculated, and reference can be made to FIG. 5.
S1041, dividing the corresponding wall point cloud according to the corresponding point cloud data.
In the step, one frame of point cloud data acquired by the first laser radar
And a frame of point cloud data acquired by a second laser radar +.>
Manually dividing the wall point cloud to obtain the wall point cloud of the first laser radar and the wall point cloud of the second laser radar.
S1042, calculating the longitudinal mean value of the wall point cloud of the first laser radar and the wall point cloud of the second laser radar.
In this step, the longitudinal mean value is the longitudinal distance mean value, that is, the mean value of the X coordinates
and />
S1043, calculating yaw angles from the first laser radar coordinate system and the second laser radar coordinate system to the reference coordinate system according to an angle compensation mode based on the longitudinal mean value.
S10431, setting a rotation matrix containing only yaw angles, where:
wherein yaw1 represents a yaw angle from the first lidar coordinate system to the reference coordinate system, and yaw2 represents a yaw angle from the second lidar coordinate system to the reference coordinate system, R yaw1 Representing a rotation matrix containing only yaw angle yaw1, R yaw2 A rotation matrix containing only yaw angle yaw2 is shown.
S10432, based on the rotation matrix, counting a first variance between each point of the corresponding wall surface point cloud and the corresponding longitudinal mean value in an angle compensation mode, and further constructing a first optimization function, wherein the first optimization function has the following formula:
wherein ,
the representation is based on R yaw1 A first optimization function constructed>
The representation is based on R yaw2 A second optimization function constructed>
Longitudinal mean value of wall point cloud representing first laser radar,/->
Representing the longitudinal mean of the wall point cloud of the second lidar,X i x represents any point of wall surface point cloud of first laser radar j And (3) representing any point of the wall point cloud of the second laser radar, wherein w1 represents the wall point cloud number of the first laser radar, and w2 represents the wall point cloud number of the second laser radar.
In the step, the variance value of the longitudinal distance X of the wall surface point cloud is minimized in an angle compensation mode.
S10433, based on the first optimization function, obtaining yaw angles from the first laser radar coordinate system and the second laser radar coordinate system to the reference coordinate system according to a nonlinear least square optimization method.
In this step, the first optimization function is solved by using a Levenberg-Marquardt (Levenberg-Marquardt) algorithm, and the optimal values of yaw1 and yaw2 can be found, so as to obtain yaw angles from the first laser radar coordinate system and the second laser radar coordinate system to the reference coordinate system, where the yaw angles are represented by the following formula:
it should be noted that, when the yaw angles yaw1 from the first lidar coordinate system to the reference coordinate system and the yaw angles yaw2 from the second lidar coordinate system to the reference coordinate system obtained by the above formula are fixed values, steps S1041 to S1043 do not need to be performed again if the second lidar is rotated again.
S105, based on the first point cloud data and the second point cloud data, according to the yaw angle, pitch angles and roll angles from the first laser radar coordinate system and the second laser radar coordinate system to the reference coordinate system are calculated.
S1051, extracting the ground point cloud of the first laser radar and the ground point cloud of the second laser radar according to the yaw angle based on the first point cloud data and the second point cloud data, and calculating the height average value.
This stepThe height average is the height distance average, i.e. the average of Z coordinates
and />
In this step, based on the first point cloud data and the second point cloud data, according to the yaw angle, the ground point cloud of the first lidar and the ground point cloud of the second lidar are extracted, which specifically includes:
s21, rotating the corresponding point cloud data around a Z axis according to the corresponding yaw angle to obtain rotated first point cloud data and second point cloud data, wherein the Z axis belongs to a reference coordinate system.
In the step, the initial calibration of the first laser radar to the second laser radar is realized by using the yaw angle from the first laser radar to the reference coordinate system and the yaw angle from the second laser radar to the reference coordinate system, namely: first point cloud data acquired by a first laser radar
And second point cloud data +.>
Rotating around a Z axis of a reference coordinate system, wherein the rotation angles are yaw1 and yaw2, and obtaining the point cloud after initial calibration>
and />
Also referred to as rotated first point cloud data and second point cloud data, respectively.
Point cloud
and />
The formula is as follows:
s22, fitting a plurality of planes to the rotated first point cloud data, extracting and dividing, comparing the included angle between the normal vector of the fitted planes and the Z axis, and taking the plane where the normal vector meeting the included angle threshold value is located as the ground point cloud of the first laser radar.
In particular, for the first lidar, the backpack is used to spread approximately parallel to the ground when in operation, and the RANSAC method (prior art) pair is used
Fitting a plurality of planes and extracting the segmentation, and recording the normal vector of the fitted planes. Because the first laser radar is approximately parallel to the ground when the backpack is unfolded, if the included angle between the normal vector of the plane and the Z axis of the reference coordinate system is smaller than 15 degrees, the plane is a candidate ground; if a plurality of candidate floors exist, comparing the normal vector of the candidate floors with the included angle of the Z axis of the reference coordinate system; if the included angles are not more than 2 degrees, merging candidate ground, and if the difference of the included angles is more than 2 degrees, the candidate ground with smaller included angles is the real ground automatically segmented by the first laser radar.
S23, fitting a plurality of planes to the rotated second point cloud data, extracting and dividing, comparing the normal vector of the fitted planes with the included angle of the Z axis, calculating the Z coordinate mean value of the fitted planes, and taking the plane with the normal vector meeting the included angle threshold and smaller Z coordinate mean value as the ground point cloud of the second laser radar.
In particular, for the second laser radar, the bolt is rotated during the backpack unfolding operation, the rotation angle of the second laser radar is between 30 and 90 degrees, and the RANSAC method (prior art) is used for the second laser radar
Fitting a plurality of planes and extracting the segmentation, and recording the normal vector of the fitted planes. If the included angle between the normal vector of the plane and the Z axis of the reference coordinate system is larger than 30 degrees and smaller than 90 degrees, the plane is a candidate ground; due to the mounting characteristics of the second lidar, there is typically only one planar normal vector that satisfies this threshold outdoors. If the roof meets the threshold value except the ground in the room, the Z coordinate mean value of the plane where the normal vector meeting the threshold value of the included angle is located is calculated, and the Z coordinate mean value of the ground is smaller than that of the roof, so that the candidate ground with smaller Z coordinate mean value is the real ground for automatic segmentation of the second laser radar.
S1052, calculating pitch angles and roll angles from the first lidar coordinate system and the second lidar coordinate system to the reference coordinate system according to the angle compensation method based on the height average value, and referring to fig. 5.
S10521, setting a rotation matrix including only pitch angles and a rotation matrix including roll angles, and the following formula:
Wherein roll1 and pitch1 are respectively the roll angle and pitch angle from the first laser radar coordinate system to the reference coordinate system, roll2 and pitch2 are respectively the roll angle and pitch angle from the second laser radar coordinate system to the reference coordinate system, R roll1 Representing a rotation matrix containing only roll angle roll1, R rool2 Representing a rotation matrix containing only roll angle roll2, R pitch1 Representing a rotation matrix containing only pitch1, R pitch2 A rotation matrix containing only pitch2 is shown.
S10522, based on the rotation matrix, counting a second variance between each point of the corresponding ground point cloud and the corresponding height mean value in an angle compensation mode, and further constructing a second optimization function, wherein the second optimization function has the following formula:
wherein ,
the representation is based on R roll1 ，R pitch1 The first optimization function is constructed and the first optimization function,
the representation is based on R roll2 R pitch2 A second optimization function constructed>
Representing the height mean value of the ground point cloud of the first lidar>
Representing the height average value, Z, of the ground point cloud of the second laser radar i ' represents any point, Z, of the ground point cloud of the first laser radar j ' represents any point of the ground point cloud of the second laser radar, g1 represents the ground point cloud number of the first laser radar, and g1 represents the ground point cloud number of the second laser radar.
In this step, the variance value of the height distance Z of the ground point cloud is minimized by means of angle compensation.
S10523, based on the second optimization function, obtaining pitch angles and roll angles from the first laser radar coordinate system and the second laser radar coordinate system to the reference coordinate system according to a nonlinear least square optimization method.
In the step, a second optimization function is solved by utilizing a Levenberg-Marquardt algorithm, and the optimal values of roll and pitch can be found, so that the pitch angle and the roll angle from a first laser radar coordinate system and a second laser radar coordinate system to a reference coordinate system are obtained, and the following formula is obtained:
the pitch angle (roll 1) and the roll angle (pitch 1) from the initial calibrated first laser radar coordinate system to the reference coordinate system are obtained through the above, and the pitch angle (roll 2) and the roll angle (pitch 2) from the initial calibrated second laser radar coordinate system to the reference coordinate system are obtained through the initial calibrated second laser radar coordinate system.
S106, calculating translation parameters from the second laser radar coordinate system to the first laser radar coordinate system according to the yaw angle, the pitch angle and the roll angle and factory installation settings of the knapsack multi-laser radar system.
S1061, according to the yaw angle, the pitch angle and the roll angle, overlapping the first laser radar coordinate system with the reference coordinate system (transformation processing), parallel the second laser radar coordinate system with the reference coordinate system (transformation processing), and obtaining a rotation matrix from the second laser radar coordinate system to the first laser radar coordinate system.
In this step, the transformation processing is realized by the following formula:
wherein ,
representing the transformed first lidar coordinate system, and (2)>
Representing a transformed second lidar coordinate system with axes parallel to each other.
In this step, the rotation matrix from the second lidar coordinate system to the first lidar coordinate system is as follows:
s1062, calculating a rotation angle theta of the second laser radar according to the rotation matrix from the second laser radar coordinate system to the first laser radar coordinate system.
In the step, the rotation angle theta of the second laser radar, namely the included angle between the Z axis of the first laser radar coordinate system and the Z axis of the second laser radar coordinate system, is also the rotation angle of the rotation bolt; specifically, the direction vector on the Z-axis of the first lidar coordinate system is given as (0 0 0 1) T The Z-axis direction vector of the second laser radar coordinate system is a rotation matrix
The last column vector. Referring to FIG. 6, a second lidar coordinate system Z-axis direction vector and vector (0 0.1) T The included angle of (2) is theta.
S1063, completing calculation of the translation parameters according to the following formula:
(tx，ty，tz) T ＝(R pitch1 R roll1 R yaw1 ) -1 (tx'，ty'，tz') T
wherein ：
tx'＝0
ty′＝l1+l2cosθ+(m+n)sinθ
tz′＝m+n+l2sinθ-(m+n)cosθ
(tx，ty，tz) T three translation parameters from the second laser radar coordinate system to the first laser radar coordinate system are represented by three-dimensional coordinates of (a); (tx ', ty ', tz ') T Representing the coordinates of the center point of the second laser radar coordinate system under the reference coordinate system; r is R pitch Representing a rotation matrix containing only pitch 1; r is R roll1 Representing a rotation matrix containing only roll angle roll 1; r is R yaw Representing a rotation matrix containing only yaw angle yaw 1; l1 represents the distance from the center of the first laser radar base to the rotating shaft; l2 represents the distance from the center of the second laser radar base to the rotating shaft; n represents the radius of the rotating shaft of the rotating bolt; m represents the height of the laser radar center point from the base.
In the step, the positive direction of the Y-axis of the reference coordinate system points to a straight line connected from the center point of the second laser radar to the center point of the first laser radar, so that the X coordinate tx' of the center point of the second laser radar is 0 in the reference coordinate system; in the backpack laser radar system, the distance l1 from the center of the first laser radar base to the rotating shaft and the distance l2 from the center of the second laser radar base to the rotating shaft are both 6cm, the radius n of the rotating shaft of the rotating bolt is 1cm, the height m of the laser radar center point from the base is 3.6cm, and the distances are fixed values when the backpack laser radar leaves the factory, and refer to fig. 6.
It is noted that the present embodiment calculates the Y coordinate ty 'and the coordinate tz' of the second lidar center point in the reference coordinate system using the known lidar installation distance position and the calculated rotation angle θ generated by the rotation bolt; the embodiment is based on the rotation matrix from the first laser radar coordinate system to the reference coordinate system 
And the coordinates (tx ', ty ', tz ') of the center point of the second lidar coordinate system in the reference coordinate system T Back calculation to obtain the center point of the second laser radar coordinate system atThree-dimensional coordinates (tx, ty, tz) in a first lidar coordinate system T 。
According to the method, when the rotary shaft rotates the second laser radar every time the back bag is unfolded, the coordinate transformation relation of the second laser radar to the first laser radar can be conveniently obtained, namely, 6 freedom degree parameters between the two laser radars can be obtained, point clouds collected by the second laser radar can be unified to the first laser radar coordinate system, and the final calibration result can be referred to fig. 7.
Those skilled in the art will appreciate that all or part of the steps in a method implementing the above embodiments may be implemented by a program to instruct related hardware, and the corresponding program may be stored in a computer readable storage medium.
It should be noted that although the method operations of the above embodiments are depicted in the drawings in a particular order, this does not require or imply that the operations must be performed in that particular order or that all illustrated operations be performed in order to achieve desirable results. Rather, the depicted steps may change the order of execution. Additionally or alternatively, certain steps may be omitted, multiple steps combined into one step to perform, and/or one step decomposed into multiple steps to perform.
Example 2:
as shown in fig. 8, the embodiment provides a multi-laser radar external parameter self-calibration system, which is applied to a backpack multi-laser radar system, the radar system comprises a first laser radar and a second laser radar, the first laser radar is fixedly and horizontally arranged, the second laser radar rotates with the first laser radar through a rotating bolt, the rotation angle is adjustable, the system comprises an acquisition unit 801, a building unit 802, a determination unit 803, a first calculation unit 804, a second calculation unit 805 and a third calculation unit 806, and the specific functions of the units are as follows:
an obtaining unit 801, configured to obtain first point cloud data and second point cloud data, where the first point cloud data is collected by a first laser radar, and the second point cloud data is collected by a second laser radar;
a building unit 802, configured to build coordinate systems with centers of the first laser radar and the second laser radar, respectively;
a determining unit 803, configured to determine a reference coordinate system, where an origin is a first laser radar center, an X-axis is perpendicular to a line connecting the first laser radar center and a second laser radar center, and a Z-axis is perpendicular to the ground;
a first calculating unit 804, configured to calculate yaw angles from the first and second lidar coordinate systems to the reference coordinate system according to the first and second point cloud data;
A second calculation unit 805 for calculating pitch angles and roll angles of the first and second lidar coordinate systems to the reference coordinate system according to the yaw angle based on the first and second point cloud data;
a third calculating unit 806, configured to calculate a translation parameter from the second lidar coordinate system to the first lidar coordinate system according to the yaw angle, the pitch angle, and the roll angle, and factory installation settings of the backpack multi-lidar system.
Example 3:
as shown in fig. 9, the present embodiment provides a terminal apparatus including a processor 902, a memory, an input device 903, a display device 904, and a network interface 905 connected through a system bus 901. The processor 902 is configured to provide computing and control capabilities, where the memory includes a nonvolatile storage medium 906 and an internal memory 907, where the nonvolatile storage medium 906 stores an operating system, a computer program, and a database, and the internal memory 907 provides an environment for the operating system and the computer program in the nonvolatile storage medium 906, and when the computer program is executed by the processor 902, the method for performing the multi-lidar external parameter self-calibration method of embodiment 1 described above is as follows:
Acquiring first point cloud data and second point cloud data, wherein the first point cloud data is acquired by a first laser radar, and the second point cloud data is acquired by a second laser radar;
respectively establishing a coordinate system by using the centers of the first laser radar and the second laser radar;
determining a reference coordinate system, wherein an origin is a first laser radar center, an X axis is perpendicular to a connecting line of the first laser radar center and a second laser radar center, and a Z axis is perpendicular to the ground;
according to the first point cloud data and the second point cloud data, calculating yaw angles from the first laser radar coordinate system and the second laser radar coordinate system to a reference coordinate system;
calculating pitch angles and roll angles from a first laser radar coordinate system and a second laser radar coordinate system to a reference coordinate system according to the yaw angle based on the first point cloud data and the second point cloud data;
and calculating translation parameters from the second laser radar coordinate system to the first laser radar coordinate system according to the yaw angle, the pitch angle and the roll angle and factory installation settings of the knapsack multi-laser radar system.
Example 4:
the present embodiment provides a storage medium, which is a computer readable storage medium storing a computer program, where the computer program when executed by a processor implements the multi-lidar external parameter self-calibration method of embodiment 1, as follows:
Acquiring first point cloud data and second point cloud data, wherein the first point cloud data is acquired by a first laser radar, and the second point cloud data is acquired by a second laser radar;
respectively establishing a coordinate system by using the centers of the first laser radar and the second laser radar;
determining a reference coordinate system, wherein an origin is a first laser radar center, an X axis is perpendicular to a connecting line of the first laser radar center and a second laser radar center, and a Z axis is perpendicular to the ground;
according to the first point cloud data and the second point cloud data, calculating yaw angles from the first laser radar coordinate system and the second laser radar coordinate system to a reference coordinate system;
calculating pitch angles and roll angles from a first laser radar coordinate system and a second laser radar coordinate system to a reference coordinate system according to the yaw angle based on the first point cloud data and the second point cloud data;
and calculating translation parameters from the second laser radar coordinate system to the first laser radar coordinate system according to the yaw angle, the pitch angle and the roll angle and factory installation settings of the knapsack multi-laser radar system.
The computer readable storage medium of the present embodiment may be a computer readable signal medium or a computer readable storage medium, or any combination of the two. The computer readable storage medium can be, for example, but not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or a combination of any of the foregoing. More specific examples of the computer-readable storage medium may include, but are not limited to: an electrical connection having one or more wires, a portable computer diskette, a hard disk, a Random Access Memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disc read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing.
In this embodiment, a computer-readable storage medium may be any tangible medium that can contain, or store a program for use by or in connection with an instruction execution system, apparatus, or device. In the present embodiment, however, the computer-readable signal medium may include a data signal propagated in baseband or as part of a carrier wave, with a computer-readable program embodied therein. Such a propagated data signal may take any of a variety of forms, including, but not limited to, electro-magnetic, optical, or any suitable combination of the foregoing. A computer readable signal medium may also be any computer readable storage medium that is not a computer readable storage medium and that can communicate, propagate, or transport a program for use by or in connection with an instruction execution system, apparatus, or device. A computer program embodied on a computer readable storage medium may be transmitted using any appropriate medium, including but not limited to: electrical wires, fiber optic cables, RF (radio frequency), and the like, or any suitable combination of the foregoing.
The computer readable storage medium may be written in one or more programming languages, including an object oriented programming language such as Java, python, C ++ and conventional procedural programming languages, such as the C-language or similar programming languages, or combinations thereof for performing the present embodiments. The program may execute entirely on the user's computer, partly on the user's computer, as a stand-alone software package, partly on the user's computer and partly on a remote computer or entirely on the remote computer or server. In the case of a remote computer, the remote computer may be connected to the user's computer through any kind of network, including a Local Area Network (LAN) or a Wide Area Network (WAN), or may be connected to an external computer (for example, through the Internet using an Internet service provider).
In summary, in the invention, the yaw angle from the second laser radar to the first laser radar is a fixed value, when the rotation bolt rotates the second laser radar each time, the ground point clouds of the first laser radar and the second laser radar are automatically extracted, and the transformed pitch angle, yaw angle and three translation parameters can be calculated only by the ground point clouds, so that the calibration of external parameters among multiple laser radars is realized, the calibration process is simple and quick, and the calibration result precision is good. The invention gets rid of environmental limitation, only the ground gradient is required to be kept consistent, and the scenes such as rooms, roads, forests and the like all meet the calibration requirement. According to the calibration method, the backpack is unfolded and the second laser radar is rotated to start the working flow without the aid of a calibration plate and manual calibration, automatic extraction of the ground point cloud and high-precision automatic calibration of the backpack rotary laser radar are achieved, the working efficiency is improved, and the problems of high and complex calibration cost in the prior art are solved.
The above-mentioned embodiments are only preferred embodiments of the present invention, but the protection scope of the present invention is not limited thereto, and any person skilled in the art can make equivalent substitutions or modifications according to the technical solution and the inventive concept of the present invention within the scope of the present invention disclosed in the present invention patent, and all those skilled in the art belong to the protection scope of the present invention.
Claims (10)
1. The utility model provides a many laser radar external parameters self-calibration method, is applied to knapsack many laser radar system, radar system includes first laser radar and second laser radar, and its characterized in that, first laser radar is fixed the level and is placed, and the second laser radar takes place rotatoryly with first laser radar through rotatory bolt, and rotation angle is adjustable, the method includes:
acquiring first point cloud data and second point cloud data, wherein the first point cloud data is acquired by a first laser radar, and the second point cloud data is acquired by a second laser radar;
respectively establishing a coordinate system by using the centers of the first laser radar and the second laser radar;
determining a reference coordinate system, wherein an origin is a first laser radar center, an X axis is perpendicular to a connecting line of the first laser radar center and a second laser radar center, and a Z axis is perpendicular to the ground;
according to the first point cloud data and the second point cloud data, calculating yaw angles from the first laser radar coordinate system and the second laser radar coordinate system to a reference coordinate system;
calculating pitch angles and roll angles from a first laser radar coordinate system and a second laser radar coordinate system to a reference coordinate system according to the yaw angle based on the first point cloud data and the second point cloud data;
And calculating translation parameters from the second laser radar coordinate system to the first laser radar coordinate system according to the yaw angle, the pitch angle and the roll angle and factory installation settings of the knapsack multi-laser radar system.
2. The method of claim 1, wherein calculating a yaw angle of the first lidar coordinate system and the second lidar coordinate system to a reference coordinate system based on the first point cloud data and the second point cloud data comprises:
according to the corresponding point cloud data, the corresponding wall point cloud is segmented, and the longitudinal mean value of the wall point cloud is calculated;
and calculating yaw angles from the first laser radar coordinate system and the second laser radar coordinate system to the reference coordinate system according to an angle compensation mode based on the longitudinal mean value.
3. The method according to claim 2, wherein calculating yaw angles of the first and second lidar coordinate systems to a reference coordinate system based on the longitudinal mean value in an angle-compensated manner comprises:
setting a rotation matrix containing only yaw angles, as follows:
wherein yaw1 represents a yaw angle from the first lidar coordinate system to the reference coordinate system, and yaw2 represents a yaw angle from the second lidar coordinate system to the reference coordinate system, R yaw1 Representing a rotation matrix containing only yaw angle yaw1, R yaw2 Representing a rotation matrix containing only yaw angle yaw 2;
based on the rotation matrix, counting a first variance between each point of the corresponding wall surface point cloud and the corresponding longitudinal mean value in an angle compensation mode, and further constructing a first optimization function, wherein the first optimization function has the following formula:
wherein ,
the representation is based on R yaw1 First optimization of constructionFunction (F)>
The representation is based on R yaw2 A second optimization function constructed>
Longitudinal mean value of wall point cloud representing first laser radar,/->
Representing longitudinal mean value, X of wall surface point cloud of second laser radar i X represents any point of wall surface point cloud of first laser radar j The method comprises the steps that any point of the wall surface point cloud of the second laser radar is represented, w1 represents the wall surface point cloud number of the first laser radar, and w2 represents the wall surface point cloud number of the second laser radar;
and obtaining yaw angles from the first laser radar coordinate system and the second laser radar coordinate system to the reference coordinate system according to the nonlinear least square optimization method based on the first optimization function.
4. The method of claim 1, wherein calculating pitch and roll angles of the first and second lidar coordinate systems to a reference coordinate system from the yaw angle based on the first and second point cloud data comprises:
Extracting the ground point cloud of the first laser radar and the ground point cloud of the second laser radar according to the yaw angle based on the first point cloud data and the second point cloud data, and calculating a height average value;
and calculating pitch angles and roll angles from the first laser radar coordinate system and the second laser radar coordinate system to a reference coordinate system according to an angle compensation mode based on the height average value.
5. The method of claim 4, wherein the extracting the ground point cloud of the first lidar and the ground point cloud of the second lidar based on the first point cloud data and the second point cloud data and based on the yaw angle comprises:
rotating the corresponding point cloud data around a Z axis according to the corresponding yaw angle to obtain rotated first point cloud data and second point cloud data, wherein the Z axis belongs to a reference coordinate system;
fitting a plurality of planes to the rotated first point cloud data, extracting and dividing, comparing the included angle between the normal vector of the fitted planes and the Z axis, and taking the plane where the normal vector meeting the included angle threshold value is located as the ground point cloud of the first laser radar;
fitting a plurality of planes to the rotated second point cloud data, extracting and dividing, comparing the magnitude of an included angle between the normal vector of the fitted plane and the Z axis, calculating the Z coordinate mean value of the fitted plane, and taking the plane which meets the normal vector of the included angle threshold and has smaller Z coordinate mean value as the ground point cloud of the second laser radar.
6. The method of claim 4, wherein calculating pitch and roll angles of the first and second lidar coordinate systems to a reference coordinate system based on the altitude average in an angle-compensated manner comprises:
setting a rotation matrix containing only pitch angles and roll angles, as follows:
wherein roll1 and pitch1 are respectively the roll angle and pitch angle from the first laser radar coordinate system to the reference coordinate system, roll2 and pitch2 are respectively the roll angle and pitch angle from the second laser radar coordinate system to the reference coordinate system, R roll1 Representing a rotation matrix containing only roll angle roll1, R roll2 Representing a rotation matrix containing only roll angle roll2, R pitch1 Representing a rotation matrix containing only pitch1, R pitch2 Representing a rotation matrix containing only pitch 2;
based on the rotation matrix, a second variance between each point of the corresponding ground point cloud and the corresponding height mean value is counted in an angle compensation mode, and a second optimization function is further constructed, wherein the second optimization function is represented by the following formula:
wherein ,
the representation is based on R roll1 ，R pitch1 A first optimization function constructed>
The representation is based on R roll2 R pitch2 A second optimization function constructed >
Representing the elevation mean of the ground point cloud of the first lidar,
representing the height average value, Z, of the ground point cloud of the second laser radar i ' represents any point, Z, of the ground point cloud of the first laser radar j ' represents any point of the ground point cloud of the second laser radar, g1 represents the ground point cloud number of the first laser radar, and g1 represents the ground point cloud number of the second laser radar;
and obtaining pitch angles and roll angles from the first laser radar coordinate system and the second laser radar coordinate system to a reference coordinate system according to a nonlinear least square optimization method based on the second optimization function.
7. The method of claim 1, wherein calculating translation parameters of the second lidar coordinate system to the first lidar coordinate system based on the yaw, pitch, and roll angles, and factory installed settings of the backpack multi-lidar system, comprises:
according to the yaw angle, the pitch angle and the roll angle, overlapping the first laser radar coordinate system with the reference coordinate system, enabling the second laser radar coordinate system to be parallel to the reference coordinate system, and obtaining a rotation matrix from the second laser radar coordinate system to the first laser radar coordinate system;
according to the rotation matrix from the second laser radar coordinate system to the first laser radar coordinate system, calculating a rotation angle theta of the second laser radar, namely a rotation angle theta generated by rotating the bolt;
The calculation of the translation parameters is done according to the following formula:
(tx，ty，tz) T ＝(R pitch1 R roll1 R yaw1 ) -1 (tx′，ty′，tz′) T
wherein ：
tx′＝0
ty′＝l1+l2cosθ+(m+n)sinθ
tz′＝m+n+l2sinθ-(m+n)cosθ
(tx，ty，tz) T three translation parameters from the second laser radar coordinate system to the first laser radar coordinate system are represented by three-dimensional coordinates of (a); (tx ', ty ', tz ') T Indicating that the center point of the second laser radar coordinate system is at the reference pointCoordinates in a coordinate system; r is R pitch1 Representing a rotation matrix containing only pitch 1; r is R roll1 Representing a rotation matrix containing only roll angle roll 1; r is R yaw1 Representing a rotation matrix containing only yaw angle yaw 1; l1 represents the distance from the center of the first laser radar base to the rotating shaft; l2 represents the distance from the center of the second laser radar base to the rotating shaft; n represents the radius of the rotating shaft of the rotating bolt; m represents the height of the laser radar center point from the base.
8. Many laser radar external parameters are from calibration system is applied to many laser radar systems of knapsack, radar system includes first laser radar and second laser radar, and its characterized in that, first laser radar is fixed the level and is placed, and the second laser radar takes place rotatoryly with first laser radar through rotatory bolt, and rotation angle is adjustable, the system includes:
the acquisition unit is used for acquiring first point cloud data and second point cloud data, wherein the first point cloud data is acquired by a first laser radar, and the second point cloud data is acquired by a second laser radar;
The establishing unit is used for respectively establishing a coordinate system by the centers of the first laser radar and the second laser radar;
the determining unit is used for determining a reference coordinate system, wherein the origin is a first laser radar center, the X axis is perpendicular to the connecting line of the first laser radar center and the second laser radar center, and the Z axis is perpendicular to the ground;
a first calculation unit for calculating yaw angles from the first and second lidar coordinate systems to a reference coordinate system according to the first and second point cloud data;
the second calculation unit is used for calculating pitch angles and roll angles from the first laser radar coordinate system and the second laser radar coordinate system to the reference coordinate system according to the yaw angle based on the first point cloud data and the second point cloud data;
and the third calculation unit is used for calculating the translation parameters from the second laser radar coordinate system to the first laser radar coordinate system according to the yaw angle, the pitch angle and the roll angle and the factory installation setting of the knapsack multi-laser radar system.
9. A terminal device comprising a processor and a memory for storing a program executable by the processor, characterized in that the method according to any of claims 1-7 is implemented when the processor executes the program stored in the memory.
10. A storage medium storing a program, which when executed by a processor, implements the method of any one of claims 1-7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211473733.8A CN116068535A (en) | 2022-11-22 | 2022-11-22 | Multi-laser radar external parameter self-calibration method, system, terminal equipment and storage medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211473733.8A CN116068535A (en) | 2022-11-22 | 2022-11-22 | Multi-laser radar external parameter self-calibration method, system, terminal equipment and storage medium |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116068535A true CN116068535A (en) | 2023-05-05 |
Family
ID=86172366
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211473733.8A Pending CN116068535A (en) | 2022-11-22 | 2022-11-22 | Multi-laser radar external parameter self-calibration method, system, terminal equipment and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116068535A (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117876504A (en) * | 2024-03-12 | 2024-04-12 | 苏州艾吉威机器人有限公司 | Laser radar external parameter calibration method and device applied to AGV |
| CN117876504B (en) * | 2024-03-12 | 2024-06-04 | 苏州艾吉威机器人有限公司 | Laser radar external parameter calibration method and device applied to AGV |
-
2022
- 2022-11-22 CN CN202211473733.8A patent/CN116068535A/en active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117876504A (en) * | 2024-03-12 | 2024-04-12 | 苏州艾吉威机器人有限公司 | Laser radar external parameter calibration method and device applied to AGV |
| CN117876504B (en) * | 2024-03-12 | 2024-06-04 | 苏州艾吉威机器人有限公司 | Laser radar external parameter calibration method and device applied to AGV |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112654886B (en) | External parameter calibration method, device, equipment and storage medium | |
| CN107179086B (en) | Drawing method, device and system based on laser radar | |
| Mill et al. | Combined 3D building surveying techniques–terrestrial laser scanning (TLS) and total station surveying for BIM data management purposes | |
| CN109101709A (en) | The site construction management system that 3D laser scanner technique is combined with BIM technology | |
| CN102622747B (en) | Camera parameter optimization method for vision measurement | |
| CN112907722A (en) | Building information model generation method, system, device and storage medium | |
| CN111006646B (en) | Method for monitoring construction progress based on unmanned aerial vehicle oblique photography measurement technology | |
| CN112305559A (en) | Power transmission line distance measuring method, device and system based on ground fixed-point laser radar scanning and electronic equipment | |
| CN112825190B (en) | Precision evaluation method, system, electronic equipment and storage medium | |
| CN111862215B (en) | Computer equipment positioning method and device, computer equipment and storage medium | |
| WO2021098808A1 (en) | Method and system for determining laser tracker station, electronic device, and medium | |
| CN114488099A (en) | Laser radar coefficient calibration method and device, electronic equipment and storage medium | |
| JP6099728B1 (en) | Laser measurement system and laser measurement method | |
| CN109163708B (en) | Optimization method, device and medium for unmanned aerial vehicle large-scale topographic map house line | |
| CN114913146A (en) | Inclined point cloud plane precision calculation method and system based on house corner points | |
| CN107504959B (en) | Method for measuring house wall base outline by utilizing inclined aerial image | |
| WO2022134328A1 (en) | Construction method and apparatus for building information model | |
| CN116068535A (en) | Multi-laser radar external parameter self-calibration method, system, terminal equipment and storage medium | |
| CN116524109B (en) | WebGL-based three-dimensional bridge visualization method and related equipment | |
| CN111694009A (en) | Positioning system, method and device | |
| CN110853098A (en) | Robot positioning method, device, equipment and storage medium | |
| CN115018983A (en) | Phase-shifting transformer site selection method, device, electronic equipment and storage medium | |
| CN115859445A (en) | Bridge assembly simulation test method based on total station scanning technology and reverse algorithm | |
| CN116129064A (en) | Electronic map generation method, device, equipment and storage medium | |
| CN114004949A (en) | Airborne point cloud assisted mobile measurement system arrangement parameter calibration method and system |
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 |