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 Abstract This 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 Terms Low 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 saputra ed tmu ac jp weihong118118 ntakesue tmu ac jp kubota tmu ac jp Yuichiro Toda is with the Graduate School of Natural Science Okayama University Okayama 700 8530 Japan e mail ytoda okayama 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 max do 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 Lsuspected Suspected 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 Ul k u 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 n0 n1 n0 n2 n0 n1 n0 n2 1 wij cos 1 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 plane s slope with vertical vectors Ob v i 1 Li Li j 0 Ti j 3 i cos 1 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 R Ni 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 R Ni kvjk kv 2 k rj vj 8 e2 1 ci j 0 j R Ni k v 2 v 1 vjk kv 2 k rj vj 9 R h1 N Rh1 v 1 e1 10 R h2 N Rh1 v 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 N hn1 N hn2 kN hn1 N hn2k 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 hn1 N hn2 10 for j 1 to R Nido 11 if R Lid i R Li j 1 then 12 ci co 1 13 else if R Lid i R Li j 2 then 14 co ci 1 15 if ci 1then 16 for j 1 to R Nido 17 if R Lid i R Li j 1 then 18 r N hn1 N hR Li j 19 v r n n 20 if kv v 2 k kv v 1 k then 21 if kv v 2 k kv 1 v 2 k then 22 v 1 v 23 else if kv v 2 k kv v 1 k then 24 if kv v 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 hn1 v 1 31 R h2 N hn1 v 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 inside the red color box has a 3442 TABLE I DD GNGPARAMETERS 1 2gmax 300100 080 008100 k 0 50 0050 0050 51000 a b c d e Fig 5 Results of topological structure using DD GNG with an obstacle a Raw point clouds data 238530 3D points b Result after segmentation process c Object detection differentiate between terrains and obstacles d Generated structure without dynamic density e Generated topological structure with dynamic density higher strength value than others The strength value of each node is used for the next process After several time steps the number of nodes in the red box area was increased As the result depicted in Figs 5 and 6 comparing with the previous model of GNG the proposed DD GNG is able to generate the dynamic density of the topological structure In Fig 5e we show that the number of nodes is denser in the speculated area while the other GNG shown in Fig 5d has lower number of nodes Results