CN105628026B - A kind of positioning and orientation method and system of mobile object - Google Patents
A kind of positioning and orientation method and system of mobile object Download PDFInfo
- Publication number
- CN105628026B CN105628026B CN201610126054.1A CN201610126054A CN105628026B CN 105628026 B CN105628026 B CN 105628026B CN 201610126054 A CN201610126054 A CN 201610126054A CN 105628026 B CN105628026 B CN 105628026B
- Authority
- CN
- China
- Prior art keywords
- initial
- attitude
- inertial
- error correction
- speed
- 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 48
- 238000005259 measurement Methods 0.000 claims abstract description 124
- 238000012937 correction Methods 0.000 claims description 108
- 238000001914 filtration Methods 0.000 claims description 62
- 239000011159 matrix material Substances 0.000 claims description 33
- 238000009499 grossing Methods 0.000 claims description 14
- 238000004364 calculation method Methods 0.000 claims description 12
- 239000013598 vector Substances 0.000 abstract description 29
- 230000004927 fusion Effects 0.000 abstract 1
- 230000036544 posture Effects 0.000 description 17
- 230000008569 process Effects 0.000 description 8
- 238000005516 engineering process Methods 0.000 description 7
- 238000010586 diagram Methods 0.000 description 6
- 238000013507 mapping Methods 0.000 description 5
- 238000009825 accumulation Methods 0.000 description 4
- 238000000605 extraction Methods 0.000 description 4
- 230000000694 effects Effects 0.000 description 3
- 230000007774 longterm Effects 0.000 description 3
- 238000004891 communication Methods 0.000 description 2
- 238000007796 conventional method Methods 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 238000005562 fading Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000004044 response Effects 0.000 description 2
- 238000005070 sampling Methods 0.000 description 2
- 230000003595 spectral effect Effects 0.000 description 2
- 230000002123 temporal effect Effects 0.000 description 2
- 230000007704 transition Effects 0.000 description 2
- 230000004075 alteration Effects 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000001186 cumulative effect Effects 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 230000035515 penetration Effects 0.000 description 1
- 230000011664 signaling Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 1
Classifications
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
The invention discloses a kind of positioning and orientation method and systems of mobile object, by Inertial Measurement Unit, odometer and laser radar, the Extended Kalman filter model that unify, fusion laser radar control target data, Inertial Measurement Unit data and mileage count is built.The model foundation is on the basis of Inertial Measurement Unit kinetic model and error model, by the way that laser radar control target data are brought into Kalman filter equation, the error state vector for calculating the combination of IMU/ odometers limits the diverging of its error, to obtain high precision position and posture.To realize in the environment of no satellite navigation signals, appearance is determined to the high accuracy positioning of mobile object.
Description
Technical Field
The invention relates to the technical field of inertial navigation, in particular to a positioning and attitude determining method and system for a moving object.
Background
In various mobile measurement systems, positioning and attitude determination are one of the core tasks. A positioning and attitude determination system (POS) used in mobile surveying is generally composed of a Global Navigation Satellite System (GNSS) and an Inertial Navigation System (INS), and provides position, speed, and attitude references for a mobile surveying system and various task loads carried by the mobile surveying system. The positioning and attitude determination technology based on the combination of the global navigation satellite system and the inertial navigation system has the following advantages: by utilizing high-precision GNSS information, error parameters such as gyro drift and accelerometer zero offset of an Inertial Navigation System (INS) can be estimated, so that the accumulation of errors along with time is inhibited; the characteristics of high INS sampling rate, short time and high precision are utilized to provide auxiliary information for the GNSS, so that the estimation of error quantities such as clock error of the GNSS receiver is more accurate, the GNSS receiver can keep lower tracking bandwidth, and the capability of recapturing satellite signals is improved.
The long-term absolute accuracy of POS is mainly dependent on GNSS. Because the GNSS precision is greatly influenced by the environment, the long-term precision of the POS is also influenced by the environment. The complex and varied reality environment presents several challenges to Mobile Mapping System (MMS) applications: in an environment with poor GNSS signals, such as a high-rise wooded urban area and a water area with a serious multipath effect, the absolute position precision of the POS can be greatly reduced to a decimeter level or even a meter level, and the requirement of high-precision mobile measurement can not be met at the moment; in an underground space without GNSS signals, positioning and attitude determination errors calculated by means of INS can quickly diverge along with time, and after a period of time, the errors exceed the upper limit of the tolerance of the measurement errors. Therefore, how to limit the error divergence of the INS and maintain high mobile measurement accuracy in the underground space without GNSS signals is another challenge for MMS application.
Conventional methods for underground space positioning mainly include surveying robot positioning, a simultaneous positioning and mapping method (SLAM), an ultra wide band positioning method (UWB), and a radio frequency identification positioning method (RFID).
The measuring robot is also called an automatic total station, and is a measuring platform integrating automatic target identification, automatic collimation, automatic angle measurement and distance measurement, automatic target tracking and automatic recording. The measuring robot can automatically find and align the target and then automatically measure. The measuring robot has high measuring precision, but the time for finding the target is long, and the high dynamic continuous positioning is difficult to realize.
The instant positioning and mapping method (SLAM) uses sensors such as cameras or lidar to deduce its position and build a map of an unknown area or update a map of a known area. SLAM algorithms can be classified into stereo vision, monocular vision and depth cameras, and radar-based scanning sensors, etc., depending on the sensor employed. SLAM mainly comprises four steps of data association, map matching, closed-loop detection and global adjustment. The SLAM algorithm firstly obtains the position increment of the frame relative to the previous frame through the matching of the overlapping area between the data frame and the frame, and then obtains the position of the frame relative to the initial time through the accumulation of the position increment. Meanwhile, the SLAM algorithm can perform global adjustment by utilizing multiple overlapping in data and a closed loop in a track to obtain the globally optimal track and feature map data. When the robot enters a completely unfamiliar area, the SLAM draws a feature map of the area while estimating the position of the robot. When the robot enters an area which has been traveled before, the SLAM calculates the current position of the robot itself by matching the current data with the feature map data, and corrects the estimation result.
The UWB technology is a novel wireless communication technology, the frequency of the UWB technology is between 3.1 GHz and 10.6GHz, and the UWB technology is characterized by being used for 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, high ranging accuracy and the like. The application of UWB technology in the ranging and positioning fields has two advantages: 1) centimeter-level or even higher ranging accuracy can be obtained theoretically, and the method has great potential in accurate positioning application; 2) due to the high time resolution, UWB has strong penetration capability, and can still complete ranging and positioning in a complex indoor environment. UWB ranging is an estimation based on time of arrival (TOA) or two-way delay ranging to measure the distance between a UWB mobile station to a base station. The two-way delay ranging is that the mobile station sends a request ranging pulse to the base station, 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 of the sending time and the receiving time after receiving the response pulse, so that the accurate time synchronization between the mobile station and the base station is not needed.
The UWB technology requires the establishment of a base station and a mobile station, and has the advantages of insensitivity to channel fading, low power spectral density of transmitted signals, low interception capability, low system complexity and the like. Theoretically, centimeter-level or even higher distance measurement accuracy can be obtained. For example, a sub-centimeter level accuracy is obtained in the range of 2.5 meters, and an accuracy of 0.1 to 0.15 meters is obtained in the range of 50 meters. However, for such long linear targets as subways, very many base stations need to be arranged to realize the UWB signal coverage of the whole road, and the installation cost and the maintenance cost are very high.
One set of RFID system is composed of RFID label and RFID reader. The RFID positioning mode is divided into two modes, wherein one mode is that a tag moves and a reader is fixed, and the other mode is that the reader moves and the tag is fixed. The basic process of mobile tag mode RFID location is: the coordinates of the readers are known, the RFID tag is communicated with the readers, then the distance between the RFID tag and each reader is calculated by adopting methods such as received signal strength detection, signal arrival direction or signal arrival time, and the like, and finally the position of the tag is calculated by the control center. The positioning mode of the mobile reader is similar to the positioning mode of the mobile tag in process, except that in the mode, calibration is fixed, coordinates are known, the reader is carried on a carrier, and the position is to be measured.
The above method is not suitable for positioning in such a long linear space as a subway tunnel. Therefore, in order to ensure the precision of the underground space movement measurement system, a low-cost, dynamic and high-precision positioning and attitude determination method suitable for underground space movement three-dimensional measurement is urgently needed.
Thus, there is a need for improvements and enhancements to the prior art.
Disclosure of Invention
In view of the above-mentioned shortcomings in the prior art, the present invention provides a method and a system for positioning and attitude determination of a mobile object, which can realize high-precision positioning and attitude determination of the mobile object in an environment without satellite navigation signals.
In order to achieve the purpose, the invention adopts the following technical scheme:
a method for positioning and determining the posture of a moving object 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.
In the method for positioning and determining the posture of a moving object, after the step E, the method further includes 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.
In the method for positioning and determining the attitude of the moving object, the step a specifically includes 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.
In the method for positioning and determining the attitude of the moving object, the measurement control point is a preset passive reflection target.
A positioning and attitude determination system for a moving object comprises the 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.
In the positioning and attitude determination system for a moving object, the system further includes:
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.
In the positioning and attitude determination system for the moving object, the inertial extended kalman filter is further configured to obtain an error correction amount, a speed error correction amount and an attitude error correction amount of the inertial measurement unit according to the position increment residual error output by the inertial filter, and obtain a position error correction amount according to the speed error correction vector and the filtering period; 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.
In the positioning and attitude determination system for the moving object, the measurement control point is a preset passive reflection target.
Compared with the prior art, the positioning and attitude determination method and system for the moving object provided by the invention construct a unified extended Kalman filtering model integrating the laser radar control target data, the inertial measurement unit data and the odometer data through the inertial measurement unit, the odometer and the laser radar. The model is established on the basis of an inertial measurement unit dynamic model and an error model, error state vectors of an IMU/odometer combination are calculated by bringing laser radar control target data into a Kalman filtering equation, and error divergence is limited, so that high-precision positions and postures are obtained. Therefore, the high-precision positioning and attitude determination of the moving object under the environment without satellite navigation signals are realized.
Drawings
Fig. 1 is a structural diagram of a cart in the positioning and attitude determination method for a moving object according to the present invention.
Fig. 2 is a flowchart of a positioning and attitude determination method for a moving object according to the present invention.
FIG. 3 is a block diagram of a LiDAR/IMU/odometer combined algorithm in the positioning and attitude determination method for a moving object according to the present invention.
Fig. 4 is a schematic diagram of a reflection target in the positioning and attitude determination method for a moving object according to the present invention.
Fig. 5 is a schematic diagram of a variance curve of the IMU/odometer combined position in the method for positioning and determining the attitude of a moving object according to the present invention.
FIG. 6 is a flow chart of a prior art IMU/odometer combination.
FIG. 7 is a schematic diagram of a LiDAR/IMU/odometer location variance curve in the positioning and attitude determination method for a moving object according to the present invention.
Fig. 8 is a block diagram of a positioning and attitude determination system for a moving object according to the present invention.
Detailed Description
The invention provides a method and a system for positioning and determining the attitude of a moving object. In order to make the objects, technical solutions and effects of the present invention clearer and clearer, the present invention is further described in detail below with reference to the accompanying drawings and examples. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.
Referring to fig. 1, the present invention provides a method for positioning and determining the posture of a moving object, which positions and determines the posture of the moving object 10 through an inertial measurement unit (i.e., IMU, not shown), a odometer 30 and a LiDAR (LiDAR)20, which are disposed inside the moving object 10. The laser radar 20 is installed on the moving object 10 and is operated to scan a measurement control point set in a scene. The three-dimensional coordinates of the measurement control points themselves are used to provide an absolute positioning of the moving object 10. The inertial measurement unit is disposed inside the moving object 10, receives information from the odometer 30, and performs relative positioning and attitude determination on the moving object 10 together with the odometer 30. The odometer 30 is mounted on the wheels of the moving object 10 to record the linear travel distance of the car. The moving object 10 may be a vehicle such as an automobile or a train, or other moving objects with wheels, but the invention is not limited thereto, and in this embodiment, the moving object is a trolley shown in fig. 1.
The use environment of the invention is as follows: the method is characterized in that the underground or indoor environment without satellite navigation signals exists, and the existing high-precision control point network or the newly established high-precision control point network exists in the scene. The high-precision control point network comprises a plurality of preset measurement control points. The inertial measurement unit, the odometer 30 and the laser radar 20 measure and acquire data under the time reference established by the high-precision time synchronizer, and the high-precision position and posture of the moving object are acquired by the positioning and posture determining method (algorithm) of the moving object provided by the invention.
Referring to fig. 2 and fig. 3, the positioning and attitude determination method for a moving object according to the present invention specifically includes the following steps:
and S10, filtering the data measured by the inertia measurement unit and the odometer by an inertia filter to obtain the initial position, the initial speed and the initial attitude of the moving object.
S20, 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. The measuring control point is a preset passive reflection target. In this embodiment, the shape of the reflection target is as shown in fig. 4, which is a square or rectangle, and is divided into four triangular regions by two diagonal lines, wherein two opposite triangular regions are dark (black) and the other two opposite triangular regions are light (white). The reflection target is low in price and easy to extract from point cloud, and the coordinates of the central point are measured by a measuring robot in advance. To facilitate the measurement, the scanning angle and direction of the lidar may be adjusted according to the specific scenario.
And S30, calculating the difference between the actual coordinate known by the measurement control point and the initial measurement coordinate to obtain a coordinate residual error.
S40, calculating the coordinate residual errors through extended Kalman filtering, and calculating the errors of the inertial measurement unit, 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; feeding back the error of the inertial measurement unit into an inertial filter. Wherein the errors of the inertial measurement unit include accelerometer zero offset and gyroscope drift.
And S50, correcting the initial position, the initial speed and the initial posture 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 posture to obtain the position, the speed and the posture of the moving object.
Therefore, the unified extended Kalman filtering model integrating the measurement data of the inertial measurement unit, the odometer and the laser radar is constructed. The model is established on the basis of an inertial measurement unit dynamic model and an error model, error state vectors of an IMU/odometer combination are calculated by bringing laser radar control target data into a Kalman filtering equation, and error divergence is limited, so that high-precision positions and postures are obtained. Therefore, the high-precision positioning and attitude determination of the moving object under the environment without satellite navigation signals are realized.
Further, after the step S50, the method further includes a step S60: 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. The filtering information includes: a gain matrix, a state transition matrix, an observation noise covariance matrix, and a system noise covariance matrix.
In the prior art, a moving object may be positioned and oriented by an Inertial Measurement Unit (IMU)/odometer combined model, but errors of the moving object may be accumulated over time, and the specific analysis is as follows:
both IMU estimation and odometer estimation are cumulative processes in which errors accumulate. Two major factors that determine the rate of IMU error accumulation are the zero-bias stability of the accelerometer and the drift of the gyroscope, respectively. The high-precision IMU is expensive, generally in the millions of yuan, the price of the medium-precision IMU is dozens of thousands yuan, and the price of the lower-precision IMU is also tens of thousands yuan. Although the price of the existing micromechanical gyroscope is low and varies from several elements to tens of thousands of elements, the accelerometer has poor zero-offset stability and large gyroscope drift, and is not suitable for the field of high-precision measurement. The high-precision odometer can reach the precision of 0.2m/1000m, is far superior to the precision of calculating mileage by a medium-high precision IMU, and has low price of only thousands of yuan. Therefore, the combined positioning and attitude determination under the GNSS-free environment by using the high-precision odometer and the medium and high-precision IMU is undoubtedly a scheme which is economical and can ensure the precision.
Specifically, in the present invention, the IMU/odometer combination model is described first. In other words, the step S10 specifically includes:
defining the axial direction of the odometer body coordinate system to be consistent with the axial direction of the carrier coordinate system and the IMU body coordinate system (b system), converting the odometer output odometer increment delta D into the coordinate increment delta D in the b systembComprises the following steps:
in the formula 4.1, θ z and θ x are respectively the azimuth angle correction amount and the pitch angle correction amount of the odometer body coordinate system relative to the b system, and k is the odometer scale factor correction amount. In the case of an IMU/odometer combination, it is necessary to add a three-dimensional odometer error state vector Δ S ═ δ k, δ θ x, δ θ z to the IMU error state vector]T(the superscript "T" denotes transpose). Thus, the combined model (i.e., the combined model of the IMU and the odometer) state variable is
Wherein, Δ P1Is the initial three-dimensional position error correction vector (i.e., the error correction amount for the initial position, which is the northeast coordinate system), Δ V1Is an error correction vector (error correction amount of initial velocity), Δ O, of the initial velocity1Is an initial attitude error correction vector (error correction amount of initial attitude), Δ a1For the initial accelerometer zero offset correction vector,the amount after conversion of the initial accelerometer zero offset correction vector into b-series, Δ e1The vector is corrected for the gyroscope drift,the corresponding is the amount after the gyroscope drift correction vector is converted into b series, and deltaS is the odometer error state vector. The odometer error state vector is generally considered to be a constant, i.e.:
here, the belt origin variable represents the differential of the original variable with respect to time. Taking δ k as an example, δ k is the differential of δ k with respect to time.
the system state equation for the IMU/odometer combination is X ' ═ F ' X ' + ξ (equation 4.4).
The coefficient matrix F' is
Wherein F is a state transition matrix in an inertial system phi angle system error model, and 0m×nRepresenting a zero matrix of m rows by n columns, i.e. 03×3And 03×15A zero matrix of 3 rows and 3 columns and a zero matrix of 3 rows and 15 columns, respectively. Position increment residual model of observation model: the observed quantity z is: z ═ Δ PI-ΔPS(equation 4.6);
wherein, Δ PIPosition increment, Δ P, deduced for IMU within a filtering periodSThe position increment calculated for utilizing IMU attitude and mileage.
The observation equation is: z ═ Δ V1δt-ΔPSx M Δ S + ξ (equation 4.7);
delta t is the filtering period, M is the coefficient matrix of the error vector of the odometer parameters, xi is the equivalent observation noise, and the concrete form of M is
In the formula 4.8, the first step,sampling interval [ n-1, n ] for one odometer]The increment of the inner odometer in the direction of the X axis is b,is the increment in the Y-axis direction; in the formula 4.9, the first step,is a rotation matrix from b to n of the navigation coordinate system.
taking a subvector X ' obtained by removing the position error vector from the state vector X ' as the state vector when the IMU/odometer set is used (corresponding rows and columns also need to be removed in an F ' matrix, and the subvector is marked as F '), wherein the observation equation is Z ═ HX ' + ξ (formula 4.10);
ξ is observation noise, and the observation matrix H is H ═ I3×3δt，-ΔPs×，03×6，M](equation 4.11);
wherein, I3×3Representing a third order identity matrix, it is noted that the filter period deltat is chosen. If δ t is too small, the observation noise is amplified, and if the period is too long, the speed error cannot be regarded as constant. Therefore, the filter period δ t may be between 0.2s and 2s, and a filter period of 0.5 s or 1 s is generally preferred.
Updating Kalman filtering process time:
X″k+1|k＝Γ″kX″k(equation 4.12);
Γ"ki + F "Δ t (equation 4.13);
and (3) updating the state:
X″k+1＝X″k+1|k+Xk+1(Zk-HX″k+1|k) (equation 4.15);
gain matrix: kk+1＝Dk+1|kHT(HPk+1|kHT+R)-1(equation 4.16);
covariance matrix: d ″)k+1＝(I-H＝Kk+1H)D″k+1|k(equation 4.17);
in the formula, Δ t is an IMU update period, R is an observation noise covariance matrix, and Q is an equivalent system noise covariance matrix. After the IMU velocity error correction is obtained, the position error correction is calculated using equation 4.19:
ΔP1＝ΔV1δ t (equation 4.18).
Note that in the kalman state update, the position error variance does not participate in the update. A model for the calculation of the position error variance at the IMU/odometer combination is further considered below. In the position recursion process, the position error is modeled as a first order Markov process, modeled as follows:
ΔPi+1＝Γp[ΔPi，ΔVi]T+ξp(equation 4.19);
Di+1＝ΓpD[ΔPi，ΔVi]Γp T+Qp(equation 4.20).
Wherein, gamma ispis a matrix of coefficients, ξpFor system model noise, QpIs the system noise variance matrix. At the filtering timing, the position error correction amount calculated by equation (4.19) is fed back to the inertial filter and then set to zero. During the recursion process, the position error is kept to be zero, and the position error variance is increased exponentially. Consider the following three aspects of an IMU/odometer combination: 1) after filtering correction, the uncertainty of the position error is reduced, and the variance is reduced at the moment; 2)the uncertainty of the positioning error of the IMU/odometer combined system can increase along with time; 3) the main effect of the position variance is to accurately distribute the position error correction to other times during the inverse smoothing. Therefore, at the moment of updating the filter, the invention designs a position variance function which grows smoothly along with time but the growth speed is far less than that of an exponential function (a logarithmic model is used for calculating the variance of the position error); in other words, the inertial filter of the present invention calculates the variance of the position error through the logarithmic model at the filtering update time, and the specific formula is as follows:
east position error covariance matrix
North position error covariance matrix
Elevation error covariance matrix
Wherein,is the base value of the variance in the east direction,is the variance base value in the north direction,is the elevation variance base value. Thus, the IMU/odometer combined positioning variance overall trend will be as shown in fig. 5: within a filtering period deltat, the position variance rapidly increases along with the time, and at the odometer data correction moment, the position error is uncertain and reduced, the variance is reduced, but is still larger than the last filtering moment, and the general trend is still increased.
Finally, the overall flow of the IMU/odometer combination is shown in fig. 6, that is, the step S10 specifically includes:
and S110, carrying out pure inertia calculation on the data measured by an Inertia Measurement Unit (IMU) to obtain an inertia position, a speed and an attitude.
And S120, converting the inertial position and attitude data to obtain a rotation matrix, and converting the odometer increment into a dead reckoning position increment.
S130, 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 quantity (accelerometer zero offset and gyroscope drift) of an inertial measurement unit, a speed error correction quantity and an attitude error correction quantity, and correcting the error correction quantity according to a speed error correction vector delta V1And the filter period deltat to obtain the position error correction quantity delta P1That is, the position error correction amount Δ P is calculated by the equation 4.181。
And S140, feeding error correction values of the inertia measurement unit back to an inertia filter (a pure inertia filter), and adding the position, speed and attitude error correction values to the inertia position, speed and attitude to obtain an initial filtering position, speed and attitude (namely, obtaining the initial filtering position, initial filtering speed and initial filtering attitude).
The invention provides a LiDAR/IMU/odometer combination model. Without the aid of a lidar, the rate of error accumulation can increase over time. Over time, the error may diverge beyond the measurement requirements. In order to obtain a long-term high-precision track, external high-precision position information needs to be introduced to correct the accumulated error. The introduced absolute position accuracy will therefore determine the absolute accuracy of the whole positioning and attitude determination system. According to the method for positioning and determining the attitude of the moving object, the LiDAR control targets (measurement control points) are arranged in the measurement field of the underground space, then the LiDAR control targets are observed by using a laser radar, and a residual observation value between an observation value and a true value is brought into a Kalman filtering variance, so that the accumulated error of an IMU/odometer system is corrected.
The step S20 specifically includes: according to the formula 4.22, the distance and angle measured by the laser radar and the initial filtering position and initial filtering attitude data calculated by the IMU/odometer combined positioning and attitude determination can be fused to obtain initial laser point cloud.
Here, superscripts g, b, 1 denote the geospatial, IMU and laser scanning coordinate systems, X, respectivelygIs the coordinate vector of the LiDAR point in the coordinate of the geospatial coordinate system, TgIs a coordinate vector of the navigation center in the geospatial coordinate system,is a rotation matrix from an IMU coordinate system to a geographic space coordinate system, and is calculated by the IMU to obtain a posture o. XlIs a coordinate vector of the laser scanning observed value in a laser coordinate system,is the translation vector from the laser scanner coordinate system to the IMU coordinate system.The attitude angle is calculated from a laser scanner, an IMU coordinate system and a rotation matrix according to the calibrated space synchronization attitude angle.
The step S30 specifically includes: measuring high-precision coordinates of the target by other high-precision measuring means, and measuring by XLExpressing, using the value as a true value, and controlling the high-precision coordinate X of the target by the laser radarLIt is the actual coordinates known to the measurement control point. XLObserved quantity X of corresponding target in initial laser point cloudgSubtracting to obtain coordinate residual observed quantity delta XG，
I.e. Δ XG＝XL-XgEquation (4.23).
The step S40 specifically includes: on one hand, after the odometer is corrected, the attitude precision of the IMU/odometer combined calculation is considered to be higher; on the other hand, when the control target center point is extracted from the point cloud, there may be a small deviation, and the influence of the attitude error may be masked by the influence of the control point extraction error. Thus, the control point residual observations will be primarily due to positioning errors. Thus, we obtain Δ XGPositioning and attitude determination error delta P of IMU/odometer2，Δo2Relation Δ X therebetweenG＝ΔP2+ ξ (equation 4.24).
Let the observed value Z2＝ΔXGThe system state vector X is the IMU error state vector, i.e.:
the observation equation is then:
z2＝H2X+ξ
H2＝[I3×3，03×12](equation 4.26);
the state equation is X ═ FX + ξ (equation 4.27).
The noise of the control point residual error observed value mainly comes from the control point coordinate error and the extraction error when the control point is extracted from the point cloud. The control point error and the extraction error respectively obey Gaussian distributionAnd Gaussian distributionThe observed value noise is distributed asIn the practice ofIn industry, the coordinate accuracy of a control point can be controlled to be 0.02m by using a measuring robot, the extraction accuracy is controlled to be 0.01m by using visualization software, and therefore the equation of the noise of the control point is 0.0005m2。
The step S50 specifically includes: the LiDAR/IMU filtering process is similar to equations 4.12-4.15. If the LiDAR/IMU filtering time intervals are unequal, after the control point is adopted for correction, the odometer calculated amount is cleared, and the filtering period of the IMU/odometer is restarted. Otherwise, a wrong amount of speed correction occurs, causing filter disruption. FIG. 7 is a graphical representation of a position variance curve for a LiDAR/IMU/odometer combination: during a LiDAR/IMU/odometer filtering cycle, the position variance increases over time, and at the time of filtering, the accumulated position error is corrected, with a consequent decrease in position variance.
The step S60 specifically includes: whether the IMU/odometer combination or the LiDAR/IMU/odometer combination, the position, velocity and attitude at the filter corrections can jump, causing them to be discontinuous in time. For real-time positioning navigation, such jitter can be neglected. For mobile mapping, these jumps need to be eliminated to ensure temporal continuity of the trace, i.e. the trace needs to be smoothed. Another purpose of smoothing is to utilize filtering.
Whether the IMU/odometer combination or the LiDAR/IMU/odometer combination, as depicted at step S60, the position, velocity and attitude at the filter corrections may jump, causing them to be discontinuous in time. For real-time positioning navigation, such jitter can be neglected. For mobile mapping, these jumps need to be eliminated to ensure temporal continuity of the trace, i.e. the trace needs to be smoothed. Another purpose of the smoothing is to use the filtered information to calculate position, velocity and attitude error corrections at other times, thereby improving overall trajectory accuracy. Common smoothing algorithms include forward and backward filtering and fixed-lag interval backward smoothing. The fixed interval inverse smoothing R-T-S algorithm is adopted, and the mathematical model is as follows:
DkS＝Dk+KkS(DkS-Dk+1|k)(KkS)T(equation 4.29);
wherein,for the smooth correction amount at time k,for filtering correction, KkSSmoothing gain matrix, DkSIs the smoothed covariance matrix.
Therefore, the unified extended Kalman filtering model fusing the LiDAR control target data, the IMU data and the odometer data is constructed. The model is established on the basis of an IMU dynamic model and an error model, and by bringing LiDAR control target data into a Kalman filtering equation, an error state vector of an IMU/odometer combination is calculated, and divergence of inertial errors is limited, so that the high-precision corrected position and posture of a carrier are obtained.
The invention solves the absolute positioning and attitude determination in the all underground/indoor environment and the environment without satellite positioning signals. Absolute coordinates are introduced through the control points, and positioning of GNSS signals is not needed.
The invention can provide high-precision positioning and attitude-determining results in large-range and large-span indoor/underground places, which cannot be compared with other traditional methods. The algorithm provided by the invention can provide 50mm absolute positioning accuracy and 2mm/10m relative positioning accuracy by combining with corresponding sensors.
The high-precision position and posture result provided by the invention can be used for high-precision movement measurement, which cannot be compared with other traditional methods. The high-precision positioning and attitude determining result can be used for a mobile measurement system to perform high-precision measurement in the industries of tracks, traffic and the like. This is not possible with conventional methods.
The method of the invention does not need to establish a base station and rely on electric wave signals, thus having relatively flexible and convenient use and low cost. The control point relied upon may be existing or rearranged, and is a passive, non-signaling and receiving static flag. Therefore, the device is easy to realize, free from power connection and interference of electromagnetic signals, and flexible and convenient to use.
The invention solves the high-precision positioning and attitude determination in the environment without satellite positioning signals and provides necessary conditions for high-precision measurement in corresponding industries.
Based on the positioning and attitude determination method for the moving object provided by the embodiment, the invention also provides a positioning and attitude determination system for the moving object. Referring to fig. 8, the system includes: the moving object, the inertial measurement unit 40, the odometer 30 and the laser radar 20 as described above, the inertial measurement unit 40, the odometer 30 and the laser radar 20 being all disposed within the moving object; the laser radar 20 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: inertial filter 50, coordinate residual calculation module 60, inertial extended kalman filter 70, and RTS smoothing filter.
The inertial filter 50 is configured to perform inertial filtering on the data measured by the inertial measurement unit 40 and the odometer 30 to obtain an initial position, an initial speed, and an initial attitude of the mobile object.
The coordinate residual error calculation module 60 is configured to fuse the ranging angle measurement data measured by the laser radar 20 with the initial position and the initial attitude to obtain an initial laser point cloud, and extract an initial measurement coordinate of the measurement control point from the initial laser point cloud; and 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 measuring control point is a preset passive reflection target.
The inertial extended kalman filter 70 is configured to calculate the coordinate residual through extended kalman filtering, and calculate an error of the inertial measurement unit, an error correction amount of the initial position, an error correction amount of the initial speed, and an error correction amount of the 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.
The RTS smoothing filter 80 is configured to perform smoothing filtering on the position, speed, and posture of the moving object and the filtering information to obtain a smoothed position, speed, and posture.
The inertial extended kalman filter 70 is further configured to obtain an error correction amount, a speed error correction amount, and an attitude error correction amount of the inertial measurement unit according to the position increment residual error output by the inertial filter 50, and obtain a position error correction amount according to the speed error correction vector and the filtering cycle; feeding back the error correction of the inertial measurement unit to the inertial filter 50;
the inertial filter 50 is specifically configured to perform 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 amount of the inertial measurement unit fed back from the inertial extended kalman filter 70; 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.
Since the positioning and attitude determination principle and the technical features of the positioning and attitude determination system for the moving object have been described in detail in the above embodiments, they are not described herein again.
It should be understood that equivalents and modifications of the technical solution and inventive concept thereof may occur to those skilled in the art, and all such modifications and alterations should fall within the scope of the appended claims.
Claims (6)
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. correcting the initial position, the initial speed and the initial attitude through the error correction of the initial position, the error correction of the initial speed and the error correction of the initial attitude to obtain the position, the speed and the attitude of the moving object;
the step A specifically comprises the following steps:
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 the speed error correction amount 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.
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 of claim 1, wherein the measurement control point is a predetermined passive reflection target.
4. 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; correcting the initial position, the initial speed and the initial attitude through the error correction of the initial position, the error correction of the initial speed and the error correction of the initial attitude to obtain the position, the speed and the attitude of the moving object;
the inertial extended Kalman filter is further used for obtaining an error correction quantity of the inertial measurement unit, a speed error correction quantity and an attitude error correction quantity according to the position increment residual error output by the inertial filter, and obtaining a position error correction quantity according to the speed error correction quantity and a filtering period; 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.
5. 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.
6. The system of claim 4, wherein the measurement control points are predetermined passive reflective targets.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610126054.1A CN105628026B (en) | 2016-03-04 | 2016-03-04 | A kind of positioning and orientation method and system of mobile object |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610126054.1A CN105628026B (en) | 2016-03-04 | 2016-03-04 | A kind of positioning and orientation method and system of mobile object |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN105628026A CN105628026A (en) | 2016-06-01 |
| CN105628026B true CN105628026B (en) | 2018-09-14 |
Family
ID=56043176
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201610126054.1A Active CN105628026B (en) | 2016-03-04 | 2016-03-04 | A kind of positioning and orientation method and system of mobile object |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN105628026B (en) |
Families Citing this family (58)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106123890A (en) * | 2016-06-14 | 2016-11-16 | 中国科学院合肥物质科学研究院 | A kind of robot localization method of Fusion |
| CN106197407B (en) * | 2016-06-23 | 2018-12-18 | 长沙学院 | A kind of subway localization method and system based on inertial sensor |
| CN107563255B (en) * | 2016-06-30 | 2020-09-29 | 北京合众思壮科技股份有限公司 | Filtering method and device for inertial measurement unit |
| CN106403998B (en) * | 2016-08-30 | 2019-05-03 | 北京云迹科技有限公司 | Violence countermeasure set and method based on IMU |
| CN108225345A (en) * | 2016-12-22 | 2018-06-29 | 乐视汽车（北京）有限公司 | The pose of movable equipment determines method, environmental modeling method and device |
| CN211236238U (en) | 2017-03-29 | 2020-08-11 | 深圳市大疆创新科技有限公司 | Light detection and ranging (LIDAR) system and unmanned vehicle |
| CN110199204A (en) | 2017-03-29 | 2019-09-03 | 深圳市大疆创新科技有限公司 | Laser radar sensor system with small form factor |
| WO2018176291A1 (en) | 2017-03-29 | 2018-10-04 | SZ DJI Technology Co., Ltd. | Hollow motor apparatuses and associated systems and methods |
| WO2018195986A1 (en) | 2017-04-28 | 2018-11-01 | SZ DJI Technology Co., Ltd. | Calibration of laser sensors |
| CN110573928B (en) | 2017-04-28 | 2021-10-08 | 深圳市大疆创新科技有限公司 | Angular calibration in light detection and ranging systems |
| WO2018195999A1 (en) | 2017-04-28 | 2018-11-01 | SZ DJI Technology Co., Ltd. | Calibration of laser and vision sensors |
| JP6891375B2 (en) | 2017-07-20 | 2021-06-18 | エスゼット ディージェイアイ テクノロジー カンパニー リミテッドＳｚ Ｄｊｉ Ｔｅｃｈｎｏｌｏｇｙ Ｃｏ．，Ｌｔｄ | Systems and methods for measuring optical distance |
| CN107368073A (en) * | 2017-07-27 | 2017-11-21 | 上海工程技术大学 | A kind of full ambient engine Multi-information acquisition intelligent detecting robot system |
| CN110914703A (en) | 2017-07-31 | 2020-03-24 | 深圳市大疆创新科技有限公司 | Correction of motion-based inaccuracies in point clouds |
| CN107562054A (en) * | 2017-08-31 | 2018-01-09 | 深圳波比机器人科技有限公司 | The independent navigation robot of view-based access control model, RFID, IMU and odometer |
| WO2019041269A1 (en) | 2017-08-31 | 2019-03-07 | SZ DJI Technology Co., Ltd. | Delay time calibration of optical distance measurement devices, and associated systems and methods |
| CN107861507A (en) * | 2017-10-13 | 2018-03-30 | 上海斐讯数据通信技术有限公司 | A kind of AGV control methods and system based on inertial navigation correction and SLAM indoor positionings |
| CN107797129B (en) * | 2017-10-13 | 2020-06-05 | 重庆市勘测院 | Point cloud data acquisition method and device under no GNSS signal |
| CN108225302B (en) * | 2017-12-27 | 2020-03-17 | 中国矿业大学 | Petrochemical plant inspection robot positioning system and method |
| CN108345005B (en) * | 2018-02-22 | 2020-02-07 | 重庆大学 | Real-time continuous autonomous positioning and orienting system and navigation positioning method of tunnel boring machine |
| CN108680159A (en) * | 2018-04-03 | 2018-10-19 | 中科微至智能制造科技江苏有限公司 | A kind of robot localization method based on data fusion |
| CN108657222B (en) * | 2018-05-03 | 2019-06-07 | 西南交通大学 | Railroad track gauge and horizontal parameters measurement method based on vehicle-mounted Lidar point cloud |
| CN109269499B (en) * | 2018-09-07 | 2022-06-17 | 东南大学 | Target joint networking positioning method based on relative navigation |
| CN109490825B (en) * | 2018-11-20 | 2021-02-09 | 武汉万集信息技术有限公司 | Positioning navigation method, device, equipment, system and storage medium |
| CN111380514A (en) * | 2018-12-29 | 2020-07-07 | 深圳市优必选科技有限公司 | Robot position and posture estimation method and device, terminal and computer storage medium |
| CN109541535A (en) * | 2019-01-11 | 2019-03-29 | 浙江智澜科技有限公司 | A method of AGV indoor positioning and navigation based on UWB and vision SLAM |
| CN109870706A (en) * | 2019-01-31 | 2019-06-11 | 深兰科技（上海）有限公司 | A kind of detection method of road surface identification, device, equipment and medium |
| CN110111356B (en) * | 2019-03-21 | 2021-05-28 | 西安交通大学 | Method for calculating rotating shaft direction and rotating angular velocity of moving rotating object |
| CN109900296A (en) * | 2019-03-22 | 2019-06-18 | 华南农业大学 | A kind of agricultural machinery working travel speed detection system and detection method |
| CN110017850B (en) * | 2019-04-19 | 2021-04-20 | 小狗电器互联网科技（北京）股份有限公司 | Gyroscope drift estimation method and device and positioning system |
| CN109917436B (en) * | 2019-04-28 | 2020-09-29 | 中国人民解放军国防科技大学 | Satellite/inertia combined real-time precise relative motion datum positioning method |
| CN110220474B (en) * | 2019-04-30 | 2021-05-18 | 浙江华东工程安全技术有限公司 | Post attitude angle correction method for mobile laser scanning system |
| JP7274366B2 (en) * | 2019-06-28 | 2023-05-16 | 日産自動車株式会社 | Self-position estimation method and self-position estimation device |
| CN110749327B (en) * | 2019-08-08 | 2023-06-09 | 南京航空航天大学 | Vehicle navigation method in cooperative environment |
| CN110657799B (en) * | 2019-09-26 | 2022-09-09 | 广东省海洋工程装备技术研究所 | Space real-time positioning method, computer device and computer readable storage medium |
| CN112572462B (en) | 2019-09-30 | 2022-09-20 | 阿波罗智能技术(北京)有限公司 | Automatic driving control method and device, electronic equipment and storage medium |
| CN112945266B (en) * | 2019-12-10 | 2024-07-19 | 炬星科技（深圳）有限公司 | Laser navigation robot and odometer calibration method thereof |
| CN111060059A (en) * | 2019-12-30 | 2020-04-24 | 武汉武船计量试验有限公司 | Total station three-dimensional measurement method under dynamic condition |
| CN111025366B (en) * | 2019-12-31 | 2022-04-01 | 芜湖哈特机器人产业技术研究院有限公司 | Grid SLAM navigation system and method based on INS and GNSS |
| CN111207740A (en) * | 2020-01-13 | 2020-05-29 | 北京京东乾石科技有限公司 | Method, device, equipment and computer readable medium for positioning vehicle |
| CN111366153B (en) * | 2020-03-19 | 2022-03-15 | 浙江大学 | Positioning method for tight coupling of laser radar and IMU |
| CN111595354B (en) * | 2020-05-27 | 2022-12-20 | 东南大学 | EKF-SLAM algorithm of self-adaptive dynamic observation domain |
| CN111637888B (en) * | 2020-06-15 | 2021-06-15 | 中南大学 | Tunneling machine positioning method and system based on inertial navigation and laser radar single-point distance measurement |
| CN111721298A (en) * | 2020-06-24 | 2020-09-29 | 重庆赛迪奇智人工智能科技有限公司 | SLAM outdoor large scene accurate positioning method |
| CN113932820A (en) * | 2020-06-29 | 2022-01-14 | 杭州海康威视数字技术股份有限公司 | Object detection method and device |
| CN112014849B (en) * | 2020-07-15 | 2023-10-20 | 广东工业大学 | Unmanned vehicle positioning correction method based on sensor information fusion |
| CN112084458B (en) * | 2020-08-07 | 2024-08-09 | 深圳市瑞立视多媒体科技有限公司 | Rigid body posture tracking method and device, equipment and storage medium thereof |
| CN112051569B (en) * | 2020-09-10 | 2024-04-05 | 北京经纬恒润科技股份有限公司 | Radar target tracking speed correction method and device |
| CN112162305B (en) * | 2020-09-27 | 2021-07-02 | 中铁电气化局集团有限公司 | Fusion positioning method and system for rail transit |
| CN112539747B (en) * | 2020-11-23 | 2023-04-28 | 华中科技大学 | Pedestrian dead reckoning method and system based on inertial sensor and radar |
| CN112461236B (en) * | 2020-11-23 | 2022-10-04 | 中国人民解放军火箭军工程大学 | Vehicle-mounted high-precision fault-tolerant integrated navigation method and system |
| CN112595325B (en) * | 2020-12-21 | 2024-08-13 | 武汉汉宁轨道交通技术有限公司 | Initial position determining method, device, electronic equipment and storage medium |
| CN112729321A (en) * | 2020-12-28 | 2021-04-30 | 上海有个机器人有限公司 | Robot map scanning method and device, storage medium and robot |
| CN112781586B (en) * | 2020-12-29 | 2022-11-04 | 上海商汤临港智能科技有限公司 | Pose data determination method and device, electronic equipment and vehicle |
| CN113267178B (en) * | 2021-03-25 | 2023-07-07 | 浙江大学 | Model pose measurement system and method based on multi-sensor fusion |
| CN113311411B (en) * | 2021-04-19 | 2022-07-12 | 杭州视熵科技有限公司 | Laser radar point cloud motion distortion correction method for mobile robot |
| CN113885046B (en) * | 2021-09-26 | 2024-06-18 | 天津大学 | Intelligent network-connected automobile laser radar positioning system and method for low-texture garage |
| CN113639722B (en) * | 2021-10-18 | 2022-02-18 | 深圳大学 | Continuous laser scanning registration auxiliary inertial positioning and attitude determination method |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103591955A (en) * | 2013-11-21 | 2014-02-19 | 西安中科光电精密工程有限公司 | Combined navigation system |
| CN105203551A (en) * | 2015-09-11 | 2015-12-30 | 尹栋 | Car-mounted laser radar tunnel detection system, autonomous positioning method based on tunnel detection system and tunnel hazard detection method |
| CN105279371A (en) * | 2015-09-21 | 2016-01-27 | 武汉海达数云技术有限公司 | Control point based method for improving POS precision of mobile measurement system |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US8589072B2 (en) * | 2011-04-13 | 2013-11-19 | Honeywell International, Inc. | Optimal combination of satellite navigation system data and inertial data |
-
2016
- 2016-03-04 CN CN201610126054.1A patent/CN105628026B/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103591955A (en) * | 2013-11-21 | 2014-02-19 | 西安中科光电精密工程有限公司 | Combined navigation system |
| CN105203551A (en) * | 2015-09-11 | 2015-12-30 | 尹栋 | Car-mounted laser radar tunnel detection system, autonomous positioning method based on tunnel detection system and tunnel hazard detection method |
| CN105279371A (en) * | 2015-09-21 | 2016-01-27 | 武汉海达数云技术有限公司 | Control point based method for improving POS precision of mobile measurement system |
Non-Patent Citations (2)
| Title |
|---|
| "A Sensor-Fusion Drivable-Region and Lane-Detection System for Autonomous Vehicle Navigation in Challenging Road Scenarios";Qingquan Li;《IEEE TRANSACTIONS ON VEHICULAR TECHNOLOGY》;20140228;第63卷(第2期);正文第540-553页 * |
| "激光雷达测量技术及其应用研究";李清泉等;《武汉测绘科技大学学报》;20001031;第2卷(第5期);正文第387-391页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN105628026A (en) | 2016-06-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN105628026B (en) | A kind of positioning and orientation method and system of mobile object | |
| Fan et al. | Data fusion for indoor mobile robot positioning based on tightly coupled INS/UWB | |
| CN110702091B (en) | High-precision positioning method for moving robot along subway rail | |
| Li et al. | Simultaneous registration and fusion of multiple dissimilar sensors for cooperative driving | |
| EP3617749B1 (en) | Method and arrangement for sourcing of location information, generating and updating maps representing the location | |
| CN102901977B (en) | Method for determining initial attitude angle of aircraft | |
| CN111426320B (en) | Vehicle autonomous navigation method based on image matching/inertial navigation/milemeter | |
| CN109708632B (en) | Laser radar/INS/landmark-pine combined navigation system and method for mobile robot | |
| CN109782289B (en) | Underwater vehicle positioning method based on baseline geometric structure constraint | |
| Ibrahim et al. | Inertial measurement unit based indoor localization for construction applications | |
| Meguro et al. | Low-cost lane-level positioning in urban area using optimized long time series GNSS and IMU data | |
| Sun et al. | Motion model-assisted GNSS/MEMS-IMU integrated navigation system for land vehicle | |
| CN104316058A (en) | Interacting multiple model adopted WSN-INS combined navigation method for mobile robot | |
| CN113419265B (en) | Positioning method and device based on multi-sensor fusion and electronic equipment | |
| US20220136832A1 (en) | Method and system for magnetic-based collaborative positioning | |
| Zhang et al. | Carrier-phase-based initial heading alignment for land vehicular MEMS GNSS/INS navigation system | |
| CN113063441B (en) | Data source correction method and device for accumulated calculation error of odometer | |
| Li et al. | The IMU/UWB/odometer fusion positioning algorithm based on EKF | |
| Lategahn et al. | Robust pedestrian localization in indoor environments with an IMU aided TDoA system | |
| CN108332749B (en) | Indoor dynamic tracking and positioning method | |
| Cahyadi et al. | Unscented Kalman filter for a low-cost GNSS/IMU-based mobile mapping application under demanding conditions | |
| Yao et al. | Tightly coupled indoor positioning using UWB/mmWave radar/IMU | |
| Yu et al. | Indoor mobile robot positioning based on uwb and low cost imu | |
| Chen et al. | Modeling and Assessment on The Tightly Coupled Integration of TWTOA-Based UWB and INS | |
| Fernández et al. | Evaluating a LKF simulation tool for collaborative navigation systems |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |