CN118032007A - Calibration method of inertial measurement unit and linear laser sensor - Google Patents
Calibration method of inertial measurement unit and linear laser sensor Download PDFInfo
- Publication number
- CN118032007A CN118032007A CN202410087850.3A CN202410087850A CN118032007A CN 118032007 A CN118032007 A CN 118032007A CN 202410087850 A CN202410087850 A CN 202410087850A CN 118032007 A CN118032007 A CN 118032007A
- Authority
- CN
- China
- Prior art keywords
- matrix
- coordinate system
- line laser
- equations
- spherical center
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000005259 measurement Methods 0.000 title claims abstract description 74
- 238000000034 method Methods 0.000 title claims abstract description 69
- 239000011159 matrix material Substances 0.000 claims abstract description 157
- 238000006243 chemical reaction Methods 0.000 claims abstract description 64
- 238000001514 detection method Methods 0.000 claims abstract description 35
- 238000013519 translation Methods 0.000 claims description 35
- 230000009466 transformation Effects 0.000 claims description 27
- 230000001131 transforming effect Effects 0.000 claims description 6
- 238000010586 diagram Methods 0.000 description 13
- 230000000694 effects Effects 0.000 description 6
- 238000004364 calculation method Methods 0.000 description 5
- 229910000831 Steel Inorganic materials 0.000 description 2
- 239000010959 steel Substances 0.000 description 2
- 102000002274 Matrix Metalloproteinases Human genes 0.000 description 1
- 108010000684 Matrix Metalloproteinases Proteins 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 238000010205 computational analysis Methods 0.000 description 1
- 239000000470 constituent Substances 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 238000011900 installation process Methods 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
Landscapes
- Length Measuring Devices By Optical Means (AREA)
Abstract
The invention provides a calibration method of an inertial measurement unit and a line laser sensor, which is characterized in that a standard ball is used as a calibration piece, the position of the standard ball is kept fixed, the pose of a track detection device is adjusted to obtain a plurality of groups of spherical center coordinates, a plurality of conversion equations containing an external reference matrix to be calibrated are established based on the plurality of groups of spherical center coordinates, and the external reference matrix between the inertial coordinate system of the inertial measurement unit and the line laser coordinate system of the line laser sensor is solved based on invariance of the spherical center coordinates under a geodetic coordinate system, so that an accurate calibration result can be obtained based on the spherical center of the standard ball, a movable three-coordinate machine is not needed in the calibration process, the calibration cost can be greatly reduced, the operation is more convenient and quick, and the calibration efficiency of a track detection site can be improved.
Description
Technical Field
The invention belongs to the technical field of multi-sensor calibration methods, and particularly relates to a calibration method of an Inertial Measurement Unit (IMU) and a line laser sensor.
Background
In order to ensure the safety of track operation, multiple detection needs to be performed on the track, so that the detection efficiency is improved, the time length and the frequency of track stop maintenance are reduced, the error condition caused by manual measurement is reduced, and at present, automatic track data acquisition equipment is increasingly adopted. In order to cover the data acquisition tasks of various detection projects, the inventor designs a track detection device capable of being pushed along a track, and a detection part on a trolley comprises a sensor group consisting of a plurality of linear laser sensors, an inclinometer and an Inertial Measurement Unit (IMU), and can cover a plurality of projects such as track profile detection, track gauge detection, track irregularity detection and the like based on the acquired data. Before such a track detection device is used for track data acquisition, calibration of the detection component, in particular, calibration of the IMU and the line laser sensor is required, and thus a corresponding calibration method is required.
At present, the calibration methods of the IMU and the line laser sensor mainly comprise two types.
One type is a calibration method based on a target ball, as shown in fig. 11, fixing the positions of an IMU and a line laser sensor, performing target shooting on the target ball by using a mobile three-coordinate machine to obtain the coordinates of the center of the target ball under the IMU coordinate system, scanning the target ball by using the line laser sensor to obtain the coordinates of the center of the target ball under the laser sensor coordinate system, placing the target ball at a plurality of different positions and repeating the steps to obtain the coordinates of the center of the target ball under two coordinate systems corresponding to a plurality of groups, and obtaining a transformation matrix between the line laser sensor coordinate system and the IMU coordinate system by using an SVD algorithm.
The method needs to use a movable three-coordinate machine, and the movable three-coordinate machine is high in price, inconvenient to carry, complex and time-consuming to operate, and complex in calibration process and high in cost. Moreover, the complexity of the SVD algorithm is high, and the coordinate data is usually required to be acquired at the track detection site and then is solved offline, so that calibration cannot be completed at one time at the track detection site.
The other type is a calibration method based on a calibration object assembly, and the method also needs to use a mobile three-coordinate machine to perform calibration on the calibration object assembly, and the calibration object assembly needs to be installed on a detection site, wherein the assembly error of the calibration object assembly exists in the installation process, and the assembly error can be increased along with the increase of time and disassembly times, so that the calibration accuracy is greatly affected.
Therefore, in order to improve the calibration efficiency, reduce the calibration cost and realize the calibration on the track detection site, a new calibration method of the IMU and the line laser sensor is needed.
Disclosure of Invention
The present invention has been made to solve the above problems, and an object of the present invention is to provide a calibration method and apparatus capable of conveniently calibrating an inertial measurement unit and a line laser sensor at a detection site with high calibration accuracy, wherein the present invention adopts the following technical scheme:
the invention provides a calibration method of an inertial measurement unit and a line laser sensor, which is used for calibrating the inertial measurement unit and the line laser sensor arranged on a track detection device to obtain an external parameter matrix, and is characterized by comprising the following steps: step S1, under a plurality of different poses of the track detection device, scanning standard balls with fixed positions through the line laser sensor to obtain profile data, and obtaining inertial measurement data under the poses through the inertial measurement unit; step S2, calculating a plurality of spherical center coordinates of the standard sphere under a line laser coordinate system of the line laser sensor based on the profile data; step S3, based on the corresponding inertial measurement data, for each pose, establishing a conversion equation for converting the corresponding spherical center coordinate from the line laser coordinate system to a geodetic coordinate system, wherein the conversion equation comprises the external parameter matrix to be calibrated; and S4, solving based on a plurality of conversion equations to obtain the extrinsic matrix.
The calibration method of the inertial measurement unit and the line laser sensor provided by the invention can also have the technical characteristics that in the step S3, the established conversion equation comprises a spherical center coordinate under the geodetic coordinate system, a spherical center coordinate under the line laser coordinate system and a conversion matrix, elements of the external reference matrix are contained in the conversion matrix, and the step S4 comprises the following substeps: s4-1, respectively transforming each transformation equation based on matrix operation, wherein the transformed transformation equation comprises spherical center coordinates under the geodetic coordinate system and the external parameter matrix; s4-2, subtracting the transformed conversion equations from each other by utilizing invariance of the spherical center coordinates in the geodetic coordinate system so as to eliminate the items of the spherical center coordinates in the geodetic coordinate system and obtain a plurality of intermediate equations; and S4-3, converting a plurality of intermediate equations into a matrix equation, and solving the matrix equation by a least square method to obtain the external parameter matrix.
The calibration method of the inertial measurement unit and the line laser sensor provided by the invention can also have the technical characteristics that in the step S3, for each pose, the established conversion equation comprises a spherical center coordinate P w of the geodetic coordinate system, a spherical center coordinate P l of the line laser coordinate system and a rotation matrix from the line laser coordinate system to the inertial coordinate systemAnd translation matrix/>Rotation matrix/>, from the inertial coordinate system to the geodetic coordinate systemTranslation matrixIn step S4-1, for each pose, the intermediate matrix M is an element containing only the spherical center coordinates P l and the rotation matrix/>, in the transformed transformation equationThe extrinsic matrix X comprises the rotation matrix/>Is not limited to the translation matrix/>Is not limited, is a single element, and is a single element.
The calibration method of the inertial measurement unit and the line laser sensor provided by the invention can also have the technical characteristics that in the step S3, for each pose, the established conversion equation is as follows:
in step S4-1, for each pose, the transformed transformation equation is:
In step S4-2, the intermediate equation is:
M′X＝T′
wherein M 'is a matrix obtained by subtracting the intermediate matrix M of the two transformed transformation equations, and T' is the translation matrix of the two transformed transformation equations The resulting matrix is subtracted.
The calibration method of the inertial measurement unit and the line laser sensor provided by the invention can also have the technical characteristics that in the step S1, the profile data and the inertial measurement data are obtained at least under six different poses, in the step S3, six conversion equations are established, in the step S4, the converted six conversion equations are subtracted from each other to obtain five intermediate equations, and the external reference matrix is obtained by solving based on the five intermediate equations.
The calibration method of the inertial measurement unit and the line laser sensor provided by the invention can also have the technical characteristics that in the step S1, for each pose, the obtained profile data is three-dimensional point cloud data of a segment of arc on the spherical surface of the standard sphere, the spherical center coordinates are three-dimensional coordinates, and the external reference matrix X comprises the rotation matrixIs of the order of 9 elements of the translation matrix/>Is a single element of 3.
The calibration method of the inertial measurement unit and the line laser sensor provided by the invention may further have the technical characteristics that in step S3, the established conversion equation includes a spherical center coordinate in the geodetic coordinate system, a spherical center coordinate in the line laser coordinate system, a rotation matrix and a translation matrix from the laser coordinate system to the inertial coordinate system, and step S4 includes the following substeps: s4-1, performing form transformation on each conversion equation based on matrix operation; s4-2, subtracting the conversion equations after form conversion by utilizing invariance of the spherical center coordinates in the geodetic coordinate system so as to eliminate the items of the spherical center coordinates in the geodetic coordinate system and the items containing the translation matrix, and obtaining a plurality of first intermediate equations; s4-3, combining a plurality of first intermediate equations, and solving the combined first intermediate equations by a least square method to obtain the rotation matrix; s4-4, subtracting the conversion equations after form conversion by two by utilizing invariance of the spherical center coordinates in the geodetic coordinate system so as to eliminate the items of the spherical center coordinates in the geodetic coordinate system and obtain a plurality of second intermediate equations; and S4-5, combining a plurality of second intermediate equations, and solving the combined second intermediate equations by a least square method to obtain the translation matrix.
The calibration method of the inertial measurement unit and the line laser sensor provided by the invention can also have the technical characteristics that in the step S3, for each pose, the established conversion equation is as follows:
Wherein P w is the spherical center coordinate in the geodetic coordinate system, P l is the spherical center coordinate in the line laser coordinate system, A rotation matrix and a translation matrix from the inertial coordinate system to the geodetic coordinate system, respectively,/>In step S4-1, for each pose, the transformed conversion equation is as follows:
In step S4-3, the first intermediate equation of the simultaneous equation is expressed as:
Wherein A, B is a matrix obtained by combining the first intermediate equations, A only includes the spherical center coordinates P l in each pose, and B only includes the translation matrix in each pose In step S4-5, the second intermediate equation, which is simultaneous, is expressed as:
Wherein C, D is a matrix obtained by combining the second intermediate equations, and C comprises only the rotation matrix in each pose D contains only the rotation matrix/>, at each poseThe spherical center coordinates P l and the rotation matrix/>, in each pose
The calibration method of the inertial measurement unit and the line laser sensor provided by the invention can also have the technical characteristics that in the step S1, the profile data and the inertial measurement data are obtained at least under four different poses, in the step S3, four conversion equations are established, in the step S4, the four conversion equations after form conversion are subtracted from each other to obtain three first intermediate equations and three second intermediate equations, and the rotation matrix and the translation matrix are obtained by solving based on the three first intermediate equations and the three second intermediate equations.
The calibration method of the inertial measurement unit and the line laser sensor provided by the invention can also have the technical characteristics that in the step S1, for each pose, the obtained profile data is three-dimensional point cloud data of a segment of arc on the spherical surface of the standard sphere, and for each pose, the step S2 comprises the following substeps: s2-1, fitting the arc segments based on the three-dimensional point cloud data to obtain fitted arc segments; s2-2, obtaining the circle center coordinates and the arc radius of the fitting arc segment under the line laser coordinate system through a least square method based on the fitting arc segment; and step S2-3, obtaining the spherical center coordinates of the standard sphere under the line laser coordinate system based on the geometric relationship between the standard sphere and the circular arc section and the sphere radius of the standard sphere.
The actions and effects of the invention
According to the calibration method of the inertial measurement unit and the line laser sensor, the standard ball is used as the calibration piece, the position of the standard ball is kept fixed, the pose of the track detection device is adjusted to obtain a plurality of groups of spherical center coordinates, a plurality of conversion equations containing an external reference matrix to be calibrated are established based on the plurality of groups of spherical center coordinates, and the external reference matrix between the inertial coordinate system of the inertial measurement unit and the line laser coordinate system of the line laser sensor is solved based on invariance of the spherical center coordinates under the geodetic coordinate system.
Drawings
FIG. 1 is a flow chart of a calibration method of an inertial measurement unit and a line laser sensor according to a first embodiment of the present invention;
FIG. 2 is a schematic diagram of the hand-eye calibration principle in accordance with the first embodiment of the present invention;
FIG. 3 is a schematic diagram showing calculation of center coordinates in accordance with an embodiment of the present invention;
FIG. 4 is a flowchart of step S2 in a first embodiment of the present invention;
FIG. 5 is a flowchart of step S4 in the first embodiment of the present invention;
FIG. 6 is an exemplary diagram of a transformation process for a transformation equation in accordance with one embodiment of the present invention;
FIG. 7 is a schematic diagram of a track detecting device according to a first embodiment of the present invention;
FIG. 8 is a flowchart of step S4 in a second embodiment of the present invention;
FIG. 9 is an exemplary diagram of resolving a rotation matrix in an embodiment of the invention;
FIG. 10 is an exemplary diagram of resolving a translation matrix in an embodiment of the present invention;
FIG. 11 is a schematic diagram of a prior art target ball based calibration method.
Reference numerals:
A track detection device 20; a vehicle body 21; a road wheel 22; a sensor group 23; a line laser sensor 231; an inertial measurement unit 24; an inclinometer 25; a calibration assembly 10; a fixing bracket 11; a standard ball 12; arc segment 121; center of circle 121A; the center 12O.
Detailed Description
In order to make the technical means, creation characteristics, achievement purposes and effects of the present invention easy to understand, the calibration method of the inertial measurement unit and the line laser sensor of the present invention will be specifically described below with reference to the embodiments and the accompanying drawings.
Example 1
The embodiment provides a calibration method for an inertial measurement unit and a line laser sensor, which is used for calibrating the inertial measurement unit and the line laser sensor on a track detection device at a track detection site.
Fig. 7 is a schematic structural diagram of the track detecting device in the present embodiment.
As shown in fig. 7, the track detecting device 20 includes a vehicle body 21, four traveling wheels 22 mounted on the vehicle body 21 by brackets, two sensor groups 23 provided on both sides of the vehicle body, an inertial measurement unit 24 provided in the middle of the vehicle body, and an inclinometer 25 provided on one side of the vehicle body.
Each side of the car body 21 is provided with two travelling wheels 22, four travelling wheels 22 respectively correspond to two steel rails, and a push rod is further arranged on the car body 21, so that a detector can push the track detection device 20 along the steel rails.
Each sensor group 23 comprises four line laser sensors 231 arranged along the rail circumference on the corresponding side, the projected line laser can cover the rail, and when used for detecting the turnout, the line laser can cover the stock rail and other rail components such as guard rails, switch rails and the like beside the stock rail. It can be seen that the four line laser sensors 231 are oriented differently. Ideally, the line laser plane formed by the line laser projected by the line laser sensor 231 is coplanar with the cross section of the track.
The inertial measurement unit 24 includes a triaxial accelerometer and a triaxial gyroscope, and is capable of measuring inertial measurement data during movement of the track detection device 20, and obtaining a triaxial angle and a triaxial angular velocity of the track detection device 20 by calculating the inertial measurement data.
The calibration method of the present embodiment is used for calibrating the inertial measurement unit 24 and the line laser sensor 231 of the track inspection device 20, and is based on the calibration member and the hand-eye calibration principle, and the principle of the calibration method will be briefly described below.
Fig. 2 is a diagram of the principle of hand-eye calibration in the present embodiment, and fig. 3 is a diagram of calculation of spherical center coordinates in the present embodiment, where O 1 is a geodetic coordinate system, O 2 is a coordinate system of IMU (hereinafter referred to as inertial coordinate system), and O 3 is a coordinate system of line laser sensor (hereinafter referred to as line laser coordinate system).
As shown in fig. 2 and 3, the calibration assembly 10 of the present embodiment includes a fixed bracket 11 and a standard ball 12. The standard ball 12 is basically a standard ball body which is precisely processed, and is arranged at the end part of the fixed bracket 11, and the fixed bracket 11 is used for keeping the position of the standard ball 12 relative to the ground unchanged in the calibration process, so that the position of the ball center 12O of the standard ball is kept unchanged in the geodetic coordinate system O 1.
The line laser sensor 231 projects a line laser in a straight line, and when the line laser is irradiated on the standard ball 12, three-dimensional point cloud data of a segment 121 of an arc on the spherical surface of the standard ball 12 is obtained. And fitting an arc segment according to the three-dimensional point cloud data, and solving the coordinates of the standard sphere 12 under the sphere center 12O laser coordinate system O 3.
Then, based on inertial measurement data measured by the IMU, the coordinate of the center of sphere 12O can be converted from the line laser coordinate system O 3 to the inertial coordinate system O 2, and then the coordinate of the center of sphere 12O can be converted to the earth coordinate system O 1 according to pose information obtained by resolving the inertial measurement data, and a conversion equation for converting the coordinate of the center of sphere 12O from O 3 to O 1 is obtained under each pose, wherein the conversion equation contains an extrinsic matrix between the IMU and the line laser sensor as an unknown.
Because the coordinates of the sphere center 12O in the geodetic coordinate system O 1 are unchanged under a plurality of different poses, a plurality of conversion equations are combined and solved according to the invariance of the coordinates of the sphere center 12O, and an external parameter matrix can be obtained, so that the calibration of the IMU and the line laser sensor is realized.
Fig. 1 is a flowchart of a calibration method of an inertial measurement unit and a line laser sensor in the present embodiment.
As shown in fig. 1, based on the above principle, the calibration method of the present embodiment specifically includes the following steps:
step S1, under a plurality of different poses of the track detection device, scanning a standard ball with a fixed position through a line laser sensor to obtain profile data, and obtaining inertial measurement data under each pose through an inertial measurement unit;
step S2, calculating a plurality of spherical center coordinates of the standard sphere under a line laser coordinate system of the line laser sensor based on the measured profile data;
Step S3, for each pose, based on corresponding inertial measurement data, establishing a conversion equation for converting corresponding spherical center coordinates from a line laser coordinate system to an inertial coordinate system of an inertial measurement unit, wherein the conversion equation comprises an external parameter matrix to be calibrated;
and S4, solving based on a plurality of conversion equations to obtain an external parameter matrix, and calibrating the inertial measurement unit and the line laser sensor.
The above steps will be described in detail below.
Step S1, under a plurality of different poses of the track detection device, scanning the standard ball with the fixed position through the line laser sensor to obtain profile data, and obtaining inertial measurement data under each pose through the inertial measurement unit.
In this embodiment, the track detection device is transformed into six different poses, and six sets of profile data and inertial measurement data are measured, so as to obtain a sufficient number of transformation equations to solve the extrinsic matrix. In the alternative, more sets of data may be measured, such as transforming eight different poses, measuring eight sets of data, and removing two sets where the error is relatively large, leaving six sets for further computational analysis.
The line laser sensor scans the spherical surface of the standard sphere 12, and the obtained profile data is three-dimensional point cloud data of a circle segment 121 on the spherical surface.
And step S2, calculating a plurality of spherical center coordinates of the standard sphere under the line laser coordinate system of the line laser sensor based on the measured profile data.
Fig. 4 is a flowchart of step S2 in the present embodiment.
As shown in fig. 3 and 4, step S2 specifically includes the following sub-steps for each pose:
and step S2-1, fitting the arc segment 121 based on the corresponding point cloud data to obtain a fitted arc segment.
Step S2-2, based on the fitting circular arc segment, the coordinates (x A,zA) and the circular arc radius r of the circle center 121A of the circular arc segment 121 under the laser coordinate system O 3 are obtained through a least square method.
In step S2-3, the coordinates (x O,yO,zO) of the sphere center 12O under the laser coordinate system O 3 are obtained according to the geometric relationship between the standard sphere 12 and the circular arc segment 121 and the sphere radius R of the standard sphere 12.
And S3, for each pose, based on corresponding inertial measurement data, establishing a conversion equation for converting corresponding spherical center coordinates from a line laser coordinate system to a geodetic coordinate system, wherein the conversion equation comprises an external reference matrix to be calibrated.
Specifically, according to the core formula ax=xb of the hand-eye calibration method, the following formula can be obtained:
In the above formula, P w is the center of sphere coordinate under the geodetic coordinate system O 1, and P l is the center of sphere coordinate under the line laser coordinate system, where P w remains unchanged under multiple poses, and P l can be obtained by the above step S2; is a rotation matrix of an inertial coordinate system O 2 to a geodetic coordinate system O 1,/> Translation matrix from inertial coordinate system O 2 to geodetic coordinate system O 1,/>And/>The inertial measurement unit can be obtained by calculation according to inertial measurement data output by the IMU; /(I)Is a rotation matrix from a linear laser coordinate system O 3 to an inertial coordinate system O 2,/>Translation matrix from linear laser coordinate system O 3 to inertial coordinate system O 2,/>And/>For unknown, matrix to be calibrated, i.e./>Is an external reference matrix to be calibrated.
For the ith pose, the above equation can be written as:
The following formulas are the same and will not be described in detail.
And S4, solving based on a plurality of conversion equations to obtain an external parameter matrix, and calibrating the inertial measurement unit and the line laser sensor.
Fig. 5 is a flowchart of step S4 in the present embodiment.
As shown in fig. 5, step S4 specifically includes the following sub-steps:
and step S4-1, respectively transforming six transformation equations based on matrix operation, wherein the transformed transformation equations comprise spherical center coordinates in a geodetic coordinate system, an external reference matrix and a translation matrix from a line laser coordinate system to the geodetic coordinate system.
Fig. 6 is an exemplary diagram of a conversion equation conversion process in the present embodiment, and a process of creating and converting a conversion equation in the first pose is exemplarily shown in fig. 6.
As shown in fig. 6, for the first pose, according to the above step S3, the resulting formula (1) may be established. And (3) transforming the formula (1) to obtain the formula (2).
And (3) writing out the elements of each matrix in the formula (2). As can be seen from the formula (3),Containing 9 unknown elements,/>Contains 3 unknown elements. In the formula (3)/>And/>The specific elements of the method can be respectively referred to a formula (7) and a formula (8) by adopting shorthand, wherein/>The rotation sequence is appointed as a Z axis, a Y axis and an X axis; /(I)Is the rotation angle of the line laser coordinate system relative to the inertial coordinate system.
And (3) performing matrix operation based on the formula (3), and sequentially obtaining the formulas (4) to (6). As shown in fig. 6, equation (6) may also be written as:
In the formula (6), M is an intermediate matrix for assisting calculation, and the intermediate matrix M contains only the elements of the center of sphere coordinate P l in the line laser coordinate system O 3 and a rotation matrix from the inertial coordinate system O 2 to the geodetic coordinate system O 1 Are known amounts of elements. Translation matrix/>Also known. The external reference matrix X is a matrix in a single column form and comprises/>9 Elements of /)Is a single element of 3.
And S4-2, subtracting two pairs of the six converted conversion equations by utilizing the fact that the spherical center coordinates are unchanged under the geodetic coordinate system, and obtaining five intermediate equations.
In the formula (6), the matrix on the left side of the equal sign is the sphere center coordinate under the geodetic coordinate system O 1, and because the standard sphere 12 is fixed in position, the matrix in the formula of each pose is the same, the matrix term can be removed by subtracting two from two, and then the intermediate equation can be obtained by simplifying, wherein each intermediate pose can be expressed as:
M′X＝T′
in the above formula, M 'is a matrix obtained by subtracting the intermediate matrix M of the two transformed transformation equations, and T' is a translation matrix of the two transformed transformation equations The resulting matrix is subtracted.
And S4-3, combining the five intermediate equations, and solving the combined intermediate equations by a least square method to obtain an external parameter matrix.
Obtaining an intermediate equation in the form of AX=B, and then obtaining the intermediate equation by using a least square methodAnd/>Thereby obtaining the external parameter matrix.
After the IMU and the line laser sensor are calibrated by the method, the three-dimensional point cloud acquired by the line laser sensor can be unified under the inertial coordinate system of the IMU, and then the three-dimensional point cloud is converted into the geodetic coordinate according to pose information obtained by resolving the inertial measurement data of the IMU. In this way, after the track detection device 20 walks along the track, the track shape can be completely restored to the three-dimensional point cloud space established by the geodetic coordinate system, and measurement of the geometrical parameters of the track can be performed on the basis of the three-dimensional point cloud space.
Example one action and Effect
According to the calibration method for the inertial measurement unit and the line laser sensor, the standard ball is used as the calibration piece, the position of the standard ball is kept fixed, the pose of the track detection device is adjusted to obtain multiple groups of spherical center coordinates, multiple conversion equations containing the external reference matrix to be calibrated are established based on multiple groups of spherical center coordinates and the hand-eye calibration principle, and the external reference matrix between the inertial coordinate system of the inertial measurement unit and the line laser coordinate system of the line laser sensor is solved based on invariance of the spherical center coordinates under the geodetic coordinate system.
In the embodiment, in step S4, elements of the rotation matrix and the translation matrix from the line laser coordinate system to the inertial coordinate system are combined into a single-column type external reference matrix by transforming the transformation equation, and by subtracting the transformed transformation equation from each other, the term of the spherical center coordinate under the geodetic coordinate system is eliminated, so as to obtain a plurality of intermediate equations, wherein only the external reference matrix in the intermediate equations is an unknown matrix, therefore, the external reference matrix can be conveniently solved by a least square method, the calculated amount is small, and the calibration result can be efficiently obtained on the track detection site.
< Example two >
In the present embodiment, the same reference numerals are given to the same constituent elements as those in the first embodiment, and the description thereof is omitted.
Compared with the first embodiment, the difference is that in step S1 of the present embodiment, at least four poses are transformed to obtain four sets of profile data and inertial measurement data.
Fig. 8 is a flowchart of step S4 in the present embodiment.
As shown in fig. 8, step S4 of the present embodiment includes the following sub-steps:
and step S4-1, performing form transformation on each conversion equation based on matrix operation.
Fig. 9 is an example diagram of a solution rotation matrix in the present embodiment.
As indicated by block 91 in fig. 9, the transformed conversion equation may be expressed as:
I.e., equation (2) shown in fig. 6.
Step S4-2, subtracting the transformed conversion equation from each other by using invariance of the spherical center coordinate P w under the geodetic coordinate system O 1 to eliminate the term of the spherical center coordinate P w and to include the translation matrixAnd obtaining a plurality of first intermediate equations.
Step S4-3, combining the plurality of first intermediate equations, and solving the combined first intermediate equations by a least square method to obtain a rotation matrix from the line laser coordinate system O 3 to the inertial coordinate system O 2
As shown in block 92 in fig. 9, the two transformed conversion equations are subtracted and reduced to obtain a plurality of first intermediate equations, which are combined and written in a matrix form, and may be represented as:
In the above formula, A, B is a matrix obtained by combining the first intermediate equations, A only includes the spherical center coordinates P l under each pose, and B only includes the translation matrix And thus are known matrices. As shown in block 93 of fig. 9, the above equation is solved by the least square method, and a rotation matrix/>, from the line laser coordinate system O 3 to the inertial coordinate system O 2, is obtained
And S4-4, subtracting the conversion equations after form transformation by two by utilizing invariance of the spherical center coordinate P w under the geodetic coordinate system O 1 so as to eliminate the term of the spherical center coordinate P w, and obtaining a plurality of second intermediate equations.
Step S4-5, combining a plurality of second intermediate equations, and solving the combined second intermediate equations by a least square method to obtain a translation matrix from the line laser coordinate system O 3 to the inertial coordinate system O 2 Thereby obtaining the external parameter matrix.
Fig. 10 is an exemplary diagram of resolving a translation matrix in the present embodiment.
As shown in block 101 in fig. 10, the two transformed conversion equations are subtracted and reduced to obtain a plurality of second intermediate equations, which are combined and written in a matrix form, and may be represented as:
in the above formula, C, D is a matrix obtained by combining the second intermediate equations, and C only includes the rotation matrix under each pose D contains only the rotation matrix/>, in each poseSpherical center coordinates P l and rotation matrix/>, under each pose The solution has been obtained by steps S4-1 to S4-3, so C, D are known matrices.
In this embodiment, other structures and methods are the same as those in the embodiment, and thus the description will not be repeated.
Example two actions and effects
According to the calibration method of the inertial measurement unit and the line laser sensor, on the basis of the action and the effect of the first embodiment, the rotation matrix and the translation matrix from the line laser coordinate system to the inertial coordinate system are solved separately, so that the number of unknown elements in a single separated solving step is correspondingly reduced, the external parameter matrix can be obtained based on the measurement data sets with fewer numbers, and the calibration efficiency is further improved.
The above examples are only for illustrating the specific embodiments of the present invention, and the present invention is not limited to the description of the above examples, it should be understood by those skilled in the art that the present invention is not limited by the above examples, the above examples and the description are merely illustrative of the principles of the present invention, and various changes and modifications can be made therein without departing from the spirit and scope of the invention as claimed. The scope of the invention is defined by the appended claims and equivalents thereof.
For example, in the above embodiments, the standard sphere is a precision machined standard sphere, and in the alternative, the standard sphere may be a precision machined hemisphere.
In the above embodiment, the track detection device is changed to six positions to measure six sets of calibration data, in an alternative scheme, more positions can be changed, and in the measured more sets of calibration data, the data with larger errors are excluded, and the calibration data of at least six positions are reserved for further calculation and analysis, so that corresponding technical effects can be realized.
Claims (10)
1. An on-line calibration method for an inertial measurement unit and a line laser sensor, which is used for on-line calibrating the inertial measurement unit and the line laser sensor arranged on a track detection device to obtain an external parameter matrix, is characterized by comprising the following steps:
Step S1, under a plurality of different poses of the track detection device, scanning a standard ball with a fixed position through the line laser sensor to obtain profile data, and obtaining inertial measurement data through the inertial measurement unit;
step S2, calculating a plurality of spherical center coordinates of the standard sphere under a line laser coordinate system of the line laser sensor based on the profile data;
Step S3, for each pose, establishing a conversion equation for converting the corresponding spherical center coordinate from the line laser coordinate system to a geodetic coordinate system based on the corresponding inertial measurement data, wherein the conversion equation comprises the external parameter matrix to be calibrated;
and S4, solving based on a plurality of conversion equations to obtain the extrinsic matrix.
2. The method for on-line calibration of an inertial measurement unit and a line laser sensor according to claim 1, wherein:
Wherein in step S3, the established conversion equation comprises the spherical center coordinates in the geodetic coordinate system, the spherical center coordinates in the line laser coordinate system and a conversion matrix,
The elements of the extrinsic matrix are included in the conversion matrix,
Step S4 comprises the following sub-steps:
S4-1, respectively transforming each transformation equation based on matrix operation, wherein the transformed transformation equation comprises spherical center coordinates under the geodetic coordinate system and the external parameter matrix;
s4-2, subtracting the transformed conversion equations from each other by utilizing invariance of the spherical center coordinates in the geodetic coordinate system so as to eliminate the items of the spherical center coordinates in the geodetic coordinate system and obtain a plurality of intermediate equations;
And S4-3, converting a plurality of intermediate equations into a matrix equation, and solving the matrix equation by a least square method to obtain the external parameter matrix.
3. The on-line calibration method of an inertial measurement unit and a line laser sensor according to claim 2, wherein:
In step S3, for each pose, the conversion equation established includes a spherical center coordinate P w of the geodetic coordinate system, a spherical center coordinate P l of the line laser coordinate system, and a rotation matrix from the line laser coordinate system to the inertial coordinate system And translation matrix/>Rotation matrix/>, from the inertial coordinate system to the geodetic coordinate systemAnd translation matrix/>In step S4-1, for each pose, the intermediate matrix M is an element containing only the spherical center coordinates P l and the rotation matrix/>, in the transformed transformation equationIs a known matrix of elements of (a),
The extrinsic matrix X comprises the rotation matrixIs not limited to the translation matrix/>Is not limited, is a single element, and is a single element.
4. An on-line calibration method of an inertial measurement unit and a line laser sensor according to claim 3, wherein:
in step S3, for each pose, the conversion equation is established as follows:
in step S4-1, for each pose, the transformed transformation equation is:
In step S4-2, the intermediate equation is:
M′X＝T′
wherein M 'is a matrix obtained by subtracting the intermediate matrix M of the two transformed transformation equations, and T' is the translation matrix of the two transformed transformation equations The resulting matrix is subtracted.
5. An on-line calibration method of an inertial measurement unit and a line laser sensor according to claim 3, wherein:
Wherein in step S1, the profile data and the inertial measurement data are obtained at least in six different poses,
In step S3, six of the conversion equations are established,
In step S4, the transformed six conversion equations are subtracted from each other to obtain five intermediate equations, and the external parameter matrix is obtained by solving the intermediate equations based on the five intermediate equations.
6. An on-line calibration method of an inertial measurement unit and a line laser sensor according to claim 3, wherein:
Wherein in step S1, for each pose, the obtained profile data is three-dimensional point cloud data of a segment of arc on the spherical surface of the standard sphere,
The coordinates of the sphere center are three-dimensional coordinates,
The extrinsic matrix X comprises the rotation matrixIs not shown) and 3 elements of the translation matrix T L I.
7. The method for on-line calibration of an inertial measurement unit and a line laser sensor according to claim 1, wherein:
Wherein in step S3, the established conversion equation comprises a spherical center coordinate under the geodetic coordinate system, a spherical center coordinate under the line laser coordinate system, a rotation matrix and a translation matrix from the laser coordinate system to the inertial coordinate system,
Step S4 comprises the following sub-steps:
s4-1, performing form transformation on each conversion equation based on matrix operation;
S4-2, subtracting the conversion equations after form conversion by utilizing invariance of the spherical center coordinates in the geodetic coordinate system so as to eliminate the items of the spherical center coordinates in the geodetic coordinate system and the items containing the translation matrix, and obtaining a plurality of first intermediate equations;
S4-3, combining a plurality of first intermediate equations, and solving the combined first intermediate equations by a least square method to obtain the rotation matrix;
s4-4, subtracting the conversion equations after form conversion by two by utilizing invariance of the spherical center coordinates in the geodetic coordinate system so as to eliminate the items of the spherical center coordinates in the geodetic coordinate system and obtain a plurality of second intermediate equations;
And S4-5, combining a plurality of second intermediate equations, and solving the combined second intermediate equations by a least square method to obtain the translation matrix.
8. The method for on-line calibration of an inertial measurement unit and a line laser sensor of claim 7, wherein:
in step S3, for each pose, the conversion equation is established as follows:
Wherein P w is the spherical center coordinate in the geodetic coordinate system, P l is the spherical center coordinate in the line laser coordinate system, A rotation matrix and a translation matrix from the inertial coordinate system to the geodetic coordinate system, respectively,/>A rotation matrix and a translation matrix from the line laser coordinate system to the inertial coordinate system, respectively,
In step S4-1, for each pose, the transformed transformation equation is:
In step S4-3, the first intermediate equation of the simultaneous equation is expressed as:
Wherein A, B is a matrix obtained by combining the first intermediate equations, A only includes the spherical center coordinates P l in each pose, and B only includes the translation matrix in each pose
In step S4-5, the second intermediate equation, which is simultaneous, is expressed as:
Wherein C, D is a matrix obtained by combining the second intermediate equations, and C comprises only the rotation matrix in each pose D contains only the rotation matrix/>, at each poseThe spherical center coordinates P l and the rotation matrix/>, in each pose
9. The method for on-line calibration of an inertial measurement unit and a line laser sensor of claim 7, wherein:
wherein in step S1, the profile data and the inertial measurement data are obtained at least in four different poses,
In step S3, four of the conversion equations are established,
In step S4, the four transformed conversion equations are subtracted from each other to obtain three first intermediate equations and three second intermediate equations, and the rotation matrix and the translation matrix are obtained by solving based on the three first intermediate equations and the three second intermediate equations.
10. The method for on-line calibration of an inertial measurement unit and a line laser sensor according to claim 1, wherein:
Wherein in step S1, for each pose, the obtained profile data is three-dimensional point cloud data of a segment of arc on the spherical surface of the standard sphere,
For each of the poses, step S2 comprises the following sub-steps:
S2-1, fitting the arc segments based on the three-dimensional point cloud data to obtain fitted arc segments;
S2-2, obtaining the circle center coordinates and the arc radius of the fitting arc segment under the line laser coordinate system through a least square method based on the fitting arc segment;
And step S2-3, obtaining the spherical center coordinates of the standard sphere under the line laser coordinate system based on the geometric relationship between the standard sphere and the circular arc section and the sphere radius of the standard sphere.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410087850.3A CN118032007A (en) | 2024-01-22 | 2024-01-22 | Calibration method of inertial measurement unit and linear laser sensor |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410087850.3A CN118032007A (en) | 2024-01-22 | 2024-01-22 | Calibration method of inertial measurement unit and linear laser sensor |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN118032007A true CN118032007A (en) | 2024-05-14 |
Family
ID=91003427
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410087850.3A Pending CN118032007A (en) | 2024-01-22 | 2024-01-22 | Calibration method of inertial measurement unit and linear laser sensor |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN118032007A (en) |
-
2024
- 2024-01-22 CN CN202410087850.3A patent/CN118032007A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107284473B (en) | Track detection system and method | |
| CN107389028B (en) | A kind of three-dimensional coordinate conversion method and device based on coordinate projection | |
| CN106274981B (en) | A kind of track detection device and detection method | |
| CN102251451B (en) | Track geometric state measurement system and method based on multisource information fusion technology | |
| CN109532937A (en) | A kind of vehicle-mounted Metro Clearance detection method and its detection system | |
| CN106524924B (en) | Optical inertia combination-based rail inspection vehicle position and attitude measurement system and method | |
| CN102337710A (en) | GPS (Global Positioning System) track irregularity detection system and method | |
| CN115597535B (en) | High-speed magnetic levitation track irregularity detection system and method based on inertial navigation | |
| CN110700029B (en) | Track ride comfort testing method and system | |
| JP6431157B1 (en) | Apparatus and measuring method for building limit in railway | |
| CN112628524B (en) | High-precision positioning method for small-diameter pipeline robot based on turning angle | |
| CN114390992B (en) | Method for determining the actual position of a track and measuring vehicle | |
| CN114719884A (en) | Attitude measurement precision evaluation method and application of inertial navigation system | |
| CN107101594A (en) | A kind of method for extracting wheel track wheel space wheel rim minimum point | |
| CN110926417B (en) | Vehicle-mounted railway tunnel detection system based on machine vision | |
| CN114132358A (en) | Multi-platform intelligent track comprehensive detection system | |
| CN112484722A (en) | Visual sensor global positioning method combined with inertial navigation system | |
| CN208453013U (en) | Mobile detection robot carries the positioning system of 2D laser radar scanning axle | |
| CN116552598B (en) | Permanent magnet track irregularity detection device and method | |
| RU2114950C1 (en) | Method and device for checking status of railway track | |
| CN109017873A (en) | The method for carrying 2D laser radar scanning axle based on mobile detection robot | |
| CN118032007A (en) | Calibration method of inertial measurement unit and linear laser sensor | |
| CN117237563A (en) | Method and device for constructing space three-dimensional geometric form of steel rail | |
| CN114162170B (en) | Track measurement system and measurement method | |
| CN112504173B (en) | Track irregularity measuring device and method based on laser profile scanning |
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 | ||
| TA01 | Transfer of patent application right |
Effective date of registration: 20240711 Address after: 450100 southwest side of Beijing Road and Jianshe Road intersection, Zhengzhou, Henan, Xingyang Applicant after: CENTRAL PLAINS LEADER RAILWAY TRACK TECHNOLOGY DEVELOPMENT Co.,Ltd. Country or region after: China Applicant after: ZHEJIANG YINLUN INTELLIGENT EQUIPMENT CO.,LTD. Applicant after: Zhejiang Tiantai Xianghe Intelligent Equipment Co.,Ltd. Address before: 450100 southwest side of Beijing Road and Jianshe Road intersection, Zhengzhou, Henan, Xingyang Applicant before: CENTRAL PLAINS LEADER RAILWAY TRACK TECHNOLOGY DEVELOPMENT Co.,Ltd. Country or region before: China Applicant before: ZHEJIANG YINLUN INTELLIGENT EQUIPMENT CO.,LTD. Applicant before: Zhejiang Tiantai hezhixiang Investment Co.,Ltd. |