CN117705097A - Prism rod device, ground object broken point measuring method, device and medium - Google Patents
Prism rod device, ground object broken point measuring method, device and medium Download PDFInfo
- Publication number
- CN117705097A CN117705097A CN202311494895.4A CN202311494895A CN117705097A CN 117705097 A CN117705097 A CN 117705097A CN 202311494895 A CN202311494895 A CN 202311494895A CN 117705097 A CN117705097 A CN 117705097A
- Authority
- CN
- China
- Prior art keywords
- moment
- error
- value
- prism
- prism rod
- 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
- 238000000034 method Methods 0.000 title claims abstract description 46
- 239000011159 matrix material Substances 0.000 claims abstract description 83
- 238000012937 correction Methods 0.000 claims abstract description 19
- 238000001914 filtration Methods 0.000 claims abstract description 19
- 230000003068 static effect Effects 0.000 claims abstract description 11
- 238000009499 grossing Methods 0.000 claims abstract description 6
- 230000006698 induction Effects 0.000 claims description 16
- 238000004590 computer program Methods 0.000 claims description 14
- 230000001133 acceleration Effects 0.000 claims description 13
- 238000000691 measurement method Methods 0.000 claims description 10
- 230000007704 transition Effects 0.000 claims description 9
- 230000008859 change Effects 0.000 claims description 7
- 230000008569 process Effects 0.000 claims description 7
- 230000009466 transformation Effects 0.000 claims description 5
- 238000005259 measurement Methods 0.000 abstract description 42
- 239000012634 fragment Substances 0.000 abstract 2
- 238000012545 processing Methods 0.000 description 5
- 230000006870 function Effects 0.000 description 4
- 230000003595 spectral effect Effects 0.000 description 4
- 238000009825 accumulation Methods 0.000 description 2
- 230000005540 biological transmission Effects 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 230000005484 gravity Effects 0.000 description 2
- 238000009434 installation Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 229910000838 Al alloy Inorganic materials 0.000 description 1
- 206010034719 Personality change Diseases 0.000 description 1
- 229910000831 Steel Inorganic materials 0.000 description 1
- 238000003491 array Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 230000000116 mitigating effect Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000005622 photoelectricity Effects 0.000 description 1
- 238000002360 preparation method Methods 0.000 description 1
- 239000010959 steel Substances 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
- 238000009423 ventilation Methods 0.000 description 1
- 238000012800 visualization Methods 0.000 description 1
Landscapes
- Navigation (AREA)
Abstract
The invention discloses a prism rod device, a ground object broken point measuring method, equipment and a medium, wherein the method comprises the following steps: performing inertial error calibration on MEMS of the prism rod device by using a six-position method; the prism rod device is arranged at a known control point, the attitude angle of the prism rod device is determined and converted into an initial attitude matrix, the prism rod device is controlled to start moving, navigation parameters are obtained by using a mechanical arrangement algorithm according to observation data of closed loop correction at each moment, and correction is carried out according to the distance between the MEMS center and the prism center; and taking the corrected navigation parameter error and the inertial error as state quantities, taking a zero-speed pseudo observed value of the MEMS when a known control point or a to-be-detected fragment point is static, an attitude angle calculated by a magnetometer and known control point position information as observed values, carrying out state estimation by using an extended Kalman filtering algorithm, and obtaining the to-be-detected fragment point position information by using an RTS smoothing filtering algorithm according to a state estimation result. The invention can realize ground object broken point measurement without meeting the requirement of the sight of a prism and a total station.
Description
Technical Field
The invention relates to the technical field of mapping, in particular to a prism rod device, a ground object breaking point measuring method, terminal equipment and a computer readable storage medium.
Background
In the prior art, technical means for performing point measurement of broken parts in urban complex environments generally comprise total station measurement and RTK measurement. The total station measurement is to calculate the coordinates of the points of the to-be-measured breaking points through known point coordinates, and the RTK measurement is to obtain the coordinates of the points of the to-be-measured breaking points through real-time difference of GNSS satellite signals. However, when total station measurement is performed in a dense scene of a building, in order to meet the viewing conditions and measurement of the broken points, a plurality of measuring stations are generally required to be arranged in a short distance, so that the measurement efficiency is affected, and meanwhile, a larger error of the broken point measurement is caused. However, RTK measurement is easily affected by the environment, and in a scene where a building is dense, ambiguity cannot be fixed, so that the broken point measurement cannot be performed accurately.
Disclosure of Invention
The invention provides a prism rod device, a ground object breaking point measuring method, equipment and a medium, wherein the ground object breaking point measurement is not influenced by a measuring environment by fusing a nine-axis MEMS sensor based on an inertia technology with a total station electro-optical distance measuring technology, so that the ground object breaking point measurement can be realized without meeting the requirements of a prism and the total station for the ventilation, the arrangement quantity of the total station is reduced, and the measurement efficiency and the measurement precision of the ground object breaking point are obviously improved.
In order to solve the technical problems described above, a first aspect of an embodiment of the present invention provides a prism rod device, including a prism rod, a prism and a nine-axis MEMS sensor;
the prism is arranged at the top of the prism rod, the nine-axis MEMS sensor is arranged on the prism rod and comprises a three-axis gyroscope, a three-axis accelerometer and a three-axis magnetometer.
Preferably, the prism rod device further comprises a first connecting rod and a second connecting rod; one end of the first connecting rod and one end of the second connecting rod are arranged on the prism rod, and the other end of the first connecting rod and the other end of the second connecting rod are connected with the nine-axis MEMS sensor.
Preferably, the prism rod device further comprises a power supply module; and the power supply end of the power supply module is connected with the power receiving end of the nine-axis MEMS sensor.
A second aspect of the embodiment of the present invention provides a ground object breaking point measurement method, which applies the prism rod device according to any one of the first aspect, and includes the following steps:
calibrating inertial errors of nine-axis MEMS sensors of the prism rod device by using a six-position method, and determining a plurality of calibrated inertial errors;
setting the prism rod device at a preset known control point, determining an attitude angle of the prism rod device at the known control point through the nine-axis MEMS sensor based on the position information of the known control point, and converting the attitude angle into an initial attitude matrix;
controlling the prism rod device to move from the known control point, detecting a specific force value, an angular velocity value and a magnetic induction intensity value at each moment in the moving process through the nine-axis MEMS sensor, and acquiring navigation parameters of the prism rod device at each moment by using a mechanical arrangement algorithm according to the initial gesture matrix, the closed-loop corrected specific force value, angular velocity value and magnetic induction intensity value at each moment; the specific force value, the angular velocity value and the magnetic induction intensity value at the initial moment are corrected in a closed loop by eliminating the calibrated inertial error, and the specific force value, the angular velocity value and the magnetic induction intensity value at the non-initial moment are corrected in a closed loop by eliminating the inertial error estimated by filtering at the last moment;
correcting the navigation parameters at each moment according to the distance between the center of the nine-axis MEMS sensor and the prism center of the prism rod device to obtain corrected navigation parameters at each moment;
taking the corrected navigation parameter error and the inertial error at each moment as state quantity, taking zero-speed pseudo-observed values obtained by the nine-axis MEMS sensor when the known control point or the to-be-detected broken point is static, attitude angles calculated by a three-axis magnetometer of the nine-axis MEMS sensor and position information of the known control point as observed values, and obtaining a state matrix, a state covariance matrix and a state transition matrix at each moment by using an extended Kalman filtering algorithm;
and obtaining the position information of the to-be-detected broken part point by using an RTS smoothing filter algorithm according to the state matrix, the state covariance matrix and the state transition matrix at each moment.
Preferably, the determining, by the nine-axis MEMS sensor, the attitude angle of the prism rod device at the known control point based on the position information of the known control point specifically includes the following steps:
determining a pitch angle and a roll angle of the prism rod device at the known control point through a triaxial accelerometer of the nine-axis MEMS sensor based on the position information of the known control point;
determining a magnetic declination angle of the prism rod device at the known control point through a triaxial magnetometer of the nine-axis MEMS sensor, and obtaining a heading angle of a geographic coordinate system according to the magnetic declination angle;
and determining the attitude angle of the prism rod device at the known control point according to the pitch angle, the roll angle and the course angle.
As a preferred solution, the navigation parameters of each moment of the prism rod device are obtained by using a mechanical arrangement algorithm according to the initial gesture matrix, the closed-loop corrected specific force value, the angular velocity value and the magnetic induction intensity value at each moment, and specifically include the following steps:
obtaining a gesture matrix at the k-1 moment according to the product of the gesture matrix at the k-1 moment, the navigation coordinate system change matrix from the k-1 moment to the k moment and the carrier coordinate system change matrix; wherein k is an integer greater than 0; when k=1, the gesture matrix at the k-1 time is the initial gesture matrix;
determining a specific force speed increment and a Gong speed increment at the kth moment according to the gesture matrix at the kth-1 moment, the specific force value and the angular speed value after closed loop correction at the kth-1 moment and the specific force value and the angular speed value after closed loop correction at the kth moment, and obtaining a speed value at the kth moment according to the speed value at the kth-1 moment, the sum of the specific force speed increment and the Gong speed increment at the kth moment;
acquiring position information of the prism rod device at the kth moment according to the speed value at the kth moment and the speed value at the kth-1 moment;
and determining navigation parameters of the prism rod device at each moment according to the gesture matrix, the speed value and the position information of the prism rod device at each moment.
As a preferred scheme, the nine-axis MEMS sensor of the prism rod device is calibrated by utilizing a six-position method, and a plurality of calibration inertial errors are determined, and the method specifically comprises the following steps:
respectively upwards and standing six planes of the nine-axis MEMS sensor to obtain an output value of a group of triaxial accelerometers, an output value of a group of triaxial gyroscopes and an output value of a group of triaxial magnetometers;
based on the obtained output value of the triaxial accelerometer, solving the following expression by using a Gauss Newton iteration method to obtain an accelerometer inertia error:
wherein θ a Representing accelerometer inertial error; k represents the time; g represents the local gravitational acceleration; h (a) s ,θ a ) Representing the output value of a triaxial accelerometerAn expression for overcorrection of errors; the accelerometer inertia errors comprise accelerometer zero offset errors, accelerometer mounting angle errors and accelerometer scale factor errors;
based on the obtained output value of the triaxial accelerometer and the output value of the triaxial gyroscope, solving the following expression by using a Gauss Newton iteration method to obtain a gyroscope inertia error:
wherein θ gry Representing gyroscope inertial errors;an output value of the triaxial accelerometer at a kth time after error correction; mu (mu) k Representing the acceleration value subjected to attitude transformation; the gyroscope inertia error comprises a gyroscope zero offset error, a gyroscope mounting angle error and a gyroscope scale factor error;
based on the obtained output value of the triaxial magnetometer, solving the following expression by using a Gauss Newton iteration method to obtain the magnetometer inertia error:
wherein θ m Representing magnetometer inertial errors; m represents the local magnetic field strength; h (m) s ,θ m ) An expression indicating that the output value of the three-axis magnetometer is error corrected; the magnetometer inertia errors comprise magnetometer zero offset errors, magnetometer mounting angle errors and magnetometer scale factor errors.
A third aspect of an embodiment of the present invention provides a terminal device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, where the processor implements the ground object breaking point measurement method according to any one of the second aspects when executing the computer program.
A fourth aspect of the embodiment of the present invention provides a computer readable storage medium, where the computer readable storage medium includes a stored computer program, where when the computer program runs, the computer readable storage medium is controlled to execute the ground object breaking point measurement method according to any one of the second aspects.
Compared with the prior art, the method has the beneficial effects that the nine-axis MEMS sensor based on the inertia technology is fused with the total station electro-optical distance measurement technology, so that ground object broken point measurement is not influenced by the measurement environment, the measurement of the ground object broken point can be realized without meeting the requirements of the prism and the total station for the visualization, the arrangement quantity of the total station is reduced, and the measurement efficiency and the measurement precision of the ground object broken point are remarkably improved.
Drawings
FIG. 1 is a schematic view of a prism rod apparatus in an embodiment of the present invention;
FIG. 2 is a flow chart of a ground object breaking point measurement method in an embodiment of the invention;
FIG. 3 is a diagram of a data processing architecture of a nine-axis MEMS processor in an embodiment of the invention;
1, a prism rod; 2. a prism; 3. nine-axis MEMS sensor; 4. a first connecting rod; 5. and a second connecting rod.
Detailed Description
The following description of the embodiments of the present invention will be made clearly and completely with reference to the accompanying drawings, in which it is apparent that the embodiments described are only some embodiments of the present invention, but not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
Referring to fig. 1, a first aspect of an embodiment of the present invention provides a prism rod apparatus comprising a prism rod 1, a prism 2 and a nine-axis MEMS sensor 3;
the prism 2 is arranged at the top of the prism rod 1, the nine-axis MEMS sensor 3 is arranged on the prism rod 1, and the nine-axis MEMS sensor 3 comprises a three-axis gyroscope, a three-axis accelerometer and a three-axis magnetometer.
It is worth noting that, through the prism 2 of locating prism pole 1 top, can utilize total powerstation photoelectricity distance measurement technique to realize the measurement of ground object broken part point, when need carry out ground object broken part point measurement under the comparatively dense scene of building, can utilize nine MEMS sensor 3 based on inertial technology to carry out ground object broken part point measurement based on the positional information of known control point for ground object broken part point measurement is not influenced by measuring environment, need not to satisfy prism 2 and total powerstation and can realize the measurement of ground object broken part point, helps reducing total powerstation's the quantity of laying, has shown improvement ground object broken part point's measurement efficiency and measurement accuracy.
Preferably, the prism rod device further comprises a first connecting rod 4 and a second connecting rod 5; one end of the first connecting rod 4 and one end of the second connecting rod 5 are arranged on the prism rod 1, and the other end of the first connecting rod 4 and the other end of the second connecting rod 5 are connected with the nine-axis MEMS sensor 3.
It should be noted that, in this embodiment, the first connecting rod 4 and the second connecting rod 5 form a double-layer fixing structure, so that the stability of the nine-axis MEMS sensor 3 on the prism rod 1 can be improved, and the vibration of the nine-axis MEMS sensor 3 is reduced.
As an alternative embodiment, the first connecting rod 4 and the second connecting rod 5 are detachably connected with the prism rod 1 and the nine-axis MEMS sensor 3. Preferably, the materials of the first connecting rod 4 and the second connecting rod 5 are light and hard aluminum alloy.
Preferably, the prism rod device further comprises a power supply module; the power supply end of the power supply module is connected with the power receiving end of the nine-axis MEMS sensor 3.
According to the prism rod device, the nine-axis MEMS sensor based on the inertia technology is integrated with the total station photoelectric ranging technology, so that ground object broken point measurement is not influenced by the measurement environment, the ground object broken point measurement can be realized without meeting the requirements of the prism and the total station for the vision, the arrangement quantity of the total station is reduced, and the measurement efficiency and the measurement precision of the ground object broken point are remarkably improved.
Referring to fig. 2 and 3, a second aspect of the embodiment of the present invention provides a ground object breaking point measurement method, which applies the prism rod device according to any one of the embodiments of the first aspect, and includes the following steps S1 to S6:
step S1, calibrating inertial errors of nine-axis MEMS sensors of a prism rod device by using a six-position method, and determining a plurality of calibrated inertial errors;
step S2, setting the prism rod device at a preset known control point, determining an attitude angle of the prism rod device at the known control point through the nine-axis MEMS sensor based on the position information of the known control point, and converting the attitude angle into an initial attitude matrix;
step S3, controlling the prism rod device to move from the known control point, detecting a specific force value, an angular velocity value and a magnetic induction intensity value at each moment in the moving process through the nine-axis MEMS sensor, and acquiring navigation parameters of the prism rod device at each moment by using a mechanical arrangement algorithm according to the initial gesture matrix, the closed-loop corrected specific force value, the angular velocity value and the magnetic induction intensity value at each moment; the specific force value, the angular velocity value and the magnetic induction intensity value at the initial moment are corrected in a closed loop by eliminating the calibrated inertial error, and the specific force value, the angular velocity value and the magnetic induction intensity value at the non-initial moment are corrected in a closed loop by eliminating the inertial error estimated by filtering at the last moment;
step S4, correcting the navigation parameters at each moment according to the distance between the center of the nine-axis MEMS sensor and the center of the prism rod device, and obtaining corrected navigation parameters at each moment;
s5, correcting navigation parameter errors and inertial errors at each moment are used as state quantities, a zero-speed pseudo observed value obtained by the nine-axis MEMS sensor when the known control point or the to-be-detected broken part point is static, an attitude angle calculated by a three-axis magnetometer of the nine-axis MEMS sensor and position information of the known control point are used as observed values, and a state matrix, a state covariance matrix and a state transition matrix at each moment are obtained by using an extended Kalman filtering algorithm;
and S6, obtaining the position information of the to-be-detected broken part point by using an RTS smoothing filter algorithm according to the state matrix, the state covariance matrix and the state transition matrix at each moment.
Specifically, the prism rod device is arranged at a preset known control point, and is stationary for a period of time, for example, 4s, whether the prism rod is located at a broken part point or a control point is distinguished by the length of the stationary time, wherein the position information of the known control point is obtained through RTK measurement or total station measurement, and the observation data of each moment is acquired through a nine-axis MEMS sensor: the specific force value, the angular velocity value and the magnetic induction intensity value are calibrated through the static state of the nine-axis MEMS sensor, meanwhile, in the static time, the measured specific force value is theoretically equal to zero, the angular velocity value is theoretically equal to the rotation angular velocity of the earth, a plurality of pseudo-observation values are added, and therefore error accumulation of the nine-axis MEMS sensor can be effectively restrained.
After the observation data are collected, MEMS data processing is needed, and MEMS error calibration and attitude initialization are needed first.
For MEMS error calibration, preferably, the inertial error calibration is performed on the nine-axis MEMS sensor of the prism rod device by using a six-position method, and a plurality of calibration inertial errors are determined, which specifically includes the following steps:
respectively upwards and standing six planes of the nine-axis MEMS sensor to obtain an output value of a group of triaxial accelerometers, an output value of a group of triaxial gyroscopes and an output value of a group of triaxial magnetometers;
based on the obtained output value of the triaxial accelerometer, solving the following expression by using a Gauss Newton iteration method to obtain an accelerometer inertia error:
wherein θ a Representing accelerometer inertial error; k represents the time; g represents the local gravitational acceleration; h (a) s ,θ a ) An expression representing that the output value of the triaxial accelerometer is subjected to error correction; the accelerometer inertia errors comprise accelerometer zero offset errors, accelerometer mounting angle errors and accelerometer scale factor errors;
based on the obtained output value of the triaxial accelerometer and the output value of the triaxial gyroscope, solving the following expression by using a Gauss Newton iteration method to obtain a gyroscope inertia error:
wherein θ gry Representing gyroscope inertial errors; a, a k s An output value of the triaxial accelerometer at a kth time after error correction; mu (mu) k Representing the acceleration value subjected to attitude transformation; the gyroscope inertia error comprises a gyroscope zero offset error, a gyroscope mounting angle error and a gyroscope scale factor error;
based on the obtained output value of the triaxial magnetometer, solving the following expression by using a Gauss Newton iteration method to obtain the magnetometer inertia error:
wherein θ m Representing magnetometer inertial errors; m represents the local magnetic field strength; h (m) s ,θ m ) An expression indicating that the output value of the three-axis magnetometer is error corrected; the magnetometer inertia errors comprise magnetometer zero offset errors, magnetometer mounting angle errors and magnetometer scale factor errors.
Specifically, the nine-axis MEMS sensor is a closed autonomous pose transmission sensor, is less interfered by the outside, and can realize high-precision transmission of the carrier pose in a short time. Pose transfer is carried out based on a nine-axis MEMS sensor, and the nine-axis MEMS sensor is firstly required to be initialized, wherein the initialization comprises two parts: MEMS inertial error calibration and attitude initialization.
The inertial error of the nine-axis MEMS sensor comprises zero offset error, scale factor error and installation angle error, and the calibration of the inertial error is generally completed indoors. In consideration of cost and practicality, the calibration of MEMS inertial errors is not carried out by any external tool, wherein the calibration of the triaxial accelerometer is restrained by local gravity acceleration, the calibration of the triaxial gyroscope is restrained by the accelerometer and attitude angle, and the calibration of the triaxial magnetometer is calibrated by the amount of the earth magnetic field. The sensor noise model is determined by the power spectral density, and the power spectral density of the noise is calibrated by the Allan variance.
The MEMS inertial error is calibrated by a six-position method: the six faces of the nine-axis MEMS sensor are respectively upwards and continuously placed for a period of time in a static mode, and a group of continuous accelerometer output data, gyroscope output data and magnetometer output data are obtained. The accuracy of the calibration is firstly dependent on the detection of the static and moving states of the MEMS, which can be detected by the covariance of the observed data of the triaxial accelerometer, and then the accelerometer is firstly calibrated based on the detected MEMS static time period. Defining accelerometer inertial error as θ a Including zero offset error of accelerometerAccelerometer mounting angle error [ alpha ] yz α zy α zx ] T Accelerometer scale factor error->Namely, the method can be expressed as:
wherein the output value of the triaxial accelerometer is subjected to an error correction expression h (a s ,θ a ) The expression is as follows:
when the nine-axis MEMS sensor is stationary, the output value of the triaxial accelerometer is equal to the local gravitational acceleration, and the local gravitational acceleration can be calculated through a gravitational model, and the following criteria exist:
and solving by using a Gauss Newton iteration method to obtain the inertial error of the accelerometer.
The MEMS gyroscope calibration is different from the accelerometer, and the accuracy of the MEMS gyroscope is low, so that the rotation angular velocity of the earth cannot be perceived, and the gyroscope calibration needs to be based on the accelerometer calibration result. Firstly, zero offset errors of a gyroscope can be obtained through segment average of gyroscope output data in a static time period, and observation data only comprising installation angle errors and scale factor errors can be obtained by subtracting zero offset errors from the output angular speed of an original gyroscope. The MEMS includes attitude changes in the upward process of different planes, and then:
respectively representing output values of the triaxial accelerometer subjected to error correction at the moment k and the moment k-1, wherein q represents a posture quaternion, and a fourth-order Runge-Kutta integral algorithm is adopted, and mu is adopted to ensure the posture precision k For the acceleration value after gesture transformation, the acceleration value after gesture transformation is equal to the output value of the triaxial accelerometer after error correction, and then the following criteria are provided:
and solving by using a Gauss Newton iteration method to obtain the inertial error of the gyroscope.
The error calibration of the magnetometer is consistent with the calibration method of the accelerometer, and zero offset error of the magnetometer is defined firstlyMounting angle error->And scale factor error->Then it can be expressed as:
the output value of the triaxial magnetometer is subjected to an error-corrected expression h (m s ,θ m ) The expression is as follows:
the output value of the triaxial magnetometer is equal to the local magnetic field intensity, and the magnetometer inertia error can be obtained by using a Gauss Newton iteration method according to the following criteria:
based on the original output of the MEMS, the Allan variances of the accelerometer, the gyroscope and the magnetometer are calculated respectively, then a double-logarithmic curve of the Allan variances is calculated, and the noise power spectral densities of different sensors are obtained according to the double-logarithmic curve.
For the attitude initialization, the attitude angle (pitch angle, roll angle and heading angle) of the carrier at the starting position relative to the geographic coordinate system is determined, in this embodiment, preferably, the attitude angle of the prism rod device at the known control point is determined by the nine-axis MEMS sensor based on the position information of the known control point, and specifically includes the following steps:
determining a pitch angle and a roll angle of the prism rod device at the known control point through a triaxial accelerometer of the nine-axis MEMS sensor based on the position information of the known control point;
determining a magnetic declination angle of the prism rod device at the known control point through a triaxial magnetometer of the nine-axis MEMS sensor, and obtaining a heading angle of a geographic coordinate system according to the magnetic declination angle;
and determining the attitude angle of the prism rod device at the known control point according to the pitch angle, the roll angle and the course angle.
It is worth to say that, when initializing the course angle, because the gyroscope precision is relatively poor, the magnetometer needs to calculate the magnetic declination, then the course angle of the geographic coordinate system is obtained by the magnetic declination, and then the attitude angle is converted into an initial attitude direction cosine matrix to be used as an initial attitude matrix.
Further, according to the initial posture matrix, the closed-loop corrected specific force value, the angular velocity value and the magnetic induction intensity value at each moment, the navigation parameters of the prism rod device at each moment are obtained by using a mechanical arrangement algorithm, and preferably, the method specifically comprises the following steps:
obtaining a gesture matrix at the k-1 moment according to the product of the gesture matrix at the k-1 moment, the navigation coordinate system change matrix from the k-1 moment to the k moment and the carrier coordinate system change matrix; wherein k is an integer greater than 0; when k=1, the gesture matrix at the k-1 time is the initial gesture matrix;
determining a specific force speed increment and a Gong speed increment at the kth moment according to the gesture matrix at the kth-1 moment, the specific force value and the angular speed value after closed loop correction at the kth-1 moment and the specific force value and the angular speed value after closed loop correction at the kth moment, and obtaining a speed value at the kth moment according to the speed value at the kth-1 moment, the sum of the specific force speed increment and the Gong speed increment at the kth moment;
acquiring position information of the prism rod device at the kth moment according to the speed value at the kth moment and the speed value at the kth-1 moment;
and determining navigation parameters of the prism rod device at each moment according to the gesture matrix, the speed value and the position information of the prism rod device at each moment.
Specifically, the mechanical arrangement of the MEMS means that attitude information, speed information and position information of the carrier at the current moment relative to the previous moment are obtained according to output values of the accelerometer and the gyroscope, and a direction cosine matrix of the initial attitude is obtained through attitude initialization.
Firstly, based on the attitude update of the output value of a gyroscope, a direction cosine matrix of a carrier coordinate system b relative to a navigation coordinate system n (northeast coordinate system) at k moment is obtainedThe direction cosine matrix is equal to the variation of the n series from k-1 to k timeDirectional cosine matrix at time k-1>And carrier coordinate system variation->Is the product of (1), namely:
φ k is the equivalent rotation vector in the b system, phi k X is its antisymmetric matrix, which can be expressed based on the dipole-like assumption as:
Δθ k 、θ k-1 respectively integrating the angular velocities at different moments;
ζ k is the equivalent rotation vector under n series, ζ k X is its antisymmetric matrix, which can be expressed as:
for projection of the rotational angular velocity of the earth under n, < >>For the link angular velocity in the n-series, Δt=t (k) -t (k-1).
The velocity update is based on the attitude update and the accelerometer output value to estimate the velocity of the carrier. The speed at the current time k can be expressed as the speed of k-1Acceleration (specific force) speed increment +.>Increased God speed>The sum of, i.e
Wherein the increase in the coriolis velocity may be expressed as:
g in n Is the acceleration of gravity in the navigation system,for the rotation angular velocity of the earth>Is the angular velocity involved;
and the specific force velocity increment is expressed as:
i is an identity matrix, ζ n(k-1),n(k) Is the angular velocity change of n series, x is the antisymmetric operation,for the direction cosine matrix at time k-1, < >>Is a specific force velocity term, wherein the angular velocity varies ζ n(k-1),n(k) Can be expressed as:
the specific force velocity increment under the dipole-like assumption is equal to:
where Δv is a specific force integral term, and Δθ is an angular velocity integral term, and are expressed as follows:
the position update is the result of integrating the speed of the carrier and is based on the height h and latitude of the geodetic ellipsoidThe location update algorithm formula for longitude λ can be expressed as follows. t is t k Ellipsoid height h at time k Can be expressed as:
h in k-1 At t k-1 Elevation of moment, v D,k-1 、v D,k Respectively t k-1 Time t k The earth velocity at the moment.
For latitude update, ignoring changes in meridian and elevation within the integration interval, t k Latitude of momentRepresented as;
in the method, in the process of the invention,is t k-1 Latitude of moment, v N,k 、v N,k-1 Respectively t k 、t k-1 North speed of R M,k-1 Is t k-1 The radius of the meridian at the moment +.>At t k 、t k-1 Average elevation of (i.e.)>
Similarly, if there is t in the available longitude updating algorithm k Longitude of time lambda k ：
λ k-1 At t k-1 Longitude of time, v E,k 、v E,k-1 Respectively t k 、t k-1 East speed of R N,k-1/2 Is the radius of the circle of the mortise at the middle moment,is t k And t k-1 Mean latitude of time, i.e.)>
Prior to mechanical programming, inertial errors of calibration or filter estimation need to be cancelled from raw observations of accelerometers and gyroscopes to achieve closed loop correction, thereby mitigating error accumulation.
Further, since the nine-axis MEMS sensor is fixed on the prism rod and does not coincide with the center of the prism, a lever arm correction between the nine-axis MEMS sensor and the prism is required, and the lever arm length is the distance between the center of the nine-axis MEMS sensor and the center of the prism, which can be measured by a steel ruler, and then the lever arm is projected to the carrier coordinate system of the MEMS to correct the navigation parameter relative to the center of the MEMS to the center of the prism, thereby obtaining the corrected navigation parameter at each moment.
Further, the extended kalman filter algorithm is a nonlinear filter estimation algorithm, and the observation matrix and the state matrix are balanced through kalman gain, so that the optimal state estimation is obtained. And performing state estimation based on the extended Kalman filtering, and firstly determining a state equation and an observation equation.
Expansion CalThe state quantity of the Mannich filter algorithm includes: position error δr, velocity error δv, attitude angle error phi, gyroscope zero offset error b g Zero offset error b of accelerometer a Scale factor error s of gyroscope g Accelerometer scale factor error s a Wherein, the gyroscope has zero offset error b g Zero offset error b of accelerometer a Scale factor error s of gyroscope g Accelerometer scale factor error s a And its noise power spectral density has been obtained by error calibration, it is worth noting that the inertial error of the filtering estimation at the current moment is used in the mechanical programming process at the next moment, so the state matrix is expressed as:
the state equation can be expressed as:
δx k ＝Φ k/k-1 δx k-1 +ω k-1
Φ k/k-1 ＝I+F(t k-1 )Δt
phi in k/k-1 For state transition matrix omega k-1 Is the noise of the state matrix, F (t k-1 ) At t k-1 A coefficient matrix of the time state quantity.
The observed quantity of the extended kalman filter algorithm includes: velocity of zero-velocity pseudo-observed value of attitude angle detected by triaxial magnetometer of nine-axis MEMS sensorAngular velocity +>Control point coordinate r z The zero-speed pseudo-observed value is obtained when the known control point or the to-be-detected broken point is stationary according to the nine-axis MEMS sensor, the known control point and the to-be-detected broken point are calibrated according to the stationary time length, and the three-axis acceleration threshold value of the original output is adopted to be smaller than 0.5m/s 2 To realize the opposite edgesZero speed detection of the mirror lever device.
The attitude angle observation equation is as follows:
the zero speed pseudo speed value observation equation is as follows:
the zero-speed pseudo angular velocity value observation equation is as follows:
control point coordinate r z Equal to the coordinate r of MEMS n Plus lever arm l b The correction of (2) is based on the observation equation of the control point coordinates;
in the middle ofIs a directional cosine matrix, G -1 The position under the northeast coordinate system is converted into a geographic coordinate system, which can be expressed as;
wherein R is M 、R N 、h、The meridian radius, the mortise unitary circle radius, the ellipsoid height and the latitude value are respectively. />
The general observation equation is:
Z＝Hδx k +R z
the observation matrix is:
the noise matrix is:
noise q of observed value m Is the noise term of the magnetometer, q vv As velocity noise term, q va As an angular velocity noise term, q r Is the control point noise term. And carrying out carrier state estimation based on an extended Kalman filtering algorithm by using an observation equation and a state equation. The whole filtering updating algorithm can be expressed as a process of time updating and measurement updating, firstly, the time updating, namely, the state and state covariance prediction is carried out, and the time updating can be expressed as follows:
δx k/k-1 ＝Φ k/k-1 δx k-1
q in p The meaning of the other symbols is as described above for the noise term of the state covariance matrix.
The measurement update may be expressed as;
δx k ＝δx k/k-1 +K k (z k -H k δx k/k-1 )
k in the above k Is the gain matrix, P is the covariance of the state quantity, H is the coefficient matrix of the observation equation, R is the noise of the observation value, z k For observational purposes, the meaning of the other symbols is as described above. The state matrix, the state covariance matrix and the state transition matrix are stored while filtering, so that preparation is made for RTS (Rauch-tune-Striebel) smoothing filtering.
Further, in order to further improve the carrier state estimation accuracy, the embodiment adopts an RTS fixed interval smoothing filtering algorithm, which is equivalent to the combination of forward filtering and backward filtering. Firstly, based on a state matrix, a state covariance matrix and a state transition matrix which are obtained by extended Kalman filtering, forward filtering is carried out from the last epoch of observed data, and finally, the ground object position information of the carrier is estimated.
δx k|n ＝δx k|k +T k (δx k+1|n -δx k+1|k )
/>
Wherein n represents the total observation epoch number, k=n-1, n-2, …,1 is epoch observation time, T k The gain matrix of the state quantity at the moment k is represented by P, the covariance matrix of the state quantity and the coefficient matrix of the state quantity.
Finally, when the prism rod device moves to the breaking point to be detected, the prism rod device is controlled to be static, for example, 2s, so that the calibration of the breaking point detected by the prism rod device is realized, and the position information of the breaking point to be detected is output after the data processing of the nine-axis MEMS sensor.
According to the ground object breaking point measuring method provided by the embodiment of the invention, the nine-axis MEMS sensor based on the inertia technology is fused with the total station electro-optical distance measuring technology, so that the ground object breaking point measurement is not influenced by the measuring environment, the ground object breaking point measurement can be realized without meeting the requirements of a prism and the total station for the vision, the arrangement quantity of the total station is reduced, and the measurement efficiency and the measurement precision of the ground object breaking point are obviously improved.
A third aspect of the embodiment of the present invention provides a terminal device, including a memory, a processor, and a computer program stored in the memory and capable of running on the processor, where the processor implements the ground object breaking point measurement method according to any one of the embodiments of the second aspect when executing the computer program.
The terminal equipment can be computing equipment such as a desktop computer, a notebook computer, a palm computer, a cloud server and the like. The terminal device may include, but is not limited to, a processor, a memory. The terminal device may also include input and output devices, network access devices, buses, and the like.
The processor may be a central processing unit (Central Processing Unit, CPU), but may also be other general purpose processors, digital signal processors (Digital Signal Processor, DSP), application specific integrated circuits (Application Specific Integrated Circuit, ASIC), field programmable gate arrays (Field-Prog rammable Gate Array, FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, or the like. The general purpose processor may be a microprocessor or the processor may be any conventional processor or the like, which is a control center of the terminal device, and which connects various parts of the entire terminal device using various interfaces and lines.
The memory may be used to store the computer program and/or module, and the processor may implement various functions of the terminal device by running or executing the computer program and/or module stored in the memory and invoking data stored in the memory. The memory may mainly include a storage program area and a storage data area, wherein the storage program area may store an operating system, an application program (such as a sound playing function, an image playing function, etc.) required for at least one function, and the like; the storage data area may store data (such as audio data, phonebook, etc.) created according to the use of the handset, etc. In addition, the memory may include high-speed random access memory, and may also include non-volatile memory, such as a hard disk, memory, plug-in hard disk, smart Media Card (SMC), secure Digital (SD) Card, flash Card (Flash Card), at least one disk storage device, flash memory device, or other volatile solid-state storage device.
A fourth aspect of the embodiments of the present invention provides a computer readable storage medium, where the computer readable storage medium includes a stored computer program, where when the computer program runs, the computer readable storage medium is controlled to execute the ground object breaking point measurement method according to any one of the embodiments of the second aspect.
From the above description of the embodiments, it will be clear to those skilled in the art that the present invention may be implemented by means of software plus necessary hardware platforms, but may of course also be implemented entirely in hardware. With such understanding, all or part of the technical solution of the present invention contributing to the background art may be embodied in the form of a software product, which may be stored in a storage medium, such as ROM/RAM, a magnetic disk, an optical disk, etc., including several instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to perform the method described in the embodiments or some parts of the embodiments of the present invention.
While the foregoing is directed to the preferred embodiments of the present invention, it will be appreciated by those skilled in the art that changes and modifications may be made without departing from the principles of the invention, such changes and modifications are also intended to be within the scope of the invention.
Claims (9)
1. The prism rod device is characterized by comprising a prism rod, a prism and a nine-axis MEMS sensor;
the prism is arranged at the top of the prism rod, the nine-axis MEMS sensor is arranged on the prism rod and comprises a three-axis gyroscope, a three-axis accelerometer and a three-axis magnetometer.
2. The prism rod apparatus of claim 1, further comprising a first connecting rod and a second connecting rod; one end of the first connecting rod and one end of the second connecting rod are arranged on the prism rod, and the other end of the first connecting rod and the other end of the second connecting rod are connected with the nine-axis MEMS sensor.
3. The prism rod apparatus of claim 1, wherein the prism rod apparatus further comprises a power module; and the power supply end of the power supply module is connected with the power receiving end of the nine-axis MEMS sensor.
4. A ground object breaking point measuring method, applying the prism rod device as claimed in any one of claims 1 to 3, characterized by comprising the steps of:
calibrating inertial errors of nine-axis MEMS sensors of the prism rod device by using a six-position method, and determining a plurality of calibrated inertial errors;
setting the prism rod device at a preset known control point, determining an attitude angle of the prism rod device at the known control point through the nine-axis MEMS sensor based on the position information of the known control point, and converting the attitude angle into an initial attitude matrix;
controlling the prism rod device to move from the known control point, detecting a specific force value, an angular velocity value and a magnetic induction intensity value at each moment in the moving process through the nine-axis MEMS sensor, and acquiring navigation parameters of the prism rod device at each moment by using a mechanical arrangement algorithm according to the initial gesture matrix, the closed-loop corrected specific force value, angular velocity value and magnetic induction intensity value at each moment; the specific force value, the angular velocity value and the magnetic induction intensity value at the initial moment are corrected in a closed loop by eliminating the calibrated inertial error, and the specific force value, the angular velocity value and the magnetic induction intensity value at the non-initial moment are corrected in a closed loop by eliminating the inertial error estimated by filtering at the last moment;
correcting the navigation parameters at each moment according to the distance between the center of the nine-axis MEMS sensor and the prism center of the prism rod device to obtain corrected navigation parameters at each moment;
taking the corrected navigation parameter error and the inertial error at each moment as state quantity, taking zero-speed pseudo-observed values obtained by the nine-axis MEMS sensor when the known control point or the to-be-detected broken point is static, attitude angles calculated by a three-axis magnetometer of the nine-axis MEMS sensor and position information of the known control point as observed values, and obtaining a state matrix, a state covariance matrix and a state transition matrix at each moment by using an extended Kalman filtering algorithm;
and obtaining the position information of the to-be-detected broken part point by using an RTS smoothing filter algorithm according to the state matrix, the state covariance matrix and the state transition matrix at each moment.
5. The ground object breaking point measuring method according to claim 4, wherein the determining the attitude angle of the prism rod device at the known control point by the nine-axis MEMS sensor based on the position information of the known control point specifically comprises the steps of:
determining a pitch angle and a roll angle of the prism rod device at the known control point through a triaxial accelerometer of the nine-axis MEMS sensor based on the position information of the known control point;
determining a magnetic declination angle of the prism rod device at the known control point through a triaxial magnetometer of the nine-axis MEMS sensor, and obtaining a heading angle of a geographic coordinate system according to the magnetic declination angle;
and determining the attitude angle of the prism rod device at the known control point according to the pitch angle, the roll angle and the course angle.
6. The method for measuring ground object breaking points according to claim 4, wherein the navigation parameters of each moment of the prism rod device are obtained by using a mechanical arrangement algorithm according to the initial posture matrix, the closed-loop corrected specific force value, the angular velocity value and the magnetic induction intensity value at each moment, and the method specifically comprises the following steps:
obtaining a gesture matrix at the k-1 moment according to the product of the gesture matrix at the k-1 moment, the navigation coordinate system change matrix from the k-1 moment to the k moment and the carrier coordinate system change matrix; wherein k is an integer greater than 0; when k=1, the gesture matrix at the k-1 time is the initial gesture matrix;
determining a specific force speed increment and a Gong speed increment at the kth moment according to the gesture matrix at the kth-1 moment, the specific force value and the angular speed value after closed loop correction at the kth-1 moment and the specific force value and the angular speed value after closed loop correction at the kth moment, and obtaining a speed value at the kth moment according to the speed value at the kth-1 moment, the sum of the specific force speed increment and the Gong speed increment at the kth moment;
acquiring position information of the prism rod device at the kth moment according to the speed value at the kth moment and the speed value at the kth-1 moment;
and determining navigation parameters of the prism rod device at each moment according to the gesture matrix, the speed value and the position information of the prism rod device at each moment.
7. The ground object breaking point measuring method according to claim 4, wherein the inertial error calibration is performed on the nine-axis MEMS sensor of the prism rod device by using a six-position method, and a plurality of calibration inertial errors are determined, and specifically comprising the steps of:
respectively upwards and standing six planes of the nine-axis MEMS sensor to obtain an output value of a group of triaxial accelerometers, an output value of a group of triaxial gyroscopes and an output value of a group of triaxial magnetometers;
based on the obtained output value of the triaxial accelerometer, solving the following expression by using a Gauss Newton iteration method to obtain an accelerometer inertia error:
wherein θ a Representing accelerometer inertial error; k represents the time; g represents the local gravitational acceleration; h (a) s ,θ a ) An expression representing that the output value of the triaxial accelerometer is subjected to error correction; the accelerometer inertia errors comprise accelerometer zero offset errors, accelerometer mounting angle errors and accelerometer scale factor errors;
based on the obtained output value of the triaxial accelerometer and the output value of the triaxial gyroscope, solving the following expression by using a Gauss Newton iteration method to obtain a gyroscope inertia error:
wherein θ gry Representing gyroscope inertial errors;an output value of the triaxial accelerometer at a kth time after error correction; mu (mu) k Representing the acceleration value subjected to attitude transformation; the gyroscope inertia error comprises a gyroscope zero offset error, a gyroscope mounting angle error and a gyroscope scale factor error;
based on the obtained output value of the triaxial magnetometer, solving the following expression by using a Gauss Newton iteration method to obtain the magnetometer inertia error:
wherein θ m Representing magnetometer inertial errors; m represents the local magnetic field strength; h (m) s ,θ m ) An expression indicating that the output value of the three-axis magnetometer is error corrected; the magnetometer inertia error comprises a magnetometer zero offset error, a magnetometer mounting angle error and a magnetometer scale factor error。
8. A terminal device comprising a memory, a processor and a computer program stored in the memory and executable on the processor, the processor implementing the ground object point measurement method according to any one of claims 4 to 7 when executing the computer program.
9. A computer readable storage medium, characterized in that the computer readable storage medium comprises a stored computer program, wherein the computer program, when run, controls a device in which the computer readable storage medium is located to perform the ground object breaking point measurement method according to any one of claims 4 to 7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311494895.4A CN117705097A (en) | 2023-11-09 | 2023-11-09 | Prism rod device, ground object broken point measuring method, device and medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311494895.4A CN117705097A (en) | 2023-11-09 | 2023-11-09 | Prism rod device, ground object broken point measuring method, device and medium |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117705097A true CN117705097A (en) | 2024-03-15 |
Family
ID=90150495
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311494895.4A Pending CN117705097A (en) | 2023-11-09 | 2023-11-09 | Prism rod device, ground object broken point measuring method, device and medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117705097A (en) |
-
2023
- 2023-11-09 CN CN202311494895.4A patent/CN117705097A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110398245B (en) | Indoor pedestrian navigation attitude estimation method based on foot-worn inertial measurement unit | |
| CN107588769B (en) | Vehicle-mounted strapdown inertial navigation, odometer and altimeter integrated navigation method | |
| JP6094026B2 (en) | Posture determination method, position calculation method, and posture determination apparatus | |
| CN107655493B (en) | SINS six-position system-level calibration method for fiber-optic gyroscope | |
| CN102257358B (en) | Method for determining a heading in the direction of true north using an inertial measurement unit | |
| EP2583059B1 (en) | Improved north finder | |
| CN110954102B (en) | Magnetometer-assisted inertial navigation system and method for robot positioning | |
| CN108731664B (en) | Robot state estimation method, device, computer equipment and storage medium | |
| CN111024070A (en) | Inertial foot binding type pedestrian positioning method based on course self-observation | |
| US11408735B2 (en) | Positioning system and positioning method | |
| CN106153069B (en) | Attitude rectification device and method in autonomous navigation system | |
| CN112798021B (en) | Inertial navigation system inter-travelling initial alignment method based on laser Doppler velocimeter | |
| CN106370178B (en) | Attitude measurement method and device of mobile terminal equipment | |
| US8521428B1 (en) | Heading determination using sensors mounted on rotatable assembly | |
| CN112577521A (en) | Combined navigation error calibration method and electronic equipment | |
| JP2012173190A (en) | Positioning system and positioning method | |
| EP2930467A1 (en) | A system and method for sensing the inclination of a moving platform with respect to gravity | |
| CN108627152A (en) | A kind of air navigation aid of the miniature drone based on Fusion | |
| CN113566850B (en) | Method and device for calibrating installation angle of inertial measurement unit and computer equipment | |
| CN109084755B (en) | Accelerometer zero offset estimation method based on gravity apparent velocity and parameter identification | |
| CN111141285B (en) | Aviation gravity measuring device | |
| CN112197765A (en) | Method for realizing fine navigation of underwater robot | |
| CN113551669B (en) | Combined navigation positioning method and device based on short base line | |
| CN117705097A (en) | Prism rod device, ground object broken point measuring method, device and medium | |
| CN113532477A (en) | Riding stopwatch equipment and automatic calibration method for initial posture of riding stopwatch |
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 |