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

本发明涉及一种为民用无人机(如：大疆无人机)提供更可靠定位的方法。本发明利用多传感器融合的思想，将IMU获取的惯性测量参数、GPS定位数据与激光雷达NDT算法计算数据相结合实现高可靠性定位。解决了无人机在一些环境下GPS信号弱，可能出现的GPS数据丢失以及无人机运动剧烈，或者现场发生变化与三维地图不一致导致的激光雷达扫描匹配失败的问题。通过本发明方法，确保无人机在能够获取当前的定位信息的情况下，能够采用更精确的定位数据。本发明首先建立GPS与激光雷达的坐标关联，以进行对定位的一致描述。对GPS定位数据和激光雷达NDT算法匹配数据进行预判断，对不可靠定位数据进行初步筛除。在此基础上利用IMU获取的惯性测量参数来比较两个定位数据的可靠性，决策出更可靠的定位数据。当GPS和激光雷达的数据均不可靠时，则利用IMU和上一时刻的定位数据来估计当前的定位值，保证无人机在特殊环境下的可靠定位。

Figure 202110422792

The present invention relates to a method for providing more reliable positioning for civil unmanned aerial vehicles (such as DJI unmanned aerial vehicles). The invention utilizes the idea of multi-sensor fusion, and combines inertial measurement parameters obtained by IMU, GPS positioning data and laser radar NDT algorithm calculation data to achieve high-reliability positioning. Solve the problem that the GPS signal of the drone is weak in some environments, the GPS data may be lost, the drone moves violently, or the lidar scan matching fails due to the inconsistency between the scene changes and the three-dimensional map. Through the method of the present invention, it is ensured that the UAV can use more accurate positioning data under the condition that the current positioning information can be obtained. The present invention first establishes the coordinate association between GPS and laser radar, so as to carry out consistent description of positioning. Pre-judgment is made between GPS positioning data and lidar NDT algorithm matching data, and unreliable positioning data is preliminarily screened. On this basis, the inertial measurement parameters obtained by the IMU are used to compare the reliability of the two positioning data, and a more reliable positioning data is determined. When both GPS and lidar data are unreliable, the IMU and the positioning data of the previous moment are used to estimate the current positioning value to ensure the reliable positioning of the UAV in a special environment.

Figure 202110422792

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.一种基于IMU将GPS与激光雷达结合进行定位的方法，本方法利用了GPS定位和激光雷达NDT算法对无人机进行定位，改进了无人机单传感器定位的模式。其特征在于：结合IMU获取的惯性测量参数，对GPS和激光雷达的定位数据进行可靠性比较，决策出最终定位数据，步骤如下：1. A method of combining GPS and lidar for positioning based on IMU. This method uses GPS positioning and lidar NDT algorithm to locate the UAV, and improves the single-sensor positioning mode of the UAV. It is characterized in that: combining the inertial measurement parameters obtained by the IMU, the reliability of the positioning data of GPS and lidar is compared, and the final positioning data is determined. The steps are as follows: 步骤1：定位数据预判断；Step 1: Pre-judgment of positioning data; 步骤2：定位数据预处理；Step 2: Positioning data preprocessing; 步骤3：结合IMU进行定位决策；Step 3: Combine IMU to make positioning decision; 步骤4：得出校准定位。Step 4: Deriving the calibration positioning. 2.根据要求1中步骤1所述的对GPS和激光雷达的定位数据进行可靠性预判断，其特征在于：当IMU获取到的实时加速度大于阈值ε时，则认为无人机处于运动状态，此时若NDT算法和GPS返回的定位数据比较前一时刻未发生改变，则认为定位异常。再获取NDT算法定位时激光雷达点云匹配的迭代次数，如果已达到最大匹配次数，则认为NDT算法失配异常。预判断阶段有以下情形：2. Carry out reliability pre-judgment to the positioning data of GPS and lidar according to step 1 in claim 1, and it is characterized in that: when the real-time acceleration obtained by IMU is greater than the threshold ε, it is considered that the drone is in a motion state, At this time, if the positioning data returned by the NDT algorithm and GPS has not changed at the previous moment, it is considered that the positioning is abnormal. Then obtain the number of iterations of lidar point cloud matching during the positioning of the NDT algorithm. If the maximum number of matching times has been reached, the mismatch of the NDT algorithm is considered abnormal. The pre-judgment stage includes the following situations: 当NDT算法异常而此时的GPS定位无异常时，返回GPS与IMU融合的定位数据作为最终定位；When the NDT algorithm is abnormal and the GPS positioning at this time is not abnormal, the positioning data fused by GPS and IMU is returned as the final positioning; 当NDT算法无异常而此时的GPS定位异常时，返回NDT定位数据作为最终定位；When there is no abnormality in the NDT algorithm and the GPS positioning at this time is abnormal, the NDT positioning data is returned as the final positioning; 当NDT算法和GPS定位均出现异常时，利用IMU实时准确加速度和上一时刻的定位数据结合，估计出该时刻的数据作为最终定位数据；When both the NDT algorithm and GPS positioning are abnormal, the IMU real-time accurate acceleration is combined with the positioning data of the previous moment, and the data at this moment is estimated as the final positioning data; 当NDT算法和GPS定位均未出现异常时，利用IMU实时准确加速度来决策出最终返回定位数据。When there is no abnormality in the NDT algorithm and GPS positioning, the IMU real-time accurate acceleration is used to decide the final return positioning data. 3.根据要求1步骤2所述的对GPS和激光雷达的定位数据进行预处理，其特征在于：建立GPS与激光雷达的坐标关联，以进行对定位的一致描述。对于GPS定位与NDT算法得到的坐标，在时间间隔内，综合上一时刻的坐标，计算得出它们的加速度。3. Preprocessing the positioning data of GPS and lidar according to step 2 of claim 1, characterized in that: establishing a coordinate association between GPS and lidar, so as to carry out a consistent description of the positioning. For the coordinates obtained by GPS positioning and NDT algorithm, in the time interval, the coordinates of the previous moment are synthesized, and their accelerations are calculated. 4.根据要求1步骤3所述的结合IMU进行定位决策，其特征在于：对于要求3得到的由GPS与NDT计算出的加速度，将它们分别与IMU实时加速度进行比较，计算出差值范数。将两个差值范数与给定阈值进行比较，有以下情形：4. carry out positioning decision according to the described combination IMU of requirement 1 step 3, it is characterized in that: for the acceleration calculated by GPS and NDT that requirement 3 obtains, compare them with IMU real-time acceleration respectively, calculate difference norm . Comparing two difference norm with a given threshold, there are the following cases: 当由NDT算法得到的差值范数和GPS得到的差值范数均小于给定阈值时，返回NDT定位数据作为最终定位；When the difference norm obtained by the NDT algorithm and the difference norm obtained by the GPS are both less than the given threshold, the NDT positioning data is returned as the final positioning; 当由NDT算法得到的差值范数小于给定阈值而由GPS得到的差值范数大于给定阈值时，返回NDT定位数据作为最终定位；When the difference norm obtained by the NDT algorithm is less than the given threshold and the difference norm obtained by the GPS is greater than the given threshold, the NDT positioning data is returned as the final positioning; 当由NDT算法得到的差值范数大于给定阈值而由GPS得到的差值范数小于给定阈值时，返回GPS定位数据作为最终定位；When the difference norm obtained by the NDT algorithm is greater than the given threshold and the difference norm obtained by the GPS is less than the given threshold, the GPS positioning data is returned as the final positioning; 当由NDT算法得到的差值范数和GPS得到的差值范数均大于给定阈值时，返回NDT与IMU融合的定位数据作为最终定位。When the difference norm obtained by the NDT algorithm and the difference norm obtained by the GPS are both greater than the given threshold, the positioning data fused by the NDT and the IMU is returned as the 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 (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113984044A (en) * 2021-10-08 2022-01-28 杭州鸿泉物联网技术股份有限公司 A vehicle pose acquisition method and device based on vehicle multi-sensing fusion
CN114777796A (en) * 2022-04-02 2022-07-22 智道网联科技(北京)有限公司 Fusion positioning method and device for vehicle, electronic equipment and storage medium
CN115930984A (en) * 2022-12-08 2023-04-07 安徽江淮汽车集团股份有限公司 Method, system, electronic device and computer readable storage medium for vehicle positioning
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 武汉理工大学 Indoor and outdoor positioning method and system for unmanned vehicles based on multi-source fusion
CN110285806A (en) * 2019-07-05 2019-09-27 电子科技大学 Fast and precise positioning algorithm for mobile robot based on multiple pose correction
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 清华大学 An intelligent vehicle localization method based on feature point calibration
CN111324848A (en) * 2020-02-17 2020-06-23 北京建筑大学 Optimization method of vehicle trajectory data for mobile lidar 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 北京联合大学 GNSS positioning reliability evaluation method for unmanned vehicles 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 武汉理工大学 Indoor and outdoor positioning method and system for unmanned vehicles based on multi-source fusion
CN110285806A (en) * 2019-07-05 2019-09-27 电子科技大学 Fast and precise positioning algorithm for mobile robot based on multiple pose correction
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 北京建筑大学 Optimization method of vehicle trajectory data for mobile lidar measurement system
CN111272165A (en) * 2020-02-27 2020-06-12 清华大学 An intelligent vehicle localization method based on feature 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 北京联合大学 GNSS positioning reliability evaluation method for unmanned vehicles 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 (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113984044A (en) * 2021-10-08 2022-01-28 杭州鸿泉物联网技术股份有限公司 A vehicle pose acquisition method and device based on vehicle multi-sensing fusion
CN114777796A (en) * 2022-04-02 2022-07-22 智道网联科技(北京)有限公司 Fusion positioning method and device for vehicle, electronic equipment and storage medium
CN115930984A (en) * 2022-12-08 2023-04-07 安徽江淮汽车集团股份有限公司 Method, system, electronic device and computer readable storage medium for vehicle positioning
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
WO2022193106A1 (en) Method for fusing gps with laser radar through inertia measurement parameter for positioning
CN113359167A (en) Method for fusing and positioning GPS and laser radar through inertial measurement parameters
CN110274588B (en) Multi-source fusion navigation method based on two-layer nested factor graph based on UAV swarm information
Kim et al. Autonomous airborne navigation in unknown terrain environments
CN107315171B (en) Radar networking target state and system error joint estimation algorithm
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
CN108519615A (en) Autonomous navigation method for mobile robot based on combined navigation and feature point matching
CN106197408A (en) A kind of multi-source navigation data fusion method based on factor graph
Miller Developing algorithms of object motion control on the basis of Kalman filtering of bearing-only measurements
Ouyang et al. Cooperative navigation of UAVs in GNSS-denied area with colored RSSI measurements
CN112346104A (en) A UAV Information Fusion Positioning Method
Lyu et al. A factor graph optimization method for high-precision IMU-based navigation system
Yu et al. Full-parameter vision navigation based on scene matching for aircrafts
CN112505718B (en) Positioning method, system and computer readable medium for autonomous vehicle
Andert et al. Optical-aided aircraft navigation using decoupled visual SLAM with range sensor augmentation
Zorina et al. Enhancement of INS/GNSS integration capabilities for aviation-related applications
CN113721188B (en) Multi-unmanned aerial vehicle self-positioning and target positioning method under refusing environment
Lombaerts et al. Development and Field Test Results of Distributed Ground-Based Sensor Fusion Object Tracking
CN114063647B (en) Multi-unmanned aerial vehicle mutual positioning method based on distance measurement
Wang et al. Micro aerial vehicle navigation with visual-inertial integration aided by structured light
Madany et al. Modelling and simulation of robust navigation for unmanned air systems (UASs) based on integration of multiple sensors fusion architecture
CN114723902A (en) Coal mine underground unmanned helicopter SLAM based on probability membrane calculation
Zhang et al. A Low-Cost UAV Swarm Relative Positioning Architecture Based on BDS/Barometer/UWB
Song An integrated GPS/vision UAV navigation system based on Kalman filter

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
WD01 Invention patent application deemed withdrawn after publication

Application publication date: 20210907