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 t
0Positioning data of
And acceleration about three directions measured in real time by the IMU
Step 1.2 according to the formula
Estimate by t
0Time t
1The magnitude of the motion displacement at the moment. Namely:
step 1.3 according to
T estimated by IMU can be obtained
1Location data of the moment. Namely:
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 t
0And 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
And
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:
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:
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 t
0And t
0The GPS and NDT coordinates of + Δ t calculate the average acceleration in three directions within Δ t, respectively. For two adjacent time instants t
0And t
0Coordinates of + Δ t
And
from the formula of displacement acceleration
The values at t obtained from GPS and NDT can be calculated separately
0At time + Δ t, i.e. t
1Acceleration values in three directions of time.
At t
0And t
1At the moment, the positioning coordinates obtained by the GPS are:
the difference vector of the displacements is:
according to
Can solve for the GPS derived signal at t
1Acceleration of time of day
Namely:
similarly, the value obtained at t from NDT1The accelerations in the three directions at the moment are:
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
And
and 7, calculating deviation to obtain calibration positioning, wherein the step mainly comprises the following steps:
step 7.1, obtain t
1Acceleration of IMU at a time in three directions
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
And
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:
step 8, setting a threshold value
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
And is
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 p
N=(x
N,y
N,z
N) As calibration coordinates.
2. If it is not
And is
The positioning deviation acquired by the NDT is considered to be large, and p is taken as p
G=(x
G,y
G,z
G) As calibration coordinates.
3. If it is not
And is
The positioning deviation obtained by the GPS is considered to be large, and p is taken as p
N=(x
N,y
N,z
N) As calibration coordinates.
4. If it is not
And is
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 1
1Time coordinate p
IUsing the fusion coordinates of NDT and IMU as calibration coordinates, i.e. taking
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
Longitude and latitude in geographic coordinates, P and three-dimensional coordinate P for the earth-fixed coordinate system
uThe corresponding relation is as follows:
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:
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:
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:
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:
whereinxk(k-1, 2, …) represents all point clouds within the grid.
The covariance matrix of the point cloud in the grid is:
the probability density function of the point cloud within the grid is:
eigenvectors of covariance matrix
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:
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 t
0And t
1And time, the optimized positioning coordinates of the two times obtained by the GPS are respectively as follows:
the difference vector of the displacements is:
according to the formula
Can solve out
By GPS at t
1The accelerations in three directions obtained at the moment are:
similarly, from NDT at t1The accelerations in three directions obtained at the moment are:
will be at t
1And 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
And
and acquiring the acceleration of the airborne IMU in three directions at the moment in real time
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
And
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:
the obtained GPS difference norm and NDT difference norm and threshold parameter
By comparison, the following 4 comparison cases can be obtained:
1. if it is not
And is
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 p
N=(x
N,y
N,z
N) As calibration coordinates.
2. If it is not
And is
The positioning deviation acquired by the NDT is considered to be large, and p is taken as p
G=(x
G,y
G,z
G) As calibration coordinates.
3. If it is not
And is
The positioning deviation obtained by the GPS is considered to be large, and p is taken as p
N=(x
N,y
N,z
N) As a schoolQuasi-coordinate.
4. If it is not
And is
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 1
1Time coordinate p
IUsing the weighted coordinates of NDT and IMU as calibration coordinates, i.e. taking
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.