Multi-modal data fusion coal mine tunnel simultaneous positioning and mapping method and system

Abstract

The invention belongs to the technical field of coal mine underground automatic driving, and provides a coal mine tunnel simultaneous positioning and mapping method and system based on multi-modal data fusion in order to solve the problem of reliable positioning and navigation of an automatic driving vehicle in a large coal mine underground tunnel. According to the invention, a high-precision mine environment three-dimensional point cloud map of a unified navigation coordinate system in a global range from a mine ground plaza to an underground haulage roadway can be established, and the six-degree-of-freedom pose of an underground autonomous vehicle is calculated. Accurate positioning of the underground automatic driving vehicle in a degraded environment such as the coal mine underground roadway is achieved, a coal mine underground roadway simultaneous positioning and mapping system is built, the posture state of the underground automatic driving vehicle can be accurately estimated in an underground space where global satellite navigation signals are lacked, the underground accurate positioning requirement is met, and the positioning accuracy of the underground automatic driving vehicle is improved. The method is of great significance in realizing application of intelligent unmanned transportation equipment in underground coal mines and promoting intelligent and unmanned construction of coal mine auxiliary transportation systems.

Description

Technical Field

[0001] The present invention belongs to the technical field of automatic driving in coal mines, and in particular relates to a method and system for simultaneous positioning and mapping of coal mine tunnels by fusion of multimodal data. Background Art

[0002] With the rapid development of intelligent coal mines in recent years, autonomous driving technology has been applied to coal mine production to improve transportation efficiency, save labor costs, and ensure operational safety. In some open-pit coal mines, autonomous driving transport fleets have been applied to actual production, bringing objective economic benefits and saving a lot of labor costs. However, in coal mines, autonomous driving technology is rarely used in actual transportation due to the challenging underground environment. Among the challenges faced by autonomous driving in coal mines, the reliable positioning and navigation of vehicles in underground tunnels without the Global Navigation Satellite System (GNSS) is the first problem to be solved. Generally speaking, almost all unmanned driving positioning solutions use a combination of absolute positioning and relative positioning to obtain the precise position of the vehicle. Unfortunately, underground vehicles usually cannot obtain absolute positioning. In addition, the state estimation of vehicles in underground mine tunnels is usually affected by perception-degraded environments such as self-symmetric structures, single features, and non-textured surfaces. Therefore, reliable positioning and navigation in underground tunnels of large coal mines still face great challenges. Summary of the invention

[0003] In order to solve at least one of the above technical problems existing in the prior art, the present invention provides a method and system for simultaneous positioning and mapping of coal mine tunnels using multimodal data fusion.

[0004] In the first aspect, the present invention is implemented by the following technical solution: a method for simultaneous positioning and mapping of coal mine tunnels by multimodal data fusion, comprising the following steps: S1: Obtain the longitude, latitude and altitude data of the vehicle's starting point, determine the true north direction and gravity direction, and construct a navigation coordinate system; S2: acquiring real-time inertial measurement data of the vehicle, constructing a motion equation based on the inertial measurement data, and then solving the system state variables at the current moment according to the motion equation, wherein the motion equation includes a nominal state motion equation and an error state motion equation, and the system state variables include a nominal state variable and an error state variable; S3: acquiring laser radar point cloud data and wheel speed meter sensor data, establishing an observation equation based on the laser radar point cloud data and the wheel speed meter sensor data, and iterating the error state variable, wherein the observation equation includes the laser radar observation equation and the wheel speed meter observation equation; S4: taking the nominal state variable as the initial value, the error state motion equation as the prior prediction, and the observation equation as the observation update, iterating the error state variable, adding the error state variable after each iteration to the nominal state variable, and then updating the current nominal state variable, and then judging whether the updated nominal state variable meets the convergence condition, the convergence condition is that the difference between the updated nominal state variable and the nominal state variable before the update meets the preset value, when the updated nominal state variable converges or reaches the maximum number of iterations, output the vehicle state estimation result, and execute step S5, otherwise the iteratively updated data is used as the initial state data to repeat step S4 until the nominal state variable converges; S5: Establishing a posture graph according to the vehicle state estimation result; S6: Obtain global position information, and simultaneously calculate the position coordinates of the current vehicle based on the global position information and the navigation coordinate system, add the global position information as a constraint edge to the pose graph, and globally optimize the pose graph.

[0005] Preferably, constructing the motion equation based on the inertial measurement data in step S2 includes: The inertial measurement data is compensated using an earth rotation compensation algorithm, and a motion equation is constructed based on the compensated inertial measurement data.

[0006] Preferably, the steps of establishing the laser radar observation equation in step S3 are: The vehicle's posture is calculated according to the nominal state variables, and then motion compensation is performed on the radar point cloud obtained by the laser radar by back propagation; Projecting the motion-compensated radar feature points to the navigation coordinate system to obtain the laser radar feature points in the navigation coordinate system; According to the point-surface residual formula and the measurement noise of the lidar, the lidar observation equation is obtained.

[0007] Preferably, the steps of establishing the wheel speed meter observation equation in step S3 are: Acquire wheel speed meter measurement data, establish a wheel speed meter coordinate system, and then acquire the vehicle speed vector according to the wheel speed meter coordinate system; Converting the speed vector from the wheel speed meter coordinate system to a speed observation value of the navigation coordinate system to obtain a speed prediction value under a nominal state variable; According to the residual model of wheel speed measurement and taking the measurement noise into consideration, the observation equation of wheel speed meter is obtained.

[0008] Preferably, step S4 further includes: The covariance of the error state variables is calculated and used as the prior distribution of the iterative error. According to the lidar observation equation and the wheel speed meter observation equation, the maximum a posteriori estimate of the error state variables is obtained.

[0009] Preferably, step S5 further includes: From the output results, key frames are selected as nodes to be optimized in the pose graph, and the pose transformations between adjacent key frames are used as constraint edges of the pose graph to establish a pose optimization graph.

[0010] Preferably, step S5 further includes: Perform loop detection optimization, obtain the identity information stored in the spherical target UWB tag, and determine whether the current area where the vehicle is located is an area that has been passed before based on the identity information. If it is an area that has been passed before, find a past key frame that matches the current key frame, and then use the nearest neighbor iteration algorithm to obtain the relative posture transformation relationship between the current key frame and the current area, and add it as a loop detection constraint between the current key frame and the past key frame, and then perform the global posture optimization of step S6.

[0011] According to a second aspect, the present invention further provides a system for simultaneous positioning and mapping of underground coal mine tunnels, which is used to implement the method for simultaneous positioning and mapping of coal mine tunnels with multimodal data fusion as described in the first aspect, comprising: a front-end odometer fusion module and a back-end nonlinear optimization module; The front-end odometer fusion module fuses sensor data of different modes using an iterative error Kalman filter framework to calculate the position and posture results of the vehicle during motion. The back-end nonlinear optimization module establishes a pose graph according to the pose results, and introduces constraint edges into the pose graph to optimize the pose graph.

[0012] Preferably, the front-end odometer fusion module includes: an explosion-proof laser radar, an intrinsically safe wheel speedometer and a fiber-optic strapdown inertial navigation system; Explosion-proof laser radar is used to obtain point cloud data of the vehicle operation process; The intrinsically safe wheel speed meter is used to obtain the vehicle speed information during the vehicle operation process; Optical fiber strapdown inertial navigation is used to obtain the acceleration and angular velocity in the navigation coordinate system during vehicle operation; The front-end odometer fusion module fuses the above data through the iterative error Kalman filter framework and calculates the position and posture results of the vehicle in six degrees of freedom during movement.

[0013] Preferably, the back-end nonlinear optimization module includes: a global position constraint factor, an odometer key frame factor and a loop detection factor; The global position constraint factors include global satellite positioning signals, downhole global position positioning signals and target global position signals; The odometer key frame factor is selected from the pose graph calculated by the front-end odometer fusion module according to a preset threshold; The loop detection factor is used to determine whether the vehicle has returned to the area it has passed before based on whether the spherical target signal can be received. A UWB tag is installed on the spherical target, and the UWB tag stores the identity information of each spherical target, and the identity information includes the identity number and the installation position of the spherical target.

[0014] Compared with the prior art, the present invention has the following beneficial effects: Through the method of the present invention, a high-precision three-dimensional point cloud map of the mine environment with a unified navigation coordinate system in the global range from the mine ground square to the underground transportation tunnel can be established, and the six-degree-of-freedom posture of the underground autonomous driving vehicle can be calculated at the same time, so as to realize the accurate positioning of the underground autonomous driving vehicle in the perception-degraded environment of the underground coal mine tunnel. In addition, a simultaneous positioning and mapping system for the underground coal mine tunnel is built, which can accurately estimate the posture state of the underground autonomous driving vehicle in the underground space where the global satellite navigation signal is missing, and meet the problem of precise underground positioning. It is of great significance to realize the application of intelligent unmanned transportation equipment in underground coal mines and promote the intelligent and unmanned construction of coal mine auxiliary transportation systems. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0016] Figure 1 Schematic diagram of the structure of a simultaneous positioning and mapping system for underground coal mine tunnels according to an embodiment of the present invention; Figure 2 It is a schematic diagram of the position installation of a spherical target according to an embodiment of the present invention; Figure 3 It is a schematic diagram of the process of the method for simultaneous positioning and mapping of coal mine tunnels by multimodal data fusion according to an embodiment of the present invention; Figure 4 It is an algorithm flow chart of an embodiment of the present invention. DETAILED DESCRIPTION

[0017] In conjunction with the drawings in the embodiments of the present invention, the technical solutions in the embodiments of the present invention are clearly and completely described. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other implementation methods obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0018] It should be noted that the structures, proportions, sizes, etc. illustrated in the drawings of this specification are only used to match the contents disclosed in the specification so that people familiar with this technology can understand and read them. They are not used to limit the conditions under which the present invention can be implemented, and therefore have no substantive technical significance. Any structural modification, change in proportional relationship or adjustment of size should fall within the scope of the technical contents disclosed in the present invention without affecting the effects and purposes that can be achieved by the present invention. It should be noted that in this specification, relational terms such as first and second are only used to distinguish one entity from several other entities, and do not necessarily require or imply any actual relationship or order between these entities.

[0019] Simultaneous localization and mapping (SLAM) has been widely used in household robots, self-driving cars, and autonomous exploration to obtain six-degree-of-freedom state estimation and environmental maps. With the growing demand for positioning and navigation in coal mines, the simultaneous localization and mapping of underground environments has attracted more and more attention and is a key technology for achieving navigation in an environment without GNSS signals. In the absence of global positioning signals, the underground tunnel map established by SLAM can provide autonomous vehicles with prior global information to achieve positioning and navigation. The transportation tunnels in coal mines are usually long and continue to extend as coal mine production proceeds. In order to enable robots and autonomous vehicles to travel in such a tunnel-like environment with degraded perception, accurate prior global maps and auxiliary positioning methods are required. However, mapping these large-scale, dynamically changing underground narrow tunnels based on traditional SLAM solutions is prone to performance deficiencies and failures: poor lighting conditions make the deployment of visual SLAM methods challenging; the lack of perceptual features (underground tunnel corridors) and the possible presence of high-density occlusions (dust and fog) cause the failure of lidar-based laser SLAM methods that rely on features or scan matching; the complex road conditions and rugged terrain of underground tunnels cause noise from inertial sensors and wheel odometers. In addition, sensors used for state estimation and mapping in underground coal mine environments are explosion-proof in design to ensure production safety. Whether limiting sensor power through intrinsically safe design or adding explosion-proof housings, it will have a negative impact on the performance of the original sensor, thereby affecting mapping and positioning accuracy.

[0020] To solve the above problems, an embodiment of the present invention provides an embodiment of a method and system for simultaneous positioning and mapping of coal mine tunnels with multimodal data fusion. It should be noted that the steps shown in the flowchart of the accompanying drawings can be executed in a computer system such as a set of computer executable instructions, and although a logical order is shown in the flowchart, in some cases, the steps shown or described may be executed in an order different from that shown here.

[0021] The embodiment of the present invention provides a system for simultaneously positioning and mapping underground mine tunnels, and its specific structure is as follows: Figure 1 As shown, it includes: a front-end odometer fusion module and a back-end nonlinear optimization module; The front-end odometer fusion module fuses sensor data of different modes using an iterative error Kalman filter framework to calculate the position and posture results of the vehicle during motion. The back-end nonlinear optimization module establishes a pose graph according to the pose results, and introduces constraint edges into the pose graph to optimize the pose graph.

[0022] In this embodiment, the front-end odometer fusion module fuses the sensor data of different modes using the iterative error Kalman filter framework, and calculates the six degrees of freedom posture results of the autonomous driving vehicle during movement. The back-end nonlinear optimization module optimizes the posture estimation results provided by the front end by establishing a posture graph and introducing multiple constraint factors, thereby eliminating the cumulative error caused by the front-end odometer due to the degraded environment of the tunnel perception. The front and back-end modules work together to achieve the precise positioning function of the autonomous driving vehicle when it runs long distances in the degraded environment of the underground coal mine tunnel. In the embodiment of the present invention, the six degrees of freedom are divided into three translational degrees of freedom and three rotational degrees of freedom. The translational degree of freedom means that the object can perform translational motion on the three mutually perpendicular coordinate axes of X, Y, and Z, that is, translation along the X-axis, translation along the Y-axis, and translation along the Z-axis; the rotational degrees of freedom are Roll, Pitch, and Yaw, where Roll: rotation around the X-axis; Pitch: rotation around the Y-axis; Yaw: rotation around the Z-axis.

[0023] Optionally, the front-end odometer fusion module includes: an explosion-proof laser radar, an intrinsically safe wheel speedometer and a fiber-optic strapdown inertial navigation system; the explosion-proof laser radar is used to obtain point cloud data of the vehicle operation process; the intrinsically safe wheel speedometer is used to obtain vehicle speed information during the vehicle operation process; the fiber-optic strapdown inertial navigation system is used to obtain acceleration and angular velocity in the navigation coordinate system during the vehicle operation process; the front-end odometer fusion module fuses the above data through an iterative error Kalman filter framework, and calculates the posture results of the six degrees of freedom of the vehicle during the movement.

[0024] In this embodiment, after the explosion-proof laser radar obtains the point cloud data of the tunnel environment, an environmental point cloud with consistent density within a unit volume is obtained through a downsampling algorithm. Then, based on the acceleration and angular velocity values ​​of the laser radar in the navigation coordinate system during the movement of the laser radar measured by the fiber-optic strapdown inertial navigation, the motion distortion of each point in the environmental point cloud after downsampling due to the movement of the laser radar within a fixed time period is calculated and compensated to obtain the point cloud data of the vehicle operation process.

[0025] The intrinsically safe wheel speed meter is installed on the left and right wheels. It can obtain the number of revolutions of the wheels on both sides per unit time, thereby inferring the rotation speed of the wheels on both sides and obtaining the vehicle speed information by taking the average.

[0026] After initialization (stationary state), the optical fiber strapdown inertial navigation system can autonomously determine the true north direction and gravity direction in the underground space, thereby determining the motion posture of the autonomous driving vehicle in the earth-fixed coordinate system (northeast sky ENU). The system initialization process will also introduce the coordinates of the vehicle's motion starting point in the geographic coordinate system (latitude, longitude and altitude LLA), thereby determining the acceleration and angular velocity of the vehicle in the navigation coordinate system during operation. In this embodiment, the optical fiber strapdown inertial navigation system is mainly composed of an accelerometer and an optical fiber gyroscope. The accelerometer is used to measure the acceleration of the vehicle during operation. The speed and displacement distance of the vehicle during operation can be obtained by integrating the acceleration. The optical fiber gyroscope is used to test the acceleration. The rotation angle can be obtained according to its integration over time. The observation noise of the two is independent of each other.

[0027] In this embodiment, the point cloud data obtained by the explosion-proof laser radar, the vehicle speed information obtained by the intrinsically safe wheel speed meter, and the carrier acceleration and angular velocity data measured by the fiber-optic strapdown inertial navigation are fused using the iterative error Kalman filter framework to calculate the position and posture results of the six degrees of freedom (X, Y, Z, Roll, Pitch, Yaw) of the autonomous driving vehicle during movement.

[0028] Optionally, the back-end nonlinear optimization module includes: a global position constraint factor, an odometer key frame factor and a loop detection factor; the global position constraint factor includes a global satellite positioning signal, an underground global position positioning signal and a target global position signal; the odometer key frame factor is selected from the pose graph calculated by the front-end odometer fusion module according to a preset threshold; the loop detection factor is used to determine whether the vehicle has returned to the area passed before based on whether the spherical target signal can be received, and a UWB tag is installed on the spherical target, and the UWB tag stores the identity information of each spherical target, and the identity information includes the identity number and the installation position of the spherical target.

[0029] In this embodiment, the global position constraint factor is to add the global position measurement value to the pose graph to achieve the elimination of the accumulated error of the odometer, including the global satellite positioning signal provided by the GNSS system above the ground square, the underground global position positioning signal provided by the UWB positioning network in the coal mine, and the target global position signal provided by the spherical target pre-deployed and installed in the coal mine tunnel.

[0030] The odometry keyframe factor is selected from the pose graph estimated by the front-end odometry fusion module according to the preset threshold. For example, if the moving distance between the current keyframe and the past keyframe exceeds 1 meter or the yaw angle exceeds 5 degrees, the current keyframe will be the main object of back-end optimization, and other frames between the two keyframes will be removed and not optimized in the pose graph, thereby reducing the amount of calculation and improving the optimization performance.

[0031] The loop detection factor is to determine whether the vehicle returns to the position it has passed before, and then optimize the area and current posture globally based on the judgment result. However, due to the influence of the similar structure of the underground tunnels, false or erroneous loop judgments are prone to occur, resulting in global optimization failure. Therefore, it is necessary to eliminate false loops and then perform point cloud matching. By placing spherical targets with UWB tags at locations where loops are likely to occur in underground tunnels, such as forks and turns, the UWB receiving station arranged on the autonomous driving vehicle can obtain the information of the target UWB tag when it approaches the spherical target, thereby determining whether it has returned to the area passed before, thereby effectively avoiding the occurrence of false or erroneous loops.

[0032] In this embodiment, the spherical targets are a series of white hollow spheres of equal size and known dimensions, with shells made of high-density reflective materials. They are easily distinguished from the tunnel background environment under laser radar illumination, and UWB tags are installed on them. The tags store the identity number of each spherical target and the position information of the center of each spherical target in the global coordinate system. The spherical targets are sparsely installed in the underground tunnel at a certain distance. Figure 2 As shown, in this embodiment, one UWB tag is arranged every 100 meters in the long underground tunnel, and it also needs to be arranged at large-angle turns and intersections in the tunnel. The effective range of the UWB tag on the spherical target is a fan-shaped area centered on the center of the spherical target, and the effective range of each spherical target is the same. When the autonomous driving vehicle enters the effective range of the spherical target UWB tag, the system matches the point cloud samples of the spherical target with the environment of the area, thereby identifying the relative position of the spherical target and the autonomous driving vehicle, and then uses the global position information stored in the spherical target to infer the global position of the autonomous driving vehicle itself, thereby eliminating and optimizing the accumulated error.

[0033] The embodiment of the present invention constructs a front-end odometer fusion module based on filtering and a back-end nonlinear optimization module based on optimization. The front-end odometer fusion module uses a tightly coupled iterative error state Kalman filter (iESKF, Iterative Error State Kalman Filter) to fuse the measurement values ​​of the lidar point, fiber strapdown inertial navigation and wheel speed meter, which can effectively alleviate the lidar degradation problem in the underground tunnel environment. At the same time, in order to eliminate the motion distortion during the sampling of the lidar point cloud and assist in attitude estimation, the embodiment of the present invention uses the precise inertial measurement data obtained by the high-precision mine strapdown inertial navigation system. The back-end nonlinear optimization module uses a spherical target equipped with a UWB tag to assist in loop detection. The position information and identity information of the spherical target are stored in their respective UWB tags. According to the point cloud registration algorithm, the system can identify the spherical targets pre-installed at the intersection and turning position of the underground tunnel. The UWB module installed on the autonomous driving vehicle can obtain the position and identity information of the spherical target, thereby triggering the global position constraint and correct loop detection, effectively eliminating the cumulative error.

[0034] The embodiment of the present invention provides a method for simultaneous positioning and mapping of coal mine tunnels by multi-modal data fusion, which can be used for the simultaneous positioning and mapping system of coal mine tunnels in the above embodiment. Figure 3 A method for simultaneously positioning and mapping coal mine tunnels by multimodal data fusion according to an embodiment of the present invention is provided. Figure 3 As shown, the specific steps include: S1: Obtain the longitude, latitude and altitude data of the vehicle's starting point, determine the true north direction and gravity direction, and build a navigation coordinate system.

[0035] In this embodiment, when the autonomous driving vehicle is in a ground square, the GNSS positioning signal can be used to determine the longitude, latitude and altitude data of its own geographic coordinate system as the origin of the navigation coordinate system. If the autonomous driving vehicle is underground, the UWB positioning network can provide positioning signals. If there is no such positioning signal, it is necessary to use a spherical target pre-installed in the tunnel to determine the relative position of itself and the target through point cloud matching. The longitude, latitude and altitude data of the center of the sphere in the geographic coordinate system stored in the UWB tag of the spherical target can be used to deduce its own position and use it as the origin of the navigation coordinate system.

[0036] Using the true north direction and gravity direction determined by the optical fiber strapdown inertial navigation system for mining, a navigation coordinate system with the northeast sky of the earth-fixed coordinate system as the coordinate direction is established, that is, the coordinate system takes the geographic east direction as the X-axis direction, the north direction as the Y-axis direction, and the direction opposite to gravity as the Z-axis direction, which belongs to the right-handed coordinate system. It should be noted that the embodiment of the present invention takes the northeast sky as an example, which does not mean that only the northeast sky coordinate system can be used. Other coordinate systems, such as the northeast ground, can also be used as needed. The coordinate systems can be interchanged through coordinate transformation and are equivalent. The present invention is not limited to this, and it does not affect the positioning result.

[0037] S2: Acquire real-time inertial measurement data of the vehicle, construct a motion equation based on the inertial measurement data, and then solve the system state variables at the current moment according to the motion equation, wherein the motion equation includes a nominal state motion equation and an error state motion equation, and the system state variables include a nominal state variable and an error state variable.

[0038] Optionally, constructing the motion equation based on the inertial measurement data includes: using an earth rotation compensation algorithm to compensate the inertial measurement data, and constructing the motion equation according to the compensated inertial measurement data.

[0039] In this embodiment, after the vehicle starts to move, the optical fiber strapdown inertial navigation system for mining obtains the three-axis acceleration and three-axis angular velocity information of the carrier, and removes the influence of the earth's rotation on the angular velocity through the earth's rotation compensation algorithm to obtain more accurate inertial measurement data. Then, based on the compensated acceleration and angular velocity data, a motion equation is constructed, i.e., a prediction equation in an iterative error Kalman filter, which divides the system state variables at the current moment into two parts: a nominal state variable and an error state variable.

[0040] In this embodiment, the acceleration and angular velocity data after the rotation of the earth are compensated, and the nominal state variable values ​​of the position, velocity and attitude of the system at each moment are obtained according to the integral operation, and the nominal state variable does not consider the influence of the system noise. The error generated by the measurement noise is put into the error state variable for processing. In this embodiment, the error generated by the measurement noise can be modeled as Gaussian noise, but the present invention is not limited to this.

[0041] System state variables It consists of nominal state variables and error state variables, which are defined as follows: In the formula, is the system state variable; the position of the system in the navigation coordinate system is , the speed is , is a three-dimensional real number set, and the posture is , is a special orthogonal group in three-dimensional space. The acceleration measurement deviation of fiber strapdown inertial navigation is And the angular velocity measurement deviation is , the acceleration due to gravity is , is a nominal state variable; is the error state variable; is the nominal state variable of the position; is the nominal state variable of speed; is the nominal state variable of the posture; is the nominal state variable of the acceleration measurement deviation; is the nominal state variable of the angular velocity measurement deviation; is the nominal state variable of gravitational acceleration; is the error state variable of position; is the error state variable of speed; is the error state variable of the posture; is the error state variable of acceleration measurement deviation; is the error state variable of angular velocity measurement deviation; is the error state variable of gravity acceleration, Represents generalized addition.

[0042] The equations of motion for the nominal state variables are: According to this formula, The nominal state of the system at that moment is derived The nominal state variables of the system after time t.

[0043] In the formula, represents an exponential map; and They respectively represent the acceleration and angular velocity values ​​measured by the optical fiber strapdown inertial navigation system for mining in the strapdown inertial navigation coordinate system; is the local Earth rotation speed, ,in is the Earth's rotation constant, which is rad / s, is the local latitude value. In this embodiment, the inertial navigation coordinate system is the strapdown inertial navigation coordinate system automatically generated after the mining optical fiber strapdown inertial navigation collects the corresponding data, and is represented by the symbol express.

[0044] In this embodiment, the motion equation of the error state variable is: In the formula, , for The Lie algebra of and Represent the noise of velocity and posture respectively, and They represent the accelerometer acceleration and the observation noise of the fiber optic gyroscope of the mine-used fiber optic strapdown inertial navigation system respectively.

[0045] Combining the above nominal state variable motion equation and error state variable equation, it can be concluded that the system The prediction equation of the error state at the moment is: In the formula, for The covariance of the system at a given moment is the degree of uncertainty of the system; is the propagation matrix of the error state; is the transpose of the propagation matrix of the error state; is the covariance matrix of motion noise; The expected value is 0 and the variance is Normal distribution of is motion noise, modeled as a Gaussian distribution.

[0046] S3: Obtain laser radar point cloud data and wheel speed meter sensor data, establish observation equations based on the laser radar point cloud data and the wheel speed meter sensor data, and iterate the error state variables, the observation equations including the laser radar observation equations and the wheel speed meter observation equations.

[0047] Optionally, the vehicle's posture is calculated based on the nominal state variables, and then the radar point cloud acquired by the lidar is motion compensated by back propagation; the motion compensated radar feature points are projected to the navigation coordinate system to obtain the lidar feature points in the navigation coordinate system; and the lidar observation equation is obtained based on the point-to-surface residual formula and the measurement noise of the lidar.

[0048] In this embodiment, the nominal state motion equation in step S2 is first used to calculate the posture, and then each feature point in a frame of radar point cloud obtained by a single scan of the laser radar is converted to the same moment by back propagation, thereby completing the motion compensation of the radar point cloud.

[0049] In this embodiment, a laser radar coordinate system is constructed, and then the feature points in the laser radar coordinate system are projected to the navigation coordinate system to obtain the laser radar feature points in the navigation coordinate system.

[0050] The transformation equation projected to the navigation coordinate system is: in, Indicates the strapdown inertial navigation coordinate system To navigation coordinates The pose transformation matrix, Indicates the coordinate system from the laser radar To the Strapdown Inertial Navigation Coordinate System The pose transformation matrix of To express attitude, Indicates the XYZ axis transformation from the strapdown inertial navigation coordinate system to the navigation coordinate system. Indicates the XYZ axis transformation from the laser radar coordinate system to the strapdown inertial navigation coordinate system. represents the rotation position, Indicates the transformation of pitch angle, roll angle and heading angle from strapdown inertial navigation coordinate system to navigation coordinate system. Indicates the pitch angle, roll angle and heading angle transformation from the laser radar coordinate system to the strapdown inertial navigation coordinate system. represents the number of LiDAR feature points, represents the laser radar feature points in the navigation coordinate system, Indicates the laser radar coordinate system feature points.

[0051] The residual equation of the point surface measured by LiDAR is: In the formula, is the point-surface residual measured by the lidar, where and Represent feature points The normal vector and center point of the nearest neighbor plane in the navigation coordinate system, For feature points The transposed matrix of the normal vector of the nearest neighbor plane in the navigation coordinate system.

[0052] According to the point-surface residual equation, the measurement noise of the laser radar is taken into account , and do a first-order Taylor expansion around the nominal state variable, and the observation equation of the laser radar is obtained as follows: In the formula, is the lidar measurement residual, yes about The error state variable at time The Jacobian matrix of ; is the measurement noise, with a mean of 0 and a variance of Gaussian distribution of represents an antisymmetric matrix, is a 1-by-3 zero vector.

[0053] Optionally, the steps for establishing the wheel speed meter observation equation in step S3 are: obtaining the measurement data of the wheel speed meter sensor, establishing the wheel speed meter coordinate system, and then obtaining the vehicle speed vector according to the wheel speed meter coordinate system; converting the speed vector from the wheel speed meter coordinate system into the speed observation value of the navigation coordinate system, and obtaining the speed prediction value under the nominal state variable; according to the residual model of the wheel speed measurement, while considering the measurement noise, obtaining the wheel speed meter observation equation.

[0054] In this embodiment, the wheel speed meter is installed on the left and right wheels, and the number of pulses of the wheels on both sides rotating within a certain period of time can be obtained respectively, thereby obtaining the driving speed of the autonomous driving vehicle. The average value of the wheel speed meter on both sides is taken as the vehicle speed observation, then Residual model of time, velocity observation Modeled as: In the formula, ,in, , , is the number of pulses rotating within a certain period of time, is the tire radius, s is the total number of pulses per tire rotation. is the interval time, Indicates the left wheel, Indicates the right wheel; is the wheel speed meter coordinate system; Represents the Strapdown Inertial Navigation Coordinate System To the navigation coordinate system The rotation transformation, Represents the tachometer coordinate system To the Strapdown Inertial Navigation Coordinate System The rotation transformation, represents the predicted value of the velocity in the nominal state motion equation, Represents the Strapdown Inertial Navigation Coordinate System To the navigation coordinate system The nominal state quantity of the rotation transformation, for The number of rotation pulses at a time.

[0055] In this embodiment, according to the residual model of wheel speed measurement, the measurement noise of the wheel speed meter is also considered. , and do a first-order Taylor expansion around the nominal state variable to obtain the observation equation of the wheel speed meter: In the formula, is a 3 by 3 zero matrix, is the 3 by 3 identity matrix, for about The error state variable at time The Jacobian matrix of for The nominal state quantity of the number of rotation pulses at the moment, is the wheel speed measurement residual, It means partial derivative.

[0056] S4: Use the nominal state variable as the initial value, the error state motion equation as the prior prediction, and the observation equation as the observation update, iterate the error state variable, add the error state variable after each iteration to the nominal state variable, and then update the current nominal state variable, and then judge whether the updated nominal state variable meets the convergence condition, and the convergence condition is that the difference between the updated nominal state variable and the nominal state variable before the update meets the preset value. When the updated nominal state variable converges or reaches the maximum number of iterations, output the vehicle state estimation result and execute step S5, otherwise use the iterative updated data as the initial state data and repeat step S4 until the nominal state variable converges.

[0057] Optionally, step S4 further includes: calculating the covariance of the error state variable, taking it as the prior distribution of the iterative error, and obtaining the maximum a posteriori estimate of the error state variable according to the laser radar observation equation and the wheel speed meter observation equation.

[0058] In this embodiment, the error state variable of the system Iterate, Represents the number of iterations. After each iteration, the error state variable after each iteration is added to the nominal state variable to calculate the nominal state variable. Update until the updated nominal state variables meet the convergence condition , is the convergence threshold, represents generalized subtraction, which is a generalized addition The inverse operation of .

[0059] In this embodiment, an iterative error Kalman filter is used to iteratively update the nominal state variables and the error state variables. During the iterative update process, the covariance of the error state variables is used as the prior distribution of the iterative error Kalman filter, and the measurement residuals of the laser radar and the wheel speed meter are used as the observation equation to determine the maximum a posteriori estimate of the error state variables. At the same time, the update value of the error variable is determined based on the preset Kalman gain and the convergence condition of the error state variable.

[0060] According to the updating process of the error state variables, the initial value of the nominal state variables for the next iteration is obtained, the residuals of the lidar measurement and the residuals of the wheel speed meter measurement are calculated again, and the iterative update of the error state variables is repeated until the nominal state variables after the next update also meet the convergence condition. After convergence, the optimized nominal state variables and the corresponding covariance are used as the initial state at the next moment to start a new round of prediction update process.

[0061] S5: Establish a posture graph according to the vehicle state estimation result.

[0062] Optionally, from the output results, key frames are selected as nodes to be optimized in the pose graph, and the pose transformations between adjacent key frames are used as constraint edges of the pose graph to establish a pose optimization graph.

[0063] Optionally, loop detection optimization is performed to obtain the identity information stored in the spherical target UWB tag, and determine whether the vehicle is in an area that has been passed before based on the identity information. If it is an area that has been passed before, the current key frame is matched to the past key frame, and then the nearest neighbor iteration algorithm is used to obtain the relative posture transformation relationship between the current key frame and the current area, and it is added as a loop detection constraint between the current key frame and the past key frame, and then the global posture optimization of step S6 is performed.

[0064] In this embodiment, according to a preset threshold condition, a key frame is selected from the output result as a node to be optimized in the pose graph, and the pose transformation between adjacent key frames is used as a constraint edge of the pose graph to establish a pose optimization graph. In the embodiment of the present invention, the selection of key frames can be determined based on the accumulation of relative pose transformations, such as when the accumulated displacement exceeds 1 meter, or when the pose angle transformation exceeds 5°.

[0065] In this embodiment, the UWB tag installed on the spherical target stores the identity information and global position information of the target. When the UWB module installed on the autonomous driving vehicle receives the target position information sent by the spherical target UWB tag, the system will use the pre-stored target laser point cloud samples and the surrounding environment point cloud for matching. After matching the spherical target, the relative position of the vehicle itself and the spherical target is calculated, and then the current global position information of the vehicle is inferred based on the received global position coordinates of the spherical target in the navigation coordinate system, and added to the pose graph as a constraint edge for optimization.

[0066] In this embodiment, Figure 2As shown in the figure, spherical targets are arranged in areas prone to loop closure, such as lane intersections and turning intersections. When a vehicle passes through the area, it can determine whether it is an area that has been visited before by the identity information stored in the received spherical target UWB tag. If the target is visited again, the past key frame that is closest to the current key frame in Euclidean distance but far away in time is found as a matching frame pair for loop detection. The point clouds of the previous and next frames of the past frame are spliced ​​into a local map. The relative pose transformation relationship between the current frame and the local map is calculated through the nearest neighbor iteration (ICP) algorithm, and it is added as a loop detection constraint between the current frame and the past frame to perform global pose optimization.

[0067] S6: Obtain global position information, and simultaneously calculate the position coordinates of the current vehicle based on the global position information and the navigation coordinate system, add the global position information as a constraint edge to the pose graph, and globally optimize the pose graph.

[0068] In this embodiment, when the system is able to receive global position information, such as when GNSS signals are available on the ground square of the mine, and when UWB positioning base stations are deployed in the underground tunnels of the coal mine to provide underground positioning signals for vehicles, effective GNSS / UWB positioning signals can be directly added as global position constraints to the back-end pose graph optimization. If the wireless global positioning signal is not available, the global position information provided by the spherical target deployed in the tunnel environment is relied on. The global position information of the spherical target can be based on the exploration point coordinate information left during the tunnel excavation process, and then the global position coordinates of the target sphere center can be marked using a total station. The exploration point coordinate information in the embodiment of the present invention is the position information under the geographic coordinate system WGS84. If other coordinate systems are used, the coordinates can also be converted to the navigation coordinate system, and the present invention is not limited to this.

[0069] The present invention relates to the technical field of automatic driving in coal mines, and discloses a method and system for simultaneous positioning and mapping of coal mine tunnels by multi-modal data fusion. Figure 4As shown, after the system is initialized, the front end establishes the navigation coordinate system, takes the longitude and latitude under the coordinate system of the starting point of the movement as the starting point of the coordinate system, and takes the northeast sky of the ground-fixed coordinate system as the direction of the coordinate axis. Then, the motion equation is established to solve the nominal state variables and error state variables at the current moment, and then the observation equation is established to obtain the point-surface residuals of the laser radar feature points and the speed residuals measured by the wheel speed meter. Then, the error state variables are iterated, the nominal state variables are updated, and the system state variables are judged whether they converge according to the iterative update results. If not, the iterative update is performed again until the system state variables converge, and then the front end outputs the results. The data is transmitted to the backend, which selects the key frames in the output results according to the predetermined threshold to establish a pose graph, takes the key frames as nodes to be optimized, and the pose transformation between frames as constraint edges. Then the autonomous driving vehicle optimizes the global position according to whether it can receive the GPS / UWB positioning signal and the point cloud matching results of the spherical target position. At the same time, it determines whether there is a false loop based on the identity information of the spherical target. When there is a false loop, loop detection optimization is performed. Finally, the environmental map is updated according to the optimization results to obtain the position map of the autonomous driving vehicle, so as to realize the accurate positioning of the autonomous driving vehicle in the perception-degraded environment of the coal mine tunnel. The algorithm framework includes a front-end odometer based on the laser inertial navigation wheel speed fusion of the iESKF iterative error state Kalman filter, and a nonlinear pose graph optimization backend driven by multiple influencing factors. In underground spaces where global satellite navigation signals are missing, the method of the present invention can effectively alleviate the problem that traditional simultaneous positioning and mapping technologies are difficult to accurately estimate the posture state of autonomous driving vehicles in underground coal mine tunnels in a degraded environment with symmetrical geometric structures and repetitive and single environmental features, and cannot meet the needs of precise positioning underground. This is of great significance for realizing the application of intelligent unmanned transportation equipment in underground coal mines and promoting the intelligent and unmanned construction of coal mine auxiliary transportation systems.

[0070] The above is only a preferred specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily thought of by a person skilled in the art within the technical scope disclosed by the present invention should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention shall be based on the protection scope of the claims.

Claims

1. A method for simultaneous positioning and mapping of coal mine tunnels using multimodal data fusion, characterized in that: The steps include: S1: Obtain the longitude, latitude and altitude data of the vehicle's starting point, determine the true north direction and gravity direction, and construct a navigation coordinate system; S2: acquiring real-time inertial measurement data of the vehicle, constructing a motion equation based on the inertial measurement data, and then solving the system state variables at the current moment according to the motion equation, wherein the motion equation includes a nominal state motion equation and an error state motion equation, and the system state variables include a nominal state variable and an error state variable; S3: acquiring laser radar point cloud data and wheel speed meter sensor data, establishing an observation equation based on the laser radar point cloud data and the wheel speed meter sensor data, and iterating the error state variable, wherein the observation equation includes the laser radar observation equation and the wheel speed meter observation equation; S4: taking the nominal state variable as the initial value, the error state motion equation as the prior prediction, and the observation equation as the observation update, iterating the error state variable, adding the error state variable after each iteration to the nominal state variable, and then updating the current nominal state variable, and then judging whether the updated nominal state variable meets the convergence condition, the convergence condition is that the difference between the updated nominal state variable and the nominal state variable before the update meets the preset value, when the updated nominal state variable converges or reaches the maximum number of iterations, output the vehicle state estimation result, and execute step S5, otherwise the iteratively updated data is used as the initial state data to repeat step S4 until the nominal state variable converges; S5: Establishing a posture graph according to the vehicle state estimation result; S6: Obtain global position information, and simultaneously calculate the position coordinates of the current vehicle based on the global position information and the navigation coordinate system, add the global position information as a constraint edge to the pose graph, and globally optimize the pose graph.

2. The method for simultaneous positioning and mapping of coal mine tunnels by multimodal data fusion according to claim 1 is characterized in that: In step S2, a motion equation is constructed based on the inertial measurement data, including: The inertial measurement data is compensated using an earth rotation compensation algorithm, and a motion equation is constructed based on the compensated inertial measurement data.

3. The method for simultaneous positioning and mapping of coal mine tunnels by multimodal data fusion according to claim 1 is characterized in that: The steps for establishing the laser radar observation equation in step S3 are: The vehicle's posture is calculated according to the nominal state variables, and then motion compensation is performed on the radar point cloud obtained by the laser radar by back propagation; Projecting the motion-compensated radar feature points to the navigation coordinate system to obtain the laser radar feature points in the navigation coordinate system; According to the point-surface residual formula and the measurement noise of the lidar, the lidar observation equation is obtained.

4. The method for simultaneous positioning and mapping of coal mine tunnels by multimodal data fusion according to claim 1, characterized in that: The steps for establishing the wheel speed meter observation equation in step S3 are: Acquire wheel speed meter measurement data, establish a wheel speed meter coordinate system, and then acquire the vehicle speed vector according to the wheel speed meter coordinate system; Converting the speed vector from the wheel speed meter coordinate system into a speed observation value of the navigation coordinate system to obtain a speed prediction value under a nominal state variable; According to the residual model of wheel speed measurement and taking the measurement noise into consideration, the observation equation of wheel speed meter is obtained.

5. The method for simultaneous positioning and mapping of coal mine tunnels by multimodal data fusion according to claim 1, characterized in that: Step S4 also includes: The covariance of the error state variables is calculated and used as the prior distribution of the iterative error. According to the lidar observation equation and the wheel speed meter observation equation, the maximum a posteriori estimate of the error state variables is obtained.

6. The method for simultaneous positioning and mapping of coal mine tunnels by multimodal data fusion according to claim 1, characterized in that: Step S5 also includes: From the output results, key frames are selected as nodes to be optimized in the pose graph, and the pose transformations between adjacent key frames are used as constraint edges of the pose graph to establish a pose optimization graph.

7. The method for simultaneous positioning and mapping of coal mine tunnels by multimodal data fusion according to claim 6, characterized in that: Step S5 also includes: Perform loop detection optimization, obtain the identity information stored in the spherical target UWB tag, and determine whether the current area where the vehicle is located is an area that has been passed before based on the identity information. If it is an area that has been passed before, find a past key frame that matches the current key frame, and then use the nearest neighbor iteration algorithm to obtain the relative posture transformation relationship between the current key frame and the current area, and add it as a loop detection constraint between the current key frame and the past key frame, and then perform the global posture optimization of step S6.

8. A system for simultaneously positioning and mapping underground coal mine tunnels, used to implement the method for simultaneously positioning and mapping underground coal mine tunnels by multimodal data fusion as described in any one of claims 1 to 7, characterized in that: include: Front-end odometer fusion module and back-end nonlinear optimization module; The front-end odometer fusion module fuses sensor data of different modes using an iterative error Kalman filter framework to calculate the position and posture results of the vehicle during motion. The back-end nonlinear optimization module establishes a pose graph according to the pose results, and introduces constraint edges into the pose graph to optimize the pose graph.

9. The system for simultaneous positioning and mapping of underground coal mine tunnels according to claim 8, characterized in that: The front-end odometer fusion module includes: explosion-proof laser radar, intrinsically safe wheel speed meter and optical fiber strapdown inertial navigation; Explosion-proof laser radar is used to obtain point cloud data of the vehicle operation process; The intrinsically safe wheel speed meter is used to obtain the vehicle speed information during the vehicle operation process; Optical fiber strapdown inertial navigation is used to obtain the acceleration and angular velocity in the navigation coordinate system during vehicle operation; The front-end odometer fusion module fuses the above data through the iterative error Kalman filter framework and calculates the position and posture results of the vehicle in six degrees of freedom during movement.

10. The simultaneous positioning and mapping system for underground coal mine tunnels according to claim 8, characterized in that: The back-end nonlinear optimization module includes: a global position constraint factor, an odometer key frame factor, and a loop detection factor; The global position constraint factors include global satellite positioning signals, downhole global position positioning signals and target global position signals; The odometer key frame factor is selected from the pose graph calculated by the front-end odometer fusion module according to a preset threshold; The loop detection factor is used to determine whether the vehicle has returned to the area it has passed before based on whether the spherical target signal can be received. A UWB tag is installed on the spherical target, and the UWB tag stores the identity information of each spherical target, and the identity information includes the identity number and the installation position of the spherical target.

Images