A 3D Representation of Obstacles in the Robot s Reachable Area Considering Occlusions

Transcription

1 A 3D Representation of Obstacles in the Robot s Reachable Area Considering Occlusions Angelika Fetzner, Christian Frese, Christian Frey Fraunhofer Institute of Optronics, System Technologies and Image Exploitation IOSB, Karlsruhe, Germany Abstract For human-robot interaction, a 3D representation of the robot s close environment based on depth measurements is proposed. First, the current close range is defined by computing the reachable space of the robot. Then obstacles and occluded space within the close range are represented in an octree structure considering obstacle occlusions as well as robot occlusions. Based on the obstacle octree, the minimum distance between the robot and the surrounding obstacles is determined. Experimental results with depth sensor data from multiple on-board sensors of a mobile manipulator are shown. 1 Introduction Current robot manufacturing applications ensure safety by strictly separating humans from moving robots, which is achieved by fences or other physical barriers. If nevertheless a human enters the robot workspace, the robot is immediately stopped. These safety installations prohibit shared workspaces of humans and robots as well as physical human-robot interaction. To overcome these issues, in the SAPHARI project new monitoring methods for an industrial human-robot interaction scenario are developed. This scenario compiles the most important components that typical applications share with regard to sensing and perception and is described in the following: A mobile manipulator, consisting of an omni-directional mobile platform and a 7 DoF manipulator, performs an assembly task, where it picks up several parts from one workbench and moves to a second workbench. On its way and at the second workbench the parts are assembled. Finally, the robot drives to a human coworker and hands him the assembled part. During the task execution an uninvolved human worker crosses the robot workcell. Based on workspace monitoring, dangerous situations have to be detected and undesired collisions between the robot and its environment have to be avoided. To ensure safety in human-robot interaction, a 3D representation of the robot s close surroundings is a fundamental prerequisite. Based on the 3D environment representation, the minimum distance between the robot and obstacles can be computed and collision detection and avoidance strategies can be performed. Therefore, the 3D representation has to meet the following requirements: The representation has to include static as well as dynamic obstacles. Furthermore, space occluded by the robot or by other objects has to be considered. To reduce the occluded and unobserved space, information from multiple heterogeneous sensors has to be fused. For the scenario considered here, the methods have to be applicable to mobile manipulators and depth sensors mounted on the robot. For safety reasons, the monitoring of the robot environment has to meet real-time requirements, in order to enable quick reactions to sudden dangers. At the same time, 3D monitoring usually leads to high computational costs. Therefore, the 3D monitoring is restricted here to a close spatial range and short time horizons to reduce the computational costs. In the following, this space is referred to as close range. In regions with higher distances to the robot (far range), less expensive 2D or 2 1 /2D algorithms for workspace monitoring are applied [1, 2]. The paper is organized as follows: After a short overview of the related work in Section 2, the problem of selecting and placing suitable senors to monitor the close robot environment in the presented scenario is discussed in Section 3. In Section 4, the 3D close range monitoring concept is explained that consists of the computation of the robot s close range as the reachable area, the 3D representation of obstacles within the close range and the minimum distance computation. Finally, experimental results are shown in Section 5. 2 Related Work For obstacle detection, different sensors have been recommended, e.g., color cameras [3], and depth sensors [4, 5, 6]. Their optimal placement has been studied as 2D coverage problem [7] and as 3D coverage problem [8]. According to [9], a good coverage of a robot workcell is achieved if the sensors are placed high in the corners of the workcell. In [10] an algorithm for the optimal placement of presence and depth sensors for a stationary robot arm is proposed considering dynamic obstacles. These approaches usually are suitable for the placement of stationary sensors. As in the case of a mobile manipulator in the scenario described above the sensor placement 85

2 is not restricted to sensors installed in the workspace, here, the problem of selecting and placing suitable sensors is discussed with regard to mobile manipulators. For monitoring obstacles in the robot workspace and detecting collisions with these obstacles different approaches exist. Several proposed methods are based on color camera systems, as e.g., [3, 11]. As color cameras do not directly provide depth information, these approaches usually require background substraction as in [3] or detect only dedicated objects as e.g., humans in [11]. These drawbacks can be overcome by using depth sensors. Based on a single depth camera, in [12] a depth space approach to compute the minimum distance between the robot and obstacles considering occlusions is proposed. As this method works directly in the sensor s depth space, it is very fast, but the fusion of information from multiple sensors is not possible. The minimum distance to the robot based on two depth cameras is computed in [6]. To achieve real-time performance, the background is removed from the retrieved depth image using a reference background image. The remaining depth points are converted into a point cloud in the robot base frame. Point clouds from different sensors are merged and the minimum distance is computed without considering occlusions. In [9] the robot surroundings within the robot workcell are represented by a volumetric evidence grid that is computed by fusing multiple 3D imaging sensors. The occupied space is classified into background, robot, and humans, assuming that the background is a-priori known and that all foreground objects are humans. Based on convex hulls, the minimum distance between the robot and obstacles is determined in [4]. Occlusions are considered within the surveilled space that is specified as the intersection of the visible volumes of all available sensors. In [13], representing the robot environment by octrees is proposed. The approach distinguishes between free, occupied, and unknown space, but with the objective of mapping large environments. Our approach for a 3D obstacle representation is founded on octrees as well. But obstacle occlusions and robot occlusions are explicitly considered, similiar to [4]. With regard to a fast robot-obstacle distance computation, the considered area is restricted to the relevant close range of the robot. Other methods like the background subtraction mentioned in [6] cannot easily be applied to reduce the computational costs in the considered scenario, as it is more difficult to gain knowledge on the background in the case of non-stationary sensors on a mobile platform. 3 Sensor setup For acquiring a 3D obstacle representation in humanrobot interaction on- and off-board sensors are conceivable which enable the robot to perceive its environment. First, different sensor categories are considered for the environment perception. Then the sensors are simulated in a generalized way in order to find an appropriate sensor selection and placement. 3.1 Categorization of Safety Relevant Sensors In the following, relevant sensors are categorized according to the measured information. Tactile sensors measure a direct physical contact and therefore are suitable for monitoring the proximate robot surroundings. Presence sensors indicate if there is an object within a specified area. The object presence can be detected either through a physical contact (e.g., bumpers) or without physical contact (e.g., light arrays). Depth sensors provide distance information. Examples for depth sensors are 2D/3D laser scanners, time-of-flight cameras, triangulation-based sensors, and stereo camera systems. Intensity cameras provide color or gray scale intensity information in the visible as well as in the invisible frequency domain, e.g., in the infrared spectrum for detecting humans. 3.2 Simulation of sensor characteristics In order to analyze the capabilities of the different sensor types, a 3D simulation of the scenario described in Section 1 is set up. As simulation tool the 3D rigid body simulator Gazebo [14] is used in conjunction with the Robot Operating System (ROS) [15]. Gazebo is capable of simulating static and dynamic objects (e.g., robots, environment) and sensors. With the previously defined sensor categories, sensors can be simulated independently of the real sensor; only the sensor category and the sensor properties like the field of view, range, and resolution have to be specified. Using this approach, sensor data can be generated, and the monitored sections of the robot environment can be visualized without a real hardware setup of the sensor system. Sets of different sensors can be evaluated to find occlusions during a test run of the scenario. 3.3 Selection of Safety Relevant Sensors for Human-Robot Interaction For monitoring the robot environment with regard to collision avoidance, the application of depth sensors is most suitable, as the measured information directly delivers 3D point clouds of the environment. Intensity cameras and presence sensors, in contrast, provide only information projected to the image plane. In principle, the depth sensors can be mounted on the robot or can be placed in the workcell. Selecting and placing 86

3 Conference ISR ROBOTIK 2014 suitable sensor is a non-trivial task, as the number of insertable sensors is restricted due to purchase costs, energy consumption (especially on mobile robots), and the required computing capacity. If the sensors are mounted in the workcell, the main working area can be covered by several sensors with different points of view to reduce the risk of occlusions. This approach is suitable for stationary robot arms and mobile robots in small limited workcells, whereas to fully cover large workspaces of mobile robots, a huge amount of sensors or sensors with long ranges and large fields of view are needed. Furthermore, this approach makes high demands on the robot localization, if the robot has to be removed from the sensor data as described in Section Otherwise, if the sensors are placed on the robot, only a smaller region around the robot has to be covered by the sensors and the sensor placement is independent of the workcell size. Therefore, in the chosen sensor setup the sensors are mounted on the mobile platform. Figure 2: Sensor simulation with visualization of measurement ranges. 4 3D Close Range Monitoring Due to limited computational resources, the detailed 3D representation of the robot environment is restricted to the close range of the robot. To specify the close range, the space that is reachable by the robot within a certain time horizon is computed. Within the close range, the space occupied or occluded by obstacles is represented in an octree. Based on this octree, the minimum distance between the robot and the surrounding objects is determined. 3.4 Experimental Sensor Setup The mobile manipulator an omni-directional platform with a 7 DoF robot arm is equipped with two 2D laser scanners and two 3D depth cameras (Kinect sensors) as shown in Figure 1. Based on the sensor simulation, the monitored area of each sensor is visualized in Figure 2. The two 2D laser scanners, each with a 270 horizontal field of view (blue), are mounted at two edges of the mobile platform and monitor the space around the platform in a plane near the ground floor. The two Kinect sensors (yellow and green) monitor the surroundings of the manipulator. Each Kinect sensor provides a triangulation-based depth camera with opening angles of and a usable measurement range from 0.6 m to approximately 7 m. They are placed in order to achieve a high sensing volume and small occlusions. 4.1 Reachable Area The robot s close range is defined here as the space that can be reached by at least one part of the robot within a certain time horizon based on the current robot state and the maximum velocities. To represent the close range, a reachability grid is computed. In [16] a method is proposed to compute the reachability grid of a manipulator. Each manipulator link is represented by a point cloud. Beginning with the end effector link, the link point cloud is rotated around the corresponding joint through the maximum possible range of motion specified by the time horizon and the joint constraints. For each rotated point, the minimum time to reach this point is calculated. The resulting point cloud is reduced by collapsing it into a voxel grid. Then, the reduced point cloud is attached to the next link point cloud and the former steps are repeated. Due to the voxilization step at each joint the method achieves an acceptable computation time even for high DoF manipulators. To apply this approach to a mobile manipulator with an omni-directional platform, it is extended by translational motions analogue to the described rotational motion. The omni-directional platform is therefore modeled as one link, a rotational joint, and two prismatic joints for the translational movements. The considered motion constraints are the joint limits and the maximum joint and platform velocities. The robot s close range specified by the reachability grid is time variant and is computed on-line. The close range of the LWR manipulator and the close range of the mobile Figure 1: Sensor setup. 87

4 Conference ISR ROBOTIK Obstacle Octree platform including the arm based on the represented robot state are illustrated in Figure 3 and Figure 4, respectively. The minimum time to reach a cell is encoded by color. The time horizon is set to 0.5 s, the maximum joint velocities correspond to the manufacturer s data. Based on the data of multiple depth sensors an octree representation of obstacles in the close range of the robot is computed. For safety reasons the representation has to include obstacle occlusions, especially if an obstacle can occur in the space between the robot and the sensor. The acceleration constraints are disregarded, as the maximum accelerations of the robot are comparatively high. Furthermore, the error due to the assumption of infinite acceleration leads only to an overestimate of the reachable space, which is uncritical for the close range definition. As the method for the reachability computation treats all joint motions independently, it is not able to recognize invalid robot configurations (e.g., due to self-collisions). But this results, like other simplifications, only in an overestimate of the reachable area. The computational load can be further reduced, if the resulting grid contains only the information, if a cell is reachable or not, instead of the minimum time to reach the cell Octree Data Structure An octree is a hierarchical data structure. The threedimensional space is represented by a set of nodes where each node corresponds to a voxel. Each node is recursively subdivided into eight sub-voxels (nodes), if necessary, till the minimum voxel size is reached [17, 13]. With the OctoMap library [13] an open source framework exists that provides amongst others an octree data structure and ray tracing methods Sensor Model All depth sensors, as e.g., laser scanners, triangulation based sensors, or time-of-flight cameras, are modeled in a generalized way by a ray based model. The sensor model is described by a set of rays beginning in the sensor origin. The rays are specified by the opening angles and resolution of the sensor. The sensor s field of view is defined by the opening angles and the minimum and maximum range. The sensor measures the distance between the sensor origin and the point, where a ray hits the first object. In the following, these points are referred to as object points. It is assumed, that the above mentioned sensor properties opening angles, maximum range, and resolution are known. Furthermore, it is assumed that the transformation between the sensor origin and the robot base frame is known. In the case of a mobile platform with sensors mounted on the platform, the transformation between sensor and robot base frame is time invariant and determined by a calibration procedure. Otherwise, if the sensors are installed in the workspace, additionally, the robot pose has to be provided. For each sensor i the set Vi (t) of all octree nodes that are located in the sensor field of view Figure 3: Visualization of the reachable area for the manipulator only, considering a time horizon of 0.5 s. Vi (t) = {n T n in the field of view of sensor i} (1) is determined in each time-step t, where T is an octree in the current robot base frame, limited to a certain volume expansion and n is an octree node. The octree bounds are chosen in a way that the robot s close range is always located within these bounds. In the case of a static transform between the sensor and the robot base frame, Vi is time independent and has to be computed only once Sensor Data Processing Figure 4: Visualization of the reachable area for the platform with manipulator, considering a time horizon of 0.5 s. If the captured sensor data contains besides obstacle points also points on the robot, the sensor data has to be filtered 88

5 to distinguish between obstacle and robot, before the captured depth sensor data can be used to create an obstacle representation. Several filters exist that remove robot points from depth sensor data based on the known robot model, as e.g. the Realtime URDF Filter [18] for depth images and the Robot Self Filter from MoveIt! [19] for point clouds. The retrieved depth data may be available in different formats, e.g., as depth image from a Kinect-like sensor, as a list of distance measures from a laser scanner or as point cloud. To obtain a sensor independent representation, a robot and an obstacle point cloud in the sensor frame is generated from the retrieved depth data. Even though the data is in the sensor frame, points that are located outside the octree s bounding box can be partly removed to reduce the computational costs. Therefore, the maximum distance between the sensor origin and the bounding box edges is computed. All points with a depth measurement higher than this distance are certainly outside the bounding box. Then, the point clouds are transformed to the robot base frame and the remaining points outside the bounding box are filtered out. Based on the pre-processed obstacle point clouds, for each sensor i, the set P i (t) of all octree nodes that contain at least one obstacle point is determined. By means of ray tracing the nodes that are occupied or occluded by obstacle points O i (t) and the nodes occupied or occluded by robot points R i (t) are computed. Then for each sensor the free space F i (t) is given as the space in the field of view without the areas occupied or occluded by an obstacle or by the robot. F i (t) = V i (t)\(o j (t) R j (t)). (2) Information Fusion Based on the reachability grid for the current joint state, all nodes that are reachable within a certain time horizon are included in the set C(t). The sensor data pre-processing and the computation of occupied and occluded nodes is done independently for each sensor in parallel processes. To obtain the final obstacle octree, the information about occupied and occluded nodes is fused according to O(t) = ( ( )) C(t) P i (t) Õi(t) (3) i space is not considered as obstacle, as it is assumed that the sensor setup provides sufficient information to avoid dangers. This assumption is necessary, as sensor configurations used for obstacle detection often do not allow monitoring all space around the robot, especially for mobile platforms. E.g., with the sensor setup shown in Section 3.4, the space at the sides of the mobile platform is only surveyed by 2D laser scanners in one plane. But the remaining risk due to this sensor setup is assessed as acceptable, as if a human is standing next to the platform, at least a part of the human is detected by the laser scanners. Similarly, space only occluded by the robot but not occluded by an obstacle is not considered as obstacle as well. The proposed principle is illustrated in Figure 5 on the basis of two depth sensors. The scene contains the known robot (blue object) and an obstacle (red object). The two upper drafts show the nodes in the sensor s field of view (green), the nodes occupied or occluded by an obstacle (red), and the nodes occupied or occluded by the robot (blue) for the two depth sensors. The lower image shows the fusion result, in which the red cells correspond to the occupied nodes in the final octree. By fusing multiple sensors, the monitored space is increased and occlusions of the robot as well as of obstacles are reduced. The second example in Figure 6 illustrates an octree with regard to regions occluded by an obstacle as well as by the robot. It points up the necessity of being aware of robot occlusions instead of treating the robot and its occlusions as free space. As for safety reasons a conservative assumption of obstacle dimension is required, regions occluded by robot and obstacle that are not detected as free by another sensor are handled as obstacle. with Õ i (t) = O i (t)\ F j (t). (4) j,j i In the final octree all nodes are marked as occupied, that include a directly measured reachable obstacle point and all reachable nodes that are in the occluded space of one sensor and are not detected as free by another sensor. That means the monitored space is the union of all sensor fields of view within the reachable area. Unsupervised Figure 5: Illustration of the obstacle octree generation principle with two sensors (first example). 89

6 Figure 6: Illustration of the obstacle octree generation principle with two sensors (second example). 4.3 Distance Computation Based on the resulting obstacle octree containing all nodes occupied or occluded by obstacles within the reachable area, the minimum distance between sample points on the robot and the centers of the obstacle nodes is computed. The computed distance between the centers of the obstacle nodes and a sample point usually differs from the real distance between the obstacle and the sample point. Assuming that a node is fully occupied by an obstacle or only a corner of the node is occupied, the distance computation relating to the node s center leads to a maximum error e max = ± 3 2 r (5) where r represents the octree resolution. The maximum error has to be considered by the collision avoidance strategies that use the computed distance. Due to occlusions, the computed distance can also be significantly smaller than the real distance to an obstacle. This feature, however, is desired with regard to safety issues. 5 Experimental Results The proposed method has been applied to sensor data acquired from the setup described in Section 3.4. The two laser scanners, that observe the area on a horizontal plane around the platform near the ground floor, are updated with 12.5 Hz. The two Kinect sensors, that monitor the surroundings of the robot arm, are operated with an update rate of 15 Hz. Their depth images are filtered by the Realtime URDF Filter [18] to distinguish between robot and other objects. This filter renders a virtual depth image based on the known robot model and the current robot state. Comparing the real depth image retrieved from the sensor with the virtual depth image allows for deciding if the pixels represent a part of the robot or of another object. In Figure 7 snap-shots of an experimental result are shown. During the experiment, a human was walking to the left of the robot and reaching out for the robot. Behind the robot there were several static obstacles. The snapshots show the current robot state and the obstacle octree with a resolution of 10 cm, whereby for better understanding the occupied nodes are visualized red, the occluded nodes transparent. The computed minimum distance between the obstacle octree and the robot is marked by a yellow line. The computation time for the octree generation depends on the amount of object points in the robot s close range and the size of the occluded space. In the executed experiments on a 2.8 GHz Intel Core i7 quad-core processor, the computation times averaged 10 ms for the reachability grid, 40 ms for the URDF filter, 80 ms to compute the occupied and occluded obstacle nodes of a Kinect sensor, 8 ms to compute the nodes occluded by the robot in a Kinect s field of view, 1 ms to compute the occupied and occluded obstacle nodes of a laser scanner, 2 ms to fuse the data in the resulting obstacle octree and 13 ms to compute the minimum distance to the robot regarding 462 sample points on the robot. 6 Conclusions In this paper the robot s close range is defined as the space that is reachable by at least one robot link within a certain time horizon considering the current robot state and the maximum velocities. Within the close range, obstacles are represented in an obstacle octree that considers both obstacle and robot occlusions. The octree is computed by fusing information from multiple heterogeneous depth sensors, whereby the monitored space is enlarged and occlusions are reduced. The minimum distance between obstacles represented by the obstacle octree and robot is computed in order to reduce the robot velocity or to apply other collision avoidance strategies. The method is applicable to all kinds of depth sensors that deliver a dense measurement. Future work will be conducted on using the obstacle representation for collision avoidance during human-robot interaction in an industrial use case investigated in the SAPHARI project. Acknowledgement This work has been funded by the European Commission s 7th Framework Programme as part of the project SAPHARI under grant agreement ICT

7 Conference ISR ROBOTIK 2014 Figure 7: Snap-shot sequence of experimental results. Nodes of the obstacle octree containing detected obstacle points are visualized red, occluded nodes transparent. The minimum distance between the robot and the obstacle octree is shown as yellow line. References [5] C. Walter, C. Vogel and N. Elkmann: A Stationary Sensor System to Support Manipulators for Safe Human-Robot Interaction, Joint 41st Int. Symposium on Robotics and 6th German Conf. on Robotics (ISR/ROBOTIK), pp.1-6, [1] C. Frese, A. Fetzner and C. Frey: Multi-Sensor Obstacle Tracking for Safe Human-Robot Interaction, Joint 45th Int. Symposium on Robotics and 8th German Conf. on Robotics (ISR/ROBOTIK), [6] Lihui Wang: Collaborations towards adaptive manufacturing, IEEE 16th Int. Conf. on Computer Supported Cooperative Work in Design (CSCWD), pp.14-21, [2] A. Fetzner, C. Frese and C. Frey: Obstacle Detection and Tracking for Safe Human-Robot Interaction Based on Multi-Sensory Point Clouds, 6th Int. Workshop on Human-Friendly Robotics (HFR), Rome, [7] J.-J. Gonzalez-Barbosa, T. Garcia-Ramirez, J. Salas, J.-B. Hurtado-Ramos and J.-d.-J. Rico-Jimenez: Optimal camera placement for total coverage, IEEE Int. Conf. on Robotics and Automation (ICRA), pp , [3] D. Henrich and T. Gecks: Multi-camera collision detection between known and unknown objects, Second ACM/IEEE Int. Conf. on Distributed Smart Cameras (ICDSC), [8] E. Becker, G. Guerra-Filho and F. Makedon: Automatic sensor placement in a 3D volume, PETRA 09, [4] M. Fischer and D. Henrich: 3D Collision Detection for Industrial Robots and Unknown Obstacles Using Multiple Depth Images, In T. Kröger and F. Wahl, Advances in Robotics Research - Theory, Implementation, Application, Springer, [9] P. Rybski, P. Anderson-Sprecher, D. Huber, C. Niessl and R. Simmons: Sensor fusion for human safety 91

8 in industrial workcells, IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS), pp , [10] F. Flacco and A. De Luca: Multiple depth/presence sensors: Integration and optimal placement for human/robot coexistence, IEEE Int. Conf. on Robotics and Automation (ICRA), [11] J.T.C. Tan, Feng Duan, Ye Zhang, R. Kato and T. Arai: Safety design and development of humanrobot collaboration in cellular manufacturing, IEEE Int. Conf. on Automation Science and Engineering (CASE), pp , [12] F. Flacco, T. Kroger, A. De Luca and O. Khatib: A depth space approach to human-robot collision avoidance, IEEE Int. Conf. on Robotics and Automation (ICRA), pp , [13] A. Hornung, K. M. Wurm, M. Bennewitz, C. Stachniss and W. Burgard: OctoMap: An Efficient Probabilistic 3D Mapping Framework Based on Octrees, Autonomous Robots, [14] Gazebo, [15] Robot Operating System (ROS), [16] P. Anderson-Sprecher and R. Simmons: Voxelbased motion bounding and workspace estimation for robotic manipulators, IEEE Int. Conf. on Robotics and Automation (ICRA), pp , [17] C. L. Jackins and S. L. Tanimoto: Oct-trees and their use in representing three-dimensional objects, Computer Graphics and Image Processing, pp , [18] N. Blodow: Realtime URDF Filter, [19] I. A. Sucan and S. Chitta: MoveIt!, 92