CN113759347A - Coordinate relation calibration method, device, equipment and medium - Google Patents
Coordinate relation calibration method, device, equipment and medium Download PDFInfo
- Publication number
- CN113759347A CN113759347A CN202011200240.8A CN202011200240A CN113759347A CN 113759347 A CN113759347 A CN 113759347A CN 202011200240 A CN202011200240 A CN 202011200240A CN 113759347 A CN113759347 A CN 113759347A
- Authority
- CN
- China
- Prior art keywords
- variation
- vehicle
- calculating
- sensor data
- laser radar
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 66
- 230000009466 transformation Effects 0.000 claims abstract description 30
- 230000008859 change Effects 0.000 claims description 28
- 238000006073 displacement reaction Methods 0.000 claims description 28
- 238000004364 calculation method Methods 0.000 claims description 15
- 239000011159 matrix material Substances 0.000 claims description 11
- 230000015654 memory Effects 0.000 claims description 11
- 206010034719 Personality change Diseases 0.000 claims description 9
- 238000005096 rolling process Methods 0.000 claims description 2
- 238000004590 computer program Methods 0.000 description 11
- 238000010586 diagram Methods 0.000 description 8
- 230000006870 function Effects 0.000 description 7
- 238000004891 communication Methods 0.000 description 6
- 230000008569 process Effects 0.000 description 5
- 238000012545 processing Methods 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 3
- 238000010276 construction Methods 0.000 description 2
- 230000010354 integration Effects 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
- 238000004806 packaging method and process Methods 0.000 description 2
- 238000007781 pre-processing Methods 0.000 description 2
- 239000000758 substrate Substances 0.000 description 2
- 108010001267 Protein Subunits Proteins 0.000 description 1
- 238000004422 calculation algorithm 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
- 239000004065 semiconductor Substances 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 239000000126 substance Substances 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/497—Means for monitoring or calibrating
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 calibration method of coordinate relationship, including: acquiring first sensor data of a laser radar and second sensor data of a vehicle; calculating a first position and posture variation of the laser radar in a preset time interval according to the first sensor data; calculating a second position and posture variation of the vehicle within the same preset time interval according to the second sensor data; judging whether the first position posture variation and the second position posture variation are effective data or not; if so, calculating a coordinate transformation relation between the laser radar and the vehicle according to the first position posture variation and the second position posture variation; and calculating a third posture variation of the vehicle according to the effective first posture variation and the effective second posture variation, and updating the second posture variation by the third posture variation. The disclosure also provides a calibration device of 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
The sensor calibration is the basis of data fusion, belongs to basic key technology in the technologies of robots and the like, and plays a role in lifting the weight. The aim of calibration between the multi-line laser radar and the vehicle is to solve the coordinate relation between the multi-line laser radar and the vehicle in a coordinate system which takes the rear axle center as the origin of coordinates and takes the front direction of the vehicle body as the x axis.
In implementing the disclosed concept, the inventors found that there are at least the following problems in the related art: calibration between the multi-line laser radar and the vehicle usually requires selecting a calibration object and taking the calibration object as a reference for calibration, and the calibration mode is complex in operation and limited by environmental conditions. Moreover, the traditional calibration mode is generally static calibration, and cannot meet the calibration requirement of dynamic calibration in actual application.
Disclosure of Invention
In view of this, the present disclosure provides a calibration method for dynamically solving the coordinate relationship between the laser radar and the vehicle body without a calibration object.
One aspect of the present disclosure provides a calibration method of a coordinate relationship, including: acquiring first sensor data of a laser radar and second sensor data of a vehicle; calculating a first position and posture variation of the laser radar in a preset time interval according to the first sensor data; calculating a second position and posture variation of the vehicle within the same preset time interval according to the second sensor data; judging whether the first position posture variation and the second position posture variation are effective data or not; if so, calculating the coordinate transformation relation between the laser radar and the vehicle according to the first position variation and the second position variation, calculating the third position variation of the vehicle according to the effective first position variation and the effective second position variation, updating the second position variation with the third position variation, and repeatedly executing the operation of calculating the coordinate transformation relation between the laser radar and the vehicle according to the effective first position variation and the effective second position variation.
According to an embodiment of the present disclosure, the calculating a first position and orientation variation of the lidar within a preset time period according to the first sensor data includes: and matching the data of the 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 the matching result.
According to an embodiment of the present disclosure, the calculating a second posture 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 variation of displacement and the variation of course angle in the preset time interval, wherein X represents the displacement of the vehicle in the X-axis direction in the coordinate system of the body of the automatic driving vehicle, Y represents the displacement of the vehicle in the Y-axis direction in the coordinate system of the body of the automatic driving vehicle,
which means that the first order of the X is inverted,
indicating the reciprocal of the first order of Y, V indicating the speed at the centre of the rear wheel of the vehicle, lfIndicating the front overhang length, l, of the vehiclerIndicating the rear overhang length of the vehicle,. phi.fIndicating the steering angle, delta, of the front wheelrThe rear wheel steering angle is expressed, the slip angle is expressed by beta, and the calculation formula is as follows:
according to an embodiment of the present disclosure, the calculating the coordinate relationship between the laser radar and the vehicle according to the first position change amount and the second position change amount includes: and solving the coordinate transformation relation according to the 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, solving the coordinate transformation relation according to the hand-eye calibration theory AX ═ XB includes: converting the AX (X, XB) into a quaternion, wherein the quaternion comprises a quaternion corresponding to a roll angle and a quaternion 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 roll angle and a pitch angle of the laser radar; and obtaining a second residual error according to the quaternion, the rolling angle and the pitch angle obtained by solving, and solving a solution when the second residual error is minimum to obtain the displacement of the vehicle in the x-axis direction, the displacement of the vehicle in the y-axis direction and the course angle in the coordinate system of the automatic driving vehicle body.
According to an embodiment of the present disclosure, the method further comprises: and subtracting the calculated roll angle and pitch angle of the laser radar, the calculated displacement of the vehicle in the x-axis direction, the calculated displacement and course angle in the y-axis direction and the calculated measured values corresponding to the parameters of the second sensor data to obtain the difference values of the parameters, and respectively taking the root mean square of the difference values of the parameters as the judgment standard of the calibration result.
According to an embodiment of the present disclosure, the first orientation variation includes a variation of a rotation angle of the lidar within the preset time interval, the second orientation variation includes a variation of a heading angle of the vehicle within the preset time interval, and the vehicle is a moving vehicle; the determining whether the first position variation and the second position variation are valid data includes: judging whether the variation of the rotation angle is equal to that of the course angle; if so, recording the first position posture variation and the second position posture variation; and if the data 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 attitude change amount of the vehicle according to the valid first attitude change amount and the valid second attitude change amount includes: calculating actual values of the front overhang length and the rear overhang length of the vehicle according to the effective first position posture variation and the effective second position posture variation; and calculating a third posture variation according to the actual values of the front overhang length and the rear overhang length and second sensor data, and updating the second posture variation by the third posture variation.
According to an embodiment of the present disclosure, before the determining whether the variation of the rotation angle is equal to the variation of the heading angle, the method further includes: and eliminating invalid data in the variable quantity of the rotating angle and the variable quantity of the course angle.
Another aspect of the present disclosure provides a calibration apparatus for coordinate relationship, 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 position and orientation variation 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 position and posture variation of the vehicle in the same preset time interval according to the second sensor data; the judging module is used for judging whether the first position posture variation and the second position posture variation are effective data or not; the third calculation module is used for calculating a coordinate transformation relation between the laser radar and the vehicle according to the effective first position and attitude variation and the effective second position and attitude variation; the fourth calculation module is used for calculating a third posture variation of the vehicle according to the effective first posture variation and the effective second posture variation; and the updating module is used for updating the second position posture variable quantity by the third position posture variable quantity.
Another aspect of the present disclosure provides an electronic device including: one or more processors; 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 for implementing the method as described above when executed.
Another aspect of the disclosure provides a computer program comprising computer executable instructions for implementing the method as described above when executed.
Drawings
The above and other objects, features and advantages of the present disclosure will become more apparent from the following description of embodiments of the present disclosure with reference to the accompanying drawings, in which:
FIG. 1 schematically illustrates an exemplary system architecture for a calibration method with coordinate relationships, according to an embodiment of the present disclosure;
FIG. 2 schematically illustrates a flow chart of a method of calibration of coordinate relationships according to an embodiment of the present disclosure;
FIG. 3 schematically illustrates a flow chart for calculating a first amount of change in attitude according to an embodiment of the present disclosure;
FIG. 4 schematically illustrates a flow chart for calculating a second amount of change in position according to an embodiment of the disclosure;
FIG. 5 schematically illustrates a flow chart 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 the first and second variations in position are valid data according to the present disclosure;
FIG. 7 schematically illustrates a flow chart for calculating a third change in attitude of the vehicle based on the effective first and second changes in attitude according to the present disclosure;
FIG. 8 schematically illustrates a block diagram of a calibration arrangement for coordinate relationships in accordance with an embodiment of the present disclosure; and
fig. 9 schematically shows a block diagram of an electronic device adapted to implement the above described method according to an embodiment of the present 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 illustrative only 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 disclosure. It may be evident, however, that one or more embodiments may be practiced without these specific details. Moreover, in the following description, descriptions of well-known structures and techniques are omitted so as to not 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 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 is noted that the terms used herein should be interpreted as having a meaning that is consistent with the context of this specification and should not be interpreted in an idealized or overly formal sense.
Where a convention analogous to "at least one of A, B and C, etc." is used, in general such a construction is intended in the sense one having skill in the art would understand the convention (e.g., "a system having at least one of A, B and C" would include but not be limited to systems that have 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 convention analogous to "A, B or at least one of C, etc." is used, in general such a construction is intended in the sense one having skill in the art would understand the convention (e.g., "a system having at least one of A, B or C" would include but not be limited to systems that have 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 of a coordinate relation for solving a coordinate relation 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 the first position and posture variation of the laser radar in a preset time interval according to the first sensor data. And calculating a second position and posture variation of the vehicle in 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 position posture variation and the second position posture variation.
Fig. 1 schematically illustrates an exemplary system architecture 100 for a calibration method with coordinate relationships in accordance with an embodiment of the present disclosure. It should be noted that fig. 1 is only an example of a system architecture to which the embodiments of the present disclosure may be applied to help those skilled in the art understand the technical content of the present disclosure, and does not mean that the embodiments of the present disclosure may not be applied to 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. Network 103 is used to provide a medium for communication links between lidar 101, vehicle 102, and server 104. Network 103 may include various connection types, such as wired and/or wireless communication links, and so forth.
The sensor data of the laser radar 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 driving scene of the vehicle can be generally selected from non-spacious point cloud data, and the motion route of the sensor of the vehicle can be an S-shaped curve.
The server 104 may be a server that provides various services, such as acquisition of sensor data of the laser radar and the vehicle, processing and calculation of the acquired sensor data.
It should be noted that the calibration method of the coordinate relationship provided by the embodiment of the present disclosure may be generally executed by the server 104. Accordingly, the calibration apparatus for coordinate relationship provided by the embodiments of the present disclosure may be generally disposed in the server 104. The calibration method of the coordinate relationship provided by the embodiment of the present disclosure may also be executed by a server or a server cluster that is different from the server 104 and is capable of communicating with the laser radar 101 and the vehicle 102, and/or the server 104. Correspondingly, the calibration device for the coordinate relationship provided in the embodiment of the present disclosure may also be disposed in a server or a server cluster that is different from the server 104 and is capable of communicating with the laser radar 101 and the vehicle 102 and/or the server 104.
For example, when the coordinate relationship is calibrated, the preprocessing process of the sensor data is not directly executed by the server 104, but executed by a server or a server cluster capable of communicating with the laser radar 101, the vehicle 102, and the server 104, and after the preprocessing of the sensor data is completed, the preprocessed data is sent to the server 104 for being processed
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 shows a flow chart of a calibration method of coordinate relationships according to 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 laser radar and second sensor data of a vehicle are acquired.
In the embodiment of the present disclosure, the first sensor data of the lidar may be, for example, point cloud data, where the point cloud data refers to scanning data recorded in the form of points, each point includes three-dimensional coordinates, and some may include color information (RGB) or reflection Intensity information (Intensity). For example, the laser radar may obtain one frame of point cloud data and a timestamp corresponding to the frame of point cloud data by scanning for one week in a time period, and thus, multiple frames of point cloud data and timestamps corresponding to each frame of point cloud data may be obtained through multiple scans. Because the data scanned by the laser radar may include a plurality of invalid points and outliers, the invalid points and the outliers in the point cloud data of the laser radar can be removed after the first sensor data is acquired.
In the disclosed embodiment, since the autonomous vehicle body coordinate system generally has the rear axle center as the origin of the body coordinate system and the forward direction along the vehicle body as the x-axis, the right-hand rule is met. The second sensor data of the vehicle may be, for example, data obtained by a code wheel sensor or a wheel speed sensor or an accelerometer sensor or a gyroscope sensor mounted on the vehicle, and the data may include, for example, the speed of the center of the rear axle, the steering angles of the front and rear wheels at various times during the running of the vehicle, and each of the speed and the steering angle corresponds to a time stamp. The environment in which the vehicle travels 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 change amount of the attitude of the lidar within a preset time interval is calculated according to the first sensor data.
In operation S203, a second change amount of the posture of the vehicle within the same preset time interval is calculated according to the second sensor data.
In operation S204, it is determined whether the first and second attitude change amounts are valid data.
If yes, operation S205 is performed, and if no, operation S201 is returned to.
In operation S205, a coordinate transformation relationship between the laser radar and the vehicle is calculated according to the effective first and second position variation amounts.
In operation S206, a third posture variation of the vehicle is calculated according to the valid first posture variation and the valid second posture variation, and the second posture variation is updated according to the third posture variation.
After the update is completed, the operation returns to operation S204.
According to the calibration method of the coordinate relationship, the first position and posture variation quantity of the laser radar is obtained directly through calculation according to the sensor data of the laser radar, the second position and posture variation quantity of the vehicle is directly calculated according to the sensor data of the vehicle, then the coordinate relationship between the laser radar and the vehicle body can be calculated based on the first position and posture variation quantity and the posture variation quantity, other calibration objects are not needed, the coordinate relationship between the laser radar and the vehicle body can be solved without the calibration objects, the requirement on the environment is lowered, and the difficulty in calibration of the coordinate relationship between the laser radar and the vehicle body is lowered to a certain extent. In the calibration process, the third posture variation of the vehicle is dynamically calculated according to the effective first posture variation and the effective second posture variation, and the second posture variation is dynamically updated according to the third posture variation, so that the dynamic calibration of the coordinate relationship is realized.
The method shown in fig. 2 is further described with reference to fig. 3-5 in conjunction with specific embodiments.
Fig. 3 schematically illustrates a flowchart of calculating a first amount of change in posture 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 is matched to obtain a matching result.
In the embodiment of the present disclosure, multiple frames of point cloud data are obtained through operation S201, generally, an iterative closest point algorithm (ICP) may be adopted to respectively match point cloud data of adjacent frames of the laser radar, and a matching result is recorded in a matching process: including for example the match matrix and match score of adjacent frames. The preset time interval refers to a time interval in which the timestamp of the current frame is used as the matching start time, and the timestamp of the next frame is used as the matching end time.
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 present disclosure, the rotational axis and the rotational angle of the laser radar corresponding to each timestamp can be obtained by performing euler transformation on the matching matrix, and the first attitude change amount of the laser radar can be obtained according to the rotational axis and the rotational angle corresponding to each timestamp. 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: rejecting invalid values that are only rotational or translational or rejecting invalid values that are non-planar motion. The culled data should include at least 200 groups.
Fig. 4 schematically illustrates a flowchart of calculating a second posture change amount according to an embodiment of the present disclosure.
As shown in fig. 4, the method may include, for example, operation S401.
In operation S401, a displacement variation amount and a heading angle variation amount of the vehicle within a preset time interval are calculated according to the second sensor data.
In the present disclosure, the kinematic equation for an autonomous vehicle (ackerman vehicle type) may be utilized:
and calculating a second posture change amount by combining the second sensing data. Wherein β represents a slip angle, and the calculation formula is:
x represents the displacement of the vehicle in the X-axis direction in the autonomous vehicle body coordinate system, Y represents the displacement of the vehicle in the Y-axis direction in the autonomous vehicle body coordinate system,
which means that the first order of the X is inverted,
indicating the reciprocal of the first order of Y, V indicating the speed at the centre of the rear wheel of the vehicle, lfIndicating the front overhang length, l, of the vehiclerIndicating the rear overhang length of the vehicle,. phi.fIndicating the steering angle, delta, of the front wheelrIndicating 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 under the odom coordinate system of the rear axle center of the vehicle body can be obtained through integration, and the posture variation can be obtained based on the data. The calculated data (X-axis displacement X, Y-axis displacement Y, and vehicle heading angle yaw) may include invalid data, and this embodiment may further include an operation of removing the invalid data, and the removing principle may be: and eliminating an invalid value of the rotation of the laser radar and the rotation of the vehicle body in a reverse direction. The culled data should include at least 200 groups.
In the above solution, the integration time interval 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 slow, the time synchronization is performed on the code wheel sensor by taking the laser radar time stamp as a main time stamp, the time interval of the code wheel sensor is divided into time intervals the same as the time interval in matching, and the x-axis direction and the y-axis direction are respectively integrated according to the matched time interval and the code wheel sensor data to obtain the pose variation of the vehicle body corresponding to the time interval. If the time stamp of the laser radar is T0, T1, T2, say, TN, the time stamp of the coded disc sensor data is T0, T1, T2, say, TN, the position and posture transformation matrix of the laser radar at the time interval of T0-T1 is known, and the vehicle body position and posture variation quantity at the time interval of T0-T1 is solved; because the update frequency of the laser radar is relatively slow, namely T1-T0 is greater than T1-T0, a timestamp closest to T1 and a timestamp closest to T0 are searched in T0, T1, T2, T.once.TN, and after the timestamps are found, the corresponding speed in the x-axis direction and the speed in the y-axis direction can be obtained, the speed in the x-axis direction and the speed in the y-axis direction are integrated, and the displacement variation of the vehicle body in the x-axis direction and the y-axis direction and the variation of the heading angle yaw at the time of T0-T1 can be approximately obtained.
Fig. 5 schematically shows a flowchart of calculating a coordinate transformation relationship between a laser radar 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 relation is solved according to the 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. And after the first position and second position variation is obtained through the solving in the steps, the first position and second position variation is converted into a matrix form edge sum to obtain A and B.
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, based on the quaternion operation, a roll (roll angle) and a pitch (pitch angle) are solved.
Specifically, AX ═ XB is converted to a quaternion:
a rotating part:
a translation part:
it is also known that:
wherein q isx(α) a quaternion representation of roll, qy(β) a quaternion representation of pitch, qzAnd (γ) a quaternion representation of yaw (heading angle).
The formula (1) is arranged to obtain a first residual error etaiComprises the following steps:
combining equation (3) and equation (4) yields
Further finishing to obtain:
based on the above formula, a solution that minimizes the residual error is found, i.e., roll and pitch can be solved.
Next, the displacement Y in the x direction X, Y and the heading angle yaw are solved.
Since the lidar is in plane motion, the Z (Z-direction displacement) value to be solved is not objective and therefore cannot be solved.
According to the formula (2) and the roll angle and pitch angle obtained by the above solution, the second residual error can be obtained as follows:
wherein the content of the first and second substances,
since the z value is not objective, further can be arranged as:
based on the above formula, the solution that minimizes the residual error is found, that is, X (X-direction displacement), Y (Y-direction displacement), and yaw (course angle) can be solved, and based on the specific values corresponding to the timestamps, the attitude variation of each physical quantity at the preset time interval can be obtained.
And taking the solving result as an initial value, and carrying out global optimization according to AX-XB ═ 0 to obtain X, Y, roll, pitch and yaw with higher accuracy.
After the solution is completed, the correctness of the solution result can be verified, and the AX-XB ═ 0, a ═ XBX, can be obtained by the solution result and the hand-eye calibration theory-1That is, A 'can be calculated by B and X, and the corresponding values of A' and A are subtracted to obtain delta X, delta Y,&oll, δ pitch, δ yaw, the root mean square of each value as the error of five axes, respectively, as the evaluation criterion for the calibration result, wherein a denotes that the second sensor data comprises the measured values for the respective parameters.
Through the specific solving method provided by the embodiment of the disclosure, the relation between the laser radar and the vehicle body coordinate can be solved dynamically without a calibration object based on the sensor data of the laser radar and the vehicle.
Fig. 6 schematically shows a flowchart of a method for determining whether a first bit position variation and a second bit position variation are valid data according to an embodiment of the disclosure.
As shown in fig. 6, based on the methods provided by the above embodiments, the method of this 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 vehicle heading angle.
If the two are equal, operation S602 is performed, and if the two are not equal, operation S201 is performed.
In operation S602, the first and second attitude change amounts are recorded.
By the method provided by the embodiment, whether the calculated data is valid or not can be dynamically verified, so that the calculation by adopting the valid data is ensured, and the calibration accuracy is improved.
Fig. 7 schematically illustrates a flowchart for calculating a third change in posture of the vehicle according to the effective first change in posture and the effective second change in posture according to an embodiment of the disclosure.
As shown in fig. 7, based on the methods provided by the above embodiments, the method of the present embodiment may include operations S701 to S702, for example.
In operation S701, a third posture change amount is calculated based on the actual values of the front overhang length and the rear overhang length and the second sensor data.
This operation may calculate the third posture change amount using a method such as operation S401 based on the actual values of the actual overhang length and the second sensor data calculation.
In operation S702, the second posture variation amount is updated with the third posture variation amount.
By the method, the pose variation of the vehicle can be dynamically updated, the dynamic calibration of the coordinate relation is realized, and the calibration accuracy is improved.
FIG. 8 schematically illustrates a block diagram of a calibration arrangement for coordinate relationships according to an embodiment of the present disclosure.
As shown in fig. 8, the calibration apparatus 800 includes an obtaining module 810, a first calculating module 820, a second calculating module 830, a determining module 840, a third calculating module 850, a fourth calculating module 860, and an updating module 870.
The obtaining module 810 is configured to obtain first sensor data of the laser radar and second sensor data of the vehicle.
The first calculating module 820 is configured to calculate a first position and orientation variation of the lidar in a preset time interval according to the first sensor data.
The second calculating module 830 is configured to calculate a second position change amount of the vehicle within the same preset time interval according to the second sensor data.
A determining module 840 for determining whether the first and second position variations are valid data
And a third calculating module 850, configured to calculate a coordinate transformation relationship between the laser radar and the vehicle according to the first position and second position variation.
A fourth calculating module 860, configured to calculate a third posture variation of the vehicle according to the effective first posture variation and the effective second posture variation;
an updating module 870, configured to update the second posture change amount with the third posture change amount.
Any number of modules, sub-modules, units, sub-units, or at least part of the functionality of any number thereof according to embodiments of the present disclosure may be implemented in one module. Any one or more of the modules, sub-modules, units, and sub-units according to the embodiments of the present disclosure may be implemented by being split into a plurality of 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 a chip, a system on a substrate, a system on a package, an Application Specific Integrated Circuit (ASIC), or may be implemented in any other reasonable manner of hardware or firmware by integrating or packaging a circuit, or in any one of or a suitable combination of software, hardware, and firmware implementations. Alternatively, one or more of the modules, sub-modules, units, sub-units according to embodiments of the disclosure may be at least partially implemented as a computer program module, which when executed may perform the corresponding functions.
For example, any plurality of the obtaining module 810, the first calculating module 820, the second calculating module 830, the judging module 840, the third calculating module 850, the fourth calculating module 860 and the updating module 870 may be combined into one module/unit/sub-unit to be implemented, or any one of the modules/units/sub-units may be divided into a plurality of modules/units/sub-units. Alternatively, at least part of the functionality of one or more of these modules/units/sub-units may be combined with at least part of the functionality of other modules/units/sub-units and implemented in one module/unit/sub-unit. According to an embodiment of the present disclosure, at least one of the obtaining module 810, the first calculating module 820, the second calculating module 830, the judging module 840, the third calculating module 850, the fourth calculating module 860 and the updating module 870 may be at least partially implemented as a hardware circuit, such as a Field Programmable Gate Array (FPGA), a Programmable Logic Array (PLA), a system on a chip, a system on a substrate, a system on a package, an Application Specific Integrated Circuit (ASIC), or may be implemented by hardware or firmware in any other reasonable manner of integrating or packaging a circuit, or implemented by any one of three implementations of software, hardware and firmware, or any suitable combination of any of them. Alternatively, at least one of the obtaining module 810, the first calculating module 820, the second calculating module 830, the determining module 840, the third calculating module 850, the fourth calculating module 860 and the updating module 870 may be at least partially implemented as a computer program module, which may perform a corresponding function when executed.
It should be noted that, the calibration device portion in the embodiment of the present disclosure 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, which is not described herein again.
Fig. 9 schematically shows a block diagram of an electronic device adapted to implement the above described method according to an embodiment of the present disclosure. The electronic device shown in fig. 9 is only an example, and should not bring any limitation to the functions and the scope of use of the embodiments of the present disclosure.
As shown in fig. 9, an electronic apparatus 900 according to an embodiment of the present disclosure includes a processor 901 which can perform various appropriate actions and processes in accordance with 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. Processor 901 may comprise, for example, a general purpose microprocessor (e.g., a CPU), an instruction set processor and/or associated chipset, and/or a special purpose microprocessor (e.g., an Application Specific Integrated Circuit (ASIC)), among others. The processor 901 may also include on-board memory for caching purposes. The processor 901 may comprise a single processing unit or a plurality of 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 through a bus 904. The processor 901 performs various operations of the method flows according to the embodiments of the present disclosure by executing programs in the ROM 902 and/or the RAM 903. Note that the programs may also 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 flows according to embodiments of the present disclosure by executing programs stored in the one or more memories.
According to embodiments of the present disclosure, method flows according to embodiments of the present disclosure may be implemented as computer software programs. 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 containing program code for performing the method illustrated by the flow chart. In such an embodiment, the computer program may be downloaded and installed from a network through the communication section 909, and/or installed from the removable medium 911. The computer program, when executed by the processor 901, performs the above-described functions defined in the system of the embodiment of the present disclosure. The systems, devices, apparatuses, modules, units, etc. described above may be implemented by computer program modules according to embodiments of the present disclosure.
The present disclosure also provides a computer-readable storage medium, which may be contained in the apparatus/device/system described in the above embodiments; or may exist separately and not be assembled into the device/apparatus/system. The computer-readable storage medium carries one or more programs which, when executed, implement the method according to an embodiment of the disclosure.
According to an embodiment 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 present 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, a computer-readable storage medium may include the ROM 902 and/or the RAM 903 described above and/or one or more memories other than the ROM 902 and the RAM 903.
The flowchart 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 various combinations and/or combinations of features recited in the various embodiments and/or claims of the present disclosure can be made, even if such combinations or combinations are not expressly recited in the present disclosure. In particular, various combinations and/or combinations of the features recited in the various embodiments and/or claims of the present disclosure may be made without departing from the spirit or teaching of the present disclosure. All such combinations and/or associations are within the scope of the present disclosure.
The embodiments of the present disclosure have been 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 separately above, this does not mean that the measures in the embodiments cannot be used in advantageous combination. The scope of the disclosure is defined by the appended claims and equivalents thereof. Various alternatives and modifications can be devised by those skilled in the art without departing from the scope of the present disclosure, and such alternatives and modifications are intended to be within the scope of the present disclosure.
Claims (13)
1. A calibration method of coordinate relationship comprises the following steps:
acquiring first sensor data of a laser radar and second sensor data of a vehicle;
calculating a first position and posture variation of the laser radar in a preset time interval according to the first sensor data;
calculating a second position and posture variation of the vehicle within the same preset time interval according to the second sensor data;
judging whether the first position posture variation and the second position posture variation are effective data or not; if so, calculating a coordinate transformation relation between the laser radar and the vehicle according to the effective first position and second position variation; and
and calculating a third position variation of the vehicle according to the effective first position variation and the effective second position variation, updating the second position variation by the third position variation, and repeatedly executing the operation of calculating the coordinate transformation relation between the laser radar and the vehicle according to the effective first position variation and the effective second position variation.
2. The calibration method according to claim 1, wherein the calculating a first change in position and orientation of the lidar within a preset time interval according to the first sensor data comprises:
and matching the data of the 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 the matching result.
3. The calibration method according to claim 1, wherein the calculating a second change in the attitude of the vehicle within the same preset time interval according to the second sensor data comprises:
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 variation of displacement and the variation of course angle in the preset time interval, wherein X represents the displacement of the vehicle in the X-axis direction in the coordinate system of the body of the automatic driving vehicle, Y represents the displacement of the vehicle in the Y-axis direction in the coordinate system of the body of the automatic driving vehicle,
which means that the first order of the X is inverted,
indicating the reciprocal of the first order of Y, V indicating the speed at the centre of the rear wheel of the vehicle, lfIndicating the front overhang length, l, of the vehiclerIndicating the rear overhang length of the vehicle,. phi.fIndicating the steering angle, delta, of the front wheelrThe rear wheel steering angle is expressed, the slip angle is expressed by beta, and the calculation formula is as follows:
5. the calibration method according to claim 1, wherein the calculating a coordinate transformation relationship between the lidar and the vehicle according to the first and second changes in the position includes:
and solving the coordinate transformation relation according to the 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 said solving the coordinate transformation relation according to the hand-eye calibration theory AX ═ XB comprises:
converting the AX (X, XB) into a quaternion, wherein the quaternion comprises a quaternion corresponding to a roll angle and a quaternion 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 roll angle and a pitch angle of the laser radar;
and obtaining a second residual error according to the quaternion, the rolling angle and the pitch angle obtained by solving, and solving a solution when the second residual error is minimum to obtain the displacement of the vehicle in the x-axis direction, the displacement of the vehicle in the y-axis direction and the course angle in the coordinate system of the automatic driving vehicle body.
7. The calibration method according to claim 6, wherein the method further comprises:
and subtracting the calculated roll angle and pitch angle of the laser radar, the calculated displacement of the vehicle in the x-axis direction, the calculated displacement and course angle in the y-axis direction and the calculated measured values corresponding to the parameters of the second sensor data to obtain the difference values of the parameters, and respectively taking the root mean square of the difference values of the parameters as the judgment standard of the calibration result.
8. The calibration method according to claim 1, wherein the first orientation change amount includes a change amount of a rotation angle of the lidar in the preset time interval, the second orientation change amount includes a change amount of a heading angle of the vehicle in the preset time interval, and the vehicle is a moving vehicle;
the determining whether the first position variation and the second position variation are valid data includes:
judging whether the variation of the rotation angle is equal to that of the course angle;
if so, recording the first position posture variation and the second position posture variation;
and if the data 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 according to claim 1, wherein the calculating a third attitude change of the vehicle according to the effective first attitude change and second attitude change comprises:
calculating actual values of the front overhang length and the rear overhang length of the vehicle according to the effective first position posture variation and the effective second position posture variation;
and calculating the third posture variation according to the actual values of the front overhang length and the rear overhang length and second sensor data.
10. The calibration method according to claim 7, before said determining whether the variation of the rotation angle is equal to the variation of the heading angle, the method further comprising:
and eliminating invalid data in the variable quantity of the rotating angle and the variable quantity of the course angle.
11. A calibration apparatus for coordinate relationships, 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 position and orientation variation 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 position and posture variation of the vehicle in the same preset time interval according to the second sensor data;
the judging module is used for judging whether the first position posture variation and the second position posture variation are effective data or not;
the third calculation module is used for calculating a coordinate transformation relation between the laser radar and the vehicle according to the effective first position and second position variation;
the fourth calculation module is used for calculating a third posture variation of the vehicle according to the effective first posture variation and the effective second posture variation;
and the updating module is used for updating the second position posture variable quantity by the third position posture variable quantity.
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-10.
13. A computer readable storage medium having stored thereon executable instructions which, when executed by a processor, cause the processor to carry out the method of any one 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 true CN113759347A (en) | 2021-12-07 |
| CN113759347B 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) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114910081A (en) * | 2022-05-26 | 2022-08-16 | 阿波罗智联(北京)科技有限公司 | Vehicle positioning method and device and electronic equipment |
| CN115018935A (en) * | 2022-08-09 | 2022-09-06 | 季华实验室 | Calibration method and device for camera and vehicle, electronic equipment and storage medium |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20170343654A1 (en) * | 2016-05-27 | 2017-11-30 | Uber Technologies, Inc. | Vehicle sensor calibration system |
| 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 |
-
2020
- 2020-10-30 CN CN202011200240.8A patent/CN113759347B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20170343654A1 (en) * | 2016-05-27 | 2017-11-30 | Uber Technologies, Inc. | Vehicle sensor calibration system |
| 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 |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114910081A (en) * | 2022-05-26 | 2022-08-16 | 阿波罗智联(北京)科技有限公司 | Vehicle positioning method and device and electronic equipment |
| CN114910081B (en) * | 2022-05-26 | 2023-03-10 | 阿波罗智联(北京)科技有限公司 | Vehicle positioning method and device and electronic equipment |
| CN115018935A (en) * | 2022-08-09 | 2022-09-06 | 季华实验室 | Calibration method and device for camera and vehicle, electronic equipment and storage medium |
| CN115018935B (en) * | 2022-08-09 | 2022-10-18 | 季华实验室 | Calibration method and device for camera and vehicle, electronic equipment and storage medium |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113759347B (en) | 2024-04-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111417871A (en) | Iterative closest point processing for integrated motion estimation using high definition maps based on lidar | |
| US20110218733A1 (en) | Method and apparatus for vision aided navigation using image registration | |
| CN108137091B (en) | Wheel slip angle estimation device and method | |
| CN112083725A (en) | Structure-shared multi-sensor fusion positioning system for automatic driving vehicle | |
| CN114018274B (en) | Vehicle positioning method and device and electronic equipment | |
| CN109781117B (en) | Combined positioning method and system | |
| CN113759347B (en) | Coordinate relation calibration method, device, equipment and medium | |
| CN107765244B (en) | InSAR baseline measurement method and device based on airborne double antennas | |
| CN113183975B (en) | Control method, device, equipment and storage medium for automatic driving vehicle | |
| CN116106864A (en) | Point cloud motion compensation method and device, storage medium and laser radar | |
| CN109141411B (en) | Positioning method, positioning device, mobile robot, and storage medium | |
| WO2024027350A1 (en) | Vehicle positioning method and apparatus, computer device and storage medium | |
| CN111753605A (en) | Lane line positioning method and device, electronic equipment and readable medium | |
| US20220254062A1 (en) | Method, device and storage medium for road slope predicating | |
| CN116952229A (en) | Unmanned aerial vehicle positioning method, device, system and storage medium | |
| WO2023226156A1 (en) | Timestamp correction method and apparatus, device, medium and computer program product | |
| CN115792985A (en) | Vehicle positioning method and device, electronic equipment, storage medium and vehicle | |
| CN115222815A (en) | Obstacle distance detection method, obstacle distance detection device, computer device, and storage medium | |
| US11461928B2 (en) | Location estimation apparatus | |
| CN113075713A (en) | Vehicle relative pose measuring method, system, equipment and storage medium | |
| WO2020223868A1 (en) | Terrain information processing method and apparatus, and unmanned vehicle | |
| WO2022193940A1 (en) | Vehicle speed measurement method and apparatus, vehicle-mounted computer device and storage medium | |
| CN113759384A (en) | Method, device, equipment and medium for determining pose conversion relation of sensor | |
| CN112859127A (en) | Navigation device and operation method thereof | |
| 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 |