Detailed Description
In order to make the technical solutions of the present application better understood, the technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application, and it is obvious that the described embodiments are only a part of the embodiments of the present application, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present application.
The application provides a pair of three-dimensional laser scanner calibration algorithm is applied to three-dimensional laser scanner, and three-dimensional laser scanner includes reflecting prism and range finding emitter, and reflecting prism is equipped with horizontal rotation axis, and horizontal rotation axis is equipped with the horizontal scale that is used for measuring its horizontal axis angle and vertical axis angle, and the projection is in reflecting prism after range finding emitter launches the laser beam.
Referring to fig. 1, the calibration algorithm specifically includes the following steps:
s100: establishing a three-dimensional coordinate system in a space where the reflecting prism and the horizontal rotating shaft are located, wherein the central point of the reflecting prism is set as an O point of the three-dimensional coordinate system, the horizontal axis is set as an X axis, the collimation axis is set as a Y axis, and the vertical axis is set as a Z axis;
s200: a three-dimensional body coordinate system can be superposed with the standing center coordinate system through rotation by taking the central point of the reflecting prism as an original point;
note that, by establishing a three-dimensional body coordinate system in which the reflecting prism and the horizontal rotation axis are located and a center-of-gravity coordinate system established with the center point of the reflecting prism as the origin, since the three-dimensional laser scanner can rotate 360 °, the three-dimensional body coordinate system is used as the coordinate system in which the three-dimensional laser scanner rotates, and the center-of-gravity coordinate system and the three-dimensional body coordinate system coincide at the initial rotation angle.
S300: analyzing the deviation amount of the incident beam according to the three-dimensional body coordinate system and the station center coordinate system, and establishing an incident beam deviation model according to the deviation amount of the incident beam and a pre-obtained original observed quantity;
it should be noted that, the deviation amount of the incident light beam can be calculated through the comparison between the three-dimensional body coordinate system and the center-of-gravity coordinate system, so as to establish an incident light beam deviation model according to the deviation amount of the incident light beam and the original observed amount, where the original observed amount is an observed amount which can be given by the three-dimensional laser scanner in advance and has a certain error, and the original observed amount is corrected through the deviation amount of the incident light beam, so as to obtain the incident light beam deviation model.
S400: projecting the center of gravity coordinate system to a three-dimensional body coordinate system so as to establish a horizontal scale eccentricity difference model;
s500: substituting the incident beam deviation model into a horizontal scale eccentricity difference model to obtain a three-dimensional laser scanner calibration model, wherein the three-dimensional laser scanner calibration model comprises calibration parameters to be solved;
s600: constructing an instrument precision error model and a collimation error model according to a pre-obtained angle measurement error, wherein the pre-obtained angle measurement error comprises a horizontal angle error, a vertical angle error and a distance measurement error, constructing an observation error equation according to the instrument precision error model and the collimation error model, analyzing the observation error equation to obtain an X-axis coordinate error, a Y-axis coordinate error and a Z-axis coordinate error, and taking the X-axis coordinate error, the Y-axis coordinate error and the Z-axis coordinate error as a basis for weighting a calibration parameter to be solved;
it should be noted that, in this embodiment, the influence of the error generated by the accuracy of the instrument itself and the collimation error on the estimation result of the calibration parameter is considered, and the horizontal angle error, the vertical angle error and the ranging error can be obtained from a user manual in the angle measurement device, after the instrument accuracy error model and the collimation error model are established according to the angle measurement error, the observation error equation is constructed according to the instrument accuracy error model and the collimation error model, so as to obtain the basis for weighting the calibration parameter, and thus the weight of each error can be given according to the basis for weighting, so as to improve the estimation accuracy.
S700: calculating partial derivatives of the incident beam deviation model and the horizontal scale eccentricity difference model to obtain a coefficient matrix of a differential equation, establishing an iterative weighted least square equation according to a fixed weight basis and pre-obtained homonymy point observation data, and solving the iterative weighted least square equation into a three-dimensional laser scanner calibration model so as to obtain a solution result of a calibration parameter to be solved;
it should be noted that after the three-dimensional laser scanner calibration model is constructed, the original observation value is substituted into the three-dimensional laser scanner calibration model, and the calibration parameter result can be obtained through iteration.
S800: and calibrating the deviation of the laser incident beam in the projection of the reflecting prism and the eccentricity difference of the calibration horizontal scale according to the solving result of the calibration parameter to be solved, thereby completing the parallel calibration of the horizontal axis formed by the laser incident reflecting prism and the horizontal rotating shaft.
In the embodiment, the three-dimensional laser scanner calibration model is constructed after the deviation value is calculated, the initial observation value is substituted into the three-dimensional laser scanner calibration model to obtain the calibration parameter result, and the deviation of the laser beam incident on the reflecting prism and the eccentricity difference of the calibration horizontal disc are calibrated according to the calibration parameter result, so that the parallel calibration of the horizontal axis and the horizontal rotating axis formed by the laser beam incident reflecting prism is completed.
The following is a detailed description of the present embodiment:
step S100 specifically includes: a three-dimensional body coordinate system O-XYZ is established in the space where the reflection prism and the horizontal rotation axis are located, as shown in the schematic diagram of the coordinate system shown in fig. 2, wherein the central point of the reflection prism is set as the O point of the three-dimensional body coordinate system, the horizontal axis is set as the X axis, the collimation axis is set as the Y axis, the vertical axis is set as the Z axis, the angle that the three-dimensional body coordinate system passes when rotating around the Z axis is the horizontal angle Φ, the angle that the three-dimensional body coordinate system passes when rotating around the Y axis is the vertical angle θ, and when there is no deviation and the reflection vector is in the same direction with the Z axis.
Step S200 specifically includes: as shown in fig. 2, with three-dimensional laser scanningStanding center coordinate system O-X of three-dimensional laser scanner established by taking central point of scanner as original pointsYsZsAnd the station center coordinate system does not change along with the horizontal angle phi and the vertical angle theta, and when the horizontal angle phi is 0, the station center coordinate system is superposed with the three-dimensional body coordinate system, wherein the distance from the original point O to the target point B is defined as r, and the target point is a reflection point of the incident light beam hitting on the reflection prism.
Step S300 specifically includes: as shown in the schematic diagram of the coordinate system shown in fig. 3, the distance measuring and transmitting device 100 transmits the incident beam 110 to generate the reflected beam 120 after being reflected by the 45 ° reflective prism 200, and the reflection point hit by the 45 ° reflective prism 200 is B;
the 45-degree reflecting prism rotates around the Y axis, and the three-dimensional body coordinate system O-XYZ does not change along with the vertical angle theta but changes along with the horizontal angle phi (around the Z axis);
in plane XOZ, if there is no aberration in incident beam 110, then incident beam 110 is incident along the Y-axis and reflected along the Z-axis; in the plane X 'OZ', if the incident light beam 110 is deviated, the incident light beam 110 is incident along AO 'and reflected along OB'.
The incident beam deviation model comprises a horizontal angle correction observed quantity phi 'and a vertical angle correction observed quantity theta';
wherein the horizontal angle correction observed quantity phi' is expressed by formula 1 as:
φ'=φ+co/sinθ+io cotθ+φoequation 1
In equation 1, phi is the original observed quantity of the horizontal angle, C0As the boresight error constant term, ioIs a constant term of error in the horizontal axis, phioIs the index difference constant term of the horizontal angle.
At the same time, horizontal axis error ioIs caused by the fact that the horizontal rotation axis of the 45 deg. reflecting prism 200 cannot be kept horizontal, i.e. due to the incident deviation deltaz of the incident beam 100 with respect to the Z axis, and does not vary with the vertical angle, while the horizontal axis error is generally a small angle, when the horizontal axis error i is smalloAt small angles, this can be expressed in terms of the deviation Δ z of incidence, which is:
s3011: according to the z-axis direction deviation Delta z of the incident beamThe distance delta y from the light source of the distance measurement transmitting device to the reflecting point of the reflecting prism represents a horizontal axis error constant term ioExpressed as:
in the formula, Δ y is a unit distance 1;
s3012: the distance Δ y from the light source of the ranging transmitter to the reflection point of the reflection prism according to the deviation Δ x of the incident beam in the x-axis direction represents a horizontal-axis error constant term φ o, expressed as:
in the formula, Δ y is a unit distance 1;
s3013: substituting equation 2 and equation 3 into equation 1, the horizontal angle correction observation φ' is converted to:
φ'=φ+co[ theta ] - Δ zcot- Δ x equation 4
Further, the vertical angle correction observation amount θ' is expressed by equation 5 as:
θ' ═ θ + ξ (θ) equation 5
In formula 5, θ is the original vertical angle observation, and ξ (θ) is the vertical angle index difference.
Meanwhile, the vertical angle index difference ξ (θ) is caused by the incident deviations Δ x and Δ z, which can be represented by Δ x and Δ z, then there are:
s3001: constructing a transpose matrix of an incident vector a of the incident ray as:
a=[Δx Δy Δz]Twherein, Δ x is the deviation of the incident beam in the x-axis direction, Δ y is the distance from the light source of the distance measuring and transmitting device to the reflection point of the reflection prism, Δ y is the unit distance 1, and Δ z is the deviation of the incident beam in the z-axis direction;
s3002: calculating a reflection vector b of the incident light reflected by the reflecting prism through a formula 6 according to the incident vector a and a unit normal vector n of the reflecting surface of the reflecting prism, wherein the formula 6 is as follows:
b-a-2 (a · n) n formula 6
In the formula, the unit normal vector n is calculated by formula 7, where formula 7 is:
then a · n in equation 6 is:
s3003: constructing a transpose matrix of the reflection vector b as follows:
b=[bx by bz]Twherein b isxIs the X-axis component of the reflection vector b, byIs the y-axis direction component of the reflection vector b; bzIs the z-axis direction component of the reflection vector b;
s3004: the component vector in each axis direction according to the reflection vector b in step S3002 is expressed as:
s3005: and projecting the reflection vector to a plane XOZ belonging to the three-dimensional body coordinate system, and then rotating the reflection vector by an angle theta by taking the Y axis as a rotating axis to obtain a vector u, wherein u is expressed as:
u=[ux uy uz]Tt is a matrix transpose,
the components of the vector u are then:
s3006: and calculating a vertical angle index difference xi (theta) according to the included angle between the vector u and the Z axis as follows:
in the formula, xioIs a vertical angular scale difference constant;
s3007: substituting equation 10 into equation 5, the vertical angle correction observation θ' is converted to:
further, step S400 specifically includes:
s4001: defining the center of gravity coordinate system as O-XsYsZsThen belongs to the horizontal plane X in the center-of-gravity coordinate systemsOYsEstablishing a polar coordinate with the horizontal angle phi as a variable as follows:
in the formula, δ X is the deviation of the X-axis direction of the horizontal scale, δ Y is the deviation of the Y-axis direction of the horizontal scale;
s4002: defining the three-dimensional body coordinate system as O-XYZ, substituting formula 12 into the polar coordinates of the three-dimensional body coordinate system O-XYZ to obtain a horizontal scale eccentricity difference corrected station center coordinate system, namely obtaining a horizontal scale eccentricity difference model, wherein the horizontal scale eccentricity difference model is as follows:
further, step S500 specifically includes:
s5001: substituting formula 4 and formula 11 into formula 13 to obtain a three-dimensional laser scanner calibration model, where the three-dimensional laser scanner calibration model is:
wherein r is an original distance measurement observed quantity, phi is an original horizontal angle observed quantity, theta is an original vertical angle observed quantity, m is a distance correction addition constant, lambda is a distance correction multiplication constant, coIs the collimation axis error constant term, xioThe constant term of the vertical angle scale difference is adopted, wherein delta X is the deviation of an incident beam in the X-axis direction, delta Z is the deviation of an incident beam in the Z-axis direction, delta X is the deviation of the horizontal scale in the X-axis direction, and delta Y is the deviation of the horizontal scale in the Y-axis direction;
it should be noted that the original distance measurement observed quantity r, the original horizontal angle observed quantity phi and the original vertical angle observed quantity theta are parameters that can be observed in the three-dimensional laser scanner in advance; and distance correction adding constant m, distance correction multiplying constant lambda and collimation axis error constant item coVertical angle scale difference constant term xioThe X-axis direction deviation delta X of the incident light beam, the Z-axis direction deviation delta Z of the incident light beam, the X-axis direction deviation delta X of the horizontal scale and the Y-axis direction deviation delta Y of the horizontal scale are calibration parameters to be solved.
Because the three-dimensional laser scanner needs distance measurement and angle measurement to obtain point cloud data, but before calibration, the distance measurement and the angle measurement have errors, the calibration scanner needs observation data of the same-name point, and when the same-name point is selected, a collimation error exists. Therefore, the effect of observation errors on the calibration parameter estimation should be taken into account when estimating the scanner calibration parameters.
Before considering the influence of the observation error on the estimation result of the calibration parameter, it is also required to verify whether the error before establishing the calibration model of the three-dimensional laser scanner is consistent with the error after establishing the calibration model of the three-dimensional laser scanner, so that the following steps are performed:
if the horizontal angle error is δ Φ and the vertical angle error is δ θ, then:
the horizontal angle error and the vertical angle error can be obtained in a user manual of the angle measuring device, since the vertical angle error δ θ is a small angle and can be ignored, so a second-order term is ignored, according to a trigonometric function formula, cot δ θ → ∞, sin δ θ ═ 0, cos δ θ ═ 1, then the formula 15 can be simplified as follows:
since the above approximation and the right angle error are neglected, it can be seen that δ θ does not affect δ Φ', so there are:
δ Φ ═ δ Φ equation 17
The same reason is also that
θ '+ δ θ' ═ θ + δ θ + ξ (θ) formula 18
δ θ ═ δ θ equation 19
From the above formula, it is known that δ Φ 'is δ Φ and δ θ' is δ θ, and there are:
sin (phi ' + delta phi) ═ cos delta phi sin phi ' + cos phi ' sin delta phi ═ sin phi ' + delta phi cos phi ' equation 20
cos (phi ' + delta phi) ═ cos delta phi cos phi ' -sin phi ' sin delta phi ═ cos phi ' -delta phi sin phi ' equation 21
The same principle is as follows:
sin (θ ' + δ θ) ═ sin θ ' + δ θ cos θ ' equation 22
cos (θ ' + δ θ) ═ cos θ ' - δ θ sin θ ' equation 23
Substituting equations 22 and 23 into the three-dimensional laser scanner calibration model, the high order quantity is truncated as follows:
cos(φ'+δφ)sin(θ'+δθ)=(cosφ'-δφsinφ')(sinθ'+δφsinθ')
the equation 24 is expressed by cos phi 'sin theta' + δ phi cos phi 'cos theta' - δ phi sin phi 'sin theta' + o (n)
sin(φ'+δφ)sin(θ'+δθ)=(sinφ'+δφcosφ')(sinθ'+δφcosθ')
Sin phi 'sin theta' + δ phi sin phi 'cos theta' + δ phi cos phi 'sin theta' + o (n) formula 25
Therefore, the error before the three-dimensional laser scanner calibration model is established is consistent with the error after the three-dimensional laser scanner calibration model is established.
Further, step S600 specifically includes:
s6001: recording the corrected distance measurement observed value r' ═ r + m + λ r, and after eliminating the high-order quantity from formula 14, obtaining an instrument precision error model as follows:
s6002: because three-dimensional laser scanner aims a homonymy point and shines, then must produce the error, can divide into horizontal component and vertical component with the error that produces, register to aim at the horizontal component of error for delta alpha, aim at the vertical component of error for delta beta, then aim at the error model and be:
s6003: because δ Φ, δ θ, δ r are independent of each other, only the square value of each term needs to be taken, and then the high-order error is eliminated according to the formula 26, the formula 27, the formula 28, and the formula 29, and the observation error equation is obtained as follows:
in the formula (I), the compound is shown in the specification,
as a basis for weighting the calibration parameters to be solved.
Further, step S700 specifically includes:
s7001: for equation 14, distance correction adding constant m, distance correction multiplying constant lambda and collimation axis error constant item coVertical angle scale difference constant term xioSolving partial derivatives of X-axis direction deviation delta X of the incident light beams, Z-axis direction deviation delta Z of the incident light beams, X-axis direction deviation delta X of the horizontal scale and Y-axis direction deviation delta Y of the horizontal scale;
specifically, the partial derivatives of the calibration parameters are:
the distance correction plus the partial derivative of the constant m is:
the partial derivative of the distance correction times the constant λ is:
quasi-axis error constant term coThe partial derivatives of (a) are:
vertical angle scale difference constant term xioThe partial derivatives of (a) are:
the partial derivative of the X-axis directional deviation Δ X of the incident beam is:
the partial derivative of the Z-axis direction deviation Δ Z of the incident beam is:
the partial derivative of the X-axis deviation deltax on the horizontal scale is:
the partial derivative of the horizontal scale Y-axis direction deviation δ Y is:
s7002: and (3) constructing a full differential equation of the three-dimensional laser scanner calibration model according to the partial derivatives of the items in the step S7001, wherein the full differential equation is as follows:
in the formula (I), the compound is shown in the specification,
the partial derivative of the constant m is added for the distance correction,
to correct the distance by the partial derivative of the constant lambda,
multiplying constant c for distance correction
oThe partial derivative of (a) of (b),
is the vertical angle scale difference constant term xi
oThe partial derivative of (a) of (b),
to enterThe partial derivative of the X-axis directional deviation deltax of the beam,
as a partial derivative of the Z-axis directional deviation az of the incident light beam,
is the partial derivative of the deviation deltax in the X-axis direction of the horizontal scale,
the partial derivative of the deviation deltay of the Y-axis direction of the horizontal scale;
solving the three-dimensional laser scanner calibration model by the iterative weighted least square equation comprises an inner coincidence calibration parameter solving process and/or an outer coincidence calibration parameter solving process:
the solving process of the internal coincidence calibration parameters specifically comprises the following steps:
s7011: the three-dimensional laser scanner is driven by a horizontal rotating shaft to carry out 360-degree scanning work, and n homonymous points are collected and recorded as B
iN, and the horizontal angle corresponding to n homonymous points is phi
oiHorizontal angle phi
oiA point measured in a range of 0 to 180 DEG of the rotation angle is defined as [ phi ]
oi,θ
oi,r
oi]The corresponding three-dimensional coordinate system result is defined as [ x ]
oi,y
oi,z
oi]Substituting the original observation results corresponding to n homonymous points within the range of 0-180 degrees of the rotation angle into a formula 14 to obtain
S7012: recording horizontal angle phi
oiThe point measured in the range of 180-360 DEG of rotation angle is defined as [ phi ]
1i,θ
1i,r
1i]The corresponding three-dimensional coordinate system result is defined as [ x ]
1i,y
1i,z
1i]Substituting the original observation results corresponding to n homonymous points within the range of the rotation angle of 180-360 degrees into the formula 14 to obtain
S7013: noting the true coordinate of the same name point as FrefFor F in equation 14sBy performing taylor expansion, one can obtain:
subtracting simultaneous formula 41, the true coordinate of the same-name point is F
refEliminated to obtain an observation equation internally conforming to the estimation of the calibration parameters, wherein the weight P array of the observed values is determined by the error equation of the observed quantity, and the vector of the calibration parameters is recorded as
The system of equations for the least squares solution can be found as:
s7014: since the calibration parameter to be estimated is a small numerical error, an initial value is given according to equation 42
Iterating by using Gauss-Newton iteration method
The result of the calibration parameter can be obtained
The solving process of the external coincidence calibration parameters specifically comprises the following steps:
s7021: providing a reference point and external orientation parameters by a total station through setting relative to the outside of the three-dimensional laser scanner, wherein the external orientation parameters comprise a three-dimensional body coordinate system of the three-dimensional laser scanner and conversion parameters of an external coordinate system, and specifically comprise an X-axis rotation angle, a Y-axis rotation angle, a Z-axis rotation angle, an X-axis translation amount, a Y-axis translation amount and a Z-axis translation amount;
s7022: rotating and translating the three-dimensional laser scanner calibration model through external orientation parameters to obtain an observation equation with external coincidence estimation as follows:
in the formula, R is a rotating cosine matrix;
s7023: solving the full differential equation for equation 43 as:
s7024: for n homonyms B
iAnd i is 1,2, and n, and the observed quantity of the spherical coordinate system measured by the three-dimensional laser scanner is recorded as
The observed quantity in the Z-axis direction is obtained by substituting the observed quantity in the formula 43
Recording the reference observed quantity of a space rectangular coordinate system given by the total station as
Taylor expansion of equation 43 yields:
then the system of equations for the least squares solution is obtained according to equation 45 as:
s7025: to ensure
And the accuracy of the final convergence result, wherein the external orientation parameter needs to be given an initial value, the external orientation parameter is given an initial value according to a formula 47, and the formula 47 is expressed as follows:
wherein svd is singular value decomposition, and MEAN is column averaging; after obtaining the fixed initial value of the external orientation parameter, setting the initial values of the other parameters as 0, and iterating
All calibration parameter results can be obtained
The above embodiments are only used to illustrate the technical solutions of the present application, and not to limit the same; although the present application has been described in detail with reference to the foregoing embodiments, it should be understood by those of ordinary skill in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some technical features may be equivalently replaced; and such modifications or substitutions do not depart from the spirit and scope of the corresponding technical solutions in the embodiments of the present application.