CN113359167A - Method for fusing and positioning GPS and laser radar through inertial measurement parameters - Google Patents

Method for fusing and positioning GPS and laser radar through inertial measurement parameters Download PDF

Info

Publication number
CN113359167A
CN113359167A CN202110422792.1A CN202110422792A CN113359167A CN 113359167 A CN113359167 A CN 113359167A CN 202110422792 A CN202110422792 A CN 202110422792A CN 113359167 A CN113359167 A CN 113359167A
Authority
CN
China
Prior art keywords
positioning
gps
ndt
data
imu
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202110422792.1A
Other languages
Chinese (zh)
Inventor
崔金钟
叶茂
柳箐汶
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
University of Electronic Science and Technology of China
Original Assignee
University of Electronic Science and Technology of China
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by University of Electronic Science and Technology of China filed Critical University of Electronic Science and Technology of China
Priority to CN202110422792.1A priority Critical patent/CN113359167A/en
Publication of CN113359167A publication Critical patent/CN113359167A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/4802—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00 using analysis of echo signal for target characterisation; Target signature; Target cross-section

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Electromagnetism (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

The invention relates to a method for providing more reliable positioning for civil unmanned aerial vehicles (such as a Dajiang unmanned aerial vehicle). The invention combines inertial measurement parameters acquired by IMU, GPS positioning data and laser radar NDT algorithm calculation data to realize high-reliability positioning by utilizing the thought of multi-sensor fusion. The problems that the GPS signal of the unmanned aerial vehicle is weak in some environments, GPS data possibly lost and the unmanned aerial vehicle moves violently, or the laser radar scanning matching fails due to the fact that the scene changes and the three-dimensional map is inconsistent are solved. By the method, the unmanned aerial vehicle can adopt more accurate positioning data under the condition that the unmanned aerial vehicle can obtain the current positioning information. The invention first establishes coordinate association of the GPS and the laser radar to carry out consistent description of positioning. And pre-judging the GPS positioning data and the laser radar NDT algorithm matching data, and preliminarily screening the unreliable positioning data. On the basis, the reliability of the two positioning data is compared by using the inertial measurement parameters acquired by the IMU, and more reliable positioning data is decided. When the data of the GPS and the laser radar are unreliable, the IMU and the positioning data at the last moment are used for estimating the current positioning value, and the reliable positioning of the unmanned aerial vehicle in a special environment is guaranteed.

Description

Method for fusing and positioning GPS and laser radar through inertial measurement parameters
Technical Field
The invention belongs to the field of positioning of unmanned aerial vehicles, and relates to a method for positioning by combining a GPS (global positioning system) and a laser radar based on an IMU (inertial measurement unit).
Background
The unmanned aerial vehicle is an unmanned aerial vehicle operated by utilizing a radio remote control device and a self-contained program control device, and the positioning technology of the unmanned aerial vehicle is the basis of the safety and the operation of the unmanned aerial vehicle. In the actual engineering of unmanned aerial vehicle positioning, a GPS positioning system can be used for acquiring positioning data of an unmanned aerial vehicle; the method comprises the steps of obtaining positioning data of the unmanned aerial vehicle by using a laser radar NDT algorithm, wherein the NDT algorithm is a technology for realizing high-precision positioning by using an existing high-precision map and laser radar real-time measurement data to perform point cloud matching; the rotation angle and the acceleration of each direction of the unmanned aerial vehicle are obtained through an Inertial Measurement Unit (IMU), and the physical quantity measured by the IMU in a small time interval can be considered to be accurate.
When the unmanned aerial vehicle is positioned in a place with shielding above the sky, such as a vehicle with an indoor tunnel and a metal ceiling, a city block erected in a tall building, a forest, even a dense forest and the like, the unmanned aerial vehicle can hardly receive satellite signals, so that the GPS signals are lost, and the GPS positioning at the moment is inaccurate. Meanwhile, although the accuracy of laser radar positioning is high, the use scene needs to be mapped in advance by using a laser radar NDT algorithm, various changes occur after time passes, and deviation may occur between a pre-established map and the actual situation during positioning. Meanwhile, under the environment with similar characteristic values or the current environment is shielded by a sudden obstacle, the NDT algorithm at the moment is difficult to perform characteristic matching, and the abnormal situation of positioning loss occurs. In such cases, the positioning resulting from the NDT algorithm is not trusted. Therefore, in some special situations, the reliability of the single sensor to obtain the positioning cannot be guaranteed.
Disclosure of Invention
The invention aims to solve the problem that the positioning by utilizing a laser radar NDT algorithm and a GPS sensor is possible, and the reliability of physical quantity obtained by an IMU in a short time is utilized, and provides a method for positioning by combining a GPS and the laser radar NDT algorithm by utilizing the IMU.
In order to achieve the above object, the present invention comprises the steps of:
step 1, GPS/NDT coordinate feasibility pre-judgment: obtaining the acceleration a obtained by the IMU, and if a is larger than a given threshold epsilon, considering that the airplane is moving, and the following cases are considered:
and 1, if the position information transmitted by the GPS is not changed, the GPS positioning is considered to be inaccurate.
And 2, if the position information calculated by the NDT is not changed, the NDT is considered to be inaccurate in positioning.
And 3, if the return value of the actual iteration times of the NDT is just the maximum iteration times when the point cloud parameters are established, the NDT is not considered to be accurately matched, and the positioning information obtained by the NDT at the moment is inaccurate.
If the three conditions do not exist, the step 2 is carried out, and data obtained by a GPS and an NDT are fused and decided by IMU data; if the GPS positioning obtained by the rule is inaccurate and the NDT data is tentatively accurate, receiving the NDT positioning coordinate as a return coordinate; in a similar way, if the NDT positioning prejudgment is not accurate and the GPS data prejudgment is accurate, the GPS positioning coordinate is received as a return coordinate; if the obtained GPS and NDT positioning is not accurate, the positioning data returned at the previous moment and the IMU real-time acceleration estimation result are used as the positioning value at the moment, and the steps are as follows:
step 1.1 obtaining last time t0Positioning data of
Figure BDA0003023520010000021
And acceleration about three directions measured in real time by the IMU
Figure BDA0003023520010000022
Step 1.2 according to the formula
Figure BDA0003023520010000023
Estimate by t0Time t1The magnitude of the motion displacement at the moment. Namely:
Figure BDA0003023520010000024
step 1.3 according to
Figure BDA0003023520010000025
T estimated by IMU can be obtained1Location data of the moment. Namely:
Figure BDA0003023520010000026
and 2, data conversion, namely converting the GPS longitude and latitude coordinates acquired by the unmanned aerial vehicle into three-dimensional coordinates under an NDT mapping coordinate system.
And 3, enabling the unmanned aerial vehicle to take off for a distance of about 2m, and calculating surface information and an initial coordinate point of the unmanned aerial vehicle.
And 4, acquiring the GPS positioning coordinate and the laser radar NDT positioning coordinate which are continuously updated in the time interval by taking the delta t as the time interval.
The method mainly comprises the following steps:
step 4.1, at a certain time t0And respectively acquiring a group of GPS positioning coordinates and a group of laser radar NDT positioning coordinates in the later delta t time interval. Are respectively marked as
Figure BDA0003023520010000027
And
Figure BDA0003023520010000028
and 4.2, optimizing the coordinate data, dividing the GPS coordinate data and the NDT coordinate data into two groups with equal number respectively, and using a space linear equation Ax + By + Cz to be 0 for the first groupFitting the set of positioning coordinate data, and obtaining a set A by using a gradient descent method1、B1、C1The distance d from the coordinate point to the straight line equation is minimized. Namely:
Figure BDA0003023520010000029
finding a straight line A1x+B1y+C1Projection equation a of 0-z on xOy plane1x+b1y is 0. Similarly, a fitting equation A of another group of coordinate points is obtained2x+B2y+C2z is 0 and its projection equation a on the xOy plane2x+b2y is 0. Two projection equations are combined, and the intersection point (x, y) of the two projection equations is obtained as the unmanned aerial vehicle at t0At a later time t1The plane coordinates of (a). In this step, can be obtained in (t)0The coordinates after the GPS and NDT optimization at + Δ t) time are:
Figure BDA00030235200100000210
Figure BDA00030235200100000211
and m and k are the coordinate quantities of the GPS and the NDT acquired in the delta t time interval respectively.
Step 5, using two adjacent time t0And t0The GPS and NDT coordinates of + Δ t calculate the average acceleration in three directions within Δ t, respectively. For two adjacent time instants t0And t0Coordinates of + Δ t
Figure BDA0003023520010000031
And
Figure BDA0003023520010000032
from the formula of displacement acceleration
Figure BDA0003023520010000033
The values at t obtained from GPS and NDT can be calculated separately0At time + Δ t, i.e. t1Acceleration values in three directions of time.
At t0And t1At the moment, the positioning coordinates obtained by the GPS are:
Figure BDA0003023520010000034
Figure BDA0003023520010000035
the difference vector of the displacements is:
Figure BDA0003023520010000036
according to
Figure BDA0003023520010000037
Can solve for the GPS derived signal at t1Acceleration of time of day
Figure BDA0003023520010000038
Namely:
Figure BDA0003023520010000039
similarly, the value obtained at t from NDT1The accelerations in the three directions at the moment are:
Figure BDA00030235200100000310
and 6, converting coordinates, namely converting the obtained GPS and NDT acceleration into an unmanned aerial vehicle body axis coordinate system from the NDT coordinate. Obtaining a converted acceleration value
Figure BDA00030235200100000311
And
Figure BDA00030235200100000312
and 7, calculating deviation to obtain calibration positioning, wherein the step mainly comprises the following steps:
step 7.1, obtain t1Acceleration of IMU at a time in three directions
Figure BDA00030235200100000313
Step 7.2, solving the difference vector of the two groups of accelerations calculated by the GPS and the NDT relative to the IMU acceleration, namely
Figure BDA00030235200100000314
And
Figure BDA00030235200100000315
and 7.3, calculating Euclidean norms of the difference vectors, wherein the obtained acceleration difference norms of the GPS and the NDT are respectively as follows:
Figure BDA00030235200100000316
Figure BDA00030235200100000317
step 8, setting a threshold value
Figure BDA00030235200100000318
And comparing the deviation to obtain calibration positioning. The difference norm obtained in step 7 is divided into the following 4 cases:
1. if it is not
Figure BDA00030235200100000319
And is
Figure BDA00030235200100000320
The GPS and NDT algorithms are considered to be stable because the lidar can achieve higher accuracy, and therefore the coordinates obtained by the lidar are taken as the final positioning coordinates, and p is taken as pN＝(xN,yN,zN) As calibration coordinates.
2. If it is not
Figure BDA00030235200100000321
And is
Figure BDA00030235200100000322
The positioning deviation acquired by the NDT is considered to be large, and p is taken as pG＝(xG,yG,zG) As calibration coordinates.
3. If it is not
Figure BDA00030235200100000323
And is
Figure BDA00030235200100000324
The positioning deviation obtained by the GPS is considered to be large, and p is taken as pN＝(xN,yN,zN) As calibration coordinates.
4. If it is not
Figure BDA0003023520010000041
And is
Figure BDA0003023520010000047
Considering that there is a possible deviation between the positions obtained by the GPS and the NDT, and taking the t estimated by the IMU and the coordinate of the last moment introduced in the step 11Time coordinate pIUsing the fusion coordinates of NDT and IMU as calibration coordinates, i.e. taking
Figure BDA0003023520010000042
Figure BDA0003023520010000043
As calibration coordinates. Wherein β is a point cloud matching rate of NDT, and α + β ═ 1.
Drawings
FIG. 1 is a model diagram of the NDT and GPS integrated positioning algorithm of the present invention.
FIG. 2 is a flow chart of NDT and GPS comparison positioning in the present invention.
Detailed description of the embodiments
1. In the NDT algorithm of the laser radar, when point cloud parameters are configured, the maximum iteration times are set for the point cloud parameters, so that the point cloud parameters are prevented from falling into endless loops when the point cloud parameters cannot be matched to the optimal values all the time. Therefore, at the initial time, if the return value of the NDT algorithm is returned when the maximum number of iterations is reached, there may be situations such as similar feature values and obstruction, and at this time, the NDT algorithm is difficult to perform feature matching, and the situation of lost positioning occurs. In this case, the accuracy of NDT positioning will be denied. That is, the iteration number n when NDT registration is successful is obtained, and if n is exactly the maximum iteration number of the point cloud parameters, it is considered that the NDT matching is problematic, and the accuracy of NDT positioning is denied.
2. The IMU can be used for judging whether the airplane is in a moving state or not, the acceleration measured by the IMU within a short time interval is reliable, if the acceleration measured by the IMU is larger than a given threshold value, the airplane is considered to be moving at the moment, and if the coordinates returned by the NDT or the GPS are not changed, the coordinates are considered to be inaccurate.
Conversion of GPS latitude and longitude and NDT three-dimensional coordinates
Is provided with
Figure BDA0003023520010000044
Longitude and latitude in geographic coordinates, P and three-dimensional coordinate P for the earth-fixed coordinate systemuThe corresponding relation is as follows:
Figure BDA0003023520010000045
where R is the radius of the earth.
Setting the coordinate of the coordinate axis origin relative to the earth-fixed coordinate system when NDT is constructed as (x)0,y0,z0) The rotation angles of the NDT coordinate axis relative to the x, y and z directions of the underestimation coordinate axis are respectively alpha, beta and gamma.
The rotation matrix is then:
Figure BDA0003023520010000046
the three-dimensional coordinate of the GPS and the NDT which are unified can be obtained as follows:
(x,y,z)＝M×(xu,yu,zu)+(x0,y0,z0)
4. unmanned aerial vehicle takeoff direction acquisition
When the laser radar NDT algorithm is used for positioning the unmanned aerial vehicle, the surface parameters of the unmanned aerial vehicle need to be acquired, the initial point coordinates of the unmanned aerial vehicle need to be acquired, and the flight direction of the unmanned aerial vehicle also needs to be acquired. We can express the direction of the drone by direction cosine and direction cosine matrices, the calculation scheme is as follows:
given that the body coordinate system of the drone is (I, J, K), and the coordinate system mapped by NDT is taken as the global coordinate system (I, J, K), the Directional Cosine Matrix (DCM) of the drone at this time is:
Figure BDA0003023520010000051
in order to obtain the direction cosine of the unmanned aerial vehicle, the flying direction of the unmanned aerial vehicle is calculated after the unmanned aerial vehicle takes off a distance of 2m according to the meter-level precision of the GPS. Acquisition of initial point (x) of drone by GPS0,y0,z0) The motion point reached 2m after takeoff is (x)1,y1,z1) The trajectory vector is (x)1-x0,y1-y0,z1-z0) According to the information, the flight direction of the unmanned aerial vehicle can be estimated, namely the direction cosine vector is as follows:
Figure BDA0003023520010000052
according to the requirement of NDT algorithm, for each divided grid, calculating the probability density function of the point cloud in the grid: when the number of points in the grid is more than 5, the mean vector of each point cloud is:
Figure BDA0003023520010000053
whereinxk(k-1, 2, …) represents all point clouds within the grid.
The covariance matrix of the point cloud in the grid is:
Figure BDA0003023520010000054
the probability density function of the point cloud within the grid is:
Figure BDA0003023520010000055
eigenvectors of covariance matrix
Figure BDA0003023520010000056
The orientation of the drone can be represented, when the number of point clouds in the grid is less than 5, the covariance matrix has no inverse matrix, its normal distribution is degenerate, and its direction is estimated by the cosine vector at this time, that is:
Figure BDA0003023520010000057
therefore, the NDT transformation can be applied for positioning no matter whether the point cloud in the grid is sparse or not.
NDT and GPS positioning IMU data based decision making
At t0And t1And time, the optimized positioning coordinates of the two times obtained by the GPS are respectively as follows:
Figure BDA0003023520010000058
Figure BDA0003023520010000059
the difference vector of the displacements is:
Figure BDA0003023520010000061
according to the formula
Figure BDA0003023520010000062
Can solve out
Figure BDA0003023520010000063
By GPS at t1The accelerations in three directions obtained at the moment are:
Figure BDA0003023520010000064
similarly, from NDT at t1The accelerations in three directions obtained at the moment are:
Figure BDA0003023520010000065
will be at t1And converting the GPS and NDT acceleration obtained at the moment into an unmanned aerial vehicle body axis coordinate system from the NDT coordinate. Obtaining a converted acceleration value
Figure BDA0003023520010000066
And
Figure BDA0003023520010000067
and acquiring the acceleration of the airborne IMU in three directions at the moment in real time
Figure BDA0003023520010000068
Arbitrating whether the coordinates given by the GPS and the NDT deviate from the actual value or not through the actual acceleration value of the IMU, and calculating a difference vector of the two groups of accelerations of the GPS and the NDT relative to the acceleration of the IMU, namely
Figure BDA0003023520010000069
And
Figure BDA00030235200100000610
the difference norm of the GPS and the NDT is obtained by calculating the Euclidean norm of the difference vector and measuring the deviation degree of the difference, and the difference norms of the GPS and the NDT are respectively:
Figure BDA00030235200100000611
Figure BDA00030235200100000612
the obtained GPS difference norm and NDT difference norm and threshold parameter
Figure BDA00030235200100000613
By comparison, the following 4 comparison cases can be obtained:
1. if it is not
Figure BDA00030235200100000614
And is
Figure BDA00030235200100000615
The GPS and NDT algorithms are considered to be stable because the lidar can achieve higher accuracy, and therefore the coordinates obtained by the lidar are taken as the final positioning coordinates, and p is taken as pN＝(xN,yN,zN) As calibration coordinates.
2. If it is not
Figure BDA00030235200100000616
And is
Figure BDA00030235200100000617
The positioning deviation acquired by the NDT is considered to be large, and p is taken as pG＝(xG,yG,zG) As calibration coordinates.
3. If it is not
Figure BDA00030235200100000618
And is
Figure BDA00030235200100000619
The positioning deviation obtained by the GPS is considered to be large, and p is taken as pN＝(xN,yN,zN) As a schoolQuasi-coordinate.
4. If it is not
Figure BDA00030235200100000620
And is
Figure BDA00030235200100000621
Considering that the GPS and the NDT have deviation in the acquired positioning, and taking the t estimated by the IMU and the coordinate of the last moment introduced in the step 11Time coordinate pIUsing the weighted coordinates of NDT and IMU as calibration coordinates, i.e. taking
Figure BDA00030235200100000622
Figure BDA00030235200100000623
As calibration coordinates. Wherein the higher the accuracy with which the theoretical NDT should be satisfied, the larger the value of β. Therefore, the point cloud matching rate of NDT may be given to β, and α + β becomes 1.

Claims (4)

1. A method for positioning by combining a GPS and a laser radar based on an IMU utilizes GPS positioning and a laser radar NDT algorithm to position an unmanned aerial vehicle, and improves a mode of single sensor positioning of the unmanned aerial vehicle. The method is characterized in that: and comparing the reliability of the positioning data of the GPS and the laser radar by combining the inertial measurement parameters acquired by the IMU, and deciding the final positioning data, wherein the steps are as follows:
step 1: pre-judging positioning data;
step 2: preprocessing positioning data;
and step 3: performing positioning decision by combining with the IMU;
and 4, step 4: and obtaining the calibration positioning.
2. The method for pre-judging the reliability of the positioning data of the GPS and the laser radar according to the step 1 in the claim 1 is characterized in that: when the real-time acceleration acquired by the IMU is larger than the threshold epsilon, the unmanned aerial vehicle is considered to be in a motion state, and at the moment, if the NDT algorithm is not changed with the positioning data returned by the GPS at the previous moment, the positioning is considered to be abnormal. And then obtaining the iteration times of laser radar point cloud matching when the NDT algorithm is positioned, and if the maximum matching times are reached, determining that the NDT algorithm is abnormal in mismatch. The pre-determination stage has the following conditions:
when the NDT algorithm is abnormal and the GPS positioning is not abnormal, returning positioning data fused by the GPS and the IMU as final positioning;
when the NDT algorithm is not abnormal and the GPS positioning is abnormal, the NDT positioning data is returned to be used as the final positioning;
when both the NDT algorithm and the GPS positioning are abnormal, the IMU is utilized to combine the real-time accurate acceleration with the positioning data of the previous moment, and the data of the moment is estimated to be used as the final positioning data;
and when the NDT algorithm and the GPS positioning are not abnormal, the final returned positioning data is decided by utilizing the real-time accurate acceleration of the IMU.
3. Preprocessing positioning data of GPS and lidar according to claim 1, step 2, characterized by: and establishing coordinate association of the GPS and the laser radar so as to carry out consistent description on positioning. And for the coordinates obtained by the GPS positioning and NDT algorithm, integrating the coordinates of the last moment in a time interval, and calculating the acceleration of the coordinates.
4. The IMU-based positioning decision making system of claim 1 or step 3, wherein: and comparing the acceleration calculated by the GPS and the NDT obtained by the requirement 3 with the IMU real-time acceleration respectively to calculate a difference norm. Comparing the two difference norms with a given threshold, there are the following cases:
when the difference norm obtained by the NDT algorithm and the difference norm obtained by the GPS are both smaller than a given threshold, returning NDT positioning data as final positioning;
when the difference norm obtained by the NDT algorithm is smaller than a given threshold value and the difference norm obtained by the GPS is larger than the given threshold value, returning NDT positioning data as final positioning;
when the difference norm obtained by the NDT algorithm is greater than a given threshold and the difference norm obtained by the GPS is less than the given threshold, returning GPS positioning data as final positioning;
and when the difference norm obtained by the NDT algorithm and the difference norm obtained by the GPS are both greater than a given threshold, returning positioning data fused by the NDT and the IMU as final positioning.
CN202110422792.1A 2021-04-16 2021-04-16 Method for fusing and positioning GPS and laser radar through inertial measurement parameters Pending CN113359167A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110422792.1A CN113359167A (en) 2021-04-16 2021-04-16 Method for fusing and positioning GPS and laser radar through inertial measurement parameters

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110422792.1A CN113359167A (en) 2021-04-16 2021-04-16 Method for fusing and positioning GPS and laser radar through inertial measurement parameters

Publications (1)

Publication Number Publication Date
CN113359167A true CN113359167A (en) 2021-09-07

Family

ID=77525264

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110422792.1A Pending CN113359167A (en) 2021-04-16 2021-04-16 Method for fusing and positioning GPS and laser radar through inertial measurement parameters

Country Status (1)

Country Link
CN (1) CN113359167A (en)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113984044A (en) * 2021-10-08 2022-01-28 杭州鸿泉物联网技术股份有限公司 Vehicle pose acquisition method and device based on vehicle-mounted multi-perception fusion
CN116184986A (en) * 2023-03-07 2023-05-30 珠海紫燕无人飞行器有限公司 Unmanned aerial vehicle fault detection method and system based on flight control log

Citations (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20140214317A1 (en) * 2011-05-10 2014-07-31 Seiko Epson Corporation Position calculating method and position calculating device
KR20140120466A (en) * 2013-04-03 2014-10-14 국방과학연구소 System and Method for estimating positions of an autonomous mobile vehicle
US20170124781A1 (en) * 2015-11-04 2017-05-04 Zoox, Inc. Calibration for autonomous vehicle operation
CN109709801A (en) * 2018-12-11 2019-05-03 智灵飞（北京）科技有限公司 A kind of indoor unmanned plane positioning system and method based on laser radar
CN110243358A (en) * 2019-04-29 2019-09-17 武汉理工大学 The unmanned vehicle indoor and outdoor localization method and system of multi-source fusion
CN110285806A (en) * 2019-07-05 2019-09-27 电子科技大学 The quick Precision Orientation Algorithm of mobile robot based on the correction of multiple pose
CN110741625A (en) * 2018-07-23 2020-01-31 深圳市大疆创新科技有限公司 Motion estimation method and mobile device
US20200150233A1 (en) * 2018-11-09 2020-05-14 Beijing Didi Infinity Technology And Development Co., Ltd. Vehicle positioning system using lidar
CN111272165A (en) * 2020-02-27 2020-06-12 清华大学 Intelligent vehicle positioning method based on characteristic point calibration
CN111324848A (en) * 2020-02-17 2020-06-23 北京建筑大学 Vehicle-mounted track data optimization method for mobile laser radar measurement system
CN111413721A (en) * 2020-01-14 2020-07-14 华为技术有限公司 Vehicle positioning method, device, controller, intelligent vehicle and system
CN111412912A (en) * 2020-04-14 2020-07-14 上海华测导航技术股份有限公司 Navigation board, multi-source data fusion method for navigation board and carrier
CN111522043A (en) * 2020-04-30 2020-08-11 北京联合大学 Unmanned vehicle laser radar rapid re-matching positioning method
KR20200109116A (en) * 2019-03-12 2020-09-22 한국과학기술원 Method and system for position estimation of unmanned aerial vehicle using graph structure based on multi module
CN111751852A (en) * 2020-06-17 2020-10-09 北京联合大学 Unmanned vehicle GNSS positioning reliability evaluation method based on point cloud registration
CN112484725A (en) * 2020-11-23 2021-03-12 吉林大学 Intelligent automobile high-precision positioning and space-time situation safety method based on multi-sensor fusion
CN112622862A (en) * 2020-12-24 2021-04-09 北京理工大学前沿技术研究院 Automatic driving automobile brake abnormity/attack on-line monitoring method and system

Patent Citations (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20140214317A1 (en) * 2011-05-10 2014-07-31 Seiko Epson Corporation Position calculating method and position calculating device
KR20140120466A (en) * 2013-04-03 2014-10-14 국방과학연구소 System and Method for estimating positions of an autonomous mobile vehicle
US20170124781A1 (en) * 2015-11-04 2017-05-04 Zoox, Inc. Calibration for autonomous vehicle operation
CN110741625A (en) * 2018-07-23 2020-01-31 深圳市大疆创新科技有限公司 Motion estimation method and mobile device
US20200150233A1 (en) * 2018-11-09 2020-05-14 Beijing Didi Infinity Technology And Development Co., Ltd. Vehicle positioning system using lidar
CN109709801A (en) * 2018-12-11 2019-05-03 智灵飞（北京）科技有限公司 A kind of indoor unmanned plane positioning system and method based on laser radar
KR20200109116A (en) * 2019-03-12 2020-09-22 한국과학기술원 Method and system for position estimation of unmanned aerial vehicle using graph structure based on multi module
CN110243358A (en) * 2019-04-29 2019-09-17 武汉理工大学 The unmanned vehicle indoor and outdoor localization method and system of multi-source fusion
CN110285806A (en) * 2019-07-05 2019-09-27 电子科技大学 The quick Precision Orientation Algorithm of mobile robot based on the correction of multiple pose
CN111413721A (en) * 2020-01-14 2020-07-14 华为技术有限公司 Vehicle positioning method, device, controller, intelligent vehicle and system
CN111324848A (en) * 2020-02-17 2020-06-23 北京建筑大学 Vehicle-mounted track data optimization method for mobile laser radar measurement system
CN111272165A (en) * 2020-02-27 2020-06-12 清华大学 Intelligent vehicle positioning method based on characteristic point calibration
CN111412912A (en) * 2020-04-14 2020-07-14 上海华测导航技术股份有限公司 Navigation board, multi-source data fusion method for navigation board and carrier
CN111522043A (en) * 2020-04-30 2020-08-11 北京联合大学 Unmanned vehicle laser radar rapid re-matching positioning method
CN111751852A (en) * 2020-06-17 2020-10-09 北京联合大学 Unmanned vehicle GNSS positioning reliability evaluation method based on point cloud registration
CN112484725A (en) * 2020-11-23 2021-03-12 吉林大学 Intelligent automobile high-precision positioning and space-time situation safety method based on multi-sensor fusion
CN112622862A (en) * 2020-12-24 2021-04-09 北京理工大学前沿技术研究院 Automatic driving automobile brake abnormity/attack on-line monitoring method and system

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
X. WANG, Y. LIAN AND L. LI: "Localization of Autonomous Cars Using Multi-Sensor Data Fusion", 《2018 CHINESE AUTOMATION CONGRESS (CAC)》, 4 December 2090 (2090-12-04), pages 4152 - 4155 *
代东: "基于激光与组合导航系统协作的三维地图构建", 《中国优秀硕士学位论文全文数据库 基础科学辑》, no. 2, 15 February 2020 (2020-02-15), pages 008 - 174 *
贾晓辉,徐文枫,刘今越,李铁军: "基于惯性测量单元辅助的激光里程计求解方法", 《仪器仪表学报》, vol. 42, 22 January 2021 (2021-01-22), pages 39 - 48 *
郭卫芳,李理: "种多传感器融合高精度定位方法", 《信息通信》, 11 May 2020 (2020-05-11), pages 24 - 25 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113984044A (en) * 2021-10-08 2022-01-28 杭州鸿泉物联网技术股份有限公司 Vehicle pose acquisition method and device based on vehicle-mounted multi-perception fusion
CN116184986A (en) * 2023-03-07 2023-05-30 珠海紫燕无人飞行器有限公司 Unmanned aerial vehicle fault detection method and system based on flight control log
CN116184986B (en) * 2023-03-07 2023-12-29 珠海紫燕无人飞行器有限公司 Unmanned aerial vehicle fault detection method and system based on flight control log

Similar Documents

Publication Publication Date Title
CN109945856B (en) Unmanned aerial vehicle autonomous positioning and mapping method based on inertia/radar
CN105628026B (en) A kind of positioning and orientation method and system of mobile object
WO2022193106A1 (en) Method for fusing gps with laser radar through inertia measurement parameter for positioning
CN109883426B (en) Dynamic distribution and correction multi-source information fusion method based on factor graph
Wang et al. A GNSS/INS integrated navigation algorithm based on Kalman filter
RU2487419C1 (en) System for complex processing of information of radio navigation and self-contained navigation equipment for determining real values of aircraft navigation parameters
CN111426320B (en) Vehicle autonomous navigation method based on image matching/inertial navigation/milemeter
CN107300697A (en) Moving target UKF filtering methods based on unmanned plane
CN112346104B (en) Unmanned aerial vehicle information fusion positioning method
CN113359167A (en) Method for fusing and positioning GPS and laser radar through inertial measurement parameters
Lombaerts et al. Distributed Ground Sensor Fusion Based Object Tracking for Autonomous Advanced Air Mobility Operations
Zorina et al. Enhancement of INS/GNSS integration capabilities for aviation-related applications
Campbell et al. Vision-based geolocation tracking system for uninhabited aerial vehicles
Simonetti et al. Robust modeling of geodetic altitude from barometric altimeter and weather data
Madany et al. Modelling and simulation of robust navigation for unmanned air systems (UASs) based on integration of multiple sensors fusion architecture
CN116482735A (en) High-precision positioning method for inside and outside of limited space
CN113721188B (en) Multi-unmanned aerial vehicle self-positioning and target positioning method under refusing environment
CN108957500B (en) Method for calculating intersection point of observation sight of sensor and earth surface
Džunda et al. Influence of mutual position of communication network users on accuracy of positioning by telemetry method
Bian et al. Integrated Navigation
Lombaerts et al. Development and Field Test Results of Distributed Ground-Based Sensor Fusion Object Tracking
CN114608572B (en) Method for realizing AGV indoor positioning by combining UWB and IMU
RU2767477C1 (en) Uav navigation method
Tokta et al. A Novel Multi-Hypothesis Filter for Terrain Aided Navigation
Yu et al. Multi-sensor Fusion Height Prediction Algorithm Based on Kalman Filtering

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
WD01 Invention patent application deemed withdrawn after publication

Application publication date: 20210907

WD01 Invention patent application deemed withdrawn after publication