The invention discloses a mining equipment
positioning system and method based on
radar, ultra
wide band and inertial navigation, and the
system is characterized in that a multi-source sensor unit, a
data synchronization and collection module and a preprocessing module are connected in sequence, and a state fusion module is connected with the preprocessing module, a UWB
ranging error compensation module, a
radar motion estimation module and a
pose output module; the upper computer is connected with the
pose output module; the method comprises the steps of collecting multi-
source data;
timestamp alignment and format conversion
processing are carried out, and feature and speed constraint
information extraction is carried out on the
point cloud data; fitting correction is carried out on the
ranging data by adopting a weighted least square method, and position information is obtained through an error weighted average
algorithm;
feature matching is carried out, and in combination with a KISS-ICP
point cloud registration method, a speed
estimation result is obtained; carrying out data fusion, and obtaining a
pose estimation result by adopting an extended
Kalman filtering algorithm; and outputting equipment position and attitude information. According to the invention, high-precision real-
time estimation of the attitude and position information of the mining equipment can be realized.