CN115371650B - Six-degree-of-freedom laser target measurement system and dynamic performance improvement method thereof - Google Patents
Six-degree-of-freedom laser target measurement system and dynamic performance improvement method thereof Download PDFInfo
- Publication number
- CN115371650B CN115371650B CN202211010946.7A CN202211010946A CN115371650B CN 115371650 B CN115371650 B CN 115371650B CN 202211010946 A CN202211010946 A CN 202211010946A CN 115371650 B CN115371650 B CN 115371650B
- Authority
- CN
- China
- Prior art keywords
- state
- vector
- unit
- inertial navigation
- matrix
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 33
- 238000005259 measurement Methods 0.000 title claims description 76
- 238000001914 filtration Methods 0.000 claims abstract description 43
- 238000001514 detection method Methods 0.000 claims abstract description 41
- 238000005562 fading Methods 0.000 claims abstract description 39
- 238000003384 imaging method Methods 0.000 claims abstract description 24
- 230000000007 visual effect Effects 0.000 claims abstract description 19
- 239000013598 vector Substances 0.000 claims description 60
- 239000011159 matrix material Substances 0.000 claims description 58
- 230000003044 adaptive effect Effects 0.000 claims description 16
- 238000012545 processing Methods 0.000 claims description 12
- 238000001228 spectrum Methods 0.000 claims description 7
- 230000003068 static effect Effects 0.000 claims description 7
- 238000007476 Maximum Likelihood Methods 0.000 claims description 4
- 238000009826 distribution Methods 0.000 claims description 4
- 238000005096 rolling process Methods 0.000 claims description 4
- 230000002159 abnormal effect Effects 0.000 claims description 3
- 238000000605 extraction Methods 0.000 claims description 3
- 238000012423 maintenance Methods 0.000 claims description 3
- 238000012360 testing method Methods 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- 238000010276 construction Methods 0.000 claims description 2
- 230000002708 enhancing effect Effects 0.000 claims 1
- 238000010586 diagram Methods 0.000 description 4
- 238000004519 manufacturing process Methods 0.000 description 4
- 238000011160 research Methods 0.000 description 4
- 230000001133 acceleration Effects 0.000 description 3
- 230000004927 fusion Effects 0.000 description 3
- 238000002955 isolation Methods 0.000 description 3
- 239000002689 soil Substances 0.000 description 3
- 241001497337 Euscorpius gamma Species 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 239000000835 fiber Substances 0.000 description 2
- 230000005484 gravity Effects 0.000 description 2
- 230000001788 irregular Effects 0.000 description 2
- 238000000691 measurement method Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000007500 overflow downdraw method Methods 0.000 description 2
- 230000005641 tunneling Effects 0.000 description 2
- 238000009825 accumulation Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 239000011435 rock Substances 0.000 description 1
- 230000035945 sensitivity Effects 0.000 description 1
- 238000003860 storage Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C15/00—Surveying instruments or accessories not provided for in groups G01C1/00 - G01C13/00
- G01C15/002—Active optical surveying means
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
The invention discloses a six-degree-of-freedom laser target measuring system and a dynamic performance improving method thereof. The dynamic performance improving method is characterized in that the measuring information of the strapdown inertial navigation measuring unit is compared with the measuring information of the inclination angle sensing unit and the visual imaging unit to obtain the error of the strapdown inertial navigation measuring unit, a fault detection algorithm is introduced to judge the working state of the inclination angle sensing unit in real time, and when the inclination angle sensing unit is normal and misaligned, a multiple suboptimal fading self-adaptive fault-tolerant Kalman filtering algorithm is used for carrying out self-adaptive fault-tolerant filtering to predict and compensate the error of the strapdown inertial navigation measuring unit.
Description
Technical Field
The invention relates to a distributed measurement system, in particular to a six-degree-of-freedom laser target measurement system and a dynamic performance improvement method thereof.
Background
The laser target six-degree-of-freedom measuring system has the characteristics of stable measuring precision, wide measuring range and flexible loading and unloading, and has wide application in the engineering fields of industrial measurement and production in China, such as ship manufacturing, shield guiding, track detection, large instrument adjustment and the like. The main working principle is that measuring light emitted by a measuring base station is reflected by a pinhole prism part of a laser target and is received by the measuring base station to obtain the position information of the measuring light, and the angle is measured by an inclinometer and the attitude value of a target to be measured is calculated by combining a vision mapping model. However, because the inclination angle sensing device inside the laser target obtains the inclination angle between the inertia axis and the horizontal plane by measuring the specific force of the gravity acceleration on the corresponding inertia axis, the method is mainly suitable for measuring static or quasi-static environments without non-gravity acceleration scenes, the requirements on the dynamic performance and engineering efficiency of measuring and assembling links are increasingly improved along with the industrial development of China, typically, in tunnel engineering of China, the laser target is used for obtaining the pose information of a shield tunneling machine (TBM) in real time, however, when the TBM performs shield tunneling, due to uneven distribution of hard rock, soft soil and cavities in the geology, various irregular interference accelerations are introduced to the inclination angle sensing unit so as to influence the pose measurement precision in shield guiding, and the efficiency and quality of engineering manufacturing are reduced, so that the precision and the robustness of the six-degree-of-freedom laser target system in a dynamic application scene are the problem to be solved.
Currently, the dynamic lifting algorithm for laser targets mainly includes: vibration isolation compensation method and sensor fusion method. The vibration isolation compensation method ([ 1]Li H X,Pan M H,Wang L,et al.Research of vibration error compensation for inclinometers[C ], advanced Materials Research,2012,590:377-384 ]) mainly finds the resonance rule by analyzing the output characteristics of the method under different vibration environments, so as to perform vibration isolation and stably output signals through a digital filtering technology to reduce vibration errors, however, the vibration caused by uneven soil in a shield scene has irregularity, and the soil structure is also changed along with different regions, so that the application range of the method is limited, and the effect of the method in the application scene of irregular vibration such as shield is poor. The sensor fusion method compensates the dynamic performance of the inclinometer by introducing other high-precision anti-vibration units, and researches on a spatial pose measurement method of orthogonal vision and inclinometer in literature (2 Shouxin dimension, ma Guolu, zeng Guoying, and the like) provide a pose measurement method of combining orthogonal binocular vision and inclinometer in 2020,44 (3): 278-282), and the pose information solved by the binocular vision is combined with the angle information output by the inclinometer to realize the measurement of the relative pose, however, as the vision measurement precision is reduced along with the measurement distance and is easily influenced by noise such as working environment and the like, the method is not suitable for large-scale outdoor operation engineering such as shield and the like. In literature ([ 3] Shore. Combination application research of inclinometer and gyroscope in shield attitude measurement [ D ]. Wuhan: university of science and technology in China, 2013.) six degrees of freedom pose measurement is carried out by utilizing fusion of a fiber optic gyroscope and a laser target, a sensor fusion algorithm based on gyroscope null shift estimation is provided, and the laser target high-precision output in a steady state environment is utilized to estimate and predict the gyroscope null shift, so that the dynamic performance and measurement precision of a combined system are improved, however, the lack of effective monitoring on the misalignment of the inclinometer in the method can lead to pollution of a filter by the output 'wild value' of the inclinometer under the dynamic condition, and the lack of an effective error compensation method of the combined system under the high dynamic vibration condition of shield and the like, the output of the gyroscope can generate serious error accumulation, and in addition, the fiber optic gyroscope has higher cost and unsatisfactory temperature coefficient, reliability and long storage.
The six-degree-of-freedom laser target measuring system is important to be applied to real-time pose measurement in the field of large engineering such as shield and the like in China and brings forward higher demands on dynamic performance of the system, however, the current dynamic performance improving method of the system has shortcomings, a high-precision and high-robustness six-degree-of-freedom laser target dynamic performance improving method is found, assembly and manufacturing demands of large engineering in China are further met, and the system has important application value.
Disclosure of Invention
The invention aims to provide a six-degree-of-freedom laser target measurement system and a dynamic performance improvement method thereof, which can provide high-precision and stable six-degree-of-freedom pose measurement values in a dynamic application scene. According to the invention, the dip angle sensing unit and the strapdown inertial navigation measuring unit (i.e. the SINS) are arranged in the laser target, and the measured value of the dip angle sensing unit is compared with the measured value of the strapdown inertial navigation measuring unit to estimate errors because the measured value of the strapdown inertial navigation measuring unit is constant. And then, a multiple suboptimal fading self-adaptive fault-tolerant Kalman filtering (SMFAFKF) self-adaptive fault-tolerant filtering algorithm is used for combining the self-adaptive adjustment of the working state of the system when the inclination angle sensing unit is normal, and the self-adaptive adjustment of a filtering measurement equation is used for predicting and compensating the attitude error by using the measurement information available for observation when the inclination angle sensing unit is abnormal.
The invention aims at realizing the following technical scheme:
the six-degree-of-freedom laser target measuring system comprises a laser tracker and a six-degree-of-freedom attitude laser target, wherein the laser tracker is used as a base station for providing redundant measurement information through high-frequency measuring points, and the laser target is fixed on a target to be measured and comprises a pyramid prism, an inclination angle sensing unit, a visual imaging unit, a strapdown inertial navigation measuring unit and a data processing unit; the data processing unit is electrically connected with the inclination angle sensing unit, the visual imaging unit and the strapdown inertial navigation measuring unit;
the visual imaging unit is matched with the pyramid prism to measure the azimuth angle of the target to be measured, the inclination angle sensing unit is used for measuring the rolling angle and pitch angle information, and the strapdown inertial navigation measuring unit is used for measuring the attitude, speed and position information of the target to be measured; the data processing unit receives measurement information from the inclination angle sensing unit, the visual imaging unit and the strapdown inertial navigation measuring unit and transmits the measurement information to the upper computer;
the upper computer establishes a state space equation of the combined filtering system, compares the measurement information of the strapdown inertial navigation measuring unit with the measurement information of the inclination angle sensing unit and the visual imaging unit to obtain an error of the strapdown inertial navigation measuring unit, introduces a fault detection algorithm to judge the working state of the inclination angle sensing unit in real time, and uses a multiple suboptimal fading self-adaptive fault-tolerant Kalman filtering algorithm to perform self-adaptive fault-tolerant filtering to predict and compensate the error of the strapdown inertial navigation measuring unit when the inclination angle sensing unit is normal and misaligned.
A method for dynamic performance enhancement of a six degree of freedom laser target measurement system, comprising:
step one: determining a system state variable and establishing a state space equation of a combined filtering system according to the attitude information acquired by the inclination angle sensing unit and the visual imaging unit and the redundant measurement information acquired by the laser tracker and the attitude, speed and position information acquired by the strapdown inertial navigation measuring unit; wherein, the redundant measurement information refers to measured speed and position information;
step two: extracting a single filtering innovation vector (innovation vector) to construct residual chi-square fault detection quantity for detection, identifying the detection quantity to judge whether the system is faulty or not, if so, executing the step three; otherwise, extracting a characteristic spectrum peak value of a residual matrix constructed by a pseudo-innovation vector in a state recursion device to further perform fault detection on an inclination angle sensing unit of the six-degree-of-freedom laser target measurement system;
step three: when the second step is that the six-degree-of-freedom laser target measuring system works normally, performing self-adaptive fault-tolerant filtering by using a multiple suboptimal fading self-adaptive fault-tolerant Kalman filtering algorithm to obtain an optimal system state error estimated value; constructing an fading factor by utilizing the state space equation constructed in the first step and combining an adaptive adjustment algorithm, and adaptively adjusting the fading factor according to the current state of the system;
when the second step judges that the six-degree-of-freedom laser target measuring system is abnormal in operation, namely when the inclination angle sensing unit is out of focus, the system adjusts the measuring matrix structure in the state space equation established in the first step, and then a multiple suboptimal fading self-adaptive fault-tolerant Kalman filtering algorithm is used for predicting the system error to obtain an optimal system state error estimated value;
step four: and compensating the current system state according to the acquired system state quantity error estimation value to acquire the current system pose information.
Further, the first step specifically includes the following steps:
s101, establishing a coordinate system Y of the inclination sensor s OZ s Acquiring pitch angle and roll angle information: the inclination angle sensing unit outputs included angles theta and eta between two inertia axes and a horizontal plane, wherein a pitch angle gamma is defined as OY s The angle between the axis and the horizontal, i.e. γ=η, the roll angle β is solved according to the following geometrical relationship:
s102, acquiring azimuth angle information: according to the acquired pitch angle and roll angle information, solving azimuth angle information through a double-vector attitude determination model according to the incident light image coordinates acquired through the monocular vision imaging unit;
s103, according to the information obtained in the S101-102, the speed and position information obtained by combining the laser tracker to receive the returned light of the laser target, and the attitude, speed and position information obtained by the strapdown inertial navigation measuring unit, a state space equation of a combined filtering system is established:
wherein the state vector
φ x ,φ y ,φ z Respectively representing the rolling angle error, the pitch angle error and the azimuth angle error of the strapdown inertial navigation measuring unit; δv x ，δv y ，δv z Respectively representing speed errors of the strapdown inertial navigation measuring unit on an X axis, a Y axis and a Z axis; δp x ，δp y ，δp z Respectively representing the position errors of the strapdown inertial navigation measuring unit on the X axis, the Y axis and the Z axis; epsilon gb ，ε gr Respectively representing static deviation and dynamic deviation epsilon of gyroscope of strapdown inertial navigation measuring unit ab ，ε ar Respectively representing the static deviation and the dynamic deviation of the accelerometer; measuring vector
wherein ,
δv S ，δp S Respectively representing the acquired attitude vector, speed vector and position vector of the strapdown inertial navigation measurement unit; phi (phi) P Representing the attitude vector obtained by the laser target, v P 、p P The speed vector and the position vector obtained by the laser target returning laser are received through a laser tracker.
Further, the fault detection algorithm in the second step is an improved residual error chi-square-eigenvalue detection algorithm, a state recursion device is adopted to construct a pseudo-innovation sequence, and an eigenvalue of a residual error matrix constructed by a normalized pseudo-innovation vector in the current recursion device is extracted as a fault detection index, and the method specifically comprises the following steps:
the following state recursion is constructed:
wherein ,
for the state vector in the state recursion at time k-1 +.>
For the state covariance matrix in the state recursion at time k-1 +.>
For the initial value of the state +.>
For the initial value of the state covariance matrix, the length of the state recursion device is set to be N to avoid state divergence, namely N times of recursion are performed, and new state recursion is started by the initial value of the state recursion device which is reassigned by the state quantity of the current original filter and the state covariance matrix. Thereby constructing a pseudo-innovation vector: />
Building a matrix from normalized pseudo-innovation vectors in the state recursion:
where n is the number of pseudo-innovation vectors for the current state recursion,
defining a residual matrix D for the corresponding pseudo-innovation covariance matrix k The method comprises the following steps:
extraction of D k Maxima lambda of characteristic spectrum k ＝max{δ{D k -constructing the following decision criteria:
when lambda is k ＞λ d When the system is in fault; when lambda is k ≤λ d When the system works normally; wherein lambda is d Is a fault detection threshold.
Further, the step three of performing adaptive fault-tolerant filtering to obtain an optimal system state error estimation value by using a multiple suboptimal fading adaptive fault-tolerant kalman filter algorithm specifically includes:
construction of an evanescent matrix L k+1 Correcting the intermediate state covariance matrix in real time, wherein the filtering recursion formula is as follows:
wherein ,Xk/k Is the state vector at time k, P k/k Is a state covariance matrix at the moment k, phi k+1/k Is a state transition matrix Γ k+1/k To control the matrix, u k As a control variable at time k, Q k+1 To control the quantity covariance matrix, H k+1 For measuring matrix, R k To measure variance matrix, Z k+1 To measure the vector r k+1 K is the innovation vector k+1 Is the kalman gain. Definition of the fading matrix L k+1 ＝diag(λ 1 ,λ 2 ,...,λ m ) Where m is the state vector dimension, the fading factor λ i The adaptive adjustment is performed according to the following rules:
the following statistics were constructed:
it can be seen that gamma k+1 Obeying chi-square distribution with degree of freedom m, i.e. gamma k+1 ～χ 2 (m) let S k+1 ＝H k+1 L k+ ，
N k+1 ＝R k+1, wherein Sk+1 I.e. representing the fading matrix L k+1 In observable portions of the graph, statistics gamma k+1 Can be expressed as:
according to the principle of maximum likelihood test, when the system is normal, there are
Alpha is the confidence level, and each component is detected respectively, and then:
wherein εi In order to detect the threshold value of the signal,
the range of values of the normal fading factors of the maintenance system is obtained by the method: />
Taking into account the convergence of the filter
For the corresponding fading factor for the unobservable state quantity, a 1 may be determined due to the lack of corresponding metrology constraints.
Compared with the prior art, the technical scheme of the invention has the following beneficial effects:
1. according to the invention, the laser tracker is used as a measuring base station, the dynamic performance of the six-degree-of-freedom laser target is improved in a mode of combining a strapdown inertial navigation measuring unit (SINS) with the laser target, the fault tolerance of the system is improved by fully utilizing the observable position and speed redundant information in the system, and the combined system can provide stable and high-precision attitude measurement values when dynamic misalignment is generated by the inclination angle sensing unit.
2. The invention provides a self-adaptive fault-tolerant Kalman filtering algorithm of multiple suboptimal fading self-adaptive fault-tolerant Kalman filtering (SMFAFKF), which can introduce suboptimal fading factors to adjust the memory length of state quantity when an inclination angle sensing unit works normally so as to enhance the influence proportion of a reliable measurement value on the optimal estimation of the state quantity and improve the state estimation precision; when the dip angle unit breaks down, the filtering measurement equation is adjusted in a self-adaptive mode, and the measurement information available for observation is utilized for predicting and compensating the attitude error, so that the measurement accuracy and the robustness of the system are improved.
3. The second step of the invention provides a fault detection algorithm for improving the residual chi-square characteristic value, which overcomes the problems of low detection sensitivity and poor detection performance on soft faults of the traditional fault detection algorithm, and improves the application range and value under complex industrial scenes.
Drawings
FIG. 1 is a flow chart of a method for improving dynamic performance of a six-degree-of-freedom laser target according to the present invention;
FIG. 2 is a schematic diagram of the overall structure of a six degree-of-freedom laser target measurement system;
FIG. 3 is a schematic diagram of the internal structure of a laser target;
FIG. 4 is a schematic diagram of the attitude measurement of the tilt sensor unit;
fig. 5 is a schematic diagram of the fault detection method in the second step.
wherein ,
201: laser tracker, 202: laser target, 301: corner cube, 302: inclination angle sensing unit, 303: visual imaging unit, 304: strapdown inertial navigation measurement unit, 305: and a data processing unit.
Detailed Description
In order to make the objects, technical solutions, advantageous effects and significant improvements of the embodiments of the present invention more apparent, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings provided in the embodiments of the present invention, and it is apparent that all of the described embodiments are only some embodiments of the present invention, but not all embodiments of the present invention; 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.
As shown in fig. 1, a dynamic performance improving method of a six-degree-of-freedom laser target measuring system is disclosed, which uses a six-degree-of-freedom laser target measuring system as shown in fig. 2-3, and includes a laser tracker 201 and a six-degree-of-freedom gesture laser target 202, wherein the laser tracker 201 is used as a base station to provide redundant measurement information through a high-frequency measurement point, and the laser target 202 is fixed on a target to be measured and includes a pyramid prism 301, an inclination angle sensing unit 302, a visual imaging unit 303, a strapdown inertial navigation measuring unit 304 and a data processing unit 305. The pyramid prism 301 is a pinhole prism, the tilt angle sensing unit 302 is a biaxial inclinometer, the strapdown inertial navigation measuring unit 304 comprises a gyroscope and an accelerometer, and the vision imaging unit 303 is a monocular vision imaging unit. As shown in fig. 3, the visual imaging unit 303 is configured to measure an azimuth angle in cooperation with the pyramid prism, the inclination angle sensing unit 302 is configured to transmit measured roll angle and pitch angle information to the data processing unit 305, and the strapdown inertial navigation measurement unit 304 is electrically connected to the data processing unit 305 and is configured to transmit measured attitude, speed and position information to the data processing unit. The data processing unit 305 receives data from the tilt sensor unit 302, the visual imaging unit 303 and the strapdown inertial navigation measurement unit 304 and transmits the data to an upper computer.
The host computer introduces an adaptive fault-tolerant filtering algorithm to perform data fusion and provide redundant measurement information through high-frequency measurement points of the laser tracker, compares the measurement information of the strapdown inertial navigation measurement unit 304 with the measurement information of the inclination angle sensing unit 302 and the visual imaging unit 303 to obtain the error of the strapdown inertial navigation measurement unit, introduces a fault detection algorithm to determine the working state of the inclination angle sensing unit 302 in real time, and predicts and compensates the error of the strapdown inertial navigation measurement unit by using the redundant measurement information provided by the high-frequency measurement points of the laser tracker when the inclination angle sensing unit 302 is misaligned.
The dynamic performance improving method of the six-degree-of-freedom laser target measuring system specifically comprises the following steps:
step one: determining system state variables and establishing a state space equation of a combined filtering system according to measurement information of the inclination angle sensing unit 302, the visual imaging unit 303 and the strapdown inertial navigation measurement unit 304;
establishing a coordinate system Y of an inclination sensor s OZ s The method comprises the steps of carrying out a first treatment on the surface of the The tilt sensor unit 302 obtains the roll angle and pitch angle information, as shown in fig. 4, the tilt sensor unit 302 outputs the angles θ and η between the two inertial axes and the horizontal plane, wherein the pitch angle γ is defined as OY s The angle between the axis and the horizontal plane, i.e., γ=η, and the roll angle β can be directly solved by the geometric relationship as follows:
the azimuth angle information is solved by combining the pitch angle and the roll angle acquired by the inclination angle sensing unit 302 and the incident light image coordinate acquired by the monocular vision imaging unit 303 through a double-vector attitude determination model, the laser tracker 201 receives the return light acquisition speed and the position information of the laser target 201, and the attitude, the speed and the position information acquired by the strapdown inertial navigation measuring unit 304 are combined to establish a combined filtering system state equation and a measuring equation:
wherein the state vector
φ x ,φ y ,φ z Respectively representing the roll angle error, pitch angle error and azimuth angle error of the strapdown inertial navigation measurement unit 304; δv x ，δv y ，δv z Respectively representing the speed errors of the strapdown inertial navigation measurement unit 304 in the X axis, the Y axis and the Z axis; δp x ，δp y ，δp z Respectively represent strapdownPosition errors of the inertial measurement unit 304 in the X-axis, Y-axis, and Z-axis; epsilon gb ，ε gr Respectively representing the static deviation and the dynamic deviation epsilon of the gyroscope of the strapdown inertial navigation measurement unit 304 ab ，ε ar Representing the static and dynamic deviations of the accelerometer, respectively. Measurement vector->
wherein ,
δv S ，δp S Respectively representing the acquired attitude vector, speed vector and position vector of the strapdown inertial navigation measurement unit 304; phi (phi) P Representing the attitude vector, v, acquired by the laser target 202 P 、p P Is a velocity vector and a position vector obtained by receiving laser beam 202 returned from the laser tracker 201.
Step two: extracting a single filtering innovation vector (innovation vector) to construct residual chi-square fault detection quantity for detection, identifying the detection quantity to judge whether the system is faulty or not, if so, executing the step three; otherwise, extracting a characteristic spectrum peak value of a residual matrix constructed by a pseudo-innovation vector in a state recursion device to further perform fault detection on an inclination angle sensing unit of the six-degree-of-freedom laser target measurement system;
the algorithm of fault detection is a residual error chi-square fault detection algorithm combined with the improved residual error chi-square-characteristic value detection algorithm provided by the invention, as shown in fig. 5, firstly extracting a current k-moment filtering innovation vector to construct residual error chi-square detection quantity for fault detection, and when the detection quantity exceeds a detection threshold value, identifying system faults; otherwise, adopting a fault detection algorithm for improving the residual chi-square-characteristic value to detect, and constructing the following state recursion:
wherein ,
for the state vector in the state recursion at time k-1 +.>
For the state covariance matrix in the state recursion at time k-1 +.>
For the initial value of the state +.>
For the initial value of the state covariance matrix, the length of the state recursion device is set to be N to avoid state divergence, namely N times of recursion are performed, and new state recursion is started by the initial value of the state recursion device which is reassigned by the state quantity of the current original filter and the state covariance matrix. Thereby constructing a pseudo-innovation vector:
building a matrix from normalized pseudo-innovation vectors in the state recursion:
where n is the number of pseudo-innovation vectors for the current state recursion,
defining a residual matrix D for the corresponding pseudo-innovation covariance matrix k The method comprises the following steps:
extraction of D k Maxima lambda of characteristic spectrum k ＝max{δ{D k -constructing the following decision criteria:
when lambda is k ＞λ d When the system fails；
When lambda is k ≤λ d And when the system works normally.
wherein ,λd Is a fault detection threshold.
Step three: when the second step judges that the six-degree-of-freedom laser target measuring system works normally, namely when the inclination angle sensing unit works normally, the adaptive adjustment of the fading factors is carried out according to the current state of the system, and the adaptive fault-tolerant filtering is carried out by combining the fading matrixes to obtain the state estimation of the system; the system regards the normal state of the inclination angle sensing unit as normal operation of the six-degree-of-freedom laser target measuring system, because the fault of the inclination angle sensing unit can cause the fault of the six-degree-of-freedom laser target measuring system;
the adaptive fault-tolerant filtering algorithm is a multiple suboptimal fading adaptive fault-tolerant kalman filtering (SMFAFKF) algorithm provided by the present invention, that is, an improvement of a state recursive algorithm based on a filter system state equation and a measurement equation (i.e., a state space equation) established in the first step, and the adaptive fault-tolerant filtering algorithm is used for improving the detection accuracy and filtering accuracy of a system by introducing a fading matrix and performing multiple fading factor adaptive adjustment according to a maximum likelihood detection principle, so as to adjust a state covariance matrix in real time, limit the memory length of a Kalman Filter (KF), and improve the utilization proportion of effective information in a residual sequence;
when the second step detects that the data of the tilt sensor unit 302 is not misaligned, a measurement matrix H of the state space equation established in the first step is defined k+1 ：
Wherein I is a 3×3 unit array;
the filter recurrence formula of SMFAFKF is:
wherein ,Lk+1 X is an evanescent matrix k/k Is the state vector at time k, P k/k Is a state covariance matrix at the moment k, phi k+1/k Is a state transition matrix Γ k+1/k To control the matrix, u k As a control variable at time k, Q k+1 To control the quantity covariance matrix, H k+1 For measuring matrix, R k To measure variance matrix, Z k+1 To measure the vector r k+1 K is the innovation vector k+1 Is the kalman gain. Definition of the fading matrix L k+1 ＝diag(λ 1 ,λ 2 ,...,λ m ) Where m is the state vector dimension, the fading factor λ i The adaptive adjustment is performed according to the following rules:
the following statistics were constructed:
it can be seen that gamma k+1 Obeying chi-square distribution with degree of freedom m, i.e. gamma k+1 ～χ 2 (m) let S k+1 ＝H k+1 L k- ，
N k+1 ＝R k+1, wherein Sk+1 I.e. representing the fading matrix L k+1 In observable portions of the graph, statistics gamma k+1 Can be expressed as:
according to the principle of maximum likelihood test, when the system is normal, there are
Alpha is the confidence level, and each component is detected respectively, and then:
wherein εi In order to detect the threshold value of the signal,
the range of values of the normal fading factors of the maintenance system is obtained by the method:
taking into account the convergence of the filter
For the corresponding fading factor for the unobservable state quantity, a 1 may be determined due to the lack of corresponding metrology constraints.
When the second step judges that the six-degree-of-freedom laser target measuring system is faulty, namely when the tilt angle sensing unit is out of order, the system adjusts the measuring structure, namely when the second step detects that the tilt angle sensing unit 302 data is out of order, a measuring matrix H of the state space equation established in the first step is defined k+1 ：
And the observable redundant measurement information is fully utilized, and the system error is predicted by using a multiple suboptimal fading self-adaptive fault-tolerant Kalman filtering algorithm so as to obtain an optimal system state error estimated value, so that SINS attitude error prediction and compensation under the condition of missing attitude measurement are realized. When the system fails, the attitude is not observably measured, so the fading factor is considered to be 1, and therefore the multiple suboptimal fading self-adaptive fault-tolerant Kalman filtering algorithm involved in the step is the same as the algorithm for judging that the six-degree-of-freedom laser target measuring system is normal in the step three, and the description is omitted here.
Step four: and compensating the current system state according to the acquired system state error estimation value to acquire the current system pose information.
The foregoing embodiments are merely for illustrating the technical solution of the present invention, and not for limiting the same, and although the present invention has been described in detail with reference to the foregoing embodiments, it should be understood by those skilled in the art that the technical solution described in the foregoing embodiments may be modified or all technical features may be equivalently replaced, and that the modification or replacement does not make the essence of the corresponding technical solution deviate from the scope of the technical solution of the embodiments of the present invention, and that non-essential improvements, modifications or replacements made by those skilled in the art according to the content of the present specification are all within the scope of the claimed invention.
Claims (4)
1. The six-degree-of-freedom laser target measurement system comprises a laser tracker (201) and a six-degree-of-freedom attitude laser target (202), and is characterized in that the laser tracker (201) is used as a base station for providing redundant measurement information through high-frequency measurement points, and the laser target (202) is fixed on a target to be measured and comprises a pyramid prism (301), an inclination angle sensing unit (302), a visual imaging unit (303), a strapdown inertial navigation measurement unit (304) and a data processing unit (305); the data processing unit (305) is electrically connected with the inclination angle sensing unit (302), the visual imaging unit (303) and the strapdown inertial navigation measuring unit (304);
the visual imaging unit (303) is matched with the pyramid prism to measure the azimuth angle of the target to be measured, the inclination angle sensing unit (302) is used for measuring the rolling angle and pitch angle information, and the strapdown inertial navigation measuring unit (304) is used for measuring the attitude, speed and position information of the target to be measured; the data processing unit (305) receives measurement information from the inclination angle sensing unit (302), the visual imaging unit (303) and the strapdown inertial navigation measuring unit (304) and transmits the measurement information to an upper computer;
the upper computer establishes a state space equation of the combined filtering system, compares measurement information of the strapdown inertial navigation measuring unit (304) with measurement information of the inclination angle sensing unit (302) and the visual imaging unit (303) to obtain an error of the strapdown inertial navigation measuring unit, introduces a fault detection algorithm to judge the working state of the inclination angle sensing unit (302) in real time, and uses a multiple suboptimal fading self-adaptive fault-tolerant Kalman filtering algorithm to perform self-adaptive fault-tolerant filtering to predict and compensate the error of the strapdown inertial navigation measuring unit when the inclination angle sensing unit (302) is normal and misaligned.
2. The method for dynamic performance enhancement of a six degree of freedom laser target measurement system of claim 1, comprising:
step one: according to the attitude information acquired by the inclination angle sensing unit (302) and the visual imaging unit (303) and the redundant measurement information acquired by the laser tracker (201), the attitude, the speed and the position information acquired by the strapdown inertial navigation measuring unit (304) are combined, a system state variable is determined, and a state space equation of a combined filtering system is established; wherein, the redundant measurement information refers to measured speed and position information;
step two: extracting a single filtering innovation vector to construct residual chi-square fault detection quantity for detection, identifying the detection quantity and judging whether a system is faulty or not, if so, executing the step three; otherwise, extracting a characteristic spectrum peak value of a residual matrix constructed by a pseudo-innovation vector in a state recursion device to further perform fault detection on an inclination angle sensing unit of the six-degree-of-freedom laser target measurement system;
step three: when the second step is that the six-degree-of-freedom laser target measuring system works normally, performing self-adaptive fault-tolerant filtering by using a multiple suboptimal fading self-adaptive fault-tolerant Kalman filtering algorithm to obtain an optimal system state error estimated value; constructing an fading factor by utilizing the state space equation constructed in the first step and combining an adaptive adjustment algorithm, and adaptively adjusting the fading factor according to the current state of the system;
when the second step judges that the six-degree-of-freedom laser target measuring system is abnormal in operation, namely when the inclination angle sensing unit is out of focus, the system adjusts the measuring matrix structure in the state space equation established in the first step, and then a multiple suboptimal fading self-adaptive fault-tolerant Kalman filtering algorithm is used for predicting the system error to obtain an optimal system state error estimated value;
step four: compensating the current system state according to the acquired system state quantity error estimation value to acquire current system pose information;
the fault detection algorithm in the second step is an improved residual error chi-square-eigenvalue detection algorithm, a state recursion device is adopted to construct a pseudo-innovation sequence, and a characteristic spectrum peak value of a residual error matrix constructed by a normalized pseudo-innovation vector in the current recursion device is extracted as a fault detection index, and the method specifically comprises the following steps:
the following state recursion is constructed:
wherein ,
for the state vector in the state recursion at time k-1 +.>
For the state covariance matrix in the state recursion at time k-1 +.>
For the initial value of the state +.>
Setting the length of the state recursion device as N for the initial value of the state covariance matrix to avoid state divergence, namely recursing N times to start new state recursion by the state quantity of the current original filter and the initial value of the state recursion device of the state covariance matrix; thereby constructing a pseudo-innovation vector:
building a matrix from normalized pseudo-innovation vectors in the state recursion:
where n is the number of pseudo-innovation vectors for the current state recursion,
defining a residual matrix D for the corresponding pseudo-innovation covariance matrix k The method comprises the following steps:
extraction of D k Maxima lambda of characteristic spectrum k ＝max{δ{D k -constructing the following decision criteria:
when lambda is k >λ d When the system is in fault;
when lambda is k ≤λ d When the system works normally;
wherein ,λd Is a fault detection threshold.
3. The dynamic performance enhancing method as claimed in claim 2, wherein said step one specifically comprises the steps of:
s101, establishing a coordinate system Y of the inclination sensor s OZ s Acquiring pitch angle and roll angle information: the inclination angle sensing unit (302) outputs included angles theta and eta between two inertia axes and the horizontal plane, wherein the pitch angle gamma is defined as OY s The angle between the axis and the horizontal, i.e. γ=η, the roll angle β is solved according to the following geometrical relationship:
s102, acquiring azimuth angle information: according to the acquired pitch angle and roll angle information, solving azimuth angle information through a double-vector attitude determination model according to the incident light image coordinates acquired through a monocular vision imaging unit (303);
s103, according to the information obtained in the S101-102 and the returned light acquisition speed and position information of the laser target (202) received by combining the laser tracker (201), and the attitude, speed and position information acquired by the strapdown inertial navigation measuring unit (304), a state space equation of a combined filtering system is established:
wherein the state vector
φ x ,φ y ,φ z Respectively representing the rolling angle error, the pitch angle error and the azimuth angle error of the strapdown inertial navigation measurement unit (304); δv x ，δv y ，δv z Respectively representing the speed errors of the strapdown inertial navigation measuring unit (304) on the X axis, the Y axis and the Z axis; δp x ，δp y ，δp z Respectively representing the position errors of the strapdown inertial navigation measuring unit (304) on the X axis, the Y axis and the Z axis; epsilon gb ，ε gr Respectively representing the static deviation and the dynamic deviation epsilon of the gyroscope of the strapdown inertial navigation measuring unit (304) ab ，ε ar Respectively representing the static deviation and the dynamic deviation of the accelerometer; measurement vector->
wherein ,
δv S ，δp S Respectively representing the acquired attitude vector, speed vector and position vector of the strapdown inertial navigation measurement unit 304; phi (phi) P Representing the attitude vector, v, acquired by the laser target (202) P 、p P The speed vector and the position vector obtained by the laser target (202) returning laser are received by the laser tracker (201).
4. The method of claim 2, wherein the step three of performing adaptive fault-tolerant filtering to obtain the optimal system state error estimate by using a multiple suboptimal fading adaptive fault-tolerant kalman filter algorithm specifically comprises:
construction of an evanescent matrix L k+1 Correcting the intermediate state covariance matrix in real time, wherein the filtering recursion formula is as follows:
wherein ,Xk/k Is the state vector at time k, P k/k Is a state covariance matrix at the moment k, phi k+1/k Is a state transition matrix Γ k+1/k To control the matrix, u k As a control variable at time k, Q k+1 To control the quantity covariance matrix, H k+1 For measuring matrix, R k To measure variance matrix, Z k+1 To measure the vector r k+1 K is the innovation vector k+1 Is Kalman gain; definition of the fading matrix L k+1 ＝diag(λ 1 ,λ 2 ,...,λ m ) Where m is the state vector dimension, the fading factor λ i The adaptive adjustment is performed according to the following rules:
the following statistics were constructed:
it can be seen that gamma k+1 Obeying chi-square distribution with degree of freedom m, i.e
Let S k+1 ＝H k+1 L k+1 ，
N k+1 ＝R k+1, wherein Sk+1 I.e. representing the fading matrix L k+1 In observable portions of the graph, statistics gamma k+1 Can be expressed as:
according to the principle of maximum likelihood test, when the system is normal, there are
Alpha is the confidence level, and each component is detected respectively, and then:
wherein εi In order to detect the threshold value of the signal,
the range of values of the normal fading factors of the maintenance system is obtained by the method:
taking into account the convergence of the filter
For the corresponding fading factor of the unobservable state quantity, a 1 is determined due to the lack of corresponding metrology constraints. />
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211010946.7A CN115371650B (en) | 2022-08-23 | 2022-08-23 | Six-degree-of-freedom laser target measurement system and dynamic performance improvement method thereof |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211010946.7A CN115371650B (en) | 2022-08-23 | 2022-08-23 | Six-degree-of-freedom laser target measurement system and dynamic performance improvement method thereof |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115371650A CN115371650A (en) | 2022-11-22 |
| CN115371650B true CN115371650B (en) | 2023-06-02 |
Family
ID=84067268
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211010946.7A Active CN115371650B (en) | 2022-08-23 | 2022-08-23 | Six-degree-of-freedom laser target measurement system and dynamic performance improvement method thereof |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115371650B (en) |
Family Cites Families (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US7142981B2 (en) * | 2003-08-05 | 2006-11-28 | The Boeing Company | Laser range finder closed-loop pointing technology of relative navigation, attitude determination, pointing and tracking for spacecraft rendezvous |
| CN111722295B (en) * | 2020-07-04 | 2021-04-23 | 东南大学 | Underwater strapdown gravity measurement data processing method |
| CN114111771A (en) * | 2021-11-25 | 2022-03-01 | 九江中船仪表有限责任公司（四四一厂） | Dynamic attitude measurement method of double-shaft stable platform |
-
2022
- 2022-08-23 CN CN202211010946.7A patent/CN115371650B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN115371650A (en) | 2022-11-22 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106647791B (en) | Three-dimensional attitude measurement and control device, mechanical equipment and three-dimensional attitude measurement and control method | |
| CN109737913B (en) | Laser tracking attitude angle measurement system and method | |
| CN108225258A (en) | Based on inertance element and laser tracker dynamic pose measuring apparatus and method | |
| CN112611380B (en) | Attitude detection method based on multi-IMU fusion and attitude detection device thereof | |
| CN110196049A (en) | The detection of four gyro redundance type Strapdown Inertial Navigation System hard faults and partition method under a kind of dynamic environment | |
| CN105973268B (en) | A kind of Transfer Alignment precision quantitative evaluating method based on the installation of cobasis seat | |
| KR20080086711A (en) | Initial alignment method of inertial navigation system | |
| CN112697131B (en) | Underground mobile equipment positioning method and system based on vision and inertial navigation system | |
| CN113804185A (en) | Novel inertial navigation system based on MEMS array | |
| CN117739972B (en) | Unmanned aerial vehicle approach stage positioning method without global satellite positioning system | |
| CN113739795A (en) | Underwater synchronous positioning and mapping method based on polarized light/inertia/vision combined navigation | |
| CN110887486A (en) | Unmanned aerial vehicle visual navigation positioning method based on laser line assistance | |
| CN110672103A (en) | Multi-sensor target tracking filtering method and system | |
| CN111141286A (en) | Unmanned aerial vehicle flight control multi-sensor attitude confidence resolving method | |
| US11620846B2 (en) | Data processing method for multi-sensor fusion, positioning apparatus and virtual reality device | |
| CN115371650B (en) | Six-degree-of-freedom laser target measurement system and dynamic performance improvement method thereof | |
| CN114252077A (en) | Dual-GPS/SINS combined navigation method and system based on federal filter | |
| CN107888289B (en) | Indoor positioning method and platform based on fusion of visible light communication and inertial sensor | |
| CN112229392B (en) | High-redundancy indoor coal yard navigation method and system | |
| CN109443333B (en) | A kind of gyro array feedback weight fusion method | |
| CN115096294B (en) | Multi-parameter underwater magnetic target positioning method | |
| CN112344966B (en) | Positioning failure detection method and device, storage medium and electronic equipment | |
| CN114526729A (en) | Course optimization method of MEMS inertial positioning system based on redundancy technology | |
| CN114705223A (en) | Inertial navigation error compensation method and system for multiple mobile intelligent bodies in target tracking | |
| Xin et al. | Study on Dynamic and Accurate 6-DOF Measurement System and Approach |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |