I Introduction
A mobile manipulator is the combination of a mobile base and a manipulator, which enables both locomotion and manipulation. Therefore, mobile manipulators have become of high research interest owning to their large work space and task flexibility. Especially, mobile manipulator is suitable for picking up an object and transporting it to the desired position. Fig. 1 illustrates an industrial assembly factory, currently the partsupply tasks are still highly engaged by human workers, human workers collect the required assembly parts from the containing trays and carry them to the assembly line, and then assembled by fixed base manipulators. While mobile manipulators can be applied to automate this partsupply process, owning to the ability of picking and moving.
In order to assemble a product P, which is comprised of several types of parts , and , stored in different trays , and , respectively, the mobile manipulator may have to move to a sequence of positions to pick up different parts from different trays. The base sequence are comprised of a series of base positions, in which the mobile manipulator is able to grasp assembly parts from the tray with avoiding selfcollision and the collision between the mobile manipulator and the environment. The mobile manipulator can move to a position to pick up the parts in only one tray, or move to a position to pick up the parts stored in multiple trays, and then carries the collected assembly parts to the designated place for further assembly.
However, with increased base sequence size, the overall operation time increases significantly for the following reasons: (1) The mobile manipulator decelerates and then accelerates at every base position of the base sequence, which lowers the overall base velocity and increase the operation time. (2) Every time the mobile manipulator experience a ”stop and manipulation”, there is risk that the arrived position significantly deviates from the desired position, then the mobile manipulator has to perform timeconsuming repositioning process, and the risk increases with expanding base sequence. Therefore, it is important to prune out unnecessary base movements to improve the overall efficiency.
To trim the base sequence for such a partsupply task, it is preferable to move the mobile manipulator to the position, in which the mobile manipulator is able to pick up the assembly parts within multiple different trays. In Fig. 1, the mobile manipulator moves to the first base position and is able to collect the assembly parts in both and , thus reduces the base sequence size by one. To obtain such positions, we first generate the base region, for every target tray in the given assembly task, where the mobile manipulator can grasp all the assembly parts from that tray without collision. Then the intersections of these base regions are given high priority in the base sequence planning, in addition, the base positioning uncertainty is incorporated into the base sequence planning by restricting the size of the applicable intersections, such that the planned base sequence is both efficient and robust in real world applications.
Ii Related Work
In this research, we aim at improving the efficiency in collecting multiple parts stored in different trays, as well as enhancing the robustness with respect to base positioning uncertainty. We presented preliminary numerical analysis on reducing the unnecessary base movements in [harada2015], while in this research, the work is conducted in a complete and feasible fashion. The completeness is achieved in by obtaining all the feasible base positions, using a resolution complete method to determine the existence of collisionfree IK solutions. And the feasibility is achieved by taking into account the base positioning uncertainty of the mobile manipulator in the real environment. Moreover, the effectiveness of our proposed method is experimentally confirmed.
Vafadar et al.[vafadar2018] also researched the optimum base movements of the mobile manipulator, while they consider neither robustness nor completeness which are the main topics of this paper. There are some research on the optimum base placement of the mobile manipulator, Du et al. [du2013] used the manipulability index to determine a suitable placement. OpenRAVE [openrave] provides an inverse reachability module, which clusters the reachability space for a baseplacement sampling distribution that can be used to find out where the robot should stand in order to perform a manipulation task. Vahrenkamp [vahrenkamp2009, vahrenkamp2012, vahrenkamp2013]
conducted a series of research on reachability analysis and its application, the base positions with high probability of reaching a target pose can be efficiently found from the inverse reachability distribution. The reachability indicates the probability of finding an IK solution, while there is no guarantee on the completeness of obtained base positions. Early work on the analysis of mobility and manipulability of mobile manipulator can be found in
[pin1990, seraji1993, yamamoto1999, tchon2000, bayle2001], they were mainly concerned with the resolution of the kinematic redundancy caused by the mobile base, coordinated control of mobile base and the manipulator and manipulability analysis of the mobile manipulator.While there has been hardly any research planning the base sequence for partsupply tasks considering completeness and base positioning uncertainty. Our main contributions are:

A resolution complete method, based on precomputed reachability database, is proposed to approximate collision free IK solutions, which is especially helpful in complex environment. And a resolution complete set of base position can be obtained by such an IK approach.

Our reachability database does not only count the number of poses in the voxel, which gives the probability of a pose being reachable, but also stores the manipulator configurations accessible to the IK query.

The base positioning uncertainty in real environment is considered in the base sequence planning. Following the planned base sequence, the mobile manipulator is able to efficiently and robustly collect all the parts even there is certain deviation from the planned base position.
Iii Method Overview
Fig. 2 shows the overview of the task. In order to assemble a specific product P, which consists of three assembly parts , and stored in three different trays , and , we focus on planning the base sequence for collecting all the required assembly parts from the containing trays, and the following information is assumed to be known: (1) The types of parts to be collected and their associated trays. (2) The geometrical parameters of the assembly parts, trays and the potential obstacles in the environment. (3) The poses of the parts, trays and obstacles.
We query the precomputed reachability database, for every sampled base position, if there exist collision free IK solutions that reach all the parts in one tray. The resolution complete set of base positions, where the mobile manipulator is able to grasp the parts from a tray without selfcollision and the collision with environment, formulate the base region for the tray. Let denote the base regions of trays , denote the intersection of and , the base movements can be reduced by moving the manipulator to the intersection of different base regions, for example, the mobile manipulator moves to , and is able to pick up and , then it moves to to pick up .
However, the intersections might be very small, the mobile manipulator will fail to collect all the parts if the actually arrived position significantly deviates from the desired base position. Considering base positioning uncertainty, we set a threshold for the size of intersections, such that the intersections are applied to reduce the number of base movements only when the radius of its inscribed circle is above the threshold, thus it is robust to a certain level of base positioning uncertainty. The threshold is determined based on base positioning performance in experiments. Then a set of robust base positions are selected from the base regions or intersections, and connected by the shortest path.
Iv Inverse Kinematics
The inverse kinematics problem is to determine a set of joint angles that bring the end effector to a desired pose. In this study, we aim at obtaining a complete set of base positions where there exist at least one collision free IK solution that reaches desired grasping poses. Since solving IK and collision check are performed separately, this leads to a planner that loses the ability of being probabilistically complete. In terms of collision check between the mobile manipulator and the environment, it helps to find a collisionfree manipulator configuration by generating a variety of candidate IK solutions that cover all kinds of manipulator configurations, thus it is not likely to miss a feasible base position in which a collision free IK solution can be found. For nonredundant manipulators, it is feasible to find out all the IK solutions and preform collision check between the manipulator and the environment. However, generally there are infinite number of IK solutions for redundant manipulators, a common IK solver returns only one or multiple but not necessarily representative IK solutions, in that case, the IK solver might only find IK solutions that consequently fail in collision check, even if there exist collision free solutions. Therefore, for redundant manipulators, it is important to find out representative IK solutions for further collision check. Parametrized IK approaches for 7DOF redundant manipulator [shimizu2008, singh2010] are able to find all the feasible IK solutions, while this method is usually manipulatorspecific. We propose a manipulator independent method of obtaining approximated representative IK solutions, by querying the vicinity of a target pose in the reachability database, and it is proved to be a resolution complete method of solving IK.
Iva Reachability Database
The reachability database is constructed by sampling the joint space of the manipulator, reachable poses are obtained by calculating forward kinematics (FK). However, this may introduce the preference of singular configurations, that large variation in joint angles only result in small difference in the grasping poses. To obtain more uniform distribution of poses in the workspace, the manipulability measure
[yoshikawa1985] can be applied to relieve the congestion of configurations near the singular configurations. The downsampled resultant poses of the Fetch robot [fetch] are illustrated in Fig. 3(Left). The reachability can also be represented by 3 dimensional voxels containing sampled grasping poses, as shown in Fig. 3(Right), the Cartesian workspace is discretized into many 3 dimensional voxels according to the positions. The color of the voxels indicate the number of grasping poses end up into the corresponding voxels.For further query of poses comprised of both translation and rotation parts, 6 dimensional voxels are adopted instead, thus the workspace is discretized in both position () and orientation (represented by roll , pitch and yaw ), the grid lengths are , , , , and , respectively. All the resultant poses calculated by forward kinematics, together with their joint angles, are stored in the corresponding 6 dimensional voxels according to their positions and orientations. For example, a grasping pose , should be stored in the voxel indexed by (, , , , , ), within the voxel is a set of poses with similar position and orientation, thus the vicinity of a target pose can be quickly found by querying the pose from such a data structure.
IvB IK Query
Instead of resolving the inverse kinemtics by an IK solver, we obtain the IK solutions by querying the grasping pose in the database and access the corresponding joint angles. However, the reachability database is only a discrete representation of the continuously varying reachable poses. Probabilistically, we will fail to find an identical grasping pose in the database. Since solving IK and checking collision are treated separately, the IK solutions should be as diverse as possible in order to be resolution complete in finding collision free IK solutions. Instead, we approximate the IK solutions of the target grasping pose by querying a range of poses in the vicinity of , for , where , and the associated manipulator configurations of are the approximated IK solutions of .
In the reachability database, every 6d voxel contains a small range of poses, firstly we find the voxel containing the target pose, and all the poses within the voxel are regarded as the vicinity of the target pose. For example, by querying pose from the database, the indexed voxel is found to have 151 poses, and Fig. 4 shows a part of the manipulator configurations among them, these are the approximated IK solutions of the target pose. As the database resolution goes to infinity, the approximation error approaches zero and the queried manipulator configurations include complete IK solutions of the target pose, such that, collision free IK solutions can be found if there exist, regardless the location of the obstacles.
The feasibility and completeness of approximating IK solutions of by the associated manipulator configurations of , are proved by the following two lemmas, they are based on the differentiable mapping between configuration space and workspace, except for singular configurations. The first lemma is to prove that, as the sampling resolution goes to infinity, we can always find a manipulator configuration within the voxel, that approaches any one of the IK solutions of the target pose. The second lemma proves that, as the voxelization resolution goes to infinity, all the manipulator configurations within the voxel approach the IK solutions of the target pose. Note that the completeness of approximating all the IK solutions of is already guaranteed by lemma 1, and lemma 2, together with lemma 1, is to guarantee the completeness of obtained base positions in the next section.
Definition: Let
be the manipulator configuration vector,
and be the sampling and voxelization resolution, be the IK solution set of , be resultant pose calculated by forward kinematics of , be corresponding manipulator configuration of in the reachability database, be the pseudoinverse of Jacobian matrix , be the voxel containing pose .Lemma 1: , , , such that .
Proof: Set is equivalent to set , if , then , thus , , , such that .
Lemma 2: , , , that .
Proof: , , where is an arbitrary vector denoting redundancy, integrate two sides of the formula by a small time step, , then , such that . Because , , thus . (replace with for nonredundant manipulators)
The above lemmas apply to both redundant and nonredundant manipulators, the only problem with this method is that the continuous mapping between joint space and workspace breaks down at singular configurations.
V Base Sequence Planning
Va Base Region Calculation
The base region for a tray is a set of base positions where the mobile manipulator is able to reach all the targets in the tray, with avoiding selfcollision and the collision with the environment. Firstly, we prepare stable grasping poses with respect to the object for every object in the tray, using a grasp planner [harada2008, uto2013, tsuji2014, harada2014]. Then sample base poses in front of the target tray, as illustrated in Fig. 5, here we assume that the orientation of the mobile manipulator is constant, because for many mobile manipulators, the joint connecting the manipulator and mobile base rotates around a vertical axis, thus counteracts the rotation of the mobile base and contributes almost nothing new. Then the set of stable grasping poses with respect to the mobile base for object is obtained for every object in the tray. Finally, if there exists an grasping pose , such that we can find a collision free manipulator configuration for a pose , then can be grasped from the base position, a base position for the tray is feasible if all the objects in the tray can be grasped. All of the feasible positions formulate the base region for grasping objects from the tray.
We compare the base regions obtained by different IK approaches. The obtained base regions using IKFast plugin to find the IK solutions are shown in Fig. 6. Fig. 7 is the base regions calculated by IK query approach proposed in this paper, it is obvious the base region is larger. In the IK solver approach, IK solutions are not found in some base positions, and some of the found IK solutions fail in the collision check. For the IK query approach, the obtained base positions are not always feasible, but the base region is complete when the resolution of reachability database goes to infinity.
VB Intersection of Base Regions
To reduce the number of base movements, the mobile manipulator had better move to the intersections where the mobile manipulator is able to pick up the objects in multiple trays. For the IK solver approach, there are 5 intersections for the obtained 4 base regions of 4 trays, all of them are the intersections of two base regions, while for the IK query approach, there are the intersections of two base regions and even the intersections of 3 base regions. The intersections and their associated trays are labeled with numbers in Fig. 8 and Fig. 9.
VC Base Positioning Uncertainty
Practically, the mobile manipulator is not able to arrive at exactly the same position as planned, the positioning uncertainty is governed by sensor accuracy and environmental complexity. It will be described in section VI that, a SLAM package is used to locate the mobile manipulator in the environment, the positioning error, as a result of numerous influencing factors, is assumed to be random and homogeneous in different directions. Let the average base positioning error be (m), the mobile manipulator is most likely to arrive at a position (m) away from the desired position. Therefore, in some base positions close to the boundary, the mobile manipulator may fail to reach the all the objects in the tray when positioning uncertainty is applied. The further the base position is from boundary, the more robust the point will be in terms of base positioning uncertainty. Therefore, the most robust base position is specified by the center of the inscribed circle of the base region or intersection (Fig. 8). If the radius of the inscribed circle of an intersection is smaller than the base positioning uncertainty level, the intersection is regarded as unreliable, and should not be applied to reduce the base sequence size. Notice that, the overall operation time is scarcely influenced by choosing different positions within the base region or intersection, due to their limited area, thus the robustness is given much higher priority in this stage without sacrificing much performance.
VD Path Planning
Given an assembly task to collect the objects in trays {}, a set of base regions {} and their intersections can be obtained following the proposed method. Let , , denote all the kth order intersections, which are the intersections of k base regions, base regions are regarded as first order intersections, and be largest k, then is the set of all the possible intersections. While considering the base positioning uncertainty, the intersection are removed from the set if its inscribed circle is smaller than . Then from the remaining set of size , we select intersections that cover all the target tray and is minimized as possible, this is equivalent to the problem of assigning zeros and ones to base sequence vector and minimizing , where , subject to: , where , is 1 if is reached by the intersection, and if the robot moves to the corresponding intersection. This is the 01 knapsack problem and can be solved by BB method [kolesar1967]. Finally, take the centers of these intersections and connect them, together with starting and goal positions, by the shortest path.
For example, in Fig. 10, there are 5 base regions of 5 trays, 1 third order intersection and 4 second order intersections, while the third order intersection , thus removed from the total set of intersections. From the remaining 9 intersections, three of them are planned to reach all of the trays, then the shortest path can be searched, that connects the starting and goal position, via the the centers of their inscribed circles. If the sequence size becomes too large for searching, the shortest path can be approximated by SA method [cerny1985].
Vi Experimental Investigation
Experiments are performed to valid our method in practical application. The Fetch robot [fetch], a single arm mobile manipulator, is used in this study. The robot navigates in an indoor environment as shown in Fig. 11, it starts from the origin and picks up 3 objects stored in 3 different trays, then carries them to the goal position.
The base regions and intersections are calculated by the proposed method, and the results are already presented in Fig. 7 and Fig. 9. In order to obtain a robust base sequence, the base positioning uncertainty and repeatability are tested by looping the mobile manipulator between two fixed positions, as shown in Fig. 12, the actually arrived positions are observed to deviate about 10 cm from the desired positions. The base positioning error is the result of map accuracy, sensor accuracy, environmental complexity and the performance of the mechanical system, with so many factors involved, it is assumed to be random and homogeneous in any direction, therefore, the base positioning uncertainty level is set as 10 cm, then a sequence of base positions can be planned by algorithm described in section V.D. As a result, the mobile manipulator should move to the center of and to collect all the required parts.
We performed two sets of experiment. The first experiment is for comparison, in order to collect the parts from three trays, we move the robot to the center of and , respectively. In each base position, the mobile manipulator pick up one object from the associated tray. In the second experiment, the mobile manipulator moves to the center of to pick up parts from and , then moves to the center of to pick up part from . The total operation time is reduced due to reduced sequence size.
Vii Conclusions
Multiple pickandplace tasks involved in the assembly factory are considered in this paper. Both the efficiency and the robustness with respect to base positioning uncertainty are of major importance for practical applications. The proposed IK query method is especially helpful for finding collision free IK solutions in complex environment. It is resolution complete for generating all the feasible base positions in a partsupply task. By incorporating the base positioning uncertainty into base sequence planning, the mobile manipulator is able to robustly finish the task even if the actual arrived position deviate from the planned position. Our experiment shows, following the planned base sequence, the operation time is reduced and the partsupply task is completed under real world uncertainty.
Comments
There are no comments yet.