Abstract
Multiarm systems become the trends of space robots, for the on-orbit servicing missions are becoming more complex and various. A hierarchical task planning method with multiconstraint for multiarm space robot is presented in this paper. The process of task planning is separated into two hierarchies: mission profile analysis and task node planning. In mission profile analysis, several kinds of primitive tasks and operators are defined. Then, a complex task can be decomposed into a sequence of primitive tasks by using hierarchical task network (HTN) with those primitive tasks and operators. In task node planning, algorithm is improved to adapt the continuous motion of manipulator. Then, some of the primitive tasks which cannot be executed directly because of constraints are further decomposed into several task nodes by using improved algorithm. Finally, manipulators execute the task by moving from one node to another with a simple path plan algorithm. The feasibility and effectiveness of the proposed task planning method are verified by simulation.
1. Introduction
With the continuous exploration of space, the on-orbit tasks like construction, maintenance, and service of spacecraft become more and more complex [1–3]. The space environment has the characteristics of high vacuum, intense radiation, and alternate change between high temperature and low temperature, so it is highly risky for the astronaut EVA (Extra-Vehicular Activity). However, in order to ensure the normal maintenance of equipment in capsule, astronaut change of on-orbit guard will also consume a lot of resources. So we can utilize space manipulator to assist or replace astronaut to execute all kinds of tasks. It can not only protect life security of astronaut, but improve the work efficiency and save operation cost.
The on-orbit task has the characteristic of complex circumstance and various constraint and kinds, so it becomes development tendency that utilizes space manipulator to execute complex and precision tasks. In most cases, the space manipulator is in the circumstance which is small, complicated, and lacking in supplement, and energy supplement and maintenance cost a great deal, so it bears the constraint from itself and environment when it is running. On the other hand, because of the deficiency of onboard computer and reliability requirement of on-orbit control system, the space manipulator can carry some kind of fundamental path planning algorithm. In the face of flexible on-orbit tasks, the traditional path planning method for space manipulator cannot satisfy the complex requirement of on-orbit service task, even if the path planning method is with many optimal abilities. So it is necessary to introduce the task planning upon path planning of the manipulator, to make the manipulator get the capacity to analyze, disassemble, and plan complex tasks and the capacity of configuration and optimization of resources in task level. Task planning can make space manipulator not only deal with more complex and precision tasks, but macroscopically optimize and configure resources and can delay maturing of manipulator and maintain the reliability of space manipulator.
Many researchers have studied task planning theory. Fikes and Nilsson [4] succeed in developing a robotic path planning system STRIPS based on resolution principle. This method is applied to Shakey robotic system, and it is widely adopted by researchers. Erol et al. [5–7] summarize the fundamentals and regulation of hierarchical task network (HTN) and give universal planning algorithm (UMCP).
For robot task planning problems, scholars carry out the related research work. Kaelbling and Lozano-Pérez [8] give an integrated hierarchical task and movement planning strategy, and design a depth-first recursive algorithm which traverses planning tree. The algorithm realizes the complicated logic of moving objects and clearing obstacles. But this algorithm only analyzes the upper logic of tasks and does not take all kinds of constraint in implementation into account. Lozano-Pérez and Kaelbling [9] give a task and movement planning method based on symbol search. At first, the method searches the feasible sequence of upper operation and then transforms local planning into a geometrical constraint satisfaction problem. It solves the decision problems about grasping position, setting position, and moving path under geometrical constraint. But the geometric constraint solver adopted by the algorithm needs too much pretreatment time, so the planning efficiency will reduce. Barry et al. [10] transform multibehaved planning problem into multimode planning problem and solve it by hierarchical algorithm to get different grasping position of the same object. But the planning process does not take the optimization of the performance parameters of the robot into account, so it does not satisfy space application requirements. Aiming at underwater robot, Honghao et al. [11] puts forward a practical task process model based on Petri net; it can be used for task modeling and management. But this model only can be appropriate for the presupposed typical task. Robot lacks independent ability in complicated circumstance. Zong et al. [12] proposed hierarchical planning method of SimMan combination task based on key state. Hierarchical reinforcement learning is adopted to sampling in state space, and the state with the most frequency is chosen as the key state which is used to decompose the compound task. But the algorithm only gives the action sequence and does not plan the detail action reasonably. So it cannot be used in robotic control system. Singh et al. [13] design task planning algorithm for planet rover which is based on algorithm. The algorithm is mainly used for obstacle avoidance and navigation of rover, and it introduces multimodule decision-making mechanism to decide next action of rover. But it does not take geometric constraints of robot body into account.
For multiarm system, Kim et al. [14, 15] put forward that if we want to use dual-arm robot to bin something, the system must combine obstacles avoidance algorithm, precision control, and compliant control together in planning process, but the algorithm will be highly complex. Thus, they come up with assisting dual-arm robot task planning to reach its goals by demonstration of kinematics. The method has high efficiency and security, but it needs too much artificial assist, so the universality of automation is very poor. Charoenseang et al. [16] use sensors to build the virtual operation environment on the basis of providing visual feedback and force feedback to manipulator; then the task planning is executed to reach the goals. Compared to teaching method, this method makes task planning of dual-arm coordination intelligent. Kim et al. [17, 18] use rapidly expanding random tree to generate assembly path and repeating capture path. These paths compose the plan result of dual-arm robot assembly task. Analogously, Vahrenkamp et al. [19] utilize the gradient descent method to random sample in reachable workspace which is calculated in advance and then use feasible inverse arithmetic which consumes lowly to get the limited joint angle and get task path by iteration. The method does not need specific parameter configuration, so it can plan independently. Aiming at mobile dual-arm robot, Takahama et al. [20] utilize the method which adds the task points that satisfy the constraint conditions in the straight line trajectory of end to generate task trajectory. But it is lacking in the high-precision collision detection algorithm, so it is not up to the precision operation tasks.
At present, the research at home and abroad related to motion planning of space manipulator mainly focuses on path planning algorithm and its optimization. Qi et al. [21] have studied obstacle avoidance path planning algorithm of space manipulator based on genetic algorithm. Chen et al. [22] put forward obstacle avoidance path planning algorithm of space manipulator based on C space. Yoo et al. [23] utilize laser and sonar sensor to finish obstacle avoidance task by adding specific points. The method does not use the information of joint angle, so the singular points are excluded, and it reduces the complexity of algorithm. The above-mentioned algorithms are all aimed at joint space planning; they cannot be used when there are strict requirements for the posture of end.
Based on the characteristic of on-orbit operation of space multimanipulator system, this paper studied the task planning method in complex circumstance with multiconstraint. First, the paper comes up with a double-layer task planning framework, and it separates the planning into complex task decomposition and action detail planning. Second, three necessary elements of HTN—primitive work, plan domain, and plan algorithm—are specially designed for space manipulator; then complex task can be decomposed into primitive tasks sequence based on HTN. In order to ensure the feasibility of the primitive task, the paper proposes a highly efficient precision collision detection algorithm based on point cloud model. Third, taking various constraints and optimization into consideration, algorithm is improved to plan task nodes for primitive task. In order to ensure the feasibility of the task, collision avoidance is given special attention. A highly efficient precision collision detection algorithm based on point cloud model is proposed, for its modeling approach is very simple and easy for spaceborne computer to use. Finally, the task planning method is verified by simulation.
2. General Idea
Manipulator control system is divided into three levels: task planning, path planning, and motion control. Task planning, as the top level of manipulator control system, is responsible for the receiving, analyzing, and resolving of tasks. It should be able to divide a task into a series of actions which can be planned and executed by manipulator directly. Due to the diversity of ways to execute a task, resources planning is also a part of task planning. Resources would be optimized in the process of the whole task, and planning result is given in the form of several task nodes and the resources allocation between two adjacent nodes.
In this paper, the task planning for space manipulator is divided into two stages.(1)Planner accepts the task target and divides complex task into simple tasks (primitive task) considering the ability of the manipulator. Logic feasibility of the task is ensured at the same time. Target task and initial state, as the input of planner, are the information including target location of objects and manipulators, the initial location of objects and manipulators, and whether the manipulator caught an object and which object the manipulator caught. We use HTN to do the dividing work. Although HTN is an existing method, it provides only ideas of decomposing complex task and how the method works, and no details about how a task is described mathematically. Those details usually depend on specific situation that HTN applies on. For manipulator, it is a problem to distinguish “move from A to B” and “move from C to D,” although they are all a type of MOVE task. The novelty of the HTN in this paper is(a)designing HTN plan domain for manipulator; in plan domain, some simple task (primitive task) that can be executed by manipulator directly is mathematically defined; so different tasks can be distinguished simply by their mask matrix (defined in Section 3);(b)proposing a mathematical method for describing the task executing effect; with the help of this method, HTN reasoning can be processed by computer, and planning process can work automatically.(2)Each simple task is decomposed into several simple paths automatically using a heuristic search algorithm in order to ensure that the tasks can be performed. The input of the heuristic search algorithm is a primitive task which is output from HTN. Primitive task usually refers to moving from one position to another including initial configuration and target position of manipulator. Initial configuration and target position are the initial state and final state of a manipulator action and can be regarded as two points in its work space, while heuristic algorithm can elegantly solve the way-finding problem between two points. But, heuristic algorithm usually gives an irregular trajectory which is difficult for manipulator to execute. So, in this paper, an improved heuristic algorithm is proposed and is used to obtain a series of intermediate nodes (task node). Then, trajectories between any two adjacent nodes can be planned by an existing simple path plan algorithm for manipulator.
It is worth noting that this task plan method can be applied to any manipulator theoretically, although it is particularly designed for space manipulator. When it is applied to a robot, a task can be assigned as “tighten the bolt.” Then, planner would decompose it into “move to spanner,” “grab (spanner),” “move to bolt,” and “tighten (bolt)” automatically. All those actions are part of predefined primitive tasks and can be executed by robot directly. However, there is usually no need to apply task planning to a manipulator which works on earth. On one hand, there is always a man nearby while the manipulator is working; the man who stands beside it could plan its task depending on the realities of situation and monitor any potential dangers on the manipulator. On the other hand, robot on earth (especially industrial robot) is always used to do very simple and repetitive works [24, 25]. It often works with a teach pendant or runs preprogrammed programs and has no need to do a task plan. However, space manipulator works in a harsh space environment. Space environment has the characteristics of difficult communication, lack of humans, and changing contents of task. So, it demands the autonomy of space manipulator, and task planning is necessary for space manipulator to do on-orbit servicing.
Figure 1 shows the double-layer framework for task planning.
3. Mission Profile Analysis Based on Hierarchical Task Network
3.1. Hierarchical Task Network and General Planning Algorithm
Hierarchical task network planning uses the idea of task decomposition which divides complex nonprimitive tasks into executable primitive tasks step by step. HTN abstracts the goal state of task into goal task, a layered structure of the task will be formed from complex to simple, and a primitive task which is indissoluble will be placed into the lowest level. HTN’s programming domain consists of a list of operators of primitive tasks and a list of operators of compound tasks; when the goal task and the current state of environment are given, the feasible basic motion sequence will be calculated by the planner with the use of method of simplifying the task in planning domain.
There are three types of tasks in HTN [6]:(1)Goal tasks, like goals in STRIPS, are properties we wish to make true in the world.(2)Primitive tasks are tasks we can directly achieve by executing the corresponding action.(3)Compound tasks denote desired changes that involve several goal tasks and primitive tasks.
Goal tasks and compound tasks are called nonprimitive tasks.
A primitive task, a goal task, and a compound task are syntactic construct of the form , ; , where is a literal; , , where are task parameters.
Tasks are connected together via the use of task networks; task network is a combination of tasks and task constraints; the form is as follows:where is a task with the label of and is a boolean formula constructed from variable binding constraints such as and , temporal ordering constraints such as , and truth constraints such as , , and , where , , is a literal, and . means that the task labeled with must precede the one labeled with ; , , and mean that must be true immediately after , immediately before , and between and , respectively.
A planning domain is a pair [7]where is a set of operators (one for each primitive task) and is a set of methods.
An operator tells the effect of a primitive task and is a syntactic construct of the formwhere is a primitive task symbol, is a variable of , denote the preconditions that must be satisfied before executing primitive task , and denote the primitive task’s effects (also called postconditions).
A primary task can be completed by performing the corresponding action. A nonprimary task is completed by telling the planner how to accomplish it using a method. A method is a construct of the formwhere is a nonprimitive task and is a task network. It says that one way to achieveis to perform the tasks specified in the network .
A planning problem is a triple.where, is a planning domain, is the initial state, and is the task network.
A plan strategy which could be used in any field is proposed in [7]. Planning process starts from task network and performs the following steps repeatedly until nonprimitive task no longer exists:(1)Find a nonprimitive task in and find a method in , wherein and are consistent.(2)Simplify to generate a new ; namely, use the task in to replace , and merge constraints of and .(3)Once there is no nonprimitive task in , find an instance that could satisfy all constraints. is a successful planning result of the primitive task.
In a given task network, the interaction between tasks is inevitable, some of which are beneficial, while others are harmful. For example, the result of a task will destroy the precondition of another task, or results and preconditions of the two tasks will conflict with each other. There is no general method to deal with conflicts; conflicts are resolved by adding a new task in task network in this paper.
3.2. Task Profile Analysis of Space Manipulator
Although a universal task planning program was given by HTN, all components of the planner should be designed for the specific application of space manipulator, for HTN’s operation is too abstract. A method for characterizing the state of scene in which space manipulator works is proposed in this section. Meanwhile, comparative method and state transfer operation for scene states are also given. Then, primitive task set and its operator for space manipulator are defined in this section. Task profile analysis for space manipulator based on HTN is realized by using these instancing planning elements.
3.2.1. Characterization of Scene State
The size of objects is not considered during task profile analysis. Only the logical relations between objects which are operated by manipulator are analyzed. So, the objects in the scene are abstracted into some operation interface vectors. When the end position of space manipulator is consistent with object interface vector, the object could be operated and the movement of the object would be abstracted into the change of interface vector. Suppose there are objects and a manipulator in the scene; then the state of scene is indicated by :where represents the end effector of manipulator, represent objects, and is a coordinate. For manipulator, indicates the coordinate of end position; for the object in the environment, it indicates operation interface vector which is usually defined on capture position of the object; indicates whether the manipulator carries an object or not, and only have two values:(a): there is no-load on the manipulator;(b): there is only one item among ; other items are all zero; it is used to indicate that the ith object is being caught by manipulator.
State mask is defined in order to help the calculation between state matrixes.where .
(1) Judging Whether State Satisfies a Condition. It is necessary to check whether the current state of scene satisfies the precondition (postcondition) of a task before (after) executing the task. Only specific several objects and their properties are concerned during the checking of state, and others are not taken into consideration. Suppose is the current state of the scene. State and mask describe the condition that needs to be satisfied by . If the following equation is set up:
then the current state of scene satisfies the condition described by and . Mask is on both sides of the formula which are consistent with each other.
Operator is an AND operation for matrix: if elements in are 0, then the elements on the corresponding position of are set to 0; if elements in are 1, then the corresponding elements of do not change.
(2) State Transfer. When an operator has an effect on the scene, the current scene will make the following change:where is the scene state after conversion, is the scene state before conversion, is mask that indicates operator’s effect, is the inverse of , and is the state matrix that indicates the effect of the operator.
3.2.2. Typical Primitive Tasks and Their Operators
Having taken the characteristics of various kinds of space manipulator in-orbit missions into consideration, three basic primitive tasks are defined to constitute the primitive task set: move, capture, and release. Complex tasks can be formed by these primitive tasks. Their corresponding operators contain both the precondition and postcondition of the task.
(1) MOVE. indicates that the end of the manipulator moves from the current position to a target position . Only the position of the manipulator in the state matrix is changed by MOVE tasks, as well as the location of the object caught by manipulator.
Precondition:
Postcondition:where, .
Since unloaded and loaded manipulator both could execute MOVE task, the explicit form of the operator of the MOVE task is determined by the element in the first column of the last line of the initial state matrix of the manipulator.
(2) CAPTURE. indicates that the uth object in the environment is captured by manipulator. The position of the object is . The end of manipulator and the object should be at the same position before capturing.
Precondition:Postcondition:
(3) RELEASE. indicates that the uth object in the environment is released by manipulator. The position of the end of the manipulator is . The object should have been caught by manipulator before releasing.
Precondition:
Postcondition:
For a multiarm system which consists of manipulators, the manipulator is represented by the first columns of the state matrix and its mask, and the object in the environment is represented by remaining columns.
3.2.3. Simplification of Space Manipulator Network Task
Hierarchical task network for space manipulator is established by using the primitive tasks and operators defined above according to HTN theory. The core of HTN planning algorithm is simplifying task and resolving conflicts between tasks. Since the manipulator executes tasks serially, its task planning proceeds sequentially. A strategy that simplifies the tasks and deals with conflicts sequentially is proposed in this paper. The structure of hierarchical network of the planning problem is shown in Figure 2.
The following steps give an explicit procedure of task profile analysis for space manipulator, and its flow diagram is shown in Figure 3.
Step 1. Extract task from the task network orderly.
Step 2. If current task is a nonprimitive task, find a corresponding in planning domain and replace this nonprimitive task. In order to find an appropriate method, the mask of current task is compared with the masks of all tasks in the method set; if the item which has a 1 in the mask of current task also has a 1 in the mask of method, then this method could be used to simplify this task. Go to Step .
Step 3. Compare the current state and the completion state of this task, if current state satisfies the effect of the task, then this task does not need to be executed and is deleted from the task network.
Step 4. If the current state satisfies preconditions of this task, then update the current state by using the operator of this task. Go to Step 1.
Step 5. If the current state does not satisfy the preconditions of this task, then find a task that could satisfy the preconditions in planning domain and add this new task in front of the current task.
Step 6. If the preconditions of the current task could not be satisfied, then return to planning failure.
When the planner starts work, its plan domain is predefined for space manipulator by using the method in Sections 3.2.1 and 3.2.2. Then, the decomposition process follows the steps given in Figure 2. For example, primitive tasks are defined according to Section 3.2.2, and goal task is to move an object U from point A to point B. Then planner would decompose the task into a primitive task sequence of “MOVE to A - CAPTURE U - MOVE to B - RELEASE U.” Section 5 shows the example of the decomposition process in detail.
4. Task Node Planning Based on the Improved A∗ Algorithm
The instantiated HTN planning can be directly applied in many fields, such as logistics transportation and disaster treatment. These benefit from the autonomous capability of the persons who execute the bottom layer of task network. However, the primitive tasks which are planned by HTN could not always be executed by the kind of executor like manipulator which has no autonomous capability. Thus, the task planner of manipulator needs to decompose the primitive task into several path sections which are determined by task nodes and can be planned by a typical trajectory planning algorithm. Then, the manipulator completes the primitive task by using the combination of some simple trajectories which connect the adjacent task nodes.
4.1. Manipulator Collision Detection Based on Point Cloud Model
Task profile analysis makes the task transform from complex to simple, but it is difficult for a primitive task to obtain feasible trajectory by using traditional path planning algorithm because of various constraints. Obstacle avoidance is the only necessary constraint for all tasks; thus it should be payed special attention. In order to satisfy the requirements of precise operation in space, a new high-precision collision detection algorithm for manipulator is proposed in this paper. Then, the task node planning method is achieved based on the collision detection.
4.1.1. Point Cloud Model of Scene
In order to detect the collision between different parts of the manipulator accurately, a mathematical method that can describe the shape of the whole manipulator and the environment is needed. Basic geometry envelope is widely used in the shape modeling of manipulator collision detection at present. However, it is obvious that if the surface of the object is not regular enough, the basic geometry envelope will cause the model distortion. This will lead to the loss of part of the manipulator’s workspace, and will result in the failure of many delicate tasks due to the imprecise collision detection. In addition, this method requires a lot of work of envelope of models, analyzing the shape of model artificially and selecting the approximate geometry. It is entirely unrealistic in space application. In this paper, a method of collision detection for manipulator based on point cloud modeling is proposed. In this method, dense scattered points are used to model for irregular object, then the AABB (Axis Aligned Bounding Box) algorithm is used to achieve the manipulator collision detection.
Point cloud model is defined as dense scattered points which distribute on the surface of the object. All the points in the model are defined in the coordinate system of the object itself. When the object is moving, the rotation matrix of the object can be used to transfer every point of the point cloud model into the inertial coordinate system.
After scanning all the objects in the scene by using Intersect Visitor tool of OSG (OpenSceneGraph), the point cloud model of the whole scene is obtained; example is shown as in Figure 4, where is the ith model’s point cloud matrix. is composed of 3-dimensional row vectors, which represent the points on the surface of ith object.
4.1.2. Build Model’s AABB Tree
After obtaining the scene point cloud model, the collision detection of the scene can be done by testing all intersections between and . Intersection test between and is operated as follows: all points in the and are converted to the same coordinate system (inertial system, e.g.), and whether distance between a pair of points and is less than a preset safety threshold is determined, where points and separately belong to and . If a pair of points and can be found in the scene, then the ith model and the jth model in the scene collide with each other. Besides, it is considered that the collision point exists in the middle of the connecting line between and , because the value of is very small.
Traversing all of the points between two point clouds is the simplest intersection test method, but it will consume large amount of computing resources. In this paper, the points of point cloud model are organized with clusters. AABBs of clusters are used to build AABB tree, and then AABB algorithm is used to detect the collision between any objects in the scene. A cluster is defined as a set of points which are near to each other in the point cloud model. The entire points of a cloud model are which is called the root of cluster. Each cluster is divided into left and right child clusters and recursively from the root cluster until only one point is contained in a cluster. By following the steps above, the point cloud of a model can be organized with a binary tree which is shown in Figure 5.
The method of cluster division affects the executing efficiency of the collision detection. In this paper, a cluster is divided into two child clusters according to the principle of dividing from the middle of the longest axis of the model, so that the center of the two child clusters could be far away; thereby the unintersected clusters can be quickly eliminated. In order to determine the longest axis of the model, the maximum and minimum points on directions are obtained by traversing the model points.
Three axial dimensional sizes of the model can be calculated:
Then, the model’s longest axis and its length are obtained.where is one of .
AABB of the model is divided into two parts along the perpendicular bisection plane of axis ; the points belong to one of the two parts of point cloud model forming a new child cluster. A more intuitive way to express the segmentation method is that the vertical plane is made to the axis at midpoint of and ; each side belongs to a new child cluster. Figure 5 shows an example for a two-dimensional point cloud model which split into binary trees. The red line represents the longest axis of the point cloud cluster . The blue line shows the line by which the points in the cluster are divided into two child clusters. The blue line would be a plane in 3D point cloud. The partition criterion for the left and right child clusters of model is as follows:where which is determined by (18) is the longest axis of the model.
All nodes of the point cloud binary trees are enveloped with AABB, and then the AABB tree of the model is obtained.
4.1.3. Intersection Test for AABB Trees
AABB binary trees are constructed for every rigid body of manipulator and environment obstacle, and trees which belong to manipulator or environment join together into an AABB forest. Thus, the scene model is divided into two subsets: manipulator model and environmental obstacle .
Collision detection between forests can be achieved by traversing the AABB trees which are from the two respective forests. Collision detection between every two AABB trees is achieved by examining nodes from the root node to the leave node between two trees. If there are two nodes that collide each other, further examination of the next layer is needed until leaf nodes still collide. If two leafs collide, a collision between the two objects which are represented by the two trees is considered, and the middle of the leaf nodes is the collision point. If there is no collision among the nodes from a layer and the objects which two trees represent do not collide, there is no need of further testing.
If there is still no intersection when the two AABB trees are traversed to the leaf nodes, then the objects which the two AABB trees represent are unintersected. The collision detection between leaf nodes is the process that compares the distance between the leaf nodes which represent model point with the safety margin . If the distance is less than , it is considered as a collision. At this moment, the midpoint of the line which connects the two leaf nodes is the collision point.
4.2. Idea for the Resolution of Task Node Planning
For a manipulator with degrees of freedom, suppose the number of task nodes is ; then there is intermediate moments. Each node has 6-dimension pose information. Therefore, the dimension of decision vector is . It can be seen that the dimension of the decision vector is related to the number of the task node, that is, the dimension of the decision vector itself is also a variable that needs to be decided. So, it is difficult for PSO, GA, and other mainstream intelligent optimization algorithms to solve the problem.
In this paper, heuristic search algorithm is adopted to avoid the problem of deciding the number of task nodes. The number of task nodes can be determined when the algorithm finishes. Because of the intervention of the heuristic item, the algorithm always conducts the solution towards the optimal resources allocation and therefore promises the minimum cost of the whole task.
algorithm is a typical heuristic search algorithm in artificial intelligence; the core of the algorithm is to introduce an evaluation function when selecting the next point at a current point [26].where is the actual cost from the initial point to in the state space and is the estimate cost from to target point.
4.3. Task Node Planning Using Improved Algorithm
This paper proposes a variable topology algorithm suitable for space manipulator tasks based on the traditional algorithm. The improvement idea is that each searching point can not only arrive from the adjacent parent point, but also arrive directly from the parent point of its parent point. These break the fixed topological relationship in traditional algorithm search space. When is being calculated, the traditional method of “the actual cost of arriving father point” + “the cost of parent point to the current point” is not used. Instead, by using parent point as the guide, the algorithm looks back to the farthest feasible start point. The costs of various ways that can reach the current point are calculated, and the plan with the least cost is selected. The improvement of calculating traditional algorithm’s :where is the actual cost from point to point , is current point, and is the father point of the current point. Boundary conditions: .
Define the pseudofather point : of as the point between “” and “ of ” which makes minimal. Boundary conditions: the of the start point is itself.
Then can be expressed aswhere is the pseudofather point of the father point of the current point.
Improved algorithm process is as follows.
Step 1. Set the search dimension of the planner as , search step of position as , and search step of attitude as .
Step 2. Set up two empty lists: OPEN list and CLOSE list.
Step 3. Set the pseudofather point of the start point (pointStart) as itself and add it to the OPEN list, and then calculate .
Step 4. If the CLOSE list contains the target point pointEnd, go to Step 12.
Step 5. If the OPEN list is empty, the target is not reachable and the planning ends.
Step 6. Select the point with smallest in OPEN list as the current node and move it to CLOSE list.
Step 7. Generate candidate point set by moving the current point ± 1 step in all dimensions.
Step 8. Eliminate the point which is in CLOSE list or in violation of the constraints from .
Step 9. Use the typical path planning algorithm to calculate of all points in , respectively, and remove the nodes in which violate constraints in the process of path planning.
Step 10. Record pseudofather point of all nodes in and move all points of in the OPEN list.
Step 11. Go to Step 4.
Step 12. Go back from the target point (pointEnd) to the start point (pointStart) through pseudofather points to derive the entire path, that is, all the task nodes.
The flowchart of improved algorithm is shown as in Figure 6.
In this paper, joint stroke during the task which is directly related to the life of the transmission system and affects the reliability of the space agency in orbit is used as the optimal resource, using “straight path planning” as the typical path planning algorithm. The cost between two points is :where is the degree of freedom of manipulator and the initial and final configuration of the straight path planning are, respectively, and .
End velocity, end acceleration, the collision interference of manipulator, and the environment obstacles are chosen as the constraints of the planner, where the velocity and acceleration constraints are guaranteed by the path planning algorithm; obstacle avoidance constraints are calculated by applying AABB algorithm to each configuration of path planning result.
The proposed task node planning method is mainly used for finding a path that satisfies one or more constraints. The planed path is given in the form of several nodes and needs a trajectory planning method to connect these nodes. For manipulator, trajectory planning is an existing and mature technology, so task node planning can be applied well to the manipulator. In addition, task node planning also can be applied in other resources, if there is trajectory planning method for the application object.
4.4. Planning for Dual-Arms Robot
For single arm system, the algorithm can decompose the path that the manipulator cannot complete directly under multiple constraints into several simple paths satisfying the constraint. In multiarm systems, one arm’s movement relative to the other can be regarded as obstacles constraints. However, this obstacle constraint which is different from general object in the environment is of time-varying dynamic properties, for multiple manipulators always work at the same time. Thus, it is difficult to avoid interference between multiarms by directly using the single arm planning algorithm to each single arm, respectively, in a multiarm system. This paper introduces the concept of virtual obstacles and proposes a planning strategy to solve the conflict between multiarms. A dual-arms task planning algorithm based on this strategy can make manipulator complete the work independently in the same space and at the same time and does not interfere with each other.
For a multiarm system consisting of manipulators, the scene model is decomposed into subset: is manipulators, using this model sets to reform virtual scenes . It means that one arm of the multiarm system is in the scene and other arms are ignored. Do single arm task planning, respectively, for the virtual scene , and this process for the multiarm system is defined as MADP (Multi-Arm Distributed Planning). After that, mission planning results can be gotten. virtual scene is planning independently, so each of the manipulators in the planning process does not take into account the impact of other manipulators’ movement. Therefore, these manipulators are very likely to interfere with each other when the planning results are overlapped in a common environment.
The direct cause of the interference of multiarm system in common working space is that the collided parts of two manipulators should not appear in the same space at the same time. One way to solve this contradiction is to introduce time and space constraints at all collision points in the scene. So, the manipulators that will have a collision could avoid a particular location at a certain time. The simplest approach to add time and space constraint is to add a small virtual cube obstacle whose length is a configurable fixed value at each collision point. Set , where is the number of collision points between manipulators after the fusion of virtual scene planning and is the virtual obstacle model added to the collision point. Environment model can be updated by using the following formula:where and is the new virtual obstacle set of the virtual scene after a MADP. Virtual obstacle elements of the set are not real objects in the scene; they are only used to avoid the collision between multiarm system in some particular moment. So, does not always need to appear during the task and just needs to be added to the scene at the time before and after the moment of collision , where is the moment of collision between manipulators and is a configurable avoiding time for safety. So, the environment model with time-varying characteristic is as follows:where expression of is
Dual-arm task planning strategy is as follows: using (26) iterative algorithm to update environment model until virtual obstacle set ; the scheme after MADP planning is feasible dual-arm system task planning results.
5. Experiment and Simulation
An 8-DOF fixed base manipulator is used as the test subject to verify the task plan method in this paper. Figure 7 shows the coordinate systems definition of manipulator, and Table 1 shows the D-H parameter of this manipulator.
A manipulator simulation system with 3D simulation unit which is based on OpenGL is built under IDE of VS2010. The whole task cycle of manipulator can be simulated visually by this simulation system. The hardware platform for simulation is CPU Intel i3 3.3 GHz, 4 GB RAM. Manipulator control cycle is 50 ms.
There are a manipulator , an obstacle , and an object to be operated in the simulation environment. The initial conditions are as follows: operation interface of is ; initial configuration of is . Planning domain of planner is configured as the three primitive task operators described in this paper and a compound tasks “transfer”:
The manipulator only has the ability to plan linear trajectory in Cartesian space and can only move in a straight line. The task goal is to transfer to the position .
5.1. Example of Task Profile Analysis for Object Transfer
The goal task network is described as . The end executor’s coordinate can be calculated as based on the initial configuration and the robot forward kinematics. So, the initial status to the environment is .(1)The goal network contains only one nonprimitive task. Thus, search the corresponding ; in the planning domain, is replaced by (28).(2)By retraversing the whole task network, is confirmed as a primitive task. Then, the current state is compared with the prerequisite of by using the state criterion: . And, mask of the wrong state is .(3)By search planning domain, operator can eliminate this difference of state. So a new task network is added in front of . Task network constraints become .(4)On retraversing task network again, initial state transfers to through operator, then transfers to through operator, then transfers to through operator, and then transfers to through operator at last.
At this moment, the task network has no conflict any more. The primitive task sequence which is given by planner is → → → . Objects are successfully transferred to C.
5.2. Simulation of Task Node Planning for Moving Task
The primitive task sequence which is given by task profile analysis is checked by typical path planning method. In this check, cannot be executed directly, because of the environmental obstacle. The initial configuration of the primitive task is ; the target point , . Set the planner parameters as follows: the end velocity constraints are <0.03 m/s, the end acceleration constraints are <0.03 m/s2, configuration constraints are to avoid collision from manipulator itself, and environmental constraint is to avoid obstacles O; planner search dimension is three and search step is 0.05 m. Use the improved algorithm to plan the task node; the movement of manipulator is shown in Figure 8.
(a) Starting point B
(b) Cause a collision without task planning
(c) Task node D
(d) Task node E
(e) Target point A
It can be seen in the figure that direct path planning between and will cause manipulator’s collisions as shown in Figure 8(b). With the introduction of task planning, the task nodes and are added into the movement of to . These make the manipulator reach from passing and E and bypass obstacles at the same time. All manipulator joints travel with minimum stroke under this condition.
On the basis of single arm task node planning simulation, a dual-arms task node planning simulation is carried out. A new manipulator which is completely symmetrical with the manipulator used before is added to the environment. Then, a dual-arms system is constructed which is shown in Figure 9.
The planning parameters in previous section are reused. In addition, side length of virtual obstacles is and avoiding time is . A new task is set: initial configurations of manipulator 1 are , initial configurations of manipulator 2 are , manipulator 1 is required to move to point , and manipulator 2 is required to move to point at the same time. When simple linear trajectory path planning is used for arms, the task is broken by a collision between these two arms, as shown in Figure 10(a). Then, multiarm task planning algorithm is used to plan the task again, and the planner of manipulator adds a task node for manipulators 1 and 2, respectively: and . At this time, the two manipulators can complete their tasks at the same time satisfying all constraints, as shown in Figure 10.
(a) Cause collision without task planning
(b) Initial configuration
(c) Task node H
(d) Task node I
(e) Target of task
It can be proved through simulation that the proposed algorithm can well decompose the object moving task and give the feasible task nodes for manipulator. And, the whole process is smooth and continuous.
6. Conclusion
In this paper, a double-layer framework of task plan for complex working environment is proposed considering the characteristic of space manipulator and its various kinds of on-orbit servicing missions. First, state matrix, state mask, and their relative operation are proposed based on the characteristic of space manipulator. Meanwhile, three typical primitive tasks and their operators of manipulator are defined in order to realize the HTN planner for space manipulator. Then, complex tasks of manipulator can be analyzed and resolved by using HTN automatically. Second, in order to ensure the feasibility of the task, collision avoidance which is a necessary constraint for all tasks is given special attention. A collision modeling method based on point cloud model is proposed in this paper and is applied to the AABB algorithm for collision detection. The improved collision detection method could get higher precision with little cost of calculating time, thus satisfying the requirement of space application. Finally, a task node planning method is designed by improving the algorithm. Based on this, manipulator could finish the task considering multiconstraint. In addition, the problem of collision between multiarms is also solved by introducing the concept of virtual obstacles.
A typical task of space manipulator is simulated based on a 3D visual manipulator simulation system. It has been shown that manipulator could finish a complex task successfully under the condition of multiconstrains by using a combination of simple paths. The task planning method has high computation efficiency, so that the computation of a typical task for an 8-DOF manipulator can be finished in 1-2 minutes on a small business computer. Thus, the method would work fine with spaceborne computer for space application.
In addition, HTN is a universal task plan method for lots of fields. However, when it is applied to a field, it may need to have some changes in order to adapt the field’s characteristic. For example, when we apply HTN to the space manipulator’s task planning, we designed a characterization method for space manipulator’s primitive task. Based on this characterization method, various kinds of manipulator tasks can be calculated mathematically, and HTN is used for space manipulator successfully. Thus, HTN can also be used for industrial related fields, but some work has to be done in order to adapt the method to related fields. Task node planning in this paper is a peculiar planning method for manipulator. It is appropriate for manipulator’s characteristic of smooth and continuous movement. So, task node planning is not suitable for other industrial fields, unless there is a trajectory planning method to guarantee the smooth and continuous movement.
Competing Interests
The authors declare that they have no competing interests.
Acknowledgments
The authors would like to thank their colleagues from the Robotics Research Group for helpful discussions and comments on this paper. This study was supported by the National Basic Research Program of China [2013CB733000] and the National Natural Science Foundation of China [61403038, 61573058, 61573066].