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
- mobile object
- error
- attitude
- 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
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 present invention relates to technical field of inertial, more particularly to the positioning and orientation method and system of a kind of mobile object.
Background technology
In all kinds of traverse measurement systems, positioning and orientation is all one of core missions.The positioning used in traverse measurement is fixed
Appearance system (POS) is generally composed of Global Navigation Satellite System (GNSS) and inertial navigation system (INS), is traverse measurement
System and its various mission payloads mounted provide position, speed and attitude reference.Based on Global Navigation Satellite System and it is used to
Property navigation system combination positioning and orientation technology have the advantage that:It is estimated that inertial navigation system using high-precision GNSS information
The gyroscopic drift of system (INS) and accelerometer bias equal error parameter, to inhibit the accumulation of its error at any time;Utilize INS
High sampling rate and in short-term high-precision feature can provide auxiliary information for GNSS, so as to GNSS receiver clock correction etc.
The estimation of the margin of error is more accurate, and makes GNSS receiver that can keep lower tracking bandwidth, improves it and captures satellite again
The ability of signal.
The long-term absolute precision of POS depends on GNSS.Since GNSS precision is affected by environment larger so that POS long
Phase precision is also affected by environment.Actual environment complicated and changeable brings several broad aspects to mobile mapping system (MMS) application
Challenge:In the poor environment of GNSS signal, such as built-up city and the serious waters of multipath effect, POS's is absolute
Positional precision will decline to a great extent to decimeter grade or even meter level, have been unable to meet high precision movement measurement request at this time;In nothing
In the underground space of GNSS signal, the simple positioning and orientation error calculated by INS can rapid divergence at any time, when by one section
Between after, by beyond measurement error tolerate the upper limit.Therefore, in the underground space of no GNSS signal, the error hair of INS how is limited
It dissipates, keeps higher traverse measurement precision, be another challenge that MMS applications face.
It includes that robot measurement positions to be traditionally used for underground space localization method mainly, immediately positioning and plotting method
(SLAM), ultra wide band location method (UWB) and radio frequency recognition positiming method (RFID).
Robot measurement is also known as automatic total instruments, is a kind of collection automatic target detection, sights automatically, automatic angle measurement and survey
Away from, Automatic Target Following, automatically record in the measuring table of one.Robot measurement energy Automatic-searching and alignment target, then
Auto-measuring.Robot measurement high certainty of measurement, but find target and more take, it is difficult to realize high dynamic consecutive tracking.
Immediately positioning and plotting method (SLAM) calculate the position of itself using sensors such as camera or laser radars, and
It builds the map of zone of ignorance or updates the map of known region.According to used sensor difference, SLAM algorithms can be by
It is divided into stereoscopic vision, monocular vision and depth camera and based on radar scanning sensor etc..SLAM includes mainly that data are closed
Four connection, map match, closed-loop detection and global adjustment steps.SLAM algorithms pass through overlay region between data frame and frame first
The matching in domain obtains positional increment of this frame relative to previous frame, then obtains this frame relative to first by the way that positional increment is cumulative
The position at moment beginning.SLAM algorithms can carry out global adjustment using the close ring in the multiple overlapping and track in data simultaneously,
Obtain track and the characteristics map data of global optimum.When robot enters a complete strange region, SLAM can be on one side
Calculate that self-position draws out the characteristics map in this region on one side.And the area run over before robot enters one
When domain, SLAM can be corrected by the current self-position of matching primitives of current data and characteristics map data and calculate result.
UWB technology is a kind of novel wireless communication technique, and between 3.1~10.6GHz, unique property exists frequency
In that can be used to communicate, precision distance measurement can be used for.UWB has, transmitting power spectrum density insensitive to channel fading
The advantages that low, low interception capability, system complexity be low and high range accuracy.Application of the UWB technology in ranging and positioning field
With both sides advantage:1) the even higher range accuracy of Centimeter Level can be obtained in theory, in being accurately positioned application
Great potential;2) due to temporal resolution height, UWB has stronger penetration capacity, it is made to have been remained in complex indoor environment
At ranging and positioning.UWB rangings are to estimate to arrive to measure UWB movement stations based on arrival time (TOA) or two-way time-delay distance finding
The distance between base station.TOA telemetrys according to base station signal reach movement station used in (being multiplied by light velocity c calculates movement station the time
To the distance of base station, therefore the time of movement station and base station needs precise synchronization.Two-way time-delay distance finding is then movement station to base station
Request ranging burst is sent, base station returns to one transponder pulse of transmission after receiving the request pulse, and movement station receives transponder pulse
Estimated at a distance from movement station and base station according to the time delays of delivery time and the time of reception afterwards, therefore between movement station and base station
Without the precise synchronization that takes time.
UWB technology needs to establish base station and movement station, and UWB is with insensitive to channel fading, transmitting power spectrum signal is close
Spend the advantages such as low, low interception capability, system complexity be low.The even higher range accuracy of Centimeter Level can theoretically be obtained.Example
If someone obtains the precision of inferior centimeter order in the range of 2.5 meters, 0.1~0.15 meter of precision is reached in the range of 50m.But
It for this kind of long linear goal of subway, to realize that system-wide section UWB signal covers, need to lay very more base stations, installation
Cost and maintenance cost are all very high.
Illustrative RFID system is made of RFID tag and RFID reader.RFID positioning methods are divided into two kinds, and one is marks
Label movement, reader are fixed, and another kind is reader movement, and label is fixed.The basic mistake of the RFID positioning of mobile tag mode
Cheng Shi:Reader coordinate is it is known that RFID tag is communicated with the foundation of multiple readers, then using received signal strength detection or letter
The methods of number arrival direction or time of arrival (toa) calculate RFID to distance between each reader, are finally calculated by control centre
To the position of label.Mobile reader positioning method is similar with mobile tag positioning method process, except that in this side
In formula, calibration fix, coordinate it is known that and reader is mounted on carrier, position is to be measured.
The above method is not particularly suited for the positioning in this long linear space of subway tunnel.Therefore, in order to ensure underground sky
Between traverse measurement system precision, there is an urgent need to it is a kind of suitable for underground space moving three dimension measure lower cost, dynamic, height
Precision positioning and orientation method.
Therefore, existing technology could be improved and improve.
Invention content
Place in view of above-mentioned deficiencies of the prior art, the purpose of the present invention is to provide a kind of positioning and orientations of mobile object
Method and system is realized in the environment of no satellite navigation signals and determines appearance to the high accuracy positioning of mobile object.
In order to achieve the above object, this invention takes following technical schemes:
A kind of positioning and orientation method of mobile object, the method pass through the inertia measurement list that is arranged in mobile object
Member, odometer and laser radar to the mobile object position and determine appearance, specifically comprise the following steps:
A, the data that Inertial Measurement Unit and odometer measure obtain the first of mobile object after the filtering of digital filter device
Beginning position, initial velocity and initial attitude;
B, laser radar is scanned to measuring control point, the distance and angle that laser radar is measured and the initial bit
It sets, after initial attitude fusion, obtains initial laser point cloud, the initial measurement at extraction measurement control point is sat from initial laser point cloud
Mark;
C, actual coordinate known to the measurement control point and the initial measurement coordinate are asked poor, obtains coordinate residual error;
D, the coordinate residual error is calculated by Extended Kalman filter, calculates the error, initial of Inertial Measurement Unit
The calibration corrections of the calibration corrections of position, the calibration corrections of initial velocity and initial attitude;By the inertia measurement list
The error of member is fed back in digital filter device;
E, pass through the error correction of the calibration corrections of initial position, the calibration corrections of initial velocity and initial attitude
Amount, is modified the initial position, initial velocity, initial attitude, obtains position, speed and the posture of mobile object.
Further include step after the step E in the positioning and orientation method of the mobile object:F, to mobile object
Position, speed, posture and filtering information carry out smothing filtering, obtain smooth position, speed and posture.
In the positioning and orientation method of the mobile object, the step A specifically includes step:
A1, pure inertia reckoning is carried out to the data that Inertial Measurement Unit measures, obtains inertial position, speed and posture;
A2, inertial position and attitude data are converted to spin matrix, and odometer increment, which is switched to dead-reckoning position, to be increased
Amount;
A3, inertial position increment and dead-reckoning position increment ask poor, obtain positional increment residual error, are brought into inertia extension
In Kalman filter, calibration corrections and velocity error correction amount and the attitude error amendment of Inertial Measurement Unit are obtained
Amount corrects vector sum filtering cycle according to velocity error and obtains site error correction amount;
A4, the calibration corrections of Inertial Measurement Unit are fed back in digital filter device, and position, speed and attitude error
Correction amount is then added with inertial position, speed and posture, obtains initial filter position, speed and posture.
It is described to measure the reflection mark that control point is preset passive type in the positioning and orientation method of the mobile object
Target.
A kind of positioning and orientation system of mobile object, the system comprises mobile object, Inertial Measurement Unit, odometer and
Laser radar, the Inertial Measurement Unit, odometer and laser radar are arranged in mobile object;The laser radar is used for
It is scanned to measuring control point, obtains measuring control point at a distance from mobile object and angle;The system also includes:
Digital filter device, the data for measuring Inertial Measurement Unit and odometer carry out digital filter, are moved
Initial position, initial velocity and the initial attitude of object;
Coordinate residual computations module, it is ranging angle measurement data for measuring laser radar and the initial position, initial
After posture fusion, initial laser point cloud is obtained, the initial measurement coordinate at extraction measurement control point from initial laser point cloud;By institute
It states actual coordinate known to measurement control point and asks poor with the initial measurement coordinate, obtain coordinate residual error;
Inertia extended Kalman filter is calculated for calculating the coordinate residual error by Extended Kalman filter
The error of the error of Inertial Measurement Unit, the calibration corrections of initial position, the calibration corrections of initial velocity and initial attitude
Correction amount;The error of the Inertial Measurement Unit is fed back in digital filter device;By the calibration corrections of initial position, just
The calibration corrections of beginning speed and the calibration corrections of initial attitude carry out the initial position, initial velocity, initial attitude
It corrects, obtains position, speed and the posture of mobile object.
In the positioning and orientation system of the mobile object, the system also includes:
RTS smoothing filters, for carrying out smothing filtering to the position of mobile object, speed, posture and filtering information,
Obtain smooth position, speed and posture.
In the positioning and orientation system of the mobile object, the inertia extended Kalman filter is additionally operable to according to used
Property filter output positional increment residual error, obtain Inertial Measurement Unit calibration corrections and velocity error correction amount and
Attitude error correction amount corrects vector sum filtering cycle according to velocity error and obtains site error correction amount;By inertia measurement list
The calibration corrections of member are fed back in digital filter device;
The digital filter implement body is used to carry out pure inertia reckoning to the data that Inertial Measurement Unit measures, and obtains inertia
Position, speed and posture;Inertial position and attitude data are converted to spin matrix, odometer increment is switched into dead reckoning
Positional increment;Inertial position increment asks poor with dead-reckoning position increment, obtains positional increment residual error, and the positional increment is residual
Inertia extended Kalman filter is given in difference output;According to the error of the Inertial Measurement Unit of inertia extended Kalman filter feedback
Correction amount;Position, speed and attitude error correction amount are added with inertial position, speed and posture, obtain initial filter position,
Speed and posture.
It is described to measure the reflection mark that control point is preset passive type in the positioning and orientation system of the mobile object
Target.
Compared to the prior art, the positioning and orientation method and system of a kind of mobile object provided by the invention, passes through inertia
Measuring unit, odometer and laser radar build unification, fusion laser radar control target data, Inertial Measurement Unit number
According to this and the Extended Kalman filter model that counts of mileage.The model foundation is in Inertial Measurement Unit kinetic model and error
On the basis of model, by the way that laser radar control target data are brought into Kalman filter equation, IMU/ odometer groups are calculated
The error state vector of conjunction limits the diverging of its error, to obtain high precision position and posture.It is led in no satellite to realize
It navigates in the environment of signal, appearance is determined to the high accuracy positioning of mobile object.
Description of the drawings
Fig. 1 is the structure chart of trolley in the positioning and orientation method of mobile object provided by the invention.
Fig. 2 is the flow chart of the positioning and orientation method of mobile object provided by the invention.
Fig. 3 is LiDAR/IMU/ odometer combinational algorithm frames in the positioning and orientation method of mobile object provided by the invention
Figure.
Fig. 4 is the schematic diagram of instrument reflection target in the positioning and orientation method of mobile object provided by the invention.
Fig. 5 is in the positioning and orientation method of mobile object provided by the invention, and IMU/ odometers combine position variance curve
Schematic diagram.
Fig. 6 is existing IMU/ odometers combination flow chart.
Fig. 7 is in the positioning and orientation method of mobile object provided by the invention, and LiDAR/IMU/ odometers position variance is bent
Line schematic diagram.
Fig. 8 is the structure diagram of the positioning and orientation system of mobile object provided by the invention.
Specific implementation mode
The present invention provides a kind of positioning and orientation method and system of mobile object.To make the purpose of the present invention, technical solution
And effect is clearer, clear, the present invention is described in more detail for the embodiment that develops simultaneously referring to the drawings.It should be appreciated that this
Place is described, and specific examples are only used to explain the present invention, is not intended to limit the present invention.
Referring to Fig. 1, the present invention provides a kind of positioning and orientation method of mobile object, the method is by being arranged in movement
20 pairs of Inertial Measurement Unit (i.e. IMU is not shown in figure), odometer 30 and laser radar (LiDAR) shiftings in object 10
Animal body 10 carries out positioning and determining appearance.The laser radar 20 is mounted on mobile object 10, and scan setting is in scene when work
Interior measurement control point.The three-dimensional coordinate for measuring control point itself is used for being supplied to 10 absolute fix of mobile object.The inertia
Measuring unit is arranged inside the mobile object 10, receives the information of odometer 30, with odometer 30 together to mobile object
10 progress relative positionings determine appearance.The odometer 30 is mounted on the wheel of the mobile object 10, for recording the line of trolley
Property operating range.The mobile object 10 can be the vehicles such as automobile, train, can also be the movement that other carry wheel
Object, the present invention are not construed as limiting, and are trolley shown in FIG. 1 in the present embodiment.
The present invention use environment be:Underground is indoor without satellite navigation signals environment, has ready-made high-precision in scene
Net of control points or newly-established high-precision control point net.The high-precision control point net includes multiple measurement controls pre-set
Point processed.The Inertial Measurement Unit, odometer 30 and laser radar 20 are under precise synchronization device institute settling time benchmark
It measures and obtains data, and the positioning and orientation method (algorithm) of the mobile object provided through the invention obtains mobile object
High precision position and posture.
Fig. 2 and Fig. 3 are please referred to, the positioning and orientation method of mobile object provided by the invention specifically comprises the following steps:
The data that S10, Inertial Measurement Unit and odometer measure obtain mobile object after the filtering of digital filter device
Initial position, initial velocity and initial attitude.
S20, laser radar are scanned to measuring control point, the distance and angle that laser radar is measured with it is described initial
After position, initial attitude fusion, initial laser point cloud is obtained, the initial measurement at extraction measurement control point from initial laser point cloud
Coordinate.It is described to measure the instrument reflection target that control point is preset passive type.In the present embodiment, the shape of the instrument reflection target is as schemed
Shown in 4, it is rectangular or rectangle, is divided into four delta-shaped regions by two diagonal lines, wherein two opposite deltas
Domain is dark (black), other two opposite delta-shaped region is light (white).The instrument reflection target is cheap, is easy
It is extracted from cloud, center point coordinate is measured using robot measurement in advance.For the ease of measuring, according to specific scene,
Scanning angle and the direction of laser radar can be adjusted.
S30, actual coordinate known to the measurement control point and the initial measurement coordinate are asked poor, it is residual obtains coordinate
Difference.
S40, the coordinate residual error is calculated by Extended Kalman filter, calculate Inertial Measurement Unit error, just
The calibration corrections of the calibration corrections of beginning position, the calibration corrections of initial velocity and initial attitude;By the inertia measurement
The error of unit is fed back in digital filter device.Wherein, the error of the Inertial Measurement Unit includes accelerometer bias and top
Spiral shell instrument drifts about.
S50, pass through the error correction of the calibration corrections of initial position, the calibration corrections of initial velocity and initial attitude
Amount, is modified the initial position, initial velocity, initial attitude, obtains position, speed and the posture of mobile object.
It follows that the present invention by Inertial Measurement Unit, odometer and laser radar, build it is unified, merged three
The Extended Kalman filter model of the measurement data of person.The model foundation is in Inertial Measurement Unit kinetic model and error model
On the basis of, by the way that laser radar control target data are brought into Kalman filter equation, calculate the combination of IMU/ odometers
Error state vector limits the diverging of its error, to obtain high precision position and posture.Believe in no satellite navigation to realize
In the environment of number, appearance is determined to the high accuracy positioning of mobile object.
Further, further include step S60 after the step S50:To the position of mobile object, speed, posture and
Filtering information carries out smothing filtering, obtains smooth position, speed and posture.The filtering information includes:Gain matrix, state
Transfer matrix, observing matrix, observation noise covariance matrix and system noise covariance matrix.
In the prior art, mobile object can be determined by Inertial Measurement Unit (IMU)/odometer built-up pattern
Appearance is determined in position, but its error can be accumulated with the accumulation of time, and concrete analysis is as follows:
Either IMU is calculated or odometer calculates, is all an accumulative process, and in the process, error also can be with
Accumulation.Determine that two principal elements of imu error accumulative speed are the bias instaility and gyroscope of accelerometer respectively
Drift.High-precision IMU is expensive, generally in members up to a million, in the price of high-precision IMU be hundreds thousand of members, lower accuracy
IMU prices also be tens of thousands of members.Although the micro-mechanical gyroscope price occurred at present is relatively low, from several members to tens of thousands of members etc.,
Its accelerometer bias stability is poor, and gyroscopic drift is big, is not suitable for field of high-precision measurement.High-precision odometer can reach
The precision of 0.2m/1000m is much better than the precision that middle high-precision IMU calculates mileage, and its price is relatively low, only needs thousands of members.Cause
This, using high precision odometer and in high-precision IMU carry out determine appearance without the integrated positioning under GNSS environment, undoubtedly one kind had been both
The economic scheme that can guarantee precision again.
Specifically, in the present invention, describing the built-up pattern of IMU/ odometers first.In other words, the step S10 tools
Body includes:
Define odometer body coordinate system and carrier coordinate system and IMU bodies coordinate system (b systems) axially retains consistent, then mileage
The mileage increment Delta D of meter output switchs to the increment of coordinate Δ D in b systemsbFor:
In formula 4.1, θ z and θ x are respectively that odometer body coordinate system is repaiied relative to the azimuth correction and pitch angle of b systems
Positive quantity, k are odometer calibration factor correction amount.When IMU/ odometers combine, need on the basis of imu error state vector
Increase three-dimensional odometer error state vector Δ S=[δ k, δ θ x, δ θ z]T(subscript " T " indicates transposition).Therefore, built-up pattern
(that is, built-up pattern of IMU and odometer) state variable is
Wherein, Δ P1It is that initial three-dimensional position Correction of Errors is vectorial (that is, the calibration corrections of initial position, are east northeast
Ground coordinate system), Δ V1Vectorial (calibration corrections of initial velocity), the Δ O for the Correction of Errors of initial velocity1It is missed for initial attitude
Difference correction is vectorial (calibration corrections of initial attitude), Δ a1Vector is corrected for initial acceleration meter zero bias,For initial acceleration
Meter zero bias correction vector is converted to the amount after b systems, Δ e1Vector is corrected for gyroscopic drift,Correspond to gyroscopic drift
Correction vector is converted to the amount after b systems, and Δ S is odometer error state vector.It is generally acknowledged that odometer error state vector is
Constant, i.e.,:
Here, the differential of former variables versus time is indicated with origin variable.By taking δ k as an example, δ k are exactly δ k time differentials.
Then the system state equation of IMU/ odometers combination is:X '=F ' X '+ξ (formula 4.4).
Coefficient matrix F ' is
Wherein, F be the angles inertia system Φ SYSTEM ERROR MODEL in state-transition matrix, 0m×nIndicate that m rows multiply n row zero moments
Battle array, that is, 03×3With 03×15The null matrix of the null matrix and 3 rows 15 row of respectively 3 rows 3 row.The positional increment residual error mould of observation model
Type:Observed quantity z is:Z=Δs PI-ΔPS(formula 4.6);
Wherein, Δ PIFor the positional increment that IMU in a filtering cycle is extrapolated, Δ PSTo utilize IMU postures and odometer
The positional increment extrapolated.
Observational equation is:Z=Δs V1δt-ΔPS× M Δ S+ ξ (formula 4.7);
δ t are filtering cycle, and M is the coefficient matrix of odometer parameter error vector, and ξ is equivalent observation noise.M's is specific
Form is
In formula 4.8,For increment of the interior odometer of odometer sampling interval [n-1, n] in b systems X-direction,For the increment in Y direction;In formula 4.9,For b systems to the spin matrix of navigational coordinate system n systems.
Here state when subvector X " after taking state vector X ' to remove position error vector is IMU/ odometer groups to
It measures (being also required to remove corresponding row and column in F ' matrixes, be denoted as F "), then observational equation is:Z=HX "+ξ (formula 4.10);
ξ is observation noise, and observing matrix H is:H=[I3×3δ t ,-Δ Ps ×, 03×6, M] and (formula 4.11);
Wherein, I3×3Indicate three rank unit matrixs, it should be noted that the selection of filtering cycle δ t.If δ t are too small, observation
Noise can be amplified, and if excessive cycle, velocity error is not to be regarded as constant.Therefore, filtering cycle δ t can be between
For 0.2s between 2s, the general filtering cycle for using 0.5 second or 1 second is preferable.
The Kalman filtering process time updates:
X″k+1|k=Γ "kX″k(formula 4.12);
Γ"k=I+F " Δs t (formula 4.13);
State updates:
X″k+1=X "k+1|k+Xk+1(Zk-HX″k+1|k) (formula 4.15);
Gain matrix:Kk+1=Dk+1|kHT(HPk+1|kHT+R)-1(formula 4.16);
Covariance matrix:D″k+1=(I-H=Kk+1H)D″k+1|k(formula 4.17);
In formula, Δ t is the IMU update cycles, and R is observation noise covariance matrix, and Q is equivalent system noise covariance square
Battle array.After obtaining the velocity error correction amount of IMU, using 4.19 calculating position calibration corrections of formula:
ΔP1=Δ V1δ t (formula 4.18).
It notices in Kalman state update, site error variance does not participate in updating.Consider further below
The computation model of site error variance when IMU/ odometers combine.In the recursive process of position, site error is modeled as single order
Markoff process, model are as follows:
ΔPi+1=Γp[ΔPi, Δ Vi]T+ξp(formula 4.19);
Di+1=ΓpD[Δ Pi, Δ Vi]Γp T+Qp(formula 4.20).
Wherein, ΓpFor coefficient matrix, ξpFor system model noise, QpFor system noise variance matrix.In filtered time instant, by
The site error correction amount that formula (4.19) calculates is fed back to digital filter device, is then zeroed out.In recursive process, position is missed
Difference remains zero, and site error variance can be exponentially continuously increased.Consider following three aspects of IMU/ odometers combination:1) it filters
After wave is corrected, site error uncertainty reduces, and variance should reduce at this time;2) IMU/ odometers combined system position error is not
Certainty can increase with the time;3) accurately by site error reduction when the main function of position variance is reversed smooth
It is assigned to other moment.Therefore, in filtering renewable time, the present invention devises a kind of growth smooth at any time, but growth rate
Much smaller than the position variance function of exponential function (a kind of logarithmic model carrys out the variance of calculating position error);In other words, this hair
Bright digital filter device is in filtering renewable time, and by logarithmic model come the variance of calculating position error, specific formula is as follows:
East is to site error covariance matrix
The north is to site error covariance matrix
Vertical error covariance matrix
Wherein,For eastern direction variance base value,For northern direction variance base value,For elevation variance base value.Therefore,
IMU/ odometer integrated positioning variance overall trends will be as shown in Figure 5:In a filtering cycle δ t, position variance is with the time
Rapid growth, at the odometer data correction moment, site error is uncertain to be reduced, and variance also decreases, but still than last filter
The wave moment is big, both generally still shows a rising trend.
Finally, the overall flow of IMU/ odometers combination is as shown in Figure 6, that is, the step S10 is specifically included:
S110, the data measured to Inertial Measurement Unit (IMU) carry out pure inertia reckoning, obtain inertial position, speed and
Posture.
S120, inertial position and attitude data are converted to spin matrix, and odometer increment is switched to dead-reckoning position
Increment.
S130, inertial position increment and dead-reckoning position increment ask poor, obtain positional increment residual error, are brought into inertia expansion
Open up Kalman filter in, obtain Inertial Measurement Unit calibration corrections (accelerometer bias and gyroscopic drift) and
Velocity error correction amount and attitude error correction amount correct vectorial Δ V according to velocity error1Position is obtained with filtering cycle δ t to miss
Poor correction amount P1, that is, site error correction amount P is calculated using formula 4.181。
S140, the calibration corrections of Inertial Measurement Unit are fed back in digital filter device (pure digital filter device), and position
Set, speed and attitude error correction amount are then added with inertial position, speed and posture, obtain initial filter position, speed and appearance
State (that is, obtaining initial filter position, initial filter speed and initial filter posture).
And it is provided by the invention, it is LiDAR/IMU/ odometer built-up patterns.If the not auxiliary of laser radar, accidentally
Poor accumulative speed can increase at any time.After a period of time, error may be diffused to more than measurement request.It is long-term in order to obtain
High-precision track, needs to introduce external high precision position information and is modified to accumulated error.Therefore, introduced absolute position
Precision will determine the absolute precision of entire positioning and orientation system.The positioning and orientation method of mobile object provided by the invention, passes through
LiDAR controls target (measuring control point) is laid in the measurement place of the underground space, then utilizes laser radar to these
LiDAR control targets are observed, and the residual error observation between observation and true value is brought into Kalman filtering variance, from
And correct the accumulated error of IMU/ mileage systems.
The step S20 is specifically included:It, can be by the distance of lidar measurement and angle and IMU/ according to formula 4.22
Odometer integrated positioning determines the calculated initial filter position of appearance and merges to obtain initial laser point cloud with initial filter attitude data.
Here, subscript g, b, 1 indicates geospatial coordinates system, IMU coordinate systems and laser scanning coordinate system, X respectivelygIt is ground
Manage the coordinate vector of LiDAR point under space coordinates coordinate, TgIt is the coordinate vector of geospatial coordinates Xi Xia navigation center,The spin matrix from IMU coordinate systems to geospatial coordinates system, by IMU calculate posture o is calculated.XlIt is sharp
Coordinate vector of the optical scanning observation under laser coordinate system,It is from laser scanner coordinate system to IMU coordinate systems
Translation vector.It is from laser scanner to IMU coordinate systems to spin matrix, by spatial synchronization attitude angle meter obtained by calibrating
It obtains.
The step S30 is specifically included:Laser radar control target high-precision is measured using other high-acruracy survey means
Coordinate uses XLIt indicates, using the value as actual value, laser radar controls target high-precision coordinate XLIt exactly measures known to control point
Actual coordinate.XLTarget observed quantity X is corresponded to initial laser point cloudgSubtract each other to obtain coordinate residual error observed quantity Δ XG,
That is Δ XG=XL-XgFormula (4.23).
The step S40 is specifically included:On the one hand, it is contemplated that after odometer is corrected, the combination of IMU/ odometers resolves
Attitude accuracy it is higher;On the other hand, from cloud when extraction control Target Center point, there may be smaller deviation, and
The influence of attitude error may be covered by the influence of Control point extraction error.Therefore, control point residual error observation will mainly by
It is caused in position error.To which we obtain Δ XGWith IMU/ odometer positioning and orientation error deltas P2, Δ o2Between relationship delta
XG=Δ P2+ ξ (formula 4.24).
Enable observation Z2=Δ XG, system mode vector X is imu error state vector, i.e.,:
Then observational equation is:
z2=H2X+ξ
H2=[I3×3, 03×12] (formula 4.26);
State equation is:X=FX+ ξ (formula 4.27).
When the noise of control point residual error observation mostlys come from control point error of coordinate and extracts control point from cloud
Extraction error.It controls point tolerance and extraction error distinguishes Gaussian distributedAnd Gaussian Profile
Then observation noise is distributed asIn actual job, we can utilize robot measurement will
Control point coordinate precision control is controlled extraction accuracy in 0.01m in 0.02m, using visual software, therefore, controls spot noise
Equation be 0.0005m2。
The step S50 is specifically included:LiDAR/IMU filterings are similar with formula 4.12~4.15.If LiDAR/
IMU filtering times interval Length discrepancy, after using control point correction, odometer reckoning amount should be reset, and be restarted in IMU/
The filtering cycle of journey meter.Otherwise the speed reduction that will appear mistake causes filtering to upset.Fig. 7 is LiDAR/IMU/ odometers
Position variance curve synoptic diagram when combination:In a LiDAR/IMU/ odometer filtering cycle, position variance is with the time
And increase, in filtered time instant, accumulation site error is corrected, and position variance reduces therewith.
The step S60 is specifically included:The either combination of IMU/ odometers or the combination of LiDAR/IMU/ odometers, is being filtered
Wave Corrections Division position, speed and posture can all beat, and cause their in time discontinuous.For real-time location navigation
For, this bounce can be ignored.And for mobile mapping, these bounces then need to be eliminated, to ensure track
Time continuity needs to be smoothed track.Another smooth purpose is to utilize filtering.
As described in step S60, the either combination of IMU/ odometers or the combination of LiDAR/IMU/ odometers, in filter correction
Place position, speed and posture can all beat, and cause their in time discontinuous.For real-time location navigation,
This bounce can be ignored.And for mobile mapping, these bounces then need to be eliminated, to ensure that the time of track connects
Continuous property, that is, need to be smoothed track.Another smooth purpose is to calculate other moment using filtering information
Position, speed and attitude error reduction, to improve whole path accuracy.Common smoothing algorithm has forward and reverse filtering and consolidates
It is reversely smooth to determine Delay Interval.Here using fixed interval, reversely smooth R-T-S algorithms, mathematical model are:
DkS=Dk+KkS(DkS-Dk+1|k)(KkS)T(formula 4.29);
Wherein,For k moment smooth reduction,For filtering correction amount, KkSFlat gain matrix, DkSAfter smooth
Covariance matrix.
It is counted it follows that the present invention constructs unified fusion LiDAR control targets data, IMU data and mileage
According to Extended Kalman filter model.The model foundation is on the basis of IMU kinetic models and error model, by by LiDAR
Control target data are brought into Kalman filter equation, calculate the error state vector of IMU/ odometers combination, limit inertia
Error dissipates, to obtain carrier with high accuracy correction position and posture.
The present invention solves full underground/indoor environment, determines appearance entirely without the absolute fix in satellite positioning signal environment.It is logical
It crosses control point and introduces absolute coordinate, be not necessarily to the positioning of GNSS signal.
The present invention can provide on a large scale, in large span room/underground in high-precision positioning and orientation posture as a result, this
It is other conventional methods not comparing.Algorithm proposed by the invention combines corresponding sensor, can provide the absolute of 50mm
The positioning precision of positioning accuracy and 2mm/10m.
High precision position provided by the invention and posture result can be used for high-precision traverse measurement, be other conventional methods
It can not compare.High-precision positioning and orientation result can be used for traverse measurement system, carry out the high-precision of the industries such as track, traffic
It measures.This, which is conventional method, to accomplish.
The present invention method without establish base station and rely on electric wave signal, therefore using relatively flexibly, conveniently, it is at low cost.
The control point relied on can be existing or be rearranged, and is a kind of static mark that passive, no signal sends out and receives
Will.Therefore, it is very easy to realize, it is flexible and convenient to use without connecing power supply and not interfered by electromagnetic signal.
The high accuracy positioning that the present invention solves in no satellite positioning signal environment determines appearance, is that the high-precision of corresponding industry is surveyed
Amount provides necessary condition.
Positioning and orientation method based on the mobile object that above-described embodiment provides, the present invention also provides a kind of mobile objects
Positioning and orientation system.Referring to Fig. 8, the system comprises:Mobile object, Inertial Measurement Unit 40, odometer as described above
30 and laser radar 20, the Inertial Measurement Unit 40, odometer 30 and laser radar 20 be arranged in mobile object;It is described
Laser radar 20 is used to be scanned to measuring control point, obtains measuring control point at a distance from mobile object and angle;It is described
System further includes:Digital filter device 50, coordinate residual computations module 60, inertia extended Kalman filter 70 and RTS are smoothly filtered
Wave device.
The digital filter device 50, the data for measuring Inertial Measurement Unit 40 and odometer 30 carry out inertia filter
Wave obtains the initial position, initial velocity and initial attitude of mobile object.
The coordinate residual computations module 60, the ranging angle measurement data for measuring laser radar 20 and the initial bit
It sets, after initial attitude fusion, obtains initial laser point cloud, the initial measurement at extraction measurement control point is sat from initial laser point cloud
Mark;Actual coordinate known to the measurement control point and the initial measurement coordinate are asked poor, obtain coordinate residual error.The measurement
Control point is the instrument reflection target of preset passive type.
The inertia extended Kalman filter 70, for the coordinate residual error to be calculated by Extended Kalman filter,
Calculate the error of Inertial Measurement Unit, the calibration corrections of initial position, initial velocity calibration corrections and initial attitude
Calibration corrections;The error of the Inertial Measurement Unit is fed back in digital filter device;It is repaiied by the error of initial position
The calibration corrections of positive quantity, the calibration corrections of initial velocity and initial attitude, to the initial position, initial velocity, initial
Posture is modified, and obtains position, speed and the posture of mobile object.
The RTS smoothing filters 80, for being put down to the position of mobile object, speed, posture and filtering information
Sliding filtering, obtains smooth position, speed and posture.
The inertia extended Kalman filter 70 is additionally operable to the positional increment residual error exported according to digital filter device 50,
The calibration corrections and velocity error correction amount and attitude error correction amount of Inertial Measurement Unit are obtained, according to velocity error
Correction vector sum filtering cycle obtains site error correction amount;The calibration corrections of Inertial Measurement Unit are fed back into digital filter
In device 50;
The digital filter device 50 is specifically used for carrying out pure inertia reckoning to the data that Inertial Measurement Unit measures, and is used to
Property position, speed and posture;Inertial position and attitude data are converted to spin matrix, odometer increment, which is switched to boat position, to be pushed away
Calculate positional increment;Inertial position increment asks poor with dead-reckoning position increment, positional increment residual error is obtained, by the positional increment
Residual error, which exports, gives inertia extended Kalman filter;According to the Inertial Measurement Unit of the feedback of inertia extended Kalman filter 70
Calibration corrections;Position, speed and attitude error correction amount are added with inertial position, speed and posture, obtain initial filter
Position, speed and posture.
Since the positioning and orientation principle and technical characteristic of the positioning and orientation system of the mobile object are in above-described embodiment
In elaborated, details are not described herein.
It, can according to the technique and scheme of the present invention and its hair it is understood that for those of ordinary skills
Bright design is subject to equivalent substitution or change, and all these changes or replacement should all belong to the guarantor of appended claims of the invention
Protect range.
Claims (6)
1. a kind of positioning and orientation method of mobile object, which is characterized in that the method is used by what is be arranged in mobile object
Property measuring unit, odometer and laser radar to the mobile object carry out position and determine appearance, specifically comprise the following steps:
A, the data that Inertial Measurement Unit and odometer measure obtain the initial bit of mobile object after the filtering of digital filter device
It sets, initial velocity and initial attitude;
B, laser radar is scanned to measuring control point, the distance and angle that laser radar is measured and the initial position,
After initial attitude fusion, initial laser point cloud is obtained, the initial measurement coordinate at extraction measurement control point from initial laser point cloud;
C, actual coordinate known to the measurement control point and the initial measurement coordinate are asked poor, obtains coordinate residual error;
D, the coordinate residual error is calculated by Extended Kalman filter, calculates error, the initial position of Inertial Measurement Unit
Calibration corrections, the calibration corrections of initial velocity and the calibration corrections of initial attitude;By the Inertial Measurement Unit
Error is fed back in digital filter device;
E, right by the calibration corrections of initial position, the calibration corrections of the calibration corrections of initial velocity and initial attitude
The initial position, initial velocity, initial attitude are modified, and obtain position, speed and the posture of mobile object;
The step A specifically includes step:
A1, pure inertia reckoning is carried out to the data that Inertial Measurement Unit measures, obtains inertial position, speed and posture;
A2, inertial position and attitude data are converted to spin matrix, and odometer increment is switched to dead-reckoning position increment;
A3, inertial position increment and dead-reckoning position increment ask poor, obtain positional increment residual error, are brought into inertia extension karr
In graceful filter, the calibration corrections and velocity error correction amount and attitude error correction amount of Inertial Measurement Unit, root are obtained
Site error correction amount is obtained according to velocity error correction amount and filtering cycle;
A4, the calibration corrections of Inertial Measurement Unit are fed back in digital filter device, and position, speed and attitude error amendment
Amount is then added with inertial position, speed and posture, obtains initial filter position, speed and posture.
2. the positioning and orientation method of mobile object according to claim 1, which is characterized in that after the step E, also wrap
Include step:F, smothing filtering is carried out to the position of mobile object, speed, posture and filtering information, obtains smooth position, speed
Degree and posture.
3. the positioning and orientation method of mobile object according to claim 1, which is characterized in that the measurement control point is pre-
If passive type instrument reflection target.
4. a kind of positioning and orientation system of mobile object, which is characterized in that the system comprises mobile object, inertia measurement lists
Member, odometer and laser radar, the Inertial Measurement Unit, odometer and laser radar are arranged in mobile object;It is described
Laser radar is used to be scanned to measuring control point, obtains measuring control point at a distance from mobile object and angle;The system
System further includes:
Digital filter device, the data for measuring Inertial Measurement Unit and odometer carry out digital filter, obtain mobile object
Initial position, initial velocity and initial attitude;
Coordinate residual computations module, the ranging angle measurement data for measuring laser radar and the initial position, initial attitude
After fusion, initial laser point cloud is obtained, the initial measurement coordinate at extraction measurement control point from initial laser point cloud;By the survey
Actual coordinate asks poor with the initial measurement coordinate known to amount control point, obtains coordinate residual error;
Inertia extended Kalman filter calculates inertia for calculating the coordinate residual error by Extended Kalman filter
The error correction of the error of measuring unit, the calibration corrections of initial position, the calibration corrections of initial velocity and initial attitude
Amount;The error of the Inertial Measurement Unit is fed back in digital filter device;Pass through the calibration corrections of initial position, initial speed
The calibration corrections of degree and the calibration corrections of initial attitude, repair the initial position, initial velocity, initial attitude
Just, position, speed and the posture of mobile object are obtained;
The inertia extended Kalman filter is additionally operable to the positional increment residual error exported according to digital filter device, obtains inertia
The calibration corrections and velocity error correction amount and attitude error correction amount of measuring unit, according to velocity error correction amount and
Filtering cycle obtains site error correction amount;The calibration corrections of Inertial Measurement Unit are fed back in digital filter device;
The digital filter implement body is used to carry out pure inertia reckoning to the data that Inertial Measurement Unit measures, and obtains inertia position
It sets, speed and posture;Inertial position and attitude data are converted to spin matrix, odometer increment is switched into dead reckoning position
Set increment;Inertial position increment asks poor with dead-reckoning position increment, positional increment residual error is obtained, by the positional increment residual error
It exports and gives inertia extended Kalman filter;It is repaiied according to the error of the Inertial Measurement Unit of inertia extended Kalman filter feedback
Positive quantity;Position, speed and attitude error correction amount are added with inertial position, speed and posture, obtain initial filter position, speed
Degree and posture.
5. the positioning and orientation system of mobile object according to claim 4, which is characterized in that the system also includes:
RTS smoothing filters are obtained for carrying out smothing filtering to the position of mobile object, speed, posture and filtering information
Smooth position, speed and posture.
6. the positioning and orientation system of mobile object according to claim 4, which is characterized in that the measurement control point is pre-
If passive type instrument reflection target.
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 (57)
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 |
EP3602749A4 (en) | 2017-03-29 | 2020-03-25 | SZ DJI Technology Co., Ltd. | Hollow motor apparatuses and associated systems and methods |
EP3602121A4 (en) | 2017-03-29 | 2021-05-12 | SZ DJI Technology Co., Ltd. | Light detection and ranging (lidar) signal processing circuitry |
EP3602122A4 (en) | 2017-03-29 | 2020-03-18 | SZ DJI Technology Co., Ltd. | A lidar sensor system with small form factor |
EP3616159A4 (en) | 2017-04-28 | 2020-05-13 | SZ DJI Technology Co., Ltd. | Calibration of laser sensors |
WO2018195999A1 (en) | 2017-04-28 | 2018-11-01 | SZ DJI Technology Co., Ltd. | Calibration of laser and vision sensors |
WO2018195998A1 (en) | 2017-04-28 | 2018-11-01 | SZ DJI Technology Co., Ltd. | Angle calibration in light detection and ranging system |
EP3455645A4 (en) | 2017-07-20 | 2019-04-24 | SZ DJI Technology Co., Ltd. | Systems and methods for optical distance measurement |
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 |
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 |
CN107562054A (en) * | 2017-08-31 | 2018-01-09 | 深圳波比机器人科技有限公司 | The independent navigation robot of view-based access control model, RFID, IMU and odometer |
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 |
CN112945266A (en) * | 2019-12-10 | 2021-06-11 | 炬星科技(深圳)有限公司 | 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 |
CN112084458A (en) * | 2020-08-07 | 2020-12-15 | 深圳市瑞立视多媒体科技有限公司 | 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 |
CN112595325A (en) * | 2020-12-21 | 2021-04-02 | 武汉汉宁轨道交通技术有限公司 | Initial position determining method and 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 |
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 | |
CN110514225B (en) | External parameter calibration and accurate positioning method for fusion of multiple sensors under mine | |
CN105547305B (en) | A kind of pose calculation method based on wireless location and laser map match | |
US7978128B2 (en) | Land survey system | |
CN101598556B (en) | Unmanned aerial vehicle vision/inertia integrated navigation method in unknown environment | |
CN110702091B (en) | High-precision positioning method for moving robot along subway rail | |
US20020180636A1 (en) | Passive ranging/tracking processing method | |
CN108519615A (en) | Mobile robot autonomous navigation method based on integrated navigation and Feature Points Matching | |
CN107289932B (en) | Single deck tape-recorder Kalman Filtering navigation device and method based on MEMS sensor and VLC positioning fusion | |
CN105928518A (en) | Indoor pedestrian UWB/INS tightly combined navigation system and method adopting pseudo range and position information | |
CN110686671B (en) | Indoor 3D real-time positioning method and device based on multi-sensor information fusion | |
CN108759834A (en) | A kind of localization method based on overall Vision | |
CN108489382A (en) | A kind of AGV dynamic pose measuring methods based on space multi-point constraint | |
CN111426320A (en) | Vehicle autonomous navigation method based on image matching/inertial navigation/milemeter | |
CN105043392A (en) | Aircraft pose determining method and aircraft pose determining device | |
CN110388939A (en) | One kind being based on the matched vehicle-mounted inertial navigation position error modification method of Aerial Images | |
CN109883416A (en) | A kind of localization method and device of the positioning of combination visible light communication and inertial navigation positioning | |
CN101650433B (en) | Method for obtaining self-movement locus of object | |
CN104316058A (en) | Interacting multiple model adopted WSN-INS combined navigation method for mobile robot | |
CN117289322A (en) | High-precision positioning algorithm based on IMU, GPS and UWB | |
CN113405560A (en) | Unified modeling method for vehicle positioning and path planning | |
CN110286371A (en) | Method is determined based on the small feature loss lander relative pose of radar dot array data | |
CN109708667A (en) | A kind of double dynamic target tracking bootstrap techniques based on laser gyro | |
CN108332749B (en) | Indoor dynamic tracking and positioning method | |
Yu et al. | Robot Localization Using Laser Positioning of Reflectors Based on ICP |
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 |