Mine track robot road sign detection and positioning system and method

Abstract

The invention provides a system and method for detecting and positioning a road sign of a mining track robot, and belongs to the technical field of track robot positioning. The system comprises a mining track robot and a positioning mark arranged on one side of a robot walking track, and a proximity switch 1, a proximity switch 2, an explosion-proof servo motor, and a controller installed on the robot. The explosion-proof servo motor is equipped with an incremental encoder and a servo driver. The method introduces a road sign detection and positioning system to eliminate the mileage positioning error of the robot during long-distance driving. By setting positioning marks along the way according to preset lengths, the robot driving path is divided into several equal sections. The robot can detect the positioning marks and determine the correctness of the road sign detection and positioning, and then compensate for the positioning error by comparing the encoder mileage value with the actual measurement value, thereby eliminating the positioning cumulative error generated during long-distance driving.

Description

Technical Field

[0001] The invention belongs to the technical field of rail robot positioning, and in particular relates to a system and method for detecting and positioning road signs of a mining rail robot. Background Art

[0002] With the development of intelligent coal mines, a large number of intelligent mining robots have been introduced into various links of coal mine production. Among them, mining rail robots are favored by the majority of users due to their small size and high space utilization, and are the most widely used. Positioning technology is one of the key technologies of rail robot systems. Positioning accuracy and reliability will directly affect the safety and operation efficiency of robots. At present, the more mature rail positioning technologies include: global positioning (GPS) technology, odometer positioning technology, feature visual recognition technology, and road sign detection and positioning technology.

[0003] Patent "CN201310071279.8 A positioning method for a track-type mobile device" is implemented through a road sign detection and positioning system. The running track of the mobile device sets multiple calibration positions according to a preset length, and the calibration positions are numbered in sequence, including the destination number of the target position of the mobile device and the number of the current position. When the moving time of the mobile device is equal to an integer multiple of the time taken for the preset moving length, the current calibration position number is obtained until the number of the current position is equal to the destination number, and the mobile device reaches the destination.

[0004] Patent "CN202010545104.6 A robot positioning device" uses a positioning camera or a combination of multiple positioning cameras to cover the specified positioning range. The actual position and the position in the picture taken by the positioning camera are pre-calibrated so that there is a corresponding relationship between the two. When the robot is working, the calculation module analyzes the picture taken by the positioning camera, identifies the robot and calculates the position of the robot in the picture, and determines the actual position of the robot through the corresponding relationship between the actual position and the picture position.

[0005] Patent "CN201810594133.4 A positioning system and method for rail robots" uses photoelectric sensors and incremental encoders on the robot, sets a fixed code plate on the track, and obtains the actual moving distance of the robot based on the code plate information. The encoder can record the robot's mileage in real time. The processor can calculate the deviation between the actual distance value and the mileage value corresponding to the code plate at the same time, and adjust the mileage value based on the deviation value to achieve correction of the rail robot's position information.

[0006] The above positioning technology or method has the following problems:

[0007] 1) GPS positioning technology cannot be used in underground environments and cannot be applied to the positioning of mining rail robots.

[0008] 2) Single mileage positioning technology: positioning errors will occur due to factors such as the robot's walking wheel slipping, and the error will accumulate as the robot moves forward.

[0009] 3) The image acquisition work of visual positioning technology is restricted by factors such as ambient light, and is not suitable for use in places with insufficient light such as underground tunnels. In addition, the image processing data calculation volume is large and requires high-performance industrial computers for data processing, which is costly.

[0010] In the patent "CN201810594133.4 A positioning system and method for rail robots", the robot is positioned by combining a landmark detection positioning system with an odometer positioning technology. Although the accumulated error of odometer positioning can be corrected, there is no corresponding measure in the system to verify the rationality of the deviation calculation. When positioning recognition fails, using the wrong deviation to correct the mileage value will increase the error. For example, in a high-dust coal mine environment, the recognition of the code plate is seriously affected, resulting in positioning failure. At this time, the calculated deviation will seriously deviate from the true value. Summary of the invention

[0011] Based on the above technical problems, the present invention provides a mining rail robot landmark detection and positioning system and a mining rail robot positioning method, which are intended to correct the positioning error caused by the odometer positioning technology that relies solely on the encoder, and realize the precise positioning of the mining rail robot for long-distance driving. Among them, the mining rail robot positioning method introduces a landmark detection and positioning system to eliminate the mileage positioning error of the robot during long-distance driving. By setting positioning marks along the way according to a preset length, the robot's driving path is divided into several equal sections. The robot can detect the positioning mark and determine the correctness of the landmark detection and positioning, and then compensate for the positioning error by comparing the encoder mileage value with the actual measurement value, thereby eliminating the cumulative positioning error generated during long-distance driving.

[0012] A road sign detection and positioning system for a mining track robot comprises: a mining track robot and a positioning mark arranged on one side of the robot's walking track, as well as a proximity switch 1, a proximity switch 2, an explosion-proof servo motor, and a controller installed on the robot. The explosion-proof servo motor is equipped with an incremental encoder and a servo driver; the proximity switch 1, the proximity switch 2, and the incremental encoder are connected to the input end of the controller, and the servo driver is connected to the output end of the controller.

[0013] The proximity switch is used to detect the positioning mark. When the proximity switch 1 detects the positioning mark, it is used as the starting time t0 of the position integration, and the controller is triggered to start the position integration. When the proximity switch 2 detects the positioning mark, it is used as the ending time t1 of the position integration, and the controller is triggered to end the position integration. The position integration value s in the time period t0 to t1 is the moving distance of the mining rail robot during the detection of the positioning mark.

[0014] The incremental encoder is used to count the number of turns of the robot's running wheels;

[0015] The controller is used to determine the accuracy of the identification positioning according to the ratio of the position integration result s and the measurement value d, wherein the measurement value d is the distance between the two proximity switches.

[0016] The controller adopts Siemens S7-1200PLC, the incremental encoder is connected to Siemens S7-1200PLC via RS485, the explosion-proof servo motor is driven by a servo driver, and the servo driver is connected to Siemens S7-1200PLC via RS485. The explosion-proof servo motor, incremental encoder, controller and servo driver are all packaged in an explosion-proof electric control box.

[0017] A method for positioning a mining rail robot is implemented based on the mining rail robot landmark detection and positioning system, and the method comprises:

[0018] Step 1: When the proximity switch detects the positioning mark, the controller triggers the servo driver to control the explosion-proof servo motor to drive the robot to decelerate and move forward at a speed v;

[0019] Step 2: Preset the encoder mileage value range to [L min ,L max ], determine whether the encoder mileage value M is within the range [L min ,L max ], if it exceeds this range, stop the machine to check the fault, otherwise go to step 3; where L min is the minimum mileage value of the encoder, L max is the maximum mileage value of the encoder;

[0020] Step 3: When proximity switch 1 detects the positioning mark, the integral calculation starts, and when proximity switch 2 detects the same positioning mark, the integral calculation ends. The position integral value s in the time period t0 to t1 is expressed as:

[0021]

[0022] In the formula, v represents the travel speed of the mining robot, and t represents the time;

[0023] Step 4: Calculate the distance ratio between the integral value s and the measured value d Set the range of distance ratio to [i min ,i max ], if the distance ratio i is in the range [i max ,i max ], it indicates that the positioning is successful, where i min is the minimum value of the distance ratio, i maxis the maximum value of the distance ratio.

[0024] The step 2 determines whether the encoder mileage value is within a reasonable range, which is specifically expressed as follows:

[0025] When the robot reaches the first positioning mark, the encoder is reset. If the encoder mileage value M when the robot continues to move forward to the second positioning mark is within [L min ,L max ] range, it indicates that the encoder mileage value is within a reasonable range; the encoder mileage value M is expressed as:

[0026] M=b / λ*γ*d s *π

[0027] In the formula, b is the encoder value, λ is the encoder resolution, γ is the reduction ratio, and d is s is the wheel diameter.

[0028] The step 4 is specifically described as follows:

[0029] If the distance ratio i is in the range [i min ,i max ], it indicates that the positioning is successful, and the number of positioning blocks is n d The count is increased by 1, the encoder mileage value M is cleared, and the robot mileage value D is calculated. D is expressed as:

[0030] D=L×n d +M

[0031] If i is not in the range [i min ,i max ], it indicates that the positioning has failed, and the number of positioning times is n c The count is increased by 1, the explosion-proof servo motor reverses and the robot moves back a distance R, where R is a distance of 2 times the position integral value s, and then the explosion-proof servo motor rotates forward and the robot moves forward, returning to step 1 and starting again. If the number of failed positioning exceeds the set value n0, it is determined that there is interference affecting the positioning.

[0032] When the robot moves backward, the installation positions of proximity switch 1 and proximity switch 2 are swapped to control the explosion-proof servo motor to reverse. The robot mileage value D' during the backward process is expressed as: D'=L×n d +Md.

[0033] The beneficial effects of the present invention are:

[0034] The present invention provides a mining track robot landmark detection and positioning system and a mining track robot positioning method, which adopt the principle of introducing a landmark detection and positioning system to correct the mileage positioning technology error to accurately position the mining track robot. The mileage positioning technology adopts the current mainstream counting encoder counting principle. The landmark detection and positioning system detects the positioning blocks equidistantly distributed along the robot's walking track according to a preset length by installing a proximity switch on the robot to obtain the actual distance, and corrects the encoder mileage value data section by section, thereby eliminating the cumulative error caused by the robot's long-distance travel. The introduction of the landmark detection and positioning system can automatically determine whether the positioning block detection is successful, avoiding the positioning failure caused by the introduction of a larger secondary error in the encoder mileage value correction due to the failure of the landmark detection. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 This is a schematic diagram of the road sign detection and positioning system of the mining rail robot in the present invention;

[0036] Figure 2 It is a rear view of the mining rail robot of the present invention;

[0037] Figure 3 This is a circuit schematic diagram of the road sign detection and positioning system for a mining rail robot in the present invention;

[0038] Figure 4 This is a flow chart of the positioning method of the mining rail robot in the present invention.

[0039] In the figure, 1. proximity switch 1; 2. proximity switch 2; 3. positioning mark; 4. explosion-proof servo motor; 5. explosion-proof electric control box; 6. incremental encoder; 7. controller; 8. servo driver. DETAILED DESCRIPTION

[0040] The invention is further described below in conjunction with the accompanying drawings and specific implementation examples. Since the use scenario of the mining rail robot is a coal mine environment, the equipment itself has rigid explosion-proof requirements. In addition, due to the special use environment restrictions such as high dust and low lighting in the coal mine, the existing global positioning (GPS) technology, feature visual recognition technology and other positioning technologies that rely on satellite positioning and optical image acquisition cannot be applied to the mining rail robot. At present, the odometer positioning technology is generally used to achieve positioning of the mining rail robot, and the number of robot walking wheels is counted by the encoder to convert and then achieve positioning. The limitation of this method is that when the robot's walking wheel slips, the mileage value calculated by the encoder count will deviate from the actual driving distance of the robot. As the robot continues to travel, the deviation will continue to accumulate, so this technology has a large error in the positioning application of the robot for long-distance travel. The present invention is a method for accurately positioning a mining rail robot, which aims to correct the positioning error caused by the odometer positioning technology that relies solely on the encoder, and realize the accurate positioning of the mining rail robot for long-distance travel.

[0041] like Figures 1-2 As shown, a road sign detection and positioning system for a mining track robot comprises: a mining track robot and a positioning mark 3 arranged on one side of the robot's walking track, as well as a proximity switch 1, a proximity switch 2, a servo motor 4, and a controller 7 installed on the robot, wherein the explosion-proof servo motor 4 is equipped with an incremental encoder 6 and a servo driver 8; the proximity switch 1, the proximity switch 2, and the incremental encoder 6 are connected to the input end of the controller 7, and the servo driver 8 is connected to the output end of the controller 7;

[0042] The proximity switch is used to detect the positioning mark. When the proximity switch 1 detects the positioning mark 3, the starting time t0 of the position integration is used as the trigger controller 7 to start the position integration. When the proximity switch 2 detects the positioning mark 3, the ending time t1 of the position integration is used as the trigger controller 7 to end the position integration. The position integral value s in the time period t0 to t1 is the moving distance of the mining rail robot during the detection of the positioning mark 3.

[0043] The incremental encoder 7 is used to count the number of turns of the robot's running wheels;

[0044] The controller 7 is used to determine the accuracy of the identification positioning according to the ratio of the detection value and the measurement value of the position integration result, wherein the measurement value is the distance between the two proximity switches, and the detection value is the distance within the time period when the two proximity switches successively detect the same positioning identification 3.

[0045] like Figure 1 As shown, the present invention sets a row of positioning marks 3 on one side of the robot walking track, and the spacing between the marks is L. The robot is equipped with two proximity switches 1 and 2 that can detect the positioning marks 3, and the distance between the two proximity switches is d. The robot walking wheel is driven by an explosion-proof servo motor 4, and the control system (including explosion-proof servo motor 4, incremental encoder 6, controller 7, servo driver 8) is encapsulated in an explosion-proof electric control box 5.

[0046] like Figure 3 As shown, the robot lower computer adopts the controller 7 of Siemens S7-1200PLC. Proximity switch 1 and proximity switch 2 transmit the collected switch quantity to Siemens S7-1200PLC. The walking drive explosion-proof servo motor 4 is equipped with an incremental encoder 6, which is electrically connected to Siemens S7-1200PLC via RS485 to count the number of walking wheels of the robot and upload the data. The explosion-proof servo motor 4 is driven by a servo driver 8, which is electrically connected to Siemens S7-1200PLC via RS485, and both are encapsulated in an explosion-proof electric control box 5.

[0047] like Figure 4As shown, a positioning method for a mining rail robot is implemented based on the landmark detection and positioning system for the mining rail robot. The method (explained by taking the robot's forward movement process as an example) includes:

[0048] Step 1: When the proximity switch detects the positioning mark, the controller triggers the servo driver to control the explosion-proof servo motor to drive the robot to decelerate and move forward at a speed v;

[0049] Step 2: Preset the encoder mileage value range to [L min ,L max ], determine whether the encoder mileage value M is within the range [L min ,L max ], if it exceeds this range, stop the machine to check the fault, otherwise go to step 3; where L min is the minimum mileage value of the encoder, L max is the maximum mileage value of the encoder;

[0050] The step 2 determines whether the encoder mileage value is within a reasonable range, which is specifically expressed as follows:

[0051] When the robot reaches the first positioning mark, the encoder is reset. If the encoder mileage value M when the robot continues to move forward to the second positioning mark is within [L min ,L max ] range, it indicates that the encoder mileage value is within a reasonable range; the encoder mileage value M is expressed as:

[0052] M=b / λ*γ*d s *π

[0053] In the formula, b is the encoder value, λ is the encoder resolution, γ is the reduction ratio, and d is s is the wheel diameter.

[0054] Step 3: When proximity switch 1 detects the positioning mark, the integral calculation starts, and when proximity switch 2 detects the same positioning mark, the integral calculation ends. The position integral value s in the time period t0 to t1 is expressed as:

[0055]

[0056] In the formula, v represents the travel speed of the mining robot, and t represents the time;

[0057] Step 4: Calculate the distance ratio between the integral value s and the measured value d Set the range of distance ratio to [i min ,i max ], if the distance ratio i is in the range [i min ,i max ], it indicates that the positioning is successful, where i minis the minimum value of the distance ratio, i max is the maximum value of the distance ratio; the step 4 is specifically expressed as:

[0058] If the distance ratio i is in the range [i min ,i max ], it indicates that the positioning is successful, and the number of positioning blocks is n d The count is increased by 1, the encoder mileage value M is cleared, the robot mileage value D is updated, and all settings are cleared at the same time; D is expressed as:

[0059] D=L×n d +M

[0060] If i is not in the range [i min ,i max ], it indicates that the positioning has failed, and the number of positioning times is n c The count is increased by 1, the explosion-proof servo motor reverses and the robot moves back a distance R, where R is a distance of 2 times the position integral value s, and then the explosion-proof servo motor rotates forward and the robot moves forward, returning to step 1 and starting again. If the number of failed positioning exceeds the set value n0, it is determined that there is interference affecting the positioning.

[0061] When the robot moves backward, the method of the present invention is used for positioning during the backward process. The control principle remains unchanged, but the installation positions of proximity switch 1 and proximity switch 2 need to be swapped, and the explosion-proof servo motor is controlled to reverse. Steps 1 to 4 are performed during the backward process. At this time, the number of positioning blocks n d The count is reduced by 1, and the robot mileage value D' is expressed as: D' = L × n d +Md.

[0062] The reasonable range of encoder mileage value introduced in the above control process [L min ,L max ]、Reasonable range of distance ratio[i min ,i max ] can automatically determine the accuracy of road sign detection and positioning, thereby ensuring the correctness of the encoder mileage value correction.

[0063] The present invention provides a method for positioning a mining rail robot, which accurately positions the mining rail robot by introducing a landmark detection and positioning system to correct the error of mileage positioning technology, wherein the mileage positioning technology adopts the current mainstream counting encoder counting principle, and the landmark detection and positioning system detects the positioning blocks equidistantly distributed along the robot's walking track according to a preset length by installing a proximity switch on the robot to obtain the actual distance, and corrects the encoder mileage value data section by section, thereby eliminating the cumulative error caused by the robot's long-distance travel. The invention is highly practical, the selected parts are mature in technology and low in price, the incremental encoder is an explosion-proof servo motor with an encoder, the acquisition device proximity switch and the metal positioning block in the landmark positioning system meet the explosion-proof requirements, and the data collection is not affected by special use environments such as high dust and low lighting. The entire positioning system is safe and reliable, low in cost, and has a long service life. The accuracy detection mechanism of the landmark detection and positioning system introduced in the invention can automatically determine whether the positioning block detection is successful, avoiding the positioning failure caused by the introduction of a larger secondary error in the encoder mileage value correction due to the failure of the landmark detection.

Claims

1. A mining track robot landmark detection and positioning system, characterized in that: include: A mining track robot and a positioning mark set on one side of the robot's walking track, as well as proximity switch 1, proximity switch 2, explosion-proof servo motor, and controller installed on the robot. The explosion-proof servo motor is equipped with an incremental encoder and a servo driver; proximity switch 1, proximity switch 2, and incremental encoder are connected to the input end of the controller, and the servo driver is connected to the output end of the controller; The proximity switch is used to detect the positioning mark. When the proximity switch 1 detects the positioning mark, it is used as the starting time t0 of the position integration, triggering the controller to start the position integration. When the proximity switch 2 detects the positioning mark, it is used as the ending time t1 of the position integration, triggering the controller to end the position integration. The position integration value s in the time period t0 to t1 is the moving distance of the mining rail robot during the detection of the positioning mark. The incremental encoder is used to count the number of turns of the robot's running wheels; The controller is used to determine the accuracy of the identification positioning according to the ratio of the position integral value s to the measurement value d, wherein the measurement value d is the distance between the two proximity switches.

2. A mining rail robot landmark detection and positioning system according to claim 1, characterized in that: The controller adopts Siemens S7-1200PLC, the incremental encoder is connected to Siemens S7-1200PLC via RS485, the explosion-proof servo motor is driven by a servo driver, and the servo driver is connected to Siemens S7-1200PLC via RS485. The explosion-proof servo motor, incremental encoder, controller and servo driver are all packaged in an explosion-proof electric control box.

3. A positioning method for a mining rail robot, implemented based on the mining rail robot landmark detection and positioning system according to any one of claims 1 to 2, characterized in that: The method comprises: Step 1: When the proximity switch detects the positioning mark, the controller triggers the servo driver to control the explosion-proof servo motor to drive the robot to decelerate and move forward at a speed v; Step 2: Preset the encoder mileage value range to [L min ,L max ], determine whether the encoder mileage value M is within the range [L min ,L max ], if it exceeds this range, stop the machine to check the fault, otherwise go to step 3; where L min is the minimum mileage value of the encoder, L max is the maximum mileage value of the encoder; Step 3: When proximity switch 1 detects the positioning mark, the integral calculation starts, and when proximity switch 2 detects the same positioning mark, the integral calculation ends. The position integral value s in the time period t0 to t1 is expressed as: In the formula, v represents the travel speed of the mining track robot, and t represents the time; Step 4: Calculate the distance ratio between the integral value s and the measured value d Set the range of distance ratio to [i min ,i max ], if the distance ratio i is in the range [i min ,i max ], it indicates that the positioning is successful, where i min is the minimum value of the distance ratio, i max is the maximum value of the distance ratio.

4. A method for positioning a mining rail robot according to claim 3, characterized in that: The step 2 determines whether the encoder mileage value is within a reasonable range, which is specifically expressed as follows: When the robot reaches the first positioning mark, the encoder is reset. If the encoder mileage value M when the robot continues to move forward to the second positioning mark is within [L min ,L max ] range, it indicates that the encoder mileage value is within a reasonable range; the encoder mileage value M is expressed as: M=b / λ*γ*d s *p In the formula, b is the encoder value, λ is the encoder resolution, γ is the reduction ratio, and d is s is the wheel diameter.

5. A method for positioning a mining rail robot according to claim 4, characterized in that: The step 4 is specifically described as follows: If the distance ratio i is in the range [i min ,i max ], it indicates that the positioning is successful, and the number of positioning marks is n d The count is increased by 1, the encoder mileage value M is cleared, and the robot mileage value D is calculated. D is expressed as: D=L×n d +M Where L represents the distance between positioning marks; If i is not in the range [i min ,i max ], it indicates that the positioning has failed, and the number of positioning times is n c The count is increased by 1, the explosion-proof servo motor reverses and the robot moves back a distance R, where R is a distance of 2 times the position integral value s, and then the explosion-proof servo motor rotates forward and the robot moves forward, returning to step 1 and starting again. If the number of failed positioning exceeds the set value n0, it is determined that there is interference affecting the positioning.

6. A method for positioning a mining rail robot according to claim 5, characterized in that: When the robot moves backward, the installation positions of proximity switch 1 and proximity switch 2 are swapped to control the explosion-proof servo motor to reverse. The robot mileage value D' during the backward process is expressed as: D'=L×n d +Md.

Images