A method and system for underground vehicle positioning based on visual target information fusion

Abstract

The present application discloses an underground vehicle positioning method based on visual target information fusion, which involves the following steps: arranging visual targets at preset intervals on the sidewalls and top of an underground tunnel; collecting multi-source sensor data; extracting the target ID and the pixel coordinates of the target in the image coordinate system; converting the extracted pixel coordinates of the target into three-dimensional coordinates in the camera coordinate system using a pre-calibrated camera intrinsic parameter matrix K and extrinsic parameter matrix based on a vehicle-mounted visual camera; querying the target coordinates of the corresponding target in the world coordinate system from a database based on the target ID; calculating the position and posture of the vehicle relative to the world coordinate system by solving the inverse projection problem based on the target coordinates and the three-dimensional coordinates, and obtaining the visual target positioning result; performing multi-sensor fusion based on the recognition status of the visual target to obtain the vehicle positioning result. The present application solves the problems of large cumulative positioning errors in long and straight underground tunnels and positioning mutations caused by misidentification of visual targets.

Description

Technical Field

[0001] The present application relates to the field of underground vehicle positioning, and in particular to an underground vehicle positioning method and system based on visual target information fusion. Background Art

[0002] With the advancement of smart mine construction, autonomous driving technology for underground vehicles has become a crucial tool for improving mine production efficiency and operational safety. Accurate and reliable vehicle positioning is a key prerequisite for autonomous driving underground, directly impacting the implementation of subsequent functions such as path planning and obstacle avoidance. However, the unique working conditions underground present numerous challenges for vehicle positioning.

[0003] Currently, the following technical solutions are primarily used for underground vehicle positioning: First, a positioning solution based on laser SLAM technology. This solution first uses lidar to construct a point cloud map of the underground tunnel, then achieves repositioning by matching real-time scanning data with a priori maps. To improve positioning stability, information from sensors such as an IMU is often integrated to output continuous pose estimates. Second, a positioning solution based on inertial navigation. This solution uses an inertial measurement unit to collect acceleration and angular velocity data, combined with vehicle wheel speed information, and employs dead reckoning algorithms to estimate vehicle position and attitude.

[0004] However, the above solution has the following technical problems in the long and straight underground tunnel environment:

[0005] First, long, straight underground tunnels have a high degree of environmental repeatability. Tunnel walls are usually relatively smooth and lack obvious geometric features, which makes laser SLAM prone to matching errors or matching failures when performing inter-frame matching. Especially in long, straight tunnels of hundreds or even thousands of meters, this "long corridor effect" causes the cumulative error of laser SLAM to increase dramatically, and in severe cases, it can lead to complete positioning failure and map drift. Secondly, the underground environment cannot receive satellite positioning signals, and the inertial navigation system can only rely on its own integration operations to calculate position. Due to the inherent drift error of inertial sensors, long-term and long-distance integration operations will cause the error to accumulate continuously, and the positioning accuracy will drop significantly over time.

[0006] To address these issues, visual target-assisted positioning technology has begun to be used underground in recent years. By placing artificial targets in the tunnel and using visual recognition to obtain an absolute position reference, cumulative errors can be effectively corrected. However, the unique characteristics of the underground environment present new challenges for visual target recognition: factors such as poor lighting conditions, high dust concentrations, and water mist interference can easily lead to target misidentification.

[0007] It's particularly important to note that in long, straight lanes, where laser SLAM generates significant cumulative errors, the system relies heavily on visual targets for position correction. If the visual target is misidentified due to environmental factors, the incorrect absolute position reference can lead to a sudden change in positioning results. This sudden change is extremely dangerous for the autonomous driving system and can cause the vehicle to deviate from its intended path or even cause a collision.

[0008] In summary, existing technologies are difficult to provide stable and reliable positioning services in long and straight underground tunnel environments. There is an urgent need for a multi-sensor fusion positioning method that can effectively identify and eliminate misidentification of visual targets and prevent positioning mutations. Summary of the Invention

[0009] In response to the problems of large cumulative errors in laser SLAM in long and straight underground tunnels and the susceptibility of visual targets to environmental interference, resulting in misidentification and sudden positioning changes, the present application provides an underground vehicle positioning method and system based on visual target information fusion. Through error self-checking, multi-source fusion, anomaly detection and multiple verifications, it effectively solves the large cumulative errors in positioning in long and straight underground tunnels and the sudden positioning changes caused by misidentification of visual targets.

[0010] One aspect of the present application provides an underground vehicle positioning method based on visual target information fusion, comprising: S1, arranging visual targets at preset intervals on the side walls and top of the underground tunnel, the visual targets having a unique identification ID and error self-checking recognition function; pre-determining the coordinates of each target in the world coordinate system, and storing the target ID and the corresponding coordinates in a database; S2, collecting multi-source sensor data, the multi-source sensor data including target images collected by the on-board visual camera, vehicle acceleration, angular velocity and attitude data collected by the inertial navigation device, and vehicle driving speed data collected by the wheel speed meter; S3, pre-processing and feature extraction of the collected target images, extracting the target ID and the pixel coordinates of the target in the image coordinate system. ; S4, according to the vehicle-mounted visual camera, using the pre-calibrated camera internal parameter matrix K and external parameter matrix , the pixel coordinates of the extracted target Convert to three-dimensional coordinates in the camera coordinate system ; S5, according to the target ID, query the target coordinates of the corresponding target in the world coordinate system from the database; according to the target coordinates and the three-dimensional coordinates obtained in step S3 By solving the inverse projection problem, the position and posture of the vehicle relative to the world coordinate system are calculated to obtain the visual target positioning result; S6, multi-sensor fusion is performed according to the recognition status of the visual target in step S3 to obtain the vehicle positioning result.

[0011] Further, S6, obtaining the vehicle positioning result, including: when the target is identified in step S3 and the identified target passes the error self-check, an adaptive Kalman filter or an adaptive particle filter algorithm is used, the visual target positioning result obtained in step S5 is used as the observation value, and the data collected based on the inertial navigation device and the wheel speed meter is used as the prediction value, and data fusion is performed to obtain the vehicle positioning result; when the target is not identified in step S3, or the identified target does not pass the error self-check, the vehicle positioning result is obtained by track calculation based only on the data collected by the inertial navigation device and the wheel speed meter.

[0012] Especially in long, straight underground tunnels, when the cumulative error of laser SLAM is large, the system relies heavily on the absolute position reference provided by the visual target. Traditional solutions directly use visual recognition results for position correction. If the target is misidentified, the incorrect position information will immediately lead to sudden positioning changes.

[0013] This solution uses "error self-checking" to verify the reliability of target recognition results before data fusion. Only target data that passes the CRC check is considered reliable and included in subsequent data fusion. This mechanism blocks the propagation path of erroneous observation data at the source.

[0014] On the one hand, when the target is correctly identified and verified, the system uses an adaptive filtering algorithm to fuse the visual positioning results as the high-weighted observation value and the inertial navigation / wheel speed meter data as the predicted value. This effectively uses the target's absolute position information to correct accumulated errors.

[0015] On the other hand, if the target is not identified or fails verification, the system automatically switches to pure dead reckoning mode. Although errors will continue to accumulate, sudden changes in positioning caused by erroneous observations are avoided.

[0016] This adaptive switching mechanism ensures that even if target recognition fails under harsh conditions such as dust obstruction and abnormal lighting, the system can still maintain the continuity and smoothness of positioning.

[0017] Furthermore, S1, the visual target has a unique identification ID and error self-checking recognition function, including: setting the visual target as a square two-dimensional matrix coding structure, dividing the target into NxN coding cells, and dividing the visual target into a first area and a second area, wherein the first area serves as a positioning frame and the second area serves as a data area and a check area; the first area is located outside the second area; the black and white states of each coding cell in the data area are represented as binary 0 and 1, respectively, to form a unique ID coding sequence; according to the ID coding sequence, a check code is generated by a cyclic redundancy check CRC algorithm, and the check code is encoded into the check area; when performing target recognition in step S3, first identify the first area to determine the target position, and then identify the coding units of the data area and the check area respectively to obtain the ID coding sequence and the check code sequence; calculate the CRC check code based on the ID coding sequence obtained by recognition, and compare it with the recognized check code sequence; when the comparison is consistent, it is judged that the corresponding target has passed the error self-checking and the target ID recognition is correct, otherwise it is judged that the error self-checking has not been passed and the corresponding target recognition result is invalid.

[0018] The visual target is an artificial marker designed specifically for visual positioning of underground vehicles. It uses a square two-dimensional matrix coding structure and integrates unique identification information and error self-checking capabilities, serving as an absolute position reference point in underground environments. In this application, it provides a stable and reliable absolute position reference to correct the accumulated errors of laser SLAM in long, straight tunnels. Through a built-in verification mechanism, it prevents misidentification and positioning mutations caused by environmental interference from the source.

[0019] In this application, the positioning frame is the outermost black border area of ​​the visual target, which occupies the width of 2 outer coding cells to form a complete square outline. It is the primary visual feature for target detection and positioning. The integrity of the frame is used to determine whether the target is partially blocked by dust, equipment, etc. The data area is a core coding area located inside the positioning frame and is used to store the unique identification ID of the target. It encodes binary information in a black and white binary mode and is a key part of target identification. Each target has a globally unique ID to avoid position confusion, and the binary encoding has good robustness to lighting changes and noise. The check area is an area specifically used to store CRC cyclic redundancy check codes. It is generated by performing mathematical operations on the data area ID and is used to verify the correctness of target identification and detect recognition errors caused by dust obstruction, uneven lighting, etc.

[0020] In particular, factors such as dust, water mist, and uneven lighting in the underground environment can seriously interfere with visual recognition. Specifically, dust can partially obscure the target, making it unrecognizable; reflections from tunnel walls can cause partial over- or underexposure of the image; water mist condensation can blur the target surface; and coal dust deposits can alter the target's color. These interfering factors can easily lead to misreading of the target ID. Traditional targets are unable to determine the accuracy of their recognition results. Incorrect IDs can directly result in incorrect coordinates being retrieved, causing positioning errors.

[0021] This application embeds verification information in the target, wherein the first area (positioning frame) adopts a simple and stable border structure, which is easy to detect even in harsh environments. Only when the complete positioning frame is successfully detected will subsequent ID recognition be performed, avoiding misjudgment of incomplete images. The first area is located on the periphery and the second area is located on the inside, that is, the positioning frame surrounds the second area. The data area stores the target ID information, and the verification area stores the corresponding CRC check code; when dust obstruction or abnormal lighting causes individual coding units to misidentify, the CRC check can immediately detect it; a failed check is directly judged as an invalid recognition, preventing the wrong ID from entering the positioning system. Compared with multi-level grayscale coding, black and white binary coding has a higher tolerance to lighting changes and dust interference. The NxN matrix structure provides sufficient coding space while maintaining the reliability of recognition.

[0022] For example, if mud and water splashed by a mine car partially obscure the target, this can cause errors in two or three digits of the ID code. Conventional targets would misinterpret the ID as another, but this solution's CRC checksum immediately detects the error and rejects the identification attempt. Furthermore, if underground vehicle headlights directly illuminate the target, causing partial overexposure, white areas can be mistakenly identified as black. CRC checksums can detect these systematic errors and prevent mispositioning.

[0023] Furthermore, S3 pre-processes the collected target image, including: enhancing the target image using an adaptive histogram equalization algorithm to compensate for uneven illumination in the well; removing salt and pepper noise caused by dust particles in the well using a median filter algorithm based on the enhanced target image to obtain a denoised target image; separating the target area image from the background area image through binarization based on the denoised target image; detecting straight lines using a Hough transform based on the target area image to identify the square outline of the target and obtain a candidate target;

[0024] In particular, the key challenge of underground scenes is the significant local illumination differences. Global histogram equalization can lead to information loss in overexposed areas. The adaptive algorithm divides the image into blocks and independently equalizes each area, which not only enhances the details in the dark areas but also protects the information in the bright areas. In addition, coal dust particles appear as isolated bright or dark spots in the image, which is exactly the salt and pepper noise that the median filter is best at processing. Compared with the mean filter, the median filter can maintain the sharpness of the target edge while removing noise. Finally, the square outline of the target is composed of straight lines. The Hough transform is specifically designed for line detection. Even in the case of partial occlusion, as long as three edges can be detected, the complete outline can still be inferred.

[0025] According to the candidate targets, the positioning frame of the first area is detected. If a complete positioning frame is detected and passes the error check, the corresponding target is determined to be a valid target, otherwise it is an invalid target.

[0026] For the detected valid targets, extract the coordinates of the four corner points of the positioning frame;

[0027] Based on the coordinates of the four corner points of the positioning frame, the target area is perspective transformed and corrected to a square. The corrected target image is divided into NxN grids, and the grayscale value of each grid is sampled one by one. It is judged as black or white based on the preset threshold. The black and white mode of the data area is read according to the predetermined coding sequence, converted into a binary sequence, and the binary sequence is decoded into the target ID. Among them, perspective transformation is a geometric transformation that projects an image from one perspective to another. It is implemented through a 3×3 homography matrix. In this solution, it is used to correct the obliquely shot target image to a standard square from the frontal perspective to ensure the accuracy of subsequent coding recognition.

[0028] Calculate the pixel coordinates of the target center point in the image coordinate system based on the corner point coordinates .

[0029] Further, S4, the pixel coordinates of the extracted target are Convert to three-dimensional coordinates in the camera coordinate system, including: obtaining the pre-calibrated camera intrinsic parameter matrix K, which includes the camera focal length and principal point coordinates ; According to the pixel coordinates of the four corner points of the target and the physical size of the target, the PnP algorithm is used to solve the position of the target relative to the camera, and the rotation matrix R and translation vector T are obtained; the depth information Z of the target relative to the camera is extracted from the translation vector T; according to the pixel coordinates of the target And depth information Z, calculate the three-dimensional coordinates of the target in the camera coordinate system : ; ; .

[0030] The PnP algorithm (Perspective-n-Point Algorithm) is used to solve the pose relationship problem between n 3D spatial points and their 2D image projections. In this solution, the precise 3D pose (rotation matrix R and translation vector T) of the target relative to the camera is calculated using the known physical dimensions of the target and the detected image coordinates. This is the core algorithm for recovering 3D position information from 2D images.

[0031] Depth information, Z, is the straight-line distance from the target center to the camera's optical center, expressed as the Z coordinate in the camera coordinate system. This is a key parameter in 3D reconstruction, directly determining the proportional relationship between 3D coordinates recovered from 2D image coordinates. In underground environments, it also serves as an important indicator for verifying the rationality of observations.

[0032] Furthermore, the depth information Z is checked to see if it is within a preset valid range, where the preset valid range is set according to the width of the underground tunnel. In particular, the tunnel width is usually 4 to 8 meters and the height is 3 to 5 meters, which are fixed physical limitations. Vehicles can only travel within the tunnel, and the distance from the camera to the tunnel wall must be within a reasonable range. The target is installed on the tunnel wall, and its depth value must comply with the tunnel's geometric constraints. This natural physical constraint provides a reliable basis for verifying the rationality of the depth information, which is lacking in the open environment on the ground.

[0033] If the depth information Z is within the preset valid range, the three-dimensional coordinate calculation result of the corresponding target is retained for the positioning calculation in the subsequent step S5; for example: in a 6-meter-wide lane, considering the vehicle's driving trajectory, the valid depth range is set to [1.5m, 5.5m]. Depth values ​​outside this range are directly judged as abnormal to block the propagation of errors.

[0034] If the depth information Z is not within the preset valid range, the corresponding target observation is calibrated as abnormal, and the following processing is performed: continuously capture multiple frames of target images, and repeat the target identification and coordinate conversion in steps S3 and S4; when the depth information Z of the same target in multiple consecutive frames is within the valid range, the average value of the multiple frames of depth information Z is used as the final depth information; otherwise, it is confirmed that the corresponding target observation has failed, and positioning is performed only based on the inertial navigation device and the wheel speed meter in step S6.

[0035] Further, S5, obtains the visual target positioning result, including: querying the database to obtain the coordinates of the corresponding target in the world coordinate system according to the target ID extracted in step S3 ; Establish the three-dimensional coordinates of the target in the camera coordinate system Coordinates in the world coordinate system The conversion relationship between them: ,in, is the rotation matrix from the world coordinate system to the camera coordinate system, is the translation vector; according to the three-dimensional coordinates of the target in the camera coordinate system Coordinates in the world coordinate system , by solving the inverse projection problem, calculate the rotation matrix and translation vectors :

[0036] When a single target is identified, the corresponding relationship is established using the four corner points of the target and the least squares method is used to solve and When multiple targets are identified at the same time, multiple sets of corresponding point relationships are established and solved by minimizing the reprojection error. and ; According to the solution and , calculate the position and attitude of the camera in the world coordinate system: Camera position: ; Camera pose: from rotation matrix Extract Euler angles from ,in, is the roll angle, is the pitch angle, is the yaw angle.

[0037] Based on the pre-calibrated fixed transformation relationship between the camera and the vehicle , convert the camera's pose in the world coordinate system to the vehicle's pose in the world coordinate system: vehicle position ; Vehicle attitude angle ,in, They are the roll angle, pitch angle, and yaw angle offsets between the camera and the vehicle, α is the vehicle roll angle, β is the vehicle pitch angle, and θ is the vehicle yaw angle; output the position of the vehicle relative to the world coordinate system and attitude angle , as the visual target positioning result.

[0038] In particular, in long straight tunnels, when the cumulative error of laser SLAM reaches tens of meters, the system is actually in a "positioning loss" state: the position provided by laser SLAM has seriously deviated from the actual position; the system urgently needs a reliable absolute position reference to "pull back" to the correct track; at this time, the visual target becomes the only life-saving straw. Once misidentified, the wrong absolute position will lead to catastrophic positioning mutations; in this case, the S5 step is responsible for "positioning correction" and must ensure that the calculated position is absolutely accurate.

[0039] Therefore, in this application, all target positions are precisely measured and stored in the database. These coordinates are absolutely accurate "ground truth" and are not affected by the system's operating status. Even if the laser SLAM fails completely, the coordinates in the database remain reliable. Based on the CRC check in the aforementioned steps, only correctly identified IDs will be included in the query. One ID corresponds to a unique world coordinate, avoiding the uncertainty of fuzzy matching. The query result is either completely correct or the query fails. There is no "partial correctness".

[0040] In addition, the target provides four corner points, each of which provides two constraint equations, for a total of eight constraints. Six unknowns (three rotations and three translations) need to be solved, which is an over-constrained problem. Over-constraint makes the solution more stable and resistant to measurement noise.

[0041] S6, perform multi-sensor fusion based on the recognition status of the visual target in step S3 to obtain the vehicle positioning result:

[0042] When the target is identified in step S3 and the identified target passes the error self-check, the adaptive Kalman filter algorithm is used for data fusion, including: constructing the system state vector , where x, y, z are the vehicle positions, is the vehicle speed, is the vehicle attitude angle; a state prediction model is established based on the inertial navigation device and wheel speed meter data, and the state prediction value and prediction covariance matrix are calculated; the visual target positioning result is used as the observation value, and the observation residual and residual covariance are calculated; according to the statistical characteristics of the observation residual, the process noise covariance matrix Q and the observation noise covariance matrix R are dynamically adjusted: when the observation residual continues to increase, the observation noise covariance matrix R is increased and the visual observation weight is reduced; when the observation residual is stable within the threshold range, the observation noise covariance matrix R is reduced and the visual observation weight is increased; the state estimation is updated through the Kalman gain to obtain the fused vehicle positioning result.

[0043] When the target is identified in step S3 and the identified target passes the error self-check, the adaptive particle filter algorithm is used for data fusion, including: initializing the particle set, each particle represents the possible position and posture state of the vehicle; predicting the next state of each particle through the motion model based on the inertial navigation device and wheel speed meter data; using the visual target positioning result as the observation value to calculate the likelihood probability of each particle; setting the adaptive resampling threshold, and performing resampling when the number of valid particles is lower than the threshold; calculating the number of valid particles ,in, is the normalized weight of the i-th particle; when Resampling is performed when N is the total number of particles; weighted average is calculated according to the particle weights to obtain the fused vehicle positioning result; when a positioning mutation is detected, the number of particles is increased and the particle distribution range is expanded to improve the robustness of the algorithm.

[0044] When the target is not identified in step S3 or the identified target fails the error self-check, dead reckoning is performed based on the inertial navigation device and the wheel speedometer, including: using the acceleration data collected by the inertial navigation device to calculate the displacement increment through quadratic integration; using the angular velocity data to calculate the attitude angle change through integration; using the wheel speedometer data to calculate the vehicle's travel distance, and combining it with the attitude angle to calculate the position increment; using the complementary filtering algorithm to fuse the position estimates of the inertial navigation and the wheel speedometer: position increment = α × inertial navigation position increment + (1-α) × wheel speedometer position increment, where α is the fusion coefficient, which is dynamically adjusted according to the sensor accuracy and reliability; establishing an error accumulation model to estimate the cumulative error of the dead reckoning, and when the cumulative error exceeds a preset threshold, increasing the visual observation weight when a valid target is identified next time.

[0045] Another aspect of the present application also provides an underground vehicle positioning method based on visual target information fusion, including: visual targets, arranged on the side walls and top of the underground tunnel at preset intervals, the visual targets having a unique identification ID and error self-checking recognition function; a target database, storing the pre-determined coordinates of each target in the world coordinate system and the corresponding target ID; a data acquisition module, including an on-board visual camera, an inertial navigation device and a wheel speed meter, wherein the on-board visual camera is used to collect target images, the inertial navigation device is used to collect vehicle acceleration, angular velocity and posture data, and the wheel speed meter is used to collect vehicle driving speed; an image processing module, which pre-processes and extracts features of the collected target images, and extracts the target ID and the pixel coordinates of the target in the image coordinate system ; The coordinate conversion module converts the extracted target pixel coordinates according to the camera intrinsic parameter matrix K and extrinsic parameter matrix [R|T] pre-calibrated by the vehicle-mounted visual camera Convert to three-dimensional coordinates in the camera coordinate system ; Positioning solution module, according to the target ID, queries the target coordinates of the corresponding target in the world coordinate system from the target database, and calculates the target coordinates and three-dimensional coordinates By solving the inverse projection problem, the position and posture of the vehicle relative to the world coordinate system are calculated to obtain the visual target positioning result; the fusion positioning module performs multi-sensor fusion according to the recognition status of the visual target to obtain the vehicle positioning result.

[0046] Among them, the world coordinate system is a global unified reference coordinate system in the underground tunnel environment. It is a fixed three-dimensional Cartesian coordinate system used to describe the absolute position of all targets, vehicles, and tunnel structures in real physical space. This coordinate system is the reference framework of the entire positioning system. All position information must ultimately be converted to this coordinate system to have actual navigation significance. The coordinate origin: usually set at the entrance or wellhead of the main tunnel of the mine to facilitate connection with the ground coordinate system. The X-axis: extends along the direction of the main tunnel, pointing to the depth of the tunnel is positive; the Y-axis: perpendicular to the side wall of the tunnel, pointing to the right is positive (facing the depth of the tunnel); the Z-axis: vertically upward is positive, in accordance with the right-hand coordinate system rule.

[0047] The image coordinate system is a two-dimensional coordinate system defined on the camera imaging plane. It is used to describe the position of targets and their feature points in digital images. It is the starting point for visual information processing, quantifying positional information in images through pixel coordinates and serving as a bridge between two-dimensional image observation and three-dimensional spatial position.

[0048] The visual target is set as a square two-dimensional matrix coding structure, which divides the target into N×N coding cells and a first area and a second area. The first area serves as a positioning frame, and the second area serves as a data area and a verification area. The first area is located outside the second area.

[0049] The image processing module includes: a pre-processing unit, which uses an adaptive histogram equalization algorithm to perform enhancement processing to compensate for uneven illumination in the well, a median filter algorithm to remove salt and pepper noise caused by dust particles in the well, separates the target area image from the background area image through binarization processing, and uses Hough transform to detect straight lines and identify the square outline of the target to obtain a candidate target; a target recognition unit, which is used to detect the first area positioning frame of the candidate target, extract the coordinates of the four corner points of the valid target, perform perspective transformation and correct the target area to a square, convert the black and white mode of the read data area into a binary sequence and decode it into a target ID, and calculate the pixel coordinates of the target center point in the image coordinate system; an error check unit, which is used to calculate the CRC check code based on the identified ID code sequence, and compare it with the identified check code sequence to determine whether the target passes the error self-check;

[0050] The coordinate conversion module includes: a posture solving unit, which is used to solve the posture of the target relative to the camera using the PnP algorithm according to the pixel coordinates of the four corner points of the target and the physical size of the target, and obtain the rotation matrix R and the translation vector T; a depth verification unit, which is used to extract the depth information Z of the target relative to the camera from the translation vector T, and check whether the depth information Z is within the preset effective range set according to the width of the underground tunnel; a coordinate calculation unit, which is used to calculate the target pixel coordinates and the depth information Z according to the target pixel coordinates and the depth information Z. , , , calculate the three-dimensional coordinates of the target in the camera coordinate system;

[0051] The positioning solution module includes: a database query unit, which is used to query the target database according to the target ID to obtain the coordinates of the corresponding target in the world coordinate system; a back projection solution unit, which is used to solve the rotation matrix from the world coordinate system to the camera coordinate system by the least squares method or minimizing the reprojection error according to the coordinate correspondence between the target in the camera coordinate system and the world coordinate system. and translation vectors ; Vehicle posture calculation unit, used to calculate the vehicle posture according to the solution and Calculate the position and attitude of the camera in the world coordinate system, and convert the camera pose into the position and attitude angle of the vehicle in the world coordinate system based on the pre-calibrated fixed transformation relationship between the camera and the vehicle;

[0052] The multi-sensor fusion module uses an adaptive Kalman filter or adaptive particle filter algorithm to fuse the visual target positioning results as the observation value and the data based on the inertial navigation device and wheel speed meter as the predicted value when the target is identified and the error self-check is passed. If the target is not identified or the error self-check is not passed, the track is calculated only based on the inertial navigation device and wheel speed meter.

[0053] Compared with the existing technology, the advantages of this application are:

[0054] This application first designs a visual target with a self-error self-checking function and adopts a CRC check mechanism to effectively identify and eliminate target misidentification caused by environmental factors such as dust and light, thus avoiding the introduction of erroneous observation data at the source. An adaptive multi-sensor fusion mechanism based on recognition status is established. When target recognition is normal and passes verification, the visual, inertial navigation, and wheel speed meter data are integrated; when target recognition is abnormal or fails verification, it automatically switches to the inertial navigation and wheel speed meter track calculation mode to ensure positioning continuity.

[0055] Secondly, through the depth information validity test and multi-frame verification mechanism, abnormal target observations are reconfirmed. Only positioning results that are consistent with multiple consecutive frames are accepted, effectively suppressing positioning mutations caused by single misidentification.

[0056] Finally, an adaptive Kalman filter or particle filter algorithm is used to dynamically adjust the fusion weight according to the observation quality, so as to ensure positioning stability while making full use of high-quality visual observation information to improve positioning accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0057] The present application will be further described in the form of exemplary embodiments, which will be described in detail with reference to the accompanying drawings. These embodiments are not limiting, and in these embodiments, the same numbers represent the same structures, wherein:

[0058] Figure 1 This is an exemplary flow chart of an underground vehicle positioning method based on visual target information fusion according to some embodiments of the present application;

[0059] Figure 2 is a schematic diagram of a target according to some embodiments of the present application;

[0060] Figure 3 This is an exemplary flow chart of an underground vehicle positioning system based on visual target information fusion according to some embodiments of the present application. DETAILED DESCRIPTION

[0061] The method and system provided in the embodiments of the present application are described in detail below with reference to the accompanying drawings.

[0062] like Figure 1 As shown in the figure, visual targets are arranged at preset intervals on the side walls and top of the underground tunnel. The visual targets have a unique identification ID and error self-checking recognition function; the coordinates of each target in the world coordinate system are pre-determined, and the target ID and the corresponding coordinates are stored in the database; multi-source sensor data are collected, and the multi-source sensor data includes target images collected by the on-board visual camera, vehicle acceleration, angular velocity and posture data collected by the inertial navigation device, and vehicle speed data collected by the wheel speed meter; the collected target images are pre-processed and feature extracted to extract the target ID and the pixel coordinates of the target in the image coordinate system. ; According to the vehicle-mounted visual camera, the pre-calibrated camera internal parameter matrix K and external parameter matrix , the pixel coordinates of the extracted target Convert to three-dimensional coordinates in the camera coordinate system ; According to the target ID, query the target coordinates of the corresponding target in the world coordinate system from the database; according to the target coordinates and the three-dimensional coordinates obtained in step S3 By solving the inverse projection problem, the position and posture of the vehicle relative to the world coordinate system are calculated to obtain the visual target positioning result; multi-sensor fusion is performed according to the recognition status of the visual target to obtain the vehicle positioning result.

[0063] Specifically, in underground mine tunnels, the placement of visual targets requires comprehensive consideration of the tunnel's structural characteristics and vehicle traffic requirements. Targets are placed every 50 to 100 meters in straight tunnel sections, with more frequent spacing at key locations like curves and intersections, shortening the intervals to 20 to 30 meters. The targets are uniformly mounted at a height of 1.6 meters above the ground, facilitating easy acquisition by vehicle-mounted cameras while minimizing obstruction by passing vehicles and personnel.

[0064] Targets on the sidewalls of tunnels are mounted vertically, ensuring the target plane is parallel to the longitudinal axis of the tunnel. In tunnels wider than 8 meters, targets are installed on both sides to provide redundant coverage. Targets on the top of tunnels are primarily deployed in areas where the view from the sidewall targets is limited, such as low tunnels or areas with dense equipment.

[0065] S1, the visual target is designed as a square with a side length of 20 cm, and is divided into 16×16 coding cells. The target structure is divided into two functional areas:

[0066] like Figure 2 As shown, the first area (positioning frame) occupies two cells on the outermost edge of the target, forming a thick black border. This border has the following functions: providing a clear visual feature to facilitate rapid target location in complex backgrounds; detecting the complete square outline to preliminarily determine whether the target is obscured; and providing the four accurate corner coordinates for subsequent perspective transformations. The second area (data area and verification area) is located within the positioning frame and is further divided into: a data area occupying 10×10 cells, used to encode the target's unique ID; and a verification area occupying the remaining 4×12 cells, storing a CRC checksum.

[0067] Each target ID uses 100-bit binary code to represent The encoding process is as follows: Each target is assigned a unique decimal ID number (e.g. 001-999), which is then converted to binary and filled with zeros if the number is less than 100.

[0068] The CRC-32 algorithm is used to calculate the 100-bit ID code and generate a 32-bit checksum. CRC-32 is chosen for its strong error detection capabilities, detecting all single-bit errors, double-bit errors, odd-numbered errors, and burst errors of 32 bits or less. The ID code and checksum are mapped to corresponding cells according to a predetermined rule, with black cells representing a binary "1" and white cells representing a binary "0." Gray code is used so that adjacent codes differ by only one bit, improving interference resistance.

[0069] The installation position of each target is precisely measured using a total station, with a measurement accuracy of ±5 mm. A unified underground world coordinate system is established, with the tunnel entrance as the origin, the tunnel extension direction as the positive X-axis, and the vertical upward direction as the positive Z-axis. The database adopts a relational structure and mainly includes the following fields: Target ID (primary key): unique identifier; world coordinates : The three-dimensional coordinates of the target center point; Installation time: used to track the service life of the target; Status mark: normal / maintenance / failure, etc.; Lane number: The lane location of the target.

[0070] The S2's onboard vision system utilizes a binocular configuration, with an industrial camera mounted on each side of the vehicle. Camera selection took into account the characteristics of the underground environment: resolution: 1920×1080 pixels, ensuring clear identification of the target code within a distance of 5 meters; frame rate: 30fps, meeting real-time acquisition requirements at a vehicle speed of 30 km / h; sensitivity: up to ISO 12800, adapting to the low-light environment underground; protection level: IP67, dustproof and waterproof. The camera mounting positions are precisely calibrated: mounting height: 1.2 meters above the ground, forming an appropriate pitch angle with the target height; lateral position: 1.5 meters from the vehicle centerline to avoid obstruction by the vehicle body; mounting angle: tilted outward 15° to maximize field of view.

[0071] Inertial Measurement Unit (IMU) key parameters: gyroscope bias stability: ≤1° / h; accelerometer bias stability: ≤50μg; data output frequency: 200Hz. The IMU is mounted near the vehicle's center of mass and isolated from vehicle body vibration via shock-absorbing mounts. Data collection includes: three-axis angular velocity (used to calculate vehicle attitude changes); three-axis acceleration (used to estimate position increments); and attitude angle output: real-time roll, pitch, and yaw angles.

[0072] The system directly reads data from the vehicle's existing ABS wheel speed sensors via the CAN bus, obtaining real-time rotational speeds for all four wheels. Data processing includes: wheel speed fusion: using median filtering to eliminate outliers and calculate average vehicle speed; slip detection: comparing wheel speed differences to identify slippage; and mileage accumulation: calculating distance traveled based on tire circumference.

[0073] S3, based on the characteristics of the underground environment, uses adaptive histogram equalization: the image is divided into 8×8 subregions; the histogram of each subregion is calculated; the contrast enhancement amplitude is limited to prevent noise amplification; and bilinear interpolation is used to smooth the subregion boundaries.

[0074] Median filtering denoising: Using a 5×5 filter window, for each pixel, the median of the 25 pixels within the window is taken, and the edge areas are filled with mirror images. In this application, the salt and pepper noise density caused by dust is reduced from 8% to below 0.5%. Preferably, the Otsu algorithm is used to automatically determine the global threshold, combined with the local mean for threshold adjustment, and morphological operations are used to fill small holes.

[0075] Hough transform line detection: Edge detection: Canny operator, low threshold 50, high threshold 150; Hough spatial resolution: 1° angle, 1 pixel distance; Line screening: length greater than 50 pixels and meeting vertical or horizontal conditions. Detected lines are combined to find the four sides of a rectangle. The geometric constraints of the rectangle are verified: opposite sides are parallel, adjacent sides are perpendicular, and the aspect ratio is close to 1. The area of ​​the rectangle is calculated, and candidate targets that meet the target size are screened.

[0076] Extract and verify the target ID, accurately divide the corrected image into 16×16 grids, and sample the average grayscale of the 3×3 area in the center of each grid. Dynamic threshold judgment: if the grayscale value is greater than the image mean, it is white (1), otherwise it is black (0). Read the binary values ​​of 100 grids in the data area in a predetermined order, perform CRC-32 calculation on the read ID sequence, read the check code of 32 grids in the check area, compare the calculated value with the read value, and pass the verification if they are completely consistent. In this application, under normal conditions, the ID correct recognition rate is 99.5%, and the CRC verification pass rate is 99.8%; in a dusty environment, the ID recognition rate drops to 85%, but the CRC verification ensures zero false recognition.

[0077] S4, camera intrinsic parameter calibration using Zhang Zhengyou calibration method: calibration board: 12×9 chessboard, grid size 30mm; collected images: 50 images at different angles and distances; calibration results: focal length: (pixels); principal point: (pixels); distortion coefficient: , The camera extrinsics are calibrated using the vehicle coordinate system: a calibration field is set up on a flat surface, and a laser tracker is used to measure the position of the camera relative to the center of the vehicle. The calibration results are stored as a 4×4 transformation matrix.

[0078] The target pose is solved using the Efficient PnP (EPnP) algorithm: Input: The image and world coordinates of the target's four corner points; Construct the coefficient matrix: A 12×12 linear system; SVD decomposition solves: Obtain the coordinates of the four control points; Recover R and T: Calculate the rotation matrix and translation vector from the control points. Pose optimization: Iterative optimization using the Levenberg-Marquardt algorithm, with the objective function of minimizing the reprojection error; Convergence criterion: Error change < 0.01 pixel or 50 iterations.

[0079] Dynamically set according to the actual width of the lane: 6-meter-wide lane: effective depth range [0.8m, 5.5m]; 8-meter-wide lane: effective depth range [0.8m, 7.5m]; consider the camera installation position and possible lateral deviation of the vehicle.

[0080] When a depth anomaly is detected (Z < Zmin or Z > Zmax): Mark the current frame as a suspicious frame, continuously acquire 5 frames of images, and perform target recognition and depth calculation independently for each frame. Statistically analyze the distribution of depth values in the 5 frames: If the depth is normal in ≥3 frames and the variance < 0.1 m, take the mean as the final depth; otherwise, determine that the observation fails and discard the target observation.

[0081] S5. Establish a hash index for the target ID, with a query time complexity of O(1). The database locally caches the information of the last 100 targets, and there is an automatic retry mechanism for query failures, up to 3 times.

[0082] Single-target positioning solution: Target world coordinates (database query); Target camera coordinates (calculated in step S4); Solution process: Construct a corresponding point set: The coordinates of the 4 corners of the target in two coordinate systems respectively; Establish a system of linear equations: 12 equations (3 for each point) to solve 12 unknowns; Perform SVD decomposition to obtain an initial solution; Orthogonalize the rotation matrix: Ensure that R satisfies the orthogonality constraint, and optimize the solution: Minimize .

[0083] Multi-target joint positioning: When 2 or more targets are recognized within the field of view: Establish an overdetermined system of equations: n targets provide 3n constraint equations; Solve using weighted least squares: Weight setting: , is the target depth; The closer the target is to the camera, the greater the weight; RANSAC robust estimation: Randomly select the minimum point set (2 targets), calculate the model parameters, count the number of inliers (reprojection error < 5 pixels), iterate 100 times, and select the model with the most inliers.

[0084] Conversion from camera pose to vehicle pose: Vehicle position calculation: , where: (camera position in the world coordinate system); (camera offset relative to the vehicle center, unit: meter).

[0085] Vehicle attitude calculation: Extract the camera Euler angles from ; ; ; .

[0086] Add the calibration offset angles: (roll angle offset); (pitch angle offset); (yaw angle offset).

[0087] S6. Multi-sensor fusion positioning, state vector definition: , including 12 state variables such as position, velocity, attitude angle, and angular velocity.

[0088] State transition equation: , where F is the state transfer matrix, constructed based on the sampling time Δt=0.05s; B is the control input matrix; The acceleration and angular velocity measured by the IMU; is the process noise.

[0089] Adaptive mechanism: residual statistics: calculation of innovation sequence: ;Statistical window: the last 20 residual samples; calculate residual covariance: .

[0090] Noise covariance adjustment: If , increase : ;like , reduce : ; Limit the adjustment range: .

[0091] When visual observation is unavailable, the system switches to pure inertial navigation / wheel speed meter mode:

[0092] IMU integral calculation: attitude update: quaternion integration method to avoid Euler angle singularity; velocity update: ; Location update: .

[0093] Wheel speed odometry: Heading hold: using the IMU's yaw rate; Distance traveled: d = ∑ (wheel speed × wheel circumference × Δt); Position increment: , .

[0094] Complementary filter fusion: Position estimate = 0.7 × IMU position + 0.3 × wheel speedometer position. This weight distribution is based on sensor characteristics: the IMU has high accuracy in the short term, and the wheel speedometer has low cumulative error over a long period of time.

[0095] In summary, this application successfully solves the positioning problem in long and straight underground tunnel environments through the organic combination of key technologies such as error self-checking target design, multi-level image processing, depth information verification, and multi-sensor adaptive fusion, providing reliable positioning guarantee for autonomous driving of underground vehicles.

[0096] Figure 3The present application is an underground vehicle positioning system based on visual target information fusion, comprising: visual targets, which are arranged on the side walls and top of the underground tunnel at preset intervals, and the visual targets have a unique identification ID and error self-checking recognition function; a target database, which stores the pre-determined coordinates of each target in the world coordinate system and the corresponding target ID; a data acquisition module, which includes an on-board visual camera, an inertial navigation device and a wheel speedometer, wherein the on-board visual camera is used to collect target images, the inertial navigation device is used to collect vehicle acceleration, angular velocity and posture data, and the wheel speedometer is used to collect vehicle driving speed; an image processing module, which pre-processes and extracts features from the collected target images, and extracts the target ID and the pixel coordinates of the target in the image coordinate system. ; The coordinate conversion module converts the extracted target pixel coordinates according to the camera intrinsic parameter matrix K and extrinsic parameter matrix [R|T] pre-calibrated by the vehicle-mounted visual camera Convert to three-dimensional coordinates in the camera coordinate system ; Positioning solution module, according to the target ID, queries the target coordinates of the corresponding target in the world coordinate system from the target database, and calculates the target coordinates and three-dimensional coordinates By solving the inverse projection problem, the position and posture of the vehicle relative to the world coordinate system are calculated to obtain the visual target positioning result; the fusion positioning module performs multi-sensor fusion according to the recognition status of the visual target to obtain the vehicle positioning result.

[0097] The visual target is set as a square two-dimensional matrix coding structure, dividing the target into NxN coding cells, and dividing the visual target into a first area and a second area. The first area serves as a positioning frame, and the second area serves as a data area and a verification area. The first area is located outside the second area.

[0098] The invention of the present application and its implementation methods are described schematically above. This description is not restrictive. Without departing from the spirit or basic features of the present application, the present application can be implemented in other specific forms. What is shown in the accompanying drawings is only one of the implementation methods of the invention of the present application, and the actual structure is not limited to this. Therefore, if a person of ordinary skill in the art is inspired by it, without departing from the purpose of the invention, a structural method and embodiment similar to the technical solution are designed without creativity, which should all fall within the scope of protection of the present application. In addition, the word "including" does not exclude other elements or steps, and the word "one" before an element does not exclude the inclusion of "multiple" elements. Words such as first and second are used to indicate names and do not indicate any specific order.

Claims

1. A method for underground vehicle positioning based on visual target information fusion, characterized in that: include: S1, visual targets are arranged at preset intervals on the side walls and top of the underground tunnel. The visual targets have unique ID and error self-checking recognition functions; Pre-determine the coordinates of each target in the world coordinate system and store the target ID and corresponding coordinates in the database; S2, collects multi-source sensor data, including target images collected by the on-board visual camera, vehicle acceleration, angular velocity and attitude data collected by the inertial navigation device, and vehicle speed data collected by the wheel speed meter; S3, preprocessing and feature extraction of the collected target image, extracting the target ID and the pixel coordinates of the target in the image coordinate system ; S4, based on the onboard visual camera, uses the pre-calibrated camera internal parameter matrix K and external parameter matrix , the pixel coordinates of the extracted target Convert to three-dimensional coordinates in the camera coordinate system ; S5, according to the target ID, query the target coordinates of the corresponding target in the world coordinate system from the database; According to the target coordinates and the three-dimensional coordinates obtained in step S3 ,By solving the inverse projection problem, the position and attitude of the vehicle relative to the world coordinate system are calculated, and the visual target positioning result is obtained; S6, performing multi-sensor fusion according to the recognition status of the visual target in step S3 to obtain the vehicle positioning result; Among them, the pixel coordinates of the extracted target Convert to three-dimensional coordinates in the camera coordinate system, including: Get the pre-calibrated camera intrinsic parameter matrix K, which includes the camera focal length and principal point coordinates ; According to the pixel coordinates of the four corner points of the target and the physical size of the target, the PnP algorithm is used to solve the position of the target relative to the camera to obtain the rotation matrix R and translation vector T; Extract the depth information Z of the target relative to the camera from the translation vector T; According to the target pixel coordinates And depth information Z, calculate the three-dimensional coordinates of the target in the camera coordinate system : ； ； ； Check whether the depth information Z is within a preset effective range, wherein the preset effective range is set according to the width of the underground tunnel; If the depth information Z is within the preset valid range, the three-dimensional coordinate calculation result of the corresponding target is retained for the positioning calculation in the subsequent step S5; If the depth information Z is not within the preset valid range, the corresponding target observation is considered abnormal and the following processing is performed: Continuously capture multiple frames of target images and repeat the target recognition and coordinate conversion in steps S3 and S4; When the depth information Z of the same target in multiple consecutive frames is within the valid range, the average of the multiple-frame depth information Z is used as the final depth information. Otherwise, it is confirmed that the observation of the corresponding target has failed, and positioning is performed only based on the inertial navigation device and the wheel speed meter in step S6.

2. The underground vehicle positioning method based on visual target information fusion according to claim 1 is characterized in that: S6, obtaining the vehicle positioning result, including: When the target is identified in step S3 and passes the error self-check, an adaptive Kalman filter or adaptive particle filter algorithm is used to fuse the visual target positioning result obtained in step S5 as the observation value and the data collected by the inertial navigation device and wheel speed meter as the prediction value to obtain the vehicle positioning result. When the target is not identified in step S3, or the identified target fails the error self-check, the vehicle positioning result is obtained by dead reckoning based only on the data collected by the inertial navigation device and the wheel speed meter.

3. The underground vehicle positioning method based on visual target information fusion according to claim 2 is characterized in that: S1, the visual target has a unique ID and error self-checking recognition function, including: The visual target is set to a square two-dimensional matrix coding structure, the target is divided into NxN coding cells, and the visual target is divided into a first area and a second area, wherein the first area serves as a positioning frame and the second area serves as a data area and a verification area; the first area is located outside the second area; The black and white states of each coding cell in the data area are represented as binary 0 and 1 respectively, forming a unique ID coding sequence; According to the ID code sequence, a check code is generated by a cyclic redundancy check CRC algorithm, and the check code is encoded into the check area; When performing target recognition in step S3, the first area is first identified to determine the target position, and then the coding units of the data area and the check area are respectively identified to obtain the ID code sequence and the check code sequence; Calculate the CRC check code based on the ID code sequence obtained by identification and compare it with the identified check code sequence; When the comparison is consistent, the corresponding target is judged to have passed the error self-check and the target ID is correctly identified. Otherwise, it is judged to have failed the error self-check and the corresponding target identification result is invalid.

4. The underground vehicle positioning method based on visual target information fusion according to claim 3 is characterized in that: S3, pre-processing the collected target image, including: Based on the target image, an adaptive histogram equalization algorithm is used for enhancement processing to compensate for the uneven illumination in the well. Based on the enhanced target image, the median filter algorithm is used to remove the salt and pepper noise caused by underground dust particles to obtain the denoised target image. According to the denoised target image, the target area image and the background area image are separated by binarization processing; According to the target area image, the straight lines are detected by Hough transform, the square outline of the target is identified, and the candidate target is obtained.

5. The underground vehicle positioning method based on visual target information fusion according to claim 4 is characterized in that: S3, extract the target ID and the pixel coordinates of the target in the image coordinate system ,include: According to the candidate targets, the positioning frame of the first area is detected. If a complete positioning frame is detected and passes the error check, the corresponding target is determined to be a valid target, otherwise it is an invalid target; For the detected valid targets, extract the coordinates of the four corner points of the positioning frame; According to the coordinates of the four corner points of the positioning frame, the target area is perspective transformed and corrected to a square. The corrected target image is divided into NxN grids, and the grayscale value of each grid is sampled one by one. It is judged as black or white according to the preset threshold. The black and white mode of the data area is read according to the predetermined coding sequence, converted into a binary sequence, and the binary sequence is decoded into the target ID; Calculate the pixel coordinates of the target center point in the image coordinate system based on the corner point coordinates .

6. The underground vehicle positioning method based on visual target information fusion according to claim 1, characterized in that: S5, obtains the visual target positioning results, including: According to the target ID extracted in step S3, query the database to obtain the coordinates of the corresponding target in the world coordinate system ; Establish the three-dimensional coordinates of the target in the camera coordinate system Coordinates in the world coordinate system The conversion relationship between them: ,in, is the rotation matrix from the world coordinate system to the camera coordinate system, is the translation vector; According to the three-dimensional coordinates of the target in the camera coordinate system Coordinates in the world coordinate system , by solving the inverse projection problem, calculate the rotation matrix and translation vectors : When a single target is identified, the corresponding relationship is established using the four corner points of the target and the least squares method is used to solve and ; When multiple targets are identified at the same time, multiple sets of corresponding point relationships are established and solved by minimizing the reprojection error. and ; According to the solution and , calculate the position and attitude of the camera in the world coordinate system: Camera Position: ; Camera pose: from rotation matrix Extract Euler angles from ,in, is the roll angle, is the pitch angle, is the yaw angle; Based on the pre-calibrated fixed transformation relationship between the camera and the vehicle , convert the camera's pose in the world coordinate system to the vehicle's pose in the world coordinate system: Vehicle location ; Vehicle attitude angle ,in, are the roll, pitch, and yaw offsets between the camera and the vehicle, α is the vehicle roll angle, β is the vehicle pitch angle, and θ is the vehicle yaw angle; Output the vehicle's position relative to the world coordinate system and attitude angle , as the visual target positioning result.

7. A system for underground vehicle positioning method based on visual target information fusion according to any one of claims 1 to 6, characterized in that: include: Visual targets are arranged at preset intervals on the side walls and top of underground tunnels. The visual targets have unique identification ID and error self-checking recognition functions. Target database, storing the pre-determined coordinates of each target in the world coordinate system and the corresponding target ID; The data acquisition module includes an on-board visual camera, an inertial navigation device, and a wheel speedometer. The on-board visual camera is used to acquire target images, the inertial navigation device is used to acquire vehicle acceleration, angular velocity, and attitude data, and the wheel speedometer is used to acquire vehicle speed. Image processing module, which preprocesses and extracts features of the collected target image, extracts the target ID and the pixel coordinates of the target in the image coordinate system ; The coordinate conversion module converts the extracted target pixel coordinates into the target pixel coordinates according to the camera intrinsic parameter matrix K and extrinsic parameter matrix [R|T] pre-calibrated by the vehicle-mounted visual camera. Convert to three-dimensional coordinates in the camera coordinate system ; The positioning solution module queries the target coordinates of the corresponding target in the world coordinate system from the target database according to the target ID, and calculates the target coordinates and the three-dimensional coordinates according to the target coordinates and the three-dimensional coordinates. ,By solving the inverse projection problem, the position and attitude of the vehicle relative to the world coordinate system are calculated, and the visual target positioning result is obtained; The fusion positioning module performs multi-sensor fusion based on the recognition status of the visual target to obtain the vehicle positioning result.

8. The system for underground vehicle positioning method based on visual target information fusion according to claim 7 is characterized in that: The visual target is set as a square two-dimensional matrix coding structure, dividing the target into NxN coding cells, and dividing the visual target into a first area and a second area. The first area serves as a positioning frame, and the second area serves as a data area and a verification area. The first area is located outside the second area.

Images