IROS2019国际学术会议论文集1504

Sampling-based Motion Planning for Aerial Pick-and-Place Hyoin Kim, Hoseong Seo, Jongchan Kim and H. Jin Kim AbstractThis 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. 23, 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 (gw0). 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(q15)2+(q28)2 0.(3) Figs. 5a5b and Figs. 5c5d 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 =12denote the body position of 739