IROS2019国际学术会议论文集0831

Dynamic Density Topological Structure Generation for Real-Time Ladder Affordance Detection Azhar Aulia Saputra, Wei Hong Chin, Yuichiro Toda, Naoyuki Takesue, and Naoyuki Kubota AbstractThis paper presents a method with dynamic density topological structure generation for low-cost real-time vertical ladder detection from 3D point cloud data. Dynamic Density Growing Neural Gas (DD-GNG) is proposed to generate a dynamic density of the topological structure. The density of the structure and the number of nodes will be increased in the targeted object area. Feature extraction model is required to classify suspected objects for being processed in the next time process. After that, rungs of the vertical ladder is processed using an inlier-outlier method. Thus, the ladder detection model represents the ladder with a set of nodes and edges. Next, affordance detection is processed for detecting the feasible grasped location. To validate the effectiveness of the proposed method, a series of experiments are conducted on a 4-legged robot with a non-GPU board for real-time vertical ladder detection and climbing to validate the effectiveness of the proposed method. Results show that our proposed method able to detect and track the ladder structure in real-time with a much lower computational cost. The affordance of the ladder provides safety information for robot grasping. Index TermsLow Cost Vertical Ladder Detection, Dynamic Density Topological Structure, Grasped Affordance detection I. INTRODUCTION The development of robotic sensory systems is increasing signifi cantly in recent years. Robots are able to extract surrounding information from image data and/or point cloud data. However, for building the environment map and also shaping objects, 3D point cloud data is more effi cient to be processed. Once facing the huge data, the process of the extraction may have a problem in computational cost. There- fore, topological map structure is one of good alternative for representing the raw data, in order to decrease the cost in the main process. There are several topological structure generation model proposed by several researchers 1, 2, 3, 4.We devel- oped topological map building based on Adaptive resonance theory (ART) 5, 6. Toda el al, has built a real-time 3D topological map building using growing neural gas (GNG). However, it is diffi cult to shape the detail object 7 and the density of structure and number of nodes must be defi ned in advance. Therefore, we proposed a method for dynamic density topological structure building based on growing neural gas (DD-GNG) for specifying the detailed shape of an object in the attention area. The density of nodes in Azhar Aulia Saputra, Wei Hong Chin, Naoyuki Takesue, and Naoyuki KubotaarewiththeGraduateSchoolofSystemDesign,Tokyo Metropolitan University, 6-6 Asahigaoka, Hino, Tokyo, 191-0065, Japan (e-mail:azhar-aulia-saputraed.tmu.ac.jp;weihong118118; ntakesuetmu.ac.jp; kubotatmu.ac.jp;). Yuichiro Toda is with the Graduate School of Natural Science, Okayama University, Okayama, 700-8530, Japan (e-mail: ytodaokayama-u.ac.jp). the topological structure will increase when the proposed segmentation model detects the speculated obstacle, while lesser nodes are generated (low density) in the safe area, such as a wall, fl at terrain, and memorized objects. We implemented the proposed DD-GNG on a 4-legged robot to detect a ladder in real-time for validation. In disaster area or hazardous environments, vertical ladders are often broken, fragile, and out of shape. Therefore, in order to perform ladder climbing, the robot has to always pay attention to the grasping area of the ladder. It requires a real-time ladder detection that able to continually track and estimate the position of rungs of the ladder. Currently there are several researcher conducting ladder and stair detection, or object detection using deep learning models 8, 9, 10. 8 applied a Convolution Neural net- work (CNN) for recognizing and localizing a staircase. The detection may failed if the detected staircase is different from the training samples. VOxNet 9 is developed based on CNN architecture integrating with a volumetric Occupancy Grid representation for object detection from RGBD and LiDaR point clouds. These methods suffer from high computational cost. Some researcher implemented ladder detection for sup- porting their behavior model. Vaillant and Yoneda imple- mented vertical ladder detection for climbing behavior on a biped robot 11, 12. The DRC-Hubo humanoid robot successfully performed the ladder climbing in real world environment 13. However, the authors did not mention the process of ladder detection and we are uncertain if the robot continuously detects the ladder during movement or just one time detection before the move. Zhang et. al. 14 presented a planning strategy for the humanoid robot Hubo-II+ to generate multi-limbed locomotion sequences automatically for a given specifi cation of a ladder. The proposed work is validated in simulation only and real-world implementation remained a concern. The development of ladder observation has been proposed by several researchers 15, 16, 17. Chen et. al. 15 de- veloped a system to fuse vision and laser point cloud data for tracking ladder in 3D. The proposed work was validated in a humanoid robot ATLAS in the DARPA Robotics Challenge. However, if the tracking error is large, the visual tracker cannot be recovered and thus lead to tracking failure. In 16, the authors apply RANSAC algorithm with voxel grid fi lter to determine the ladder and its rung from 3D point clouds and represent them in lines. The method is suffer of high computational cost. Anna 17 implemented a supervised learning algorithm that trains a Markov random fi eld (MRF) 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) Macau, China, November 4-8, 2019 978-1-7281-4003-2/19/$31.00 2019 IEEE3439 to segment 3D point clouds for detecting support surfaces of climbable structures such ladder. However, this system requires a pre-training process. 7, 18, 19 proposed a GNG for generated topological map. The proposed method works well for time series 3D point cloud data by providing a rough segmentation. However, the work is unable to generate details topological structure of objects such as a ladder without providing safe and unsafe grasping. The contribution of this paper are i) a real time ladder detection method is developed for detecting and tracking rungs of a ladder from 3D point cloud data, meanwhile continuously tracking and updating the rungs condition and position with low computational cost; ii) the density of structure generated by the proposed method is continuously changing for adapting the speculated objects. The paper is organized as follows. Section II presents the proposed DD-GNG details with the algorithmic explanation. The ladder affordance detection model is explained in Sec- tion III. Then, Section IV shows experimental results. Finally, Section V shows the conclusion of the proposed model. II. DYNAMICDENSITYGROWINGNEURALGAS Fig. 1.Design of proposed 3D Point Cloud Sensor The proposed model generates a topological structure with a dynamic density of nodes for representing the sensory input data. The topological structure model is generated from the incoming sensory data in an unsupervised learning manner. Algorithm 1 Dynamic Topological Structure 1:Init: generate two nodes at random position 2:ci,j= 0 (i,j), A = 1,2, r = 2,t = 0 3:loop 4:P raw data of proposed sensors 5: 0 6:for i 1 to maxdo 7:if t mod 2 = 0Vobstacle detected then 8:V a random of P in suspected obstacle 9:else 10:V a random of P 11: +1 12:GNG main process (V) / (main layer) 13:if r rthen 14:Triangulation process (c,h) 15:L Segmentation Process (c,h) 16:LsuspectedSuspected obstacle detection (L) 17:L(size) suspected Get suspected obstacle area (L) The proposed algorithm is detailed in Algorithm 1. The proposed method automatically determines the speculated (a)(b) Fig. 2.(a) Illustration of triangulation. Black color lines are the edges generated by the proposed GNG, blue color line is the supported edge of rectangle area (green area), and green color lines represent the supported edge of pentagonal area (yellow area). (b) llustration of normal vector calculation in triangle plane. areas from the input raw data (P). In the initial process, two nodes are randomly generated. The main process of GNG is triggered and continually update the topological structure by adding or deleting nodes according to incoming sensory data. The detail of basic GNG process can be seen in the previous research 7. V represents raw point cloud input data. hiand w represent the nth dimensional of a node value and weight value respectively. A is composed as a set of hi; cijand gij represent edges value and its age between the ith and jth nodes. The GNG determines the nearest unit (winner) nodes (s1) and the second-nearest nodes (s2) from the set of nodes A. If there is no connection between node s1and s2, a new connection is created to connect these nodes (cs1,s2= 1). In GNG-U, each node has the utility value Ui. All nodes contain a degree of strength represented by parameter that generated in Alg. 2 when nodes are created. The node with the minimum utility value is removed from the topological structure, Eu/Ulku. When the number of nodes (r) is more than a preset threshold (r), triangulation and segmentation are performed. After the GNG process, feature extraction is performed to determine triangle surfaces that composed of three nodes. Therefore, non-triangle sur- face is neglected during the fi rst triangulation process. The illustratrion of triangulation is shown in Fig. 2a. After triangle surface detection, the normal vector of each triangle surface is calculated using Eq. 1 and illustrated in Fig. 2b. Next, weight connections between one triangle surface to surrounding surface are generated. In this case, one surface will have maximum three connection. The value of weight is represented by angle between two normal vector that calculated in Eq. 2. N = (n0n1)(n0n2) |(n0n1)(n0n2| (1) wij= cos1 ? Ni Nj | Ni| Nj| ? (2) A. Segmentation Process After building the weight connection, segmentation is executed to segment the topological structure into several sections (wall, safe terrain, and unknown objects). The segmentation steps are explained as follows: 1) Set the labeling id parameter as 0 (ci= 0,i B), B is the number of surface. Set number of label as 0 3440 (nl= 0). Set a number of surface in a set of label as null value (La= 0) and a set vector in label (T = 0) . Set incremental value as 0 (i = 0). 2) If the label id is 0 (ci= 0), then add a new label (n = n+1,ci n), Ln+,Tn,Ln Ni. 3) Analyze the weight value between ith surface and neighbor surface with wijparameter. if wijlower than the preset threshold (), then jth surface joins to ith surface, cj= ci,Lci= Lci+1,Tci,+ Nj, go to step 6. 4) Analyze the weight value between ith surface and neighbors surface with wijparameter. if wijlower than the preset threshold (), then label id jth surface is moved to label of ith surface, Lci+ = Lcj,Tci,+=Lcj Tcj 5) If all surface are selected (i = B), then stop segmenta- tion process. Otherwise, select next surface (i = i+1) and go to step 2. In segmentation process, the number of label (n), number of surface of each label (L), and normal vector of surface of each label (T) are collected and calculated in Eq. 3. The segmentation process also generates the objects position (Ob(position) and objects size (Ob(size). After that, we classify into 3 types of object such as wall, safe ground, and obstacles. Wall label is selected when the average degree of surface and normal vertical vector v = 0,0,1 is close to 90 degree. Safe terrain is considered when the degree between normal vector of object and vertical vector within certain range of degree. Otherwise, we consider as obstacle that have to be analyzed further. The degree of plane in every surrounding is calculated using Eq. 4 beforehand. Eq. 5 shows the condition of determining the type of object. The safety degree is calculated by considering the degree of planes slope with vertical vectors. Ob(v) i = 1 Li Li j=0 Ti,j(3) i= cos1 ? Ob(v)i N(ver) | Ob(v)i| N(ver)| ? ,N(ver)= 0,1,0(4) Obi= Safe groundif n is even Wallif n is odd ObstacleOtherwise (5) B. Speculated Object Detection In order to determine the speculated object, we fi rst extract the static and moving objects from the sensory data. The static object is considered as a speculated object when it is located within the robot moving space. Also, moving objects that move towards the robot moving space will be considered as speculated objects as well. If the ith object is considered as the speculated obstacle, then Ob(sus) i = 1. After speculated objects are determined, the density of topological structure in the region of speculated objects will be increased. Then, the topological structure is used for action decision in behavior adaptation. Algorithm 2 generate the strength of node (hi) 1: 1 2:for j 1 to Ob(num)do 3:if Objis suspected obstacle then 4:Amin= Ob(position) i Ob(size) i /2 5:Amax= Ob(position) i +Ob(size) i /2 6:if hi Amin Vh i 0 is the number of detected rungs. For each iteration, detected rungs are evaluated and its position is updated depending on the movement of nodes. Fig. 3.Illustration of evaluation when generated nodes n1and n2is not connected Fig. 4.Generated nodes n1and n2 is considered as a rung. The fi gure show the parameter involved for evaluation and position correction A. Determine Perspective Rung In order to fi nd perspective rungs, two nodes (hn1, hn2) in the speculated area are selected randomly. After that, we evaluate the nodes by considering the nodes connectivity and density in the surrounding line through node n1and n2. Fig. 4 3441 shows the example condition of accepted connectivity. Then, Fig. 3 shows the failed ladder detection, thus the next step will not processed when face this condition The evaluation steps are shown in Algorithm 4. The evaluation value () is calculated using Eq. 6. ciand corepresents the number of nodes in the inlier area and outliers area respectively. n(1) and n(2)is the starting nodes and end nodes of the rung. If the evaluation value lower than the given threshold , then the detected rung is rejected. Algorithm 3 Main algorithm of ladder affordance detection 1:data: R as the data struct of rung, N as the data struct of node DD-GNG 2:if there is suspected object then 3:if R N 0 then 4:rung position correction(R) 5:calculating age of rung(R) 6:searching prospective rung 7:n1= get random suspected node 8:n2= get random suspected node, n26= n1 9:Evaluating rung(N,R,n1,n2), (Alg. 4) R = (ci)/(1+co) kn(2)n(1)k (6) Eval. result = ? Pass, R N+if Rejectotherwise (7) B.Rung Position Correction and Update In order to correct the position of the rung, the proposed method tracks the moving nodes in the inlier and outlier area. Based on the defi nition in Fig. 4, the error movement of starting node and end node are calculated using Eqs. 8 and 9, where ciis the number of node in inlier area. RNi is the number of nodes involve in ith detected rung. After calculating the error position of detected rungs, the start and end points of the rungs are updated using Eqs. 10 and 9. R h1and R h2are the start and end point of the detected rung. e1= 1 ci j=0 j=RNi kvjk kv(2)k(rj vj)(8) e2= 1 ci j=0 j=RNi k(v(2)v(1)vjk kv(2)k (rjvj)(9) R h1= (N Rh1v(1)e1(10) R h2= (N Rh1v(2)e2(11) C. Calculating Age of Rung Once the speculated rung is fulfi lled the evaluation stage, it does not imply that the rung is detected. It takes several iteration for measuring the confi dence level the speculated rung by calculating the their age. To do that, the initial age value of the rung is given (R g g1).The evaluation value (R ) of each speculated rung is vary for each Algorithm 4 Evaluating Rung(N,R,n1,n2) 1: n = Nhn1Nhn2 kNhn1Nhn2k 2:i = R N, get rung id 3:R Ni= 1; R Li,0= n1; R Lid i,n1 = n1 4: c 0, defi ne initial connection condition 5:Get Connection Condition(N,R,n1,n2, n) 6:co 0,ci 0 7:if c = 1 then 8:v(1)= 0 9:v(2)= N hn1N hn2 10:for j 1 to R Nido 11:if R Lid i,RLi,j = 1 then 12:ci co+1 13:else if R Lid i,RLi,j = 2 then 14:co ci+1 15:if ci 1then 16:for j 1 to R Nido 17:if R Lid i,RLi,j = 1 then 18:r = N hn1N hRLi,j 19:v = (r n) n 20:if kvv(2)k kvv(1)k then 21:if kvv(2)k kv(1)v(2)k then 22:v(1)= v 23:else if kvv(2)k kvv(1)k then 24:if kvv(1)k kv(1)v(2)k then 25:v(2)= v 26:calculating evaluation value 27: Eq. 6 28:if Passed then 29:R N+ 30:R h1= N hn1v(1) 31:R h2= N hn1v(2) iteration. If R , the age value is increased. When the age is more than a trusted threshold (g2), it means that the rung is detected. IV. EXPERIMENTALRESULT The proposed model is applied in the four legged robot used in our previous research 20. The robot is equipped various sensors for example, Quad Time of Flight (ToF) sensor as shown in Fig. 1 for detecting rungs position; set of force and touch sensors in the end effectors for grasping mechanism, and an inertial measurement unit (IMU) for pose analysis. Quad Pico sensors is used for sensing the environment and generating 3D point cloud. The robot also implements the locomotion system proposed in the previous research 21. The parameters setting of the DD-GNG is tabulated in Table I. ig. 5a shows the raw sensory data generated by the sensor. Then, the segmentation model is processed as shown in Fig. 5b. The obstacle detection is representing with a red color box (Fig. 5c). Nodes that fall insid