CN113759347B - Coordinate relation calibration method, device, equipment and medium - Google Patents
Coordinate relation calibration method, device, equipment and medium Download PDFInfo
- Publication number
- CN113759347B CN113759347B CN202011200240.8A CN202011200240A CN113759347B CN 113759347 B CN113759347 B CN 113759347B CN 202011200240 A CN202011200240 A CN 202011200240A CN 113759347 B CN113759347 B CN 113759347B
- Authority
- CN
- China
- Prior art keywords
- change amount
- pose change
- vehicle
- pose
- calculating
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 69
- 230000008859 change Effects 0.000 claims abstract description 155
- 230000009466 transformation Effects 0.000 claims abstract description 29
- 238000004364 calculation method Methods 0.000 claims description 30
- 238000006073 displacement reaction Methods 0.000 claims description 27
- 239000000725 suspension Substances 0.000 claims description 13
- 230000015654 memory Effects 0.000 claims description 11
- 239000011159 matrix material Substances 0.000 claims description 10
- 238000005096 rolling process Methods 0.000 claims description 4
- 238000004590 computer program Methods 0.000 description 11
- 238000010586 diagram Methods 0.000 description 8
- 230000008569 process Effects 0.000 description 7
- 238000004891 communication Methods 0.000 description 6
- 230000006870 function Effects 0.000 description 6
- 238000012545 processing Methods 0.000 description 5
- 238000005516 engineering process Methods 0.000 description 3
- 238000009472 formulation Methods 0.000 description 2
- 230000014509 gene expression Effects 0.000 description 2
- 230000010354 integration Effects 0.000 description 2
- 239000000203 mixture Substances 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 238000007781 pre-processing Methods 0.000 description 2
- 230000002441 reversible effect Effects 0.000 description 2
- 239000000758 substrate Substances 0.000 description 2
- 238000013519 translation Methods 0.000 description 2
- 206010034719 Personality change Diseases 0.000 description 1
- 108010001267 Protein Subunits Proteins 0.000 description 1
- 238000004422 calculation algorithm Methods 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 239000004973 liquid crystal related substance Substances 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 238000004806 packaging method and process Methods 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
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
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)
- Traffic Control Systems (AREA)
Abstract
The present disclosure provides a method for calibrating a coordinate relationship, including: acquiring first sensor data of a laser radar and second sensor data of a vehicle; calculating a first pose variation amount of the laser radar in a preset time interval according to the first sensor data; calculating a second pose change amount of the vehicle within the same preset time interval according to the second sensor data; judging whether the first pose change amount and the second pose change amount are effective data or not; if yes, calculating a coordinate transformation relation between the laser radar and the vehicle according to the first pose change amount and the second pose change amount; and calculating a third pose change amount of the vehicle according to the effective first pose change amount and the second pose change amount, and updating the second pose change amount by using the third pose change amount. The disclosure also provides a calibration device for the coordinate relationship, an electronic device and a readable storage medium.
Description
Technical Field
The present disclosure relates to the field of electronic technologies, and in particular, to a method, an apparatus, a device, and a medium for calibrating a coordinate relationship.
Background
Sensor calibration is the basis of data fusion, belongs to the basic key technology in technologies such as robots, and plays a role in importance. The aim of the calibration between the multi-line laser radar and the vehicle is to solve the coordinate relation between the multi-line laser radar and the coordinate system with the center of the rear axle as the origin of coordinates and the front direction of the vehicle body as the x axis.
In the process of implementing the disclosed concept, the inventor finds that at least the following problems exist in the related art: the calibration between the multi-line laser radar and the vehicle generally needs to be carried out by selecting a calibration object and taking the calibration object as a reference, and the calibration mode is complex to operate and limited by environmental conditions. In addition, the traditional calibration mode is generally static calibration, and cannot meet the dynamic calibration requirement in actual application.
Disclosure of Invention
In view of the above, the present disclosure provides a calibration method for dynamically solving the coordinate relationship between a laser radar and a vehicle body without a calibration object.
One aspect of the present disclosure provides a method for calibrating a coordinate relationship, including: acquiring first sensor data of a laser radar and second sensor data of a vehicle; calculating a first pose variation amount of the laser radar in a preset time interval according to the first sensor data; calculating a second pose change amount of the vehicle within the same preset time interval according to the second sensor data; judging whether the first pose change amount and the second pose change amount are effective data or not; if so, calculating a coordinate transformation relation between the laser radar and the vehicle according to the first pose change amount and the second pose change amount, calculating a third pose change amount of the vehicle according to the effective first pose change amount and the second pose change amount, updating the second pose change amount according to the third pose change amount, and repeatedly executing the operation of calculating the coordinate transformation relation between the laser radar and the vehicle according to the effective first pose change amount and the effective second pose change amount.
According to an embodiment of the disclosure, calculating the first pose change amount of the lidar in the preset time period according to the first sensor data includes: and matching the data of adjacent frames in the first sensor data, and calculating the variation of the rotation angle of the laser radar in the preset time interval according to a matching result.
According to an embodiment of the disclosure, calculating the second pose transformation matrix of the vehicle within the same preset time period according to the second sensor data includes: and calculating the displacement variation and the course angle variation of the vehicle in the preset time interval according to the second sensor data.
According to an embodiment of the present disclosure, according to the kinetic equation:
calculating the displacement variation and the course angle variation in the preset time interval, wherein X represents the displacement of the vehicle in the X-axis direction in the automatic driving vehicle body coordinate system, Y represents the displacement of the vehicle in the Y-axis direction in the automatic driving vehicle body coordinate system,representing the first order reciprocal of X, ++>Representing the first order inversion of YNumber V represents the speed of the center of the rear wheel of the vehicle, l f Indicating the front suspension length of the vehicle, l r Represents the rear overhang length of the vehicle, ψ represents the heading angle, δ f Indicating the steering angle delta of the front wheel r The rear wheel steering angle is represented, beta represents the slip angle, and the calculation formula is as follows:
according to an embodiment of the disclosure, the calculating the coordinate relationship between the lidar and the vehicle according to the first pose change amount and the second pose change amount includes: and solving the coordinate transformation relation according to a hand-eye calibration theory AX=XB, wherein A is a pose transformation matrix obtained by converting the first pose variation, B is a pose transformation matrix obtained by converting the second pose variation, and X is the coordinate transformation relation.
According to an embodiment of the present disclosure, the solving the coordinate transformation relationship according to the hand-eye calibration theory ax=xb includes: converting the ax=xb into quaternions, where the quaternions include quaternions corresponding to a roll angle and quaternions corresponding to a pitch angle of the laser radar; obtaining a first residual error according to the quaternion, and solving a solution when the first residual error is minimum to obtain a rolling angle and a pitch angle of the laser radar; and obtaining a second residual error according to the quaternion and the roll angle and pitch angle obtained by solving, and solving a solution when the second residual error is minimum to obtain displacement of the vehicle in the X-axis direction, displacement of the vehicle in the y-axis direction and a course angle in an automatic driving vehicle body coordinate system.
According to an embodiment of the present disclosure, the method further comprises: and carrying out difference on the obtained rolling angle and pitch angle of the laser radar, displacement of the vehicle in the x-axis direction and displacement and course angle of the vehicle in the y-axis direction and the measured value corresponding to each parameter included in the second sensor data to obtain a difference value of each parameter, and respectively taking root mean square of the difference value of each parameter as a judgment standard of a calibration result.
According to an embodiment of the disclosure, the first pose change amount includes a change amount of a rotation angle of the laser radar within the preset time interval, the second pose change amount includes a change amount of a heading angle of the vehicle within the preset time interval, and the vehicle is a moving vehicle; the judging whether the first pose change amount and the second pose change amount are valid data comprises: judging whether the variation of the rotation angle is equal to the variation of the course angle or not; if the first pose change amount and the second pose change amount are equal, recording the first pose change amount and the second pose change amount; and if the first sensor data and the second sensor data of the laser radar are not equal, returning to the operation of acquiring the first sensor data of the laser radar and the second sensor data of the vehicle.
According to an embodiment of the present disclosure, the calculating the third pose change amount of the vehicle according to the effective first pose change amount and second pose change amount includes: calculating actual values of the front suspension length and the rear suspension length of the vehicle according to the effective first pose change amount and the second pose change amount; and calculating a third pose change amount according to the actual values of the front overhang length and the rear overhang length and the second sensor data, and updating the second pose change amount by using the third pose change amount.
According to an embodiment of the present disclosure, before the determining whether the amount of change in the rotation angle is equal to the amount of change in the heading angle, the method further includes: and eliminating invalid data in the variation of the rotation angle and the variation of the course angle.
Another aspect of the present disclosure provides a coordinate relationship calibration apparatus, including: the acquisition module is used for acquiring first sensor data of the laser radar and second sensor data of the vehicle; the first calculation module is used for calculating a first pose variation amount of the laser radar in a preset time interval according to the first sensor data; the second calculation module is used for calculating a second pose change amount of the vehicle within the same preset time interval according to the second sensor data; the judging module is used for judging whether the first pose change amount and the second pose change amount are effective data or not; the third calculation module is used for calculating the coordinate transformation relation between the laser radar and the vehicle according to the effective first pose variation and the second pose variation; the fourth calculation module is used for calculating a third pose change amount of the vehicle according to the effective first pose change amount and the second pose change amount; and the updating module is used for updating the second pose change amount according to the third pose change amount.
Another aspect of the present disclosure provides an electronic device, comprising: one or more processors; and a memory for storing one or more programs, wherein the one or more programs, when executed by the one or more processors, cause the one or more processors to implement the method as described above.
Another aspect of the present disclosure provides a computer-readable storage medium storing computer-executable instructions that, when executed, are configured to implement a method as described above.
Another aspect of the present disclosure provides a computer program comprising computer executable instructions which when executed are for implementing a method as described above.
Drawings
The above and other objects, features and advantages of the present disclosure will become more apparent from the following description of embodiments thereof with reference to the accompanying drawings in which:
FIG. 1 schematically illustrates an exemplary system architecture of a calibration method that may be related by coordinates in accordance with an embodiment of the present disclosure;
FIG. 2 schematically illustrates a flow chart of a method of calibrating a coordinate relationship in accordance with an embodiment of the present disclosure;
FIG. 3 schematically illustrates a flowchart of calculating a first pose variation according to an embodiment of the disclosure;
FIG. 4 schematically illustrates a flowchart for calculating a second pose variation according to an embodiment of the disclosure;
FIG. 5 schematically illustrates a flowchart for calculating a coordinate transformation relationship between a lidar and a vehicle according to an embodiment of the disclosure;
FIG. 6 schematically illustrates a flow chart for determining whether a first pose change amount and a second pose change amount are valid data according to the present disclosure;
FIG. 7 schematically illustrates a flow chart for calculating a third amount of pose change of a vehicle according to the present disclosure based on an effective first amount of pose change and a second amount of pose change;
FIG. 8 schematically illustrates a block diagram of a calibration device of coordinate relationships according to an embodiment of the disclosure; and
fig. 9 schematically shows a block diagram of an electronic device adapted to implement the method described above, according to an embodiment of the disclosure.
Detailed Description
Hereinafter, embodiments of the present disclosure will be described with reference to the accompanying drawings. It should be understood that the description is only exemplary and is not intended to limit the scope of the present disclosure. In the following detailed description, for purposes of explanation, numerous specific details are set forth in order to provide a thorough understanding of the embodiments of the present disclosure. It may be evident, however, that one or more embodiments may be practiced without these specific details. In addition, in the following description, descriptions of well-known structures and techniques are omitted so as not to unnecessarily obscure the concepts of the present disclosure.
The terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting of the disclosure. The terms "comprises," "comprising," and/or the like, as used herein, specify the presence of stated features, steps, operations, and/or components, but do not preclude the presence or addition of one or more other features, steps, operations, or components.
All terms (including technical and scientific terms) used herein have the same meaning as commonly understood by one of ordinary skill in the art unless otherwise defined. It should be noted that the terms used herein should be construed to have meanings consistent with the context of the present specification and should not be construed in an idealized or overly formal manner.
Where expressions like at least one of "A, B and C, etc. are used, the expressions should generally be interpreted in accordance with the meaning as commonly understood by those skilled in the art (e.g.," a system having at least one of A, B and C "shall include, but not be limited to, a system having a alone, B alone, C alone, a and B together, a and C together, B and C together, and/or A, B, C together, etc.). Where a formulation similar to at least one of "A, B or C, etc." is used, in general such a formulation should be interpreted in accordance with the ordinary understanding of one skilled in the art (e.g. "a system with at least one of A, B or C" would include but not be limited to systems with a alone, B alone, C alone, a and B together, a and C together, B and C together, and/or A, B, C together, etc.).
The embodiment of the disclosure provides a calibration method for solving a coordinate relationship between a laser radar and a vehicle body and a device capable of applying the method. The method includes acquiring first sensor data of a lidar and second sensor data of a vehicle. And calculating a first pose change amount of the laser radar in a preset time interval according to the first sensor data. And calculating a second pose change amount of the vehicle within the same preset time interval according to the second sensor data. And calculating the coordinate transformation relation between the laser radar and the vehicle according to the first pose change amount and the second pose change amount.
FIG. 1 schematically illustrates an exemplary system architecture 100 of a calibration method that may be related by coordinates according to an embodiment of the disclosure. It should be noted that fig. 1 is only an example of a system architecture to which embodiments of the present disclosure may be applied to assist those skilled in the art in understanding the technical content of the present disclosure, but does not mean that embodiments of the present disclosure may not be used in other devices, systems, environments, or scenarios.
As shown in fig. 1, a system architecture 100 according to this embodiment may include a lidar 101, a vehicle 102, a network 103, and a server 104. The network 103 is a medium used to provide a communication link between the lidar 101, the vehicle 102, and the server 104. The network 103 may include various connection types, such as wired and/or wireless communication links, and the like.
The sensor data of the lidar 101 may include, for example, a plurality of point cloud data and time stamps corresponding to the point cloud data, and the sensor data of the vehicle 102 may include, for example, a rear wheel center speed, a front-rear wheel steering angle, and a time stamp at each time. The running scene of the vehicle can generally select non-spacious point cloud data, and the movement route of the sensor of the vehicle can be an S-shaped curve.
The server 104 may be a server providing various services, such as acquiring sensor data of a lidar and a vehicle, and processing and calculating the acquired sensor data.
It should be noted that, the calibration method of the coordinate relationship provided by the embodiments of the present disclosure may be generally performed by the server 104. Accordingly, the calibration device for coordinate relationships provided in the embodiments of the present disclosure may be generally disposed in the server 104. The method of calibrating the coordinate relationship provided by the embodiments of the present disclosure may also be performed by a server or cluster of servers other than the server 104 and capable of communicating with the lidar 101 and the vehicle 102, and/or the server 104. Accordingly, the calibration means for the coordinate relationship provided by the embodiments of the present disclosure may also be provided in a server or a server cluster different from the server 104 and capable of communicating with the lidar 101 with the vehicle 102 and/or the server 104.
For example, when the coordinate relationship is calibrated, the preprocessing of the sensor data is not directly performed by the server 104, but performed by a server or a server cluster capable of communicating with the lidar 101, the vehicle 102, and the server 104, and after the preprocessing of the sensor data is completed, the preprocessed data is transmitted to the server 104 for processing
It should be understood that the number of networks and servers in fig. 1 is merely illustrative. There may be any number of networks and servers, as desired for implementation.
FIG. 2 schematically illustrates a flow chart of a method of calibrating a coordinate relationship in accordance with an embodiment of the present disclosure.
As shown in fig. 2, the method includes operations S201 to S206.
In operation S201, first sensor data of a lidar and second sensor data of a vehicle are acquired.
In the embodiment of the disclosure, the first sensor data of the lidar may be, for example, point cloud data, where the point cloud data refers to scan data recorded in the form of points, and each point includes three-dimensional coordinates, and some points may include color information (RGB) or reflection Intensity information (Intensity). For example, the laser radar scans for one circle in the time period, so that one frame of point cloud data and a timestamp corresponding to the frame of point cloud data can be obtained, and multiple frames of point cloud data and timestamps corresponding to each frame of point cloud data can be obtained after multiple times of scanning. Because the data scanned by the laser radar may include a plurality of invalid points and outliers, the invalid points and outliers in the point cloud data of the laser radar can be removed after the first sensor data is acquired.
In the embodiment of the disclosure, since the automatic driving vehicle body coordinate system generally takes the center of a rear axis as the origin of the vehicle body coordinate system, the vehicle body is forwards along an x axis, and the right hand rule is met. The second sensor data of the vehicle may be, for example, data acquired by a code wheel sensor or a wheel speed meter sensor or an accelerometer sensor or a gyroscope sensor mounted on the vehicle, where the data may include, for example, a speed of a rear axle center and a front-rear wheel steering angle at each moment in the running process of the vehicle, and each speed and steering angle corresponds to a time stamp. The environment in which the vehicle is traveling may be, for example, a non-open environment, and the movement path of the sensor may be, for example, an S-shaped curve.
In operation S202, a first pose change amount of the lidar within a preset time interval is calculated from the first sensor data.
In operation S203, a second pose change amount of the vehicle within the same preset time interval is calculated from the second sensor data.
In operation S204, it is determined whether the first pose change amount and the second pose change amount are valid data.
If yes, operation S205 is executed, and if no, operation S201 is returned to.
In operation S205, a coordinate transformation relationship between the lidar and the vehicle is calculated from the effective first and second pose changes.
In operation S206, a third pose change amount of the vehicle is calculated based on the valid first pose change amount and second pose change amount, and the second pose change amount is updated with the third pose change amount.
After the update is completed, operation S204 is returned.
According to the calibration method of the coordinate relationship provided by the embodiment of the disclosure, the first pose change amount of the laser radar is directly calculated according to the sensor data of the laser radar, the second pose change amount of the vehicle is directly calculated according to the sensor data of the vehicle, and then the coordinate relationship between the laser radar and the vehicle body can be calculated based on the first pose change amount and the pose change amount, so that no other calibration objects are needed, the coordinate relationship between the laser radar and the vehicle body can be solved without the use of the calibration objects, the requirement on the environment is reduced, and the difficulty in calibrating the coordinate relationship between the laser radar and the vehicle body is reduced to a certain extent. In the calibration process, the third pose change amount of the vehicle is dynamically calculated according to the effective first pose change amount and the second pose change amount, and the second pose change amount is dynamically updated according to the third pose change amount, so that the dynamic calibration of the coordinate relation is realized.
The method shown in fig. 2 is further described below with reference to fig. 3-5 in conjunction with the exemplary embodiment.
Fig. 3 schematically illustrates a flowchart of calculating a first pose variation amount according to an embodiment of the present disclosure.
As shown in fig. 3, the method may include operations S301 to S302, for example.
In operation S301, data of adjacent frames in the first sensor data are matched to obtain a matching result.
In the embodiment of the present disclosure, after the multi-frame point cloud data is obtained through operation S201, generally, an iterative closest point algorithm (ICP) may be used to match point cloud data of adjacent frames of the laser radar, and a matching result is recorded in a matching process: for example, a matching matrix and a matching score for adjacent frames. The adjacent time stamps corresponding to the cloud data of each frame point are adjacent, and the preset time interval refers to, for example, taking the time stamp of the current frame as the matched starting time, taking the time stamp of the next frame as the matched ending time, and taking the time stamp as a time interval.
In operation S302, a first pose change amount of the lidar in a preset time interval is calculated according to the matching result.
In the embodiment of the disclosure, the rotation axis and the rotation angle corresponding to each time stamp of the laser radar can be obtained by performing euler conversion on the matching matrix, and the first pose variation of the laser radar can be obtained according to the rotation axis and the rotation angle corresponding to each time stamp. The calculated data (rotation angle) may include invalid data, and this embodiment may further include an operation of removing the invalid data, where the removing principle may be: reject only rotation or translation null values or reject null values that are non-planar motion. The culled data may include at least 200 groups.
Fig. 4 schematically illustrates a flowchart of calculating a second pose change amount according to an embodiment of the disclosure.
As shown in fig. 4, the method may include, for example, operation S401.
In operation S401, a displacement variation and a variation of a heading angle of the vehicle within a preset time interval are calculated according to the second sensor data.
In the present disclosure, the kinematic equation of the autonomous vehicle (ackerman model) may be utilized:
and calculating a second pose change amount by combining the second sensing data. Wherein, beta represents a slip angle, and the calculation formula is:
x represents displacement of the vehicle in the X-axis direction in the body coordinate system of the autonomous vehicle, Y represents displacement of the vehicle in the Y-axis direction in the body coordinate system of the autonomous vehicle,representing the first order reciprocal of X, ++>Representing the first order reciprocal of Y, V representing the speed of the rear wheel center of the vehicle, l f Indicating the front suspension length of the vehicle, l r Represents the rear overhang length of the vehicle, ψ represents the heading angle, δ f Indicating the steering angle delta of the front wheel r Indicating the rear wheel steering angle.
According to the kinematic equation and the data obtained by the code wheel sensor, the displacement X along the X axis, the displacement Y along the Y axis and the heading angle yaw of the vehicle of the center of the rear axle of the vehicle body under the odom coordinate system can be obtained through integration, and the attitude change quantity can be obtained based on the data. The calculated data (displacement X of the X axis, displacement Y of the Y axis, and heading angle yaw of the vehicle) may include invalid data, and the embodiment may further include an operation of rejecting invalid data, where a rejection principle may be: and eliminating invalid values of the rotation reverse to the rotation of the laser radar and the rotation of the vehicle body. The culled data may include at least 200 groups.
In the above-described solving process, the time interval of integration generally needs to be synchronized with the matching time interval for calculating the first pose change amount. Specifically, because the output frequencies of the laser radar and the code wheel sensor are different, generally, the update frequency of the laser radar is slower, the code wheel sensor is time-synchronized by taking the laser radar time stamp as the main time stamp, the time interval of the code wheel sensor is divided into the same time interval as the matching time interval, and the pose change amounts of the corresponding time interval of the vehicle body are obtained by integrating the x-axis direction and the y-axis direction respectively according to the matched time interval and the code wheel sensor data. If the time stamp of the laser radar is T0, T1 and T2, & gt, TN, the time stamp of the data of the code disc sensor is T0, T1 and T2, & gt, TN, knowing the pose transformation matrix of the laser radar at the time interval of T0-T1, and solving the pose variation quantity of the vehicle body at the time interval of T0-T1; because the laser radar update frequency is relatively slow, i.e. T1-T0 is greater than T1-T0, so that it needs T0, T1, T2.
Fig. 5 schematically illustrates a flowchart of calculating a coordinate transformation relationship between a lidar and a vehicle according to an embodiment of the present disclosure.
As shown in fig. 5, the method may include, for example, operation S501.
In operation S501, the coordinate transformation relationship is solved according to the hand-eye calibration theory ax=xb.
Wherein A is a pose transformation matrix obtained by converting a first pose variation, B is a pose transformation matrix obtained by converting a second pose variation, and X is the coordinate transformation relation. After the first pose change amount and the second pose change amount are obtained through the solving of the steps, the first pose change amount and the second pose change amount are converted into matrix-form edges, and A and B are obtained.
In the embodiment of the present disclosure, a specific solving process based on the hand-eye calibration theory ax=xb may be as follows:
first, roll (roll angle) and pitch (pitch angle) are first solved based on quaternion operation.
Specifically, ax=xb is converted into a quaternion:
a rotating part:
translation part:
also known is:
wherein q x (alpha) represents the quaternion representation of roll (roll angle), q y (beta) represents the quaternion representation of pitch, q z (γ) represents a quaternion representation of yaw (heading angle).
Finishing the formula (1) to obtain a first residual error eta i The method comprises the following steps:
from the combination of equation (3) and equation (4)
And (3) further finishing to obtain:
based on the above formula, find the solution that makes the residual error minimum, can solve roll (roll angle) and pitch (pitch angle).
Next, the displacement Y in the x direction X, Y direction and the heading angle yw are solved.
Since lidar is a planar motion, the Z (Z-direction displacement) value to be solved is not objective and therefore cannot be solved.
According to formula (2) and the roll (pitch) obtained by solving the above, a second residual error can be obtained as follows:
wherein,
since the z value is not objective, it can be further sorted as:
based on the above formula, find the solution that makes the residual error minimum, can solve and namely X (X direction displacement), Y (Y direction displacement), yaw (course angle), based on the concrete value that each timestamp corresponds, can obtain the gesture variation of each physical quantity of preset time interval.
And taking the solving result as an initial value, and performing global optimization according to AX-XB=0 to obtain X, Y, roll, pitch, yaw with higher accuracy.
After the solving is completed, the correctness of the solving result can be verified, and AX-XB=0 and A=XBX are obtained by the solving result and the hand-eye calibration theory -1 Namely, A 'can be calculated by B and X, and the corresponding values of A' and A are differenced to obtain δX, δY,&oll, δpitch, δyaw, root mean square of each value is used as an error of five axes, and is used as an evaluation criterion of calibration results, where a represents that the second sensor data includes measured values corresponding to the respective parameters.
By the specific solving method provided by the embodiment of the disclosure, the coordinate relation between the laser radar and the vehicle body can be solved dynamically without a calibration object based on the sensor data of the laser radar and the vehicle.
Fig. 6 schematically illustrates a flowchart of a method of determining whether a first amount of pose change and the second amount of pose change are valid data according to an embodiment of the present disclosure.
As shown in fig. 6, based on the method provided in the above embodiment, the method in the present embodiment may include operations S601 to S602, for example.
In operation S601, it is determined whether the amount of change in the radar rotation angle is equal to the amount of change in the heading angle of the vehicle.
If the two are equal, operation S602 is executed, and if the two are not equal, operation S201 is executed.
In operation S602, a first pose change amount and a second pose change amount are recorded.
By the method provided by the embodiment, whether the calculated data are valid or not can be dynamically verified, so that calculation by adopting the valid data is ensured, and the calibration accuracy is improved.
Fig. 7 schematically illustrates a flowchart for calculating a third pose change amount of a vehicle according to an effective first pose change amount and second pose change amount according to an embodiment of the present disclosure.
As shown in fig. 7, based on the method provided in the above embodiment, the method of the present embodiment may include operations S701 to S702, for example.
In operation S701, a third pose change amount is calculated from actual values of the front suspension length and the rear suspension length and the second sensor data.
This operation may calculate the third pose change amount using a method as in operation S401 based on actual values of the actual front suspension length and the rear suspension length and the second sensor data.
In operation S702, the second pose change amount is updated with the third pose change amount.
By the method, the pose change of the vehicle can be dynamically updated, the dynamic calibration of the coordinate relationship is realized, and the calibration accuracy is improved.
FIG. 8 schematically illustrates a block diagram of a calibration device of coordinate relationships according to an embodiment of the disclosure.
As shown in fig. 8, the calibration device 800 includes an acquisition module 810, a first calculation module 820, a second calculation module 830, a determination module 840, a third calculation module 850, a fourth calculation module 860, and an update module 870.
The acquiring module 810 is configured to acquire first sensor data of a lidar and second sensor data of a vehicle.
The first calculation module 820 is configured to calculate a first pose variation amount of the lidar within a preset time interval according to the first sensor data.
The second calculating module 830 is configured to calculate a second pose variation amount of the vehicle within the same preset time interval according to the second sensor data.
A judging module 840 for judging whether the first pose change amount and the second pose change amount are valid data
And a third calculation module 850, configured to calculate a coordinate transformation relationship between the lidar and the vehicle according to the first pose change amount and the second pose change amount.
A fourth calculating module 860, configured to calculate a third pose change amount of the vehicle according to the effective first pose change amount and the second pose change amount;
an updating module 870 for updating the second pose change amount with the third pose change amount.
Any number of modules, sub-modules, units, sub-units, or at least some of the functionality of any number of the sub-units according to embodiments of the present disclosure may be implemented in one module. Any one or more of the modules, sub-modules, units, sub-units according to embodiments of the present disclosure may be implemented as split into multiple modules. Any one or more of the modules, sub-modules, units, sub-units according to embodiments of the present disclosure may be implemented at least in part as a hardware circuit, such as a Field Programmable Gate Array (FPGA), a Programmable Logic Array (PLA), a system-on-chip, a system-on-substrate, a system-on-package, an Application Specific Integrated Circuit (ASIC), or in any other reasonable manner of hardware or firmware that integrates or encapsulates the circuit, or in any one of or a suitable combination of three of software, hardware, and firmware. Alternatively, one or more of the modules, sub-modules, units, sub-units according to embodiments of the present disclosure may be at least partially implemented as computer program modules, which when executed, may perform the corresponding functions.
For example, any number of the acquisition module 810, the first calculation module 820, the second calculation module 830, the determination module 840, the third calculation module 850, the fourth calculation module 860, and the update module 870 may be combined in one module/unit/subunit, or any one of the modules/units/subunits may be split into a plurality of modules/units/subunits. Alternatively, at least some of the functionality of one or more of these modules/units/sub-units may be combined with at least some of the functionality of other modules/units/sub-units and implemented in one module/unit/sub-unit. According to embodiments of the present disclosure, at least one of the acquisition module 810, the first calculation module 820, the second calculation module 830, the determination module 840, the third calculation module 850, the fourth calculation module 860, and the update module 870 may be implemented at least in part as hardware circuitry, such as a Field Programmable Gate Array (FPGA), a Programmable Logic Array (PLA), a system-on-chip, a system-on-substrate, a system-on-package, an Application Specific Integrated Circuit (ASIC), or as hardware or firmware in any other reasonable manner of integrating or packaging the circuitry, or as any one of or a suitable combination of three of software, hardware, and firmware. Alternatively, at least one of the acquisition module 810, the first calculation module 820, the second calculation module 830, the determination module 840, the third calculation module 850, the fourth calculation module 860, and the update module 870 may be at least partially implemented as a computer program module, which when executed, may perform the corresponding functions.
It should be noted that, in the embodiment of the present disclosure, the calibration device portion corresponds to the calibration method portion in the embodiment of the present disclosure, and the description of the calibration device portion specifically refers to the calibration method portion and is not described herein again.
Fig. 9 schematically shows a block diagram of an electronic device adapted to implement the method described above, according to an embodiment of the disclosure. The electronic device shown in fig. 9 is merely an example, and should not impose any limitations on the functionality and scope of use of embodiments of the present disclosure.
As shown in fig. 9, an electronic device 900 according to an embodiment of the present disclosure includes a processor 901 that can perform various appropriate actions and processes according to a program stored in a Read Only Memory (ROM) 902 or a program loaded from a storage portion 908 into a Random Access Memory (RAM) 903. The processor 901 may include, for example, a general purpose microprocessor (e.g., a CPU), an instruction set processor and/or an associated chipset and/or a special purpose microprocessor (e.g., an Application Specific Integrated Circuit (ASIC)), or the like. Processor 901 may also include on-board memory for caching purposes. Processor 901 may include a single processing unit or multiple processing units for performing the different actions of the method flows according to embodiments of the present disclosure.
In the RAM 903, various programs and data necessary for the operation of the system 900 are stored. The processor 901, the ROM 902, and the RAM 903 are connected to each other by a bus 904. The processor 901 performs various operations of the method flow according to the embodiments of the present disclosure by executing programs in the ROM 902 and/or the RAM 903. Note that the program may be stored in one or more memories other than the ROM 902 and the RAM 903. The processor 901 may also perform various operations of the method flow according to embodiments of the present disclosure by executing programs stored in the one or more memories.
According to an embodiment of the disclosure, the system 900 may also include an input/output (I/O) interface 905, the input/output (I/O) interface 905 also being connected to the bus 904. The system 900 may also include one or more of the following components connected to the I/O interface 905: an input section 906 including a keyboard, a mouse, and the like; an output portion 907 including a display such as a Cathode Ray Tube (CRT), a Liquid Crystal Display (LCD), and a speaker; a storage portion 908 including a hard disk or the like; and a communication section 909 including a network interface card such as a LAN card, a modem, or the like. The communication section 909 performs communication processing via a network such as the internet. The drive 910 is also connected to the I/O interface 905 as needed. A removable medium 911 such as a magnetic disk, an optical disk, a magneto-optical disk, a semiconductor memory, or the like is installed as needed on the drive 910 so that a computer program read out therefrom is installed into the storage section 908 as needed.
According to embodiments of the present disclosure, the method flow according to embodiments of the present disclosure may be implemented as a computer software program. For example, embodiments of the present disclosure include a computer program product comprising a computer program embodied on a computer readable storage medium, the computer program comprising program code for performing the method shown in the flowcharts. In such an embodiment, the computer program may be downloaded and installed from the network via the communication portion 909 and/or installed from the removable medium 911. The above-described functions defined in the system of the embodiments of the present disclosure are performed when the computer program is executed by the processor 901. The systems, devices, apparatus, modules, units, etc. described above may be implemented by computer program modules according to embodiments of the disclosure.
The present disclosure also provides a computer-readable storage medium that may be embodied in the apparatus/device/system described in the above embodiments; or may exist alone without being assembled into the apparatus/device/system. The computer-readable storage medium carries one or more programs which, when executed, implement methods in accordance with embodiments of the present disclosure.
According to embodiments of the present disclosure, the computer-readable storage medium may be a non-volatile computer-readable storage medium. Examples may include, but are not limited to: 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), 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 the context of this disclosure, 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.
For example, according to embodiments of the present disclosure, the computer-readable storage medium may include ROM 902 and/or RAM 903 and/or one or more memories other than ROM 902 and RAM 903 described above.
The flowcharts and block diagrams in the figures illustrate the architecture, functionality, and operation of possible implementations of systems, methods and computer program products according to various embodiments of the present disclosure. In this regard, each block in the flowchart or block diagrams may represent a module, segment, or portion of code, which comprises one or more executable instructions for implementing the specified logical function(s). It should also be noted that, in some alternative implementations, the functions noted in the block may occur out of the order noted in the figures. For example, two blocks shown in succession may, in fact, be executed substantially concurrently, or the blocks may sometimes be executed in the reverse order, depending upon the functionality involved. It will also be noted that each block of the block diagrams or flowchart illustration, and combinations of blocks in the block diagrams or flowchart illustration, can be implemented by special purpose hardware-based systems which perform the specified functions or acts, or combinations of special purpose hardware and computer instructions.
Those skilled in the art will appreciate that the features recited in the various embodiments of the disclosure and/or in the claims may be combined in various combinations and/or combinations, even if such combinations or combinations are not explicitly recited in the disclosure. In particular, the features recited in the various embodiments of the present disclosure and/or the claims may be variously combined and/or combined without departing from the spirit and teachings of the present disclosure. All such combinations and/or combinations fall within the scope of the present disclosure.
The embodiments of the present disclosure are described above. However, these examples are for illustrative purposes only and are not intended to limit the scope of the present disclosure. Although the embodiments are described above separately, this does not mean that the measures in the embodiments cannot be used advantageously in combination. The scope of the disclosure is defined by the appended claims and equivalents thereof. Various alternatives and modifications can be made by those skilled in the art without departing from the scope of the disclosure, and such alternatives and modifications are intended to fall within the scope of the disclosure.
Claims (13)
1. A calibration method of a coordinate relation comprises the following steps:
acquiring first sensor data of a laser radar and second sensor data of a vehicle;
Calculating a first pose variation amount of the laser radar in a preset time interval according to the first sensor data;
calculating a second pose change amount of the vehicle within the same preset time interval according to the second sensor data;
judging whether the first pose change amount and the second pose change amount are effective data or not; if yes, calculating a coordinate transformation relation between the laser radar and the vehicle according to the effective first pose change amount and the effective second pose change amount; and
and calculating a third pose change amount of the vehicle according to the effective first pose change amount and the second pose change amount, updating the second pose change amount by using the third pose change amount, and repeatedly executing the operation of calculating the coordinate transformation relation between the laser radar and the vehicle according to the effective first pose change amount and the effective second pose change amount.
2. The calibration method according to claim 1, wherein the calculating a first pose change amount of the lidar within a preset time interval according to the first sensor data includes:
and matching the data of adjacent frames in the first sensor data, and calculating the variation of the rotation angle of the laser radar in the preset time interval according to a matching result.
3. The calibration method according to claim 1, wherein the calculating the second pose change amount of the vehicle within the same preset time interval from the second sensor data includes:
and calculating the displacement variation and the course angle variation of the vehicle in the preset time interval according to the second sensor data.
4. A calibration method according to claim 3, wherein, according to the kinetic equation:
calculating the displacement variation and the course angle variation in the preset time interval, wherein X represents the displacement of the vehicle in the X-axis direction in the automatic driving vehicle body coordinate system, Y represents the displacement of the vehicle in the Y-axis direction in the automatic driving vehicle body coordinate system,representing the first order reciprocal of X, ++>Representing the first order reciprocal of Y, V representing the speed of the rear wheel center of the vehicle, l f Representing the front suspension of a vehicleLength, l r Represents the rear overhang length of the vehicle, ψ represents the heading angle, δ f Indicating the steering angle delta of the front wheel r The rear wheel steering angle is represented, beta represents the slip angle, and the calculation formula is as follows:
5. the calibration method according to claim 1, wherein the calculating the coordinate transformation relationship between the lidar and the vehicle according to the first pose change amount and the second pose change amount includes:
And solving the coordinate transformation relation according to a hand-eye calibration theory AX=XB, wherein A is a pose transformation matrix obtained by converting the first pose variation, B is a pose transformation matrix obtained by converting the second pose variation, and X is the coordinate transformation relation.
6. The calibration method according to claim 5, wherein the solving the coordinate transformation relationship according to the hand-eye calibration theory ax=xb includes:
converting the ax=xb into quaternions, where the quaternions include quaternions corresponding to a roll angle and quaternions corresponding to a pitch angle of the laser radar;
obtaining a first residual error according to the quaternion, and solving a solution when the first residual error is minimum to obtain a rolling angle and a pitch angle of the laser radar;
and obtaining a second residual error according to the quaternion and the roll angle and pitch angle obtained by solving, and solving a solution when the second residual error is minimum to obtain displacement of the vehicle in the X-axis direction, displacement of the vehicle in the y-axis direction and a course angle in an automatic driving vehicle body coordinate system.
7. The calibration method of claim 6, wherein the method further comprises:
and carrying out difference on the obtained rolling angle and pitch angle of the laser radar, displacement of the vehicle in the x-axis direction and displacement and course angle of the vehicle in the y-axis direction and the measured value corresponding to each parameter included in the second sensor data to obtain a difference value of each parameter, and respectively taking root mean square of the difference value of each parameter as a judgment standard of a calibration result.
8. The calibration method according to claim 1, wherein the first pose change amount includes a change amount of a rotation angle of the laser radar within the preset time interval, the second pose change amount includes a change amount of a heading angle of the vehicle within the preset time interval, and the vehicle is a moving vehicle;
the judging whether the first pose change amount and the second pose change amount are valid data comprises:
judging whether the variation of the rotation angle is equal to the variation of the course angle or not;
if the first pose change amount and the second pose change amount are equal, recording the first pose change amount and the second pose change amount;
and if the first sensor data and the second sensor data of the laser radar are not equal, returning to the operation of acquiring the first sensor data of the laser radar and the second sensor data of the vehicle.
9. The calibration method of claim 1, the calculating a third pose change amount of the vehicle from the effective first and second pose change amounts comprising:
calculating actual values of the front suspension length and the rear suspension length of the vehicle according to the effective first pose change amount and the second pose change amount;
and calculating the third pose change amount according to the actual values of the front suspension length and the rear suspension length and the second sensor data.
10. The calibration method according to claim 8, before the determination as to whether the amount of change in the rotation angle is equal to the amount of change in the heading angle, the method further comprising:
and eliminating invalid data in the variation of the rotation angle and the variation of the course angle.
11. A coordinate relationship calibration device, comprising:
the acquisition module is used for acquiring first sensor data of the laser radar and second sensor data of the vehicle;
the first calculation module is used for calculating a first pose variation amount of the laser radar in a preset time interval according to the first sensor data;
the second calculation module is used for calculating a second pose change amount of the vehicle within the same preset time interval according to the second sensor data;
the judging module is used for judging whether the first pose change amount and the second pose change amount are effective data or not;
the third calculation module is used for calculating the coordinate transformation relation between the laser radar and the vehicle according to the effective first pose change amount and the effective second pose change amount;
the fourth calculation module is used for calculating a third pose change amount of the vehicle according to the effective first pose change amount and the second pose change amount;
And the updating module is used for updating the second pose change amount according to the third pose change amount.
12. An electronic device, comprising:
one or more processors;
a memory for storing one or more programs,
wherein the one or more programs, when executed by the one or more processors, cause the one or more processors to implement the method of any of claims 1 to 10.
13. A computer readable storage medium having stored thereon executable instructions which when executed by a processor cause the processor to implement the method of any of claims 1 to 10.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011200240.8A CN113759347B (en) | 2020-10-30 | 2020-10-30 | Coordinate relation calibration method, device, equipment and medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011200240.8A CN113759347B (en) | 2020-10-30 | 2020-10-30 | Coordinate relation calibration method, device, equipment and medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113759347A CN113759347A (en) | 2021-12-07 |
| CN113759347B true CN113759347B (en) | 2024-04-05 |
Family
ID=78785940
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011200240.8A Active CN113759347B (en) | 2020-10-30 | 2020-10-30 | Coordinate relation calibration method, device, equipment and medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113759347B (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114910081B (en) * | 2022-05-26 | 2023-03-10 | 阿波罗智联(北京)科技有限公司 | Vehicle positioning method and device and electronic equipment |
| CN115018935B (en) * | 2022-08-09 | 2022-10-18 | 季华实验室 | Calibration method and device for camera and vehicle, electronic equipment and storage medium |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109767475A (en) * | 2018-12-28 | 2019-05-17 | 广州小鹏汽车科技有限公司 | A kind of method for calibrating external parameters and system of sensor |
| CN109901123A (en) * | 2018-12-24 | 2019-06-18 | 文远知行有限公司 | Transducer calibration method, device, computer equipment and storage medium |
| CN110673115A (en) * | 2019-09-25 | 2020-01-10 | 杭州飞步科技有限公司 | Combined calibration method, device, equipment and medium for radar and integrated navigation system |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9952317B2 (en) * | 2016-05-27 | 2018-04-24 | Uber Technologies, Inc. | Vehicle sensor calibration system |
-
2020
- 2020-10-30 CN CN202011200240.8A patent/CN113759347B/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109901123A (en) * | 2018-12-24 | 2019-06-18 | 文远知行有限公司 | Transducer calibration method, device, computer equipment and storage medium |
| CN109767475A (en) * | 2018-12-28 | 2019-05-17 | 广州小鹏汽车科技有限公司 | A kind of method for calibrating external parameters and system of sensor |
| CN110673115A (en) * | 2019-09-25 | 2020-01-10 | 杭州飞步科技有限公司 | Combined calibration method, device, equipment and medium for radar and integrated navigation system |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113759347A (en) | 2021-12-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US11002840B2 (en) | Multi-sensor calibration method, multi-sensor calibration device, computer device, medium and vehicle | |
| JP6862409B2 (en) | Map generation and moving subject positioning methods and devices | |
| CN110673115B (en) | Combined calibration method, device, equipment and medium for radar and integrated navigation system | |
| CN107817502B (en) | Laser point cloud data processing method and device | |
| CN109946701B (en) | Point cloud coordinate conversion method and device | |
| US9547910B2 (en) | Method and apparatus for vision aided navigation using image registration | |
| US20160305784A1 (en) | Iterative kalman smoother for robust 3d localization for vision-aided inertial navigation | |
| CN113759347B (en) | Coordinate relation calibration method, device, equipment and medium | |
| WO2021097983A1 (en) | Positioning method, apparatus, and device, and storage medium | |
| CN116106864A (en) | Point cloud motion compensation method and device, storage medium and laser radar | |
| WO2023071442A1 (en) | Data processing method and apparatus | |
| CN114018274A (en) | Vehicle positioning method and device and electronic equipment | |
| CN111080682A (en) | Point cloud data registration method and device | |
| CN114485698A (en) | Intersection guide line generating method and system | |
| CN110515110B (en) | Method, device, equipment and computer readable storage medium for data evaluation | |
| CN109141411B (en) | Positioning method, positioning device, mobile robot, and storage medium | |
| CN116047481A (en) | Method, device, equipment and storage medium for correcting point cloud data distortion | |
| CN115239758A (en) | Timestamp correction method, apparatus, device, medium, and computer program product | |
| CN111812668B (en) | Winding inspection device, positioning method thereof and storage medium | |
| US20210074020A1 (en) | Location estimation apparatus | |
| CN114694107A (en) | Image processing method and device, electronic equipment and storage medium | |
| CN113946151A (en) | Data processing method and device for automatic driving vehicle and automatic driving vehicle | |
| JP2021169990A (en) | Positioning system, method and program | |
| CN113759384A (en) | Method, device, equipment and medium for determining pose conversion relation of sensor | |
| CN113587920B (en) | Motion measurement method, motion measurement device, electronic equipment and computer readable 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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |