IROS2019国际学术会议论文集 1504

Sampling based Motion Planning for Aerial Pick and Place Hyoin Kim Hoseong Seo Jongchan Kim and H Jin Kim Abstract This paper presents a motion planning approach for an aerial pick and place task where an aerial manipulator is supposed to pick up or place an object at locations specifi ed as waypoints In particular we focus on situations where such way point constraints are imposed on certain partial state variables rather than on full state variables Our proposed framework based on rapidly exploring random trees star RRT in a bidirectional manner enables an aerial manipulator to fi nd an optimal trajectory that satisfi es waypoint constraints with only partial specifi cations Here we suggest an extra merging process to integrate the trees each originated from the start and goal point In the merging process we search various candidate points satisfying a given condition that partially constrains state variables and select a waypoint with full specifi cations optimal in the perspective of the entire trajectory Simulation and experiment results are included to validate the proposed framework I AERIALMANIPULATION Aerial manipulation can bring a new level of fl exibility to various applications such as construction manufacturing and transportation Particularly for tasks which demand active physical interaction such as holding or throwing an object some researchers attach a robotic arm with a grasper gripper to aerial robots 1 2 3 In applications of this type of aerial manipulators there exists an issue of high dimension ality which is inherent from attaching a multi DoF robotic arm to aerial robots Moreover fl ight stability and motion effi ciency should be addressed for aerial manipulators in the perspectives of the endurance and safety of the platform In the planning process these issues can be handled properly by effi ciently coordinating the full state of the platform under the dynamic constraints 4 5 This paper focuses on the planning for an aerial pick and place task where an aerial manipulator grasps an object and releases it on the target location Fig 1 These specifi ed movements on particular points are usually treated as way point constraints In particular we note that waypoints are often given in partially constrained conditions For example oftentimes when we pick an object only the end effector po sition is constrained to match the location of the target object This work was supported by Institute of InformationCommunications Technology Planning Evaluation IITP grant funded by the Korea govern ment MSIT No 2019 0 00399 Development of A I based recognition judgement and control solution for autonomous vehicle corresponding to atypical driving environment This material is based upon work supported by the Ministry of Trade Industry Energy MOTIE Korea under Indus trial Technology Innovation Program No 10051673 Hyoin Kim Hoseong Seo and H Jin Kim are with Department of Mechanical and Aerospace Engineering and Jongchan Kim is with Depart ment of Electrical and Computer Engineering Seoul National University Seoul Korea hyoinism hosung37 kjc4491 hjinkim at snu ac kr Fig 1 The manipulation part end effector of the aerial manipulator must pass yellow points to pick or place the object Each red and green line shows the trajectory of the body position and the end effector of aerial manipulator respectively and the wanted values for the other state variables are not specifi ed Then in the planning process the full trajectory of aerial manipulator should be computed by choosing the optimal values for all the unspecifi ed state variables under the waypoint constraint However most optimal planning techniques have a limitation in considering such a partially specifi ed waypoint Our objective in this paper is to develop a planning algorithm suitable for a pick and place task using an aerial manipulator in particular one that can incorporate partially constrained waypoints One method to consider the waypoint in trajectory op timization is to include waypoint terms in the objective function by penalizing the motion that does not pass through the waypoint 6 7 However since we consider not only the waypoint penalty but other objective terms such as minimal input or travel distance the performance is sensitive to the weight selection The other method is to consider the waypoint as an equality constraint In our previous work 8 we formulated a static nonlinear optimization problem NLP for an aerial manipulation system by using a specifi c representation of polynomial trajectory as a function of con tinuous time The waypoints and the dynamics of the system are considered as constraints in a unifi ed way However the conversion to static NLP using parametrization restricts a search space for an optimal solution so that the global optimality may be lost Thus the conventional optimization techniques have limitations described above to be utilized as a motion planner passing the waypoints that are partially given In addition for a high dimensional system such an 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 IEEE7396 aerial manipulator we should consider the computational load On the other hand due to the straightforward application even for high dimensional systems sampling based motion planner have been widely studied By using random compu tations instead of solving a diffi cult problem the sampling based planning quickly gives an initial solution Among the various types of sampling based motion planners rapidly exploring random trees star RRT 9 has been commonly used for optimal planning in a way that it guarantees the global optimality asymptotically However the variants of RRT do not concern the problem of a waypoint especially given with partially constrained states A possible solution for waypoint planning using RRT is to execute RRT twice and connect the start waypoint goal states However by running the RRT separately the optimality of the entire trajectory cannot be ensured since the simple RRT does not have the process to calculate the optimal waypoint In this paper we suggest a sampling based motion plan ning approach considering waypoint constraints that are partially specifi ed Our previous work addresses a motion planning problem using RRT for a single or coopera tive aerial transportation using an aerial manipulator 10 11 Here we augment the previous RRT algorithm for aerial manipulator in a bidirectional way With the proposed sampling and connecting process in waypoint space we incrementally search for optimal waypoints and improve the optimality of the entire trajectory consequently The remainder of the paper is organized as follows The background is briefl y described in section II In section III we describe the proposed planning framework Simulations and experimental results are presented in sections IV and V respectively Finally section VI contains conclusions II BACKGROUND RRT 9 is an extended form of the rapidly exploring random trees RRT 12 which quickly fi nds a solution of a feasible path in a high dimensional space RRT alleviates the lack of optimality in RRT by reducing unnecessary motions which should be discouraged in aerial manipulators in the sense of effi ciency endurance and safety In order to accelerate the convergence in RRT the informed RRT 13 which is the RRT via direct sampling using an ellip soidal heuristic technique is proposed for a specifi c purpose of the path length minimization By setting starting and goal points as foci and the current path length as the major axis and sampling nodes inside the ellipsoid the informed RRT searches for nodes which are expected to provide less expensive paths Also the convergence of the informed RRT gets accelerated for the same level of optimization comparing with the RRT In the following paragraph we briefl y describe the process of the informed RRT The more detailed descriptions for the Informed RRT are listed in 13 Let Q and qR denote the confi guration space and the state vector in RRT respectively The superscript R is used to denote that the corresponding variables are related to a b Fig 2 Examples of candidate states for a fi xed end effector position for an aerial manipulator a When the full states are given the pose of aerial manipulator is restricted to a specifi c pose b Practically for the aerial pick and place often only the position of end effector is given which gives various candidate states under the waypoint constraint The yellow area indicates the possible values of body position of multi rotor under the waypoint constraint the RRT algorithm During the execution RRT builds a treeTwhich stores the information of nodes vertices V represented in Q and their connectivity edges E The cost to reach a corresponding node is also stored in the tree In each iteration RRT samples a random node and fi nds the nearest node from the sampled node After generating a new node using local planning from the nearest to reach the sampled node the feasibility of the local path is checked If it is feasible the new node and its connection information are inserted in the tree with the corresponding cost Then the RRT searches a set of nearby nodes from the new node and executes rewiring process In the rewiring process the algorithm rearranges the connection between the nearby nodes and the new node if the cost can be reduced by the new connection The process up to this point is the same for RRT and Informed RRT After the initial path is generated in the informed RRT the sampling space gets downsized to the inside of ellipsoid whose major axis is the travel distance of the current path The algorithm continues to run until the number of generated nodes reaches a specifi ed value improving the quality of the tree III APPROACH In this section we fi rst defi ne the waypoint constraints considered in our approach Then the description of the proposed framework follows The details of the sampling process for waypoint are given at the end of the section A Waypoint constraint setup In an aerial pick and place task the location of the target object for grasping motion can be formulated as a waypoint constraint The goal point is defi ned as the location at which the object needs to be placed Let q q1 qn denotes the confi guration of the plat form in the n dimensional confi guration space If the way point is specifi ed for the full state constraint as q t qw the 7397 a b c Fig 3 Examples of an aerial manipulator i e a multi rotor with a 2 Dof arm grasping an object with various candidates for waypoint states The end effector of the aerial manipulator must pass P 1 1 0 3 a There are various paths satisfying the given waypoint constraint The red lines are the trajectories of body positions of multi rotor and the green lines are the trajectories of the position of the end effector All of the trajectories of the end effector pass the target point The graphs of the b maximum speed of rotor and c travel distance are listed in the domain of joint angles on waypoint Here yaw angle of the multi rotor on the waypoint is assumed to zero for the visual comparison planning via the waypoint is relatively easy by treating qw as a temporary goal state and executing the algorithm again from qwto the real goal However the full state constraint at waypoint fi xes the pose of the platform at a specifi ed state which may cause an ineffi cient or even dangerous robot movement in the perspective of whole trajectory On the other hand partially constrained waypoints provide more fl exibility than fully specifi ed waypoints There are various candidates of confi gurations under the given constraint so we can choose the state at the waypoint which improves the optimality of the entire path In general the waypoint constraints are given as fw q1 t q2 t qn t 0 1 gw q1 t q2 t qn t 0 2 at a point in time trajectory Here we defi ne Qwas the way point space which is the collection of possible confi gurations satisfying the above conditions In the aerial pick and place depending on the geometry or kinematics of the object and the aerial manipulator only some elements of the vector q at the waypoint are specifi ed For example in Figs 2 3 the position of the target object pw R3 specifi es only the position of end effector of the aerial manipulator We assume that we can pick an object in any direction and gives only location of an object as a constraint Other grasping conditions such as grasping angle of the gripper may be considered additionally In Fig 3 we listed various candidate states for the waypoint which demand different maximum thrust and travel distance Among them the optimal state will be selected which scores the minimum value of travel distance satisfying that the maximum motor RPM is under the limitation B Bidirectional process of RRT In order to obtain a tree stopping by in the waypoint space two RRT trees are built in a bidirectional manner 14 which originate from the start and goal confi gurations respectively For the variables in the RRT which starts from the goal confi guration we use superscript I In order to satisfy the dynamic property of the aerial manipulator we use the RRT algorithm listed in our previous work 10 11 We notice that the simple implementation of bidirectional RRT cannot give the optimal confi guration at the waypoint since there is no process to search the optimal waypoint In our new algorithm we propose the sampling process for waypoint With the proposed sampling process we search the waypoint among the various candidates which improves optimality of the whole trajectory Fig 4 gives an overview of the proposed framework First each RRT or informed RRT builds a tree from the start T and goal state T I respectively We notice that the cost defi nition of each RRT should be identical Here with the purpose of minimizing travel distance we can exploit the ellipsoidal sampling techniques of the informed RRT So far the processes are the same as the simple bidirectional RRT After the number of iteration reaches the user defi ned number our algorithm gathers the nearby nodes in each tree from the waypoint space Each set of near nodes is stored to Vw nearand VI w near Now we merge the trees to fi nd the waypoint for global optimality through the iterative process In each iteration the algorithm samples random nodes in the waypoint space From each possible tuple which consists of the three nodes each fromVw near the sampled waypoint state andVI w near the free paths and their global cost are computed Then we save the minimum cost and corresponding tuple as a candidate By repeating the process of sampling and computing the better candidate the algorithm incrementally reaches the global optimality while consequently fi nding the best waypoint states from the optimized tuple C Waypoint sampling During the waypoint sampling process independent vari ables of the confi guration vector are randomly sampled in the waypoint space The dependent variables are determined 7398 Fig 4 Overview of the proposed motion planning approach considering the partially constrained waypoint from the inverse function of waypoint constraints with respect to the independent variables We notice that the ellipsoidal sampling technique of informed RRT cannot be used to sample the waypoint states as there is no fi xed start or goal confi guration in the waypoint sampling process IV SIMULATION We validate the proposed algorithm with the agent which has two dimensional confi guration space and the pick and place aerial manipulation The objective is to generate an optimal path with minimum travel length as to use ellipsoidal sampling technique of informed RRT A 2 Dimensional waypoint task Consider the path planning problem for a simple agent in a two dimensional space Here we simulate two cases with a b c d Fig 5 Simulation results for a two dimensional agent with a b the line fw 0 and c d ellipsoidal waypoint space gw 0 The gray and red points are starting and goal points The yellow area indicates the waypoint space In a and c the light blue and green lines show the trees connecting the starting point to waypoint space and goal point to a waypoint respectively The red dotted circles are sampling space at the fi nal iteration In b and d the red line shows the entire paths from the proposed planner Here the number of the total nodes is 2 000 different constraints of the waypoints In the fi rst case the agent must intersect the line fw q1 q2 Aq b 0 for A 1 1 and b 8 For the second case we consider the waypoint space satisfying gw q1 q2 4 q1 5 2 q2 8 2 0 3 Figs 5a 5b and Figs 5c 5d show the simulation results from each case respectively The waypoint spaces are shown in yellow In Fig 5a and Fig 5c the trees from informed RRT are shown in bidirectional manner The blue and green lines show the edges inTandT I respectively The red circles show the sampling spaces In Figs 5b and 5d each red line shows the resulting trajectory respectively B Pick and place aerial manipulation We consider an aerial manipulator which is a combined system of a multi rotor and 2 DoF robotic arm see Fig 6 The state vector of the combined platform is represented by the collection of state variables of the multi rotor and robotic arm as q p b The vectors pb x y z and 1 2 denote the body position of 7399 Fig 6 Confi guration space of an aerial manipulator i e a multi rotor with a 2 Dof arm in our set ups multirotor Euler and joint angles of multirotor and robotic arm respectively As described before we assume that we can pick an object in any direction and give the location of an object as a constraint Thus the waypoint for pick and place task is given for the end effector position and equation 1 can be formulated as fw x y z 1 2 pe pw 4 g2 2 g1 1 g0 p0 pw 5 0 where gi s for i 0 1 2 are the transformation matrices in 15 p denotes a vector defi ned as p p 1 Here we collect three independent variables in the