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

Claim 3:
4. The method for drivable area detection and autonomous obstacle avoidance of unmanned haulage equipment in deep confined spaces according to claim 1, wherein in step S24, said using a particle swarm path planning method designed for the deep confined roadway comprises the following steps:
S241: performing grid encoding and decoding on the grid map established based on drivable area detection in step S21 and the risk grid map established based on the projection of the drivable area of step S1 in step S22; where according to the features of the grid map, an encoding method of a collision-free local optimal path is: Xi is defined to be an obstacle-free path from a current location of a coal mine robot to a specified target point, which may be represented by all grids that constitute the path, i.e.,

Xi={V1,V2, . . . ,Vn}, where, V1,V2, . . . ,Vn represents the serial numbers of all the grids through which the path Xi passes, there is no complete risk grid with a risk level of 5, and the serial numbers are not repeated mutually; and the serial numbers of the grids are continuously arranged from top to bottom and left to right by taking the first grid at the upper left corner of the grid map as 1 until the last grid at the lower right corner reaches, that is,, V
   n
  
  =
  
   
    
     (
     
      
       v
       
        n
        ,
        1
       
      
      -
      
       d
       2
      
     
     )
    
    ×
    
     G
     row
    
   
   +
   
    v
    
     n
     ,
     2
    
   
   +
   
    d
    2, where, vn,1 and vn,2 represent a x-coordinate and y-coordinate of a current grid, which are the coordinates of a center point of the grid but not the coordinates of a vertex of the grid; d is a side length of the grid; Grow is the number of one row of grids in the current grid map; and, after path points are computed, the following formula is used during inverse solving:, [
   
    
     v
     
      n
      ,
      1
     
    
    ,
    
     v
     
      n
      ,
      2
     
    
   
   ]
  
  =
  
   [
   
    
     
      mod
      ⁢
      
       (
       
        
         
          V
          n
         
         -
         1
        
        ,
        
         G
         
          r
          ⁢
          o
          ⁢
          w
         
        
       
       )
      
     
     +
     
      d
      2
     
    
    ,
    
     
      ceil
      ⁡
      (
      
       
        V
        n
       
       -
       
        1
        /
        
         G
         
          r
          ⁢
          o
          ⁢
          w
         
        
       
      
      )
     
     -
     
      d
      2
     
    
   
   ], so as to complete the grid encoding and decoding;
S242: processing obstacle information according to the drivable area detection information in step S1 and the grid map established in step S21, and initializing a particle swarm population; comprising the following steps:
S2421: initially establishing a square matrix of which the numbers of rows and columns are both the total number Voverall of the grid, and establishing a grid connection matrix Mlink;
S24211: computing whether the current grid is adjacent to all other grids by use of cyclic traversal, and judging whether the adjacent grids are obstacles;
S24212: if the current grid is not adjacent to all other grids or the adjacent grids are obstacles, setting a corresponding matrix element to 0; and if the current grid is adjacent to all other grids and the adjacent grids are not obstacles, computing a distance between the adjacent grids by using a formula as follows:

Vdn,i=√{square root over ((vn,1−vi,1)2+(vn,2−vi,2)2)}, where, Vdn,i is a grid distance between a grid n and a grid i, and v1 and v2 represent a x-coordinate and y-coordinate of the current grid;
S2422: determining a start point VS of the coal mine robot and a final target point VE thereof, and placing the two at the head and tail nodes of an initial route X0 through the encoding method in step S241;
S2423: randomly selecting, from a starting node VS, a next grid connected to the start point according to the connection matrix Mlink established in step 1; and
S2424: repeating step S2423 according to the connection matrix Mlink until an encoding combination of a complete path connecting VS to VE is completed, and outputting an initial path;
S243: based on the grid decoding/encoding methods in S241 and the initialized population in S242, updating the velocity and location of particles in a particle swarm by using a formula as follows:

vint+1={ϕ1,ϕ2,ϕ3}, a particle velocity consists of three replacement sets; where, Φ1, Φ2 and Φ3 represent three parts of the particle velocity: a self-velocity term, a cognitive term and a social term, and the last two terms are determined by a cognitive factor c1, an individual historical optimal solution pbestti, a social factor c2, and a global optimal solution gbestt, which is specifically as follows:, ϕ
   1
  
  =
  
   {
   
    
     
      
       
        
         0
         ,
          
        
       
       
        
         ω
         <
         
          R
          i
         
        
       
      
      
       
        
         
          
           rand
            
           (
           
            X
            i
           
           )
          
          =
          
           rand
           (
           
            
             V
             1
            
            ,
            
             V
             2
            
            ,
            …
               
            ,
            
             V
             n
            
           
           )
          
         
         ,
          
        
       
       
        otherwise
       
      
     
     ⁢
     
     
      V
      n
     
    
    =
    
     
      
       
        round
        (
        
         
          rand
          (
          
           0
           ,
           1
          
          )
         
         ×
         
          G
          row
         
        
        )
       
       ×
       
        G
        col
       
      
      +
      
       round
       (
       
        
         rand
         (
         
          0
          ,
          1
         
         )
        
        ×
        
         G
         col
        
       
       )
      
      +
      
       d
       ⁢
       
       
        ϕ
        2
       
      
     
     =
     
      {
      
       
        
         
          
           
            {
            
             
              
               
                
                 
                  SN
                  ⁡
                  (
                  
                   pbest
                   
                    i
                    ⁢
                    j
                   
                   t
                  
                  )
                 
                 ,
                 
                  
                   pbest
                   
                    i
                    ⁢
                    j
                   
                   t
                  
                  =
                 
                
               
               
                
                 
                  
                   
                    x
                    
                     i
                     ⁢
                     n
                    
                    t
                   
                   &
                  
                  ⁢
                  j
                 
                 ≠
                 n
                
               
              
              
               
                
                 0
                 ,
                
               
               
                otherwise
               
              
             
             ,
             otherwise
            
           
          
          
            
          
         
         
          
           
            0
            ,
           
          
          
           
            
             c
             1
            
            <
            
             R
             i
            
           
          
         
        
        ⁢
        
        
         ϕ
         3
        
       
       =
       
        {
        
         
          
           
            
             
              {
              
               
                
                 
                  
                   
                    SN
                    ⁡
                    (
                    
                     gbest
                     j
                     t
                    
                    )
                   
                   ,
                   
                    
                     gbest
                     j
                     t
                    
                    =
                   
                  
                 
                 
                  
                   
                    
                     
                      x
                      
                       i
                       ⁢
                       n
                      
                      t
                     
                     &
                    
                    ⁢
                    j
                   
                   ≠
                   n
                  
                 
                
                
                 
                  
                   0
                   ,
                  
                 
                 
                  otherwise
                 
                
               
               ,
               otherwise
              
             
            
            
              
            
           
           
            
             
              0
              ,
             
            
            
             
              
               c
               1
              
              <
              
               R
               i
              
             
            
           
          
          ⁢
          
          
           R
           i
          
         
         =
         
          
           R
           min
          
          +
          
           rand
           ⁢
            
           
            (
            
             0
             ,
             1
            
            )
           
           ×
           
            (
            
             
              R
              max
             
             -
             
              R
              min
             
            
            ), where, the self-velocity term is recorded by using random distribution to calculate grid coordinates and using the encoding method in step S241 to calculate corresponding grids, ω is a velocity inertia weight, and Gcol is the number of one column of grids in the current grid map; the cognitive term is recorded using the serial numbers of same positions in a set of paths Xi represented by a current particle i in the individual historical optimal solution pbestti, and part of the serial numbers are set to 0 according to a certain probability; and the social term is updated by using the same strategy to obtain part of the serial numbers of the same positions in a set of paths Xi represented by the current particle i and in the global optimal solution gbestt; Ri represents a replacement rate;

xit+1=ƒ(replace(xit))

replace(xit)=comb(xit,vit+1)=comb(xit,{ϕ1,ϕ2,ϕ3}), a position update item is a fitness value of a set of paths Xi represented by the current particle i;
the path Xi represented by the current particle i is subjected to position replacement based on the three replacement sets in the particle velocity; ƒ(⋅) is a fitness function, comb(⋅) is a permutation & combination function, and replace(⋅) is a replace function, indicating a replacement made between the current path Xi and the particle velocity vit+1; and
S244: determining whether a maximum number of iterations is reached, if so, outputting an optimal autonomous obstacle avoidance path corresponding to a current end point, otherwise, returning to S243 to continue iteration.