Disclosure of Invention
Therefore, the invention provides a multi-sensor fusion positioning method, which realizes the accurate and stable positioning of the underground unmanned vehicle by constructing a multi-sensor fusion positioning method of a vehicle-mounted UWB, a laser radar and an Inertial Measurement Unit (IMU).
The multi-sensor fusion positioning method comprises the following steps:
S1, constructing an IMU-based vehicle position and posture estimation model;
S2, constructing an EKF measurement fusion model based on UWB ranging;
S3, constructing a matching positioning model of the laser radar point cloud;
S4, constructing a fusion positioning model based on S1, S2 and S3.
Further, the method for constructing the vehicle position and posture estimation model in the step S1 specifically includes the following steps:
s101, calculating a deterministic error of the IMU inertial navigation system, including:
platform angle error:
Wherein,The misalignment angle of the inertial navigation system in the navigation coordinate system is set; a rotational angular velocity vector of the ground system relative to the inertial system; A rotational angular velocity vector of the navigation system relative to the ground system; epsilonn is the gyro random error.
Speed error:
Wherein fn is an acceleration vector in a navigation coordinate system; vn is the velocity vector of the carrier in the navigation coordinate system; Is the accelerometer random error.
Position error:
Wherein,Carrier latitude, longitude and altitude error rates, respectively; rN is the radius of curvature of the meridian; rE is the radius of curvature of the meridian; δvN、δvW、δvU is the speed error in north, west and sky directions, respectively.
S102, building a vehicle position and posture estimation model:
firstly, establishing a state equation of an inertial navigation system of the IMU:
After discretization, the following steps are obtained:
Xk+1＝ΦXk+Γwk；
Wherein,
X is an inertial navigation state variable;
FINS is an inertial navigation state matrix;
w is inertial navigation error variable;
G is an inertial navigation error sensitive matrix;
Phi is a state transition matrix;
Γ is the discretized input matrix;
And setting the EKF filtering update period as T, and calculating to obtain:
Φ＝I+FT；
Γ＝GT；
then, an integral update expression of the state variable is calculated:
after the quaternion is normalized, the F matrix and the G matrix are recalculated to obtain:
Pk+1＝(I+FT)Pk(I+FT)T+T2GQGT；
To predict covariance.
Further, in step S3, an EKF measurement fusion model based on UWB ranging is constructed, which specifically includes the following steps:
s201, calculating a position vector r of the UWB tag in a North west sky coordinate system with the UWB base station as an origin:
wherein, LU、λU、hU is the latitude, longitude and altitude of the label respectively; lB、λB、hB is the latitude, longitude and altitude of the base station, respectively.
S202, calculating a distance observation value between the UWB tag and the base station:
ρu＝ρtrue+εu；
Wherein ρtrue and ρ are the actual distance and the observed distance of the tag to the base station respectively; δr is the distance error vector from the tag to the base station; A rotation matrix from a body coordinate system to a navigation coordinate system; lu is the lever arm vector of the tag to the inertial navigation center; phi is an inertial navigation installation angle error vector.
S203, obtaining an observation equation:
Zu＝HuX+Vu。
Wherein Zu is UWB observables; hu is the observation sensitivity matrix; vu is observation noise.
Further, the observation equation of the combined system based on the laser radar position and heading is that
Zlidar＝HlidarX+Vlidar；
Wherein:
calculating a filtering update gain matrix:
K＝Pk+1HT(HPk+1HT+R)-1；
updating the filter state variables:
xk+1＝xk+1|k+K(Zk+1-yk+1)；
Updating the filter covariance:
Pk+1＝Pk+1|k-KHPk+1|k。
compared with the prior art, the technical scheme provided by the invention has the following advantages:
According to the invention, the vehicle position and the attitude are predicted by using the IMU by constructing an Extended Kalman Filter (EKF) based state estimation model, the UWB ranging information and the laser radar point cloud matching positioning data are used for updating, the accurate position and the attitude of the vehicle in the pit are estimated in real time, and the problem of accurate positioning in the pit is solved.
Detailed Description
The following description of the embodiments of the present invention will be made clearly and completely with reference to the accompanying drawings, in which it is apparent that the embodiments described are only some embodiments of the present invention, but not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
The inertial navigation system has a plurality of error sources, mainly including errors of an inertial instrument, installation errors and scale errors of the inertial instrument, initial condition errors of the system, calculation errors of the system, errors caused by various interferences and the like. Inertial navigation errors can be divided into two categories: deterministic errors and random errors. Deterministic errors include platform angle errors, speed errors, and position errors, random errors mainly being gyro drift and zero bias of the accelerometer, etc.
The method of this embodiment, as shown in fig. 1, includes the following steps:
The platform error angle equation of the inertial navigation system is:
Wherein,The misalignment angle of the inertial navigation system in the navigation coordinate system is set; a rotational angular velocity vector of the ground system relative to the inertial system; A rotational angular velocity vector of the navigation system relative to the ground system; epsilonn is the gyro random error.
Wherein:
the velocity error equation of the inertial navigation system is described below
From the following componentsIs available in the form of
Consider δgn＝0,δfn＝fp-fn where fp is the actual output of the accelerometer. The measurement error of the accelerometer is designed to beThen
Therefore, it is
The velocity error equation is then derived as:
Wherein fn is an acceleration vector in a navigation coordinate system; vn is the velocity vector of the carrier in the navigation coordinate system; Is the accelerometer random error.
From the following components
The available position error equation is:
Wherein,Carrier latitude, longitude and altitude error rates, respectively; rN is the radius of curvature of the meridian; rE is the radius of curvature of the meridian; δvN、δvW、δvU is the speed error in north, west and sky directions, respectively.
By combining the above formulas, the state equation of the integrated navigation system can be obtained:
Based on the jacobian matrix, the following discretized state equation can be established:
Xk+1＝ΦXk+Γwk
where Φ is the available state transition matrix and Γ is the discretized input matrix. Let EKF filter update period be T, then their expressions are respectively:
Φ＝I+FT
Γ＝GT
In order to improve the state variable prediction precision, a fourth-order Dragon-Gregory tower method is adopted to solve the differential equation.
The integral update expression of the state variable is:
After the state prediction is completed, the quaternion needs to be standardized, the F and G matrixes are recalculated, and then the covariance is predicted by adopting the following formula
Pk+1＝(I+FT)Pk(I+FT)T+T2GQGT
Constructing an EKF measurement fusion model of UWB ranging;
the position vector r of the UWB tag in the North Western-day coordinate system with the UWB base station as the origin can be expressed as
Wherein, LU、λU、hU is the latitude, longitude and altitude of the label respectively; lB、λB、hB is the latitude, longitude and altitude of the base station, respectively.
The position vector r of the UWB tag in the North Western-day coordinate system with the UWB base station as the origin can be expressed as
The range observations between the on-board UWB tag and the base station can be expressed as
The range observations between the on-board UWB tag and the base station can be expressed as
Wherein ρtrue and ρ are the actual distance and the observed distance of the tag to the base station respectively; δr is the distance error vector from the tag to the base station; A rotation matrix from a body coordinate system to a navigation coordinate system; lu is the lever arm vector of the tag to the inertial navigation center; phi is an inertial navigation installation angle error vector.
UWB range measurements can be expressed as
ρu＝ρtrue+εu
Therefore, the observation equation can be expressed as
Zu＝HuX+Vu
Wherein Zu is UWB observables; hu is the observation sensitivity matrix; vu is observation noise.
A laser radar point cloud matching positioning model is constructed below
After the laser radar sensor data are acquired, firstly invalid point rejection and point cloud filtering are needed to obtain effective point cloud information, the effective point cloud information is transmitted to a laser point cloud positioning module, the effective point cloud information is used as an input item of real-time scanning positioning, an IMU data processing module performs data synchronization and pre-integration processing to obtain conversion information of the IMU sensor in adjacent moments, the conversion information is used as a scanning matching initial value input of the laser point cloud positioning module, a map provided by a first party is loaded by a map loading module, and the map is input to IMU observables in a time sequence of a laser point cloud positioning module [ t, t+delta t ]. The observation model of the IMU is as follows:
after the point cloud information of the current scanning and the IMU pre-integration information are obtained in the front, the scanning matching between the current scanning frame and the existing map can be realized, the data constraint of the laser point cloud is obtained, then the data constraint is fused with the IMU pre-integration result, the pose optimization is carried out by using an EKF-based optimization method, and finally the laser point cloud positioning result is obtained.
A laser radar/IMU fusion positioning model is constructed below.
The laser radar point cloud is matched with the three-dimensional point cloud map, so that position and gesture data of the vehicle under a geodetic coordinate system can be obtained, the position comprises longitude, latitude and altitude, and the heading of the vehicle can be extracted according to the gesture data.
The observation equation of the combined system based on the laser radar position and heading is that
Zlidar＝HlidarX+Vlidar
Wherein the method comprises the steps of
Equation of position observation
In a lidar/IMU fusion positioning system, the real-time position given by the inertial navigation system is the geographic latitude, longitude and altitude, but contains errors. Let LI、λI and hI be latitude, longitude and altitude of inertial navigation output, respectively, and L, lambda and h be true latitude, longitude and altitude, respectively, then there are
Lidar point cloud matching also gives the real-time position of the vehicle, with the main source of error being the error in the point cloud matching process. Let Llidar、λlidar、hlidar be latitude, longitude and altitude of the laser radar matching output, respectively
Wherein NN、NW、NU is the position matching error of the laser radar in the north, west and sky directions.
When the position difference between INS and GPS is taken as the observed quantity, the observation equation can be represented by the following formula
The above is expressed as
ZP＝HPX+VP
In the middle of
VP＝[NN NW NU]T
The distance measurement noise VP is treated as white noise.
Calculating a course observation equation:
let the real azimuth of the vehicle be ψ, the inertial navigation solution error be δψI, the error angle obtained by the laser radar point cloud matching be δψG, the azimuth of the inertial navigation and the laser radar are respectively:
ψI＝ψ+δψI
ψG＝ψ+δψG
The azimuth angle observance of the system is:
Zψ＝ψI-ψM＝δψI-δψM＝HψX+Vψ
Wherein:
Vψ is laser radar point cloud matching azimuth observation noise.
The filter update gain matrix can be calculated according to the UWB and laser radar observation equation constructed above.
K＝Pk+1HT(HPk+1HT+R)-1
The filter state variables are updated.
Xk+1＝Xk+1|k+K(Zk+1-yk+1)
The filter covariance is updated.
Pk+1＝Pk+1|k-KHPk+1|k
According to the state equation, the observation equation and the fusion processing model, the fusion positioning of UWB, laser radar and IMU can be realized.
According to the invention, 1 vehicle-mounted UWB device, a plurality of UWB base stations, 1 IMU and 1 multi-line laser radar are adopted, and on the basis of constructing a downhole high-precision map, the laser radar is adopted to perform point cloud matching to obtain position and attitude data. By constructing a state equation with the IMU as a core and establishing UWB and laser radar observation equations, the fusion and calculation of the accurate position, speed and attitude of the unmanned vehicle in the underground mining environment without GNSS signals are realized, and accurate and stable navigation positioning information can be provided for an underground mining unmanned driving system.
It is apparent that the above examples are given by way of illustration only and are not limiting of the embodiments. Other variations or modifications of the above teachings will be apparent to those of ordinary skill in the art. It is not necessary here nor is it exhaustive of all embodiments. While still being apparent from variations or modifications that may be made by those skilled in the art are within the scope of the invention.