Patent ID: 11880208
Assignee: CHINA UNIVERSITY OF MINING AND TECHNOLOGY
Field: Control (Instruments)
Classification: CPC G | IPC G

Claim 0:
1. A method for drivable area detection and autonomous obstacle avoidance of unmanned haulage equipment in deep confined spaces, comprising the following steps:
S1: performing environmental perception by use of binocular vision, and detecting a drivable area of an unmanned auxiliary haulage vehicle in a deep coal mine roadway; specifically comprising:
S11: collecting, by a binocular camera, a video image of the auxiliary haulage vehicle driving in the coal mine roadway, and preprocessing a coal mine roadway image;
S12: for a preprocessed coal mine roadway image obtained in step S11, designing a stereo matching algorithm specific to a stereo matching task of the coal mine roadway to implement a generation of a depth map of the coal mine roadway and computation of 3D point cloud data; specifically comprising:
S121: constructing an atrous convolution window with a specification of 5*5, and assigning different weights to different positions of the window according to a 2D Gaussian distribution function, where the weights in a sequence from left to right and top to bottom are respectively 3, 0, 21, 0, 3, 0, 0, 0, 0, 0, 21, 0, 1, 0, 21, 0, 0, 0, 0, 0, 3, 0, 21, 0, 3;
S122: covering a left view of the coal mine roadway image with the convolution window constructed in step S121, and selecting pixel points in all coverage areas;
S123: covering a right view of the coal mine roadway image with the convolution window constructed in step S121, and selecting pixel points in all coverage areas;
S124: finding an absolute value of a gray value difference of all corresponding pixel points in the convolution window coverage areas of the left and right coal mine roadway views in step S122 and step S123, and according to the weights of the different positions of the window in step S121, performing weighted summation on the weights as a matching cost by using a formula as follows:, C
    
     matching
     ⁢
        
     cost
    
   
   (
   
    p
    ,
    d
   
   )
  
  =
  
   
    ∑
    
     q
     ∈
     
      N
      p
     
    
   
   
    (
    
     
      
       ❘
       "\[LeftBracketingBar]"
      
      
       
        
         I
         L
        
        (
        q
        )
       
       -
       
        
         I
         R
        
        (
        qd
        )
       
      
      
       ❘
       "\[RightBracketingBar]"
      
     
     ·
     
      w
      q
     
    
    ), where, p is a pixel of the coal mine roadway image, d is a disparity of the coal mine roadway image, IL (q) and IR (qd) are window elements taking q and qd as image centers on the left and right coal mine roadway images respectively, w q is the weight of the different positions of the convolution window, and N p is a 5*5 Gaussian atrous convolution window;
S125: performing matching cost aggregation according to a matching cost computation method in step S124, where a matching cost aggregation step size d step is adaptively changed according to a pixel luminance of the coal mine roadway image by using a formula as follows:, d
   
    s
    ⁢
    t
    ⁢
    e
    ⁢
    p
   
  
  =
  
   
    
     
      
       D
       max
      
      -
      1
     
     
      
       G
       max
      
      -
      
       G
       min
      
     
    
    ·
    g
   
   +
   
    
     
      G
      max
     
     -
     
      
       G
       min
      
      ⁢
      
       D
       max
      
     
    
    
     
      G
      max
     
     -
     
      G
      min, where, Dmax is a maximum disparity of a binocular image of the coal mine roadway, Gmax and Gmin are a maximum gray value and a minimum gray value of a gray image of the coal mine roadway respectively, and g is a gray value of the gray image of the coal mine roadway;
S126: based on matching costs in an adaptive matching cost aggregation step size dstep range obtained in step S125, finding a window with a minimum matching cost value as a disparity by using a winner-take-all (WTA) algorithm, and performing cyclic computation to obtain disparity maps of the coal mine roadway;
S127: based on the disparity maps of the coal mine roadway obtained in step S126, performing disparity image optimization in accordance with a left-right consistency constraint criterion by using a formula as follows:, D
  =
  
   {
   
    
     
      
       
        D
        l
       
          
      
     
     
      
       
        if
        ⁢
         
        
         
          ❘
          "\[LeftBracketingBar]"
         
         
          
           D
           l
          
          -
          
           D
           r
          
         
         
          ❘
          "\[RightBracketingBar]"
         
        
       
       ≤
       1
      
     
    
    
     
      
       
        D
        invalid
       
        
      
     
     
      otherwise, where, Dl is a left disparity map, Dr is a right disparity map, and Dinvalid is an occlusion point where no disparity exists; and
S128: based on a disparity optimization image of the coal mine roadway obtained in step S127, performing disparity-map to 3D-data computation according to a binocular stereo vision principle, and obtaining 3D point cloud information (Xw, Yw, Zw) of the coal mine roadway in the advancing direction of the unmanned haulage vehicle by using a formula as follows:, {
  
   
    
     
      
       
        X
        w
       
       (
       
        x
        ,
        y
       
       )
      
      =
      
       
        b
        ·
        f
        ·
        
         (
         
          x
          -
          u
         
         )
        
       
       
        α
        ·
        
         d
         ⁡
         (
         
          x
          ,
          y
         
         )
        
       
      
     
    
   
   
    
     
      
       
        Y
        w
       
       (
       
        x
        ,
        y
       
       )
      
      =
      
       
        b
        ·
        f
        ·
        
         (
         
          y
          -
          v
         
         )
        
       
       
        β
        ·
        
         d
         ⁡
         (
         
          x
          ,
          y
         
         )
        
       
      
     
    
   
   
    
     
      
       
        Z
        w
       
       (
       
        x
        ,
        y
       
       )
      
      =
      
       
        b
        ·
        f
       
       
        d
        ⁡
        (
        
         x
         ,
         y
        
        ), where, b is a distance between left and right optical centers of a binocular camera, ƒ is a focal distance of the camera, d is a disparity of the coal mine roadway image, (x, y) indicates pixel coordinates of the coal mine roadway image, (u, v) indicates coordinates of an origin of an image coordinate system in a pixel coordinate system, and α and β are the focal distances of a pixel in the x and y directions of an image plane respectively;
S13: for the preprocessed coal mine roadway image obtained in step S11, designing a deep learning model for a semantic segmentation task of the coal mine roadway to implement the drivable area semantic segmentation of a 2D image of the coal mine roadway; specifically comprising:
S131: making a semantic segmentation data set of the coal mine roadway in which only a drivable area of the roadway is marked, which specifically comprises:
S1311: performing marking by using labelme image marking software, and starting the labelme software;
S1312: opening a coal mine roadway image folder, and selecting an image;
S1313: only selecting the drivable area of the coal mine roadway with a frame, and naming the drivable area drivable area; and
S1314: repeating steps S1312 and S1313, so as to finally complete the making of the drivable area semantic segmentation data set of the 2D image of the coal mine roadway;
S132: pre-training a semantic segmentation model deeplabv3+ model based on a PASCAL VOC data set;
S133: based on a pre-trained deeplabv3+ model obtained in step S132 and the drivable area semantic segmentation data set of the coal mine roadway obtained in step S131, performing pre-trained model fine-tuning;
S134: performing real-time drivable area semantic segmentation of the 2D image of the coal mine roadway according to the deep learning model fine-tuned with the data set of the coal mine roadway obtained in step S133 to obtain a 2D image drivable area;
S14: according to the 3D point cloud data of the coal mine roadway obtained in step S12 and a 2D drivable area segmentation image of the coal mine roadway obtained in step S13, designing a 2D-image to 3D-point-cloud mapping method to implement the drivable area semantic segmentation of a 3D point cloud of the coal mine roadway; specifically comprising:
S141: according to the 2D image drivable area obtained in step S134, performing drivable area image processing based on morphological opening operation; and
S142: according to a left coal mine roadway view of the unmanned auxiliary haulage vehicle, performing segmentation for mapping the 2D image drivable area obtained in step S134 to the 3D point cloud so as to obtain a 3D point cloud drivable area of the coal mine roadway by using a formula as follows:, P
   ⁡
   (
   
    x
    ,
    y
    ,
    z
   
   )
  
  =
  
   {
   
    
     
      
       P
       ⁡
       (
       
        x
        ,
        y
        ,
        z
       
       )
      
     
     
      
       
        (
        
         x
         ,
         y
        
        )
       
       ∈
       
        I
        ⁡
        (
        
         x
         ,
         y
        
        )
       
      
     
    
    
     
      0
     
     
      
       
        (
        
         x
         ,
         y
        
        )
       
       ∉
       
        I
        ⁡
        (
        
         x
         ,
         y
        
        ), where, P (x, y, z) is the 3D point cloud data of the coal mine roadway obtained in step S128, I (x, y) is a drivable area image of the coal mine roadway obtained after morphological processing is performed in step S141, and (x, y) indicates pixel coordinates of the left coal mine roadway view collected by the binocular camera;
S2: determining a drivable area of a deep confined roadway according to drivable area detection information in step S1, and performing safe driving of the unmanned auxiliary haulage vehicle in the deep confined roadway by using an autonomous obstacle avoidance algorithm based on a particle swarm optimization algorithm; specifically comprising:
S21: establishing a 2D workspace grid map of the unmanned auxiliary haulage vehicle based on the drivable area detection information in step S1, and intercepting a roadway grid map with an appropriate length, where the roadway grid map comprises deformation and roof leakage of blanket nets and support materials at the top of the roadway, deformation, destruction, leakage and cracking on both sides of the deep roadway, and water puddles and other obstacles on the ground surface of the roadway;
S22: establishing a risk grid map based on the projection of the drivable area in step S1, where the risk grid map contains a safety area, a partial safety area and a complete risk area; the safety area is a part of the drivable area in step S1, in which the unmanned auxiliary haulage vehicle is allowed to drive directly; the complete risk area is an undrivable area, in which the unmanned auxiliary haulage vehicle is completely not allowed to drive; the partial safety area is a drivable area obtained by segmenting in step S1 between the safety area and the undrivable area, in which there is a risk when the unmanned auxiliary haulage vehicle drives; and in autonomous obstacle avoidance planning, a driving route of the vehicle should be planned in the safety area as much as possible, and cannot be planned in a safety risk area, and under a certain condition, the driving route of the vehicle is allowed to include the partial safety area, but should be far away from the complete risk area as much as possible; and a rule of establishment among the three kinds of areas is as follows:
S221: area risk levels are established according to the drivable area of the vehicle obtained by segmenting in step S1: the undrivable area itself is at a highest level 5; eight neighbor grids of a current grid are set to be at a risk level 4 by taking a grid where the undrivable area is located as a reference point; repeating is performed in a similar fashion until a risk level 1 reaches; the risk levels of other grids that are not at risk are still 0, that is, they are fully passable; if there is a conflict in the risk level of a grid, the grid with the conflict is assessed with the highest risk level; and the undrivable area itself is an absolute complete risk area in which vehicles are not allowed to drive, and the safety area and the partial safety area are both drivable areas, where a grid with the risk level of 0 is a safety area;
S23: intercepting a map with an appropriate length, selecting, in the map, a grid allowed to serve as a local end point by taking the unmanned auxiliary haulage vehicle as a start point of the current map, and recording the grid into a table of end points to be selected in accordance with a rule as follows: an end point to be selected is in the last column of a local grid map; the grid is not an obstacle grid; the neighbor grids of the end point to be selected at least comprise one passable grid; and the grid is not surrounded by obstacles;
S24: performing autonomous obstacle avoidance path planning by using a particle swarm path planning method designed for the deep confined roadway;
S25: obtaining an optimal end point to be selected of a driving path by using a greedy strategy, and enabling the unmanned auxiliary haulage vehicle to drive according to the optimal end point and an optimal path; and
S26: repeating steps S21 to S25 to complete the autonomous obstacle avoidance of the unmanned auxiliary haulage vehicle in the deep confined roadway until the unmanned auxiliary haulage vehicle arrives at a task destination;
wherein in step S11, the process of collecting, by a binocular camera, a video image of the auxiliary haulage vehicle driving in the coal mine roadway, and preprocessing a coal mine roadway image comprises the following steps:
S111: performing coal mine roadway image correction processing by using a Hartley image correction algorithm;
S112: performing coal mine roadway image enhancement processing on a corrected image obtained in step S111 by using an image enhancement algorithm based on logarithmic Log transformation; and
S113: performing image filtering processing on an enhanced image obtained in step S112 by using an image filtering algorithm based on bilateral filtering; and
wherein in step S111, the process of performing coal mine roadway image correction processing by using a Hartley image correction algorithm comprises the following steps:
S1111: obtaining an epipolar constraint relationship of the left and right coal mine roadway images obtained according to a camera calibration algorithm, and finding epipolar points p and p′ in the left and right coal mine roadway images;
S1112: computing a transformation matrix H′ mapping p′ to an infinity point (1, 0, 0)T;
S1113: computing a photographic transformation matrix H matched with the transformation matrix H′, and satisfying a least square constraint, so as to minimize the following formula:, min
  ⁢
  
   
    ∑
    i
   
   
    
     J
     ⁡
     (
     
      
       H
       ⁢
       
        m
        
         1
         ⁢
         i
        
       
      
      ,
      
       
        H
        ′
       
       ⁢
       
        m
        
         2
         ⁢
         i
        
       
      
     
     )
    
    2, where, m1i=(u1,v1,1), m2i=(u2,v2,1), and J represents a cost function error, and (u1, v1) and (u2, v2) are a pair of matching points on original left and right images; and
S1114: allowing the transformation matrix H′ in step S1112 and the photographic transformation matrix H in step S1113 to respectively act on the left and right coal mine roadway images to obtain a corrected coal mine roadway image.