IROS2019国际学术会议论文集 0514

1 Abstract For building a disaster response robot WAREC 1 s fully automated system for manipulating a valve this paper proposes a method for 1 detecting a valve which is far away from the robot 2 estimating the position and orientation for grasping the valve by the robot at a closer position Our methods do not need any prior information about a valve for the above mentioned detection and estimation for grasping In addition our estimation for grasping provides useful information by which WAREC 1 can rotate a valve autonomously The method 1 uses the RGB image and the point cloud data captured by Multisense SL as the input and estimate the position and orientation of a valve far away from the robot The method 2 uses both the RGB and depth images captured by KinectV2 as input and estimate information for grasping the valve Our experiments are conducted using a real disaster response robot Our experimental results show the error of the estimation by the two methods are small enough to achieve a fully automated system for detecting and rotating the valve by WAREC 1 I INTRODUCTION In recent years there have been a variety of disasters such as large earthquakes and nuclear power plants accidents These disaster sites are dangerous for people who have to dedicate themselves to rescue or repair activities Therefore the demand for a disaster response robot which can work on behalf of the people in disaster sites has been increasing To respond to these increasing demand a disaster response robot called WAREC 1 1 has been developed Its hardware is designed to enable the robot to not only move on uneven terrain but execute the required tasks in disaster sites However a system for controlling the robot is based on a tele operation system In case of a disaster the communication disruption can occur During the disruption the tele operation system for controlling the robot cannot be available which means the robot can no longer achieve the tasks To deal with this situation a disaster response robot needs to execute the required tasks autonomously on the basis of information captured by sensor mounted on the robot One of the tasks we assume WAREC 1 must conduct autonomously is rotating a valve whose shape is round This task was also used in DARPA Robotics Challenge DRC 2 as benchmark therefore the necessity of achieving this task has been recognized globally However many robots in DRC were controlled via the tele operation system to execute the task of rotating the valve The detection of the valve and the This work was done when the corresponding author belonged to Waseda University Keishi Nishikawa1 is the corresponding author working for Mitsubishi Electric Nishikawa Keishi eb MitsubishiElectric co jp Asaki Imai2 Kazuya Miyakawa2 Takuya Kanda2 Takashi Matsuzawa2 Kazuya Miyakwa2 Takuya Kanda2 Atsuo Takanishi2 are with the Department of Modern Mechanical Engineering Waseda University estimation of grasping information depend on the operator in the tele operation system or exploiting a prior detailed information of the target In this paper we propose a method for detecting the valve and estimating the grasping information automatically In addition we apply our proposed method to the disaster response robot WAREC 1 for enabling the robot to detect grasp and manipulate the valve Here we show a flow that WAREC 1 conducts the task for rotating a valve autonomously based on the sensorial information in Fig 1 We assume that the flow consists of three phases In the first phase the valve is detected and the position and orientation of the valve are estimated by one of our proposed method After the detection WAREC 1 moves from the far away in Fig 1 four meters position from the valve to Kenji Hashimoto3 is an associate professor of the Department of Mechanical Engineering Informatics Meiji University Hiroyuki Ogata4 is a professor of the Department of System Design Engineering Seikei Univeristy Disaster Response Robot s Autonomous Manipulation of Valves in Disaster Sites Based on Visual Analyses of RGBD Images Keishi Nishikawa1 Asaki Imai2 Kazuya Miyakawa2 Takuya Kanda2 Takashi Matsuzawa2 Kenji Hashimoto3 Atsuo Takanishi2 Hiroyuki Ogata4 and Jun Ohya2 a b c Fig 1 The entire flow of the task of rotating a valve by WAREC 1 a shows all the phases in fully automated system for manipulating a valve by WAREC 1 b WAREC 1 rotates a valve based on our proposal method c The estimated information for grasping in the captured point cloud data 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 IEEE4790 the position about two meters away from the valve autonomously based on the result of the detection In the second phase after the robot reaches the position two meters away from the valve candidates of the position and orientation for grasping the valve are estimated by our other proposed method After the estimation the posture of the robot for manipulating the valve is optimized using the estimated candidates for the grasping position and orientation as one of the input to the optimization and one of the candidates which optimize the posture of the robot is decided In the third phase WAREC 1 moves to the position about 0 6 0 8 meters away from the valve to generate the posture optimized in the previous phase Before grasping the valve the position and orientation for grasping the valve are estimated again to get the more precise information for grasping and compute the optimized trajectory of the hand of the robot After the estimation and optimization the robot grasps the valve based on the estimated information and the optimized trajectory of its hand II RELATED WORKS There has been many disaster response robots developed all over the world In particular those robots participating in DRC were designed to be able to execute tasks in disaster sites In 3 4 their robot autonomously or manually detects a valve and estimate the position and orientation for grasping or manipulating it based on sensorial information Their method uses a prior detailed information about the valve such as a 3D CAD model of the valve for the detection and the estimation by a fitting algorithm However there would not be any guarantee that the shape of the prior detailed information of a valve is accurately same to that of the valve for use in an unknown environment such as a disaster site Therefore it must be difficult that those robots detect the valve and estimate the information for grasping it for executing the task autonomously Norton et al also indicated in 5 the robots in DRC depended on the prior detailed information of the objects to be used or manipulated for the required tasks and the environments For these reasons a method that does not need to depend on the prior detailed information about a valve needs to be proposed Recently studies for estimating the information for grasping objects with robotic gripper by combining image processing and machine learning have been also extended In particular Lentz et al 6 proposed a method exploiting a deep learning system which takes the RGB D images as the input and predicts the grasping position and orientation as the outputs The system is trained in end to end style Following their proposed method a lot of related methods have been proposed 7 8 9 These methods exploit convolution neural network and predict the proper grasping position and orientation for a robotic gripper These methods have benefits that they do not have to use prior detailed information about the object which should be grasped However these assume that the objects are on the plane such as a table and a desk and target at grasping objects based on the estimation and picking them up stably In case of disaster response robots they have to use the objects for required tasks after grasping it Therefore the estimated results must provide grasping information which enables the robot to execute the required tasks using the objects Applying the above methods to building autonomy in executing the tasks by the disaster response robot is not sufficient To deal with the above problems our proposed method detects a valve and estimates the information for grasping the valve without using prior detailed information of the valve In addition to this our method provides information for grasping a valve which is not for just grasping but useful for executing the task of rotating a valve III PROPOSED METHOD As shown in Fig 1 first a valve four meters away from WAREC 1 needs to be detected Second candidates of the position and orientation of grasping the valve when WAREC 1 generates the optimal posture and manipulates the valve need to be estimated A Detecting a valve four meters away from WAREC 1 A flow of detecting a valve far away from WAREC 1 is shown in Fig 2 Our proposed method in this section is similar to our previous work 10 In our previous work a depth camera is used for getting RGB and depth images as the input However we found that in a real indoor environment the enough point cloud data captured with a depth camera such as KinectV2 11 cannot be obtained in a case where an object is far away from the depth camera In this paper we use point Fig 2 A flow to detect a valve far away from WAREC 1 Fig 3 Field of View computed from a bounding box 4791 cloud data captured by rotating a Laser Range Finder LRF mounted on Multisense SL 12 as the input instead of a depth image captured by depth camera such as KinectV2 1 Extracting point cloud data by using Frustum Culling Algorithm For specifying the area of the extraction a 2D image information should be associated with the point cloud data In this paper Frustum Culling Algorithm 13 is exploited The inputs to this algorithm are the point cloud data matrix corresponding to the position and orientation of the camera and the field of view of the camera The output from the algorithm is the point cloud data inside of the field of view The second one of the above inputs is constant because Multisense SL is fixed on the body of WAREC 1 and WAREC 1 is stable when the sensor captures the data For extracting the point cloud data corresponding to a valve the field of view should be computed from a bounding box which is a result of the detection in the RGB image and encloses the valve in the image as shown Fig 3 The detection in the RGB image is done by applying Faster RCNN 16 The encoder is VGG CNN M 1024 which is initialized with a pre trained model on ImageNet Faster RCNN is fine tuned with a dataset that we gather the images available on the Internet and apply data augmentation to them so that it can detect the valve in given RGB image Here we explain about how to compute the field of view from the bounding box in the horizontal direction As shown in Fig 4 the field of view is computed with a vertex named R is d 2 I 1 where I is the width of the RGB image is the field of view in the horizontal direction of the image and d is the distance between the vertex R and the vertical axis passing through the center of the image Here we define a function of Frustum Culling whose output is the point cloud data inside the field of view which is computed with a bounding box as s FC 2 where is the input point cloud data is the field of view in the vertical axis in the image and s is the point cloud data computed by Frustum Culling FC In addition for instance if the vertex of the bounding box is put on the positive area as shown in Fig 4 a the extracted point cloud data computed with the vertex and Frustum Culling should be the half one corresponding to the positive area of the RGB image For doing this here we define a function to extract the half of the point cloud data by using Eq 2 ss sign FC 3 where ss is the output of the function indicates whether the location of the vertex which is used to compute Eq 2 is in the positive area indicated by or negative area indicated by in the RGB image as shown in Fig 4 a As the next step we define the operators for operating the two different sets of point cloud data If the two sets of point cloud data should be merged the representation of the operation is a b c Fig 5 The difference of Frustum Culling according to the location of a bounding box a All the vertices of a bounding box are right to the center of the image b a bounding box is crossing over the center of the image c All the vertices of a bounding box are left to the center of the image a b Fig 4 An original image and the detected bounding box 4792 3 1 2 4 where 1 2 are two sets of point cloud data which should be operated and 3 is a result of the operation In addition to this we also define the operation for subtracting the point cloud data from the other The operation is represented as 3 1 2 5 where 1 must have more points than 2 and 3 is the result of this operation This operation actually means removing the sub space of the point cloud data from the original one We assume that how to extract the point cloud data would be different according to the location of the bounding box as shown in Fig 5 In Fig 5 the red line in each picture indicates the area where the bounding box is placed in the input RGB image and the capital letter L and R show the vertices in the left and right in the bounding box respectively For instance if the bounding box is placed as shown in Fig 5 a the point cloud corresponding to the bounding box should be a sign FC d 2 I sign FC d 2 I 6 where d and d are the distance between the vertical axis passing through the center of the RGB image s vertices in the right and left side of the box respectively a is the result of the case shown in Fig 5 a For other cases as shown in Fig 5 b and c the point cloud data corresponding to the valve can be obtained by combining Eqs 3 4 and 5 After the computation in the horizontal direction the extraction of the point cloud corresponding to the bounding box in the vertical direction can be computed similar to the horizontal case Note that in this case input point cloud data is the result of computation in the horizontal direction and the input variables should be changed appropriately according to the case of the vertical direction 2 Clustering and Non maximum suppression In our previous work 10 Euclidean Clustering and a non maximum suppression is exploited for extracting the point cloud data corresponding to the valve The exploited clustering algorithm in this paper is same as that in 10 However a non maximum suppression in this paper is different from that in 10 In 10 a cluster corresponding to the valve is selected on the basis of the number of the points which the cluster has If an object behind the valve is larger than it it is possible that the cluster corresponding to the object is selected as a valve To deal with this we define a function for evaluating each cluster based on the number of its points and the distance from the sensor to the cluster is given by n d 7 where is a set of the clusters is an element of n is a function that outputs the number of points has d is a function that outputs the distance between the position of a sensor and the centroid of is an experimental coefficient By using Eq 7 a cluster that maximizes is selected as the one corresponding to a valve In Eq 7 d is a penalty term therefore a large cluster that is more distant from a sensor hardly maximizes 3 Following Processing The processing modules following to clustering and non maximum suppression are same as those in 10 B Estimation of position and orientation for grasping a valve by WAREC 1 In our previous work 10 the proposed method just estimates the parameter describing the valve such as the position orientation and radius for detecting the valve However the estimated result would not provide the sufficient information for the robot to grasp and manipulate the valve autonomously Therefore we extend our previous work to estimate the information for grasping the valve so that the robot can rotate it autonomously In following we explain the flow of the method First of all the definition of grasping the valve is shown in Fig 6 The position for grasping is an intersection of a circumference of a valve and a line that starts from a center of a valve and passes through the centroid of the hole The orientation for grasping is a 3D orthogonal coordinate system matrix that determines the orientation of WAREC 1 s end effector s posture uniquely The first vector of the coordinate system is a normal vector of the valve The third one in the coordinate system is a unit vector that starts from a center of a valve and passes through the centroid of the hole The second one is an outer product of the first one and the third Fig 6 The definition of the position and orientation for grasping Fig 7 A flow of estimation of information for grasping 4793 one In this paper our estimation is applied to all the holes of the valve because the all the estimated results would be input to a solver of an optimization problem which finds the optimal posture of WAREC 1 for grasping and manipulating the valve A flow of our proposed method to estimate information for grasping is shown in Fig 7 For detecting the valve which means estimating the parameters of the valve such as the center coordinate system radius and normal vector our proposed method in 10 is used Note that a module for clustering and non maximum suppression are replaced by Eq 7 in this paper On the basis of the estimated parameters and the point cloud data corresponding to the valve the following modules shown in Fig 7 estimate the information for grasping 1 Compute Coordinate system With a normal vector that is one of the parameters estimated by our proposed in 10 a 3D orthogonal coordinate system matrix for the point cloud data corresponding to a valve is computed The estimated normal vector is used as the first axis of the coordinate to compute the second one and the third one 8 where is a unit vector that represents z axis in a sensor