CN114460564A - Laser radar calibration method and device and operation machine - Google Patents
Laser radar calibration method and device and operation machine Download PDFInfo
- Publication number
- CN114460564A CN114460564A CN202210173064.6A CN202210173064A CN114460564A CN 114460564 A CN114460564 A CN 114460564A CN 202210173064 A CN202210173064 A CN 202210173064A CN 114460564 A CN114460564 A CN 114460564A
- Authority
- CN
- China
- Prior art keywords
- coordinate system
- coordinate
- laser radar
- determining
- lower vehicle
- 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
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
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
- G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
- G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles
Abstract
The invention relates to the field of engineering machinery, and provides a laser radar calibration method, a laser radar calibration device and operation machinery, wherein the method comprises the following steps: respectively acquiring a first data set of a preset calibration plate in a lower vehicle coordinate system and a second data set of the preset calibration plate in each laser radar coordinate system; the first data set comprises first coordinate values of all characteristic points in a preset calibration plate in a lower vehicle coordinate system, and the second data set comprises second coordinate values of all the characteristic points in a corresponding laser radar coordinate system; acquiring a first attitude relationship between each laser radar coordinate system and a lower vehicle coordinate system based on the first coordinate value and the second coordinate value; and determining a second attitude relationship between the upper vehicle body coordinate system and the lower vehicle body coordinate system of the working machine in the second coordinate value acquisition process, and determining a third attitude relationship between each laser radar coordinate system and the upper vehicle body coordinate system based on the first attitude relationship and the second attitude relationship. The invention can keep the coordinate system of each laser radar consistent with the coordinate system of the upper vehicle body of the working machine.
Description
Technical Field
The invention relates to the technical field of engineering machinery, in particular to a laser radar calibration method, a laser radar calibration device and operation machinery.
Background
With the continuous promotion of the construction of novel infrastructures, the engineering machinery is continuously developed to digitization and intellectualization, and is widely applied to the fields of construction and construction, resource exploitation, emergency rescue and relief, archaeological excavation and the like. Because the engineering machinery is usually in a remote working environment with severe conditions, the skilled operation of the engineering machinery has high requirements on the capability of drivers and long training period. The unmanned engineering machinery is a preferred scheme for solving the problems, can replace workers to work under severe working conditions, can greatly reduce safety accidents during operation, and can carry out complicated and repeated work for a long time, thereby improving the overall operation efficiency.
The unmanned engineering machine mainly comprises three parts of perception, planning and control, wherein a perception module is mainly responsible for perceiving the surrounding environment and transmitting perception information to a planning layer, the planning layer plans based on the perception information and transmits the information to a control layer, and the control layer transmits a control command to an execution mechanism to perform corresponding execution. The sensing layer plays an important role in the unmanned construction machine as a necessary input of the unmanned construction machine.
The laser radar can feed back the depth information of the surrounding environment in real time, so that the laser radar is a necessary sensor on unmanned engineering machinery. The laser radar mainly comprises a mechanical laser radar and a solid laser radar, wherein the mechanical laser radar adopts a traditional photoelectric module, and the more the wire harness is, the larger the size is; solid-state laser radar mainly includes rotating mirror formula laser radar and galvanometer formula laser radar, and is inequality with traditional mechanical type laser radar principle, and solid-state laser radar can accomplish higher pencil with less volume, and the scanning effect is better, but solid-state laser radar self shortcoming lies in that its horizontal field of view angle is less, and observation scope is limited.
Calibration work of multiple lidar is especially necessary because the field of view of a single lidar is limited, and multiple lidar is usually required to be deployed on a construction machine to realize coverage of an area of interest. The existing calibration of multiple laser radars mainly aims at calibrating the coordinate system relation among the laser radars, and the engineering machinery is difficult to be effectively controlled according to the sensing information of each laser radar.
Disclosure of Invention
Aiming at the problems in the prior art, the invention provides a laser radar calibration method, a laser radar calibration device and an operation machine.
The invention provides a laser radar calibration method, which comprises the following steps:
respectively acquiring a first data set of a preset calibration plate in a lower vehicle coordinate system of the operating machine and a second data set of the preset calibration plate in each laser radar coordinate system; the first data group comprises first coordinate values of all feature points in the preset calibration plate in the lower vehicle coordinate system, and the second data group comprises second coordinate values of all feature points in the preset calibration plate in the corresponding laser radar coordinate system;
acquiring a first attitude relationship between each laser radar coordinate system and the lower vehicle coordinate system based on the first coordinate value and the second coordinate value;
and determining a second attitude relationship between an upper vehicle body coordinate system and a lower vehicle body coordinate system of the working machine in the second coordinate value acquisition process, determining a third attitude relationship between each laser radar coordinate system and the upper vehicle body coordinate system based on the first attitude relationship and the second attitude relationship, and calibrating each laser radar according to the third attitude relationship.
According to the laser radar calibration method provided by the invention, bulges are arranged at each characteristic point in the preset calibration plate, and a calibration probe is arranged at a working device of the operation machine;
the method for acquiring a first data set of a preset calibration plate in a lower vehicle coordinate system of the operating machine comprises the following steps:
controlling the calibration probe to move so that the calibration probe touches each protrusion respectively;
and determining a first coordinate value of the corresponding characteristic point according to the position of the calibration probe when the calibration probe touches the bulge.
According to the laser radar calibration method provided by the invention, the obtaining of the first attitude relationship between each laser radar coordinate system and the lower vehicle coordinate system based on the first coordinate value and the second coordinate value comprises the following steps:
determining a homogeneous transformation matrix between the laser radar coordinate system and the lower vehicle body coordinate system so as to minimize an error between the first coordinate value and the second coordinate value after coordinate transformation; the second coordinate value after coordinate transformation is obtained by performing coordinate transformation according to the homogeneous transformation matrix;
and taking the homogeneous transformation matrix as a first attitude relation between the laser radar coordinate system and the lower vehicle body coordinate system.
According to the laser radar calibration method provided by the invention, the determining of the homogeneous transformation matrix between the laser radar coordinate system and the lower vehicle coordinate system so as to minimize the error between the first coordinate value and the second coordinate value after coordinate transformation comprises the following steps:
determining a first mass center of each feature point in the lower vehicle coordinate system based on a first coordinate value of each feature point, and determining a second mass center of each feature point in the laser radar coordinate system based on a second coordinate value of each feature point;
determining a first difference between each of the first coordinate values and the first centroid and a second difference between each of the second coordinate values and the second centroid;
constructing a difference matrix based on each first difference value and each second difference value;
decomposing the difference matrix by adopting a Singular Value Decomposition (SVD) method;
determining the homogeneous transformation matrix based on a decomposition result.
According to the laser radar calibration method provided by the invention, the homogeneous transformation matrix comprises a rotation matrix and a translation matrix;
said determining said homogeneous transformation matrix based on decomposition results comprises:
determining the rotation matrix based on the decomposition result under the condition that the decomposition result meets the preset condition;
based on the rotation matrix, determining the translation matrix with the first centroid and the second centroid coincident as targets.
According to the laser radar calibration method provided by the invention, the determining of the third attitude relationship between each laser radar coordinate system and the upper vehicle body coordinate system based on the first attitude relationship and the second attitude relationship comprises the following steps:
and multiplying the coordinate transformation matrix corresponding to the first position relation and the coordinate transformation matrix corresponding to the second position relation to obtain a third position relation between the laser radar coordinate system and the upper vehicle body coordinate system.
The invention also provides a laser radar calibration device, comprising:
the system comprises a data acquisition module, a data processing module and a data processing module, wherein the data acquisition module is used for respectively acquiring a first data set of a preset calibration plate in a lower vehicle coordinate system of the working machine and a second data set of the preset calibration plate in each laser radar coordinate system; the first data group comprises first coordinate values of all feature points in the preset calibration plate in the lower vehicle coordinate system, and the second data group comprises second coordinate values of all feature points in the preset calibration plate in the corresponding laser radar coordinate system;
the first calculation module is used for acquiring a first attitude relationship between each laser radar coordinate system and the lower vehicle coordinate system based on the first coordinate value and the second coordinate value;
and the second calculation module is used for determining a second attitude relationship between an upper vehicle body coordinate system and a lower vehicle body coordinate system of the working machine in the second coordinate value acquisition process, determining a third attitude relationship between each laser radar coordinate system and the upper vehicle body coordinate system based on the first attitude relationship and the second attitude relationship, and calibrating each laser radar according to the third attitude relationship.
The invention also provides a working machine comprising a control device for executing the laser radar calibration method according to any one of the above.
The invention also provides an electronic device, which comprises a memory, a processor and a computer program stored on the memory and capable of running on the processor, wherein the processor executes the program to realize the laser radar calibration method.
The present invention also provides a non-transitory computer readable storage medium having stored thereon a computer program which, when executed by a processor, implements a lidar calibration method as described in any of the above.
According to the laser radar calibration method, the laser radar calibration device and the operation machine, the first coordinate value of each characteristic point in the calibration plate in the lower vehicle coordinate system of the operation machine and the second coordinate value of each characteristic point in each laser radar coordinate system are preset, so that the first attitude relationship between each laser radar coordinate system and the lower vehicle coordinate system of the operation machine is obtained; meanwhile, a third posture relation between each laser radar coordinate system and the upper vehicle body coordinate system is determined through a second posture relation between the upper vehicle body coordinate system and the lower vehicle body coordinate system of the working machine in the first posture relation and the second coordinate value obtaining process, and each laser radar is calibrated according to the third posture relation, so that each laser radar coordinate system is consistent with the upper vehicle body coordinate system of the working machine, and the working machine can be effectively controlled according to sensing information of each laser radar.
Drawings
In order to more clearly illustrate the technical solutions of the present invention or the prior art, the drawings needed to be used in the description of the embodiments or the prior art will be briefly described below, and it is obvious that the drawings in the following description are some embodiments of the present invention, and it is obvious for those skilled in the art to obtain other drawings based on these drawings without creative efforts.
FIG. 1 is a schematic flow chart of a laser radar calibration method provided by the present invention;
FIG. 2 is a schematic structural diagram of a preset calibration plate provided in the present invention;
FIG. 3 is a schematic diagram of a coordinate system transformation among a laser radar coordinate system, a lower vehicle body coordinate system and an upper vehicle body coordinate system provided by the present invention;
FIG. 4 is a schematic structural diagram of a lidar calibration apparatus provided by the present invention;
fig. 5 is a schematic structural diagram of an electronic device provided in the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention clearer, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings, and it is obvious that the described embodiments are some, but not all embodiments of the present invention. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
The lidar calibration method of the present invention will be described with reference to fig. 1 to 3. The laser radar calibration method is executed by a control device of the working machine or software and/or hardware in the control device. Fig. 1 is a schematic flow chart of a laser radar calibration method of the present invention, and as shown in fig. 1, the laser radar calibration method of the present invention includes:
s101, respectively acquiring a first data set of a preset calibration plate in a lower vehicle coordinate system of the working machine and a second data set of the preset calibration plate in each laser radar coordinate system; the first data set comprises first coordinate values of all feature points in the preset calibration plate in the lower vehicle coordinate system, and the second data set comprises second coordinate values of all feature points in the preset calibration plate in the corresponding laser radar coordinate system.
Specifically, a working machine such as an excavator, a loading/unloading machine, or the like includes an upper vehicle body and a lower vehicle body, and a plurality of laser radars are mounted on the upper vehicle body of the working machine. The preset calibration plate can be set according to actual requirements, and the preset calibration plate comprises a plurality of feature points. A first data set of the preset calibration plate in a lower vehicle coordinate system of the working machine comprises first coordinate values of all characteristic points in the lower vehicle coordinate system; and each laser radar coordinate system corresponds to a second data group, and the second data group comprises second coordinate values of each characteristic point in the corresponding laser radar coordinate system. By acquiring a first coordinate value of each feature point on the calibration plate in a lower vehicle coordinate system of the working machine and a second coordinate value of each feature point on the calibration plate in a corresponding laser radar coordinate system, the pose relationship between each laser radar coordinate system and the lower vehicle coordinate system can be determined, that is, the lower vehicle coordinate system can be used as a parent coordinate system to establish a coordinate tree, so that the relative pose relationship between each laser radar coordinate system and the pose relationship of each laser radar coordinate system relative to the lower vehicle coordinate system can be determined quickly and accurately. As an alternative embodiment, the preset calibration board includes 9 feature points, and it is understood that the feature points may also be set according to the precision requirement. The method for acquiring the first coordinate value of each feature point on the calibration plate in the lower body coordinate system of the working machine may be set according to actual requirements, for example, manual measurement may be performed by a measuring tool such as a tape measure, or automatic measurement may be performed by a control working machine. The method for obtaining the second coordinate values of the feature points on the calibration plate in the laser radar coordinate systems may also be set according to actual requirements, for example, the operation machine is rotated, so that the laser radar to be calibrated can detect the feature points on the preset calibration plate, the laser radar is used to scan the feature points on the preset calibration plate to obtain point cloud data, the point cloud data is used to fit the profiles of the feature points on the preset calibration plate, and the centers of the fitting results of the profiles are used as the second coordinate values.
S102, acquiring a first attitude relationship between each laser radar coordinate system and the lower vehicle coordinate system based on the first coordinate value and the second coordinate value.
Specifically, a first attitude relationship between the laser radar coordinate system and the lower vehicle body coordinate system is a coordinate transformation matrix for transforming the laser radar coordinate system to the lower vehicle body coordinate system. The coordinate transformation matrix for transforming the laser radar coordinate system to the lower vehicle coordinate system may be obtained based on each first coordinate Value and the second coordinate Value corresponding to the laser radar to be calibrated, and the specific obtaining method may be set according to actual requirements, for example, the coordinate transformation matrix is calculated by an ICP (Iterative Closest Point) algorithm or an SVD (Singular Value Decomposition) algorithm.
S103, determining a second attitude relationship between an upper vehicle body coordinate system and a lower vehicle body coordinate system of the working machine in the second coordinate value obtaining process, determining a third attitude relationship between each laser radar coordinate system and the upper vehicle body coordinate system based on the first attitude relationship and the second attitude relationship, and calibrating each laser radar according to the third attitude relationship.
Specifically, part of the working machine is such that the upper body swings on the lower body during working, such as an excavator, and the lidar is generally mounted on the upper body, and therefore, the above body coordinate system is required as a reference, that is, each lidar coordinate system is required to be converted into the upper body coordinate system, during automatic control of the working machine based on sensing data of the lidar. In the embodiment of the invention, a second attitude relationship between the upper vehicle body coordinate system and the lower vehicle body coordinate system in the second coordinate value acquisition process is determined; under the condition that the upper vehicle body cannot rotate on the lower vehicle body, the upper vehicle body coordinate system is the same as the lower vehicle body coordinate system, namely the third attitude relationship is the same as the first attitude relationship; under the condition that the upper vehicle body can rotate on the lower vehicle body, the second attitude relationship between the upper vehicle body coordinate system and the lower vehicle body coordinate system can be obtained through the motion model of the operating machine, and the third attitude relationship is determined according to the first attitude relationship and the second attitude relationship. After the third attitude relationship is determined, the corresponding laser radars can be calibrated according to the third attitude relationship, so that the coordinate systems of the laser radars are consistent with the coordinate system of the upper vehicle body of the working machine, and the working machine can be effectively controlled according to the sensing information of the laser radars.
Therefore, in the embodiment of the invention, a first coordinate value of each feature point in the calibration plate in the vehicle coordinate system of the working machine and a second coordinate value of each feature point in each laser radar coordinate system are preset to obtain a first attitude relationship between each laser radar coordinate system and the vehicle coordinate system of the working machine; meanwhile, a third posture relation between each laser radar coordinate system and the upper vehicle body coordinate system is determined through a second posture relation between the upper vehicle body coordinate system and the lower vehicle body coordinate system of the working machine in the first posture relation and the second coordinate value obtaining process, and each laser radar is calibrated according to the third posture relation, so that each laser radar coordinate system is consistent with the upper vehicle body coordinate system of the working machine, and the working machine can be effectively controlled according to sensing information of each laser radar.
Based on the embodiment, the preset calibration plate is provided with the bulges at the characteristic points, and the working device of the working machine is provided with the calibration probe;
the method for acquiring a first data set of a preset calibration plate in a lower vehicle coordinate system of the operating machine comprises the following steps:
controlling the calibration probe to move so that the calibration probe touches each protrusion respectively;
and determining a first coordinate value of the corresponding characteristic point according to the position of the calibration probe when the calibration probe touches the bulge.
Specifically, each feature point of the preset calibration plate is provided with a protrusion, so that the calibration probe can be conveniently touched, the center of the protrusion is overlapped with the center of the corresponding feature point, the shape of the protrusion can be set according to actual requirements, the position is not particularly limited, and the protrusion can be cylindrical. The calibration probe may be provided in a work apparatus, or may be used in place of (i.e. to unload) a work apparatus, such as a bucket of an excavator or loader, and to mount the calibration probe at the location of the work apparatus. Therefore, the calibration probe can be controlled to move correspondingly by operating the control handle of the operation machine, so that the calibration probe respectively touches each protrusion, and the first coordinate value of each characteristic point can be determined by calibrating the position of the probe according to the touch of each protrusion. Because the calibration probe is arranged at the working device of the working machine, the position of the calibration probe when the projection is touched can be directly read from a controller of the working machine, and the position of the calibration probe when the projection is touched is the first coordinate value of the characteristic point, so that the automatic acquisition of the first coordinate value of each characteristic point in the preset calibration plate in a body coordinate system of the working machine is realized.
In the existing method, in the process of obtaining the coordinate values of the feature points on the calibration plate in the vehicle body coordinate system, measurement tools such as a tape measure, a plumb line and a laser level meter are generally needed to be used for manual measurement, so that the measurement precision is low, and time and labor are wasted. The embodiment of the invention can automatically acquire the first coordinate value by setting the calibration plate and the calibration probe, effectively improves the precision of the first coordinate value and saves time and labor.
In addition, the preset calibration plate can be further provided with a standard block, the standard block can be triangular and used for indicating the position and posture of the preset calibration plate, so that when the first coordinate value and the second coordinate value are obtained, the position and posture of the preset calibration plate can be effectively distinguished, each feature point on the preset calibration plate is further distinguished, and the accuracy of the calculation result of the first position and posture relation between the laser radar coordinate system and the lower vehicle body coordinate system is guaranteed. Taking the example of including 9 feature points, a schematic structural diagram of the preset calibration board is shown in fig. 2.
Based on any one of the above embodiments, the obtaining a first attitude relationship between each of the lidar coordinate systems and the lower vehicle coordinate system based on the first coordinate value and the second coordinate value includes:
determining a homogeneous transformation matrix between the laser radar coordinate system and the lower vehicle body coordinate system so as to minimize an error between the first coordinate value and the second coordinate value after coordinate transformation; the second coordinate value after coordinate transformation is obtained by performing coordinate transformation according to the homogeneous transformation matrix;
and taking the homogeneous transformation matrix as a first attitude relation between the laser radar coordinate system and the lower vehicle body coordinate system.
Specifically, the error may be a mean square error, that is, a homogeneous transformation matrix between the laser radar coordinate system and the lower vehicle coordinate system is determined with a target of minimum mean square error of each first coordinate value and the coordinate-transformed second coordinate value, and the homogeneous transformation matrix represents a first attitude relationship between the laser radar coordinate system and the lower vehicle coordinate system. Mean square error RE of each first coordinate value and coordinate-transformed second coordinate value2As shown in formula (1):
in the formula, N is the number of characteristic points in a preset calibration plate; p is a radical ofi、qiA first coordinate value and a second coordinate value of the ith characteristic point respectively, and a vector of three rows and one column (the first coordinate value and the second coordinate value both comprise coordinates in three directions of x, y and z); r, t are rotation and translation matrices in the homogeneous transformation matrix, respectively.
The embodiment of the invention determines the homogeneous transformation matrix of the laser radar coordinate system and the lower vehicle coordinate system by taking the minimum mean square error of each first coordinate value and the coordinate-transformed second coordinate value as a target, and can ensure the effectiveness and accuracy of the homogeneous transformation matrix.
Based on any one of the above embodiments, the determining a homogeneous transformation matrix between the laser radar coordinate system and the lower vehicle coordinate system to minimize an error between the first coordinate value and the coordinate-transformed second coordinate value includes:
determining a first mass center of each feature point in the lower vehicle coordinate system based on a first coordinate value of each feature point, and determining a second mass center of each feature point in the laser radar coordinate system based on a second coordinate value of each feature point;
determining a first difference between each of the first coordinate values and the first centroid and a second difference between each of the second coordinate values and the second centroid;
constructing a difference matrix based on each first difference value and each second difference value;
decomposing the difference matrix by adopting a Singular Value Decomposition (SVD) method;
determining the homogeneous transformation matrix based on a decomposition result.
Specifically, the first centroid is the mean value of the coordinate values of each feature point in the preset calibration plate in the lower vehicle coordinate system, the second centroid is the mean value of the coordinate values of each feature point in the preset calibration plate in the laser radar coordinate system, and the laser radar coordinate systems are unified to the lower vehicle coordinate system, so that the coincidence of the first centroid and the second centroid needs to be ensured. First center of mass
And a second center of mass
Respectively shown in formula (2) and formula (3):
the calculation of the first difference between each first coordinate value and the first centroid and the calculation of the second difference between each second coordinate value and the second centroid are respectively shown as formula (4) and formula (5):
in the formula, pi' is a distance from the first coordinate value of the ith feature point to the first centroid; q. q ofi' is a distance from the second coordinate value of the ith feature point to the second centroid.
After obtaining the first difference and the second difference, each first difference p is comparedi' with a corresponding second difference qiMultiplying and then summing to obtain a difference matrix H, as shown in formula (6):
in the formula, T is transposition operation; p is a radical ofi' and qi' are vectors of three rows and one column, and the obtained difference matrix H is the vector of three rows and three columns.
The difference matrix H is subjected to SVD decomposition as shown in equation (7):
H＝USVT (7)
in the formula, U and V are both unitary matrixes and are both 3 x 3 matrixes; s is an eigenvalue matrix, which is a 3 × 3 matrix, and is 0 except for elements on the main diagonal, and each element on the main diagonal is called a singular value.
According to the unitary matrix U and V obtained after decomposition, a homogeneous transformation matrix can be determined, and a specific method for determining the homogeneous transformation matrix can be set according to actual requirements without specific limitation. The homogeneous transformation matrix determined by the unitary matrixes U and V can effectively ensure that the mean square error of each first coordinate value and the second coordinate value after coordinate transformation is minimum.
In the prior art, an ICP (inductively coupled plasma) registration method is generally adopted to perform point cloud registration, so that the problem of mismatching exists. According to the embodiment of the invention, the first difference value of each first coordinate value and the first mass center and the second difference value of each second coordinate value and the second mass center are determined, the difference matrix is constructed based on each first difference value and each second difference value, the SVD decomposition is carried out on the difference matrix, the homogeneous transformation matrix is determined according to the decomposition result, the homogeneous transformation matrix can be determined by taking the first mass center and the second mass center as the reference, the effectiveness and the accuracy of the obtained homogeneous transformation matrix are ensured, and meanwhile, the problem of mismatching in the ICP algorithm can be effectively avoided.
Based on any of the above embodiments, the homogeneous transformation matrix comprises a rotation matrix and a translation matrix;
said determining said homogeneous transformation matrix based on decomposition results comprises:
determining the rotation matrix based on the decomposition result under the condition that the decomposition result meets the preset condition;
based on the rotation matrix, determining the translation matrix with the first centroid and the second centroid coincident as targets.
Specifically, the homogeneous transformation matrix includes a rotation matrix R and a translation matrix t, where the rotation matrix R has a greater influence on the first attitude relationship between the laser radar coordinate system and the lower vehicle body coordinate system, and therefore, each first coordinate value and the coordinate-transformed second coordinate valueMean square error of value RE2The translation matrix t can be ignored, and the simplification result is shown in equation (8):
a homogeneous transformation matrix is determined based on the decomposition results, i.e. a rotation matrix R is determined based on the decomposition results. Setting matrix X ═ VUTX is a substitute symbol without specific meaning; and calculating a determinant det (X) of X, if det (X) is satisfied, the rotation matrix R is X, if det (X) is 1, the acquired first coordinate value and/or second coordinate value is invalid, the first coordinate value and the second coordinate value need to be checked or acquired again, and the first attitude relationship between the corresponding laser radar coordinate system and the lower vehicle body coordinate system is acquired according to the acquired first coordinate value and second coordinate value.
After the rotation matrix R is obtained, the translation matrix t can be determined by taking the coincidence of the first centroid and the second centroid as a target, as shown in formula (9):
and after the rotation matrix R and the translation matrix t are determined, the first attitude relationship between each laser radar coordinate system and the lower vehicle coordinate system can be obtained.
By the method provided by the embodiment of the invention, the effectiveness of the first coordinate value and/or the second coordinate value can be effectively detected, so that the effectiveness and the accuracy of the first attitude relationship between the obtained laser radar coordinate system and the lower vehicle coordinate system are ensured.
Based on any one of the above embodiments, the determining a third attitude relationship between each of the lidar coordinate systems and the upper vehicle body coordinate system based on the first attitude relationship and the second attitude relationship includes:
and multiplying the coordinate transformation matrix corresponding to the first position relation and the coordinate transformation matrix corresponding to the second position relation to obtain a third position relation between the laser radar coordinate system and the upper vehicle body coordinate system.
Specifically, a coordinate transformation matrix corresponding to the first attitude relationship is a coordinate transformation matrix transformed from a laser radar coordinate system to a lower vehicle body coordinate system, a coordinate transformation matrix corresponding to the second attitude relationship is a coordinate transformation matrix transformed from a lower vehicle body coordinate system to an upper vehicle body coordinate system, and a matrix obtained by multiplying the coordinate transformation matrix corresponding to the first attitude relationship and the coordinate transformation matrix corresponding to the second attitude relationship is a coordinate transformation matrix transformed from the laser radar coordinate system to the upper vehicle body coordinate system, and the matrix represents a third attitude relationship between the laser radar coordinate system and the upper vehicle body coordinate system. A schematic diagram of the coordinate system transformation between the lidar coordinate system, the lower body coordinate system, and the upper body coordinate system is shown in fig. 3. In fig. 3, UC is a lower body coordinate system, C is an upper body coordinate system, L is a lidar coordinate system,
is a coordinate transformation matrix corresponding to the first attitude relationship between the laser radar coordinate system and the lower vehicle body coordinate system,
is a coordinate transformation matrix corresponding to the second attitude relationship between the lower vehicle body coordinate system and the upper vehicle body coordinate system,
a coordinate transformation matrix corresponding to the third attitude relationship between the lidar coordinate system and the upper vehicle body coordinate system, i.e.
The laser radar calibration device provided by the invention is described below, and the laser radar calibration device described below and the laser radar calibration method described above can be referred to correspondingly. As shown in fig. 4, the apparatus includes:
a data obtaining module 410, configured to obtain a first data set of a preset calibration plate in a lower vehicle coordinate system of a working machine and a second data set of the preset calibration plate in each laser radar coordinate system, respectively; the first data group comprises first coordinate values of all feature points in the preset calibration plate in the lower vehicle coordinate system, and the second data group comprises second coordinate values of all feature points in the preset calibration plate in the corresponding laser radar coordinate system;
a first calculating module 420, configured to obtain a first attitude relationship between each laser radar coordinate system and the lower vehicle coordinate system based on the first coordinate value and the second coordinate value;
a second calculating module 430, configured to determine a second pose relationship between an upper vehicle coordinate system and the lower vehicle coordinate system of the working machine in the second coordinate value obtaining process, determine a third pose relationship between each laser radar coordinate system and the upper vehicle coordinate system based on the first pose relationship and the second pose relationship, and calibrate each laser radar according to the third pose relationship.
Based on the embodiment, the preset calibration plate is provided with the bulges at the characteristic points, and the working device of the working machine is provided with the calibration probe;
the data obtaining module 410 is specifically configured to:
controlling the calibration probe to move so that the calibration probe touches each protrusion respectively;
and determining a first coordinate value of the corresponding characteristic point according to the position of the calibration probe when the calibration probe touches the bulge.
Based on any of the above embodiments, the first calculating module 420 is specifically configured to:
determining a homogeneous transformation matrix between the laser radar coordinate system and the lower vehicle body coordinate system so as to minimize an error between the first coordinate value and the second coordinate value after coordinate transformation; the second coordinate value after coordinate transformation is obtained by performing coordinate transformation according to the homogeneous transformation matrix;
and taking the homogeneous transformation matrix as a first attitude relation between the laser radar coordinate system and the lower vehicle body coordinate system.
Based on any of the above embodiments, the first calculating module 420 is specifically configured to:
determining a first mass center of each feature point in the lower vehicle coordinate system based on a first coordinate value of each feature point, and determining a second mass center of each feature point in the laser radar coordinate system based on a second coordinate value of each feature point;
determining a first difference between each of the first coordinate values and the first centroid and a second difference between each of the second coordinate values and the second centroid;
constructing a difference matrix based on each first difference value and each second difference value;
decomposing the difference matrix by adopting a Singular Value Decomposition (SVD) method;
determining the homogeneous transformation matrix based on a decomposition result.
Based on any of the above embodiments, the homogeneous transformation matrix comprises a rotation matrix and a translation matrix;
the first computing module 420 is specifically configured to:
determining the rotation matrix based on the decomposition result under the condition that the decomposition result meets the preset condition;
based on the rotation matrix, determining the translation matrix with the first centroid and the second centroid coincident as targets.
Based on any of the above embodiments, the second calculating module 430 is specifically configured to:
and multiplying the coordinate transformation matrix corresponding to the first position relation and the coordinate transformation matrix corresponding to the second position relation to obtain a third position relation between the laser radar coordinate system and the upper vehicle body coordinate system.
The invention also provides a working machine comprising a control device for executing the steps of the lidar calibration method according to any of the embodiments.
Specifically, the work machine may be a construction machine such as an excavator, a loader, or the like.
Fig. 5 illustrates a physical structure diagram of an electronic device, which may include, as shown in fig. 5: a processor (processor)510, a communication Interface (Communications Interface)520, a memory (memory)530 and a communication bus 540, wherein the processor 510, the communication Interface 520 and the memory 530 communicate with each other via the communication bus 540. Processor 510 may invoke logic instructions in memory 530 to perform a lidar calibration method that includes: respectively acquiring a first data set of a preset calibration plate in a lower vehicle coordinate system of the operating machine and a second data set of the preset calibration plate in each laser radar coordinate system; the first data group comprises first coordinate values of all feature points in the preset calibration plate in the lower vehicle coordinate system, and the second data group comprises second coordinate values of all feature points in the preset calibration plate in the corresponding laser radar coordinate system;
acquiring a first attitude relationship between each laser radar coordinate system and the lower vehicle coordinate system based on the first coordinate value and the second coordinate value;
and determining a second attitude relationship between an upper vehicle body coordinate system and a lower vehicle body coordinate system of the working machine in the second coordinate value acquisition process, determining a third attitude relationship between each laser radar coordinate system and the upper vehicle body coordinate system based on the first attitude relationship and the second attitude relationship, and calibrating each laser radar according to the third attitude relationship.
Furthermore, the logic instructions in the memory 530 may be implemented in the form of software functional units and stored in a computer readable storage medium when the software functional units are sold or used as independent products. Based on such understanding, the technical solution of the present invention may be embodied in the form of a software product, which is stored in a storage medium and includes instructions for causing a computer device (which may be a personal computer, a server, or a network device) to execute all or part of the steps of the method according to the embodiments of the present invention. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), a magnetic disk or an optical disk, and other various media capable of storing program codes.
In another aspect, the present invention also provides a computer program product, the computer program product including a computer program stored on a non-transitory computer-readable storage medium, the computer program including program instructions, when the program instructions are executed by a computer, the computer being capable of executing the lidar calibration method provided by the above methods, the method including: respectively acquiring a first data set of a preset calibration plate in a lower vehicle coordinate system of the operating machine and a second data set of the preset calibration plate in each laser radar coordinate system; the first data group comprises first coordinate values of all feature points in the preset calibration plate in the lower vehicle coordinate system, and the second data group comprises second coordinate values of all feature points in the preset calibration plate in the corresponding laser radar coordinate system;
acquiring a first attitude relationship between each laser radar coordinate system and the lower vehicle coordinate system based on the first coordinate value and the second coordinate value;
and determining a second attitude relationship between an upper vehicle body coordinate system and a lower vehicle body coordinate system of the working machine in the second coordinate value acquisition process, determining a third attitude relationship between each laser radar coordinate system and the upper vehicle body coordinate system based on the first attitude relationship and the second attitude relationship, and calibrating each laser radar according to the third attitude relationship.
In yet another aspect, the present invention further provides a non-transitory computer-readable storage medium, on which a computer program is stored, the computer program being implemented by a processor to perform the laser radar calibration method provided in the above aspects, the method including: respectively acquiring a first data set of a preset calibration plate in a lower vehicle coordinate system of the operating machine and a second data set of the preset calibration plate in each laser radar coordinate system; the first data group comprises first coordinate values of all feature points in the preset calibration plate in the lower vehicle coordinate system, and the second data group comprises second coordinate values of all feature points in the preset calibration plate in the corresponding laser radar coordinate system;
acquiring a first attitude relationship between each laser radar coordinate system and the lower vehicle coordinate system based on the first coordinate value and the second coordinate value;
and determining a second attitude relationship between an upper vehicle body coordinate system and a lower vehicle body coordinate system of the working machine in the second coordinate value acquisition process, determining a third attitude relationship between each laser radar coordinate system and the upper vehicle body coordinate system based on the first attitude relationship and the second attitude relationship, and calibrating each laser radar according to the third attitude relationship.
The above-described embodiments of the apparatus are merely illustrative, and the units described as separate parts may or may not be physically separate, and parts displayed as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the solution of the present embodiment. One of ordinary skill in the art can understand and implement it without inventive effort.
Through the above description of the embodiments, those skilled in the art will clearly understand that each embodiment can be implemented by software plus a necessary general hardware platform, and certainly can also be implemented by hardware. With this understanding in mind, the above-described technical solutions may be embodied in the form of a software product, which can be stored in a computer-readable storage medium such as ROM/RAM, magnetic disk, optical disk, etc., and includes instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to execute the methods described in the embodiments or some parts of the embodiments.
Finally, it should be noted that: the above examples are only intended to illustrate the technical solution of the present invention, but not to limit it; although the present invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some technical features may be equivalently replaced; and such modifications or substitutions do not depart from the spirit and scope of the corresponding technical solutions of the embodiments of the present invention.
Claims (10)
1. A laser radar calibration method is characterized by comprising the following steps:
respectively acquiring a first data set of a preset calibration plate in a lower vehicle coordinate system of the operating machine and a second data set of the preset calibration plate in each laser radar coordinate system; the first data group comprises first coordinate values of all feature points in the preset calibration plate in the lower vehicle coordinate system, and the second data group comprises second coordinate values of all feature points in the preset calibration plate in the corresponding laser radar coordinate system;
acquiring a first attitude relationship between each laser radar coordinate system and the lower vehicle coordinate system based on the first coordinate value and the second coordinate value;
and determining a second attitude relationship between an upper vehicle body coordinate system and a lower vehicle body coordinate system of the working machine in the second coordinate value acquisition process, determining a third attitude relationship between each laser radar coordinate system and the upper vehicle body coordinate system based on the first attitude relationship and the second attitude relationship, and calibrating each laser radar according to the third attitude relationship.
2. The lidar calibration method according to claim 1, wherein a protrusion is provided at each characteristic point in the preset calibration plate, and a calibration probe is provided at a working device of the working machine;
the method for acquiring a first data set of a preset calibration plate in a lower vehicle coordinate system of the operating machine comprises the following steps:
controlling the calibration probe to move so that the calibration probe touches each protrusion respectively;
and determining a first coordinate value of the corresponding characteristic point according to the position of the calibration probe when the calibration probe touches the bulge.
3. The lidar calibration method according to claim 1, wherein obtaining a first attitude relationship between each lidar coordinate system and the lower vehicle coordinate system based on the first coordinate value and the second coordinate value comprises:
determining a homogeneous transformation matrix between the laser radar coordinate system and the lower vehicle body coordinate system so as to minimize an error between the first coordinate value and the second coordinate value after coordinate transformation; the second coordinate value after coordinate transformation is obtained by performing coordinate transformation according to the homogeneous transformation matrix;
and taking the homogeneous transformation matrix as a first attitude relation between the laser radar coordinate system and the lower vehicle body coordinate system.
4. The lidar calibration method according to claim 3, wherein the determining a homogeneous transformation matrix between the lidar coordinate system and the lower vehicle coordinate system to minimize an error between the first coordinate value and the second coordinate value after coordinate transformation comprises:
determining a first mass center of each feature point in the lower vehicle coordinate system based on a first coordinate value of each feature point, and determining a second mass center of each feature point in the laser radar coordinate system based on a second coordinate value of each feature point;
determining a first difference between each of the first coordinate values and the first centroid and a second difference between each of the second coordinate values and the second centroid;
constructing a difference matrix based on each first difference value and each second difference value;
decomposing the difference matrix by adopting a Singular Value Decomposition (SVD) method;
determining the homogeneous transformation matrix based on a decomposition result.
5. The lidar calibration method of claim 4, wherein the homogeneous transformation matrix comprises a rotation matrix and a translation matrix;
said determining said homogeneous transformation matrix based on decomposition results comprises:
determining the rotation matrix based on the decomposition result under the condition that the decomposition result meets the preset condition;
based on the rotation matrix, determining the translation matrix with the first centroid and the second centroid coincident as targets.
6. The lidar calibration method of claim 1, wherein the determining a third pose relationship between each of the lidar coordinate systems and the upper vehicle coordinate system based on the first and second pose relationships comprises:
and multiplying the coordinate transformation matrix corresponding to the first position relation and the coordinate transformation matrix corresponding to the second position relation to obtain a third position relation between the laser radar coordinate system and the upper vehicle body coordinate system.
7. A laser radar calibration device is characterized by comprising:
the system comprises a data acquisition module, a data processing module and a data processing module, wherein the data acquisition module is used for respectively acquiring a first data set of a preset calibration plate in a lower vehicle coordinate system of the working machine and a second data set of the preset calibration plate in each laser radar coordinate system; the first data group comprises first coordinate values of all feature points in the preset calibration plate in the lower vehicle coordinate system, and the second data group comprises second coordinate values of all feature points in the preset calibration plate in the corresponding laser radar coordinate system;
the first calculation module is used for acquiring a first attitude relationship between each laser radar coordinate system and the lower vehicle coordinate system based on the first coordinate value and the second coordinate value;
and the second calculation module is used for determining a second attitude relationship between an upper vehicle body coordinate system and a lower vehicle body coordinate system of the working machine in the second coordinate value acquisition process, determining a third attitude relationship between each laser radar coordinate system and the upper vehicle body coordinate system based on the first attitude relationship and the second attitude relationship, and calibrating each laser radar according to the third attitude relationship.
8. A work machine comprising control means for performing a lidar calibration method according to any of claims 1 to 6.
9. An electronic device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, wherein the processor implements the lidar calibration method according to any of claims 1 to 6 when executing the program.
10. A non-transitory computer readable storage medium having stored thereon a computer program, wherein the computer program when executed by a processor implements the lidar calibration method according to any of claims 1 to 6.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210173064.6A CN114460564A (en) | 2022-02-24 | 2022-02-24 | Laser radar calibration method and device and operation machine |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210173064.6A CN114460564A (en) | 2022-02-24 | 2022-02-24 | Laser radar calibration method and device and operation machine |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114460564A true CN114460564A (en) | 2022-05-10 |
Family
ID=81415694
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210173064.6A Pending CN114460564A (en) | 2022-02-24 | 2022-02-24 | Laser radar calibration method and device and operation machine |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114460564A (en) |
-
2022
- 2022-02-24 CN CN202210173064.6A patent/CN114460564A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112654886B (en) | External parameter calibration method, device, equipment and storage medium | |
| CN109975773B (en) | Millimeter wave radar calibration method, device, equipment and storage medium | |
| CN111590593B (en) | Calibration method, device and system of mechanical arm and storage medium | |
| CN112684432B (en) | Laser radar calibration method, device, equipment and storage medium | |
| CN111366938B (en) | Trailer included angle measuring method and device and vehicle | |
| EP3126785B1 (en) | Automatic identification of sensors | |
| CN111380510B (en) | Repositioning method and device and robot | |
| EP3657455A1 (en) | Methods and systems for detecting intrusions in a monitored volume | |
| US20180321352A1 (en) | Pose Estimation Using Radio Frequency Identification (RFID) Tags | |
| CN112241989A (en) | External parameter calibration method and device, computer equipment and storage medium | |
| CN112558043A (en) | Laser radar calibration method and electronic equipment | |
| CN115980680A (en) | Calibration method and device | |
| WO2019114631A1 (en) | Method and device for acquiring tcp coordinates of robot | |
| CN113240745A (en) | Point cloud data calibration method and device, computer equipment and storage medium | |
| CN114460564A (en) | Laser radar calibration method and device and operation machine | |
| CN115100287A (en) | External reference calibration method and robot | |
| CN114485385B (en) | Workpiece coordinate system calibration method, device and system | |
| CN113405532B (en) | Forward intersection measuring method and system based on structural parameters of vision system | |
| CN112344966B (en) | Positioning failure detection method and device, storage medium and electronic equipment | |
| KR102489701B1 (en) | Method and device for generating a pulse signal at a specific position of a moving element | |
| CN113935904A (en) | Laser odometer error correction method, system, storage medium and computing equipment | |
| JP2022031083A (en) | Information processing method, information processing system, and program | |
| CN112612016A (en) | Sensor combined calibration method and device, electronic equipment and storage medium | |
| WO2017007527A9 (en) | Three dimensional direction finder with one dimensional sensor array | |
| WO2020154936A1 (en) | Method and apparatus for calibrating external parameters of radar, and storage medium |
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 |