CN105628026A - Positioning and posture determining method and system of mobile object - Google Patents

Abstract

The invention discloses a method and system for positioning and attitude determination of a moving object. By using an inertial measurement unit, an odometer and a laser radar, a unified and integrated laser radar control target data, inertial measurement unit data and odometer data are constructed. Mann filter model. This model is based on the dynamics model and error model of the inertial measurement unit. By bringing the lidar control target data into the Kalman filter equation, the error state vector of the IMU/odometer combination is calculated to limit its error divergence, thereby Get high-precision position and attitude. In this way, the high-precision positioning and attitude determination of moving objects is realized in the environment without satellite navigation signals.

Description

Method and system for positioning and attitude determination of a moving object

technical field

The invention relates to the technical field of inertial navigation, in particular to a method and system for positioning and attitude determination of a moving object.

Background technique

In all kinds of mobile measurement systems, attitude determination is one of the core tasks. The positioning and attitude determination system (POS) used in mobile measurement is generally composed of global navigation satellite system (GNSS) and inertial navigation system (INS), which provides position, velocity and Attitude benchmark. The positioning and attitude determination technology based on the combination of global navigation satellite system and inertial navigation system has the following advantages: using high-precision GNSS information, the error parameters such as gyro drift and accelerometer zero bias of the inertial navigation system (INS) can be estimated, thereby suppressing its error from varying Time accumulation; using the characteristics of high sampling rate and short-term high precision of INS, it can provide auxiliary information for GNSS, so that the estimation of errors such as clock error of GNSS receiver is more accurate, and the GNSS receiver can keep a low tracking bandwidth, improving its ability to reacquire satellite signals.

The long-term absolute accuracy of POS mainly relies on GNSS. Since GNSS accuracy is greatly affected by the environment, the long-term accuracy of POS is also affected by the environment. The complex and changeable real environment brings several major challenges to the application of Mobile Mapping System (MMS): In an environment with poor GNSS signals, such as urban areas with high-rise buildings and waters with serious multipath effects, the absolute position of POS The accuracy will be greatly reduced to the decimeter level or even the meter level. At this time, the requirements for high-precision mobile measurement can no longer be met; in the underground space without GNSS signals, the positioning and attitude errors calculated solely by INS will diverge rapidly over time. After a period of time, the upper error tolerance limit of the measurement will be exceeded. Therefore, in the underground space without GNSS signals, how to limit the error divergence of INS and maintain high mobile measurement accuracy is another challenge for MMS applications.

Traditional positioning methods for underground space mainly include surveying robot positioning, instant localization and mapping (SLAM), ultra-wideband positioning (UWB) and radio frequency identification positioning (RFID).

Measuring robot, also known as automatic total station, is a measuring platform integrating automatic target recognition, automatic sighting, automatic angle measurement and distance measurement, automatic target tracking, and automatic recording. The measuring robot can automatically find and align the target, and then measure automatically. The measurement robot has high measurement accuracy, but it is time-consuming to find the target, and it is difficult to achieve high dynamic continuous positioning.

Simultaneous localization and mapping methods (SLAM) use sensors such as cameras or lidar to infer its own position and build maps of unknown areas or update maps of known areas. According to the different sensors used, SLAM algorithms can be divided into stereo vision, monocular vision and depth cameras, and radar-based scanning sensors. SLAM mainly includes four steps: data association, map matching, closed-loop detection and global adjustment. The SLAM algorithm first obtains the position increment of this frame relative to the previous frame by matching the overlapping areas between data frames and frames, and then obtains the position of this frame relative to the initial moment by accumulating the position increments. At the same time, the SLAM algorithm will use multiple overlaps in the data and closed loops in the trajectory to perform global adjustments to obtain globally optimal trajectory and feature map data. When the robot enters a completely unfamiliar area, SLAM will draw a feature map of the area while estimating its own position. When the robot enters an area that has been driven before, SLAM will calculate the current position by matching the current data with the feature map data, and correct the calculation result.

UWB technology is a new type of wireless communication technology. Its frequency is between 3.1 and 10.6 GHz. Its unique feature is that it can be used for both communication and high-precision distance measurement. UWB has the advantages of insensitivity to channel fading, low power spectral density of transmitted signals, low interception capability, low system complexity, and high ranging accuracy. The application of UWB technology in the field of ranging and positioning has two advantages: 1) It can theoretically obtain centimeter-level or even higher ranging accuracy, which has great potential in precise positioning applications; 2) Due to the high time resolution, UWB has a strong penetrating ability, so that it can still complete ranging and positioning in complex indoor environments. UWB ranging is estimated based on time of arrival (TOA) or two-way delay ranging to measure the distance between a UWB mobile station and a base station. The TOA ranging method calculates the distance from the mobile station to the base station based on the time taken by the base station signal to reach the mobile station (multiplied by the speed of light c, so the time between the mobile station and the base station needs to be accurately synchronized. The two-way delay ranging is the transmission from the mobile station to the base station Request a ranging pulse, the base station returns to send a response pulse after receiving the request pulse, and the mobile station estimates the distance between the mobile station and the base station according to the time delay between the sending time and the receiving time after receiving the response pulse, so the distance between the mobile station and the base station No precise time synchronization is required.

UWB technology requires the establishment of base stations and mobile stations. UWB has the advantages of insensitivity to channel fading, low power spectral density of transmitted signals, low interception capability, and low system complexity. Theoretically, a centimeter-level or even higher ranging accuracy can be obtained. For example, someone can obtain sub-centimeter accuracy within a range of 2.5 meters, and achieve an accuracy of 0.1-0.15 meters within a range of 50 m. However, for long-term targets such as subways, a large number of base stations need to be deployed to achieve UWB signal coverage throughout the road, and the installation and maintenance costs are very high.

An RFID system consists of RFID tags and RFID readers. There are two types of RFID positioning methods, one is that the tag moves and the reader is fixed, and the other is that the reader moves and the tag is fixed. The basic process of RFID positioning in the mobile tag mode is: the coordinates of the reader are known, the RFID tag establishes communication with multiple readers, and then uses methods such as received signal strength detection, signal arrival direction or signal arrival time to calculate the distance between the RFID and each reader. The distance between them is finally calculated by the control center to get the position of the label. The mobile reader positioning method is similar to the mobile tag positioning method, the difference is that in this method, the calibration is fixed, the coordinates are known, and the reader is mounted on the carrier, and the position is to be measured.

The above method is not suitable for positioning in a long linear space such as a subway tunnel. Therefore, in order to ensure the accuracy of the underground space mobile measurement system, a low-cost, dynamic, high-precision positioning and attitude determination method suitable for underground space mobile 3D measurement is urgently needed.

Therefore, the existing technology still needs to be improved and improved.

Contents of the invention

In view of the shortcomings of the above-mentioned prior art, the object of the present invention is to provide a method and system for positioning and attitude determination of a moving object, which can realize high-precision positioning and attitude determination of a moving object in an environment without satellite navigation signals.

In order to achieve the above object, the present invention has taken the following technical solutions:

A method for positioning and attitude determination of a moving object, the method uses an inertial measurement unit, an odometer and a laser radar arranged in the moving object to locate and determine the attitude of the moving object, specifically comprising the following steps:

A. The data measured by the inertial measurement unit and the odometer are filtered by an inertial filter to obtain the initial position, initial velocity and initial attitude of the moving object;

B. The laser radar scans the measurement control point, and after the distance and angle measured by the laser radar are fused with the initial position and initial attitude, the initial laser point cloud is obtained, and the initial measurement of the measurement control point is extracted from the initial laser point cloud coordinate;

C. Calculate the difference between the known actual coordinates of the measurement control point and the initial measurement coordinates to obtain coordinate residuals;

D. The coordinate residual is calculated by the extended Kalman filter to calculate the error of the inertial measurement unit, the error correction amount of the initial position, the error correction amount of the initial velocity and the error correction amount of the initial attitude; the inertial measurement unit The error is fed back to the inertial filter;

E. Correct the initial position, initial velocity, and initial attitude by using the error correction amount of the initial position, the initial velocity error correction amount, and the initial attitude error correction amount to obtain the position, velocity, and attitude of the moving object.

In the method for positioning and attitude determination of a moving object, after the step E, further includes a step: F, performing smooth filtering on the position, velocity, attitude and filtering information of the moving object to obtain a smooth position, velocity and attitude.

In the method for positioning and attitude determination of a moving object, the step A specifically includes the steps of:

A1. Perform pure inertial calculation on the data measured by the inertial measurement unit to obtain the inertial position, velocity and attitude;

A2. The inertial position and attitude data are converted to obtain a rotation matrix, and the odometer increment is converted into a dead reckoning position increment;

A3. Calculate the difference between the inertial position increment and the dead reckoning position increment to obtain the position increment residual, which is brought into the inertial extended Kalman filter to obtain the error correction amount of the inertial measurement unit, as well as the velocity error correction amount and attitude Error correction amount, the position error correction amount is obtained according to the speed error correction vector and the filter period;

A4. Feedback the error correction amount of the inertial measurement unit to the inertial filter, and add the position, velocity, and attitude error correction amount to the inertial position, velocity, and attitude to obtain the initial filtered position, velocity, and attitude.

In the method for positioning and attitude determination of a moving object, the measurement control point is a preset passive reflection target.

A positioning and attitude determination system for a moving object, the system includes a moving object, an inertial measurement unit, an odometer and a laser radar, and the inertial measurement unit, the odometer and a laser radar are all arranged in the moving object; the laser radar is used For scanning the measurement control point, the distance and angle between the measurement control point and the moving object are obtained; the system also includes:

The inertial filter is used to perform inertial filtering on the data measured by the inertial measurement unit and the odometer to obtain the initial position, initial velocity and initial attitude of the moving object;

The coordinate residual calculation module is used to obtain the initial laser point cloud after the ranging and angle measurement data measured by the laser radar are fused with the initial position and the initial attitude, and the initial measurement coordinates of the measurement control points are extracted from the initial laser point cloud ; Calculate the difference between the known actual coordinates of the measurement control point and the initial measurement coordinates to obtain coordinate residuals;

The inertial extended Kalman filter is used to calculate the coordinate residual error through the extended Kalman filter to calculate the error of the inertial measurement unit, the error correction amount of the initial position, the error correction amount of the initial velocity and the error correction amount of the initial attitude ; The error of the inertial measurement unit is fed back into the inertial filter; the initial position, the initial velocity, and the initial attitude are performed by the error correction amount of the initial position, the error correction amount of the initial velocity, and the error correction amount of the initial attitude Correction to get the position, velocity and attitude of the moving object.

In the positioning and attitude determination system of the moving object, the system also includes:

The RTS smoothing filter is used to smooth and filter the position, velocity, attitude and filtering information of the moving object to obtain a smooth position, velocity and attitude.

In the positioning and attitude determination system of the moving object, the inertial extended Kalman filter is also used to obtain the error correction amount of the inertial measurement unit and the velocity error correction amount according to the position incremental residual output by the inertial filter and the attitude error correction amount, the position error correction amount is obtained according to the velocity error correction vector and the filter cycle; the error correction amount of the inertial measurement unit is fed back to the inertial filter;

The inertial filter is specifically used to perform pure inertial reckoning on the data measured by the inertial measurement unit to obtain inertial position, velocity and attitude; convert the inertial position and attitude data to obtain a rotation matrix, and convert the odometer increment into dead reckoning Position increment; the difference between the inertial position increment and the dead reckoning position increment is obtained to obtain the position increment residual, and the position increment residual is output to the inertial extended Kalman filter; according to the inertial extended Kalman filter feedback The error correction of the inertial measurement unit; the position, velocity and attitude error correction are added to the inertial position, velocity and attitude to obtain the initial filtered position, velocity and attitude.

In the positioning and attitude determination system for a moving object, the measurement control point is a preset passive reflection target.

Compared with the prior art, the present invention provides a method and system for positioning and attitude determination of a moving object. Through the inertial measurement unit, odometer and laser radar, a unified and integrated laser radar control target data and inertial measurement unit data are constructed. and an extended Kalman filter model for odometry data. This model is based on the dynamics model and error model of the inertial measurement unit. By bringing the lidar control target data into the Kalman filter equation, the error state vector of the IMU/odometer combination is calculated to limit its error divergence, thereby Get high-precision position and attitude. In this way, the high-precision positioning and attitude determination of moving objects is realized in the environment without satellite navigation signals.

Description of drawings

Fig. 1 is a structural diagram of a trolley in the positioning and attitude determination method of a moving object provided by the present invention.

Fig. 2 is a flow chart of the method for positioning and attitude determination of a moving object provided by the present invention.

Fig. 3 is a block diagram of LiDAR/IMU/odometer combined algorithm in the positioning and attitude determination method of a moving object provided by the present invention.

FIG. 4 is a schematic diagram of a reflective target in the method for positioning and attitude determination of a moving object provided by the present invention.

FIG. 5 is a schematic diagram of the IMU/odometer combined position variance curve in the positioning and attitude determination method for a moving object provided by the present invention.

Fig. 6 is a flow chart of an existing IMU/odometer combination.

Fig. 7 is a schematic diagram of LiDAR/IMU/odometer position variance curve in the positioning and attitude determination method of a moving object provided by the present invention.

Fig. 8 is a structural block diagram of the positioning and attitude determination system for a moving object provided by the present invention.

detailed description

The invention provides a positioning and attitude determination method and system for a moving object. In order to make the object, technical solution and effect of the present invention more clear and definite, the present invention will be further described in detail below with reference to the accompanying drawings and examples. It should be understood that the specific embodiments described here are only used to explain the present invention, not to limit the present invention.

Please refer to FIG. 1 , the present invention provides a method for positioning and attitude determination of a moving object, which uses an inertial measurement unit (i.e. IMU, not shown in the figure), an odometer 30 and a laser radar ( LiDAR) 20 locates and fixes the posture of the moving object 10. The laser radar 20 is installed on the moving object 10 and scans the measurement control points set in the scene during operation. Measuring the three-dimensional coordinates of the control points themselves is used to provide absolute positioning of the moving object 10 . The inertial measurement unit is arranged inside the moving object 10 , receives information from the odometer 30 , and performs relative positioning and attitude determination of the moving object 10 together with the odometer 30 . The odometer 30 is mounted on the wheels of the moving object 10 and is used to record the linear travel distance of the car. The moving object 10 can be vehicles such as automobiles and trains, or other moving objects with wheels, which is not limited in the present invention. In this embodiment, it is a trolley as shown in FIG. 1 .

The application environment of the present invention is: an underground or indoor environment without satellite navigation signals, and there is a ready-made high-precision control point network or a newly established high-precision control point network in the scene. The high-precision control point network includes a plurality of preset measurement control points. The inertial measurement unit, the odometer 30 and the laser radar 20 measure and obtain data under the time reference established by the high-precision time synchronizer, and obtain the position and attitude of the moving object through the method (algorithm) of the positioning and attitude determination method (algorithm) of the moving object provided by the present invention. High-precision position and attitude.

Please refer to Fig. 2 and Fig. 3, the method for positioning and attitude determination of a moving object provided by the present invention specifically includes the following steps:

S10, the data measured by the inertial measurement unit and the odometer are filtered by an inertial filter to obtain the initial position, initial velocity and initial attitude of the moving object.

S20, the laser radar scans the measurement control point, and after the distance and angle measured by the laser radar are fused with the initial position and initial attitude, an initial laser point cloud is obtained, and the initial measurement of the measurement control point is extracted from the initial laser point cloud coordinate. The measurement control point is a preset passive reflection target. In this embodiment, the shape of the reflective target is as shown in Figure 4, which is square or rectangular, divided into four triangular areas by two diagonal lines, wherein the two opposite triangular areas are dark (black) ), and the other two opposite triangular areas are light colored (white). The reflective target is cheap and easy to extract from the point cloud, and its central point coordinates are measured by a measuring robot in advance. In order to facilitate the measurement, the scanning angle and direction of the lidar can be adjusted according to the specific scene.

S30. Calculate the difference between the known actual coordinates of the measurement control point and the initial measurement coordinates to obtain coordinate residuals.

S40. Calculate the coordinate residual by using an extended Kalman filter to calculate the error of the inertial measurement unit, the error correction amount of the initial position, the error correction amount of the initial velocity, and the error correction amount of the initial attitude; the inertial measurement unit The error is fed back to the inertial filter. Wherein, the error of the inertial measurement unit includes accelerometer zero bias and gyroscope drift.

S50. Using the error correction amount of the initial position, the error correction amount of the initial velocity, and the error correction amount of the initial attitude, correct the initial position, initial velocity, and initial attitude to obtain the position, velocity, and attitude of the moving object.

It can be seen that, the present invention uses the inertial measurement unit, the odometer and the laser radar to construct a unified extended Kalman filter model that integrates the measurement data of the three. This model is based on the dynamics model and error model of the inertial measurement unit. By bringing the lidar control target data into the Kalman filter equation, the error state vector of the IMU/odometer combination is calculated to limit its error divergence, thereby Get high-precision position and attitude. In this way, the high-precision positioning and attitude determination of moving objects is realized in the environment without satellite navigation signals.

Further, after the step S50, a step S60 is further included: smoothing and filtering the position, velocity, attitude and filtering information of the moving object to obtain a smooth position, velocity and attitude. The filtering information includes: gain matrix, state transition matrix, observation matrix, observation noise covariance matrix and system noise covariance matrix.

In the prior art, the positioning and attitude determination of moving objects can be performed through the inertial measurement unit (IMU)/odometer combination model, but the error will accumulate over time. The specific analysis is as follows:

Whether it is IMU calculation or odometer calculation, it is a cumulative process, during which errors will also accumulate. The two main factors that determine the speed at which IMU errors accumulate are the bias stability of the accelerometer and the drift of the gyroscope. High-precision IMUs are expensive, usually in the millions of yuan, medium and high-precision IMUs cost hundreds of thousands of yuan, and low-precision IMUs cost tens of thousands of yuan. Although the price of micromechanical gyroscopes that appear at present is relatively low, ranging from a few yuan to tens of thousands of yuan, their accelerometers have poor zero-bias stability and large gyro drift, which is not suitable for high-precision measurement fields. The high-precision odometer can reach the accuracy of 0.2m/1000m, which is far better than the accuracy of medium and high-precision IMU to calculate the mileage, and its price is relatively low, only a few thousand yuan. Therefore, using a high-precision odometer and a medium-high-precision IMU for combined positioning and attitude determination in a GNSS-free environment is undoubtedly a solution that is both economical and accurate.

Specifically, in the present invention, the combination model of IMU/odometer is described first. In other words, the step S10 specifically includes:

Define the axis of the odometer body coordinate system to be consistent with the vehicle coordinate system and the IMU body coordinate system (b system), then the mileage increment ΔD output by the odometer is converted to the coordinate increment ΔD ^b in the b system: ${\mathrm{\ΔD}}^b=\left[\begin{array}{c}{\mathrm{\θ}}_z\\ 1-k\\ {\mathrm{\θ}}_x\end{array}\right]\mathrm{\Δ}\mathrm{D.}$ (Formula 4.1).

In formula 4.1, θz and θx are the azimuth correction and pitch correction of the body coordinate system of the odometer relative to the b system, respectively, and k is the correction of the scale factor of the odometer. In the IMU/odometer combination, it is necessary to add the three-dimensional odometer error state vector ΔS=[δk,δθx,δθz] ^T on the basis of the IMU error state vector (the superscript “T” means transpose). Therefore, the combined model (i.e., the combined model of IMU and odometry) state variable is $x^{,}={\[{\mathrm{\ΔP}}_1,{\mathrm{\ΔV}}_1,{\mathrm{\Δo}}_1,{\mathrm{\Δa}}_1^b,{\mathrm{\Δe}}_1^b,\mathrm{\Δ}S\]}^T$ (Formula 4.2);

Among them, ΔP ₁ is the initial three-dimensional position error correction vector (that is, the error correction amount of the initial position, which is the northeast coordinate system), ΔV ₁ is the error correction vector of the initial velocity (the error correction amount of the initial velocity), Δo ₁ is the initial attitude error correction vector (the error correction amount of the initial attitude), Δa ₁ is the initial accelerometer zero bias correction vector, is the amount after the initial accelerometer zero bias correction vector is transformed into the b system, Δe ₁ is the gyroscope drift correction vector, Corresponding to the amount after the gyroscope drift correction vector is transformed into the b system, ΔS is the odometer error state vector. It is generally considered that the odometer error state vector is a constant, namely: $\left[\begin{array}{c}\mathrm{\δ}\stackrel{\&Center\; Dot;}{k}\\ \mathrm{\δ}\stackrel{.}{\mathrm{\θ}x}\\ \mathrm{\δ}\stackrel{.}{\mathrm{\θ}z}\end{array}\right]=\left[\begin{array}{c}0\\ 0\\ 0\end{array}\right]$ (Formula 4.3). Here, the variable with origin represents the differential of the original variable with respect to time. by For example, It is the differential of δk with respect to time.

Then the system state equation of IMU/odometer combination is: (Formula 4.4).

The coefficient matrix F' is $f^{\′}=\left[\begin{array}{cc}f& 0_{3\×3}\\ 0_{3\×15}& 0_{3\×3}\end{array}\right]$ (Formula 4.5);

Among them, F is the state transition matrix in the error model of the inertial system Φ angle system, 0 _m×n represents the zero matrix of m rows by n columns, that is, 0 _3×3 and 0 _3×15 are zero matrices of 3 rows and 3 columns and a matrix of zeros with 3 rows and 15 columns. The position incremental residual model of the observation model: the observation z is: z=ΔP _I -ΔP _S (Formula 4.6);

Among them, ΔP _I is the position increment calculated by the IMU within a filtering cycle, and ΔP _S is the position increment calculated by using the IMU attitude and odometer.

The observation equation is: z=ΔV ₁ δt-ΔP _S ×MΔS+ξ (Formula 4.7);

δt is the filter period, M is the coefficient matrix of the odometer parameter error vector, and ξ is the equivalent observation noise. The specific form of M is $m_{\mathrm{no}}={\left[\begin{array}{ccc}{\mathrm{\δS}}_x^b& 0& {\mathrm{\δS}}_{\mathrm{the\; y}}^b\\ {\mathrm{\δS}}_{\mathrm{the\; y}}^b& {\mathrm{\δS}}_x^b& -{\mathrm{\δS}}_x^b\\ {\mathrm{\δS}}_x^b& -{\mathrm{\δS}}_{\mathrm{the\; y}}^b& 0\end{array}\right]}_{\mathrm{no}}$ (Equation 4.8), $m={\mathrm{\Σ}}_{\mathrm{no}}{C}_{b\mathrm{no}}^{\mathrm{no}}{m}_{\mathrm{no}}$ (Formula 4.9);

In Equation 4.8, is the increment of the odometer in the X-axis direction of the b system within an odometer sampling interval [n-1,n], is the increment in the Y-axis direction; in formula 4.9, is the rotation matrix from the b system to the n system of the navigation coordinate system.

Here, the sub-vector X” of the state vector X’ after removing the position error vector is taken as the state vector of the IMU/odometer group (the corresponding rows and columns also need to be removed from the F’ matrix, denoted as F”), then the observation equation is : z=HX"+ξ (formula 4.10);

ξ is the observation noise, and the observation matrix H is: H=[I _3×3 δt,-ΔPs×,0 _3×6 ,M] (Formula 4.11);

Among them, I3×3 represents the third-order identity matrix, and it is worth noting the selection of the filtering period δt. If δt is too small, the observation noise will be amplified, while if the period is too long, the velocity error cannot be considered constant. Therefore, the filtering period δt may be between 0.2s and 2s, and generally a filtering period of 0.5 second or 1 second is preferred.

Kalman filter process time update:

X” _k+1|k = Γ” _k X” _k (Formula 4.12);

Γ” _k = I+F”Δt (Formula 4.13);

${{\mathrm{D.}}^{\′\′}}_{k+1|k}={{\mathrm{\Γ}}^{\′\′}}_k{{\mathrm{D.}}^{\′\′}}_k{{\mathrm{\Γ}}^{\′\′}}_k^T+Q$ (Formula 4.14);

Status update:

X" _k+1 = X" _k+1|k +X _k+1 (Z _k -HX" _k+1|k ) (Formula 4.15);

Gain matrix: K _k+1 = D _k+1|k H ^T (HP _k+1|k H ^T +R) ^-1 (Equation 4.16);

Covariance matrix: D” _k+1 = (IH=K _k+1 H)D” _k+1|k (Formula 4.17);

In the formula, Δt is the IMU update period, R is the observation noise covariance matrix, and Q is the equivalent system noise covariance matrix. After obtaining the speed error correction amount of the IMU, use the formula 4.19 to calculate the position error correction amount: ΔP ₁ =ΔV ₁ δt (formula 4.18).

Note that in the Kalman state update, the position error variance does not participate in the update. The calculation model of the position error variance when the IMU/odometer combination is further considered below. In the position recursion process, the position error is modeled as a first-order Markov process, and its model is as follows:

ΔP _i+1 = Γ _p [ΔP _i , ΔV _i ] ^T +ξ _p (Equation 4.19);

${\mathrm{D.}}_{i+1}={\mathrm{\Γ}}_p{\mathrm{D.}}_{[{\mathrm{\ΔP}}_i,\mathrm{\Δ}{V}_i]}{{\mathrm{\Γ}}_p}^T+Q_p$ (Equation 4.20).

Among them, Γ _p is the coefficient matrix, ξ _p is the system model noise, and Q _p is the system noise variance matrix. At the moment of filtering, the position error correction calculated by formula (4.19) is fed back to the inertial filter and then set to zero. During the recursion, the position error remains zero and the variance of the position error increases exponentially. Consider the following three aspects of the IMU/odometer combination: 1) After filtering correction, the position error uncertainty decreases, and the variance should decrease at this time; 2) The positioning error uncertainty of the IMU/odometer combination system will increase with time. 3) The main role of the position variance is to accurately distribute the position error correction to other moments during reverse smoothing. Therefore, at the filtering update moment, the present invention designs a position variance function (a logarithmic model to calculate the variance of the position error) that grows smoothly over time, but the growth rate is much smaller than the exponential function; in other words, the present invention The inertial filter calculates the variance of the position error through the logarithmic model at the moment of filtering update. The specific formula is as follows:

East direction position error covariance matrix $\begin{array}{cc}{\mathrm{D.}}_{P_{\mathrm{E.}}}={\mathrm{\σ}}_{\mathrm{E.}}^{2}I\mathrm{no}& (e+t)\end{array}$

North direction position error covariance matrix $\begin{array}{cc}{\mathrm{D.}}_{P_N}={\mathrm{\σ}}_N^{2}I\mathrm{no}& (e+t)\end{array}$

Elevation error covariance matrix $\begin{array}{cc}{\mathrm{D.}}_{P_h}={\mathrm{\σ}}_h^{2}I\mathrm{no}& (e+t)\end{array}$ (Equation 4.21).

in, is the base value of variance in the east direction, is the base value of variance in the north direction, is the base value of elevation variance. Therefore, the overall trend of the IMU/odometer combined positioning variance will be shown in Figure 5: within a filtering period δt, the position variance increases rapidly with time, and at the moment of odometer data correction, the uncertainty of the position error decreases, and the variance also follows Decrease, but still greater than the previous filtering time, that is, the overall trend is still increasing.

Finally, the overall flow of the IMU/odometer combination is shown in Figure 6, that is, the step S10 specifically includes:

S110 , performing pure inertial calculation on data measured by an inertial measurement unit (IMU), to obtain an inertial position, velocity, and attitude.

S120. The inertial position and attitude data are converted to obtain a rotation matrix, and the odometer increment is converted into a dead reckoning position increment.

S130. Calculate the difference between the inertial position increment and the dead reckoning position increment to obtain the position increment residual, which is brought into the inertial extended Kalman filter to obtain the error correction amount of the inertial measurement unit (accelerometer zero bias and gyroscope Drift), as well as the velocity error correction and attitude error correction, the position error correction ΔP ₁ is obtained according to the velocity error correction vector ΔV ₁ and the filter period δt, that is, the position error correction ΔP ₁ is obtained by using formula 4.18.

S140. Feedback the error correction amount of the inertial measurement unit to the inertial filter (pure inertial filter), and add the position, velocity and attitude error correction amount to the inertial position, velocity and attitude to obtain the initial filtered position, velocity and attitude. Attitude (i.e., get initial filtered position, initial filtered velocity, and initial filtered pose).

What the present invention provides is the LiDAR/IMU/odometer combined model. Without the assistance of lidar, the error accumulation rate will increase over time. Over time, errors may diverge beyond measurement requirements. In order to obtain a long-term high-precision trajectory, it is necessary to introduce external high-precision position information to correct the accumulated error. Therefore, the introduced absolute position accuracy will determine the absolute accuracy of the entire positioning and attitude determination system. The method for positioning and attitude determination of a moving object provided by the present invention is to lay out LiDAR control targets (measurement control points) in the measurement site of the underground space, and then use the laser radar to observe these LiDAR control targets, and compare the observed value with the true value The residual observations between are brought into the Kalman filter variance to correct the cumulative error of the IMU/odometer system.

The step S20 specifically includes: According to the formula 4.22, the distance and angle measured by the lidar can be fused with the initial filtered position calculated by the combined positioning and attitude determination of the IMU/odometer and the initial filtered attitude data to obtain the initial laser point cloud.

$x^g=T^g+R_b^g\&Center\; Dot;(R_l^b\·x^l+T_l^b)$ Formula (4.22)

Here, the superscripts g, b, and l represent the geospatial coordinate system, the IMU coordinate system, and the laser scanning coordinate system respectively, X ^g is the coordinate vector of the LiDAR point in the geospatial coordinate system, and T ^g is the navigation center in the geospatial coordinate system the coordinate vector of is the rotation matrix from the IMU coordinate system to the geospatial coordinate system, and is calculated from the attitude x calculated by the IMU. X ^l is the coordinate vector of the laser scanning observation value in the laser coordinate system, is the translation vector from the laser scanner coordinate system to the IMU coordinate system. From the laser scanner to the IMU coordinate system to the rotation matrix, it is calculated from the space synchronization attitude angle obtained by calibration.

The step S30 specifically includes: using other high-precision measurement means to measure the high-precision coordinates of the laser radar control target, expressed as _XL , and taking this value as the real value, and the high-precision coordinates of the laser radar control target _XL is the measurement control point Known actual coordinates. X _L is subtracted from the corresponding target observation X ^g in the initial laser point cloud to obtain the coordinate residual observation ΔX _G , that is, ΔX _G =X _L -X ^g formula (4.23).

The step S40 specifically includes: on the one hand, considering that after the odometer correction, the attitude accuracy of the combined IMU/odometer solution is relatively high; on the other hand, when extracting the center point of the control target from the point cloud, there may be small deviation, while the effect of attitude error may be masked by the effect of control point extraction error. Therefore, the control point residual observations will be mainly due to positioning errors. Thus, we get the relationship between ΔX _G and IMU/odometer positioning and attitude errors ΔP ₂ , Δo ₂ ΔX _G =ΔP ₂ +ξ (Formula 4.24).

Let the observed value z ₂ =ΔX _G , the system state vector X is the IMU error state vector, namely:

$x={\left[\begin{array}{ccccc}{\mathrm{\ΔP}}_2& {\mathrm{\ΔV}}_2& {\mathrm{\Δo}}_2& {\mathrm{\Δa}}_2^b& {\mathrm{\Δe}}_2^b\end{array}\right]}^T$ (Formula 4.25);

Then the observation equation is:

z ₂ =H ₂ X+ξ

H ₂ =[I _3×3 ,0 _3×12 ] (Formula 4.26);

The state equation is: $\stackrel{\·}{x}=\mathrm{FX}+\mathrm{\ξ}$ (Equation 4.27).

The noise of control point residual observations mainly comes from control point coordinate errors and extraction errors when extracting control points from point clouds. Control point error and extraction error obey Gaussian distribution respectively and a Gaussian distribution Then the distribution of observation noise is In actual operation, we can use the measuring robot to control the control point coordinate accuracy to 0.02m, and use the visualization software to control the extraction accuracy to 0.01m. Therefore, the control point noise equation is 0.0005m ² .

The step S50 specifically includes: the LiDAR/IMU filtering process is similar to formulas 4.12-4.15. If the LiDAR/IMU filter time interval is not equal, after the control point correction is adopted, the odometer calculation should be cleared, and the IMU/ odometer filter cycle should be restarted. Otherwise, wrong speed correction will occur, causing filter disturbance. Figure 7 is a schematic diagram of the position variance curve when the LiDAR/IMU/odometer is combined: in a LiDAR/IMU/odometer filtering cycle, the position variance increases with time, and at the filtering time, the accumulated position error is corrected, and the position variance increases with time. decrease.

The step S60 specifically includes: Whether it is an IMU/odometer combination or a LiDAR/IMU/odometer combination, the position, speed and attitude will jump at the filter correction point, causing them to be discontinuous in time. For real-time positioning and navigation, this jump can be ignored. For mobile mapping, these jumps need to be eliminated to ensure the time continuity of the trajectory, that is, the trajectory needs to be smoothed. Another purpose of smoothing is to take advantage of filtering.

As described in step S60, whether it is an IMU/odometer combination or a LiDAR/IMU/odometer combination, the position, velocity and attitude will jump at the filter correction point, causing them to be discontinuous in time. For real-time positioning and navigation, this jump can be ignored. For mobile mapping, these jumps need to be eliminated to ensure the time continuity of the trajectory, that is, the trajectory needs to be smoothed. Another purpose of smoothing is to use the filtered information to calculate the position, velocity, and attitude error corrections at other times, thereby improving the overall trajectory accuracy. Commonly used smoothing algorithms include forward and reverse filtering and fixed lag interval reverse smoothing. Here, the fixed interval reverse smoothing R-T-S algorithm is used, and its mathematical model is:

${\hat{x}}_k^S={\hat{x}}_k+K_k^S({\hat{x}}_{k+1}^S-{\mathrm{\Γ}}_{k+1|k}{\hat{x}}_k)$ (Formula 4.28);

D _kS ＝D _k +K _kS (D _kS -D _k+1|k )(K _kS ) ^T (Formula 4.29);

$K_k^S={\mathrm{D.}}_k{\mathrm{\Γ}}_{k+1|k}^T{\mathrm{D.}}_{k+1|k}^{-1}$ (Formula 4.30);

in, is the smoothing correction amount at time k, is the filtering correction value, K _kS is the smoothing gain matrix, and D _kS is the covariance matrix after smoothing.

It can be seen that the present invention constructs a unified extended Kalman filter model that fuses LiDAR control target data, IMU data and odometer data. The model is based on the IMU dynamics model and error model. By bringing the LiDAR control target data into the Kalman filter equation, the error state vector of the IMU/odometer combination is calculated, and the inertial error is limited to obtain the vehicle height. Precision corrected position and attitude.

The invention solves the absolute positioning and attitude determination in the whole underground/indoor environment and the environment without satellite positioning signals. Absolute coordinates are introduced via control points, positioning without GNSS signals.

The present invention can provide high-precision positioning, attitude and attitude determination results in large-scale and large-span indoor/underground places, which is unmatched by other traditional methods. The algorithm proposed by the invention is combined with corresponding sensors, and can provide an absolute positioning accuracy of 50mm and a relative positioning accuracy of 2mm/10m.

The high-precision position and attitude results provided by the invention can be used for high-precision movement measurement, which is unmatched by other traditional methods. The high-precision positioning and attitude determination results can be used in mobile measurement systems for high-precision measurement in industries such as rail and transportation. This cannot be done by traditional methods.

The method of the invention does not need to establish base stations and rely on radio signals, so it is relatively flexible, convenient and low in cost. The dependent control point can be existing or rearranged, and is a passive, static sign with no signal sending and receiving. Therefore, it is easy to implement, does not need to be connected to a power supply and is free from electromagnetic signal interference, and is flexible and convenient to use.

The invention solves high-precision positioning and attitude determination in an environment without satellite positioning signals, and provides necessary conditions for high-precision measurement in corresponding industries.

Based on the method for positioning and attitude determination of a moving object provided in the above embodiments, the present invention further provides a system for positioning and attitude determination of a moving object. Referring to Fig. 8, the system includes: the above-mentioned mobile object, inertial measurement unit 40, odometer 30 and laser radar 20, and the inertial measurement unit 40, odometer 30 and laser radar 20 are all arranged in the mobile object The laser radar 20 is used to scan the measurement control point to obtain the distance and angle between the measurement control point and the moving object; the system also includes: an inertial filter 50, a coordinate residual calculation module 60, and an inertial extended Kalman filter 70 and the RTS smoothing filter.

The inertial filter 50 is used to perform inertial filtering on the data measured by the inertial measurement unit 40 and the odometer 30 to obtain the initial position, initial velocity and initial attitude of the moving object.

The coordinate residual calculation module 60 is used to fuse the ranging and angle measuring data measured by the laser radar 20 with the initial position and initial attitude to obtain an initial laser point cloud, and extract measurement control points from the initial laser point cloud The initial measurement coordinates of the measurement control point; the difference between the known actual coordinates of the measurement control point and the initial measurement coordinates is obtained to obtain the coordinate residual. The measurement control point is a preset passive reflection target.

The inertial extended Kalman filter 70 is used to calculate the coordinate residual error through the extended Kalman filter to calculate the error of the inertial measurement unit, the error correction amount of the initial position, the error correction amount of the initial velocity and the initial attitude Error correction amount; the error of the inertial measurement unit is fed back to the inertial filter; through the error correction amount of the initial position, the error correction amount of the initial velocity and the error correction amount of the initial attitude, the initial position, the initial velocity, the error correction amount The initial attitude is corrected to obtain the position, velocity and attitude of the moving object.

The RTS smoothing filter 80 is used for smoothing and filtering the position, velocity, attitude and filtering information of the moving object to obtain a smooth position, velocity and attitude.

The inertial extended Kalman filter 70 is also used to obtain the error correction amount of the inertial measurement unit, the velocity error correction amount and the attitude error correction amount according to the position incremental residual output by the inertial filter 50, and the velocity error correction amount and the attitude error correction amount are obtained according to the velocity error correction amount The vector and the filter cycle obtain the position error correction amount; the error correction amount of the inertial measurement unit is fed back to the inertial filter 50;

The inertial filter 50 is specifically used to perform pure inertial calculation on the data measured by the inertial measurement unit to obtain the inertial position, velocity and attitude; convert the inertial position and attitude data to obtain a rotation matrix, and convert the odometer increment into the navigation position Calculate the position increment; calculate the difference between the inertial position increment and the dead reckoning position increment to obtain the position increment residual, and output the position increment residual to the inertial extended Kalman filter; according to the inertial extended Kalman filter 70 The error correction of the inertial measurement unit fed back; the position, velocity and attitude error correction are added to the inertial position, velocity and attitude to obtain the initial filtered position, velocity and attitude.

Since the positioning and attitude determination principle and technical features of the positioning and attitude determination system for the moving object have been described in detail in the above embodiments, they will not be repeated here.

It can be understood that those skilled in the art can make equivalent replacements or changes according to the technical solutions and inventive concepts of the present invention, and all these changes or replacements should belong to the protection scope of the appended claims of the present invention.

Claims (8)

1. A method for positioning and determining the posture of a moving object is characterized in that the method is used for positioning and determining the posture of the moving object through an inertia measurement unit, a speedometer and a laser radar which are arranged in the moving object, and specifically comprises the following steps:

A. the data measured by the inertia measurement unit and the odometer are filtered by an inertia filter to obtain the initial position, the initial speed and the initial attitude of the moving object;

B. scanning the measurement control point by the laser radar, fusing the distance and the angle measured by the laser radar with the initial position and the initial attitude to obtain initial laser point cloud, and extracting the initial measurement coordinate of the measurement control point from the initial laser point cloud;

C. calculating the difference between the actual coordinate known by the measurement control point and the initial measurement coordinate to obtain a coordinate residual error;

D. calculating the coordinate residual errors through extended Kalman filtering to calculate errors of an inertial measurement unit, error correction of an initial position, error correction of an initial speed and error correction of an initial attitude; feeding back the error of the inertial measurement unit into an inertial filter;

E. and correcting the initial position, the initial speed and the initial attitude through the error correction amount of the initial position, the error correction amount of the initial speed and the error correction amount of the initial attitude to obtain the position, the speed and the attitude of the moving object.

2. The method of determining the position and orientation of a moving object according to claim 1, further comprising, after said step E, the steps of: F. and carrying out smooth filtering on the position, the speed, the attitude and the filtering information of the moving object to obtain the smooth position, the smooth speed and the smooth attitude.

3. The method as claimed in claim 1, wherein the step a comprises the steps of:

a1, carrying out pure inertia calculation on the data measured by the inertia measurement unit to obtain the inertia position, speed and attitude;

a2, converting inertial position and attitude data to obtain a rotation matrix, and converting the odometer increment into dead reckoning position increment;

a3, subtracting the inertial position increment and the dead reckoning position increment to obtain a position increment residual error, bringing the position increment residual error into an inertial extended Kalman filter to obtain an error correction amount of an inertial measurement unit, a speed error correction amount and an attitude error correction amount, and obtaining the position error correction amount according to a speed error correction vector and a filtering period;

and A4, feeding error correction of the inertial measurement unit back to the inertial filter, and adding the position, speed and attitude error correction to the inertial position, speed and attitude to obtain an initial filtering position, speed and attitude.

4. The method of claim 1, wherein the measurement control point is a predetermined passive reflection target.

5. The system is characterized by comprising a moving object, an inertia measurement unit, a speedometer and a laser radar, wherein the inertia measurement unit, the speedometer and the laser radar are all arranged in the moving object; the laser radar is used for scanning the measurement control point to obtain the distance and the angle between the measurement control point and the moving object; the system further comprises:

the inertial filter is used for carrying out inertial filtering on the data measured by the inertial measurement unit and the odometer to obtain the initial position, the initial speed and the initial attitude of the moving object;

the coordinate residual error calculation module is used for fusing the ranging angle measurement data measured by the laser radar with the initial position and the initial attitude to obtain initial laser point cloud, and extracting the initial measurement coordinate of the measurement control point from the initial laser point cloud; calculating the difference between the actual coordinate known by the measurement control point and the initial measurement coordinate to obtain a coordinate residual error;

the inertial extended Kalman filter is used for calculating the coordinate residual error through extended Kalman filtering to calculate the error of an inertial measurement unit, the error correction of an initial position, the error correction of an initial speed and the error correction of an initial attitude; feeding back the error of the inertial measurement unit into an inertial filter; and correcting the initial position, the initial speed and the initial attitude through the error correction amount of the initial position, the error correction amount of the initial speed and the error correction amount of the initial attitude to obtain the position, the speed and the attitude of the moving object.

6. The position and attitude determination system for a moving object according to claim 4, characterized in that it further comprises:

and the RTS smoothing filter is used for smoothing and filtering the position, the speed, the attitude and the filtering information of the moving object to obtain the smoothed position, speed and attitude.

7. The system according to claim 5, wherein the inertial extended kalman filter is further configured to obtain an error correction amount, a velocity error correction amount, and an attitude error correction amount of the inertial measurement unit based on the position increment residual outputted from the inertial filter, and obtain the position error correction amount based on the velocity error correction vector and the filtering cycle; feeding back the error correction of the inertial measurement unit to an inertial filter;

the inertial filter is specifically used for carrying out pure inertial calculation on the data measured by the inertial measurement unit to obtain an inertial position, a velocity and an attitude; converting the inertial position and attitude data to obtain a rotation matrix, and converting the odometer increment into a dead reckoning position increment; the inertial position increment and the dead reckoning position increment are subtracted to obtain a position increment residual error, and the position increment residual error is output to an inertial extended Kalman filter; error correction of an inertial measurement unit fed back by an inertial extended Kalman filter; and adding the position, speed and attitude error correction quantity with the inertial position, speed and attitude to obtain an initial filtering position, speed and attitude.

8. The system of claim 5, wherein the measurement control points are predetermined passive reflective targets.